From c5a49ab720a42efff963efa32147d9c47ea71b98 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Sun, 24 May 2020 15:04:44 +0200 Subject: [PATCH 001/199] Fix imu link (#566) * Fix IMU link * IMU link published even if `publish_tf` and `publish_map_tf` are set to `false` * Minor fix --- latest_changes.md | 4 + .../zed_nodelet/src/zed_wrapper_nodelet.cpp | 89 +++++++++---------- zed_wrapper/params/common.yaml | 4 +- 3 files changed, 48 insertions(+), 49 deletions(-) diff --git a/latest_changes.md b/latest_changes.md index 6108d7c0..8ca82587 100644 --- a/latest_changes.md +++ b/latest_changes.md @@ -1,6 +1,10 @@ LATEST CHANGES ============== +IMU fix (2020-05-24) +-------------------- +- Fix issue with IMU frame link when `publish_tf` and `publish_map_tf` are disabled + New package: zed_nodelets (2020-03-20) --------------------------------------- - Added the new `zed_interfaces/RGBDSensors` custom topic that contains RGB, Depth, IMU and Magnetometer synchronized topics 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 b2720233..0895709a 100644 --- a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp @@ -520,7 +520,7 @@ void ZEDWrapperNodelet::onInit() { mSensPeriodMean_usec.reset(new sl_tools::CSmartMean(mSensPubRate / 2)); - } + } // Publish camera imu transform in a latched topic if (mZedRealCamModel != sl::MODEL::ZED) { @@ -1454,7 +1454,7 @@ void ZEDWrapperNodelet::start_pos_tracking() { } void ZEDWrapperNodelet::publishOdom(tf2::Transform odom2baseTransf, sl::Pose& slPose, ros::Time t) { - nav_msgs::OdometryPtr odomMsg = boost::make_shared(); + nav_msgs::OdometryPtr odomMsg = boost::make_shared(); odomMsg->header.stamp = t; odomMsg->header.frame_id = mOdometryFrameId; // frame @@ -2854,56 +2854,51 @@ void ZEDWrapperNodelet::pubSensCallback(const ros::TimerEvent& e) { mPubImuRaw.publish(imuRawMsg); } - // Publish IMU tf only if enabled - if (mPublishTf) { - // Camera to pose transform from TF buffer - tf2::Transform cam_to_pose; - - std::string poseFrame; - static bool first_error = true; - - // Look up the transformation from base frame to map link - try { - poseFrame = mPublishMapTf ? mMapFrameId : mOdometryFrameId; + // Publish IMU tf + // Camera to pose transform from TF buffer + tf2::Transform cam_to_odom; - // Save the transformation from base to frame - geometry_msgs::TransformStamped c2p = - mTfBuffer->lookupTransform(poseFrame, mCameraFrameId, ros::Time(0)); - // Get the TF2 transformation - tf2::fromMsg(c2p.transform, cam_to_pose); - } catch (tf2::TransformException& ex) { - if(!first_error) { - NODELET_DEBUG_THROTTLE(1.0, "Transform error: %s", ex.what()); - NODELET_WARN_THROTTLE(1.0, "The tf from '%s' to '%s' is not available.", - mCameraFrameId.c_str(), mMapFrameId.c_str()); - NODELET_WARN_THROTTLE(1.0, "Note: one of the possible cause of the problem is the absense of an instance " - "of the `robot_state_publisher` node publishing the correct static TF transformations " - "or a modified URDF not correctly reproducing the ZED " - "TF chain '%s' -> '%s' -> '%s'", - mBaseFrameId.c_str(), mCameraFrameId.c_str(),mDepthFrameId.c_str()); - first_error=false; - } + //std::string poseFrame; + static bool first_error = true; - return; + // Look up the transformation from imu frame to odom link + try { + // Save the transformation from base to frame + geometry_msgs::TransformStamped c20 = + mTfBuffer->lookupTransform(mOdometryFrameId, mCameraFrameId, ros::Time(0)); + // Get the TF2 transformation + tf2::fromMsg(c20.transform, cam_to_odom); + } catch (tf2::TransformException& ex) { + if(!first_error) { + NODELET_DEBUG_THROTTLE(1.0, "Transform error: %s", ex.what()); + NODELET_WARN_THROTTLE(1.0, "The tf from '%s' to '%s' is not available.", + mCameraFrameId.c_str(), mMapFrameId.c_str()); + NODELET_WARN_THROTTLE(1.0, "Note: one of the possible cause of the problem is the absense of a publisher " + "of the base_link -> odom transform. " + "This happens if `publish_tf` is `false` and no other nodes publish the " + "TF chain '%s' -> '%s' -> '%s'", + mOdometryFrameId.c_str(), mBaseFrameId.c_str(), mCameraFrameId.c_str()); + first_error=false; } - // IMU Quaternion in Map frame - tf2::Quaternion imu_q; - imu_q.setX(sens_data.imu.pose.getOrientation()[0]); - imu_q.setY(sens_data.imu.pose.getOrientation()[1]); - imu_q.setZ(sens_data.imu.pose.getOrientation()[2]); - imu_q.setW(sens_data.imu.pose.getOrientation()[3]); - // Pose Quaternion from ZED Camera - tf2::Quaternion map_q = cam_to_pose.getRotation(); - // Difference between IMU and ZED Quaternion - tf2::Quaternion delta_q = imu_q * map_q.inverse(); - tf2::Transform imu_pose; - imu_pose.setIdentity(); - imu_pose.setRotation(delta_q); - // Note, the frame is published, but its values will only change if someone - // has subscribed to IMU - publishImuFrame(imu_pose, mFrameTimestamp); // publish the imu Frame + return; } + + // IMU Quaternion in Map frame + tf2::Quaternion imu_q; + imu_q.setX(sens_data.imu.pose.getOrientation()[0]); + imu_q.setY(sens_data.imu.pose.getOrientation()[1]); + imu_q.setZ(sens_data.imu.pose.getOrientation()[2]); + imu_q.setW(sens_data.imu.pose.getOrientation()[3]); + // Pose Quaternion from ZED Camera + tf2::Quaternion map_q = cam_to_odom.getRotation(); + // Difference between IMU and ZED Quaternion + tf2::Quaternion delta_q = imu_q * map_q.inverse(); + tf2::Transform imu_pose; + imu_pose.setIdentity(); + imu_pose.setRotation(delta_q); + + publishImuFrame(imu_pose, mFrameTimestamp); // publish the imu Frame } void ZEDWrapperNodelet::device_poll_thread_func() { diff --git a/zed_wrapper/params/common.yaml b/zed_wrapper/params/common.yaml index cd8b97d2..bc3c92fd 100644 --- a/zed_wrapper/params/common.yaml +++ b/zed_wrapper/params/common.yaml @@ -24,7 +24,7 @@ general: zed_id: 0 serial_number: 0 resolution: 2 # '0': HD2K, '1': HD1080, '2': HD720, '3': VGA - grab_frame_rate: 30 # Frequency of frame grabbing for internal SDK operations + grab_frame_rate: 15 # Frequency of frame grabbing for internal SDK operations gpu_id: -1 base_frame: 'base_link' # must be equal to the frame_id used in the URDF file verbose: false # Enable info message by the ZED SDK @@ -37,7 +37,7 @@ video: extrinsic_in_camera_frame: true # if `false` extrinsic parameter in `camera_info` will use ROS native frame (X FORWARD, Z UP) instead of the camera frame (Z FORWARD, Y DOWN) [`true` use old behavior as for version < v3.1] depth: - quality: 3 # '0': NONE, '1': PERFORMANCE, '2': QUALITY, '3': ULTRA + quality: 1 # '0': NONE, '1': PERFORMANCE, '2': QUALITY, '3': ULTRA sensing_mode: 0 # '0': STANDARD, '1': FILL (not use FILL for robotic applications) depth_stabilization: 1 # `0`: disabled, `1`: enabled openni_depth_mode: 0 # '0': 32bit float meters, '1': 16bit uchar millimeters From 98702be8bfafd67428e44f131e0aa227100a0c9c Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Sun, 24 May 2020 18:32:15 +0200 Subject: [PATCH 002/199] Fix `imu/data_raw` topic timestamp --- zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 0895709a..cb4549fd 100644 --- a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp @@ -2813,7 +2813,7 @@ void ZEDWrapperNodelet::pubSensCallback(const ros::TimerEvent& e) { if (imu_RawSubNumber > 0) { sensor_msgs::ImuPtr imuRawMsg = boost::make_shared(); - imuRawMsg->header.stamp = mFrameTimestamp; // t; + imuRawMsg->header.stamp = ts_imu; // t; imuRawMsg->header.frame_id = mImuFrameId; imuRawMsg->angular_velocity.x = sens_data.imu.angular_velocity[0] * DEG2RAD; imuRawMsg->angular_velocity.y = sens_data.imu.angular_velocity[1] * DEG2RAD; From 636f019fa0334cd22a2cb0dfadda87b37185df70 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Wed, 3 Jun 2020 13:40:30 +0200 Subject: [PATCH 003/199] Fix timestamp (#570) * Fix IMU link * IMU link published even if `publish_tf` and `publish_map_tf` are set to `false` * Minor fix * Improve MR #557 about problems with timestamp coherency * Update changelog --- latest_changes.md | 4 + zed_nodelets/CMakeLists.txt | 1 + .../include/zed_wrapper_nodelet.hpp | 2 +- .../zed_nodelet/src/zed_wrapper_nodelet.cpp | 88 +++++++++++-------- 4 files changed, 56 insertions(+), 39 deletions(-) diff --git a/latest_changes.md b/latest_changes.md index 8ca82587..4a353a3b 100644 --- a/latest_changes.md +++ b/latest_changes.md @@ -1,6 +1,10 @@ LATEST CHANGES ============== +Timestamp fix (2020-06-03) +-------------------------- +- Fix timestamp update coherency due to parallel threads. Thanks to @matlabbe + IMU fix (2020-05-24) -------------------- - Fix issue with IMU frame link when `publish_tf` and `publish_map_tf` are disabled diff --git a/zed_nodelets/CMakeLists.txt b/zed_nodelets/CMakeLists.txt index 0c887d95..ae7fde12 100644 --- a/zed_nodelets/CMakeLists.txt +++ b/zed_nodelets/CMakeLists.txt @@ -74,6 +74,7 @@ catkin_package( zed_interfaces ) + ############################################################################### # SOURCES 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 0a30b6c6..12b60523 100644 --- a/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp +++ b/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp @@ -367,7 +367,7 @@ class ZEDWrapperNodelet : public nodelet::Nodelet { /* \brief Perform object detection and publish result */ - void detectObjects(bool publishObj, bool publishViz); + void detectObjects(bool publishObj, bool publishViz, ros::Time t); /* \brief Generates an univoque color for each object class ID */ 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 cb4549fd..7d97e59e 100644 --- a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp @@ -1505,7 +1505,7 @@ void ZEDWrapperNodelet::publishPose(ros::Time t) { } std_msgs::Header header; - header.stamp = mFrameTimestamp; + header.stamp = t; header.frame_id = mMapFrameId; geometry_msgs::Pose pose; @@ -1813,7 +1813,7 @@ void ZEDWrapperNodelet::pubFusedPointCloudCallback(const ros::TimerEvent& e) { return; } - pointcloudFusedMsg->header.stamp = mFrameTimestamp; + //pointcloudFusedMsg->header.stamp = t; mZed.requestSpatialMapAsync(); while (mZed.getSpatialMapRequestStatusAsync() == sl::ERROR_CODE::FAILURE) { @@ -1833,7 +1833,7 @@ void ZEDWrapperNodelet::pubFusedPointCloudCallback(const ros::TimerEvent& e) { if (pointcloudFusedMsg->width != ptsCount || pointcloudFusedMsg->height != 1) { // Initialize Point Cloud message - // https://github.com/ros/common_msgs/blob/jade-devel/sensor_msgs/include/sensor_msgs/point_cloud2_iterator.h + // https://github.com/ros/common_msgs/blob/jade-devel/sensor_msgs/include/sensor_msgs/point_cloud2_iterator.h pointcloudFusedMsg->header.frame_id = mMapFrameId; // Set the header values of the ROS message pointcloudFusedMsg->is_bigendian = false; pointcloudFusedMsg->is_dense = false; @@ -1854,12 +1854,15 @@ void ZEDWrapperNodelet::pubFusedPointCloudCallback(const ros::TimerEvent& e) { //NODELET_INFO_STREAM("Chunks: " << mFusedPC.chunks.size()); + + int index = 0; float* ptCloudPtr = (float*)(&pointcloudFusedMsg->data[0]); int updated = 0; for (int c = 0; c < mFusedPC.chunks.size(); c++) { if (mFusedPC.chunks[c].has_been_updated || resized) { + updated++; size_t chunkSize = mFusedPC.chunks[c].vertices.size(); @@ -1871,6 +1874,8 @@ void ZEDWrapperNodelet::pubFusedPointCloudCallback(const ros::TimerEvent& e) { memcpy(ptCloudPtr, cloud_pts, 4 * chunkSize * sizeof(float)); ptCloudPtr += 4 * chunkSize; + + pointcloudFusedMsg->header.stamp = sl_tools::slTime2Ros(mFusedPC.chunks[c].timestamp); } } else { @@ -1971,6 +1976,7 @@ void ZEDWrapperNodelet::fillCamInfo(sl::Camera& zed, sensor_msgs::CameraInfoPtr #else if (rawParam) { if(mUseOldExtrinsic) { // Camera frame (Z forward, Y down, X right) + std::vector R_ = sl_tools::convertRodrigues(zedParam.R); float* p = R_.data(); @@ -2245,7 +2251,7 @@ void ZEDWrapperNodelet::dynamicReconfCallback(zed_nodelets::ZedConfig& config, u } void ZEDWrapperNodelet::pubVideoDepthCallback(const ros::TimerEvent& e) { - static sl::Timestamp lastTs = 0; + static sl::Timestamp lastZedTs = 0; // Used to calculate stable publish frequency uint32_t rgbSubnumber = mPubRgb.getNumSubscribers(); uint32_t rgbRawSubnumber = mPubRawRgb.getNumSubscribers(); @@ -2275,7 +2281,7 @@ void ZEDWrapperNodelet::pubVideoDepthCallback(const ros::TimerEvent& e) { stereoSubNumber+stereoRawSubNumber == 0) { mPublishingData = false; - lastTs = 0; + lastZedTs = 0; return; } @@ -2284,35 +2290,36 @@ void ZEDWrapperNodelet::pubVideoDepthCallback(const ros::TimerEvent& e) { sl::Mat leftZEDMat, rightZEDMat, leftGrayZEDMat, rightGrayZEDMat, depthZEDMat, disparityZEDMat, confMapZEDMat; - - // Retrieve RGBA Left image and use Timestamp to check if image is new mZed.retrieveImage(leftZEDMat, sl::VIEW::LEFT, sl::MEM::CPU, mMatResolVideo); - if(leftZEDMat.timestamp==lastTs) { + if(leftZEDMat.timestamp==lastZedTs) { return; // No new image! } - if(lastTs.data_ns!=0) { - double period_sec = static_cast(leftZEDMat.timestamp.data_ns - lastTs.data_ns)/1e9; + if(lastZedTs.data_ns!=0) { + double period_sec = static_cast(leftZEDMat.timestamp.data_ns - lastZedTs.data_ns)/1e9; //NODELET_DEBUG_STREAM( "PUBLISHING PERIOD: " << period_sec << " sec @" << 1./period_sec << " Hz") ; mVideoDepthPeriodMean_sec->addValue(period_sec); //NODELET_DEBUG_STREAM( "MEAN PUBLISHING PERIOD: " << mVideoDepthPeriodMean_sec->getMean() << " sec @" << 1./mVideoDepthPeriodMean_sec->getMean() << " Hz") ; } - lastTs = leftZEDMat.timestamp; + lastZedTs = leftZEDMat.timestamp; + + // Timestamp to be used for topics headers + ros::Time stamp = mFrameTimestamp; // Publish the left == rgb image if someone has subscribed to if (leftSubnumber > 0 || rgbSubnumber > 0) { if (leftSubnumber > 0) { sensor_msgs::ImagePtr leftImgMsg = boost::make_shared(); - publishImage(leftImgMsg, leftZEDMat, mPubLeft, mLeftCamInfoMsg, mLeftCamOptFrameId, mFrameTimestamp); + publishImage(leftImgMsg, leftZEDMat, mPubLeft, mLeftCamInfoMsg, mLeftCamOptFrameId, stamp); } if (rgbSubnumber > 0) { sensor_msgs::ImagePtr rgbImgMsg = boost::make_shared(); // rgb is the left image - publishImage(rgbImgMsg, leftZEDMat, mPubRgb, mRgbCamInfoMsg, mDepthOptFrameId, mFrameTimestamp); + publishImage(rgbImgMsg, leftZEDMat, mPubRgb, mRgbCamInfoMsg, mDepthOptFrameId, stamp); } } @@ -2327,14 +2334,14 @@ void ZEDWrapperNodelet::pubVideoDepthCallback(const ros::TimerEvent& e) { sensor_msgs::ImagePtr leftGrayImgMsg = boost::make_shared(); publishImage(leftGrayImgMsg, leftGrayZEDMat, mPubLeftGray, mLeftCamInfoMsg, mLeftCamOptFrameId, - mFrameTimestamp); + stamp); } if (rgbGraySubnumber > 0) { sensor_msgs::ImagePtr rgbGrayImgMsg = boost::make_shared(); publishImage(rgbGrayImgMsg, leftGrayZEDMat, mPubRgbGray, mRgbCamInfoMsg, mDepthOptFrameId, - mFrameTimestamp); // rgb is the left image + stamp); // rgb is the left image } } @@ -2348,14 +2355,14 @@ void ZEDWrapperNodelet::pubVideoDepthCallback(const ros::TimerEvent& e) { sensor_msgs::ImagePtr rawLeftImgMsg = boost::make_shared(); publishImage(rawLeftImgMsg, leftZEDMat, mPubRawLeft, mLeftCamInfoRawMsg, mLeftCamOptFrameId, - mFrameTimestamp); + stamp); } if (rgbRawSubnumber > 0) { sensor_msgs::ImagePtr rawRgbImgMsg = boost::make_shared(); publishImage(rawRgbImgMsg, leftZEDMat, mPubRawRgb, mRgbCamInfoRawMsg, mDepthOptFrameId, - mFrameTimestamp); + stamp); } } @@ -2369,14 +2376,14 @@ void ZEDWrapperNodelet::pubVideoDepthCallback(const ros::TimerEvent& e) { sensor_msgs::ImagePtr rawLeftGrayImgMsg = boost::make_shared(); publishImage(rawLeftGrayImgMsg, leftGrayZEDMat, mPubRawLeftGray, mLeftCamInfoRawMsg, mLeftCamOptFrameId, - mFrameTimestamp); + stamp); } if (rgbGrayRawSubnumber > 0) { sensor_msgs::ImagePtr rawRgbGrayImgMsg = boost::make_shared(); publishImage(rawRgbGrayImgMsg, leftGrayZEDMat, mPubRawRgbGray, mRgbCamInfoRawMsg, mDepthOptFrameId, - mFrameTimestamp); + stamp); } } @@ -2388,7 +2395,7 @@ void ZEDWrapperNodelet::pubVideoDepthCallback(const ros::TimerEvent& e) { sensor_msgs::ImagePtr rightImgMsg = boost::make_shared(); - publishImage(rightImgMsg, rightZEDMat, mPubRight, mRightCamInfoMsg, mRightCamOptFrameId, mFrameTimestamp); + publishImage(rightImgMsg, rightZEDMat, mPubRight, mRightCamInfoMsg, mRightCamOptFrameId, stamp); } // Publish the right image GRAY if someone has subscribed to @@ -2398,7 +2405,7 @@ void ZEDWrapperNodelet::pubVideoDepthCallback(const ros::TimerEvent& e) { mZed.retrieveImage(rightGrayZEDMat, sl::VIEW::RIGHT_GRAY, sl::MEM::CPU, mMatResolVideo); sensor_msgs::ImagePtr rightGrayImgMsg = boost::make_shared(); - publishImage(rightGrayImgMsg, rightGrayZEDMat, mPubRightGray, mRightCamInfoMsg, mRightCamOptFrameId, mFrameTimestamp); + publishImage(rightGrayImgMsg, rightGrayZEDMat, mPubRightGray, mRightCamInfoMsg, mRightCamOptFrameId, stamp); } // Publish the right raw image if someone has subscribed to @@ -2409,7 +2416,7 @@ void ZEDWrapperNodelet::pubVideoDepthCallback(const ros::TimerEvent& e) { sensor_msgs::ImagePtr rawRightImgMsg = boost::make_shared(); - publishImage(rawRightImgMsg, rightZEDMat, mPubRawRight, mRightCamInfoRawMsg, mRightCamOptFrameId, mFrameTimestamp); + publishImage(rawRightImgMsg, rightZEDMat, mPubRawRight, mRightCamInfoRawMsg, mRightCamOptFrameId, stamp); } // Publish the right raw image GRAY if someone has subscribed to @@ -2419,7 +2426,7 @@ void ZEDWrapperNodelet::pubVideoDepthCallback(const ros::TimerEvent& e) { sensor_msgs::ImagePtr rawRightGrayImgMsg = boost::make_shared(); - publishImage(rawRightGrayImgMsg, rightGrayZEDMat, mPubRawRightGray, mRightCamInfoRawMsg, mRightCamOptFrameId, mFrameTimestamp); + publishImage(rawRightGrayImgMsg, rightGrayZEDMat, mPubRawRightGray, mRightCamInfoRawMsg, mRightCamOptFrameId, stamp); } // Stereo couple side-by-side @@ -2431,7 +2438,7 @@ void ZEDWrapperNodelet::pubVideoDepthCallback(const ros::TimerEvent& e) { sensor_msgs::ImagePtr stereoImgMsg = boost::make_shared(); - sl_tools::imagesToROSmsg(stereoImgMsg, leftZEDMat, rightZEDMat, mCameraFrameId, mFrameTimestamp); + sl_tools::imagesToROSmsg(stereoImgMsg, leftZEDMat, rightZEDMat, mCameraFrameId, stamp); mPubStereo.publish(stereoImgMsg); } @@ -2445,7 +2452,7 @@ void ZEDWrapperNodelet::pubVideoDepthCallback(const ros::TimerEvent& e) { sensor_msgs::ImagePtr rawStereoImgMsg = boost::make_shared(); - sl_tools::imagesToROSmsg(rawStereoImgMsg, leftZEDMat, rightZEDMat, mCameraFrameId, mFrameTimestamp); + sl_tools::imagesToROSmsg(rawStereoImgMsg, leftZEDMat, rightZEDMat, mCameraFrameId, stamp); mPubRawStereo.publish(rawStereoImgMsg); } @@ -2457,14 +2464,14 @@ void ZEDWrapperNodelet::pubVideoDepthCallback(const ros::TimerEvent& e) { sensor_msgs::ImagePtr depthImgMsg = boost::make_shared(); - publishDepth(depthImgMsg, depthZEDMat, mFrameTimestamp); // in meters + publishDepth(depthImgMsg, depthZEDMat, stamp); // in meters } // Publish the disparity image if someone has subscribed to if (disparitySubnumber > 0) { mZed.retrieveMeasure(disparityZEDMat, sl::MEASURE::DISPARITY, sl::MEM::CPU, mMatResolDepth); - publishDisparity(disparityZEDMat, mFrameTimestamp); + publishDisparity(disparityZEDMat, stamp); } // Publish the confidence map if someone has subscribed to @@ -2474,7 +2481,7 @@ void ZEDWrapperNodelet::pubVideoDepthCallback(const ros::TimerEvent& e) { sensor_msgs::ImagePtr confMapMsg = boost::make_shared(); - sl_tools::imageToROSmsg(confMapMsg, confMapZEDMat, mConfidenceOptFrameId, mFrameTimestamp); + sl_tools::imageToROSmsg(confMapMsg, confMapZEDMat, mConfidenceOptFrameId, stamp); mPubConfMap.publish(confMapMsg); } @@ -2813,7 +2820,7 @@ void ZEDWrapperNodelet::pubSensCallback(const ros::TimerEvent& e) { if (imu_RawSubNumber > 0) { sensor_msgs::ImuPtr imuRawMsg = boost::make_shared(); - imuRawMsg->header.stamp = ts_imu; // t; + imuRawMsg->header.stamp = ts_imu; imuRawMsg->header.frame_id = mImuFrameId; imuRawMsg->angular_velocity.x = sens_data.imu.angular_velocity[0] * DEG2RAD; imuRawMsg->angular_velocity.y = sens_data.imu.angular_velocity[1] * DEG2RAD; @@ -3038,7 +3045,11 @@ void ZEDWrapperNodelet::device_poll_thread_func() { if (mComputeDepth) { runParams.confidence_threshold = mCamDepthConfidence; +#if ZED_SDK_MAJOR_VERSION==3 && ZED_SDK_MINOR_VERSION<2 runParams.textureness_confidence_threshold = mCamDepthTextureConf; +#else + runParams.texture_confidence_threshold = mCamDepthTextureConf; +#endif runParams.enable_depth = true; // Ask to compute the depth } else { runParams.enable_depth = false; // Ask to not compute the depth @@ -3148,6 +3159,8 @@ void ZEDWrapperNodelet::device_poll_thread_func() { mFrameTimestamp = sl_tools::slTime2Ros(mZed.getTimestamp(sl::TIME_REFERENCE::IMAGE)); } + ros::Time stamp = mFrameTimestamp; // Timestamp + // ----> Camera Settings if( !mSvoMode && mFrameCount%5 == 0 ) { //NODELET_DEBUG_STREAM( "[" << mFrameCount << "] device_poll_thread_func MUTEX LOCK"); @@ -3255,7 +3268,7 @@ void ZEDWrapperNodelet::device_poll_thread_func() { mZed.retrieveMeasure(mCloud, sl::MEASURE::XYZBGRA, sl::MEM::CPU, mMatResolDepth); mPointCloudFrameId = mDepthFrameId; - mPointCloudTime = mFrameTimestamp; + mPointCloudTime = stamp; // Signal Pointcloud thread that a new pointcloud is ready mPcDataReadyCondVar.notify_one(); @@ -3270,7 +3283,7 @@ void ZEDWrapperNodelet::device_poll_thread_func() { mObjDetMutex.lock(); if (mObjDetRunning) { std::chrono::steady_clock::time_point start = std::chrono::steady_clock::now(); - detectObjects(objDetSubnumber > 0, objDetVizSubnumber > 0); + detectObjects(objDetSubnumber > 0, objDetVizSubnumber > 0, stamp); std::chrono::steady_clock::time_point now = std::chrono::steady_clock::now(); double elapsed_msec = std::chrono::duration_cast(now - start).count(); @@ -3355,7 +3368,7 @@ void ZEDWrapperNodelet::device_poll_thread_func() { // Publish odometry message if (odomSubnumber > 0) { - publishOdom(mOdom2BaseTransf, deltaOdom, mFrameTimestamp); + publishOdom(mOdom2BaseTransf, deltaOdom, stamp); } mTrackingReady = true; @@ -3469,7 +3482,7 @@ void ZEDWrapperNodelet::device_poll_thread_func() { // Publish Pose message if ((poseSubnumber + poseCovSubnumber) > 0) { - publishPose(mFrameTimestamp); + publishPose(stamp); } mTrackingReady = true; @@ -3482,12 +3495,12 @@ void ZEDWrapperNodelet::device_poll_thread_func() { if (mPublishTf) { // Note, the frame is published, but its values will only change if // someone has subscribed to odom - publishOdomFrame(mOdom2BaseTransf, mFrameTimestamp); // publish the base Frame in odometry frame + publishOdomFrame(mOdom2BaseTransf, stamp); // publish the base Frame in odometry frame if (mPublishMapTf) { // Note, the frame is published, but its values will only change if // someone has subscribed to map - publishPoseFrame(mMap2OdomTransf, mFrameTimestamp); // publish the odometry Frame in map frame + publishPoseFrame(mMap2OdomTransf, stamp); // publish the odometry Frame in map frame } } @@ -3982,7 +3995,7 @@ bool ZEDWrapperNodelet::on_stop_object_detection(zed_interfaces::stop_object_det return res.done; } -void ZEDWrapperNodelet::detectObjects(bool publishObj, bool publishViz) { +void ZEDWrapperNodelet::detectObjects(bool publishObj, bool publishViz, ros::Time t) { sl::ObjectDetectionRuntimeParameters objectTracker_parameters_rt; objectTracker_parameters_rt.detection_confidence_threshold = mObjDetConfidence; @@ -4007,8 +4020,7 @@ void ZEDWrapperNodelet::detectObjects(bool publishObj, bool publishViz) { objMsg.objects.resize(objCount); std_msgs::Header header; - header.stamp = mFrameTimestamp; - //header.frame_id = mCameraFrameId; + header.stamp = t; header.frame_id = mLeftCamFrameId; visualization_msgs::MarkerArray objMarkersMsg; From f70b24ff7dc1f01b874292bb02ba112a91b15f41 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Wed, 15 Jul 2020 10:18:40 +0200 Subject: [PATCH 004/199] Add -Wno-deprecated-declarations to remove compiling warnings (#590) --- zed_nodelets/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/zed_nodelets/CMakeLists.txt b/zed_nodelets/CMakeLists.txt index ae7fde12..08f41abd 100644 --- a/zed_nodelets/CMakeLists.txt +++ b/zed_nodelets/CMakeLists.txt @@ -107,7 +107,7 @@ link_directories(${CUDA_LIBRARY_DIRS}) ############################################################################### # ZED WRAPPER NODELET -add_definitions(-std=c++11) +add_definitions(-std=c++11 -Wno-deprecated-declarations) set(LINK_LIBRARIES ${catkin_LIBRARIES} ${ZED_LIBRARIES} From 82334a2008455a5f61e572ff8dc1b6338a946feb Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Thu, 16 Jul 2020 11:56:26 +0200 Subject: [PATCH 005/199] Fix OpenNi mode for depth (#592) --- zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp | 9 +++++---- zed_wrapper/params/common.yaml | 2 +- 2 files changed, 6 insertions(+), 5 deletions(-) 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 7d97e59e..9c48fd87 100644 --- a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp @@ -122,11 +122,9 @@ void ZEDWrapperNodelet::onInit() { string depth_topic_root = "depth"; if (mOpenniDepthMode) { - NODELET_INFO_STREAM("Openni depth mode activated"); - depth_topic_root += "/depth_raw_registered"; - } else { - depth_topic_root += "/depth_registered"; + NODELET_INFO_STREAM("Openni depth mode activated -> Units: mm, Encoding: MONO16"); } + depth_topic_root += "/depth_registered"; string pointcloud_topic = "point_cloud/cloud_registered"; @@ -1644,9 +1642,12 @@ void ZEDWrapperNodelet::publishDepth(sensor_msgs::ImagePtr imgMsgPtr, sl::Mat de mDepthCamInfoMsg->header.stamp = t; + NODELET_DEBUG_STREAM("mOpenniDepthMode: " << mOpenniDepthMode); + if (!mOpenniDepthMode) { sl_tools::imageToROSmsg(imgMsgPtr, depth, mDepthOptFrameId, t); mPubDepth.publish(imgMsgPtr, mDepthCamInfoMsg); + return; } diff --git a/zed_wrapper/params/common.yaml b/zed_wrapper/params/common.yaml index bc3c92fd..89df86f1 100644 --- a/zed_wrapper/params/common.yaml +++ b/zed_wrapper/params/common.yaml @@ -40,7 +40,7 @@ depth: quality: 1 # '0': NONE, '1': PERFORMANCE, '2': QUALITY, '3': ULTRA sensing_mode: 0 # '0': STANDARD, '1': FILL (not use FILL for robotic applications) depth_stabilization: 1 # `0`: disabled, `1`: enabled - openni_depth_mode: 0 # '0': 32bit float meters, '1': 16bit uchar millimeters + openni_depth_mode: true # 'false': 32bit float meters, 'true': 16bit uchar millimeters depth_downsample_factor: 1.0 # Resample factor for depth data matrices [0.01,1.0] The SDK works with native data sizes, but publishes rescaled matrices (depth map, point cloud, ...) pos_tracking: From 7c6ee40b030338660cddc1bd496cd5c1657845f3 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Tue, 28 Jul 2020 16:10:36 +0200 Subject: [PATCH 006/199] Disable default "OpenNI mode" Solve #602 --- zed_wrapper/params/common.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/zed_wrapper/params/common.yaml b/zed_wrapper/params/common.yaml index 89df86f1..5d5618d1 100644 --- a/zed_wrapper/params/common.yaml +++ b/zed_wrapper/params/common.yaml @@ -40,7 +40,7 @@ depth: quality: 1 # '0': NONE, '1': PERFORMANCE, '2': QUALITY, '3': ULTRA sensing_mode: 0 # '0': STANDARD, '1': FILL (not use FILL for robotic applications) depth_stabilization: 1 # `0`: disabled, `1`: enabled - openni_depth_mode: true # 'false': 32bit float meters, 'true': 16bit uchar millimeters + openni_depth_mode: false # 'false': 32bit float meters, 'true': 16bit uchar millimeters depth_downsample_factor: 1.0 # Resample factor for depth data matrices [0.01,1.0] The SDK works with native data sizes, but publishes rescaled matrices (depth map, point cloud, ...) pos_tracking: From 269dc78f88a4825bc0d134885d86c4e172287738 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Tue, 25 Aug 2020 15:54:59 +0200 Subject: [PATCH 007/199] Minor changes to CMakeLists.txt --- zed_interfaces/CMakeLists.txt | 3 +++ zed_nodelets/CMakeLists.txt | 32 +++++++++++++++++--------------- 2 files changed, 20 insertions(+), 15 deletions(-) diff --git a/zed_interfaces/CMakeLists.txt b/zed_interfaces/CMakeLists.txt index 7c76ec09..159c2e88 100644 --- a/zed_interfaces/CMakeLists.txt +++ b/zed_interfaces/CMakeLists.txt @@ -2,6 +2,9 @@ cmake_minimum_required(VERSION 2.8.7) project(zed_interfaces) +#Fix for QtCretor +list(APPEND CMAKE_PREFIX_PATH "/opt/ros/$ENV{ROS_DISTRO}/") + find_package(catkin REQUIRED COMPONENTS std_msgs geometry_msgs diff --git a/zed_nodelets/CMakeLists.txt b/zed_nodelets/CMakeLists.txt index 08f41abd..574b9077 100644 --- a/zed_nodelets/CMakeLists.txt +++ b/zed_nodelets/CMakeLists.txt @@ -2,6 +2,9 @@ cmake_minimum_required(VERSION 2.8.7) project(zed_nodelets) +#Fix for QtCretor +list(APPEND CMAKE_PREFIX_PATH "/opt/ros/$ENV{ROS_DISTRO}/") + # if CMAKE_BUILD_TYPE is not specified, take 'Release' as default IF(NOT CMAKE_BUILD_TYPE) SET(CMAKE_BUILD_TYPE Release) @@ -39,20 +42,20 @@ if (OPENMP_FOUND) endif() find_package(catkin COMPONENTS - image_transport - roscpp - rosconsole - sensor_msgs - stereo_msgs - std_msgs - message_filters - tf2_ros - nodelet - tf2_geometry_msgs - message_generation - diagnostic_updater - zed_interfaces - dynamic_reconfigure + roscpp + image_transport + rosconsole + sensor_msgs + stereo_msgs + std_msgs + message_filters + tf2_ros + nodelet + tf2_geometry_msgs + message_generation + diagnostic_updater + dynamic_reconfigure + zed_interfaces ) generate_dynamic_reconfigure_options( @@ -74,7 +77,6 @@ catkin_package( zed_interfaces ) - ############################################################################### # SOURCES From a1aaf74683520480c3f7678ddc5b167fed5ee977 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Thu, 3 Sep 2020 14:56:06 +0200 Subject: [PATCH 008/199] Remove unuseful debug output Solve issue #621 --- zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 9c48fd87..a5a753b3 100644 --- a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp @@ -1642,7 +1642,7 @@ void ZEDWrapperNodelet::publishDepth(sensor_msgs::ImagePtr imgMsgPtr, sl::Mat de mDepthCamInfoMsg->header.stamp = t; - NODELET_DEBUG_STREAM("mOpenniDepthMode: " << mOpenniDepthMode); + //NODELET_DEBUG_STREAM("mOpenniDepthMode: " << mOpenniDepthMode); if (!mOpenniDepthMode) { sl_tools::imageToROSmsg(imgMsgPtr, depth, mDepthOptFrameId, t); From 447da828b0c5ce36f871161949bd3ff74dc04b3d Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Fri, 18 Sep 2020 09:10:08 +0200 Subject: [PATCH 009/199] Fix imu duplicated ts (#627) * - Add new parameter `sensors/publish_imu_tf` to enable/disable IMU TF broadcasting - Fix duplicated IMU timestamp issue (see ticket #577) - Fix problem with IMU TF in Rviz: `odom` and `zed_camera_center` TFs are now published at the same frequency of the IMU TF, if available) - IMU TF is now published even if the IMU topic is not subscribed * Update changelog * Publish IMU frame once as static TF * Update latest_changes.md --- latest_changes.md | 7 + zed_interfaces/CMakeLists.txt | 2 - .../include/rgbd_sensor_demux.hpp | 4 +- .../include/rgbd_sensor_sync.hpp | 12 +- zed_nodelets/src/tools/include/sl_tools.h | 16 +- .../include/zed_wrapper_nodelet.hpp | 111 ++++--- .../zed_nodelet/src/zed_wrapper_nodelet.cpp | 304 +++++++++++++----- zed_wrapper/nodelet_plugins.xml | 7 - zed_wrapper/params/zed2.yaml | 1 + zed_wrapper/params/zedm.yaml | 1 + 10 files changed, 298 insertions(+), 167 deletions(-) delete mode 100644 zed_wrapper/nodelet_plugins.xml diff --git a/latest_changes.md b/latest_changes.md index 4a353a3b..afee8ba2 100644 --- a/latest_changes.md +++ b/latest_changes.md @@ -1,6 +1,13 @@ LATEST CHANGES ============== +IMU timestamp fix (2020-08-25) +------------------------------ +- Added new parameter `sensors/publish_imu_tf` to enable/disable IMU TF broadcasting +- Fixed duplicated IMU timestamp issue (see ticket #577) +- Fixed problem with IMU TF in Rviz: `odom` and `zed_camera_center` TFs are now published at the same frequency of the IMU TF, if available) +- IMU TF is now published once as static TF even if the IMU topic is not subscribed + Timestamp fix (2020-06-03) -------------------------- - Fix timestamp update coherency due to parallel threads. Thanks to @matlabbe diff --git a/zed_interfaces/CMakeLists.txt b/zed_interfaces/CMakeLists.txt index 159c2e88..7a1f361f 100644 --- a/zed_interfaces/CMakeLists.txt +++ b/zed_interfaces/CMakeLists.txt @@ -9,8 +9,6 @@ find_package(catkin REQUIRED COMPONENTS std_msgs geometry_msgs sensor_msgs - - #actionlib_msgs message_generation ) diff --git a/zed_nodelets/src/rgbd_sensors_demux_nodelet/include/rgbd_sensor_demux.hpp b/zed_nodelets/src/rgbd_sensors_demux_nodelet/include/rgbd_sensor_demux.hpp index ba5a20b4..4074103a 100644 --- a/zed_nodelets/src/rgbd_sensors_demux_nodelet/include/rgbd_sensor_demux.hpp +++ b/zed_nodelets/src/rgbd_sensors_demux_nodelet/include/rgbd_sensor_demux.hpp @@ -44,11 +44,11 @@ class RgbdSensorsDemuxNodelet : public nodelet::Nodelet { virtual ~RgbdSensorsDemuxNodelet(); protected: - /* \brief Initialization function called by the Nodelet base class + /*! \brief Initialization function called by the Nodelet base class */ virtual void onInit(); - /* \brief Callback for full topics synchronization + /*! \brief Callback for full topics synchronization */ void msgCallback( const zed_interfaces::RGBDSensorsPtr& msg ); diff --git a/zed_nodelets/src/rgbd_sensors_sync_nodelet/include/rgbd_sensor_sync.hpp b/zed_nodelets/src/rgbd_sensors_sync_nodelet/include/rgbd_sensor_sync.hpp index a073b297..81db0af5 100644 --- a/zed_nodelets/src/rgbd_sensors_sync_nodelet/include/rgbd_sensor_sync.hpp +++ b/zed_nodelets/src/rgbd_sensors_sync_nodelet/include/rgbd_sensor_sync.hpp @@ -48,15 +48,15 @@ class RgbdSensorsSyncNodelet : public nodelet::Nodelet { virtual ~RgbdSensorsSyncNodelet(); protected: - /* \brief Initialization function called by the Nodelet base class + /*! \brief Initialization function called by the Nodelet base class */ virtual void onInit(); - /* \brief Reads parameters from the param server + /*! \brief Reads parameters from the param server */ void readParameters(); - /* \brief Callback for full topics synchronization + /*! \brief Callback for full topics synchronization */ void callbackFull( const sensor_msgs::ImageConstPtr& rgb, @@ -66,7 +66,7 @@ class RgbdSensorsSyncNodelet : public nodelet::Nodelet { const sensor_msgs::ImuConstPtr& imu, const sensor_msgs::MagneticFieldConstPtr& mag ); - /* \brief Callback for RGBD topics synchronization + /*! \brief Callback for RGBD topics synchronization */ void callbackRGBD( const sensor_msgs::ImageConstPtr& rgb, @@ -74,7 +74,7 @@ class RgbdSensorsSyncNodelet : public nodelet::Nodelet { const sensor_msgs::CameraInfoConstPtr& rgbCameraInfo, const sensor_msgs::CameraInfoConstPtr& depthCameraInfo ); - /* \brief Callback for RGBD + IMU topics synchronization + /*! \brief Callback for RGBD + IMU topics synchronization */ void callbackRGBDIMU( const sensor_msgs::ImageConstPtr& rgb, @@ -83,7 +83,7 @@ class RgbdSensorsSyncNodelet : public nodelet::Nodelet { const sensor_msgs::CameraInfoConstPtr& depthCameraInfo, const sensor_msgs::ImuConstPtr& imu); - /* \brief Callback for RGBD + Mag topics synchronization + /*! \brief Callback for RGBD + Mag topics synchronization */ void callbackRGBDMag( const sensor_msgs::ImageConstPtr& rgb, diff --git a/zed_nodelets/src/tools/include/sl_tools.h b/zed_nodelets/src/tools/include/sl_tools.h index 0a93042b..2caadbda 100644 --- a/zed_nodelets/src/tools/include/sl_tools.h +++ b/zed_nodelets/src/tools/include/sl_tools.h @@ -29,36 +29,36 @@ namespace sl_tools { - /* \brief Check if a ZED camera is ready + /*! \brief Check if a ZED camera is ready * \param serial_number : the serial number of the camera to be checked */ int checkCameraReady(unsigned int serial_number); - /* \brief Get ZED camera properties + /*! \brief Get ZED camera properties * \param serial_number : the serial number of the camera */ sl::DeviceProperties getZEDFromSN(unsigned int serial_number); std::vector convertRodrigues(sl::float3 r); - /* \brief Test if a file exist + /*! \brief Test if a file exist * \param name : the path to the file */ bool file_exist(const std::string& name); - /* \brief Get Stereolabs SDK version + /*! \brief Get Stereolabs SDK version * \param major : major value for version * \param minor : minor value for version * \param sub_minor _ sub_minor value for version */ std::string getSDKVersion(int& major, int& minor, int& sub_minor); - /* \brief Convert StereoLabs timestamp to ROS timestamp + /*! \brief Convert StereoLabs timestamp to ROS timestamp * \param t : Stereolabs timestamp to be converted */ ros::Time slTime2Ros(sl::Timestamp t); - /* \brief sl::Mat to ros message conversion + /*! \brief sl::Mat to ros message conversion * \param imgMsgPtr : the image topic message to publish * \param img : the image to publish * \param frameId : the id of the reference frame of the image @@ -66,7 +66,7 @@ namespace sl_tools { */ void imageToROSmsg(sensor_msgs::ImagePtr imgMsgPtr, sl::Mat img, std::string frameId, ros::Time t); - /* \brief Two sl::Mat to ros message conversion + /*! \brief Two sl::Mat to ros message conversion * \param imgMsgPtr : the image topic message to publish * \param left : the left image to publish * \param right : the right image to publish @@ -75,7 +75,7 @@ namespace sl_tools { */ void imagesToROSmsg(sensor_msgs::ImagePtr imgMsgPtr, sl::Mat left, sl::Mat right, std::string frameId, ros::Time t); - /* \brief String tokenization + /*! \brief String tokenization */ std::vector split_string(const std::string& s, char seperator); 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 12b60523..e1553429 100644 --- a/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp +++ b/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp @@ -1,4 +1,4 @@ -#ifndef ZED_WRAPPER_NODELET_H +#ifndef ZED_WRAPPER_NODELET_H #define ZED_WRAPPER_NODELET_H /////////////////////////////////////////////////////////////////////////// @@ -30,6 +30,7 @@ #include #include #include +#include #include #include #include @@ -98,37 +99,37 @@ class ZEDWrapperNodelet : public nodelet::Nodelet { } DynParams; public: - /* \brief Default constructor + /*! \brief Default constructor */ ZEDWrapperNodelet(); - /* \brief \ref ZEDWrapperNodelet destructor + /*! \brief \ref ZEDWrapperNodelet destructor */ virtual ~ZEDWrapperNodelet(); protected: - /* \brief Initialization function called by the Nodelet base class + /*! \brief Initialization function called by the Nodelet base class */ virtual void onInit(); - /* \brief Reads parameters from the param server + /*! \brief Reads parameters from the param server */ void readParameters(); - /* \brief ZED camera polling thread function + /*! \brief ZED camera polling thread function */ void device_poll_thread_func(); - /* \brief Pointcloud publishing function + /*! \brief Pointcloud publishing function */ void pointcloud_thread_func(); - /* \brief Publish the pose of the camera in "Map" frame with a ros Publisher + /*! \brief Publish the pose of the camera in "Map" frame with a ros Publisher * \param t : the ros::Time to stamp the image */ void publishPose(ros::Time t); - /* \brief Publish the pose of the camera in "Odom" frame with a ros Publisher + /*! \brief Publish the pose of the camera in "Odom" frame with a ros Publisher * \param base2odomTransf : Transformation representing the camera pose * from base frame to odom frame * \param slPose : latest odom pose from ZED SDK @@ -136,14 +137,14 @@ class ZEDWrapperNodelet : public nodelet::Nodelet { */ void publishOdom(tf2::Transform odom2baseTransf, sl::Pose& slPose, ros::Time t); - /* \brief Publish the pose of the camera in "Map" frame as a transformation + /*! \brief Publish the pose of the camera in "Map" frame as a transformation * \param baseTransform : Transformation representing the camera pose from * odom frame to map frame * \param t : the ros::Time to stamp the image */ void publishPoseFrame(tf2::Transform baseTransform, ros::Time t); - /* \brief Publish the odometry of the camera in "Odom" frame as a + /*! \brief Publish the odometry of the camera in "Odom" frame as a * transformation * \param odomTransf : Transformation representing the camera pose from * base frame to odom frame @@ -151,14 +152,12 @@ class ZEDWrapperNodelet : public nodelet::Nodelet { */ void publishOdomFrame(tf2::Transform odomTransf, ros::Time t); - /* \brief Publish the pose of the imu in "Odom" frame as a transformation - * \param imuTransform : Transformation representing the imu pose from base - * frame to odom framevoid - * \param t : the ros::Time to stamp the image - */ - void publishImuFrame(tf2::Transform imuTransform, ros::Time t); + /*! + * \brief Publish IMU frame once as static TF + */ + void publishStaticImuFrame(); - /* \brief Publish a sl::Mat image with a ros Publisher + /*! \brief Publish a sl::Mat image with a ros Publisher * \param imgMsgPtr : the image message to publish * \param img : the image to publish * \param pubImg : the publisher object to use (different image publishers @@ -171,22 +170,22 @@ class ZEDWrapperNodelet : public nodelet::Nodelet { void publishImage(sensor_msgs::ImagePtr imgMsgPtr, sl::Mat img, image_transport::CameraPublisher& pubImg, sensor_msgs::CameraInfoPtr camInfoMsg, string imgFrameId, ros::Time t); - /* \brief Publish a sl::Mat depth image with a ros Publisher + /*! \brief Publish a sl::Mat depth image with a ros Publisher * \param imgMsgPtr : the depth image topic message to publish * \param depth : the depth image to publish * \param t : the ros::Time to stamp the depth image */ void publishDepth(sensor_msgs::ImagePtr imgMsgPtr, sl::Mat depth, ros::Time t); - /* \brief Publish a single pointCloud with a ros Publisher + /*! \brief Publish a single pointCloud with a ros Publisher */ void publishPointCloud(); - /* \brief Publish a fused pointCloud with a ros Publisher + /*! \brief Publish a fused pointCloud with a ros Publisher */ void pubFusedPointCloudCallback(const ros::TimerEvent& e); - /* \brief Publish the informations of a camera with a ros Publisher + /*! \brief Publish the informations of a camera with a ros Publisher * \param cam_info_msg : the information message to publish * \param pub_cam_info : the publisher object to use * \param t : the ros::Time to stamp the message @@ -194,13 +193,13 @@ class ZEDWrapperNodelet : public nodelet::Nodelet { void publishCamInfo(sensor_msgs::CameraInfoPtr camInfoMsg, ros::Publisher pubCamInfo, ros::Time t); - /* \brief Publish a sl::Mat disparity image with a ros Publisher + /*! \brief Publish a sl::Mat disparity image with a ros Publisher * \param disparity : the disparity image to publish * \param t : the ros::Time to stamp the depth image */ void publishDisparity(sl::Mat disparity, ros::Time t); - /* \brief Get the information of the ZED cameras and store them in an + /*! \brief Get the information of the ZED cameras and store them in an * information message * \param zed : the sl::zed::Camera* pointer to an instance * \param left_cam_info_msg : the information message to fill with the left @@ -215,7 +214,7 @@ class ZEDWrapperNodelet : public nodelet::Nodelet { string leftFrameId, string rightFrameId, bool rawParam = false); - /* \brief Get the information of the ZED cameras and store them in an + /*! \brief Get the information of the ZED cameras and store them in an * information message for depth topics * \param zed : the sl::zed::Camera* pointer to an instance * \param depth_info_msg : the information message to fill with the left @@ -230,38 +229,38 @@ class ZEDWrapperNodelet : public nodelet::Nodelet { */ void checkResolFps(); - /* \brief Callback to handle dynamic reconfigure events in ROS + /*! \brief Callback to handle dynamic reconfigure events in ROS */ void dynamicReconfCallback(zed_nodelets::ZedConfig& config, uint32_t level); - /* \brief Callback to publish Video and Depth data + /*! \brief Callback to publish Video and Depth data * \param e : the ros::TimerEvent binded to the callback */ void pubVideoDepthCallback(const ros::TimerEvent& e); - /* \brief Callback to publish Path data with a ROS publisher. + /*! \brief Callback to publish Path data with a ROS publisher. * \param e : the ros::TimerEvent binded to the callback */ void pubPathCallback(const ros::TimerEvent& e); - /* \brief Callback to publish Sensor Data with a ROS publisher. + /*! \brief Callback to publish Sensor Data with a ROS publisher. * \param e : the ros::TimerEvent binded to the callback */ void pubSensCallback(const ros::TimerEvent& e); - /* \brief Callback to update node diagnostic status + /*! \brief Callback to update node diagnostic status * \param stat : node status */ void updateDiagnostic(diagnostic_updater::DiagnosticStatusWrapper& stat); - /* \brief Service callback to reset_tracking service + /*! \brief Service callback to reset_tracking service * Tracking pose is reinitialized to the value available in the ROS Param * server */ bool on_reset_tracking(zed_interfaces::reset_tracking::Request& req, zed_interfaces::reset_tracking::Response& res); - /* \brief Service callback to reset_odometry service + /*! \brief Service callback to reset_odometry service * Odometry is reset to clear drift and odometry frame gets the latest * pose * from ZED tracking. @@ -269,79 +268,79 @@ class ZEDWrapperNodelet : public nodelet::Nodelet { bool on_reset_odometry(zed_interfaces::reset_odometry::Request& req, zed_interfaces::reset_odometry::Response& res); - /* \brief Service callback to set_pose service + /*! \brief Service callback to set_pose service * Tracking pose is set to the new values */ bool on_set_pose(zed_interfaces::set_pose::Request& req, zed_interfaces::set_pose::Response& res); - /* \brief Service callback to start_svo_recording service + /*! \brief Service callback to start_svo_recording service */ bool on_start_svo_recording(zed_interfaces::start_svo_recording::Request& req, zed_interfaces::start_svo_recording::Response& res); - /* \brief Service callback to stop_svo_recording service + /*! \brief Service callback to stop_svo_recording service */ bool on_stop_svo_recording(zed_interfaces::stop_svo_recording::Request& req, zed_interfaces::stop_svo_recording::Response& res); - /* \brief Service callback to start_remote_stream service + /*! \brief Service callback to start_remote_stream service */ bool on_start_remote_stream(zed_interfaces::start_remote_stream::Request& req, zed_interfaces::start_remote_stream::Response& res); - /* \brief Service callback to stop_remote_stream service + /*! \brief Service callback to stop_remote_stream service */ bool on_stop_remote_stream(zed_interfaces::stop_remote_stream::Request& req, zed_interfaces::stop_remote_stream::Response& res); - /* \brief Service callback to set_led_status service + /*! \brief Service callback to set_led_status service */ bool on_set_led_status(zed_interfaces::set_led_status::Request& req, zed_interfaces::set_led_status::Response& res); - /* \brief Service callback to toggle_led service + /*! \brief Service callback to toggle_led service */ bool on_toggle_led(zed_interfaces::toggle_led::Request& req, zed_interfaces::toggle_led::Response& res); - /* \brief Service callback to start_3d_mapping service + /*! \brief Service callback to start_3d_mapping service */ bool on_start_3d_mapping(zed_interfaces::start_3d_mapping::Request& req, zed_interfaces::start_3d_mapping::Response& res); - /* \brief Service callback to stop_3d_mapping service + /*! \brief Service callback to stop_3d_mapping service */ bool on_stop_3d_mapping(zed_interfaces::stop_3d_mapping::Request& req, zed_interfaces::stop_3d_mapping::Response& res); - /* \brief Service callback to start_object_detection service + /*! \brief Service callback to start_object_detection service */ bool on_start_object_detection(zed_interfaces::start_object_detection::Request& req, zed_interfaces::start_object_detection::Response& res); - /* \brief Service callback to stop_object_detection service + /*! \brief Service callback to stop_object_detection service */ bool on_stop_object_detection(zed_interfaces::stop_object_detection::Request& req, zed_interfaces::stop_object_detection::Response& res); - /* \brief Utility to initialize the pose variables + /*! \brief Utility to initialize the pose variables */ bool set_pose(float xt, float yt, float zt, float rr, float pr, float yr); - /* \brief Utility to initialize the most used transforms + /*! \brief Utility to initialize the most used transforms */ void initTransforms(); - /* \brief Utility to initialize the static transform from Sensor to Base + /*! \brief Utility to initialize the static transform from Sensor to Base */ bool getSens2BaseTransform(); - /* \brief Utility to initialize the static transform from Sensor to Camera + /*! \brief Utility to initialize the static transform from Sensor to Camera */ bool getSens2CameraTransform(); - /* \brief Utility to initialize the static transform from Camera to Base + /*! \brief Utility to initialize the static transform from Camera to Base */ bool getCamera2BaseTransform(); @@ -365,11 +364,11 @@ class ZEDWrapperNodelet : public nodelet::Nodelet { */ void stop_obj_detect(); - /* \brief Perform object detection and publish result + /*! \brief Perform object detection and publish result */ void detectObjects(bool publishObj, bool publishViz, ros::Time t); - /* \brief Generates an univoque color for each object class ID + /*! \brief Generates an univoque color for each object class ID */ inline sl::float3 generateColorClass(int idx) { sl::float3 clr; @@ -379,7 +378,7 @@ class ZEDWrapperNodelet : public nodelet::Nodelet { return clr / 255.f; } - /* \brief Update Dynamic reconfigure parameters + /*! \brief Update Dynamic reconfigure parameters */ void updateDynamicReconfigure(); @@ -473,7 +472,7 @@ class ZEDWrapperNodelet : public nodelet::Nodelet { // ROS TF tf2_ros::TransformBroadcaster mTransformPoseBroadcaster; tf2_ros::TransformBroadcaster mTransformOdomBroadcaster; - tf2_ros::TransformBroadcaster mTransformImuBroadcaster; + tf2_ros::StaticTransformBroadcaster mStaticTransformImuBroadcaster; std::string mRgbFrameId; std::string mRgbOptFrameId; @@ -508,6 +507,7 @@ class ZEDWrapperNodelet : public nodelet::Nodelet { bool mPublishTf; bool mPublishMapTf; + bool mPublishImuTf; bool mCameraFlip; bool mCameraSelfCalib; @@ -551,12 +551,15 @@ class ZEDWrapperNodelet : public nodelet::Nodelet { ros::Time mFrameTimestamp; // Positional Tracking variables - sl::Pose mLastZedPose; // Sensor to Map transform + sl::Pose mLastZedPose; // Sensor to Map transform sl::Transform mInitialPoseSl; std::vector mInitialBasePose; std::vector mOdomPath; std::vector mMapPath; + // IMU TF + tf2::Transform mLastImuPose; + // TF Transforms tf2::Transform mMap2OdomTransf; // Coordinates of the odometry frame in map frame tf2::Transform mOdom2BaseTransf; // Coordinates of the base in odometry frame @@ -569,6 +572,7 @@ class ZEDWrapperNodelet : public nodelet::Nodelet { bool mSensor2BaseTransfValid = false; bool mSensor2CameraTransfValid = false; bool mCamera2BaseTransfValid = false; + bool mStaticImuFramePublished = false; // initialization Transform listener boost::shared_ptr mTfBuffer; @@ -666,6 +670,7 @@ class ZEDWrapperNodelet : public nodelet::Nodelet { // Camera IMU transform sl::Transform mSlCamImuTransf; + geometry_msgs::TransformStamped mStaticImuTransformStamped; // Spatial mapping bool mMappingEnabled; 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 a5a753b3..70e0d5c7 100644 --- a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp @@ -33,6 +33,10 @@ #include "visualization_msgs/Marker.h" #include "visualization_msgs/MarkerArray.h" + + +//#define DEBUG_SENS_TS 1 + namespace zed_nodelets { #ifndef DEG2RAD @@ -513,7 +517,7 @@ void ZEDWrapperNodelet::onInit() { } mFrameTimestamp = ros::Time::now(); - mImuTimer = mNhNs.createTimer(ros::Duration(1.0 / mSensPubRate), + mImuTimer = mNhNs.createTimer(ros::Duration(1.0 / (mSensPubRate*1.5) ), &ZEDWrapperNodelet::pubSensCallback, this); mSensPeriodMean_usec.reset(new sl_tools::CSmartMean(mSensPubRate / 2)); @@ -807,6 +811,8 @@ void ZEDWrapperNodelet::readParameters() { mNhNs.param("pos_tracking/publish_map_tf", mPublishMapTf, true); NODELET_INFO_STREAM(" * Broadcast map pose TF\t-> " << (mPublishTf ? (mPublishMapTf ? "ENABLED" : "DISABLED") : "DISABLED")); + mNhNs.param("sensors/publish_imu_tf", mPublishImuTf, true); + NODELET_INFO_STREAM(" * Broadcast IMU pose TF\t-> " << ( mPublishImuTf ? "ENABLED" : "DISABLED" ) ); // <---- TF broadcasting // ----> Dynamic @@ -1561,6 +1567,37 @@ void ZEDWrapperNodelet::publishPose(ros::Time t) { } +void ZEDWrapperNodelet::publishStaticImuFrame() { + // Publish IMU TF as static TF + if( !mPublishImuTf ) { + return; + } + + if(mStaticImuFramePublished) { + return; + } + + mStaticImuTransformStamped.header.stamp = ros::Time::now(); + mStaticImuTransformStamped.header.frame_id = mLeftCamFrameId; + mStaticImuTransformStamped.child_frame_id = mImuFrameId; + sl::Translation sl_tr = mSlCamImuTransf.getTranslation(); + mStaticImuTransformStamped.transform.translation.x = sl_tr.x; + mStaticImuTransformStamped.transform.translation.y = sl_tr.y; + mStaticImuTransformStamped.transform.translation.z = sl_tr.z; + sl::Orientation sl_or = mSlCamImuTransf.getOrientation(); + mStaticImuTransformStamped.transform.rotation.x = sl_or.ox; + mStaticImuTransformStamped.transform.rotation.y = sl_or.oy; + mStaticImuTransformStamped.transform.rotation.z = sl_or.oz; + mStaticImuTransformStamped.transform.rotation.w = sl_or.ow; + + // Publish transformation + mStaticTransformImuBroadcaster.sendTransform(mStaticImuTransformStamped); + + NODELET_INFO_STREAM("Published static transform '" << mImuFrameId << "' -> '" << mLeftCamFrameId << "'" ); + + mStaticImuFramePublished = true; +} + void ZEDWrapperNodelet::publishOdomFrame(tf2::Transform odomTransf, ros::Time t) { if (!mSensor2BaseTransfValid) { getSens2BaseTransform(); @@ -1582,6 +1619,8 @@ void ZEDWrapperNodelet::publishOdomFrame(tf2::Transform odomTransf, ros::Time t) transformStamped.transform = tf2::toMsg(odomTransf); // Publish transformation mTransformOdomBroadcaster.sendTransform(transformStamped); + + //NODELET_INFO_STREAM( "Published ODOM TF with TS: " << t ); } void ZEDWrapperNodelet::publishPoseFrame(tf2::Transform baseTransform, ros::Time t) { @@ -1605,29 +1644,8 @@ void ZEDWrapperNodelet::publishPoseFrame(tf2::Transform baseTransform, ros::Time transformStamped.transform = tf2::toMsg(baseTransform); // Publish transformation mTransformPoseBroadcaster.sendTransform(transformStamped); -} - -void ZEDWrapperNodelet::publishImuFrame(tf2::Transform imuTransform, ros::Time t) { - if (!mSensor2BaseTransfValid) { - getSens2BaseTransform(); - } - - if (!mSensor2CameraTransfValid) { - getSens2CameraTransform(); - } - if (!mCamera2BaseTransfValid) { - getCamera2BaseTransform(); - } - - geometry_msgs::TransformStamped transformStamped; - transformStamped.header.stamp = t; - transformStamped.header.frame_id = mCameraFrameId; - transformStamped.child_frame_id = mImuFrameId; - // conversion from Tranform to message - transformStamped.transform = tf2::toMsg(imuTransform); - // Publish transformation - mTransformImuBroadcaster.sendTransform(transformStamped); + //NODELET_INFO_STREAM( "Published POSE TF with TS: " << t ); } void ZEDWrapperNodelet::publishImage(sensor_msgs::ImagePtr imgMsgPtr, sl::Mat img, @@ -1834,7 +1852,7 @@ void ZEDWrapperNodelet::pubFusedPointCloudCallback(const ros::TimerEvent& e) { if (pointcloudFusedMsg->width != ptsCount || pointcloudFusedMsg->height != 1) { // Initialize Point Cloud message - // https://github.com/ros/common_msgs/blob/jade-devel/sensor_msgs/include/sensor_msgs/point_cloud2_iterator.h + // https://github.com/ros/common_msgs/blob/jade-devel/sensor_msgs/include/sensor_msgs/point_cloud2_iterator.h pointcloudFusedMsg->header.frame_id = mMapFrameId; // Set the header values of the ROS message pointcloudFusedMsg->is_bigendian = false; pointcloudFusedMsg->is_dense = false; @@ -2591,8 +2609,9 @@ void ZEDWrapperNodelet::pubSensCallback(const ros::TimerEvent& e) { ros::Time ts_imu; ros::Time ts_baro; ros::Time ts_mag; - ros::Time ts_mag_raw; + //ros::Time ts_mag_raw; + static ros::Time lastTs_imu = ros::Time(); static ros::Time lastTs_baro = ros::Time(); static ros::Time lastT_mag = ros::Time(); //static ros::Time lastT_mag_raw = ros::Time(); @@ -2624,7 +2643,7 @@ void ZEDWrapperNodelet::pubSensCallback(const ros::TimerEvent& e) { ts_imu = mFrameTimestamp; ts_baro = mFrameTimestamp; ts_mag = mFrameTimestamp; - ts_mag_raw = mFrameTimestamp; + //ts_mag_raw = mFrameTimestamp; } else { ts_imu = sl_tools::slTime2Ros(sens_data.imu.timestamp); ts_baro = sl_tools::slTime2Ros(sens_data.barometer.timestamp); @@ -2633,14 +2652,22 @@ void ZEDWrapperNodelet::pubSensCallback(const ros::TimerEvent& e) { } } + bool new_imu_data = ts_imu!=lastTs_imu; + bool new_baro_data = ts_baro!=lastTs_baro; + bool new_mag_data = ts_mag!=lastT_mag; + + if( !new_imu_data && !new_baro_data && !new_mag_data) { + return; + } + if( mZedRealCamModel == sl::MODEL::ZED2 ) { // Update temperatures for Diagnostic sens_data.temperature.get( sl::SensorsData::TemperatureData::SENSOR_LOCATION::ONBOARD_LEFT, mTempLeft); sens_data.temperature.get( sl::SensorsData::TemperatureData::SENSOR_LOCATION::ONBOARD_RIGHT, mTempRight); } - if (totSub<1) { // Nothing to publish - return; + if(totSub<1 && !mPublishImuTf) { + return; // Nothing to publish } if( imu_SubNumber > 0 || imu_RawSubNumber > 0 || @@ -2664,6 +2691,15 @@ void ZEDWrapperNodelet::pubSensCallback(const ros::TimerEvent& e) { sensor_msgs::TemperaturePtr imuTempMsg = boost::make_shared(); imuTempMsg->header.stamp = ts_imu; + +#ifdef DEBUG_SENS_TS + static ros::Time old_ts; + if(old_ts==imuTempMsg->header.stamp) { + NODELET_WARN_STREAM("Publishing IMU data with old timestamp " << old_ts ); + } + old_ts = imuTempMsg->header.stamp; +#endif + imuTempMsg->header.frame_id = mImuFrameId; float imu_temp; sens_data.temperature.get( sl::SensorsData::TemperatureData::SENSOR_LOCATION::IMU, imu_temp); @@ -2673,14 +2709,21 @@ void ZEDWrapperNodelet::pubSensCallback(const ros::TimerEvent& e) { mPubImuTemp.publish(imuTempMsg); } - - if( sens_data.barometer.is_available && lastTs_baro != ts_baro ) { + if( sens_data.barometer.is_available && new_baro_data ) { lastTs_baro = ts_baro; if( pressSubNumber>0 ) { sensor_msgs::FluidPressurePtr pressMsg = boost::make_shared(); pressMsg->header.stamp = ts_baro; + +#ifdef DEBUG_SENS_TS + static ros::Time old_ts; + if(old_ts==pressMsg->header.stamp) { + NODELET_WARN_STREAM("Publishing BARO data with old timestamp " << old_ts ); + } + old_ts = pressMsg->header.stamp; +#endif pressMsg->header.frame_id = mBaroFrameId; pressMsg->fluid_pressure = sens_data.barometer.pressure * 1e-2; // Pascal pressMsg->variance = 1.0585e-2; @@ -2692,6 +2735,15 @@ void ZEDWrapperNodelet::pubSensCallback(const ros::TimerEvent& e) { sensor_msgs::TemperaturePtr tempLeftMsg = boost::make_shared(); tempLeftMsg->header.stamp = ts_baro; + +#ifdef DEBUG_SENS_TS + static ros::Time old_ts; + if(old_ts==tempLeftMsg->header.stamp) { + NODELET_WARN_STREAM("Publishing BARO data with old timestamp " << old_ts ); + } + old_ts = tempLeftMsg->header.stamp; +#endif + tempLeftMsg->header.frame_id = mTempLeftFrameId; tempLeftMsg->temperature = static_cast(mTempLeft); tempLeftMsg->variance = 0.0; @@ -2703,6 +2755,15 @@ void ZEDWrapperNodelet::pubSensCallback(const ros::TimerEvent& e) { sensor_msgs::TemperaturePtr tempRightMsg = boost::make_shared(); tempRightMsg->header.stamp = ts_baro; + +#ifdef DEBUG_SENS_TS + static ros::Time old_ts; + if(old_ts==tempRightMsg->header.stamp) { + NODELET_WARN_STREAM("Publishing BARO data with old timestamp " << old_ts ); + } + old_ts = tempRightMsg->header.stamp; +#endif + tempRightMsg->header.frame_id = mTempRightFrameId; tempRightMsg->temperature = static_cast(mTempRight); tempRightMsg->variance = 0.0; @@ -2711,13 +2772,22 @@ void ZEDWrapperNodelet::pubSensCallback(const ros::TimerEvent& e) { } } - if( imu_MagSubNumber>0 ) { - if( sens_data.magnetometer.is_available && lastT_mag != ts_mag ) { + if( imu_MagSubNumber>0) { + if( sens_data.magnetometer.is_available && new_mag_data ) { lastT_mag = ts_mag; sensor_msgs::MagneticFieldPtr magMsg = boost::make_shared(); magMsg->header.stamp = ts_mag; + +#ifdef DEBUG_SENS_TS + static ros::Time old_ts; + if(old_ts==magMsg->header.stamp) { + NODELET_WARN_STREAM("Publishing MAG data with old timestamp " << old_ts ); + } + old_ts = magMsg->header.stamp; +#endif + magMsg->header.frame_id = mMagFrameId; magMsg->magnetic_field.x = sens_data.magnetometer.magnetic_field_calibrated.x*1e-6; // Tesla magMsg->magnetic_field.y = sens_data.magnetometer.magnetic_field_calibrated.y*1e-6; // Tesla @@ -2762,10 +2832,24 @@ void ZEDWrapperNodelet::pubSensCallback(const ros::TimerEvent& e) { // } // } - if (imu_SubNumber > 0) { + if( (imu_SubNumber > 0 || mPublishImuTf) && new_imu_data) { + lastTs_imu = ts_imu; + sensor_msgs::ImuPtr imuMsg = boost::make_shared(); imuMsg->header.stamp = ts_imu; + +#ifdef DEBUG_SENS_TS + static ros::Time old_ts; + if(old_ts==imuMsg->header.stamp) { + NODELET_WARN_STREAM("Publishing IMU data with old timestamp " << old_ts ); + } else { + NODELET_INFO_STREAM("Publishing IMU data with new timestamp. Freq: " << 1./(ts_imu.toSec()-old_ts.toSec()) ); + old_ts = imuMsg->header.stamp; + + } +#endif + imuMsg->header.frame_id = mImuFrameId; imuMsg->orientation.x = sens_data.imu.pose.getOrientation()[0]; @@ -2815,10 +2899,12 @@ void ZEDWrapperNodelet::pubSensCallback(const ros::TimerEvent& e) { sens_data.imu.angular_velocity_covariance.r[r * 3 + 2] * DEG2RAD * DEG2RAD; } - mPubImu.publish(imuMsg); + if(imu_SubNumber > 0) { + mPubImu.publish(imuMsg); + } } - if (imu_RawSubNumber > 0) { + if (imu_RawSubNumber > 0 && new_imu_data) { sensor_msgs::ImuPtr imuRawMsg = boost::make_shared(); imuRawMsg->header.stamp = ts_imu; @@ -2862,53 +2948,79 @@ void ZEDWrapperNodelet::pubSensCallback(const ros::TimerEvent& e) { mPubImuRaw.publish(imuRawMsg); } - // Publish IMU tf - // Camera to pose transform from TF buffer - tf2::Transform cam_to_odom; + if(new_imu_data && mPublishImuTf) { + // Publish odometry tf only if enabled + if (mPublishTf) { + if(!mTrackingReady) { + return; + } - //std::string poseFrame; - static bool first_error = true; + publishOdomFrame(mOdom2BaseTransf, ts_imu); // publish the base Frame in odometry frame - // Look up the transformation from imu frame to odom link - try { - // Save the transformation from base to frame - geometry_msgs::TransformStamped c20 = - mTfBuffer->lookupTransform(mOdometryFrameId, mCameraFrameId, ros::Time(0)); - // Get the TF2 transformation - tf2::fromMsg(c20.transform, cam_to_odom); - } catch (tf2::TransformException& ex) { - if(!first_error) { - NODELET_DEBUG_THROTTLE(1.0, "Transform error: %s", ex.what()); - NODELET_WARN_THROTTLE(1.0, "The tf from '%s' to '%s' is not available.", - mCameraFrameId.c_str(), mMapFrameId.c_str()); - NODELET_WARN_THROTTLE(1.0, "Note: one of the possible cause of the problem is the absense of a publisher " - "of the base_link -> odom transform. " - "This happens if `publish_tf` is `false` and no other nodes publish the " - "TF chain '%s' -> '%s' -> '%s'", - mOdometryFrameId.c_str(), mBaseFrameId.c_str(), mCameraFrameId.c_str()); - first_error=false; - } + if (mPublishMapTf) { + publishPoseFrame(mMap2OdomTransf, ts_imu); // publish the odometry Frame in map frame + } - return; + if(mPublishImuTf && !mStaticImuFramePublished ) + { + publishStaticImuFrame(); + } + } } - // IMU Quaternion in Map frame - tf2::Quaternion imu_q; - imu_q.setX(sens_data.imu.pose.getOrientation()[0]); - imu_q.setY(sens_data.imu.pose.getOrientation()[1]); - imu_q.setZ(sens_data.imu.pose.getOrientation()[2]); - imu_q.setW(sens_data.imu.pose.getOrientation()[3]); - // Pose Quaternion from ZED Camera - tf2::Quaternion map_q = cam_to_odom.getRotation(); - // Difference between IMU and ZED Quaternion - tf2::Quaternion delta_q = imu_q * map_q.inverse(); - tf2::Transform imu_pose; - imu_pose.setIdentity(); - imu_pose.setRotation(delta_q); - - publishImuFrame(imu_pose, mFrameTimestamp); // publish the imu Frame + // // Publish IMU tf + // // Left camera to odom transform from TF buffer + // tf2::Transform cam_to_odom; + + // //std::string poseFrame; + // static bool first_error = false; + + // // Look up the transformation from imu frame to odom link + // try { + // // Save the transformation from base to frame + // geometry_msgs::TransformStamped c2o = + // mTfBuffer->lookupTransform(mOdometryFrameId, mCameraFrameId, ros::Time(0), ros::Duration(0.1)); + // // Get the TF2 transformation + // tf2::fromMsg(c2o.transform, cam_to_odom); + // } catch (tf2::TransformException& ex) { + // if(!first_error) { + // NODELET_DEBUG_THROTTLE(1.0, "Transform error: %s", ex.what()); + // NODELET_WARN_THROTTLE(1.0, "The tf from '%s' to '%s' is not available.", + // mCameraFrameId.c_str(), mMapFrameId.c_str()); + // NODELET_WARN_THROTTLE(1.0, "Note: one of the possible cause of the problem is the absense of a publisher " + // "of the base_link -> odom transform. " + // "This happens if `publish_tf` is `false` and no other nodes publish the " + // "TF chain '%s' -> '%s' -> '%s'", + // mOdometryFrameId.c_str(), mBaseFrameId.c_str(), mCameraFrameId.c_str()); + // first_error=false; + // } + + // return; + // } + + // // ----> Update IMU pose for TF + + // // IMU Quaternion in Map frame + // tf2::Quaternion imu_q; + // imu_q.setX(sens_data.imu.pose.getOrientation()[0]); + // imu_q.setY(sens_data.imu.pose.getOrientation()[1]); + // imu_q.setZ(sens_data.imu.pose.getOrientation()[2]); + // imu_q.setW(sens_data.imu.pose.getOrientation()[3]); + + // // Pose Quaternion from ZED Camera + // tf2::Quaternion odom_q = cam_to_odom.getRotation(); + // // Difference between IMU and ZED Quaternion + // tf2::Quaternion delta_q = imu_q * odom_q.inverse(); + + // mLastImuPose.setIdentity(); + // mLastImuPose.setRotation(delta_q); + + // publishImuFrame(mLastImuPose, ts_imu); + // // <---- Update IMU pose for TF + // } } + void ZEDWrapperNodelet::device_poll_thread_func() { ros::Rate loop_rate(mCamFrameRate); @@ -3492,17 +3604,24 @@ void ZEDWrapperNodelet::device_poll_thread_func() { oldStatus = mTrackingStatus; } + if(mZedRealCamModel == sl::MODEL::ZED || !mPublishImuTf) { // Publish pose tf only if enabled - if (mPublishTf) { + if(mPublishTf) { // Note, the frame is published, but its values will only change if // someone has subscribed to odom publishOdomFrame(mOdom2BaseTransf, stamp); // publish the base Frame in odometry frame - if (mPublishMapTf) { + if(mPublishMapTf) { // Note, the frame is published, but its values will only change if // someone has subscribed to map publishPoseFrame(mMap2OdomTransf, stamp); // publish the odometry Frame in map frame } + + if(mPublishImuTf && !mStaticImuFramePublished ) + { + publishStaticImuFrame(); + } + } } #if 0 //#ifndef NDEBUG // Enable for TF checking @@ -3563,20 +3682,27 @@ void ZEDWrapperNodelet::device_poll_thread_func() { } else { NODELET_DEBUG_THROTTLE(5.0, "No topics subscribed by users"); - // Publish odometry tf only if enabled - if (mPublishTf) { - ros::Time t; + if(mZedRealCamModel == sl::MODEL::ZED || !mPublishImuTf) { + // Publish odometry tf only if enabled + if (mPublishTf) { + ros::Time t; - if (mSvoMode) { - t = ros::Time::now(); - } else { - t = sl_tools::slTime2Ros(mZed.getTimestamp(sl::TIME_REFERENCE::CURRENT)); - } + if (mSvoMode) { + t = ros::Time::now(); + } else { + t = sl_tools::slTime2Ros(mZed.getTimestamp(sl::TIME_REFERENCE::CURRENT)); + } - publishOdomFrame(mOdom2BaseTransf, mFrameTimestamp); // publish the base Frame in odometry frame + publishOdomFrame(mOdom2BaseTransf, mFrameTimestamp); // publish the base Frame in odometry frame - if (mPublishMapTf) { - publishPoseFrame(mMap2OdomTransf, mFrameTimestamp); // publish the odometry Frame in map frame + if (mPublishMapTf) { + publishPoseFrame(mMap2OdomTransf, mFrameTimestamp); // publish the odometry Frame in map frame + } + + if(mPublishImuTf && !mStaticImuFramePublished ) + { + publishStaticImuFrame(); + } } } @@ -3979,7 +4105,7 @@ bool ZEDWrapperNodelet::on_start_object_detection(zed_interfaces::start_object_d return res.done; } -/* \brief Service callback to stop_object_detection service +/*! \brief Service callback to stop_object_detection service */ bool ZEDWrapperNodelet::on_stop_object_detection(zed_interfaces::stop_object_detection::Request& req, zed_interfaces::stop_object_detection::Response& res) { diff --git a/zed_wrapper/nodelet_plugins.xml b/zed_wrapper/nodelet_plugins.xml deleted file mode 100644 index b956da92..00000000 --- a/zed_wrapper/nodelet_plugins.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - This is the nodelet of ROS interface for Stereolabs ZED Camera. - - diff --git a/zed_wrapper/params/zed2.yaml b/zed_wrapper/params/zed2.yaml index b3026fbf..2122cde3 100644 --- a/zed_wrapper/params/zed2.yaml +++ b/zed_wrapper/params/zed2.yaml @@ -14,6 +14,7 @@ pos_tracking: sensors: sensors_timestamp_sync: false # Synchronize Sensors messages timestamp with latest received frame + publish_imu_tf: true # publish `IMU -> _left_camera_frame` TF object_detection: od_enabled: false # True to enable Object Detection [only ZED 2] diff --git a/zed_wrapper/params/zedm.yaml b/zed_wrapper/params/zedm.yaml index 03d9e7d0..e8e9ab3d 100644 --- a/zed_wrapper/params/zedm.yaml +++ b/zed_wrapper/params/zedm.yaml @@ -14,3 +14,4 @@ pos_tracking: sensors: sensors_timestamp_sync: false # Synchronize Sensors messages timestamp with latest received frame + publish_imu_tf: true # publish `IMU -> _left_camera_frame` TF From 4aca554dcf60314c0ee0fb027b99c01db6e7a13b Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Fri, 18 Sep 2020 09:11:45 +0200 Subject: [PATCH 010/199] Object detection is now asynchronous (#628) Add OD frequency to diagnostic info Improve diagnostic info --- .../include/zed_wrapper_nodelet.hpp | 2 +- .../zed_nodelet/src/zed_wrapper_nodelet.cpp | 202 +++--------------- zed_wrapper/params/common.yaml | 2 +- 3 files changed, 36 insertions(+), 170 deletions(-) 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 e1553429..5adba6c3 100644 --- a/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp +++ b/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp @@ -542,7 +542,7 @@ class ZEDWrapperNodelet : public nodelet::Nodelet { bool mGrabActive = false; // Indicate if camera grabbing is active (at least one topic subscribed) sl::ERROR_CODE mConnStatus; sl::ERROR_CODE mGrabStatus; - sl::POSITIONAL_TRACKING_STATE mTrackingStatus; + sl::POSITIONAL_TRACKING_STATE mPosTrackingStatus; bool mSensPublishing = false; bool mPcPublishing = 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 70e0d5c7..2558284a 100644 --- a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp @@ -169,7 +169,7 @@ void ZEDWrapperNodelet::onInit() { if (!mSvoFilepath.empty() || !mRemoteStreamAddr.empty()) { if (!mSvoFilepath.empty()) { mZedParams.input.setFromSVOFile(mSvoFilepath.c_str()); - mZedParams.svo_real_time_mode = false; + mZedParams.svo_real_time_mode = true; } else if (!mRemoteStreamAddr.empty()) { std::vector configStream = sl_tools::split_string(mRemoteStreamAddr, ':'); sl::String ip = sl::String(configStream.at(0).c_str()); @@ -1345,7 +1345,8 @@ bool ZEDWrapperNodelet::start_obj_detect() { sl::ObjectDetectionParameters od_p; od_p.enable_mask_output = false; od_p.enable_tracking = mObjDetTracking; - od_p.image_sync = true; + //od_p.image_sync = true; + od_p.image_sync = false; // Asynchronous object detection sl::ERROR_CODE objDetError = mZed.enableObjectDetection(od_p); @@ -3395,12 +3396,12 @@ void ZEDWrapperNodelet::device_poll_thread_func() { mObjDetMutex.lock(); if (mObjDetRunning) { - std::chrono::steady_clock::time_point start = std::chrono::steady_clock::now(); + //std::chrono::steady_clock::time_point start = std::chrono::steady_clock::now(); detectObjects(objDetSubnumber > 0, objDetVizSubnumber > 0, stamp); - std::chrono::steady_clock::time_point now = std::chrono::steady_clock::now(); + //std::chrono::steady_clock::time_point now = std::chrono::steady_clock::now(); - double elapsed_msec = std::chrono::duration_cast(now - start).count(); - mObjDetPeriodMean_msec->addValue(elapsed_msec); + //double elapsed_msec = std::chrono::duration_cast(now - start).count(); + //mObjDetPeriodMean_msec->addValue(elapsed_msec); } mObjDetMutex.unlock(); @@ -3421,7 +3422,7 @@ void ZEDWrapperNodelet::device_poll_thread_func() { if (!mInitOdomWithPose) { sl::Pose deltaOdom; - mTrackingStatus = mZed.getPosition(deltaOdom, sl::REFERENCE_FRAME::CAMERA); + mPosTrackingStatus = mZed.getPosition(deltaOdom, sl::REFERENCE_FRAME::CAMERA); sl::Translation translation = deltaOdom.getTranslation(); sl::Orientation quat = deltaOdom.getOrientation(); @@ -3435,8 +3436,8 @@ void ZEDWrapperNodelet::device_poll_thread_func() { NODELET_DEBUG_STREAM("ODOM -> Tracking Status: " << sl::toString(mTrackingStatus)); #endif - if (mTrackingStatus == sl::POSITIONAL_TRACKING_STATE::OK || mTrackingStatus == sl::POSITIONAL_TRACKING_STATE::SEARCHING || - mTrackingStatus == sl::POSITIONAL_TRACKING_STATE::FPS_TOO_LOW) { + if (mPosTrackingStatus == sl::POSITIONAL_TRACKING_STATE::OK || mPosTrackingStatus == sl::POSITIONAL_TRACKING_STATE::SEARCHING || + mPosTrackingStatus == sl::POSITIONAL_TRACKING_STATE::FPS_TOO_LOW) { // Transform ZED delta odom pose in TF2 Transformation geometry_msgs::Transform deltaTransf; deltaTransf.translation.x = translation(0); @@ -3495,7 +3496,7 @@ void ZEDWrapperNodelet::device_poll_thread_func() { // Publish the zed camera pose if someone has subscribed to if (computeTracking) { static sl::POSITIONAL_TRACKING_STATE oldStatus; - mTrackingStatus = mZed.getPosition(mLastZedPose, sl::REFERENCE_FRAME::WORLD); + mPosTrackingStatus = mZed.getPosition(mLastZedPose, sl::REFERENCE_FRAME::WORLD); sl::Translation translation = mLastZedPose.getTranslation(); sl::Orientation quat = mLastZedPose.getOrientation(); @@ -3513,8 +3514,8 @@ void ZEDWrapperNodelet::device_poll_thread_func() { #endif - if (mTrackingStatus == sl::POSITIONAL_TRACKING_STATE::OK || - mTrackingStatus == sl::POSITIONAL_TRACKING_STATE::SEARCHING /*|| status == sl::POSITIONAL_TRACKING_STATE::FPS_TOO_LOW*/) { + if (mPosTrackingStatus == sl::POSITIONAL_TRACKING_STATE::OK || + mPosTrackingStatus == sl::POSITIONAL_TRACKING_STATE::SEARCHING /*|| status == sl::POSITIONAL_TRACKING_STATE::FPS_TOO_LOW*/) { // Transform ZED pose in TF2 Transformation geometry_msgs::Transform map2sensTransf; @@ -3559,7 +3560,7 @@ void ZEDWrapperNodelet::device_poll_thread_func() { if (!(mFloorAlignment)) { initOdom = mInitOdomWithPose; } else { - initOdom = (mTrackingStatus == sl::POSITIONAL_TRACKING_STATE::OK) & mInitOdomWithPose; + initOdom = (mPosTrackingStatus == sl::POSITIONAL_TRACKING_STATE::OK) & mInitOdomWithPose; } if (initOdom || mResetOdom) { @@ -3601,7 +3602,7 @@ void ZEDWrapperNodelet::device_poll_thread_func() { mTrackingReady = true; } - oldStatus = mTrackingStatus; + oldStatus = mPosTrackingStatus; } if(mZedRealCamModel == sl::MODEL::ZED || !mPublishImuTf) { @@ -3744,7 +3745,7 @@ void ZEDWrapperNodelet::updateDiagnostic(diagnostic_updater::DiagnosticStatusWra double freq_perc = 100.*freq / mCamFrameRate; stat.addf("Capture", "Mean Frequency: %.1f Hz (%.1f%%)", freq, freq_perc); - stat.addf("Processing Time", "Mean time: %.3f sec (Max. %.3f sec)", mElabPeriodMean_sec->getMean(), 1. / mCamFrameRate); + stat.addf("General Processing", "Mean Time: %.3f sec (Max. %.3f sec)", mElabPeriodMean_sec->getMean(), 1. / mCamFrameRate); if(mPublishingData) { freq = 1. / mVideoDepthPeriodMean_sec->getMean(); @@ -3780,13 +3781,15 @@ void ZEDWrapperNodelet::updateDiagnostic(diagnostic_updater::DiagnosticStatusWra } if (mTrackingActivated) { - stat.addf("Tracking status", "%s", sl::toString(mTrackingStatus).c_str()); + stat.addf("Tracking status", "%s", sl::toString(mPosTrackingStatus).c_str()); } else { - stat.add("Tracking status", "INACTIVE"); + stat.add("Pos. Tracking status", "INACTIVE"); } if (mObjDetRunning) { - stat.addf("Object data processing", "%.3f sec", mObjDetPeriodMean_msec->getMean() / 1000.); + double freq = 1000./mObjDetPeriodMean_msec->getMean(); + double freq_perc = 100.*freq / mCamFrameRate; + stat.addf("Object detection", "Mean Frequency: %.3f Hz (%.1f%%)", freq, freq_perc ); } else { stat.add("Object Detection", "INACTIVE"); } @@ -4123,6 +4126,7 @@ bool ZEDWrapperNodelet::on_stop_object_detection(zed_interfaces::stop_object_det } void ZEDWrapperNodelet::detectObjects(bool publishObj, bool publishViz, ros::Time t) { + static std::chrono::steady_clock::time_point old_time = std::chrono::steady_clock::now(); sl::ObjectDetectionRuntimeParameters objectTracker_parameters_rt; objectTracker_parameters_rt.detection_confidence_threshold = mObjDetConfidence; @@ -4137,6 +4141,18 @@ void ZEDWrapperNodelet::detectObjects(bool publishObj, bool publishViz, ros::Tim return; } + if(!objects.is_new) + { + return; + } + + // ----> Diagnostic information update + std::chrono::steady_clock::time_point now = std::chrono::steady_clock::now(); + double elapsed_msec = std::chrono::duration_cast(now - old_time).count(); + mObjDetPeriodMean_msec->addValue(elapsed_msec); + old_time = now; + // <---- Diagnostic information update + // NODELET_DEBUG_STREAM("Detected " << objects.object_list.size() << " objects"); size_t objCount = objects.object_list.size(); @@ -4187,7 +4203,6 @@ void ZEDWrapperNodelet::detectObjects(bool publishObj, bool publishViz, ros::Tim } if (publishViz) { - if( data.bounding_box.size()!=8 ) { continue; // No 3D information available } @@ -4241,155 +4256,6 @@ void ZEDWrapperNodelet::detectObjects(bool publishObj, bool publishViz, ros::Tim label.text = std::to_string(data.id) + ". " + std::string(sl::toString(data.label).c_str()); objMarkersMsg.markers.push_back(label); - - // visualization_msgs::Marker lines; - // visualization_msgs::Marker label; - // visualization_msgs::Marker spheres; - - // lines.pose.orientation.w = label.pose.orientation.w = spheres.pose.orientation.w = 1.0; - - // lines.header = header; - // lines.lifetime = ros::Duration(0.3); - // lines.action = visualization_msgs::Marker::ADD; - // lines.id = data.id; - // lines.ns = "bbox"; - // lines.type = visualization_msgs::Marker::LINE_LIST; - // lines.scale.x = 0.005; - - // label.header = header; - // label.lifetime = ros::Duration(0.3); - // label.action = visualization_msgs::Marker::ADD; - // label.id = data.id; - // label.ns = "label"; - // label.type = visualization_msgs::Marker::TEXT_VIEW_FACING; - // //label.scale.x = 0.05; - // //label.scale.y = 0.05; - // label.scale.z = 0.1; - - // spheres.header = header; - // spheres.lifetime = ros::Duration(0.3); - // spheres.action = visualization_msgs::Marker::ADD; - // spheres.id = data.id; - // spheres.ns = "spheres"; - // spheres.type = visualization_msgs::Marker::SPHERE_LIST; - // spheres.scale.x = 0.02; - // spheres.scale.y = 0.02; - // spheres.scale.z = 0.02; - - // spheres.color.r = data.tracking_state == sl::OBJECT_TRACKING_STATE::OK ? - // 0.1f : (data.tracking_state == sl::OBJECT_TRACKING_STATE::SEARCHING ? - // 0.9f : 0.3f); - // spheres.color.g = data.tracking_state == sl::OBJECT_TRACKING_STATE::OK ? - // 0.9f : (data.tracking_state == sl::OBJECT_TRACKING_STATE::SEARCHING ? - // 0.1f : 0.3f); - // spheres.color.b = data.tracking_state == sl::OBJECT_TRACKING_STATE::OK ? - // 0.1f : (data.tracking_state == sl::OBJECT_TRACKING_STATE::SEARCHING ? - // 0.1f : 0.3f); - // spheres.color.a = 0.75f; - - // sl::float3 color = generateColorClass(data.id); - - // lines.color.r = color.r; - // lines.color.g = color.g; - // lines.color.b = color.b; - // lines.color.a = 0.75f; - - // label.color.r = color.r; - // label.color.g = color.g; - // label.color.b = color.b; - // label.color.a = 0.75f; - - // label.pose.position.x = data.position.x; - // label.pose.position.y = data.position.y; - // label.pose.position.z = data.position.z; - - // label.text = std::to_string(data.id) + ". " + std::string(sl::toString(data.label).c_str()); - - // geometry_msgs::Point p0; - // geometry_msgs::Point p1; - - // // Centroid - // p0.x = data.position.x; - // p0.y = data.position.y; - // p0.z = data.position.z; - - // spheres.points.resize(9); - // spheres.points[8] = p0;; - - // lines.points.resize(24); - - // int linePtIdx = 0; - - // // Top square - // for (int v = 0; v < 3; v++) { - // lines.points[linePtIdx].x = data.bounding_box[v].x; - // lines.points[linePtIdx].y = data.bounding_box[v].y; - // lines.points[linePtIdx].z = data.bounding_box[v].z; - // linePtIdx++; - - // lines.points[linePtIdx].x = data.bounding_box[v + 1].x; - // lines.points[linePtIdx].y = data.bounding_box[v + 1].y; - // lines.points[linePtIdx].z = data.bounding_box[v + 1].z; - // linePtIdx++; - // } - - // lines.points[linePtIdx].x = data.bounding_box[3].x; - // lines.points[linePtIdx].y = data.bounding_box[3].y; - // lines.points[linePtIdx].z = data.bounding_box[3].z; - // linePtIdx++; - - // lines.points[linePtIdx].x = data.bounding_box[0].x; - // lines.points[linePtIdx].y = data.bounding_box[0].y; - // lines.points[linePtIdx].z = data.bounding_box[0].z; - // linePtIdx++; - - // // Bottom square - // for (int v = 4; v < 7; v++) { - // lines.points[linePtIdx].x = data.bounding_box[v].x; - // lines.points[linePtIdx].y = data.bounding_box[v].y; - // lines.points[linePtIdx].z = data.bounding_box[v].z; - // linePtIdx++; - - // lines.points[linePtIdx].x = data.bounding_box[v + 1].x; - // lines.points[linePtIdx].y = data.bounding_box[v + 1].y; - // lines.points[linePtIdx].z = data.bounding_box[v + 1].z; - // linePtIdx++; - // } - - // lines.points[linePtIdx].x = data.bounding_box[7].x; - // lines.points[linePtIdx].y = data.bounding_box[7].y; - // lines.points[linePtIdx].z = data.bounding_box[7].z; - // linePtIdx++; - - // lines.points[linePtIdx].x = data.bounding_box[4].x; - // lines.points[linePtIdx].y = data.bounding_box[4].y; - // lines.points[linePtIdx].z = data.bounding_box[4].z; - // linePtIdx++; - - // // Lateral lines and vertex spheres - // for (int v = 0; v < 4; v++) { - // lines.points[linePtIdx].x = data.bounding_box[v].x; - // lines.points[linePtIdx].y = data.bounding_box[v].y; - // lines.points[linePtIdx].z = data.bounding_box[v].z; - // linePtIdx++; - - // lines.points[linePtIdx].x = data.bounding_box[v + 4].x; - // lines.points[linePtIdx].y = data.bounding_box[v + 4].y; - // lines.points[linePtIdx].z = data.bounding_box[v + 4].z; - // linePtIdx++; - - // spheres.points[v * 2].x = data.bounding_box[v].x; - // spheres.points[v * 2].y = data.bounding_box[v].y; - // spheres.points[v * 2].z = data.bounding_box[v].z; - - // spheres.points[v * 2 + 1].x = data.bounding_box[v + 4].x; - // spheres.points[v * 2 + 1].y = data.bounding_box[v + 4].y; - // spheres.points[v * 2 + 1].z = data.bounding_box[v + 4].z; - // } - - // objMarkersMsg.markers[i * 3] = lines; - // objMarkersMsg.markers[i * 3 + 1] = label; - // objMarkersMsg.markers[i * 3 + 2] = spheres; } } diff --git a/zed_wrapper/params/common.yaml b/zed_wrapper/params/common.yaml index 5d5618d1..a200b0ff 100644 --- a/zed_wrapper/params/common.yaml +++ b/zed_wrapper/params/common.yaml @@ -24,7 +24,7 @@ general: zed_id: 0 serial_number: 0 resolution: 2 # '0': HD2K, '1': HD1080, '2': HD720, '3': VGA - grab_frame_rate: 15 # Frequency of frame grabbing for internal SDK operations + grab_frame_rate: 30 # Frequency of frame grabbing for internal SDK operations gpu_id: -1 base_frame: 'base_link' # must be equal to the frame_id used in the URDF file verbose: false # Enable info message by the ZED SDK From a042543b367b387dd11d9c74c42d6cff662622d2 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Fri, 18 Sep 2020 09:14:59 +0200 Subject: [PATCH 011/199] Update latest_changes.md --- latest_changes.md | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/latest_changes.md b/latest_changes.md index afee8ba2..0c8704bd 100644 --- a/latest_changes.md +++ b/latest_changes.md @@ -1,6 +1,10 @@ LATEST CHANGES ============== +ASYNC OBject Detection (2020-09-18) +----------------------------------- +- Object Detection now runs asynchronously respect to data grabbing and Object Detected data are published only when available not affecting the frequency of the publishing of the other data types + IMU timestamp fix (2020-08-25) ------------------------------ - Added new parameter `sensors/publish_imu_tf` to enable/disable IMU TF broadcasting From d167afaa53be561913985403f263dcb3d0d35ecc Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Fri, 18 Sep 2020 09:26:02 +0200 Subject: [PATCH 012/199] Update latest_changes.md --- latest_changes.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/latest_changes.md b/latest_changes.md index 0c8704bd..1da2de3a 100644 --- a/latest_changes.md +++ b/latest_changes.md @@ -1,7 +1,7 @@ LATEST CHANGES ============== -ASYNC OBject Detection (2020-09-18) +ASYNC Object Detection (2020-09-18) ----------------------------------- - Object Detection now runs asynchronously respect to data grabbing and Object Detected data are published only when available not affecting the frequency of the publishing of the other data types From bb3a5e53d8d1921e99ea3db31e4ca49584434bb2 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Wed, 30 Sep 2020 10:25:15 +0200 Subject: [PATCH 013/199] Update latest_changes.md --- latest_changes.md | 1 + 1 file changed, 1 insertion(+) diff --git a/latest_changes.md b/latest_changes.md index 1da2de3a..0ad18d89 100644 --- a/latest_changes.md +++ b/latest_changes.md @@ -4,6 +4,7 @@ LATEST CHANGES ASYNC Object Detection (2020-09-18) ----------------------------------- - Object Detection now runs asynchronously respect to data grabbing and Object Detected data are published only when available not affecting the frequency of the publishing of the other data types +- Depth OpenNI topic name changed from `depth/depth_raw_registered` to `depth/depth_registered` IMU timestamp fix (2020-08-25) ------------------------------ From ac1c162d0995c226111f08caf22948b57b299627 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Fri, 9 Oct 2020 12:58:24 +0200 Subject: [PATCH 014/199] Fix to RGB/Depth desync issue --- .../include/zed_wrapper_nodelet.hpp | 14 +- .../zed_nodelet/src/zed_wrapper_nodelet.cpp | 386 +++++++++--------- 2 files changed, 203 insertions(+), 197 deletions(-) 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 5adba6c3..877e42bb 100644 --- a/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp +++ b/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp @@ -183,7 +183,7 @@ class ZEDWrapperNodelet : public nodelet::Nodelet { /*! \brief Publish a fused pointCloud with a ros Publisher */ - void pubFusedPointCloudCallback(const ros::TimerEvent& e); + void callback_pubFusedPointCloud(const ros::TimerEvent& e); /*! \brief Publish the informations of a camera with a ros Publisher * \param cam_info_msg : the information message to publish @@ -231,27 +231,27 @@ class ZEDWrapperNodelet : public nodelet::Nodelet { /*! \brief Callback to handle dynamic reconfigure events in ROS */ - void dynamicReconfCallback(zed_nodelets::ZedConfig& config, uint32_t level); + void callback_dynamicReconf(zed_nodelets::ZedConfig& config, uint32_t level); /*! \brief Callback to publish Video and Depth data * \param e : the ros::TimerEvent binded to the callback */ - void pubVideoDepthCallback(const ros::TimerEvent& e); + void callback_pubVideoDepth(const ros::TimerEvent& e); /*! \brief Callback to publish Path data with a ROS publisher. * \param e : the ros::TimerEvent binded to the callback */ - void pubPathCallback(const ros::TimerEvent& e); + void callback_pubPath(const ros::TimerEvent& e); /*! \brief Callback to publish Sensor Data with a ROS publisher. * \param e : the ros::TimerEvent binded to the callback */ - void pubSensCallback(const ros::TimerEvent& e); + void callback_pubSens(const ros::TimerEvent& e); /*! \brief Callback to update node diagnostic status * \param stat : node status */ - void updateDiagnostic(diagnostic_updater::DiagnosticStatusWrapper& stat); + void callback_updateDiagnostic(diagnostic_updater::DiagnosticStatusWrapper& stat); /*! \brief Service callback to reset_tracking service * Tracking pose is reinitialized to the value available in the ROS Param @@ -646,6 +646,8 @@ class ZEDWrapperNodelet : public nodelet::Nodelet { std::mutex mObjDetMutex; std::condition_variable mPcDataReadyCondVar; bool mPcDataReady; + std::condition_variable mRgbDepthDataRetrievedCondVar; + bool mRgbDepthDataRetrieved; // Point cloud variables sl::Mat mCloud; 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 2558284a..c944805a 100644 --- a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp @@ -67,6 +67,7 @@ void ZEDWrapperNodelet::onInit() { mStopNode = false; mPcDataReady = false; + mRgbDepthDataRetrieved = true; #ifndef NDEBUG @@ -235,7 +236,7 @@ void ZEDWrapperNodelet::onInit() { mZedParams.enable_image_enhancement = true; // Always active - mDiagUpdater.add("ZED Diagnostic", this, &ZEDWrapperNodelet::updateDiagnostic); + mDiagUpdater.add("ZED Diagnostic", this, &ZEDWrapperNodelet::callback_updateDiagnostic); mDiagUpdater.setHardwareID("ZED camera"); mConnStatus = sl::ERROR_CODE::CAMERA_NOT_DETECTED; @@ -359,7 +360,7 @@ void ZEDWrapperNodelet::onInit() { // ----> Dynamic Reconfigure parameters mDynRecServer = boost::make_shared>(mDynServerMutex); dynamic_reconfigure::Server::CallbackType f; - f = boost::bind(&ZEDWrapperNodelet::dynamicReconfCallback, this, _1, _2); + f = boost::bind(&ZEDWrapperNodelet::callback_dynamicReconf, this, _1, _2); mDynRecServer->setCallback(f); // Update parameters zed_nodelets::ZedConfig config; @@ -467,7 +468,7 @@ void ZEDWrapperNodelet::onInit() { NODELET_INFO_STREAM("Advertised on topic " << mPubMapPath.getTopic()); mPathTimer = mNhNs.createTimer(ros::Duration(1.0 / mPathPubRate), - &ZEDWrapperNodelet::pubPathCallback, this); + &ZEDWrapperNodelet::callback_pubPath, this); if (mPathMaxCount != -1) { NODELET_DEBUG_STREAM("Path vectors reserved " << mPathMaxCount << " poses."); @@ -518,7 +519,7 @@ void ZEDWrapperNodelet::onInit() { mFrameTimestamp = ros::Time::now(); mImuTimer = mNhNs.createTimer(ros::Duration(1.0 / (mSensPubRate*1.5) ), - &ZEDWrapperNodelet::pubSensCallback, this); + &ZEDWrapperNodelet::callback_pubSens, this); mSensPeriodMean_usec.reset(new sl_tools::CSmartMean(mSensPubRate / 2)); @@ -577,7 +578,7 @@ void ZEDWrapperNodelet::onInit() { mDevicePollThread = std::thread(&ZEDWrapperNodelet::device_poll_thread_func, this); // Start data publishing timer - mVideoDepthTimer = mNhNs.createTimer(ros::Duration(1.0 / mVideoDepthFreq), &ZEDWrapperNodelet::pubVideoDepthCallback, + mVideoDepthTimer = mNhNs.createTimer(ros::Duration(1.0 / mVideoDepthFreq), &ZEDWrapperNodelet::callback_pubVideoDepth, this); } @@ -1298,7 +1299,7 @@ bool ZEDWrapperNodelet::start_3d_mapping() { mMappingRunning = true; - mFusedPcTimer = mNhNs.createTimer(ros::Duration(1.0 / mFusedPcPubFreq), &ZEDWrapperNodelet::pubFusedPointCloudCallback, + mFusedPcTimer = mNhNs.createTimer(ros::Duration(1.0 / mFusedPcPubFreq), &ZEDWrapperNodelet::callback_pubFusedPointCloud, this); NODELET_INFO_STREAM(" * Resolution: " << params.resolution_meter << " m"); @@ -1757,13 +1758,11 @@ void ZEDWrapperNodelet::pointcloud_thread_func() { if (elapsed_msec < pc_period_msec) { std::this_thread::sleep_for(std::chrono::milliseconds(static_cast(pc_period_msec - elapsed_msec))); } - // <---- Check publishing frequency last_time = std::chrono::steady_clock::now(); publishPointCloud(); - mPcDataReady = false; } @@ -1817,7 +1816,7 @@ void ZEDWrapperNodelet::publishPointCloud() { mPubCloud.publish(pointcloudMsg); } -void ZEDWrapperNodelet::pubFusedPointCloudCallback(const ros::TimerEvent& e) { +void ZEDWrapperNodelet::callback_pubFusedPointCloud(const ros::TimerEvent& e) { sensor_msgs::PointCloud2Ptr pointcloudFusedMsg = boost::make_shared(); @@ -2105,7 +2104,7 @@ void ZEDWrapperNodelet::updateDynamicReconfigure() { //NODELET_DEBUG_STREAM( "updateDynamicReconfigure MUTEX UNLOCK"); } -void ZEDWrapperNodelet::dynamicReconfCallback(zed_nodelets::ZedConfig& config, uint32_t level) { +void ZEDWrapperNodelet::callback_dynamicReconf(zed_nodelets::ZedConfig& config, uint32_t level) { //NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX LOCK"); mDynParMutex.lock(); DynParams param = static_cast(level); @@ -2270,244 +2269,237 @@ void ZEDWrapperNodelet::dynamicReconfCallback(zed_nodelets::ZedConfig& config, u } } -void ZEDWrapperNodelet::pubVideoDepthCallback(const ros::TimerEvent& e) { +void ZEDWrapperNodelet::callback_pubVideoDepth(const ros::TimerEvent& e) { static sl::Timestamp lastZedTs = 0; // Used to calculate stable publish frequency - uint32_t rgbSubnumber = mPubRgb.getNumSubscribers(); - uint32_t rgbRawSubnumber = mPubRawRgb.getNumSubscribers(); - uint32_t leftSubnumber = mPubLeft.getNumSubscribers(); - uint32_t leftRawSubnumber = mPubRawLeft.getNumSubscribers(); - uint32_t rightSubnumber = mPubRight.getNumSubscribers(); - uint32_t rightRawSubnumber = mPubRawRight.getNumSubscribers(); - uint32_t rgbGraySubnumber = mPubRgbGray.getNumSubscribers(); - uint32_t rgbGrayRawSubnumber = mPubRawRgbGray.getNumSubscribers(); - uint32_t leftGraySubnumber = mPubLeftGray.getNumSubscribers(); - uint32_t leftGrayRawSubnumber = mPubRawLeftGray.getNumSubscribers(); - uint32_t rightGraySubnumber = mPubRightGray.getNumSubscribers(); - uint32_t rightGrayRawSubnumber = mPubRawRightGray.getNumSubscribers(); - uint32_t depthSubnumber = mPubDepth.getNumSubscribers(); - uint32_t disparitySubnumber = mPubDisparity.getNumSubscribers(); - uint32_t confMapSubnumber = mPubConfMap.getNumSubscribers(); - uint32_t stereoSubNumber = mPubStereo.getNumSubscribers(); - uint32_t stereoRawSubNumber = mPubRawStereo.getNumSubscribers(); - - if(rgbSubnumber+rgbRawSubnumber+ - leftSubnumber+leftRawSubnumber+ - rightSubnumber+rightRawSubnumber+ - rgbGraySubnumber+rgbGrayRawSubnumber+ - leftGraySubnumber+leftGrayRawSubnumber+ - rightGraySubnumber+rightGrayRawSubnumber+ - depthSubnumber+disparitySubnumber+confMapSubnumber+ - stereoSubNumber+stereoRawSubNumber == 0) { + uint32_t rgbSubnumber = mPubRgb.getNumSubscribers(); // + uint32_t rgbRawSubnumber = mPubRawRgb.getNumSubscribers(); // + uint32_t leftSubnumber = mPubLeft.getNumSubscribers(); // + uint32_t leftRawSubnumber = mPubRawLeft.getNumSubscribers(); // + uint32_t rightSubnumber = mPubRight.getNumSubscribers(); // + uint32_t rightRawSubnumber = mPubRawRight.getNumSubscribers(); // + uint32_t rgbGraySubnumber = mPubRgbGray.getNumSubscribers(); // + uint32_t rgbGrayRawSubnumber = mPubRawRgbGray.getNumSubscribers(); // + uint32_t leftGraySubnumber = mPubLeftGray.getNumSubscribers(); // + uint32_t leftGrayRawSubnumber = mPubRawLeftGray.getNumSubscribers(); // + uint32_t rightGraySubnumber = mPubRightGray.getNumSubscribers(); // + uint32_t rightGrayRawSubnumber = mPubRawRightGray.getNumSubscribers(); // + uint32_t depthSubnumber = mPubDepth.getNumSubscribers(); // + uint32_t disparitySubnumber = mPubDisparity.getNumSubscribers(); // + uint32_t confMapSubnumber = mPubConfMap.getNumSubscribers(); // + uint32_t stereoSubNumber = mPubStereo.getNumSubscribers(); // + uint32_t stereoRawSubNumber = mPubRawStereo.getNumSubscribers(); // + + bool retrieved = false; + + sl::Mat mat_left,mat_left_raw; + sl::Mat mat_right,mat_right_raw; + sl::Mat mat_left_gray,mat_left_raw_gray; + 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 grab_ts=0; + + // ----> Retrieve all required data + std::unique_lock lock(mCamDataMutex, std::defer_lock); + + if (lock.try_lock()) { + if(rgbSubnumber+leftSubnumber>0) { + mZed.retrieveImage(mat_left, sl::VIEW::LEFT, sl::MEM::CPU, mMatResolVideo); + retrieved = true; + ts_rgb=mat_left.timestamp; + grab_ts=mat_left.timestamp; + } + if(rgbRawSubnumber+leftRawSubnumber>0) { + mZed.retrieveImage(mat_left_raw, sl::VIEW::LEFT_UNRECTIFIED, sl::MEM::CPU, mMatResolVideo); + retrieved = true; + grab_ts=mat_left_raw.timestamp; + } + if(rightSubnumber>0) { + mZed.retrieveImage(mat_right, sl::VIEW::RIGHT, sl::MEM::CPU, mMatResolVideo); + retrieved = true; + grab_ts=mat_right.timestamp; + } + if(rightRawSubnumber>0) { + mZed.retrieveImage(mat_right_raw, sl::VIEW::RIGHT_UNRECTIFIED, sl::MEM::CPU, mMatResolVideo); + retrieved = true; + grab_ts=mat_right_raw.timestamp; + } + if(rgbGraySubnumber+leftGraySubnumber>0) { + mZed.retrieveImage(mat_left_gray, sl::VIEW::LEFT_GRAY, sl::MEM::CPU, mMatResolVideo); + retrieved = true; + grab_ts=mat_left_gray.timestamp; + } + if(rgbGrayRawSubnumber+leftGrayRawSubnumber>0) { + mZed.retrieveImage(mat_left_raw_gray, sl::VIEW::LEFT_UNRECTIFIED_GRAY, sl::MEM::CPU, mMatResolVideo); + retrieved = true; + grab_ts=mat_left_raw_gray.timestamp; + } + if(rightGraySubnumber>0) { + mZed.retrieveImage(mat_right_gray, sl::VIEW::RIGHT_GRAY, sl::MEM::CPU, mMatResolVideo); + retrieved = true; + grab_ts=mat_right_gray.timestamp; + } + if(rightGrayRawSubnumber>0) { + mZed.retrieveImage(mat_right_raw_gray, sl::VIEW::RIGHT_UNRECTIFIED_GRAY, sl::MEM::CPU, mMatResolVideo); + retrieved = true; + grab_ts=mat_right_raw_gray.timestamp; + } + if(depthSubnumber>0) { + mZed.retrieveMeasure(mat_depth, sl::MEASURE::DEPTH, sl::MEM::CPU, mMatResolDepth); + retrieved = true; + grab_ts=mat_depth.timestamp; + + ts_depth = mat_depth.timestamp; + if( ts_rgb.data_ns!=0 && (ts_depth.data_ns!=ts_rgb.data_ns) ) { + NODELET_WARN_STREAM( "!!!!! DEPTH/RGB ASYNC !!!!! - Delta: " << 1e-9*static_cast(ts_depth-ts_rgb) << " sec"); + } + } + if(disparitySubnumber>0) { + mZed.retrieveMeasure(mat_disp, sl::MEASURE::DISPARITY, sl::MEM::CPU, mMatResolDepth); + retrieved = true; + grab_ts=mat_disp.timestamp; + } + if(confMapSubnumber) { + mZed.retrieveMeasure(mat_conf, sl::MEASURE::CONFIDENCE, sl::MEM::CPU, mMatResolDepth); + retrieved = true; + grab_ts=mat_conf.timestamp; + } + } + // <---- Retrieve all required data + + // ----> Notify grab thread that all data are synchronized and a new grab can be done + mRgbDepthDataRetrievedCondVar.notify_one(); + mRgbDepthDataRetrieved = true; + // <---- Notify grab thread that all data are synchronized and a new grab can be done + + if(!retrieved) { mPublishingData = false; lastZedTs = 0; return; } - mPublishingData = true; - sl::Mat leftZEDMat, rightZEDMat, leftGrayZEDMat, rightGrayZEDMat, - depthZEDMat, disparityZEDMat, confMapZEDMat; - - // Retrieve RGBA Left image and use Timestamp to check if image is new - mZed.retrieveImage(leftZEDMat, sl::VIEW::LEFT, sl::MEM::CPU, mMatResolVideo); - if(leftZEDMat.timestamp==lastZedTs) { - return; // No new image! + // ----> Check if a grab has been done before publishing the same images + if( grab_ts.data_ns==lastZedTs.data_ns ) { + // Data not updated by a grab calling in the grab thread + return; } if(lastZedTs.data_ns!=0) { - double period_sec = static_cast(leftZEDMat.timestamp.data_ns - lastZedTs.data_ns)/1e9; + double period_sec = static_cast(grab_ts.data_ns - lastZedTs.data_ns)/1e9; //NODELET_DEBUG_STREAM( "PUBLISHING PERIOD: " << period_sec << " sec @" << 1./period_sec << " Hz") ; mVideoDepthPeriodMean_sec->addValue(period_sec); //NODELET_DEBUG_STREAM( "MEAN PUBLISHING PERIOD: " << mVideoDepthPeriodMean_sec->getMean() << " sec @" << 1./mVideoDepthPeriodMean_sec->getMean() << " Hz") ; } - lastZedTs = leftZEDMat.timestamp; - - // Timestamp to be used for topics headers - ros::Time stamp = mFrameTimestamp; - - // Publish the left == rgb image if someone has subscribed to - if (leftSubnumber > 0 || rgbSubnumber > 0) { - if (leftSubnumber > 0) { - sensor_msgs::ImagePtr leftImgMsg = boost::make_shared(); + lastZedTs = grab_ts; + // <---- Check if a grab has been done before publishing the same images - publishImage(leftImgMsg, leftZEDMat, mPubLeft, mLeftCamInfoMsg, mLeftCamOptFrameId, stamp); - } - - if (rgbSubnumber > 0) { - sensor_msgs::ImagePtr rgbImgMsg = boost::make_shared(); + ros::Time stamp = sl_tools::slTime2Ros(grab_ts); - // rgb is the left image - publishImage(rgbImgMsg, leftZEDMat, mPubRgb, mRgbCamInfoMsg, mDepthOptFrameId, stamp); - } + // Publish the left = rgb image if someone has subscribed to + if (leftSubnumber > 0) { + sensor_msgs::ImagePtr leftImgMsg = boost::make_shared(); + publishImage(leftImgMsg, mat_left, mPubLeft, mLeftCamInfoMsg, mLeftCamOptFrameId, stamp); } - - // Publish the left == rgb GRAY image if someone has subscribed to - if (leftGraySubnumber > 0 || rgbGraySubnumber > 0) { - - // Retrieve RGBA Left image - mZed.retrieveImage(leftGrayZEDMat, sl::VIEW::LEFT_GRAY, sl::MEM::CPU, mMatResolVideo); - - if (leftGraySubnumber > 0) { - sensor_msgs::ImagePtr leftGrayImgMsg = boost::make_shared(); - - publishImage(leftGrayImgMsg, leftGrayZEDMat, mPubLeftGray, mLeftCamInfoMsg, mLeftCamOptFrameId, - stamp); - } - - if (rgbGraySubnumber > 0) { - sensor_msgs::ImagePtr rgbGrayImgMsg = boost::make_shared(); - - publishImage(rgbGrayImgMsg, leftGrayZEDMat, mPubRgbGray, mRgbCamInfoMsg, mDepthOptFrameId, - stamp); // rgb is the left image - } + if (rgbSubnumber > 0) { + sensor_msgs::ImagePtr rgbImgMsg = boost::make_shared(); + publishImage(rgbImgMsg, mat_left, mPubRgb, mRgbCamInfoMsg, mDepthOptFrameId, stamp); } - // Publish the left_raw == rgb_raw image if someone has subscribed to - if (leftRawSubnumber > 0 || rgbRawSubnumber > 0) { - - // Retrieve RGBA Left image - mZed.retrieveImage(leftZEDMat, sl::VIEW::LEFT_UNRECTIFIED, sl::MEM::CPU, mMatResolVideo); - - if (leftRawSubnumber > 0) { - sensor_msgs::ImagePtr rawLeftImgMsg = boost::make_shared(); - - publishImage(rawLeftImgMsg, leftZEDMat, mPubRawLeft, mLeftCamInfoRawMsg, mLeftCamOptFrameId, - stamp); - } - - if (rgbRawSubnumber > 0) { - sensor_msgs::ImagePtr rawRgbImgMsg = boost::make_shared(); + // Publish the left = rgb GRAY image if someone has subscribed to + if (leftGraySubnumber > 0) { + sensor_msgs::ImagePtr leftGrayImgMsg = boost::make_shared(); + publishImage(leftGrayImgMsg, mat_left_gray, mPubLeftGray, mLeftCamInfoMsg, mLeftCamOptFrameId, stamp); + } + if (rgbGraySubnumber > 0) { + sensor_msgs::ImagePtr rgbGrayImgMsg = boost::make_shared(); + publishImage(rgbGrayImgMsg, mat_left_gray, mPubRgbGray, mRgbCamInfoMsg, mDepthOptFrameId, stamp); + } - publishImage(rawRgbImgMsg, leftZEDMat, mPubRawRgb, mRgbCamInfoRawMsg, mDepthOptFrameId, - stamp); - } + // Publish the left_raw = rgb_raw image if someone has subscribed to + if (leftRawSubnumber > 0) { + sensor_msgs::ImagePtr rawLeftImgMsg = boost::make_shared(); + publishImage(rawLeftImgMsg, mat_left_raw, mPubRawLeft, mLeftCamInfoRawMsg, mLeftCamOptFrameId, stamp); + } + if (rgbRawSubnumber > 0) { + sensor_msgs::ImagePtr rawRgbImgMsg = boost::make_shared(); + publishImage(rawRgbImgMsg, mat_left_raw, mPubRawRgb, mRgbCamInfoRawMsg, mDepthOptFrameId, stamp); } // Publish the left_raw == rgb_raw GRAY image if someone has subscribed to - if (leftGrayRawSubnumber > 0 || rgbGrayRawSubnumber > 0) { - - // Retrieve RGBA Left image - mZed.retrieveImage(leftGrayZEDMat, sl::VIEW::LEFT_UNRECTIFIED_GRAY, sl::MEM::CPU, mMatResolVideo); + if (leftGrayRawSubnumber > 0) { + sensor_msgs::ImagePtr rawLeftGrayImgMsg = boost::make_shared(); - if (leftGrayRawSubnumber > 0) { - sensor_msgs::ImagePtr rawLeftGrayImgMsg = boost::make_shared(); - - publishImage(rawLeftGrayImgMsg, leftGrayZEDMat, mPubRawLeftGray, mLeftCamInfoRawMsg, mLeftCamOptFrameId, - stamp); - } - - if (rgbGrayRawSubnumber > 0) { - sensor_msgs::ImagePtr rawRgbGrayImgMsg = boost::make_shared(); - - publishImage(rawRgbGrayImgMsg, leftGrayZEDMat, mPubRawRgbGray, mRgbCamInfoRawMsg, mDepthOptFrameId, - stamp); - } + publishImage(rawLeftGrayImgMsg, mat_left_raw_gray, mPubRawLeftGray, mLeftCamInfoRawMsg, mLeftCamOptFrameId, stamp); + } + if (rgbGrayRawSubnumber > 0) { + sensor_msgs::ImagePtr rawRgbGrayImgMsg = boost::make_shared(); + publishImage(rawRgbGrayImgMsg, mat_left_raw_gray, mPubRawRgbGray, mRgbCamInfoRawMsg, mDepthOptFrameId, stamp); } // Publish the right image if someone has subscribed to if (rightSubnumber > 0) { - - // Retrieve RGBA Right image - mZed.retrieveImage(rightZEDMat, sl::VIEW::RIGHT, sl::MEM::CPU, mMatResolVideo); - sensor_msgs::ImagePtr rightImgMsg = boost::make_shared(); - - publishImage(rightImgMsg, rightZEDMat, mPubRight, mRightCamInfoMsg, mRightCamOptFrameId, stamp); + publishImage(rightImgMsg, mat_right, mPubRight, mRightCamInfoMsg, mRightCamOptFrameId, stamp); } // Publish the right image GRAY if someone has subscribed to if (rightGraySubnumber > 0) { - - // Retrieve RGBA Right image - mZed.retrieveImage(rightGrayZEDMat, sl::VIEW::RIGHT_GRAY, sl::MEM::CPU, mMatResolVideo); sensor_msgs::ImagePtr rightGrayImgMsg = boost::make_shared(); - - publishImage(rightGrayImgMsg, rightGrayZEDMat, mPubRightGray, mRightCamInfoMsg, mRightCamOptFrameId, stamp); + publishImage(rightGrayImgMsg, mat_right_gray, mPubRightGray, mRightCamInfoMsg, mRightCamOptFrameId, stamp); } // Publish the right raw image if someone has subscribed to if (rightRawSubnumber > 0) { - - // Retrieve RGBA Right image - mZed.retrieveImage(rightZEDMat, sl::VIEW::RIGHT_UNRECTIFIED, sl::MEM::CPU, mMatResolVideo); - sensor_msgs::ImagePtr rawRightImgMsg = boost::make_shared(); - - publishImage(rawRightImgMsg, rightZEDMat, mPubRawRight, mRightCamInfoRawMsg, mRightCamOptFrameId, stamp); + publishImage(rawRightImgMsg, mat_right_raw, mPubRawRight, mRightCamInfoRawMsg, mRightCamOptFrameId, stamp); } // Publish the right raw image GRAY if someone has subscribed to if (rightGrayRawSubnumber > 0) { - // Retrieve RGBA Right image - mZed.retrieveImage(rightGrayZEDMat, sl::VIEW::RIGHT_UNRECTIFIED_GRAY, sl::MEM::CPU, mMatResolVideo); - sensor_msgs::ImagePtr rawRightGrayImgMsg = boost::make_shared(); - - publishImage(rawRightGrayImgMsg, rightGrayZEDMat, mPubRawRightGray, mRightCamInfoRawMsg, mRightCamOptFrameId, stamp); + publishImage(rawRightGrayImgMsg, mat_right_raw_gray, mPubRawRightGray, mRightCamInfoRawMsg, mRightCamOptFrameId, stamp); } // Stereo couple side-by-side if (stereoSubNumber > 0) { - - // Retrieve RGBA Right image - mZed.retrieveImage(rightZEDMat, sl::VIEW::RIGHT, sl::MEM::CPU, mMatResolVideo); - mZed.retrieveImage(leftZEDMat, sl::VIEW::LEFT, sl::MEM::CPU, mMatResolVideo); - sensor_msgs::ImagePtr stereoImgMsg = boost::make_shared(); - - sl_tools::imagesToROSmsg(stereoImgMsg, leftZEDMat, rightZEDMat, mCameraFrameId, stamp); - + sl_tools::imagesToROSmsg(stereoImgMsg, mat_left, mat_right, mCameraFrameId, stamp); mPubStereo.publish(stereoImgMsg); } // Stereo RAW couple side-by-side if (stereoRawSubNumber > 0) { - - // Retrieve RGBA Right image - mZed.retrieveImage(rightZEDMat, sl::VIEW::RIGHT_UNRECTIFIED, sl::MEM::CPU, mMatResolVideo); - mZed.retrieveImage(leftZEDMat, sl::VIEW::LEFT_UNRECTIFIED, sl::MEM::CPU, mMatResolVideo); - sensor_msgs::ImagePtr rawStereoImgMsg = boost::make_shared(); - - sl_tools::imagesToROSmsg(rawStereoImgMsg, leftZEDMat, rightZEDMat, mCameraFrameId, stamp); - + sl_tools::imagesToROSmsg(rawStereoImgMsg, mat_left_raw, mat_right_raw, mCameraFrameId, stamp); mPubRawStereo.publish(rawStereoImgMsg); } // Publish the depth image if someone has subscribed to - if (depthSubnumber > 0 || disparitySubnumber > 0) { - - mZed.retrieveMeasure(depthZEDMat, sl::MEASURE::DEPTH, sl::MEM::CPU, mMatResolDepth); - + if (depthSubnumber > 0) { sensor_msgs::ImagePtr depthImgMsg = boost::make_shared(); - - publishDepth(depthImgMsg, depthZEDMat, stamp); // in meters + publishDepth(depthImgMsg, mat_depth, stamp); } // Publish the disparity image if someone has subscribed to if (disparitySubnumber > 0) { - - mZed.retrieveMeasure(disparityZEDMat, sl::MEASURE::DISPARITY, sl::MEM::CPU, mMatResolDepth); - publishDisparity(disparityZEDMat, stamp); + publishDisparity(mat_disp, stamp); } // Publish the confidence map if someone has subscribed to if (confMapSubnumber > 0) { - - mZed.retrieveMeasure(confMapZEDMat, sl::MEASURE::CONFIDENCE, sl::MEM::CPU, mMatResolDepth); - sensor_msgs::ImagePtr confMapMsg = boost::make_shared(); - - sl_tools::imageToROSmsg(confMapMsg, confMapZEDMat, mConfidenceOptFrameId, stamp); - + sl_tools::imageToROSmsg(confMapMsg, mat_conf, mConfidenceOptFrameId, stamp); mPubConfMap.publish(confMapMsg); } } -void ZEDWrapperNodelet::pubPathCallback(const ros::TimerEvent& e) { +void ZEDWrapperNodelet::callback_pubPath(const ros::TimerEvent& e) { uint32_t mapPathSub = mPubMapPath.getNumSubscribers(); uint32_t odomPathSub = mPubOdomPath.getNumSubscribers(); @@ -2579,7 +2571,7 @@ void ZEDWrapperNodelet::pubPathCallback(const ros::TimerEvent& e) { } } -void ZEDWrapperNodelet::pubSensCallback(const ros::TimerEvent& e) { +void ZEDWrapperNodelet::callback_pubSens(const ros::TimerEvent& e) { std::lock_guard lock(mCloseZedMutex); if (!mZed.isOpened()) { @@ -3169,6 +3161,21 @@ void ZEDWrapperNodelet::device_poll_thread_func() { runParams.enable_depth = false; // Ask to not compute the depth } + // ----> Wait for RGB/Depth synchronization before grabbing + std::unique_lock datalock(mCamDataMutex); + while (!mRgbDepthDataRetrieved) { // loop to avoid spurious wakeups + if (mRgbDepthDataRetrievedCondVar.wait_for(datalock, std::chrono::milliseconds(500)) == std::cv_status::timeout) { + // Check thread stopping + if (mStopNode) { + return; + } else { + continue; + } + } + } + // <---- Wait for RGB/Depth synchronization before grabbing + + mRgbDepthDataRetrieved = false; mGrabStatus = mZed.grab(runParams); std::chrono::steady_clock::time_point start_elab = std::chrono::steady_clock::now(); @@ -3368,8 +3375,6 @@ void ZEDWrapperNodelet::device_poll_thread_func() { } // <---- Camera Settings - mCamDataMutex.lock(); - // Publish the point cloud if someone has subscribed to if (cloudSubnumber > 0) { @@ -3392,7 +3397,6 @@ void ZEDWrapperNodelet::device_poll_thread_func() { } else { mPcPublishing = false; } - mCamDataMutex.unlock(); mObjDetMutex.lock(); if (mObjDetRunning) { @@ -3606,24 +3610,24 @@ void ZEDWrapperNodelet::device_poll_thread_func() { } if(mZedRealCamModel == sl::MODEL::ZED || !mPublishImuTf) { - // Publish pose tf only if enabled - if(mPublishTf) { - // Note, the frame is published, but its values will only change if - // someone has subscribed to odom - publishOdomFrame(mOdom2BaseTransf, stamp); // publish the base Frame in odometry frame - - if(mPublishMapTf) { + // Publish pose tf only if enabled + if(mPublishTf) { // Note, the frame is published, but its values will only change if - // someone has subscribed to map - publishPoseFrame(mMap2OdomTransf, stamp); // publish the odometry Frame in map frame - } + // someone has subscribed to odom + publishOdomFrame(mOdom2BaseTransf, stamp); // publish the base Frame in odometry frame + + if(mPublishMapTf) { + // Note, the frame is published, but its values will only change if + // someone has subscribed to map + publishPoseFrame(mMap2OdomTransf, stamp); // publish the odometry Frame in map frame + } - if(mPublishImuTf && !mStaticImuFramePublished ) - { - publishStaticImuFrame(); + if(mPublishImuTf && !mStaticImuFramePublished ) + { + publishStaticImuFrame(); + } } } - } #if 0 //#ifndef NDEBUG // Enable for TF checking // Double check: map_to_pose must be equal to mMap2BaseTransf @@ -3729,7 +3733,7 @@ void ZEDWrapperNodelet::device_poll_thread_func() { NODELET_DEBUG("ZED pool thread finished"); } -void ZEDWrapperNodelet::updateDiagnostic(diagnostic_updater::DiagnosticStatusWrapper& stat) { +void ZEDWrapperNodelet::callback_updateDiagnostic(diagnostic_updater::DiagnosticStatusWrapper& stat) { if (mConnStatus != sl::ERROR_CODE::SUCCESS) { stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, sl::toString(mConnStatus).c_str()); From 6d26090baefae2b3c816edfb4d76ec97f3a1755c Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Fri, 9 Oct 2020 16:32:41 +0200 Subject: [PATCH 015/199] Fix stereo couple publishing --- .../zed_nodelet/src/zed_wrapper_nodelet.cpp | 42 +++++++++---------- 1 file changed, 21 insertions(+), 21 deletions(-) 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 c944805a..727551da 100644 --- a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp @@ -2272,23 +2272,23 @@ void ZEDWrapperNodelet::callback_dynamicReconf(zed_nodelets::ZedConfig& config, void ZEDWrapperNodelet::callback_pubVideoDepth(const ros::TimerEvent& e) { static sl::Timestamp lastZedTs = 0; // Used to calculate stable publish frequency - uint32_t rgbSubnumber = mPubRgb.getNumSubscribers(); // - uint32_t rgbRawSubnumber = mPubRawRgb.getNumSubscribers(); // - uint32_t leftSubnumber = mPubLeft.getNumSubscribers(); // - uint32_t leftRawSubnumber = mPubRawLeft.getNumSubscribers(); // - uint32_t rightSubnumber = mPubRight.getNumSubscribers(); // - uint32_t rightRawSubnumber = mPubRawRight.getNumSubscribers(); // - uint32_t rgbGraySubnumber = mPubRgbGray.getNumSubscribers(); // - uint32_t rgbGrayRawSubnumber = mPubRawRgbGray.getNumSubscribers(); // - uint32_t leftGraySubnumber = mPubLeftGray.getNumSubscribers(); // - uint32_t leftGrayRawSubnumber = mPubRawLeftGray.getNumSubscribers(); // - uint32_t rightGraySubnumber = mPubRightGray.getNumSubscribers(); // - uint32_t rightGrayRawSubnumber = mPubRawRightGray.getNumSubscribers(); // - uint32_t depthSubnumber = mPubDepth.getNumSubscribers(); // - uint32_t disparitySubnumber = mPubDisparity.getNumSubscribers(); // - uint32_t confMapSubnumber = mPubConfMap.getNumSubscribers(); // - uint32_t stereoSubNumber = mPubStereo.getNumSubscribers(); // - uint32_t stereoRawSubNumber = mPubRawStereo.getNumSubscribers(); // + uint32_t rgbSubnumber = mPubRgb.getNumSubscribers(); + uint32_t rgbRawSubnumber = mPubRawRgb.getNumSubscribers(); + uint32_t leftSubnumber = mPubLeft.getNumSubscribers(); + uint32_t leftRawSubnumber = mPubRawLeft.getNumSubscribers(); + uint32_t rightSubnumber = mPubRight.getNumSubscribers(); + uint32_t rightRawSubnumber = mPubRawRight.getNumSubscribers(); + uint32_t rgbGraySubnumber = mPubRgbGray.getNumSubscribers(); + uint32_t rgbGrayRawSubnumber = mPubRawRgbGray.getNumSubscribers(); + uint32_t leftGraySubnumber = mPubLeftGray.getNumSubscribers(); + uint32_t leftGrayRawSubnumber = mPubRawLeftGray.getNumSubscribers(); + uint32_t rightGraySubnumber = mPubRightGray.getNumSubscribers(); + uint32_t rightGrayRawSubnumber = mPubRawRightGray.getNumSubscribers(); + uint32_t depthSubnumber = mPubDepth.getNumSubscribers(); + uint32_t disparitySubnumber = mPubDisparity.getNumSubscribers(); + uint32_t confMapSubnumber = mPubConfMap.getNumSubscribers(); + uint32_t stereoSubNumber = mPubStereo.getNumSubscribers(); + uint32_t stereoRawSubNumber = mPubRawStereo.getNumSubscribers(); bool retrieved = false; @@ -2306,23 +2306,23 @@ void ZEDWrapperNodelet::callback_pubVideoDepth(const ros::TimerEvent& e) { std::unique_lock lock(mCamDataMutex, std::defer_lock); if (lock.try_lock()) { - if(rgbSubnumber+leftSubnumber>0) { + if(rgbSubnumber+leftSubnumber+stereoSubNumber>0) { mZed.retrieveImage(mat_left, sl::VIEW::LEFT, sl::MEM::CPU, mMatResolVideo); retrieved = true; ts_rgb=mat_left.timestamp; grab_ts=mat_left.timestamp; } - if(rgbRawSubnumber+leftRawSubnumber>0) { + if(rgbRawSubnumber+leftRawSubnumber+stereoRawSubNumber>0) { mZed.retrieveImage(mat_left_raw, sl::VIEW::LEFT_UNRECTIFIED, sl::MEM::CPU, mMatResolVideo); retrieved = true; grab_ts=mat_left_raw.timestamp; } - if(rightSubnumber>0) { + if(rightSubnumber+stereoSubNumber>0) { mZed.retrieveImage(mat_right, sl::VIEW::RIGHT, sl::MEM::CPU, mMatResolVideo); retrieved = true; grab_ts=mat_right.timestamp; } - if(rightRawSubnumber>0) { + if(rightRawSubnumber+stereoRawSubNumber>0) { mZed.retrieveImage(mat_right_raw, sl::VIEW::RIGHT_UNRECTIFIED, sl::MEM::CPU, mMatResolVideo); retrieved = true; grab_ts=mat_right_raw.timestamp; From c4842fc87c8618c9af4bc7612846966b42190b11 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Mon, 26 Oct 2020 14:15:17 +0100 Subject: [PATCH 016/199] minor fix --- zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 727551da..ef12f99d 100644 --- a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp @@ -2825,7 +2825,7 @@ void ZEDWrapperNodelet::callback_pubSens(const ros::TimerEvent& e) { // } // } - if( (imu_SubNumber > 0 || mPublishImuTf) && new_imu_data) { + if( imu_SubNumber > 0 && new_imu_data) { lastTs_imu = ts_imu; sensor_msgs::ImuPtr imuMsg = boost::make_shared(); From 292dfab2e078e16b53b75231ed2643cc326d1235 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Tue, 27 Oct 2020 17:53:15 +0100 Subject: [PATCH 017/199] Sensor/Camera desync potentially fixed. More test and QA to do --- .../include/zed_wrapper_nodelet.hpp | 14 +- .../zed_nodelet/src/zed_wrapper_nodelet.cpp | 438 +++++++----------- zed_wrapper/params/zed2.yaml | 2 +- 3 files changed, 191 insertions(+), 263 deletions(-) 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 877e42bb..03da5dc4 100644 --- a/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp +++ b/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp @@ -199,6 +199,11 @@ class ZEDWrapperNodelet : public nodelet::Nodelet { */ void publishDisparity(sl::Mat disparity, ros::Time t); + /*! \brief Publish sensors data and TF + * \param t : the ros::Time to stamp the depth image + */ + void publishSensData(ros::Time t = ros::Time(0)); + /*! \brief Get the information of the ZED cameras and store them in an * information message * \param zed : the sl::zed::Camera* pointer to an instance @@ -246,7 +251,7 @@ class ZEDWrapperNodelet : public nodelet::Nodelet { /*! \brief Callback to publish Sensor Data with a ROS publisher. * \param e : the ros::TimerEvent binded to the callback */ - void callback_pubSens(const ros::TimerEvent& e); + void callback_pubSensorsData(const ros::TimerEvent& e); /*! \brief Callback to update node diagnostic status * \param stat : node status @@ -396,7 +401,9 @@ class ZEDWrapperNodelet : public nodelet::Nodelet { std::thread mDevicePollThread; std::thread mPcThread; // Point Cloud thread - bool mStopNode; + bool mStopNode = false; + + const double mSensPubRate = 400.0; // Publishers image_transport::CameraPublisher mPubRgb; // @@ -522,8 +529,7 @@ class ZEDWrapperNodelet : public nodelet::Nodelet { int mDepthStabilization; std::string mAreaMemDbPath; std::string mSvoFilepath; - std::string mRemoteStreamAddr; - double mSensPubRate = 400.0; + std::string mRemoteStreamAddr; bool mSensTimestampSync; double mPathPubRate; int mPathMaxCount; 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 ef12f99d..897b509e 100644 --- a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp @@ -482,47 +482,29 @@ void ZEDWrapperNodelet::onInit() { } // Sensor publishers - /*if (!mSvoMode)*/ { - if (mSensPubRate > 0 && mZedRealCamModel != sl::MODEL::ZED) { - // IMU Publishers - mPubImu = mNhNs.advertise(imu_topic, 1/*static_cast(mSensPubRate)*/); - NODELET_INFO_STREAM("Advertised on topic " << mPubImu.getTopic() << " @ " - << mSensPubRate << " Hz"); - mPubImuRaw = mNhNs.advertise(imu_topic_raw, 1/*static_cast(mSensPubRate)*/); - NODELET_INFO_STREAM("Advertised on topic " << mPubImuRaw.getTopic() << " @ " - << mSensPubRate << " Hz"); - mPubImuMag = mNhNs.advertise(imu_mag_topic, 1/*MAG_FREQ*/); - NODELET_INFO_STREAM("Advertised on topic " << mPubImuMag.getTopic() << " @ " - << std::min(MAG_FREQ,mSensPubRate) << " Hz"); - //mPubImuMagRaw = mNhNs.advertise(imu_mag_topic_raw, static_cast(MAG_FREQ)); - //NODELET_INFO_STREAM("Advertised on topic " << mPubImuMagRaw.getTopic() << " @ " - // << std::min(MAG_FREQ,mSensPubRate) << " Hz"); - - if( mZedRealCamModel == sl::MODEL::ZED2 ) { - // IMU temperature sensor - mPubImuTemp = mNhNs.advertise(imu_temp_topic, 1/*static_cast(mSensPubRate)*/); - NODELET_INFO_STREAM("Advertised on topic " << mPubImuTemp.getTopic() << " @ " << mSensPubRate << " Hz"); - - // Atmospheric pressure - mPubPressure = mNhNs.advertise(pressure_topic, 1/*static_cast(BARO_FREQ)*/); - NODELET_INFO_STREAM("Advertised on topic " << mPubPressure.getTopic() << " @ " - << std::min(BARO_FREQ,mSensPubRate ) << " Hz"); - - // CMOS sensor temperatures - mPubTempL = mNhNs.advertise(temp_topic_left, 1/*static_cast(BARO_FREQ)*/); - NODELET_INFO_STREAM("Advertised on topic " << mPubTempL.getTopic() << " @ " - << std::min(BARO_FREQ,mSensPubRate ) << " Hz"); - mPubTempR = mNhNs.advertise(temp_topic_right, 1/*static_cast(BARO_FREQ)*/); - NODELET_INFO_STREAM("Advertised on topic " << mPubTempR.getTopic() << " @ " - << std::min(BARO_FREQ,mSensPubRate ) << " Hz"); - } - - mFrameTimestamp = ros::Time::now(); - mImuTimer = mNhNs.createTimer(ros::Duration(1.0 / (mSensPubRate*1.5) ), - &ZEDWrapperNodelet::callback_pubSens, this); - mSensPeriodMean_usec.reset(new sl_tools::CSmartMean(mSensPubRate / 2)); - - + if (mZedRealCamModel != sl::MODEL::ZED) { + // IMU Publishers + mPubImu = mNhNs.advertise(imu_topic, 1/*static_cast(mSensPubRate)*/); + NODELET_INFO_STREAM("Advertised on topic " << mPubImu.getTopic() ); + mPubImuRaw = mNhNs.advertise(imu_topic_raw, 1/*static_cast(mSensPubRate)*/); + NODELET_INFO_STREAM("Advertised on topic " << mPubImuRaw.getTopic() ); + mPubImuMag = mNhNs.advertise(imu_mag_topic, 1/*MAG_FREQ*/); + NODELET_INFO_STREAM("Advertised on topic " << mPubImuMag.getTopic() ); + + if( mZedRealCamModel == sl::MODEL::ZED2 ) { + // IMU temperature sensor + mPubImuTemp = mNhNs.advertise(imu_temp_topic, 1/*static_cast(mSensPubRate)*/); + NODELET_INFO_STREAM("Advertised on topic " << mPubImuTemp.getTopic() ); + + // Atmospheric pressure + mPubPressure = mNhNs.advertise(pressure_topic, 1/*static_cast(BARO_FREQ)*/); + NODELET_INFO_STREAM("Advertised on topic " << mPubPressure.getTopic() ); + + // CMOS sensor temperatures + mPubTempL = mNhNs.advertise(temp_topic_left, 1/*static_cast(BARO_FREQ)*/); + NODELET_INFO_STREAM("Advertised on topic " << mPubTempL.getTopic() ); + mPubTempR = mNhNs.advertise(temp_topic_right, 1/*static_cast(BARO_FREQ)*/); + NODELET_INFO_STREAM("Advertised on topic " << mPubTempR.getTopic() ); } // Publish camera imu transform in a latched topic @@ -551,6 +533,15 @@ void ZEDWrapperNodelet::onInit() { NODELET_INFO_STREAM("Advertised on topic " << mPubCamImuTransf.getTopic() << " [LATCHED]"); } + + if(!mSvoMode) { + mFrameTimestamp = ros::Time::now(); + mImuTimer = mNhNs.createTimer(ros::Duration(1.0 / (mSensPubRate*1.5) ), + &ZEDWrapperNodelet::callback_pubSensorsData, this); + mSensPeriodMean_usec.reset(new sl_tools::CSmartMean(mSensPubRate / 2)); + } else { + mSensPeriodMean_usec.reset(new sl_tools::CSmartMean(mCamFrameRate / 2)); + } } // Services @@ -2302,78 +2293,96 @@ void ZEDWrapperNodelet::callback_pubVideoDepth(const ros::TimerEvent& e) { sl::Timestamp ts_depth=0; // used to check RGB/Depth sync sl::Timestamp grab_ts=0; - // ----> Retrieve all required data - std::unique_lock lock(mCamDataMutex, std::defer_lock); - - if (lock.try_lock()) { - if(rgbSubnumber+leftSubnumber+stereoSubNumber>0) { - mZed.retrieveImage(mat_left, sl::VIEW::LEFT, sl::MEM::CPU, mMatResolVideo); - retrieved = true; - ts_rgb=mat_left.timestamp; - grab_ts=mat_left.timestamp; - } - if(rgbRawSubnumber+leftRawSubnumber+stereoRawSubNumber>0) { - mZed.retrieveImage(mat_left_raw, sl::VIEW::LEFT_UNRECTIFIED, sl::MEM::CPU, mMatResolVideo); - retrieved = true; - grab_ts=mat_left_raw.timestamp; - } - if(rightSubnumber+stereoSubNumber>0) { - mZed.retrieveImage(mat_right, sl::VIEW::RIGHT, sl::MEM::CPU, mMatResolVideo); - retrieved = true; - grab_ts=mat_right.timestamp; - } - if(rightRawSubnumber+stereoRawSubNumber>0) { - mZed.retrieveImage(mat_right_raw, sl::VIEW::RIGHT_UNRECTIFIED, sl::MEM::CPU, mMatResolVideo); - retrieved = true; - grab_ts=mat_right_raw.timestamp; + mCamDataMutex.lock(); + + // ----> Retrieve all required image data + if(rgbSubnumber+leftSubnumber+stereoSubNumber>0) { + mZed.retrieveImage(mat_left, sl::VIEW::LEFT, sl::MEM::CPU, mMatResolVideo); + retrieved = true; + ts_rgb=mat_left.timestamp; + grab_ts=mat_left.timestamp; + } + if(rgbRawSubnumber+leftRawSubnumber+stereoRawSubNumber>0) { + mZed.retrieveImage(mat_left_raw, sl::VIEW::LEFT_UNRECTIFIED, sl::MEM::CPU, mMatResolVideo); + retrieved = true; + grab_ts=mat_left_raw.timestamp; + } + if(rightSubnumber+stereoSubNumber>0) { + mZed.retrieveImage(mat_right, sl::VIEW::RIGHT, sl::MEM::CPU, mMatResolVideo); + retrieved = true; + grab_ts=mat_right.timestamp; + } + if(rightRawSubnumber+stereoRawSubNumber>0) { + mZed.retrieveImage(mat_right_raw, sl::VIEW::RIGHT_UNRECTIFIED, sl::MEM::CPU, mMatResolVideo); + retrieved = true; + grab_ts=mat_right_raw.timestamp; + } + if(rgbGraySubnumber+leftGraySubnumber>0) { + mZed.retrieveImage(mat_left_gray, sl::VIEW::LEFT_GRAY, sl::MEM::CPU, mMatResolVideo); + retrieved = true; + grab_ts=mat_left_gray.timestamp; + } + if(rgbGrayRawSubnumber+leftGrayRawSubnumber>0) { + mZed.retrieveImage(mat_left_raw_gray, sl::VIEW::LEFT_UNRECTIFIED_GRAY, sl::MEM::CPU, mMatResolVideo); + retrieved = true; + grab_ts=mat_left_raw_gray.timestamp; + } + if(rightGraySubnumber>0) { + mZed.retrieveImage(mat_right_gray, sl::VIEW::RIGHT_GRAY, sl::MEM::CPU, mMatResolVideo); + retrieved = true; + grab_ts=mat_right_gray.timestamp; + } + if(rightGrayRawSubnumber>0) { + mZed.retrieveImage(mat_right_raw_gray, sl::VIEW::RIGHT_UNRECTIFIED_GRAY, sl::MEM::CPU, mMatResolVideo); + retrieved = true; + grab_ts=mat_right_raw_gray.timestamp; + } + if(depthSubnumber>0) { + mZed.retrieveMeasure(mat_depth, sl::MEASURE::DEPTH, sl::MEM::CPU, mMatResolDepth); + retrieved = true; + grab_ts=mat_depth.timestamp; + + ts_depth = mat_depth.timestamp; + + if( ts_rgb.data_ns!=0 && (ts_depth.data_ns!=ts_rgb.data_ns) ) { + NODELET_WARN_STREAM( "!!!!! DEPTH/RGB ASYNC !!!!! - Delta: " << 1e-9*static_cast(ts_depth-ts_rgb) << " sec"); } - if(rgbGraySubnumber+leftGraySubnumber>0) { - mZed.retrieveImage(mat_left_gray, sl::VIEW::LEFT_GRAY, sl::MEM::CPU, mMatResolVideo); - retrieved = true; - grab_ts=mat_left_gray.timestamp; - } - if(rgbGrayRawSubnumber+leftGrayRawSubnumber>0) { - mZed.retrieveImage(mat_left_raw_gray, sl::VIEW::LEFT_UNRECTIFIED_GRAY, sl::MEM::CPU, mMatResolVideo); - retrieved = true; - grab_ts=mat_left_raw_gray.timestamp; - } - if(rightGraySubnumber>0) { - mZed.retrieveImage(mat_right_gray, sl::VIEW::RIGHT_GRAY, sl::MEM::CPU, mMatResolVideo); - retrieved = true; - grab_ts=mat_right_gray.timestamp; - } - if(rightGrayRawSubnumber>0) { - mZed.retrieveImage(mat_right_raw_gray, sl::VIEW::RIGHT_UNRECTIFIED_GRAY, sl::MEM::CPU, mMatResolVideo); - retrieved = true; - grab_ts=mat_right_raw_gray.timestamp; - } - if(depthSubnumber>0) { - mZed.retrieveMeasure(mat_depth, sl::MEASURE::DEPTH, sl::MEM::CPU, mMatResolDepth); - retrieved = true; - grab_ts=mat_depth.timestamp; + } + if(disparitySubnumber>0) { + mZed.retrieveMeasure(mat_disp, sl::MEASURE::DISPARITY, sl::MEM::CPU, mMatResolDepth); + retrieved = true; + grab_ts=mat_disp.timestamp; + } + if(confMapSubnumber) { + mZed.retrieveMeasure(mat_conf, sl::MEASURE::CONFIDENCE, sl::MEM::CPU, mMatResolDepth); + retrieved = true; + grab_ts=mat_conf.timestamp; + } + // <---- Retrieve all required image data - ts_depth = mat_depth.timestamp; + // ----> Data ROS timestamp + ros::Time stamp = sl_tools::slTime2Ros(grab_ts); + if(mSvoMode) { + stamp = ros::Time::now(); + } + // <---- Data ROS timestamp - if( ts_rgb.data_ns!=0 && (ts_depth.data_ns!=ts_rgb.data_ns) ) { - NODELET_WARN_STREAM( "!!!!! DEPTH/RGB ASYNC !!!!! - Delta: " << 1e-9*static_cast(ts_depth-ts_rgb) << " sec"); + // ----> Publish sensor data if sync is required by user or SVO + if( mZedRealCamModel!=sl::MODEL::ZED ) + { + if(mSensTimestampSync || mSvoMode) { + if(retrieved) { + publishSensData(stamp); } } - if(disparitySubnumber>0) { - mZed.retrieveMeasure(mat_disp, sl::MEASURE::DISPARITY, sl::MEM::CPU, mMatResolDepth); - retrieved = true; - grab_ts=mat_disp.timestamp; - } - if(confMapSubnumber) { - mZed.retrieveMeasure(mat_conf, sl::MEASURE::CONFIDENCE, sl::MEM::CPU, mMatResolDepth); - retrieved = true; - grab_ts=mat_conf.timestamp; - } } - // <---- Retrieve all required data + // <---- Publish sensor data if sync is required by user or SVO + + mCamDataMutex.unlock(); // ----> Notify grab thread that all data are synchronized and a new grab can be done - mRgbDepthDataRetrievedCondVar.notify_one(); - mRgbDepthDataRetrieved = true; + //mRgbDepthDataRetrievedCondVar.notify_one(); + //mRgbDepthDataRetrieved = true; // <---- Notify grab thread that all data are synchronized and a new grab can be done if(!retrieved) { @@ -2398,9 +2407,6 @@ void ZEDWrapperNodelet::callback_pubVideoDepth(const ros::TimerEvent& e) { lastZedTs = grab_ts; // <---- Check if a grab has been done before publishing the same images - ros::Time stamp = sl_tools::slTime2Ros(grab_ts); - - // Publish the left = rgb image if someone has subscribed to if (leftSubnumber > 0) { sensor_msgs::ImagePtr leftImgMsg = boost::make_shared(); @@ -2571,18 +2577,24 @@ void ZEDWrapperNodelet::callback_pubPath(const ros::TimerEvent& e) { } } -void ZEDWrapperNodelet::callback_pubSens(const ros::TimerEvent& e) { +void ZEDWrapperNodelet::callback_pubSensorsData(const ros::TimerEvent& e) { + NODELET_DEBUG("callback_pubSensorsData"); std::lock_guard lock(mCloseZedMutex); if (!mZed.isOpened()) { return; } + publishSensData(); +} + +void ZEDWrapperNodelet::publishSensData(ros::Time t) { + NODELET_DEBUG("publishSensData"); + uint32_t imu_SubNumber = mPubImu.getNumSubscribers(); uint32_t imu_RawSubNumber = mPubImuRaw.getNumSubscribers(); uint32_t imu_TempSubNumber = 0; uint32_t imu_MagSubNumber = 0; - //uint32_t imu_MagRawSubNumber = 0; uint32_t pressSubNumber = 0; uint32_t tempLeftSubNumber = 0; uint32_t tempRightSubNumber = 0; @@ -2590,66 +2602,73 @@ void ZEDWrapperNodelet::callback_pubSens(const ros::TimerEvent& e) { if( mZedRealCamModel == sl::MODEL::ZED2 ) { imu_TempSubNumber = mPubImuTemp.getNumSubscribers(); imu_MagSubNumber = mPubImuMag.getNumSubscribers(); - //imu_MagRawSubNumber = mPubImuMagRaw.getNumSubscribers(); pressSubNumber = mPubPressure.getNumSubscribers(); tempLeftSubNumber = mPubTempL.getNumSubscribers(); tempRightSubNumber = mPubTempR.getNumSubscribers(); } - int totSub = imu_SubNumber + imu_RawSubNumber + imu_TempSubNumber + imu_MagSubNumber + /*imu_MagRawSubNumber +*/ - pressSubNumber + tempLeftSubNumber + tempRightSubNumber; - ros::Time ts_imu; ros::Time ts_baro; ros::Time ts_mag; - //ros::Time ts_mag_raw; static ros::Time lastTs_imu = ros::Time(); static ros::Time lastTs_baro = ros::Time(); static ros::Time lastT_mag = ros::Time(); - //static ros::Time lastT_mag_raw = ros::Time(); sl::SensorsData sens_data; - if(mSvoMode) { - if( mZed.getSensorsData(sens_data, sl::TIME_REFERENCE::IMAGE) != sl::ERROR_CODE::SUCCESS ) + if(mSvoMode || mSensTimestampSync) { + if( mZed.getSensorsData(sens_data, sl::TIME_REFERENCE::IMAGE) != sl::ERROR_CODE::SUCCESS ) { + NODELET_DEBUG("Not retrieved sensors data in IMAGE REFERENCE TIME"); return; + } } else { - if ( mSensTimestampSync && mGrabActive) { - if( mZed.getSensorsData(sens_data, sl::TIME_REFERENCE::IMAGE) != sl::ERROR_CODE::SUCCESS ) - return; - } else if ( !mSensTimestampSync ) { - if( mZed.getSensorsData(sens_data, sl::TIME_REFERENCE::CURRENT) != sl::ERROR_CODE::SUCCESS ) - return; - } else { + if( mZed.getSensorsData(sens_data, sl::TIME_REFERENCE::CURRENT) != sl::ERROR_CODE::SUCCESS ) { + NODELET_DEBUG("Not retrieved sensors data in CURRENT REFERENCE TIME"); return; } } - if (mSvoMode) { + if(mSvoMode) { ts_imu = ros::Time::now(); ts_baro = ros::Time::now(); ts_mag = ros::Time::now(); //ts_mag_raw = ros::Time::now(); } else { - if (mSensTimestampSync && mGrabActive) { - ts_imu = mFrameTimestamp; - ts_baro = mFrameTimestamp; - ts_mag = mFrameTimestamp; - //ts_mag_raw = mFrameTimestamp; + if(t!=ros::Time(0)) { + ts_imu = t; + ts_baro = t; + ts_mag = t; } else { ts_imu = sl_tools::slTime2Ros(sens_data.imu.timestamp); ts_baro = sl_tools::slTime2Ros(sens_data.barometer.timestamp); ts_mag = sl_tools::slTime2Ros(sens_data.magnetometer.timestamp); - //ts_mag_raw = sl_tools::slTime2Ros(sens_data.magnetometer.timestamp); } } + // ----> Publish odometry tf only if enabled + if (mPublishTf && mTrackingReady) { + NODELET_DEBUG("Publishing TF"); + + publishOdomFrame(mOdom2BaseTransf, ts_imu); // publish the base Frame in odometry frame + + if (mPublishMapTf) { + publishPoseFrame(mMap2OdomTransf, ts_imu); // publish the odometry Frame in map frame + } + + if(mPublishImuTf && !mStaticImuFramePublished ) + { + publishStaticImuFrame(); + } + } + // <---- Publish odometry tf only if enabled + bool new_imu_data = ts_imu!=lastTs_imu; bool new_baro_data = ts_baro!=lastTs_baro; bool new_mag_data = ts_mag!=lastT_mag; if( !new_imu_data && !new_baro_data && !new_mag_data) { + NODELET_DEBUG("No updated sensors data"); return; } @@ -2659,13 +2678,9 @@ void ZEDWrapperNodelet::callback_pubSens(const ros::TimerEvent& e) { sens_data.temperature.get( sl::SensorsData::TemperatureData::SENSOR_LOCATION::ONBOARD_RIGHT, mTempRight); } - if(totSub<1 && !mPublishImuTf) { - return; // Nothing to publish - } - if( imu_SubNumber > 0 || imu_RawSubNumber > 0 || imu_TempSubNumber > 0 || pressSubNumber > 0 || - imu_MagSubNumber > 0 /*|| imu_MagRawSubNumber > 0*/ ) { + imu_MagSubNumber > 0 ) { // Publish freq calculation static std::chrono::steady_clock::time_point last_time = std::chrono::steady_clock::now(); std::chrono::steady_clock::time_point now = std::chrono::steady_clock::now(); @@ -2700,8 +2715,11 @@ void ZEDWrapperNodelet::callback_pubSens(const ros::TimerEvent& e) { imuTempMsg->variance = 0.0; mPubImuTemp.publish(imuTempMsg); + } else { + NODELET_DEBUG("No new IMU temp."); } + if( sens_data.barometer.is_available && new_baro_data ) { lastTs_baro = ts_baro; @@ -2763,6 +2781,8 @@ void ZEDWrapperNodelet::callback_pubSens(const ros::TimerEvent& e) { mPubTempR.publish(tempRightMsg); } + } else { + NODELET_DEBUG("No new BAROM. DATA"); } if( imu_MagSubNumber>0) { @@ -2797,34 +2817,10 @@ void ZEDWrapperNodelet::callback_pubSens(const ros::TimerEvent& e) { mPubImuMag.publish(magMsg); } + } else { + NODELET_DEBUG("No new MAG. DATA"); } - // if( imu_MagRawSubNumber>0 ) { - // if( sens_data.magnetometer.is_available && lastT_mag_raw != ts_mag_raw ) { - // lastT_mag_raw = ts_mag_raw; - - // sensor_msgs::MagneticFieldPtr mMagRawMsg = boost::make_shared(); - - - // mMagRawMsg->header.stamp = ts_mag; - // mMagRawMsg->header.frame_id = mImuFrameId; - // mMagRawMsg->magnetic_field.x = sens_data.magnetometer.magnetic_field_uncalibrated.x*1e-6; // Tesla - // mMagRawMsg->magnetic_field.y = sens_data.magnetometer.magnetic_field_uncalibrated.y*1e-6; // Tesla - // mMagRawMsg->magnetic_field.z = sens_data.magnetometer.magnetic_field_uncalibrated.z*1e-6; // Tesla - // mMagRawMsg->magnetic_field_covariance[0] = 0.039e-6; - // mMagRawMsg->magnetic_field_covariance[1] = 0.0f; - // mMagRawMsg->magnetic_field_covariance[2] = 0.0f; - // mMagRawMsg->magnetic_field_covariance[3] = 0.0f; - // mMagRawMsg->magnetic_field_covariance[4] = 0.037e-6; - // mMagRawMsg->magnetic_field_covariance[5] = 0.0f; - // mMagRawMsg->magnetic_field_covariance[6] = 0.0f; - // mMagRawMsg->magnetic_field_covariance[7] = 0.0f; - // mMagRawMsg->magnetic_field_covariance[8] = 0.047e-6; - - // mPubImuMagRaw.publish(mMagRawMsg); - // } - // } - if( imu_SubNumber > 0 && new_imu_data) { lastTs_imu = ts_imu; @@ -2892,9 +2888,9 @@ void ZEDWrapperNodelet::callback_pubSens(const ros::TimerEvent& e) { sens_data.imu.angular_velocity_covariance.r[r * 3 + 2] * DEG2RAD * DEG2RAD; } - if(imu_SubNumber > 0) { - mPubImu.publish(imuMsg); - } + mPubImu.publish(imuMsg); + } else { + NODELET_DEBUG("No new IMU DATA"); } if (imu_RawSubNumber > 0 && new_imu_data) { @@ -2935,85 +2931,14 @@ void ZEDWrapperNodelet::callback_pubSens(const ros::TimerEvent& e) { sens_data.imu.angular_velocity_covariance.r[r * 3 + 2] * DEG2RAD * DEG2RAD; } - imuRawMsg->orientation_covariance[0] = - -1; // Orientation data is not available in "data_raw" -> See ROS REP145 + // Orientation data is not available in "data_raw" -> See ROS REP145 // http://www.ros.org/reps/rep-0145.html#topics + imuRawMsg->orientation_covariance[0] = + -1; mPubImuRaw.publish(imuRawMsg); } - - if(new_imu_data && mPublishImuTf) { - // Publish odometry tf only if enabled - if (mPublishTf) { - if(!mTrackingReady) { - return; - } - - publishOdomFrame(mOdom2BaseTransf, ts_imu); // publish the base Frame in odometry frame - - if (mPublishMapTf) { - publishPoseFrame(mMap2OdomTransf, ts_imu); // publish the odometry Frame in map frame - } - - if(mPublishImuTf && !mStaticImuFramePublished ) - { - publishStaticImuFrame(); - } - } - } - - // // Publish IMU tf - // // Left camera to odom transform from TF buffer - // tf2::Transform cam_to_odom; - - // //std::string poseFrame; - // static bool first_error = false; - - // // Look up the transformation from imu frame to odom link - // try { - // // Save the transformation from base to frame - // geometry_msgs::TransformStamped c2o = - // mTfBuffer->lookupTransform(mOdometryFrameId, mCameraFrameId, ros::Time(0), ros::Duration(0.1)); - // // Get the TF2 transformation - // tf2::fromMsg(c2o.transform, cam_to_odom); - // } catch (tf2::TransformException& ex) { - // if(!first_error) { - // NODELET_DEBUG_THROTTLE(1.0, "Transform error: %s", ex.what()); - // NODELET_WARN_THROTTLE(1.0, "The tf from '%s' to '%s' is not available.", - // mCameraFrameId.c_str(), mMapFrameId.c_str()); - // NODELET_WARN_THROTTLE(1.0, "Note: one of the possible cause of the problem is the absense of a publisher " - // "of the base_link -> odom transform. " - // "This happens if `publish_tf` is `false` and no other nodes publish the " - // "TF chain '%s' -> '%s' -> '%s'", - // mOdometryFrameId.c_str(), mBaseFrameId.c_str(), mCameraFrameId.c_str()); - // first_error=false; - // } - - // return; - // } - - // // ----> Update IMU pose for TF - - // // IMU Quaternion in Map frame - // tf2::Quaternion imu_q; - // imu_q.setX(sens_data.imu.pose.getOrientation()[0]); - // imu_q.setY(sens_data.imu.pose.getOrientation()[1]); - // imu_q.setZ(sens_data.imu.pose.getOrientation()[2]); - // imu_q.setW(sens_data.imu.pose.getOrientation()[3]); - - // // Pose Quaternion from ZED Camera - // tf2::Quaternion odom_q = cam_to_odom.getRotation(); - // // Difference between IMU and ZED Quaternion - // tf2::Quaternion delta_q = imu_q * odom_q.inverse(); - - // mLastImuPose.setIdentity(); - // mLastImuPose.setRotation(delta_q); - - // publishImuFrame(mLastImuPose, ts_imu); - // // <---- Update IMU pose for TF - // } } - void ZEDWrapperNodelet::device_poll_thread_func() { ros::Rate loop_rate(mCamFrameRate); @@ -3162,21 +3087,23 @@ void ZEDWrapperNodelet::device_poll_thread_func() { } // ----> Wait for RGB/Depth synchronization before grabbing - std::unique_lock datalock(mCamDataMutex); - while (!mRgbDepthDataRetrieved) { // loop to avoid spurious wakeups - if (mRgbDepthDataRetrievedCondVar.wait_for(datalock, std::chrono::milliseconds(500)) == std::cv_status::timeout) { - // Check thread stopping - if (mStopNode) { - return; - } else { - continue; - } - } - } + // std::unique_lock datalock(mCamDataMutex); + // while (!mRgbDepthDataRetrieved) { // loop to avoid spurious wakeups + // if (mRgbDepthDataRetrievedCondVar.wait_for(datalock, std::chrono::milliseconds(500)) == std::cv_status::timeout) { + // // Check thread stopping + // if (mStopNode) { + // return; + // } else { + // continue; + // } + // } + // } // <---- Wait for RGB/Depth synchronization before grabbing + mCamDataMutex.lock(); mRgbDepthDataRetrieved = false; mGrabStatus = mZed.grab(runParams); + mCamDataMutex.unlock(); std::chrono::steady_clock::time_point start_elab = std::chrono::steady_clock::now(); @@ -3609,7 +3536,7 @@ void ZEDWrapperNodelet::device_poll_thread_func() { oldStatus = mPosTrackingStatus; } - if(mZedRealCamModel == sl::MODEL::ZED || !mPublishImuTf) { + if(mZedRealCamModel == sl::MODEL::ZED) { // Publish pose tf only if enabled if(mPublishTf) { // Note, the frame is published, but its values will only change if @@ -3621,11 +3548,6 @@ void ZEDWrapperNodelet::device_poll_thread_func() { // someone has subscribed to map publishPoseFrame(mMap2OdomTransf, stamp); // publish the odometry Frame in map frame } - - if(mPublishImuTf && !mStaticImuFramePublished ) - { - publishStaticImuFrame(); - } } } @@ -3817,7 +3739,7 @@ void ZEDWrapperNodelet::callback_updateDiagnostic(diagnostic_updater::Diagnostic stat.add("IMU", "Topics not subscribed"); } - if( mSensPubRate > 0 && mZedRealCamModel == sl::MODEL::ZED2 ) { + if( mZedRealCamModel == sl::MODEL::ZED2 ) { stat.addf("Left CMOS Temp.", "%.1f °C", mTempLeft); stat.addf("Right CMOS Temp.", "%.1f °C", mTempRight); diff --git a/zed_wrapper/params/zed2.yaml b/zed_wrapper/params/zed2.yaml index 2122cde3..816a71d4 100644 --- a/zed_wrapper/params/zed2.yaml +++ b/zed_wrapper/params/zed2.yaml @@ -13,7 +13,7 @@ pos_tracking: imu_fusion: true # enable/disable IMU fusion. When set to false, only the optical odometry will be used. sensors: - sensors_timestamp_sync: false # Synchronize Sensors messages timestamp with latest received frame + sensors_timestamp_sync: true # Synchronize Sensors messages timestamp with latest received frame publish_imu_tf: true # publish `IMU -> _left_camera_frame` TF object_detection: From 7a761bb4207798b637101421a8c3b12508570596 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Wed, 28 Oct 2020 11:16:02 +0100 Subject: [PATCH 018/199] Fix Sensors data freq diagnostic info --- .../zed_nodelet/src/zed_wrapper_nodelet.cpp | 48 ++++++++++++------- zed_wrapper/params/zed2.yaml | 2 +- 2 files changed, 32 insertions(+), 18 deletions(-) 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 897b509e..571f75d5 100644 --- a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp @@ -2607,6 +2607,17 @@ void ZEDWrapperNodelet::publishSensData(ros::Time t) { tempRightSubNumber = mPubTempR.getNumSubscribers(); } + uint32_t tot_sub = imu_SubNumber+imu_RawSubNumber+imu_TempSubNumber+imu_MagSubNumber+pressSubNumber+ + tempLeftSubNumber+tempRightSubNumber; + + if(tot_sub>0) { + mSensPublishing=true; + } else { + mSensPublishing=false; + } + + bool sensors_data_published = false; + ros::Time ts_imu; ros::Time ts_baro; ros::Time ts_mag; @@ -2678,23 +2689,6 @@ void ZEDWrapperNodelet::publishSensData(ros::Time t) { sens_data.temperature.get( sl::SensorsData::TemperatureData::SENSOR_LOCATION::ONBOARD_RIGHT, mTempRight); } - if( imu_SubNumber > 0 || imu_RawSubNumber > 0 || - imu_TempSubNumber > 0 || pressSubNumber > 0 || - imu_MagSubNumber > 0 ) { - // Publish freq calculation - static std::chrono::steady_clock::time_point last_time = std::chrono::steady_clock::now(); - std::chrono::steady_clock::time_point now = std::chrono::steady_clock::now(); - - double elapsed_usec = std::chrono::duration_cast(now - last_time).count(); - last_time = now; - - mSensPeriodMean_usec->addValue(elapsed_usec); - - mSensPublishing = true; - } else { - mSensPublishing = false; - } - if (imu_TempSubNumber>0) { sensor_msgs::TemperaturePtr imuTempMsg = boost::make_shared(); @@ -2714,6 +2708,7 @@ void ZEDWrapperNodelet::publishSensData(ros::Time t) { imuTempMsg->temperature = static_cast(imu_temp); imuTempMsg->variance = 0.0; + sensors_data_published = true; mPubImuTemp.publish(imuTempMsg); } else { NODELET_DEBUG("No new IMU temp."); @@ -2739,6 +2734,7 @@ void ZEDWrapperNodelet::publishSensData(ros::Time t) { pressMsg->fluid_pressure = sens_data.barometer.pressure * 1e-2; // Pascal pressMsg->variance = 1.0585e-2; + sensors_data_published = true; mPubPressure.publish(pressMsg); } @@ -2759,6 +2755,7 @@ void ZEDWrapperNodelet::publishSensData(ros::Time t) { tempLeftMsg->temperature = static_cast(mTempLeft); tempLeftMsg->variance = 0.0; + sensors_data_published = true; mPubTempL.publish(tempLeftMsg); } @@ -2779,6 +2776,7 @@ void ZEDWrapperNodelet::publishSensData(ros::Time t) { tempRightMsg->temperature = static_cast(mTempRight); tempRightMsg->variance = 0.0; + sensors_data_published = true; mPubTempR.publish(tempRightMsg); } } else { @@ -2815,6 +2813,7 @@ void ZEDWrapperNodelet::publishSensData(ros::Time t) { magMsg->magnetic_field_covariance[7] = 0.0f; magMsg->magnetic_field_covariance[8] = 0.047e-6; + sensors_data_published = true; mPubImuMag.publish(magMsg); } } else { @@ -2888,6 +2887,7 @@ void ZEDWrapperNodelet::publishSensData(ros::Time t) { sens_data.imu.angular_velocity_covariance.r[r * 3 + 2] * DEG2RAD * DEG2RAD; } + sensors_data_published = true; mPubImu.publish(imuMsg); } else { NODELET_DEBUG("No new IMU DATA"); @@ -2935,8 +2935,22 @@ void ZEDWrapperNodelet::publishSensData(ros::Time t) { // http://www.ros.org/reps/rep-0145.html#topics imuRawMsg->orientation_covariance[0] = -1; + sensors_data_published = true; mPubImuRaw.publish(imuRawMsg); } + + // ----> Update Diagnostic + if( sensors_data_published ) { + // Publish freq calculation + static std::chrono::steady_clock::time_point last_time = std::chrono::steady_clock::now(); + std::chrono::steady_clock::time_point now = std::chrono::steady_clock::now(); + + double elapsed_usec = std::chrono::duration_cast(now - last_time).count(); + last_time = now; + + mSensPeriodMean_usec->addValue(elapsed_usec); + } + // <---- Update Diagnostic } void ZEDWrapperNodelet::device_poll_thread_func() { diff --git a/zed_wrapper/params/zed2.yaml b/zed_wrapper/params/zed2.yaml index 816a71d4..2122cde3 100644 --- a/zed_wrapper/params/zed2.yaml +++ b/zed_wrapper/params/zed2.yaml @@ -13,7 +13,7 @@ pos_tracking: imu_fusion: true # enable/disable IMU fusion. When set to false, only the optical odometry will be used. sensors: - sensors_timestamp_sync: true # Synchronize Sensors messages timestamp with latest received frame + sensors_timestamp_sync: false # Synchronize Sensors messages timestamp with latest received frame publish_imu_tf: true # publish `IMU -> _left_camera_frame` TF object_detection: From 170723d9c8240af5652ab59311b409933e3fbee4 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Wed, 28 Oct 2020 11:42:42 +0100 Subject: [PATCH 019/199] Fix Sensors pub freq when Camera/Sensors sync is active --- .../src/zed_nodelet/src/zed_wrapper_nodelet.cpp | 17 ++++++++++++----- 1 file changed, 12 insertions(+), 5 deletions(-) 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 571f75d5..e1c013db 100644 --- a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp @@ -534,7 +534,7 @@ void ZEDWrapperNodelet::onInit() { NODELET_INFO_STREAM("Advertised on topic " << mPubCamImuTransf.getTopic() << " [LATCHED]"); } - if(!mSvoMode) { + if(!mSvoMode&&!mSensTimestampSync) { mFrameTimestamp = ros::Time::now(); mImuTimer = mNhNs.createTimer(ros::Duration(1.0 / (mSensPubRate*1.5) ), &ZEDWrapperNodelet::callback_pubSensorsData, this); @@ -2281,6 +2281,11 @@ void ZEDWrapperNodelet::callback_pubVideoDepth(const ros::TimerEvent& e) { uint32_t stereoSubNumber = mPubStereo.getNumSubscribers(); uint32_t stereoRawSubNumber = mPubRawStereo.getNumSubscribers(); + uint32_t tot_sub = rgbSubnumber+rgbRawSubnumber+leftSubnumber+leftRawSubnumber+rightSubnumber+rightRawSubnumber+ + rgbGraySubnumber+rgbGrayRawSubnumber+leftGraySubnumber+leftGrayRawSubnumber+ + rightGraySubnumber+rightGrayRawSubnumber+depthSubnumber+disparitySubnumber+confMapSubnumber+ + stereoSubNumber+stereoRawSubNumber; + bool retrieved = false; sl::Mat mat_left,mat_left_raw; @@ -2353,7 +2358,7 @@ void ZEDWrapperNodelet::callback_pubVideoDepth(const ros::TimerEvent& e) { retrieved = true; grab_ts=mat_disp.timestamp; } - if(confMapSubnumber) { + if(confMapSubnumber>0) { mZed.retrieveMeasure(mat_conf, sl::MEASURE::CONFIDENCE, sl::MEM::CPU, mMatResolDepth); retrieved = true; grab_ts=mat_conf.timestamp; @@ -2371,7 +2376,9 @@ void ZEDWrapperNodelet::callback_pubVideoDepth(const ros::TimerEvent& e) { if( mZedRealCamModel!=sl::MODEL::ZED ) { if(mSensTimestampSync || mSvoMode) { - if(retrieved) { + //NODELET_INFO_STREAM("tot_sub: " << tot_sub << " - retrieved: " << retrieved << " - grab_ts.data_ns!=lastZedTs.data_ns: " << (grab_ts.data_ns!=lastZedTs.data_ns)); + if(tot_sub>0 && retrieved && (grab_ts.data_ns!=lastZedTs.data_ns)) { + //NODELET_INFO("CALLBACK"); publishSensData(stamp); } } @@ -2578,7 +2585,7 @@ void ZEDWrapperNodelet::callback_pubPath(const ros::TimerEvent& e) { } void ZEDWrapperNodelet::callback_pubSensorsData(const ros::TimerEvent& e) { - NODELET_DEBUG("callback_pubSensorsData"); + //NODELET_INFO("callback_pubSensorsData"); std::lock_guard lock(mCloseZedMutex); if (!mZed.isOpened()) { @@ -2589,7 +2596,7 @@ void ZEDWrapperNodelet::callback_pubSensorsData(const ros::TimerEvent& e) { } void ZEDWrapperNodelet::publishSensData(ros::Time t) { - NODELET_DEBUG("publishSensData"); + //NODELET_INFO("publishSensData"); uint32_t imu_SubNumber = mPubImu.getNumSubscribers(); uint32_t imu_RawSubNumber = mPubImuRaw.getNumSubscribers(); From e88122d26e61217338a6606d8e2bfd183bd830ec Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Wed, 28 Oct 2020 12:08:42 +0100 Subject: [PATCH 020/199] Fix sensors data timestamp with SVO --- .../zed_nodelet/src/zed_wrapper_nodelet.cpp | 25 ++++++++----------- 1 file changed, 10 insertions(+), 15 deletions(-) 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 e1c013db..990d594e 100644 --- a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp @@ -2375,12 +2375,14 @@ void ZEDWrapperNodelet::callback_pubVideoDepth(const ros::TimerEvent& e) { // ----> Publish sensor data if sync is required by user or SVO if( mZedRealCamModel!=sl::MODEL::ZED ) { - if(mSensTimestampSync || mSvoMode) { + if(mSensTimestampSync) { //NODELET_INFO_STREAM("tot_sub: " << tot_sub << " - retrieved: " << retrieved << " - grab_ts.data_ns!=lastZedTs.data_ns: " << (grab_ts.data_ns!=lastZedTs.data_ns)); if(tot_sub>0 && retrieved && (grab_ts.data_ns!=lastZedTs.data_ns)) { //NODELET_INFO("CALLBACK"); publishSensData(stamp); } + } else if(mSvoMode) { + publishSensData(stamp); } } // <---- Publish sensor data if sync is required by user or SVO @@ -2647,21 +2649,14 @@ void ZEDWrapperNodelet::publishSensData(ros::Time t) { } } - if(mSvoMode) { - ts_imu = ros::Time::now(); - ts_baro = ros::Time::now(); - ts_mag = ros::Time::now(); - //ts_mag_raw = ros::Time::now(); + if(t!=ros::Time(0)) { + ts_imu = t; + ts_baro = t; + ts_mag = t; } else { - if(t!=ros::Time(0)) { - ts_imu = t; - ts_baro = t; - ts_mag = t; - } else { - ts_imu = sl_tools::slTime2Ros(sens_data.imu.timestamp); - ts_baro = sl_tools::slTime2Ros(sens_data.barometer.timestamp); - ts_mag = sl_tools::slTime2Ros(sens_data.magnetometer.timestamp); - } + ts_imu = sl_tools::slTime2Ros(sens_data.imu.timestamp); + ts_baro = sl_tools::slTime2Ros(sens_data.barometer.timestamp); + ts_mag = sl_tools::slTime2Ros(sens_data.magnetometer.timestamp); } // ----> Publish odometry tf only if enabled From a52af066198c0f2c585235984412d6b687b7354c Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Mon, 2 Nov 2020 17:22:02 +0100 Subject: [PATCH 021/199] Update latest_changes.md --- latest_changes.md | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/latest_changes.md b/latest_changes.md index 0ad18d89..a7d97ed7 100644 --- a/latest_changes.md +++ b/latest_changes.md @@ -1,6 +1,11 @@ LATEST CHANGES ============== +RGB/Depth sync fix #629 (2020-11-02) +------------------------------- +- Fixed sync issue between RGB and Depth data (Thx @dennisVi) +- Fixed issues with SVO and sensors data (Thx @dennisVi) + ASYNC Object Detection (2020-09-18) ----------------------------------- - Object Detection now runs asynchronously respect to data grabbing and Object Detected data are published only when available not affecting the frequency of the publishing of the other data types From e686fe58835fc695ab2e59a47da6acb7139a573f Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Thu, 19 Nov 2020 17:35:29 +0100 Subject: [PATCH 022/199] Update CMakeLists.txt Fix for issue #625 --- zed_wrapper/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/zed_wrapper/CMakeLists.txt b/zed_wrapper/CMakeLists.txt index 2f0ea326..a68f89e9 100644 --- a/zed_wrapper/CMakeLists.txt +++ b/zed_wrapper/CMakeLists.txt @@ -3,7 +3,7 @@ cmake_minimum_required(VERSION 2.8.7) project(zed_wrapper) #Fix for QtCretor -list(APPEND CMAKE_PREFIX_PATH "/opt/ros/$ENV{ROS_DISTRO}/") +#list(APPEND CMAKE_PREFIX_PATH "/opt/ros/$ENV{ROS_DISTRO}/") # if CMAKE_BUILD_TYPE is not specified, take 'Release' as default IF(NOT CMAKE_BUILD_TYPE) From 20346b393f7d6dc549a4c310691971b962ce8fc9 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Thu, 19 Nov 2020 17:35:50 +0100 Subject: [PATCH 023/199] Update CMakeLists.txt Fix for issue #625 --- zed_interfaces/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/zed_interfaces/CMakeLists.txt b/zed_interfaces/CMakeLists.txt index 7a1f361f..02463c5d 100644 --- a/zed_interfaces/CMakeLists.txt +++ b/zed_interfaces/CMakeLists.txt @@ -3,7 +3,7 @@ cmake_minimum_required(VERSION 2.8.7) project(zed_interfaces) #Fix for QtCretor -list(APPEND CMAKE_PREFIX_PATH "/opt/ros/$ENV{ROS_DISTRO}/") +#list(APPEND CMAKE_PREFIX_PATH "/opt/ros/$ENV{ROS_DISTRO}/") find_package(catkin REQUIRED COMPONENTS std_msgs From 614781b419b6a8d5f34ec6e30dca9c5d1c59c813 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Thu, 19 Nov 2020 17:36:08 +0100 Subject: [PATCH 024/199] Update CMakeLists.txt Fix for issue #625 --- zed_nodelets/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/zed_nodelets/CMakeLists.txt b/zed_nodelets/CMakeLists.txt index 574b9077..d1a11b0b 100644 --- a/zed_nodelets/CMakeLists.txt +++ b/zed_nodelets/CMakeLists.txt @@ -3,7 +3,7 @@ cmake_minimum_required(VERSION 2.8.7) project(zed_nodelets) #Fix for QtCretor -list(APPEND CMAKE_PREFIX_PATH "/opt/ros/$ENV{ROS_DISTRO}/") +#list(APPEND CMAKE_PREFIX_PATH "/opt/ros/$ENV{ROS_DISTRO}/") # if CMAKE_BUILD_TYPE is not specified, take 'Release' as default IF(NOT CMAKE_BUILD_TYPE) From 865364cd210231f241cc78e1efa6c04f0557f313 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Thu, 19 Nov 2020 17:39:02 +0100 Subject: [PATCH 025/199] Update CMakeLists.txt --- zed_wrapper/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/zed_wrapper/CMakeLists.txt b/zed_wrapper/CMakeLists.txt index a68f89e9..afa94b05 100644 --- a/zed_wrapper/CMakeLists.txt +++ b/zed_wrapper/CMakeLists.txt @@ -10,7 +10,7 @@ IF(NOT CMAKE_BUILD_TYPE) SET(CMAKE_BUILD_TYPE Release) ENDIF(NOT CMAKE_BUILD_TYPE) -find_package(catkin COMPONENTS +find_package(catkin REQUIRED COMPONENTS roscpp rosconsole nodelet From f5a691c39b01c565b7d3897c1f36f413c21d5782 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Thu, 19 Nov 2020 17:40:08 +0100 Subject: [PATCH 026/199] Update CMakeLists.txt --- zed_nodelets/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/zed_nodelets/CMakeLists.txt b/zed_nodelets/CMakeLists.txt index d1a11b0b..a56ba72d 100644 --- a/zed_nodelets/CMakeLists.txt +++ b/zed_nodelets/CMakeLists.txt @@ -41,7 +41,7 @@ if (OPENMP_FOUND) set (CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}") endif() -find_package(catkin COMPONENTS +find_package(catkin REQUIRED COMPONENTS roscpp image_transport rosconsole From 53119e06632b5d0b2f1aca8a97e64c679e50a886 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Thu, 25 Feb 2021 16:12:45 +0100 Subject: [PATCH 027/199] Devel v3.4 (#675) * Add support for new DEPTH16_MM depth mode * Update latest_changes.md * Remove debug settings * Fix issue #660 * Update latest_changes.md * New OD service and topics * New OD parameters handling * Delete c_cpp_properties.json * Delete settings.json * Delete c_cpp_properties.json * Delete settings.json * Add `.vscode` to gitignore * New OD Start service options * Init OD parameters * New OD module working * CLang formatting * Add OD detection max_range * Minor fixes * Fix OD tracking behavior * default OD params --- .gitignore | 3 + latest_changes.md | 5 + zed_interfaces/CMakeLists.txt | 14 +- zed_interfaces/msg/BoundingBox2Df.msg | 6 + zed_interfaces/msg/BoundingBox2Di.msg | 6 + zed_interfaces/msg/BoundingBox3D.msg | 8 + zed_interfaces/msg/Keypoint2Df.msg | 1 + zed_interfaces/msg/Keypoint2Di.msg | 1 + zed_interfaces/msg/Keypoint3D.msg | 1 + zed_interfaces/msg/Object.msg | 61 + zed_interfaces/msg/ObjectStamped.msg | 41 - zed_interfaces/msg/Objects.msg | 2 - zed_interfaces/msg/ObjectsStamped.msg | 5 + zed_interfaces/msg/Skeleton2D.msg | 21 + zed_interfaces/msg/Skeleton3D.msg | 21 + zed_interfaces/srv/start_object_detection.srv | 26 +- zed_nodelets/CMakeLists.txt | 24 +- .../include/rgbd_sensor_demux.hpp | 54 +- .../src/rgbd_sensor_demux.cpp | 139 +- .../include/rgbd_sensor_sync.hpp | 199 +- .../src/rgbd_sensor_sync.cpp | 748 +- zed_nodelets/src/tools/include/sl_tools.h | 184 +- zed_nodelets/src/tools/src/sl_tools.cpp | 695 +- .../include/zed_wrapper_nodelet.hpp | 1278 +-- .../zed_nodelet/src/zed_wrapper_nodelet.cpp | 7565 +++++++++-------- zed_wrapper/.gitignore | 4 + zed_wrapper/CMakeLists.txt | 5 +- zed_wrapper/params/common.yaml | 6 +- zed_wrapper/params/zed2.yaml | 13 +- zed_wrapper/src/zed_wrapper_node.cpp | 19 +- 30 files changed, 5907 insertions(+), 5248 deletions(-) create mode 100644 zed_interfaces/msg/BoundingBox2Df.msg create mode 100644 zed_interfaces/msg/BoundingBox2Di.msg create mode 100644 zed_interfaces/msg/BoundingBox3D.msg create mode 100644 zed_interfaces/msg/Keypoint2Df.msg create mode 100644 zed_interfaces/msg/Keypoint2Di.msg create mode 100644 zed_interfaces/msg/Keypoint3D.msg create mode 100644 zed_interfaces/msg/Object.msg delete mode 100644 zed_interfaces/msg/ObjectStamped.msg delete mode 100644 zed_interfaces/msg/Objects.msg create mode 100644 zed_interfaces/msg/ObjectsStamped.msg create mode 100644 zed_interfaces/msg/Skeleton2D.msg create mode 100644 zed_interfaces/msg/Skeleton3D.msg diff --git a/.gitignore b/.gitignore index 6732e720..4326ae65 100644 --- a/.gitignore +++ b/.gitignore @@ -37,3 +37,6 @@ Makefile* # QtCtreator CMake CMakeLists.txt.user* +#VSCode +*vscode* + diff --git a/latest_changes.md b/latest_changes.md index a7d97ed7..0d23ef0f 100644 --- a/latest_changes.md +++ b/latest_changes.md @@ -1,6 +1,11 @@ LATEST CHANGES ============== +v3.4 +--------- +- Add support for new DEPTH16_MM data type for depth (OPENNI MODE) +- Fix issue #660: detected objects topic not published if depth computation not active + RGB/Depth sync fix #629 (2020-11-02) ------------------------------- - Fixed sync issue between RGB and Depth data (Thx @dennisVi) diff --git a/zed_interfaces/CMakeLists.txt b/zed_interfaces/CMakeLists.txt index 02463c5d..924ad018 100644 --- a/zed_interfaces/CMakeLists.txt +++ b/zed_interfaces/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.8.7) +cmake_minimum_required(VERSION 3.5) project(zed_interfaces) @@ -16,8 +16,16 @@ find_package(catkin REQUIRED COMPONENTS add_message_files( DIRECTORY msg FILES - ObjectStamped.msg - Objects.msg + BoundingBox2Df.msg + BoundingBox2Di.msg + BoundingBox3D.msg + Keypoint2Df.msg + Keypoint2Di.msg + Keypoint3D.msg + Object.msg + ObjectsStamped.msg + Skeleton2D.msg + Skeleton3D.msg RGBDSensors.msg ) diff --git a/zed_interfaces/msg/BoundingBox2Df.msg b/zed_interfaces/msg/BoundingBox2Df.msg new file mode 100644 index 00000000..cd11758e --- /dev/null +++ b/zed_interfaces/msg/BoundingBox2Df.msg @@ -0,0 +1,6 @@ +# 0 ------- 1 +# | | +# | | +# | | +# 3 ------- 2 +zed_interfaces/Keypoint2Df[4] corners diff --git a/zed_interfaces/msg/BoundingBox2Di.msg b/zed_interfaces/msg/BoundingBox2Di.msg new file mode 100644 index 00000000..bc5fd790 --- /dev/null +++ b/zed_interfaces/msg/BoundingBox2Di.msg @@ -0,0 +1,6 @@ +# 0 ------- 1 +# | | +# | | +# | | +# 3 ------- 2 +zed_interfaces/Keypoint2Di[4] corners diff --git a/zed_interfaces/msg/BoundingBox3D.msg b/zed_interfaces/msg/BoundingBox3D.msg new file mode 100644 index 00000000..89b3d647 --- /dev/null +++ b/zed_interfaces/msg/BoundingBox3D.msg @@ -0,0 +1,8 @@ +# 1 ------- 2 +# /. /| +# 0 ------- 3 | +# | . | | +# | 5.......| 6 +# |. |/ +# 4 ------- 7 +zed_interfaces/Keypoint3D[8] corners diff --git a/zed_interfaces/msg/Keypoint2Df.msg b/zed_interfaces/msg/Keypoint2Df.msg new file mode 100644 index 00000000..dbadcaf2 --- /dev/null +++ b/zed_interfaces/msg/Keypoint2Df.msg @@ -0,0 +1 @@ +float32[2] kp diff --git a/zed_interfaces/msg/Keypoint2Di.msg b/zed_interfaces/msg/Keypoint2Di.msg new file mode 100644 index 00000000..16a8348d --- /dev/null +++ b/zed_interfaces/msg/Keypoint2Di.msg @@ -0,0 +1 @@ +uint32[2] kp diff --git a/zed_interfaces/msg/Keypoint3D.msg b/zed_interfaces/msg/Keypoint3D.msg new file mode 100644 index 00000000..aa21f09f --- /dev/null +++ b/zed_interfaces/msg/Keypoint3D.msg @@ -0,0 +1 @@ +float32[3] kp diff --git a/zed_interfaces/msg/Object.msg b/zed_interfaces/msg/Object.msg new file mode 100644 index 00000000..df86fda5 --- /dev/null +++ b/zed_interfaces/msg/Object.msg @@ -0,0 +1,61 @@ +# Object label +string label + +# Object label ID +int16 label_id + +# Object sub +string sublabel + +# Object confidence level (1-99) +float32 confidence + +# Object centroid position +float32[3] position + +# Position covariance +float32[6] position_covariance + +# Object velocity +float32[3] velocity + +# Tracking available +bool tracking_available + +# Tracking state +# 0 -> OFF (object not valid) +# 1 -> OK +# 2 -> SEARCHING (occlusion occurred, trajectory is estimated) +int8 tracking_state + +# Action state +# 0 -> IDLE +# 2 -> MOVING +int8 action_state + +# 2D Bounding box projected to Camera image +zed_interfaces/BoundingBox2Di bounding_box_2d + +# 3D Bounding box in world frame +zed_interfaces/BoundingBox3D bounding_box_3d + +# 3D dimensions (width, height, lenght) +float32[3] dimensions_3d + +# Is skeleton available? +bool skeleton_available + +# 2D Bounding box projected to Camera image of the person head +zed_interfaces/BoundingBox2Df head_bounding_box_2d + +# 3D Bounding box in world frame of the person head +zed_interfaces/BoundingBox3D head_bounding_box_3d + +# 3D position of the centroid of the person head +float32[3] head_position + +# 2D Person skeleton projected to Camera image +zed_interfaces/Skeleton2D skeleton_2d + +# 3D Person skeleton in world frame +zed_interfaces/Skeleton3D skeleton_3d diff --git a/zed_interfaces/msg/ObjectStamped.msg b/zed_interfaces/msg/ObjectStamped.msg deleted file mode 100644 index ae477b74..00000000 --- a/zed_interfaces/msg/ObjectStamped.msg +++ /dev/null @@ -1,41 +0,0 @@ -# Standard Header -Header header - -# Object label -string label - -# Object label ID -int16 label_id - -# Object confidence level (1-99) -float32 confidence - -# Object centroid -geometry_msgs/Point32 position - -# Object velocity -geometry_msgs/Vector3 linear_vel - -# Tracking state -# 0 -> OFF (object not valid) -# 1 -> OK -# 2 -> SEARCHING (occlusion occurred, trajectory is estimated) -int8 tracking_state - -# 2D Bounding box projected to Camera image -# 0 ------- 1 -# | | -# | | -# | | -# 3 ------- 2 -geometry_msgs/Point32[4] bbox_2d - -# 3D Bounding box in world frame -# 1 ------- 2 -# /. /| -# 0 ------- 3 | -# | . | | -# | 5.......| 6 -# |. |/ -# 4 ------- 7 -geometry_msgs/Point32[8] bbox_3d diff --git a/zed_interfaces/msg/Objects.msg b/zed_interfaces/msg/Objects.msg deleted file mode 100644 index 1eadcc2b..00000000 --- a/zed_interfaces/msg/Objects.msg +++ /dev/null @@ -1,2 +0,0 @@ -# Array of `object_stamped` topics -zed_interfaces/ObjectStamped[] objects diff --git a/zed_interfaces/msg/ObjectsStamped.msg b/zed_interfaces/msg/ObjectsStamped.msg new file mode 100644 index 00000000..6a74cf7b --- /dev/null +++ b/zed_interfaces/msg/ObjectsStamped.msg @@ -0,0 +1,5 @@ +# Standard Header +std_msgs/Header header + +# Array of `object_stamped` topics +zed_interfaces/Object[] objects diff --git a/zed_interfaces/msg/Skeleton2D.msg b/zed_interfaces/msg/Skeleton2D.msg new file mode 100644 index 00000000..2e64d28e --- /dev/null +++ b/zed_interfaces/msg/Skeleton2D.msg @@ -0,0 +1,21 @@ +# Skeleton joints indices +# 16-14 15-17 +# \ / +# 0 +# | +# 2------1------5 +# | | | | +# | | | | +# 3 | | 6 +# | | | | +# | | | | +# 4 8 11 7 +# | | +# | | +# | | +# 9 12 +# | | +# | | +# | | +# 10 13 +zed_interfaces/Keypoint2Df[18] keypoints diff --git a/zed_interfaces/msg/Skeleton3D.msg b/zed_interfaces/msg/Skeleton3D.msg new file mode 100644 index 00000000..136859f0 --- /dev/null +++ b/zed_interfaces/msg/Skeleton3D.msg @@ -0,0 +1,21 @@ +# Skeleton joints indices +# 16-14 15-17 +# \ / +# 0 +# | +# 2------1------5 +# | | | | +# | | | | +# 3 | | 6 +# | | | | +# | | | | +# 4 8 11 7 +# | | +# | | +# | | +# 9 12 +# | | +# | | +# | | +# 10 13 +zed_interfaces/Keypoint3D[18] keypoints diff --git a/zed_interfaces/srv/start_object_detection.srv b/zed_interfaces/srv/start_object_detection.srv index cc7882d6..099e3176 100644 --- a/zed_interfaces/srv/start_object_detection.srv +++ b/zed_interfaces/srv/start_object_detection.srv @@ -1,18 +1,38 @@ # Starts object detection, if not automatically enabled with the parameter `object_detection/od_enabled` +# OD Model +# '0': MULTI_CLASS_BOX - '1': MULTI_CLASS_BOX_ACCURATE - '2': HUMAN_BODY_FAST - '3': HUMAN_BODY_ACCURATE +int8 model + # Minimum Confidence level float32 confidence +# MAx detection range +float32 max_range + # Object tracking bool tracking -# Detect people -bool people +# Body Fitting +bool sk_body_fitting + +# Detect people (valid for Multi-class model) +bool mc_people # Detect vehicles -bool vehicles +bool mc_vehicles + +# Detect bags +bool mc_bag + +# Detect animals +bool mc_animal +# Detect electronic devices +bool mc_electronics +# Detect fruits and vegetables +bool mc_fruit_vegetable --- bool done diff --git a/zed_nodelets/CMakeLists.txt b/zed_nodelets/CMakeLists.txt index a56ba72d..b7ee9438 100644 --- a/zed_nodelets/CMakeLists.txt +++ b/zed_nodelets/CMakeLists.txt @@ -1,10 +1,7 @@ -cmake_minimum_required(VERSION 2.8.7) +cmake_minimum_required(VERSION 3.5) project(zed_nodelets) -#Fix for QtCretor -#list(APPEND CMAKE_PREFIX_PATH "/opt/ros/$ENV{ROS_DISTRO}/") - # if CMAKE_BUILD_TYPE is not specified, take 'Release' as default IF(NOT CMAKE_BUILD_TYPE) SET(CMAKE_BUILD_TYPE Release) @@ -73,7 +70,7 @@ catkin_package( dynamic_reconfigure tf2_ros tf2_geometry_msgs - message_runtime + message_runtime zed_interfaces ) @@ -91,14 +88,14 @@ set(RGBD_SENS_DEMUX_SRC ${CMAKE_CURRENT_SOURCE_DIR}/src/rgbd_sensors_demux_nodel # INCLUDES # Specify locations of header files. -include_directories( - ${catkin_INCLUDE_DIRS} - ${CUDA_INCLUDE_DIRS} - ${ZED_INCLUDE_DIRS} - ${CMAKE_CURRENT_SOURCE_DIR}/src/tools/include - ${CMAKE_CURRENT_SOURCE_DIR}/src/zed_nodelet/include - ${CMAKE_CURRENT_SOURCE_DIR}/src/rgbd_sensors_sync_nodelet/include - ${CMAKE_CURRENT_SOURCE_DIR}/src/rgbd_sensors_demux_nodelet/include +set(INCLUDE_DIRS + ${catkin_INCLUDE_DIRS} + ${CUDA_INCLUDE_DIRS} + ${ZED_INCLUDE_DIRS} + ${CMAKE_CURRENT_SOURCE_DIR}/src/tools/include + ${CMAKE_CURRENT_SOURCE_DIR}/src/zed_nodelet/include + ${CMAKE_CURRENT_SOURCE_DIR}/src/rgbd_sensors_sync_nodelet/include + ${CMAKE_CURRENT_SOURCE_DIR}/src/rgbd_sensors_demux_nodelet/include ) link_directories(${ZED_LIBRARY_DIR}) @@ -122,6 +119,7 @@ add_library(ZEDNodelets ${RGBD_SENS_SYNC_SRC} ${RGBD_SENS_DEMUX_SRC} ) +target_include_directories(ZEDNodelets PRIVATE ${INCLUDE_DIRS}) target_link_libraries(ZEDNodelets ${LINK_LIBRARIES}) add_dependencies( ZEDNodelets diff --git a/zed_nodelets/src/rgbd_sensors_demux_nodelet/include/rgbd_sensor_demux.hpp b/zed_nodelets/src/rgbd_sensors_demux_nodelet/include/rgbd_sensor_demux.hpp index 4074103a..581bd8c1 100644 --- a/zed_nodelets/src/rgbd_sensors_demux_nodelet/include/rgbd_sensor_demux.hpp +++ b/zed_nodelets/src/rgbd_sensors_demux_nodelet/include/rgbd_sensor_demux.hpp @@ -21,8 +21,8 @@ #ifndef RGBD_SENSOR_DEMUX_HPP #define RGBD_SENSOR_DEMUX_HPP -#include #include +#include #include #include @@ -35,41 +35,41 @@ #include "zed_interfaces/RGBDSensors.h" -namespace zed_nodelets { - -class RgbdSensorsDemuxNodelet : public nodelet::Nodelet { - +namespace zed_nodelets +{ +class RgbdSensorsDemuxNodelet : public nodelet::Nodelet +{ public: - RgbdSensorsDemuxNodelet(); - virtual ~RgbdSensorsDemuxNodelet(); + RgbdSensorsDemuxNodelet(); + virtual ~RgbdSensorsDemuxNodelet(); protected: - /*! \brief Initialization function called by the Nodelet base class - */ - virtual void onInit(); + /*! \brief Initialization function called by the Nodelet base class + */ + virtual void onInit(); - /*! \brief Callback for full topics synchronization - */ - void msgCallback( const zed_interfaces::RGBDSensorsPtr& msg ); + /*! \brief Callback for full topics synchronization + */ + void msgCallback(const zed_interfaces::RGBDSensorsPtr& msg); private: - // Node handlers - ros::NodeHandle mNh; // Node handler - ros::NodeHandle mNhP; // Private Node handler - - // Publishers - image_transport::CameraPublisher mPubRgb; - image_transport::CameraPublisher mPubDepth; - ros::Publisher mPubIMU; - ros::Publisher mPubMag; - - // Subscribers - ros::Subscriber mSub; + // Node handlers + ros::NodeHandle mNh; // Node handler + ros::NodeHandle mNhP; // Private Node handler + + // Publishers + image_transport::CameraPublisher mPubRgb; + image_transport::CameraPublisher mPubDepth; + ros::Publisher mPubIMU; + ros::Publisher mPubMag; + + // Subscribers + ros::Subscriber mSub; }; -} +} // namespace zed_nodelets #include PLUGINLIB_EXPORT_CLASS(zed_nodelets::RgbdSensorsDemuxNodelet, nodelet::Nodelet) -#endif // RGBD_SENSOR_DEMUX_HPP +#endif // RGBD_SENSOR_DEMUX_HPP diff --git a/zed_nodelets/src/rgbd_sensors_demux_nodelet/src/rgbd_sensor_demux.cpp b/zed_nodelets/src/rgbd_sensors_demux_nodelet/src/rgbd_sensor_demux.cpp index 81145b4f..f4199d53 100644 --- a/zed_nodelets/src/rgbd_sensors_demux_nodelet/src/rgbd_sensor_demux.cpp +++ b/zed_nodelets/src/rgbd_sensors_demux_nodelet/src/rgbd_sensor_demux.cpp @@ -26,91 +26,104 @@ #include -namespace zed_nodelets { - -RgbdSensorsDemuxNodelet::RgbdSensorsDemuxNodelet() { - +namespace zed_nodelets +{ +RgbdSensorsDemuxNodelet::RgbdSensorsDemuxNodelet() +{ } -RgbdSensorsDemuxNodelet::~RgbdSensorsDemuxNodelet() { - +RgbdSensorsDemuxNodelet::~RgbdSensorsDemuxNodelet() +{ } -void RgbdSensorsDemuxNodelet::onInit() { - // Node handlers - mNh = getNodeHandle(); - mNhP = getPrivateNodeHandle(); +void RgbdSensorsDemuxNodelet::onInit() +{ + // Node handlers + mNh = getNodeHandle(); + mNhP = getPrivateNodeHandle(); #ifndef NDEBUG - if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, - ros::console::levels::Debug)) { - ros::console::notifyLoggerLevelsChanged(); - } + if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug)) + { + ros::console::notifyLoggerLevelsChanged(); + } #endif - NODELET_INFO( "********** Starting nodelet '%s' **********",getName().c_str() ); + NODELET_INFO("********** Starting nodelet '%s' **********", getName().c_str()); - mSub = mNh.subscribe( "rgbd_sens", 1, &RgbdSensorsDemuxNodelet::msgCallback, this ); - NODELET_INFO_STREAM( " * Subscribed to topic: " << mSub.getTopic().c_str() ); + mSub = mNh.subscribe("rgbd_sens", 1, &RgbdSensorsDemuxNodelet::msgCallback, this); + NODELET_INFO_STREAM(" * Subscribed to topic: " << mSub.getTopic().c_str()); } -void RgbdSensorsDemuxNodelet::msgCallback(const zed_interfaces::RGBDSensorsPtr &msg) { - if( !msg->rgb.header.stamp.isZero() ) { - if( mPubRgb.getTopic().empty() ) { - ros::NodeHandle rgb_pnh(mNhP, "rgb"); - image_transport::ImageTransport it(rgb_pnh); - - mPubRgb = it.advertiseCamera("image_rect_color", 1); // rgb - NODELET_INFO_STREAM("Advertised on topic " << mPubRgb.getTopic()); - NODELET_INFO_STREAM("Advertised on topic " << mPubRgb.getInfoTopic()); - } - - if(mPubRgb.getNumSubscribers() > 0 ){ - mPubRgb.publish( msg->rgb, msg->rgbCameraInfo ); - } +void RgbdSensorsDemuxNodelet::msgCallback(const zed_interfaces::RGBDSensorsPtr &msg) +{ + if (!msg->rgb.header.stamp.isZero()) + { + if (mPubRgb.getTopic().empty()) + { + ros::NodeHandle rgb_pnh(mNhP, "rgb"); + image_transport::ImageTransport it(rgb_pnh); + + mPubRgb = it.advertiseCamera("image_rect_color", 1); // rgb + NODELET_INFO_STREAM("Advertised on topic " << mPubRgb.getTopic()); + NODELET_INFO_STREAM("Advertised on topic " << mPubRgb.getInfoTopic()); } - if( !msg->depth.header.stamp.isZero() ) { - if( mPubDepth.getTopic().empty() ) { - ros::NodeHandle depth_pnh(mNhP, "depth"); - image_transport::ImageTransport it(depth_pnh); - - mPubDepth = it.advertiseCamera("depth_registered", 1); // depth - NODELET_INFO_STREAM("Advertised on topic " << mPubDepth.getTopic()); - NODELET_INFO_STREAM("Advertised on topic " << mPubDepth.getInfoTopic()); - } - - if(mPubDepth.getNumSubscribers() > 0 ){ - mPubDepth.publish( msg->depth, msg->depthCameraInfo ); - } + if (mPubRgb.getNumSubscribers() > 0) + { + mPubRgb.publish(msg->rgb, msg->rgbCameraInfo); + } + } + + if (!msg->depth.header.stamp.isZero()) + { + if (mPubDepth.getTopic().empty()) + { + ros::NodeHandle depth_pnh(mNhP, "depth"); + image_transport::ImageTransport it(depth_pnh); + + mPubDepth = it.advertiseCamera("depth_registered", 1); // depth + NODELET_INFO_STREAM("Advertised on topic " << mPubDepth.getTopic()); + NODELET_INFO_STREAM("Advertised on topic " << mPubDepth.getInfoTopic()); } - if( !msg->imu.header.stamp.isZero() ) { - if( mPubIMU.getTopic().empty() ) { - ros::NodeHandle imu_pnh(mNhP, "imu"); + if (mPubDepth.getNumSubscribers() > 0) + { + mPubDepth.publish(msg->depth, msg->depthCameraInfo); + } + } - mPubIMU = imu_pnh.advertise("data", 1); // IMU - NODELET_INFO_STREAM("Advertised on topic " << mPubIMU.getTopic()); - } + if (!msg->imu.header.stamp.isZero()) + { + if (mPubIMU.getTopic().empty()) + { + ros::NodeHandle imu_pnh(mNhP, "imu"); - if(mPubIMU.getNumSubscribers() > 0 ){ - mPubIMU.publish( msg->imu ); - } + mPubIMU = imu_pnh.advertise("data", 1); // IMU + NODELET_INFO_STREAM("Advertised on topic " << mPubIMU.getTopic()); } - if( !msg->mag.header.stamp.isZero() ) { - if( mPubMag.getTopic().empty() ) { - ros::NodeHandle imu_pnh(mNhP, "imu"); + if (mPubIMU.getNumSubscribers() > 0) + { + mPubIMU.publish(msg->imu); + } + } - mPubMag = imu_pnh.advertise("mag", 1); // IMU - NODELET_INFO_STREAM("Advertised on topic " << mPubMag.getTopic()); - } + if (!msg->mag.header.stamp.isZero()) + { + if (mPubMag.getTopic().empty()) + { + ros::NodeHandle imu_pnh(mNhP, "imu"); - if(mPubMag.getNumSubscribers() > 0 ){ - mPubMag.publish( msg->mag ); - } + mPubMag = imu_pnh.advertise("mag", 1); // IMU + NODELET_INFO_STREAM("Advertised on topic " << mPubMag.getTopic()); } + if (mPubMag.getNumSubscribers() > 0) + { + mPubMag.publish(msg->mag); + } + } } -} +} // namespace zed_nodelets diff --git a/zed_nodelets/src/rgbd_sensors_sync_nodelet/include/rgbd_sensor_sync.hpp b/zed_nodelets/src/rgbd_sensors_sync_nodelet/include/rgbd_sensor_sync.hpp index 81db0af5..d25c1659 100644 --- a/zed_nodelets/src/rgbd_sensors_sync_nodelet/include/rgbd_sensor_sync.hpp +++ b/zed_nodelets/src/rgbd_sensors_sync_nodelet/include/rgbd_sensor_sync.hpp @@ -21,8 +21,8 @@ #ifndef RGBD_SENSOR_SYNC_HPP #define RGBD_SENSOR_SYNC_HPP -#include #include +#include #include #include @@ -30,115 +30,124 @@ #include #include -#include -#include #include +#include +#include #include #include #include "zed_interfaces/RGBDSensors.h" -namespace zed_nodelets { - -class RgbdSensorsSyncNodelet : public nodelet::Nodelet { - +namespace zed_nodelets +{ +class RgbdSensorsSyncNodelet : public nodelet::Nodelet +{ public: - RgbdSensorsSyncNodelet(); - virtual ~RgbdSensorsSyncNodelet(); + RgbdSensorsSyncNodelet(); + virtual ~RgbdSensorsSyncNodelet(); protected: - /*! \brief Initialization function called by the Nodelet base class - */ - virtual void onInit(); - - /*! \brief Reads parameters from the param server - */ - void readParameters(); - - /*! \brief Callback for full topics synchronization - */ - void callbackFull( - const sensor_msgs::ImageConstPtr& rgb, - const sensor_msgs::ImageConstPtr& depth, - const sensor_msgs::CameraInfoConstPtr& rgbCameraInfo, - const sensor_msgs::CameraInfoConstPtr& depthCameraInfo, - const sensor_msgs::ImuConstPtr& imu, - const sensor_msgs::MagneticFieldConstPtr& mag ); - - /*! \brief Callback for RGBD topics synchronization - */ - void callbackRGBD( - const sensor_msgs::ImageConstPtr& rgb, - const sensor_msgs::ImageConstPtr& depth, - const sensor_msgs::CameraInfoConstPtr& rgbCameraInfo, - const sensor_msgs::CameraInfoConstPtr& depthCameraInfo ); - - /*! \brief Callback for RGBD + IMU topics synchronization - */ - void callbackRGBDIMU( - const sensor_msgs::ImageConstPtr& rgb, - const sensor_msgs::ImageConstPtr& depth, - const sensor_msgs::CameraInfoConstPtr& rgbCameraInfo, - const sensor_msgs::CameraInfoConstPtr& depthCameraInfo, - const sensor_msgs::ImuConstPtr& imu); - - /*! \brief Callback for RGBD + Mag topics synchronization - */ - void callbackRGBDMag( - const sensor_msgs::ImageConstPtr& rgb, - const sensor_msgs::ImageConstPtr& depth, - const sensor_msgs::CameraInfoConstPtr& rgbCameraInfo, - const sensor_msgs::CameraInfoConstPtr& depthCameraInfo, - const sensor_msgs::MagneticFieldConstPtr& mag ); + /*! \brief Initialization function called by the Nodelet base class + */ + virtual void onInit(); + + /*! \brief Reads parameters from the param server + */ + void readParameters(); + + /*! \brief Callback for full topics synchronization + */ + void callbackFull(const sensor_msgs::ImageConstPtr& rgb, const sensor_msgs::ImageConstPtr& depth, + const sensor_msgs::CameraInfoConstPtr& rgbCameraInfo, + const sensor_msgs::CameraInfoConstPtr& depthCameraInfo, const sensor_msgs::ImuConstPtr& imu, + const sensor_msgs::MagneticFieldConstPtr& mag); + + /*! \brief Callback for RGBD topics synchronization + */ + void callbackRGBD(const sensor_msgs::ImageConstPtr& rgb, const sensor_msgs::ImageConstPtr& depth, + const sensor_msgs::CameraInfoConstPtr& rgbCameraInfo, + const sensor_msgs::CameraInfoConstPtr& depthCameraInfo); + + /*! \brief Callback for RGBD + IMU topics synchronization + */ + void callbackRGBDIMU(const sensor_msgs::ImageConstPtr& rgb, const sensor_msgs::ImageConstPtr& depth, + const sensor_msgs::CameraInfoConstPtr& rgbCameraInfo, + const sensor_msgs::CameraInfoConstPtr& depthCameraInfo, const sensor_msgs::ImuConstPtr& imu); + + /*! \brief Callback for RGBD + Mag topics synchronization + */ + void callbackRGBDMag(const sensor_msgs::ImageConstPtr& rgb, const sensor_msgs::ImageConstPtr& depth, + const sensor_msgs::CameraInfoConstPtr& rgbCameraInfo, + const sensor_msgs::CameraInfoConstPtr& depthCameraInfo, + const sensor_msgs::MagneticFieldConstPtr& mag); private: - // Node handlers - ros::NodeHandle mNh; // Node handler - ros::NodeHandle mNhP; // Private Node handler - - // Publishers - ros::Publisher mPubRaw; - - // Subscribers - image_transport::SubscriberFilter mSubRgbImage; - image_transport::SubscriberFilter mSubDepthImage; - message_filters::Subscriber mSubRgbCamInfo; - message_filters::Subscriber mSubDepthCamInfo; - message_filters::Subscriber mSubImu; - message_filters::Subscriber mSubMag; - - // Approx sync policies - typedef message_filters::sync_policies::ApproximateTime ApproxFullSyncPolicy; - typedef message_filters::sync_policies::ApproximateTime ApproxRgbdImuSyncPolicy; - typedef message_filters::sync_policies::ApproximateTime ApproxRgbdMagSyncPolicy; - typedef message_filters::sync_policies::ApproximateTime ApproxRgbdSyncPolicy; - message_filters::Synchronizer* mApproxFullSync = nullptr; - message_filters::Synchronizer* mApproxRgbdImuSync = nullptr; - message_filters::Synchronizer* mApproxRgbdMagSync = nullptr; - message_filters::Synchronizer* mApproxRgbdSync = nullptr; - - // Exact sync policies - typedef message_filters::sync_policies::ExactTime ExactFullSyncPolicy; - typedef message_filters::sync_policies::ExactTime ExactRgbdImuSyncPolicy; - typedef message_filters::sync_policies::ExactTime ExactRgbdMagSyncPolicy; - typedef message_filters::sync_policies::ExactTime ExactRgbdSyncPolicy; - message_filters::Synchronizer* mExactFullSync = nullptr; - message_filters::Synchronizer* mExactRgbdImuSync = nullptr; - message_filters::Synchronizer* mExactRgbdMagSync = nullptr; - message_filters::Synchronizer* mExactRgbdSync = nullptr; - - // Params - std::string mZedNodeletName = "zed_node"; - bool mUseApproxSync = true; - bool mUseImu = true; - bool mUseMag = true; - int mQueueSize = 50; + // Node handlers + ros::NodeHandle mNh; // Node handler + ros::NodeHandle mNhP; // Private Node handler + + // Publishers + ros::Publisher mPubRaw; + + // Subscribers + image_transport::SubscriberFilter mSubRgbImage; + image_transport::SubscriberFilter mSubDepthImage; + message_filters::Subscriber mSubRgbCamInfo; + message_filters::Subscriber mSubDepthCamInfo; + message_filters::Subscriber mSubImu; + message_filters::Subscriber mSubMag; + + // Approx sync policies + typedef message_filters::sync_policies::ApproximateTime + ApproxFullSyncPolicy; + typedef message_filters::sync_policies::ApproximateTime< + sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo, sensor_msgs::Imu> + ApproxRgbdImuSyncPolicy; + typedef message_filters::sync_policies::ApproximateTime + ApproxRgbdMagSyncPolicy; + typedef message_filters::sync_policies::ApproximateTime + ApproxRgbdSyncPolicy; + message_filters::Synchronizer* mApproxFullSync = nullptr; + message_filters::Synchronizer* mApproxRgbdImuSync = nullptr; + message_filters::Synchronizer* mApproxRgbdMagSync = nullptr; + message_filters::Synchronizer* mApproxRgbdSync = nullptr; + + // Exact sync policies + typedef message_filters::sync_policies::ExactTime + ExactFullSyncPolicy; + typedef message_filters::sync_policies::ExactTime + ExactRgbdImuSyncPolicy; + typedef message_filters::sync_policies::ExactTime + ExactRgbdMagSyncPolicy; + typedef message_filters::sync_policies::ExactTime + ExactRgbdSyncPolicy; + message_filters::Synchronizer* mExactFullSync = nullptr; + message_filters::Synchronizer* mExactRgbdImuSync = nullptr; + message_filters::Synchronizer* mExactRgbdMagSync = nullptr; + message_filters::Synchronizer* mExactRgbdSync = nullptr; + + // Params + std::string mZedNodeletName = "zed_node"; + bool mUseApproxSync = true; + bool mUseImu = true; + bool mUseMag = true; + int mQueueSize = 50; }; -} +} // namespace zed_nodelets #include PLUGINLIB_EXPORT_CLASS(zed_nodelets::RgbdSensorsSyncNodelet, nodelet::Nodelet) -#endif // RGBD_SENSOR_SYNC_HPP +#endif // RGBD_SENSOR_SYNC_HPP diff --git a/zed_nodelets/src/rgbd_sensors_sync_nodelet/src/rgbd_sensor_sync.cpp b/zed_nodelets/src/rgbd_sensors_sync_nodelet/src/rgbd_sensor_sync.cpp index e3147857..efa8dbf1 100644 --- a/zed_nodelets/src/rgbd_sensors_sync_nodelet/src/rgbd_sensor_sync.cpp +++ b/zed_nodelets/src/rgbd_sensors_sync_nodelet/src/rgbd_sensor_sync.cpp @@ -26,423 +26,419 @@ #include -namespace zed_nodelets { - -RgbdSensorsSyncNodelet::RgbdSensorsSyncNodelet() { - +namespace zed_nodelets +{ +RgbdSensorsSyncNodelet::RgbdSensorsSyncNodelet() +{ } -RgbdSensorsSyncNodelet::~RgbdSensorsSyncNodelet() { - if(mApproxFullSync) - delete mApproxFullSync; - if(mApproxRgbdImuSync) - delete mApproxRgbdImuSync; - if(mApproxRgbdMagSync) - delete mApproxRgbdMagSync; - if(mApproxRgbdSync) - delete mApproxRgbdSync; - - if(mExactFullSync) - delete mExactFullSync; - if(mExactRgbdImuSync) - delete mExactRgbdImuSync; - if(mExactRgbdMagSync) - delete mExactRgbdMagSync; - if(mExactRgbdSync) - delete mExactRgbdSync; +RgbdSensorsSyncNodelet::~RgbdSensorsSyncNodelet() +{ + if (mApproxFullSync) + delete mApproxFullSync; + if (mApproxRgbdImuSync) + delete mApproxRgbdImuSync; + if (mApproxRgbdMagSync) + delete mApproxRgbdMagSync; + if (mApproxRgbdSync) + delete mApproxRgbdSync; + + if (mExactFullSync) + delete mExactFullSync; + if (mExactRgbdImuSync) + delete mExactRgbdImuSync; + if (mExactRgbdMagSync) + delete mExactRgbdMagSync; + if (mExactRgbdSync) + delete mExactRgbdSync; } -void RgbdSensorsSyncNodelet::onInit() { - // Node handlers - mNh = getNodeHandle(); - mNhP = getPrivateNodeHandle(); +void RgbdSensorsSyncNodelet::onInit() +{ + // Node handlers + mNh = getNodeHandle(); + mNhP = getPrivateNodeHandle(); #ifndef NDEBUG - if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, - ros::console::levels::Debug)) { - ros::console::notifyLoggerLevelsChanged(); - } + if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug)) + { + ros::console::notifyLoggerLevelsChanged(); + } #endif - NODELET_INFO( "********** Starting nodelet '%s' **********",getName().c_str() ); - - readParameters(); - - mPubRaw = mNhP.advertise("rgbd_sens", 1); - NODELET_INFO_STREAM("Advertised on topic " << mPubRaw.getTopic()); - - if( mUseApproxSync ) { - NODELET_DEBUG( "Using Approximate Time sync"); - - if( mUseImu && mUseMag ) { - mApproxFullSync = new message_filters::Synchronizer(ApproxFullSyncPolicy(mQueueSize), - mSubRgbImage, mSubDepthImage, - mSubRgbCamInfo, mSubDepthCamInfo, - mSubImu,mSubMag); - mApproxFullSync->registerCallback(boost::bind(&RgbdSensorsSyncNodelet::callbackFull, this, - _1, _2, _3, _4, _5, _6)); - - NODELET_DEBUG("RGB + Depth + IMU + Magnetometer Sync" ); - - } else if( mUseImu ) { - NODELET_DEBUG("RGB + Depth + IMU Sync" ); - - mApproxRgbdImuSync = new message_filters::Synchronizer(ApproxRgbdImuSyncPolicy(mQueueSize), - mSubRgbImage, mSubDepthImage, - mSubRgbCamInfo, mSubDepthCamInfo, - mSubImu); - mApproxRgbdImuSync->registerCallback(boost::bind(&RgbdSensorsSyncNodelet::callbackRGBDIMU, this, - _1, _2, _3, _4, _5)); - - - } else if( mUseMag ) { - NODELET_DEBUG("RGB + Depth + Magnetometer Sync" ); - - mApproxRgbdMagSync = new message_filters::Synchronizer(ApproxRgbdMagSyncPolicy(mQueueSize), - mSubRgbImage, mSubDepthImage, - mSubRgbCamInfo, mSubDepthCamInfo, - mSubMag); - mApproxRgbdMagSync->registerCallback(boost::bind(&RgbdSensorsSyncNodelet::callbackRGBDMag, this, - _1, _2, _3, _4, _5)); + NODELET_INFO("********** Starting nodelet '%s' **********", getName().c_str()); - } else { - NODELET_DEBUG("RGB + Depth Sync" ); + readParameters(); - mApproxRgbdSync = new message_filters::Synchronizer(ApproxRgbdSyncPolicy(mQueueSize), - mSubRgbImage, mSubDepthImage, - mSubRgbCamInfo, mSubDepthCamInfo); - mApproxRgbdSync->registerCallback(boost::bind(&RgbdSensorsSyncNodelet::callbackRGBD, this, - _1, _2, _3, _4)); - } + mPubRaw = mNhP.advertise("rgbd_sens", 1); + NODELET_INFO_STREAM("Advertised on topic " << mPubRaw.getTopic()); - } else { - NODELET_DEBUG( "Using Exact Time sync"); + if (mUseApproxSync) + { + NODELET_DEBUG("Using Approximate Time sync"); - if( mUseImu && mUseMag ) { - mExactFullSync = new message_filters::Synchronizer(ExactFullSyncPolicy(mQueueSize), - mSubRgbImage, mSubDepthImage, - mSubRgbCamInfo, mSubDepthCamInfo, - mSubImu,mSubMag); - mExactFullSync->registerCallback(boost::bind(&RgbdSensorsSyncNodelet::callbackFull, this, - _1, _2, _3, _4, _5, _6)); - - NODELET_DEBUG("RGB + Depth + IMU + Magnetometer Sync" ); - - } else if( mUseImu ) { - NODELET_DEBUG("RGB + Depth + IMU Sync" ); - - mExactRgbdImuSync = new message_filters::Synchronizer(ExactRgbdImuSyncPolicy(mQueueSize), - mSubRgbImage, mSubDepthImage, - mSubRgbCamInfo, mSubDepthCamInfo, - mSubImu); - mExactRgbdImuSync->registerCallback(boost::bind(&RgbdSensorsSyncNodelet::callbackRGBDIMU, this, - _1, _2, _3, _4, _5)); - - - } else if( mUseMag ) { - NODELET_DEBUG("RGB + Depth + Magnetometer Sync" ); - - mExactRgbdMagSync = new message_filters::Synchronizer(ExactRgbdMagSyncPolicy(mQueueSize), - mSubRgbImage, mSubDepthImage, - mSubRgbCamInfo, mSubDepthCamInfo, - mSubMag); - mExactRgbdMagSync->registerCallback(boost::bind(&RgbdSensorsSyncNodelet::callbackRGBDMag, this, - _1, _2, _3, _4, _5)); + if (mUseImu && mUseMag) + { + mApproxFullSync = new message_filters::Synchronizer( + ApproxFullSyncPolicy(mQueueSize), mSubRgbImage, mSubDepthImage, mSubRgbCamInfo, mSubDepthCamInfo, mSubImu, + mSubMag); + mApproxFullSync->registerCallback( + boost::bind(&RgbdSensorsSyncNodelet::callbackFull, this, _1, _2, _3, _4, _5, _6)); - } else { - NODELET_DEBUG("RGB + Depth Sync" ); + NODELET_DEBUG("RGB + Depth + IMU + Magnetometer Sync"); + } + else if (mUseImu) + { + NODELET_DEBUG("RGB + Depth + IMU Sync"); - mExactRgbdSync = new message_filters::Synchronizer(ExactRgbdSyncPolicy(mQueueSize), - mSubRgbImage, mSubDepthImage, - mSubRgbCamInfo, mSubDepthCamInfo); - mExactRgbdSync->registerCallback(boost::bind(&RgbdSensorsSyncNodelet::callbackRGBD, this, - _1, _2, _3, _4)); - } + mApproxRgbdImuSync = new message_filters::Synchronizer( + ApproxRgbdImuSyncPolicy(mQueueSize), mSubRgbImage, mSubDepthImage, mSubRgbCamInfo, mSubDepthCamInfo, mSubImu); + mApproxRgbdImuSync->registerCallback( + boost::bind(&RgbdSensorsSyncNodelet::callbackRGBDIMU, this, _1, _2, _3, _4, _5)); } + else if (mUseMag) + { + NODELET_DEBUG("RGB + Depth + Magnetometer Sync"); - // Create remappings - ros::NodeHandle rgb_nh(mNh, mZedNodeletName+"/rgb"); - ros::NodeHandle depth_nh(mNh, mZedNodeletName+"/depth"); - ros::NodeHandle rgb_pnh(mNhP, mZedNodeletName+"/rgb"); - ros::NodeHandle depth_pnh(mNhP, mZedNodeletName+"/depth"); - ros::NodeHandle imu_nh(mNh, mZedNodeletName+"/imu"); + mApproxRgbdMagSync = new message_filters::Synchronizer( + ApproxRgbdMagSyncPolicy(mQueueSize), mSubRgbImage, mSubDepthImage, mSubRgbCamInfo, mSubDepthCamInfo, mSubMag); + mApproxRgbdMagSync->registerCallback( + boost::bind(&RgbdSensorsSyncNodelet::callbackRGBDMag, this, _1, _2, _3, _4, _5)); + } + else + { + NODELET_DEBUG("RGB + Depth Sync"); - image_transport::ImageTransport rgb_it(rgb_nh); - image_transport::ImageTransport depth_it(depth_nh); + mApproxRgbdSync = new message_filters::Synchronizer( + ApproxRgbdSyncPolicy(mQueueSize), mSubRgbImage, mSubDepthImage, mSubRgbCamInfo, mSubDepthCamInfo); + mApproxRgbdSync->registerCallback(boost::bind(&RgbdSensorsSyncNodelet::callbackRGBD, this, _1, _2, _3, _4)); + } + } + else + { + NODELET_DEBUG("Using Exact Time sync"); - image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh); - image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh); + if (mUseImu && mUseMag) + { + mExactFullSync = new message_filters::Synchronizer( + ExactFullSyncPolicy(mQueueSize), mSubRgbImage, mSubDepthImage, mSubRgbCamInfo, mSubDepthCamInfo, mSubImu, + mSubMag); + mExactFullSync->registerCallback( + boost::bind(&RgbdSensorsSyncNodelet::callbackFull, this, _1, _2, _3, _4, _5, _6)); - mSubRgbImage.subscribe(rgb_it, rgb_nh.resolveName("image_rect_color"), 1, hintsRgb); - mSubDepthImage.subscribe(depth_it, depth_nh.resolveName("depth_registered"), 1, hintsDepth); - mSubRgbCamInfo.subscribe(rgb_nh, "camera_info", 1); - mSubDepthCamInfo.subscribe(depth_nh, "camera_info", 1); + NODELET_DEBUG("RGB + Depth + IMU + Magnetometer Sync"); + } + else if (mUseImu) + { + NODELET_DEBUG("RGB + Depth + IMU Sync"); - NODELET_INFO_STREAM( " * Subscribed to topic: " << mSubRgbImage.getTopic().c_str()); - NODELET_INFO_STREAM( " * Subscribed to topic: " << mSubRgbCamInfo.getTopic().c_str()); - NODELET_INFO_STREAM( " * Subscribed to topic: " << mSubDepthImage.getTopic().c_str()); - NODELET_INFO_STREAM( " * Subscribed to topic: " << mSubDepthCamInfo.getTopic().c_str()); + mExactRgbdImuSync = new message_filters::Synchronizer( + ExactRgbdImuSyncPolicy(mQueueSize), mSubRgbImage, mSubDepthImage, mSubRgbCamInfo, mSubDepthCamInfo, mSubImu); + mExactRgbdImuSync->registerCallback( + boost::bind(&RgbdSensorsSyncNodelet::callbackRGBDIMU, this, _1, _2, _3, _4, _5)); + } + else if (mUseMag) + { + NODELET_DEBUG("RGB + Depth + Magnetometer Sync"); - if( mUseImu ) { - mSubImu.subscribe(imu_nh,"data",1); - NODELET_INFO_STREAM( " * Subscribed to topic: " << mSubImu.getTopic().c_str()); + mExactRgbdMagSync = new message_filters::Synchronizer( + ExactRgbdMagSyncPolicy(mQueueSize), mSubRgbImage, mSubDepthImage, mSubRgbCamInfo, mSubDepthCamInfo, mSubMag); + mExactRgbdMagSync->registerCallback( + boost::bind(&RgbdSensorsSyncNodelet::callbackRGBDMag, this, _1, _2, _3, _4, _5)); } + else + { + NODELET_DEBUG("RGB + Depth Sync"); - if(mUseMag) { - mSubMag.subscribe(imu_nh,"mag",1); - NODELET_INFO_STREAM( " * Subscribed to topic: " << mSubMag.getTopic().c_str()); + mExactRgbdSync = new message_filters::Synchronizer( + ExactRgbdSyncPolicy(mQueueSize), mSubRgbImage, mSubDepthImage, mSubRgbCamInfo, mSubDepthCamInfo); + mExactRgbdSync->registerCallback(boost::bind(&RgbdSensorsSyncNodelet::callbackRGBD, this, _1, _2, _3, _4)); } + } + + // Create remappings + ros::NodeHandle rgb_nh(mNh, mZedNodeletName + "/rgb"); + ros::NodeHandle depth_nh(mNh, mZedNodeletName + "/depth"); + ros::NodeHandle rgb_pnh(mNhP, mZedNodeletName + "/rgb"); + ros::NodeHandle depth_pnh(mNhP, mZedNodeletName + "/depth"); + ros::NodeHandle imu_nh(mNh, mZedNodeletName + "/imu"); + + image_transport::ImageTransport rgb_it(rgb_nh); + image_transport::ImageTransport depth_it(depth_nh); + + image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh); + image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh); + + mSubRgbImage.subscribe(rgb_it, rgb_nh.resolveName("image_rect_color"), 1, hintsRgb); + mSubDepthImage.subscribe(depth_it, depth_nh.resolveName("depth_registered"), 1, hintsDepth); + mSubRgbCamInfo.subscribe(rgb_nh, "camera_info", 1); + mSubDepthCamInfo.subscribe(depth_nh, "camera_info", 1); + + NODELET_INFO_STREAM(" * Subscribed to topic: " << mSubRgbImage.getTopic().c_str()); + NODELET_INFO_STREAM(" * Subscribed to topic: " << mSubRgbCamInfo.getTopic().c_str()); + NODELET_INFO_STREAM(" * Subscribed to topic: " << mSubDepthImage.getTopic().c_str()); + NODELET_INFO_STREAM(" * Subscribed to topic: " << mSubDepthCamInfo.getTopic().c_str()); + + if (mUseImu) + { + mSubImu.subscribe(imu_nh, "data", 1); + NODELET_INFO_STREAM(" * Subscribed to topic: " << mSubImu.getTopic().c_str()); + } + + if (mUseMag) + { + mSubMag.subscribe(imu_nh, "mag", 1); + NODELET_INFO_STREAM(" * Subscribed to topic: " << mSubMag.getTopic().c_str()); + } } -void RgbdSensorsSyncNodelet::readParameters() { - NODELET_INFO("*** PARAMETERS [%s]***", getName().c_str()); - - mNhP.getParam("zed_nodelet_name", mZedNodeletName); - mNhP.getParam("approx_sync", mUseApproxSync); - mNhP.getParam("queue_size", mQueueSize); - mNhP.getParam("sub_imu", mUseImu); - mNhP.getParam("sub_mag", mUseMag); - - NODELET_INFO(" * zed_nodelet_name -> %s", mZedNodeletName.c_str()); - NODELET_INFO(" * approx_sync -> %s", mUseApproxSync?"true":"false"); - NODELET_INFO(" * queue_size -> %d", mQueueSize); - NODELET_INFO(" * sub_imu -> %s", mUseImu?"true":"false"); - NODELET_INFO(" * sub_mag -> %s", mUseMag?"true":"false"); +void RgbdSensorsSyncNodelet::readParameters() +{ + NODELET_INFO("*** PARAMETERS [%s]***", getName().c_str()); + + mNhP.getParam("zed_nodelet_name", mZedNodeletName); + mNhP.getParam("approx_sync", mUseApproxSync); + mNhP.getParam("queue_size", mQueueSize); + mNhP.getParam("sub_imu", mUseImu); + mNhP.getParam("sub_mag", mUseMag); + + NODELET_INFO(" * zed_nodelet_name -> %s", mZedNodeletName.c_str()); + NODELET_INFO(" * approx_sync -> %s", mUseApproxSync ? "true" : "false"); + NODELET_INFO(" * queue_size -> %d", mQueueSize); + NODELET_INFO(" * sub_imu -> %s", mUseImu ? "true" : "false"); + NODELET_INFO(" * sub_mag -> %s", mUseMag ? "true" : "false"); } -void RgbdSensorsSyncNodelet::callbackRGBD(const sensor_msgs::ImageConstPtr &rgb, const sensor_msgs::ImageConstPtr &depth, +void RgbdSensorsSyncNodelet::callbackRGBD(const sensor_msgs::ImageConstPtr &rgb, + const sensor_msgs::ImageConstPtr &depth, const sensor_msgs::CameraInfoConstPtr &rgbCameraInfo, - const sensor_msgs::CameraInfoConstPtr &depthCameraInfo) { - // ----> Frequency calculation - static std::chrono::steady_clock::time_point last_time = std::chrono::steady_clock::now(); - std::chrono::steady_clock::time_point now = std::chrono::steady_clock::now(); - - double elapsed_usec = std::chrono::duration_cast(now - last_time).count(); - last_time = now; - - double freq = 1e6/elapsed_usec; - NODELET_DEBUG( "Freq: %.2f", freq); - // <---- Frequency calculation - - double rgbStamp = rgb->header.stamp.toSec(); - double depthStamp = depth->header.stamp.toSec(); - double rgbInfoStamp = rgbCameraInfo->header.stamp.toSec(); - double depthInfoStamp = depthCameraInfo->header.stamp.toSec(); - - uint32_t subraw = mPubRaw.getNumSubscribers(); - - if(subraw==0) { - return; - } - - zed_interfaces::RGBDSensorsPtr outSyncMsg = boost::make_shared(); - - outSyncMsg->header.frame_id = rgb->header.frame_id; - outSyncMsg->header.stamp = rgb->header.stamp; - outSyncMsg->rgbCameraInfo = *rgbCameraInfo; - outSyncMsg->depthCameraInfo = *depthCameraInfo; - - outSyncMsg->rgb = *rgb; - outSyncMsg->depth = *depth; - - mPubRaw.publish(outSyncMsg); - - if( rgbStamp != rgb->header.stamp.toSec() ) - { - NODELET_ERROR("Input stamps changed between the beginning and the end of the callback! Make " - "sure the node publishing the topics doesn't override the same data after publishing them. A " - "solution is to use this node within another nodelet manager. "); - NODELET_ERROR("Stamps: " - "rgb=%f->%f", - rgbStamp, rgb->header.stamp.toSec()); - } + const sensor_msgs::CameraInfoConstPtr &depthCameraInfo) +{ + // ----> Frequency calculation + static std::chrono::steady_clock::time_point last_time = std::chrono::steady_clock::now(); + std::chrono::steady_clock::time_point now = std::chrono::steady_clock::now(); + + double elapsed_usec = std::chrono::duration_cast(now - last_time).count(); + last_time = now; + + double freq = 1e6 / elapsed_usec; + NODELET_DEBUG("Freq: %.2f", freq); + // <---- Frequency calculation + + double rgbStamp = rgb->header.stamp.toSec(); + double depthStamp = depth->header.stamp.toSec(); + double rgbInfoStamp = rgbCameraInfo->header.stamp.toSec(); + double depthInfoStamp = depthCameraInfo->header.stamp.toSec(); + + uint32_t subraw = mPubRaw.getNumSubscribers(); + + if (subraw == 0) + { + return; + } + + zed_interfaces::RGBDSensorsPtr outSyncMsg = boost::make_shared(); + + outSyncMsg->header.frame_id = rgb->header.frame_id; + outSyncMsg->header.stamp = rgb->header.stamp; + outSyncMsg->rgbCameraInfo = *rgbCameraInfo; + outSyncMsg->depthCameraInfo = *depthCameraInfo; + + outSyncMsg->rgb = *rgb; + outSyncMsg->depth = *depth; + + mPubRaw.publish(outSyncMsg); + + if (rgbStamp != rgb->header.stamp.toSec()) + { + NODELET_ERROR("Input stamps changed between the beginning and the end of the callback! Make " + "sure the node publishing the topics doesn't override the same data after publishing them. A " + "solution is to use this node within another nodelet manager. "); + NODELET_ERROR("Stamps: " + "rgb=%f->%f", + rgbStamp, rgb->header.stamp.toSec()); + } } -void RgbdSensorsSyncNodelet::callbackRGBDIMU(const sensor_msgs::ImageConstPtr &rgb, const sensor_msgs::ImageConstPtr &depth, +void RgbdSensorsSyncNodelet::callbackRGBDIMU(const sensor_msgs::ImageConstPtr &rgb, + const sensor_msgs::ImageConstPtr &depth, const sensor_msgs::CameraInfoConstPtr &rgbCameraInfo, const sensor_msgs::CameraInfoConstPtr &depthCameraInfo, - const sensor_msgs::ImuConstPtr &imu) { - // ----> Frequency calculation - static std::chrono::steady_clock::time_point last_time = std::chrono::steady_clock::now(); - std::chrono::steady_clock::time_point now = std::chrono::steady_clock::now(); - - double elapsed_usec = std::chrono::duration_cast(now - last_time).count(); - last_time = now; - - double freq = 1e6/elapsed_usec; - NODELET_DEBUG( "Freq: %.2f", freq); - // <---- Frequency calculation - - double rgbStamp = rgb->header.stamp.toSec(); - double depthStamp = depth->header.stamp.toSec(); - double rgbInfoStamp = rgbCameraInfo->header.stamp.toSec(); - double depthInfoStamp = depthCameraInfo->header.stamp.toSec(); - double imuStamp = imu->header.stamp.toSec(); - - double diff = rgbStamp-imuStamp; - - NODELET_DEBUG( "Callback RGBD+IMU - RGB TS: %.9f - IMU TS: %.9f - Diff: %.9f", - rgbStamp, imuStamp, diff); - - uint32_t subraw = mPubRaw.getNumSubscribers(); - - if(subraw==0) { - return; - } - - zed_interfaces::RGBDSensorsPtr outSyncMsg = boost::make_shared(); - - outSyncMsg->header.frame_id = rgb->header.frame_id; - outSyncMsg->header.stamp = rgb->header.stamp; - outSyncMsg->rgbCameraInfo = *rgbCameraInfo; - outSyncMsg->depthCameraInfo = *depthCameraInfo; - - outSyncMsg->rgb = *rgb; - outSyncMsg->depth = *depth; - outSyncMsg->imu = *imu; - - mPubRaw.publish(outSyncMsg); - - if( rgbStamp != rgb->header.stamp.toSec() || - imuStamp != imu->header.stamp.toSec() ) - { - NODELET_ERROR("Input stamps changed between the beginning and the end of the callback! Make " - "sure the node publishing the topics doesn't override the same data after publishing them. A " - "solution is to use this node within another nodelet manager. "); - NODELET_ERROR("Stamps: " - "rgb=%f->%f IMU=%f->%f", - rgbStamp, rgb->header.stamp.toSec(), - imuStamp, imu->header.stamp.toSec()); - } - + const sensor_msgs::ImuConstPtr &imu) +{ + // ----> Frequency calculation + static std::chrono::steady_clock::time_point last_time = std::chrono::steady_clock::now(); + std::chrono::steady_clock::time_point now = std::chrono::steady_clock::now(); + + double elapsed_usec = std::chrono::duration_cast(now - last_time).count(); + last_time = now; + + double freq = 1e6 / elapsed_usec; + NODELET_DEBUG("Freq: %.2f", freq); + // <---- Frequency calculation + + double rgbStamp = rgb->header.stamp.toSec(); + double depthStamp = depth->header.stamp.toSec(); + double rgbInfoStamp = rgbCameraInfo->header.stamp.toSec(); + double depthInfoStamp = depthCameraInfo->header.stamp.toSec(); + double imuStamp = imu->header.stamp.toSec(); + + double diff = rgbStamp - imuStamp; + + NODELET_DEBUG("Callback RGBD+IMU - RGB TS: %.9f - IMU TS: %.9f - Diff: %.9f", rgbStamp, imuStamp, diff); + + uint32_t subraw = mPubRaw.getNumSubscribers(); + + if (subraw == 0) + { + return; + } + + zed_interfaces::RGBDSensorsPtr outSyncMsg = boost::make_shared(); + + outSyncMsg->header.frame_id = rgb->header.frame_id; + outSyncMsg->header.stamp = rgb->header.stamp; + outSyncMsg->rgbCameraInfo = *rgbCameraInfo; + outSyncMsg->depthCameraInfo = *depthCameraInfo; + + outSyncMsg->rgb = *rgb; + outSyncMsg->depth = *depth; + outSyncMsg->imu = *imu; + + mPubRaw.publish(outSyncMsg); + + if (rgbStamp != rgb->header.stamp.toSec() || imuStamp != imu->header.stamp.toSec()) + { + NODELET_ERROR("Input stamps changed between the beginning and the end of the callback! Make " + "sure the node publishing the topics doesn't override the same data after publishing them. A " + "solution is to use this node within another nodelet manager. "); + NODELET_ERROR("Stamps: " + "rgb=%f->%f IMU=%f->%f", + rgbStamp, rgb->header.stamp.toSec(), imuStamp, imu->header.stamp.toSec()); + } } -void RgbdSensorsSyncNodelet::callbackRGBDMag(const sensor_msgs::ImageConstPtr &rgb, const sensor_msgs::ImageConstPtr &depth, +void RgbdSensorsSyncNodelet::callbackRGBDMag(const sensor_msgs::ImageConstPtr &rgb, + const sensor_msgs::ImageConstPtr &depth, const sensor_msgs::CameraInfoConstPtr &rgbCameraInfo, const sensor_msgs::CameraInfoConstPtr &depthCameraInfo, - const sensor_msgs::MagneticFieldConstPtr &mag) { - // ----> Frequency calculation - static std::chrono::steady_clock::time_point last_time = std::chrono::steady_clock::now(); - std::chrono::steady_clock::time_point now = std::chrono::steady_clock::now(); - - double elapsed_usec = std::chrono::duration_cast(now - last_time).count(); - last_time = now; - - double freq = 1e6/elapsed_usec; - NODELET_DEBUG( "Freq: %.2f", freq); - // <---- Frequency calculation - - - double rgbStamp = rgb->header.stamp.toSec(); - double depthStamp = depth->header.stamp.toSec(); - double rgbInfoStamp = rgbCameraInfo->header.stamp.toSec(); - double depthInfoStamp = depthCameraInfo->header.stamp.toSec(); - double magStamp = mag->header.stamp.toSec(); - - double diffMag = rgbStamp-magStamp; - - NODELET_DEBUG( "Callback rgbd+mag - RGB TS: %.9f - MAG TS: %.9f - Diff MAG: %.9f", - rgbStamp, magStamp, diffMag); - - uint32_t subraw = mPubRaw.getNumSubscribers(); - - if(subraw==0) { - return; - } - - zed_interfaces::RGBDSensorsPtr outSyncMsg = boost::make_shared(); - - outSyncMsg->header.frame_id = rgb->header.frame_id; - outSyncMsg->header.stamp = rgb->header.stamp; - outSyncMsg->rgbCameraInfo = *rgbCameraInfo; - outSyncMsg->depthCameraInfo = *depthCameraInfo; - - outSyncMsg->rgb = *rgb; - outSyncMsg->depth = *depth; - outSyncMsg->mag = *mag; - - mPubRaw.publish(outSyncMsg); - - if( rgbStamp != rgb->header.stamp.toSec() || - magStamp != mag->header.stamp.toSec() ) - { - NODELET_ERROR("Input stamps changed between the beginning and the end of the callback! Make " - "sure the node publishing the topics doesn't override the same data after publishing them. A " - "solution is to use this node within another nodelet manager. "); - NODELET_ERROR("Stamps: " - "rgb=%f->%f MAG=%f->%f", - rgbStamp, rgb->header.stamp.toSec(), - magStamp, mag->header.stamp.toSec()); - } - - + const sensor_msgs::MagneticFieldConstPtr &mag) +{ + // ----> Frequency calculation + static std::chrono::steady_clock::time_point last_time = std::chrono::steady_clock::now(); + std::chrono::steady_clock::time_point now = std::chrono::steady_clock::now(); + + double elapsed_usec = std::chrono::duration_cast(now - last_time).count(); + last_time = now; + + double freq = 1e6 / elapsed_usec; + NODELET_DEBUG("Freq: %.2f", freq); + // <---- Frequency calculation + + double rgbStamp = rgb->header.stamp.toSec(); + double depthStamp = depth->header.stamp.toSec(); + double rgbInfoStamp = rgbCameraInfo->header.stamp.toSec(); + double depthInfoStamp = depthCameraInfo->header.stamp.toSec(); + double magStamp = mag->header.stamp.toSec(); + + double diffMag = rgbStamp - magStamp; + + NODELET_DEBUG("Callback rgbd+mag - RGB TS: %.9f - MAG TS: %.9f - Diff MAG: %.9f", rgbStamp, magStamp, diffMag); + + uint32_t subraw = mPubRaw.getNumSubscribers(); + + if (subraw == 0) + { + return; + } + + zed_interfaces::RGBDSensorsPtr outSyncMsg = boost::make_shared(); + + outSyncMsg->header.frame_id = rgb->header.frame_id; + outSyncMsg->header.stamp = rgb->header.stamp; + outSyncMsg->rgbCameraInfo = *rgbCameraInfo; + outSyncMsg->depthCameraInfo = *depthCameraInfo; + + outSyncMsg->rgb = *rgb; + outSyncMsg->depth = *depth; + outSyncMsg->mag = *mag; + + mPubRaw.publish(outSyncMsg); + + if (rgbStamp != rgb->header.stamp.toSec() || magStamp != mag->header.stamp.toSec()) + { + NODELET_ERROR("Input stamps changed between the beginning and the end of the callback! Make " + "sure the node publishing the topics doesn't override the same data after publishing them. A " + "solution is to use this node within another nodelet manager. "); + NODELET_ERROR("Stamps: " + "rgb=%f->%f MAG=%f->%f", + rgbStamp, rgb->header.stamp.toSec(), magStamp, mag->header.stamp.toSec()); + } } -void RgbdSensorsSyncNodelet::callbackFull(const sensor_msgs::ImageConstPtr &rgb, const sensor_msgs::ImageConstPtr &depth, +void RgbdSensorsSyncNodelet::callbackFull(const sensor_msgs::ImageConstPtr &rgb, + const sensor_msgs::ImageConstPtr &depth, const sensor_msgs::CameraInfoConstPtr &rgbCameraInfo, const sensor_msgs::CameraInfoConstPtr &depthCameraInfo, const sensor_msgs::ImuConstPtr &imu, - const sensor_msgs::MagneticFieldConstPtr &mag) { - // ----> Frequency calculation - static std::chrono::steady_clock::time_point last_time = std::chrono::steady_clock::now(); - std::chrono::steady_clock::time_point now = std::chrono::steady_clock::now(); - - double elapsed_usec = std::chrono::duration_cast(now - last_time).count(); - last_time = now; - - double freq = 1e6/elapsed_usec; - NODELET_DEBUG( "Freq: %.2f", freq); - // <---- Frequency calculation - - double rgbStamp = rgb->header.stamp.toSec(); - double depthStamp = depth->header.stamp.toSec(); - double rgbInfoStamp = rgbCameraInfo->header.stamp.toSec(); - double depthInfoStamp = depthCameraInfo->header.stamp.toSec(); - double imuStamp = imu->header.stamp.toSec(); - double magStamp = mag->header.stamp.toSec(); - - double diffImu = rgbStamp-imuStamp; - double diffMag = rgbStamp-magStamp; - - NODELET_DEBUG( "Callback FULL - RGB TS: %.9f - IMU TS: %.9f - Diff IMU: %.9f", - rgbStamp, imuStamp, diffImu); - NODELET_DEBUG( "Callback FULL - RGB TS: %.9f - MAG TS: %.9f - Diff MAG: %.9f", - rgbStamp, magStamp, diffMag); - - - uint32_t subraw = mPubRaw.getNumSubscribers(); - - if(subraw==0) { - return; - } - - zed_interfaces::RGBDSensorsPtr outSyncMsg = boost::make_shared(); - - outSyncMsg->header.frame_id = rgb->header.frame_id; - outSyncMsg->header.stamp = rgb->header.stamp; - outSyncMsg->rgbCameraInfo = *rgbCameraInfo; - outSyncMsg->depthCameraInfo = *depthCameraInfo; - - outSyncMsg->rgb = *rgb; - outSyncMsg->depth = *depth; - outSyncMsg->imu = *imu; - outSyncMsg->mag = *mag; - - mPubRaw.publish(outSyncMsg); - - if( rgbStamp != rgb->header.stamp.toSec() || - imuStamp != imu->header.stamp.toSec() || - magStamp != mag->header.stamp.toSec() ) - { - NODELET_ERROR("Input stamps changed between the beginning and the end of the callback! Make " - "sure the node publishing the topics doesn't override the same data after publishing them. A " - "solution is to use this node within another nodelet manager. "); - NODELET_ERROR("Stamps: " - "rgb=%f->%f IMU=%f->%f MAG=%f->%f", - rgbStamp, rgb->header.stamp.toSec(), - imuStamp, imu->header.stamp.toSec(), - magStamp, mag->header.stamp.toSec()); - } + const sensor_msgs::MagneticFieldConstPtr &mag) +{ + // ----> Frequency calculation + static std::chrono::steady_clock::time_point last_time = std::chrono::steady_clock::now(); + std::chrono::steady_clock::time_point now = std::chrono::steady_clock::now(); + + double elapsed_usec = std::chrono::duration_cast(now - last_time).count(); + last_time = now; + + double freq = 1e6 / elapsed_usec; + NODELET_DEBUG("Freq: %.2f", freq); + // <---- Frequency calculation + + double rgbStamp = rgb->header.stamp.toSec(); + double depthStamp = depth->header.stamp.toSec(); + double rgbInfoStamp = rgbCameraInfo->header.stamp.toSec(); + double depthInfoStamp = depthCameraInfo->header.stamp.toSec(); + double imuStamp = imu->header.stamp.toSec(); + double magStamp = mag->header.stamp.toSec(); + + double diffImu = rgbStamp - imuStamp; + double diffMag = rgbStamp - magStamp; + + NODELET_DEBUG("Callback FULL - RGB TS: %.9f - IMU TS: %.9f - Diff IMU: %.9f", rgbStamp, imuStamp, diffImu); + NODELET_DEBUG("Callback FULL - RGB TS: %.9f - MAG TS: %.9f - Diff MAG: %.9f", rgbStamp, magStamp, diffMag); + + uint32_t subraw = mPubRaw.getNumSubscribers(); + + if (subraw == 0) + { + return; + } + + zed_interfaces::RGBDSensorsPtr outSyncMsg = boost::make_shared(); + + outSyncMsg->header.frame_id = rgb->header.frame_id; + outSyncMsg->header.stamp = rgb->header.stamp; + outSyncMsg->rgbCameraInfo = *rgbCameraInfo; + outSyncMsg->depthCameraInfo = *depthCameraInfo; + + outSyncMsg->rgb = *rgb; + outSyncMsg->depth = *depth; + outSyncMsg->imu = *imu; + outSyncMsg->mag = *mag; + + mPubRaw.publish(outSyncMsg); + + if (rgbStamp != rgb->header.stamp.toSec() || imuStamp != imu->header.stamp.toSec() || + magStamp != mag->header.stamp.toSec()) + { + NODELET_ERROR("Input stamps changed between the beginning and the end of the callback! Make " + "sure the node publishing the topics doesn't override the same data after publishing them. A " + "solution is to use this node within another nodelet manager. "); + NODELET_ERROR("Stamps: " + "rgb=%f->%f IMU=%f->%f MAG=%f->%f", + rgbStamp, rgb->header.stamp.toSec(), imuStamp, imu->header.stamp.toSec(), magStamp, + mag->header.stamp.toSec()); + } } -} +} // namespace zed_nodelets diff --git a/zed_nodelets/src/tools/include/sl_tools.h b/zed_nodelets/src/tools/include/sl_tools.h index 2caadbda..e8025009 100644 --- a/zed_nodelets/src/tools/include/sl_tools.h +++ b/zed_nodelets/src/tools/include/sl_tools.h @@ -27,96 +27,98 @@ #include #include -namespace sl_tools { - - /*! \brief Check if a ZED camera is ready - * \param serial_number : the serial number of the camera to be checked - */ - int checkCameraReady(unsigned int serial_number); - - /*! \brief Get ZED camera properties - * \param serial_number : the serial number of the camera - */ - sl::DeviceProperties getZEDFromSN(unsigned int serial_number); - - std::vector convertRodrigues(sl::float3 r); - - /*! \brief Test if a file exist - * \param name : the path to the file - */ - bool file_exist(const std::string& name); - - /*! \brief Get Stereolabs SDK version - * \param major : major value for version - * \param minor : minor value for version - * \param sub_minor _ sub_minor value for version - */ - std::string getSDKVersion(int& major, int& minor, int& sub_minor); - - /*! \brief Convert StereoLabs timestamp to ROS timestamp - * \param t : Stereolabs timestamp to be converted - */ - ros::Time slTime2Ros(sl::Timestamp t); - - /*! \brief sl::Mat to ros message conversion - * \param imgMsgPtr : the image topic message to publish - * \param img : the image to publish - * \param frameId : the id of the reference frame of the image - * \param t : the ros::Time to stamp the image - */ - void imageToROSmsg(sensor_msgs::ImagePtr imgMsgPtr, sl::Mat img, std::string frameId, ros::Time t); - - /*! \brief Two sl::Mat to ros message conversion - * \param imgMsgPtr : the image topic message to publish - * \param left : the left image to publish - * \param right : the right image to publish - * \param frameId : the id of the reference frame of the image - * \param t : the ros::Time to stamp the image - */ - void imagesToROSmsg(sensor_msgs::ImagePtr imgMsgPtr, sl::Mat left, sl::Mat right, std::string frameId, ros::Time t); - - /*! \brief String tokenization - */ - std::vector split_string(const std::string& s, char seperator); - - /*! - * \brief The CSmartMean class is used to - * make a mobile window mean of a sequence of values - * and reject outliers. - * Tutorial: - * https://www.myzhar.com/blog/tutorials/tutorial-exponential-weighted-average-good-moving-windows-average/ - */ - class CSmartMean { - public: - CSmartMean(int winSize); - - int getValCount() { - return mValCount; ///< Return the number of values in the sequence - } - - double getMean() { - return mMean; ///< Return the updated mean - } - - /*! - * \brief addValue - * Add a value to the sequence - * \param val value to be added - * \return mean value - */ - double addValue(double val); - - private: - int mWinSize; ///< The size of the window (number of values ti evaluate) - int mValCount; ///< The number of values in sequence - - double mMeanCorr; ///< Used for bias correction - double mMean; ///< The mean of the last \ref mWinSize values - - double mGamma; ///< Weight value - }; - - -} // namespace sl_tools +namespace sl_tools +{ +/*! \brief Check if a ZED camera is ready + * \param serial_number : the serial number of the camera to be checked + */ +int checkCameraReady(unsigned int serial_number); + +/*! \brief Get ZED camera properties + * \param serial_number : the serial number of the camera + */ +sl::DeviceProperties getZEDFromSN(unsigned int serial_number); + +std::vector convertRodrigues(sl::float3 r); + +/*! \brief Test if a file exist + * \param name : the path to the file + */ +bool file_exist(const std::string& name); + +/*! \brief Get Stereolabs SDK version + * \param major : major value for version + * \param minor : minor value for version + * \param sub_minor _ sub_minor value for version + */ +std::string getSDKVersion(int& major, int& minor, int& sub_minor); + +/*! \brief Convert StereoLabs timestamp to ROS timestamp + * \param t : Stereolabs timestamp to be converted + */ +ros::Time slTime2Ros(sl::Timestamp t); + +/*! \brief sl::Mat to ros message conversion + * \param imgMsgPtr : the image topic message to publish + * \param img : the image to publish + * \param frameId : the id of the reference frame of the image + * \param t : the ros::Time to stamp the image + */ +void imageToROSmsg(sensor_msgs::ImagePtr imgMsgPtr, sl::Mat img, std::string frameId, ros::Time t); + +/*! \brief Two sl::Mat to ros message conversion + * \param imgMsgPtr : the image topic message to publish + * \param left : the left image to publish + * \param right : the right image to publish + * \param frameId : the id of the reference frame of the image + * \param t : the ros::Time to stamp the image + */ +void imagesToROSmsg(sensor_msgs::ImagePtr imgMsgPtr, sl::Mat left, sl::Mat right, std::string frameId, ros::Time t); + +/*! \brief String tokenization + */ +std::vector split_string(const std::string& s, char seperator); + +/*! + * \brief The CSmartMean class is used to + * make a mobile window mean of a sequence of values + * and reject outliers. + * Tutorial: + * https://www.myzhar.com/blog/tutorials/tutorial-exponential-weighted-average-good-moving-windows-average/ + */ +class CSmartMean +{ +public: + CSmartMean(int winSize); + + int getValCount() + { + return mValCount; ///< Return the number of values in the sequence + } + + double getMean() + { + return mMean; ///< Return the updated mean + } + + /*! + * \brief addValue + * Add a value to the sequence + * \param val value to be added + * \return mean value + */ + double addValue(double val); + +private: + int mWinSize; ///< The size of the window (number of values ti evaluate) + int mValCount; ///< The number of values in sequence + + double mMeanCorr; ///< Used for bias correction + double mMean; ///< The mean of the last \ref mWinSize values + + double mGamma; ///< Weight value +}; + +} // namespace sl_tools #endif // SL_TOOLS_H diff --git a/zed_nodelets/src/tools/src/sl_tools.cpp b/zed_nodelets/src/tools/src/sl_tools.cpp index 8dc02507..01e04572 100644 --- a/zed_nodelets/src/tools/src/sl_tools.cpp +++ b/zed_nodelets/src/tools/src/sl_tools.cpp @@ -20,355 +20,370 @@ #include "sl_tools.h" -#include #include +#include #include #include #include -namespace sl_tools { - - int checkCameraReady(unsigned int serial_number) { - int id = -1; - auto f = sl::Camera::getDeviceList(); - - for (auto& it : f) - if (it.serial_number == serial_number && - it.camera_state == sl::CAMERA_STATE::AVAILABLE) { - id = it.id; - } - - return id; - } - - sl::DeviceProperties getZEDFromSN(unsigned int serial_number) { - sl::DeviceProperties prop; - auto f = sl::Camera::getDeviceList(); - - for (auto& it : f) { - if (it.serial_number == serial_number && - it.camera_state == sl::CAMERA_STATE::AVAILABLE) { - prop = it; - } - } - - return prop; - } - - std::vector convertRodrigues(sl::float3 r) { - float theta = sqrt(r.x * r.x + r.y * r.y + r.z * r.z); - - std::vector R = {1.0f, 0.0f, 0.0f, - 0.0f, 1.0f, 0.0f, - 0.0f, 0.0f, 1.0f - }; - - if (theta < DBL_EPSILON) { - return R; - } else { - float c = cos(theta); - float s = sin(theta); - float c1 = 1.f - c; - float itheta = theta ? 1.f / theta : 0.f; - - r *= itheta; - - std::vector rrt = {1.0f, 0.0f, 0.0f, - 0.0f, 1.0f, 0.0f, - 0.0f, 0.0f, 1.0f - }; - - float* p = rrt.data(); - p[0] = r.x * r.x; - p[1] = r.x * r.y; - p[2] = r.x * r.z; - p[3] = r.x * r.y; - p[4] = r.y * r.y; - p[5] = r.y * r.z; - p[6] = r.x * r.z; - p[7] = r.y * r.z; - p[8] = r.z * r.z; - - std::vector r_x = {1.0f, 0.0f, 0.0f, - 0.0f, 1.0f, 0.0f, - 0.0f, 0.0f, 1.0f - }; - p = r_x.data(); - p[0] = 0; - p[1] = -r.z; - p[2] = r.y; - p[3] = r.z; - p[4] = 0; - p[5] = -r.x; - p[6] = -r.y; - p[7] = r.x; - p[8] = 0; - - // R = cos(theta)*I + (1 - cos(theta))*r*rT + sin(theta)*[r_x] - - sl::Matrix3f eye; - eye.setIdentity(); - - sl::Matrix3f sl_R(R.data()); - sl::Matrix3f sl_rrt(rrt.data()); - sl::Matrix3f sl_r_x(r_x.data()); - - sl_R = eye * c + sl_rrt * c1 + sl_r_x * s; - - R[0] = sl_R.r00; - R[1] = sl_R.r01; - R[2] = sl_R.r02; - R[3] = sl_R.r10; - R[4] = sl_R.r11; - R[5] = sl_R.r12; - R[6] = sl_R.r20; - R[7] = sl_R.r21; - R[8] = sl_R.r22; - } - - return R; - } - - - bool file_exist(const std::string& name) { - struct stat buffer; - return (stat(name.c_str(), &buffer) == 0); +namespace sl_tools +{ +int checkCameraReady(unsigned int serial_number) +{ + int id = -1; + auto f = sl::Camera::getDeviceList(); + + for (auto& it : f) + if (it.serial_number == serial_number && it.camera_state == sl::CAMERA_STATE::AVAILABLE) + { + id = it.id; } - std::string getSDKVersion(int& major, int& minor, int& sub_minor) { - std::string ver = sl::Camera::getSDKVersion().c_str(); - std::vector strings; - std::istringstream f(ver); - std::string s; - - while (getline(f, s, '.')) { - strings.push_back(s); - } - - major = 0; - minor = 0; - sub_minor = 0; - - switch (strings.size()) { - case 3: - sub_minor = std::stoi(strings[2]); + return id; +} - case 2: - minor = std::stoi(strings[1]); - - case 1: - major = std::stoi(strings[0]); - } - - return ver; - } +sl::DeviceProperties getZEDFromSN(unsigned int serial_number) +{ + sl::DeviceProperties prop; + auto f = sl::Camera::getDeviceList(); - ros::Time slTime2Ros(sl::Timestamp t) { - uint32_t sec = static_cast(t.getNanoseconds() / 1000000000); - uint32_t nsec = static_cast(t.getNanoseconds() % 1000000000); - return ros::Time(sec, nsec); + for (auto& it : f) + { + if (it.serial_number == serial_number && it.camera_state == sl::CAMERA_STATE::AVAILABLE) + { + prop = it; } - - void imageToROSmsg(sensor_msgs::ImagePtr imgMsgPtr, sl::Mat img, std::string frameId, ros::Time t) { - - if(!imgMsgPtr) { - return; - } - - imgMsgPtr->header.stamp = t; - imgMsgPtr->header.frame_id = frameId; - imgMsgPtr->height = img.getHeight(); - imgMsgPtr->width = img.getWidth(); - - int num = 1; // for endianness detection - imgMsgPtr->is_bigendian = !(*(char*)&num == 1); - - imgMsgPtr->step = img.getStepBytes(); - - size_t size = imgMsgPtr->step * imgMsgPtr->height; - imgMsgPtr->data.resize(size); - - sl::MAT_TYPE dataType = img.getDataType(); - - switch (dataType) { - case sl::MAT_TYPE::F32_C1: /**< float 1 channel.*/ - imgMsgPtr->encoding = sensor_msgs::image_encodings::TYPE_32FC1; - memcpy((char*)(&imgMsgPtr->data[0]), img.getPtr(), size); - break; - - case sl::MAT_TYPE::F32_C2: /**< float 2 channels.*/ - imgMsgPtr->encoding = sensor_msgs::image_encodings::TYPE_32FC2; - memcpy((char*)(&imgMsgPtr->data[0]), img.getPtr(), size); - break; - - case sl::MAT_TYPE::F32_C3: /**< float 3 channels.*/ - imgMsgPtr->encoding = sensor_msgs::image_encodings::TYPE_32FC3; - memcpy((char*)(&imgMsgPtr->data[0]), img.getPtr(), size); - break; - - case sl::MAT_TYPE::F32_C4: /**< float 4 channels.*/ - imgMsgPtr->encoding = sensor_msgs::image_encodings::TYPE_32FC4; - memcpy((char*)(&imgMsgPtr->data[0]), img.getPtr(), size); - break; - - case sl::MAT_TYPE::U8_C1: /**< unsigned char 1 channel.*/ - imgMsgPtr->encoding = sensor_msgs::image_encodings::MONO8; - memcpy((char*)(&imgMsgPtr->data[0]), img.getPtr(), size); - break; - - case sl::MAT_TYPE::U8_C2: /**< unsigned char 2 channels.*/ - imgMsgPtr->encoding = sensor_msgs::image_encodings::TYPE_8UC2; - memcpy((char*)(&imgMsgPtr->data[0]), img.getPtr(), size); - break; - - case sl::MAT_TYPE::U8_C3: /**< unsigned char 3 channels.*/ - imgMsgPtr->encoding = sensor_msgs::image_encodings::BGR8; - memcpy((char*)(&imgMsgPtr->data[0]), img.getPtr(), size); - break; - - case sl::MAT_TYPE::U8_C4: /**< unsigned char 4 channels.*/ - imgMsgPtr->encoding = sensor_msgs::image_encodings::BGRA8; - memcpy((char*)(&imgMsgPtr->data[0]), img.getPtr(), size); - break; - } - } - - void imagesToROSmsg(sensor_msgs::ImagePtr imgMsgPtr, sl::Mat left, sl::Mat right, std::string frameId, ros::Time t) { - - if (left.getWidth() != right.getWidth() || - left.getHeight() != right.getHeight() || - left.getChannels() != right.getChannels() || - left.getDataType() != right.getDataType()) { - return; - } - - if(!imgMsgPtr) { - imgMsgPtr = boost::make_shared(); - } - - imgMsgPtr->header.stamp = t; - imgMsgPtr->header.frame_id = frameId; - imgMsgPtr->height = left.getHeight(); - imgMsgPtr->width = 2 * left.getWidth(); - - int num = 1; // for endianness detection - imgMsgPtr->is_bigendian = !(*(char*)&num == 1); - - imgMsgPtr->step = 2 * left.getStepBytes(); - - size_t size = imgMsgPtr->step * imgMsgPtr->height; - imgMsgPtr->data.resize(size); - - sl::MAT_TYPE dataType = left.getDataType(); - - int dataSize = 0; - char* srcL; - char* srcR; - - switch (dataType) { - case sl::MAT_TYPE::F32_C1: /**< float 1 channel.*/ - imgMsgPtr->encoding = sensor_msgs::image_encodings::TYPE_32FC1; - dataSize = sizeof(float); - srcL = (char*)left.getPtr(); - srcR = (char*)right.getPtr(); - break; - - case sl::MAT_TYPE::F32_C2: /**< float 2 channels.*/ - imgMsgPtr->encoding = sensor_msgs::image_encodings::TYPE_32FC2; - dataSize = 2 * sizeof(float); - srcL = (char*)left.getPtr(); - srcR = (char*)right.getPtr(); - break; - - case sl::MAT_TYPE::F32_C3: /**< float 3 channels.*/ - imgMsgPtr->encoding = sensor_msgs::image_encodings::TYPE_32FC3; - dataSize = 3 * sizeof(float); - srcL = (char*)left.getPtr(); - srcR = (char*)right.getPtr(); - break; - - case sl::MAT_TYPE::F32_C4: /**< float 4 channels.*/ - imgMsgPtr->encoding = sensor_msgs::image_encodings::TYPE_32FC4; - dataSize = 4 * sizeof(float); - srcL = (char*)left.getPtr(); - srcR = (char*)right.getPtr(); - break; - - case sl::MAT_TYPE::U8_C1: /**< unsigned char 1 channel.*/ - imgMsgPtr->encoding = sensor_msgs::image_encodings::MONO8; - dataSize = sizeof(char); - srcL = (char*)left.getPtr(); - srcR = (char*)right.getPtr(); - break; - - case sl::MAT_TYPE::U8_C2: /**< unsigned char 2 channels.*/ - imgMsgPtr->encoding = sensor_msgs::image_encodings::TYPE_8UC2; - dataSize = 2 * sizeof(char); - srcL = (char*)left.getPtr(); - srcR = (char*)right.getPtr(); - break; - - case sl::MAT_TYPE::U8_C3: /**< unsigned char 3 channels.*/ - imgMsgPtr->encoding = sensor_msgs::image_encodings::BGR8; - dataSize = 3 * sizeof(char); - srcL = (char*)left.getPtr(); - srcR = (char*)right.getPtr(); - break; - - case sl::MAT_TYPE::U8_C4: /**< unsigned char 4 channels.*/ - imgMsgPtr->encoding = sensor_msgs::image_encodings::BGRA8; - dataSize = 4 * sizeof(char); - srcL = (char*)left.getPtr(); - srcR = (char*)right.getPtr(); - break; - } - - char* dest = (char*)(&imgMsgPtr->data[0]); - - for (int i = 0; i < left.getHeight(); i++) { - memcpy(dest, srcL, left.getStepBytes()); - dest += left.getStepBytes(); - memcpy(dest, srcR, right.getStepBytes()); - dest += right.getStepBytes(); - - srcL += left.getStepBytes(); - srcR += right.getStepBytes(); - } - } - - std::vector split_string(const std::string& s, char seperator) { - std::vector output; - std::string::size_type prev_pos = 0, pos = 0; - - while ((pos = s.find(seperator, pos)) != std::string::npos) { - std::string substring(s.substr(prev_pos, pos - prev_pos)); - output.push_back(substring); - prev_pos = ++pos; - } - - output.push_back(s.substr(prev_pos, pos - prev_pos)); - return output; - } - - CSmartMean::CSmartMean(int winSize) { - mValCount = 0; - - mMeanCorr = 0.0; - mMean = 0.0; - mWinSize = winSize; - - mGamma = (static_cast(mWinSize) - 1.) / static_cast(mWinSize); - } - - double CSmartMean::addValue(double val) { - mValCount++; - - mMeanCorr = mGamma * mMeanCorr + (1. - mGamma) * val; - mMean = mMeanCorr / (1. - pow(mGamma, mValCount)); - - return mMean; - } - -} // namespace + } + + return prop; +} + +std::vector convertRodrigues(sl::float3 r) +{ + float theta = sqrt(r.x * r.x + r.y * r.y + r.z * r.z); + + std::vector R = { 1.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 1.0f }; + + if (theta < DBL_EPSILON) + { + return R; + } + else + { + float c = cos(theta); + float s = sin(theta); + float c1 = 1.f - c; + float itheta = theta ? 1.f / theta : 0.f; + + r *= itheta; + + std::vector rrt = { 1.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 1.0f }; + + float* p = rrt.data(); + p[0] = r.x * r.x; + p[1] = r.x * r.y; + p[2] = r.x * r.z; + p[3] = r.x * r.y; + p[4] = r.y * r.y; + p[5] = r.y * r.z; + p[6] = r.x * r.z; + p[7] = r.y * r.z; + p[8] = r.z * r.z; + + std::vector r_x = { 1.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 1.0f }; + p = r_x.data(); + p[0] = 0; + p[1] = -r.z; + p[2] = r.y; + p[3] = r.z; + p[4] = 0; + p[5] = -r.x; + p[6] = -r.y; + p[7] = r.x; + p[8] = 0; + + // R = cos(theta)*I + (1 - cos(theta))*r*rT + sin(theta)*[r_x] + + sl::Matrix3f eye; + eye.setIdentity(); + + sl::Matrix3f sl_R(R.data()); + sl::Matrix3f sl_rrt(rrt.data()); + sl::Matrix3f sl_r_x(r_x.data()); + + sl_R = eye * c + sl_rrt * c1 + sl_r_x * s; + + R[0] = sl_R.r00; + R[1] = sl_R.r01; + R[2] = sl_R.r02; + R[3] = sl_R.r10; + R[4] = sl_R.r11; + R[5] = sl_R.r12; + R[6] = sl_R.r20; + R[7] = sl_R.r21; + R[8] = sl_R.r22; + } + + return R; +} + +bool file_exist(const std::string& name) +{ + struct stat buffer; + return (stat(name.c_str(), &buffer) == 0); +} + +std::string getSDKVersion(int& major, int& minor, int& sub_minor) +{ + std::string ver = sl::Camera::getSDKVersion().c_str(); + std::vector strings; + std::istringstream f(ver); + std::string s; + + while (getline(f, s, '.')) + { + strings.push_back(s); + } + + major = 0; + minor = 0; + sub_minor = 0; + + switch (strings.size()) + { + case 3: + sub_minor = std::stoi(strings[2]); + + case 2: + minor = std::stoi(strings[1]); + + case 1: + major = std::stoi(strings[0]); + } + + return ver; +} + +ros::Time slTime2Ros(sl::Timestamp t) +{ + uint32_t sec = static_cast(t.getNanoseconds() / 1000000000); + uint32_t nsec = static_cast(t.getNanoseconds() % 1000000000); + return ros::Time(sec, nsec); +} + +void imageToROSmsg(sensor_msgs::ImagePtr imgMsgPtr, sl::Mat img, std::string frameId, ros::Time t) +{ + if (!imgMsgPtr) + { + return; + } + + imgMsgPtr->header.stamp = t; + imgMsgPtr->header.frame_id = frameId; + imgMsgPtr->height = img.getHeight(); + imgMsgPtr->width = img.getWidth(); + + int num = 1; // for endianness detection + imgMsgPtr->is_bigendian = !(*(char*)&num == 1); + + imgMsgPtr->step = img.getStepBytes(); + + size_t size = imgMsgPtr->step * imgMsgPtr->height; + imgMsgPtr->data.resize(size); + + sl::MAT_TYPE dataType = img.getDataType(); + + switch (dataType) + { + case sl::MAT_TYPE::F32_C1: /**< float 1 channel.*/ + imgMsgPtr->encoding = sensor_msgs::image_encodings::TYPE_32FC1; + memcpy((char*)(&imgMsgPtr->data[0]), img.getPtr(), size); + break; + + case sl::MAT_TYPE::F32_C2: /**< float 2 channels.*/ + imgMsgPtr->encoding = sensor_msgs::image_encodings::TYPE_32FC2; + memcpy((char*)(&imgMsgPtr->data[0]), img.getPtr(), size); + break; + + case sl::MAT_TYPE::F32_C3: /**< float 3 channels.*/ + imgMsgPtr->encoding = sensor_msgs::image_encodings::TYPE_32FC3; + memcpy((char*)(&imgMsgPtr->data[0]), img.getPtr(), size); + break; + + case sl::MAT_TYPE::F32_C4: /**< float 4 channels.*/ + imgMsgPtr->encoding = sensor_msgs::image_encodings::TYPE_32FC4; + memcpy((char*)(&imgMsgPtr->data[0]), img.getPtr(), size); + break; + + case sl::MAT_TYPE::U8_C1: /**< unsigned char 1 channel.*/ + imgMsgPtr->encoding = sensor_msgs::image_encodings::MONO8; + memcpy((char*)(&imgMsgPtr->data[0]), img.getPtr(), size); + break; + + case sl::MAT_TYPE::U8_C2: /**< unsigned char 2 channels.*/ + imgMsgPtr->encoding = sensor_msgs::image_encodings::TYPE_8UC2; + memcpy((char*)(&imgMsgPtr->data[0]), img.getPtr(), size); + break; + + case sl::MAT_TYPE::U8_C3: /**< unsigned char 3 channels.*/ + imgMsgPtr->encoding = sensor_msgs::image_encodings::BGR8; + memcpy((char*)(&imgMsgPtr->data[0]), img.getPtr(), size); + break; + + case sl::MAT_TYPE::U8_C4: /**< unsigned char 4 channels.*/ + imgMsgPtr->encoding = sensor_msgs::image_encodings::BGRA8; + memcpy((char*)(&imgMsgPtr->data[0]), img.getPtr(), size); + break; + + case sl::MAT_TYPE::U16_C1: /**< unsigned short 1 channel.*/ + imgMsgPtr->encoding = sensor_msgs::image_encodings::MONO16; + memcpy((uint16_t*)(&imgMsgPtr->data[0]), img.getPtr(), size); + break; + } +} + +void imagesToROSmsg(sensor_msgs::ImagePtr imgMsgPtr, sl::Mat left, sl::Mat right, std::string frameId, ros::Time t) +{ + if (left.getWidth() != right.getWidth() || left.getHeight() != right.getHeight() || + left.getChannels() != right.getChannels() || left.getDataType() != right.getDataType()) + { + return; + } + + if (!imgMsgPtr) + { + imgMsgPtr = boost::make_shared(); + } + + imgMsgPtr->header.stamp = t; + imgMsgPtr->header.frame_id = frameId; + imgMsgPtr->height = left.getHeight(); + imgMsgPtr->width = 2 * left.getWidth(); + + int num = 1; // for endianness detection + imgMsgPtr->is_bigendian = !(*(char*)&num == 1); + + imgMsgPtr->step = 2 * left.getStepBytes(); + + size_t size = imgMsgPtr->step * imgMsgPtr->height; + imgMsgPtr->data.resize(size); + + sl::MAT_TYPE dataType = left.getDataType(); + + int dataSize = 0; + char* srcL; + char* srcR; + + switch (dataType) + { + case sl::MAT_TYPE::F32_C1: /**< float 1 channel.*/ + imgMsgPtr->encoding = sensor_msgs::image_encodings::TYPE_32FC1; + dataSize = sizeof(float); + srcL = (char*)left.getPtr(); + srcR = (char*)right.getPtr(); + break; + + case sl::MAT_TYPE::F32_C2: /**< float 2 channels.*/ + imgMsgPtr->encoding = sensor_msgs::image_encodings::TYPE_32FC2; + dataSize = 2 * sizeof(float); + srcL = (char*)left.getPtr(); + srcR = (char*)right.getPtr(); + break; + + case sl::MAT_TYPE::F32_C3: /**< float 3 channels.*/ + imgMsgPtr->encoding = sensor_msgs::image_encodings::TYPE_32FC3; + dataSize = 3 * sizeof(float); + srcL = (char*)left.getPtr(); + srcR = (char*)right.getPtr(); + break; + + case sl::MAT_TYPE::F32_C4: /**< float 4 channels.*/ + imgMsgPtr->encoding = sensor_msgs::image_encodings::TYPE_32FC4; + dataSize = 4 * sizeof(float); + srcL = (char*)left.getPtr(); + srcR = (char*)right.getPtr(); + break; + + case sl::MAT_TYPE::U8_C1: /**< unsigned char 1 channel.*/ + imgMsgPtr->encoding = sensor_msgs::image_encodings::MONO8; + dataSize = sizeof(char); + srcL = (char*)left.getPtr(); + srcR = (char*)right.getPtr(); + break; + + case sl::MAT_TYPE::U8_C2: /**< unsigned char 2 channels.*/ + imgMsgPtr->encoding = sensor_msgs::image_encodings::TYPE_8UC2; + dataSize = 2 * sizeof(char); + srcL = (char*)left.getPtr(); + srcR = (char*)right.getPtr(); + break; + + case sl::MAT_TYPE::U8_C3: /**< unsigned char 3 channels.*/ + imgMsgPtr->encoding = sensor_msgs::image_encodings::BGR8; + dataSize = 3 * sizeof(char); + srcL = (char*)left.getPtr(); + srcR = (char*)right.getPtr(); + break; + + case sl::MAT_TYPE::U8_C4: /**< unsigned char 4 channels.*/ + imgMsgPtr->encoding = sensor_msgs::image_encodings::BGRA8; + dataSize = 4 * sizeof(char); + srcL = (char*)left.getPtr(); + srcR = (char*)right.getPtr(); + break; + } + + char* dest = (char*)(&imgMsgPtr->data[0]); + + for (int i = 0; i < left.getHeight(); i++) + { + memcpy(dest, srcL, left.getStepBytes()); + dest += left.getStepBytes(); + memcpy(dest, srcR, right.getStepBytes()); + dest += right.getStepBytes(); + + srcL += left.getStepBytes(); + srcR += right.getStepBytes(); + } +} + +std::vector split_string(const std::string& s, char seperator) +{ + std::vector output; + std::string::size_type prev_pos = 0, pos = 0; + + while ((pos = s.find(seperator, pos)) != std::string::npos) + { + std::string substring(s.substr(prev_pos, pos - prev_pos)); + output.push_back(substring); + prev_pos = ++pos; + } + + output.push_back(s.substr(prev_pos, pos - prev_pos)); + return output; +} + +CSmartMean::CSmartMean(int winSize) +{ + mValCount = 0; + + mMeanCorr = 0.0; + mMean = 0.0; + mWinSize = winSize; + + mGamma = (static_cast(mWinSize) - 1.) / static_cast(mWinSize); +} + +double CSmartMean::addValue(double val) +{ + mValCount++; + + mMeanCorr = mGamma * mMeanCorr + (1. - mGamma) * val; + mMean = mMeanCorr / (1. - pow(mGamma, mValCount)); + + return mMean; +} + +} // namespace sl_tools 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 03da5dc4..a3535d0e 100644 --- a/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp +++ b/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp @@ -25,684 +25,684 @@ #include -#include -#include -#include -#include -#include -#include -#include +#include #include #include +#include +#include +#include #include -#include +#include +#include +#include +#include // Dynamic reconfiguration #include // Services -#include -#include #include -#include -#include -#include -#include +#include #include -#include +#include #include -#include #include +#include +#include +#include #include +#include +#include +#include // Topics +#include #include #include -#include #include -#include +#include #include +#include #include #include #include #include -#include #include +#include #include #include #include -#include using namespace std; -namespace zed_nodelets { - -class ZEDWrapperNodelet : public nodelet::Nodelet { - - typedef enum _dyn_params { - DATAPUB_FREQ = 0, - CONFIDENCE = 1, - TEXTURE_CONF = 2, - POINTCLOUD_FREQ = 3, - BRIGHTNESS = 4, - CONTRAST = 5, - HUE = 6, - SATURATION = 7, - SHARPNESS = 8, - GAMMA = 9, - AUTO_EXP_GAIN = 10, - GAIN = 11, - EXPOSURE = 12, - AUTO_WB = 13, - WB_TEMP = 14 - } DynParams; +namespace zed_nodelets +{ +class ZEDWrapperNodelet : public nodelet::Nodelet +{ + typedef enum _dyn_params + { + DATAPUB_FREQ = 0, + CONFIDENCE = 1, + TEXTURE_CONF = 2, + POINTCLOUD_FREQ = 3, + BRIGHTNESS = 4, + CONTRAST = 5, + HUE = 6, + SATURATION = 7, + SHARPNESS = 8, + GAMMA = 9, + AUTO_EXP_GAIN = 10, + GAIN = 11, + EXPOSURE = 12, + AUTO_WB = 13, + WB_TEMP = 14 + } DynParams; public: - /*! \brief Default constructor - */ - ZEDWrapperNodelet(); + /*! \brief Default constructor + */ + ZEDWrapperNodelet(); - /*! \brief \ref ZEDWrapperNodelet destructor - */ - virtual ~ZEDWrapperNodelet(); + /*! \brief \ref ZEDWrapperNodelet destructor + */ + virtual ~ZEDWrapperNodelet(); protected: - /*! \brief Initialization function called by the Nodelet base class - */ - virtual void onInit(); - - /*! \brief Reads parameters from the param server - */ - void readParameters(); - - /*! \brief ZED camera polling thread function - */ - void device_poll_thread_func(); - - /*! \brief Pointcloud publishing function - */ - void pointcloud_thread_func(); - - /*! \brief Publish the pose of the camera in "Map" frame with a ros Publisher - * \param t : the ros::Time to stamp the image - */ - void publishPose(ros::Time t); - - /*! \brief Publish the pose of the camera in "Odom" frame with a ros Publisher - * \param base2odomTransf : Transformation representing the camera pose - * from base frame to odom frame - * \param slPose : latest odom pose from ZED SDK - * \param t : the ros::Time to stamp the image - */ - void publishOdom(tf2::Transform odom2baseTransf, sl::Pose& slPose, ros::Time t); - - /*! \brief Publish the pose of the camera in "Map" frame as a transformation - * \param baseTransform : Transformation representing the camera pose from - * odom frame to map frame - * \param t : the ros::Time to stamp the image - */ - void publishPoseFrame(tf2::Transform baseTransform, ros::Time t); - - /*! \brief Publish the odometry of the camera in "Odom" frame as a - * transformation - * \param odomTransf : Transformation representing the camera pose from - * base frame to odom frame - * \param t : the ros::Time to stamp the image - */ - void publishOdomFrame(tf2::Transform odomTransf, ros::Time t); - - /*! - * \brief Publish IMU frame once as static TF - */ - void publishStaticImuFrame(); - - /*! \brief Publish a sl::Mat image with a ros Publisher - * \param imgMsgPtr : the image message to publish - * \param img : the image to publish - * \param pubImg : the publisher object to use (different image publishers - * exist) - * \param camInfoMsg : the camera_info to be published with image - * \param imgFrameId : the id of the reference frame of the image (different - * image frames exist) - * \param t : the ros::Time to stamp the image - */ - void publishImage(sensor_msgs::ImagePtr imgMsgPtr, sl::Mat img, image_transport::CameraPublisher& pubImg, sensor_msgs::CameraInfoPtr camInfoMsg, - string imgFrameId, ros::Time t); - - /*! \brief Publish a sl::Mat depth image with a ros Publisher - * \param imgMsgPtr : the depth image topic message to publish - * \param depth : the depth image to publish - * \param t : the ros::Time to stamp the depth image - */ - void publishDepth(sensor_msgs::ImagePtr imgMsgPtr, sl::Mat depth, ros::Time t); - - /*! \brief Publish a single pointCloud with a ros Publisher - */ - void publishPointCloud(); - - /*! \brief Publish a fused pointCloud with a ros Publisher - */ - void callback_pubFusedPointCloud(const ros::TimerEvent& e); - - /*! \brief Publish the informations of a camera with a ros Publisher - * \param cam_info_msg : the information message to publish - * \param pub_cam_info : the publisher object to use - * \param t : the ros::Time to stamp the message - */ - void publishCamInfo(sensor_msgs::CameraInfoPtr camInfoMsg, - ros::Publisher pubCamInfo, ros::Time t); - - /*! \brief Publish a sl::Mat disparity image with a ros Publisher - * \param disparity : the disparity image to publish - * \param t : the ros::Time to stamp the depth image - */ - void publishDisparity(sl::Mat disparity, ros::Time t); - - /*! \brief Publish sensors data and TF - * \param t : the ros::Time to stamp the depth image - */ - void publishSensData(ros::Time t = ros::Time(0)); - - /*! \brief Get the information of the ZED cameras and store them in an - * information message - * \param zed : the sl::zed::Camera* pointer to an instance - * \param left_cam_info_msg : the information message to fill with the left - * camera informations - * \param right_cam_info_msg : the information message to fill with the right - * camera informations - * \param left_frame_id : the id of the reference frame of the left camera - * \param right_frame_id : the id of the reference frame of the right camera - */ - void fillCamInfo(sl::Camera& zed, sensor_msgs::CameraInfoPtr leftCamInfoMsg, - sensor_msgs::CameraInfoPtr rightCamInfoMsg, - string leftFrameId, string rightFrameId, - bool rawParam = false); - - /*! \brief Get the information of the ZED cameras and store them in an - * information message for depth topics - * \param zed : the sl::zed::Camera* pointer to an instance - * \param depth_info_msg : the information message to fill with the left - * camera informations - * \param frame_id : the id of the reference frame of the left camera - */ - void fillCamDepthInfo(sl::Camera& zed, sensor_msgs::CameraInfoPtr depth_info_msg, - string frame_id ); - - /* \bried Check if FPS and Resolution chosen by user are correct. - * Modifies FPS to match correct value. - */ - void checkResolFps(); - - /*! \brief Callback to handle dynamic reconfigure events in ROS - */ - void callback_dynamicReconf(zed_nodelets::ZedConfig& config, uint32_t level); - - /*! \brief Callback to publish Video and Depth data - * \param e : the ros::TimerEvent binded to the callback - */ - void callback_pubVideoDepth(const ros::TimerEvent& e); - - /*! \brief Callback to publish Path data with a ROS publisher. - * \param e : the ros::TimerEvent binded to the callback - */ - void callback_pubPath(const ros::TimerEvent& e); - - /*! \brief Callback to publish Sensor Data with a ROS publisher. - * \param e : the ros::TimerEvent binded to the callback - */ - void callback_pubSensorsData(const ros::TimerEvent& e); - - /*! \brief Callback to update node diagnostic status - * \param stat : node status - */ - void callback_updateDiagnostic(diagnostic_updater::DiagnosticStatusWrapper& stat); - - /*! \brief Service callback to reset_tracking service - * Tracking pose is reinitialized to the value available in the ROS Param - * server - */ - bool on_reset_tracking(zed_interfaces::reset_tracking::Request& req, - zed_interfaces::reset_tracking::Response& res); - - /*! \brief Service callback to reset_odometry service - * Odometry is reset to clear drift and odometry frame gets the latest - * pose - * from ZED tracking. - */ - bool on_reset_odometry(zed_interfaces::reset_odometry::Request& req, - zed_interfaces::reset_odometry::Response& res); - - /*! \brief Service callback to set_pose service - * Tracking pose is set to the new values - */ - bool on_set_pose(zed_interfaces::set_pose::Request& req, - zed_interfaces::set_pose::Response& res); - - /*! \brief Service callback to start_svo_recording service - */ - bool on_start_svo_recording(zed_interfaces::start_svo_recording::Request& req, - zed_interfaces::start_svo_recording::Response& res); - - /*! \brief Service callback to stop_svo_recording service - */ - bool on_stop_svo_recording(zed_interfaces::stop_svo_recording::Request& req, - zed_interfaces::stop_svo_recording::Response& res); - - /*! \brief Service callback to start_remote_stream service - */ - bool on_start_remote_stream(zed_interfaces::start_remote_stream::Request& req, - zed_interfaces::start_remote_stream::Response& res); - - /*! \brief Service callback to stop_remote_stream service - */ - bool on_stop_remote_stream(zed_interfaces::stop_remote_stream::Request& req, - zed_interfaces::stop_remote_stream::Response& res); - - /*! \brief Service callback to set_led_status service - */ - bool on_set_led_status(zed_interfaces::set_led_status::Request& req, - zed_interfaces::set_led_status::Response& res); - - /*! \brief Service callback to toggle_led service - */ - bool on_toggle_led(zed_interfaces::toggle_led::Request& req, - zed_interfaces::toggle_led::Response& res); - - /*! \brief Service callback to start_3d_mapping service - */ - bool on_start_3d_mapping(zed_interfaces::start_3d_mapping::Request& req, - zed_interfaces::start_3d_mapping::Response& res); - - /*! \brief Service callback to stop_3d_mapping service - */ - bool on_stop_3d_mapping(zed_interfaces::stop_3d_mapping::Request& req, - zed_interfaces::stop_3d_mapping::Response& res); - - /*! \brief Service callback to start_object_detection service - */ - bool on_start_object_detection(zed_interfaces::start_object_detection::Request& req, - zed_interfaces::start_object_detection::Response& res); - - /*! \brief Service callback to stop_object_detection service - */ - bool on_stop_object_detection(zed_interfaces::stop_object_detection::Request& req, - zed_interfaces::stop_object_detection::Response& res); - - /*! \brief Utility to initialize the pose variables - */ - bool set_pose(float xt, float yt, float zt, float rr, float pr, float yr); - - /*! \brief Utility to initialize the most used transforms - */ - void initTransforms(); - - /*! \brief Utility to initialize the static transform from Sensor to Base - */ - bool getSens2BaseTransform(); - - /*! \brief Utility to initialize the static transform from Sensor to Camera - */ - bool getSens2CameraTransform(); - - /*! \brief Utility to initialize the static transform from Camera to Base - */ - bool getCamera2BaseTransform(); - - /* \bried Start tracking - */ - void start_pos_tracking(); - - /* \bried Start spatial mapping - */ - bool start_3d_mapping(); - - /* \bried Stop spatial mapping - */ - void stop_3d_mapping(); - - /* \bried Start object detection - */ - bool start_obj_detect(); - - /* \bried Stop object detection - */ - void stop_obj_detect(); - - /*! \brief Perform object detection and publish result - */ - void detectObjects(bool publishObj, bool publishViz, ros::Time t); - - /*! \brief Generates an univoque color for each object class ID - */ - inline sl::float3 generateColorClass(int idx) { - sl::float3 clr; - clr.r = static_cast(33 + (idx * 456262)); - clr.g = static_cast(233 + (idx * 1564684)); - clr.b = static_cast(133 + (idx * 76873242)); - return clr / 255.f; - } - - /*! \brief Update Dynamic reconfigure parameters - */ - void updateDynamicReconfigure(); + /*! \brief Initialization function called by the Nodelet base class + */ + virtual void onInit(); + + /*! \brief Reads parameters from the param server + */ + void readParameters(); + + /*! \brief ZED camera polling thread function + */ + void device_poll_thread_func(); + + /*! \brief Pointcloud publishing function + */ + void pointcloud_thread_func(); + + /*! \brief Publish the pose of the camera in "Map" frame with a ros Publisher + * \param t : the ros::Time to stamp the image + */ + void publishPose(ros::Time t); + + /*! \brief Publish the pose of the camera in "Odom" frame with a ros Publisher + * \param base2odomTransf : Transformation representing the camera pose + * from base frame to odom frame + * \param slPose : latest odom pose from ZED SDK + * \param t : the ros::Time to stamp the image + */ + void publishOdom(tf2::Transform odom2baseTransf, sl::Pose& slPose, ros::Time t); + + /*! \brief Publish the pose of the camera in "Map" frame as a transformation + * \param baseTransform : Transformation representing the camera pose from + * odom frame to map frame + * \param t : the ros::Time to stamp the image + */ + void publishPoseFrame(tf2::Transform baseTransform, ros::Time t); + + /*! \brief Publish the odometry of the camera in "Odom" frame as a + * transformation + * \param odomTransf : Transformation representing the camera pose from + * base frame to odom frame + * \param t : the ros::Time to stamp the image + */ + void publishOdomFrame(tf2::Transform odomTransf, ros::Time t); + + /*! + * \brief Publish IMU frame once as static TF + */ + void publishStaticImuFrame(); + + /*! \brief Publish a sl::Mat image with a ros Publisher + * \param imgMsgPtr : the image message to publish + * \param img : the image to publish + * \param pubImg : the publisher object to use (different image publishers + * exist) + * \param camInfoMsg : the camera_info to be published with image + * \param imgFrameId : the id of the reference frame of the image (different + * image frames exist) + * \param t : the ros::Time to stamp the image + */ + void publishImage(sensor_msgs::ImagePtr imgMsgPtr, sl::Mat img, image_transport::CameraPublisher& pubImg, + sensor_msgs::CameraInfoPtr camInfoMsg, string imgFrameId, ros::Time t); + + /*! \brief Publish a sl::Mat depth image with a ros Publisher + * \param imgMsgPtr : the depth image topic message to publish + * \param depth : the depth image to publish + * \param t : the ros::Time to stamp the depth image + */ + void publishDepth(sensor_msgs::ImagePtr imgMsgPtr, sl::Mat depth, ros::Time t); + + /*! \brief Publish a single pointCloud with a ros Publisher + */ + void publishPointCloud(); + + /*! \brief Publish a fused pointCloud with a ros Publisher + */ + void callback_pubFusedPointCloud(const ros::TimerEvent& e); + + /*! \brief Publish the informations of a camera with a ros Publisher + * \param cam_info_msg : the information message to publish + * \param pub_cam_info : the publisher object to use + * \param t : the ros::Time to stamp the message + */ + void publishCamInfo(sensor_msgs::CameraInfoPtr camInfoMsg, ros::Publisher pubCamInfo, ros::Time t); + + /*! \brief Publish a sl::Mat disparity image with a ros Publisher + * \param disparity : the disparity image to publish + * \param t : the ros::Time to stamp the depth image + */ + void publishDisparity(sl::Mat disparity, ros::Time t); + + /*! \brief Publish sensors data and TF + * \param t : the ros::Time to stamp the depth image + */ + void publishSensData(ros::Time t = ros::Time(0)); + + /*! \brief Get the information of the ZED cameras and store them in an + * information message + * \param zed : the sl::zed::Camera* pointer to an instance + * \param left_cam_info_msg : the information message to fill with the left + * camera informations + * \param right_cam_info_msg : the information message to fill with the right + * camera informations + * \param left_frame_id : the id of the reference frame of the left camera + * \param right_frame_id : the id of the reference frame of the right camera + */ + void fillCamInfo(sl::Camera& zed, sensor_msgs::CameraInfoPtr leftCamInfoMsg, + sensor_msgs::CameraInfoPtr rightCamInfoMsg, string leftFrameId, string rightFrameId, + bool rawParam = false); + + /*! \brief Get the information of the ZED cameras and store them in an + * information message for depth topics + * \param zed : the sl::zed::Camera* pointer to an instance + * \param depth_info_msg : the information message to fill with the left + * camera informations + * \param frame_id : the id of the reference frame of the left camera + */ + void fillCamDepthInfo(sl::Camera& zed, sensor_msgs::CameraInfoPtr depth_info_msg, string frame_id); + + /* \bried Check if FPS and Resolution chosen by user are correct. + * Modifies FPS to match correct value. + */ + void checkResolFps(); + + /*! \brief Callback to handle dynamic reconfigure events in ROS + */ + void callback_dynamicReconf(zed_nodelets::ZedConfig& config, uint32_t level); + + /*! \brief Callback to publish Video and Depth data + * \param e : the ros::TimerEvent binded to the callback + */ + void callback_pubVideoDepth(const ros::TimerEvent& e); + + /*! \brief Callback to publish Path data with a ROS publisher. + * \param e : the ros::TimerEvent binded to the callback + */ + void callback_pubPath(const ros::TimerEvent& e); + + /*! \brief Callback to publish Sensor Data with a ROS publisher. + * \param e : the ros::TimerEvent binded to the callback + */ + void callback_pubSensorsData(const ros::TimerEvent& e); + + /*! \brief Callback to update node diagnostic status + * \param stat : node status + */ + void callback_updateDiagnostic(diagnostic_updater::DiagnosticStatusWrapper& stat); + + /*! \brief Service callback to reset_tracking service + * Tracking pose is reinitialized to the value available in the ROS Param + * server + */ + bool on_reset_tracking(zed_interfaces::reset_tracking::Request& req, zed_interfaces::reset_tracking::Response& res); + + /*! \brief Service callback to reset_odometry service + * Odometry is reset to clear drift and odometry frame gets the latest + * pose + * from ZED tracking. + */ + bool on_reset_odometry(zed_interfaces::reset_odometry::Request& req, zed_interfaces::reset_odometry::Response& res); + + /*! \brief Service callback to set_pose service + * Tracking pose is set to the new values + */ + bool on_set_pose(zed_interfaces::set_pose::Request& req, zed_interfaces::set_pose::Response& res); + + /*! \brief Service callback to start_svo_recording service + */ + bool on_start_svo_recording(zed_interfaces::start_svo_recording::Request& req, + zed_interfaces::start_svo_recording::Response& res); + + /*! \brief Service callback to stop_svo_recording service + */ + bool on_stop_svo_recording(zed_interfaces::stop_svo_recording::Request& req, + zed_interfaces::stop_svo_recording::Response& res); + + /*! \brief Service callback to start_remote_stream service + */ + bool on_start_remote_stream(zed_interfaces::start_remote_stream::Request& req, + zed_interfaces::start_remote_stream::Response& res); + + /*! \brief Service callback to stop_remote_stream service + */ + bool on_stop_remote_stream(zed_interfaces::stop_remote_stream::Request& req, + zed_interfaces::stop_remote_stream::Response& res); + + /*! \brief Service callback to set_led_status service + */ + bool on_set_led_status(zed_interfaces::set_led_status::Request& req, zed_interfaces::set_led_status::Response& res); + + /*! \brief Service callback to toggle_led service + */ + bool on_toggle_led(zed_interfaces::toggle_led::Request& req, zed_interfaces::toggle_led::Response& res); + + /*! \brief Service callback to start_3d_mapping service + */ + bool on_start_3d_mapping(zed_interfaces::start_3d_mapping::Request& req, + zed_interfaces::start_3d_mapping::Response& res); + + /*! \brief Service callback to stop_3d_mapping service + */ + bool on_stop_3d_mapping(zed_interfaces::stop_3d_mapping::Request& req, + zed_interfaces::stop_3d_mapping::Response& res); + + /*! \brief Service callback to start_object_detection service + */ + bool on_start_object_detection(zed_interfaces::start_object_detection::Request& req, + zed_interfaces::start_object_detection::Response& res); + + /*! \brief Service callback to stop_object_detection service + */ + bool on_stop_object_detection(zed_interfaces::stop_object_detection::Request& req, + zed_interfaces::stop_object_detection::Response& res); + + /*! \brief Utility to initialize the pose variables + */ + bool set_pose(float xt, float yt, float zt, float rr, float pr, float yr); + + /*! \brief Utility to initialize the most used transforms + */ + void initTransforms(); + + /*! \brief Utility to initialize the static transform from Sensor to Base + */ + bool getSens2BaseTransform(); + + /*! \brief Utility to initialize the static transform from Sensor to Camera + */ + bool getSens2CameraTransform(); + + /*! \brief Utility to initialize the static transform from Camera to Base + */ + bool getCamera2BaseTransform(); + + /* \bried Start tracking + */ + void start_pos_tracking(); + + /* \bried Start spatial mapping + */ + bool start_3d_mapping(); + + /* \bried Stop spatial mapping + */ + void stop_3d_mapping(); + + /* \bried Start object detection + */ + bool start_obj_detect(); + + /* \bried Stop object detection + */ + void stop_obj_detect(); + + /*! \brief Publish object detection results + */ + void processDetectedObjects(ros::Time t); + + /*! \brief Generates an univoque color for each object class ID + */ + inline sl::float3 generateColorClass(int idx) + { + sl::float3 clr; + clr.r = static_cast(33 + (idx * 456262)); + clr.g = static_cast(233 + (idx * 1564684)); + clr.b = static_cast(133 + (idx * 76873242)); + return clr / 255.f; + } + + /*! \brief Update Dynamic reconfigure parameters + */ + void updateDynamicReconfigure(); private: - uint64_t mFrameCount = 0; - - // SDK version - int mVerMajor; - int mVerMinor; - int mVerSubMinor; - - // ROS - ros::NodeHandle mNh; - ros::NodeHandle mNhNs; - std::thread mDevicePollThread; - std::thread mPcThread; // Point Cloud thread - - bool mStopNode = false; - - const double mSensPubRate = 400.0; - - // Publishers - image_transport::CameraPublisher mPubRgb; // - image_transport::CameraPublisher mPubRawRgb; // - image_transport::CameraPublisher mPubLeft; // - image_transport::CameraPublisher mPubRawLeft; // - image_transport::CameraPublisher mPubRight; // - image_transport::CameraPublisher mPubRawRight; // - image_transport::CameraPublisher mPubDepth; // - image_transport::Publisher mPubStereo; - image_transport::Publisher mPubRawStereo; - - image_transport::CameraPublisher mPubRgbGray; - image_transport::CameraPublisher mPubRawRgbGray; - image_transport::CameraPublisher mPubLeftGray; - image_transport::CameraPublisher mPubRawLeftGray; - image_transport::CameraPublisher mPubRightGray; - image_transport::CameraPublisher mPubRawRightGray; - - ros::Publisher mPubConfMap; // - ros::Publisher mPubDisparity; // - ros::Publisher mPubCloud; - ros::Publisher mPubFusedCloud; - ros::Publisher mPubPose; - ros::Publisher mPubPoseCov; - ros::Publisher mPubOdom; - ros::Publisher mPubOdomPath; - ros::Publisher mPubMapPath; - ros::Publisher mPubImu; - ros::Publisher mPubImuRaw; - ros::Publisher mPubImuTemp; - ros::Publisher mPubImuMag; - //ros::Publisher mPubImuMagRaw; - ros::Publisher mPubPressure; - ros::Publisher mPubTempL; - ros::Publisher mPubTempR; - ros::Publisher mPubCamImuTransf; - - // Timers - ros::Timer mImuTimer; - ros::Timer mPathTimer; - ros::Timer mFusedPcTimer; - ros::Timer mVideoDepthTimer; - - // Services - ros::ServiceServer mSrvSetInitPose; - ros::ServiceServer mSrvResetOdometry; - ros::ServiceServer mSrvResetTracking; - ros::ServiceServer mSrvSvoStartRecording; - ros::ServiceServer mSrvSvoStopRecording; - ros::ServiceServer mSrvSvoStartStream; - ros::ServiceServer mSrvSvoStopStream; - ros::ServiceServer mSrvSetLedStatus; - ros::ServiceServer mSrvToggleLed; - ros::ServiceServer mSrvStartMapping; - ros::ServiceServer mSrvStopMapping; - ros::ServiceServer mSrvStartObjDet; - ros::ServiceServer mSrvStopObjDet; - - // ----> Topics (ONLY THOSE NOT CHANGING WHILE NODE RUNS) - // Camera info - sensor_msgs::CameraInfoPtr mRgbCamInfoMsg; - sensor_msgs::CameraInfoPtr mLeftCamInfoMsg; - sensor_msgs::CameraInfoPtr mRightCamInfoMsg; - sensor_msgs::CameraInfoPtr mRgbCamInfoRawMsg; - sensor_msgs::CameraInfoPtr mLeftCamInfoRawMsg; - sensor_msgs::CameraInfoPtr mRightCamInfoRawMsg; - sensor_msgs::CameraInfoPtr mDepthCamInfoMsg; - - geometry_msgs::TransformPtr mCameraImuTransfMgs; - // <---- Topics - - // ROS TF - tf2_ros::TransformBroadcaster mTransformPoseBroadcaster; - tf2_ros::TransformBroadcaster mTransformOdomBroadcaster; - tf2_ros::StaticTransformBroadcaster mStaticTransformImuBroadcaster; - - std::string mRgbFrameId; - std::string mRgbOptFrameId; - - std::string mDepthFrameId; - std::string mDepthOptFrameId; - - std::string mDisparityFrameId; - std::string mDisparityOptFrameId; - - std::string mConfidenceFrameId; - std::string mConfidenceOptFrameId; - - std::string mCloudFrameId; - std::string mPointCloudFrameId; - - std::string mMapFrameId; - std::string mOdometryFrameId; - std::string mBaseFrameId; - std::string mCameraFrameId; - - std::string mRightCamFrameId; - std::string mRightCamOptFrameId; - std::string mLeftCamFrameId; - std::string mLeftCamOptFrameId; - std::string mImuFrameId; - - std::string mBaroFrameId; - std::string mMagFrameId; - std::string mTempLeftFrameId; - std::string mTempRightFrameId; - - bool mPublishTf; - bool mPublishMapTf; - bool mPublishImuTf; - bool mCameraFlip; - bool mCameraSelfCalib; - - // Launch file parameters - std::string mCameraName; - sl::RESOLUTION mCamResol; - int mCamFrameRate; - sl::DEPTH_MODE mDepthMode; - sl::SENSING_MODE mCamSensingMode; - int mGpuId; - int mZedId; - int mDepthStabilization; - std::string mAreaMemDbPath; - std::string mSvoFilepath; - std::string mRemoteStreamAddr; - bool mSensTimestampSync; - double mPathPubRate; - int mPathMaxCount; - bool mVerbose; - bool mSvoMode = false; - double mCamMinDepth; - double mCamMaxDepth; - - bool mTrackingActivated; - - bool mTrackingReady; - bool mTwoDMode = false; - double mFixedZValue = 0.0; - bool mFloorAlignment = false; - bool mImuFusion = true; - bool mGrabActive = false; // Indicate if camera grabbing is active (at least one topic subscribed) - sl::ERROR_CODE mConnStatus; - sl::ERROR_CODE mGrabStatus; - sl::POSITIONAL_TRACKING_STATE mPosTrackingStatus; - bool mSensPublishing = false; - bool mPcPublishing = false; - - // Last frame time - ros::Time mPrevFrameTimestamp; - ros::Time mFrameTimestamp; - - // Positional Tracking variables - sl::Pose mLastZedPose; // Sensor to Map transform - sl::Transform mInitialPoseSl; - std::vector mInitialBasePose; - std::vector mOdomPath; - std::vector mMapPath; - - // IMU TF - tf2::Transform mLastImuPose; - - // TF Transforms - tf2::Transform mMap2OdomTransf; // Coordinates of the odometry frame in map frame - tf2::Transform mOdom2BaseTransf; // Coordinates of the base in odometry frame - tf2::Transform mMap2BaseTransf; // Coordinates of the base in map frame - tf2::Transform mMap2CameraTransf; // Coordinates of the camera in base frame - tf2::Transform mSensor2BaseTransf; // Coordinates of the base frame in sensor frame - tf2::Transform mSensor2CameraTransf; // Coordinates of the camera frame in sensor frame - tf2::Transform mCamera2BaseTransf; // Coordinates of the base frame in camera frame - - bool mSensor2BaseTransfValid = false; - bool mSensor2CameraTransfValid = false; - bool mCamera2BaseTransfValid = false; - bool mStaticImuFramePublished = false; - - // initialization Transform listener - boost::shared_ptr mTfBuffer; - boost::shared_ptr mTfListener; - - // Zed object - sl::InitParameters mZedParams; - sl::Camera mZed; - unsigned int mZedSerialNumber; - sl::MODEL mZedUserCamModel; // Camera model set by ROS Param - sl::MODEL mZedRealCamModel; // Real camera model by SDK API - unsigned int mCamFwVersion; // Camera FW version - unsigned int mSensFwVersion; // Sensors FW version - - // Dynamic Parameters - int mCamBrightness = 4; - int mCamContrast = 4; - int mCamHue = 0; - int mCamSaturation = 4; - int mCamSharpness = 3; - int mCamGamma = 1; - bool mCamAutoExposure = true; - int mCamGain = 100; - int mCamExposure = 100; - bool mCamAutoWB = true; - int mCamWB = 4200; - - int mCamDepthConfidence = 50; - int mCamDepthTextureConf = 100; - double mPointCloudFreq = 15.; - double mVideoDepthFreq = 15.; - - double mCamImageResizeFactor = 1.0; - double mCamDepthResizeFactor = 1.0; - - // flags - bool mTriggerAutoExposure = true; - bool mTriggerAutoWB = true; - bool mComputeDepth; - bool mOpenniDepthMode; // 16 bit UC data in mm else 32F in m, for more info -> http://www.ros.org/reps/rep-0118.html - bool mPoseSmoothing = false; // Always disabled. Enable only for AR/VR applications - bool mAreaMemory; - bool mInitOdomWithPose; - bool mResetOdom = false; - bool mUseOldExtrinsic = false; - bool mUpdateDynParams = false; - bool mPublishingData = false; - - // SVO recording - bool mRecording = false; - sl::RecordingStatus mRecStatus; - sl::SVO_COMPRESSION_MODE mSvoComprMode; - - // Streaming - bool mStreaming = false; - - // Mat - int mCamWidth; - int mCamHeight; - sl::Resolution mMatResolVideo; - sl::Resolution mMatResolDepth; - - // Thread Sync - std::mutex mCloseZedMutex; - std::mutex mCamDataMutex; - std::mutex mPcMutex; - std::mutex mRecMutex; - std::mutex mPosTrkMutex; - std::mutex mDynParMutex; - std::mutex mMappingMutex; - std::mutex mObjDetMutex; - std::condition_variable mPcDataReadyCondVar; - bool mPcDataReady; - std::condition_variable mRgbDepthDataRetrievedCondVar; - bool mRgbDepthDataRetrieved; - - // Point cloud variables - sl::Mat mCloud; - sl::FusedPointCloud mFusedPC; - ros::Time mPointCloudTime; - - // Dynamic reconfigure - boost::recursive_mutex mDynServerMutex; // To avoid Dynamic Reconfigure Server warning - boost::shared_ptr> mDynRecServer; - - // Diagnostic - float mTempLeft = -273.15f; - float mTempRight = -273.15f; - std::unique_ptr mElabPeriodMean_sec; - std::unique_ptr mGrabPeriodMean_usec; - std::unique_ptr mVideoDepthPeriodMean_sec; - std::unique_ptr mPcPeriodMean_usec; - std::unique_ptr mSensPeriodMean_usec; - std::unique_ptr mObjDetPeriodMean_msec; - - diagnostic_updater::Updater mDiagUpdater; // Diagnostic Updater - - // Camera IMU transform - sl::Transform mSlCamImuTransf; - geometry_msgs::TransformStamped mStaticImuTransformStamped; - - // Spatial mapping - bool mMappingEnabled; - bool mMappingRunning; - float mMappingRes = 0.1; - float mMaxMappingRange = -1; - double mFusedPcPubFreq = 2.0; - - // Object Detection - bool mObjDetEnabled = false; - bool mObjDetRunning = false; - float mObjDetConfidence = 50.f; - bool mObjDetTracking = true; - bool mObjDetPeople = true; - bool mObjDetVehicles = true; - std::vector mObjDetFilter; - - ros::Publisher mPubObjDet; - ros::Publisher mPubObjDetViz; -}; // class ZEDROSWrapperNodelet -} // namespace + uint64_t mFrameCount = 0; + + // SDK version + int mVerMajor; + int mVerMinor; + int mVerSubMinor; + + // ROS + ros::NodeHandle mNh; + ros::NodeHandle mNhNs; + std::thread mDevicePollThread; + std::thread mPcThread; // Point Cloud thread + + bool mStopNode = false; + + const double mSensPubRate = 400.0; + + // Publishers + image_transport::CameraPublisher mPubRgb; // + image_transport::CameraPublisher mPubRawRgb; // + image_transport::CameraPublisher mPubLeft; // + image_transport::CameraPublisher mPubRawLeft; // + image_transport::CameraPublisher mPubRight; // + image_transport::CameraPublisher mPubRawRight; // + image_transport::CameraPublisher mPubDepth; // + image_transport::Publisher mPubStereo; + image_transport::Publisher mPubRawStereo; + + image_transport::CameraPublisher mPubRgbGray; + image_transport::CameraPublisher mPubRawRgbGray; + image_transport::CameraPublisher mPubLeftGray; + image_transport::CameraPublisher mPubRawLeftGray; + image_transport::CameraPublisher mPubRightGray; + image_transport::CameraPublisher mPubRawRightGray; + + ros::Publisher mPubConfMap; // + ros::Publisher mPubDisparity; // + ros::Publisher mPubCloud; + ros::Publisher mPubFusedCloud; + ros::Publisher mPubPose; + ros::Publisher mPubPoseCov; + ros::Publisher mPubOdom; + ros::Publisher mPubOdomPath; + ros::Publisher mPubMapPath; + ros::Publisher mPubImu; + ros::Publisher mPubImuRaw; + ros::Publisher mPubImuTemp; + ros::Publisher mPubImuMag; + // ros::Publisher mPubImuMagRaw; + ros::Publisher mPubPressure; + ros::Publisher mPubTempL; + ros::Publisher mPubTempR; + ros::Publisher mPubCamImuTransf; + + // Timers + ros::Timer mImuTimer; + ros::Timer mPathTimer; + ros::Timer mFusedPcTimer; + ros::Timer mVideoDepthTimer; + + // Services + ros::ServiceServer mSrvSetInitPose; + ros::ServiceServer mSrvResetOdometry; + ros::ServiceServer mSrvResetTracking; + ros::ServiceServer mSrvSvoStartRecording; + ros::ServiceServer mSrvSvoStopRecording; + ros::ServiceServer mSrvSvoStartStream; + ros::ServiceServer mSrvSvoStopStream; + ros::ServiceServer mSrvSetLedStatus; + ros::ServiceServer mSrvToggleLed; + ros::ServiceServer mSrvStartMapping; + ros::ServiceServer mSrvStopMapping; + ros::ServiceServer mSrvStartObjDet; + ros::ServiceServer mSrvStopObjDet; + + // ----> Topics (ONLY THOSE NOT CHANGING WHILE NODE RUNS) + // Camera info + sensor_msgs::CameraInfoPtr mRgbCamInfoMsg; + sensor_msgs::CameraInfoPtr mLeftCamInfoMsg; + sensor_msgs::CameraInfoPtr mRightCamInfoMsg; + sensor_msgs::CameraInfoPtr mRgbCamInfoRawMsg; + sensor_msgs::CameraInfoPtr mLeftCamInfoRawMsg; + sensor_msgs::CameraInfoPtr mRightCamInfoRawMsg; + sensor_msgs::CameraInfoPtr mDepthCamInfoMsg; + + geometry_msgs::TransformPtr mCameraImuTransfMgs; + // <---- Topics + + // ROS TF + tf2_ros::TransformBroadcaster mTransformPoseBroadcaster; + tf2_ros::TransformBroadcaster mTransformOdomBroadcaster; + tf2_ros::StaticTransformBroadcaster mStaticTransformImuBroadcaster; + + std::string mRgbFrameId; + std::string mRgbOptFrameId; + + std::string mDepthFrameId; + std::string mDepthOptFrameId; + + std::string mDisparityFrameId; + std::string mDisparityOptFrameId; + + std::string mConfidenceFrameId; + std::string mConfidenceOptFrameId; + + std::string mCloudFrameId; + std::string mPointCloudFrameId; + + std::string mMapFrameId; + std::string mOdometryFrameId; + std::string mBaseFrameId; + std::string mCameraFrameId; + + std::string mRightCamFrameId; + std::string mRightCamOptFrameId; + std::string mLeftCamFrameId; + std::string mLeftCamOptFrameId; + std::string mImuFrameId; + + std::string mBaroFrameId; + std::string mMagFrameId; + std::string mTempLeftFrameId; + std::string mTempRightFrameId; + + bool mPublishTf; + bool mPublishMapTf; + bool mPublishImuTf; + bool mCameraFlip; + bool mCameraSelfCalib; + + // Launch file parameters + std::string mCameraName; + sl::RESOLUTION mCamResol; + int mCamFrameRate; + sl::DEPTH_MODE mDepthMode; + sl::SENSING_MODE mCamSensingMode; + int mGpuId; + int mZedId; + int mDepthStabilization; + std::string mAreaMemDbPath; + std::string mSvoFilepath; + std::string mRemoteStreamAddr; + bool mSensTimestampSync; + double mPathPubRate; + int mPathMaxCount; + bool mVerbose; + bool mSvoMode = false; + double mCamMinDepth; + double mCamMaxDepth; + + bool mTrackingActivated; + + bool mTrackingReady; + bool mTwoDMode = false; + double mFixedZValue = 0.0; + bool mFloorAlignment = false; + bool mImuFusion = true; + bool mGrabActive = false; // Indicate if camera grabbing is active (at least one topic subscribed) + sl::ERROR_CODE mConnStatus; + sl::ERROR_CODE mGrabStatus; + sl::POSITIONAL_TRACKING_STATE mPosTrackingStatus; + bool mSensPublishing = false; + bool mPcPublishing = false; + + // Last frame time + ros::Time mPrevFrameTimestamp; + ros::Time mFrameTimestamp; + + // Positional Tracking variables + sl::Pose mLastZedPose; // Sensor to Map transform + sl::Transform mInitialPoseSl; + std::vector mInitialBasePose; + std::vector mOdomPath; + std::vector mMapPath; + + // IMU TF + tf2::Transform mLastImuPose; + + // TF Transforms + tf2::Transform mMap2OdomTransf; // Coordinates of the odometry frame in map frame + tf2::Transform mOdom2BaseTransf; // Coordinates of the base in odometry frame + tf2::Transform mMap2BaseTransf; // Coordinates of the base in map frame + tf2::Transform mMap2CameraTransf; // Coordinates of the camera in base frame + tf2::Transform mSensor2BaseTransf; // Coordinates of the base frame in sensor frame + tf2::Transform mSensor2CameraTransf; // Coordinates of the camera frame in sensor frame + tf2::Transform mCamera2BaseTransf; // Coordinates of the base frame in camera frame + + bool mSensor2BaseTransfValid = false; + bool mSensor2CameraTransfValid = false; + bool mCamera2BaseTransfValid = false; + bool mStaticImuFramePublished = false; + + // initialization Transform listener + boost::shared_ptr mTfBuffer; + boost::shared_ptr mTfListener; + + // Zed object + sl::InitParameters mZedParams; + sl::Camera mZed; + unsigned int mZedSerialNumber; + sl::MODEL mZedUserCamModel; // Camera model set by ROS Param + sl::MODEL mZedRealCamModel; // Real camera model by SDK API + unsigned int mCamFwVersion; // Camera FW version + unsigned int mSensFwVersion; // Sensors FW version + + // Dynamic Parameters + int mCamBrightness = 4; + int mCamContrast = 4; + int mCamHue = 0; + int mCamSaturation = 4; + int mCamSharpness = 3; + int mCamGamma = 1; + bool mCamAutoExposure = true; + int mCamGain = 100; + int mCamExposure = 100; + bool mCamAutoWB = true; + int mCamWB = 4200; + + int mCamDepthConfidence = 50; + int mCamDepthTextureConf = 100; + double mPointCloudFreq = 15.; + double mVideoDepthFreq = 15.; + + double mCamImageResizeFactor = 1.0; + double mCamDepthResizeFactor = 1.0; + + // flags + bool mTriggerAutoExposure = true; + bool mTriggerAutoWB = true; + bool mComputeDepth; + bool mOpenniDepthMode; // 16 bit UC data in mm else 32F in m, for more info -> http://www.ros.org/reps/rep-0118.html + bool mPoseSmoothing = false; // Always disabled. Enable only for AR/VR applications + bool mAreaMemory; + bool mInitOdomWithPose; + bool mResetOdom = false; + bool mUseOldExtrinsic = false; + bool mUpdateDynParams = false; + bool mPublishingData = false; + + // SVO recording + bool mRecording = false; + sl::RecordingStatus mRecStatus; + sl::SVO_COMPRESSION_MODE mSvoComprMode; + + // Streaming + bool mStreaming = false; + + // Mat + int mCamWidth; + int mCamHeight; + sl::Resolution mMatResolVideo; + sl::Resolution mMatResolDepth; + + // Thread Sync + std::mutex mCloseZedMutex; + std::mutex mCamDataMutex; + std::mutex mPcMutex; + std::mutex mRecMutex; + std::mutex mPosTrkMutex; + std::mutex mDynParMutex; + std::mutex mMappingMutex; + std::mutex mObjDetMutex; + std::condition_variable mPcDataReadyCondVar; + bool mPcDataReady; + std::condition_variable mRgbDepthDataRetrievedCondVar; + bool mRgbDepthDataRetrieved; + + // Point cloud variables + sl::Mat mCloud; + sl::FusedPointCloud mFusedPC; + ros::Time mPointCloudTime; + + // Dynamic reconfigure + boost::recursive_mutex mDynServerMutex; // To avoid Dynamic Reconfigure Server warning + boost::shared_ptr> mDynRecServer; + + // Diagnostic + float mTempLeft = -273.15f; + float mTempRight = -273.15f; + std::unique_ptr mElabPeriodMean_sec; + std::unique_ptr mGrabPeriodMean_usec; + std::unique_ptr mVideoDepthPeriodMean_sec; + std::unique_ptr mPcPeriodMean_usec; + std::unique_ptr mSensPeriodMean_usec; + std::unique_ptr mObjDetPeriodMean_msec; + + diagnostic_updater::Updater mDiagUpdater; // Diagnostic Updater + + // Camera IMU transform + sl::Transform mSlCamImuTransf; + geometry_msgs::TransformStamped mStaticImuTransformStamped; + + // Spatial mapping + bool mMappingEnabled; + bool mMappingRunning; + float mMappingRes = 0.1; + float mMaxMappingRange = -1; + double mFusedPcPubFreq = 2.0; + + // Object Detection + bool mObjDetEnabled = false; + bool mObjDetRunning = false; + bool mObjDetTracking = true; + bool mObjDetBodyFitting = true; + float mObjDetConfidence = 50.f; + float mObjDetMaxRange = 10.f; + std::vector mObjDetFilter; + bool mObjDetPeopleEnable = true; + bool mObjDetVehiclesEnable = true; + bool mObjDetBagsEnable = true; + bool mObjDetAnimalsEnable = true; + bool mObjDetElectronicsEnable = true; + bool mObjDetFruitsEnable = true; + + sl::DETECTION_MODEL mObjDetModel = sl::DETECTION_MODEL::MULTI_CLASS_BOX; + + ros::Publisher mPubObjDet; +}; // class ZEDROSWrapperNodelet +} // namespace zed_nodelets #include PLUGINLIB_EXPORT_CLASS(zed_nodelets::ZEDWrapperNodelet, nodelet::Nodelet); - -#endif // ZED_WRAPPER_NODELET_H +#endif // ZED_WRAPPER_NODELET_H 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 990d594e..c68e9746 100644 --- a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp @@ -20,3359 +20,3753 @@ #include "zed_wrapper_nodelet.hpp" -#include #include +#include #ifndef NDEBUG #include #endif -#include "zed_interfaces/ObjectStamped.h" -#include "zed_interfaces/Objects.h" - -#include "visualization_msgs/Marker.h" -#include "visualization_msgs/MarkerArray.h" - - +#include "zed_interfaces/Object.h" +#include "zed_interfaces/ObjectsStamped.h" //#define DEBUG_SENS_TS 1 -namespace zed_nodelets { - +namespace zed_nodelets +{ #ifndef DEG2RAD #define DEG2RAD 0.017453293 #define RAD2DEG 57.295777937 #endif -#define MAG_FREQ 50. -#define BARO_FREQ 25. - -ZEDWrapperNodelet::ZEDWrapperNodelet() : Nodelet() {} - -ZEDWrapperNodelet::~ZEDWrapperNodelet() { - if (mDevicePollThread.joinable()) { - mDevicePollThread.join(); - } +#define MAG_FREQ 50. +#define BARO_FREQ 25. - if (mPcThread.joinable()) { - mPcThread.join(); - } +ZEDWrapperNodelet::ZEDWrapperNodelet() : Nodelet() +{ } -void ZEDWrapperNodelet::onInit() { +ZEDWrapperNodelet::~ZEDWrapperNodelet() +{ + if (mDevicePollThread.joinable()) + { + mDevicePollThread.join(); + } + + if (mPcThread.joinable()) + { + mPcThread.join(); + } +} - // Node handlers - mNh = getMTNodeHandle(); - mNhNs = getMTPrivateNodeHandle(); +void ZEDWrapperNodelet::onInit() +{ + // Node handlers + mNh = getMTNodeHandle(); + mNhNs = getMTPrivateNodeHandle(); - mStopNode = false; - mPcDataReady = false; - mRgbDepthDataRetrieved = true; + mStopNode = false; + mPcDataReady = false; + mRgbDepthDataRetrieved = true; #ifndef NDEBUG - if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, - ros::console::levels::Debug)) { - ros::console::notifyLoggerLevelsChanged(); - } + if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug)) + { + ros::console::notifyLoggerLevelsChanged(); + } #endif - NODELET_INFO( "********** Starting nodelet '%s' **********",getName().c_str() ); - - std::string ver = sl_tools::getSDKVersion(mVerMajor, mVerMinor, mVerSubMinor); - NODELET_INFO_STREAM("SDK version : " << ver); + NODELET_INFO("********** Starting nodelet '%s' **********", getName().c_str()); + + std::string ver = sl_tools::getSDKVersion(mVerMajor, mVerMinor, mVerSubMinor); + NODELET_INFO_STREAM("SDK version : " << ver); + + if (mVerMajor < 3) + { + NODELET_ERROR("This version of the ZED ROS Wrapper is designed to be used with ZED SDK v3.x"); + ros::shutdown(); + raise(SIGINT); + raise(SIGABRT); + exit(-1); + } + + readParameters(); + + initTransforms(); + + // Set the video topic names + std::string rgbTopicRoot = "rgb"; + std::string rightTopicRoot = "right"; + std::string leftTopicRoot = "left"; + std::string stereoTopicRoot = "stereo"; + std::string img_topic = "/image_rect_color"; + std::string img_raw_topic = "/image_raw_color"; + std::string img_gray_topic = "/image_rect_gray"; + std::string img_raw_gray_topic_ = "/image_raw_gray"; + std::string raw_suffix = "_raw"; + string left_topic = leftTopicRoot + img_topic; + string left_raw_topic = leftTopicRoot + raw_suffix + img_raw_topic; + string right_topic = rightTopicRoot + img_topic; + string right_raw_topic = rightTopicRoot + raw_suffix + img_raw_topic; + string rgb_topic = rgbTopicRoot + img_topic; + string rgb_raw_topic = rgbTopicRoot + raw_suffix + img_raw_topic; + string stereo_topic = stereoTopicRoot + img_topic; + string stereo_raw_topic = stereoTopicRoot + raw_suffix + img_raw_topic; + string left_gray_topic = leftTopicRoot + img_gray_topic; + string left_raw_gray_topic = leftTopicRoot + raw_suffix + img_raw_gray_topic_; + string right_gray_topic = rightTopicRoot + img_gray_topic; + string right_raw_gray_topic = rightTopicRoot + raw_suffix + img_raw_gray_topic_; + string rgb_gray_topic = rgbTopicRoot + img_gray_topic; + string rgb_raw_gray_topic = rgbTopicRoot + raw_suffix + img_raw_gray_topic_; + + // Set the disparity topic name + std::string disparityTopic = "disparity/disparity_image"; + + // Set the depth topic names + string depth_topic_root = "depth"; + + if (mOpenniDepthMode) + { + NODELET_INFO_STREAM("Openni depth mode activated -> Units: mm, Encoding: MONO16"); + } + depth_topic_root += "/depth_registered"; + + string pointcloud_topic = "point_cloud/cloud_registered"; + + string pointcloud_fused_topic = "mapping/fused_cloud"; + + string object_det_topic_root = "obj_det"; + string object_det_topic = object_det_topic_root + "/objects"; + + std::string confImgRoot = "confidence"; + string conf_map_topic_name = "confidence_map"; + string conf_map_topic = confImgRoot + "/" + conf_map_topic_name; + + // Set the positional tracking topic names + std::string poseTopic = "pose"; + string pose_cov_topic; + pose_cov_topic = poseTopic + "_with_covariance"; + + std::string odometryTopic = "odom"; + string odom_path_topic = "path_odom"; + string map_path_topic = "path_map"; + + // Create camera info + mRgbCamInfoMsg.reset(new sensor_msgs::CameraInfo()); + mLeftCamInfoMsg.reset(new sensor_msgs::CameraInfo()); + mRightCamInfoMsg.reset(new sensor_msgs::CameraInfo()); + mRgbCamInfoRawMsg.reset(new sensor_msgs::CameraInfo()); + mLeftCamInfoRawMsg.reset(new sensor_msgs::CameraInfo()); + mRightCamInfoRawMsg.reset(new sensor_msgs::CameraInfo()); + mDepthCamInfoMsg.reset(new sensor_msgs::CameraInfo()); + + // Initialization transformation listener + mTfBuffer.reset(new tf2_ros::Buffer); + mTfListener.reset(new tf2_ros::TransformListener(*mTfBuffer)); + + // Try to initialize the ZED + if (!mSvoFilepath.empty() || !mRemoteStreamAddr.empty()) + { + if (!mSvoFilepath.empty()) + { + mZedParams.input.setFromSVOFile(mSvoFilepath.c_str()); + mZedParams.svo_real_time_mode = true; - if( mVerMajor < 3 ) { - NODELET_ERROR( "This version of the ZED ROS Wrapper is designed to be used with ZED SDK v3.x"); - ros::shutdown(); - raise(SIGINT); - raise(SIGABRT); - exit(-1); + // TODO ADD PARAMETER FOR SVO REAL TIME } - - readParameters(); - - initTransforms(); - - // Set the video topic names - std::string rgbTopicRoot = "rgb"; - std::string rightTopicRoot = "right"; - std::string leftTopicRoot = "left"; - std::string stereoTopicRoot = "stereo"; - std::string img_topic = "/image_rect_color"; - std::string img_raw_topic = "/image_raw_color"; - std::string img_gray_topic = "/image_rect_gray"; - std::string img_raw_gray_topic_ = "/image_raw_gray"; - std::string raw_suffix = "_raw"; - string left_topic = leftTopicRoot + img_topic; - string left_raw_topic = leftTopicRoot + raw_suffix + img_raw_topic; - string right_topic = rightTopicRoot + img_topic; - string right_raw_topic = rightTopicRoot + raw_suffix + img_raw_topic; - string rgb_topic = rgbTopicRoot + img_topic; - string rgb_raw_topic = rgbTopicRoot + raw_suffix + img_raw_topic; - string stereo_topic = stereoTopicRoot + img_topic; - string stereo_raw_topic = stereoTopicRoot + raw_suffix + img_raw_topic; - string left_gray_topic = leftTopicRoot + img_gray_topic; - string left_raw_gray_topic = leftTopicRoot + raw_suffix + img_raw_gray_topic_; - string right_gray_topic = rightTopicRoot + img_gray_topic; - string right_raw_gray_topic = rightTopicRoot + raw_suffix + img_raw_gray_topic_; - string rgb_gray_topic = rgbTopicRoot + img_gray_topic; - string rgb_raw_gray_topic = rgbTopicRoot + raw_suffix + img_raw_gray_topic_; - - // Set the disparity topic name - std::string disparityTopic = "disparity/disparity_image"; - - // Set the depth topic names - string depth_topic_root = "depth"; - - if (mOpenniDepthMode) { - NODELET_INFO_STREAM("Openni depth mode activated -> Units: mm, Encoding: MONO16"); + else if (!mRemoteStreamAddr.empty()) + { + std::vector configStream = sl_tools::split_string(mRemoteStreamAddr, ':'); + sl::String ip = sl::String(configStream.at(0).c_str()); + + if (configStream.size() == 2) + { + mZedParams.input.setFromStream(ip, atoi(configStream.at(1).c_str())); + } + else + { + mZedParams.input.setFromStream(ip); + } + } + + mSvoMode = true; + } + else + { + mZedParams.camera_fps = mCamFrameRate; + mZedParams.camera_resolution = static_cast(mCamResol); + + if (mZedSerialNumber == 0) + { + mZedParams.input.setFromCameraID(mZedId); } - depth_topic_root += "/depth_registered"; - + else + { + bool waiting_for_camera = true; - string pointcloud_topic = "point_cloud/cloud_registered"; + while (waiting_for_camera) + { + // Ctrl+C check + if (!mNhNs.ok()) + { + mStopNode = true; // Stops other threads - string pointcloud_fused_topic = "mapping/fused_cloud"; + std::lock_guard lock(mCloseZedMutex); + NODELET_DEBUG("Closing ZED"); + mZed.close(); - string object_det_topic_root = "obj_det"; - string object_det_topic = object_det_topic_root + "/objects"; - string object_det_rviz_topic = object_det_topic_root + "/object_markers"; - - std::string confImgRoot = "confidence"; - string conf_map_topic_name = "confidence_map"; - string conf_map_topic = confImgRoot + "/" + conf_map_topic_name; - - // Set the positional tracking topic names - std::string poseTopic = "pose"; - string pose_cov_topic; - pose_cov_topic = poseTopic + "_with_covariance"; - - std::string odometryTopic = "odom"; - string odom_path_topic = "path_odom"; - string map_path_topic = "path_map"; - - // Create camera info - mRgbCamInfoMsg.reset(new sensor_msgs::CameraInfo()); - mLeftCamInfoMsg.reset(new sensor_msgs::CameraInfo()); - mRightCamInfoMsg.reset(new sensor_msgs::CameraInfo()); - mRgbCamInfoRawMsg.reset(new sensor_msgs::CameraInfo()); - mLeftCamInfoRawMsg.reset(new sensor_msgs::CameraInfo()); - mRightCamInfoRawMsg.reset(new sensor_msgs::CameraInfo()); - mDepthCamInfoMsg.reset(new sensor_msgs::CameraInfo()); - - // Initialization transformation listener - mTfBuffer.reset(new tf2_ros::Buffer); - mTfListener.reset(new tf2_ros::TransformListener(*mTfBuffer)); - - // Try to initialize the ZED - if (!mSvoFilepath.empty() || !mRemoteStreamAddr.empty()) { - if (!mSvoFilepath.empty()) { - mZedParams.input.setFromSVOFile(mSvoFilepath.c_str()); - mZedParams.svo_real_time_mode = true; - } else if (!mRemoteStreamAddr.empty()) { - std::vector configStream = sl_tools::split_string(mRemoteStreamAddr, ':'); - sl::String ip = sl::String(configStream.at(0).c_str()); - - if (configStream.size() == 2) { - mZedParams.input.setFromStream(ip, atoi(configStream.at(1).c_str())); - } else { - mZedParams.input.setFromStream(ip); - } + NODELET_DEBUG("ZED pool thread finished"); + return; } - mSvoMode = true; - } else { - mZedParams.camera_fps = mCamFrameRate; - mZedParams.camera_resolution = static_cast(mCamResol); - - if (mZedSerialNumber == 0) { - mZedParams.input.setFromCameraID(mZedId); - } else { - bool waiting_for_camera = true; - - while (waiting_for_camera) { - // Ctrl+C check - if (!mNhNs.ok()) { - mStopNode = true; // Stops other threads - - std::lock_guard lock(mCloseZedMutex); - NODELET_DEBUG("Closing ZED"); - mZed.close(); - - NODELET_DEBUG("ZED pool thread finished"); - return; - } - - sl::DeviceProperties prop = sl_tools::getZEDFromSN(mZedSerialNumber); - - if (prop.id < -1 || - prop.camera_state == sl::CAMERA_STATE::NOT_AVAILABLE) { - std::string msg = "ZED SN" + to_string(mZedSerialNumber) + - " not detected ! Please connect this ZED"; - NODELET_INFO_STREAM(msg.c_str()); - std::this_thread::sleep_for(std::chrono::milliseconds(2000)); - } else { - waiting_for_camera = false; - mZedParams.input.setFromCameraID(prop.id); - } - } + sl::DeviceProperties prop = sl_tools::getZEDFromSN(mZedSerialNumber); + + if (prop.id < -1 || prop.camera_state == sl::CAMERA_STATE::NOT_AVAILABLE) + { + std::string msg = "ZED SN" + to_string(mZedSerialNumber) + " not detected ! Please connect this ZED"; + NODELET_INFO_STREAM(msg.c_str()); + std::this_thread::sleep_for(std::chrono::milliseconds(2000)); + } + else + { + waiting_for_camera = false; + mZedParams.input.setFromCameraID(prop.id); } + } } + } - mZedParams.coordinate_system = sl::COORDINATE_SYSTEM::RIGHT_HANDED_Z_UP_X_FWD; - NODELET_INFO_STREAM(" * Camera coordinate system\t-> " << sl::toString(mZedParams.coordinate_system)); - - mZedParams.coordinate_units = sl::UNIT::METER; - mZedParams.depth_mode = static_cast(mDepthMode); - mZedParams.sdk_verbose = mVerbose; - mZedParams.sdk_gpu_id = mGpuId; - mZedParams.depth_stabilization = mDepthStabilization; - mZedParams.camera_image_flip = mCameraFlip; - mZedParams.depth_minimum_distance = static_cast(mCamMinDepth); - mZedParams.depth_maximum_distance = static_cast(mCamMaxDepth); - mZedParams.camera_disable_self_calib = !mCameraSelfCalib; + mZedParams.coordinate_system = sl::COORDINATE_SYSTEM::RIGHT_HANDED_Z_UP_X_FWD; + NODELET_INFO_STREAM(" * Camera coordinate system\t-> " << sl::toString(mZedParams.coordinate_system)); - mZedParams.enable_image_enhancement = true; // Always active + mZedParams.coordinate_units = sl::UNIT::METER; + mZedParams.depth_mode = static_cast(mDepthMode); + mZedParams.sdk_verbose = mVerbose; + mZedParams.sdk_gpu_id = mGpuId; + mZedParams.depth_stabilization = mDepthStabilization; + mZedParams.camera_image_flip = mCameraFlip; + mZedParams.depth_minimum_distance = static_cast(mCamMinDepth); + mZedParams.depth_maximum_distance = static_cast(mCamMaxDepth); + mZedParams.camera_disable_self_calib = !mCameraSelfCalib; - mDiagUpdater.add("ZED Diagnostic", this, &ZEDWrapperNodelet::callback_updateDiagnostic); - mDiagUpdater.setHardwareID("ZED camera"); + mZedParams.enable_image_enhancement = true; // Always active - mConnStatus = sl::ERROR_CODE::CAMERA_NOT_DETECTED; + mDiagUpdater.add("ZED Diagnostic", this, &ZEDWrapperNodelet::callback_updateDiagnostic); + mDiagUpdater.setHardwareID("ZED camera"); - NODELET_INFO_STREAM(" *** Opening " << sl::toString( mZedUserCamModel) << "..."); - while (mConnStatus != sl::ERROR_CODE::SUCCESS) { - mConnStatus = mZed.open(mZedParams); - NODELET_INFO_STREAM("ZED connection -> " << sl::toString(mConnStatus)); - std::this_thread::sleep_for(std::chrono::milliseconds(2000)); + mConnStatus = sl::ERROR_CODE::CAMERA_NOT_DETECTED; - if (!mNhNs.ok()) { - mStopNode = true; // Stops other threads + NODELET_INFO_STREAM(" *** Opening " << sl::toString(mZedUserCamModel) << "..."); + while (mConnStatus != sl::ERROR_CODE::SUCCESS) + { + mConnStatus = mZed.open(mZedParams); + NODELET_INFO_STREAM("ZED connection -> " << sl::toString(mConnStatus)); + std::this_thread::sleep_for(std::chrono::milliseconds(2000)); - std::lock_guard lock(mCloseZedMutex); - NODELET_DEBUG("Closing ZED"); - mZed.close(); + if (!mNhNs.ok()) + { + mStopNode = true; // Stops other threads - NODELET_DEBUG("ZED pool thread finished"); - return; - } + std::lock_guard lock(mCloseZedMutex); + NODELET_DEBUG("Closing ZED"); + mZed.close(); - mDiagUpdater.update(); + NODELET_DEBUG("ZED pool thread finished"); + return; } - NODELET_INFO_STREAM(" ... " << sl::toString( mZedRealCamModel) << " ready"); - //CUdevice devid; - cuCtxGetDevice(&mGpuId); + mDiagUpdater.update(); + } + NODELET_INFO_STREAM(" ... " << sl::toString(mZedRealCamModel) << " ready"); - NODELET_INFO_STREAM("ZED SDK running on GPU #" << mGpuId); + // CUdevice devid; + cuCtxGetDevice(&mGpuId); - // Disable AEC_AGC and Auto Whitebalance to trigger it if use set to automatic - mZed.setCameraSettings(sl::VIDEO_SETTINGS::AEC_AGC, 0); - mZed.setCameraSettings(sl::VIDEO_SETTINGS::WHITEBALANCE_AUTO, 0); + NODELET_INFO_STREAM("ZED SDK running on GPU #" << mGpuId); - mZedRealCamModel = mZed.getCameraInformation().camera_model; + // Disable AEC_AGC and Auto Whitebalance to trigger it if use set to automatic + mZed.setCameraSettings(sl::VIDEO_SETTINGS::AEC_AGC, 0); + mZed.setCameraSettings(sl::VIDEO_SETTINGS::WHITEBALANCE_AUTO, 0); - if (mZedRealCamModel == sl::MODEL::ZED) { - if (mZedUserCamModel != mZedRealCamModel) { - NODELET_WARN("Camera model does not match user parameter. Please modify " - "the value of the parameter 'camera_model' to 'zed'"); - } - } else if (mZedRealCamModel == sl::MODEL::ZED_M) { - if (mZedUserCamModel != mZedRealCamModel) { - NODELET_WARN("Camera model does not match user parameter. Please modify " - "the value of the parameter 'camera_model' to 'zedm'"); - } -#if ZED_SDK_MAJOR_VERSION==3 && ZED_SDK_MINOR_VERSION<1 - mSlCamImuTransf = mZed.getCameraInformation().camera_imu_transform; + mZedRealCamModel = mZed.getCameraInformation().camera_model; + + if (mZedRealCamModel == sl::MODEL::ZED) + { + if (mZedUserCamModel != mZedRealCamModel) + { + NODELET_WARN("Camera model does not match user parameter. Please modify " + "the value of the parameter 'camera_model' to 'zed'"); + } + } + else if (mZedRealCamModel == sl::MODEL::ZED_M) + { + if (mZedUserCamModel != mZedRealCamModel) + { + NODELET_WARN("Camera model does not match user parameter. Please modify " + "the value of the parameter 'camera_model' to 'zedm'"); + } +#if ZED_SDK_MAJOR_VERSION == 3 && ZED_SDK_MINOR_VERSION < 1 + mSlCamImuTransf = mZed.getCameraInformation().camera_imu_transform; #else - mSlCamImuTransf = mZed.getCameraInformation().sensors_configuration.camera_imu_transform; + mSlCamImuTransf = mZed.getCameraInformation().sensors_configuration.camera_imu_transform; #endif - NODELET_INFO( "Camera-IMU Transform: \n %s", mSlCamImuTransf.getInfos().c_str() ); - } else if (mZedRealCamModel == sl::MODEL::ZED2) { - if (mZedUserCamModel != mZedRealCamModel) { - NODELET_WARN("Camera model does not match user parameter. Please modify " - "the value of the parameter 'camera_model' to 'zed2'"); - } + NODELET_INFO("Camera-IMU Transform: \n %s", mSlCamImuTransf.getInfos().c_str()); + } + else if (mZedRealCamModel == sl::MODEL::ZED2) + { + if (mZedUserCamModel != mZedRealCamModel) + { + NODELET_WARN("Camera model does not match user parameter. Please modify " + "the value of the parameter 'camera_model' to 'zed2'"); + } -#if ZED_SDK_MAJOR_VERSION==3 && ZED_SDK_MINOR_VERSION<1 - mSlCamImuTransf = mZed.getCameraInformation().camera_imu_transform; +#if ZED_SDK_MAJOR_VERSION == 3 && ZED_SDK_MINOR_VERSION < 1 + mSlCamImuTransf = mZed.getCameraInformation().camera_imu_transform; #else - mSlCamImuTransf = mZed.getCameraInformation().sensors_configuration.camera_imu_transform; + mSlCamImuTransf = mZed.getCameraInformation().sensors_configuration.camera_imu_transform; #endif - NODELET_INFO( "Camera-IMU Transform: \n %s", mSlCamImuTransf.getInfos().c_str() ); - } + NODELET_INFO("Camera-IMU Transform: \n %s", mSlCamImuTransf.getInfos().c_str()); + } - NODELET_INFO_STREAM(" * CAMERA MODEL\t -> " << sl::toString(mZedRealCamModel).c_str()); - mZedSerialNumber = mZed.getCameraInformation().serial_number; - NODELET_INFO_STREAM(" * Serial Number -> " << mZedSerialNumber); + NODELET_INFO_STREAM(" * CAMERA MODEL\t -> " << sl::toString(mZedRealCamModel).c_str()); + mZedSerialNumber = mZed.getCameraInformation().serial_number; + NODELET_INFO_STREAM(" * Serial Number -> " << mZedSerialNumber); - if (!mSvoMode) { -#if ZED_SDK_MAJOR_VERSION==3 && ZED_SDK_MINOR_VERSION<1 - mCamFwVersion = mZed.getCameraInformation().camera_firmware_version; + if (!mSvoMode) + { +#if ZED_SDK_MAJOR_VERSION == 3 && ZED_SDK_MINOR_VERSION < 1 + mCamFwVersion = mZed.getCameraInformation().camera_firmware_version; #else - mCamFwVersion = mZed.getCameraInformation().camera_configuration.firmware_version; + mCamFwVersion = mZed.getCameraInformation().camera_configuration.firmware_version; #endif - NODELET_INFO_STREAM(" * Camera FW Version -> " << mCamFwVersion); - if(mZedRealCamModel!=sl::MODEL::ZED) { -#if ZED_SDK_MAJOR_VERSION==3 && ZED_SDK_MINOR_VERSION<1 - mSensFwVersion = mZed.getCameraInformation().sensors_firmware_version; + NODELET_INFO_STREAM(" * Camera FW Version -> " << mCamFwVersion); + if (mZedRealCamModel != sl::MODEL::ZED) + { +#if ZED_SDK_MAJOR_VERSION == 3 && ZED_SDK_MINOR_VERSION < 1 + mSensFwVersion = mZed.getCameraInformation().sensors_firmware_version; #else - mSensFwVersion = mZed.getCameraInformation().sensors_configuration.firmware_version; + mSensFwVersion = mZed.getCameraInformation().sensors_configuration.firmware_version; #endif - NODELET_INFO_STREAM(" * Sensors FW Version -> " << mSensFwVersion); - } - } else { - NODELET_INFO_STREAM(" * Input type\t -> " << sl::toString(mZed.getCameraInformation().input_type).c_str()); - } - - // Set the IMU topic names using real camera model - string imu_topic; - string imu_topic_raw; - string imu_temp_topic; - string imu_mag_topic; - //string imu_mag_topic_raw; - string pressure_topic; - string temp_topic_root = "temperature"; - string temp_topic_left = temp_topic_root + "/left"; - string temp_topic_right = temp_topic_root + "/right"; - - if (mZedRealCamModel != sl::MODEL::ZED) { - std::string imuTopicRoot = "imu"; - string imu_topic_name = "data"; - string imu_topic_raw_name = "data_raw"; - string imu_topic_mag_name = "mag"; - //string imu_topic_mag_raw_name = "mag_raw"; - string pressure_topic_name = "atm_press"; - imu_topic = imuTopicRoot + "/" + imu_topic_name; - imu_topic_raw = imuTopicRoot + "/" + imu_topic_raw_name; - imu_temp_topic = temp_topic_root + "/" + imuTopicRoot; - imu_mag_topic = imuTopicRoot + "/" + imu_topic_mag_name; - //imu_mag_topic_raw = imuTopicRoot + "/" + imu_topic_mag_raw_name; - pressure_topic = /*imuTopicRoot + "/" +*/ pressure_topic_name; - } - - mDiagUpdater.setHardwareIDf("%s - s/n: %d [GPU #%d]", sl::toString(mZedRealCamModel).c_str(), mZedSerialNumber, mGpuId); - - // ----> Dynamic Reconfigure parameters - mDynRecServer = boost::make_shared>(mDynServerMutex); - dynamic_reconfigure::Server::CallbackType f; - f = boost::bind(&ZEDWrapperNodelet::callback_dynamicReconf, this, _1, _2); - mDynRecServer->setCallback(f); - // Update parameters - zed_nodelets::ZedConfig config; - mDynRecServer->getConfigDefault(config); - mDynServerMutex.lock(); - mDynRecServer->updateConfig(config); - mDynServerMutex.unlock(); - - updateDynamicReconfigure(); - // <---- Dynamic Reconfigure parameters - - // Create all the publishers - // Image publishers - image_transport::ImageTransport it_zed(mNhNs); - - mPubRgb = it_zed.advertiseCamera(rgb_topic, 1); // rgb - NODELET_INFO_STREAM("Advertised on topic " << mPubRgb.getTopic()); - NODELET_INFO_STREAM("Advertised on topic " << mPubRgb.getInfoTopic()); - mPubRawRgb = it_zed.advertiseCamera(rgb_raw_topic, 1); // rgb raw - NODELET_INFO_STREAM("Advertised on topic " << mPubRawRgb.getTopic()); - NODELET_INFO_STREAM("Advertised on topic " << mPubRawRgb.getInfoTopic()); - mPubLeft = it_zed.advertiseCamera(left_topic, 1); // left - NODELET_INFO_STREAM("Advertised on topic " << mPubLeft.getTopic()); - NODELET_INFO_STREAM("Advertised on topic " << mPubLeft.getInfoTopic()); - mPubRawLeft = it_zed.advertiseCamera(left_raw_topic, 1); // left raw - NODELET_INFO_STREAM("Advertised on topic " << mPubRawLeft.getTopic()); - NODELET_INFO_STREAM("Advertised on topic " << mPubRawLeft.getInfoTopic()); - mPubRight = it_zed.advertiseCamera(right_topic, 1); // right - NODELET_INFO_STREAM("Advertised on topic " << mPubRight.getTopic()); - NODELET_INFO_STREAM("Advertised on topic " << mPubRight.getInfoTopic()); - mPubRawRight = it_zed.advertiseCamera(right_raw_topic, 1); // right raw - NODELET_INFO_STREAM("Advertised on topic " << mPubRawRight.getTopic()); - NODELET_INFO_STREAM("Advertised on topic " << mPubRawRight.getInfoTopic()); - - mPubRgbGray = it_zed.advertiseCamera(rgb_gray_topic, 1); // rgb - NODELET_INFO_STREAM("Advertised on topic " << mPubRgbGray.getTopic()); - NODELET_INFO_STREAM("Advertised on topic " << mPubRgbGray.getInfoTopic()); - mPubRawRgbGray = it_zed.advertiseCamera(rgb_raw_gray_topic, 1); // rgb raw - NODELET_INFO_STREAM("Advertised on topic " << mPubRawRgbGray.getTopic()); - NODELET_INFO_STREAM("Advertised on topic " << mPubRawRgbGray.getInfoTopic()); - mPubLeftGray = it_zed.advertiseCamera(left_gray_topic, 1); // left - NODELET_INFO_STREAM("Advertised on topic " << mPubLeftGray.getTopic()); - NODELET_INFO_STREAM("Advertised on topic " << mPubLeftGray.getInfoTopic()); - mPubRawLeftGray = it_zed.advertiseCamera(left_raw_gray_topic, 1); // left raw - NODELET_INFO_STREAM("Advertised on topic " << mPubRawLeftGray.getTopic()); - NODELET_INFO_STREAM("Advertised on topic " << mPubRawLeftGray.getInfoTopic()); - mPubRightGray = it_zed.advertiseCamera(right_gray_topic, 1); // right - NODELET_INFO_STREAM("Advertised on topic " << mPubRightGray.getTopic()); - NODELET_INFO_STREAM("Advertised on topic " << mPubRightGray.getInfoTopic()); - mPubRawRightGray = it_zed.advertiseCamera(right_raw_gray_topic, 1); // right raw - NODELET_INFO_STREAM("Advertised on topic " << mPubRawRightGray.getTopic()); - NODELET_INFO_STREAM("Advertised on topic " << mPubRawRightGray.getInfoTopic()); - - mPubDepth = it_zed.advertiseCamera(depth_topic_root, 1); // depth - NODELET_INFO_STREAM("Advertised on topic " << mPubDepth.getTopic()); - NODELET_INFO_STREAM("Advertised on topic " << mPubDepth.getInfoTopic()); - - mPubStereo = it_zed.advertise(stereo_topic, 1); - NODELET_INFO_STREAM("Advertised on topic " << mPubStereo.getTopic()); - mPubRawStereo = it_zed.advertise(stereo_raw_topic, 1); - NODELET_INFO_STREAM("Advertised on topic " << mPubRawStereo.getTopic()); - - // Confidence Map publisher - mPubConfMap = mNhNs.advertise(conf_map_topic, 1); // confidence map - NODELET_INFO_STREAM("Advertised on topic " << mPubConfMap.getTopic()); - - // Disparity publisher - mPubDisparity = mNhNs.advertise(disparityTopic, static_cast(mVideoDepthFreq)); - NODELET_INFO_STREAM("Advertised on topic " << mPubDisparity.getTopic()); - - // PointCloud publishers - mPubCloud = mNhNs.advertise(pointcloud_topic, 1); - NODELET_INFO_STREAM("Advertised on topic " << mPubCloud.getTopic()); - - if (mMappingEnabled) { - mPubFusedCloud = mNhNs.advertise(pointcloud_fused_topic, 1); - NODELET_INFO_STREAM("Advertised on topic " << mPubFusedCloud.getTopic() << " @ " << mFusedPcPubFreq << " Hz"); - } + NODELET_INFO_STREAM(" * Sensors FW Version -> " << mSensFwVersion); + } + } + else + { + NODELET_INFO_STREAM(" * Input type\t -> " << sl::toString(mZed.getCameraInformation().input_type).c_str()); + } + + // Set the IMU topic names using real camera model + string imu_topic; + string imu_topic_raw; + string imu_temp_topic; + string imu_mag_topic; + // string imu_mag_topic_raw; + string pressure_topic; + string temp_topic_root = "temperature"; + string temp_topic_left = temp_topic_root + "/left"; + string temp_topic_right = temp_topic_root + "/right"; + + if (mZedRealCamModel != sl::MODEL::ZED) + { + std::string imuTopicRoot = "imu"; + string imu_topic_name = "data"; + string imu_topic_raw_name = "data_raw"; + string imu_topic_mag_name = "mag"; + // string imu_topic_mag_raw_name = "mag_raw"; + string pressure_topic_name = "atm_press"; + imu_topic = imuTopicRoot + "/" + imu_topic_name; + imu_topic_raw = imuTopicRoot + "/" + imu_topic_raw_name; + imu_temp_topic = temp_topic_root + "/" + imuTopicRoot; + imu_mag_topic = imuTopicRoot + "/" + imu_topic_mag_name; + // imu_mag_topic_raw = imuTopicRoot + "/" + imu_topic_mag_raw_name; + pressure_topic = /*imuTopicRoot + "/" +*/ pressure_topic_name; + } + + mDiagUpdater.setHardwareIDf("%s - s/n: %d [GPU #%d]", sl::toString(mZedRealCamModel).c_str(), mZedSerialNumber, + mGpuId); + + // ----> Dynamic Reconfigure parameters + mDynRecServer = boost::make_shared>(mDynServerMutex); + dynamic_reconfigure::Server::CallbackType f; + f = boost::bind(&ZEDWrapperNodelet::callback_dynamicReconf, this, _1, _2); + mDynRecServer->setCallback(f); + // Update parameters + zed_nodelets::ZedConfig config; + mDynRecServer->getConfigDefault(config); + mDynServerMutex.lock(); + mDynRecServer->updateConfig(config); + mDynServerMutex.unlock(); + + updateDynamicReconfigure(); + // <---- Dynamic Reconfigure parameters + + // Create all the publishers + // Image publishers + image_transport::ImageTransport it_zed(mNhNs); + + mPubRgb = it_zed.advertiseCamera(rgb_topic, 1); // rgb + NODELET_INFO_STREAM("Advertised on topic " << mPubRgb.getTopic()); + NODELET_INFO_STREAM("Advertised on topic " << mPubRgb.getInfoTopic()); + mPubRawRgb = it_zed.advertiseCamera(rgb_raw_topic, 1); // rgb raw + NODELET_INFO_STREAM("Advertised on topic " << mPubRawRgb.getTopic()); + NODELET_INFO_STREAM("Advertised on topic " << mPubRawRgb.getInfoTopic()); + mPubLeft = it_zed.advertiseCamera(left_topic, 1); // left + NODELET_INFO_STREAM("Advertised on topic " << mPubLeft.getTopic()); + NODELET_INFO_STREAM("Advertised on topic " << mPubLeft.getInfoTopic()); + mPubRawLeft = it_zed.advertiseCamera(left_raw_topic, 1); // left raw + NODELET_INFO_STREAM("Advertised on topic " << mPubRawLeft.getTopic()); + NODELET_INFO_STREAM("Advertised on topic " << mPubRawLeft.getInfoTopic()); + mPubRight = it_zed.advertiseCamera(right_topic, 1); // right + NODELET_INFO_STREAM("Advertised on topic " << mPubRight.getTopic()); + NODELET_INFO_STREAM("Advertised on topic " << mPubRight.getInfoTopic()); + mPubRawRight = it_zed.advertiseCamera(right_raw_topic, 1); // right raw + NODELET_INFO_STREAM("Advertised on topic " << mPubRawRight.getTopic()); + NODELET_INFO_STREAM("Advertised on topic " << mPubRawRight.getInfoTopic()); + + mPubRgbGray = it_zed.advertiseCamera(rgb_gray_topic, 1); // rgb + NODELET_INFO_STREAM("Advertised on topic " << mPubRgbGray.getTopic()); + NODELET_INFO_STREAM("Advertised on topic " << mPubRgbGray.getInfoTopic()); + mPubRawRgbGray = it_zed.advertiseCamera(rgb_raw_gray_topic, 1); // rgb raw + NODELET_INFO_STREAM("Advertised on topic " << mPubRawRgbGray.getTopic()); + NODELET_INFO_STREAM("Advertised on topic " << mPubRawRgbGray.getInfoTopic()); + mPubLeftGray = it_zed.advertiseCamera(left_gray_topic, 1); // left + NODELET_INFO_STREAM("Advertised on topic " << mPubLeftGray.getTopic()); + NODELET_INFO_STREAM("Advertised on topic " << mPubLeftGray.getInfoTopic()); + mPubRawLeftGray = it_zed.advertiseCamera(left_raw_gray_topic, 1); // left raw + NODELET_INFO_STREAM("Advertised on topic " << mPubRawLeftGray.getTopic()); + NODELET_INFO_STREAM("Advertised on topic " << mPubRawLeftGray.getInfoTopic()); + mPubRightGray = it_zed.advertiseCamera(right_gray_topic, 1); // right + NODELET_INFO_STREAM("Advertised on topic " << mPubRightGray.getTopic()); + NODELET_INFO_STREAM("Advertised on topic " << mPubRightGray.getInfoTopic()); + mPubRawRightGray = it_zed.advertiseCamera(right_raw_gray_topic, 1); // right raw + NODELET_INFO_STREAM("Advertised on topic " << mPubRawRightGray.getTopic()); + NODELET_INFO_STREAM("Advertised on topic " << mPubRawRightGray.getInfoTopic()); + + mPubDepth = it_zed.advertiseCamera(depth_topic_root, 1); // depth + NODELET_INFO_STREAM("Advertised on topic " << mPubDepth.getTopic()); + NODELET_INFO_STREAM("Advertised on topic " << mPubDepth.getInfoTopic()); + + mPubStereo = it_zed.advertise(stereo_topic, 1); + NODELET_INFO_STREAM("Advertised on topic " << mPubStereo.getTopic()); + mPubRawStereo = it_zed.advertise(stereo_raw_topic, 1); + NODELET_INFO_STREAM("Advertised on topic " << mPubRawStereo.getTopic()); + + // Confidence Map publisher + mPubConfMap = mNhNs.advertise(conf_map_topic, 1); // confidence map + NODELET_INFO_STREAM("Advertised on topic " << mPubConfMap.getTopic()); + + // Disparity publisher + mPubDisparity = mNhNs.advertise(disparityTopic, static_cast(mVideoDepthFreq)); + NODELET_INFO_STREAM("Advertised on topic " << mPubDisparity.getTopic()); + + // PointCloud publishers + mPubCloud = mNhNs.advertise(pointcloud_topic, 1); + NODELET_INFO_STREAM("Advertised on topic " << mPubCloud.getTopic()); + + if (mMappingEnabled) + { + mPubFusedCloud = mNhNs.advertise(pointcloud_fused_topic, 1); + NODELET_INFO_STREAM("Advertised on topic " << mPubFusedCloud.getTopic() << " @ " << mFusedPcPubFreq << " Hz"); + } + + // Object detection publishers + if (mObjDetEnabled) + { + mPubObjDet = mNhNs.advertise(object_det_topic, 1); + NODELET_INFO_STREAM("Advertised on topic " << mPubObjDet.getTopic()); + } + + // Odometry and Pose publisher + mPubPose = mNhNs.advertise(poseTopic, 1); + NODELET_INFO_STREAM("Advertised on topic " << mPubPose.getTopic()); + + mPubPoseCov = mNhNs.advertise(pose_cov_topic, 1); + NODELET_INFO_STREAM("Advertised on topic " << mPubPoseCov.getTopic()); + + mPubOdom = mNhNs.advertise(odometryTopic, 1); + NODELET_INFO_STREAM("Advertised on topic " << mPubOdom.getTopic()); + + // Camera Path + if (mPathPubRate > 0) + { + mPubOdomPath = mNhNs.advertise(odom_path_topic, 1, true); + NODELET_INFO_STREAM("Advertised on topic " << mPubOdomPath.getTopic()); + mPubMapPath = mNhNs.advertise(map_path_topic, 1, true); + NODELET_INFO_STREAM("Advertised on topic " << mPubMapPath.getTopic()); + + mPathTimer = mNhNs.createTimer(ros::Duration(1.0 / mPathPubRate), &ZEDWrapperNodelet::callback_pubPath, this); + + if (mPathMaxCount != -1) + { + NODELET_DEBUG_STREAM("Path vectors reserved " << mPathMaxCount << " poses."); + mOdomPath.reserve(mPathMaxCount); + mMapPath.reserve(mPathMaxCount); + + NODELET_DEBUG_STREAM("Path vector sizes: " << mOdomPath.size() << " " << mMapPath.size()); + } + } + else + { + NODELET_INFO_STREAM("Path topics not published -> mPathPubRate: " << mPathPubRate); + } + + // Sensor publishers + if (mZedRealCamModel != sl::MODEL::ZED) + { + // IMU Publishers + mPubImu = mNhNs.advertise(imu_topic, 1 /*static_cast(mSensPubRate)*/); + NODELET_INFO_STREAM("Advertised on topic " << mPubImu.getTopic()); + mPubImuRaw = mNhNs.advertise(imu_topic_raw, 1 /*static_cast(mSensPubRate)*/); + NODELET_INFO_STREAM("Advertised on topic " << mPubImuRaw.getTopic()); + mPubImuMag = mNhNs.advertise(imu_mag_topic, 1 /*MAG_FREQ*/); + NODELET_INFO_STREAM("Advertised on topic " << mPubImuMag.getTopic()); + + if (mZedRealCamModel == sl::MODEL::ZED2) + { + // IMU temperature sensor + mPubImuTemp = mNhNs.advertise(imu_temp_topic, 1 /*static_cast(mSensPubRate)*/); + NODELET_INFO_STREAM("Advertised on topic " << mPubImuTemp.getTopic()); - // Object detection publishers - if (mObjDetEnabled) { - mPubObjDet = mNhNs.advertise(object_det_topic, 1); + // Atmospheric pressure + mPubPressure = mNhNs.advertise(pressure_topic, 1 /*static_cast(BARO_FREQ)*/); + NODELET_INFO_STREAM("Advertised on topic " << mPubPressure.getTopic()); - NODELET_INFO_STREAM("Advertised on topic " << mPubObjDet.getTopic()); - mPubObjDetViz = mNhNs.advertise(object_det_rviz_topic, 1); - NODELET_INFO_STREAM("Advertised on topic " << mPubObjDetViz.getTopic()); + // CMOS sensor temperatures + mPubTempL = mNhNs.advertise(temp_topic_left, 1 /*static_cast(BARO_FREQ)*/); + NODELET_INFO_STREAM("Advertised on topic " << mPubTempL.getTopic()); + mPubTempR = mNhNs.advertise(temp_topic_right, 1 /*static_cast(BARO_FREQ)*/); + NODELET_INFO_STREAM("Advertised on topic " << mPubTempR.getTopic()); } - // Odometry and Pose publisher - mPubPose = mNhNs.advertise(poseTopic, 1); - NODELET_INFO_STREAM("Advertised on topic " << mPubPose.getTopic()); + // Publish camera imu transform in a latched topic + if (mZedRealCamModel != sl::MODEL::ZED) + { + string cam_imu_tr_topic = "left_cam_imu_transform"; + mPubCamImuTransf = mNhNs.advertise(cam_imu_tr_topic, 1, true); - mPubPoseCov = mNhNs.advertise(pose_cov_topic, 1); - NODELET_INFO_STREAM("Advertised on topic " << mPubPoseCov.getTopic()); + sl::Orientation sl_rot = mSlCamImuTransf.getOrientation(); + sl::Translation sl_tr = mSlCamImuTransf.getTranslation(); + mCameraImuTransfMgs = boost::make_shared(); - mPubOdom = mNhNs.advertise(odometryTopic, 1); - NODELET_INFO_STREAM("Advertised on topic " << mPubOdom.getTopic()); + mCameraImuTransfMgs->rotation.x = sl_rot.ox; + mCameraImuTransfMgs->rotation.y = sl_rot.oy; + mCameraImuTransfMgs->rotation.z = sl_rot.oz; + mCameraImuTransfMgs->rotation.w = sl_rot.ow; - // Camera Path - if (mPathPubRate > 0) { - mPubOdomPath = mNhNs.advertise(odom_path_topic, 1, true); - NODELET_INFO_STREAM("Advertised on topic " << mPubOdomPath.getTopic()); - mPubMapPath = mNhNs.advertise(map_path_topic, 1, true); - NODELET_INFO_STREAM("Advertised on topic " << mPubMapPath.getTopic()); + mCameraImuTransfMgs->translation.x = sl_tr.x; + mCameraImuTransfMgs->translation.y = sl_tr.y; + mCameraImuTransfMgs->translation.z = sl_tr.z; - mPathTimer = mNhNs.createTimer(ros::Duration(1.0 / mPathPubRate), - &ZEDWrapperNodelet::callback_pubPath, this); + NODELET_DEBUG("Camera-IMU Rotation: \n %s", sl_rot.getRotationMatrix().getInfos().c_str()); + NODELET_DEBUG("Camera-IMU Translation: \n %g %g %g", sl_tr.x, sl_tr.y, sl_tr.z); - if (mPathMaxCount != -1) { - NODELET_DEBUG_STREAM("Path vectors reserved " << mPathMaxCount << " poses."); - mOdomPath.reserve(mPathMaxCount); - mMapPath.reserve(mPathMaxCount); + mPubCamImuTransf.publish(mCameraImuTransfMgs); - NODELET_DEBUG_STREAM("Path vector sizes: " << mOdomPath.size() << " " << mMapPath.size()); - } - } else { - NODELET_INFO_STREAM("Path topics not published -> mPathPubRate: " << mPathPubRate); + NODELET_INFO_STREAM("Advertised on topic " << mPubCamImuTransf.getTopic() << " [LATCHED]"); } - // Sensor publishers - if (mZedRealCamModel != sl::MODEL::ZED) { - // IMU Publishers - mPubImu = mNhNs.advertise(imu_topic, 1/*static_cast(mSensPubRate)*/); - NODELET_INFO_STREAM("Advertised on topic " << mPubImu.getTopic() ); - mPubImuRaw = mNhNs.advertise(imu_topic_raw, 1/*static_cast(mSensPubRate)*/); - NODELET_INFO_STREAM("Advertised on topic " << mPubImuRaw.getTopic() ); - mPubImuMag = mNhNs.advertise(imu_mag_topic, 1/*MAG_FREQ*/); - NODELET_INFO_STREAM("Advertised on topic " << mPubImuMag.getTopic() ); - - if( mZedRealCamModel == sl::MODEL::ZED2 ) { - // IMU temperature sensor - mPubImuTemp = mNhNs.advertise(imu_temp_topic, 1/*static_cast(mSensPubRate)*/); - NODELET_INFO_STREAM("Advertised on topic " << mPubImuTemp.getTopic() ); - - // Atmospheric pressure - mPubPressure = mNhNs.advertise(pressure_topic, 1/*static_cast(BARO_FREQ)*/); - NODELET_INFO_STREAM("Advertised on topic " << mPubPressure.getTopic() ); - - // CMOS sensor temperatures - mPubTempL = mNhNs.advertise(temp_topic_left, 1/*static_cast(BARO_FREQ)*/); - NODELET_INFO_STREAM("Advertised on topic " << mPubTempL.getTopic() ); - mPubTempR = mNhNs.advertise(temp_topic_right, 1/*static_cast(BARO_FREQ)*/); - NODELET_INFO_STREAM("Advertised on topic " << mPubTempR.getTopic() ); - } - - // Publish camera imu transform in a latched topic - if (mZedRealCamModel != sl::MODEL::ZED) { - string cam_imu_tr_topic = "left_cam_imu_transform"; - mPubCamImuTransf = mNhNs.advertise( cam_imu_tr_topic, 1, true ); - - sl::Orientation sl_rot = mSlCamImuTransf.getOrientation(); - sl::Translation sl_tr = mSlCamImuTransf.getTranslation(); - - mCameraImuTransfMgs = boost::make_shared(); - - mCameraImuTransfMgs->rotation.x = sl_rot.ox; - mCameraImuTransfMgs->rotation.y = sl_rot.oy; - mCameraImuTransfMgs->rotation.z = sl_rot.oz; - mCameraImuTransfMgs->rotation.w = sl_rot.ow; - - mCameraImuTransfMgs->translation.x = sl_tr.x; - mCameraImuTransfMgs->translation.y = sl_tr.y; - mCameraImuTransfMgs->translation.z = sl_tr.z; - - NODELET_DEBUG( "Camera-IMU Rotation: \n %s", sl_rot.getRotationMatrix().getInfos().c_str() ); - NODELET_DEBUG( "Camera-IMU Translation: \n %g %g %g", sl_tr.x, sl_tr.y, sl_tr.z ); - - mPubCamImuTransf.publish( mCameraImuTransfMgs ); - - NODELET_INFO_STREAM("Advertised on topic " << mPubCamImuTransf.getTopic() << " [LATCHED]"); - } - - if(!mSvoMode&&!mSensTimestampSync) { - mFrameTimestamp = ros::Time::now(); - mImuTimer = mNhNs.createTimer(ros::Duration(1.0 / (mSensPubRate*1.5) ), - &ZEDWrapperNodelet::callback_pubSensorsData, this); - mSensPeriodMean_usec.reset(new sl_tools::CSmartMean(mSensPubRate / 2)); - } else { - mSensPeriodMean_usec.reset(new sl_tools::CSmartMean(mCamFrameRate / 2)); - } + if (!mSvoMode && !mSensTimestampSync) + { + mFrameTimestamp = ros::Time::now(); + mImuTimer = mNhNs.createTimer(ros::Duration(1.0 / (mSensPubRate * 1.5)), + &ZEDWrapperNodelet::callback_pubSensorsData, this); + mSensPeriodMean_usec.reset(new sl_tools::CSmartMean(mSensPubRate / 2)); } + else + { + mSensPeriodMean_usec.reset(new sl_tools::CSmartMean(mCamFrameRate / 2)); + } + } - // Services - mSrvSetInitPose = mNhNs.advertiseService("set_pose", &ZEDWrapperNodelet::on_set_pose, this); - mSrvResetOdometry = mNhNs.advertiseService("reset_odometry", &ZEDWrapperNodelet::on_reset_odometry, this); - mSrvResetTracking = mNhNs.advertiseService("reset_tracking", &ZEDWrapperNodelet::on_reset_tracking, this); - mSrvSvoStartRecording = mNhNs.advertiseService("start_svo_recording", &ZEDWrapperNodelet::on_start_svo_recording, this); - mSrvSvoStopRecording = mNhNs.advertiseService("stop_svo_recording", &ZEDWrapperNodelet::on_stop_svo_recording, this); + // Services + mSrvSetInitPose = mNhNs.advertiseService("set_pose", &ZEDWrapperNodelet::on_set_pose, this); + mSrvResetOdometry = mNhNs.advertiseService("reset_odometry", &ZEDWrapperNodelet::on_reset_odometry, this); + mSrvResetTracking = mNhNs.advertiseService("reset_tracking", &ZEDWrapperNodelet::on_reset_tracking, this); + mSrvSvoStartRecording = + mNhNs.advertiseService("start_svo_recording", &ZEDWrapperNodelet::on_start_svo_recording, this); + mSrvSvoStopRecording = mNhNs.advertiseService("stop_svo_recording", &ZEDWrapperNodelet::on_stop_svo_recording, this); - mSrvSetLedStatus = mNhNs.advertiseService("set_led_status", &ZEDWrapperNodelet::on_set_led_status, this); - mSrvToggleLed = mNhNs.advertiseService("toggle_led", &ZEDWrapperNodelet::on_toggle_led, this); - mSrvSvoStartStream = mNhNs.advertiseService("start_remote_stream", &ZEDWrapperNodelet::on_start_remote_stream, this); - mSrvSvoStopStream = mNhNs.advertiseService("stop_remote_stream", &ZEDWrapperNodelet::on_stop_remote_stream, this); + mSrvSetLedStatus = mNhNs.advertiseService("set_led_status", &ZEDWrapperNodelet::on_set_led_status, this); + mSrvToggleLed = mNhNs.advertiseService("toggle_led", &ZEDWrapperNodelet::on_toggle_led, this); + mSrvSvoStartStream = mNhNs.advertiseService("start_remote_stream", &ZEDWrapperNodelet::on_start_remote_stream, this); + mSrvSvoStopStream = mNhNs.advertiseService("stop_remote_stream", &ZEDWrapperNodelet::on_stop_remote_stream, this); - mSrvStartMapping = mNhNs.advertiseService("start_3d_mapping", &ZEDWrapperNodelet::on_start_3d_mapping, this); - mSrvStopMapping = mNhNs.advertiseService("stop_3d_mapping", &ZEDWrapperNodelet::on_stop_3d_mapping, this); + mSrvStartMapping = mNhNs.advertiseService("start_3d_mapping", &ZEDWrapperNodelet::on_start_3d_mapping, this); + mSrvStopMapping = mNhNs.advertiseService("stop_3d_mapping", &ZEDWrapperNodelet::on_stop_3d_mapping, this); - mSrvStartObjDet = mNhNs.advertiseService("start_object_detection", &ZEDWrapperNodelet::on_start_object_detection, this); - mSrvStopObjDet = mNhNs.advertiseService("stop_object_detection", &ZEDWrapperNodelet::on_stop_object_detection, this); + mSrvStartObjDet = + mNhNs.advertiseService("start_object_detection", &ZEDWrapperNodelet::on_start_object_detection, this); + mSrvStopObjDet = mNhNs.advertiseService("stop_object_detection", &ZEDWrapperNodelet::on_stop_object_detection, this); - // Start Pointcloud thread - mPcThread = std::thread(&ZEDWrapperNodelet::pointcloud_thread_func, this); + // Start Pointcloud thread + mPcThread = std::thread(&ZEDWrapperNodelet::pointcloud_thread_func, this); - // Start pool thread - mDevicePollThread = std::thread(&ZEDWrapperNodelet::device_poll_thread_func, this); + // Start pool thread + mDevicePollThread = std::thread(&ZEDWrapperNodelet::device_poll_thread_func, this); - // Start data publishing timer - mVideoDepthTimer = mNhNs.createTimer(ros::Duration(1.0 / mVideoDepthFreq), &ZEDWrapperNodelet::callback_pubVideoDepth, - this); + // Start data publishing timer + mVideoDepthTimer = + mNhNs.createTimer(ros::Duration(1.0 / mVideoDepthFreq), &ZEDWrapperNodelet::callback_pubVideoDepth, this); } -void ZEDWrapperNodelet::readParameters() { - - NODELET_INFO_STREAM("*** PARAMETERS ***"); - - // ----> General - // Get parameters from param files - mNhNs.getParam("general/camera_name", mCameraName); - NODELET_INFO_STREAM(" * Camera Name\t\t\t-> " << mCameraName.c_str()); - int resol; - mNhNs.getParam("general/resolution", resol); - mCamResol = static_cast(resol); - NODELET_INFO_STREAM(" * Camera Resolution\t\t-> " << sl::toString(mCamResol).c_str()); - mNhNs.getParam("general/grab_frame_rate", mCamFrameRate); - checkResolFps(); - NODELET_INFO_STREAM(" * Camera Grab Framerate\t-> " << mCamFrameRate); - - mNhNs.getParam("general/gpu_id", mGpuId); - NODELET_INFO_STREAM(" * Gpu ID\t\t\t-> " << mGpuId); - mNhNs.getParam("general/zed_id", mZedId); - NODELET_INFO_STREAM(" * Camera ID\t\t\t-> " << mGpuId); - mNhNs.getParam("general/verbose", mVerbose); - NODELET_INFO_STREAM(" * Verbose\t\t\t-> " << (mVerbose ? "ENABLED" : "DISABLED")); - mNhNs.param("general/camera_flip", mCameraFlip, false); - NODELET_INFO_STREAM(" * Camera Flip\t\t\t-> " << (mCameraFlip ? "ENABLED" : "DISABLED")); - mNhNs.param("general/self_calib", mCameraSelfCalib, true); - NODELET_INFO_STREAM(" * Self calibration\t\t-> " << (mCameraSelfCalib ? "ENABLED" : "DISABLED")); - - int tmp_sn = 0; - mNhNs.getParam("general/serial_number", tmp_sn); - - if (tmp_sn > 0) { - mZedSerialNumber = static_cast(tmp_sn); - NODELET_INFO_STREAM(" * Serial number\t\t-> " << mZedSerialNumber); - } - - - string camera_model; - mNhNs.getParam("general/camera_model", camera_model); - - if (camera_model == "zed") { - mZedUserCamModel = sl::MODEL::ZED; - NODELET_INFO_STREAM(" * Camera Model by param\t-> " << camera_model); - } else if (camera_model == "zedm") { - mZedUserCamModel = sl::MODEL::ZED_M; - NODELET_INFO_STREAM(" * Camera Model by param\t-> " << camera_model); - } else if (camera_model == "zed2") { - mZedUserCamModel = sl::MODEL::ZED2; - NODELET_INFO_STREAM(" * Camera Model by param\t-> " << camera_model); - } else { - NODELET_ERROR_STREAM("Camera model not valid: " << camera_model); - } - // <---- General - - // ----> Video - mNhNs.getParam("video/img_downsample_factor", mCamImageResizeFactor); - NODELET_INFO_STREAM(" * Image resample factor\t-> " << mCamImageResizeFactor); - - mNhNs.getParam("video/extrinsic_in_camera_frame", mUseOldExtrinsic); - NODELET_INFO_STREAM(" * Extrinsic param. frame\t-> " << (mUseOldExtrinsic?"X RIGHT - Y DOWN - Z FWD":"X FWD - Y LEFT - Z UP")); - // <---- Video - - // -----> Depth - int depth_mode; - mNhNs.getParam("depth/quality", depth_mode); - mDepthMode = static_cast(depth_mode); - NODELET_INFO_STREAM(" * Depth quality\t\t-> " << sl::toString(mDepthMode).c_str()); - int sensing_mode; - mNhNs.getParam("depth/sensing_mode", sensing_mode ); - mCamSensingMode = static_cast(sensing_mode); - NODELET_INFO_STREAM(" * Depth Sensing mode\t\t-> " << sl::toString(mCamSensingMode).c_str()); - mNhNs.getParam("depth/openni_depth_mode", mOpenniDepthMode); - NODELET_INFO_STREAM(" * OpenNI mode\t\t\t-> " << (mOpenniDepthMode ? "ENABLED" : "DISABLED")); - mNhNs.getParam("depth/depth_stabilization", mDepthStabilization); - NODELET_INFO_STREAM(" * Depth Stabilization\t\t-> " << (mDepthStabilization ? "ENABLED" : "DISABLED")); - mNhNs.getParam("depth/min_depth", mCamMinDepth); - NODELET_INFO_STREAM(" * Minimum depth\t\t-> " << mCamMinDepth << " m"); - mNhNs.getParam("depth/max_depth", mCamMaxDepth); - NODELET_INFO_STREAM(" * Maximum depth\t\t-> " << mCamMaxDepth << " m"); - mNhNs.getParam("depth/depth_downsample_factor", mCamDepthResizeFactor); - NODELET_INFO_STREAM(" * Depth resample factor\t-> " << mCamDepthResizeFactor); - // <----- Depth - - // ----> Tracking - 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)); - - if (mPathMaxCount < 2 && mPathMaxCount != -1) { - mPathMaxCount = 2; - } - - mNhNs.getParam("pos_tracking/initial_base_pose", mInitialBasePose); - - mNhNs.getParam("pos_tracking/area_memory_db_path", mAreaMemDbPath); - NODELET_INFO_STREAM(" * Odometry DB path\t\t-> " << mAreaMemDbPath.c_str()); - mNhNs.param("pos_tracking/area_memory", mAreaMemory, false); - NODELET_INFO_STREAM(" * Spatial Memory\t\t-> " << (mAreaMemory ? "ENABLED" : "DISABLED")); - mNhNs.param("pos_tracking/imu_fusion", mImuFusion, true); - NODELET_INFO_STREAM(" * IMU Fusion\t\t\t-> " << (mImuFusion ? "ENABLED" : "DISABLED")); - mNhNs.param("pos_tracking/floor_alignment", mFloorAlignment, false); - NODELET_INFO_STREAM(" * Floor alignment\t\t-> " << (mFloorAlignment ? "ENABLED" : "DISABLED")); - mNhNs.param("pos_tracking/init_odom_with_first_valid_pose", mInitOdomWithPose, true); - NODELET_INFO_STREAM(" * Init Odometry with first valid pose data -> " << (mInitOdomWithPose ? "ENABLED" : "DISABLED")); - mNhNs.param("pos_tracking/two_d_mode", mTwoDMode, false); - NODELET_INFO_STREAM(" * Two D mode\t\t\t-> " << (mTwoDMode ? "ENABLED" : "DISABLED")); - mNhNs.param("pos_tracking/fixed_z_value", mFixedZValue, 0.0); - - if (mTwoDMode) { - NODELET_INFO_STREAM(" * Fixed Z value\t\t-> " << mFixedZValue); +void ZEDWrapperNodelet::readParameters() +{ + NODELET_INFO_STREAM("*** GENERAL PARAMETERS ***"); + + // ----> General + // Get parameters from param files + mNhNs.getParam("general/camera_name", mCameraName); + NODELET_INFO_STREAM(" * Camera Name\t\t\t-> " << mCameraName.c_str()); + int resol; + mNhNs.getParam("general/resolution", resol); + mCamResol = static_cast(resol); + NODELET_INFO_STREAM(" * Camera Resolution\t\t-> " << sl::toString(mCamResol).c_str()); + mNhNs.getParam("general/grab_frame_rate", mCamFrameRate); + checkResolFps(); + NODELET_INFO_STREAM(" * Camera Grab Framerate\t-> " << mCamFrameRate); + + mNhNs.getParam("general/gpu_id", mGpuId); + NODELET_INFO_STREAM(" * Gpu ID\t\t\t-> " << mGpuId); + mNhNs.getParam("general/zed_id", mZedId); + NODELET_INFO_STREAM(" * Camera ID\t\t\t-> " << mGpuId); + mNhNs.getParam("general/verbose", mVerbose); + NODELET_INFO_STREAM(" * Verbose\t\t\t-> " << (mVerbose ? "ENABLED" : "DISABLED")); + mNhNs.param("general/camera_flip", mCameraFlip, false); + NODELET_INFO_STREAM(" * Camera Flip\t\t\t-> " << (mCameraFlip ? "ENABLED" : "DISABLED")); + mNhNs.param("general/self_calib", mCameraSelfCalib, true); + NODELET_INFO_STREAM(" * Self calibration\t\t-> " << (mCameraSelfCalib ? "ENABLED" : "DISABLED")); + + int tmp_sn = 0; + mNhNs.getParam("general/serial_number", tmp_sn); + + if (tmp_sn > 0) + { + mZedSerialNumber = static_cast(tmp_sn); + NODELET_INFO_STREAM(" * Serial number\t\t-> " << mZedSerialNumber); + } + + string camera_model; + mNhNs.getParam("general/camera_model", camera_model); + + if (camera_model == "zed") + { + mZedUserCamModel = sl::MODEL::ZED; + NODELET_INFO_STREAM(" * Camera Model by param\t-> " << camera_model); + } + else if (camera_model == "zedm") + { + mZedUserCamModel = sl::MODEL::ZED_M; + NODELET_INFO_STREAM(" * Camera Model by param\t-> " << camera_model); + } + else if (camera_model == "zed2") + { + mZedUserCamModel = sl::MODEL::ZED2; + NODELET_INFO_STREAM(" * Camera Model by param\t-> " << camera_model); + } + else + { + NODELET_ERROR_STREAM("Camera model not valid: " << camera_model); + } + // <---- General + + NODELET_INFO_STREAM("*** VIDEO PARAMETERS ***"); + + // ----> Video + mNhNs.getParam("video/img_downsample_factor", mCamImageResizeFactor); + NODELET_INFO_STREAM(" * Image resample factor\t-> " << mCamImageResizeFactor); + + mNhNs.getParam("video/extrinsic_in_camera_frame", mUseOldExtrinsic); + NODELET_INFO_STREAM(" * Extrinsic param. frame\t-> " + << (mUseOldExtrinsic ? "X RIGHT - Y DOWN - Z FWD" : "X FWD - Y LEFT - Z UP")); + // <---- Video + + NODELET_INFO_STREAM("*** DEPTH PARAMETERS ***"); + + // -----> Depth + int depth_mode; + mNhNs.getParam("depth/quality", depth_mode); + mDepthMode = static_cast(depth_mode); + NODELET_INFO_STREAM(" * Depth quality\t\t-> " << sl::toString(mDepthMode).c_str()); + int sensing_mode; + mNhNs.getParam("depth/sensing_mode", sensing_mode); + mCamSensingMode = static_cast(sensing_mode); + NODELET_INFO_STREAM(" * Depth Sensing mode\t\t-> " << sl::toString(mCamSensingMode).c_str()); + mNhNs.getParam("depth/openni_depth_mode", mOpenniDepthMode); + NODELET_INFO_STREAM(" * OpenNI mode\t\t\t-> " << (mOpenniDepthMode ? "ENABLED" : "DISABLED")); + mNhNs.getParam("depth/depth_stabilization", mDepthStabilization); + NODELET_INFO_STREAM(" * Depth Stabilization\t\t-> " << (mDepthStabilization ? "ENABLED" : "DISABLED")); + mNhNs.getParam("depth/min_depth", mCamMinDepth); + NODELET_INFO_STREAM(" * Minimum depth\t\t-> " << mCamMinDepth << " m"); + mNhNs.getParam("depth/max_depth", mCamMaxDepth); + NODELET_INFO_STREAM(" * Maximum depth\t\t-> " << mCamMaxDepth << " m"); + mNhNs.getParam("depth/depth_downsample_factor", mCamDepthResizeFactor); + NODELET_INFO_STREAM(" * Depth resample factor\t-> " << mCamDepthResizeFactor); + // <----- Depth + + NODELET_INFO_STREAM("*** POSITIONAL TRACKING PARAMETERS ***"); + + // ----> Tracking + 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)); + + if (mPathMaxCount < 2 && mPathMaxCount != -1) + { + mPathMaxCount = 2; + } + + mNhNs.getParam("pos_tracking/initial_base_pose", mInitialBasePose); + + mNhNs.getParam("pos_tracking/area_memory_db_path", mAreaMemDbPath); + NODELET_INFO_STREAM(" * Odometry DB path\t\t-> " << mAreaMemDbPath.c_str()); + mNhNs.param("pos_tracking/area_memory", mAreaMemory, false); + NODELET_INFO_STREAM(" * Spatial Memory\t\t-> " << (mAreaMemory ? "ENABLED" : "DISABLED")); + mNhNs.param("pos_tracking/imu_fusion", mImuFusion, true); + NODELET_INFO_STREAM(" * IMU Fusion\t\t\t-> " << (mImuFusion ? "ENABLED" : "DISABLED")); + mNhNs.param("pos_tracking/floor_alignment", mFloorAlignment, false); + NODELET_INFO_STREAM(" * Floor alignment\t\t-> " << (mFloorAlignment ? "ENABLED" : "DISABLED")); + mNhNs.param("pos_tracking/init_odom_with_first_valid_pose", mInitOdomWithPose, true); + NODELET_INFO_STREAM(" * Init Odometry with first valid pose data -> " + << (mInitOdomWithPose ? "ENABLED" : "DISABLED")); + mNhNs.param("pos_tracking/two_d_mode", mTwoDMode, false); + NODELET_INFO_STREAM(" * Two D mode\t\t\t-> " << (mTwoDMode ? "ENABLED" : "DISABLED")); + mNhNs.param("pos_tracking/fixed_z_value", mFixedZValue, 0.0); + + if (mTwoDMode) + { + NODELET_INFO_STREAM(" * Fixed Z value\t\t-> " << mFixedZValue); + } + // <---- Tracking + + NODELET_INFO_STREAM("*** MAPPING PARAMETERS ***"); + + // ----> Mapping + mNhNs.param("mapping/mapping_enabled", mMappingEnabled, false); + + if (mMappingEnabled) + { + NODELET_INFO_STREAM(" * Mapping\t\t\t-> ENABLED"); + + mNhNs.getParam("mapping/resolution", mMappingRes); + NODELET_INFO_STREAM(" * Mapping resolution\t\t-> " << mMappingRes << " m"); + + mNhNs.getParam("mapping/max_mapping_range", mMaxMappingRange); + NODELET_INFO_STREAM(" * Mapping max range\t\t-> " << mMaxMappingRange << " m" + << ((mMaxMappingRange < 0.0) ? " [AUTO]" : "")); + + mNhNs.getParam("mapping/fused_pointcloud_freq", mFusedPcPubFreq); + NODELET_INFO_STREAM(" * Fused point cloud freq:\t-> " << mFusedPcPubFreq << " Hz"); + } + else + { + NODELET_INFO_STREAM(" * Mapping\t\t\t-> DISABLED"); + } + // <---- Mapping + + NODELET_INFO_STREAM("*** OBJECT DETECTION PARAMETERS ***"); + + // ----> Object Detection + mNhNs.param("object_detection/od_enabled", mObjDetEnabled, false); + + if (mObjDetEnabled) + { + NODELET_INFO_STREAM(" * Object Detection\t\t-> ENABLED"); + mNhNs.getParam("object_detection/confidence_threshold", mObjDetConfidence); + NODELET_INFO_STREAM(" * Object confidence\t\t-> " << mObjDetConfidence); + mNhNs.getParam("object_detection/object_tracking_enabled", mObjDetTracking); + NODELET_INFO_STREAM(" * Object tracking\t\t-> " << (mObjDetTracking ? "ENABLED" : "DISABLED")); + mNhNs.getParam("object_detection/max_range", mObjDetMaxRange); + if (mObjDetMaxRange > mCamMaxDepth) + { + NODELET_WARN("Detection max range cannot be major than depth max range. Automatically fixed."); + mObjDetMaxRange = mCamMaxDepth; } - // <---- Tracking - - // ----> Mapping - mNhNs.param("mapping/mapping_enabled", mMappingEnabled, false); - - if (mMappingEnabled) { - NODELET_INFO_STREAM(" * Mapping\t\t\t-> ENABLED"); - - mNhNs.getParam("mapping/resolution", mMappingRes); - NODELET_INFO_STREAM(" * Mapping resolution\t\t-> " << mMappingRes << " m" ); + NODELET_INFO_STREAM(" * Detection max range\t\t-> " << mObjDetMaxRange); - mNhNs.getParam("mapping/max_mapping_range", mMaxMappingRange); - NODELET_INFO_STREAM(" * Mapping max range\t\t-> " << mMaxMappingRange << " m" << ((mMaxMappingRange < 0.0)?" [AUTO]":"")); - - mNhNs.getParam("mapping/fused_pointcloud_freq", mFusedPcPubFreq); - NODELET_INFO_STREAM(" * Fused point cloud freq:\t-> " << mFusedPcPubFreq << " Hz"); - } else { - NODELET_INFO_STREAM(" * Mapping\t\t\t-> DISABLED"); - } - // <---- Mapping - - // ----> Object Detection - mNhNs.param("object_detection/od_enabled", mObjDetEnabled, false); - - if (mObjDetEnabled) { - NODELET_INFO_STREAM(" * Object Detection\t\t-> ENABLED"); - mNhNs.getParam("object_detection/confidence_threshold", mObjDetConfidence); - NODELET_INFO_STREAM(" * Object confidence\t\t-> " << mObjDetConfidence); - mNhNs.getParam("object_detection/object_tracking_enabled", mObjDetTracking); - NODELET_INFO_STREAM(" * Object tracking\t\t-> " << (mObjDetTracking?"ENABLED":"DISABLED")); - mNhNs.getParam("object_detection/people_detection", mObjDetPeople); - NODELET_INFO_STREAM(" * People detection\t\t-> " << (mObjDetPeople?"ENABLED":"DISABLED")); - mNhNs.getParam("object_detection/vehicle_detection", mObjDetVehicles); - NODELET_INFO_STREAM(" * Vehicles detection\t\t-> " << (mObjDetVehicles?"ENABLED":"DISABLED")); - } else { - NODELET_INFO_STREAM(" * Object Detection\t\t-> DISABLED"); + int model; + mNhNs.getParam("object_detection/model", model); + if (model < 0 || model >= static_cast(sl::DETECTION_MODEL::LAST)) + { + NODELET_WARN("Detection model not valid. Forced to the default value"); + model = static_cast(mObjDetModel); } - // <---- Object Detection - - // ----> Sensors - mNhNs.getParam("sensors/sensors_timestamp_sync", mSensTimestampSync); - NODELET_INFO_STREAM(" * Sensors timestamp sync\t-> " << (mSensTimestampSync ? "ENABLED" : "DISABLED")); - // <---- Sensors + mObjDetModel = static_cast(model); - // ----> SVO - mNhNs.param("svo_file", mSvoFilepath, std::string()); - NODELET_INFO_STREAM(" * SVO input file: \t\t-> " << mSvoFilepath.c_str()); + NODELET_INFO_STREAM(" * Detection model\t\t-> " << sl::toString(mObjDetModel)); - int svo_compr = 0; - mNhNs.getParam("general/svo_compression", svo_compr); - - if (svo_compr >= static_cast(sl::SVO_COMPRESSION_MODE::LAST)) { - NODELET_WARN_STREAM("The parameter `general/svo_compression` has an invalid value. Please check it in the configuration file `common.yaml`"); - - svo_compr = 0; + if (mObjDetModel == sl::DETECTION_MODEL::HUMAN_BODY_FAST || + mObjDetModel == sl::DETECTION_MODEL::HUMAN_BODY_ACCURATE) + { + mNhNs.getParam("object_detection/body_fitting", mObjDetBodyFitting); + NODELET_INFO_STREAM(" * Body fitting\t\t\t-> " << (mObjDetBodyFitting ? "ENABLED" : "DISABLED")); } - - mSvoComprMode = static_cast(svo_compr); - - NODELET_INFO_STREAM(" * SVO REC compression\t\t-> " << sl::toString(mSvoComprMode)); - // <---- SVO - - // Remote Stream - mNhNs.param("stream", mRemoteStreamAddr, std::string()); - - // ----> Coordinate frames - mNhNs.param("pos_tracking/map_frame", mMapFrameId, "map"); - mNhNs.param("pos_tracking/odometry_frame", mOdometryFrameId, "odom"); - mNhNs.param("general/base_frame", mBaseFrameId, "base_link"); - - mCameraFrameId = mCameraName + "_camera_center"; - mImuFrameId = mCameraName + "_imu_link"; - mLeftCamFrameId = mCameraName + "_left_camera_frame"; - mLeftCamOptFrameId = mCameraName + "_left_camera_optical_frame"; - mRightCamFrameId = mCameraName + "_right_camera_frame"; - mRightCamOptFrameId = mCameraName + "_right_camera_optical_frame"; - - mBaroFrameId = mCameraName + "_baro_link"; - mMagFrameId = mCameraName + "_mag_link"; - mTempLeftFrameId = mCameraName + "_temp_left_link"; - mTempRightFrameId = mCameraName + "_temp_right_link"; - - mDepthFrameId = mLeftCamFrameId; - mDepthOptFrameId = mLeftCamOptFrameId; - - // Note: Depth image frame id must match color image frame id - mCloudFrameId = mDepthOptFrameId; - mRgbFrameId = mDepthFrameId; - mRgbOptFrameId = mCloudFrameId; - mDisparityFrameId = mDepthFrameId; - mDisparityOptFrameId = mDepthOptFrameId; - mConfidenceFrameId = mDepthFrameId; - mConfidenceOptFrameId = mDepthOptFrameId; - - // Print TF frames - NODELET_INFO_STREAM(" * map_frame\t\t\t-> " << mMapFrameId); - NODELET_INFO_STREAM(" * odometry_frame\t\t-> " << mOdometryFrameId); - NODELET_INFO_STREAM(" * base_frame\t\t\t-> " << mBaseFrameId); - NODELET_INFO_STREAM(" * camera_frame\t\t\t-> " << mCameraFrameId); - NODELET_INFO_STREAM(" * imu_link\t\t\t-> " << mImuFrameId); - NODELET_INFO_STREAM(" * left_camera_frame\t\t-> " << mLeftCamFrameId); - NODELET_INFO_STREAM(" * left_camera_optical_frame\t-> " << mLeftCamOptFrameId); - NODELET_INFO_STREAM(" * right_camera_frame\t\t-> " << mRightCamFrameId); - NODELET_INFO_STREAM(" * right_camera_optical_frame\t-> " << mRightCamOptFrameId); - NODELET_INFO_STREAM(" * depth_frame\t\t\t-> " << mDepthFrameId); - NODELET_INFO_STREAM(" * depth_optical_frame\t\t-> " << mDepthOptFrameId); - NODELET_INFO_STREAM(" * disparity_frame\t\t-> " << mDisparityFrameId); - NODELET_INFO_STREAM(" * disparity_optical_frame\t-> " << mDisparityOptFrameId); - NODELET_INFO_STREAM(" * confidence_frame\t\t-> " << mConfidenceFrameId); - NODELET_INFO_STREAM(" * confidence_optical_frame\t-> " << mConfidenceOptFrameId); - // <---- Coordinate frames - - // ----> TF broadcasting - mNhNs.param("pos_tracking/publish_tf", mPublishTf, true); - NODELET_INFO_STREAM(" * Broadcast odometry TF\t-> " << (mPublishTf ? "ENABLED" : "DISABLED")); - mNhNs.param("pos_tracking/publish_map_tf", mPublishMapTf, true); - NODELET_INFO_STREAM(" * Broadcast map pose TF\t-> " << (mPublishTf ? (mPublishMapTf ? "ENABLED" : "DISABLED") : - "DISABLED")); - mNhNs.param("sensors/publish_imu_tf", mPublishImuTf, true); - NODELET_INFO_STREAM(" * Broadcast IMU pose TF\t-> " << ( mPublishImuTf ? "ENABLED" : "DISABLED" ) ); - // <---- TF broadcasting - - // ----> Dynamic - mNhNs.getParam("depth_confidence", mCamDepthConfidence); - NODELET_INFO_STREAM(" * [DYN] Depth confidence\t-> " << mCamDepthConfidence); - mNhNs.getParam("depth_texture_conf", mCamDepthTextureConf); - NODELET_INFO_STREAM(" * [DYN] Depth texture conf.\t-> " << mCamDepthTextureConf); - - mNhNs.getParam("pub_frame_rate", mVideoDepthFreq); - NODELET_INFO_STREAM(" * [DYN] pub_frame_rate\t\t-> " << mVideoDepthFreq << " Hz"); - mNhNs.getParam("point_cloud_freq", mPointCloudFreq); - NODELET_INFO_STREAM(" * [DYN] point_cloud_freq\t-> " << mPointCloudFreq << " Hz"); - mNhNs.getParam("brightness", mCamBrightness); - NODELET_INFO_STREAM(" * [DYN] brightness\t\t-> " << mCamBrightness); - mNhNs.getParam("contrast", mCamContrast); - NODELET_INFO_STREAM(" * [DYN] contrast\t\t-> " << mCamContrast); - mNhNs.getParam("hue", mCamHue); - NODELET_INFO_STREAM(" * [DYN] hue\t\t\t-> " << mCamHue); - mNhNs.getParam("saturation", mCamSaturation); - NODELET_INFO_STREAM(" * [DYN] saturation\t\t-> " << mCamSaturation); - mNhNs.getParam("sharpness", mCamSharpness); - NODELET_INFO_STREAM(" * [DYN] sharpness\t\t-> " << mCamSharpness); -#if (ZED_SDK_MAJOR_VERSION==3 && ZED_SDK_MINOR_VERSION>=1) - mNhNs.getParam("gamma", mCamGamma); - NODELET_INFO_STREAM(" * [DYN] gamma\t\t\t-> " << mCamGamma); + else + { + mNhNs.getParam("object_detection/mc_people", mObjDetPeopleEnable); + NODELET_INFO_STREAM(" * Detect people\t\t-> " << (mObjDetPeopleEnable ? "ENABLED" : "DISABLED")); + mNhNs.getParam("object_detection/mc_vehicle", mObjDetVehiclesEnable); + NODELET_INFO_STREAM(" * Detect vehicles\t\t-> " << (mObjDetVehiclesEnable ? "ENABLED" : "DISABLED")); + mNhNs.getParam("object_detection/mc_bag", mObjDetBagsEnable); + NODELET_INFO_STREAM(" * Detect bags\t\t\t-> " << (mObjDetBagsEnable ? "ENABLED" : "DISABLED")); + mNhNs.getParam("object_detection/mc_animal", mObjDetAnimalsEnable); + NODELET_INFO_STREAM(" * Detect animals\t\t-> " << (mObjDetAnimalsEnable ? "ENABLED" : "DISABLED")); + mNhNs.getParam("object_detection/mc_electronics", mObjDetElectronicsEnable); + NODELET_INFO_STREAM(" * Detect electronics\t\t-> " << (mObjDetElectronicsEnable ? "ENABLED" : "DISABLED")); + mNhNs.getParam("object_detection/mc_fruit_vegetable", mObjDetFruitsEnable); + NODELET_INFO_STREAM(" * Detect fruit and vegetables\t-> " << (mObjDetFruitsEnable ? "ENABLED" : "DISABLED")); + } + } + else + { + NODELET_INFO_STREAM(" * Object Detection\t\t-> DISABLED"); + } + // <---- Object Detection + + NODELET_INFO_STREAM("*** SENSORS PARAMETERS ***"); + + // ----> Sensors + mNhNs.getParam("sensors/sensors_timestamp_sync", mSensTimestampSync); + NODELET_INFO_STREAM(" * Sensors timestamp sync\t-> " << (mSensTimestampSync ? "ENABLED" : "DISABLED")); + // <---- Sensors + + NODELET_INFO_STREAM("*** SVO PARAMETERS ***"); + + // ----> SVO + mNhNs.param("svo_file", mSvoFilepath, std::string()); + NODELET_INFO_STREAM(" * SVO input file: \t\t-> " << mSvoFilepath.c_str()); + + int svo_compr = 0; + mNhNs.getParam("general/svo_compression", svo_compr); + + if (svo_compr >= static_cast(sl::SVO_COMPRESSION_MODE::LAST)) + { + NODELET_WARN_STREAM("The parameter `general/svo_compression` has an invalid value. Please check it in the " + "configuration file `common.yaml`"); + + svo_compr = 0; + } + + mSvoComprMode = static_cast(svo_compr); + + NODELET_INFO_STREAM(" * SVO REC compression\t\t-> " << sl::toString(mSvoComprMode)); + // <---- SVO + + // Remote Stream + mNhNs.param("stream", mRemoteStreamAddr, std::string()); + + NODELET_INFO_STREAM("*** COORDINATE FRAMES ***"); + + // ----> Coordinate frames + mNhNs.param("pos_tracking/map_frame", mMapFrameId, "map"); + mNhNs.param("pos_tracking/odometry_frame", mOdometryFrameId, "odom"); + mNhNs.param("general/base_frame", mBaseFrameId, "base_link"); + + mCameraFrameId = mCameraName + "_camera_center"; + mImuFrameId = mCameraName + "_imu_link"; + mLeftCamFrameId = mCameraName + "_left_camera_frame"; + mLeftCamOptFrameId = mCameraName + "_left_camera_optical_frame"; + mRightCamFrameId = mCameraName + "_right_camera_frame"; + mRightCamOptFrameId = mCameraName + "_right_camera_optical_frame"; + + mBaroFrameId = mCameraName + "_baro_link"; + mMagFrameId = mCameraName + "_mag_link"; + mTempLeftFrameId = mCameraName + "_temp_left_link"; + mTempRightFrameId = mCameraName + "_temp_right_link"; + + mDepthFrameId = mLeftCamFrameId; + mDepthOptFrameId = mLeftCamOptFrameId; + + // Note: Depth image frame id must match color image frame id + mCloudFrameId = mDepthOptFrameId; + mRgbFrameId = mDepthFrameId; + mRgbOptFrameId = mCloudFrameId; + mDisparityFrameId = mDepthFrameId; + mDisparityOptFrameId = mDepthOptFrameId; + mConfidenceFrameId = mDepthFrameId; + mConfidenceOptFrameId = mDepthOptFrameId; + + // Print TF frames + NODELET_INFO_STREAM(" * map_frame\t\t\t-> " << mMapFrameId); + NODELET_INFO_STREAM(" * odometry_frame\t\t-> " << mOdometryFrameId); + NODELET_INFO_STREAM(" * base_frame\t\t\t-> " << mBaseFrameId); + NODELET_INFO_STREAM(" * camera_frame\t\t\t-> " << mCameraFrameId); + NODELET_INFO_STREAM(" * imu_link\t\t\t-> " << mImuFrameId); + NODELET_INFO_STREAM(" * left_camera_frame\t\t-> " << mLeftCamFrameId); + NODELET_INFO_STREAM(" * left_camera_optical_frame\t-> " << mLeftCamOptFrameId); + NODELET_INFO_STREAM(" * right_camera_frame\t\t-> " << mRightCamFrameId); + NODELET_INFO_STREAM(" * right_camera_optical_frame\t-> " << mRightCamOptFrameId); + NODELET_INFO_STREAM(" * depth_frame\t\t\t-> " << mDepthFrameId); + NODELET_INFO_STREAM(" * depth_optical_frame\t\t-> " << mDepthOptFrameId); + NODELET_INFO_STREAM(" * disparity_frame\t\t-> " << mDisparityFrameId); + NODELET_INFO_STREAM(" * disparity_optical_frame\t-> " << mDisparityOptFrameId); + NODELET_INFO_STREAM(" * confidence_frame\t\t-> " << mConfidenceFrameId); + NODELET_INFO_STREAM(" * confidence_optical_frame\t-> " << mConfidenceOptFrameId); + // <---- Coordinate frames + + // ----> TF broadcasting + mNhNs.param("pos_tracking/publish_tf", mPublishTf, true); + NODELET_INFO_STREAM(" * Broadcast odometry TF\t-> " << (mPublishTf ? "ENABLED" : "DISABLED")); + mNhNs.param("pos_tracking/publish_map_tf", mPublishMapTf, true); + NODELET_INFO_STREAM(" * Broadcast map pose TF\t-> " + << (mPublishTf ? (mPublishMapTf ? "ENABLED" : "DISABLED") : "DISABLED")); + mNhNs.param("sensors/publish_imu_tf", mPublishImuTf, true); + NODELET_INFO_STREAM(" * Broadcast IMU pose TF\t-> " << (mPublishImuTf ? "ENABLED" : "DISABLED")); + // <---- TF broadcasting + + NODELET_INFO_STREAM("*** DYNAMIC PARAMETERS (Init. values) ***"); + + // ----> Dynamic + mNhNs.getParam("depth_confidence", mCamDepthConfidence); + NODELET_INFO_STREAM(" * [DYN] Depth confidence\t-> " << mCamDepthConfidence); + mNhNs.getParam("depth_texture_conf", mCamDepthTextureConf); + NODELET_INFO_STREAM(" * [DYN] Depth texture conf.\t-> " << mCamDepthTextureConf); + + mNhNs.getParam("pub_frame_rate", mVideoDepthFreq); + NODELET_INFO_STREAM(" * [DYN] pub_frame_rate\t\t-> " << mVideoDepthFreq << " Hz"); + mNhNs.getParam("point_cloud_freq", mPointCloudFreq); + NODELET_INFO_STREAM(" * [DYN] point_cloud_freq\t-> " << mPointCloudFreq << " Hz"); + mNhNs.getParam("brightness", mCamBrightness); + NODELET_INFO_STREAM(" * [DYN] brightness\t\t-> " << mCamBrightness); + mNhNs.getParam("contrast", mCamContrast); + NODELET_INFO_STREAM(" * [DYN] contrast\t\t-> " << mCamContrast); + mNhNs.getParam("hue", mCamHue); + NODELET_INFO_STREAM(" * [DYN] hue\t\t\t-> " << mCamHue); + mNhNs.getParam("saturation", mCamSaturation); + NODELET_INFO_STREAM(" * [DYN] saturation\t\t-> " << mCamSaturation); + mNhNs.getParam("sharpness", mCamSharpness); + NODELET_INFO_STREAM(" * [DYN] sharpness\t\t-> " << mCamSharpness); +#if (ZED_SDK_MAJOR_VERSION == 3 && ZED_SDK_MINOR_VERSION >= 1) + mNhNs.getParam("gamma", mCamGamma); + NODELET_INFO_STREAM(" * [DYN] gamma\t\t\t-> " << mCamGamma); #endif - mNhNs.getParam("auto_exposure_gain", mCamAutoExposure); - NODELET_INFO_STREAM(" * [DYN] auto_exposure_gain\t-> " << (mCamAutoExposure ? "ENABLED" : "DISABLED")); - mNhNs.getParam("gain", mCamGain); - mNhNs.getParam("exposure", mCamExposure); - if(!mCamAutoExposure) { - NODELET_INFO_STREAM(" * [DYN] gain\t\t-> " << mCamGain); - NODELET_INFO_STREAM(" * [DYN] exposure\t\t-> " << mCamExposure); - } - mNhNs.getParam("auto_whitebalance", mCamAutoWB); - NODELET_INFO_STREAM(" * [DYN] auto_whitebalance\t-> " << (mCamAutoWB ? "ENABLED" : "DISABLED")); - mNhNs.getParam("whitebalance_temperature", mCamWB); - if(!mCamAutoWB) { - NODELET_INFO_STREAM(" * [DYN] whitebalance_temperature\t\t-> " << mCamWB); - } - - if (mCamAutoExposure) { - mTriggerAutoExposure = true; - } - if (mCamAutoWB) { - mTriggerAutoWB = true; - } - // <---- Dynamic - + mNhNs.getParam("auto_exposure_gain", mCamAutoExposure); + NODELET_INFO_STREAM(" * [DYN] auto_exposure_gain\t-> " << (mCamAutoExposure ? "ENABLED" : "DISABLED")); + mNhNs.getParam("gain", mCamGain); + mNhNs.getParam("exposure", mCamExposure); + if (!mCamAutoExposure) + { + NODELET_INFO_STREAM(" * [DYN] gain\t\t-> " << mCamGain); + NODELET_INFO_STREAM(" * [DYN] exposure\t\t-> " << mCamExposure); + } + mNhNs.getParam("auto_whitebalance", mCamAutoWB); + NODELET_INFO_STREAM(" * [DYN] auto_whitebalance\t-> " << (mCamAutoWB ? "ENABLED" : "DISABLED")); + mNhNs.getParam("whitebalance_temperature", mCamWB); + if (!mCamAutoWB) + { + NODELET_INFO_STREAM(" * [DYN] whitebalance_temperature\t\t-> " << mCamWB); + } + + if (mCamAutoExposure) + { + mTriggerAutoExposure = true; + } + if (mCamAutoWB) + { + mTriggerAutoWB = true; + } + // <---- Dynamic } -void ZEDWrapperNodelet::checkResolFps() { - switch (mCamResol) { +void ZEDWrapperNodelet::checkResolFps() +{ + switch (mCamResol) + { case sl::RESOLUTION::HD2K: - if (mCamFrameRate != 15) { - NODELET_WARN_STREAM("Wrong FrameRate (" - << mCamFrameRate - << ") for the resolution HD2K. Set to 15 FPS."); - mCamFrameRate = 15; - } + if (mCamFrameRate != 15) + { + NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD2K. Set to 15 FPS."); + mCamFrameRate = 15; + } - break; + break; case sl::RESOLUTION::HD1080: - if (mCamFrameRate == 15 || mCamFrameRate == 30) { - break; - } - - if (mCamFrameRate > 15 && mCamFrameRate < 30) { - NODELET_WARN_STREAM("Wrong FrameRate (" - << mCamFrameRate - << ") for the resolution HD1080. Set to 15 FPS."); - mCamFrameRate = 15; - } else if (mCamFrameRate > 30) { - NODELET_WARN_STREAM("Wrong FrameRate (" - << mCamFrameRate - << ") for the resolution HD1080. Set to 30 FPS."); - mCamFrameRate = 30; - } else { - NODELET_WARN_STREAM("Wrong FrameRate (" - << mCamFrameRate - << ") for the resolution HD1080. Set to 15 FPS."); - mCamFrameRate = 15; - } - + if (mCamFrameRate == 15 || mCamFrameRate == 30) + { break; + } + + if (mCamFrameRate > 15 && mCamFrameRate < 30) + { + NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD1080. Set to 15 FPS."); + mCamFrameRate = 15; + } + else if (mCamFrameRate > 30) + { + NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD1080. Set to 30 FPS."); + mCamFrameRate = 30; + } + else + { + NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD1080. Set to 15 FPS."); + mCamFrameRate = 15; + } - case sl::RESOLUTION::HD720: - if (mCamFrameRate == 15 || mCamFrameRate == 30 || mCamFrameRate == 60) { - break; - } - - if (mCamFrameRate > 15 && mCamFrameRate < 30) { - NODELET_WARN_STREAM("Wrong FrameRate (" - << mCamFrameRate - << ") for the resolution HD720. Set to 15 FPS."); - mCamFrameRate = 15; - } else if (mCamFrameRate > 30 && mCamFrameRate < 60) { - NODELET_WARN_STREAM("Wrong FrameRate (" - << mCamFrameRate - << ") for the resolution HD720. Set to 30 FPS."); - mCamFrameRate = 30; - } else if (mCamFrameRate > 60) { - NODELET_WARN_STREAM("Wrong FrameRate (" - << mCamFrameRate - << ") for the resolution HD720. Set to 60 FPS."); - mCamFrameRate = 60; - } else { - NODELET_WARN_STREAM("Wrong FrameRate (" - << mCamFrameRate - << ") for the resolution HD720. Set to 15 FPS."); - mCamFrameRate = 15; - } + break; + case sl::RESOLUTION::HD720: + if (mCamFrameRate == 15 || mCamFrameRate == 30 || mCamFrameRate == 60) + { break; + } + + if (mCamFrameRate > 15 && mCamFrameRate < 30) + { + NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD720. Set to 15 FPS."); + mCamFrameRate = 15; + } + else if (mCamFrameRate > 30 && mCamFrameRate < 60) + { + NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD720. Set to 30 FPS."); + mCamFrameRate = 30; + } + else if (mCamFrameRate > 60) + { + NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD720. Set to 60 FPS."); + mCamFrameRate = 60; + } + else + { + NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD720. Set to 15 FPS."); + mCamFrameRate = 15; + } + + break; case sl::RESOLUTION::VGA: - if (mCamFrameRate == 15 || mCamFrameRate == 30 || mCamFrameRate == 60 || - mCamFrameRate == 100) { - break; - } - - if (mCamFrameRate > 15 && mCamFrameRate < 30) { - NODELET_WARN_STREAM("Wrong FrameRate (" - << mCamFrameRate - << ") for the resolution VGA. Set to 15 FPS."); - mCamFrameRate = 15; - } else if (mCamFrameRate > 30 && mCamFrameRate < 60) { - NODELET_WARN_STREAM("Wrong FrameRate (" - << mCamFrameRate - << ") for the resolution VGA. Set to 30 FPS."); - mCamFrameRate = 30; - } else if (mCamFrameRate > 60 && mCamFrameRate < 100) { - NODELET_WARN_STREAM("Wrong FrameRate (" - << mCamFrameRate - << ") for the resolution VGA. Set to 60 FPS."); - mCamFrameRate = 60; - } else if (mCamFrameRate > 100) { - NODELET_WARN_STREAM("Wrong FrameRate (" - << mCamFrameRate - << ") for the resolution VGA. Set to 100 FPS."); - mCamFrameRate = 100; - } else { - NODELET_WARN_STREAM("Wrong FrameRate (" - << mCamFrameRate - << ") for the resolution VGA. Set to 15 FPS."); - mCamFrameRate = 15; - } - + if (mCamFrameRate == 15 || mCamFrameRate == 30 || mCamFrameRate == 60 || mCamFrameRate == 100) + { break; + } + + if (mCamFrameRate > 15 && mCamFrameRate < 30) + { + NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution VGA. Set to 15 FPS."); + mCamFrameRate = 15; + } + else if (mCamFrameRate > 30 && mCamFrameRate < 60) + { + NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution VGA. Set to 30 FPS."); + mCamFrameRate = 30; + } + else if (mCamFrameRate > 60 && mCamFrameRate < 100) + { + NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution VGA. Set to 60 FPS."); + mCamFrameRate = 60; + } + else if (mCamFrameRate > 100) + { + NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution VGA. Set to 100 FPS."); + mCamFrameRate = 100; + } + else + { + NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution VGA. Set to 15 FPS."); + mCamFrameRate = 15; + } + + break; default: - NODELET_WARN_STREAM("Invalid resolution. Set to HD720 @ 30 FPS"); - mCamResol = sl::RESOLUTION::HD720; - mCamFrameRate = 30; - } + NODELET_WARN_STREAM("Invalid resolution. Set to HD720 @ 30 FPS"); + mCamResol = sl::RESOLUTION::HD720; + mCamFrameRate = 30; + } } -void ZEDWrapperNodelet::initTransforms() { - // According to REP 105 -> http://www.ros.org/reps/rep-0105.html - - // base_link <- odom <- map - // ^ | - // | | - // ------------------- - - // ----> Dynamic transforms - mOdom2BaseTransf.setIdentity(); // broadcasted if `publish_tf` is true - mMap2OdomTransf.setIdentity(); // broadcasted if `publish_map_tf` is true - mMap2BaseTransf.setIdentity(); // used internally, but not broadcasted - mMap2CameraTransf.setIdentity(); // used internally, but not broadcasted - // <---- Dynamic transforms +void ZEDWrapperNodelet::initTransforms() +{ + // According to REP 105 -> http://www.ros.org/reps/rep-0105.html + + // base_link <- odom <- map + // ^ | + // | | + // ------------------- + + // ----> Dynamic transforms + mOdom2BaseTransf.setIdentity(); // broadcasted if `publish_tf` is true + mMap2OdomTransf.setIdentity(); // broadcasted if `publish_map_tf` is true + mMap2BaseTransf.setIdentity(); // used internally, but not broadcasted + mMap2CameraTransf.setIdentity(); // used internally, but not broadcasted + // <---- Dynamic transforms } -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; - - - // ----> Static transforms - // Sensor to Base link - try { - // Save the transformation - geometry_msgs::TransformStamped c2b = - mTfBuffer->lookupTransform(mCameraFrameId, mBaseFrameId, ros::Time(0), ros::Duration(0.1)); - - // Get the TF2 transformation - tf2::fromMsg(c2b.transform, mCamera2BaseTransf); - - double roll, pitch, yaw; - tf2::Matrix3x3(mCamera2BaseTransf.getRotation()).getRPY(roll, pitch, yaw); - - NODELET_INFO("Static transform Camera Center to Base [%s -> %s]", - mCameraFrameId.c_str(), mBaseFrameId.c_str()); - NODELET_INFO(" * Translation: {%.3f,%.3f,%.3f}", - mCamera2BaseTransf.getOrigin().x(), mCamera2BaseTransf.getOrigin().y(), mCamera2BaseTransf.getOrigin().z()); - NODELET_INFO(" * Rotation: {%.3f,%.3f,%.3f}", - roll * RAD2DEG, pitch * RAD2DEG, yaw * RAD2DEG); - - } catch (tf2::TransformException& ex) { - if(!first_error) { - NODELET_DEBUG_THROTTLE(1.0, "Transform error: %s", ex.what()); - NODELET_WARN_THROTTLE(1.0, "The tf from '%s' to '%s' is not available.", - mCameraFrameId.c_str(), mBaseFrameId.c_str()); - NODELET_WARN_THROTTLE(1.0, "Note: one of the possible cause of the problem is the absense of an instance " - "of the `robot_state_publisher` node publishing the correct static TF transformations " - "or a modified URDF not correctly reproducing the ZED " - "TF chain '%s' -> '%s' -> '%s'", - mBaseFrameId.c_str(), mCameraFrameId.c_str(),mDepthFrameId.c_str()); - first_error=false; - } - - mCamera2BaseTransf.setIdentity(); - return false; - } +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; + + // ----> Static transforms + // Sensor to Base link + try + { + // Save the transformation + geometry_msgs::TransformStamped c2b = + mTfBuffer->lookupTransform(mCameraFrameId, mBaseFrameId, ros::Time(0), ros::Duration(0.1)); + + // Get the TF2 transformation + tf2::fromMsg(c2b.transform, mCamera2BaseTransf); + + double roll, pitch, yaw; + tf2::Matrix3x3(mCamera2BaseTransf.getRotation()).getRPY(roll, pitch, yaw); + + NODELET_INFO("Static transform Camera Center to Base [%s -> %s]", mCameraFrameId.c_str(), mBaseFrameId.c_str()); + NODELET_INFO(" * Translation: {%.3f,%.3f,%.3f}", mCamera2BaseTransf.getOrigin().x(), + mCamera2BaseTransf.getOrigin().y(), mCamera2BaseTransf.getOrigin().z()); + NODELET_INFO(" * Rotation: {%.3f,%.3f,%.3f}", roll * RAD2DEG, pitch * RAD2DEG, yaw * RAD2DEG); + } + catch (tf2::TransformException& ex) + { + if (!first_error) + { + NODELET_DEBUG_THROTTLE(1.0, "Transform error: %s", ex.what()); + NODELET_WARN_THROTTLE(1.0, "The tf from '%s' to '%s' is not available.", mCameraFrameId.c_str(), + mBaseFrameId.c_str()); + NODELET_WARN_THROTTLE(1.0, + "Note: one of the possible cause of the problem is the absense of an instance " + "of the `robot_state_publisher` node publishing the correct static TF transformations " + "or a modified URDF not correctly reproducing the ZED " + "TF chain '%s' -> '%s' -> '%s'", + mBaseFrameId.c_str(), mCameraFrameId.c_str(), mDepthFrameId.c_str()); + first_error = false; + } + + mCamera2BaseTransf.setIdentity(); + return false; + } - // <---- Static transforms - mCamera2BaseTransfValid = true; - return true; + // <---- Static transforms + mCamera2BaseTransfValid = true; + return true; } -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; - - // ----> Static transforms - // Sensor to Camera Center - try { - // Save the transformation - geometry_msgs::TransformStamped s2c = - mTfBuffer->lookupTransform(mDepthFrameId, mCameraFrameId, ros::Time(0), ros::Duration(0.1)); - // Get the TF2 transformation - tf2::fromMsg(s2c.transform, mSensor2CameraTransf); - - double roll, pitch, yaw; - tf2::Matrix3x3(mSensor2CameraTransf.getRotation()).getRPY(roll, pitch, yaw); - - NODELET_INFO("Static transform Sensor to Camera Center [%s -> %s]", - mDepthFrameId.c_str(), mCameraFrameId.c_str()); - NODELET_INFO(" * Translation: {%.3f,%.3f,%.3f}", - mSensor2CameraTransf.getOrigin().x(), mSensor2CameraTransf.getOrigin().y(), mSensor2CameraTransf.getOrigin().z()); - NODELET_INFO(" * Rotation: {%.3f,%.3f,%.3f}", - roll * RAD2DEG, pitch * RAD2DEG, yaw * RAD2DEG); - } catch (tf2::TransformException& ex) { - if(!first_error) { - NODELET_DEBUG_THROTTLE(1.0, "Transform error: %s", ex.what()); - NODELET_WARN_THROTTLE(1.0, "The tf from '%s' to '%s' is not available.", - mDepthFrameId.c_str(), mCameraFrameId.c_str()); - NODELET_WARN_THROTTLE(1.0, "Note: one of the possible cause of the problem is the absense of an instance " - "of the `robot_state_publisher` node publishing the correct static TF transformations " - "or a modified URDF not correctly reproducing the ZED " - "TF chain '%s' -> '%s' -> '%s'", - mBaseFrameId.c_str(), mCameraFrameId.c_str(),mDepthFrameId.c_str()); - first_error = false; - } - - mSensor2CameraTransf.setIdentity(); - return false; - } +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; + + // ----> Static transforms + // Sensor to Camera Center + try + { + // Save the transformation + geometry_msgs::TransformStamped s2c = + mTfBuffer->lookupTransform(mDepthFrameId, mCameraFrameId, ros::Time(0), ros::Duration(0.1)); + // Get the TF2 transformation + tf2::fromMsg(s2c.transform, mSensor2CameraTransf); + + double roll, pitch, yaw; + tf2::Matrix3x3(mSensor2CameraTransf.getRotation()).getRPY(roll, pitch, yaw); + + NODELET_INFO("Static transform Sensor to Camera Center [%s -> %s]", mDepthFrameId.c_str(), mCameraFrameId.c_str()); + NODELET_INFO(" * Translation: {%.3f,%.3f,%.3f}", mSensor2CameraTransf.getOrigin().x(), + mSensor2CameraTransf.getOrigin().y(), mSensor2CameraTransf.getOrigin().z()); + NODELET_INFO(" * Rotation: {%.3f,%.3f,%.3f}", roll * RAD2DEG, pitch * RAD2DEG, yaw * RAD2DEG); + } + catch (tf2::TransformException& ex) + { + if (!first_error) + { + NODELET_DEBUG_THROTTLE(1.0, "Transform error: %s", ex.what()); + NODELET_WARN_THROTTLE(1.0, "The tf from '%s' to '%s' is not available.", mDepthFrameId.c_str(), + mCameraFrameId.c_str()); + NODELET_WARN_THROTTLE(1.0, + "Note: one of the possible cause of the problem is the absense of an instance " + "of the `robot_state_publisher` node publishing the correct static TF transformations " + "or a modified URDF not correctly reproducing the ZED " + "TF chain '%s' -> '%s' -> '%s'", + mBaseFrameId.c_str(), mCameraFrameId.c_str(), mDepthFrameId.c_str()); + first_error = false; + } + + mSensor2CameraTransf.setIdentity(); + return false; + } - // <---- Static transforms + // <---- Static transforms - mSensor2CameraTransfValid = true; - return true; + mSensor2CameraTransfValid = true; + return true; } -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; - - // ----> Static transforms - // Sensor to Base link - try { - // Save the transformation - geometry_msgs::TransformStamped s2b = - mTfBuffer->lookupTransform(mDepthFrameId, mBaseFrameId, ros::Time(0), ros::Duration(0.1)); - // Get the TF2 transformation - tf2::fromMsg(s2b.transform, mSensor2BaseTransf); - - double roll, pitch, yaw; - tf2::Matrix3x3(mSensor2BaseTransf.getRotation()).getRPY(roll, pitch, yaw); - - NODELET_INFO("Static transform Sensor to Base [%s -> %s]", - mDepthFrameId.c_str(), mBaseFrameId.c_str()); - NODELET_INFO(" * Translation: {%.3f,%.3f,%.3f}", - mSensor2BaseTransf.getOrigin().x(), mSensor2BaseTransf.getOrigin().y(), mSensor2BaseTransf.getOrigin().z()); - NODELET_INFO(" * Rotation: {%.3f,%.3f,%.3f}", - roll * RAD2DEG, pitch * RAD2DEG, yaw * RAD2DEG); - - } catch (tf2::TransformException& ex) { - if(!first_error) { - NODELET_DEBUG_THROTTLE(1.0, "Transform error: %s", ex.what()); - NODELET_WARN_THROTTLE(1.0, "The tf from '%s' to '%s' is not available.", - mDepthFrameId.c_str(), mBaseFrameId.c_str()); - NODELET_WARN_THROTTLE(1.0, "Note: one of the possible cause of the problem is the absense of an instance " - "of the `robot_state_publisher` node publishing the correct static TF transformations " - "or a modified URDF not correctly reproducing the ZED " - "TF chain '%s' -> '%s' -> '%s'", - mBaseFrameId.c_str(), mCameraFrameId.c_str(),mDepthFrameId.c_str()); - first_error = false; - } +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; + + // ----> Static transforms + // Sensor to Base link + try + { + // Save the transformation + geometry_msgs::TransformStamped s2b = + mTfBuffer->lookupTransform(mDepthFrameId, mBaseFrameId, ros::Time(0), ros::Duration(0.1)); + // Get the TF2 transformation + tf2::fromMsg(s2b.transform, mSensor2BaseTransf); + + double roll, pitch, yaw; + tf2::Matrix3x3(mSensor2BaseTransf.getRotation()).getRPY(roll, pitch, yaw); + + NODELET_INFO("Static transform Sensor to Base [%s -> %s]", mDepthFrameId.c_str(), mBaseFrameId.c_str()); + NODELET_INFO(" * Translation: {%.3f,%.3f,%.3f}", mSensor2BaseTransf.getOrigin().x(), + mSensor2BaseTransf.getOrigin().y(), mSensor2BaseTransf.getOrigin().z()); + NODELET_INFO(" * Rotation: {%.3f,%.3f,%.3f}", roll * RAD2DEG, pitch * RAD2DEG, yaw * RAD2DEG); + } + catch (tf2::TransformException& ex) + { + if (!first_error) + { + NODELET_DEBUG_THROTTLE(1.0, "Transform error: %s", ex.what()); + NODELET_WARN_THROTTLE(1.0, "The tf from '%s' to '%s' is not available.", mDepthFrameId.c_str(), + mBaseFrameId.c_str()); + NODELET_WARN_THROTTLE(1.0, + "Note: one of the possible cause of the problem is the absense of an instance " + "of the `robot_state_publisher` node publishing the correct static TF transformations " + "or a modified URDF not correctly reproducing the ZED " + "TF chain '%s' -> '%s' -> '%s'", + mBaseFrameId.c_str(), mCameraFrameId.c_str(), mDepthFrameId.c_str()); + first_error = false; + } + + mSensor2BaseTransf.setIdentity(); + return false; + } - mSensor2BaseTransf.setIdentity(); - return false; - } + // <---- Static transforms - // <---- Static transforms + mSensor2BaseTransfValid = true; + return true; +} - mSensor2BaseTransfValid = true; - return true; +bool ZEDWrapperNodelet::set_pose(float xt, float yt, float zt, float rr, float pr, float yr) +{ + initTransforms(); + + if (!mSensor2BaseTransfValid) + { + getSens2BaseTransform(); + } + + if (!mSensor2CameraTransfValid) + { + getSens2CameraTransform(); + } + + if (!mCamera2BaseTransfValid) + { + getCamera2BaseTransform(); + } + + // Apply Base to sensor transform + tf2::Transform initPose; + tf2::Vector3 origin(xt, yt, zt); + initPose.setOrigin(origin); + tf2::Quaternion quat; + quat.setRPY(rr, pr, yr); + initPose.setRotation(quat); + + initPose = initPose * mSensor2BaseTransf.inverse(); + + // SL pose + sl::float3 t_vec; + t_vec[0] = initPose.getOrigin().x(); + t_vec[1] = initPose.getOrigin().y(); + t_vec[2] = initPose.getOrigin().z(); + + sl::float4 q_vec; + q_vec[0] = initPose.getRotation().x(); + q_vec[1] = initPose.getRotation().y(); + q_vec[2] = initPose.getRotation().z(); + q_vec[3] = initPose.getRotation().w(); + + sl::Translation trasl(t_vec); + sl::Orientation orient(q_vec); + mInitialPoseSl.setTranslation(trasl); + mInitialPoseSl.setOrientation(orient); + + return (mSensor2BaseTransfValid & mSensor2CameraTransfValid & mCamera2BaseTransfValid); } -bool ZEDWrapperNodelet::set_pose(float xt, float yt, float zt, float rr, - float pr, float yr) { - initTransforms(); +bool ZEDWrapperNodelet::on_set_pose(zed_interfaces::set_pose::Request& req, zed_interfaces::set_pose::Response& res) +{ + mInitialBasePose.resize(6); + mInitialBasePose[0] = req.x; + mInitialBasePose[1] = req.y; + mInitialBasePose[2] = req.z; + mInitialBasePose[3] = req.R; + mInitialBasePose[4] = req.P; + mInitialBasePose[5] = req.Y; + + if (!set_pose(mInitialBasePose[0], mInitialBasePose[1], mInitialBasePose[2], mInitialBasePose[3], mInitialBasePose[4], + mInitialBasePose[5])) + { + res.done = false; + return false; + } - if (!mSensor2BaseTransfValid) { - getSens2BaseTransform(); - } + std::lock_guard lock(mPosTrkMutex); - if (!mSensor2CameraTransfValid) { - getSens2CameraTransform(); - } + // Disable tracking + mTrackingActivated = false; + mZed.disablePositionalTracking(); - if (!mCamera2BaseTransfValid) { - getCamera2BaseTransform(); - } + // Restart tracking + start_pos_tracking(); + + res.done = true; + return true; +} - // Apply Base to sensor transform - tf2::Transform initPose; - tf2::Vector3 origin(xt, yt, zt); - initPose.setOrigin(origin); - tf2::Quaternion quat; - quat.setRPY(rr, pr, yr); - initPose.setRotation(quat); +bool ZEDWrapperNodelet::on_reset_tracking(zed_interfaces::reset_tracking::Request& req, + zed_interfaces::reset_tracking::Response& res) +{ + if (!mTrackingActivated) + { + res.reset_done = false; + return false; + } - initPose = initPose * mSensor2BaseTransf.inverse(); + if (!set_pose(mInitialBasePose[0], mInitialBasePose[1], mInitialBasePose[2], mInitialBasePose[3], mInitialBasePose[4], + mInitialBasePose[5])) + { + res.reset_done = false; + return false; + } - // SL pose - sl::float3 t_vec; - t_vec[0] = initPose.getOrigin().x(); - t_vec[1] = initPose.getOrigin().y(); - t_vec[2] = initPose.getOrigin().z(); + std::lock_guard lock(mPosTrkMutex); - sl::float4 q_vec; - q_vec[0] = initPose.getRotation().x(); - q_vec[1] = initPose.getRotation().y(); - q_vec[2] = initPose.getRotation().z(); - q_vec[3] = initPose.getRotation().w(); + // Disable tracking + mTrackingActivated = false; + mZed.disablePositionalTracking(); - sl::Translation trasl(t_vec); - sl::Orientation orient(q_vec); - mInitialPoseSl.setTranslation(trasl); - mInitialPoseSl.setOrientation(orient); + // Restart tracking + start_pos_tracking(); - return (mSensor2BaseTransfValid & mSensor2CameraTransfValid & mCamera2BaseTransfValid); + res.reset_done = true; + return true; +} +bool ZEDWrapperNodelet::on_reset_odometry(zed_interfaces::reset_odometry::Request& req, + zed_interfaces::reset_odometry::Response& res) +{ + mResetOdom = true; + res.reset_done = true; + return true; } -bool ZEDWrapperNodelet::on_set_pose( - zed_interfaces::set_pose::Request& req, - zed_interfaces::set_pose::Response& res) { - mInitialBasePose.resize(6); - mInitialBasePose[0] = req.x; - mInitialBasePose[1] = req.y; - mInitialBasePose[2] = req.z; - mInitialBasePose[3] = req.R; - mInitialBasePose[4] = req.P; - mInitialBasePose[5] = req.Y; - - if (!set_pose(mInitialBasePose[0], mInitialBasePose[1], mInitialBasePose[2], - mInitialBasePose[3], mInitialBasePose[4], mInitialBasePose[5])) { - res.done = false; - return false; +bool ZEDWrapperNodelet::start_3d_mapping() +{ + if (!mMappingEnabled) + { + return false; + } + + NODELET_INFO_STREAM("*** Starting Spatial Mapping ***"); + + sl::SpatialMappingParameters params; + params.map_type = sl::SpatialMappingParameters::SPATIAL_MAP_TYPE::FUSED_POINT_CLOUD; + params.use_chunk_only = true; + + sl::SpatialMappingParameters spMapPar; + + float lRes = spMapPar.allowed_resolution.first; + float hRes = spMapPar.allowed_resolution.second; + + if (mMappingRes < lRes) + { + NODELET_WARN_STREAM("'mapping/resolution' value (" + << mMappingRes << " m) is lower than the allowed resolution values. Fixed automatically"); + mMappingRes = lRes; + } + if (mMappingRes > hRes) + { + NODELET_WARN_STREAM("'mapping/resolution' value (" + << mMappingRes << " m) is higher than the allowed resolution values. Fixed automatically"); + mMappingRes = hRes; + } + + params.resolution_meter = mMappingRes; + + float lRng = spMapPar.allowed_range.first; + float hRng = spMapPar.allowed_range.second; + + if (mMaxMappingRange < 0) + { + mMaxMappingRange = sl::SpatialMappingParameters::getRecommendedRange(mMappingRes, mZed); + NODELET_INFO_STREAM("Mapping: max range set to " << mMaxMappingRange << " m for a resolution of " << mMappingRes + << " m"); + } + else if (mMaxMappingRange < lRng) + { + NODELET_WARN_STREAM("'mapping/max_mapping_range_m' value (" + << mMaxMappingRange << " m) is lower than the allowed resolution values. Fixed automatically"); + mMaxMappingRange = lRng; + } + else if (mMaxMappingRange > hRng) + { + NODELET_WARN_STREAM("'mapping/max_mapping_range_m' value (" + << mMaxMappingRange << " m) is higher than the allowed resolution values. Fixed automatically"); + mMaxMappingRange = hRng; + } + + params.range_meter = mMaxMappingRange; + + sl::ERROR_CODE err = mZed.enableSpatialMapping(params); + + if (err == sl::ERROR_CODE::SUCCESS) + { + if (mPubFusedCloud.getTopic().empty()) + { + string pointcloud_fused_topic = "mapping/fused_cloud"; + mPubFusedCloud = mNhNs.advertise(pointcloud_fused_topic, 1); + NODELET_INFO_STREAM("Advertised on topic " << mPubFusedCloud.getTopic() << " @ " << mFusedPcPubFreq << " Hz"); } - std::lock_guard lock(mPosTrkMutex); + mMappingRunning = true; - // Disable tracking - mTrackingActivated = false; - mZed.disablePositionalTracking(); + mFusedPcTimer = + mNhNs.createTimer(ros::Duration(1.0 / mFusedPcPubFreq), &ZEDWrapperNodelet::callback_pubFusedPointCloud, this); - // Restart tracking - start_pos_tracking(); + NODELET_INFO_STREAM(" * Resolution: " << params.resolution_meter << " m"); + NODELET_INFO_STREAM(" * Max Mapping Range: " << params.range_meter << " m"); + NODELET_INFO_STREAM(" * Map point cloud publishing rate: " << mFusedPcPubFreq << " Hz"); - res.done = true; return true; + } + else + { + mMappingRunning = false; + mFusedPcTimer.stop(); + + NODELET_WARN("Mapping not activated: %s", sl::toString(err).c_str()); + + return false; + } } -bool ZEDWrapperNodelet::on_reset_tracking( - zed_interfaces::reset_tracking::Request& req, - zed_interfaces::reset_tracking::Response& res) { - if (!mTrackingActivated) { - res.reset_done = false; - return false; - } +void ZEDWrapperNodelet::stop_3d_mapping() +{ + mFusedPcTimer.stop(); + mMappingRunning = false; + mMappingEnabled = false; + mZed.disableSpatialMapping(); - if (!set_pose(mInitialBasePose[0], mInitialBasePose[1], mInitialBasePose[2], - mInitialBasePose[3], mInitialBasePose[4], mInitialBasePose[5])) { - res.reset_done = false; - return false; - } + NODELET_INFO("*** Spatial Mapping stopped ***"); +} - std::lock_guard lock(mPosTrkMutex); +bool ZEDWrapperNodelet::start_obj_detect() +{ + if (mZedRealCamModel != sl::MODEL::ZED2) + { + NODELET_ERROR_STREAM("Object detection not started. OD is available only using a ZED2 camera model"); + return false; + } - // Disable tracking - mTrackingActivated = false; - mZed.disablePositionalTracking(); + if (!mObjDetEnabled) + { + return false; + } - // Restart tracking - start_pos_tracking(); + if (!mCamera2BaseTransfValid || !mSensor2CameraTransfValid || !mSensor2BaseTransfValid) + { + NODELET_DEBUG("Tracking transforms not yet ready, OD starting postponed"); + return false; + } - res.reset_done = true; - return true; -} + NODELET_INFO_STREAM("*** Starting Object Detection ***"); -bool ZEDWrapperNodelet::on_reset_odometry( - zed_interfaces::reset_odometry::Request& req, - zed_interfaces::reset_odometry::Response& res) { - mResetOdom = true; - res.reset_done = true; - return true; -} + sl::ObjectDetectionParameters od_p; + od_p.enable_mask_output = false; + od_p.enable_tracking = mObjDetTracking; + od_p.image_sync = false; // Asynchronous object detection + od_p.detection_model = mObjDetModel; + od_p.enable_body_fitting = mObjDetBodyFitting; + od_p.max_range = mObjDetMaxRange; -bool ZEDWrapperNodelet::start_3d_mapping() { - if (!mMappingEnabled) { - return false; - } + sl::ERROR_CODE objDetError = mZed.enableObjectDetection(od_p); + + if (objDetError != sl::ERROR_CODE::SUCCESS) + { + NODELET_ERROR_STREAM("Object detection error: " << sl::toString(objDetError)); - NODELET_INFO_STREAM("*** Starting Spatial Mapping ***"); + mObjDetRunning = false; + return false; + } - sl::SpatialMappingParameters params; - params.map_type = sl::SpatialMappingParameters::SPATIAL_MAP_TYPE::FUSED_POINT_CLOUD; - params.use_chunk_only = true; + if (mPubObjDet.getTopic().empty()) + { + string object_det_topic_root = "obj_det"; + string object_det_topic = object_det_topic_root + "/objects"; - sl::SpatialMappingParameters spMapPar; + mPubObjDet = mNhNs.advertise(object_det_topic, 1); + NODELET_INFO_STREAM("Advertised on topic " << mPubObjDet.getTopic()); + } - float lRes = spMapPar.allowed_resolution.first; - float hRes = spMapPar.allowed_resolution.second; + mObjDetFilter.clear(); - if(mMappingRes < lRes) { - NODELET_WARN_STREAM( "'mapping/resolution' value (" << mMappingRes << " m) is lower than the allowed resolution values. Fixed automatically" ); - mMappingRes = lRes; + if (mObjDetModel == sl::DETECTION_MODEL::MULTI_CLASS_BOX || + mObjDetModel == sl::DETECTION_MODEL::MULTI_CLASS_BOX_ACCURATE) + { + if (mObjDetPeopleEnable) + { + mObjDetFilter.push_back(sl::OBJECT_CLASS::PERSON); + } + if (mObjDetVehiclesEnable) + { + mObjDetFilter.push_back(sl::OBJECT_CLASS::VEHICLE); + } + if (mObjDetBagsEnable) + { + mObjDetFilter.push_back(sl::OBJECT_CLASS::BAG); + } + if (mObjDetAnimalsEnable) + { + mObjDetFilter.push_back(sl::OBJECT_CLASS::ANIMAL); + } + if (mObjDetElectronicsEnable) + { + mObjDetFilter.push_back(sl::OBJECT_CLASS::ELECTRONICS); } - if(mMappingRes > hRes) { - NODELET_WARN_STREAM( "'mapping/resolution' value (" << mMappingRes << " m) is higher than the allowed resolution values. Fixed automatically" ); - mMappingRes = hRes; + if (mObjDetFruitsEnable) + { + mObjDetFilter.push_back(sl::OBJECT_CLASS::FRUIT_VEGETABLE); } + } - params.resolution_meter = mMappingRes; + mObjDetRunning = true; + return false; +} - float lRng = spMapPar.allowed_range.first; - float hRng = spMapPar.allowed_range.second; +void ZEDWrapperNodelet::stop_obj_detect() +{ + if (mObjDetRunning) + { + NODELET_INFO_STREAM("*** Stopping Object Detection ***"); + mObjDetRunning = false; + mObjDetEnabled = false; + mZed.disableObjectDetection(); + } +} - if(mMaxMappingRange < 0) { - mMaxMappingRange = sl::SpatialMappingParameters::getRecommendedRange( mMappingRes, mZed ); - NODELET_INFO_STREAM("Mapping: max range set to " << mMaxMappingRange << " m for a resolution of " << mMappingRes << " m" ); - } else if(mMaxMappingRange < lRng) { - NODELET_WARN_STREAM( "'mapping/max_mapping_range_m' value (" << mMaxMappingRange << " m) is lower than the allowed resolution values. Fixed automatically" ); - mMaxMappingRange = lRng; - } else if(mMaxMappingRange > hRng) { - NODELET_WARN_STREAM( "'mapping/max_mapping_range_m' value (" << mMaxMappingRange << " m) is higher than the allowed resolution values. Fixed automatically" ); - mMaxMappingRange = hRng; - } +void ZEDWrapperNodelet::start_pos_tracking() +{ + NODELET_INFO_STREAM("*** Starting Positional Tracking ***"); - params.range_meter = mMaxMappingRange; + NODELET_INFO(" * Waiting for valid static transformations..."); - sl::ERROR_CODE err = mZed.enableSpatialMapping(params); + bool transformOk = false; + double elapsed = 0.0; - if (err == sl::ERROR_CODE::SUCCESS) { - if(mPubFusedCloud.getTopic().empty()) { - string pointcloud_fused_topic = "mapping/fused_cloud"; - mPubFusedCloud = mNhNs.advertise(pointcloud_fused_topic, 1); - NODELET_INFO_STREAM("Advertised on topic " << mPubFusedCloud.getTopic() << " @ " << mFusedPcPubFreq << " Hz"); - } + auto start = std::chrono::high_resolution_clock::now(); - mMappingRunning = true; + do + { + transformOk = set_pose(mInitialBasePose[0], mInitialBasePose[1], mInitialBasePose[2], mInitialBasePose[3], + mInitialBasePose[4], mInitialBasePose[5]); - mFusedPcTimer = mNhNs.createTimer(ros::Duration(1.0 / mFusedPcPubFreq), &ZEDWrapperNodelet::callback_pubFusedPointCloud, - this); + elapsed = std::chrono::duration_cast(std::chrono::high_resolution_clock::now() - start) + .count(); - NODELET_INFO_STREAM(" * Resolution: " << params.resolution_meter << " m"); - NODELET_INFO_STREAM(" * Max Mapping Range: " << params.range_meter << " m"); - NODELET_INFO_STREAM(" * Map point cloud publishing rate: " << mFusedPcPubFreq << " Hz"); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); - return true; - } else { - mMappingRunning = false; - mFusedPcTimer.stop(); + if (elapsed > 10000) + { + NODELET_WARN(" !!! Failed to get static transforms. Is the 'ROBOT STATE PUBLISHER' node correctly working? "); + break; + } - NODELET_WARN("Mapping not activated: %s", sl::toString(err).c_str()); + } while (transformOk == false); - return false; - } -} + if (transformOk) + { + NODELET_DEBUG("Time required to get valid static transforms: %g sec", elapsed / 1000.); + } -void ZEDWrapperNodelet::stop_3d_mapping() { - mFusedPcTimer.stop(); - mMappingRunning = false; - mMappingEnabled = false; - mZed.disableSpatialMapping(); + NODELET_INFO("Initial ZED left camera pose (ZED pos. tracking): "); + NODELET_INFO(" * T: [%g,%g,%g]", mInitialPoseSl.getTranslation().x, mInitialPoseSl.getTranslation().y, + mInitialPoseSl.getTranslation().z); + NODELET_INFO(" * Q: [%g,%g,%g,%g]", mInitialPoseSl.getOrientation().ox, mInitialPoseSl.getOrientation().oy, + mInitialPoseSl.getOrientation().oz, mInitialPoseSl.getOrientation().ow); - NODELET_INFO("*** Spatial Mapping stopped ***"); -} + if (mAreaMemDbPath != "" && !sl_tools::file_exist(mAreaMemDbPath)) + { + mAreaMemDbPath = ""; + NODELET_WARN("area_memory_db_path path doesn't exist or is unreachable."); + } -bool ZEDWrapperNodelet::start_obj_detect() { - if(mZedRealCamModel!=sl::MODEL::ZED2) { - NODELET_ERROR_STREAM( "Object detection not started. OD is available only using a ZED2 camera model"); - return false; - } + // Tracking parameters + sl::PositionalTrackingParameters trackParams; - if(!mObjDetEnabled) { - return false; - } + trackParams.area_file_path = mAreaMemDbPath.c_str(); - if( !mCamera2BaseTransfValid || !mSensor2CameraTransfValid || !mSensor2BaseTransfValid) { - NODELET_DEBUG( "Tracking transforms not yet ready, OD starting postponed"); - return false; - } + mPoseSmoothing = false; // Always false. Pose Smoothing is to be enabled only for VR/AR applications + trackParams.enable_pose_smoothing = mPoseSmoothing; - NODELET_INFO_STREAM("*** Starting Object Detection ***"); + trackParams.enable_area_memory = mAreaMemory; + trackParams.enable_imu_fusion = mImuFusion; + trackParams.initial_world_transform = mInitialPoseSl; - sl::ObjectDetectionParameters od_p; - od_p.enable_mask_output = false; - od_p.enable_tracking = mObjDetTracking; - //od_p.image_sync = true; - od_p.image_sync = false; // Asynchronous object detection + trackParams.set_floor_as_origin = mFloorAlignment; - sl::ERROR_CODE objDetError = mZed.enableObjectDetection(od_p); + sl::ERROR_CODE err = mZed.enablePositionalTracking(trackParams); - if (objDetError != sl::ERROR_CODE::SUCCESS) { - NODELET_ERROR_STREAM("Object detection error: " << sl::toString(objDetError)); + if (err == sl::ERROR_CODE::SUCCESS) + { + mTrackingActivated = true; + } + else + { + mTrackingActivated = false; - mObjDetRunning = false; - return false; - } + NODELET_WARN("Tracking not activated: %s", sl::toString(err).c_str()); + } +} - if(mPubObjDet.getTopic().empty()) { - string object_det_topic_root = "obj_det"; - string object_det_topic = object_det_topic_root + "/objects"; - string object_det_rviz_topic = object_det_topic_root + "/object_markers"; +void ZEDWrapperNodelet::publishOdom(tf2::Transform odom2baseTransf, sl::Pose& slPose, ros::Time t) +{ + nav_msgs::OdometryPtr odomMsg = boost::make_shared(); + + odomMsg->header.stamp = t; + odomMsg->header.frame_id = mOdometryFrameId; // frame + odomMsg->child_frame_id = mBaseFrameId; // camera_frame + // conversion from Tranform to message + geometry_msgs::Transform base2odom = tf2::toMsg(odom2baseTransf); + // Add all value in odometry message + odomMsg->pose.pose.position.x = base2odom.translation.x; + odomMsg->pose.pose.position.y = base2odom.translation.y; + odomMsg->pose.pose.position.z = base2odom.translation.z; + odomMsg->pose.pose.orientation.x = base2odom.rotation.x; + odomMsg->pose.pose.orientation.y = base2odom.rotation.y; + odomMsg->pose.pose.orientation.z = base2odom.rotation.z; + odomMsg->pose.pose.orientation.w = base2odom.rotation.w; + + // Odometry pose covariance + + for (size_t i = 0; i < odomMsg->pose.covariance.size(); i++) + { + odomMsg->pose.covariance[i] = static_cast(slPose.pose_covariance[i]); + + if (mTwoDMode) + { + if (i == 14 || i == 21 || i == 28) + { + odomMsg->pose.covariance[i] = 1e-9; // Very low covariance if 2D mode + } + else if ((i >= 2 && i <= 4) || (i >= 8 && i <= 10) || (i >= 12 && i <= 13) || (i >= 15 && i <= 16) || + (i >= 18 && i <= 20) || (i == 22) || (i >= 24 && i <= 27)) + { + odomMsg->pose.covariance[i] = 0.0; + } + } + } + + // Publish odometry message + mPubOdom.publish(odomMsg); +} - mPubObjDet = mNhNs.advertise(object_det_topic, 1); - NODELET_INFO_STREAM("Advertised on topic " << mPubObjDet.getTopic()); - mPubObjDetViz = mNhNs.advertise(object_det_rviz_topic, 1); - NODELET_INFO_STREAM("Advertised on topic " << mPubObjDetViz.getTopic()); - } +void ZEDWrapperNodelet::publishPose(ros::Time t) +{ + tf2::Transform base_pose; + base_pose.setIdentity(); + + if (mPublishMapTf) + { + base_pose = mMap2BaseTransf; + } + else if (mPublishTf) + { + base_pose = mOdom2BaseTransf; + } + + std_msgs::Header header; + header.stamp = t; + header.frame_id = mMapFrameId; + geometry_msgs::Pose pose; + + // conversion from Tranform to message + geometry_msgs::Transform base2frame = tf2::toMsg(base_pose); + + // Add all value in Pose message + pose.position.x = base2frame.translation.x; + pose.position.y = base2frame.translation.y; + pose.position.z = base2frame.translation.z; + pose.orientation.x = base2frame.rotation.x; + pose.orientation.y = base2frame.rotation.y; + pose.orientation.z = base2frame.rotation.z; + pose.orientation.w = base2frame.rotation.w; + + if (mPubPose.getNumSubscribers() > 0) + { + geometry_msgs::PoseStamped poseNoCov; + + poseNoCov.header = header; + poseNoCov.pose = pose; + + // Publish pose stamped message + mPubPose.publish(poseNoCov); + } + + if (mPubPoseCov.getNumSubscribers() > 0) + { + geometry_msgs::PoseWithCovarianceStampedPtr poseCovMsg = + boost::make_shared(); + + poseCovMsg->header = header; + poseCovMsg->pose.pose = pose; + + // Odometry pose covariance if available + for (size_t i = 0; i < poseCovMsg->pose.covariance.size(); i++) + { + poseCovMsg->pose.covariance[i] = static_cast(mLastZedPose.pose_covariance[i]); - mObjDetFilter.clear(); - if(mObjDetPeople) { - mObjDetFilter.push_back(sl::OBJECT_CLASS::PERSON); - } - if(mObjDetVehicles) { - mObjDetFilter.push_back(sl::OBJECT_CLASS::VEHICLE); + if (mTwoDMode) + { + if (i == 14 || i == 21 || i == 28) + { + poseCovMsg->pose.covariance[i] = 1e-9; // Very low covariance if 2D mode + } + else if ((i >= 2 && i <= 4) || (i >= 8 && i <= 10) || (i >= 12 && i <= 13) || (i >= 15 && i <= 16) || + (i >= 18 && i <= 20) || (i == 22) || (i >= 24 && i <= 27)) + { + poseCovMsg->pose.covariance[i] = 0.0; + } + } } + // Publish pose with covariance stamped message + mPubPoseCov.publish(poseCovMsg); + } +} +void ZEDWrapperNodelet::publishStaticImuFrame() +{ + // Publish IMU TF as static TF + if (!mPublishImuTf) + { + return; + } + + if (mStaticImuFramePublished) + { + return; + } + + mStaticImuTransformStamped.header.stamp = ros::Time::now(); + mStaticImuTransformStamped.header.frame_id = mLeftCamFrameId; + mStaticImuTransformStamped.child_frame_id = mImuFrameId; + sl::Translation sl_tr = mSlCamImuTransf.getTranslation(); + mStaticImuTransformStamped.transform.translation.x = sl_tr.x; + mStaticImuTransformStamped.transform.translation.y = sl_tr.y; + mStaticImuTransformStamped.transform.translation.z = sl_tr.z; + sl::Orientation sl_or = mSlCamImuTransf.getOrientation(); + mStaticImuTransformStamped.transform.rotation.x = sl_or.ox; + mStaticImuTransformStamped.transform.rotation.y = sl_or.oy; + mStaticImuTransformStamped.transform.rotation.z = sl_or.oz; + mStaticImuTransformStamped.transform.rotation.w = sl_or.ow; + + // Publish transformation + mStaticTransformImuBroadcaster.sendTransform(mStaticImuTransformStamped); + + NODELET_INFO_STREAM("Published static transform '" << mImuFrameId << "' -> '" << mLeftCamFrameId << "'"); + + mStaticImuFramePublished = true; +} - mObjDetRunning = true; - return false; +void ZEDWrapperNodelet::publishOdomFrame(tf2::Transform odomTransf, ros::Time t) +{ + if (!mSensor2BaseTransfValid) + { + getSens2BaseTransform(); + } + + if (!mSensor2CameraTransfValid) + { + getSens2CameraTransform(); + } + + if (!mCamera2BaseTransfValid) + { + getCamera2BaseTransform(); + } + + geometry_msgs::TransformStamped transformStamped; + transformStamped.header.stamp = t; + transformStamped.header.frame_id = mOdometryFrameId; + transformStamped.child_frame_id = mBaseFrameId; + // conversion from Tranform to message + transformStamped.transform = tf2::toMsg(odomTransf); + // Publish transformation + mTransformOdomBroadcaster.sendTransform(transformStamped); + + // NODELET_INFO_STREAM( "Published ODOM TF with TS: " << t ); } -void ZEDWrapperNodelet::stop_obj_detect() { - if (mObjDetRunning) { - NODELET_INFO_STREAM("*** Stopping Object Detection ***"); - mObjDetRunning = false; - mObjDetEnabled = false; - mZed.disableObjectDetection(); - } +void ZEDWrapperNodelet::publishPoseFrame(tf2::Transform baseTransform, ros::Time t) +{ + if (!mSensor2BaseTransfValid) + { + getSens2BaseTransform(); + } + + if (!mSensor2CameraTransfValid) + { + getSens2CameraTransform(); + } + + if (!mCamera2BaseTransfValid) + { + getCamera2BaseTransform(); + } + + geometry_msgs::TransformStamped transformStamped; + transformStamped.header.stamp = t; + transformStamped.header.frame_id = mMapFrameId; + transformStamped.child_frame_id = mOdometryFrameId; + // conversion from Tranform to message + transformStamped.transform = tf2::toMsg(baseTransform); + // Publish transformation + mTransformPoseBroadcaster.sendTransform(transformStamped); + + // NODELET_INFO_STREAM( "Published POSE TF with TS: " << t ); +} + +void ZEDWrapperNodelet::publishImage(sensor_msgs::ImagePtr imgMsgPtr, sl::Mat img, + image_transport::CameraPublisher& pubImg, sensor_msgs::CameraInfoPtr camInfoMsg, + string imgFrameId, ros::Time t) +{ + camInfoMsg->header.stamp = t; + sl_tools::imageToROSmsg(imgMsgPtr, img, imgFrameId, t); + pubImg.publish(imgMsgPtr, camInfoMsg); } -void ZEDWrapperNodelet::start_pos_tracking() { - NODELET_INFO_STREAM("*** Starting Positional Tracking ***"); +void ZEDWrapperNodelet::publishDepth(sensor_msgs::ImagePtr imgMsgPtr, sl::Mat depth, ros::Time t) +{ + mDepthCamInfoMsg->header.stamp = t; - NODELET_INFO(" * Waiting for valid static transformations..."); + // NODELET_DEBUG_STREAM("mOpenniDepthMode: " << mOpenniDepthMode); - bool transformOk = false; - double elapsed = 0.0; + if (!mOpenniDepthMode) + { + // NODELET_INFO("Using float32"); + sl_tools::imageToROSmsg(imgMsgPtr, depth, mDepthOptFrameId, t); + mPubDepth.publish(imgMsgPtr, mDepthCamInfoMsg); - auto start = std::chrono::high_resolution_clock::now(); + return; + } - do { - transformOk = set_pose(mInitialBasePose[0], mInitialBasePose[1], mInitialBasePose[2], - mInitialBasePose[3], mInitialBasePose[4], mInitialBasePose[5]); +#if (ZED_SDK_MAJOR_VERSION == 3 && ZED_SDK_MINOR_VERSION < 4) + // OPENNI CONVERSION (meter -> millimeters - float32 -> uint16) + if (!imgMsgPtr) + { + imgMsgPtr = boost::make_shared(); + } - elapsed = std::chrono::duration_cast(std::chrono::high_resolution_clock::now() - - start).count(); + imgMsgPtr->header.stamp = t; + imgMsgPtr->header.frame_id = mDepthOptFrameId; + imgMsgPtr->height = depth.getHeight(); + imgMsgPtr->width = depth.getWidth(); - std::this_thread::sleep_for(std::chrono::milliseconds(100)); + int num = 1; // for endianness detection + imgMsgPtr->is_bigendian = !(*(char*)&num == 1); - if (elapsed > 10000) { - NODELET_WARN(" !!! Failed to get static transforms. Is the 'ROBOT STATE PUBLISHER' node correctly working? "); - break; - } + imgMsgPtr->step = imgMsgPtr->width * sizeof(uint16_t); + imgMsgPtr->encoding = sensor_msgs::image_encodings::MONO16; - } while (transformOk == false); + size_t size = imgMsgPtr->step * imgMsgPtr->height; + imgMsgPtr->data.resize(size); - if (transformOk) { - NODELET_DEBUG("Time required to get valid static transforms: %g sec", elapsed / 1000.); - } + uint16_t* data = (uint16_t*)(&imgMsgPtr->data[0]); - NODELET_INFO("Initial ZED left camera pose (ZED pos. tracking): "); - NODELET_INFO(" * T: [%g,%g,%g]", - mInitialPoseSl.getTranslation().x, mInitialPoseSl.getTranslation().y, mInitialPoseSl.getTranslation().z); - NODELET_INFO(" * Q: [%g,%g,%g,%g]", - mInitialPoseSl.getOrientation().ox, mInitialPoseSl.getOrientation().oy, - mInitialPoseSl.getOrientation().oz, mInitialPoseSl.getOrientation().ow); + int dataSize = imgMsgPtr->width * imgMsgPtr->height; + sl::float1* depthDataPtr = depth.getPtr(); - if (mAreaMemDbPath != "" && !sl_tools::file_exist(mAreaMemDbPath)) { - mAreaMemDbPath = ""; - NODELET_WARN("area_memory_db_path path doesn't exist or is unreachable."); - } + for (int i = 0; i < dataSize; i++) + { + *(data++) = static_cast(std::round(*(depthDataPtr++) * 1000)); // in mm, rounded + } + mPubDepth.publish(imgMsgPtr, mDepthCamInfoMsg); +#else + // NODELET_INFO("Using depth16"); + sl_tools::imageToROSmsg(imgMsgPtr, depth, mDepthOptFrameId, t); + mPubDepth.publish(imgMsgPtr, mDepthCamInfoMsg); +#endif +} - // Tracking parameters - sl::PositionalTrackingParameters trackParams; +void ZEDWrapperNodelet::publishDisparity(sl::Mat disparity, ros::Time t) +{ + sl::CameraInformation zedParam = mZed.getCameraInformation(mMatResolDepth); - trackParams.area_file_path = mAreaMemDbPath.c_str(); + sensor_msgs::ImagePtr disparityImgMsg = boost::make_shared(); + stereo_msgs::DisparityImagePtr disparityMsg = boost::make_shared(); - mPoseSmoothing = false; // Always false. Pose Smoothing is to be enabled only for VR/AR applications - trackParams.enable_pose_smoothing = mPoseSmoothing; + sl_tools::imageToROSmsg(disparityImgMsg, disparity, mDisparityFrameId, t); - trackParams.enable_area_memory = mAreaMemory; - trackParams.enable_imu_fusion = mImuFusion; - trackParams.initial_world_transform = mInitialPoseSl; + disparityMsg->image = *disparityImgMsg; + disparityMsg->header = disparityMsg->image.header; - trackParams.set_floor_as_origin = mFloorAlignment; +#if ZED_SDK_MAJOR_VERSION == 3 && ZED_SDK_MINOR_VERSION < 1 + disparityMsg->f = zedParam.calibration_parameters.left_cam.fx; + disparityMsg->T = zedParam.calibration_parameters.T.x; +#else + disparityMsg->f = zedParam.camera_configuration.calibration_parameters.left_cam.fx; + disparityMsg->T = zedParam.camera_configuration.calibration_parameters.getCameraBaseline(); +#endif - sl::ERROR_CODE err = mZed.enablePositionalTracking(trackParams); + if (disparityMsg->T > 0) + { + disparityMsg->T *= -1.0f; + } - if (err == sl::ERROR_CODE::SUCCESS) { - mTrackingActivated = true; - } else { - mTrackingActivated = false; + disparityMsg->min_disparity = disparityMsg->f * disparityMsg->T / mZed.getInitParameters().depth_minimum_distance; + disparityMsg->max_disparity = disparityMsg->f * disparityMsg->T / mZed.getInitParameters().depth_maximum_distance; - NODELET_WARN("Tracking not activated: %s", sl::toString(err).c_str()); - } + mPubDisparity.publish(disparityMsg); } -void ZEDWrapperNodelet::publishOdom(tf2::Transform odom2baseTransf, sl::Pose& slPose, ros::Time t) { - nav_msgs::OdometryPtr odomMsg = boost::make_shared(); - - odomMsg->header.stamp = t; - odomMsg->header.frame_id = mOdometryFrameId; // frame - odomMsg->child_frame_id = mBaseFrameId; // camera_frame - // conversion from Tranform to message - geometry_msgs::Transform base2odom = tf2::toMsg(odom2baseTransf); - // Add all value in odometry message - odomMsg->pose.pose.position.x = base2odom.translation.x; - odomMsg->pose.pose.position.y = base2odom.translation.y; - odomMsg->pose.pose.position.z = base2odom.translation.z; - odomMsg->pose.pose.orientation.x = base2odom.rotation.x; - odomMsg->pose.pose.orientation.y = base2odom.rotation.y; - odomMsg->pose.pose.orientation.z = base2odom.rotation.z; - odomMsg->pose.pose.orientation.w = base2odom.rotation.w; - - // Odometry pose covariance - - for (size_t i = 0; i < odomMsg->pose.covariance.size(); i++) { - odomMsg->pose.covariance[i] = static_cast(slPose.pose_covariance[i]); - - if (mTwoDMode) { - if (i == 14 || i == 21 || i == 28) { - odomMsg->pose.covariance[i] = 1e-9; // Very low covariance if 2D mode - } else if ((i >= 2 && i <= 4) || - (i >= 8 && i <= 10) || - (i >= 12 && i <= 13) || - (i >= 15 && i <= 16) || - (i >= 18 && i <= 20) || - (i == 22) || - (i >= 24 && i <= 27)) { - odomMsg->pose.covariance[i] = 0.0; - } +void ZEDWrapperNodelet::pointcloud_thread_func() +{ + std::unique_lock lock(mPcMutex); + + while (!mStopNode) + { + while (!mPcDataReady) + { // loop to avoid spurious wakeups + if (mPcDataReadyCondVar.wait_for(lock, std::chrono::milliseconds(500)) == std::cv_status::timeout) + { + // Check thread stopping + if (mStopNode) + { + return; + } + else + { + continue; } + } } - // Publish odometry message - mPubOdom.publish(odomMsg); -} + // ----> Check publishing frequency + double pc_period_msec = 1000.0 / mPointCloudFreq; -void ZEDWrapperNodelet::publishPose(ros::Time t) { - tf2::Transform base_pose; - base_pose.setIdentity(); + static std::chrono::steady_clock::time_point last_time = std::chrono::steady_clock::now(); + std::chrono::steady_clock::time_point now = std::chrono::steady_clock::now(); - if (mPublishMapTf) { - base_pose = mMap2BaseTransf; - } else if (mPublishTf) { - base_pose = mOdom2BaseTransf; - } + double elapsed_msec = std::chrono::duration_cast(now - last_time).count(); - std_msgs::Header header; - header.stamp = t; - header.frame_id = mMapFrameId; - geometry_msgs::Pose pose; + if (elapsed_msec < pc_period_msec) + { + std::this_thread::sleep_for(std::chrono::milliseconds(static_cast(pc_period_msec - elapsed_msec))); + } + // <---- Check publishing frequency - // conversion from Tranform to message - geometry_msgs::Transform base2frame = tf2::toMsg(base_pose); + last_time = std::chrono::steady_clock::now(); + publishPointCloud(); - // Add all value in Pose message - pose.position.x = base2frame.translation.x; - pose.position.y = base2frame.translation.y; - pose.position.z = base2frame.translation.z; - pose.orientation.x = base2frame.rotation.x; - pose.orientation.y = base2frame.rotation.y; - pose.orientation.z = base2frame.rotation.z; - pose.orientation.w = base2frame.rotation.w; + mPcDataReady = false; + } - if (mPubPose.getNumSubscribers() > 0) { + NODELET_DEBUG("Pointcloud thread finished"); +} - geometry_msgs::PoseStamped poseNoCov; +void ZEDWrapperNodelet::publishPointCloud() +{ + sensor_msgs::PointCloud2Ptr pointcloudMsg = boost::make_shared(); - poseNoCov.header = header; - poseNoCov.pose = pose; + // Publish freq calculation + static std::chrono::steady_clock::time_point last_time = std::chrono::steady_clock::now(); + std::chrono::steady_clock::time_point now = std::chrono::steady_clock::now(); - // Publish pose stamped message - mPubPose.publish(poseNoCov); - } + double elapsed_usec = std::chrono::duration_cast(now - last_time).count(); + last_time = now; - if (mPubPoseCov.getNumSubscribers() > 0) { - geometry_msgs::PoseWithCovarianceStampedPtr poseCovMsg = boost::make_shared(); - - poseCovMsg->header = header; - poseCovMsg->pose.pose = pose; - - // Odometry pose covariance if available - for (size_t i = 0; i < poseCovMsg->pose.covariance.size(); i++) { - poseCovMsg->pose.covariance[i] = static_cast(mLastZedPose.pose_covariance[i]); - - if (mTwoDMode) { - if (i == 14 || i == 21 || i == 28) { - poseCovMsg->pose.covariance[i] = 1e-9; // Very low covariance if 2D mode - } else if ((i >= 2 && i <= 4) || - (i >= 8 && i <= 10) || - (i >= 12 && i <= 13) || - (i >= 15 && i <= 16) || - (i >= 18 && i <= 20) || - (i == 22) || - (i >= 24 && i <= 27)) { - poseCovMsg->pose.covariance[i] = 0.0; - } - } - } + mPcPeriodMean_usec->addValue(elapsed_usec); - // Publish pose with covariance stamped message - mPubPoseCov.publish(poseCovMsg); - } + // Initialize Point Cloud message + // https://github.com/ros/common_msgs/blob/jade-devel/sensor_msgs/include/sensor_msgs/point_cloud2_iterator.h -} + int ptsCount = mMatResolDepth.width * mMatResolDepth.height; -void ZEDWrapperNodelet::publishStaticImuFrame() { - // Publish IMU TF as static TF - if( !mPublishImuTf ) { - return; - } + pointcloudMsg->header.stamp = mPointCloudTime; - if(mStaticImuFramePublished) { - return; - } + if (pointcloudMsg->width != mMatResolDepth.width || pointcloudMsg->height != mMatResolDepth.height) + { + pointcloudMsg->header.frame_id = mPointCloudFrameId; // Set the header values of the ROS message - mStaticImuTransformStamped.header.stamp = ros::Time::now(); - mStaticImuTransformStamped.header.frame_id = mLeftCamFrameId; - mStaticImuTransformStamped.child_frame_id = mImuFrameId; - sl::Translation sl_tr = mSlCamImuTransf.getTranslation(); - mStaticImuTransformStamped.transform.translation.x = sl_tr.x; - mStaticImuTransformStamped.transform.translation.y = sl_tr.y; - mStaticImuTransformStamped.transform.translation.z = sl_tr.z; - sl::Orientation sl_or = mSlCamImuTransf.getOrientation(); - mStaticImuTransformStamped.transform.rotation.x = sl_or.ox; - mStaticImuTransformStamped.transform.rotation.y = sl_or.oy; - mStaticImuTransformStamped.transform.rotation.z = sl_or.oz; - mStaticImuTransformStamped.transform.rotation.w = sl_or.ow; - - // Publish transformation - mStaticTransformImuBroadcaster.sendTransform(mStaticImuTransformStamped); - - NODELET_INFO_STREAM("Published static transform '" << mImuFrameId << "' -> '" << mLeftCamFrameId << "'" ); - - mStaticImuFramePublished = true; -} + pointcloudMsg->is_bigendian = false; + pointcloudMsg->is_dense = false; -void ZEDWrapperNodelet::publishOdomFrame(tf2::Transform odomTransf, ros::Time t) { - if (!mSensor2BaseTransfValid) { - getSens2BaseTransform(); - } + pointcloudMsg->width = mMatResolDepth.width; + pointcloudMsg->height = mMatResolDepth.height; - if (!mSensor2CameraTransfValid) { - getSens2CameraTransform(); - } + sensor_msgs::PointCloud2Modifier modifier(*pointcloudMsg); + modifier.setPointCloud2Fields(4, "x", 1, sensor_msgs::PointField::FLOAT32, "y", 1, sensor_msgs::PointField::FLOAT32, + "z", 1, sensor_msgs::PointField::FLOAT32, "rgb", 1, sensor_msgs::PointField::FLOAT32); + } - if (!mCamera2BaseTransfValid) { - getCamera2BaseTransform(); - } + // Data copy + sl::Vector4* cpu_cloud = mCloud.getPtr(); + float* ptCloudPtr = (float*)(&pointcloudMsg->data[0]); - geometry_msgs::TransformStamped transformStamped; - transformStamped.header.stamp = t; - transformStamped.header.frame_id = mOdometryFrameId; - transformStamped.child_frame_id = mBaseFrameId; - // conversion from Tranform to message - transformStamped.transform = tf2::toMsg(odomTransf); - // Publish transformation - mTransformOdomBroadcaster.sendTransform(transformStamped); + // We can do a direct memcpy since data organization is the same + memcpy(ptCloudPtr, (float*)cpu_cloud, 4 * ptsCount * sizeof(float)); - //NODELET_INFO_STREAM( "Published ODOM TF with TS: " << t ); + // Pointcloud publishing + mPubCloud.publish(pointcloudMsg); } -void ZEDWrapperNodelet::publishPoseFrame(tf2::Transform baseTransform, ros::Time t) { - if (!mSensor2BaseTransfValid) { - getSens2BaseTransform(); - } +void ZEDWrapperNodelet::callback_pubFusedPointCloud(const ros::TimerEvent& e) +{ + sensor_msgs::PointCloud2Ptr pointcloudFusedMsg = boost::make_shared(); - if (!mSensor2CameraTransfValid) { - getSens2CameraTransform(); - } + uint32_t fusedCloudSubnumber = mPubFusedCloud.getNumSubscribers(); - if (!mCamera2BaseTransfValid) { - getCamera2BaseTransform(); - } + if (fusedCloudSubnumber == 0) + { + return; + } - geometry_msgs::TransformStamped transformStamped; - transformStamped.header.stamp = t; - transformStamped.header.frame_id = mMapFrameId; - transformStamped.child_frame_id = mOdometryFrameId; - // conversion from Tranform to message - transformStamped.transform = tf2::toMsg(baseTransform); - // Publish transformation - mTransformPoseBroadcaster.sendTransform(transformStamped); + std::lock_guard lock(mCloseZedMutex); - //NODELET_INFO_STREAM( "Published POSE TF with TS: " << t ); -} + if (!mZed.isOpened()) + { + return; + } -void ZEDWrapperNodelet::publishImage(sensor_msgs::ImagePtr imgMsgPtr, sl::Mat img, - image_transport::CameraPublisher& pubImg, sensor_msgs::CameraInfoPtr camInfoMsg, - string imgFrameId, ros::Time t) { - camInfoMsg->header.stamp = t; - sl_tools::imageToROSmsg( imgMsgPtr, img, imgFrameId, t); - pubImg.publish(imgMsgPtr, camInfoMsg); -} - -void ZEDWrapperNodelet::publishDepth(sensor_msgs::ImagePtr imgMsgPtr, sl::Mat depth, ros::Time t) { - - mDepthCamInfoMsg->header.stamp = t; - - //NODELET_DEBUG_STREAM("mOpenniDepthMode: " << mOpenniDepthMode); - - if (!mOpenniDepthMode) { - sl_tools::imageToROSmsg(imgMsgPtr, depth, mDepthOptFrameId, t); - mPubDepth.publish(imgMsgPtr, mDepthCamInfoMsg); - - return; - } - - // OPENNI CONVERSION (meter -> millimeters - float32 -> uint16) - if(!imgMsgPtr) { - imgMsgPtr = boost::make_shared(); - } - - imgMsgPtr->header.stamp = t; - imgMsgPtr->header.frame_id = mDepthOptFrameId; - imgMsgPtr->height = depth.getHeight(); - imgMsgPtr->width = depth.getWidth(); - - int num = 1; // for endianness detection - imgMsgPtr->is_bigendian = !(*(char*)&num == 1); - - imgMsgPtr->step = imgMsgPtr->width * sizeof(uint16_t); - imgMsgPtr->encoding = sensor_msgs::image_encodings::MONO16; - - size_t size = imgMsgPtr->step * imgMsgPtr->height; - imgMsgPtr->data.resize(size); - - uint16_t* data = (uint16_t*)(&imgMsgPtr->data[0]); - - int dataSize = imgMsgPtr->width * imgMsgPtr->height; - sl::float1* depthDataPtr = depth.getPtr(); - - for (int i = 0; i < dataSize; i++) { - *(data++) = static_cast(std::round(*(depthDataPtr++) * 1000)); // in mm, rounded - } - - mPubDepth.publish(imgMsgPtr, mDepthCamInfoMsg); -} - -void ZEDWrapperNodelet::publishDisparity(sl::Mat disparity, ros::Time t) { + // pointcloudFusedMsg->header.stamp = t; + mZed.requestSpatialMapAsync(); - sl::CameraInformation zedParam = mZed.getCameraInformation(mMatResolDepth); + while (mZed.getSpatialMapRequestStatusAsync() == sl::ERROR_CODE::FAILURE) + { + // Mesh is still generating + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } - sensor_msgs::ImagePtr disparityImgMsg = boost::make_shared(); - stereo_msgs::DisparityImagePtr disparityMsg = boost::make_shared(); + sl::ERROR_CODE res = mZed.retrieveSpatialMapAsync(mFusedPC); - sl_tools::imageToROSmsg(disparityImgMsg, disparity, mDisparityFrameId, t); + if (res != sl::ERROR_CODE::SUCCESS) + { + NODELET_WARN_STREAM("Fused point cloud not extracted: " << sl::toString(res).c_str()); + return; + } - disparityMsg->image = *disparityImgMsg; - disparityMsg->header = disparityMsg->image.header; - -#if ZED_SDK_MAJOR_VERSION==3 && ZED_SDK_MINOR_VERSION<1 - disparityMsg->f = zedParam.calibration_parameters.left_cam.fx; - disparityMsg->T = zedParam.calibration_parameters.T.x; -#else - disparityMsg->f = zedParam.camera_configuration.calibration_parameters.left_cam.fx; - disparityMsg->T = zedParam.camera_configuration.calibration_parameters.getCameraBaseline(); -#endif - - if (disparityMsg->T > 0) { - disparityMsg->T *= -1.0f; - } - - disparityMsg->min_disparity = disparityMsg->f * disparityMsg->T / mZed.getInitParameters().depth_minimum_distance; - disparityMsg->max_disparity = disparityMsg->f * disparityMsg->T / mZed.getInitParameters().depth_maximum_distance; - - mPubDisparity.publish(disparityMsg); -} - -void ZEDWrapperNodelet::pointcloud_thread_func() { - std::unique_lock lock(mPcMutex); - - while (!mStopNode) { - while (!mPcDataReady) { // loop to avoid spurious wakeups - if (mPcDataReadyCondVar.wait_for(lock, std::chrono::milliseconds(500)) == std::cv_status::timeout) { - // Check thread stopping - if (mStopNode) { - return; - } else { - continue; - } - } - } - - // ----> Check publishing frequency - double pc_period_msec = 1000.0 / mPointCloudFreq; - - static std::chrono::steady_clock::time_point last_time = std::chrono::steady_clock::now(); - std::chrono::steady_clock::time_point now = std::chrono::steady_clock::now(); - - double elapsed_msec = std::chrono::duration_cast(now - last_time).count(); - - if (elapsed_msec < pc_period_msec) { - std::this_thread::sleep_for(std::chrono::milliseconds(static_cast(pc_period_msec - elapsed_msec))); - } - // <---- Check publishing frequency - - last_time = std::chrono::steady_clock::now(); - publishPointCloud(); - - mPcDataReady = false; - } - - NODELET_DEBUG("Pointcloud thread finished"); -} - -void ZEDWrapperNodelet::publishPointCloud() { - sensor_msgs::PointCloud2Ptr pointcloudMsg = boost::make_shared(); - - // Publish freq calculation - static std::chrono::steady_clock::time_point last_time = std::chrono::steady_clock::now(); - std::chrono::steady_clock::time_point now = std::chrono::steady_clock::now(); - - double elapsed_usec = std::chrono::duration_cast(now - last_time).count(); - last_time = now; - - mPcPeriodMean_usec->addValue(elapsed_usec); + size_t ptsCount = mFusedPC.getNumberOfPoints(); + bool resized = false; + if (pointcloudFusedMsg->width != ptsCount || pointcloudFusedMsg->height != 1) + { // Initialize Point Cloud message // https://github.com/ros/common_msgs/blob/jade-devel/sensor_msgs/include/sensor_msgs/point_cloud2_iterator.h + pointcloudFusedMsg->header.frame_id = mMapFrameId; // Set the header values of the ROS message + pointcloudFusedMsg->is_bigendian = false; + pointcloudFusedMsg->is_dense = false; + pointcloudFusedMsg->width = ptsCount; + pointcloudFusedMsg->height = 1; - int ptsCount = mMatResolDepth.width * mMatResolDepth.height; + sensor_msgs::PointCloud2Modifier modifier(*pointcloudFusedMsg); + modifier.setPointCloud2Fields(4, "x", 1, sensor_msgs::PointField::FLOAT32, "y", 1, sensor_msgs::PointField::FLOAT32, + "z", 1, sensor_msgs::PointField::FLOAT32, "rgb", 1, sensor_msgs::PointField::FLOAT32); - pointcloudMsg->header.stamp = mPointCloudTime; + resized = true; + } - if (pointcloudMsg->width != mMatResolDepth.width || pointcloudMsg->height != mMatResolDepth.height) { - pointcloudMsg->header.frame_id = mPointCloudFrameId; // Set the header values of the ROS message + std::chrono::steady_clock::time_point start_time = std::chrono::steady_clock::now(); - pointcloudMsg->is_bigendian = false; - pointcloudMsg->is_dense = false; + // NODELET_INFO_STREAM("Chunks: " << mFusedPC.chunks.size()); - pointcloudMsg->width = mMatResolDepth.width; - pointcloudMsg->height = mMatResolDepth.height; - - sensor_msgs::PointCloud2Modifier modifier(*pointcloudMsg); - modifier.setPointCloud2Fields(4, - "x", 1, sensor_msgs::PointField::FLOAT32, - "y", 1, sensor_msgs::PointField::FLOAT32, - "z", 1, sensor_msgs::PointField::FLOAT32, - "rgb", 1, sensor_msgs::PointField::FLOAT32); - } - - // Data copy - sl::Vector4* cpu_cloud = mCloud.getPtr(); - float* ptCloudPtr = (float*)(&pointcloudMsg->data[0]); - - // We can do a direct memcpy since data organization is the same - memcpy(ptCloudPtr, (float*)cpu_cloud, 4 * ptsCount * sizeof(float)); - - // Pointcloud publishing - mPubCloud.publish(pointcloudMsg); -} - -void ZEDWrapperNodelet::callback_pubFusedPointCloud(const ros::TimerEvent& e) { - - sensor_msgs::PointCloud2Ptr pointcloudFusedMsg = boost::make_shared(); - - uint32_t fusedCloudSubnumber = mPubFusedCloud.getNumSubscribers(); - - if (fusedCloudSubnumber == 0) { - return; - } + int index = 0; + float* ptCloudPtr = (float*)(&pointcloudFusedMsg->data[0]); + int updated = 0; - std::lock_guard lock(mCloseZedMutex); - - if (!mZed.isOpened()) { - return; - } + for (int c = 0; c < mFusedPC.chunks.size(); c++) + { + if (mFusedPC.chunks[c].has_been_updated || resized) + { + updated++; - //pointcloudFusedMsg->header.stamp = t; - mZed.requestSpatialMapAsync(); + size_t chunkSize = mFusedPC.chunks[c].vertices.size(); - while (mZed.getSpatialMapRequestStatusAsync() == sl::ERROR_CODE::FAILURE) { - //Mesh is still generating - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } + if (chunkSize > 0) + { + float* cloud_pts = (float*)(mFusedPC.chunks[c].vertices.data()); - sl::ERROR_CODE res = mZed.retrieveSpatialMapAsync(mFusedPC); + memcpy(ptCloudPtr, cloud_pts, 4 * chunkSize * sizeof(float)); - if (res != sl::ERROR_CODE::SUCCESS) { - NODELET_WARN_STREAM("Fused point cloud not extracted: " << sl::toString(res).c_str()); - return; - } + ptCloudPtr += 4 * chunkSize; - size_t ptsCount = mFusedPC.getNumberOfPoints(); - bool resized = false; - - if (pointcloudFusedMsg->width != ptsCount || pointcloudFusedMsg->height != 1) { - // Initialize Point Cloud message - // https://github.com/ros/common_msgs/blob/jade-devel/sensor_msgs/include/sensor_msgs/point_cloud2_iterator.h - pointcloudFusedMsg->header.frame_id = mMapFrameId; // Set the header values of the ROS message - pointcloudFusedMsg->is_bigendian = false; - pointcloudFusedMsg->is_dense = false; - pointcloudFusedMsg->width = ptsCount; - pointcloudFusedMsg->height = 1; - - sensor_msgs::PointCloud2Modifier modifier(*pointcloudFusedMsg); - modifier.setPointCloud2Fields(4, - "x", 1, sensor_msgs::PointField::FLOAT32, - "y", 1, sensor_msgs::PointField::FLOAT32, - "z", 1, sensor_msgs::PointField::FLOAT32, - "rgb", 1, sensor_msgs::PointField::FLOAT32); - - resized = true; + pointcloudFusedMsg->header.stamp = sl_tools::slTime2Ros(mFusedPC.chunks[c].timestamp); + } } - - 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; - - for (int c = 0; c < mFusedPC.chunks.size(); c++) { - if (mFusedPC.chunks[c].has_been_updated || resized) { - - updated++; - - size_t chunkSize = mFusedPC.chunks[c].vertices.size(); - - if (chunkSize > 0) { - - float* cloud_pts = (float*)(mFusedPC.chunks[c].vertices.data()); - - memcpy(ptCloudPtr, cloud_pts, 4 * chunkSize * sizeof(float)); - - ptCloudPtr += 4 * chunkSize; - - pointcloudFusedMsg->header.stamp = sl_tools::slTime2Ros(mFusedPC.chunks[c].timestamp); - } - - } else { - index += mFusedPC.chunks[c].vertices.size(); - } + 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); + // NODELET_INFO_STREAM("Updated: " << updated); + // double elapsed_usec = std::chrono::duration_cast(end_time - start_time).count(); - //double elapsed_usec = std::chrono::duration_cast(end_time - start_time).count(); + // NODELET_INFO_STREAM("Data copy: " << elapsed_usec << " usec [" << ptsCount << "] - " << (static_cast + // (ptsCount) / elapsed_usec) << " pts/usec"); - - // NODELET_INFO_STREAM("Data copy: " << elapsed_usec << " usec [" << ptsCount << "] - " << (static_cast - // (ptsCount) / elapsed_usec) << " pts/usec"); - - // Pointcloud publishing - mPubFusedCloud.publish(pointcloudFusedMsg); + // Pointcloud publishing + mPubFusedCloud.publish(pointcloudFusedMsg); } -void ZEDWrapperNodelet::publishCamInfo(sensor_msgs::CameraInfoPtr camInfoMsg, - ros::Publisher pubCamInfo, ros::Time t) { - static int seq = 0; - camInfoMsg->header.stamp = t; - camInfoMsg->header.seq = seq; - pubCamInfo.publish(camInfoMsg); - seq++; +void ZEDWrapperNodelet::publishCamInfo(sensor_msgs::CameraInfoPtr camInfoMsg, ros::Publisher pubCamInfo, ros::Time t) +{ + static int seq = 0; + camInfoMsg->header.stamp = t; + camInfoMsg->header.seq = seq; + pubCamInfo.publish(camInfoMsg); + seq++; } void ZEDWrapperNodelet::fillCamInfo(sl::Camera& zed, sensor_msgs::CameraInfoPtr leftCamInfoMsg, - sensor_msgs::CameraInfoPtr rightCamInfoMsg, string leftFrameId, - string rightFrameId, bool rawParam /*= false*/) { - sl::CalibrationParameters zedParam; - -#if ZED_SDK_MAJOR_VERSION==3 && ZED_SDK_MINOR_VERSION<1 - if (rawParam) { - zedParam = zed.getCameraInformation(mMatResolVideo).calibration_parameters_raw; // ok - } else { - zedParam = zed.getCameraInformation(mMatResolVideo).calibration_parameters; // ok - } + sensor_msgs::CameraInfoPtr rightCamInfoMsg, string leftFrameId, string rightFrameId, + bool rawParam /*= false*/) +{ + sl::CalibrationParameters zedParam; + +#if ZED_SDK_MAJOR_VERSION == 3 && ZED_SDK_MINOR_VERSION < 1 + if (rawParam) + { + zedParam = zed.getCameraInformation(mMatResolVideo).calibration_parameters_raw; // ok + } + else + { + zedParam = zed.getCameraInformation(mMatResolVideo).calibration_parameters; // ok + } #else - if (rawParam) { - zedParam = zed.getCameraInformation(mMatResolVideo).camera_configuration.calibration_parameters_raw; - } else { - zedParam = zed.getCameraInformation(mMatResolVideo).camera_configuration.calibration_parameters; - } + if (rawParam) + { + zedParam = zed.getCameraInformation(mMatResolVideo).camera_configuration.calibration_parameters_raw; + } + else + { + zedParam = zed.getCameraInformation(mMatResolVideo).camera_configuration.calibration_parameters; + } #endif - float baseline = zedParam.getCameraBaseline(); - leftCamInfoMsg->distortion_model = - sensor_msgs::distortion_models::PLUMB_BOB; - rightCamInfoMsg->distortion_model = - sensor_msgs::distortion_models::PLUMB_BOB; - leftCamInfoMsg->D.resize(5); - rightCamInfoMsg->D.resize(5); - leftCamInfoMsg->D[0] = zedParam.left_cam.disto[0]; // k1 - leftCamInfoMsg->D[1] = zedParam.left_cam.disto[1]; // k2 - leftCamInfoMsg->D[2] = zedParam.left_cam.disto[4]; // k3 - leftCamInfoMsg->D[3] = zedParam.left_cam.disto[2]; // p1 - leftCamInfoMsg->D[4] = zedParam.left_cam.disto[3]; // p2 - rightCamInfoMsg->D[0] = zedParam.right_cam.disto[0]; // k1 - rightCamInfoMsg->D[1] = zedParam.right_cam.disto[1]; // k2 - rightCamInfoMsg->D[2] = zedParam.right_cam.disto[4]; // k3 - rightCamInfoMsg->D[3] = zedParam.right_cam.disto[2]; // p1 - rightCamInfoMsg->D[4] = zedParam.right_cam.disto[3]; // p2 - leftCamInfoMsg->K.fill(0.0); - rightCamInfoMsg->K.fill(0.0); - leftCamInfoMsg->K[0] = static_cast(zedParam.left_cam.fx); - leftCamInfoMsg->K[2] = static_cast(zedParam.left_cam.cx); - leftCamInfoMsg->K[4] = static_cast(zedParam.left_cam.fy); - leftCamInfoMsg->K[5] = static_cast(zedParam.left_cam.cy); - leftCamInfoMsg->K[8] = 1.0; - rightCamInfoMsg->K[0] = static_cast(zedParam.right_cam.fx); - rightCamInfoMsg->K[2] = static_cast(zedParam.right_cam.cx); - rightCamInfoMsg->K[4] = static_cast(zedParam.right_cam.fy); - rightCamInfoMsg->K[5] = static_cast(zedParam.right_cam.cy); - rightCamInfoMsg->K[8] = 1.0; - leftCamInfoMsg->R.fill(0.0); - rightCamInfoMsg->R.fill(0.0); - - for (size_t i = 0; i < 3; i++) { - // identity - rightCamInfoMsg->R[i + i * 3] = 1; - leftCamInfoMsg->R[i + i * 3] = 1; - } - -#if ZED_SDK_MAJOR_VERSION==3 && ZED_SDK_MINOR_VERSION<1 - if (rawParam) { - std::vector R_ = sl_tools::convertRodrigues(zedParam.R); - float* p = R_.data(); - - for (int i = 0; i < 9; i++) { - rightCamInfoMsg->R[i] = p[i]; - } + float baseline = zedParam.getCameraBaseline(); + leftCamInfoMsg->distortion_model = sensor_msgs::distortion_models::PLUMB_BOB; + rightCamInfoMsg->distortion_model = sensor_msgs::distortion_models::PLUMB_BOB; + leftCamInfoMsg->D.resize(5); + rightCamInfoMsg->D.resize(5); + leftCamInfoMsg->D[0] = zedParam.left_cam.disto[0]; // k1 + leftCamInfoMsg->D[1] = zedParam.left_cam.disto[1]; // k2 + leftCamInfoMsg->D[2] = zedParam.left_cam.disto[4]; // k3 + leftCamInfoMsg->D[3] = zedParam.left_cam.disto[2]; // p1 + leftCamInfoMsg->D[4] = zedParam.left_cam.disto[3]; // p2 + rightCamInfoMsg->D[0] = zedParam.right_cam.disto[0]; // k1 + rightCamInfoMsg->D[1] = zedParam.right_cam.disto[1]; // k2 + rightCamInfoMsg->D[2] = zedParam.right_cam.disto[4]; // k3 + rightCamInfoMsg->D[3] = zedParam.right_cam.disto[2]; // p1 + rightCamInfoMsg->D[4] = zedParam.right_cam.disto[3]; // p2 + leftCamInfoMsg->K.fill(0.0); + rightCamInfoMsg->K.fill(0.0); + leftCamInfoMsg->K[0] = static_cast(zedParam.left_cam.fx); + leftCamInfoMsg->K[2] = static_cast(zedParam.left_cam.cx); + leftCamInfoMsg->K[4] = static_cast(zedParam.left_cam.fy); + leftCamInfoMsg->K[5] = static_cast(zedParam.left_cam.cy); + leftCamInfoMsg->K[8] = 1.0; + rightCamInfoMsg->K[0] = static_cast(zedParam.right_cam.fx); + rightCamInfoMsg->K[2] = static_cast(zedParam.right_cam.cx); + rightCamInfoMsg->K[4] = static_cast(zedParam.right_cam.fy); + rightCamInfoMsg->K[5] = static_cast(zedParam.right_cam.cy); + rightCamInfoMsg->K[8] = 1.0; + leftCamInfoMsg->R.fill(0.0); + rightCamInfoMsg->R.fill(0.0); + + for (size_t i = 0; i < 3; i++) + { + // identity + rightCamInfoMsg->R[i + i * 3] = 1; + leftCamInfoMsg->R[i + i * 3] = 1; + } + +#if ZED_SDK_MAJOR_VERSION == 3 && ZED_SDK_MINOR_VERSION < 1 + if (rawParam) + { + std::vector R_ = sl_tools::convertRodrigues(zedParam.R); + float* p = R_.data(); + + for (int i = 0; i < 9; i++) + { + rightCamInfoMsg->R[i] = p[i]; } + } #else - if (rawParam) { - if(mUseOldExtrinsic) { // Camera frame (Z forward, Y down, X right) - - std::vector R_ = sl_tools::convertRodrigues(zedParam.R); - float* p = R_.data(); - - for (int i = 0; i < 9; i++) { - rightCamInfoMsg->R[i] = p[i]; - } - } else { // ROS frame (X forward, Z up, Y left) - for (int i = 0; i < 9; i++) { - rightCamInfoMsg->R[i] = zedParam.stereo_transform.getRotationMatrix().r[i]; - } - } - } + if (rawParam) + { + if (mUseOldExtrinsic) + { // Camera frame (Z forward, Y down, X right) + + std::vector R_ = sl_tools::convertRodrigues(zedParam.R); + float* p = R_.data(); + + for (int i = 0; i < 9; i++) + { + rightCamInfoMsg->R[i] = p[i]; + } + } + else + { // ROS frame (X forward, Z up, Y left) + for (int i = 0; i < 9; i++) + { + rightCamInfoMsg->R[i] = zedParam.stereo_transform.getRotationMatrix().r[i]; + } + } + } #endif - leftCamInfoMsg->P.fill(0.0); - rightCamInfoMsg->P.fill(0.0); - leftCamInfoMsg->P[0] = static_cast(zedParam.left_cam.fx); - leftCamInfoMsg->P[2] = static_cast(zedParam.left_cam.cx); - leftCamInfoMsg->P[5] = static_cast(zedParam.left_cam.fy); - leftCamInfoMsg->P[6] = static_cast(zedParam.left_cam.cy); - leftCamInfoMsg->P[10] = 1.0; - // http://docs.ros.org/api/sensor_msgs/html/msg/CameraInfo.html - rightCamInfoMsg->P[3] = static_cast(-1 * zedParam.left_cam.fx * baseline); - rightCamInfoMsg->P[0] = static_cast(zedParam.right_cam.fx); - rightCamInfoMsg->P[2] = static_cast(zedParam.right_cam.cx); - rightCamInfoMsg->P[5] = static_cast(zedParam.right_cam.fy); - rightCamInfoMsg->P[6] = static_cast(zedParam.right_cam.cy); - rightCamInfoMsg->P[10] = 1.0; - leftCamInfoMsg->width = rightCamInfoMsg->width = static_cast(mMatResolVideo.width); - leftCamInfoMsg->height = rightCamInfoMsg->height = static_cast(mMatResolVideo.height); - leftCamInfoMsg->header.frame_id = leftFrameId; - rightCamInfoMsg->header.frame_id = rightFrameId; + leftCamInfoMsg->P.fill(0.0); + rightCamInfoMsg->P.fill(0.0); + leftCamInfoMsg->P[0] = static_cast(zedParam.left_cam.fx); + leftCamInfoMsg->P[2] = static_cast(zedParam.left_cam.cx); + leftCamInfoMsg->P[5] = static_cast(zedParam.left_cam.fy); + leftCamInfoMsg->P[6] = static_cast(zedParam.left_cam.cy); + leftCamInfoMsg->P[10] = 1.0; + // http://docs.ros.org/api/sensor_msgs/html/msg/CameraInfo.html + rightCamInfoMsg->P[3] = static_cast(-1 * zedParam.left_cam.fx * baseline); + rightCamInfoMsg->P[0] = static_cast(zedParam.right_cam.fx); + rightCamInfoMsg->P[2] = static_cast(zedParam.right_cam.cx); + rightCamInfoMsg->P[5] = static_cast(zedParam.right_cam.fy); + rightCamInfoMsg->P[6] = static_cast(zedParam.right_cam.cy); + rightCamInfoMsg->P[10] = 1.0; + leftCamInfoMsg->width = rightCamInfoMsg->width = static_cast(mMatResolVideo.width); + leftCamInfoMsg->height = rightCamInfoMsg->height = static_cast(mMatResolVideo.height); + leftCamInfoMsg->header.frame_id = leftFrameId; + rightCamInfoMsg->header.frame_id = rightFrameId; } -void ZEDWrapperNodelet::fillCamDepthInfo(sl::Camera& zed, sensor_msgs::CameraInfoPtr depth_info_msg, - string frame_id ) { - sl::CalibrationParameters zedParam; +void ZEDWrapperNodelet::fillCamDepthInfo(sl::Camera& zed, sensor_msgs::CameraInfoPtr depth_info_msg, string frame_id) +{ + sl::CalibrationParameters zedParam; -#if ZED_SDK_MAJOR_VERSION==3 && ZED_SDK_MINOR_VERSION<1 - zedParam = zed.getCameraInformation(mMatResolDepth).calibration_parameters; +#if ZED_SDK_MAJOR_VERSION == 3 && ZED_SDK_MINOR_VERSION < 1 + zedParam = zed.getCameraInformation(mMatResolDepth).calibration_parameters; #else - zedParam = zed.getCameraInformation(mMatResolDepth).camera_configuration.calibration_parameters; + zedParam = zed.getCameraInformation(mMatResolDepth).camera_configuration.calibration_parameters; #endif - 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 - depth_info_msg->D[1] = zedParam.left_cam.disto[1]; // k2 - depth_info_msg->D[2] = zedParam.left_cam.disto[4]; // k3 - depth_info_msg->D[3] = zedParam.left_cam.disto[2]; // p1 - depth_info_msg->D[4] = zedParam.left_cam.disto[3]; // p2 - depth_info_msg->K.fill(0.0); - depth_info_msg->K[0] = static_cast(zedParam.left_cam.fx); - depth_info_msg->K[2] = static_cast(zedParam.left_cam.cx); - depth_info_msg->K[4] = static_cast(zedParam.left_cam.fy); - depth_info_msg->K[5] = static_cast(zedParam.left_cam.cy); - depth_info_msg->K[8] = 1.0; - depth_info_msg->R.fill(0.0); - - for (size_t i = 0; i < 3; i++) { - // identity - depth_info_msg->R[i + i * 3] = 1; - } - - depth_info_msg->P.fill(0.0); - depth_info_msg->P[0] = static_cast(zedParam.left_cam.fx); - depth_info_msg->P[2] = static_cast(zedParam.left_cam.cx); - depth_info_msg->P[5] = static_cast(zedParam.left_cam.fy); - depth_info_msg->P[6] = static_cast(zedParam.left_cam.cy); - depth_info_msg->P[10] = 1.0; - // http://docs.ros.org/api/sensor_msgs/html/msg/CameraInfo.html - depth_info_msg->width = static_cast(mMatResolDepth.width); - depth_info_msg->height = static_cast(mMatResolDepth.height); - depth_info_msg->header.frame_id = frame_id; + 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 + depth_info_msg->D[1] = zedParam.left_cam.disto[1]; // k2 + depth_info_msg->D[2] = zedParam.left_cam.disto[4]; // k3 + depth_info_msg->D[3] = zedParam.left_cam.disto[2]; // p1 + depth_info_msg->D[4] = zedParam.left_cam.disto[3]; // p2 + depth_info_msg->K.fill(0.0); + depth_info_msg->K[0] = static_cast(zedParam.left_cam.fx); + depth_info_msg->K[2] = static_cast(zedParam.left_cam.cx); + depth_info_msg->K[4] = static_cast(zedParam.left_cam.fy); + depth_info_msg->K[5] = static_cast(zedParam.left_cam.cy); + depth_info_msg->K[8] = 1.0; + depth_info_msg->R.fill(0.0); + + for (size_t i = 0; i < 3; i++) + { + // identity + depth_info_msg->R[i + i * 3] = 1; + } + + depth_info_msg->P.fill(0.0); + depth_info_msg->P[0] = static_cast(zedParam.left_cam.fx); + depth_info_msg->P[2] = static_cast(zedParam.left_cam.cx); + depth_info_msg->P[5] = static_cast(zedParam.left_cam.fy); + depth_info_msg->P[6] = static_cast(zedParam.left_cam.cy); + depth_info_msg->P[10] = 1.0; + // http://docs.ros.org/api/sensor_msgs/html/msg/CameraInfo.html + depth_info_msg->width = static_cast(mMatResolDepth.width); + depth_info_msg->height = static_cast(mMatResolDepth.height); + depth_info_msg->header.frame_id = frame_id; } -void ZEDWrapperNodelet::updateDynamicReconfigure() { - //NODELET_DEBUG_STREAM( "updateDynamicReconfigure MUTEX LOCK"); - mDynParMutex.lock(); - zed_nodelets::ZedConfig config; - - config.auto_exposure_gain = mCamAutoExposure; - config.auto_whitebalance = mCamAutoWB; - config.brightness = mCamBrightness; - config.depth_confidence = mCamDepthConfidence; - config.depth_texture_conf = mCamDepthTextureConf; - config.contrast = mCamContrast; - config.exposure = mCamExposure; - config.gain = mCamGain; - config.hue = mCamHue; - config.saturation = mCamSaturation; - config.sharpness = mCamSharpness; - config.gamma = mCamGamma; - config.whitebalance_temperature = mCamWB/100; - config.point_cloud_freq = mPointCloudFreq; - config.pub_frame_rate = mVideoDepthFreq; - mDynParMutex.unlock(); - - mDynServerMutex.lock(); - mDynRecServer->updateConfig(config); - mDynServerMutex.unlock(); - - mUpdateDynParams = false; - - //NODELET_DEBUG_STREAM( "updateDynamicReconfigure MUTEX UNLOCK"); +void ZEDWrapperNodelet::updateDynamicReconfigure() +{ + // NODELET_DEBUG_STREAM( "updateDynamicReconfigure MUTEX LOCK"); + mDynParMutex.lock(); + zed_nodelets::ZedConfig config; + + config.auto_exposure_gain = mCamAutoExposure; + config.auto_whitebalance = mCamAutoWB; + config.brightness = mCamBrightness; + config.depth_confidence = mCamDepthConfidence; + config.depth_texture_conf = mCamDepthTextureConf; + config.contrast = mCamContrast; + config.exposure = mCamExposure; + config.gain = mCamGain; + config.hue = mCamHue; + config.saturation = mCamSaturation; + config.sharpness = mCamSharpness; + config.gamma = mCamGamma; + config.whitebalance_temperature = mCamWB / 100; + config.point_cloud_freq = mPointCloudFreq; + config.pub_frame_rate = mVideoDepthFreq; + mDynParMutex.unlock(); + + mDynServerMutex.lock(); + mDynRecServer->updateConfig(config); + mDynServerMutex.unlock(); + + mUpdateDynParams = false; + + // NODELET_DEBUG_STREAM( "updateDynamicReconfigure MUTEX UNLOCK"); } -void ZEDWrapperNodelet::callback_dynamicReconf(zed_nodelets::ZedConfig& config, uint32_t level) { - //NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX LOCK"); - mDynParMutex.lock(); - DynParams param = static_cast(level); +void ZEDWrapperNodelet::callback_dynamicReconf(zed_nodelets::ZedConfig& config, uint32_t level) +{ + // NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX LOCK"); + mDynParMutex.lock(); + DynParams param = static_cast(level); - switch (param) { + switch (param) + { case DATAPUB_FREQ: - if(config.pub_frame_rate>mCamFrameRate) { - mVideoDepthFreq = mCamFrameRate; - NODELET_WARN_STREAM( "'pub_frame_rate' cannot be major than camera grabbing framerate. Set to " << mVideoDepthFreq ); - - mUpdateDynParams = true; - } else { - mVideoDepthFreq = config.pub_frame_rate; - NODELET_INFO("Reconfigure Video and Depth pub. frequency: %g", mVideoDepthFreq); - } - - mVideoDepthTimer.setPeriod( ros::Duration(1.0 / mVideoDepthFreq) ); - - mDynParMutex.unlock(); - //NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); - break; + if (config.pub_frame_rate > mCamFrameRate) + { + mVideoDepthFreq = mCamFrameRate; + NODELET_WARN_STREAM("'pub_frame_rate' cannot be major than camera grabbing framerate. Set to " + << mVideoDepthFreq); + + mUpdateDynParams = true; + } + else + { + mVideoDepthFreq = config.pub_frame_rate; + NODELET_INFO("Reconfigure Video and Depth pub. frequency: %g", mVideoDepthFreq); + } + + mVideoDepthTimer.setPeriod(ros::Duration(1.0 / mVideoDepthFreq)); + + mDynParMutex.unlock(); + // NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); + break; case CONFIDENCE: - mCamDepthConfidence = config.depth_confidence; - NODELET_INFO("Reconfigure confidence threshold: %d", mCamDepthConfidence); - mDynParMutex.unlock(); - //NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); - break; + mCamDepthConfidence = config.depth_confidence; + NODELET_INFO("Reconfigure confidence threshold: %d", mCamDepthConfidence); + mDynParMutex.unlock(); + // NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); + break; case TEXTURE_CONF: - mCamDepthTextureConf = config.depth_texture_conf; - NODELET_INFO("Reconfigure texture confidence threshold: %d", mCamDepthTextureConf); - mDynParMutex.unlock(); - //NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); - break; + mCamDepthTextureConf = config.depth_texture_conf; + NODELET_INFO("Reconfigure texture confidence threshold: %d", mCamDepthTextureConf); + mDynParMutex.unlock(); + // NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); + break; case POINTCLOUD_FREQ: - if(config.point_cloud_freq>mCamFrameRate) { - mPointCloudFreq = mCamFrameRate; - NODELET_WARN_STREAM( "'point_cloud_freq' cannot be major than camera grabbing framerate. Set to " << mPointCloudFreq ); - - mUpdateDynParams = true; - } else { - mPointCloudFreq = config.point_cloud_freq; - NODELET_INFO("Reconfigure point cloud pub. frequency: %g", mPointCloudFreq); - } - - mDynParMutex.unlock(); - //NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); - break; + if (config.point_cloud_freq > mCamFrameRate) + { + mPointCloudFreq = mCamFrameRate; + NODELET_WARN_STREAM("'point_cloud_freq' cannot be major than camera grabbing framerate. Set to " + << mPointCloudFreq); + + mUpdateDynParams = true; + } + else + { + mPointCloudFreq = config.point_cloud_freq; + NODELET_INFO("Reconfigure point cloud pub. frequency: %g", mPointCloudFreq); + } + + mDynParMutex.unlock(); + // NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); + break; case BRIGHTNESS: - mCamBrightness = config.brightness; - NODELET_INFO("Reconfigure image brightness: %d", mCamBrightness); - mDynParMutex.unlock(); - //NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); - break; + mCamBrightness = config.brightness; + NODELET_INFO("Reconfigure image brightness: %d", mCamBrightness); + mDynParMutex.unlock(); + // NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); + break; case CONTRAST: - mCamContrast = config.contrast; - NODELET_INFO("Reconfigure image contrast: %d", mCamContrast); - mDynParMutex.unlock(); - //NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); - break; + mCamContrast = config.contrast; + NODELET_INFO("Reconfigure image contrast: %d", mCamContrast); + mDynParMutex.unlock(); + // NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); + break; case HUE: - mCamHue = config.hue; - NODELET_INFO("Reconfigure image hue: %d", mCamHue); - mDynParMutex.unlock(); - //NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); - break; + mCamHue = config.hue; + NODELET_INFO("Reconfigure image hue: %d", mCamHue); + mDynParMutex.unlock(); + // NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); + break; case SATURATION: - mCamSaturation = config.saturation; - NODELET_INFO("Reconfigure image saturation: %d", mCamSaturation); - mDynParMutex.unlock(); - //NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); - break; + mCamSaturation = config.saturation; + NODELET_INFO("Reconfigure image saturation: %d", mCamSaturation); + mDynParMutex.unlock(); + // NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); + break; case SHARPNESS: - mCamSharpness = config.sharpness; - NODELET_INFO("Reconfigure image sharpness: %d", mCamSharpness); - mDynParMutex.unlock(); - //NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); - break; + mCamSharpness = config.sharpness; + NODELET_INFO("Reconfigure image sharpness: %d", mCamSharpness); + mDynParMutex.unlock(); + // NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); + break; case GAMMA: -#if (ZED_SDK_MAJOR_VERSION==3 && ZED_SDK_MINOR_VERSION>=1) - mCamGamma = config.gamma; - NODELET_INFO("Reconfigure image gamma: %d", mCamGamma); - mDynParMutex.unlock(); - //NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); +#if (ZED_SDK_MAJOR_VERSION == 3 && ZED_SDK_MINOR_VERSION >= 1) + mCamGamma = config.gamma; + NODELET_INFO("Reconfigure image gamma: %d", mCamGamma); + mDynParMutex.unlock(); + // NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); #else - NODELET_DEBUG_STREAM( "Gamma Control is not available for SDK older that v3.1"); - mDynParMutex.unlock(); + NODELET_DEBUG_STREAM("Gamma Control is not available for SDK older that v3.1"); + mDynParMutex.unlock(); #endif - break; + break; case AUTO_EXP_GAIN: - mCamAutoExposure = config.auto_exposure_gain; - NODELET_INFO_STREAM("Reconfigure auto exposure/gain: " << mCamAutoExposure?"ENABLED":"DISABLED"); - if( !mCamAutoExposure ) { - mZed.setCameraSettings(sl::VIDEO_SETTINGS::AEC_AGC, 0 ); - mTriggerAutoExposure = false; - } else { - mTriggerAutoExposure = true; - } - mDynParMutex.unlock(); - //NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); - break; + mCamAutoExposure = config.auto_exposure_gain; + NODELET_INFO_STREAM("Reconfigure auto exposure/gain: " << mCamAutoExposure ? "ENABLED" : "DISABLED"); + if (!mCamAutoExposure) + { + mZed.setCameraSettings(sl::VIDEO_SETTINGS::AEC_AGC, 0); + mTriggerAutoExposure = false; + } + else + { + mTriggerAutoExposure = true; + } + mDynParMutex.unlock(); + // NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); + break; case GAIN: - mCamGain = config.gain; - if(mCamAutoExposure) { - NODELET_WARN("Reconfigure gain has no effect if 'auto_exposure_gain' is enabled"); - } else { - NODELET_INFO("Reconfigure gain: %d", mCamGain); - } - mDynParMutex.unlock(); - //NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); - break; + mCamGain = config.gain; + if (mCamAutoExposure) + { + NODELET_WARN("Reconfigure gain has no effect if 'auto_exposure_gain' is enabled"); + } + else + { + NODELET_INFO("Reconfigure gain: %d", mCamGain); + } + mDynParMutex.unlock(); + // NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); + break; case EXPOSURE: - mCamExposure = config.exposure; - if(mCamAutoExposure) { - NODELET_WARN("Reconfigure exposure has no effect if 'auto_exposure_gain' is enabled"); - } else { - NODELET_INFO("Reconfigure exposure: %d", mCamExposure); - } - mDynParMutex.unlock(); - //NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); - break; + mCamExposure = config.exposure; + if (mCamAutoExposure) + { + NODELET_WARN("Reconfigure exposure has no effect if 'auto_exposure_gain' is enabled"); + } + else + { + NODELET_INFO("Reconfigure exposure: %d", mCamExposure); + } + mDynParMutex.unlock(); + // NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); + break; case AUTO_WB: - mCamAutoWB = config.auto_whitebalance; - NODELET_INFO_STREAM("Reconfigure auto white balance: " << mCamAutoWB?"ENABLED":"DISABLED"); - if( !mCamAutoWB ) { - mZed.setCameraSettings(sl::VIDEO_SETTINGS::WHITEBALANCE_AUTO, 0 ); - mTriggerAutoWB = false; - } else { - mTriggerAutoWB = true; - } - mDynParMutex.unlock(); - //NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); - break; + mCamAutoWB = config.auto_whitebalance; + NODELET_INFO_STREAM("Reconfigure auto white balance: " << mCamAutoWB ? "ENABLED" : "DISABLED"); + if (!mCamAutoWB) + { + mZed.setCameraSettings(sl::VIDEO_SETTINGS::WHITEBALANCE_AUTO, 0); + mTriggerAutoWB = false; + } + else + { + mTriggerAutoWB = true; + } + mDynParMutex.unlock(); + // NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); + break; case WB_TEMP: - mCamWB = config.whitebalance_temperature*100; - if(mCamAutoWB) { - NODELET_WARN("Reconfigure white balance temperature has no effect if 'auto_whitebalance' is enabled"); - } else { - NODELET_INFO("Reconfigure white balance temperature: %d", mCamWB); - } - mDynParMutex.unlock(); - //NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); - break; + mCamWB = config.whitebalance_temperature * 100; + if (mCamAutoWB) + { + NODELET_WARN("Reconfigure white balance temperature has no effect if 'auto_whitebalance' is enabled"); + } + else + { + NODELET_INFO("Reconfigure white balance temperature: %d", mCamWB); + } + mDynParMutex.unlock(); + // NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); + break; default: - NODELET_DEBUG_STREAM( "dynamicReconfCallback Unknown param: " << level); - mDynParMutex.unlock(); - //NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); - } + NODELET_DEBUG_STREAM("dynamicReconfCallback Unknown param: " << level); + mDynParMutex.unlock(); + // NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); + } } -void ZEDWrapperNodelet::callback_pubVideoDepth(const ros::TimerEvent& e) { - static sl::Timestamp lastZedTs = 0; // Used to calculate stable publish frequency - - uint32_t rgbSubnumber = mPubRgb.getNumSubscribers(); - uint32_t rgbRawSubnumber = mPubRawRgb.getNumSubscribers(); - uint32_t leftSubnumber = mPubLeft.getNumSubscribers(); - uint32_t leftRawSubnumber = mPubRawLeft.getNumSubscribers(); - uint32_t rightSubnumber = mPubRight.getNumSubscribers(); - uint32_t rightRawSubnumber = mPubRawRight.getNumSubscribers(); - uint32_t rgbGraySubnumber = mPubRgbGray.getNumSubscribers(); - uint32_t rgbGrayRawSubnumber = mPubRawRgbGray.getNumSubscribers(); - uint32_t leftGraySubnumber = mPubLeftGray.getNumSubscribers(); - uint32_t leftGrayRawSubnumber = mPubRawLeftGray.getNumSubscribers(); - uint32_t rightGraySubnumber = mPubRightGray.getNumSubscribers(); - uint32_t rightGrayRawSubnumber = mPubRawRightGray.getNumSubscribers(); - uint32_t depthSubnumber = mPubDepth.getNumSubscribers(); - uint32_t disparitySubnumber = mPubDisparity.getNumSubscribers(); - uint32_t confMapSubnumber = mPubConfMap.getNumSubscribers(); - uint32_t stereoSubNumber = mPubStereo.getNumSubscribers(); - uint32_t stereoRawSubNumber = mPubRawStereo.getNumSubscribers(); - - uint32_t tot_sub = rgbSubnumber+rgbRawSubnumber+leftSubnumber+leftRawSubnumber+rightSubnumber+rightRawSubnumber+ - rgbGraySubnumber+rgbGrayRawSubnumber+leftGraySubnumber+leftGrayRawSubnumber+ - rightGraySubnumber+rightGrayRawSubnumber+depthSubnumber+disparitySubnumber+confMapSubnumber+ - stereoSubNumber+stereoRawSubNumber; - - bool retrieved = false; - - sl::Mat mat_left,mat_left_raw; - sl::Mat mat_right,mat_right_raw; - sl::Mat mat_left_gray,mat_left_raw_gray; - 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 grab_ts=0; - - mCamDataMutex.lock(); - - // ----> Retrieve all required image data - if(rgbSubnumber+leftSubnumber+stereoSubNumber>0) { - mZed.retrieveImage(mat_left, sl::VIEW::LEFT, sl::MEM::CPU, mMatResolVideo); - retrieved = true; - ts_rgb=mat_left.timestamp; - grab_ts=mat_left.timestamp; - } - if(rgbRawSubnumber+leftRawSubnumber+stereoRawSubNumber>0) { - mZed.retrieveImage(mat_left_raw, sl::VIEW::LEFT_UNRECTIFIED, sl::MEM::CPU, mMatResolVideo); - retrieved = true; - grab_ts=mat_left_raw.timestamp; - } - if(rightSubnumber+stereoSubNumber>0) { - mZed.retrieveImage(mat_right, sl::VIEW::RIGHT, sl::MEM::CPU, mMatResolVideo); - retrieved = true; - grab_ts=mat_right.timestamp; - } - if(rightRawSubnumber+stereoRawSubNumber>0) { - mZed.retrieveImage(mat_right_raw, sl::VIEW::RIGHT_UNRECTIFIED, sl::MEM::CPU, mMatResolVideo); - retrieved = true; - grab_ts=mat_right_raw.timestamp; - } - if(rgbGraySubnumber+leftGraySubnumber>0) { - mZed.retrieveImage(mat_left_gray, sl::VIEW::LEFT_GRAY, sl::MEM::CPU, mMatResolVideo); - retrieved = true; - grab_ts=mat_left_gray.timestamp; - } - if(rgbGrayRawSubnumber+leftGrayRawSubnumber>0) { - mZed.retrieveImage(mat_left_raw_gray, sl::VIEW::LEFT_UNRECTIFIED_GRAY, sl::MEM::CPU, mMatResolVideo); - retrieved = true; - grab_ts=mat_left_raw_gray.timestamp; - } - if(rightGraySubnumber>0) { - mZed.retrieveImage(mat_right_gray, sl::VIEW::RIGHT_GRAY, sl::MEM::CPU, mMatResolVideo); - retrieved = true; - grab_ts=mat_right_gray.timestamp; - } - if(rightGrayRawSubnumber>0) { - mZed.retrieveImage(mat_right_raw_gray, sl::VIEW::RIGHT_UNRECTIFIED_GRAY, sl::MEM::CPU, mMatResolVideo); - retrieved = true; - grab_ts=mat_right_raw_gray.timestamp; - } - if(depthSubnumber>0) { - mZed.retrieveMeasure(mat_depth, sl::MEASURE::DEPTH, sl::MEM::CPU, mMatResolDepth); - retrieved = true; - grab_ts=mat_depth.timestamp; - - ts_depth = mat_depth.timestamp; - - if( ts_rgb.data_ns!=0 && (ts_depth.data_ns!=ts_rgb.data_ns) ) { - NODELET_WARN_STREAM( "!!!!! DEPTH/RGB ASYNC !!!!! - Delta: " << 1e-9*static_cast(ts_depth-ts_rgb) << " sec"); - } - } - if(disparitySubnumber>0) { - mZed.retrieveMeasure(mat_disp, sl::MEASURE::DISPARITY, sl::MEM::CPU, mMatResolDepth); - retrieved = true; - grab_ts=mat_disp.timestamp; - } - if(confMapSubnumber>0) { - mZed.retrieveMeasure(mat_conf, sl::MEASURE::CONFIDENCE, sl::MEM::CPU, mMatResolDepth); - retrieved = true; - grab_ts=mat_conf.timestamp; - } - // <---- Retrieve all required image data - - // ----> Data ROS timestamp - ros::Time stamp = sl_tools::slTime2Ros(grab_ts); - if(mSvoMode) { - stamp = ros::Time::now(); - } - // <---- Data ROS timestamp - - // ----> Publish sensor data if sync is required by user or SVO - if( mZedRealCamModel!=sl::MODEL::ZED ) +void ZEDWrapperNodelet::callback_pubVideoDepth(const ros::TimerEvent& e) +{ + static sl::Timestamp lastZedTs = 0; // Used to calculate stable publish frequency + + uint32_t rgbSubnumber = mPubRgb.getNumSubscribers(); + uint32_t rgbRawSubnumber = mPubRawRgb.getNumSubscribers(); + uint32_t leftSubnumber = mPubLeft.getNumSubscribers(); + uint32_t leftRawSubnumber = mPubRawLeft.getNumSubscribers(); + uint32_t rightSubnumber = mPubRight.getNumSubscribers(); + uint32_t rightRawSubnumber = mPubRawRight.getNumSubscribers(); + uint32_t rgbGraySubnumber = mPubRgbGray.getNumSubscribers(); + uint32_t rgbGrayRawSubnumber = mPubRawRgbGray.getNumSubscribers(); + uint32_t leftGraySubnumber = mPubLeftGray.getNumSubscribers(); + uint32_t leftGrayRawSubnumber = mPubRawLeftGray.getNumSubscribers(); + uint32_t rightGraySubnumber = mPubRightGray.getNumSubscribers(); + uint32_t rightGrayRawSubnumber = mPubRawRightGray.getNumSubscribers(); + uint32_t depthSubnumber = mPubDepth.getNumSubscribers(); + uint32_t disparitySubnumber = mPubDisparity.getNumSubscribers(); + uint32_t confMapSubnumber = mPubConfMap.getNumSubscribers(); + uint32_t stereoSubNumber = mPubStereo.getNumSubscribers(); + uint32_t stereoRawSubNumber = mPubRawStereo.getNumSubscribers(); + + uint32_t tot_sub = rgbSubnumber + rgbRawSubnumber + leftSubnumber + leftRawSubnumber + rightSubnumber + + rightRawSubnumber + rgbGraySubnumber + rgbGrayRawSubnumber + leftGraySubnumber + + leftGrayRawSubnumber + rightGraySubnumber + rightGrayRawSubnumber + depthSubnumber + + disparitySubnumber + confMapSubnumber + stereoSubNumber + stereoRawSubNumber; + + bool retrieved = false; + + sl::Mat mat_left, mat_left_raw; + sl::Mat mat_right, mat_right_raw; + sl::Mat mat_left_gray, mat_left_raw_gray; + 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 grab_ts = 0; + + mCamDataMutex.lock(); + + // ----> Retrieve all required image data + if (rgbSubnumber + leftSubnumber + stereoSubNumber > 0) + { + mZed.retrieveImage(mat_left, sl::VIEW::LEFT, sl::MEM::CPU, mMatResolVideo); + retrieved = true; + ts_rgb = mat_left.timestamp; + grab_ts = mat_left.timestamp; + } + if (rgbRawSubnumber + leftRawSubnumber + stereoRawSubNumber > 0) + { + mZed.retrieveImage(mat_left_raw, sl::VIEW::LEFT_UNRECTIFIED, sl::MEM::CPU, mMatResolVideo); + retrieved = true; + grab_ts = mat_left_raw.timestamp; + } + if (rightSubnumber + stereoSubNumber > 0) + { + mZed.retrieveImage(mat_right, sl::VIEW::RIGHT, sl::MEM::CPU, mMatResolVideo); + retrieved = true; + grab_ts = mat_right.timestamp; + } + if (rightRawSubnumber + stereoRawSubNumber > 0) + { + mZed.retrieveImage(mat_right_raw, sl::VIEW::RIGHT_UNRECTIFIED, sl::MEM::CPU, mMatResolVideo); + retrieved = true; + grab_ts = mat_right_raw.timestamp; + } + if (rgbGraySubnumber + leftGraySubnumber > 0) + { + mZed.retrieveImage(mat_left_gray, sl::VIEW::LEFT_GRAY, sl::MEM::CPU, mMatResolVideo); + retrieved = true; + grab_ts = mat_left_gray.timestamp; + } + if (rgbGrayRawSubnumber + leftGrayRawSubnumber > 0) + { + mZed.retrieveImage(mat_left_raw_gray, sl::VIEW::LEFT_UNRECTIFIED_GRAY, sl::MEM::CPU, mMatResolVideo); + retrieved = true; + grab_ts = mat_left_raw_gray.timestamp; + } + if (rightGraySubnumber > 0) + { + mZed.retrieveImage(mat_right_gray, sl::VIEW::RIGHT_GRAY, sl::MEM::CPU, mMatResolVideo); + retrieved = true; + grab_ts = mat_right_gray.timestamp; + } + if (rightGrayRawSubnumber > 0) + { + mZed.retrieveImage(mat_right_raw_gray, sl::VIEW::RIGHT_UNRECTIFIED_GRAY, sl::MEM::CPU, mMatResolVideo); + retrieved = true; + grab_ts = mat_right_raw_gray.timestamp; + } + if (depthSubnumber > 0) + { +#if (ZED_SDK_MAJOR_VERSION == 3 && ZED_SDK_MINOR_VERSION < 4) + mZed.retrieveMeasure(mat_depth, sl::MEASURE::DEPTH, sl::MEM::CPU, mMatResolDepth); +#else + if (!mOpenniDepthMode) { - if(mSensTimestampSync) { - //NODELET_INFO_STREAM("tot_sub: " << tot_sub << " - retrieved: " << retrieved << " - grab_ts.data_ns!=lastZedTs.data_ns: " << (grab_ts.data_ns!=lastZedTs.data_ns)); - if(tot_sub>0 && retrieved && (grab_ts.data_ns!=lastZedTs.data_ns)) { - //NODELET_INFO("CALLBACK"); - publishSensData(stamp); - } - } else if(mSvoMode) { - publishSensData(stamp); - } - } - // <---- Publish sensor data if sync is required by user or SVO - - mCamDataMutex.unlock(); - - // ----> Notify grab thread that all data are synchronized and a new grab can be done - //mRgbDepthDataRetrievedCondVar.notify_one(); - //mRgbDepthDataRetrieved = true; - // <---- Notify grab thread that all data are synchronized and a new grab can be done - - if(!retrieved) { - mPublishingData = false; - lastZedTs = 0; - return; - } - mPublishingData = true; - - // ----> Check if a grab has been done before publishing the same images - if( grab_ts.data_ns==lastZedTs.data_ns ) { - // Data not updated by a grab calling in the grab thread - return; - } - if(lastZedTs.data_ns!=0) { - double period_sec = static_cast(grab_ts.data_ns - lastZedTs.data_ns)/1e9; - //NODELET_DEBUG_STREAM( "PUBLISHING PERIOD: " << period_sec << " sec @" << 1./period_sec << " Hz") ; - - mVideoDepthPeriodMean_sec->addValue(period_sec); - //NODELET_DEBUG_STREAM( "MEAN PUBLISHING PERIOD: " << mVideoDepthPeriodMean_sec->getMean() << " sec @" << 1./mVideoDepthPeriodMean_sec->getMean() << " Hz") ; + mZed.retrieveMeasure(mat_depth, sl::MEASURE::DEPTH, sl::MEM::CPU, mMatResolDepth); } - lastZedTs = grab_ts; - // <---- Check if a grab has been done before publishing the same images - - // Publish the left = rgb image if someone has subscribed to - if (leftSubnumber > 0) { - sensor_msgs::ImagePtr leftImgMsg = boost::make_shared(); - publishImage(leftImgMsg, mat_left, mPubLeft, mLeftCamInfoMsg, mLeftCamOptFrameId, stamp); - } - if (rgbSubnumber > 0) { - sensor_msgs::ImagePtr rgbImgMsg = boost::make_shared(); - publishImage(rgbImgMsg, mat_left, mPubRgb, mRgbCamInfoMsg, mDepthOptFrameId, stamp); - } - - // Publish the left = rgb GRAY image if someone has subscribed to - if (leftGraySubnumber > 0) { - sensor_msgs::ImagePtr leftGrayImgMsg = boost::make_shared(); - publishImage(leftGrayImgMsg, mat_left_gray, mPubLeftGray, mLeftCamInfoMsg, mLeftCamOptFrameId, stamp); - } - if (rgbGraySubnumber > 0) { - sensor_msgs::ImagePtr rgbGrayImgMsg = boost::make_shared(); - publishImage(rgbGrayImgMsg, mat_left_gray, mPubRgbGray, mRgbCamInfoMsg, mDepthOptFrameId, stamp); - } - - // Publish the left_raw = rgb_raw image if someone has subscribed to - if (leftRawSubnumber > 0) { - sensor_msgs::ImagePtr rawLeftImgMsg = boost::make_shared(); - publishImage(rawLeftImgMsg, mat_left_raw, mPubRawLeft, mLeftCamInfoRawMsg, mLeftCamOptFrameId, stamp); - } - if (rgbRawSubnumber > 0) { - sensor_msgs::ImagePtr rawRgbImgMsg = boost::make_shared(); - publishImage(rawRgbImgMsg, mat_left_raw, mPubRawRgb, mRgbCamInfoRawMsg, mDepthOptFrameId, stamp); - } - - // Publish the left_raw == rgb_raw GRAY image if someone has subscribed to - if (leftGrayRawSubnumber > 0) { - sensor_msgs::ImagePtr rawLeftGrayImgMsg = boost::make_shared(); - - publishImage(rawLeftGrayImgMsg, mat_left_raw_gray, mPubRawLeftGray, mLeftCamInfoRawMsg, mLeftCamOptFrameId, stamp); - } - if (rgbGrayRawSubnumber > 0) { - sensor_msgs::ImagePtr rawRgbGrayImgMsg = boost::make_shared(); - publishImage(rawRgbGrayImgMsg, mat_left_raw_gray, mPubRawRgbGray, mRgbCamInfoRawMsg, mDepthOptFrameId, stamp); - } - - // Publish the right image if someone has subscribed to - if (rightSubnumber > 0) { - sensor_msgs::ImagePtr rightImgMsg = boost::make_shared(); - publishImage(rightImgMsg, mat_right, mPubRight, mRightCamInfoMsg, mRightCamOptFrameId, stamp); - } - - // Publish the right image GRAY if someone has subscribed to - if (rightGraySubnumber > 0) { - sensor_msgs::ImagePtr rightGrayImgMsg = boost::make_shared(); - publishImage(rightGrayImgMsg, mat_right_gray, mPubRightGray, mRightCamInfoMsg, mRightCamOptFrameId, stamp); - } - - // Publish the right raw image if someone has subscribed to - if (rightRawSubnumber > 0) { - sensor_msgs::ImagePtr rawRightImgMsg = boost::make_shared(); - publishImage(rawRightImgMsg, mat_right_raw, mPubRawRight, mRightCamInfoRawMsg, mRightCamOptFrameId, stamp); - } - - // Publish the right raw image GRAY if someone has subscribed to - if (rightGrayRawSubnumber > 0) { - sensor_msgs::ImagePtr rawRightGrayImgMsg = boost::make_shared(); - publishImage(rawRightGrayImgMsg, mat_right_raw_gray, mPubRawRightGray, mRightCamInfoRawMsg, mRightCamOptFrameId, stamp); - } - - // Stereo couple side-by-side - if (stereoSubNumber > 0) { - sensor_msgs::ImagePtr stereoImgMsg = boost::make_shared(); - sl_tools::imagesToROSmsg(stereoImgMsg, mat_left, mat_right, mCameraFrameId, stamp); - mPubStereo.publish(stereoImgMsg); - } - - // Stereo RAW couple side-by-side - if (stereoRawSubNumber > 0) { - sensor_msgs::ImagePtr rawStereoImgMsg = boost::make_shared(); - sl_tools::imagesToROSmsg(rawStereoImgMsg, mat_left_raw, mat_right_raw, mCameraFrameId, stamp); - mPubRawStereo.publish(rawStereoImgMsg); - } - - // Publish the depth image if someone has subscribed to - if (depthSubnumber > 0) { - sensor_msgs::ImagePtr depthImgMsg = boost::make_shared(); - publishDepth(depthImgMsg, mat_depth, stamp); + else + { + mZed.retrieveMeasure(mat_depth, sl::MEASURE::DEPTH_U16_MM, sl::MEM::CPU, mMatResolDepth); } +#endif + retrieved = true; + grab_ts = mat_depth.timestamp; - // Publish the disparity image if someone has subscribed to - if (disparitySubnumber > 0) { - publishDisparity(mat_disp, stamp); - } + ts_depth = mat_depth.timestamp; - // Publish the confidence map if someone has subscribed to - if (confMapSubnumber > 0) { - sensor_msgs::ImagePtr confMapMsg = boost::make_shared(); - sl_tools::imageToROSmsg(confMapMsg, mat_conf, mConfidenceOptFrameId, stamp); - mPubConfMap.publish(confMapMsg); - } + if (ts_rgb.data_ns != 0 && (ts_depth.data_ns != ts_rgb.data_ns)) + { + NODELET_WARN_STREAM("!!!!! DEPTH/RGB ASYNC !!!!! - Delta: " << 1e-9 * static_cast(ts_depth - ts_rgb) + << " sec"); + } + } + if (disparitySubnumber > 0) + { + mZed.retrieveMeasure(mat_disp, sl::MEASURE::DISPARITY, sl::MEM::CPU, mMatResolDepth); + retrieved = true; + grab_ts = mat_disp.timestamp; + } + if (confMapSubnumber > 0) + { + mZed.retrieveMeasure(mat_conf, sl::MEASURE::CONFIDENCE, sl::MEM::CPU, mMatResolDepth); + retrieved = true; + grab_ts = mat_conf.timestamp; + } + // <---- Retrieve all required image data + + // ----> Data ROS timestamp + ros::Time stamp = sl_tools::slTime2Ros(grab_ts); + if (mSvoMode) + { + stamp = ros::Time::now(); + } + // <---- Data ROS timestamp + + // ----> Publish sensor data if sync is required by user or SVO + if (mZedRealCamModel != sl::MODEL::ZED) + { + if (mSensTimestampSync) + { + // NODELET_INFO_STREAM("tot_sub: " << tot_sub << " - retrieved: " << retrieved << " - + // grab_ts.data_ns!=lastZedTs.data_ns: " << (grab_ts.data_ns!=lastZedTs.data_ns)); + if (tot_sub > 0 && retrieved && (grab_ts.data_ns != lastZedTs.data_ns)) + { + // NODELET_INFO("CALLBACK"); + publishSensData(stamp); + } + } + else if (mSvoMode) + { + publishSensData(stamp); + } + } + // <---- Publish sensor data if sync is required by user or SVO + + mCamDataMutex.unlock(); + + // ----> Notify grab thread that all data are synchronized and a new grab can be done + // mRgbDepthDataRetrievedCondVar.notify_one(); + // mRgbDepthDataRetrieved = true; + // <---- Notify grab thread that all data are synchronized and a new grab can be done + + if (!retrieved) + { + mPublishingData = false; + lastZedTs = 0; + return; + } + mPublishingData = true; + + // ----> Check if a grab has been done before publishing the same images + if (grab_ts.data_ns == lastZedTs.data_ns) + { + // Data not updated by a grab calling in the grab thread + return; + } + if (lastZedTs.data_ns != 0) + { + double period_sec = static_cast(grab_ts.data_ns - lastZedTs.data_ns) / 1e9; + // NODELET_DEBUG_STREAM( "PUBLISHING PERIOD: " << period_sec << " sec @" << 1./period_sec << " Hz") ; + + mVideoDepthPeriodMean_sec->addValue(period_sec); + // NODELET_DEBUG_STREAM( "MEAN PUBLISHING PERIOD: " << mVideoDepthPeriodMean_sec->getMean() << " sec @" + // << 1./mVideoDepthPeriodMean_sec->getMean() << " Hz") ; + } + lastZedTs = grab_ts; + // <---- Check if a grab has been done before publishing the same images + + // Publish the left = rgb image if someone has subscribed to + if (leftSubnumber > 0) + { + sensor_msgs::ImagePtr leftImgMsg = boost::make_shared(); + publishImage(leftImgMsg, mat_left, mPubLeft, mLeftCamInfoMsg, mLeftCamOptFrameId, stamp); + } + if (rgbSubnumber > 0) + { + sensor_msgs::ImagePtr rgbImgMsg = boost::make_shared(); + publishImage(rgbImgMsg, mat_left, mPubRgb, mRgbCamInfoMsg, mDepthOptFrameId, stamp); + } + + // Publish the left = rgb GRAY image if someone has subscribed to + if (leftGraySubnumber > 0) + { + sensor_msgs::ImagePtr leftGrayImgMsg = boost::make_shared(); + publishImage(leftGrayImgMsg, mat_left_gray, mPubLeftGray, mLeftCamInfoMsg, mLeftCamOptFrameId, stamp); + } + if (rgbGraySubnumber > 0) + { + sensor_msgs::ImagePtr rgbGrayImgMsg = boost::make_shared(); + publishImage(rgbGrayImgMsg, mat_left_gray, mPubRgbGray, mRgbCamInfoMsg, mDepthOptFrameId, stamp); + } + + // Publish the left_raw = rgb_raw image if someone has subscribed to + if (leftRawSubnumber > 0) + { + sensor_msgs::ImagePtr rawLeftImgMsg = boost::make_shared(); + publishImage(rawLeftImgMsg, mat_left_raw, mPubRawLeft, mLeftCamInfoRawMsg, mLeftCamOptFrameId, stamp); + } + if (rgbRawSubnumber > 0) + { + sensor_msgs::ImagePtr rawRgbImgMsg = boost::make_shared(); + publishImage(rawRgbImgMsg, mat_left_raw, mPubRawRgb, mRgbCamInfoRawMsg, mDepthOptFrameId, stamp); + } + + // Publish the left_raw == rgb_raw GRAY image if someone has subscribed to + if (leftGrayRawSubnumber > 0) + { + sensor_msgs::ImagePtr rawLeftGrayImgMsg = boost::make_shared(); + + publishImage(rawLeftGrayImgMsg, mat_left_raw_gray, mPubRawLeftGray, mLeftCamInfoRawMsg, mLeftCamOptFrameId, stamp); + } + if (rgbGrayRawSubnumber > 0) + { + sensor_msgs::ImagePtr rawRgbGrayImgMsg = boost::make_shared(); + publishImage(rawRgbGrayImgMsg, mat_left_raw_gray, mPubRawRgbGray, mRgbCamInfoRawMsg, mDepthOptFrameId, stamp); + } + + // Publish the right image if someone has subscribed to + if (rightSubnumber > 0) + { + sensor_msgs::ImagePtr rightImgMsg = boost::make_shared(); + publishImage(rightImgMsg, mat_right, mPubRight, mRightCamInfoMsg, mRightCamOptFrameId, stamp); + } + + // Publish the right image GRAY if someone has subscribed to + if (rightGraySubnumber > 0) + { + sensor_msgs::ImagePtr rightGrayImgMsg = boost::make_shared(); + publishImage(rightGrayImgMsg, mat_right_gray, mPubRightGray, mRightCamInfoMsg, mRightCamOptFrameId, stamp); + } + + // Publish the right raw image if someone has subscribed to + if (rightRawSubnumber > 0) + { + sensor_msgs::ImagePtr rawRightImgMsg = boost::make_shared(); + publishImage(rawRightImgMsg, mat_right_raw, mPubRawRight, mRightCamInfoRawMsg, mRightCamOptFrameId, stamp); + } + + // Publish the right raw image GRAY if someone has subscribed to + if (rightGrayRawSubnumber > 0) + { + sensor_msgs::ImagePtr rawRightGrayImgMsg = boost::make_shared(); + publishImage(rawRightGrayImgMsg, mat_right_raw_gray, mPubRawRightGray, mRightCamInfoRawMsg, mRightCamOptFrameId, + stamp); + } + + // Stereo couple side-by-side + if (stereoSubNumber > 0) + { + sensor_msgs::ImagePtr stereoImgMsg = boost::make_shared(); + sl_tools::imagesToROSmsg(stereoImgMsg, mat_left, mat_right, mCameraFrameId, stamp); + mPubStereo.publish(stereoImgMsg); + } + + // Stereo RAW couple side-by-side + if (stereoRawSubNumber > 0) + { + sensor_msgs::ImagePtr rawStereoImgMsg = boost::make_shared(); + sl_tools::imagesToROSmsg(rawStereoImgMsg, mat_left_raw, mat_right_raw, mCameraFrameId, stamp); + mPubRawStereo.publish(rawStereoImgMsg); + } + + // Publish the depth image if someone has subscribed to + if (depthSubnumber > 0) + { + sensor_msgs::ImagePtr depthImgMsg = boost::make_shared(); + publishDepth(depthImgMsg, mat_depth, stamp); + } + + // Publish the disparity image if someone has subscribed to + if (disparitySubnumber > 0) + { + publishDisparity(mat_disp, stamp); + } + + // Publish the confidence map if someone has subscribed to + if (confMapSubnumber > 0) + { + sensor_msgs::ImagePtr confMapMsg = boost::make_shared(); + sl_tools::imageToROSmsg(confMapMsg, mat_conf, mConfidenceOptFrameId, stamp); + mPubConfMap.publish(confMapMsg); + } } -void ZEDWrapperNodelet::callback_pubPath(const ros::TimerEvent& e) { - uint32_t mapPathSub = mPubMapPath.getNumSubscribers(); - uint32_t odomPathSub = mPubOdomPath.getNumSubscribers(); - - geometry_msgs::PoseStamped odomPose; - geometry_msgs::PoseStamped mapPose; - - odomPose.header.stamp = mFrameTimestamp; - odomPose.header.frame_id = mMapFrameId; // frame - // conversion from Tranform to message - geometry_msgs::Transform base2odom = tf2::toMsg(mOdom2BaseTransf); - // Add all value in Pose message - odomPose.pose.position.x = base2odom.translation.x; - odomPose.pose.position.y = base2odom.translation.y; - odomPose.pose.position.z = base2odom.translation.z; - odomPose.pose.orientation.x = base2odom.rotation.x; - odomPose.pose.orientation.y = base2odom.rotation.y; - odomPose.pose.orientation.z = base2odom.rotation.z; - odomPose.pose.orientation.w = base2odom.rotation.w; - - mapPose.header.stamp = mFrameTimestamp; - mapPose.header.frame_id = mMapFrameId; // frame - // conversion from Tranform to message - geometry_msgs::Transform base2map = tf2::toMsg(mMap2BaseTransf); - // Add all value in Pose message - mapPose.pose.position.x = base2map.translation.x; - mapPose.pose.position.y = base2map.translation.y; - mapPose.pose.position.z = base2map.translation.z; - mapPose.pose.orientation.x = base2map.rotation.x; - mapPose.pose.orientation.y = base2map.rotation.y; - mapPose.pose.orientation.z = base2map.rotation.z; - mapPose.pose.orientation.w = base2map.rotation.w; - - // Circular vector - if (mPathMaxCount != -1) { - if (mOdomPath.size() == mPathMaxCount) { - NODELET_DEBUG("Path vectors full: rotating "); - std::rotate(mOdomPath.begin(), mOdomPath.begin() + 1, mOdomPath.end()); - std::rotate(mMapPath.begin(), mMapPath.begin() + 1, mMapPath.end()); - - mMapPath[mPathMaxCount - 1] = mapPose; - mOdomPath[mPathMaxCount - 1] = odomPose; - } else { - //NODELET_DEBUG_STREAM("Path vectors adding last available poses"); - mMapPath.push_back(mapPose); - mOdomPath.push_back(odomPose); - } - } else { - //NODELET_DEBUG_STREAM("No limit path vectors, adding last available poses"); - mMapPath.push_back(mapPose); - mOdomPath.push_back(odomPose); - } - - if (mapPathSub > 0) { - nav_msgs::PathPtr mapPath = boost::make_shared(); - mapPath->header.frame_id = mMapFrameId; - mapPath->header.stamp = mFrameTimestamp; - mapPath->poses = mMapPath; - - mPubMapPath.publish(mapPath); - } - - if (odomPathSub > 0) { - nav_msgs::PathPtr odomPath = boost::make_shared(); - odomPath->header.frame_id = mMapFrameId; - odomPath->header.stamp = mFrameTimestamp; - odomPath->poses = mOdomPath; +void ZEDWrapperNodelet::callback_pubPath(const ros::TimerEvent& e) +{ + uint32_t mapPathSub = mPubMapPath.getNumSubscribers(); + uint32_t odomPathSub = mPubOdomPath.getNumSubscribers(); + + geometry_msgs::PoseStamped odomPose; + geometry_msgs::PoseStamped mapPose; + + odomPose.header.stamp = mFrameTimestamp; + odomPose.header.frame_id = mMapFrameId; // frame + // conversion from Tranform to message + geometry_msgs::Transform base2odom = tf2::toMsg(mOdom2BaseTransf); + // Add all value in Pose message + odomPose.pose.position.x = base2odom.translation.x; + odomPose.pose.position.y = base2odom.translation.y; + odomPose.pose.position.z = base2odom.translation.z; + odomPose.pose.orientation.x = base2odom.rotation.x; + odomPose.pose.orientation.y = base2odom.rotation.y; + odomPose.pose.orientation.z = base2odom.rotation.z; + odomPose.pose.orientation.w = base2odom.rotation.w; + + mapPose.header.stamp = mFrameTimestamp; + mapPose.header.frame_id = mMapFrameId; // frame + // conversion from Tranform to message + geometry_msgs::Transform base2map = tf2::toMsg(mMap2BaseTransf); + // Add all value in Pose message + mapPose.pose.position.x = base2map.translation.x; + mapPose.pose.position.y = base2map.translation.y; + mapPose.pose.position.z = base2map.translation.z; + mapPose.pose.orientation.x = base2map.rotation.x; + mapPose.pose.orientation.y = base2map.rotation.y; + mapPose.pose.orientation.z = base2map.rotation.z; + mapPose.pose.orientation.w = base2map.rotation.w; + + // Circular vector + if (mPathMaxCount != -1) + { + if (mOdomPath.size() == mPathMaxCount) + { + NODELET_DEBUG("Path vectors full: rotating "); + std::rotate(mOdomPath.begin(), mOdomPath.begin() + 1, mOdomPath.end()); + std::rotate(mMapPath.begin(), mMapPath.begin() + 1, mMapPath.end()); - mPubOdomPath.publish(odomPath); + mMapPath[mPathMaxCount - 1] = mapPose; + mOdomPath[mPathMaxCount - 1] = odomPose; } + else + { + // NODELET_DEBUG_STREAM("Path vectors adding last available poses"); + mMapPath.push_back(mapPose); + mOdomPath.push_back(odomPose); + } + } + else + { + // NODELET_DEBUG_STREAM("No limit path vectors, adding last available poses"); + mMapPath.push_back(mapPose); + mOdomPath.push_back(odomPose); + } + + if (mapPathSub > 0) + { + nav_msgs::PathPtr mapPath = boost::make_shared(); + mapPath->header.frame_id = mMapFrameId; + mapPath->header.stamp = mFrameTimestamp; + mapPath->poses = mMapPath; + + mPubMapPath.publish(mapPath); + } + + if (odomPathSub > 0) + { + nav_msgs::PathPtr odomPath = boost::make_shared(); + odomPath->header.frame_id = mMapFrameId; + odomPath->header.stamp = mFrameTimestamp; + odomPath->poses = mOdomPath; + + mPubOdomPath.publish(odomPath); + } } -void ZEDWrapperNodelet::callback_pubSensorsData(const ros::TimerEvent& e) { - //NODELET_INFO("callback_pubSensorsData"); - std::lock_guard lock(mCloseZedMutex); +void ZEDWrapperNodelet::callback_pubSensorsData(const ros::TimerEvent& e) +{ + // NODELET_INFO("callback_pubSensorsData"); + std::lock_guard lock(mCloseZedMutex); - if (!mZed.isOpened()) { - return; - } + if (!mZed.isOpened()) + { + return; + } - publishSensData(); + publishSensData(); } -void ZEDWrapperNodelet::publishSensData(ros::Time t) { - //NODELET_INFO("publishSensData"); - - uint32_t imu_SubNumber = mPubImu.getNumSubscribers(); - uint32_t imu_RawSubNumber = mPubImuRaw.getNumSubscribers(); - uint32_t imu_TempSubNumber = 0; - uint32_t imu_MagSubNumber = 0; - uint32_t pressSubNumber = 0; - uint32_t tempLeftSubNumber = 0; - uint32_t tempRightSubNumber = 0; - - if( mZedRealCamModel == sl::MODEL::ZED2 ) { - imu_TempSubNumber = mPubImuTemp.getNumSubscribers(); - imu_MagSubNumber = mPubImuMag.getNumSubscribers(); - pressSubNumber = mPubPressure.getNumSubscribers(); - tempLeftSubNumber = mPubTempL.getNumSubscribers(); - tempRightSubNumber = mPubTempR.getNumSubscribers(); - } - - uint32_t tot_sub = imu_SubNumber+imu_RawSubNumber+imu_TempSubNumber+imu_MagSubNumber+pressSubNumber+ - tempLeftSubNumber+tempRightSubNumber; - - if(tot_sub>0) { - mSensPublishing=true; - } else { - mSensPublishing=false; - } - - bool sensors_data_published = false; - - ros::Time ts_imu; - ros::Time ts_baro; - ros::Time ts_mag; - - static ros::Time lastTs_imu = ros::Time(); - static ros::Time lastTs_baro = ros::Time(); - static ros::Time lastT_mag = ros::Time(); - - sl::SensorsData sens_data; - - if(mSvoMode || mSensTimestampSync) { - if( mZed.getSensorsData(sens_data, sl::TIME_REFERENCE::IMAGE) != sl::ERROR_CODE::SUCCESS ) { - NODELET_DEBUG("Not retrieved sensors data in IMAGE REFERENCE TIME"); - return; - } - } else { - if( mZed.getSensorsData(sens_data, sl::TIME_REFERENCE::CURRENT) != sl::ERROR_CODE::SUCCESS ) { - NODELET_DEBUG("Not retrieved sensors data in CURRENT REFERENCE TIME"); - return; - } +void ZEDWrapperNodelet::publishSensData(ros::Time t) +{ + // NODELET_INFO("publishSensData"); + + uint32_t imu_SubNumber = mPubImu.getNumSubscribers(); + uint32_t imu_RawSubNumber = mPubImuRaw.getNumSubscribers(); + uint32_t imu_TempSubNumber = 0; + uint32_t imu_MagSubNumber = 0; + uint32_t pressSubNumber = 0; + uint32_t tempLeftSubNumber = 0; + uint32_t tempRightSubNumber = 0; + + if (mZedRealCamModel == sl::MODEL::ZED2) + { + imu_TempSubNumber = mPubImuTemp.getNumSubscribers(); + imu_MagSubNumber = mPubImuMag.getNumSubscribers(); + pressSubNumber = mPubPressure.getNumSubscribers(); + tempLeftSubNumber = mPubTempL.getNumSubscribers(); + tempRightSubNumber = mPubTempR.getNumSubscribers(); + } + + uint32_t tot_sub = imu_SubNumber + imu_RawSubNumber + imu_TempSubNumber + imu_MagSubNumber + pressSubNumber + + tempLeftSubNumber + tempRightSubNumber; + + if (tot_sub > 0) + { + mSensPublishing = true; + } + else + { + mSensPublishing = false; + } + + bool sensors_data_published = false; + + ros::Time ts_imu; + ros::Time ts_baro; + ros::Time ts_mag; + + static ros::Time lastTs_imu = ros::Time(); + static ros::Time lastTs_baro = ros::Time(); + static ros::Time lastT_mag = ros::Time(); + + sl::SensorsData sens_data; + + if (mSvoMode || mSensTimestampSync) + { + if (mZed.getSensorsData(sens_data, sl::TIME_REFERENCE::IMAGE) != sl::ERROR_CODE::SUCCESS) + { + NODELET_DEBUG("Not retrieved sensors data in IMAGE REFERENCE TIME"); + return; } - - if(t!=ros::Time(0)) { - ts_imu = t; - ts_baro = t; - ts_mag = t; - } else { - ts_imu = sl_tools::slTime2Ros(sens_data.imu.timestamp); - ts_baro = sl_tools::slTime2Ros(sens_data.barometer.timestamp); - ts_mag = sl_tools::slTime2Ros(sens_data.magnetometer.timestamp); + } + else + { + if (mZed.getSensorsData(sens_data, sl::TIME_REFERENCE::CURRENT) != sl::ERROR_CODE::SUCCESS) + { + NODELET_DEBUG("Not retrieved sensors data in CURRENT REFERENCE TIME"); + return; + } + } + + if (t != ros::Time(0)) + { + ts_imu = t; + ts_baro = t; + ts_mag = t; + } + else + { + ts_imu = sl_tools::slTime2Ros(sens_data.imu.timestamp); + ts_baro = sl_tools::slTime2Ros(sens_data.barometer.timestamp); + ts_mag = sl_tools::slTime2Ros(sens_data.magnetometer.timestamp); + } + + // ----> Publish odometry tf only if enabled + if (mPublishTf && mTrackingReady) + { + NODELET_DEBUG("Publishing TF"); + + publishOdomFrame(mOdom2BaseTransf, ts_imu); // publish the base Frame in odometry frame + + if (mPublishMapTf) + { + publishPoseFrame(mMap2OdomTransf, ts_imu); // publish the odometry Frame in map frame } - // ----> Publish odometry tf only if enabled - if (mPublishTf && mTrackingReady) { - NODELET_DEBUG("Publishing TF"); - - publishOdomFrame(mOdom2BaseTransf, ts_imu); // publish the base Frame in odometry frame - - if (mPublishMapTf) { - publishPoseFrame(mMap2OdomTransf, ts_imu); // publish the odometry Frame in map frame - } - - if(mPublishImuTf && !mStaticImuFramePublished ) - { - publishStaticImuFrame(); - } + if (mPublishImuTf && !mStaticImuFramePublished) + { + publishStaticImuFrame(); } - // <---- Publish odometry tf only if enabled + } + // <---- Publish odometry tf only if enabled - bool new_imu_data = ts_imu!=lastTs_imu; - bool new_baro_data = ts_baro!=lastTs_baro; - bool new_mag_data = ts_mag!=lastT_mag; + bool new_imu_data = ts_imu != lastTs_imu; + bool new_baro_data = ts_baro != lastTs_baro; + bool new_mag_data = ts_mag != lastT_mag; - if( !new_imu_data && !new_baro_data && !new_mag_data) { - NODELET_DEBUG("No updated sensors data"); - return; - } + if (!new_imu_data && !new_baro_data && !new_mag_data) + { + NODELET_DEBUG("No updated sensors data"); + return; + } - if( mZedRealCamModel == sl::MODEL::ZED2 ) { - // Update temperatures for Diagnostic - sens_data.temperature.get( sl::SensorsData::TemperatureData::SENSOR_LOCATION::ONBOARD_LEFT, mTempLeft); - sens_data.temperature.get( sl::SensorsData::TemperatureData::SENSOR_LOCATION::ONBOARD_RIGHT, mTempRight); - } + if (mZedRealCamModel == sl::MODEL::ZED2) + { + // Update temperatures for Diagnostic + sens_data.temperature.get(sl::SensorsData::TemperatureData::SENSOR_LOCATION::ONBOARD_LEFT, mTempLeft); + sens_data.temperature.get(sl::SensorsData::TemperatureData::SENSOR_LOCATION::ONBOARD_RIGHT, mTempRight); + } - if (imu_TempSubNumber>0) { - sensor_msgs::TemperaturePtr imuTempMsg = boost::make_shared(); + if (imu_TempSubNumber > 0) + { + sensor_msgs::TemperaturePtr imuTempMsg = boost::make_shared(); - imuTempMsg->header.stamp = ts_imu; + imuTempMsg->header.stamp = ts_imu; #ifdef DEBUG_SENS_TS - static ros::Time old_ts; - if(old_ts==imuTempMsg->header.stamp) { - NODELET_WARN_STREAM("Publishing IMU data with old timestamp " << old_ts ); - } - old_ts = imuTempMsg->header.stamp; -#endif - - imuTempMsg->header.frame_id = mImuFrameId; - float imu_temp; - sens_data.temperature.get( sl::SensorsData::TemperatureData::SENSOR_LOCATION::IMU, imu_temp); - imuTempMsg->temperature = static_cast(imu_temp); - imuTempMsg->variance = 0.0; - - sensors_data_published = true; - mPubImuTemp.publish(imuTempMsg); - } else { - NODELET_DEBUG("No new IMU temp."); + static ros::Time old_ts; + if (old_ts == imuTempMsg->header.stamp) + { + NODELET_WARN_STREAM("Publishing IMU data with old timestamp " << old_ts); } + old_ts = imuTempMsg->header.stamp; +#endif + imuTempMsg->header.frame_id = mImuFrameId; + float imu_temp; + sens_data.temperature.get(sl::SensorsData::TemperatureData::SENSOR_LOCATION::IMU, imu_temp); + imuTempMsg->temperature = static_cast(imu_temp); + imuTempMsg->variance = 0.0; + + sensors_data_published = true; + mPubImuTemp.publish(imuTempMsg); + } + else + { + NODELET_DEBUG("No new IMU temp."); + } + + if (sens_data.barometer.is_available && new_baro_data) + { + lastTs_baro = ts_baro; + + if (pressSubNumber > 0) + { + sensor_msgs::FluidPressurePtr pressMsg = boost::make_shared(); - if( sens_data.barometer.is_available && new_baro_data ) { - lastTs_baro = ts_baro; - - if( pressSubNumber>0 ) { - sensor_msgs::FluidPressurePtr pressMsg = boost::make_shared(); - - pressMsg->header.stamp = ts_baro; + pressMsg->header.stamp = ts_baro; #ifdef DEBUG_SENS_TS - static ros::Time old_ts; - if(old_ts==pressMsg->header.stamp) { - NODELET_WARN_STREAM("Publishing BARO data with old timestamp " << old_ts ); - } - old_ts = pressMsg->header.stamp; + static ros::Time old_ts; + if (old_ts == pressMsg->header.stamp) + { + NODELET_WARN_STREAM("Publishing BARO data with old timestamp " << old_ts); + } + old_ts = pressMsg->header.stamp; #endif - pressMsg->header.frame_id = mBaroFrameId; - pressMsg->fluid_pressure = sens_data.barometer.pressure * 1e-2; // Pascal - pressMsg->variance = 1.0585e-2; + pressMsg->header.frame_id = mBaroFrameId; + pressMsg->fluid_pressure = sens_data.barometer.pressure * 1e-2; // Pascal + pressMsg->variance = 1.0585e-2; - sensors_data_published = true; - mPubPressure.publish(pressMsg); - } + sensors_data_published = true; + mPubPressure.publish(pressMsg); + } - if( tempLeftSubNumber>0 ) { - sensor_msgs::TemperaturePtr tempLeftMsg = boost::make_shared(); + if (tempLeftSubNumber > 0) + { + sensor_msgs::TemperaturePtr tempLeftMsg = boost::make_shared(); - tempLeftMsg->header.stamp = ts_baro; + tempLeftMsg->header.stamp = ts_baro; #ifdef DEBUG_SENS_TS - static ros::Time old_ts; - if(old_ts==tempLeftMsg->header.stamp) { - NODELET_WARN_STREAM("Publishing BARO data with old timestamp " << old_ts ); - } - old_ts = tempLeftMsg->header.stamp; + static ros::Time old_ts; + if (old_ts == tempLeftMsg->header.stamp) + { + NODELET_WARN_STREAM("Publishing BARO data with old timestamp " << old_ts); + } + old_ts = tempLeftMsg->header.stamp; #endif - tempLeftMsg->header.frame_id = mTempLeftFrameId; - tempLeftMsg->temperature = static_cast(mTempLeft); - tempLeftMsg->variance = 0.0; + tempLeftMsg->header.frame_id = mTempLeftFrameId; + tempLeftMsg->temperature = static_cast(mTempLeft); + tempLeftMsg->variance = 0.0; - sensors_data_published = true; - mPubTempL.publish(tempLeftMsg); - } + sensors_data_published = true; + mPubTempL.publish(tempLeftMsg); + } - if( tempRightSubNumber>0 ) { - sensor_msgs::TemperaturePtr tempRightMsg = boost::make_shared(); + if (tempRightSubNumber > 0) + { + sensor_msgs::TemperaturePtr tempRightMsg = boost::make_shared(); - tempRightMsg->header.stamp = ts_baro; + tempRightMsg->header.stamp = ts_baro; #ifdef DEBUG_SENS_TS - static ros::Time old_ts; - if(old_ts==tempRightMsg->header.stamp) { - NODELET_WARN_STREAM("Publishing BARO data with old timestamp " << old_ts ); - } - old_ts = tempRightMsg->header.stamp; + static ros::Time old_ts; + if (old_ts == tempRightMsg->header.stamp) + { + NODELET_WARN_STREAM("Publishing BARO data with old timestamp " << old_ts); + } + old_ts = tempRightMsg->header.stamp; #endif - tempRightMsg->header.frame_id = mTempRightFrameId; - tempRightMsg->temperature = static_cast(mTempRight); - tempRightMsg->variance = 0.0; + tempRightMsg->header.frame_id = mTempRightFrameId; + tempRightMsg->temperature = static_cast(mTempRight); + tempRightMsg->variance = 0.0; - sensors_data_published = true; - mPubTempR.publish(tempRightMsg); - } - } else { - NODELET_DEBUG("No new BAROM. DATA"); + sensors_data_published = true; + mPubTempR.publish(tempRightMsg); } + } + else + { + NODELET_DEBUG("No new BAROM. DATA"); + } - if( imu_MagSubNumber>0) { - if( sens_data.magnetometer.is_available && new_mag_data ) { - lastT_mag = ts_mag; + if (imu_MagSubNumber > 0) + { + if (sens_data.magnetometer.is_available && new_mag_data) + { + lastT_mag = ts_mag; - sensor_msgs::MagneticFieldPtr magMsg = boost::make_shared(); + sensor_msgs::MagneticFieldPtr magMsg = boost::make_shared(); - magMsg->header.stamp = ts_mag; + magMsg->header.stamp = ts_mag; #ifdef DEBUG_SENS_TS - static ros::Time old_ts; - if(old_ts==magMsg->header.stamp) { - NODELET_WARN_STREAM("Publishing MAG data with old timestamp " << old_ts ); - } - old_ts = magMsg->header.stamp; + static ros::Time old_ts; + if (old_ts == magMsg->header.stamp) + { + NODELET_WARN_STREAM("Publishing MAG data with old timestamp " << old_ts); + } + old_ts = magMsg->header.stamp; #endif - magMsg->header.frame_id = mMagFrameId; - magMsg->magnetic_field.x = sens_data.magnetometer.magnetic_field_calibrated.x*1e-6; // Tesla - magMsg->magnetic_field.y = sens_data.magnetometer.magnetic_field_calibrated.y*1e-6; // Tesla - magMsg->magnetic_field.z = sens_data.magnetometer.magnetic_field_calibrated.z*1e-6; // Tesla - magMsg->magnetic_field_covariance[0] = 0.039e-6; - magMsg->magnetic_field_covariance[1] = 0.0f; - magMsg->magnetic_field_covariance[2] = 0.0f; - magMsg->magnetic_field_covariance[3] = 0.0f; - magMsg->magnetic_field_covariance[4] = 0.037e-6; - magMsg->magnetic_field_covariance[5] = 0.0f; - magMsg->magnetic_field_covariance[6] = 0.0f; - magMsg->magnetic_field_covariance[7] = 0.0f; - magMsg->magnetic_field_covariance[8] = 0.047e-6; - - sensors_data_published = true; - mPubImuMag.publish(magMsg); - } - } else { - NODELET_DEBUG("No new MAG. DATA"); - } - - if( imu_SubNumber > 0 && new_imu_data) { - lastTs_imu = ts_imu; - - sensor_msgs::ImuPtr imuMsg = boost::make_shared(); - - imuMsg->header.stamp = ts_imu; + magMsg->header.frame_id = mMagFrameId; + magMsg->magnetic_field.x = sens_data.magnetometer.magnetic_field_calibrated.x * 1e-6; // Tesla + magMsg->magnetic_field.y = sens_data.magnetometer.magnetic_field_calibrated.y * 1e-6; // Tesla + magMsg->magnetic_field.z = sens_data.magnetometer.magnetic_field_calibrated.z * 1e-6; // Tesla + magMsg->magnetic_field_covariance[0] = 0.039e-6; + magMsg->magnetic_field_covariance[1] = 0.0f; + magMsg->magnetic_field_covariance[2] = 0.0f; + magMsg->magnetic_field_covariance[3] = 0.0f; + magMsg->magnetic_field_covariance[4] = 0.037e-6; + magMsg->magnetic_field_covariance[5] = 0.0f; + magMsg->magnetic_field_covariance[6] = 0.0f; + magMsg->magnetic_field_covariance[7] = 0.0f; + magMsg->magnetic_field_covariance[8] = 0.047e-6; + + sensors_data_published = true; + mPubImuMag.publish(magMsg); + } + } + else + { + NODELET_DEBUG("No new MAG. DATA"); + } + + if (imu_SubNumber > 0 && new_imu_data) + { + lastTs_imu = ts_imu; + + sensor_msgs::ImuPtr imuMsg = boost::make_shared(); + + imuMsg->header.stamp = ts_imu; #ifdef DEBUG_SENS_TS - static ros::Time old_ts; - if(old_ts==imuMsg->header.stamp) { - NODELET_WARN_STREAM("Publishing IMU data with old timestamp " << old_ts ); - } else { - NODELET_INFO_STREAM("Publishing IMU data with new timestamp. Freq: " << 1./(ts_imu.toSec()-old_ts.toSec()) ); - old_ts = imuMsg->header.stamp; - - } + static ros::Time old_ts; + if (old_ts == imuMsg->header.stamp) + { + NODELET_WARN_STREAM("Publishing IMU data with old timestamp " << old_ts); + } + else + { + NODELET_INFO_STREAM("Publishing IMU data with new timestamp. Freq: " << 1. / (ts_imu.toSec() - old_ts.toSec())); + old_ts = imuMsg->header.stamp; + } #endif - imuMsg->header.frame_id = mImuFrameId; - - imuMsg->orientation.x = sens_data.imu.pose.getOrientation()[0]; - imuMsg->orientation.y = sens_data.imu.pose.getOrientation()[1]; - imuMsg->orientation.z = sens_data.imu.pose.getOrientation()[2]; - imuMsg->orientation.w = sens_data.imu.pose.getOrientation()[3]; - - imuMsg->angular_velocity.x = sens_data.imu.angular_velocity[0] * DEG2RAD; - imuMsg->angular_velocity.y = sens_data.imu.angular_velocity[1] * DEG2RAD; - imuMsg->angular_velocity.z = sens_data.imu.angular_velocity[2] * DEG2RAD; + imuMsg->header.frame_id = mImuFrameId; - imuMsg->linear_acceleration.x = sens_data.imu.linear_acceleration[0]; - imuMsg->linear_acceleration.y = sens_data.imu.linear_acceleration[1]; - imuMsg->linear_acceleration.z = sens_data.imu.linear_acceleration[2]; + imuMsg->orientation.x = sens_data.imu.pose.getOrientation()[0]; + imuMsg->orientation.y = sens_data.imu.pose.getOrientation()[1]; + imuMsg->orientation.z = sens_data.imu.pose.getOrientation()[2]; + imuMsg->orientation.w = sens_data.imu.pose.getOrientation()[3]; - for (int i = 0; i < 3; ++i) { + imuMsg->angular_velocity.x = sens_data.imu.angular_velocity[0] * DEG2RAD; + imuMsg->angular_velocity.y = sens_data.imu.angular_velocity[1] * DEG2RAD; + imuMsg->angular_velocity.z = sens_data.imu.angular_velocity[2] * DEG2RAD; - int r = 0; - - if (i == 0) { - r = 0; - } else if (i == 1) { - r = 1; - } else { - r = 2; - } + imuMsg->linear_acceleration.x = sens_data.imu.linear_acceleration[0]; + imuMsg->linear_acceleration.y = sens_data.imu.linear_acceleration[1]; + imuMsg->linear_acceleration.z = sens_data.imu.linear_acceleration[2]; - imuMsg->orientation_covariance[i * 3 + 0] = - sens_data.imu.pose_covariance.r[r * 3 + 0] * DEG2RAD * DEG2RAD; - imuMsg->orientation_covariance[i * 3 + 1] = - sens_data.imu.pose_covariance.r[r * 3 + 1] * DEG2RAD * DEG2RAD; - imuMsg->orientation_covariance[i * 3 + 2] = - sens_data.imu.pose_covariance.r[r * 3 + 2] * DEG2RAD * DEG2RAD; - - imuMsg->linear_acceleration_covariance[i * 3 + 0] = - sens_data.imu.linear_acceleration_covariance.r[r * 3 + 0]; - imuMsg->linear_acceleration_covariance[i * 3 + 1] = - sens_data.imu.linear_acceleration_covariance.r[r * 3 + 1]; - imuMsg->linear_acceleration_covariance[i * 3 + 2] = - sens_data.imu.linear_acceleration_covariance.r[r * 3 + 2]; - - imuMsg->angular_velocity_covariance[i * 3 + 0] = - sens_data.imu.angular_velocity_covariance.r[r * 3 + 0] * DEG2RAD * DEG2RAD; - imuMsg->angular_velocity_covariance[i * 3 + 1] = - sens_data.imu.angular_velocity_covariance.r[r * 3 + 1] * DEG2RAD * DEG2RAD; - imuMsg->angular_velocity_covariance[i * 3 + 2] = - sens_data.imu.angular_velocity_covariance.r[r * 3 + 2] * DEG2RAD * DEG2RAD; - } - - sensors_data_published = true; - mPubImu.publish(imuMsg); - } else { - NODELET_DEBUG("No new IMU DATA"); - } - - if (imu_RawSubNumber > 0 && new_imu_data) { - sensor_msgs::ImuPtr imuRawMsg = boost::make_shared(); - - imuRawMsg->header.stamp = ts_imu; - imuRawMsg->header.frame_id = mImuFrameId; - imuRawMsg->angular_velocity.x = sens_data.imu.angular_velocity[0] * DEG2RAD; - imuRawMsg->angular_velocity.y = sens_data.imu.angular_velocity[1] * DEG2RAD; - imuRawMsg->angular_velocity.z = sens_data.imu.angular_velocity[2] * DEG2RAD; - imuRawMsg->linear_acceleration.x = sens_data.imu.linear_acceleration[0]; - imuRawMsg->linear_acceleration.y = sens_data.imu.linear_acceleration[1]; - imuRawMsg->linear_acceleration.z = sens_data.imu.linear_acceleration[2]; - - for (int i = 0; i < 3; ++i) { + for (int i = 0; i < 3; ++i) + { + int r = 0; + + if (i == 0) + { + r = 0; + } + else if (i == 1) + { + r = 1; + } + else + { + r = 2; + } + + imuMsg->orientation_covariance[i * 3 + 0] = sens_data.imu.pose_covariance.r[r * 3 + 0] * DEG2RAD * DEG2RAD; + imuMsg->orientation_covariance[i * 3 + 1] = sens_data.imu.pose_covariance.r[r * 3 + 1] * DEG2RAD * DEG2RAD; + imuMsg->orientation_covariance[i * 3 + 2] = sens_data.imu.pose_covariance.r[r * 3 + 2] * DEG2RAD * DEG2RAD; + + imuMsg->linear_acceleration_covariance[i * 3 + 0] = sens_data.imu.linear_acceleration_covariance.r[r * 3 + 0]; + imuMsg->linear_acceleration_covariance[i * 3 + 1] = sens_data.imu.linear_acceleration_covariance.r[r * 3 + 1]; + imuMsg->linear_acceleration_covariance[i * 3 + 2] = sens_data.imu.linear_acceleration_covariance.r[r * 3 + 2]; + + imuMsg->angular_velocity_covariance[i * 3 + 0] = + sens_data.imu.angular_velocity_covariance.r[r * 3 + 0] * DEG2RAD * DEG2RAD; + imuMsg->angular_velocity_covariance[i * 3 + 1] = + sens_data.imu.angular_velocity_covariance.r[r * 3 + 1] * DEG2RAD * DEG2RAD; + imuMsg->angular_velocity_covariance[i * 3 + 2] = + sens_data.imu.angular_velocity_covariance.r[r * 3 + 2] * DEG2RAD * DEG2RAD; + } + + sensors_data_published = true; + mPubImu.publish(imuMsg); + } + else + { + NODELET_DEBUG("No new IMU DATA"); + } + + if (imu_RawSubNumber > 0 && new_imu_data) + { + sensor_msgs::ImuPtr imuRawMsg = boost::make_shared(); + + imuRawMsg->header.stamp = ts_imu; + imuRawMsg->header.frame_id = mImuFrameId; + imuRawMsg->angular_velocity.x = sens_data.imu.angular_velocity[0] * DEG2RAD; + imuRawMsg->angular_velocity.y = sens_data.imu.angular_velocity[1] * DEG2RAD; + imuRawMsg->angular_velocity.z = sens_data.imu.angular_velocity[2] * DEG2RAD; + imuRawMsg->linear_acceleration.x = sens_data.imu.linear_acceleration[0]; + imuRawMsg->linear_acceleration.y = sens_data.imu.linear_acceleration[1]; + imuRawMsg->linear_acceleration.z = sens_data.imu.linear_acceleration[2]; + + for (int i = 0; i < 3; ++i) + { + int r = 0; + + if (i == 0) + { + r = 0; + } + else if (i == 1) + { + r = 1; + } + else + { + r = 2; + } + + imuRawMsg->linear_acceleration_covariance[i * 3 + 0] = sens_data.imu.linear_acceleration_covariance.r[r * 3 + 0]; + imuRawMsg->linear_acceleration_covariance[i * 3 + 1] = sens_data.imu.linear_acceleration_covariance.r[r * 3 + 1]; + imuRawMsg->linear_acceleration_covariance[i * 3 + 2] = sens_data.imu.linear_acceleration_covariance.r[r * 3 + 2]; + imuRawMsg->angular_velocity_covariance[i * 3 + 0] = + sens_data.imu.angular_velocity_covariance.r[r * 3 + 0] * DEG2RAD * DEG2RAD; + imuRawMsg->angular_velocity_covariance[i * 3 + 1] = + sens_data.imu.angular_velocity_covariance.r[r * 3 + 1] * DEG2RAD * DEG2RAD; + imuRawMsg->angular_velocity_covariance[i * 3 + 2] = + sens_data.imu.angular_velocity_covariance.r[r * 3 + 2] * DEG2RAD * DEG2RAD; + } + + // Orientation data is not available in "data_raw" -> See ROS REP145 + // http://www.ros.org/reps/rep-0145.html#topics + imuRawMsg->orientation_covariance[0] = -1; + sensors_data_published = true; + mPubImuRaw.publish(imuRawMsg); + } + + // ----> Update Diagnostic + if (sensors_data_published) + { + // Publish freq calculation + static std::chrono::steady_clock::time_point last_time = std::chrono::steady_clock::now(); + std::chrono::steady_clock::time_point now = std::chrono::steady_clock::now(); - int r = 0; + double elapsed_usec = std::chrono::duration_cast(now - last_time).count(); + last_time = now; - if (i == 0) { - r = 0; - } else if (i == 1) { - r = 1; - } else { - r = 2; - } + mSensPeriodMean_usec->addValue(elapsed_usec); + } + // <---- Update Diagnostic +} - imuRawMsg->linear_acceleration_covariance[i * 3 + 0] = - sens_data.imu.linear_acceleration_covariance.r[r * 3 + 0]; - imuRawMsg->linear_acceleration_covariance[i * 3 + 1] = - sens_data.imu.linear_acceleration_covariance.r[r * 3 + 1]; - imuRawMsg->linear_acceleration_covariance[i * 3 + 2] = - sens_data.imu.linear_acceleration_covariance.r[r * 3 + 2]; - imuRawMsg->angular_velocity_covariance[i * 3 + 0] = - sens_data.imu.angular_velocity_covariance.r[r * 3 + 0] * DEG2RAD * DEG2RAD; - imuRawMsg->angular_velocity_covariance[i * 3 + 1] = - sens_data.imu.angular_velocity_covariance.r[r * 3 + 1] * DEG2RAD * DEG2RAD; - imuRawMsg->angular_velocity_covariance[i * 3 + 2] = - sens_data.imu.angular_velocity_covariance.r[r * 3 + 2] * DEG2RAD * DEG2RAD; - } +void ZEDWrapperNodelet::device_poll_thread_func() +{ + ros::Rate loop_rate(mCamFrameRate); + + mRecording = false; + + mElabPeriodMean_sec.reset(new sl_tools::CSmartMean(mCamFrameRate)); + mGrabPeriodMean_usec.reset(new sl_tools::CSmartMean(mCamFrameRate)); + mVideoDepthPeriodMean_sec.reset(new sl_tools::CSmartMean(mCamFrameRate)); + mPcPeriodMean_usec.reset(new sl_tools::CSmartMean(mCamFrameRate)); + mObjDetPeriodMean_msec.reset(new sl_tools::CSmartMean(mCamFrameRate)); + + // Timestamp initialization + if (mSvoMode) + { + mFrameTimestamp = ros::Time::now(); + } + else + { + mFrameTimestamp = sl_tools::slTime2Ros(mZed.getTimestamp(sl::TIME_REFERENCE::CURRENT)); + } + + mPrevFrameTimestamp = mFrameTimestamp; + + mTrackingActivated = false; + mMappingRunning = false; + mRecording = false; + + // Get the parameters of the ZED images +#if ZED_SDK_MAJOR_VERSION == 3 && ZED_SDK_MINOR_VERSION < 1 + mCamWidth = mZed.getCameraInformation().camera_resolution.width; + mCamHeight = mZed.getCameraInformation().camera_resolution.height; +#else + mCamWidth = mZed.getCameraInformation().camera_configuration.resolution.width; + mCamHeight = mZed.getCameraInformation().camera_configuration.resolution.height; +#endif + NODELET_DEBUG_STREAM("Camera Frame size : " << mCamWidth << "x" << mCamHeight); + int v_w = static_cast(mCamWidth * mCamImageResizeFactor); + int v_h = static_cast(mCamHeight * mCamImageResizeFactor); + mMatResolVideo = sl::Resolution(v_w, v_h); + NODELET_DEBUG_STREAM("Image Mat size : " << mMatResolVideo.width << "x" << mMatResolVideo.height); + int d_w = static_cast(mCamWidth * mCamDepthResizeFactor); + int d_h = static_cast(mCamHeight * mCamDepthResizeFactor); + mMatResolDepth = sl::Resolution(d_w, d_h); + NODELET_DEBUG_STREAM("Depth Mat size : " << mMatResolDepth.width << "x" << mMatResolDepth.height); + + // Create and fill the camera information messages + fillCamInfo(mZed, mLeftCamInfoMsg, mRightCamInfoMsg, mLeftCamOptFrameId, mRightCamOptFrameId); + fillCamInfo(mZed, mLeftCamInfoRawMsg, mRightCamInfoRawMsg, mLeftCamOptFrameId, mRightCamOptFrameId, true); + fillCamDepthInfo(mZed, mDepthCamInfoMsg, mLeftCamOptFrameId); + + // the reference camera is the Left one (next to the ZED logo) + + mRgbCamInfoMsg = mLeftCamInfoMsg; + mRgbCamInfoRawMsg = mLeftCamInfoRawMsg; + + sl::RuntimeParameters runParams; + runParams.sensing_mode = static_cast(mCamSensingMode); + + // Main loop + while (mNhNs.ok()) + { + // Check for subscribers + uint32_t rgbSubnumber = mPubRgb.getNumSubscribers(); + uint32_t rgbRawSubnumber = mPubRawRgb.getNumSubscribers(); + uint32_t leftSubnumber = mPubLeft.getNumSubscribers(); + uint32_t leftRawSubnumber = mPubRawLeft.getNumSubscribers(); + uint32_t rightSubnumber = mPubRight.getNumSubscribers(); + uint32_t rightRawSubnumber = mPubRawRight.getNumSubscribers(); + uint32_t rgbGraySubnumber = mPubRgbGray.getNumSubscribers(); + uint32_t rgbGrayRawSubnumber = mPubRawRgbGray.getNumSubscribers(); + uint32_t leftGraySubnumber = mPubLeftGray.getNumSubscribers(); + uint32_t leftGrayRawSubnumber = mPubRawLeftGray.getNumSubscribers(); + uint32_t rightGraySubnumber = mPubRightGray.getNumSubscribers(); + uint32_t rightGrayRawSubnumber = mPubRawRightGray.getNumSubscribers(); + uint32_t depthSubnumber = mPubDepth.getNumSubscribers(); + uint32_t disparitySubnumber = mPubDisparity.getNumSubscribers(); + uint32_t cloudSubnumber = mPubCloud.getNumSubscribers(); + uint32_t fusedCloudSubnumber = mPubFusedCloud.getNumSubscribers(); + uint32_t poseSubnumber = mPubPose.getNumSubscribers(); + uint32_t poseCovSubnumber = mPubPoseCov.getNumSubscribers(); + uint32_t odomSubnumber = mPubOdom.getNumSubscribers(); + uint32_t confMapSubnumber = mPubConfMap.getNumSubscribers(); + uint32_t pathSubNumber = mPubMapPath.getNumSubscribers() + mPubOdomPath.getNumSubscribers(); + uint32_t stereoSubNumber = mPubStereo.getNumSubscribers(); + uint32_t stereoRawSubNumber = mPubRawStereo.getNumSubscribers(); - // Orientation data is not available in "data_raw" -> See ROS REP145 - // http://www.ros.org/reps/rep-0145.html#topics - imuRawMsg->orientation_covariance[0] = - -1; - sensors_data_published = true; - mPubImuRaw.publish(imuRawMsg); + uint32_t objDetSubnumber = 0; + if (mObjDetEnabled && mObjDetRunning) + { + objDetSubnumber = mPubObjDet.getNumSubscribers(); } - // ----> Update Diagnostic - if( sensors_data_published ) { - // Publish freq calculation - static std::chrono::steady_clock::time_point last_time = std::chrono::steady_clock::now(); - std::chrono::steady_clock::time_point now = std::chrono::steady_clock::now(); - - double elapsed_usec = std::chrono::duration_cast(now - last_time).count(); - last_time = now; - - mSensPeriodMean_usec->addValue(elapsed_usec); - } - // <---- Update Diagnostic -} + mGrabActive = + mRecording || mStreaming || mMappingEnabled || mObjDetEnabled || mTrackingActivated || + ((rgbSubnumber + rgbRawSubnumber + leftSubnumber + leftRawSubnumber + rightSubnumber + rightRawSubnumber + + rgbGraySubnumber + rgbGrayRawSubnumber + leftGraySubnumber + leftGrayRawSubnumber + rightGraySubnumber + + rightGrayRawSubnumber + depthSubnumber + disparitySubnumber + cloudSubnumber + poseSubnumber + + poseCovSubnumber + odomSubnumber + confMapSubnumber /*+ imuSubnumber + imuRawsubnumber*/ + pathSubNumber + + stereoSubNumber + stereoRawSubNumber + objDetSubnumber) > 0); -void ZEDWrapperNodelet::device_poll_thread_func() { - ros::Rate loop_rate(mCamFrameRate); + // Run the loop only if there is some subscribers or SVO is active + if (mGrabActive) + { + std::lock_guard lock(mPosTrkMutex); + + // Note: ones tracking is started it is never stopped anymore to not lose tracking information + bool computeTracking = (mMappingEnabled || mObjDetEnabled || (mComputeDepth & mDepthStabilization) || + poseSubnumber > 0 || poseCovSubnumber > 0 || odomSubnumber > 0 || pathSubNumber > 0); + + // Start the tracking? + if ((computeTracking) && !mTrackingActivated && (mDepthMode != sl::DEPTH_MODE::NONE)) + { + start_pos_tracking(); + } + + // Start the mapping? + mMappingMutex.lock(); + if (mMappingEnabled && !mMappingRunning) + { + start_3d_mapping(); + } + mMappingMutex.unlock(); + + // Start the object detection? + mObjDetMutex.lock(); + if (mObjDetEnabled && !mObjDetRunning) + { + start_obj_detect(); + } + mObjDetMutex.unlock(); + + // Detect if one of the subscriber need to have the depth information + mComputeDepth = mDepthMode != sl::DEPTH_MODE::NONE && + ((depthSubnumber + disparitySubnumber + cloudSubnumber + fusedCloudSubnumber + poseSubnumber + + poseCovSubnumber + odomSubnumber + confMapSubnumber + objDetSubnumber) > 0); + + if (mComputeDepth) + { + runParams.confidence_threshold = mCamDepthConfidence; +#if ZED_SDK_MAJOR_VERSION == 3 && ZED_SDK_MINOR_VERSION < 2 + runParams.textureness_confidence_threshold = mCamDepthTextureConf; +#else + runParams.texture_confidence_threshold = mCamDepthTextureConf; +#endif + runParams.enable_depth = true; // Ask to compute the depth + } + else + { + runParams.enable_depth = false; // Ask to not compute the depth + } - mRecording = false; + mCamDataMutex.lock(); + mRgbDepthDataRetrieved = false; + mGrabStatus = mZed.grab(runParams); + mCamDataMutex.unlock(); - mElabPeriodMean_sec.reset(new sl_tools::CSmartMean(mCamFrameRate)); - mGrabPeriodMean_usec.reset(new sl_tools::CSmartMean(mCamFrameRate)); - mVideoDepthPeriodMean_sec.reset(new sl_tools::CSmartMean(mCamFrameRate)); - mPcPeriodMean_usec.reset(new sl_tools::CSmartMean(mCamFrameRate)); - mObjDetPeriodMean_msec.reset(new sl_tools::CSmartMean(mCamFrameRate)); + std::chrono::steady_clock::time_point start_elab = std::chrono::steady_clock::now(); - // Timestamp initialization - if (mSvoMode) { - mFrameTimestamp = ros::Time::now(); - } else { - mFrameTimestamp = sl_tools::slTime2Ros(mZed.getTimestamp(sl::TIME_REFERENCE::CURRENT)); - } + // cout << toString(grab_status) << endl; + if (mGrabStatus != sl::ERROR_CODE::SUCCESS) + { + // Detect if a error occurred (for example: + // the zed have been disconnected) and + // re-initialize the ZED - mPrevFrameTimestamp = mFrameTimestamp; + NODELET_INFO_STREAM_ONCE(toString(mGrabStatus)); - mTrackingActivated = false; - mMappingRunning = false; - mRecording = false; + std::this_thread::sleep_for(std::chrono::milliseconds(1)); - // Get the parameters of the ZED images -#if ZED_SDK_MAJOR_VERSION==3 && ZED_SDK_MINOR_VERSION<1 - mCamWidth = mZed.getCameraInformation().camera_resolution.width; - mCamHeight = mZed.getCameraInformation().camera_resolution.height; -#else - mCamWidth = mZed.getCameraInformation().camera_configuration.resolution.width; - mCamHeight = mZed.getCameraInformation().camera_configuration.resolution.height; -#endif - NODELET_DEBUG_STREAM("Camera Frame size : " << mCamWidth << "x" << mCamHeight); - int v_w = static_cast(mCamWidth * mCamImageResizeFactor); - int v_h = static_cast(mCamHeight * mCamImageResizeFactor); - mMatResolVideo = sl::Resolution(v_w,v_h); - NODELET_DEBUG_STREAM("Image Mat size : " << mMatResolVideo.width << "x" << mMatResolVideo.height); - int d_w = static_cast(mCamWidth * mCamDepthResizeFactor); - int d_h = static_cast(mCamHeight * mCamDepthResizeFactor); - mMatResolDepth = sl::Resolution(d_w,d_h); - NODELET_DEBUG_STREAM("Depth Mat size : " << mMatResolDepth.width << "x" << mMatResolDepth.height); - - // Create and fill the camera information messages - fillCamInfo(mZed, mLeftCamInfoMsg, mRightCamInfoMsg, mLeftCamOptFrameId, mRightCamOptFrameId); - fillCamInfo(mZed, mLeftCamInfoRawMsg, mRightCamInfoRawMsg, mLeftCamOptFrameId, mRightCamOptFrameId, true); - fillCamDepthInfo(mZed,mDepthCamInfoMsg,mLeftCamOptFrameId); - - // the reference camera is the Left one (next to the ZED logo) - - mRgbCamInfoMsg = mLeftCamInfoMsg; - mRgbCamInfoRawMsg = mLeftCamInfoRawMsg; - - sl::RuntimeParameters runParams; - runParams.sensing_mode = static_cast(mCamSensingMode); - - // Main loop - while (mNhNs.ok()) { - // Check for subscribers - uint32_t rgbSubnumber = mPubRgb.getNumSubscribers(); - uint32_t rgbRawSubnumber = mPubRawRgb.getNumSubscribers(); - uint32_t leftSubnumber = mPubLeft.getNumSubscribers(); - uint32_t leftRawSubnumber = mPubRawLeft.getNumSubscribers(); - uint32_t rightSubnumber = mPubRight.getNumSubscribers(); - uint32_t rightRawSubnumber = mPubRawRight.getNumSubscribers(); - uint32_t rgbGraySubnumber = mPubRgbGray.getNumSubscribers(); - uint32_t rgbGrayRawSubnumber = mPubRawRgbGray.getNumSubscribers(); - uint32_t leftGraySubnumber = mPubLeftGray.getNumSubscribers(); - uint32_t leftGrayRawSubnumber = mPubRawLeftGray.getNumSubscribers(); - uint32_t rightGraySubnumber = mPubRightGray.getNumSubscribers(); - uint32_t rightGrayRawSubnumber = mPubRawRightGray.getNumSubscribers(); - uint32_t depthSubnumber = mPubDepth.getNumSubscribers(); - uint32_t disparitySubnumber = mPubDisparity.getNumSubscribers(); - uint32_t cloudSubnumber = mPubCloud.getNumSubscribers(); - uint32_t fusedCloudSubnumber = mPubFusedCloud.getNumSubscribers(); - uint32_t poseSubnumber = mPubPose.getNumSubscribers(); - uint32_t poseCovSubnumber = mPubPoseCov.getNumSubscribers(); - uint32_t odomSubnumber = mPubOdom.getNumSubscribers(); - uint32_t confMapSubnumber = mPubConfMap.getNumSubscribers(); - uint32_t pathSubNumber = mPubMapPath.getNumSubscribers() + mPubOdomPath.getNumSubscribers(); - uint32_t stereoSubNumber = mPubStereo.getNumSubscribers(); - uint32_t stereoRawSubNumber = mPubRawStereo.getNumSubscribers(); - - uint32_t objDetSubnumber = 0; - uint32_t objDetVizSubnumber = 0; - bool objDetActive = false; - if (mObjDetEnabled) { - objDetSubnumber = mPubObjDet.getNumSubscribers(); - objDetVizSubnumber = mPubObjDetViz.getNumSubscribers(); - if (objDetSubnumber > 0 || objDetVizSubnumber > 0) { - objDetActive = true; + if ((ros::Time::now() - mPrevFrameTimestamp).toSec() > 5 && !mSvoMode) + { + mCloseZedMutex.lock(); + mZed.close(); + mCloseZedMutex.unlock(); + + mConnStatus = sl::ERROR_CODE::CAMERA_NOT_DETECTED; + + while (mConnStatus != sl::ERROR_CODE::SUCCESS) + { + if (!mNhNs.ok()) + { + mStopNode = true; + + std::lock_guard lock(mCloseZedMutex); + NODELET_DEBUG("Closing ZED"); + if (mRecording) + { + mRecording = false; + mZed.disableRecording(); + } + mZed.close(); + + NODELET_DEBUG("ZED pool thread finished"); + return; } - } - mGrabActive = mRecording || mStreaming || mMappingEnabled || mObjDetEnabled || mTrackingActivated || - ((rgbSubnumber + rgbRawSubnumber + leftSubnumber + - leftRawSubnumber + rightSubnumber + rightRawSubnumber + - rgbGraySubnumber + rgbGrayRawSubnumber + leftGraySubnumber + - leftGrayRawSubnumber + rightGraySubnumber + rightGrayRawSubnumber + - depthSubnumber + disparitySubnumber + cloudSubnumber + - poseSubnumber + poseCovSubnumber + odomSubnumber + - confMapSubnumber /*+ imuSubnumber + imuRawsubnumber*/ + pathSubNumber + - stereoSubNumber + stereoRawSubNumber) > 0); - - // Run the loop only if there is some subscribers or SVO is active - if (mGrabActive) { - std::lock_guard lock(mPosTrkMutex); - - // Note: ones tracking is started it is never stopped anymore to not lose tracking information - bool computeTracking = (mMappingEnabled || mObjDetEnabled || (mComputeDepth & mDepthStabilization) || poseSubnumber > 0 || - poseCovSubnumber > 0 || odomSubnumber > 0 || pathSubNumber > 0); - - // Start the tracking? - if ((computeTracking) && !mTrackingActivated && (mDepthMode != sl::DEPTH_MODE::NONE)) { - start_pos_tracking(); - } + int id = sl_tools::checkCameraReady(mZedSerialNumber); - // Start the mapping? - mMappingMutex.lock(); - if (mMappingEnabled && !mMappingRunning) { - start_3d_mapping(); + if (id >= 0) + { + mZedParams.input.setFromCameraID(id); + mConnStatus = mZed.open(mZedParams); // Try to initialize the ZED + NODELET_INFO_STREAM(toString(mConnStatus)); } - mMappingMutex.unlock(); - - // Start the object detection? - mObjDetMutex.lock(); - if (mObjDetEnabled && !mObjDetRunning) { - start_obj_detect(); + else + { + NODELET_INFO_STREAM("Waiting for the ZED (S/N " << mZedSerialNumber << ") to be re-connected"); } - mObjDetMutex.unlock(); - // Detect if one of the subscriber need to have the depth information - mComputeDepth = mDepthMode != sl::DEPTH_MODE::NONE && - ((depthSubnumber + disparitySubnumber + cloudSubnumber + fusedCloudSubnumber + - poseSubnumber + poseCovSubnumber + odomSubnumber + confMapSubnumber) > 0); + mDiagUpdater.force_update(); + std::this_thread::sleep_for(std::chrono::milliseconds(2000)); + } - if (mComputeDepth) { - runParams.confidence_threshold = mCamDepthConfidence; -#if ZED_SDK_MAJOR_VERSION==3 && ZED_SDK_MINOR_VERSION<2 - runParams.textureness_confidence_threshold = mCamDepthTextureConf; -#else - runParams.texture_confidence_threshold = mCamDepthTextureConf; -#endif - runParams.enable_depth = true; // Ask to compute the depth - } else { - runParams.enable_depth = false; // Ask to not compute the depth - } + mTrackingActivated = false; - // ----> Wait for RGB/Depth synchronization before grabbing - // std::unique_lock datalock(mCamDataMutex); - // while (!mRgbDepthDataRetrieved) { // loop to avoid spurious wakeups - // if (mRgbDepthDataRetrievedCondVar.wait_for(datalock, std::chrono::milliseconds(500)) == std::cv_status::timeout) { - // // Check thread stopping - // if (mStopNode) { - // return; - // } else { - // continue; - // } - // } - // } - // <---- Wait for RGB/Depth synchronization before grabbing - - mCamDataMutex.lock(); - mRgbDepthDataRetrieved = false; - mGrabStatus = mZed.grab(runParams); - mCamDataMutex.unlock(); - - std::chrono::steady_clock::time_point start_elab = std::chrono::steady_clock::now(); - - // cout << toString(grab_status) << endl; - if (mGrabStatus != sl::ERROR_CODE::SUCCESS) { - // Detect if a error occurred (for example: - // the zed have been disconnected) and - // re-initialize the ZED - - NODELET_INFO_STREAM_ONCE(toString(mGrabStatus)); - - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - - if ((ros::Time::now() - mPrevFrameTimestamp).toSec() > 5 && !mSvoMode) { - mCloseZedMutex.lock(); - mZed.close(); - mCloseZedMutex.unlock(); - - mConnStatus = sl::ERROR_CODE::CAMERA_NOT_DETECTED; - - while (mConnStatus != sl::ERROR_CODE::SUCCESS) { - if (!mNhNs.ok()) { - mStopNode = true; - - std::lock_guard lock(mCloseZedMutex); - NODELET_DEBUG("Closing ZED"); - if(mRecording) { - mRecording=false; - mZed.disableRecording(); - } - mZed.close(); - - NODELET_DEBUG("ZED pool thread finished"); - return; - } - - int id = sl_tools::checkCameraReady(mZedSerialNumber); - - if (id >= 0) { - mZedParams.input.setFromCameraID(id); - mConnStatus = mZed.open(mZedParams); // Try to initialize the ZED - NODELET_INFO_STREAM(toString(mConnStatus)); - } else { - NODELET_INFO_STREAM("Waiting for the ZED (S/N " << mZedSerialNumber << ") to be re-connected"); - } - - mDiagUpdater.force_update(); - std::this_thread::sleep_for(std::chrono::milliseconds(2000)); - } - - mTrackingActivated = false; - - computeTracking = mDepthStabilization || poseSubnumber > 0 || poseCovSubnumber > 0 || - odomSubnumber > 0; - - if (computeTracking) { // Start the tracking - start_pos_tracking(); - } - } - - mDiagUpdater.update(); - - continue; - } + computeTracking = mDepthStabilization || poseSubnumber > 0 || poseCovSubnumber > 0 || odomSubnumber > 0; - mFrameCount++; + if (computeTracking) + { // Start the tracking + start_pos_tracking(); + } + } - // SVO recording - mRecMutex.lock(); + mDiagUpdater.update(); - if (mRecording) { - mRecStatus = mZed.getRecordingStatus(); + continue; + } - if (!mRecStatus.status) { - NODELET_ERROR_THROTTLE(1.0, "Error saving frame to SVO"); - } + mFrameCount++; - mDiagUpdater.force_update(); - } + // SVO recording + mRecMutex.lock(); - mRecMutex.unlock(); + if (mRecording) + { + mRecStatus = mZed.getRecordingStatus(); - // Timestamp - mPrevFrameTimestamp = mFrameTimestamp; + if (!mRecStatus.status) + { + NODELET_ERROR_THROTTLE(1.0, "Error saving frame to SVO"); + } - // Publish freq calculation - static std::chrono::steady_clock::time_point last_time = std::chrono::steady_clock::now(); - std::chrono::steady_clock::time_point now = std::chrono::steady_clock::now(); + mDiagUpdater.force_update(); + } - double elapsed_usec = std::chrono::duration_cast(now - last_time).count(); - last_time = now; + mRecMutex.unlock(); - mGrabPeriodMean_usec->addValue(elapsed_usec); + // Timestamp + mPrevFrameTimestamp = mFrameTimestamp; - // NODELET_INFO_STREAM("Grab time: " << elapsed_usec / 1000 << " msec"); + // Publish freq calculation + static std::chrono::steady_clock::time_point last_time = std::chrono::steady_clock::now(); + std::chrono::steady_clock::time_point now = std::chrono::steady_clock::now(); - // Timestamp - if (mSvoMode) { - mFrameTimestamp = ros::Time::now(); - } else { - mFrameTimestamp = sl_tools::slTime2Ros(mZed.getTimestamp(sl::TIME_REFERENCE::IMAGE)); - } + double elapsed_usec = std::chrono::duration_cast(now - last_time).count(); + last_time = now; - ros::Time stamp = mFrameTimestamp; // Timestamp - - // ----> Camera Settings - if( !mSvoMode && mFrameCount%5 == 0 ) { - //NODELET_DEBUG_STREAM( "[" << mFrameCount << "] device_poll_thread_func MUTEX LOCK"); - mDynParMutex.lock(); - - int brightness = mZed.getCameraSettings(sl::VIDEO_SETTINGS::BRIGHTNESS); - if( brightness != mCamBrightness ) { - mZed.setCameraSettings(sl::VIDEO_SETTINGS::BRIGHTNESS, mCamBrightness); - NODELET_DEBUG_STREAM( "mCamBrightness changed: " << mCamBrightness << " <- " << brightness); - mUpdateDynParams = true; - } - - int contrast = mZed.getCameraSettings(sl::VIDEO_SETTINGS::CONTRAST); - if( contrast != mCamContrast ) { - mZed.setCameraSettings(sl::VIDEO_SETTINGS::CONTRAST, mCamContrast); - NODELET_DEBUG_STREAM( "mCamContrast changed: " << mCamContrast << " <- " << contrast); - mUpdateDynParams = true; - } - - int hue = mZed.getCameraSettings(sl::VIDEO_SETTINGS::HUE); - if( hue != mCamHue ) { - mZed.setCameraSettings(sl::VIDEO_SETTINGS::HUE, mCamHue); - NODELET_DEBUG_STREAM( "mCamHue changed: " << mCamHue << " <- " << hue); - mUpdateDynParams = true; - } - - int saturation = mZed.getCameraSettings(sl::VIDEO_SETTINGS::SATURATION); - if( saturation != mCamSaturation ) { - mZed.setCameraSettings(sl::VIDEO_SETTINGS::SATURATION, mCamSaturation); - NODELET_DEBUG_STREAM( "mCamSaturation changed: " << mCamSaturation << " <- " << saturation); - mUpdateDynParams = true; - } - - int sharpness = mZed.getCameraSettings(sl::VIDEO_SETTINGS::SHARPNESS); - if( sharpness != mCamSharpness ) { - mZed.setCameraSettings(sl::VIDEO_SETTINGS::SHARPNESS, mCamSharpness); - NODELET_DEBUG_STREAM( "mCamSharpness changed: " << mCamSharpness << " <- " << sharpness); - mUpdateDynParams = true; - } - -#if (ZED_SDK_MAJOR_VERSION==3 && ZED_SDK_MINOR_VERSION>=1) - int gamma = mZed.getCameraSettings(sl::VIDEO_SETTINGS::GAMMA); - if( gamma != mCamGamma ) { - mZed.setCameraSettings(sl::VIDEO_SETTINGS::GAMMA, mCamGamma); - NODELET_DEBUG_STREAM( "mCamGamma changed: " << mCamGamma << " <- " << gamma); - mUpdateDynParams = true; - } -#endif + mGrabPeriodMean_usec->addValue(elapsed_usec); - if (mCamAutoExposure) { - if( mTriggerAutoExposure ) { - mZed.setCameraSettings(sl::VIDEO_SETTINGS::AEC_AGC, 1); - mTriggerAutoExposure = false; - } - } else { - int exposure = mZed.getCameraSettings(sl::VIDEO_SETTINGS::EXPOSURE); - if (exposure != mCamExposure) { - mZed.setCameraSettings(sl::VIDEO_SETTINGS::EXPOSURE, mCamExposure); - NODELET_DEBUG_STREAM( "mCamExposure changed: " << mCamExposure << " <- " << exposure); - mUpdateDynParams = true; - } - - int gain = mZed.getCameraSettings(sl::VIDEO_SETTINGS::GAIN); - if (gain != mCamGain) { - mZed.setCameraSettings(sl::VIDEO_SETTINGS::GAIN, mCamGain); - NODELET_DEBUG_STREAM( "mCamGain changed: " << mCamGain << " <- " << gain); - mUpdateDynParams = true; - } - } - - if (mCamAutoWB ) { - if(mTriggerAutoWB) { - mZed.setCameraSettings(sl::VIDEO_SETTINGS::WHITEBALANCE_AUTO, 1); - mTriggerAutoWB = false; - } - } else { - int wb = mZed.getCameraSettings(sl::VIDEO_SETTINGS::WHITEBALANCE_TEMPERATURE); - if (wb != mCamWB) { - mZed.setCameraSettings(sl::VIDEO_SETTINGS::WHITEBALANCE_TEMPERATURE, mCamWB); - NODELET_DEBUG_STREAM( "mCamWB changed: " << mCamWB << " <- " << wb); - mUpdateDynParams = true; - } - } - mDynParMutex.unlock(); - //NODELET_DEBUG_STREAM( "device_poll_thread_func MUTEX UNLOCK"); - } + // NODELET_INFO_STREAM("Grab time: " << elapsed_usec / 1000 << " msec"); - if(mUpdateDynParams) { - NODELET_DEBUG( "Update Dynamic Parameters"); - updateDynamicReconfigure(); - } - // <---- Camera Settings + // Timestamp + if (mSvoMode) + { + mFrameTimestamp = ros::Time::now(); + } + else + { + mFrameTimestamp = sl_tools::slTime2Ros(mZed.getTimestamp(sl::TIME_REFERENCE::IMAGE)); + } + + ros::Time stamp = mFrameTimestamp; // Timestamp + + // ----> Camera Settings + if (!mSvoMode && mFrameCount % 5 == 0) + { + // NODELET_DEBUG_STREAM( "[" << mFrameCount << "] device_poll_thread_func MUTEX LOCK"); + mDynParMutex.lock(); + + int brightness = mZed.getCameraSettings(sl::VIDEO_SETTINGS::BRIGHTNESS); + if (brightness != mCamBrightness) + { + mZed.setCameraSettings(sl::VIDEO_SETTINGS::BRIGHTNESS, mCamBrightness); + NODELET_DEBUG_STREAM("mCamBrightness changed: " << mCamBrightness << " <- " << brightness); + mUpdateDynParams = true; + } - // Publish the point cloud if someone has subscribed to - if (cloudSubnumber > 0) { + int contrast = mZed.getCameraSettings(sl::VIDEO_SETTINGS::CONTRAST); + if (contrast != mCamContrast) + { + mZed.setCameraSettings(sl::VIDEO_SETTINGS::CONTRAST, mCamContrast); + NODELET_DEBUG_STREAM("mCamContrast changed: " << mCamContrast << " <- " << contrast); + mUpdateDynParams = true; + } - // Run the point cloud conversion asynchronously to avoid slowing down - // all the program - // Retrieve raw pointCloud data if latest Pointcloud is ready - std::unique_lock lock(mPcMutex, std::defer_lock); + int hue = mZed.getCameraSettings(sl::VIDEO_SETTINGS::HUE); + if (hue != mCamHue) + { + mZed.setCameraSettings(sl::VIDEO_SETTINGS::HUE, mCamHue); + NODELET_DEBUG_STREAM("mCamHue changed: " << mCamHue << " <- " << hue); + mUpdateDynParams = true; + } - if (lock.try_lock()) { - mZed.retrieveMeasure(mCloud, sl::MEASURE::XYZBGRA, sl::MEM::CPU, mMatResolDepth); + int saturation = mZed.getCameraSettings(sl::VIDEO_SETTINGS::SATURATION); + if (saturation != mCamSaturation) + { + mZed.setCameraSettings(sl::VIDEO_SETTINGS::SATURATION, mCamSaturation); + NODELET_DEBUG_STREAM("mCamSaturation changed: " << mCamSaturation << " <- " << saturation); + mUpdateDynParams = true; + } - mPointCloudFrameId = mDepthFrameId; - mPointCloudTime = stamp; + int sharpness = mZed.getCameraSettings(sl::VIDEO_SETTINGS::SHARPNESS); + if (sharpness != mCamSharpness) + { + mZed.setCameraSettings(sl::VIDEO_SETTINGS::SHARPNESS, mCamSharpness); + NODELET_DEBUG_STREAM("mCamSharpness changed: " << mCamSharpness << " <- " << sharpness); + mUpdateDynParams = true; + } - // Signal Pointcloud thread that a new pointcloud is ready - mPcDataReadyCondVar.notify_one(); - mPcDataReady = true; - mPcPublishing = true; - } - } else { - mPcPublishing = false; - } +#if (ZED_SDK_MAJOR_VERSION == 3 && ZED_SDK_MINOR_VERSION >= 1) + int gamma = mZed.getCameraSettings(sl::VIDEO_SETTINGS::GAMMA); + if (gamma != mCamGamma) + { + mZed.setCameraSettings(sl::VIDEO_SETTINGS::GAMMA, mCamGamma); + NODELET_DEBUG_STREAM("mCamGamma changed: " << mCamGamma << " <- " << gamma); + mUpdateDynParams = true; + } +#endif - mObjDetMutex.lock(); - if (mObjDetRunning) { - //std::chrono::steady_clock::time_point start = std::chrono::steady_clock::now(); - detectObjects(objDetSubnumber > 0, objDetVizSubnumber > 0, stamp); - //std::chrono::steady_clock::time_point now = std::chrono::steady_clock::now(); + if (mCamAutoExposure) + { + if (mTriggerAutoExposure) + { + mZed.setCameraSettings(sl::VIDEO_SETTINGS::AEC_AGC, 1); + mTriggerAutoExposure = false; + } + } + else + { + int exposure = mZed.getCameraSettings(sl::VIDEO_SETTINGS::EXPOSURE); + if (exposure != mCamExposure) + { + mZed.setCameraSettings(sl::VIDEO_SETTINGS::EXPOSURE, mCamExposure); + NODELET_DEBUG_STREAM("mCamExposure changed: " << mCamExposure << " <- " << exposure); + mUpdateDynParams = true; + } - //double elapsed_msec = std::chrono::duration_cast(now - start).count(); - //mObjDetPeriodMean_msec->addValue(elapsed_msec); - } - mObjDetMutex.unlock(); + int gain = mZed.getCameraSettings(sl::VIDEO_SETTINGS::GAIN); + if (gain != mCamGain) + { + mZed.setCameraSettings(sl::VIDEO_SETTINGS::GAIN, mCamGain); + NODELET_DEBUG_STREAM("mCamGain changed: " << mCamGain << " <- " << gain); + mUpdateDynParams = true; + } + } + + if (mCamAutoWB) + { + if (mTriggerAutoWB) + { + mZed.setCameraSettings(sl::VIDEO_SETTINGS::WHITEBALANCE_AUTO, 1); + mTriggerAutoWB = false; + } + } + else + { + int wb = mZed.getCameraSettings(sl::VIDEO_SETTINGS::WHITEBALANCE_TEMPERATURE); + if (wb != mCamWB) + { + mZed.setCameraSettings(sl::VIDEO_SETTINGS::WHITEBALANCE_TEMPERATURE, mCamWB); + NODELET_DEBUG_STREAM("mCamWB changed: " << mCamWB << " <- " << wb); + mUpdateDynParams = true; + } + } + mDynParMutex.unlock(); + // NODELET_DEBUG_STREAM( "device_poll_thread_func MUTEX UNLOCK"); + } + + if (mUpdateDynParams) + { + NODELET_DEBUG("Update Dynamic Parameters"); + updateDynamicReconfigure(); + } + // <---- Camera Settings + + // Publish the point cloud if someone has subscribed to + if (cloudSubnumber > 0) + { + // Run the point cloud conversion asynchronously to avoid slowing down + // all the program + // Retrieve raw pointCloud data if latest Pointcloud is ready + std::unique_lock lock(mPcMutex, std::defer_lock); + + if (lock.try_lock()) + { + mZed.retrieveMeasure(mCloud, sl::MEASURE::XYZBGRA, sl::MEM::CPU, mMatResolDepth); - // Publish the odometry if someone has subscribed to - if (computeTracking) { + mPointCloudFrameId = mDepthFrameId; + mPointCloudTime = stamp; - if (!mSensor2BaseTransfValid) { - getSens2BaseTransform(); - } + // Signal Pointcloud thread that a new pointcloud is ready + mPcDataReadyCondVar.notify_one(); + mPcDataReady = true; + mPcPublishing = true; + } + } + else + { + mPcPublishing = false; + } + + mObjDetMutex.lock(); + if (mObjDetRunning && objDetSubnumber > 0) + { + processDetectedObjects(stamp); + } + mObjDetMutex.unlock(); + + // Publish the odometry if someone has subscribed to + if (computeTracking) + { + if (!mSensor2BaseTransfValid) + { + getSens2BaseTransform(); + } - if (!mSensor2CameraTransfValid) { - getSens2CameraTransform(); - } + if (!mSensor2CameraTransfValid) + { + getSens2CameraTransform(); + } - if (!mCamera2BaseTransfValid) { - getCamera2BaseTransform(); - } + if (!mCamera2BaseTransfValid) + { + getCamera2BaseTransform(); + } - if (!mInitOdomWithPose) { - sl::Pose deltaOdom; - mPosTrackingStatus = mZed.getPosition(deltaOdom, sl::REFERENCE_FRAME::CAMERA); + if (!mInitOdomWithPose) + { + sl::Pose deltaOdom; + mPosTrackingStatus = mZed.getPosition(deltaOdom, sl::REFERENCE_FRAME::CAMERA); - sl::Translation translation = deltaOdom.getTranslation(); - sl::Orientation quat = deltaOdom.getOrientation(); + sl::Translation translation = deltaOdom.getTranslation(); + sl::Orientation quat = deltaOdom.getOrientation(); #if 0 NODELET_DEBUG("delta ODOM [%s] - %.2f,%.2f,%.2f %.2f,%.2f,%.2f,%.2f", @@ -3383,41 +3777,43 @@ void ZEDWrapperNodelet::device_poll_thread_func() { NODELET_DEBUG_STREAM("ODOM -> Tracking Status: " << sl::toString(mTrackingStatus)); #endif - if (mPosTrackingStatus == sl::POSITIONAL_TRACKING_STATE::OK || mPosTrackingStatus == sl::POSITIONAL_TRACKING_STATE::SEARCHING || - mPosTrackingStatus == sl::POSITIONAL_TRACKING_STATE::FPS_TOO_LOW) { - // Transform ZED delta odom pose in TF2 Transformation - geometry_msgs::Transform deltaTransf; - deltaTransf.translation.x = translation(0); - deltaTransf.translation.y = translation(1); - deltaTransf.translation.z = translation(2); - deltaTransf.rotation.x = quat(0); - deltaTransf.rotation.y = quat(1); - deltaTransf.rotation.z = quat(2); - deltaTransf.rotation.w = quat(3); - tf2::Transform deltaOdomTf; - tf2::fromMsg(deltaTransf, deltaOdomTf); - // delta odom from sensor to base frame - tf2::Transform deltaOdomTf_base = - mSensor2BaseTransf.inverse() * deltaOdomTf * mSensor2BaseTransf; - - // Propagate Odom transform in time - mOdom2BaseTransf = mOdom2BaseTransf * deltaOdomTf_base; - - if (mTwoDMode) { - tf2::Vector3 tr_2d = mOdom2BaseTransf.getOrigin(); - tr_2d.setZ(mFixedZValue); - mOdom2BaseTransf.setOrigin(tr_2d); - - double roll, pitch, yaw; - tf2::Matrix3x3(mOdom2BaseTransf.getRotation()).getRPY(roll, pitch, yaw); - - tf2::Quaternion quat_2d; - quat_2d.setRPY(0.0, 0.0, yaw); - - mOdom2BaseTransf.setRotation(quat_2d); - } - -#if 0 //#ifndef NDEBUG // Enable for TF checking + if (mPosTrackingStatus == sl::POSITIONAL_TRACKING_STATE::OK || + mPosTrackingStatus == sl::POSITIONAL_TRACKING_STATE::SEARCHING || + mPosTrackingStatus == sl::POSITIONAL_TRACKING_STATE::FPS_TOO_LOW) + { + // Transform ZED delta odom pose in TF2 Transformation + geometry_msgs::Transform deltaTransf; + deltaTransf.translation.x = translation(0); + deltaTransf.translation.y = translation(1); + deltaTransf.translation.z = translation(2); + deltaTransf.rotation.x = quat(0); + deltaTransf.rotation.y = quat(1); + deltaTransf.rotation.z = quat(2); + deltaTransf.rotation.w = quat(3); + tf2::Transform deltaOdomTf; + tf2::fromMsg(deltaTransf, deltaOdomTf); + // delta odom from sensor to base frame + tf2::Transform deltaOdomTf_base = mSensor2BaseTransf.inverse() * deltaOdomTf * mSensor2BaseTransf; + + // Propagate Odom transform in time + mOdom2BaseTransf = mOdom2BaseTransf * deltaOdomTf_base; + + if (mTwoDMode) + { + tf2::Vector3 tr_2d = mOdom2BaseTransf.getOrigin(); + tr_2d.setZ(mFixedZValue); + mOdom2BaseTransf.setOrigin(tr_2d); + + double roll, pitch, yaw; + tf2::Matrix3x3(mOdom2BaseTransf.getRotation()).getRPY(roll, pitch, yaw); + + tf2::Quaternion quat_2d; + quat_2d.setRPY(0.0, 0.0, yaw); + + mOdom2BaseTransf.setRotation(quat_2d); + } + +#if 0 //#ifndef NDEBUG // Enable for TF checking double roll, pitch, yaw; tf2::Matrix3x3(mOdom2BaseTransf.getRotation()).getRPY(roll, pitch, yaw); @@ -3427,28 +3823,32 @@ void ZEDWrapperNodelet::device_poll_thread_func() { roll * RAD2DEG, pitch * RAD2DEG, yaw * RAD2DEG); #endif - // Publish odometry message - if (odomSubnumber > 0) { - publishOdom(mOdom2BaseTransf, deltaOdom, stamp); - } - - mTrackingReady = true; - } - } else if (mFloorAlignment) { - NODELET_WARN_THROTTLE(5.0, "Odometry will be published as soon as the floor as been detected for the first time"); - } - + // Publish odometry message + if (odomSubnumber > 0) + { + publishOdom(mOdom2BaseTransf, deltaOdom, stamp); } - // Publish the zed camera pose if someone has subscribed to - if (computeTracking) { - static sl::POSITIONAL_TRACKING_STATE oldStatus; - mPosTrackingStatus = mZed.getPosition(mLastZedPose, sl::REFERENCE_FRAME::WORLD); + mTrackingReady = true; + } + } + else if (mFloorAlignment) + { + NODELET_WARN_THROTTLE(5.0, "Odometry will be published as soon as the floor as been detected for the first " + "time"); + } + } + + // Publish the zed camera pose if someone has subscribed to + if (computeTracking) + { + static sl::POSITIONAL_TRACKING_STATE oldStatus; + mPosTrackingStatus = mZed.getPosition(mLastZedPose, sl::REFERENCE_FRAME::WORLD); - sl::Translation translation = mLastZedPose.getTranslation(); - sl::Orientation quat = mLastZedPose.getOrientation(); + sl::Translation translation = mLastZedPose.getTranslation(); + sl::Orientation quat = mLastZedPose.getOrientation(); -#if 0 //#ifndef NDEBUG // Enable for TF checking +#if 0 //#ifndef NDEBUG // Enable for TF checking double roll, pitch, yaw; tf2::Matrix3x3(tf2::Quaternion(quat.ox, quat.oy, quat.oz, quat.ow)).getRPY(roll, pitch, yaw); @@ -3461,38 +3861,41 @@ void ZEDWrapperNodelet::device_poll_thread_func() { #endif - if (mPosTrackingStatus == sl::POSITIONAL_TRACKING_STATE::OK || - mPosTrackingStatus == sl::POSITIONAL_TRACKING_STATE::SEARCHING /*|| status == sl::POSITIONAL_TRACKING_STATE::FPS_TOO_LOW*/) { - // Transform ZED pose in TF2 Transformation - geometry_msgs::Transform map2sensTransf; - - map2sensTransf.translation.x = translation(0); - map2sensTransf.translation.y = translation(1); - map2sensTransf.translation.z = translation(2); - map2sensTransf.rotation.x = quat(0); - map2sensTransf.rotation.y = quat(1); - map2sensTransf.rotation.z = quat(2); - map2sensTransf.rotation.w = quat(3); - tf2::Transform map_to_sens_transf; - tf2::fromMsg(map2sensTransf, map_to_sens_transf); - - mMap2BaseTransf = map_to_sens_transf * mSensor2BaseTransf; // Base position in map frame - - if (mTwoDMode) { - tf2::Vector3 tr_2d = mMap2BaseTransf.getOrigin(); - tr_2d.setZ(mFixedZValue); - mMap2BaseTransf.setOrigin(tr_2d); + if (mPosTrackingStatus == sl::POSITIONAL_TRACKING_STATE::OK || + mPosTrackingStatus == + sl::POSITIONAL_TRACKING_STATE::SEARCHING /*|| status == sl::POSITIONAL_TRACKING_STATE::FPS_TOO_LOW*/) + { + // Transform ZED pose in TF2 Transformation + geometry_msgs::Transform map2sensTransf; + + map2sensTransf.translation.x = translation(0); + map2sensTransf.translation.y = translation(1); + map2sensTransf.translation.z = translation(2); + map2sensTransf.rotation.x = quat(0); + map2sensTransf.rotation.y = quat(1); + map2sensTransf.rotation.z = quat(2); + map2sensTransf.rotation.w = quat(3); + tf2::Transform map_to_sens_transf; + tf2::fromMsg(map2sensTransf, map_to_sens_transf); + + mMap2BaseTransf = map_to_sens_transf * mSensor2BaseTransf; // Base position in map frame + + if (mTwoDMode) + { + tf2::Vector3 tr_2d = mMap2BaseTransf.getOrigin(); + tr_2d.setZ(mFixedZValue); + mMap2BaseTransf.setOrigin(tr_2d); - double roll, pitch, yaw; - tf2::Matrix3x3(mMap2BaseTransf.getRotation()).getRPY(roll, pitch, yaw); + double roll, pitch, yaw; + tf2::Matrix3x3(mMap2BaseTransf.getRotation()).getRPY(roll, pitch, yaw); - tf2::Quaternion quat_2d; - quat_2d.setRPY(0.0, 0.0, yaw); + tf2::Quaternion quat_2d; + quat_2d.setRPY(0.0, 0.0, yaw); - mMap2BaseTransf.setRotation(quat_2d); - } + mMap2BaseTransf.setRotation(quat_2d); + } -#if 0 //#ifndef NDEBUG // Enable for TF checking +#if 0 //#ifndef NDEBUG // Enable for TF checking double roll, pitch, yaw; tf2::Matrix3x3(mMap2BaseTransf.getRotation()).getRPY(roll, pitch, yaw); @@ -3502,35 +3905,41 @@ void ZEDWrapperNodelet::device_poll_thread_func() { roll * RAD2DEG, pitch * RAD2DEG, yaw * RAD2DEG); #endif - bool initOdom = false; - - if (!(mFloorAlignment)) { - initOdom = mInitOdomWithPose; - } else { - initOdom = (mPosTrackingStatus == sl::POSITIONAL_TRACKING_STATE::OK) & mInitOdomWithPose; - } - - if (initOdom || mResetOdom) { - - NODELET_INFO("Odometry aligned to last tracking pose"); - - // Propagate Odom transform in time - mOdom2BaseTransf = mMap2BaseTransf; - mMap2BaseTransf.setIdentity(); - - if (odomSubnumber > 0) { - // Publish odometry message - publishOdom(mOdom2BaseTransf, mLastZedPose, mFrameTimestamp); - } + bool initOdom = false; + + if (!(mFloorAlignment)) + { + initOdom = mInitOdomWithPose; + } + else + { + initOdom = (mPosTrackingStatus == sl::POSITIONAL_TRACKING_STATE::OK) & mInitOdomWithPose; + } + + if (initOdom || mResetOdom) + { + NODELET_INFO("Odometry aligned to last tracking pose"); + + // Propagate Odom transform in time + mOdom2BaseTransf = mMap2BaseTransf; + mMap2BaseTransf.setIdentity(); + + if (odomSubnumber > 0) + { + // Publish odometry message + publishOdom(mOdom2BaseTransf, mLastZedPose, mFrameTimestamp); + } - mInitOdomWithPose = false; - mResetOdom = false; - } else { - // Transformation from map to odometry frame - //mMap2OdomTransf = mOdom2BaseTransf.inverse() * mMap2BaseTransf; - mMap2OdomTransf = mMap2BaseTransf * mOdom2BaseTransf.inverse(); + mInitOdomWithPose = false; + mResetOdom = false; + } + else + { + // Transformation from map to odometry frame + // mMap2OdomTransf = mOdom2BaseTransf.inverse() * mMap2BaseTransf; + mMap2OdomTransf = mMap2BaseTransf * mOdom2BaseTransf.inverse(); -#if 0 //#ifndef NDEBUG // Enable for TF checking +#if 0 //#ifndef NDEBUG // Enable for TF checking double roll, pitch, yaw; tf2::Matrix3x3(mMap2OdomTransf.getRotation()).getRPY(roll, pitch, yaw); @@ -3539,36 +3948,40 @@ void ZEDWrapperNodelet::device_poll_thread_func() { mMap2OdomTransf.getOrigin().x(), mMap2OdomTransf.getOrigin().y(), mMap2OdomTransf.getOrigin().z(), roll * RAD2DEG, pitch * RAD2DEG, yaw * RAD2DEG); #endif - } + } - // Publish Pose message - if ((poseSubnumber + poseCovSubnumber) > 0) { - publishPose(stamp); - } + // Publish Pose message + if ((poseSubnumber + poseCovSubnumber) > 0) + { + publishPose(stamp); + } - mTrackingReady = true; - } + mTrackingReady = true; + } - oldStatus = mPosTrackingStatus; - } + oldStatus = mPosTrackingStatus; + } - if(mZedRealCamModel == sl::MODEL::ZED) { - // Publish pose tf only if enabled - if(mPublishTf) { - // Note, the frame is published, but its values will only change if - // someone has subscribed to odom - publishOdomFrame(mOdom2BaseTransf, stamp); // publish the base Frame in odometry frame - - if(mPublishMapTf) { - // Note, the frame is published, but its values will only change if - // someone has subscribed to map - publishPoseFrame(mMap2OdomTransf, stamp); // publish the odometry Frame in map frame - } - } - } + if (mZedRealCamModel == sl::MODEL::ZED) + { + // Publish pose tf only if enabled + if (mPublishTf) + { + // Note, the frame is published, but its values will only change if + // someone has subscribed to odom + publishOdomFrame(mOdom2BaseTransf, stamp); // publish the base Frame in odometry frame + + if (mPublishMapTf) + { + // Note, the frame is published, but its values will only change if + // someone has subscribed to map + publishPoseFrame(mMap2OdomTransf, stamp); // publish the odometry Frame in map frame + } + } + } -#if 0 //#ifndef NDEBUG // Enable for TF checking - // Double check: map_to_pose must be equal to mMap2BaseTransf +#if 0 //#ifndef NDEBUG // Enable for TF checking + // Double check: map_to_pose must be equal to mMap2BaseTransf tf2::Transform map_to_base; @@ -3594,621 +4007,701 @@ void ZEDWrapperNodelet::device_poll_thread_func() { NODELET_DEBUG("*******************************"); #endif - std::chrono::steady_clock::time_point end_elab = std::chrono::steady_clock::now(); - - 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; - - if (!loop_rate.sleep()) { - if (mean_elab_sec > (1. / mCamFrameRate)) { - if (++count_warn > 10) { - NODELET_DEBUG_THROTTLE( - 1.0, - "Working thread is not synchronized with the Camera frame 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 (" << mean_elab_sec << " sec) than requested " - "by the FPS rate (" << loop_rate.expectedCycleTime() << " sec). Please consider to " - "lower the 'frame_rate' setting or to reduce the power requirements reducing the resolutions."); - } - - loop_rate.reset(); - } else { - count_warn = 0; - } - } - } else { - NODELET_DEBUG_THROTTLE(5.0, "No topics subscribed by users"); - - if(mZedRealCamModel == sl::MODEL::ZED || !mPublishImuTf) { - // Publish odometry tf only if enabled - if (mPublishTf) { - ros::Time t; - - if (mSvoMode) { - t = ros::Time::now(); - } else { - t = sl_tools::slTime2Ros(mZed.getTimestamp(sl::TIME_REFERENCE::CURRENT)); - } - - publishOdomFrame(mOdom2BaseTransf, mFrameTimestamp); // publish the base Frame in odometry frame - - if (mPublishMapTf) { - publishPoseFrame(mMap2OdomTransf, mFrameTimestamp); // publish the odometry Frame in map frame - } - - if(mPublishImuTf && !mStaticImuFramePublished ) - { - publishStaticImuFrame(); - } - } - } + std::chrono::steady_clock::time_point end_elab = std::chrono::steady_clock::now(); - std::this_thread::sleep_for( - std::chrono::milliseconds(10)); // No subscribers, we just wait - loop_rate.reset(); - } + double elab_usec = std::chrono::duration_cast(end_elab - start_elab).count(); - mDiagUpdater.update(); - } // while loop + double mean_elab_sec = mElabPeriodMean_sec->addValue(elab_usec / 1000000.); + + static int count_warn = 0; - mStopNode = true; // Stops other threads + if (!loop_rate.sleep()) + { + if (mean_elab_sec > (1. / mCamFrameRate)) + { + if (++count_warn > 10) + { + NODELET_DEBUG_THROTTLE(1.0, "Working thread is not synchronized with the Camera frame 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 (" << mean_elab_sec + << " sec) than requested " + "by the FPS rate (" + << loop_rate.expectedCycleTime() + << " sec). Please consider to " + "lower the 'frame_rate' setting or to " + "reduce the power requirements reducing " + "the resolutions."); + } + + loop_rate.reset(); + } + else + { + count_warn = 0; + } + } + } + else + { + NODELET_DEBUG_THROTTLE(5.0, "No topics subscribed by users"); - std::lock_guard lock(mCloseZedMutex); - NODELET_DEBUG("Closing ZED"); + if (mZedRealCamModel == sl::MODEL::ZED || !mPublishImuTf) + { + // Publish odometry tf only if enabled + if (mPublishTf) + { + ros::Time t; + + if (mSvoMode) + { + t = ros::Time::now(); + } + else + { + t = sl_tools::slTime2Ros(mZed.getTimestamp(sl::TIME_REFERENCE::CURRENT)); + } + + publishOdomFrame(mOdom2BaseTransf, mFrameTimestamp); // publish the base Frame in odometry frame + + if (mPublishMapTf) + { + publishPoseFrame(mMap2OdomTransf, mFrameTimestamp); // publish the odometry Frame in map frame + } + + if (mPublishImuTf && !mStaticImuFramePublished) + { + publishStaticImuFrame(); + } + } + } - if(mRecording) { - mRecording=false; - mZed.disableRecording(); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); // No subscribers, we just wait + loop_rate.reset(); } - mZed.close(); - NODELET_DEBUG("ZED pool thread finished"); -} + mDiagUpdater.update(); + } // while loop -void ZEDWrapperNodelet::callback_updateDiagnostic(diagnostic_updater::DiagnosticStatusWrapper& stat) { + mStopNode = true; // Stops other threads - if (mConnStatus != sl::ERROR_CODE::SUCCESS) { - stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, sl::toString(mConnStatus).c_str()); - return; - } + std::lock_guard lock(mCloseZedMutex); + NODELET_DEBUG("Closing ZED"); - if (mGrabActive) { - if (mGrabStatus == sl::ERROR_CODE::SUCCESS /*|| mGrabStatus == sl::ERROR_CODE::NOT_A_NEW_FRAME*/) { + if (mRecording) + { + mRecording = false; + mZed.disableRecording(); + } + mZed.close(); - stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Camera grabbing"); + NODELET_DEBUG("ZED pool thread finished"); +} - double freq = 1000000. / mGrabPeriodMean_usec->getMean(); - double freq_perc = 100.*freq / mCamFrameRate; - stat.addf("Capture", "Mean Frequency: %.1f Hz (%.1f%%)", freq, freq_perc); +void ZEDWrapperNodelet::callback_updateDiagnostic(diagnostic_updater::DiagnosticStatusWrapper& stat) +{ + if (mConnStatus != sl::ERROR_CODE::SUCCESS) + { + stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, sl::toString(mConnStatus).c_str()); + return; + } + + if (mGrabActive) + { + if (mGrabStatus == sl::ERROR_CODE::SUCCESS /*|| mGrabStatus == sl::ERROR_CODE::NOT_A_NEW_FRAME*/) + { + stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Camera grabbing"); - stat.addf("General Processing", "Mean Time: %.3f sec (Max. %.3f sec)", mElabPeriodMean_sec->getMean(), 1. / mCamFrameRate); + double freq = 1000000. / mGrabPeriodMean_usec->getMean(); + double freq_perc = 100. * freq / mCamFrameRate; + stat.addf("Capture", "Mean Frequency: %.1f Hz (%.1f%%)", freq, freq_perc); - if(mPublishingData) { - freq = 1. / mVideoDepthPeriodMean_sec->getMean(); - freq_perc = 100.*freq / mVideoDepthFreq; - stat.addf("Video/Depth Publish", "Mean Frequency: %.1f Hz (%.1f%%)", freq, freq_perc); - } + stat.addf("General Processing", "Mean Time: %.3f sec (Max. %.3f sec)", mElabPeriodMean_sec->getMean(), + 1. / mCamFrameRate); - if( mSvoMode ) { - int frame = mZed.getSVOPosition(); - int totFrames = mZed.getSVONumberOfFrames(); - double svo_perc = 100.*(static_cast(frame)/totFrames); + if (mPublishingData) + { + freq = 1. / mVideoDepthPeriodMean_sec->getMean(); + freq_perc = 100. * freq / mVideoDepthFreq; + stat.addf("Video/Depth Publish", "Mean Frequency: %.1f Hz (%.1f%%)", freq, freq_perc); + } - stat.addf("Playing SVO", "Frame: %d/%d (%.1f%%)", frame,totFrames, svo_perc); - } + if (mSvoMode) + { + int frame = mZed.getSVOPosition(); + int totFrames = mZed.getSVONumberOfFrames(); + double svo_perc = 100. * (static_cast(frame) / totFrames); - if (mComputeDepth) { - stat.add("Depth status", "ACTIVE"); - - if (mPcPublishing) { - double freq = 1000000. / mPcPeriodMean_usec->getMean(); - double freq_perc = 100.*freq / mPointCloudFreq; - stat.addf("Point Cloud", "Mean Frequency: %.1f Hz (%.1f%%)", freq, freq_perc); - } else { - stat.add("Point Cloud", "Topic not subscribed"); - } - - if (mFloorAlignment) { - if (mInitOdomWithPose) { - stat.add("Floor Detection", "NOT INITIALIZED"); - } else { - stat.add("Floor Detection", "INITIALIZED"); - } - } - - if (mTrackingActivated) { - stat.addf("Tracking status", "%s", sl::toString(mPosTrackingStatus).c_str()); - } else { - stat.add("Pos. Tracking status", "INACTIVE"); - } - - if (mObjDetRunning) { - double freq = 1000./mObjDetPeriodMean_msec->getMean(); - double freq_perc = 100.*freq / mCamFrameRate; - stat.addf("Object detection", "Mean Frequency: %.3f Hz (%.1f%%)", freq, freq_perc ); - } else { - stat.add("Object Detection", "INACTIVE"); - } - } else { - stat.add("Depth status", "INACTIVE"); - } - } else { - stat.summaryf(diagnostic_msgs::DiagnosticStatus::ERROR, "Camera error: %s", sl::toString(mGrabStatus).c_str()); - } + stat.addf("Playing SVO", "Frame: %d/%d (%.1f%%)", frame, totFrames, svo_perc); + } - } else { - stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Waiting for data subscriber"); - stat.add("Capture", "INACTIVE"); - } + if (mComputeDepth) + { + stat.add("Depth status", "ACTIVE"); - if (mSensPublishing) { - double freq = 1000000. / mSensPeriodMean_usec->getMean(); - double freq_perc = 100.*freq / mSensPubRate; - stat.addf("IMU", "Mean Frequency: %.1f Hz (%.1f%%)", freq, freq_perc); - } else { - stat.add("IMU", "Topics not subscribed"); - } + if (mPcPublishing) + { + double freq = 1000000. / mPcPeriodMean_usec->getMean(); + double freq_perc = 100. * freq / mPointCloudFreq; + stat.addf("Point Cloud", "Mean Frequency: %.1f Hz (%.1f%%)", freq, freq_perc); + } + else + { + stat.add("Point Cloud", "Topic not subscribed"); + } - if( mZedRealCamModel == sl::MODEL::ZED2 ) { - stat.addf("Left CMOS Temp.", "%.1f °C", mTempLeft); - stat.addf("Right CMOS Temp.", "%.1f °C", mTempRight); + if (mFloorAlignment) + { + if (mInitOdomWithPose) + { + stat.add("Floor Detection", "NOT INITIALIZED"); + } + else + { + stat.add("Floor Detection", "INITIALIZED"); + } + } - if( mTempLeft>70.f || mTempRight>70.f ) { - stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "Camera temperature"); + if (mTrackingActivated) + { + stat.addf("Tracking status", "%s", sl::toString(mPosTrackingStatus).c_str()); + } + else + { + stat.add("Pos. Tracking status", "INACTIVE"); } - } else { - stat.add("Left CMOS Temp.", "N/A"); - stat.add("Right CMOS Temp.", "N/A"); - } - if (mRecording) { - if (!mRecStatus.status) { - if (mGrabActive) { - stat.add("SVO Recording", "ERROR"); - stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, - "Error adding frames to SVO file while recording. Check free disk space"); - } else { - stat.add("SVO Recording", "WAITING"); - } - } else { - stat.add("SVO Recording", "ACTIVE"); - stat.addf("SVO compression time", "%g msec", mRecStatus.average_compression_time); - stat.addf("SVO compression ratio", "%.1f%%", mRecStatus.average_compression_ratio); + if (mObjDetRunning) + { + double freq = 1000. / mObjDetPeriodMean_msec->getMean(); + double freq_perc = 100. * freq / mCamFrameRate; + stat.addf("Object detection", "Mean Frequency: %.3f Hz (%.1f%%)", freq, freq_perc); + } + else + { + stat.add("Object Detection", "INACTIVE"); } - } else { - stat.add("SVO Recording", "NOT ACTIVE"); + } + else + { + stat.add("Depth status", "INACTIVE"); + } } + else + { + stat.summaryf(diagnostic_msgs::DiagnosticStatus::ERROR, "Camera error: %s", sl::toString(mGrabStatus).c_str()); + } + } + else + { + stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Waiting for data subscriber"); + stat.add("Capture", "INACTIVE"); + } + + if (mSensPublishing) + { + double freq = 1000000. / mSensPeriodMean_usec->getMean(); + double freq_perc = 100. * freq / mSensPubRate; + stat.addf("IMU", "Mean Frequency: %.1f Hz (%.1f%%)", freq, freq_perc); + } + else + { + stat.add("IMU", "Topics not subscribed"); + } + + if (mZedRealCamModel == sl::MODEL::ZED2) + { + stat.addf("Left CMOS Temp.", "%.1f °C", mTempLeft); + stat.addf("Right CMOS Temp.", "%.1f °C", mTempRight); + + if (mTempLeft > 70.f || mTempRight > 70.f) + { + stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "Camera temperature"); + } + } + else + { + stat.add("Left CMOS Temp.", "N/A"); + stat.add("Right CMOS Temp.", "N/A"); + } + + if (mRecording) + { + if (!mRecStatus.status) + { + if (mGrabActive) + { + stat.add("SVO Recording", "ERROR"); + stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "Error adding frames to SVO file while recording. Check " + "free disk space"); + } + else + { + stat.add("SVO Recording", "WAITING"); + } + } + else + { + stat.add("SVO Recording", "ACTIVE"); + stat.addf("SVO compression time", "%g msec", mRecStatus.average_compression_time); + stat.addf("SVO compression ratio", "%.1f%%", mRecStatus.average_compression_ratio); + } + } + else + { + stat.add("SVO Recording", "NOT ACTIVE"); + } } bool ZEDWrapperNodelet::on_start_svo_recording(zed_interfaces::start_svo_recording::Request& req, - zed_interfaces::start_svo_recording::Response& res) { - std::lock_guard lock(mRecMutex); + zed_interfaces::start_svo_recording::Response& res) +{ + std::lock_guard lock(mRecMutex); + + if (mRecording) + { + res.result = false; + res.info = "Recording was just active"; + return false; + } - if (mRecording) { - res.result = false; - res.info = "Recording was just active"; - return false; - } + // Check filename + if (req.svo_filename.empty()) + { + req.svo_filename = "zed.svo"; + } - // Check filename - if (req.svo_filename.empty()) { - req.svo_filename = "zed.svo"; - } + sl::ERROR_CODE err; - sl::ERROR_CODE err; + sl::RecordingParameters recParams; + recParams.compression_mode = mSvoComprMode; + recParams.video_filename = req.svo_filename.c_str(); + err = mZed.enableRecording(recParams); - sl::RecordingParameters recParams; - recParams.compression_mode = mSvoComprMode; - recParams.video_filename = req.svo_filename.c_str(); - err = mZed.enableRecording(recParams); + if (err == sl::ERROR_CODE::SVO_UNSUPPORTED_COMPRESSION) + { + recParams.compression_mode = mSvoComprMode == sl::SVO_COMPRESSION_MODE::H265 ? sl::SVO_COMPRESSION_MODE::H264 : + sl::SVO_COMPRESSION_MODE::H265; - if (err == sl::ERROR_CODE::SVO_UNSUPPORTED_COMPRESSION) { - recParams.compression_mode = mSvoComprMode == sl::SVO_COMPRESSION_MODE::H265 ? sl::SVO_COMPRESSION_MODE::H264 : - sl::SVO_COMPRESSION_MODE::H265; + NODELET_WARN_STREAM("The chosen " << sl::toString(mSvoComprMode).c_str() << "mode is not available. Trying " + << sl::toString(recParams.compression_mode).c_str()); - NODELET_WARN_STREAM("The chosen " << sl::toString(mSvoComprMode).c_str() << "mode is not available. Trying " << - sl::toString(recParams.compression_mode).c_str()); + err = mZed.enableRecording(recParams); + if (err == sl::ERROR_CODE::SVO_UNSUPPORTED_COMPRESSION) + { + NODELET_WARN_STREAM(sl::toString(recParams.compression_mode).c_str() + << "not available. Trying " << sl::toString(sl::SVO_COMPRESSION_MODE::H264).c_str()); + recParams.compression_mode = sl::SVO_COMPRESSION_MODE::H264; + err = mZed.enableRecording(recParams); + + if (err == sl::ERROR_CODE::SVO_UNSUPPORTED_COMPRESSION) + { + recParams.compression_mode = sl::SVO_COMPRESSION_MODE::LOSSLESS; err = mZed.enableRecording(recParams); - - if (err == sl::ERROR_CODE::SVO_UNSUPPORTED_COMPRESSION) { - NODELET_WARN_STREAM(sl::toString(recParams.compression_mode).c_str() << "not available. Trying " << sl::toString( - sl::SVO_COMPRESSION_MODE::H264).c_str()); - recParams.compression_mode = sl::SVO_COMPRESSION_MODE::H264; - err = mZed.enableRecording(recParams); - - if (err == sl::ERROR_CODE::SVO_UNSUPPORTED_COMPRESSION) { - recParams.compression_mode = sl::SVO_COMPRESSION_MODE::LOSSLESS; - err = mZed.enableRecording(recParams); - } - } + } } + } - if (err != sl::ERROR_CODE::SUCCESS) { - res.result = false; - res.info = sl::toString(err).c_str(); - mRecording = false; - return false; - } + if (err != sl::ERROR_CODE::SUCCESS) + { + res.result = false; + res.info = sl::toString(err).c_str(); + mRecording = false; + return false; + } - mSvoComprMode = recParams.compression_mode; - mRecording = true; - res.info = "Recording started ("; - res.info += sl::toString(mSvoComprMode).c_str(); - res.info += ")"; - res.result = true; + mSvoComprMode = recParams.compression_mode; + mRecording = true; + res.info = "Recording started ("; + res.info += sl::toString(mSvoComprMode).c_str(); + res.info += ")"; + res.result = true; - NODELET_INFO_STREAM("SVO recording STARTED: " << req.svo_filename << " (" << sl::toString(mSvoComprMode).c_str() << ")"); + NODELET_INFO_STREAM("SVO recording STARTED: " << req.svo_filename << " (" << sl::toString(mSvoComprMode).c_str() + << ")"); - return true; + return true; } bool ZEDWrapperNodelet::on_stop_svo_recording(zed_interfaces::stop_svo_recording::Request& req, - zed_interfaces::stop_svo_recording::Response& res) { - std::lock_guard lock(mRecMutex); - - if (!mRecording) { - res.done = false; - res.info = "Recording was not active"; - return false; - } + zed_interfaces::stop_svo_recording::Response& res) +{ + std::lock_guard lock(mRecMutex); + + if (!mRecording) + { + res.done = false; + res.info = "Recording was not active"; + return false; + } - mZed.disableRecording(); - mRecording = false; - res.info = "Recording stopped"; - res.done = true; + mZed.disableRecording(); + mRecording = false; + res.info = "Recording stopped"; + res.done = true; - NODELET_INFO_STREAM("SVO recording STOPPED"); + NODELET_INFO_STREAM("SVO recording STOPPED"); - return true; + return true; } bool ZEDWrapperNodelet::on_start_remote_stream(zed_interfaces::start_remote_stream::Request& req, - zed_interfaces::start_remote_stream::Response& res) { - if (mStreaming) { - res.result = false; - res.info = "SVO remote streaming was just active"; - return false; - } - - sl::StreamingParameters params; - params.codec = static_cast(req.codec); - params.port = req.port; - params.bitrate = req.bitrate; - params.gop_size = req.gop_size; - params.adaptative_bitrate = req.adaptative_bitrate; + zed_interfaces::start_remote_stream::Response& res) +{ + if (mStreaming) + { + res.result = false; + res.info = "SVO remote streaming was just active"; + return false; + } - if (params.gop_size < -1 || params.gop_size > 256) { + sl::StreamingParameters params; + params.codec = static_cast(req.codec); + params.port = req.port; + params.bitrate = req.bitrate; + params.gop_size = req.gop_size; + params.adaptative_bitrate = req.adaptative_bitrate; - mStreaming = false; + if ((params.gop_size < -1) || (params.gop_size > 256)) + { + mStreaming = false; - res.result = false; - res.info = "`gop_size` wrong ("; - res.info += params.gop_size; - res.info += "). Remote streaming not started"; + res.result = false; + res.info = "`gop_size` wrong ("; + res.info += params.gop_size; + res.info += "). Remote streaming not started"; - NODELET_ERROR_STREAM(res.info); - return false; - } + NODELET_ERROR_STREAM(res.info); + return false; + } - if (params.port % 2 != 0) { - mStreaming = false; + if (params.port % 2 != 0) + { + mStreaming = false; - res.result = false; - res.info = "`port` must be an even number. Remote streaming not started"; + res.result = false; + res.info = "`port` must be an even number. Remote streaming not started"; - NODELET_ERROR_STREAM(res.info); - return false; - } + NODELET_ERROR_STREAM(res.info); + return false; + } - sl::ERROR_CODE err = mZed.enableStreaming(params); + sl::ERROR_CODE err = mZed.enableStreaming(params); - if (err != sl::ERROR_CODE::SUCCESS) { - mStreaming = false; + if (err != sl::ERROR_CODE::SUCCESS) + { + mStreaming = false; - res.result = false; - res.info = sl::toString(err).c_str(); + res.result = false; + res.info = sl::toString(err).c_str(); - NODELET_ERROR_STREAM("Remote streaming not started (" << res.info << ")"); + NODELET_ERROR_STREAM("Remote streaming not started (" << res.info << ")"); - return false; - } + return false; + } - mStreaming = true; + mStreaming = true; - NODELET_INFO_STREAM("Remote streaming STARTED"); + NODELET_INFO_STREAM("Remote streaming STARTED"); - res.result = true; - res.info = "Remote streaming STARTED"; - return true; + res.result = true; + res.info = "Remote streaming STARTED"; + return true; } bool ZEDWrapperNodelet::on_stop_remote_stream(zed_interfaces::stop_remote_stream::Request& req, - zed_interfaces::stop_remote_stream::Response& res) { - if (mStreaming) { - mZed.disableStreaming(); - } - - mStreaming = false; - NODELET_INFO_STREAM("SVO remote streaming STOPPED"); - - res.done = true; - return true; + zed_interfaces::stop_remote_stream::Response& res) +{ + if (mStreaming) + { + mZed.disableStreaming(); + } + + mStreaming = false; + NODELET_INFO_STREAM("SVO remote streaming STOPPED"); + + res.done = true; + return true; } bool ZEDWrapperNodelet::on_set_led_status(zed_interfaces::set_led_status::Request& req, - zed_interfaces::set_led_status::Response& res) { - if (mCamFwVersion < 1523) { - NODELET_WARN_STREAM("To set the status of the blue LED the camera must be updated to FW 1523 or newer"); - return false; - } + zed_interfaces::set_led_status::Response& res) +{ + if (mCamFwVersion < 1523) + { + NODELET_WARN_STREAM("To set the status of the blue LED the camera must be updated to FW 1523 or newer"); + return false; + } - mZed.setCameraSettings(sl::VIDEO_SETTINGS::LED_STATUS, req.led_enabled ? 1 : 0); + mZed.setCameraSettings(sl::VIDEO_SETTINGS::LED_STATUS, req.led_enabled ? 1 : 0); - return true; + return true; } bool ZEDWrapperNodelet::on_toggle_led(zed_interfaces::toggle_led::Request& req, - zed_interfaces::toggle_led::Response& res) { - if (mCamFwVersion < 1523) { - NODELET_WARN_STREAM("To set the status of the blue LED the camera must be updated to FW 1523 or newer"); - return false; - } + zed_interfaces::toggle_led::Response& res) +{ + if (mCamFwVersion < 1523) + { + NODELET_WARN_STREAM("To set the status of the blue LED the camera must be updated to FW 1523 or newer"); + return false; + } - int status = mZed.getCameraSettings(sl::VIDEO_SETTINGS::LED_STATUS); - int new_status = status == 0 ? 1 : 0; - mZed.setCameraSettings(sl::VIDEO_SETTINGS::LED_STATUS, new_status); + int status = mZed.getCameraSettings(sl::VIDEO_SETTINGS::LED_STATUS); + int new_status = status == 0 ? 1 : 0; + mZed.setCameraSettings(sl::VIDEO_SETTINGS::LED_STATUS, new_status); - return (new_status == 1); + return (new_status == 1); } - bool ZEDWrapperNodelet::on_start_3d_mapping(zed_interfaces::start_3d_mapping::Request& req, - zed_interfaces::start_3d_mapping::Response& res) { - if( mMappingEnabled && mMappingRunning) { - NODELET_WARN_STREAM("Spatial mapping was just running"); - - res.done = false; - return res.done; - } - - mMappingRunning = false; - - mMappingRes = req.resolution; - mMaxMappingRange = req.max_mapping_range; - mFusedPcPubFreq = req.fused_pointcloud_freq; - - NODELET_DEBUG_STREAM(" * Received mapping resolution\t\t-> " << mMappingRes << " m"); + zed_interfaces::start_3d_mapping::Response& res) +{ + if (mMappingEnabled && mMappingRunning) + { + NODELET_WARN_STREAM("Spatial mapping was just running"); + res.done = false; + return res.done; + } + mMappingRunning = false; - NODELET_DEBUG_STREAM(" * Received mapping max range\t-> " << mMaxMappingRange << " m" ); + mMappingRes = req.resolution; + mMaxMappingRange = req.max_mapping_range; + mFusedPcPubFreq = req.fused_pointcloud_freq; - NODELET_DEBUG_STREAM(" * Received fused point cloud freq:\t-> " << mFusedPcPubFreq << " Hz"); + NODELET_DEBUG_STREAM(" * Received mapping resolution\t\t-> " << mMappingRes << " m"); + NODELET_DEBUG_STREAM(" * Received mapping max range\t-> " << mMaxMappingRange << " m"); + NODELET_DEBUG_STREAM(" * Received fused point cloud freq:\t-> " << mFusedPcPubFreq << " Hz"); - mMappingEnabled = true; - res.done = true; + mMappingEnabled = true; + res.done = true; - return res.done; + return res.done; } bool ZEDWrapperNodelet::on_stop_3d_mapping(zed_interfaces::stop_3d_mapping::Request& req, - zed_interfaces::stop_3d_mapping::Response& res) { + zed_interfaces::stop_3d_mapping::Response& res) +{ + if (mMappingEnabled) + { + mPubFusedCloud.shutdown(); + mMappingMutex.lock(); + stop_3d_mapping(); + mMappingMutex.unlock(); - if( mMappingEnabled ) { - mPubFusedCloud.shutdown(); - mMappingMutex.lock(); - stop_3d_mapping(); - mMappingMutex.unlock(); - - res.done = true; - } else { - res.done = false; - } + res.done = true; + } + else + { + res.done = false; + } - return res.done; + return res.done; } bool ZEDWrapperNodelet::on_start_object_detection(zed_interfaces::start_object_detection::Request& req, - zed_interfaces::start_object_detection::Response& res) { - if(mZedRealCamModel!=sl::MODEL::ZED2) { - mObjDetEnabled = false; - mObjDetRunning = false; - - NODELET_ERROR_STREAM( "Object detection not started. OD is available only using a ZED2 camera model"); - return false; - } - - if( mObjDetEnabled && mObjDetRunning) { - NODELET_WARN_STREAM("Object Detection was just running"); - - res.done = false; - return res.done; - } + zed_interfaces::start_object_detection::Response& res) +{ + NODELET_INFO("Called 'start_object_detection' service"); + if (mZedRealCamModel != sl::MODEL::ZED2) + { + mObjDetEnabled = false; mObjDetRunning = false; - mObjDetConfidence = req.confidence; - mObjDetTracking = req.tracking; - mObjDetPeople = req.people; - mObjDetVehicles = req.vehicles; - - NODELET_DEBUG_STREAM(" * Object min. confidence\t-> " << mObjDetConfidence); - NODELET_DEBUG_STREAM(" * Object tracking\t\t-> " << (mObjDetTracking?"ENABLED":"DISABLED")); - NODELET_DEBUG_STREAM(" * People detection\t\t-> " << (mObjDetPeople?"ENABLED":"DISABLED")); - NODELET_DEBUG_STREAM(" * Vehicles detection\t\t-> " << (mObjDetVehicles?"ENABLED":"DISABLED")); + NODELET_ERROR_STREAM("Object detection not started. OD is available only using a ZED2 camera model"); + res.done = false; + return res.done; + } - mObjDetEnabled = true; - res.done = true; + if (mObjDetEnabled && mObjDetRunning) + { + NODELET_WARN_STREAM("Object Detection was just running"); + res.done = false; return res.done; -} + } -/*! \brief Service callback to stop_object_detection service - */ -bool ZEDWrapperNodelet::on_stop_object_detection(zed_interfaces::stop_object_detection::Request& req, - zed_interfaces::stop_object_detection::Response& res) { - if( mObjDetEnabled ) { - mObjDetMutex.lock(); - stop_obj_detect(); - mObjDetMutex.unlock(); - - res.done = true; - } else { - res.done = false; - } + mObjDetRunning = false; + mObjDetConfidence = req.confidence; + mObjDetTracking = req.tracking; + if (req.model < 0 || req.model >= static_cast(sl::DETECTION_MODEL::LAST)) + { + NODELET_ERROR_STREAM("Object Detection model not valid."); + res.done = false; return res.done; + } + mObjDetModel = static_cast(req.model); + + mObjDetMaxRange = req.max_range; + if (mObjDetMaxRange > mCamMaxDepth) + { + NODELET_WARN("Detection max range cannot be major than depth max range. Automatically fixed."); + mObjDetMaxRange = mCamMaxDepth; + } + NODELET_INFO_STREAM(" * Detection max range\t\t-> " << mObjDetMaxRange); + + NODELET_INFO_STREAM(" * Object min. confidence\t-> " << mObjDetConfidence); + NODELET_INFO_STREAM(" * Object tracking\t\t-> " << (mObjDetTracking ? "ENABLED" : "DISABLED")); + NODELET_INFO_STREAM(" * Detection model\t\t-> " << sl::toString(mObjDetModel)); + + if (mObjDetModel == sl::DETECTION_MODEL::HUMAN_BODY_FAST || mObjDetModel == sl::DETECTION_MODEL::HUMAN_BODY_ACCURATE) + { + mObjDetBodyFitting = req.sk_body_fitting; + NODELET_INFO_STREAM(" * Body fitting\t\t\t-> " << (mObjDetBodyFitting ? "ENABLED" : "DISABLED")); + } + else + { + mObjDetPeopleEnable = req.mc_people; + NODELET_INFO_STREAM(" * Detect people\t\t-> " << (mObjDetPeopleEnable ? "ENABLED" : "DISABLED")); + mObjDetVehiclesEnable = req.mc_vehicles; + NODELET_INFO_STREAM(" * Detect vehicles\t\t-> " << (mObjDetVehiclesEnable ? "ENABLED" : "DISABLED")); + mObjDetBagsEnable = req.mc_bag; + NODELET_INFO_STREAM(" * Detect bags\t\t\t-> " << (mObjDetBagsEnable ? "ENABLED" : "DISABLED")); + mObjDetAnimalsEnable = req.mc_animal; + NODELET_INFO_STREAM(" * Detect animals\t\t-> " << (mObjDetAnimalsEnable ? "ENABLED" : "DISABLED")); + mObjDetElectronicsEnable = req.mc_electronics; + NODELET_INFO_STREAM(" * Detect electronics\t\t-> " << (mObjDetElectronicsEnable ? "ENABLED" : "DISABLED")); + mObjDetFruitsEnable = req.mc_fruit_vegetable; + NODELET_INFO_STREAM(" * Detect fruit and vegetables\t-> " << (mObjDetFruitsEnable ? "ENABLED" : "DISABLED")); + } + + mObjDetRunning = false; + mObjDetEnabled = true; + res.done = true; + + return res.done; } -void ZEDWrapperNodelet::detectObjects(bool publishObj, bool publishViz, ros::Time t) { - static std::chrono::steady_clock::time_point old_time = std::chrono::steady_clock::now(); - - sl::ObjectDetectionRuntimeParameters objectTracker_parameters_rt; - objectTracker_parameters_rt.detection_confidence_threshold = mObjDetConfidence; - objectTracker_parameters_rt.object_class_filter = mObjDetFilter; - - sl::Objects objects; - - sl::ERROR_CODE objDetRes = mZed.retrieveObjects(objects, objectTracker_parameters_rt); - - if (objDetRes != sl::ERROR_CODE::SUCCESS) { - NODELET_WARN_STREAM("Object Detection error: " << sl::toString(objDetRes)); - return; - } - - if(!objects.is_new) - { - return; - } - - // ----> Diagnostic information update - std::chrono::steady_clock::time_point now = std::chrono::steady_clock::now(); - double elapsed_msec = std::chrono::duration_cast(now - old_time).count(); - mObjDetPeriodMean_msec->addValue(elapsed_msec); - old_time = now; - // <---- Diagnostic information update +/*! \brief Service callback to stop_object_detection service + */ +bool ZEDWrapperNodelet::on_stop_object_detection(zed_interfaces::stop_object_detection::Request& req, + zed_interfaces::stop_object_detection::Response& res) +{ + if (mObjDetEnabled) + { + mObjDetMutex.lock(); + stop_obj_detect(); + mObjDetMutex.unlock(); - // NODELET_DEBUG_STREAM("Detected " << objects.object_list.size() << " objects"); + res.done = true; + } + else + { + res.done = false; + } - size_t objCount = objects.object_list.size(); + return res.done; +} - zed_interfaces::Objects objMsg; +void ZEDWrapperNodelet::processDetectedObjects(ros::Time t) +{ + static std::chrono::steady_clock::time_point old_time = std::chrono::steady_clock::now(); + sl::ObjectDetectionRuntimeParameters objectTracker_parameters_rt; + objectTracker_parameters_rt.detection_confidence_threshold = mObjDetConfidence; + objectTracker_parameters_rt.object_class_filter = mObjDetFilter; - objMsg.objects.resize(objCount); + sl::Objects objects; - std_msgs::Header header; - header.stamp = t; - header.frame_id = mLeftCamFrameId; + sl::ERROR_CODE objDetRes = mZed.retrieveObjects(objects, objectTracker_parameters_rt); - visualization_msgs::MarkerArray objMarkersMsg; - //objMarkersMsg.markers.resize(objCount * 3); + if (objDetRes != sl::ERROR_CODE::SUCCESS) + { + NODELET_WARN_STREAM("Object Detection error: " << sl::toString(objDetRes)); + return; + } - for (size_t i = 0; i < objCount; i++) { - sl::ObjectData data = objects.object_list[i]; + if (!objects.is_new) + { + return; + } - if (publishObj) { - objMsg.objects[i].header = header; + // ----> Diagnostic information update + std::chrono::steady_clock::time_point now = std::chrono::steady_clock::now(); + double elapsed_msec = std::chrono::duration_cast(now - old_time).count(); + mObjDetPeriodMean_msec->addValue(elapsed_msec); + old_time = now; + // <---- Diagnostic information update - objMsg.objects[i].label = sl::toString(data.label).c_str(); - objMsg.objects[i].label_id = data.id; - objMsg.objects[i].confidence = data.confidence; + NODELET_DEBUG_STREAM("Detected " << objects.object_list.size() << " objects"); - objMsg.objects[i].tracking_state = static_cast(data.tracking_state); + size_t objCount = objects.object_list.size(); - objMsg.objects[i].position.x = data.position.x; - objMsg.objects[i].position.y = data.position.y; - objMsg.objects[i].position.z = data.position.z; + zed_interfaces::ObjectsStampedPtr objMsg = boost::make_shared(); + objMsg->header.stamp = t; + objMsg->header.frame_id = mLeftCamFrameId; - objMsg.objects[i].linear_vel.x = data.velocity.x; - objMsg.objects[i].linear_vel.y = data.velocity.y; - objMsg.objects[i].linear_vel.z = data.velocity.z; + objMsg->objects.resize(objCount); - for (int c = 0; c < data.bounding_box_2d.size(); c++) { - objMsg.objects[i].bbox_2d[c].x = data.bounding_box_2d[c].x; - objMsg.objects[i].bbox_2d[c].y = data.bounding_box_2d[c].y; - objMsg.objects[i].bbox_2d[c].z = 0.0f; - } + size_t idx = 0; + for (auto data : objects.object_list) + { + objMsg->objects[idx].label = sl::toString(data.label).c_str(); + objMsg->objects[idx].sublabel = sl::toString(data.sublabel).c_str(); + objMsg->objects[idx].label_id = data.id; + objMsg->objects[idx].confidence = data.confidence; - for (int c = 0; c < data.bounding_box.size(); c++) { - objMsg.objects[i].bbox_3d[c].x = data.bounding_box[c].x; - objMsg.objects[i].bbox_3d[c].y = data.bounding_box[c].y; - objMsg.objects[i].bbox_3d[c].z = data.bounding_box[c].z; - } - } + memcpy(&(objMsg->objects[idx].position[0]), &(data.position[0]), 3 * sizeof(float)); + memcpy(&(objMsg->objects[idx].position_covariance[0]), &(data.position_covariance[0]), 6 * sizeof(float)); + memcpy(&(objMsg->objects[idx].velocity[0]), &(data.velocity[0]), 3 * sizeof(float)); - if (publishViz) { - if( data.bounding_box.size()!=8 ) { - continue; // No 3D information available - } + objMsg->objects[idx].tracking_available = mObjDetTracking; + objMsg->objects[idx].tracking_state = static_cast(data.tracking_state); + //NODELET_INFO_STREAM( "[" << idx << "] Tracking: " << sl::toString(static_cast(data.tracking_state))); + objMsg->objects[idx].action_state = static_cast(data.action_state); - visualization_msgs::Marker bbx_marker; - - bbx_marker.header = header; - bbx_marker.ns = "bbox"; - bbx_marker.id = data.id; - bbx_marker.type = visualization_msgs::Marker::CUBE; - bbx_marker.action = visualization_msgs::Marker::ADD; - bbx_marker.pose.position.x = data.position.x; - bbx_marker.pose.position.y = data.position.y; - bbx_marker.pose.position.z = data.position.z; - bbx_marker.pose.orientation.x = 0.0; - bbx_marker.pose.orientation.y = 0.0; - bbx_marker.pose.orientation.z = 0.0; - bbx_marker.pose.orientation.w = 1.0; - - bbx_marker.scale.x = fabs(data.bounding_box[0].x - data.bounding_box[1].x); - bbx_marker.scale.y = fabs(data.bounding_box[0].y - data.bounding_box[3].y); - bbx_marker.scale.z = fabs(data.bounding_box[0].z - data.bounding_box[4].z); - sl::float3 color = generateColorClass(data.id); - bbx_marker.color.b = color.b; - bbx_marker.color.g = color.g; - bbx_marker.color.r = color.r; - bbx_marker.color.a = 0.4; - bbx_marker.lifetime = ros::Duration(0.3); - bbx_marker.frame_locked = true; - - objMarkersMsg.markers.push_back(bbx_marker); - - visualization_msgs::Marker label; - label.header = header; - label.lifetime = ros::Duration(0.3); - label.action = visualization_msgs::Marker::ADD; - label.id = data.id; - label.ns = "label"; - label.type = visualization_msgs::Marker::TEXT_VIEW_FACING; - label.scale.z = 0.1; - - label.color.r = 255-color.r; - label.color.g = 255-color.g; - label.color.b = 255-color.b; - label.color.a = 0.75f; - - label.pose.position.x = data.position.x; - label.pose.position.y = data.position.y; - label.pose.position.z = data.position.z+1.1*bbx_marker.scale.z/2; - - label.text = std::to_string(data.id) + ". " + std::string(sl::toString(data.label).c_str()); - - objMarkersMsg.markers.push_back(label); - } + if (data.bounding_box_2d.size() == 4) + { + memcpy(&(objMsg->objects[idx].bounding_box_2d.corners[0]), &(data.bounding_box_2d[0]), 8 * sizeof(unsigned int)); } - - if (publishObj) { - mPubObjDet.publish(objMsg); + if (data.bounding_box.size() == 8) + { + memcpy(&(objMsg->objects[idx].bounding_box_3d.corners[0]), &(data.bounding_box[0]), 24 * sizeof(float)); } - if (mPubObjDetViz) { - mPubObjDetViz.publish(objMarkersMsg); + memcpy(&(objMsg->objects[idx].dimensions_3d[0]), &(data.dimensions[0]), 3 * sizeof(float)); + + if (mObjDetModel == sl::DETECTION_MODEL::HUMAN_BODY_ACCURATE || + mObjDetModel == sl::DETECTION_MODEL::HUMAN_BODY_FAST) + { + objMsg->objects[idx].skeleton_available = true; + + if (data.head_bounding_box_2d.size() == 4) + { + memcpy(&(objMsg->objects[idx].head_bounding_box_2d.corners[0]), &(data.head_bounding_box_2d[0]), + 8 * sizeof(unsigned int)); + } + if (data.head_bounding_box.size() == 8) + { + memcpy(&(objMsg->objects[idx].head_bounding_box_3d.corners[0]), &(data.head_bounding_box[0]), + 24 * sizeof(float)); + } + memcpy(&(objMsg->objects[idx].head_position[0]), &(data.head_position[0]), 3 * sizeof(float)); + + if (data.keypoint_2d.size() == 18) + { + memcpy(&(objMsg->objects[idx].skeleton_2d.keypoints[0]), &(data.keypoint_2d[0]), 36 * sizeof(float)); + } + if (data.keypoint_2d.size() == 18) + { + memcpy(&(objMsg->objects[idx].skeleton_3d.keypoints[0]), &(data.keypoint[0]), 54 * sizeof(float)); + } + } + else + { + objMsg->objects[idx].skeleton_available = false; } + // at the end of the loop + idx++; + } + + mPubObjDet.publish(objMsg); } -} // namespace +} // namespace zed_nodelets diff --git a/zed_wrapper/.gitignore b/zed_wrapper/.gitignore index 7a8a3cd7..20e7ef7a 100644 --- a/zed_wrapper/.gitignore +++ b/zed_wrapper/.gitignore @@ -39,3 +39,7 @@ Makefile* # QtCtreator CMake CMakeLists.txt.user* +# VS code +.vscode +.vscode/* + diff --git a/zed_wrapper/CMakeLists.txt b/zed_wrapper/CMakeLists.txt index afa94b05..2d2c1c56 100644 --- a/zed_wrapper/CMakeLists.txt +++ b/zed_wrapper/CMakeLists.txt @@ -1,10 +1,7 @@ -cmake_minimum_required(VERSION 2.8.7) +cmake_minimum_required(VERSION 3.5) project(zed_wrapper) -#Fix for QtCretor -#list(APPEND CMAKE_PREFIX_PATH "/opt/ros/$ENV{ROS_DISTRO}/") - # if CMAKE_BUILD_TYPE is not specified, take 'Release' as default IF(NOT CMAKE_BUILD_TYPE) SET(CMAKE_BUILD_TYPE Release) diff --git a/zed_wrapper/params/common.yaml b/zed_wrapper/params/common.yaml index a200b0ff..ff4e7f02 100644 --- a/zed_wrapper/params/common.yaml +++ b/zed_wrapper/params/common.yaml @@ -1,4 +1,4 @@ -# params/common_yaml +# params/common.yaml # Common parameters to Stereolabs ZED and ZED mini cameras --- @@ -20,7 +20,7 @@ pub_frame_rate: 15.0 # Dynamic - freq point_cloud_freq: 15.0 # Dynamic - frequency of the pointcloud publishing (equal or less to `grab_frame_rate` value) general: - camera_name: zed # A name for the camera (can be different from camera model and node name) + camera_name: zed # A name for the camera (can be different from camera model and node name and can be overwritten by the launch file) zed_id: 0 serial_number: 0 resolution: 2 # '0': HD2K, '1': HD1080, '2': HD720, '3': VGA @@ -37,7 +37,7 @@ video: extrinsic_in_camera_frame: true # if `false` extrinsic parameter in `camera_info` will use ROS native frame (X FORWARD, Z UP) instead of the camera frame (Z FORWARD, Y DOWN) [`true` use old behavior as for version < v3.1] depth: - quality: 1 # '0': NONE, '1': PERFORMANCE, '2': QUALITY, '3': ULTRA + quality: 3 # '0': NONE, '1': PERFORMANCE, '2': QUALITY, '3': ULTRA sensing_mode: 0 # '0': STANDARD, '1': FILL (not use FILL for robotic applications) depth_stabilization: 1 # `0`: disabled, `1`: enabled openni_depth_mode: false # 'false': 32bit float meters, 'true': 16bit uchar millimeters diff --git a/zed_wrapper/params/zed2.yaml b/zed_wrapper/params/zed2.yaml index 2122cde3..a076c0cf 100644 --- a/zed_wrapper/params/zed2.yaml +++ b/zed_wrapper/params/zed2.yaml @@ -1,4 +1,4 @@ -# params/zed2_yaml +# params/zed2.yaml # Parameters for Stereolabs ZED2 camera --- @@ -18,7 +18,14 @@ sensors: object_detection: od_enabled: false # True to enable Object Detection [only ZED 2] + model: 0 # '0': MULTI_CLASS_BOX - '1': MULTI_CLASS_BOX_ACCURATE - '2': HUMAN_BODY_FAST - '3': HUMAN_BODY_ACCURATE confidence_threshold: 50 # Minimum value of the detection confidence of an object [0,100] + max_range: 15. # Maximum detection range object_tracking_enabled: true # Enable/disable the tracking of the detected objects - people_detection: true # Enable/disable the detection of persons - vehicle_detection: false # Enable/disable the detection of vehicles + body_fitting: false # Enable/disable body fitting for 'HUMAN_BODY_FAST' and 'HUMAN_BODY_ACCURATE' models + mc_people: true # Enable/disable the detection of persons for 'MULTI_CLASS_BOX' and 'MULTI_CLASS_BOX_ACCURATE' models + mc_vehicle: true # Enable/disable the detection of vehicles for 'MULTI_CLASS_BOX' and 'MULTI_CLASS_BOX_ACCURATE' models + mc_bag: true # Enable/disable the detection of bags for 'MULTI_CLASS_BOX' and 'MULTI_CLASS_BOX_ACCURATE' models + mc_animal: true # Enable/disable the detection of animals for 'MULTI_CLASS_BOX' and 'MULTI_CLASS_BOX_ACCURATE' models + mc_electronics: true # Enable/disable the detection of electronic devices for 'MULTI_CLASS_BOX' and 'MULTI_CLASS_BOX_ACCURATE' models + mc_fruit_vegetable: true # Enable/disable the detection of fruits and vegetables for 'MULTI_CLASS_BOX' and 'MULTI_CLASS_BOX_ACCURATE' models diff --git a/zed_wrapper/src/zed_wrapper_node.cpp b/zed_wrapper/src/zed_wrapper_node.cpp index 2134a00e..050fb02f 100644 --- a/zed_wrapper/src/zed_wrapper_node.cpp +++ b/zed_wrapper/src/zed_wrapper_node.cpp @@ -22,16 +22,17 @@ #include #include -int main(int argc, char** argv) { - ros::init(argc, argv, "zed_camera_node"); +int main(int argc, char** argv) +{ + ros::init(argc, argv, "zed_camera_node"); - // Start the ZED Nodelet - nodelet::Loader nodelet; - nodelet::M_string remap(ros::names::getRemappings()); - nodelet::V_string nargv; - nodelet.load(ros::this_node::getName(), "zed_nodelets/ZEDWrapperNodelet", remap, nargv); + // Start the ZED Nodelet + nodelet::Loader nodelet; + nodelet::M_string remap(ros::names::getRemappings()); + nodelet::V_string nargv; + nodelet.load(ros::this_node::getName(), "zed_nodelets/ZEDWrapperNodelet", remap, nargv); - ros::spin(); + ros::spin(); - return 0; + return 0; } From 55df6d03f442b438e1e695ca92750a3684713871 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Thu, 25 Feb 2021 16:16:55 +0100 Subject: [PATCH 028/199] Update latest_changes.md --- latest_changes.md | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/latest_changes.md b/latest_changes.md index 0d23ef0f..b4579d2c 100644 --- a/latest_changes.md +++ b/latest_changes.md @@ -1,10 +1,13 @@ LATEST CHANGES ============== -v3.4 +v3.4.x --------- - Add support for new DEPTH16_MM data type for depth (OPENNI MODE) - Fix issue #660: detected objects topic not published if depth computation not active +- Improved support for ZED Object Detection +- Add Skeleton Tracking support +- New Rviz plugin for Object Detection in `zed-ros-examples` RGB/Depth sync fix #629 (2020-11-02) ------------------------------- From 3b3a075806e18f169e8b1846d4a4ea9bcd7c6c31 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Tue, 2 Mar 2021 14:55:42 +0100 Subject: [PATCH 029/199] Update latest_changes.md --- latest_changes.md | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/latest_changes.md b/latest_changes.md index b4579d2c..8c4a862f 100644 --- a/latest_changes.md +++ b/latest_changes.md @@ -8,6 +8,16 @@ v3.4.x - Improved support for ZED Object Detection - Add Skeleton Tracking support - New Rviz plugin for Object Detection in `zed-ros-examples` +- New parameters and name changing to fit the new OD features, also the `start_object_detection` service has been modified to match the new features: + - new `model` parameter to choose the AI model + - new `max_range` parameter to limit the detection range + - new `sk_body_fitting` parameter to enable Skeleton fitting for skeleton AI models + - `people` -> `mc_people` to indicate that it is related to multiclass AI models + - `vehicles`-> `mc_vehicles` to indicate that it is related to multiclass AI models + - new `mc_bag` parameter to enable bags detection with multiclass AI models + - new `mc_animal` parameter to enable animals detection with multiclass AI models + - new `mc_electronics` parameter to enable electronic devices detection with multiclass AI models + - new `mc_fruit_vegetable` parameter to enable fruits and vegetables detection with multiclass AI models RGB/Depth sync fix #629 (2020-11-02) ------------------------------- From af3a77a3861e265b19b3429e206ee4d47d3f78a9 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Tue, 23 Mar 2021 14:54:33 +0100 Subject: [PATCH 030/199] Fix `TF_REPEATED_DATA` warning issue #681 --- README.md | 8 +++---- .../zed_nodelet/src/zed_wrapper_nodelet.cpp | 24 +++++++++---------- zed_wrapper/params/common.yaml | 2 +- 3 files changed, 17 insertions(+), 17 deletions(-) diff --git a/README.md b/README.md index c99416e4..63b240bf 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,6 @@ ![](./images/Picto+STEREOLABS_Black.jpg) -# Stereolabs ZED Camera - ROS Integration +# Stereolabs ZED Camera - ROS Noetic Ninjemis Integration This package lets you use the ZED stereo camera with ROS. It outputs the camera left and right images, depth map, point cloud, pose information and supports the use of multiple ZED cameras. @@ -14,9 +14,9 @@ This package lets you use the ZED stereo camera with ROS. It outputs the camera ### Prerequisites -- Ubuntu 16.04 or newer (Ubuntu 18 recommended) -- [ZED SDK **≥ 3.0**](https://www.stereolabs.com/developers/) and its dependency [CUDA](https://developer.nvidia.com/cuda-downloads) -- [ROS Kinetic](http://wiki.ros.org/kinetic/Installation/Ubuntu) or [ROS Melodic](http://wiki.ros.org/melodic/Installation/Ubuntu) +- Ubuntu 20.04 +- [ZED SDK **≥ 3.4**](https://www.stereolabs.com/developers/) and its dependency [CUDA](https://developer.nvidia.com/cuda-downloads) +- [ROS Noetic](http://wiki.ros.org/noetic/Installation/Ubuntu) *Note:* an older version of the wrapper compatible with the **SDK v2.8.x** is available [here](https://github.com/stereolabs/zed-ros-wrapper/releases/tag/v2.x) 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 c68e9746..81d2fbdd 100644 --- a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp @@ -1813,7 +1813,7 @@ void ZEDWrapperNodelet::publishOdomFrame(tf2::Transform odomTransf, ros::Time t) // Publish transformation mTransformOdomBroadcaster.sendTransform(transformStamped); - // NODELET_INFO_STREAM( "Published ODOM TF with TS: " << t ); + //NODELET_INFO_STREAM( "Published ODOM TF with TS: " << t ); } void ZEDWrapperNodelet::publishPoseFrame(tf2::Transform baseTransform, ros::Time t) @@ -2999,8 +2999,18 @@ void ZEDWrapperNodelet::publishSensData(ros::Time t) ts_mag = sl_tools::slTime2Ros(sens_data.magnetometer.timestamp); } + bool new_imu_data = ts_imu != lastTs_imu; + bool new_baro_data = ts_baro != lastTs_baro; + bool new_mag_data = ts_mag != lastT_mag; + + if (!new_imu_data && !new_baro_data && !new_mag_data) + { + NODELET_DEBUG("No updated sensors data"); + return; + } + // ----> Publish odometry tf only if enabled - if (mPublishTf && mTrackingReady) + if (mPublishTf && mTrackingReady && new_imu_data) { NODELET_DEBUG("Publishing TF"); @@ -3018,16 +3028,6 @@ void ZEDWrapperNodelet::publishSensData(ros::Time t) } // <---- Publish odometry tf only if enabled - bool new_imu_data = ts_imu != lastTs_imu; - bool new_baro_data = ts_baro != lastTs_baro; - bool new_mag_data = ts_mag != lastT_mag; - - if (!new_imu_data && !new_baro_data && !new_mag_data) - { - NODELET_DEBUG("No updated sensors data"); - return; - } - if (mZedRealCamModel == sl::MODEL::ZED2) { // Update temperatures for Diagnostic diff --git a/zed_wrapper/params/common.yaml b/zed_wrapper/params/common.yaml index ff4e7f02..4584473d 100644 --- a/zed_wrapper/params/common.yaml +++ b/zed_wrapper/params/common.yaml @@ -37,7 +37,7 @@ video: extrinsic_in_camera_frame: true # if `false` extrinsic parameter in `camera_info` will use ROS native frame (X FORWARD, Z UP) instead of the camera frame (Z FORWARD, Y DOWN) [`true` use old behavior as for version < v3.1] depth: - quality: 3 # '0': NONE, '1': PERFORMANCE, '2': QUALITY, '3': ULTRA + quality: 1 # '0': NONE, '1': PERFORMANCE, '2': QUALITY, '3': ULTRA sensing_mode: 0 # '0': STANDARD, '1': FILL (not use FILL for robotic applications) depth_stabilization: 1 # `0`: disabled, `1`: enabled openni_depth_mode: false # 'false': 32bit float meters, 'true': 16bit uchar millimeters From 9cc2aa22491ac00dc1aa25ee9e2a8a7d6b0b9d6b Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Tue, 23 Mar 2021 15:18:30 +0100 Subject: [PATCH 031/199] Fix duplicated IMU RAW data publishing Minor fixes --- .../zed_nodelet/src/zed_wrapper_nodelet.cpp | 26 ++++++++++++++++++- 1 file changed, 25 insertions(+), 1 deletion(-) 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 81d2fbdd..c25ce0c7 100644 --- a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp @@ -1789,6 +1789,16 @@ void ZEDWrapperNodelet::publishStaticImuFrame() void ZEDWrapperNodelet::publishOdomFrame(tf2::Transform odomTransf, ros::Time t) { + // ----> Avoid duplicated TF publishing + static ros::Time last_stamp; + + if( t==last_stamp ) + { + return; + } + last_stamp = t; + // <---- Avoid duplicated TF publishing + if (!mSensor2BaseTransfValid) { getSens2BaseTransform(); @@ -1818,6 +1828,16 @@ void ZEDWrapperNodelet::publishOdomFrame(tf2::Transform odomTransf, ros::Time t) void ZEDWrapperNodelet::publishPoseFrame(tf2::Transform baseTransform, ros::Time t) { + // ----> Avoid duplicated TF publishing + static ros::Time last_stamp; + + if( t==last_stamp ) + { + return; + } + last_stamp = t; + // <---- Avoid duplicated TF publishing + if (!mSensor2BaseTransfValid) { getSens2BaseTransform(); @@ -3035,8 +3055,10 @@ void ZEDWrapperNodelet::publishSensData(ros::Time t) sens_data.temperature.get(sl::SensorsData::TemperatureData::SENSOR_LOCATION::ONBOARD_RIGHT, mTempRight); } - if (imu_TempSubNumber > 0) + if (imu_TempSubNumber > 0 && new_imu_data) { + lastTs_imu = ts_imu; + sensor_msgs::TemperaturePtr imuTempMsg = boost::make_shared(); imuTempMsg->header.stamp = ts_imu; @@ -3262,6 +3284,8 @@ void ZEDWrapperNodelet::publishSensData(ros::Time t) if (imu_RawSubNumber > 0 && new_imu_data) { + lastTs_imu = ts_imu; + sensor_msgs::ImuPtr imuRawMsg = boost::make_shared(); imuRawMsg->header.stamp = ts_imu; From 845abe05fb4a16348c3fdd70eac18b39a3b116b4 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Tue, 23 Mar 2021 17:00:13 +0100 Subject: [PATCH 032/199] Update .gitlab-ci.yml --- .gitlab-ci.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml index f702a38b..75b1d044 100644 --- a/.gitlab-ci.yml +++ b/.gitlab-ci.yml @@ -1,4 +1,4 @@ -image: stereolabs/zed:3.0-ros-cuda10.0-ubuntu18.04 +image: stereolabs/zed:3.4-ros-devel-cuda11.1-ubuntu18.04 build: stage: build From 6c11b36aeb1208614933493d793e33974be84576 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Tue, 23 Mar 2021 17:00:34 +0100 Subject: [PATCH 033/199] Update .gitlab-ci.yml --- .gitlab-ci.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml index f702a38b..75b1d044 100644 --- a/.gitlab-ci.yml +++ b/.gitlab-ci.yml @@ -1,4 +1,4 @@ -image: stereolabs/zed:3.0-ros-cuda10.0-ubuntu18.04 +image: stereolabs/zed:3.4-ros-devel-cuda11.1-ubuntu18.04 build: stage: build From 4f3bfe82eb3535d3c0dac946180392acc4440fc1 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Tue, 23 Mar 2021 17:35:09 +0100 Subject: [PATCH 034/199] Disable automatic object detection --- zed_wrapper/params/zed2.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/zed_wrapper/params/zed2.yaml b/zed_wrapper/params/zed2.yaml index a076c0cf..3c289deb 100644 --- a/zed_wrapper/params/zed2.yaml +++ b/zed_wrapper/params/zed2.yaml @@ -6,7 +6,7 @@ general: camera_model: 'zed2' depth: - min_depth: 0.7 # Min: 0.2, Max: 3.0 - Default 0.7 - Note: reducing this value wil require more computational power and GPU memory + min_depth: 0.3 # Min: 0.2, Max: 3.0 - Default 0.7 - Note: reducing this value wil require more computational power and GPU memory max_depth: 20.0 # Max: 40.0 pos_tracking: @@ -18,7 +18,7 @@ sensors: object_detection: od_enabled: false # True to enable Object Detection [only ZED 2] - model: 0 # '0': MULTI_CLASS_BOX - '1': MULTI_CLASS_BOX_ACCURATE - '2': HUMAN_BODY_FAST - '3': HUMAN_BODY_ACCURATE + model: 3 # '0': MULTI_CLASS_BOX - '1': MULTI_CLASS_BOX_ACCURATE - '2': HUMAN_BODY_FAST - '3': HUMAN_BODY_ACCURATE confidence_threshold: 50 # Minimum value of the detection confidence of an object [0,100] max_range: 15. # Maximum detection range object_tracking_enabled: true # Enable/disable the tracking of the detected objects From b6c66f81840698e62cde89c5d20b4970b617bd44 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Mon, 17 May 2021 17:39:09 +0200 Subject: [PATCH 035/199] Add new param to start Positional Tracking when the node starts Fix wrong behavior not publishing TF when point cloud, odom or pose were not subscribed --- README.md | 6 +++- latest_changes.md | 5 +++ .../include/zed_wrapper_nodelet.hpp | 9 ++--- .../zed_nodelet/src/zed_wrapper_nodelet.cpp | 36 ++++++++++--------- zed_wrapper/params/common.yaml | 1 + 5 files changed, 35 insertions(+), 22 deletions(-) diff --git a/README.md b/README.md index 63b240bf..bb45dc32 100644 --- a/README.md +++ b/README.md @@ -15,9 +15,13 @@ This package lets you use the ZED stereo camera with ROS. It outputs the camera ### Prerequisites - Ubuntu 20.04 -- [ZED SDK **≥ 3.4**](https://www.stereolabs.com/developers/) and its dependency [CUDA](https://developer.nvidia.com/cuda-downloads) +- [ZED SDK **≥ 3.5**](https://www.stereolabs.com/developers/) and its dependency [CUDA](https://developer.nvidia.com/cuda-downloads) - [ROS Noetic](http://wiki.ros.org/noetic/Installation/Ubuntu) +- Ubuntu 18.04 +- [ZED SDK **≥ 3.5**](https://www.stereolabs.com/developers/) and its dependency [CUDA](https://developer.nvidia.com/cuda-downloads) +- [ROS Melodic](http://wiki.ros.org/melodic/Installation/Ubuntu) + *Note:* an older version of the wrapper compatible with the **SDK v2.8.x** is available [here](https://github.com/stereolabs/zed-ros-wrapper/releases/tag/v2.x) ### Build the program diff --git a/latest_changes.md b/latest_changes.md index 8c4a862f..95f0ab30 100644 --- a/latest_changes.md +++ b/latest_changes.md @@ -1,6 +1,11 @@ LATEST CHANGES ============== +v3.5.x +--------- +- Add support for ROS Noetic +- Add new parameter `pos_tracking/pos_tracking_enabled` to enable positional tracking from start even if not required by any subscribed topic. This is useful, for example, to keep the TF always updated. + v3.4.x --------- - Add support for new DEPTH16_MM data type for depth (OPENNI MODE) 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 a3535d0e..6459be59 100644 --- a/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp +++ b/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp @@ -532,9 +532,10 @@ class ZEDWrapperNodelet : public nodelet::Nodelet double mCamMinDepth; double mCamMaxDepth; - bool mTrackingActivated; - - bool mTrackingReady; + // Positional tracking + bool mPosTrackingEnabled=false; + bool mPosTrackingActivated=false; + bool mPosTrackingReady=false; bool mTwoDMode = false; double mFixedZValue = 0.0; bool mFloorAlignment = false; @@ -608,7 +609,7 @@ class ZEDWrapperNodelet : public nodelet::Nodelet double mCamImageResizeFactor = 1.0; double mCamDepthResizeFactor = 1.0; - // flags + // flags bool mTriggerAutoExposure = true; bool mTriggerAutoWB = true; bool mComputeDepth; 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 c25ce0c7..c0d5fd12 100644 --- a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp @@ -712,6 +712,8 @@ void ZEDWrapperNodelet::readParameters() NODELET_INFO_STREAM("*** POSITIONAL TRACKING PARAMETERS ***"); // ----> Tracking + mNhNs.param("pos_tracking/pos_tracking_enabled", mPosTrackingEnabled, true); + NODELET_INFO_STREAM(" * Positional tracking\t\t-> " << (mPosTrackingEnabled ? "ENABLED" : "DISABLED")); 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); @@ -1322,7 +1324,7 @@ bool ZEDWrapperNodelet::on_set_pose(zed_interfaces::set_pose::Request& req, zed_ std::lock_guard lock(mPosTrkMutex); // Disable tracking - mTrackingActivated = false; + mPosTrackingActivated = false; mZed.disablePositionalTracking(); // Restart tracking @@ -1335,7 +1337,7 @@ bool ZEDWrapperNodelet::on_set_pose(zed_interfaces::set_pose::Request& req, zed_ bool ZEDWrapperNodelet::on_reset_tracking(zed_interfaces::reset_tracking::Request& req, zed_interfaces::reset_tracking::Response& res) { - if (!mTrackingActivated) + if (!mPosTrackingActivated) { res.reset_done = false; return false; @@ -1351,7 +1353,7 @@ bool ZEDWrapperNodelet::on_reset_tracking(zed_interfaces::reset_tracking::Reques std::lock_guard lock(mPosTrkMutex); // Disable tracking - mTrackingActivated = false; + mPosTrackingActivated = false; mZed.disablePositionalTracking(); // Restart tracking @@ -1627,11 +1629,11 @@ void ZEDWrapperNodelet::start_pos_tracking() if (err == sl::ERROR_CODE::SUCCESS) { - mTrackingActivated = true; + mPosTrackingActivated = true; } else { - mTrackingActivated = false; + mPosTrackingActivated = false; NODELET_WARN("Tracking not activated: %s", sl::toString(err).c_str()); } @@ -3030,7 +3032,7 @@ void ZEDWrapperNodelet::publishSensData(ros::Time t) } // ----> Publish odometry tf only if enabled - if (mPublishTf && mTrackingReady && new_imu_data) + if (mPublishTf && mPosTrackingReady && new_imu_data) { NODELET_DEBUG("Publishing TF"); @@ -3371,7 +3373,7 @@ void ZEDWrapperNodelet::device_poll_thread_func() mPrevFrameTimestamp = mFrameTimestamp; - mTrackingActivated = false; + mPosTrackingActivated = false; mMappingRunning = false; mRecording = false; @@ -3441,7 +3443,7 @@ void ZEDWrapperNodelet::device_poll_thread_func() } mGrabActive = - mRecording || mStreaming || mMappingEnabled || mObjDetEnabled || mTrackingActivated || + mRecording || mStreaming || mMappingEnabled || mObjDetEnabled || mPosTrackingActivated || ((rgbSubnumber + rgbRawSubnumber + leftSubnumber + leftRawSubnumber + rightSubnumber + rightRawSubnumber + rgbGraySubnumber + rgbGrayRawSubnumber + leftGraySubnumber + leftGrayRawSubnumber + rightGraySubnumber + rightGrayRawSubnumber + depthSubnumber + disparitySubnumber + cloudSubnumber + poseSubnumber + @@ -3454,11 +3456,11 @@ void ZEDWrapperNodelet::device_poll_thread_func() std::lock_guard lock(mPosTrkMutex); // Note: ones tracking is started it is never stopped anymore to not lose tracking information - bool computeTracking = (mMappingEnabled || mObjDetEnabled || (mComputeDepth & mDepthStabilization) || + bool computeTracking = (mPosTrackingEnabled || mPosTrackingActivated || mMappingEnabled || mObjDetEnabled || (mComputeDepth & mDepthStabilization) || poseSubnumber > 0 || poseCovSubnumber > 0 || odomSubnumber > 0 || pathSubNumber > 0); // Start the tracking? - if ((computeTracking) && !mTrackingActivated && (mDepthMode != sl::DEPTH_MODE::NONE)) + if ((computeTracking) && !mPosTrackingActivated && (mDepthMode != sl::DEPTH_MODE::NONE)) { start_pos_tracking(); } @@ -3481,8 +3483,8 @@ void ZEDWrapperNodelet::device_poll_thread_func() // Detect if one of the subscriber need to have the depth information mComputeDepth = mDepthMode != sl::DEPTH_MODE::NONE && - ((depthSubnumber + disparitySubnumber + cloudSubnumber + fusedCloudSubnumber + poseSubnumber + - poseCovSubnumber + odomSubnumber + confMapSubnumber + objDetSubnumber) > 0); + (computeTracking || ((depthSubnumber + disparitySubnumber + cloudSubnumber + fusedCloudSubnumber + poseSubnumber + + poseCovSubnumber + odomSubnumber + confMapSubnumber + objDetSubnumber) > 0)); if (mComputeDepth) { @@ -3561,9 +3563,9 @@ void ZEDWrapperNodelet::device_poll_thread_func() std::this_thread::sleep_for(std::chrono::milliseconds(2000)); } - mTrackingActivated = false; + mPosTrackingActivated = false; - computeTracking = mDepthStabilization || poseSubnumber > 0 || poseCovSubnumber > 0 || odomSubnumber > 0; + computeTracking = mPosTrackingEnabled || mDepthStabilization || poseSubnumber > 0 || poseCovSubnumber > 0 || odomSubnumber > 0; if (computeTracking) { // Start the tracking @@ -3853,7 +3855,7 @@ void ZEDWrapperNodelet::device_poll_thread_func() publishOdom(mOdom2BaseTransf, deltaOdom, stamp); } - mTrackingReady = true; + mPosTrackingReady = true; } } else if (mFloorAlignment) @@ -3980,7 +3982,7 @@ void ZEDWrapperNodelet::device_poll_thread_func() publishPose(stamp); } - mTrackingReady = true; + mPosTrackingReady = true; } oldStatus = mPosTrackingStatus; @@ -4186,7 +4188,7 @@ void ZEDWrapperNodelet::callback_updateDiagnostic(diagnostic_updater::Diagnostic } } - if (mTrackingActivated) + if (mPosTrackingActivated) { stat.addf("Tracking status", "%s", sl::toString(mPosTrackingStatus).c_str()); } diff --git a/zed_wrapper/params/common.yaml b/zed_wrapper/params/common.yaml index 4584473d..705c1943 100644 --- a/zed_wrapper/params/common.yaml +++ b/zed_wrapper/params/common.yaml @@ -44,6 +44,7 @@ depth: depth_downsample_factor: 1.0 # Resample factor for depth data matrices [0.01,1.0] The SDK works with native data sizes, but publishes rescaled matrices (depth map, point cloud, ...) pos_tracking: + pos_tracking_enabled: true # True to enable positional tracking from start publish_tf: true # publish `odom -> base_link` TF publish_map_tf: true # publish `map -> odom` TF map_frame: 'map' From 29ff7ac21eaf980112aaf863cceb03538f6ad962 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Wed, 19 May 2021 09:11:53 +0200 Subject: [PATCH 036/199] Improve Positional Tracking start behavior --- zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 c0d5fd12..6d35dcd0 100644 --- a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp @@ -3443,7 +3443,7 @@ void ZEDWrapperNodelet::device_poll_thread_func() } mGrabActive = - mRecording || mStreaming || mMappingEnabled || mObjDetEnabled || mPosTrackingActivated || + mRecording || mStreaming || mMappingEnabled || mObjDetEnabled || mPosTrackingEnabled || mPosTrackingActivated || ((rgbSubnumber + rgbRawSubnumber + leftSubnumber + leftRawSubnumber + rightSubnumber + rightRawSubnumber + rgbGraySubnumber + rgbGrayRawSubnumber + leftGraySubnumber + leftGrayRawSubnumber + rightGraySubnumber + rightGrayRawSubnumber + depthSubnumber + disparitySubnumber + cloudSubnumber + poseSubnumber + From fe6f8762a4b08b6b2f08f22a72c15c2dc26130f6 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Fri, 28 May 2021 13:06:59 +0200 Subject: [PATCH 037/199] Update changelog --- latest_changes.md | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/latest_changes.md b/latest_changes.md index 95f0ab30..5522270b 100644 --- a/latest_changes.md +++ b/latest_changes.md @@ -4,7 +4,11 @@ LATEST CHANGES v3.5.x --------- - Add support for ROS Noetic +- Add support for SDK v3.5 +- Add support for the new ZED 2i - Add new parameter `pos_tracking/pos_tracking_enabled` to enable positional tracking from start even if not required by any subscribed topic. This is useful, for example, to keep the TF always updated. +- Add new example to start multiple ZED Nodelets inside the same nodelet manager +- Fixed issue #690 v3.4.x --------- From 3505fbed0c0ec4836e2ac9735861c7edc3fd0052 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Fri, 28 May 2021 13:08:18 +0200 Subject: [PATCH 038/199] add launch file and param file for ZED 2i --- zed_wrapper/launch/zed2i.launch | 55 ++++++++++++++++++++++++++++++ zed_wrapper/params/zed2i.yaml | 31 +++++++++++++++++ zed_wrapper/urdf/models/zed2i.stl | Bin 0 -> 48684 bytes 3 files changed, 86 insertions(+) create mode 100644 zed_wrapper/launch/zed2i.launch create mode 100644 zed_wrapper/params/zed2i.yaml create mode 100644 zed_wrapper/urdf/models/zed2i.stl diff --git a/zed_wrapper/launch/zed2i.launch b/zed_wrapper/launch/zed2i.launch new file mode 100644 index 00000000..9fd6025c --- /dev/null +++ b/zed_wrapper/launch/zed2i.launch @@ -0,0 +1,55 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/zed_wrapper/params/zed2i.yaml b/zed_wrapper/params/zed2i.yaml new file mode 100644 index 00000000..acac6ad9 --- /dev/null +++ b/zed_wrapper/params/zed2i.yaml @@ -0,0 +1,31 @@ +# params/zed2i.yaml +# Parameters for Stereolabs ZED2 camera +--- + +general: + camera_model: 'zed2i' + +depth: + min_depth: 0.3 # Min: 0.2, Max: 3.0 - Default 0.7 - Note: reducing this value wil require more computational power and GPU memory + max_depth: 20.0 # Max: 40.0 + +pos_tracking: + imu_fusion: true # enable/disable IMU fusion. When set to false, only the optical odometry will be used. + +sensors: + sensors_timestamp_sync: false # Synchronize Sensors messages timestamp with latest received frame + publish_imu_tf: true # publish `IMU -> _left_camera_frame` TF + +object_detection: + od_enabled: false # True to enable Object Detection [only ZED 2] + model: 3 # '0': MULTI_CLASS_BOX - '1': MULTI_CLASS_BOX_ACCURATE - '2': HUMAN_BODY_FAST - '3': HUMAN_BODY_ACCURATE + confidence_threshold: 50 # Minimum value of the detection confidence of an object [0,100] + max_range: 15. # Maximum detection range + object_tracking_enabled: true # Enable/disable the tracking of the detected objects + body_fitting: false # Enable/disable body fitting for 'HUMAN_BODY_FAST' and 'HUMAN_BODY_ACCURATE' models + mc_people: true # Enable/disable the detection of persons for 'MULTI_CLASS_BOX' and 'MULTI_CLASS_BOX_ACCURATE' models + mc_vehicle: true # Enable/disable the detection of vehicles for 'MULTI_CLASS_BOX' and 'MULTI_CLASS_BOX_ACCURATE' models + mc_bag: true # Enable/disable the detection of bags for 'MULTI_CLASS_BOX' and 'MULTI_CLASS_BOX_ACCURATE' models + mc_animal: true # Enable/disable the detection of animals for 'MULTI_CLASS_BOX' and 'MULTI_CLASS_BOX_ACCURATE' models + mc_electronics: true # Enable/disable the detection of electronic devices for 'MULTI_CLASS_BOX' and 'MULTI_CLASS_BOX_ACCURATE' models + mc_fruit_vegetable: true # Enable/disable the detection of fruits and vegetables for 'MULTI_CLASS_BOX' and 'MULTI_CLASS_BOX_ACCURATE' models diff --git a/zed_wrapper/urdf/models/zed2i.stl b/zed_wrapper/urdf/models/zed2i.stl new file mode 100644 index 0000000000000000000000000000000000000000..913d2cd9657a77cda8be4f1be69c8728a6cce9a5 GIT binary patch literal 48684 zcmbuId%RUuz4o`4R|qd5nuO*nh^Yty8b@W#JynRXP*KkT6)z~l2DYG7Mkrn}QK45X zO}wNaHvth5!XvD;CLJQIlcMImG&QmCyp?pkrv+y|qw(;0#%*mspR@n^edh1^{+==B z8gtIE=NfCyf1A+qozdfmf3xeq4LhalIbR#p^?&*Iqn+g2Mw>HoaA zM{V)os_}LS%_x!f=>F06>LDL(Hy-L&e`k-{m(Sic3;UrNCDhw()sk(it2_VnOBs5u zem(msDmCDhyX7(XhjzVTddg{?g-RJcA%eVD2 z9_lx;GH`9T_fCv>s7n1B5*7AC{T|-ZvC{4BJmR4$t(zvw-d6Z|lr7xhx5xa2m5bl$ z9q~|=)=d-ZrhHiW&4>G%TI!c34k$+%Wribk;26q4Z}Xwv;21$Gp;MkDH!4AzS`}MDqFXTBlxoscE44nABJ{4RAu*%QK?^=u%ChPP}(IlqlEqZBdt=E9ZNOz zBTZ;V2|JE!*l%vDRAr9|SMQcxXlmdq$0TD50uyAL2?B@lch$0`T)l5L!zKd%cQ`D^=Mmnej*yno+`DcOxFE zvR7nte547@C}HOh5f4?_nT7F46Pi&%=RxjB)OnPuly->@*S0jTn|n{OY~|?Y{f@q= zUdH*&hfZc(Aa)5=DQ)8%m0aSMpG}5G{>F*_+N^}#USPXRL{v&gJkGnSrFq0Vi;Dle zc#vs(kCzq{KN<4RKApUEanZ8(P|xG-D>F5l*d;V_x|JlT2r}4UKp{W zIO~X|HMdnzt-WDI@lH!q9q}7yK2&sF)1yANtTgexhl=IhcdokxYSpLAFWzq+?zBo< z8xnL^zqAMT)z{`ME&lq<;ZCb|FIzZ$@{;1X9}V$ZHTjyB=J6Yr76)B^m}wRF5BzqW zwyaq4xud;(rS95u^>+KAwGbcw*2BfyT{|?Zw@Y*@`_XIBqT)AS7+lx3gYJCM!s6uJ zMtS{ELR+G=>(R6H=-V={*s{wAv#(h9z|!O7-_I@narGq6L))vL8d}BumiEvVru*?> z`FR}JX-;wQrx(_>CHt2i&keq>xOUBDUaNGEB0hNf%HqfSt;hzKBk{FEmKPJR-niPf=-$;nrTd}1MO^*Ms$%`qA61?x_u)xDT2b`; z_+GE)(ke6BZQIS6=&c2Fit6?Yn%!1`U`Be|jQw`nh`GhI+28aCZLhX4-70N6;#WUd zRjhsL&FY-1Tbhr2_K{-csYS(Ym7YtqC7^k>a|v}=>kkKX{zIAc_Nynky*u{9gaKwsy z^MIvUUVa9Lu6iWj`|aJb&ZS@f$5-Zk_n7C6E41Y5Gat&2s`qGqr1U%Ny@&EW4%@l; z3+48yH>}7%9pB`Q5_l9-=I7g*hdZs(*1A2{y4u3D2hM%ZPhOJmd)ts^?4M2dEzL*0 zaJU)yY_01tvTW6q4NLQ-Qx0?X5Bj&~v}O6fe(q>*Uun5fEf1H!}-{QI@Eo? zxW_4F+wWSqDBu3Y!OhwyRO&H$VZLCeQQqEFLR+P@+bUcQZW=Q$-+i|cX8*7*M&;1W zbMx6(O!7Rm?fR*qRq=79th}${_b_#%&dlcJ@jDDwYO*$ykavY!kqZN6}yZ4%&bC0+l;%-K}ZF?Qt|F~dIUS18n(XJWk zt+`T{SA)E~8hC`ZMEe9f-YOi=zx-fTzWKl3tl*3~_}NGDYugo7?VsBZJ(t+CsA&lZ zp6y&h>uL+r9(wN6bE3A^^-z`ir3u};+7IoY>!Ig9Jtt~wT>}4SJpE|r{K0E3tROyb zUB?~qw$_gM^H=@08WN{$?UMiU`B$qTF8Plx`A_O24SmA}oy_)-P?b*)dTN1pJxATE zPdsFY$C8-`=F^A$X)U%RhIipQ+x3tOcGRiGFC z{=s74ZXZWHn1M=()i*CGPT$tqJ8HESVyY1T>dwW*eou7u&bylNiPt+WDq7ckCE~$a zAgDsTcHM&F($*~E!HiE_{)c(Rzw9|E;-LgppttRPfAM&0IWutnG=o@)Ef>xyzWoD# zw&W93Vf&)d_Z7DcJU!yU3{*;BMqb_ni1b4VszBQ~;=zol>WJ&Vm{GL*WpiX)G2;_w zynItp`}f%qf*GGU`-{_yLw~q2LNMbKpI<$#n77m05rP??*!|~M7QJ8lG(s@r6X*Q& zvSQel?Gb_*pD6bs?zTie58b=E->&+_%;GQKzb)_yn##ae^72m|s2MGZ0jP<`pjNhpIT1KN>Bv?SGagTn9;h3FFN7S&p=Q`kNCA#udhCVxkT7=5S(TBM0^IID(!jGf#dV| znL!Dv(2vUK9*Lea1CtGZ(_bP4Gd{8PoId&fLpMbTW_)5)-Z%gI&GRD!Gd?l;slNHXT`!0b%=pCG zZGH1s|NPsv;V5CoCngOWmB+6iN>By*>gUhRy`NoY?^7s{p8Pru`-v_VF<5vS_d}79w@8$8U0W&`FU$@?v$FBy=_{4>OzB!Ly z4VdwXcgN1m<5vS_eBy;IGxJTGTDyg#M6b2>Dw+u$nfrJu%&D3|*qK2VdN5<_?jY>! zBSJ9a6Ly9ZA(-(AJBx}C%=mTeXM|Drz`AVc!N>Bwq zJ6~yopbCVYue3o>1;Wl(+90R`VdpE6eWm@N3IyjX;oen(DiHY3JANHxM(ZL@|9{R* zjEK(})I(|B4`|#ErR|w7vUi!$-Xg}mfq2iApb7-$L~(){t&6zvj109@MUVKt(pIUT zoj*i+&J63)4|Ae;Ka`*fG-fz)f*GiMBHj=6Q2MtcdL%rQpbCDUbnKZwa_zjx@u37& zAod#BEC1m3d)gqV0&)7NBlFMCyR{91Di9x6dgq@uPiupq3dDJ1`{cJQ9nc0r6^J{& z);I64@9+AD$A|WuDnrcfn-4E8Z;P-h5Z`>JZ~jEfe#bTTpb7-%D~$=NK-ig6Wb87d zbrIW{Q-ol~C+y5ALNMbKc77H4JeWxk)v)J!-qj=6&buP5Qi3Y@*?CtR1XUpHysHg@ zDiC(w)doQo2s`g;gP;n8op(j{uJ)WN5DjN!xEo<+091jnmE!kHb`Vs7DDP|K~#Df{Fi@3Z~oR{~IYvB>41XUo) z`^kBEpE@!UnbEq4%lp@Pc`rQTp#)VR%KPGZc^^LF!Hm{LT;7Mz%e(cFGlLRTfhh0k z=jA>9$nnAN2cW{Y4!o~{IR3qXs+8tkk#O%a0}r2w-;?28d3>+NHCOl!i7TK&5531u zI!z!`9;s$M99k8*105Mx^w7Gx_9twW-eV`-m|#ob!PO~fVf>!H_QNMwDQuPAm1hg_ zJucTwxrAoW5)$ct;QKnRbOJ#?t>k(*JuF+o)pXd-Z#i5-dy91dO2qdSYT+}-J#TH5 zw$}B~y62T6kt+|=tx^xIE1y&3496b&9{di;ZI!BYWfi>Rgl2q#-=4W1sv2DOoU6@H zDNbm{C-|M7>!B)L!-Pt4LNh+W?l{>zUGos!%EY%_uWU{Jq?7TzMLL=(nMy zKO5nw%8}1?q@jm?8%jTn>9z?~={Y#`;JFt2hkj|oSvyp=gzJ_-^BU}Wpe3r(*+85? zEuB+wMrMyetZng*LG{p;Jg7uJw^hDM=Rt7-wR9H77NS*cuSn58rTYODTY_^f*ZyET zXM^c}_yjAtgsKp89Z}i?8J!z)FW~&hb#ktUWDG-I(C4Sm`vOtDZoRb_rjlGxD|xj6SYUg$HTZ!&m8l7Ht!H4u%J5 z*TYxwT$?83m@xAz&b91z%(cDqE^2uyb9T1d@jtakjIH#}PQ7zX_FsK2uFvlIV&&Dt z&(8kw%a;0KE4Ng-TzEq^cGblmp&2EH^?Tkn3d^1!k&4($@-e{of<{e6d2CQWN~_UMD{ZmYfgw;7dA z1O2_L#M9lbt6l#4Gnd>pv$d{!B<-Q9^uFqO{`a$wmyWGmIJmXGsqffqm-El4ymHN$ zy6zFzLo-U~UT}%uP5pLl;Ysr=6UVmJwYQVn-&k9H&Hu?R-NWy>61o?Zc0C3*&&oEw z(7pT2{;l=tBc^A)Ztv9n+>PVw+CP`jj1o=zPR&+z+o$`8gU5NJec=t~){Z`Lr|i%p zTI<^OE0&F`H67HP?O(g30ik_TI^8O5iSB8aP}NH>O~|@DdU*GFlg2oorc2zip;8-q zQqOGrbK~pU_K~ZPuif>9G1-g@{C+5*dqHW}W8V)u)Q-9D`s|N?Dv#QAd)F2ncWUDU3-s+lXZpj9YnOL9n=Fh7y-O`eE`_s6(db_PsmHMTLAx{oi`)TLf zvhmMMsNeIA=IXTnds%kq=y7%Rc0E+3eraORr)O8z{d`u|rFlaAfdkeZ_v!_cv+un) zwyxf;hpN;sP3YA?uK?=pdZvmf0`U(5gW__;JYn5h{ zIPBA#Yvr}8dUqH9EUIlEvtV5I)%E?V?@t|5*RzDvjVATbx?{Gt*7|<)$?Bn}`>oP_ zb?gB%v+_z--F|XwUHx1SB^*`qyu0DPskJR9oKf9)WounqHRBt%*UD>Gb>&HZ&$Ycu zXeGB*+V*b8U6qyBrP{*sNhBg_BGF*m6g|}+Q-kf);F|#H9LRLcD2#3`u$KsRoW-lL-(S#N?V&IRHd^Tm(aFP zex-L-UYBY&^}e`%!l;9@^9S{*{jFb%_jxFxD(#c&p>-drd{`;3SGAt=FRoAS{$AzA z2TrSIK*^*+d-g)5;d^>aP6?osV0 zttqcpwWY;`dY4_-_rLP$%W8)`;CpCYrPGAg?b+x5Rm!B*Gn32TTs6=q5tY?6gLZ1kwv`grGP@e*&v`c8KBD8r< zl}qTXNT2VdelDSPl};0?(%FmpxgI(<(C0g)39YMin$Wp{KHo|G(ydZDO=w-6C)nd7 z@+>5+tAti^Tcx#h{-DoyatW=ggjRA1t)=q^eZEtgP!FYDLTl;#L7(-MCbX{7E}^w_ zE~3wRatW=agjRA1Rq6k@X%Efl|DX0692q4_sE5*StF)H>H>ppDN)uXFX_wGi`k!9< zDNssiU8P+Qt)>6(=nODTXkDdULTjliJ+s7b+3}4Vo?4~P99aAJ@<}n}_OJi2(X$gk z^Em?`KKk;CeAJWvGo4h0n9r{OQF(n?{`y~gdjEN8OY~_AAT|x0n|J8uKW9SgBIdIt zK>YEo#rc6JpW=By#phQ*b?VXu`KkFu&J!fuy{mN*^Qi%84}FpbRBzw4IN!6+Q0Lhh zp$F^gulqrFxo1J%e(FVDKhy)Q;xkJ?4BoUX|INZ)&8~+&4*;r7{Z{0cy}pxa6=|11 zT{}wZAf9NRn{V$mslgK+^w|O+df&A&A9UN?OubzXCG_bBO1p%r&_X`f0>q?Wugd?s z{+PE)PgRKdYzYu=T(>fRYU4t0wINh|RuoicEGeHl^nAw*+xa9Vw^hjCn>ak}3FDa0 zqXLcXEU9-we8I3L2uUss#B*fE57?|AEP3TO7Z8BX-hD!`0OrJ zdVk>D;#Xb$=XjwOpV#I4#XXcj%x5ftzJAi;;>v!fc_R@jK2Z#+0ZSJY*XI|Ram5z8 zJ=ePS_%KhG0{!Ox3yP7W#&{k$KKN8Os0QA>xOl(U(7Jku9(+9?X7^4d<3Q9>06K4}U>ySr8vXWuh7 zGIsftELABT_JgAu#8tmuRou4ZG4oUvRtgE88OGgtQ+>wD(LJ}E`yF$Z!1nlPxBIHD z1X{&PQ4e!Op;e$!S812Px8D5S)#gP9nBQI9-}{w$*v?;9xdc?7+jM5}FV$T$pJ1hs zs5YHbT-xtCLy(3CeLJJM>V3D)kNt{+>@gK(JC{;?Li`r0Dn1N2XP5VMy>-hVUQ(4|+EyZvENh z^3!Z;Ryrj33s9&?z=Ph6iLdN+&5nMM4hjBd6doktLGQ-IthcV;@d=U+3I56y9wgvF z@5V&`V{Y6rDoLja)a7qf;YZpfG~*LT|LkUSq`8Eu5c5~9s1zqO;}g4|H`5&dp$F@N z;O}5jDNbm{C(2*J6y1(mcyQ>!xgYM?kg(?^<3R!*^lnVpv#3Ldggw6+6$yCIyD?$U=MEhb_BvryB;Y~s#)Q2_ zIdn+yDL2@oB;Y}Bmw?J%Urk*lSji<&%U*>IK^oiR1U&4_A|O~P^sqA)bN(Q$-Yx+T z%!)8WQa=!^P~nT|zVJhwW*P@|=-#Mn*pn{M}{f!3;c{c@wcez z0fLo64<*VoPRMjfwJnwWA-T zLxR8Ch6f3F(7Q2Fp09R%f}}%&zo~`?33$-EF;SjVUK5{A^{IpYE0PkhnbO)4hef+G9D!0LGQ+d zJr6o`NZ9kMQIUWLy&DtueD2U8VXqTLMFJl5ZcNx~ltYJvy>c2A33$-kB{1Ub^)(<^ z$t6(BUWE-o8r$OpJnYOOAXq8%u(Jts{vfU1E&&hDchnCAE4c(RoJT30CiH%ruI9Jf zBX28+sPMg!vjX33m9DN=KmC1+OQ^g0rB^hnht^H6h}Vn~>9yyYQ6jyPTr*0f*M)0F ziS#OO%_xyxv#l8=(krkvqlB*hc0WPQD3Sg>j%Jid|5ipbO6V_1+*WBu3H{B6OK3(3 zT`%quI(BsxtED5q+ESI$>CvvWl-4!LZmX0~m9F?s6ROg6=xIV#y5=%Xs7lvdrU_N) zO3*Z+DqWwNCRC*>Xw!tMbX{+nP?fGWP7|uqb=GM@Rl34FO{hxO)~5+orH>CCiK?>O zBflocw>|vERlmW~@21>V`6{IkI%Q??`2Sy#H72M+rT8~-nnA1tzq<`RSPKMIi23be zNHF6Q{N}eYK^16z=NS^rK&3?d`&zAqm@35l?l$yb#wX(6*J>@qR3VOkQ>_`F;5V^h zt5^#JRfzfRVn{HPAiVFLwU)%@`{DS?8c~_=PpPuoBj2TJ2C)+HbuvCd6}EF#N7!>_ zpi&~fA_XdDe1fZNLJuXV0v%tyqZ!0XaFtEy!CD}wLL6Tw<9kqr?eTR>n$fz5xymMN z6>F&qaeOtEW_%*Ph6O5Se1hw8!d59k73lWGs{Dij?|M(n(hOoH;%jlBV$U)6(N$sm zoWfQy1A^G`_B$)A6Q*lg(1ot~UbafkB>k?4uIwI0&758CCaHRuOh_RjlalBRV z(A7<>3nKk_aKHO>oY2)&s7o3#KTWqEnnBDFo?hdnD#SdZ(8Bo2GgTqxQLC%o!d6iQ zf@cPJaJ^qhPz8c#IE)y+ODZI&0>SgHu3HQVUO#j#7HD3HLV_88#TKuqA;AoKt1GK` zoev3S{B=^CeS`!v@ZcII^n){;kYEM`*D!&IuUpa#2(CQBcCJVYJ#?l=6=<$!3JIOJ zkp>Z8bB7E*L9CQTOYEmHLPzFTR4Hv^+?nzS`*z^UJHz*{^G(5s z@!sEi7Pjk-*zJd^l(z3OHkHzZs`&Qf&_gpy*mp8VJd{us-{Ks4XhsS9Zt1Ksp(?(O zIwUlsgndVL#6t;H@vYjShh~(p@A{5-D4{C89X#~Vj1py~O8MUKh=&rYvT-f+(2Nr0 z9;uY?4Uc#zp(-0kJT#+(eTRHxTq&U{zI8tAhh~)MRQ6$c*_XzIs%%^h3C$?6rTi?8 zE)ejmJu{S_V4yG<^NH7DH(u~7a zkw7hu5~vu59?bYHVWrT61ZrXQ-8$g@ zZw<{j^kBws2`h#DAc0!;Q!^fnLl0(9OKHZThaGX|{Hip!heS-JEeu;_k0|4zb-6wC zh^bJQ+d~g~1~494m)k>+mX8xfx*9#KZ6b-6wCh^bJQ+d~hmVc;4l zi8I7pW#xY6zDnuzN++u_b*Vxn8++dsX+~nR4kW(PNmRyzDr~oL#Df`884s>{3VW^u zRiJGg@nA+&#v{INNmQmERAIY~BOc6%%6M?qQ`iqBr~++c?>~CYNX!-SSl3w#Jy;6_ z*G5Sk=?B-IipunZtDZs+C8z>zMnLm6|pNI!D zP$}`kF-PWS&t4z#P=YGZKltmB`HTIwN8Yu?3{*;-)uDI(;X_L!9!gLJdgSch`3KXk zi+C^tl@j~S@16hcy_pdYC8z>D>yzI3xdU4w@1ti1DkZM^NALXWm2X5ml%NXqdk6H% zw~l{ltvlNHe&g~T?+r%GqlnwXchSXEc>f%>^KHAKht{PE1dqp%U`FdA=KFaY6I6lV z(G?QRXkEk{`60oKPw*YUjR~qibL58vGf*kPw+1&Rr~=KA9}>(!r3BwI+?b#WG)I0& zFawnm9LFKSj8E`g#*GQ8Ky&1W1T#=6(fIhlaR7qX62yE%V(6hNzIl;!c#YCpRN=aW znAerigYK$A%(pOx1T#LtD^X*DD$u;HG$yD5!8bjI1T$I}F|R9)393NwD$$sr3IyLK z84}EBUBtXfG$yD5!S_#w1T$I}F|QJh393Nwjg=w6jMhcWt3+dhDiG=8Lp`Vh!K16O z2UQ^W2J*%PRUr80&5&S5>muem%Nr9^8IR~2JvF0s5%XQ0p$9WQ!FP4K1g_h6Sa;)R z-$rWUbgQgNX%i>kWvZVb_gk9KmO!O6W49k5INFs!%@-&n1xI^+W3-=607*mD1|x63Fn{ zr*#o?yGy7_Y4vjnRk1GTqVR~n7nzwD9iQ)V#;P8Ox!rA*s^CEt{lZou!x^jAMa=D? zht{P^{agYW&L*`kVs3W{RVl5lbqQoRo7B39x!omHrL_9F1TvgWYF)(K?h>j}TK!xC z8D5vPE@EzX2~{brej%Y(GS*FdXkDn3W*mCxm5eH-T>>7eQkt<#pcb!QpsB)k#xCKj zlxFM_sHIm>=M38 zX~r%A57q*$|Kl=t316i&W0!yjYk}7PycxTMuTq+^OTdG74U|UA?Jl7zr4h$lg$(b0 zXkEnI9(vGSY4vjnWOx@x>mufMmr#|`>gN*3@ZOTvMa=Ckp(>@-&n1xIJu5aSK)c&|FQ$7lM!ivLaEZSC=SyAo8v19A8scVzIk_vyp_ zw3e;H8|4{ed)NGYWa86^;7^n`?};71j` z;{;TC7NsBQupi8@B}%t_W}qKvo-texzb;kmQ{3Z}tzCAErERSp^XISnt+_@a4nINE zQkBv-T+k`9cR?_NSP8`8-t~#6AML#3Ohj+D=O9$2ewX}5m;5L7k=~iwC(c{famR6v zIDTeOmHNRWe$+DK`?cLFe41MSMT8|-4PEZr~+-{hzB#GG9KkBL_BRg;-LgppluxSU`ABNqg+jir=~|dl%NW< zjUyh+h{|}Bt3u1qqsjZ#mZ&nsR7o6PlZsmAN%BgRt6LNOuqs1P1rHlX`oWB-Oh3wX zy@}65393NbIO4&KsEkLs+Bh+;l%NWpa1{> literal 0 HcmV?d00001 From 8eff874583458c97b68a0107734a1fcbb71699ff Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Fri, 28 May 2021 15:21:58 +0200 Subject: [PATCH 039/199] Update ZED2i 3D model and description --- zed_wrapper/urdf/include/materials.urdf.xacro | 3 +++ zed_wrapper/urdf/models/zed2i.stl | Bin 48684 -> 718084 bytes zed_wrapper/urdf/zed_descr.urdf.xacro | 3 +++ 3 files changed, 6 insertions(+) diff --git a/zed_wrapper/urdf/include/materials.urdf.xacro b/zed_wrapper/urdf/include/materials.urdf.xacro index ccbf7480..8ccb50b1 100644 --- a/zed_wrapper/urdf/include/materials.urdf.xacro +++ b/zed_wrapper/urdf/include/materials.urdf.xacro @@ -28,4 +28,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + + + diff --git a/zed_wrapper/urdf/models/zed2i.stl b/zed_wrapper/urdf/models/zed2i.stl index 913d2cd9657a77cda8be4f1be69c8728a6cce9a5..76a08d0cbb7d54f694d6c40420a3c25eb15cf28b 100644 GIT binary patch literal 718084 zcmb513AokM{{N35Bn?D^s8EJzppfe9t;nw^N$93aMX59>&EsiOqzI*G64F4^rJ`DU z3rRu)4cDboDJrwd`xf6sHD=X3Y_^ZLBM!}_kZK6{_`zOG55Cbwqi zGTX$p0U{!2kV!|5vJJR@Ra^L)@tLgVT#AuE?&izfYYfTefqLO)9?Z z*VRMoY`(9ZD?09`H~9LHGupX-U3$YcE_2V#dt9T|gVR~~&nlKsJpb3TYPrnPTn~3F zf?4veHNQ8AYs>9%m%i68-FDoyDXZAJjCcRp&Yd`U9*AKl4RxoxUg@_sJ(`bD7AxIY z<32Zh?57~+P59ECP^oizWJ%`g9O|aM+BRMJtzG#DWznO- zg!Zn%gwh~>o44D&dcqy)Hy-1uq!#{_0-8#yXGU5MUTXy2vSszbGv2_^_8Q8@4)vX<3Oom6Z&WK|C=3Q}^q8b=`>MU{6S~br~0)-rHSp?r0E?&L8XWFb(Wq}aNQrz{%iD$V8{{9*KX*QoF7iGgQ@`(X)X(PQJygW^3uugQAX@s&Rl z9moFLdq}Z$8F%TJ<9dv_7amhbPIA@So|H@$4&%xa%A!Yw?{nhV{p5lV-2bh+Fj->Q zwcbODt;_h<-9y~ZbLzq4g{@QE>Azf>EIK1RAC^!SJr9?w2J!!4b4Q*!9q@SIyhS@gK2(6Bg{yff=9cmD8uk`2dv?Pnh; zwk~5gewe%D+|Mz?9T@kLYuCPQ@^H^EUs*y~^!RA`usA1PcBgX%$M;OW{oLc;LyE0? z7#{lz40q4x^n*v%FE6`=Eqf*ZH7(4$B5W2tDqTF>g}MFCMN3`7^@EawcdzuTffP$H zE_2y%ce`J4E?cq4UH0RkV`19bAGf&uBHjA(Gip)EH4v5O}>b~>b)CPreLdf4qYSd+s`l zuvzrro!z!BYq`s8%^fA4kYWkO9Ai11jt7y#NK9cQ_FG#zX=~XmzRnTar^&@2ZhUgJ z>x0pLYR;v& z6kC@upZUV8Zv=7Rp-Jw_Ff-&MltmBD7#Ag%u|YD^5N_S@huA6+BkW`_wI4UDu6D4)%l;TbD7{#44xv zgGaabN4w*&a-LjlKt4iQ^zdu2zgG8Sbsmq^`L1StJR!x_Wz4m`U9B$g_zXLM!q@?v zQZ?KUODKyT+)IRe-XM3E;XS0-x{SF;DSG$6;4u_CnFiR&TwAS0K0;aa;GQUq-O<=B z9mH;FUcDxskYejH=3eWH?Aq}796PcC*pa>Tet14Cp)7iEZx^1yYWMAOS7O&U;)$x> zLyE1-n0v+>zbpZd8#WDdTd=d-d@wxcmQWTwxO)wA$*J>qyMOG{`x{SH^t>5^+ zn6J1)ehxe2OWq3el_iu#5AL^8VD3&oAHkhIv*@8t z#%1c{b`zeEV(T(izvD73Ps~YQG3YK>j#LFPkkl3X|eY`Atf)d`jNiw@Y7RYcbOI-Hs3ux`^H- z`npqIZ}dhus^`8RlRaVHU7nDVmuT=qA9v@e(?Ia3mTb8{yZq2mo{*B4s5!Ea8{6fB zNLOxLCK3!adYmuT6hr>p+V_DHmRXQGj=G+Zl9!m2?BO11ToeRH$>E+axv32bctT2E;`>v3xQ4y!EDhIc zT+xND`-)M1BudFkl>h!g_xRjDsrk@8;d*2`PCA z&SSH$D-k`mU`}j!YXwh8$xCpqJoSlh;z%rpdH2_%@B5J`B`?AGc;GX0;z+EHxqU^4 zQ#>IhFQGZ#W!4@Xo~ly2cXD>=71_2QDBWdLoknYixn@`y>>l1_Zu)t6suzgwiU)%I z61xUVF#i0dVeW@(3omn-<{&P>il+I^5O4lc@Ik{iyqqb zyUZCNMuKP?2`RQNbr~0))WiK;f;;^VAWpunW9q^--Moh-ltmBi`dy|mh^_THrYc85 ziml7|xCPx@fwgVmaTSO^hJKjZ@K-EDdJjt|iyqqbhkO16h^HeV#nxqf{zIKy@lsd7<8mBT^2F-tPKzpe z4@)SE9@_Q0%y&4dDj-HhLW-@+xOmTwuHF4b;js(E=_PJScUxA#dssqQ^w6$9oJTnj z^&=t0)@3|tQ+xOCt3JY9!qHyy`F81_KU?bOD@!QLpU08j;4=_)LEIV%^#isp<2kS2 z=Q){g<61DRXR4k$R<5s8Fa+y9LzQ<9mOw>-$4D-`4(qB8AP|b zfskSe#=Hu{*SrLeuRv75)!p^kQtx32WzmE8DSTqm0L1Me+C)N%t;?8q^OBW+!Q&PX zCt;MdUsfP(YuPM+9!EYg*@H8$0^<5ea3u1x6iYDXs9aa|;@aUEtcTIwV^JmVVF_i? zgU?O)#DwQj7R1O%NU?Pp^XcmU`Hk?n4Mbl&wJ#2A=shfzL!-M;hS1^lK zZPU$rSVCF!;CviDG3f~6VGva!A;s2Z%vrngxBMJuFooR6#|1*Yj3pS) z!enhxUENJz1D8E4~t$FgmA!s8?m1x8+#eC3g% zNn6Wi@pbhgE;AEHRTIRxNJy~+jkk#uwM?=jye%9Udh?tgdom zV#Vfm-op~gqKA4B{FMc+?#We8OuQKhDYh=-4@UNOWu9*dkF(&h=G|(p_xJ6+hb5Fn z5A`B}SZfF=wl3pBt9rN^54V8FIq>NI>z%1ev)gzNODKyT>P1}UFPukh5H%tp#nxrq zzC?HTN7*Lu_yfcX``=6*SF@S-u!OScp%gNH zh@A^hO8>iJw)e1vvgn~+#AO!YsFs3wF%nX2UB(yR*4f>9$p!G>6VPn~W&96)Ua>j3dF0q8N=%HQ&D-Mq8 zP7sTW2SSRi%lPSUJGdohufSaLD2M}3G*7?y%q!l*63U{7dJ>7Ky!)kYejH{$p=@ zcfmEc!Q*BSGmdMU9>tnA`h-c(r;5 z-{aU?HcPLEj)fk12`ZLQJovMMwOpnLh*mhN)@!P!Y%QC`*Li-SW2ucZe;34ocLO2C z5{!8j?wd6k9yfyc4fkW=Gp~3LODKyTxgARmh@Bv|M?#9N%b0ib(V5@C<0%kNVw4mw z{+su(gtF+NUL=g&+8{>|B4iC=l zDa<|xuWsf&ETJrVs26dWvLKckLW-@+n6urzyW7L#JP;RQ7OgzHjrXvGvgn~+1idrP z{5cSpM?#9N%b2tF^u`au<0}xIS66dGzHje6ETJrVs29QC4uaT#^oVqUpys?*@Yn}pGIkl?lx&!fP!>JZi-eub3J_Z&A;s2Z%w5fz zNe$sq1n0s1Rg-~Ly@w@~MGy5N*y-b_=74x45>jkk#@wAPn0FaG9s_X>c75)VqTa(2 z%A$vQ5tlg?M^z0(uSiI-bs2Ln+AC8K9w&mBjos_k4?pp9i6xXp5A`Bpl*|HA;G;lD zv2_`9xBN`kRhX~-hx7OzyYO+Fhj|Z6D2pEIMO@}S5E&3R>wQLq&&+S;sg4hM(;&FkHVhP6TURAEBSdOq}aNQ z-)=g@oz-p?Je~$|_gNK__ia2bX=~XmzOG)xWwJs17sO?ekYWkOSDcsQ{&(MT%Uz}^ zh;ttpd)?EPALHahX>^j2TrhS?2XXNU?Ppf7W1-yYaWP;qg3(#V2e`Y}j$7 z_ppSr=%HT3WyXN`3&i?JNU?PpPq}n}yJUD3c#H(`Ve3Z{U-!PjdssqQ^iVH?=L1Bg zHjgB}i-Z(gm+^6*_jM~9)`ka1N!v2tWxukciTALCvgn~+1n-xCSakMx*)t;{#nxrq z=0W^b*j9e#)gQ#{o#kBC>Zabq63U{7dJ%l`1LBTd<=o+S10luMWnAX?p04m833v<# z@x^I3q*|3~jkk#_sv)ii<8!)yaJoudNd*P?lnlmo-9+prRJ=BZ1%o-3^y!cb< zp-4!vbs1Oa(#4(rWf6F60x|i|lIaV+Dds&ap)7i+7jc=DAf5n`jD!?hm+_~rliQs9 z63_WM5Sw19mVW1~U4AaHgtF+NUIc&F2VymdT_pk`#nxr~bEo@Vy-_c~qYsGXhw7z2 zC^N--SVCF!P%jdm^H)Kf83`%2F5`K_JGzxwSHWWuJgU^ZIbGzpvfjfI%A$wqSU}VV zar^IqkYejHZdbUYYcO*+{$gtoh+QY#>U$hp%Vz2I(6I~zu`>@r#S)5V-1gTs;TgOQ zM^)yxvMF23X7P3PBKVs-c$5KA|Mx&hu>@mYh07itQw#fc5KrRj&Mh;=dssqQ^iVGn zj%o{t%OfGh)@97QS@?`i@Zjuo;=F3P5n69}D8@NJy~+V?JF2=3WGk6gP1}UNe~l2G>U{2 zTbD6syKl$b3=httR~Eh@bz7-M-op~gqKA5sFt>LAaYrPi*t(24YxjMtIXpPGzlinY zw|AR*4@)SE9_mH#$q$HAuzvg=2`RQNW3E0oOyg%0BS74ab#VHQCf>sm%A$vQkuVY~ zfp{?zQfytuTtzP~dK)}AUwwl0{Kwumcn?b`iyrDlTxJ4@5?IgIMM8?L%b2@?aSi?t z9^8?=ja|m(9ank}ODKyT>P1}UIS?O$_%ISuY+c6O)hvFqIy~lr_z}CMwM)x-4@)SE z9_mH#cS|6agJ}ACAf(v3jJZ3z;j**gQ4hof?D__8JkEPqLRs`sFM@YJK+FSid?cjU zx{SFO{p!14Fqd#WKLh*SVSV59bBQICMGy5N_-hdm#XW>t7zpwo308C)By-<{|E8^f;nJ$MO`2jUbl% z6bSV)mSEiY${}vr_cPGrOa?Lg%qq#Ty&5HKEt|#H)r+{y3K0E3+}t}5QY^vv)6F^V zlecePh8+Ni8THOf-gv`H-op~gqDOAW(ho#|>&{EQRX-3?Y+c4<_6&AE&YS^{Q6Pp- zEt0I+c!&3}gtF+NUIhQ*5X9jrMUwL)A;s2Z-0Jy3u43g)@K_CE-L!8K)6OlJw6$y& zUso@JSro*!>E9%VMnZ}u7#Aur(A`;>|K{fj5H-6lPV_E!uJ^Envgn~+#AOzMSl(rE zqHiRm*t(2&H0kfs-HO3uK8Sn%-6hep$EDuG63U{7dJ()|0^(&5eIg;n)@9tdbzfKd zk}~jk8N?Njf0Uh^P~Cf2LRs`sFM@x=3*t-=iAYGXbs1mPqPMH{aXEO9C{_Mc*ZP_( zy@w@~MGy5Nm_

DtD^8Hxg28UB+!1_jHG^D+`YqAWkoJY3jpUs(BAfD2pEIMZ*1P z17bxaq}aNQU6Tjhsz>;*IcCA*ixp3&(x+7P9+prRJ=BZ1%oGr7L3|VmDYh=-#SeCK zbs4{KR~PqeYktN;;-TkCrN6EGj`y&Hvgn~+ z#ATL&7!IOfl|V?bbs3kb*V&b>{SZ7Hh{6wCoSy%`k>0}+%A$vQ5tr!+qCAN0p9MmS zt;_g|oKCJ`x)?lqfY@~573o+1RnU7_LRs|4?N~TpO$PDV?SYVD>oWeiOegp4Swqm{ ztO2p3%ay*zv9)ZLUJo71u?Q-bQ2c(iJ?L>bCl1F^4f|KYl&xj6__}%#{N(_MKS5Nw zBM?$7!I)R!`%ik*a+%&BZpPK!^}mtc!xGA(hk6m0c@xCet{120d=>~Pwk~7d%>%2J zz+(xB@falsD!=1BETJrVhb5Fn z5A`BpTpicsgmj-sNU?Pp^XY0ozesJoe+7>d2W(7jT2nl2YuPNmu3p4tCV^On=ls)1 zNU;QC&Jv>+od*xjK5t|8S$|4J?_mjL(L=pR*k!B(u`v=-Y+c5j?W#55cPDV(?TA@) z%`Mfuhb5Fn5A`B}=xPWlwk~7N+U*Kn4G*pc8^UVfJuIOtdZ-r(t6E$?JR!x_Wz5xQ z=A@eNm;;X)VI9mzD2pEIMO@|;5NQx~CRF!?6kC@uSJ5HgvNz!B+y$%iz#f<8Ba}rC z^&&3w9}o*b+#d-kwk~7t1}a{42|N~p_!GN~d&`~cJuIOtdZ-scCk$dSh#8TPV(T*I zu4dK7Qt)^bL|N>XJ~+3a_ppSr=%HQ&|3VtXlOU=`LW-@+n0vTGEl+^Q1`s!4pSY>f z4*v{VLRs`sFM^&EL=_Mh+#Cogwk~7tGFMgm7|;1&5bLqu9dyG>-op~gqKA4Be5(qG z4?sLpKM+!EUB=uky91Np@dk*;unYfJuSVX(63U`SZpVU7B0U+zUA+S##nxra-r&o^ zHQ+%4JxFrgFON7|%VzQQ+>V9o$5as0e+q;YOE6aVg1P|r3&aEv<&SUe3H36TVEk3F!EWK#P0-`K0OGG(3MUU8u9&d3Y!+Wv zFTzg&nx<u2tUnen!X3b;z&rbbs0bXVo!J6((mCh3B((_ zPfa~>UQzF131!hky+|MygLoV+gKLkRGt;_hM ze|2*^zFh>5+3>it+NM;q`fI(1C6q-E^&)&r`9+prRJ=BZvUA3m^1t6Y@gcMts@ydI;x-YN09v%}wJhJ?h^t^+2dJjt| ziyrDlTxLCpz98QEF%VL0UB(x#?cyp`UW<8m4T#Otiltlry3NnjmQWTwayu3t)d|y! zrEC2e2r0HM;~xigalIaRV@Vk89U2t(J&vtqv-EoCSolPB%tKJIgyKS{uR)K)r}iuy zmHTy@v$bp%Uso@JT?U9nIP=&42!s?%Fy>XL{Yryc;ab(h)lD3{Gi7VpEWWN@B+QBH zK{Wj-5K=6`n0Is4D^J1WaU9hn7$w{4PWK*`P!>JZi|`%#rs))jR3xO>x{Nt06Hlh# z!FBLajP@7nuk{|5P!>IMJC+3?=2hL4`aTj;Y+c5Dx?b+L3m$VoG{RF`^vCbKhb5Fn z5A`Bp7JUQ6A3p>_iml6-v&4HNv#?L(s&)XgPuX7!q-`ym#n;t~;O|92ylx07mSD`; zu0mOU=EZUK8fMW)&nxOZETJrVs22$<&O8ufA|b`rWz1Ro>++|=V=_GMz-ln>+~VHD z63U{7dXaEc<3P-hgcMtsF;}0y&Ch_xba>QUFe`h)d#8I3ODKyT>P7gM98J@`K#Yro z6kC@uSJ8<}*w=D(J{{}%qg4ud4@)SE9_mHHYETjD`I<;bv2_`9&#=AKN$}t-dMxUhU9m{YKmxCBrIS}e)EW!ASJ_Fp+ ztES_l0FJWuhEWWN@1fLIrCJ962mWe(R)}zS@cjZ60X%J*F2VRk&t5R zGH!lJKX>7Cmx1mH{AIUfv?{b|j?Ox{McH-PcVk`xrcO zKsRJ?a^F41-7I`hBv`IlS3>SVCF! zP%nbrD~OHP_sK3D2`RQN;}6R8cK;nT4IU#wtoql?6~BGI)q7Y%S@cjZ;xZ3`CW7X(F4R=Ri~zA?3my^ETJrVs29OsS%CNm z#IQ(6v2__=bz65=q2cB5=nZ1-;w7n?TkrQCmQWTw)Qg0{L zoR_k-Y!+WvFM_|ng2#sE)}%&u4}=s;Fy`H?yr2a{cn?b`iyrDl@OPFVwu6`u2`RQNV?JGL zySxAoJ|BfL15(}hE%Y9iP!>IMI~LAxIe5;eMM8?L%b2r7(>Y7wF#<%92mO5IJuIOt zdZ-r(Gxb-PuO54Rl_#Xwx{Nv7&AIShcyOlv0ki1RVjt%tltmBqBIqDN+y-JnB&687 z-Vby8`c@xBkB_h#oba6O-n)wyco!+eCY=%HT3Ww>YP0HQ%8 zq}aNQxf|#{Ck+o0KVn}}x#SA(VF_i?gZq`xv5W(8JoY66N(4fRt;?9ZnvZJEhX?1~ z*4SVD>w*`(hb5Fn5A`DWYg`b;K>QjBDYhjkk#`-qY;IaFU{_f#sL%oM3 zltmBqB4L!2%NpQXM?#9N%UIu^8jk9n@}IlLXFupYETJra9!EMBUfsjxK6lSWLS2fj z%Q*MDSb64uwmCcX)rbEI=Mih!EPALH3C~BLd(KWZj)W9TFxI!a;(Z@@Y@U8?YWCc# zy@w@~MGy5N!DBm!$&rv^>oV5&z=qL2c5kcHo?S(~hb5Fn5A`D9>Slx3zdI08Y+c6s z=GbrtXO!rZdSlBfKVMlwS@cjZ62|U$5KnFmgcMtsvA%0Ic%1vnu++Ex8+s2*D2pDs z9SiSyc@RSe211Ih%UIt|8^-R3#UJuLj;&?0^m^!6=#iJ8VhP3Jn{K&&T!^Fky?;Yz zYuPNmp4+i-7A*>*@4!Gvu>@m%18%s3ui)zbv}Kj|u!OScpGC2af?*KdOH-BAiF8WwYp^UL@=?-ko*hifbbw#S)D5jl{vD_3)C}1>V3n zVVYVti?6E}2_7k|gJmKi#S)D59me4~zZC2F9U}&K4@)SE9_mHH4qzVE^N)uILW-@+ zSl@aah+f#2JU;k7?_mjL(L=pR*u7RNULf&VP9UV%x{UQb$$_}^rYed36&rdFODKyT z>P5m6H5>b@#TNuZiml67->e*-k0#hBmK<5ZdssqQ^iVGnc4YIgPnZyvDB+@)63ShkB7P5=mTs zC=gO?UB>#p=kU~a?>8b*tJC`{Y%QC`*WWnOu{?=q@Ocn-3=D)6OE6aV628TJJ-%W5 z_rg!2tIyrBa1Exw1oS?L%2cVKJA$xCRKz;Ezm4e@`=!1wy}%Ln|e9maLW z+~wZcFwV!hzu}gbkdlw6cUSyHoqr;R#4OR<#wL~>IjSK#DtjdS1qL1A;`VpDJty(+ zEc1Fu$$L)xjfn6YZjm_RVF_iCUbU{3JK=MF?_gdJDa^7&?r*r|^{|ApNdK|CjjJ++ z-;t8nLkhDjp=yvODK!ypFWvxg=f!$S(Q zETP|a%YPo0P!{R!?MAo(4Tc}XLkhDjq2G4P?_mjLk^Zs&LvCa;3wY;Bigct~NECG;D0`8_P5EYf^7_e>ab3=b*HvIL*`@C#sh&)gEqBF#DG#!b_X z;UR@tme6n1&M>y|OD}`B>(68G4GodWf>=`DN zJpULTQkZ24_9EdI^70;)C6q;)y-#M?C&yR~q%g}8`W?Ld=V1wDk!BBfRCjk|#gW1+ zOQ^G?N7j)4By*C(-ksuqVO9NL))fPe?oyBRob6kuFSmsHOiM84>&@}^kodLD*6Bqg zhP*q)|5B_aly&rZv~8XK4~d*e=sav)`EB=o?S>AIy?WO5{gSC$JEm+co5fhyFr0ab zK=|Ju)R~iJ7Q0{3?A1MyCBiCs3DJSr)~!o&dfRkD?yPGG#_C@GnNSuhDGtQDr~Ny5 z*2d$LRoQiq`_F{3=%F|e^)D)vd|}5O-b0G5%UJ#HKNHHLhvGmq>AoSc?U|0=LyE1-SpDul z6Uw58;y}Fd@~A|U|MvDCQfytu>UaN{P!>HD2cqh>ec4l+4E7#UY+c6ccmJ7C7CjUP zV*VHBxP9w#yoVH9m$CZYe>oQh{{Lh54=%F|e!!9qAZZqj7 z?;*w3WvmYQp9y8rLvbL^>sd4X_K3^9hZI|vu{z{`CX_`F#ev9b+cf?0{}uNhQfytu z>X84LP!>HD2cl4?cIly2clo(piml67J@r2m%A$wjKx{j`XL?%yXT66MTbHqV>VGDb zMGwV+`0}rT>4%%1<2|ILJZ+~Jk<1@lvOOjnCGYdJS*$(2xYO7Vt?jCKF9r--v3$eA;s2Z%=@JNJS*$( z2xZYjaquXDan+;hF7F}5)@97`rj9-<>+c9<(L-_Y_zVB~Wz+xR-&-2R)@97+Mjd@t z*547zVkO1FBR=QeLyE1-n9sjD`mC(KBa}rC#ld4B=Bsv-Zt@;dY+c5j$JEhhW&ItY zEP5!8*J|R=sk8UA_8wAfUB;Y`)zRZx{SBckdMFMaasBWfQfytu#Z5<_mGyUovgn~W zc(lSgcy-eO-b0G5%b4q<-UG(d z2xZYjaq#GZed5+-32@D@E%fZUB=u8>pg(1 ztiL0aMGwWnBktS1hZI|vG57O&*B~qF?+9hlLvir90{zE<9p`%wDYh(d z2xZYjaqx(Jl=qNg>oQiS|IdW7=%F|e><4GGP5ZfBiml7|s4g|@?+9hlLviqkU8?tx zV(T(iPfcP-lB+HL^J^^?cGaw_{+#i=`fX!JZwOrGTqQ4|o;tsWYLQSD<62L)bvcEG zgiir*DdH6eM31yMytaRj)Y7zbpT0&VQ)*RQ>O*<4ni4);Hq}aNQ zIlCVDq*{dcu!OQm3_iKL8*seUyhKltp4=&pvZ6Xz63QY`e(s>Z86{F|UB=wo9r>jCC=W|0i^Ljy$JVi*4=J`T zWA0Ipd{TXshb5FnqE&~XfAgG6v2_`9uYKf`>Z3d?p)3;j+%xQN<|`?-E@Sn(`DZvw zD2v3+WrqLFyeq}lWz62^$S2iDorfiqMS>mKkx#1smQWUn&0~iD&59$%)@5w_+M~y= zC6q;??5o57X62M(>nc`Xd$dPhf>|W!-R!t?!zT?#ow*cSm$ACn{CCh2$|Aw5-nrg? zj^QE2)@7{jHNS@?ltsdJsYjo=C6q;iu2+v2_`%vpnK4#P5+! zEt|!dYw)o>q~s;kS?2e!gtAC;FLCUvD}`B>P-mIn!xG9O%{|evM~M_>Swfv7^_b_LL9k+)4}@x2f^myE zZQbqulWGz(K%92Ra%XGVEP01B=TSWiqG0nt=sYYTzuH|&VdZ=s#QEiWCZ`@4?fY{} z@I07B4_$S9k8uA2i7p@-?+=6&TbFU2jqTiZZ_R_pMi4*MxHox5{hi*!63U{7Mvu!J zkE5CmVt*v0*t(2&)^6{{*FO=n=;46{5|hW=n5=oQlz+d(63U{7o&tQU3W)h2Dn>$z zt;=|0g^up$6_>%|Sr9M&Stx(2hF{QC6q-E&0{W8WpIJSlBorf9U>vc)@59?N>_LNq>k{|fUA3H>!pd3 z^E!JEODKyTnvY%P%K-%vh1x7l{P9X4q}aNQ-#)*)TlDn+cr?a&oc36)M3=$+yoV)} zMGvhjF7qIc>d=^4i3cJf#nxqf)3rU_kg5E8pOZmw(p|NFkoT~Jvgn}|H9sN6)@3}S zQ=jOu80T^7W3^KE$D^`@vgo08-enqrcx7y@RQE_ov2__=_(=bFt@h*UUfp_W>Y{i* zETJrVXour6n|bDKmZrXm_d|-U%XrbCLGhknicxahv;yf`agS@h6O&1KF8@#oY6 z={g!$aX)D5GJdnj8`HJpQ)>xj(L;M}m-!vUToC6+LW-@+_=K9n;#~3vh_`Fpn;sNrA4@2U9_j$_ zi_kdp&p~{3eVE~-*t&DzvFX6DI49l>96;NSyY70qKA4Pmnnr&axI7+ai*4H z3C5qc9PVdr{E{+=Y0C$u+uRja14}529_ry-rXt4GBoO?md8^pEj2jFc9@nUHAjW{W z{jRX8sg}*sd4zXTOX8^JfOxt^Ab3<%EFnLxqAqg*h@Lp}L;FT2Y%QC`*Le-YJEt;?9t`DZWlx3JC3;5{s%EP8N`3Ga*E0*@atmt6Njb5BUI zbs2Le+FWHQJPu)8)xw-ud|u~#gtF+txiY*j`T)-STM&P~5(p`_E@RH9k2W6-kL@4| zVs7s+xS#j1gtF+tIX}EFN`k9F$4E%Abs2MIxH9W;cwEkz8gJ~sy?&7Qu!OSc!L=)R zL?TOs6kC@uSGeHu3_MQ6T6<4CDoZGf9$bUNwfY?Aao5;diEfdQV(T*IN*%6MFA&#b z&rmMj4@)SE9^6ZWd;SNGY7dAXoVq!BiwVYAAevcgEh)uD@!Pg9^4a!vAYOI z^&b%ZA|b`rWz3yZ7`xo{#XX$&u!OSc!M$C020sO{2z$7j&{-oZqfh7vzvNCN?u~hNqt?5 zrk8`DM|SJ`-J~%?JRv17apCkn?u9S;&DnNs$JJeaK2jj<2+AD$wxfi zD-yg`)t2Pc=@vZ>MkO!N{G?vysJ6EFt;_iE*sm zpD7@CR7>u;KfBMPLp&iRFTp$b`*eQGKZyyr=S9DK$`exZ5*!Ol`h6ddD)nkko!-CC z^#sS&s4q@jmY3iN{kBP=W#K&7K`!`lo+qT_Bk+6#f=Bh#;+z!^Mh`w8RPqvh(&!z| zV<(>4m#RPSJ*4C%_%w&};Amfhnc#j46iCps<$MT|a)`8N?D0?>-&~c0%kk zEWxRgzf-#N&W94Vmd&!eewX>Hz{JFxApW-_5b7W-Q4StUhYxeh z%5H*3nUf|a8fWfI7cBOv_Yh&TxN@@p3_E~=II3TMxig&tVHHa-uC#Ti+dcg(>_v-# z_`Ldc=`T7IPv#|*#n;s*;5)EEEC6wQ$3RH2bs5+Geuz8n#5(YJ0L1I>S4jWoof_W5 z63U{7`UICb0cYNRZH08}NJz1D8NV{=}Cv0cn?b`iyrC|T;>lD zf4q~Go*xM*wl3ojkk#vAYG<1YMt6g*mi z;ClY`WLNcs4Sr@dZu1C zNU?Pp_if!hUaN;dlv%bgQ8!*)ODKyT>Jwb%bP&tmSeR%R2`RQNRlnQfytu<9}-(=aND=s)N7W znOqZRA4@2U9_kZZ=BiUBCW>V4Og>RG%vVxuUB&}nyf4m)4M5ylzf%Vv? zWoy|izRqhHK9#uxSNB>FsYpn%1Y=(5FQ0k=9!U_ZaL*SM`_y|_LRs|SSO}lWWZ|f; z!990H10luMWy}%t(`&!NqX>vEF%q|SD4w>pY!+YVm=2%H9Ol&pu`v=-EWwx~|JlR^ z%U$Lacx=ZLHS-<(M!BhFv-mon&G4zrQV?T6d=LpKmSD^$c+#xv;4uZnpYLQPpE%US zdssqQ^xzy5K9%_n=Wz$-lKzp9V(T*IOmyh*zu{3B9(6G%HY(ZHdssqQ^x#|>K9zX^ z#0!`cn?*v3t;?7*>YkQe;gJJk$$QlkuXXR{JuIOtdT`DUpURX5F&cCG@<>Rrbs2MI z*nLGGc-#bnyYK^(dwLH`D2pCkyMjlfJcJZmmoZnk;PD`cR#-U~#G|r=vgpAzI9w~U z*7~E8V(T*IN*%5hSA)y3XQ&ykt|gR35AG$xJ?9?oRqPo$L_&(K%a}WkaL>QTC~1nl z&!9L;ETJrVa8DG*Zc`9vgIEy>DYhoVp}Hq0f*gSZ5H z>cesNv4pa;Zx3@~MVxsF5Zf@rS;f|6%zb;96H9=|!A^hV&WG|5%2Lk|=JpLZssSKg z+YtzT5@YMCPY84S2Ow6UV_84;HD_ztEWWPZ2cN!zcn`!TtRGgf1Y>nbE>o?<-o)HK z_om%*v$L)8ZvV&XRA@WQHANpKI&?+vffxwlju!%Q=nD%|9d-G;atIhEnj6vTIXN~d4kaIyEWgtF+Neh0tS3?c<$aU`VJx{Nn(8SI+Xx(Oan zgScVD_o;=~)bk#eP!>JZ@8G=*5buMS6$vS}F5@9N1Ksjo_rT+75F-lBOcguvcJE;c zWzj?Zj>}vKVj_ssBO%4sWqj5*{oKy`9)w2%#HhU&rP_>b?L91^EPANl!Mg?^nt`}G z5>jkk#>ZXV$9*)cKRj*%!9J>N_qN`{63U{7`kf<0R=G$>v2_`L)w5^xxD_75_g;ig zX2N+`LRs`szk^?i0r3uqTO%RG)@8go)jeLTF(7&snwj|Hgm6DBp)7i+-*K7hAT9(^ zA>I!uwl3q2mpu^g`K2KCkN7?@HI5QXD2pEIcU-0rh>t-mh=dedmvQk=I>)hl=&Zeo z_IpYvlN-bHVF_i?L;a4+%!0>w5TC~9LyE1-`1<$nkI&$I5I;1!D*1GLYAvBGdZ^#Q z?-hag3dHM?kYejHPM7Z(=Mvt*I)fV}PmA-FC6q-E^*iWmL0kjkp-4!vbs3+qr+u6g zPXRIK^_I!=PY<)GC6q;v+^(oZB#w`S6kC_^>I&`S+UHAFb2fYD^S3$h>+-zrS*(`a7mGevx3qb6S1kZztCFIwl*Y9`_ znP)yLT&t9=WwZFY`W=_~6C-gEh#!je_kjkk#vC!%p12Dhy+It`*pJ<`t!1JZ@3_qOxK^)$_+vvLq}aNQ`2>H}^9p!yloWmF`^1!M z>Uj@KD2pEIcks73AXZ{7c`g!CY+c5jiEdfg5FX7z^uY{w>WQ~|4@)SE9_n|(T6;Q( zQz9Y7)@95Ywa*OvF->L^Jo;g#zIkkG?_mjL(L?S?Uqm$J=E{GOa~C}fp{|#Qfytu+yRDX@LwQy zVK2HiKDCxm7CqGOxXf1|eg)Ah5>jkk#@z3Qx#VXMJF!FF8D}3$D2pEIcktV1AU+2% zW>%Qtq}aNQxg!sAB3Dk#qUo|l!z^kEWvOQfb36NwxgeSq>7TNSt;?AGN0{4Jf*6U8 z<$0_Ic?o6l_1v!L5)dzd=<8er@N(Y^$ug_Nh954jJm2 zppOzAx}xDA&e`55J$G9m)Gt|ral^Jl-L~$H(Bq5-QTfaI=}}j=N!VI8i?6HS!MlSX z_U^8qeywI8q*#J+g`z`UwStS_u?9r%$~Dt1J1_MfmQWTwa=W5lAa;UCbqa(OTbFUK z5%_mSbAN#c&-{YRE2MAvE#o~bp)7i+-*K6@K&-A-Aw4S+Qfytu`<}>gH{N}A?QkAn zPdPO`^t4h*Tgztgb@e+g^9wv)1o2cPq*#J+(RT;A^b`Lq-Pn6rLRs`szvD8W!-K1u2r0HM<4>mbj2=Am#n+TfycCbh63U{7`W=^f z55%b;-iw43TbJ?0wI7VviX-u>nd1|C6cyN*gXhh{*+UbqvL3|gtF+N ze#d1FfS3#7nMg>nbs7Ivx^sL6zX9=PwF*f+=ax_wJ=E{G%vPQUh$rK7F2&aM@$>h` zxrD?^m1`zj$Jys7B4*J;{SJOT0>o?(RpJaM#nxr~+}MtBPMiqhkKOf?Gp`A=s3nv| zkKC?^S9d#z=5eN$V(T()zNQwLO?VXkA`m<(DwdF6m)AFUZ!viXblfvVQW3s{a^5*SVCF!$nAVt~=V1wD(L?=?%WMPj7>GsjJfzsVjJZ;WYxNa~ ztFdQzJziZ)D2pEIcknOuK%5F5x{SHw2=|=laVB;$*Tzv|31!hk{f^6UhPxR= zkw{3fbs2N#6vpmPAU0t~)+vs5ODKyT>UUh`9T3YwEQ*8_TbD8SiQyS^AhNI*y&yid zmQWTw)bHT8WkGz0z3ARZNU?Ppb0-_-lKvoOR<4|85MTWRml_tSs9Ej_lDw{s%z*O&H31!hk{SMv(05KNCYx@Hs#nxr~LX8~v zUZ;)l=nP`*U4_%{4%_5CETJrVYE_CJzoKQILmx$tkvQ|IU)4?7S~iQXtKV^%r$BTE z@ogleSb{OHbg5)x^f*s|I3IU#>Vc`=!xGA(hx#4-{Vs^EATE!D6kC@u$JOsUo`Xkw z5bH1!*ALs|JuIOt?{TCnx)sDB5OYOD#nxrak^jp&{-#As5N9s@HnHLB%y1uKE-5^JZ@8EMz5PLw3iG&ne zmoay;xpPT-?2vDXvyUZ|MGy5m_-%4{+zX=5{xHKyv2_`9M;_)xj;njI(|_jcFz;GI zS@cl9<1$-7)B|z%-atsPbs4k&2y^>-5TB!Cd1GZ*4J@H7dgOLRyt)n06^&XG2r0HM zV|7R_v%1i2$$1-!qz|lVmTi^w^PjD=_};;;5&9_6p)2YJ;`)t6(tDN#Lj95@70{)+dogtF+7 z+ZDYB;sp?kA|b`rWnAOpfo}0-8{qLai1l?>rsn+lvG=fqvgo0H$7SY$xU=5M)UZfM zv2_`b$1jCnQR63gECTWA2@_M`NGYf{RbY)u5FU| zD_&hoD2pEIcfv?)TDM7Jb0nnLx{UV}crf1cRq$AT!o)jkk#*2qP5T8NLiCg#Y zPV|gVttFI2kKC^4brA1?_%ISuY+c6H#&wNz$qEpEw9QI3h_jC+ltmBqJ1)aBCvhke zQfytur+AH{3+1MG%DYh6jD0O%+JnS~iQX ztKSJnH41a$&ykQ~3C5gJKg{_S9$ag4Ft^|G$7$Zf63U{7`W=^<4T5uf^GHasbs2MI z*t|Le56%o+afWs+>OCx>EPANl$xleJbs2Mo3m!bG$FSDk8IQ^m%A$w*9hc#XQv+-5 zjggRI>oVp_9j+D6BaJ=73F&Y@ETJrVsNccg`+)d2_6+}x_d|-U%b5F;aL?ZX@eXz} z1>-2OgtF+Nekbf1x`TKu5>jkk#@spOj@=#D!|jiw-4e>8hx#3STPTjIJoa$)A|b`r zWy~F5cn0~@uE)-@YkX=gp)7i+-*FkPI1a?Sk&t5RGUiS;%q1kw#h&`+WnsRugtF+N ze#d3TN8+9pfskVBGUkpv%!!YKD2JVX)2uM>T0&X$P`~3c{Xkp>qH~;grP#WR*?)w& zo&CqT=vbb5Kdc6pP!>IMyP^#s7NaXF8rKggwk~6JNcaxAS{;+St8Yv_zOGERRl1d* zTIaEO1N^tOhz?!Rr687psI)N<>X$6Rxa9Z&uGCi(S84#-?A;s2ZT(4z6xBK8cc#t?$@9xxw0k3)wODKyT zxm^*7Uko9|)@A%`x4y1Uxi{f)A&8S7DwW!|^&RhF31!hk{SN-69*9CCN~OMzgcMts zapD1d)6|3y;K6ZK_L9l&v|;PKhb5Fn5A{3Y%*Rxk?8-($iml7|V5{D4-~oOH$@9Px zHRP7FKlUD$P!>JZ@3_oGfyfde#nxqX;&W~ZWzj?Z4!#W$9{&UJaD2|C*t(2cZR-~2lBytj99oe`#ret- z%A$w*9ha#DqBDrvalVpb>oRU}Z#Oe1VirwY6K7FND2pEIcfu3Z5QOGkDYh=-jU~Iq zx&0Cx)qDSsvGV|rs@mFc=nbLl8vhaEZ5Kh}zdj8TTb zS;3LoRsCEpc>T7r93_S&Cf2wVrEpuzQy?SsvQ$nOYxbY9rF(|w9Am$e$6;7ECb zCu<}JbS@o*<6Q69cGXyjRy8d_k0xNF#q?=fYMmXUMoLRu&~K#C&;+BP!S#-pT}95S z${=a&Pg{Z>ofkG*ezUZuqT$9UpNvsPLlcaG2G={5@JSzB;|O|mUfAftNnZ_i8pRqu z;yXNSrP0s?qo5I96`@iulUnklBk0k2VWSfDoIX`FKwSNNIp2w|w;ByiFbW!6?|9j$ zqK`;TBtehP3mX;nK;b!}f&NiiYI~V6yN!k>7zGWkckDP=Kx%s-N6@45!p6w3^s!9Q zxK%W;j%t40mqtSqjDiN&JK-^^nIq`Yd0}IOvlF>kju5p}nz%=E8wRa6s8Jvc_rb#t(XQUf5hA$tR<02KkpR$Ot^WI*_bA_})o=s~>5*DtbwX*M;b? z))HI^X#zIC2iD65F>uV_z`Z@Er085a3dh5%qF01?SctKXphpw1`OUHN){7AP8}|r2 z(BXv9&;+BP!S#-O;x5FPrab~rw6O#|IxlQ~*Q~s2AVkvLEdm8Utl-nRbQF$ry(4dO z2ywbvi$I}OmY_!yu=(w@_Fg?-qFUgEGd`oC2}VJK>m7ULLxp(B5%lQ1u=#zqR^zLD ziGj2g4;u|lFbW!6@5nxbT*rHP69cOpL66Q0o8N$Iul$o`>%60i zs*g4rnqU+(xZbg6g;DJtN6@458awojz33m6eo9HH<<2UCa8c0UddF(~{Buf5f+OhB zd13RLeC?Hgvus_;d3Sd;!6;~Oy(6Cq%UQj^00$K3d#2}VI9yeh&oml4PJv>QL@(RpF>o0UUjluyQ}4LX;O;&s@O6QjXQ z(U3995L^jqf_nCwZb1x{D=*h`il=kwC>#&3ia@j#Vx%MJ(FAOMd$YY)o8$@h>u|zo zXo6AD;Ce^i9TbiGg_zjU67=Z2u=#z@wq5O%w)?@#3IUxmA$LiwiN+5%lQ1u*2U^jVD+`D%_FN&ln9&FbW!6@7O1JScv_OphxG0&2PH4 z*Ky~Gl$4~4BaDV77zGWkcdSM|84c<=f*ze0HoqI&5*Tsbtv=dlXo6AD;Cd&PphxF5 zb_IUVHtyA(Kc{%txU-5NTog38-mzEST8JW!phxG0&F|l~M6%2PesOnK6O4ie*E^P2 zFT^@W(4+Ih<~MfR{xMDFKJ8p9(FCKQ!S#;4SGNhV$r1GEys-Hl-}bEL$&BoA*FQAD zC}?oKV~J5hyy^&gbY9r}R&YBS{9V08;49b9HNhxoaJ?gcZy-O0|JvuLJu8_jtr$6w;jFjkaN9UXY z1m`H0*e<`iFipOPKAK=<0KrNmzriy;f8f-LVZnJ5^84PJ+&}rgE&aW`&o20=)~V-` zcbw?&ZSV2YM>sxszWma-yhkYS?@q`cI4MLyN6@1Q*vQn$5HASvqY!iD|8*`MMeTz- zeiMzqhc(`8G&F(hKom5}R)5wTHx!?ewVsqe zkY9-BnplD!ofr0>EuQs89>d>~l^3E{w2*L{!6)4ZwOIJ&Z@o;Upj&wO~A%+dsfASXf`Ro?~^zLqo9%c>p(}0 zko;>W=J)M$1U))0?0v`O*Hz4QJSN1gO*Z=~x_hMwMnPlcAA{WUm@8+sT8MM*UeTlT z!fyH9Aon~P2=U&;>--t+32K5-(0E|)VArm`lvdJ5h%JtwN9TpzV9j9Hu9gZhpj#9F z0oM{W!6;~aF?@*YA64Y6778)W5%lQ1u(LZ4as8u_5Xs|v`TM#)s0l_vz4A-FG6X%EfDLV1uO>>Q z)GPlSSK&0lC~BitdfDe~z4A-F@@bFGOFdLn`J_QKq+a=lH{KjW;A&ZSaSz}7WSM+o z(n9X8)GNQ#D?`wu^TNiRUif;Z5T%9aDNk^(v=p3`CK!d|XbTOR6kR9Zzbj|;u@LgHofkG*{*hXD3$afK zX(7Hsa`ifwj>2*D%^_9W3URYsd72PcJAxifz(xMTTYN6@1Q*cd(RpEG9JKA~S|J8Xbvxu*q9zyx4UF2he^iz$UoS)tN6@45!p3-R`^SSq zNWBU^?fRf57zGW^iKIu#SxLPLj&psG9-S99XHH)Bnr$P3|6N$XS0Xq$dHpXLrj2%x zSyA&^qfN_zjpJyEzplPq#}YeATuf=0)*?lFqKQ*G26}J((9;oJzTclRpyEP9&=XCJ zn={aR?_z~o@@oZhR;4R1Oj&Ytzai*}CYC%l(EI52>Qb-P3Q@OhiZan2uPA5Mt6Lfq>JdNcw1)hmO&jhW-udf7vTX#QP&|8=*O@atSU3dcEr z@UmCQm47J2fmk0xN}={?w6|IAOKaYBe{lE0f!I+u>Z@$f7oNzN*d5Ca@Rk0xMu zdwY<#=5RTg0hAV^uv~Q)xq6*TN8xyQmN8w3KE74H#~neBCSarW*s->?5Ci4urOH#( zxpWkcqu@NrtOl>t;ZNRDPySL*i0bkLrJe+J zE**vAXbW~1XP{j9J|Q;UY6*HY0UIsms$CsKW3~{rB!7RQbS@o*<7m@%7w0=6irTho z2zoRD8!f-vpZJaSkA!$uu3G9<48bU9pf}syk27-Rr&Cr1dN_g}ofkIxxjm~zLVW*V zw%1yoqRyqGa2$2T?&AC;XEm`&w%6Mc^k@P$j@uot--P(%zu74R;t-622CA#Q4m`oG zO|w&y9YK%I3mbLU-CZFXN^iQ!UAZP01r6@cc-f2Otge^ZzSt4;=)ABoe%R;njSzQ9 z9qHkoxh5C|4UBfSUCj`pv=A>kf*ze0HpW5Qt|kics8qMMuI*}qQP9AsZTrV|x$^l! z%yMm)9-S99#&g?0-WQ^tROuqFM`?ml&~HtGCFs!v zY&^{hBkq;C&*wrESa^efT=5G=LlcaG25dW9$`)eVTQ~T(I)WaZ7dG~4-Z#5Z7ljB) z^md_iE**vA;n`9xx$@tOUGN=p1U;I7J-6*3cUDz|ST0w+S?-t4rK4~>JX=~PXI1=< zIlj*vL60V2V?Mvp>|Y%eqVJ}CDd*)W>RdVs$5B`8Z0UMAt2exTDW5xn9!=q&1iQJG$Xq%KnK%!Uzq`Yg3-P-n=+OjhWD56>uj?lVUvPa;6O4ie z=Rsa}UAgjILKJmT4cpbKi-YFW=3WEowZE24tsF!N?08rtk%9Rh2mJ&_iIuM29;n~u5 zc^+kwlY?WWHENH}3mbR(*trriFZoi4vI}nrww9I>O)v__VcSvdynE))JAxja7dG0} zSo3M$B_X6<1-42{(YbUKj&mO5W!I4_KUwTT;2THKqY2n(`H5x@hsw}YuKIx7FP%$A z;dpqqv{BA#mfYFz96^sJV56U>na>9g3sGkCKJRyViaM8$!g16UJ6l5aSs&Qv?Q{e^ znt+YtznM=hO9`=k^S+dgaR^31gYzKyE^j%je}el`E<1uAofkIhuDuTQ+Tqfhu9cRe zbLl7?=R8Q>rF@GXlL6Ms?TjgNWC&= zMUT!48{?pDSAPhRPpVrD*Ag|sC}?mVB)w}tL z6f`&wlFugO%C`yObA6B=ofkIdCAOctdgZ_9YKbNo1*$<-_WORkpCqw~VXCqlMf z{rGIt;E+xg{Ih4jm7+aYUz=g>D0aC>+bViSh~7f9nPUmgqci~fM}y=6O@o^^mGFP?wI%4$1Z=dPPtDFYh?#RQ_}Xj@7!6G@3L4?r zQgb013NhRf^ys{>(JCvNIb0nfj&2F~E(xV`=_niz&zAm>D<8cr;Cssv^k@P$Rx)-U z?i$gkda$4GPq})XOGn{&c(!z_5ZB3l`ppsaXaY8NSnX#lcL?#NJnzc#6m>2gh2y9z zcD7VWh;nn^O1Z-k^k@P$j@voI4MLQd{gziP4#6mBgl9{43b9v+DvqE>=Y@@(ZF?Oz z3i0pP{Q`&Gm1}}g(BM2szFS?0Wfo?XDW0#{qeQNzziH2}a>K zuH24lU4-~~&V|4VN6@45!bXd+XVp}QOs&et$HTLwVnWnA*e`I-5%g#RHhS;~^Nv?(A-2lXt1VAa=h9I) zj=ExJOI3vUOquyNep-HJj?kqUQv9D-5M2+x+v2yufDNsgdL=Y@^BYpnGm9w^uGh{UeTio*cdF@mJV?H4 zQi$V1d@gNEdvspd$Yk49dm&zw5$%Gst!RQ#IL>*H{5_`-?S#1E+AcjhFKmp|wtw6v zL@BA#(_J6b1f!tAd62DFm4uLbWzLEoofkIdCAOcVGW3(tW}2%dnqU+(B4(!gJ`viBtx5f8k*%m37sz4DMH=+Sv$quZeJ^9V5W~v0@TSEf7zGW^gXFuZg!r{w3vZMo z=+Sv$^L-8Zgja|mCCUbRyDQfOqo5ICt&%Ba?j|APTm6CUB2? zf|_6yG&m3PveBb{Zuv|AtxVFcTEaW?De^U zb2L=Q6hX(4+Ih=DRIkHu~T$(fD54md>T4cpbKix_T8Ttqi`J0#*WT>#PUl;#L}Gyxke z-@Ye4Q?Q3_*`3VDmj7`Tj&9`pVOrAkSUr(or~$ zx?*QbD}?x2YKc@IL(roM*f?(ADFSg;YNAxQ7=lsI2+x+#2d7I-e9{s0=)AD`u95Av zWu^bMaaXPhMnQw~Ao+w>G`^ME-X_i7D|&QZ*vMp`2Z(!QjB4thpe7gv4bFq)@9u;s zDa2EbphxG0jd9Snt7$@vl9lZJ@a&=+OjhjMTP&xO(N2 zdSx^;!6;~O9wdLEAR3)yB-rQrAU!%SY}^ss&oM9AFQd&#R~a6Z_r6>YHvj6z%Z?`Ki6$}{ba8LQ=SDD!^tV%d>tZ$V*#!QI22tB~KkDT>Qm;O2 zyc1&of+t#oo@ioKk4L-{1@Y@cxe<)wS>Y~(hz#BbUQJ+c z!2Sh~JFAd}CKv^J-}O&;=f+PBYlH}T5T%Jzzdz}1nzA5F;4;iJrze`gh-&xrqt8kc zi~`-GTraQvsjRR@=&a~LlqNb(=)$nm`?kXeFVu(gdSG|1fj3cjeyY zks9W#=s}bwPL>_(&Au=*k}zjQPc(tmYebI!;ZR*axaR_>%3AAJTw=&UZr3prXM!z}n+T=JI^dL$T=)v|KmS_!4 zFbXvKf7AW*<7m)>C{3Wo45|6u*CHf>EF`@ABO= zCXNO@h|&b+c3Zw`ABUhPn!ubfel5`iqd;ShI$(POXo4P1 zz{YX=?)mLqgCBs%*gn_OxpWk@c^wG}n&5S4f_h=1%mYLGfi^wO-wA6%+q{SN${k_; zB9~W=GvSJb8eFj?B#_XaXo9rSNcirVzWz6!2>L+KqY2nt`{YhA3YlnIV*Ayd{bjz$ z>(?Hg7dF>Exf6^+Cfb&`C*wi?siRem20c12Y_67aCm00{+Lrja&F%iYmHkG89-S99 zS4+7QjDiMjOFa0WBK`$OI~WambY9q8U*%3P3L3O6(W?Jm-;r58jRrkBFKn)_awixC z4ceAiyL^JL<-vhQgC3n1HrH3V6O4ieZA;w!?WvTxEk_y+dURgcT#@BYFbW#9E%Dy5 zir%T+QXP~>=Y`D`S?&a*kcqY>e1A0v)NMV?Xwakc!sZ$-cY;ySplyjECEf|V`r9)` zgC3n1HrH^u6O4ieZA+A!c`k6xiLOS29-S99SADq?jDiMjON^;eA^6zLW=4Y^ofkG& zeYq2if(C6%+>l-;xNTewqd||(3!AIH+zCcOgSI6So@gEXqERWML66Q0o9o2f2}VJK zwj~Pp?jFpje%#b{dURgcTqovEFbW#9E%9y1^xzvqXB!QAbY9q8S>{eK3L3O6as0~g z;P}=RjRrkBFKn(wb0-)D4cdmt;7nY5bY9wArD8>Ec2hIr5s2dP5RrlFc)oSTfcEIT zuyK7{r6wfgL@)}OXxr<+^LTUUY@ggq#RQL4&r{C?@S{K=tEBgC3n1 zHrgB4wFwD15sZQcZL9Hbx7NXZjYX>hJ0~RML@){(w5>)L83${(9%eM?(RpEGeB^37 zAt5J%QP7}mHFh1Z=>5EVl+mC^=Y`ERwILF6A{Ye?+LpK|^O9LDM;Z-!bY9q)uW)^y zkdPC>C}_~O8ehx&s_DUjMuQ%m7dGZyT%RW-C}_~O8gAZhH0aTJVPihe6@5ZN zP6VT%LEBx2tUrD|TGeRKqw~VXx`cZGaveDkjDiMjtKrsBMuQ%m7dF@Jxf6_n25n1x zE9=3`CxWK7)1&jkj##B8&tBf%&Ty;Fze(4*+ld0}IY8v4{SLPHab0`cmNLvyGM z^ys{>xlWAj=bB&?2&@1?pISzol_nTvhy_D)sG^IUN9To&l}zYU%LolkFbafT-G#;v z({?q%C=maAJ~W5%gC3oiHdl9%8qoyTgU}vRET8?HGv+$r(RpEW4Hx@7m`g{&=87zL zf>9vwoH9;-6X&ex(RpF(wOZtrYl2ZAxW0-#D@`y81lshWHr3a~67=Z2u(`gEu^O6S6bPnPY5ALHLEdZG!gWMVZm!6?ue z=i|2$dJv@vu4Fm^ys{>xyp#u&;+AEU>+R5iqfO=!q)4L$arXb!6<6md$qt3PmE141g}HqrQV=#zLD?c94SQgr@Hy~KE5G_z;z%B8hlP( z_M@vO`+n-t%|EuACFs$4VXxiO-y2(KyNo#Xh4}fu4gFspC~elYnqU+(*h0PR11l%{ zjyG!PuiyxJbY9pqrabG-n0&p|tGXXf_8mP?+MnF0q0!I;qoBdQ>1FqjvpVoqY5xL8 z(4+IhUO#q#cjMNUqH$X8RjFalGfNgGA$2A%2w>GCU5!C}?n8 z@v>_PQD9s`V2C5=(RpD{zG{%Wjz@&3DlMd{yK+r13K|?y<-Kruf^(!5ra6Khofr1q zw+?pCqn8keq=nRWPf!z#f(GXj^1TK^d?rLQN6@45!fso6h-+7;r6t}e(S^RYZC4YF zf(B~VxH@R z^ys{>PqiLuda#%MmJm`)f>N(cKi34Kput(K{OytuPb-2Rofme`p+i-@a)eLk(owt) zyTgJi+D9~46LD6oqRdM@R8)H%H_Da&`uGN;p$S|E>n`r0-7l#lclTV6ZozKS51~$v z&I=oNx^Maa*7LIS3XvdBaI@5tXo68Vj<#U;OAbjZ`Cf=hj-W^9g^d<-{oc`{QD0i( zCTWRnq{``BIts_prtR)WMIrVG(cBUAXu{}8|M;oTEYWyWi0RTolBB-tTsjKJ(Kqd0 z#uM^9rb;Vp;0Ss&0UJH|jt7>C1}Xz;$uJqGbS@o*JACd62&a2N}9E**vAsFn6Q?iS+XF$uos9YK#KV56ei>v&j*!O}uXx+~WNqo9GB zZ=c6&LM#y?!4dT6ys$Ac*ymAD+SP1nA(h<|)C8lTfw9ZBt2^Wg78c@|Bk0k2VPk}| z?dn%)iS;G=jBAOSU=%bk2HXBoTFxp{h|@9_YLCtf8zZ&tA4x*U80i1j^+D#+QOLwO zk^H5e+$$La{pqd`(xVC3oH@z2U(_Dye`)^j-lgWllPkYECV6I_L0)_LewTG$k1;I+ zHja189_Y0>JV`1;aUrg5@}2kIzjacyCz=>=aG>|*zwbC=P%!A_sWQzF^h6V1y)EBK zUf^>fN{GgW>!*1?ctJzZ6HV;sJ7Si-MBT2# zH-4ley_Ibaj-0Yq? zJ<&w#W$C6Z*ypiLh#Q&@-+*UMPc(r{g_Hk~@4CkmeAu3q2ZEkx0%yCp`~5=TtdfGk zl$w>N8G@c@0(ay{n}^p}joOu_rHtGdGz2}-1n%yXhn|yJINEN3F6Idu0?*^}xYZv; z6L_*2XFhdABWWeqNgu&=&=XCd^&pegxOkzCSG4JOhM*^!Kno3@RSjwFNdbEu^h6Wr z1@_8|$#r}vJ?b@gujq*;(9`VQ1%Y1s{=aq1S3F43DNiKWr4!UE$nqDf>F>2&oX)}y5xJ| z;IcqTN6@45!Y(C$&-q;|{C?CCAzGI^=_R!8Vl*_tC}^PH*jdI=AyS06(#jI_=)ACT zJTov`G>!}La@mt9d7OqO7zGW^AH3{ugy=3r0Y}iI^TPg0{tDY($CE;2N}DR`u3Qt0 zf(GXg@=G*AOcA1tBk0k2VHe*uz&(#M(snmX3wf!$edd~A6f`)0@Upkd6KwHAA^!;X z%<0j2VXyh?S=X+zg}7Gor@NM@2}VI9Jj?iAh>}8NID#IX7xvKB&$|9`UWoVVxA0eT zeNYpOf<}0j(Lk{Tv9!*fM-j7pcG?*$x zBWWRS8)%$dw!1hJ+ z67*;SHd_8o)A89v9_dk8(xz^e`mS^7C>%%Mw7WQu3vss)H#veHO~6JEzIJjG(bz9U zX=zhZeT;@C7zGW~6}yXbhg^AIsU^QTf*ze0HjdBg(MB|?3bCR5N$*4)f>F>wt+dzi zrJPk4X@&V@EYu#I7d9%Yy^dZ&ye751fxB|%(ox97{TcZMNg+x~E4(6Oq4sD3Hbw^f zJZ_ZhNRbg|i+h61rK6AuW0!4LyM=g0h^~&HM-#9y!r69pS%}ioruw^ofkI7bK5`839(eJ+UNS9CKv?`?$3DHk4xL#AjCY^2kFsyVPjrm z`}t!+^pK}_r#yF^OGn{2=S1@Br$Rg;#D87&p+^(2IdhWl;>EPLfAB~147zGW^gXH_Eh1g%G zbnwGGmY_%Hg}v)W`L@tJ4~j;Cx9{>V$n#@hTb&14NzkF9m+zc9+r61f!tAd63L3$cmwOOMVA z``dfc)$^DqEo75>f|_6yG&m2kt>hIUb~%C`ofme6iSjL*rd|Cg&*QPWrTx#jwyOz7 zL4)%kFZ)R$QiN!o*S1}HbY9pmZ z@$hWPFK1P3$6fxcvzDMo6R>fo-#wKk^DES=EO~-buZ)H!7zK^+Z0W2JQm_20rJiVy z&I=nY=IB>jL?cnI5Z_3T zIw|#C=h9I)&UuiR-Asr!LL7AjJ(_@x9(>i0WkusUA(Ew*)RR$1=h9I)j=ExJOIPGx zZGUQO%0rHzM-#Ad{LX|bqLCmPMW5R0wTMG73L4?r(%+)-y;RZGj-W^9g^h}8uj8~3 z`J_@y^)c6>2}VI9JX@+JXZ3~QzAM zl{qVVbY9pP2W`7Tz3M7st<)>i5;egnXmB1RpVtbJEJT~UmY_%Hg^iKg_74zJuY#p7 z*dC<`MnNMyTl!fDw5e);S%My&7dGZ4wx6F7;!$~eAIVeHxpWkchi6N!UIoAGZ3%ib z0h==?8P%4z^Ur8fG+2M{^c3y6b@S-tFDec;d!eN5Y-!4}cK(@7iw0+Gvjpc+nt+|1 zIN0kv_Fb7@%@g9mxAO%H-;(CjxpWkca~>q0h6^!rX};ilw_1W8O~78iYmisx^K+uH zQ;5cuzYo+H^N!Kb1f!r4o-Ms7#P%xR2ZoQf1U))0>gy=u|ok0F7mY_%Hh28Cf|2(jnJ zG~XNU$~D0#XmB3nWv>w8&zsVGO&mdw&I@~dmVDA+p2s>N8cCZP=AOAG7zGW^gJkzZ zh|i@>jdahP9-SBVuDa=}U2T{Aue!FY2}VJK^B^z#E77<{@}J4DZI>RM7k0HH16=?3 zOo-uf)wkYYdz2;^1r5%F3kJA;UPXuk&5HWHFKlJd1f!r4 zo-HjCqOUyfM_etTN9ToI_N4*tQ<+zU=psbnFQ$7smyY6f*eY63h)3kCp4@H;oE1Ep zpx!orN*#GmMu?Wn^7+SCm44{%2?<;WqHvt^ATRq@Ax16D=g+#;67=Z2uyLnfn6OUv z$WX5u$uqAh^+e~=Q8>Y-Vpu~>+gr4J64`W{U%3dh59G*B7-lFIOyj5<1(j>2)&6+2s+BgBJ3q&k8gO~A(SmZ=*>1B7q)bT2Iq z!6;~;R@&JTuKW(Ei4Qn}9-S99DyqGX)j~9u+MeXDToa6f2IoOu_B%rSE496mBk0k2 zVPpKT&%@QLfYdAV1U11ZXmB3nWe*dLxzeUixMxm}&I=nOoNZT=g*Yntm$(h`|1P2Oq=dURgcoH=>fUzB{tzw5#G1IK6A zP0=2FuF6jlN!!`dULlSMQPdHfM`;3f`0WH}?7n|tU`UxnpU$PDaGdiX*%1~ZONcd& zphpw1`PPD$y+?@CMW+NN4(wz!G{Gona2_PT8!p7TVp9V5JZA}dbY9qe&%w+7Scsbk zbqX9WI>l&cf>F>2&z80bu~CRm9YK%I3!867$S<-9@pGxfz~}}GjfN%|1r5%F?7doY zO=4h@Bk0k2Ve?%I`OPrV=q*2M{@R1@8x2h`3L2;@cDA%nh?YW3cLY5;FKisQ?`43f zzNBsnH8jB}XmB3nWusl4T2wdXZKpwx&I_CGYuL8?S*b+dqwdNz!6;~O9wd8xqVZbk zMBj_|+q+AT&I>#I?gxmT(x#qrPf!z#f(GY7@>}sjOdrt6_eOEsO6bvfVe>5#FMG2P zw-lS=JLcN1CKv?`&VyurCB%3krli}pOOMVAo9~@?*?1n?<*NI;9;FFJL4)%k+oQG$ z@tGs&(RpF>O%+S*m#25iRR&Ek3L4?r5?Z2EAK$;TZ1tf>=Y`F8TWs3}@%`+&p3bGC zcpbKiqFo&pqKG3nL(&BG>{}M7B~q_^!^$KEbS@o*iu*}(4+Ih<{LL&_6{LFl{QsEs+`WHqi~$_ATRs8 z5MBCr^8MxrdNct${PxbbLd=&wSfRl}qoD~#K_fg{+9|{(>4U`_L66Q0o9_YHdv%M{ zlKC>~=v+Dq$5B`8YzeLX7pWyueGEa5CSc>ZeWwU5v7@bUF$ANa!FiB9t2&CHN9Tpj zca3cS_)=-t&u{m`UP%qavR6Xo8vcJ+Pu)V&=T+ zss0*u3I%8HD-lCL4^hZevvYedb>c!HmTc9^{ ztG1;2x7>dw&|}ZDF$ANa(QRR>che^?3Q^S8$vpY0Oj zO5rqr{R`UzYtF5VAs7XX2E`urK7Dhu5S9Mv=&!ruqd@6_I}JgP&I@~O=SRK2-ug+1 z4*yB>FSxiO@aoLZVhBb-BhQnMds)c`h4}DzNB^4M^8&@+Ic5lYbY9qFZt3Q2-SM{& z^L=Ul@d+;nhE0nl7zK^VWuEkgTzguG(z`qQXKf!DXm-~*qd||(3wxihhd1%}g2{=Q zC0eKXXI%GKp!$n{#t@8xMxo-pyux2x5{Bx_g*m?%%!7{sb$L3-c_^m3en}mj{fa^&wCH{ENC?7(FE-KNswNY zt9zvRH?R7@Yw$t77=lsIXf{>eTN+b9hz9R=^k4qAtvB<5s|-Pp&I|jVchkKYNu`8% zw^y2f&W@=YhNKsWAs7XX2EPsTx|GA+O?;=LfA)YW$)gTf4SIB5*m;@{@&282jS$cD zO!J>Q)IR0pk^DwO6O4jJy(YuFRUPvSvGDzl{)5HdPZ?V!(Gc|Lys#S_8sXjic5xxv zbxZS~Seugq>*pZs>&5cKH07eyoC{;}Sf z(`Y4a8>jiJOwI89JpEJzp`)G@jYI3ldOzKATr@uZx}*Qfhf{r9d;Sv89!m7rSe%sR|6t#I-|sIUh#?q-OuvtO-n;8DRG;sDm*+8XxvynjOVFe9!rqxU!5gvk zkPy4er}?Kmy3V(C)kdSC2}VKVgOU@xvx%#P7+R#0|6Gf0KJVI~A?VS0VV7Pw(R;Ge zCLun(l&}1Fv?sb^potchN~P1f!ra_=QQ{{a+6j;y(4;g|Uofr190+YSU zl^O`~(5O`ZsJeOm)!#1`Lof;&^)J5QUCulybB04pJNXOzl+V90t*{~J(RpFNIcKsr zzQK7R1~*If@4K;}e`D*HJ)KKO;rPjhFM40hOA%uHp-%qmekL34MA@h9}I+u>39UQlHttBSpLLiEno-eUV2z6E|+N1Nr#+9#Gvt?ak z<{-K9{r1XZ2u2|j?%_RUKN8{_xx0ySch{#C_GypK3mea7UB}zjCuX*gCpbc$VD0yd z#Sn}_CbWgCXI2qnleCiY(n>B@ddLv;=)ADe#@;>9R|wS-V+ck;18w@?HC=^Jt=$mx z=)ADeSAMMYx)6U#k18uY>Qd23F$ANa@w@cRHMdU{0=>4p^xAwy78-&cofkIx{OWZd z3b98j!$zqLk36wBhF}ylP-Fay-xgw|RGGu=q>2vxcaso@rHcM2RkU5+1NN+(N9To&8vIhr6GAMNNN3_fiBw`eSpS;jTR-%lA;CD9P{=)ABoM_JhG zYSHK`Gnt2vv`_ivh&?M!FbW!&;S}C=MKrFg=;&W2v!#~~TxB%q(RpEGu9cj(sAw#f z8Clhjr*0UZULb~G6f`im8@AwI(Woo4zVY9+l{bd120c12Y|I(2O)erDd1PjJPG**^ zKFDV@G{GonV2;{#_XW}DD6`kMWcJ#+XF)^Iqw~VX40%D7LZUHTX2`2$hWz2ZSIjfl z1f!sVnRADJzlla`nS~#fS@>gZ>=UF%=Y@?q{9S)(VF-G3Uf5WRT-*JO5O2$hWt6N~Ql}k@As7V>tbHyY{7i@qvMSml ztD@5@EJ2UX3ma>y`|m#_M5?U3UXhj8?3te#4NWi#8d$@X@vRo3oUHDe$?C4$z@3Jm zN9To&wc+NTn}xU}E64`2g4}s-WemY6Xkabc`N6qDG3qq!5X++FmZJ?Z-1-GXy<4FKn#Uo3)%EL_t~6@0At( zb9>>B*?cwa-%qw~VXp2MW8nhVjy?M&R3eRmAOC}?2! zqdzyB`N+_v7l~cN&5oofkItPBxq2}XhDURJCIJ&4jo*@^AFNe@hqqoD~#f#zOTtOh-Z(nO_v zoxG)!R>slL1fxK6FDq7q9zA@rtNBiQJ}e( z6+xJ`OAn$nv8sPB@0<46FHwEaXlQ~_pu;;}iZB}VAW9Rne|_59aRz%!iZB|QU=(QX zWknE1gC0a_qTBNWyweTJ#L>_Mqd;>nD^`OZL}_CDE%J?|aWyo-DA1Sz+8r--<>or* zL6jz128QQwuQb6Z(A>+4y$*U1rHSk2W#sV8HNhy*+{=p9pa)T!D7JNsYP$B>n z^*d|pT(B9%b%NdT!o7NT!`W5YZyc+wJ<&v)o#W#4K~2DB6m0HgMP7&YL=zt^8y}~Z zL=%j{aqeZsYS4oyO}v{nF-}d?xnMI2Huth(HRy>Zy6>A5r?%@{uo(rLds(p>^h6UU zOTHLqjMBMaGYU5MvSKypi6&|nn;d7X)wy6Z3O4t$A~X`TCz@!pZE~DBLo~rC91rh! zseW!W=s}bwI^Q!TcJ32HFbZ^d$17Tc9z<#4mDDL_b{2c(nqU+)>>gP>4R{cx3Fu)q z7+-?~qd;@_E0&;#*MYpSao=h+FStH;f>9v2`xQ&jqw~T>i%AJ!rXPLfnqU+N?taA* z^ys{>(en2l!-^$ZLlcYw!QHP|f*ze0HhS>cTb9Jp&;+AEaQ7=#gC3n1HY!ow_qW8+ z&;+AEaQ7=#gC3n1HtJ(~;UjT0G{Gnk-2IBxphxG0jgjHO-G9c>&;+AEaQDk<9PrMr z^1XR%wf5+|9Itw`&AY)8`KR|;Rb~Co2tr4}#)xY7$aE{wo@fH&?&zDa=NJA5h0?pm8SPgm*r3tLtB5FxULlcYw z&E2n94SEoz39LUOYGO!36O01Q-LF^;dJv@vtfM1pdq_hQi~`NwuUHLw5Tyz12Skif zAq`D13N&}WVm0VNlqRsR5i!<=G&I2|(BVC@=;}icqBMd1nTR<!)V-n+ek%;bN8gn7=PBPJs6Cp1&G)kG zS>;493YlnI;%s&ef8GLhj0QbAFKoWal{>*GXwbIAx}O{SKUq@QXwakc8okgvUbym{ z2)sFhH^IzxL=X=*?Ch^FFt5E1=h1m#^Gz;$Ryh%jf(C6%eDGdx|DDHn1hhxzh0Qm) zawiytOtdXA2tuFKk?&ePc_mBS(T!$OOBldB^MASG{TulDj+ZfeA)~9-S99o|AoJ3)cZ+Y#f47 z(4cLed4_9OMuQ%m7dGF^ifC6c1f!rq+iG-?{!wmVUcdI}ys-IRmet6KU=%XZwi>IY zpMSQbveBSN=Y@^_Z{OI$y~>GT6f|gCjcln`|K_h_H0aTJjb7-DEokIKpxWWRE^{54 z$XG6Qw@CRG_BxzL=Y`Gpvh2OeiC`2oXxr;JA>+r^QC*D&JvuMP68pv$t|K0yqc}!| zh>RsN4j%g9X}|Vp0yf4+`^FYDav~VTu{K0x6p`_KUBMwngHbv!Y>e~vjV);8L@){( zwC%mhf9123Z&!~r8uaMAurXh;Z)`y$CxTJXplzSvGvAu|mC>L_=Y@@VmwjUk8aWY+ zf(C7?(OKq+W%~^`8uaMAurYtMZ)`y$CxTJXplvmV%Y1jn=yan&kIoAl^I-eN7Bq4q z7zGX5R^vmNw_ktnQ$~XxofkId^Y)D`XyimN3L3PnMnKjdpYP~sH0aTJVPjol-`Iji zP6VT%LECEVk#*FjkqwOoJvuLJtgq}FThPdfU=%cHTa9aEJ=pc5JB$WBIxlRj6YU#Y z(8!5k6f|gCjmfgEy>hIC(V$1?h0XV}awixC4ceA)>vNy>=)AD8Zntl2;W~067==u< zt;P=7XQ*{{iP4}(=Y@^^2m8j>XU%8U1d$YnU=%c9=QHnk$$m-AZFaxJXwakc!sdHf ziJ4_&&fs=fVhBb-12*=95;N;ns+r?X5eaC-e`8DMg3bLT*!cZo>@6*t&bbxtvi3w1 z(8Eqq1d$ObREgD6d)Z{qI< zV>L9vDA4$Qy|d$|MiS;a=s}bwP-F1-gRvT#U=(QlPTwkK&1Y!X}>V5i7xKev@}-2obf>9vwi^q?2 ztrACr9-S99*6sM)zE}-SFbV{It@+Wvc1Df0+N1Nr#{L6#abh)?OGm-RZ&uI!^KBHIxlSQ zyu@l~f>9vwWY5noxIUJkN9TpjJ(=7IMu9*Jy?$zP91VJOUfA4uiPg{qqd=giy|lA$ z91VJOUfA4uiPg{qqd=hg{Pf!7I2!cmyhhLMyjYF%QsLSbeq)u+rK4E+n(d$OG%oMH zw$?otnuKa1Jeq*bofoTdQ7U!6_qMOpxpWkcqiSP^CGyHOp`&19lt{1rP8@=sXoCAF zu^O6S6ljcg6}zp8qd^a%G{Jq8SPe}u3N%LTdnG9Mm427dCfAVl|jcN5RHQ|JW2?91VJ+3G5(X zza&;e6N~~K-u)O-wfP5G{!ziv+vl1$Yq-zw{y&w>nQWK+i%8<*#oZ$a=F$Xg9DmB} zun1A(L#tul_t6BSBCov8txrb~ybhh0df%9P)uT?0nx!8Y5)8EIY4(;h0UJ>uaM#b9 zCs_NZvRP$?*wm(Hfa=(-fz6!|YTzy`n%=Z}jVz}T(4J@l^iO6_=F=M&XMOuj-{AZw zf`*_+6R^1}vS|8{CDU>w7zJ(E`OTipxXQ<~c3j;#SmukoLG96bVRKhx(e$eR(^=bv zC>@7j6f(iCZuVsS-HNVE%6Kq%`e;?7L66Q0o4X>5ra!WzRE`9rpaJ`w*^?Q0;^vh< zx4Au-x3b@8(4+Ih=B~)1=_7kr&5>XfG+=KsdookT*IW6(e~JVb9PMB<=+Sv$b01~V z^!uvU$dO^;90^821NKO>!_uNr zhm~uWPYAR;IM8U&qw~V%9>=2TZshc{ly!+cz-n^D0jRrkBFKq5{ zESmn>&@MR=jDiO2;%0~Cmg0}BeD7Grl#{zh84Y@LUfA5@STw!iur4_gjDiO2(`JXI z$*pNCeSbCZ)one@Xwakc!sZ^wqUqh`UgbnE3L3D#Fgq-LZ)&}AaEW((ul)9m(V$1? zh0Q&VMbodBXPy(mC}_aG-R!W8tlMy9xtZsD*PQ5TH0aTJVTboi#)(GR90*211NIfO z!_sa2-7Ck`sNjEWW;3HfkIsuZe0cYxhV+kkgpPs+h}mYpWLmlFR$iZ8$G>%44WmJi zCSY^NV$t+LQcH3o7zGX3!_0n3p{y%eSGu+K?`u@bXwakc!sd>}qUjGwP0Wd46f|H@ zF#9DdYHiCZ(!0BVboJw=w$r2Y!sd>}qUkl)?8=c~6f|IWHTxyY%8k$Zwq&~h&7rf6 z20c12Z0=YrnqI}<#}M=55R8Hb?8nW1$xScp{NRTx!~Nr1S2P;*=)ADGW3g!Z)&A3l zm=K3x6f|JJ<@QT3)6Fp7Q=~mQFKzB+P_UOE@aea0# z<0}8@90^7t6YNQ5_v7!In$*Pekh^O%=+Sv$<2l*AjPxbb4Dm)Bf>F?beb(%LWY*eN zv$(XY^y=CUMY87QdQ~ao5s~J8uaMAu(@NgXnOjRQaKWgf(GmpX7}TX^>-(Ykb2d9 zW;3HfkIu{5W%n}ftzIK8p`)OI%GcEFevGTzFzN5t&-u!o=xQ|R(FAPNW4kl)>REq| z1f!q&7m1f!q$pU6gsrtd=8<20c12Y|K~e&cw`5Qgb921r68>&0a>cMjet4%KWO?!GT7D9-S99 z=3RDYVqW>22}VH!_71a`(RN4Eq&_lF{CZYTqd||(3mfxCyE9Qto_TK}4#XiC1r6A9 z&0faL@%55w%6#|TqaBO}JvuLJ%!BPdgRF6ql5!vz1r69m%+7?&+mqb9-DuFG^TNh_ z-tIHVygf&PQP6`cfys-{~<84Y@LUfA3lFzcwAZXFdvFbW#5UpG4wvL3AI)`OWGvFeZUN3Wfj^p9+gJU@m?SZWcWU_1L z+z3X2#@X7nb2LE@qHy1^`qkPI*Aa78n!uB_>)L1yULo=_3O3rCU8P17^dJf?AM4Ej z|5<4QebcTWqc!wdF$x-}7OJpyu0^mm)H> zx{iX)8BXjIq$ird*kxBHq6Rn}oic!$O zoXD2C}?n28~Z%y;hAW{t|p>2Gyxk?ps_Zv z^L9m;CrA&^L=#+1*p-A7A9Ed?S%P5IhRqflx{i!^^-2@0#~}iMIVEG3CyFKL zL6jyqtIeHY6ll)vVhMT>r3ubzb0-)Dnsd|$!n9qsA4KVKm+j3E!SQWhUODI1MKvCq zQ#5%&daBoCPTO^l!>%yN*f@Urwp6dT`D>w!qd?c&^ssmT`KEC+=s}bwI-H*nNrW^s!6?u-tm)#NP8t(OgC0a_;)zS+ zBZ-iPCKv^J{q0@7Gf&Qqqd^a%G%;i2^N~bILlcYw-Jp0k@AennH{(S-4|))#i8d|9 zMG_$mO)v`dTkW6pF8^mooa>+mQJSc_d`u(}($EB>K!0|+r&sO3S8+7xL6jz@ewPtR zgfuk4DA0vA_w}x7^nDx+dJv_F?@x}1BtjaRU=-*soBDgd)py_U7x6skL6jz@o*EKK zgfuk4DA4#_p}DX8;;sYLXW#9^vf$AbM^_^Bm3uxqX}=2zXO)m%I%5?Gdf4Y#yU@=g z2~e7V%_!LDD+jCBTpy|o01QD-G=aYPzX?WxMqlZ(pm!V%dJv@v^v(ZGFbXvKN^<^} z<7m)>C{3Vm{%?X&pwU+b-0Hpy$vh9C{3Vm{%?X&pwU;_{;)re20e(<1p4OxCKzR|L;7HygNNg2px?oR zC{3Vm{%?X&pwVkjRKxc=4_Z8;M#0ok{sz~*n!fzGs_U-184~0>0COB+UWC%nals^Vu%ME!TO;I>a{XNnf5nl)xTw^KVd~` zK~1 zR|zq|5%lQ1uyOnwQ?IT|XrDFuy{`U8x}G;0nqU+(*yj^7qX~L+Uf4L^*VL=}cRruh z_)K%ZzgPL7&ZVPpoPA!tDgDj@D?y|6O4ie`+Q<%G(nHf3meA=n|k$6tyU}R{hH-FeyE?(&;+BP!9Jgu z8BNfm^TNjQcBWqKy(x9&Q_E6)t#27>G&I2|Xs}8oW=0eA=)ACTysoKNe=g~=@?iN_ zQ>3pO4NWi#8sX{_P0*wB!p8BROuYiJy8NpiH8jB}Xs}kw?`A|1^ys{>aok?VXL7Ih zEK3bEb@xgWjDiMheqttwFBCzK&I=pI?eoZ%Xa2;mS%JgunQMYk<~l;78i?+S;Haka z8a*LwySiT5Zda)z8LsVW0yd(c!7*6A*C2|ZN9To&aNPFuOsOS%q>hv+U~35p9R&@}i4rrT33@aE8^>+E8YLC(YB{5q zU4_#GqoBdLo%}|XR8bI99YK%I3meC6-K}_~UDjTC7AcR~+O7#kL4$MD#LQ@d9-S99 zj@$8LV(SW7$D~a)la``$=_nlMTstxI#nyAOjtbGz5%g#RHjdkI@WdaGz@|Bg-7S59%fF7nLquJTod=|Myb6zmyW`5 z++Dku@xvb*YPfr42zoRD8^_0+-H+E=SE%`&Ji$~MLv$`3h2v-?b}u8Ephpw1ar{2B z`%&UbyPEr??cV+9d845TMnMDZ*zRSNy0Wk47ed_Q2zqp0*f{=++5H%QXMv%$Bw0jxR1U;I7jpM(Y-H+F+R!TY~wWM%?T1G{m_cEl48bYe5A?VS0SsyQ%-4ChMNv^g>5IPDPsPlF& zBbuN`6R>goF0=b_p;oJ;4pJ449O`E@G{GonV7#(>8PNniIxlP-Pc*w9GIA!lvDRp4 zf>Gu=LOT;6+*oVR%6W8NqbJ9IHM<`&14wdnh8P0lIijF}`Geidh$iUKd12%D9JBiY z!p(h*h9(#V4a|e=o=h}BkIoAl$L)26}xO)v@?n3uXeG9m6#1U))0Y#g`kN@mDOZcc5kLlcaG2Ikkce?$}X=)ACT z-1ZNd=_k3hfzi+eqo9Fxg6-!Z+}gkp^ys{>aoqOvQBoN$%Npmj{L{H~6pmxPW9wBk zL60V2gIjvohFRj64=R>C7r+c8QLjDiN%(RLh+Cg{<5VdJ?djuY*xdcFpURkU`5*0xCh(g-n3v>EFpBhP`S$k^kpUfgcqTk|9LM*v&X?c$2@wDc zK@Xxd!TS~>GU5@80*&vNT_V5p6CwZ@4SEoz2|oW2kr9tz6lne)Ply0uH0VK;CfM3T zL`FPq;D;0PBYLK@5kf>E&f>pCGKq(M(K!I3&dgfuk4DA4?Moe&Yypa)T!;EW?ggfuk4 zDA4?Moe&Yypa)T!;LIt4Fi%hui~`MH*NGs^^PmS&n&1pDf-oAIU=(Qnx=sXPH0VK; zCODIgAdH447zLWY#}iA?W3I!^qhVt{k9mnDGRI$BwW{N-D|Id%WoG^ES9>h+bOry1fyW%*IZ)9bM1*Huw#LlOsocTfnXGD{3;FR5)p*CSM)>^+&hUN%)QbC zqd?=ga$@H`+5=k?$n<}Foq4>K(-+3CNF_8OA!GRwDhjFIb5cT@BuPT0d7it?_cka} zicmDk)IgD;l-_fYWK2RbN6C~~PL%pR&))m#uI|0%51;ip&-$+C9rxbrytnfnu=e42 zwSTLqUiQ2 z%m#~FpHsssmSD_zh3^1^dAtgV-j{X`D2RQcu+!F^5%YkIQx`mt7nf$ zF5Z$%S;f|6%p>C8C4~KO#G*n7WwDZuVe$D#9Nz5N-a`i_r(QbOc}TH!8SC9e#pgc- zkAo4)qKB@l;`0+fPkDCbBVCi9?fb-eNU?Pp>zekzjZ0#4A%wE%q1Y2YSH3a%#d#&t zR)t#xp)6KX?1^jYHB2_EdY1E$V(T*27;|VsS@clsiMtZjlJn=(b{HD zdt%)1)roJ5_HiCkY+c5h9}i6^iyn$Sv2o$(#4$Uwore@#m$BygLlerPhhk5(Zk?5= zvwM{DkYejH)_Ud8gtF+N*b{y}&vhPBY+c4$sU4b77CjX2->Q`MLyE1-SS!v$6Uw58 zVo%_Fq;x){*t(3hay~SnEP5#R#CyY6r#6M_N{X$^SiQla31!hku_xYL_;+ezxId)W zy7pb2o>*T9VY6%}En4^1eGl@xnob>$n==bvA~c}TH!8LMwUG@&edDE7qqN4lnS zF{@d{)@97^AN>0VUp#Euvm{0pLMV%s^o<2PiZpxn?x6$I>n@q=JfzsVjQKr^Uo*@r za^pb>Wzj?5ycD0`;gf%sosvBwef>8{=OM+`Wz27i{JWP&71?}HLRs|CcT2_RPi@j- z+0xND>8&$zW2@M@{9NB(6`!AbYuy6~DVAWYw?h}7Kl#?W7t-UW8M%?IWwYeX)nM`Y zBm4dD0D{{~#S-#MJEFxW|6EuR$Li{Dk_lVOW-;b5oP+0s#pm0#C`SXk{?v0$e$Lo? zRQ~v%3%O35vn83Z3P&6fjJdYsca~g>ZayfXELLLdx9YINn_a|J-;_(|Iu9wfE@Q6k z_}y#q`8h>yJSd?odN6*%5m+Z)6xNB(LyE1-n5(`!uec8$oYBHMu>hegdMNfpSSKc} zV(T*I+HT>U{$EsOWgU!A7AqcIs zTsEh+^N?ceGUnQ@a&P}Hk+QN5MktFOioHiotP>ZV+}wFcv2_`9ZTIhm`@()4j8GOm z6nl^ESSLQ(s=f1&V(T*I+Acl5c-=!2%A$v2PdtQm;#WocI1eedE@Q6kK5bPhcpQvS z7CjVukD`l4Cra$hb{aXtzqltmB4-s3y06Wi_jnMiz6ZknA;s3^ zyu!D@=iP%d3n6Tln{f^x_F^-_-omd6qu2pPZ#$4OwjqL{`l*LMly+>FlIu9wfE@Q6x^5&?65z3;6 zV($^wi3zLNx{SHD^K&iMi5G=+VgW)~ti;&QwR;Y0b`jT!uU#_N5mIbj#$1>9HNz~- z=LaK{MUT9dj9vR!#nxr4RaEi$%NOOw6T6O1zc6EUbm~vpbzeVjTy*!$a;yGcH>Pf- zQsbgh(;Bbj=glu07u|@z*orf^HQOFHX+PTi>q)CvLh;gnj;|eOR^)lag%Qk>ckQJ^ zKvX+zdtCL6Vd<9B>ZPn=>t?Wp71xZ7j+nIs#MHyb#5-0GPG>gVbpTq8Aod12ORZvGLF?jnid!RwzIyiJ}iBMUU2R=0r7n z-v(mmny=$4W|T`0tl!)bQfytujei>z4L_?th_8;D5H}otWV&JXHU$V}(WBToqoR99 zj090@%~$c6JwHgjJ+!MMq}aNQ`yM|c`hMS75N$5MBVK&hi>cG9^(jCoiyrqj93J(o zIuS&o>lbnDtbVDkEwddV#nxp!xA@TLnxF0fG3l*IaotsQQbkLTC_pHS9;esIj_zJ{ z7l?D0ej4w7-P!>HdI%`nynE$}X z@%W!gCnoIiTP4NTWxVdK{!xcX6X5aaFVo^(PuEGzEmDwB7Cq7@_7BJElQTYyOHX|w z(PM5uw^dSXUB+WS=o3|0$TK)_@_q5A*)Jxh9PiJEC6q;vYcA;%&iQjKHpK0p+MDPw zoOj`x_k83qWbXo{*&2pyz{(doB96!u!OSc5jXB0?!leY*TwxCUY#tFQ_*=y zv2_`*f4gh+WA&5a@!TErHk&`yc}TH! z8DBZ9bM#2gFnHYk-;?ppZ3ib$c-W7-mQWTw*4FGC`LR8F<%;;C4Wp91w!PwJ11YvH zw#xDvPFN^JOcg`}-zv-ml$(Cp^tfVk$q zb@2dP?Zt8`Izoyi81r74wC!9FwLYC4FT)*m+x)r(2xZZOciNi^8i8noyY@!hwPnZF zbA%LImoe}8W7b^*;#iCfk6~okeRP`wgtF+t5#yZY9YL(X=u-=$Pp>IAJ3@-B%a~(V z`Wq6{F~Yr%5pL@7eF_lDqK6y#W()>lMo~vdv2_`93~t?g5QxX}M(P5DvgpAvf7#Qc zLF8gKc+k%Vj*w#OGUiU;sj>SVx>nEAe2Q9HwU|8br@#nhcG*LXqoK@DYh*dyj6ztkYejH<{G8ctvAABH&!x@u##y|-CtLhP!>J7 zCi2(rmsl-bj@8nT`pun(6kC@u*IE}vb>J}@E3(6}B75vDe}7m)S@huA&fkOUu#gzQl_iu#53aTSm^eDD!tW~|yYWhjt;?7z{aa5T3Xe&7 zEByk5vgn~sCeECf*G)J=imj{8g8fdMX?sLYdgsVzqbA*o*FEmv*>(R~J0NO|xoFtP zId!W%J=n$ky!QtKqJC34)rvDWfq1XPQ&FkXZ#hCrexmj60nxt?%y|(b9Ec}wofn;W zU8RV%q~s^+*BuxYf3Zo}su90Uj&53VizB4uCpJwV7~OQ#+(3-^xJPu@n2C;%lAqYV zcVIN7_d6iCRpp!1h`zXLmLsI(CuR;C6iwayD+uoSwtIi98~0n}2r2oA;rj+fo5viE zGsvU+N987U{}{H+5mNFK&)qdRx@2;35cD{E!W}O)tni#8q~s?qsW2p3`T3E7n05T3 z)psP9Izmc*V%sA_qWVYv7mn57%yIGU6&`hjl>Ee+v$CTa4}KMn)ri-|#HVCuJEF>r z?5M}sL@9+43vwr1 zc7Vrf5mfRM^#1SIcR=vE;(ok-b7wuulNr~*7hDeRztU3?g%OQ2_D0x&sPdW z8=Q~9+1ZYelAqvtdi#b%AdWcv+PKoO_c}sKeuC$`+BesM;C?)X>+1I8Qb+JASvRvo z?fe97wmo17V4g2xWrgRA;2a)gxp z1n%=yAtgV- zacp6O69TcokBN?ulAqvMdDJ~WR(m860nSr0P-|@Y{HV{{Vxa(w3 zNU;Rtj+b_cey?}TsyOo*h$C9{PG+^gD`9KdEPk$4f1G&&#Bm@twDW`%OEB(Gs%!N8 z^he+^7sU2Y+a_On`WxqA31!hEuQ&Jy#99#h0wKlLW&CuXZc)xF8F*|4aZ{iA$p+t@ zn6$NQ7C+ajKhC`R=eX3{Ab#HN2`QFfJoD-v(dU!Sz)I!|5Y2a=k(~2uZRcSLWzj>c z{x~xMTU7+anm|agbs6uwt5>w_r+VAf(v3jC=3uAALW$8$9j?asT?G z6L)_!$az>oS@h7VKh8V=VgiUMfskVBGQRrPLDB46`@&-^h@9qi<6FySI}b}Jiym6_ z$C*(eZZL!tTbFT}pR=RCPT)$v8;D{Jx~I7r7zio0F5{Co z4UdYx-2omigLrn#yQ%g2+B*+RD2pCi_4{Kr4a8@GkYejH9yxVX)Ou@Ecr1g*i^EH$ zyN+z+JS?FsdT7-jXNKWwzYfILfskVBGX8I0PE_v1i{Nn;h^j}_NH1Pi#d%mlS@h7V zKhE5`b6n~f5UZZ?gcMts@%Z9nqRu@^!6O&Mw&Sl&w_H`!d00YO^w6q5&NK$`8Hiqi zkYejHe)Z!q(ao2v$5_$}M7c*gr#F4R!i`s!P!>J3>W?%10`Yzzq}aNQYj+tN^%~U! z9-~1l{dz#UW1^1pu!OP>><#AN488#3oO*QF|1-UPAKk8qBVV(T*IXxDezaCkfhVt)PZsk1ut zD?liV9vqMT6O*nWPQ|!eDG*X@UB(=>FZhMeaYln!b*-B}oQEZpMGwv^{)q|as6CiJ z+Lq0BgcMtsF=wA5Wyix~F^C+@YIl7!r~siXdT@U9PfV78xZMy^Y+c5jMbBB33y;@8 z^up|XbGp0pu!OSc!Fk?4F_{mdxu4G+A;s2Z%+)~aC-_{AD}XjwW%Rq`h604L=)v`h ze`3-B#MvP32!s?{moZm0CogLQkB%T}VYT$&ueF_rC6q-EuDkpblU=ylD}Y!R2r0HM zW3JBXUt1d<-+<_kRbQL$PIMlYP!>J(EF{hxhP(Dj5Fc#!gcMtsG1sD%pEwR4Wk77k z>UGJ}-}uodJbkfQ^x!(!KQZCh{x*oyp7DefOEBhY`OZ^6#CWv`#8FsKd;>?#IN`iQ|ohPK&x{TQy%)X%?JiZ3e4BbS!!s^)8vRV9GeMy|@4Wc!O2Tt~c z6iYBx_Y!B)(?3r2Kf7zP>;un4Rw=r2a@_@0x<)O~QxYb211G@7;mZBEqZ6`^i^@D zC5X8lnLW-@+_|c)=qf?L4Duc)kgcM6K?tDSd=!);ly%c9o1#v~G6O*e(R&ySfP!>JZi{NPnj_zm> z3j!g<)@9tTLGP&D2i4*6K8W2lzE5l}-^h7bLRs`sFA`@;V5?pP@kJn{*t(3bSkO0m z_q!|MQ3^zxZ8H+R`?Pf)mQWTw)Qg~_2eEhSj6}~sNU?PpXX+1#>bGeHkMlvSt$IXa z<)+Tg!xGA(hk6meRYj{EkysrFDYh=-mU{<9CqL2_9>>Dtt=Fr^Lx1b)JS?FsdZ-tP zGsl4_xvqLVI1o~7UB=~J&W>u{(FPv8O7{J8Q|gR`9h`?HltmBqB5`IV&R`=D4FVy> z)@8h+&+zEKQ=7x%4iGs%y`1`At(MNi63U{7dXYF&3|sXbh+i-DgcMtsanIjJMr&TL z3y)vm@#r&0rsq|ToQEZpMGy5N_^uBguY>3o2r0HMc_5 zcOI5d7CqF9#F-aCd=H{QAf(v3j2leOjb_$393C-)7CqF9;IEKC zoC{)TAf(v3jO#ZZ6D>b!HO7+JAZFatI=$z?r<{i+ltmBqBL4m$(c~ddNU?PpPhCGI zI_t9Q;n4*i0}ktyuJT4T=V1wD(L=om{*nVkUl2zHLW-@+xM!KM(ae)i#@Id##B(*e zxgN*XvRV4vcPv8-A*fhF@#xP>)Q&SGdSR<-zELe@YuPM*u3jY0+yY_{h&}5(A;l7m zc@(-Vm<5k(K`g}4E%DG(&chPQqDNlGaxaLkI&SLE63U{7dJ#Xu zJq+T)KuEE58FSR0*K`0pzK2II%m%r?b#)$=P!>JZi^Q2fK$HP7IuKH9UB;Y!CUhMH z53aA?!92KjQ)lO431!hky-1uXi!)fv&x4MTV(T*IEV|^>KJbV@w8ebhp-6MYyoQEZpMGy5NaV86U-XFy5KuEE58FO{^^pf-8@ee#Mz^ZT5 zou!L%oQf4VHrF z6bLD{E@Q5iAMCRh;}z$RjaY@}Y#Qr4ETJrV)1I&ZBf;fI`qvUBzdL?Wvo5j!7i^Q41 zAoh-FlpGrfDVAVdb4~Z?i63%SdE$yEYbQ?{`m*z|gtF+7*RgP`+JX2p+Y?f3UB(Z5 z-6J}_^ICXx1u^9K%E@6H4@=rwHjAID7l|_iKy(L@69_4mV7#MaujsQbhrNWqxdZXt z#1hFPUOd%#SVCF!P%jc^cs`2VQ6l-@3QtI}bs5*I+b6na_et>JetdEG#>CDo7dsD2 zD2pEIMX-hg(FMe=KuEE58INt#FFJQzRd|dCF}MAliQk7@={ziwh4@)SE9_mH#n zGS%+HCeFhW%A$vQ5&XRmhu zh(G6RL0l0CDYh=-Z%!H+jhIpy9{0ne=3{@RmX$rvd00YO^iVJ2_xvOfZv{e%t;=|K z`O#6CZ;yos&tUC+<L=%HQ&eJzM4AT9}n6kC_^eOQaupZF*|jsdZKSN-&aQ)W94ODKyT z>P6yAeGp%Q_~v9!NU?PpznC>9dL`=;c+>;Yt5&mgpFO8K4@)SE9(f&05=3th{R1Jz z)@A&89sFyK$G71xwk`*8^byy&9>>u z2%ZUpCgNwfNU?PpbN0D%E}u>CeB6S0@bN9p3lPephk6lz&Z~o16bLD{E@RH3 z=ak~VkLn1Ir!b!v&A!rkSVCF!P%q+VgA*{Hzc<7aQfytuTn$XS`YL!l1dlaXWo+7V zvGcHmvgn~+1pPUPP9RSG+7nW2UB+D1yzp`jc(e!cE7n)#UOd%#SVCF!P%q+VoIN0Z zTj2>Qwk~6?&aOQFBzW|L#|W(YdTcz*d00YO^iVGnXWD@n3}Rd$q}aNQxfcEY*S|5A zv;ol)tJm{~zU;;lODKyT>P7rkT@T{>Y)?qBbs2NDydY;8Jo>_;7}o7)E$QVvETJrV zV}E@Sov^Nwo@4-(_iP2@Z`FSfO87C+DHSbBgM3*wSMNU;QCbuV}( zJga8m&< z$=qh1kYejHPJh!os(ism@OTTv&GUXvw5?boX=~Xmey&~w-#UZ17euQ-NU;Rta((+o zHyy)&^Rp7f)ZQ;7hMiW?d00YO^iVGnXPyS}5{RLJkYejHPF&nSO7|-bkEI}PySaB_ z(7^MYhb5Fn5A`B(<^>Q{Knw_k6kC_^#!3UD3g?~-k7r3tT_0U`e+}nh31!hky$HVL z1ThRm6bLD{F5~ad8ywaC;52xUIPvtO;`Wza>^v-?EPALH!Cwx5Sa#Y`amPSNv2_{8 zmu5%*HaHa?^Ffq3@x0W#*H(8PmQWTw)QkA@(E-HjKuEE58F#pNShV&|{%ei}@c4T5 z1F3ZJ%Fe?Q%A$vQ5j<%C@ivI{fskVBGVWe>M0DDY;_$#Yl73|P#?&`&mUAAKP!>JZ zi};sA;s2Z{6IW1I(*|#xIf;5$K+;*r#pA~*WH7bP!>JZi^Q23Ao_x6)YTJG zY+c49|2H~%s6C&tJOEq0a5_ z=nJB5-*eL4KA+$`ETJrVs29Qa)F7&ZnDm(^q}aNQcNWQw8l_9aV;+bn&%P*q?~Ns# zhb5FnkGziMUl9F4tZwBADYh=-`)cM!zZ{>79_Mcm&-cFA^*FYc&C=(-W7$~SYHJ_{S%)RJp|%;TbmsU zI&o$Lh%UHmm%UjoZEM*qey(1`pO3C1Hl`j4gcM6K<|r{~*%|QQ=(7f+&xYcaorfiq zMGy5N{tUhcVq+kr*t(24+Es7P-%jAT+a06mo7Yx%9+prRJ=BYMqK_e@*t(24YIiPi zDLgnEyzgfN=V1wD(L=pRoOu#Nm_HmL#nxra+2`?@wcxP`#C*(Z$@^;*Ae2Q9^&P7I4Fo?1s-VcNnTbD6c%khqx@aP6& z9oFry4Qb*$ETJrVP4_Z2JsGvv4N0c>oP9&NS|opuzv7Z3}V%!?-FIo zFLxf6P!>J%I+i&gir)2I;&qfgrBJS?FsdZ-uiTU8#!F@ca`>oR_) z#(?OrnxDagTlMDC^%753Et#~nY!*LPFM_|O;(k0+FY#<3q*#LS)h`T;I)B6UF0b9= zo?Z|wexr=@u!OScp-|i)jSa610luMW!&QY5z&@kmcgSRh`+0Sl$w9#+s?xh z%A$vQ5&T6wh*H%*N;M0F6kC_^#)c!KfyJi4BNxOk?`BfdE}!Q-ETJrVs2AaPmMzk| zL6i%G6kC_^EoY93zHM*?Jf?zp^`+wJH~zfId00YO^iVItCyKbEKrH>s6H;tl#{aAw z6;-+5ZH&8vKs1|II(_`VU%HXn63U`SUdPfN#H}E<@AiZgTbJ?q#YRVi`>tG$e*p?& z_0{EEk7H}uEPd`fmRmr)l#ifd3B@JLyonyCBZ$t}s>}ZUGPbpB7C%=n!tbeDq%Q}t zYPTn(Sb{N+LY=3st{rD`Kzxj&yW`KBQnr@O;^*o`_++$2`biKI{_=zrOEBiyT>I2? zc-#wO4X%JZi})GmR@~>81VW0f z%b26Y8xyjyPUQTtP7I)1c-i^ zKNbf`Zf~W-I&Ok`9 zbs4iaC|kNbJRSw{FuI8bd)v9O-4e>8M_$LmmCPy-gNw9xgcMtsvAP#LiTj{yx?9c7 ziQB4Fj;!+ekXdy*|JgTcf*wb-?^woxxDdp&3p}A-#uAKo9MdmaamhS<*GFP+#|?=M zslS|uC6q-E^&)us3ZibO4T(9iC#2ZAjPJO)Uv$J%OE900260izR}yVXUX`%5Y!*LP zFM{u2xc63lmXE+5K?Si z#@p`bADyzKF+6fX+*$kX#JIB_a~_sZ7CqF9U_J-&HHc1ukYejHUfXs+w6pCnc#H(` zLbW!DMETXu!xGA(M_$K5kK?PiNz@926kC^Z@1_Hzxu@O@k5M3=D0^(8XyRSxVF_i? zL%j$_Q4mL$IX1B?@`Myym+|SX21OMnJ_wIHNi@uk_WiTTd00YO^iVH?zfJ(L?27DY zZy==Dx{O2je|*MySfXUeqKA4B z^qe3TgZSeIPe`$K8Mp2?Ec$(IM|eB{qD{3qsc6gn&chPQqKA4Btgk?H0&z+pq}aNQ zw^tt?Rcdr0JZ6FD@WS%co11z#4@)SE9_mHVL4vpn#6KT_)$a^PNU;QCp3MuMZUc{du~j2*m6ZFqM*%`v^iVJ2uf(%JObUb)TbD7f z%Ffj$!Nc4i&chPQqKA4Bzg2^9e^lLazaymBx{P^uz1{m!c-({iD1p27iXWdTKq!kI zc^wN!(PJ_rQvVBt6kC@uM~N1TR={ICi2fL_`cGZ!JS?FsdZ-uiBisgzR|5kf#nxra z(QeV%ufv1mZfA_5&y@bad00YO^x$~xI~EexgLpa+Qfyu4hp~M_yY*o|_G11h_WNe% zVF`}y%%X>S5&U%uh~M*(ApZSl zlOv?qx{NuCZY%RPJUD-3V?JLWy<31#7CqF9;9n1b*jxJ8M0UaxQfytuTn+SJl!nJZ z5HqnZ=~jNV^RR@n=%HT3uVi+EI6DwhY+c4&)vT|z6duFiF$=4uS!X@wJS?FsdZ-ui z=i?0!$v{Z4bs2MY)}g};cyQI%9jm?{FQ4E%ETJrVs29PvQ6TDr7*yXAQfytuTxDkc zoDC1I$Xa3b+OFhP&chPQqKA4Bzd~*cV)ap;kYejH=9+rpPnW@itMH0gg`brE%ZJZi}sYww>{wp9z!Oqz zUB>EOu*#^BoqlxK_{8|0Z`HNRark{e{rZ|{-?7XCaXEQZc7#`=X%e8U2w$;B-a7f!5{u(fO!KUXh;=TRVrf+!UTDVAWY-vq_CA0S?9 zTs84xrAE%f63U{7dJ%lj0Ag{Is)=g@A;s2ZtX~nu_Y5GODpxddLQWUwVF_i?L%oRK z^DoL4O*9FF6kC@uf5FiIBIzu6e0So~sP^~~&chPQqKA4Bzg4p;ERAXgLW-@+Sie+? zwGTX6jV&LQTsh8pSVCF!P%nZN0EoZFmyeDQgcMtsv3}bWD`XIfhp$;Hg)@7_;L&Zu4#OnWs#{*lAaUPaX7CqF9_^nFs86Mvr2r0HM zWBq=rKf14-{zcsMqyf&u63TM>vA<*C(f#+dFXD#-p)SSNWt{h0tUOkK-F8yy#&>V@ z`w?o{EPALH@z?J2ARY;X6iYDHuey5T!iVdn_Ab8Ed00YO^iVH?Z)>qtmx0(32r0HM zWBm@S_c->ucBxU{mU14JP!>JZi}<4(f%s{gC#2ZAjP=X0{tOl?pPf2u(^@xPSwdO# zP%nbtJ;#20eL{BXrOlp@V(T*2Z_Rq5`%`06_hvV89+prRJ@Psh9;=QZW)1d)6kC_E zex25P>?(J=>v3!?o2Acv$HM*iCm%t@5{msV-EymL!B*vDH;Qd7o5j!bI+lAti~zBC zuqUKgg0X%9*Iy;Ia6U$CTI)P4p)7i+7x8y(HqJ-?&7P2A>oV4F=KArf6t0q1-&Z(UA;s2Ztl#DJ z#6P%eo4s_$;h=VbyR#nxra)qwv!WUhT4z`A7T=q}E~63U{7 zdJ%sH_hMZ#Eyoj5Y+c6son%jZi}lqfl^Qt@ODKyT>P7H$30t)g>#G;e@`Myym$80X z*%Md$b)xgIgtF+NUc?jiK-8aD$q`a)UB>z?X20J3)vtHmcx4G?(L=q6C;kL+-H(4d zLW-@+Sij!viRoB{ciVY?0YX{yP%q+-?j#Tw?(&2bTbHqZ@7doU{n1U-?)lbgTgzs- zV-=pa;5!+dk4Hel{;BmYL7 zLlMnGmgq01Bv$R;s$6ZA-4ecmL0h=|?g`QMBl&lh`8}lM-zUAPZ9lQ!!xG9O{nV$o zM~8pG-yO{FA%$6%$a{xdAohD$LRqBm*)~3^I-9?dlHWrLvn-)Ewe2VNdssqQq$@u= zF8XW2-Gz8aVU{KIrnZ6}mQWVyhprhLy)xyIJdgcbC52g*XuCW&YW3r@c|;cfW0sWs zgx>8|aH}k#EYj=#8x@VNoi4;f3bQPsce@qzu!OQm@4I<;)TZXTLOi4}%MyCGTR{&? zD2sHZhlfO^7QS_$N3K;ETSD)4+wZae=&BY8WifvAiviJ&+&2sHkdmL!o7xI`SVCE( zOP=31+PHR2As$kgWr+#%dq!0rTzMejt`aHv3BB9xfUR=J$`Z;V-Ku4`Xy2tz7UCg= zS(ebd-3oeGLRpS()j4`-*UUmZt_cdWETMP174)!#vPidEd0RB%>T!j5NMV*G^lrC; z9+prR=~`3UM-y7|H@5PRS5lZ|3BB9x0K%OQODK!<#;vzT?b@Gqpob%*Fv}7by>d&` z{%?M>oqwyOpC6q;) z^ZaA)uPVet3bQPschnW!4@)SEG}kNZK7O_k4=K#Dgx*nC(8ChSBF%Nz?1@hl;vt1u zme4!u3VK*VS){psEP37og?LC|mL<4GO;j3N2q7gup|{!<+$u{bi!|5TYc}zBg!7+s zDa^8j-fDMfLRqBQGt4Z1W+5I@m}LpQ2d|)qC6q;)y-#NBhlR`rQkZ24c47YQr}_8X z63QaY9`1ndZvTuUg;|zRXUYA@%3YnzNnZ8pZ1;v$^@IOjIO4!Ab$`zpwM}1W3H6zl zV9d{3DN0spi4R-ch!Ar{(L=E( zzPqkU@~~5r&O?f=%UJ#Hp$TQtL$N2${$I)D@+~(w4=J`TWA(d-CX_`F#hw_`|NX?q z2fI5DDYhEn4^1eG9*RA&^y^dNAK%My9#U*w#_D$uO(=^Viak;C@A|1aEk-yGDYh4LlerPhhk5>SY}ZAfe{Zm4=J`T zWA)UBCX_`F#h%!;Z)AF4^HZFM6kC_Ey6`+=Kp}*(=)rhy=;*U@wVJVtt*co5ITtbR z8}!`52xjT?{X{PJqkr>LQdY5b8FT;CpJ!zqj8GOUDRz6FyBz0Z#)yZUhZI|vG0&6w z^Q^3c5z3;6V()P*uB(C7wmJ_fwk~5{Z|dl?vJOTliyn%-2k(y}_}7zGv2_{qzEMY? zm31&eS*)bkdxZPkc}TH!8T0;EN1v5-FhW`MQ0zT!#&|V)W@G0e#nxraaZDY3R@T7? zWzj>i_jnEC?z!9BI}a(gE@O_z>gcnw4n`=89*Vt3m_M9{6kC@u=M{DISy=}oltmB4 z-s3#XgSA?Wa2`@@UB;Xr^&LP~*1-s6(L=HKSikj@`1|*AoQD)!moevgeFu=0budC% z^ib?PvY!9y$7p8r(auAPt;?9}6@3Sgbx=ZC^ib?Q?#245)ltk+~7Q<*t(3l4%T-7 zc-!hupyD2pD7J;8o(P?xkD+ojmLj1TBivkpcmiyn%-N9a%J|C4GFem@MMEYjZ>8y8*vF8}>`{{4``EKBe_?SE2zfQKcNMf$&H zW25oconEJ)hZJU6g4f&rC)EddSVCE(|Ha!j5gaS zM!ApmD#Swyvn;{;fB%zedAKuZ31yMysI>n{wFq|xEukzDEys-;GR1|iml6-v&8-<)$(v#WeH`GDAp`Hs(RABg?LD@ zbs2NE+yA8c01rzji$v4XgAU>$#nxraS$qGJ>H|D1p)3;1w)8v5v65ozGUn=I|C8zi zJS?Fs5?mAQe^Px=LRlpCecJmVSBVr`moe9N`=3-F;9&`6k(gYe=RxidDYhnc`Xd!R>tLY*+ZTOL-?KWR8%&!yP9jMcpsJcE`{76~5pUiF_X#6ya$ z%UIoOK@UqPi-he`58QK0D2oKIw+d5=y;RUciml67z38C{Ws%^0bKS3eKWF|mgA`kr zv3k)%6UrjN`~Uc_S{C9V#nxr4UbLWxC6q;i&hpTNvPf|58oJ{4LbghZt;<-Q<$jM`E|G05o5h%OaN!4gqPSHdzh~aHKA;l7mxAqNk=`- zG#^$pF$~1jKY2omC7!^3^#5dR)Zo=6@VEy=#fv*8=UnlP^AKUP=%K48&ipj8Xku~A zj>#PjJt4&sjOSJw8{OCN2#liJK+KwaO|sU`6W#YEmQWTwbQj3y>*<2C6q-E-T(Nv;UIEBoVSc!>H(rncd;>CW!UzS0oNw(#v^RLRs|C zc#OAN;VPL4qF5lL*t(3{+%`N~_QMEx{0@)zr_@e#A3e-@SVCF!(7Y07PJzcF5IqAS z#nxr~A$|iWcMkvF=Qnt81buD8DCc1bWzj=3Y96sR5K?Si#-l3?3Lf*}(P2vM)PS&6 zmQWTwv@-B}z8FOBKuEE58UM4pUpQ9vu;=fzUy<7Tls_MqP!>J3!ih6Yu~pqboD|N7 z6kC^Z&tiSTIo~&;XyW@hMbquWRbmNc(L*aW{F`cc95J_O`l>)kv2_{GZq+kfyV)RG z@2!%4JY4OTP!>J3MvXH~Mi))Y0nt1VQfytu#rAd!_u$4Euk!WXssP* zzQR`B3gXT{NU?PpAAfvTGnQCzxDUklH~Cr363Wtk_-|5IgSZRCzP6s=R#CBa z<;Pjn?|EbF`KX^JC2TF5#m{*R{WqzX!s7-IjRPUY5{!AIOE>)r9!U^i;|%V(;v45- z31!iP*Mk2hbt|r`pFwO6gcMtsF|U|?M^&t|e~wCHj!N2EHjAJ0n)ctMzK*NCiNAIo zA;l7mdF8hq+YlbNgJ_I9YR0{F3J}Vo2k%Y)O)BTX=RjN=2r0HMW8T4^KgoAtbB_8N zW63S=w00hrP!>Hn#`y0=&jhgtM2kR3v2_`9B-&JU3_LiNe1j1VbCmP2gtF+tvC@Ap z`Yz7kl_1InLW-@+m?P?4ttY`_42ZTEse6nb<~%H+EP8Ov_uq?h<}7)4?NqlwNU?Pp zb7r_WYbrcAGGL0zU9(}7^RR@n=)t+mdz|cvED=&{UB;Z@yhlS28!&Tft1O``dT_--37?#07znV(T*IO4g4h zUxPRTYw9s!^s$7p=)twN9}|~g75+Nb)PFSeBb*dlmoeAveoPz)qB>UkSN!b9T}vp7 z9_$(X*#0W6lIuZWj&h@@6kAt)f*;$fmCH%auDK*`)-|hc#d&kR~5wb%@)Lax}_W;B|j1WG$1NlaR&%`)LcF*eyYsL5o<}w zPgJThFsjt?c+3VQT5p&Xr=Muy2r2oAv7-k@ZCYFyh;MK05uY?A*AY_k6aRcWFnaV` z{xUZ`+EltU{$<;AM@Y#}9Cpi~sNZ#M1F^Zy?$r<1c*GG>@)MK48Wf$9=?a3!s`~Pr z)&2G?c7&AtL>2rV+4RnRf(OT|XZ~2?2q^`Kot7y2Vou#&!J|r0@)PSu4>4PH*^RsF zeh~IUN`9i{VcBNSajYU8D=GPjwnMYS(dC{$vSCtm;R1gKH++@t&QyM)YEArVwmat} zcE2zy`n8jh!Tp&2LJpqbFLng4tBGG9u_`~oEA*GE8J9F_d}s172GaBi!l`t`EyPWj-rdo_^U)peuATG-gWf?M(Tat z{FNvrKfw{)U%T|+Y|yl8mb-SP&s#F-z8%}6{wZD6|kwC5wvGU_=hUT|mKqMbTN zEisqybM`xa#+iKNjKoe5Ur+Lc6iYBJf!?6q|N3K0oCD(ChCS0=zrHrxdy~5Z`MdH>)+3LSVCF!P@jNbRt0eth^2v$V(T()T6Iu#_MVCG zXa|Dx`46)OIS)%HiyrC|_7hnmq}aNQi&f7K9*z8dB(xuvP!>JZC&ZajAg+J2M&jkL zA5v^x#tSM9569|D+=H*Ld?wL599>H&iyrC|@Eh3lSoKWel0ZnYbs2a5V`Mnz^Ff@j zHY+(cTqTxJ7CqD_#F?u>WM0ckJ{AZmwl3pMxjEt5MaJ5@PS@ck!fbXe6 zybj{=KuEE5UHo}&xCe)U*ifTE^5bs)K0km6S@ck!5ND1B5ra57+~-nkUB+u)8xzKo zk|1{eeN*zy(tf?W3BtJ;8g8-!JCUB-{0k{{cj2l4W>fyoc2JRjvJl*P~Up2{=_@dk(iQ#~QY z);$Ft|NC#On|=JqupY!0Q=X4)Et@58|IOWUc&r4`d8#M)M2U(e5sxA31ThI`O~F8EkGzs*Oh-NlZCCi z4Cg#9??>Dt@h|^RR@n=)rr_Kb7HKn+EY*Af(v3F2;R6b3p@mGzRfMj3pCx zHFq8d5Fv{m9Ao@bnVBH=VJw*z2r0HMV~#|-{>2xx8SeQz7!zBR@8mozp)7iEtn^Q1 z{=z8Q7Gq-5KuEE58FNJ4-nI`s&WA@gjMOjn@8>)$p)7iE%=b@aE(OsP#L7TOv2_`9 zX4rO7b~sjCh3}X($az>oS@huClmDY+c4&arkq7ABYB6 z$#e}@i6xXp53Y&)waa65E{O4gkYejH=E})myPVZN!HR5dxY{kDEP8Nl=kGxt-EAQ5 z41^S0moZm>{vON$F&Qh%YTf;PZV6@4L(f~{%n=}lfanznDYh4I zOE6~t;m7s|Kx{+D(t6tSv8`pZ__=zYIP(X#Dg|Q2Bu_}O1Y>nb_$|E?zDvx`?wI~_ z*22gtuMNMu?xxjUq88|*MEjZZUJwt1SP}^JOO{}~`@b$xt7|);$9WLM#24G9_mm!< zu(fO!KUcpKXHF>pU7{6;-^+MHiX|A=N_UOo$!pDT%H%b5a_}k9I63U`SURTr; zTh$dr_drOobs7KhPq%2&RY%o{Gd(~|YjR2Y!Y4~4Z7rL{&(-hXecQ)>m)HWLXdt9m zg7MHtdqg#JE`Z0cAYRyBA-(7QbDW1IltmBqJNUZ{Y}GIjr8as(iml7|>LtCRmbDwh z<6022$N!dEbXk4pVF_i?L;ViE%K-5fhy{U=V(T)VTeNTV(%{?RaVd!LB_B_fKBATL zu!OScp?)XMoDE_Ih%$kYV(T(4F|&X4O_u@ir~~5K@BWwSG^M@su!OScp?)XM)Ba_lyoQEZpMGy5m`-vcAo;# zXM2U@zv2F{gtDB+{;uciZr+}FBV%y}tFz#AHS@cl9gEL*$a=MApV=TFwRdXOZ(wx&Y2))fJn?; z7+b~Gm0z2|d+;4HiQBQ~n4?np3GN58___KWyki8!Js{o-gcMtsF^}{ghw*)0qe0Zc z87%q4+s?xh%A!YJSM(c*!$EWjgcMtsF|U|QLl1;oRFkYejH=16qyGmYSJGl)_c6L%cm%6V8q zS@cl96K6gEF%)CscY%;%>oVqunmr$XjG1`^M8c2M&chPQqKEn&zsh(6#Pw6!J3@-B z%a}7m$G8JLI>Uoq(TV*#6(E#F5A{27rc*vbiml6-Go1Gbb5sg*l=HBJvgo0HC(g8j zM+?lgw**3pt;?7*wLez%K#cHf2IpZ3Wzj?ZPMm2CVk(HDNBA=+#nxra6^B3PoX=mv zO6IX}l~_Vq^iaPOXSnBS5OV_|#nxram6N}Axgwi|71^KRYPW>4=%IcGJr0P8AS!P3 zca#)cmoZm>{vP}b>%^zAvaI{0zt1h9EPANl!M7jqcp1cR;XapQ>oVqg*N-LrK_sw3 zJ~oU#mQWTw^1339)R%*}@o_)GNwIYqb4BjQM9$7VvC_wkUD18;7y@Ef z8Ba*Dbs4k&@MHUQ5bRiX&hoQ?C6q;vysn6MExV${fskVBGFFEaXMS#ZVsg`$P0}xa zHY2jintIdgo;j{t)Es@3Xx|k*2cq~_P0}qt@r3#%OE8|ax?A*R|0d{hmV@|UTf_9c z+8q+Mmd)bl>UZMI`yiHsIK7T1q*#LSrY_y1>Lr%J<3kYJE~u3r*kgtBu!OSck=GT~ zz5c}HEg)+5^n?^!mvPOJcnZAuZ+KJ(F}QlA^uXO2=V1wD(L?=CoLP;nI_1Jj=@0+) zgcMts@kM|3jIO!$q&jhCEr|KEk4nFO^odDZ%VzO&^*eE94~V89z6gXAOE4~#?j5D? zsR@riK>RarbE?WORh@?=ltmBqJ8|Yc5Ir8=oT?lMDYh=-iW~Yy#Y*zuRP%f^c;dd) z>z^i_hb5Fn5A{2EG793q$L~w634|0|m+>vf4Tv5s-x?mDgJ^zP`PB1?Yn+EAltmBq zJ8|YC5Mw~R7zio0F5_ZD2Sw$gTZ0GtsEO&O&chPQqKEpOIP)buIIGUZKy45BoM4S|qi>oU$X8WxV#Qy^}9;=aT;;pkdIS@cl9 z6K6Jocn-wVfskVBGOl^|h;Yt%*B4=#keI4Fjj%A$w*ojB70 zLkCrfn^o#n096#F-@^9s;p75K=6`n0N5? z@0<*e6(G*WSkmm5s?Ng_%A$w*9rQRLHef8dE)Y^|UB(=VW;f)&MGkXR0&|q}u!OSc zp?)XMEP}_|7!%hALW-@+m?LVf>k{xtgLnrc_1eTW&chPQqKEn&KdT)FVnraN*t(24 zGc3w(2#-(U!5L>#x~cQ9gtF+NekacGy0U~6TbD6sIPbx+y%lE8)nThFp)7i+-|_R{ z-5{0)LW-@+m@~CMRvZ~F#hT&8aC9x9EPANli8C8P90g)yAf(v3jJe|Q=RC|&Da=vs zSXn|@^iaPOXa0gmGpv1%4TKb1moZmP{@UFI;*Qxzr5A^*-4e>8hx#3Vm2?M@b&S8G zq}aNQxlZ)=;H&U>0c+9E!d+_#Wzj?Z4*s42tJe=f^!V403{q@e#$3tzv4q5YtdJY_ z@Z*&wltmBqJAQUZMI3m`B@u{Urd zoD^G^G5ZfcwzL2E4?WHopZWR263U`SURT6f?K2RUeBucywk~6JNcb+}y;{jOovNoN z#$zL^oO$z%y5BGA5j900CE9mI!$8yrF(>5-^-Gpu{M#ozqNSB;p#K;FqSb9x(%=4c zdBWDRS^Ql64ti%0T|qn;2q~6e+_PKHs9lwr@aP4i^Yl~G$sKc?hb5Fn5A{2EVP^en?Ojhbs5*5)H~{M z!QplA{y`8IKmAK;>kmbewwBG}=jwOjOn(rYL2L+w6iYDv=cvBX6%{JLV!u0Tk!bs5iH+bJZ@8Ivq zK(qpJWgw*3x{S*|IUssy_r>ra@nwx7sXk{^bsm;b7CqGO;GJP0ri17f2r0HM<4+e4 zifaE|CwO4S$$hZ$h0eng%A$w*9lSRj9-N~@NU?Pp4}We*@E8wbc#R^7VPUH*p)7i+ z-@(812Jt?Ku7QwZ>oTtU+|Y2WcwHs`$w{0Uj;UaDZTmqs(Af(v3jN7yv8Lr(a@VM*gUlMPHtKAaHqKEpO zIP)Tibs+i$LW-@+xcrBs!acYG#0R$=lYBbdwU$s8J=E{SnX5o71u=b`9~q?Bx{Nm; zlM}|0D?!{b{nX@bVf3+tvgpC}wg0AQCWx^hcK_%{I4QO+oTrhKR1l+qe0Z@R6V)t6+atTLRs|4>xym$aV?1B zUh{+$TbJ?0H|4t72Y)FGq9usZua1pvEt{qN@G~d(qY8+oVpQvuDf0@MsL;yjzb+)*Abf^RR@n=#keItpd>$*Y4rtJR!x_ zWy~x8@Avpli>E-e!5#J74@J_pmd)bl>UaEoz5~SkKuEC!W8T4^w9I-5Pu$`0E=Go9 z=A7U>ETJrVsNeBp`(i^#v2_`9Bq}ku3_Pa6<4TNh|EpHnd00YO^iaR!=fR8onCJ*8 zwk~6isMRm22oH`|<1kWVjw(PXiyrEC;>-jPZ-5vS2r0HMW6likRp;MX-UWg)&I6S% zbRL#a7CqGO6eOhBx{Nu)c@JK@-(lwL7`Dn1%A$w*oj5ZQ#6%E70wKlLWz3n{A1e~) zV+GJSoDWMViyrEC{LyU+;_^U9v2_`9#o^C6*CkD`_W5SEzpgByEPANli8F&ioCu;+ zxDut;eWHLOizzVr)7_Tg$EPANl@vHEwK&%La6kC@u zSLA+79 z!T8D-@QkHNHawPs_^VS^`r6f4E1Ft1i=V6Ci8D`tII442x@;h%Sb}lq?tP*szgP&5 zg&;cb*p|BPtEZfYC6q;vysqdC5Rd+}EwwBVQfytuKi=Fodg1){;qf|%vh`m{b>91d z^RR@n=%Icm&b$obVi0WtA;s2ZJZWOT=%Si`!XpJ@;^8w=4Zi);d00YO^iaPOXBL1s z>4+JrO9COq)@5AbtNzjQZ;!0w&*0j6%~MCFizaO?o5j!7@8Dl3z#|t#(LhMC1mo{p z4~&`@Ee#J6vp(J!H~#Nv=V1wD(L?{hYnI-jQ$2 z5)I61BUQ39Ej-W^9h214Ao3+m=X=&I^0nzJado?h@jOY8wLCU2E3_qoBd{j=Vc4#I`3k1fIHXdlWr7FYL}Y z40e6+eIXW~J|5_L#P)MdFbW!6@5owBh%bfs*7b9GbY9qJFAjEN$!Z~zJIY^kd~U}p zO)v@?T<^#Zy%3%dvm8N>&I>!oLiy~@jEQ)HUtPR8SjvsNnqU+(xZaU((h*{Z5ThJH zkIoBwPQjsWY+o$IvE8|XT{qa-Kog9DMs!t#{_%qle}*kVkIoDGRPs&rR{#CLM#Wi#Co(z$dLj&r?Z`^PpR zIyr(KO~A&TzOPb&wRU8vAy06`=fjMKCKv?`u6O(hMurMPtab!FIxlRrm`~?*6OFY( zbeER+_>m<>LlcaG2HLc($k5s|ojD$e9JT~KIxlRr{9oq$Pc$%d4v{`Mr%LC?P~ON6@45!bSyX`yl#9VW~w6yI!jaMnQw?9b3I#kXrP# zBk0k2VWX0@V+m%1f2F3bvEGhXnqU+(xZbf z1f!tA^^Ux`EW~snzD}|PJvuLJtUv77eo%;ovSO*R#?A(sU=%c>tD-4FTo>Z>7E92h z^TOr|$&Vyg?Hqir!k*CB{Y5 zdWDAKdx{jG}geny{X*4v!C}>1iMIdSmQP~mn=)ABeC3pAg&#qk6YK*+RKD4u3 zazN+OQ8>=^jvvV@8r`m}4?X;_CFs!v?6+5U_wu*vD;l>6@m=2qp_1K)8VyY_3L0GR z_>sGXcuI&b96^uH3;RTy9^U2EQ$^!WA*xiK6k2n`Y@?wGMnQw?9jj49h%JtwN9Too z=>DGG@xPaf#yz5OuzK6jkU=Yqh9(#Vjp(YVq7a`{Ya5#82zqp0*rN*c@{)^xAsP<} z@#v^Rq2CT{HX52>6g0Tr@gpULNPewQ=yylZqw~W4`PSavhwp56*HQNVS$@e8JB@}W z7zGWkcl^kGqOteBS$;7`(4+IhE?ugRH~0)bgT%eU6xHzd?~H~f7zGWkcl^k`mPjH& zkIoDGzAAm4Mkyh>-8aj7z@3#Q7zGWkcWi5Ka{nwZt0U;qd13deE#Dhu?iHTLsZoUj zTio5%1f!tA^^PC8Lx`_N779H5gMEVZ=)ACZ)#>k^d0`<6)@U18>RO2=7zGWkcjS9H zg?L$r-j1M0=Y`$(kpZsl787DqrAdKmnQZ^i1f!tA^^W{aoDelCPYOKb`UgEaFYGQ! z16>~k(W38yz{{`Ney#~dL4)fZKY}r_oDkDpKc`3Mg&qEApc_j-{Be1GprjjpG{Gon zaJ?gM6$|mF5KGJ2@roXu7xtz*2e~n^j1Wa~$X`?4X2)GkFbW!6@A#3dLNpYj(;b$e zN9Tq8RM$anY|kY`xkvT{E`4Wb15Gdr8qrk|#>5j9_5{jow*);puhAPl$bDBBTFEa$ zto^QtuXE`rUWc7IvkTEe&T8~_OW>^F(FFA>)SDsS6^84WFIO&q?`1SJf$KmNG`QZ8 z_hf_^oBf->Q$;O7kIrlK({9fjju@7Q+ruMmHhwFEty zfQ=UOO5OZx<(t1mW2m&m4_+N=G&I2|XmGtFe`PAfKYbPip6G4~dURgcX!-S8))kHN zLX4C?_|FZqjfN%|1r4ru{K&0BY>_@V#u4=Bys*)OE9~np8byV;O-6=~2CXm}nqU+( zFvi#lxrh+SLVVx|dURgc7>PPhnIsx$SH)f{6!_=BW}~4AMnQw?9ea28$(VT65%lQ1 zurZ?U&pt~uN{dD_8Qbp~vD0X1f>F@mddJQmp9pb}Bk0k2VPj@k(0-0+aK@>3`*%h| z6O4ie*E{h9JvuLJ%y3rYez}fC&;+BP!S#+GDJMj8sTtmP&x0PF7d9#m`^?dHmr3pOtZOBjU=%dC-mw+g z7O8!%ID#IX7d9#<+jh~e9+Qe}oonryU=%dC-m&*8AjC>X(4+IhMg?g5AnL@$QdypG z{ah1_f(F++ex!g9uL+T>hwbO|=)AB|$=b05_3mz|sgujw(MJ=Ef(F++c8)qF#E)*g zqDSY2jk?{AiKz7JN~M2$0XvFnf>F@mddJp2^@Pac2zqp0*jQQEu^qL~Oj)t4*l%Y8 zO)v@?(N$4Uhz>&hzS|P?=)ADGLXuDWR`&}o_#h?JZB-~)d+@!J{8m5Gc2)F-5YvQc z`ne^z64C^0eh;i40nv9v|4^mQ6Fr?vN8xyMRrHn+t%Mlq2zoRDo8KHOpSTOrt4^oT zU5!r|4NWi#8eH%Ak*Pu)s@*Bn>sd?Cqw~V%cg@N>cS39}(;&3_vmybVOGn{2*E{ld zn-IU1Z4f%M!V>go0ye*$*50ev@{|d^em-C{G{GonaJ^&Cs#)$bp;CWZf*ze0HownS z-hhy^3T4g~dSpo}qoD~#L4)fZOH34^h9l_Fd13P#aOJHq(YRsBTL0}c-HnDO7zGWk zckC0Kxp=KV%MtYGys-J5x%R9so$&mmD?^NiCKv?`u6OL+J^!2MXLkfWIxlQ~%dULl zF4uu|)RMBpjfN%|1r4ru?3H7$Z>b~b(Rq!X`i;HlgWFGfURifmX$Thu4X$^rMwL^Z z_xojg<@D&hu=!2C_R1?PTI=2NhkYKJU=%dC-m!Ogh!7Xu^PorPh0X8&wP!UeOSZt; z#kO5(f>F@mdPlxHL9Tp5=4^rgxpqa5&I>#GJ;LLKcpz_?z|*dOXo6AD;Cjbu3>IR~ zIom(z(RpF-a^Fa7i85sy1d6Y;y;c*9f(F++u15*+wd?2f=)AD`9mcj@O|H`^P~DAJ znqU+(xZaUp5|As;B}DV)cD$lT=Y`F0J(hQ)gvd9de_(buJMLgb zyK$EuofkI0C)pAcK1c~{``FF~nqZW##E?Mjx4@0wWh$f-50SP|tqTErUx<5flHqi z3F%xq3dgzLv1e86;Rb;Mj-W>qu+j4EcNL@8R+b)BF@mdMBQsN9Q$m>i2BpUfm&cZ3%Z)X$Thu4X$_Wn0Q)sTqc!vCl&jjDiN&JNE9blbT_zBk0k2Ve=cit;S@jWS(`cL=%jH2G={b z4;~aEyCdk)d13QAzU`RUPinX-u77BPQPALe$JUASrG^{!r|lo~=)AD`t>AX9y(YEj z1=nje!6;~Oy<_X$u~LhEw8D-I^ys{>`90#6s3tXac{g5Zf>F@mddHqsAtA!g+VP4W zofkI0dE7q1SEbU==SERYFbW!6@7T86RfuVhphxG0&F?C=#QU;hX}`+O2AW_LG~!o9 zuY7I^dURgcTp`(SuE+Q3=bMn?#eO?FYX%UkQ7mz<{OZCM`5yXMf+GV6jzoUsrZHJU z-z*svzHfY%z`GNAB>%i&fS0M~2W!fn>XY2;y#e0VPH(Ki@%bkPcrD~TLiyxpyd^G; zwgf$zfQ?Me4UtudMWRti{$J<1qm;0-;Bza=Xr#DNZV!ojh( zhjlI;h2sMk_4m%T?JmR=IjcKgs1v?8#uD^s0(Sp5`+Kc^?kvPkA!O+$mR`U#>ouU=)r|+S=F4UUZfa zt;S{vg@pL;HA~Q=^TNK_m*4+bjL+_d3h|Wa?2xCZbLl7?$M3%sTKPXAa!d3|p-9l9 z3D`Jp&nk})-;K=@_||D?f>F@eGOf2GE=c~>LhN(|JvuM!dUNF0Rm^pSMWaNWje&gb zUTK0+(C9L`mwO)NM59~nje(kuphxG0y<=W4_dM>CXMS+Z?ZGPU32K5-&^WN7r)yW0 zCS(bHHTL#k1J_FE(RpDX`nIQQSG6W)3HcrB1WUWNs|iLyqh7Y2u7Bj0D=#O+?c;3Q zrAOz5eb0S8UH@n*#HrU_4n|y$(gdTR(Q9@O&-U{dU(XWyM~GJ)L66Q0`&zh%>A~{t z_(E)7GAQ_l+%ug^N8$M5em&fHHAo_5gbOYcQhPK38`^ff8Y{%FLd+UtM>yuvQPjp* zDKn1bUM|Gk*DZlu97UOzdKgjtNE;!J%9Wp&wxx6FC>+N<{N$@e@`*_Uxw|r61!cT4 z1U;I7jXOPe+8iND2qEKD@NsD=I+u>ZakPbMb#kutBO9e%l@a2UBk0itY_yp0msy2q zFJt<*(sm2KP{(L!f>F>wo1Xe$Wg$Ar*#4X3?%m1n3!$S0x=P_QcI*(j^ zEWs!oN8cP!vat|v$lXnnK3K~U^ys{>(Ss*Fj?Zy6%f0$Xp0|t)Mne;ff(FKw(a*LP zqLTFUcgAH2$arN4dURgcIBw4>AjClV??3{AQP9BXYKdp%tlpCUwm5SvmQU&$kQ-fBWH$ftKG+NdEkEifN-w zr7Eghakyz2uyGu1?2lE~D$9GwLM+aE#hcZ*fu}vO#3R1E`*FOp5Nn0leDr(ozan!D zK~F3(^Y`A~Cs&G8v_y%bbG>hcf(AXYME|?`cxy_RmGKH1E1ER$?vgVDK~F64*dFgc|Owe4Z6OvBaUOecg57?$$o? zy+5Ity;q<2?rZKzEKy=kUw3!i^YHmR=!qrJ2kjGFEgDbGy)l$0Y@azju>^WEp1-^o z?uh*13F~pM=!qqeX{I+u2weHcd9V2IHE!U8peL5V*={(C-_+xkPbxOo5cI?nxFgMf z+Tw^p61_<77c}UJC2)6_E~U9sP$#bpnOwGKUAz1h2yLr{Kz%A@{xg+fuW9|M-#BodhA?#hiD9!r}u(9MV(7W;W+w@tr=F~->IxF}bi zJGe5`!Cko~7zGX14}PSeXp|A6sUzsod0|f<(a$}PWFgkvmnXc)JwZ({3L2~*Cfm>Z3en+9S0CMr$I+(kF3w@OyE*UA6HajiJ(_@xmfzto z{KonkA$rPHw~?#YxpWkcqi@>Xj|Os9s{$)S&pU!1O~6JEwrAB_i0__^_|4=g>RdVs z$1$$hU7VA0R%7c#{4S25M-#Ad-0pat72?k)BVMlr1f!sV(bZnZU^%NUbs}CpN6@45 z!p693ucMg|#bl&TaaXPhMnMC!ge8JP+$_X6N6@45!p8hzpT`Cv2Fo0E{%8BlHNhxo zV79aEYO2hGFUmMF{fH&#(RpEG9<=T1P9ZS5JyhDZT}?0w8kn_h|9D;Q?j9k&aBY_! zofkIdbK5_j5JK)#@JZLBG{GonM0at9$d$h?gzwp2OOMVA8<}iB$Cy|{W}7~B?Z}`B zMnQu$ksrAtL`5N5xUqyDofkItQS5ki_WJGNRg+&K5n;Hcb}jfN%|1r6A?TKY%M>VXgL48EAx67=Z2u(6Btso9Ns zSBMEYF9$M-j?Sf{a6DQqRTSb}uFHXgA6kMQO~4*euD3g@;zBGwH!HAF?w8J`qi{T0 zEv=O+&;94Dz;;K_qY2ok=f5!fSNnu`^NYRS6?yJDmyW`5j4QTUDk*0*Y5iVrrz7am z1Z*6yZg$m*2+{J3z5dn&1f!tAI>?V4m$UlL-|Js>1U))0Y^=!abzBjm#ravGu)A_i zFbW#1gZ#)TAs!QAmm}!Wd13E)u%CM#-wSax_vH|upe7gv4c0+^32&ec2@ga2t4!G96^sJ zV57y1G@tfe6(TIrn}yQ3bQF$9t0i1Vb!oe+9YK#KV58+{Gd0{9A+DdB6*?$auXE`r z9FJB@*M(Rw#1D?3M-#BogIk)<2hRzSb>m+Dh&*?lOGn{2#uZyF6_(Lwdw8$^r6cIk z1Z*5XYd*ESO^B1yZZ{<$7zGX1L9&jLvpOHz>)jx2OM7%)*cf;1b)eS{ma)A+vb|T# zrK6CEb&wyqU(Twv^w_EHUeTio*qA@;^Qb1o5t*YZN!!x7bQF$bwzKVOp%5cwj+*A4 zIX#+yjd{?vtDl7EC!<@-T(<3Mf>F?59VFkyDp#IEh$i_gL66Q08<}kX*d)YVQUN?E zEk);zNWMo{&gy0%+MTupJ(_@xy2SSLZ9+UQv(0NVPw8Ab3df_>5}x^HA@2Rw z67*;SHa-!u<5jg@b;CQG6$uXc=tEC?vKL4(cNDu^r0ppBRL{EMwk?VTpPy_A)=`>( zjmmkh*$W>hMCVO;f~EJo8qm3P6prKW+Sy>0oYnR(@&qRwv;;kxfQ|O{wb|JoAjI1< zF9%L<2^kGdFbW#cYN>z_Yi3^#e81Te^ys{>(RPcP8t%GWM-_?g_U>h)p$SGoBU&w8 z5#l)^Iyr(KofkG%GIk&Cst^r+cr}o7W1gVSrK4~>S}o<3>!@?!)j)Pf(4z_1*kQGw zv0M|4YBN9d+BPp@G&I2|Xkc8i)zWbxo}c}p*V_^F=)ACT+|~>ygeW`fL%&Y~f>F?5 z9VFjREkwQ9ANo8idURgc*x9z%abAd82VM0JmDJBwsbBXh2zm`DVrk-JAxifz{Vbj z?dP|kB{nY-?ksIf=h9I)&N@hbi%y6}LR6S#33@aE8+XL+x8EqlGeY#9`Ju0K=_qR3 zQ50uYLx_UX8ns6g)Z1u2mB}i^r44z)umA9BD3-w0A_~WG*X?ZZk37LVa(5qc1U))0 zY&;LUs}>QWzC6K+(o%FT9fjk#ayzT-7NYQbmqR5TL60V2qs7>>La)7B+HQW)(YbUK zj)SnX^UZSQ^(Ft~j-W>qu+j1dnSD6itIqph4V9Iv*ST~Qjz_Dd@8zr($$gSuYY2KY z0UJGdh2hrGY{$ z7GkJtyY%S1urX8H{xL|1$7Pg$XP51RnqU+(qSaCpxx3wkc;aVE(4+IhMqOh2c|Rfc z$o%x?d^<8|f>F?jR!gtQS)G$=>F8UQphxG0jXPq;s|Am~65jX8roif38hF}+Pf7VH zB56B{UJ&Bk(oKPPOIw0md0~Cn5f6u__SB zWeIvT0UIj-`#!@$A?i(?9jM;0yV1}Dqo9H3Z)dehLcB9=cHph2EJ2UX3md(_z5y{) zh}{jk2X>2&&ZVPpJX$R+7UIA9-2-=sj`nB*HhPzRXW}CvmgFfISRhv)OE3z@VcRML zPjIXd3mrj^&I_CGQpmR<3URSe18=fCcb!W|;W)+>TP=b3U5F`;phpw1aooO_;WT_| zXo6ADU>#)7Ds$lmsRTVbFKoWAA>V8yXSFC#!O%zU$~D0#Xs{0QBlCoqD8#3ZphxG0 zjZD$!F`+^C&^-6dHNhxounv;%XcUbu4ZDY~zGI&`JvuLJ%qzBCEfwO=sk1|UTuamh zqo5J3mc|LuLWn$#Y}=(r=Y@^E0oy;`5aMc!RiVLoZI99fqo5J3mgWkPqvfj5zpf9` zqw~VX9kKoVJt6jfvMF?`pdA@B!6;}%tEKma*eS$sw^@Q7ofkIWZSf=j6Jm!DzZ7iX z>s&gD*I`G|cZE18M1DuGLed2F>{}KfR?5Aqp0{8~=h9I)9<7$f2r*IaRj1sRphpw1 zai{IOACrZ+f7Ceo|8!|7 zI+u>ZaXcG4JI@m0squ+j4Edop-}E#*FaAook>(or}L!d4mRwdjK~!Wn`d zO~B@RKz3xfP^f|bmOMqBOGn{2#uZyFVf6V;2pN40L60V2O)X)T@pn=iV zUWe20-gX-F=)AD`u919yqG;5X{x{KGxh5C|jcB!m9<@xy_U{})kIoAlnWE2Qz06VT z-4oOVqoBb$NdD$WG?vK7^MCpsi%+$7jtP?S|jDiO1AV2b}5NCzBEte(e(RpFxj@W*FP>5q6 zZwjo+VaF0pFbW#cYUxcOHpm<};XzB#qw~UMeFoe|6(WVhMU;iH3KM zac{(DL@e(r2R2@U7#cyfU-9CD5P;c{PE(0s9v`?yOQZG{Gp)rIwHME{>iQ)kr1i zL6jzD)*az(nD{}Iz-5?cPERa>8P)FT$DWlY7zMi3suZuuspV0P)U%=oQJUEH?+|Zj z)o_$ZJu7-*3C!pAO^(>J(gdSGUvE9oYw^go1RC@pN)s5h?OzGSYG{H{pznUaua|ql z*9kP}L6jyw{HdpRCgrOH1U<0?Mus$3j(e4YC`~X5$8$XRs<&s$rnDM{pa)T!7(J(p zS9-?hX$jLx=!qpTj-_cOsb{4LMuBe9_$BXprNwDA%vsTcC{5ID($SlFd3svHoE1H> z1XizUdQ|FJX@XIppZolIZ&2-#2{hAg#Ft-*Gf_~G{Gp)mEUORjc(i|t%kXB zdJv_FJ;&O5&$TO_mM~XNPb`7=PU6NyqoD~#fiAzHjo0qdH_;I;^*ZQ5lqO1-Y2*Fx z&->F7=B((6CHOZ@siP<~;s{28MyBJdC(8RJ>h7iz@E}SPI79pQQ5g}80*&h%-(_Zk z8HXNT2lB$kJ-j$G>$>=}q9>NX)3kpv5_?vfU=(Pyh2oQv6KK$bC{3WfP5P#50)n1c z0`1uTtxW7$X@XIp(Qi(?Ga-QnJ&4i-da!+mC00Wdi~^1RU-$d>5@^taC{19D>GkN+ z1Oz>?1V%gi=5p*=X@XIpF;*7ousVSTJ&4i-#^Xne`3VSmVhPL)_V2u6&q@=F0*!g4 zP0dgO4SEoz3Cvw{cdt!A&=X5wPE35SG{Gp)n1i?NU!FjN9z&5vQ{w)i2}XfN-4(cdL;?+Z5Tyy!cANG!NkGsOOQ2>< zJeFvJQJ_(yPA^z8fd)N@(gZ5!#G|Mt7zG;Da%9Bb1fwWDh|&aB7Ktl;dSVH#GBEBY zrTD>N!P*;Vdx+vHW#D(cQ*9jQ%EFIiZWS)433@aE8^`TmK_30ExC;EE+FiG=pV5~Phr z(y;DbgFRmi2SCuH3D{iwWK1v$nP^)gbI#VmWxF#4wMXZL&9zU)1f!6Nwk7hX)C}f7 zTGD9Hqw~V%YAIuaQP7}miQk?n9n4fLXf){2d0}(4lrh05XwbGqse5w-R~>0;H0aTJ zVRLxxUJnU=%cHTcX$EF@Y98^femv=)ADGzRH+j z6f|gC;?&Qlyjcy08V!1MUf5ibWlS&%8ni9($xzV6U=Y@^_&y{deQhEfVph4Sew3hK|$n<(fgC3n1HpVfo zgp-oeBNzn@+E!z;jJx-pXlFF&(RpEGJm%UtDJeaIQP7}mHQfARH0aTJVPjt5+BqpH zJ%UltplvlCka@65qd`W49-S99=0~o!lakUS7zGX5R%6GpqW*6?hZzlebY9q)=egQW zN=lDl6f|fXjik>GpYoa> zK2J(Yk6;uuXj_f{Nu9WTMrWfzkIoAl^&{8kNlEDujDiMjtC3&o-Hk_@8V!1MUf8IE zxuQ=>N{?U^G-z85SGOAtdURgcsOPz&PfALUU=%cHTaCQ3{v@L=4VDA^hW^AWN=Y^eSm1-fww4PmZaYcLALmVX9#hsHmh;H>D;d10gVq<(72 z>o6LcU=)Zs6_xs_Wf~1lFbV`}yVOrD z(4VRdT^mo(qw~V%`YL0BQ6SKp`y6VYK!YBg7dF>d z@fw<76bP=CGA0-W0wdqTT`399iXNR8HdjmW8k%4f2(EoHCKv?*WB%gDMkhEcdURgc zT>HdpXo687xROaFWM3vJb;g0Zj)IN(G4Z{kCzjw!CSF4mi~@~$K5;9d2T_{fN+wkU zmtn3$6O01QbxA6bl89gw2-HN0`v*NbFKn($QZ*o9&Po%E0>M>ADv^?iU=#?{kBP?; zdURgcTxG;-Xo687PzNU-Md{IbVe9or+VM&gi~@nGpP$_sBK68O!6*=1GsrKQeKsL5 z@8;gYVIz|Q@PNR4SE!Zzef|Rjy}d{OsgjoXWaWgw7$MrGSc17U0sFni1Kr;{&k>@# z5YLZHvU`1Q^`oPxZSU2Ma#ov#cp=3SybeuJuiuY9%J*{qwPHfxr%oM$qdKlP8k)d$ zAPO3MPV#+ZLL|M^Ay~!{^ys{>JAO968+p@KnQ?v=;)&YTgCFd>)vRkZ!6<03h5C_2 zt0x3LeX@FR>wZhnqw~TZ9qI2)olru?tK34=+<$BE;ab&=h9(#V4fahx(qz?yz`H`^ zbOb#*FYFFC^!M)C+)y;m$n(hGVSOOaOC5}cCKv?`jxm0ur4Szpko&fP0| zbY9qJTlaF$W2m&Nccg_(cTZ3gjDiMh2|x0PoK>L%w}yXt(zX(MbY9roU+n4HRcRq^ zlKiJ!OVk9Tpuq}9-fx$)+9>(o6CLf*d10Sf*u(XYAwoPXSKUDFS1iFO9A`BsZ$LrwqVl}UjJ6|#CKv?`R<-h*_iH8u{>#=oe7_rg=+Sv$ zugu=VjaSQrI4MN_(RPH>1fzHzc5M)pvwB3%s+}7}aaJ5fNl*_XsvpTN#4>49*%#y zu=^#+(n_YtGaqum67*;SHd;)H-NQu#y|$6GsiC#18x2h`3L0qBcK4&WoKzGeqNiAs&@c`Y##ZbuJx+FqXamDUs%o5`4@TAaSnWwZz=YZ-Wu(sQu3Qt0f(FKX`#jzeV!jZV3$;h*g^iiPK9Afo<9O1h zI=CmuTsjJwFn8H@b-T3oi9%eGxlnsF0UI-%ZCB%kI3f8T+hNu zKkkSvmn-`l>Krx%J+Z{>_I%r>_zjw|M^u!Wq`F&1acEn@t1}ER{ z2zp|P)&FRXhSw^4>)lz=6H8pH(bru^E;*~4w+QjYyXU-GQ|&XSCzjapV_(x2{0N8*Le!W#cs-suJ+TBb zWl#Q7zU!W6Rq9-29|S$I1kU!Oj!!sZpj`Q^yC)lho>&5R*+S(&c!T2k0xO6*x19HH}`kxgI@_TEV)7W_kD{3I+u>Z@o1HC)9Ov}!q(L?UlvEw&|4;LzDG&I2|Xhf@w;c`}Q9=j=gUtvqoqw~V9F~660 z_*s0q)KG|SKP-}Oj%;8wG{Gonuzv6(9ffEt#7;-hqw~Vf@j-9z@YDGHs0W1j@7CXa zd5T6u6O4ie`i-qJj>{8#zToeErbd>aN9To&<8wkYMdOGNPZ#>#yY4hJ!6<03e()nz z<*W`!D?H)|dURgcPpgO)v@?(JG_95Y2?x>j-*uUf4NT_jAwV zWg#j^3t8cwxh5C|jcAooP_BH(v73Se+%u;~=Y`#AQh(R3atN_Y@_*`Dq9zyx4b~5S z95QJ2_$zEFr;r1sff$c_w}U=%c16Zw&Ta^SvmQUUlu$RY%-{7xJuW}p7vDjos#_TD?QD(OOdwKQc#GTgR6$SbhHHP zC{4ibJg%qrQ?{K_Uo{tE>(+M4;c+j zFbW#1gZ#)+A+8Eh$`SPFys)e7@9lkdb)aZ$5@KMd&HlJSRgH!w7zGWCE4ErXEktt1 z&3-pW(4+Ih#_?m>u}5}Ph>@K)dmR!GjDiO1Ao+HDA@aVo*&FQ$dURgcxyST%*Rf29 z{(rv_NOo7Q2}VJKb&wx9D8!FKT;F2vEGymJqwW~EkJSfqlT}#viqoBb$$d5cHEwQx_w_HDNT01>DFYLF6 z_jmoHxe#aMs*hf;==)AC-{3+KVyGDMbzYyOD@xV))eVt23@jC1%x6J-xzix4l%mDj%bMo8z-+f(lDZs~{GqY2o!(;uH|DfLx- zAwn`L98UT*j$jlrMXRNGa#qLXnIDl6NPBc%*l00F_H7ak^r-J_+YQGOjKXo&L4M>0 zIjhssruxW8sy#X{Y_$A^xihczBj-gUNqSU48Q)_GM&Wq0TKYtY%R=1k2zqp0*yzDG z|5{Kqz7k@Yj3onQp3=E=6pmwDvDMO6Ar1>M*b($-0yd66m{eRez7}GvjETb&5R8Hb z>mYk~E7(!goE1GfFKmpc_BvLH#{JSlYP&1f1f!tAI>?V)7GkRqCuJ_w9-S99W(NB_ zFb}>XZK|q!g3P6(kcoAWANg3G`NA814JB2!&zv4jz{Wgi+toH9)=G3=*Ag|sC}>2h zr6Y1y1BCcR=0fe!d0}Iww*BLj5V%hdx<1HUItrOs2lqB+_Q0b za=n*&nY~cbwpuzP#J6>FhW+m>!8%G4u2)) zL4KsR5bYOb4!>2x67*;ScKcPmyvpBR6piLWtSRoqoBb$NPbCKh=>qxID#IX7xw4l<#U7e zWkqA45Iah>42@ivIjD2#C>&=UB>Qke3@Onv^pYd!(FE*IFn&kr20kJH@YG zH>c6i1f!sVam7|kV}i??SZN zHN|V~2zqp0*f&1e*Ima5A=XQq8sM&66O4ie>mWZeK!`3vyzK~jbY9rg_sAy==6T?m zj~u=UX=6Zg#N(RpDvtk=)AtMx*BTKt#5{1n>~HNhxo zunv-6wicqM5SK(pdvspd=jZox{o{Edrpr}NzQ^_`=F(Bf#5%~flGlXzsiY<7(FE+$ zzw~qce4c0o<>{T-ZAS)8FbW#cYH6YnErqDH*An#Tys&S&)X#k?^Og{8gvh>Uim!9& zC|-vhMHdS3un?v8S^{SUk0z+MCFoU_-_8`Grd)aUds>EcE**vAtb_c>vqD@E;-kAP zL60V2<9WP3X07az^$?oqoBb$$d9}v#6oFP?Hoam&I=nY|I|}6M8l0&0U58tI+u>Z@o2R) zN;KL@9~|xodNctWJ^1!6t3=}+A?DbT!DwiLQP9A+VymU;LOd=+bD7z+N9TnNqG5|K zL}RuPlXgt;TO}YEg-oo2{KzLl6q7NrvLooxd0}Hjwb!vrh@3LE=XO`F2}VI9S}oyu z%(+ttnG3Z?=Y@^=!#)p;4C7_S*&{PsEWs!oXC356UJzoc5NjPlkIoAlGn{Q#ZG~7Q z(ccQCbLl7?k5)@fh4?@S%!S&c3D}saZU1oNRanNWID%2g#5%~2ToR2Qa%Yo@TY?^) z7dGk=+s}U%;s$wouWYv?gC-aSjcBzrK?oVI!ZqDkLXXZ1o0XI7;^gfftWxvSP-ICJ zPkZqBFh4~kZL6hKLUela)6jb}Ex|fU6R@LiCwwDBnd);xGxB5$=v+Dq$5{vYksI@M z5BiVK4gGbiCFs!vY`(Q1>nPEvam&Qe!v4*Th9(#V4c0-j&mhFbToXf&^sxjzIxlR# z=io)zU9Qoa@^>R4u0^=+Sv$^UVm^zY?Ny-fW>w)#e%v zO)v@?tb=6bCB*K0*+T0aL66Q0o9|NiksU$|k)Ji6U-MI=p$SGogLRM}*(=29c~$&( z96^uH3meDndl}ye(Ro1?Z%P7!QP5x={lVWLr~K(@dGj-W^9g&lqO1J68{w5f*f32K5-=6R%6OXyJ#4rm_e=~@XZB%K#F z-y)IqhY+PC|B`;TC2E3E(1=z`9}1CQ@^>3*33_y1*nICqW&Detok0xOAO%-|9S%~WLyfZJgBZDRw1&wI6G+Btilb;5%?6w3wIxlR# z+u}zS3(-i3J7nRebLl8vhaE*P2~nZOr-6ezEP=CvM-$YuZ&~~##7ojbo|0arbLl7? zXC356W(e^_&AEYFZm|SCnt;vsXZ*-&A%2x--lBhVqoD~#L4$RWANfOwJUJ!?t_`vT zJvuLJzHuYFwL-ioZK__*iAF;cjDkkATKZOqzofP2bOb#*FYM^sJLu=zq)k0lZLZPK z1f!tAI>?Xg6r!H=!FrCMN9Tpj_kiR#4uxoLM+T#z2}VH!F?59b}*2 zSZRfw9YK%I3!85z`H{nNRz0On?UcDt=h9I)j=9TLOWTAvEpt>RnG3Z?6R`O{ll*d) z5LYFCEtv~r2}a>K>mb<|mH9kL^8eJ+67=Z2u=xg*9er@sk=(XNX@XJEU>zjiFfL~m z5TaZ`OVFe9!sa_u@;QzWJLKt2kryI$E**vA(P{~Ou$mAr&9DSLnt;v9$-dq8q`Yky za&MloPsF~l#dd7ppj#qu&^31NS#p+%B{(z4&fTTv!P7Ht3XiRq!~6T=4awbCPw=w$ z?z0wlqn{>t0onO~eyX2*tjc&VD0a3vPdZ{;y&S&wXo8vc)>u+C+nm{DTLjBgz9~Fr zU!FJudWb@%z6Hj6Eym6jV*ZBa!K%w|43~Opmm%oUd11fu!f>F?Ddw#5U_gCYE$Q)=MJh9~0(48so7=j+17j~gKD%K7MnU6^H%58OlYbCm&9P>|Z@au3y13-1A?VS0VK*#0%GvF1JN{D^qoD~#LF4#i-My8~vIsH%Q)#=oKlO$b&t?dEbY9qp7IyVYEzB)M(+(|z zh1N#Ao6g%Os0l_vW6zZ?-t^i@LR9#?Suod;qJffk+q0ra=Y`!UxwF@`Q+6R*H*Xp2 zT=Mb2)^1nKGuH&8pi$&_C$HSK3(~G;ZfzEvp5wW|r)RGjf*ze0cHzt&z4K?#O8VE9 zwmUH;@X3@@aRj5F(J|)>-pPB8iN=Nl&4RsGP6|xzeA*E7=)AD6eGZ;$7TnZ#aiDu9OVFe9!XDkLjW=Y$K_T`OZW)~S z+}gnVE59%rnqU+(*0pcrUC6ddh}k)s2Rk>|68N}4*bwySys*zFxAb1TV}lSEuC@p^ z{A^F)u@^pyBN%0_V{uFG<%u(e$oydQV2^JP1{P+YYtPDgbY9r&1~l_#qE($k}GJA?VS0VW(7V;*D=GQiv@}TLj<8 ze>w0$hiBplMnPl6+NNIbHkF0&2R085f9Bsn`(jTSf*ze0_O=0yy<&G%6XL01ErP?U zWD1u3G*=wKC}_-kyNP#g&hJt)oLs&et$7}!ltheV~Pl!ebn+N-!%^J-8(0D`8qY2paS2gnfYgSW;68WSq z`Rpe1eHJ>Gj-nkNwfXb^n?MvZy_RRC5bCTv?a_H*Ayi9@BNzn@wCNx6wG%?Mc0FtD(vyThuN@-2c2@SehM-61g^fPHYVBu2T$Yi+lab+(7e0z3 z7zGWCF~N@(3Nc4UpZYTTtS%5X1U))0Y>ZuB*4!z?E*arIkrD2Lm0!dWj560Dqv*i@ zHVAP*M$w%ziuTF0-=3B8=)ABo2EWnpgb-6@r2bh(>RfNd6O4ie#{6=d_X+W(%myQ6 zHt62@w9%kP=Y@^Aq+Gu%LX4CdXQ<3L$A;iDKqClnK{4gb|sEr6f`iS4k>d{G|I~CeC2S_KwY(j#jWYM;TSTLm zR5Fc!YT}*v$)1%a7zGVfIN5hx7mXXFT6#sQrAhm5HX8Kkys%MgC1=Vh8uO$g%k<@> z^<#S97)LM)8mR3Ceej=XRFbN1yi|RQYFG_=bY9q~84D!m5RL0EwhUgA%ChOQ%tk{K zjDiMg)OI^Bi^kJZy`Jj&x8JgJRzuLE^TI}j{6X=XM5B*X$Tu}B7Ft*7x_RcBU=%b^ zIX8XvtZ3AiDm+iyK=UF%=Y@?*|DjNlXk?N~f4Wrqt;hdmG&I2|Xkaz5dD&sn zC@rgr8|-Sr5cKH0Tv-(8cwUIjvSPVKRxE+ZN7E2GimRe)KWrBwB&(tWvMSoSh57#-CgbTjUlkQgGc9ujkV#% z&KreDk`?4%vV#2g;)*zeQP99zv~|tdLezAt)WT<$8G;_27dF>d@5?^ys{>vF9-E=K4Yuayt_%FFhPbFbW#j{kXB(KQf+OLP5x^gMPq^N;#84coO_|Z_j+4 z*PsVcnmD_9ly|k<^#mH4U=(QXWm%0!2R~5ptEmN7X%B1?n0*#}I>HN&%9d=2k}35n zF25^R8bU{LzOp-Bx<_eGEb-a66z|6-*e_8;QY^tJ9FOjJDI!ICU~6L3w87rC^VnNb zgwbFw5R8J&y{t5Z(V!=m`22c5?@YDZ5@=|GQJ}e(6|X@LqBJqRZLf4RG{Gp)+{=pB zpa)T!*tNA=I`>Kwi~`NQtauH25T%LV>b{)LGuH&8KyxoEUV|P)Y2t=|JF2#e?`^|B zoeMUjU~?}kUW1-kqW{_U3HqSU1)EW@xtA5MK~F5Pv~1f1V~NfMn^Ca2mldx;Pb?AM z*)qYHsB^()6m0Hg#cR+LOEfyvEWy~WbHQd5Z0=>nYtR!*)NS7+!5pP?!DbX}?q$Vm z&=X56d#Q1PxmM?b%_!L1%SxkRS|UBMM4zjT64VSj7i>nsj_!EHj&StE5*>>+im!dl zS!se%prbopu^RLsN)y+1KW(bB_$${0qo`r`$P#J5gD6cv57l5|4HAq3&E2nff*xK6 z^1{ZQu2?_ox{L`%f#B{}JVB4n3mYxQ3!&1Fy>d-33Iun*;t6_mUf5{)dyitp604yJ zMuFh&S3E(F&I=no_`*H&6KH6HQ6RYc6|X^$&I=nOQI$_OCD70Wqd;)?D_(;hofkGn z)ZW>DN}!<$MuFh&SG)#2IxlR@3^jNDl|Vxii~_;kuXqi5bY9q);W{Any-L;41fxK3_bXn5 z9-S99>gzPmJXJ#zi~_;kuXqi5bY9q4e?;4Ex#9IHw!bUaqdJ$4;!4Kuk)f45IOu_j zg{BpJRC{6xtasA%L316N;Hn5wu(|t{_BymDmcY6#%~+DE!CW911)IBH@f!5R5?Ftx z852`AG{Gp)-2IBzpa)T!z&bk3*q*AP2}XhD?pM49J&4i-_5;$)QK=f5U=(QXe#L9h zgD6d4Un9+2o2sD+MuCp*k?GM#dthq<`!i{3hExsa0>LQQ-2IBb4tinFK>U9LGM~Q|7I(@jDwX12Js-SYLZI z!QDaoj@Nhfr!SjhHR1@q??vra=B=>OIm<2I`SZZ=JD2K(v`6QK9eu|uN&bPDo`7H! zGSSApN=iEQeUI?Vf6X@<^ys{>`6gG!1f!rq+Y%QCy%2u&^a-OukIoC5Z*pZ!FbW#9 zEio@wKV}eo8plyj6;hV$1Zfj;V=+Sxksf>Nc3#}v(p`#uVjp$opAO?-v zA9|?LOJVKN1Z=*SWzQ--f>Fps+Y)n^O$)s>ytmPyN9TpjH@Pw<7zGX5mIyvmI#l@8 z!A64~ofkIWlOQph4Rb3;*5j{jzGP(V$1? zh0XV}GA0-W4ceA?>-nyMWmyLp4SIB5*nBT5V}eo8plylzS=I#39`9x}=+Sv$^G&Xd z2}VJKwk5jdx)eAutew%IN9TpjH@Pw<7zGX5miYe1g26L|8W;_FbY9qeFDqk$QP7}m ziM*G~1y5wDY&7W6d13QSu8avrL4&p>-u6^)3hx6#Xu=ysJJ*)HxMnQwNyN*w~1m8TmEu=j*#DZh!n*%`W?Ay;A;EtKXEPe~=)AD` zCYQVkSijKn^aw^l1NQgktuQ=6R({%}^U}6&Z2eyZqL^u@dB7_KhuQq(?9c8nmrO zmh0QSpH~ev8uaMAuu-qrH@2XW9>FMR&^8)Ljea)umC>L_=Y@^B%f7J%jr0gcL4&r{ zXf1VO!B+2{w^ys`?i`X}|a2+2eAaoShI8ovc zSw}4zT0N*ent+Yv@PM*=K<}}d0}JSZr|9#b)-iy3YlnIjnlHv@WNm7 zjRrkBFKp~T*f+M0)t_D-#PbOVMnMC1X7i4h?3a|^W%o;r20c12Y`&M3ZBFsL-;{GZ zEO7**paC2ELD}ZaC|*9@ogxy@Nc_f@&IOzMNwD$z#n@Y#H-)to?y~m8641j=Q5qs8 zmS7Z)<2TAS?S3;YVKnGLlqPTw@%MxA8k%4fX#8^8uURvvC5#3=h|&bw0{(t5UPBX% z0*zmQ%Ot;xoQ5zO^dL$T+$l;!n0uuOMuEmJ**!G7TLKMw5Tyz9P5k{}yoM$i1scDv zcVYCTw1l}1dJv@v?gv?o(q;Eod~D|Yl{yz}Mq%8=?*x7^@dKxEPjbnM-tk~X?a>5m z?gv?oyFR(8V&B&q#Sx4`Cj5Hgv!|9jjsLaXUh!n*jw`iC=f!c1`Pf@ZbFY##p`&19 zCc<9>rXf;d2}a>KeuZ&J)o=oW9-S99W>ow&U>Xf`R+?ZG2>eQA^GCKN(4a@>g^kJp ze+?L~p$SHTz^`KFTJUuO4SIB5*r;&u*MRXFnqU+N{JQ7ql&=zK(4+IhMx}<&3`ujMvZvqd?#n zUH_}JH~~SA&I=nW0Q@yzyoM$i1%i7u854{Gf#0&7ad~1U<0? zcX8r1G{Gp)u(2oee-nt}S)E!vF&zRPotI~bJ(}kIoC5`zY}mnqU+NR3B|?h7xGdqw~V%K1!-a3RXormyUvsDr(;DwFxxni6yv? z60e~NMuA2(_~riP2{hvpK?DCl7|amxc!9Z^hH6Hgrv8iF28z~-)q)o?2ob5@#Q z6bP(+f_IOIxpM8%d0}%`B<BP(F_r_oKO2b1RiAU#iA{u-~Y&*;~>CY(#;;UH{uW z!AU0yE(Z~6)Hy_T?AE~MP6#z{7v@c=y1IcOL?fg|OoW>4n%QXefJ-o0zM*Nb68 z(4z_1Xm56hrEuA0=@N{BHtZ~BPiF5O$ChWx**d&zcc!rR=)ADGD>83Nhv1pznbRQ{ zg-o!^nmw5g9dfS7mr^r)>(P=%gC3n1Huq8HO=*5COS%N3paJ`$*^?Q1qST7to+%y9 zR4iyT=+SvOCfXgA!CguwBy9BFDa=+Ol1=nhMk3&C^= zMnMC16SF5%e_P!ZBYNx(eK(`C(V$1?h0Q&Vc~fevZjdg)C}_YQYIazjt<`izuf=0R zEq>^0H0aTJVRMgT-juS1(=^XkBdfn1f!q<`;6IPsdZn=71=LU z3sh}1$Y{``^TOsH$Gj=`%e_jEU=%c9?=d?pFW=p0#eh6Z18<$}ZZzo8d0}&pW8ReF z^2|X@PCzgU8n8>79hPxbs;@XQ?P8$hiFQVV9-S99_c-QFX)A5_=X3~0K?C-6v%}JN z-NP%|mn#w+IK7_HphxG0%^iz*Q*J3+E?t6A(11PD?3eT@bo+|xy(A2JA6r zzhp_pEz5Is=@=Yd_Lv#l>Ct&%bH`%dl=7>0q)RXg8nD}${gRZzqnEGD+dDXI&`hI2 zkIoC5I~FoWl`Xe?l@RYFAQ%M=*w35&lI~-^S$6sQ;9$>&MU4hMIxlSQSV-Fqo-stP z1O%g?0sBL@UxIZ+ius-*?a_H@b1x%f0#VF#$n1VJ@AXHytM)nq+N1Nr#`W2~jHYFm zrAsghnP87IyC2>?b;`TDYc%N5d12!@*}aS_fmh1o2~Hg}Gmc;sG+!bZQbdl}L{Dx^m+ z3YlO}G`k`FyB`DAJzSxcj8{XZ z*E1UQ=)ABoj@iA8TxH9pOE3x=uV-cWwYUU_t0*r-?R&IB};r$aCbnPAT~dl?ODHLdWr)K|~`(AQ|t zqw~T>-DP(s9w?kX!6<0J-e&ePp4?Wq!h2FDZlBTFXwakc!bbgQcP8pz2&PLg3L3Cy zo4t&WMpvznU+UeBN17T9dURgc+#8rTC6{OvNQYn)G+^g2I}?LXl&Wyj*6l`v9-Wsp zyxnIQ+NER#SLvrAbQCnO0tlF$iS`|GR>&{wkNihV8iF28z{a}7?lUx%@v1;N1f!q< zd!yNzkabjfw~jIz^ys{>xi?_eQRUq_Dvn?jG+<9NI}@@VEbrEXW^AWN=Y@@RqTOc* z&YxU?Jc!q|ls-T(htX@V=V_$$}a0?< zx{iX)3MXEJo>&5NmtA=&4fD)3!6?v}QSCY^mY@ewnA@=u`u{&GO`!U)Yn)gOeO8Qu z25KU^Vu>Z_K@{#Z)*S!;XBA7>)r8V8&qJRTY(znWRc-w9poeFo3A>tz)zAcNM1jWI zz}D@GFi(&ko{1*7ny@Pg8GOuju(AZfQ5!Z}XzF#OBpRGs z4vZ%lMeUU4v+TFlnX~$;bo8v?(RrztmdNwapyfK3j>7SC)5m&a=Hz?~Z?UF?pF1{X zMb?SK%WID&VE1~kr6Sr2F`~rJMuWL@6f*sFbd2}J-*ugabn%q(Q(jxCJ+Z{AGh3x4 z%*db#*o=aG{lsYROob6j1Bmv-5>4K0la@%;U@j1hf?a;;Yu@=6XD860Czd#yxm{W! zRYMbu0)4LPD6jN*_sw`=2~Po{J+L*==eOt65&+@|MuGOHjqtAByDhI1|H0VK;CT`f;IW3W@p$SHTzIBtl(OB!31RC@pN)ySmx~3&k zH8jB}&>J=l@XkKwzTq#;^PmS&ns{?&kF-Rph9(#V8ow(v`>oUNIxzYyx_{7e_B)I| zh|(hw`rspLez)I+WP0u9NyE#t7r~BoF z(RpE`pa0(kqd=f{z4Aeq1RC_{ys*(X|8Ig(AkbHmv%Hx=gC3n1Hu~oOqwGAuq$rv< z4w4oDL2?F30usDHy4$@KB#9)EsG=YU7!VadGI%B=CqeO$u%Z$Kk-J0X?snHeP%sNf zFklX-AczPm{Oj$W+I@F+LH|6D-&aq+^{d)RJ=HxuH~&tk7YJM{V;ehv3khe5`Ns;! zg^la{-wE{sforAX_Phk!p$f-^jcfDY3H1VjYi0cXyAtG3h2z4;wfXOadV#>Ta{V`Z z6XZ~ZiAW%`m=be?xR|2vwMXjrQKA$`E2RatIORSnQ>_(PMZ58-E*Y zh^rk!`wu2GUvEQH>DMqjz0o9pp$&sV9E-isuDygO@Yo5jA92O2>zn=brXl{bx12ON zm{2d|&|bn5a1*L4s&HJ`Xusd|R}H^Q%K7^CYW}&$x|S@dm)GR5}tsYP=yKDXrEyE zs}iky=lu9{cE*WAV@wVv)C)PZmyo}K?IKj+xUkXQ*YsDr8Vt($VEv$sZjC0H989Pe za%eB%3AhPWI4*3ow=@0K(KUl}{;9UeC)c{k!GwAtN3{2G6RL1r*l7RW^j9DjR$CO* z989Pea%f-a3AhPWI4*3o+wE|+Jf!=I3H3q_?eio1Dnh8jabcs~9*;AvdgrA7k{$Zm zIp$2L7jo#m%M$?MB|;UB3mfhBylN`vZnGgLL-U<;mkISk4!sATx? z^dl+j2E`HTg-AN*;0gTRr=cN|9YPh33mfhBe!k-O!J7+9KayH=QIKP?7ut2sLB2jz zqQ4mSHVD_6g z8T+c$xC*;j^8g$dYbpKfM9mRwgO`Exmf1AobmBh*XJ1v{4k z;tL@L3Bd}-g^ly-Dl_}>%DKMDE9BfgddtZ;LcI_P=dqp3$UnEo5WhQwDjXL!+K-so zkJ`RhWQ{_M>L@V@kbRDQBb~Ia0Kh$-#trAqV;u zJC_0CFCtXoxUkXQ#LRw350~Qfi6#dV>V+KWAMIR*n^1-0!bbZUGy5Sub&Ausn;cB2 z7jmGVw{sb8LKTh+8|}@_?8iT?dZ)CIUg5h#V@wVv)C)Oqzp`@~ZbB7~3mfeoGy5U; z+7zivaOX5Rm{2d|zo3H3q_)DLzp!%e8dabcr< znVJ0n;nY4R2NUXr9H@ircDM;uI4*3o+wE|+95tND!GwAt2kJR{JlupT92Yj)?eQqt zx_8PUsYOpZ$D9fELJriW_Pn~-A^va(RX8qewA=GaD&!QWrZ(HbgnA(dYHfRcxCvD_ zE^M^h>q9F26lZK;axkG@$boT!z0PGck@A%tO&CHIj;rGYdz}xLo}s#oao%zI5+&FR zIWXR_{gsN1itx$y5z!ydMW+0{Pp(;k%>I2=r7UVj&}Sd*A?=ep9o=o7%Q-u z(ES!6A~`e`2=#)Ezc0H+zVj0yA~{rX6MB|Jh)515)C;tJk0(Mza;O3?CiFax5Rn{A zs26Dc9#4dbJ)Q^=$)O6om_Q$c-X}staxkG@p!It^mUzDG zyIcNweSRxeU@O7BohAOdy~38~zwNe#3HCy}eviiz?>>5Z%kbWfQ&?dFwtlrIvK?m2 zS#cA3XNVAH%iV-}p{>5b6b6zsD0HA~{rX6MCnP5Rn{A zs26Dcx=w_Mb)5(i$)O6on9#~8hA70bV5=8w{kl#JVUC9? zZbB=-7{cUWLcKui*L7kDlS37FF`<=g3}JFGpIE9#%89Ri zSb@z1BE{4`=&vHb3W}T1xuuw26-THSXzY$@vqp$fd1z!i+S*9#Ns1zNwA z6HllDFDB3vVGjAignEJ2Z{@@js=$j0^xBvUzc8U*p!Hih@q{YyVgh#wd}Ab@P$iCV zo&sE$z+DZ`3P9_ZZ{i76;Kc;)qIjBdVM4t?>z8lh302_51nLJomANpXUZC~MH}Qlj z@L~d04W2Gtm{2d!`sJH=LKS#1f$A*rb}*q{p!Lf)@i|n17Za#U6CV#I)C;tJ`6fPx zD)3?g)iTbQ3lr)ETECAIPpASfCNMt}y`tWc(Zs;sjdM5_Z1vJn2EKe_b9^r&md)RG zGh-b6a*mtO@7P#kgN%w!y*@uO#=&=R+yq8Q@qLMoia@|tFWCB3n%FI8#Z6$O7vCqk z3H3s|ew8LZhbr)50wdP=zMW&iRxjB4Rhsx5s<;V^yyNdt91FI3!Ny2G{yxZxo4}ky z{Jqvqs2AEXV-bHpX9YGBmk?Mngw6*&w|Gn8a!D^vd%%C> z$xNRTsJ!q^9Q`6R+I0@d6Ue;TyBWmSPi6*LVFI>(UB?sHP`KQA3H8$4*bYzN!A3Q+ zuN*(wzy7d4#0tlSjXh$&C6rsZoFUdFAk+(ybRT*GMT_*z9z9{Ke}0=4CWk5<7dFlk z`%NU-j`I@gg&cZbc>;5Po}2yWGb8=G&K@v1RN=U=^_;eU8yCdR1cZ7ahuW5y*>IqL zQnQj_RyZzfy*4jQs23usZHeJmb@I1pQQzcHh2z53-si%EdLf6}mYAK<(jQo!W^$;) zabasOdSODnkV9=tTsE*D3tDO zhbkNwwpN@MCe#Z#)V4&*+G(LG&hb!%d>_b$oSULcI`4ZA375EcUj@Muywq9VM4tSNo`B)Z8$JoRPG^;4?Mx$AyjBZtYC_MpdCg=Ofe$k<_+1MoOL7v_*ZBLluq-8@1ih@*goW1`fFo0q4V9I9|!*r@Fqj<#cn+5dYK%ma9r4^?am~f zbGGArgnA)|+BU~$YyQZ1yWbF#LlusT>qehiMu}Z6g1vA>#S-63o%ms~yZx*%0UNd5 z!G2}EHplr0^+F`IEs-X5VzHCsO%7E!E^O3xJC9Xxa-5G)FXT|$=5XpnlS37b3maA6 z&mAf|InGC@7jmd=b2xRP$)O6zg^k)Sn&W(gdLf6}HiuIunjETdT-d1XqWkK6gnA)| z+BU}ssS_(Y$3qp43mdgv^q8NIP%q?A+vX@UxgfO5Ij>aVxUf<6g&*-a$K!m2dLf6} zHplH!C$5&WofVD?8&zNQ8ayANUWlZ&%~4(I!~(g_S>d>_QT0XplJgPjg-B}K9POn} z{7(8SRyZzfRDIDt@qC1OA(GlQhxDRh^t-HZT-d1kqJ8`M2=ziFwQUZkPBb}G;kd9- z^+oSd=Ofe$In=f}oH{Xs6^;uVwVl1!N}ZVO)QNF~dLa^Qd#^24v}ba6sT2FQU111S zI4*3|C8!2GfuV9gKOdo9$Puk%xb|U%OOWauy-Hyn|y|o2b-jTCh!nS_ueM+(e6s6N9U3*G)jE z;wBoeyg&HzD@q)1vOP6v?TTD@slo(~?EbgM1grj1+sScluRoHeFDY%#5>?zp;nH^o zL))u6#G$6i$&KIrI~lQ5aT6sbj0o4`N^}34WWvg zC{t%Z@b)9S9b$ObzNwG&dnFfJt_l-nrM<>=eS@3YZE%R~WztfTKHQniikt8j>l0j6 zWR^p`_hN^Xm7hFe2vyv~ppLzRC7-_5A)0KtCgtqU-3+0Mo9IxfS8!{$LJsjqa<7yd z+P|BGU!{thm^Y#u_1Ca`@^>~r4k_t$P-Kjq?Y_T_?5g$W!d^ojc?4am1S zMppVF`S|{3CWjL2g-F=z@3o)o5NH4SCV5hBpUClm!USyel|zfpcZg~Ye@e5%Qz zvDgbaaF+bOZ>2+YDsxZrjL9d=cBtYe(7Sf`yyOrq+dq^XtW|Ui$AYb1<;2Drx@kt( zA%0p=Eal6S`;u8<0yg^Nve&%l5FPfGPWj{3LOE_iz0i*9rdqo5#=5`#tV$`Z+E!0t zh2z3T58mOrADtW*m#&-g?R#k^hsI(rM8eg)XWB`Jc<*4dlv3XgH#t;c0yg^mvo{th zu*baEi{6w0x6Cs+m{2d|Kp%5tLNSLJxukc>w$-^NhbkNwHtrH_FTL0ya#HU|d9T?K zlY( zvBCsw+`GE|Qq3V=9{+JlU~>P~ZbH34;NEq^^y&^F9gnFps)@3qLr)(nGT61|_w=E@ zQaipPV?}Kx?j7697hjRVOO-@Kv?AkQ#RP14fo}A3Z(p<`V?q_pr^G`i`uL(1S;PNL z+A_b!pDFNCg^8-tzV5+(zGy{u?Z$Ufisn9>!-|{OG-seMT9GwRU6>LKEir^DZsJYP zO}=PFw(vlYlttIhFoY^@;ydjQ@gyiE*JYs6((RGD|MeQz9M7A zO`Q66oG)6D$uT$o;H8S2=$t><7p=&cP{mEm^Je;@6&Vw%xQW@*AM!;jGA2}U6R&1J z;)_;fOsL`}R=+BL$um-sF`EyT9M_w_(t-#pHw!4DsE!fs=2;sMYb-#bZX|_ zow>-N3KKP?{b-wczGy}Ew^U@CGTu#M#Z7d5WS%ctk!3m+83NPUTpRiwj2r*u+j5HE3#XpBI|je zSvJRFFSKK?M=P>Fq$0adDl%4>fQ`N~T9H+hifpM=WNt#e(2lbtz9M6VWtyE+Oq#|R53E1e5qZOH7DzfUs3gx&7^+G$Yn`lMWPAam= zZK|iR!f|1v2ai@{XG+yg`B*A4H=$l=$JHFI$hJvE_LEd(tZ-b|=<}l$Ss|&&u9S+* zO{f>z(Z@t9vi?$$rAkG{3deitzETP*Cf)yry8{zeS6{_pxI3K}Yuhnkj?LTx*f*hzlsU=3dz}?TVYOuU&RU&vwm#jZSg?U1UZ;suczgz?Ne!{ zljFMuA8l#-TFo462P;g>t=Pg_YqK{Uy`3k}y-w4Yum9r-)0Z&8UI(Q2Y5(?guO*!8 z+#C;9nD}0L)FlHRO+c{1M8(Yd-VY0Rc7BzU!yFGL*y}Focc&eABtZ^VnApC)YHSYK z2KzMS^iMN9lCc0O?g!V3iBLi|U!Cq%qm5trInFbJo6(-W3EoqK<{PAFg ziQestn)50`L~<~}Ue!t$Gv{uEus=$E6)Q{>zWP-90l7XRL?j0j>=kZ&k-2IkL?j0* zOr&^!O#er&+6xoxRlD*R=|`n6i4c)r#W~n(QM1CPzlso%U&RU&zHSH7w@9BDAtE`L zV6Rhqw&w?X-Rg@FksPcraiZ4#^doX#i4e#k|7C`mV6T~Ff6c$H$?ya@SYhG;A!_v= z9*oby1bf}zvt+>shi^-egB2$379y*}hy*#9V6R~_sugs+cVvPbtS~WHh_XvZIyusw z%*lQB=;F25S4^xBvS@=3po3OSx&C59+n(p2FWCVb>oC{UIv zeP`F2bZ1bdeW_If_&3GbO=Z4(fz zEG_kvH>+^#|HrRl;@8s4y!Ag%Nsxno6?+}adD{EO@}vX=D^vbi?#;it<^SVXF>&~Z z72e$+HA|3#e-(Sx7`M{9sAtnC5joo@Oxqixe5#sK$~T{-xbU-TQ4<*>^-tg50 z)hgeW;8(G7v|`7C1%<6w?5}cfx#nPE=_efvI=`7{%US7PwsXPKbnErsa=3}#8=miY zsKP|!!<`EbZcrj!Mt2?hEit1zKIU)9G1&L<2p>jtJnHiVwijEGJL^mrb6&B+#Pc$` z>(ykGGv=K6@rvAQs&q00D@@!aqr2c$!)83l3KKo$JZ}5(?dBK8UQw(t z@uZx)`(C^w0l^9rBjwzEVdR}r!a3}jVOE%cz4aHn-#h`|$9b(sEot&H&RsX5USG($ zyZ!GwefF+4waRC&UfX<$Iony`xJS2NT~OrLM8Ap^Ci>UwSkU#8LQ$?E=Yrdg(GiRgtZ>|$k998Se7_RirKdiy ze}kE=S=@ndDA8Q*&Qm*8 zFa#@@ht;^8+Y|U+?#^wWx3lN0Fo8K)-6naTQ|`{QuDK%aSFsmnSTRbqdv}7|of|B; z#t^LV@!;ICN02Kre)7W=Cy2=3d(EtHMg4+MmBQX4;YadeuNv7rVz2L2$h&lf`%6vm zQpHVdY1!gG5bCA5TTFY}u7QmF6uzf$i%5b6c`t6rCS zC;nQHAcrdOVq$#DTK|DiFVGwPRlLtm3nj?GXCI%H*l+a*kBt(U=zr*0aOAcrdOVgg4thKS@~LcKs^zkR#&x&%2?ffo}vvN1#?2NUWA8hiTg zp9a}-)VrP<3+zPhMIf%YY5vkM!$ytMzht3^Sy zFG*XJoja+bH-{C_O2Gd9r$R2G%bX^y-Gq8+?!-L6o4&6Q?_3sREk`9N8Ema(ytN zUZAn3V|s>24prdA1dePB5y`=XdV$8Cj_Da9IaGlc6F9OlL?j0j>IE8mI;LlcCrHY%VJ$FU=)HjP$e}$jrCe#b0kmK>2*5$_$CWk8Uaud5Z?TF10A)vSk(Ep84`qYy% z@<-nPeS#dSXbvT?eTfNG+ywU9&rRP?kb?>J0*&KTt^6km2vy*PV?O54XOScL@4t!( zoRzc6eVrf&|0?xDKAh8w`hS;zs9s@VJ`{L$Ip8GS=qf82tBN#RSgKnDYu(RNVE!=P|Y~@pY~WehCxEhwCx%mLryK zhhDYVzL@JAKg#?nRX|{0q5se=|My?T1db;9t_%Mv^+FDu3+VYgf!puMN?Y^r1O6hH zPYGh3Hr@=v+dwbK7t>$w*V8vf-eg3(zC9#=r>JjM-tXrg@MAkzVM6V3=B>sjB*!|N z!c@i@mV!Il4~&6UmW&ytn_) zA1e4+VFGrCKl}SOn>Qy94$Vq?``#}8OQzM0Bh(9#bOpXAFlb;_-uo?m{#xUk8A26~ zyIykK=DWdH!@M~;V|Z3t@z%-yJ#V#%A=nE!bOpXAuxDsi-ky1l{I`zkU;7Tubaw)D~)LodkQ zbrb4^_UM}y>2mIVTx(9Kwj4oLI4O1?C(}R?7Wq-O!|^OZN3Rjle67Ts2AGxZ52=8f5Wo!ipp(vhg?yta9r4@ z@*Z{Gt{EfSadu>x@LO`#x(W3{d-M&SvV-Iv)wz0jpY#l@a9r3s*FWa#Xx`iYRQjur z=QRqW_i+>Ih4$#1NTp<7ebg#B?0LJ5AynbGus<3#(|4(P|NP>cv(g@H=?mX4y(q_G zFSJMBs9JGbR^G0AyM)nGv%&=Ie!tH0^)&C%PnPp)`tjc3`EoaK6Y7QQJo;wXeNr>* zUo$A&Q0_Raa9r4mdZ8U}JKDDzuj!wa_w2(Dgu6GLVhB|@F6={d=lMpN@6o5-m6aw{ zpBW8vEcViN`>y4V-)9R zx4$L*?p>og#1ZO+9Qv-cCon;d$6)E(w|47p2vs;PY}^^_yZR4HU-GluKlY617e}ZU za_Af2oBur{XC9PFXYg-)eqNZsC4j-Z=RFSKI>5IIZo zr0T=iz!0i%T-X>{*uR~yRO*s%Wc+cjoQWKZz0i)4j6K`ONDb$VaSWjf6RDj-}FjwwT^<+Hwo%9SG zi@nf}k){2+A8C@~eHn9(klu$CCSYS+`;qw@TVCl)ipaQj|66V12=ziFjF9bLBN-z7 z)kiXRt}eYOD;!tH(Id@YV zUo!LT-Z(O{=RwEz88L#V=WVPi(8xcOUQzsXqbu+011(q>s4pY#m(F) zfgC!E=LvlBMeWU-rRp2hbV~50Ki@SKQ#PKlJlz!fnNfJ3C(Bg-Rr9Vl2lHrzLruUb1e2kJL zVFEVV?O*YjG4+;|(lRsgk<3hREcQY>>bZ92ucWyNRhWQ{cKcU6GS-Yvc~z>u$7F_u zW3ks}$$@(A8S_`tyh6O<5Uel(TW56S@6flNkaAF}KB*b}ZbH4#j(YAh^HGIRHn%-nG- z_Ch=Axf{*jRdW-nFaaCw_V0cSn4Fn1SY{x{$P6UMVlT9#o(r1459cOSVFEVV?cXmc zT4hQ~ZK?V~GS|nk*bD8b=Wbh!zjNm%RAB-(+U?(ADRtY}l%Z1fJuY*79E-isj;ilo z^Y{APgepwH)){CSd0jLzrC8ffRtCH z>MJ&~pUJ_5dLaj@zE8|kfTeXlNzM_XghQypabcs~J~!BY`px7+QuW;_bA23(z0i(& zuBLgK;U-jJ0yf(1^N*B68G{)snP=KB19 z$y^`DVlT9#I$LUlFaq_>&TN)j>TSRN7XmaJbit% zODL(M5H%b^6((S#-9E3ylU-RTf+xJ09E-iQeVFr?ZX-k?R+vy5Pn11@8C^n~o$UxF zBH)E~Y7>7`W3D{`2a{w#q49Hn0cY`uAYrwJC3+=dxUkV~=a$Y^ zo|5yR)FrRTY!t_0FSMhQsbc=Nx|>ji3D{`2b7WF2~o>pwx*b2NUXr9H=axF@J~NO{l_gVWZv7SxWVq_C_+-$FbN8?Wp0;n7`TYCRAYpHrnm%VEZ+ra{@A& zm?Lw29E-hlT(Zb~NuUD}tS|vvXSqFrrBiRoIViK|U&>scn@}&bW8`(C`M!ahP=({d zM!TIIoPDfE&JY>h?UcDbj>TSR#|ZKd^Q7UiWAA1U7vfEaP=yKDXt&QMGCOX~-YetU z^71r;W3d<7F%sTpo|w1^RhWQ{cKeLwe{Tn}@r1Xw%=K|B_Ch;G^hM2+A2*>26R^>4 zpB3GD@UiS~WS*h1%=K|B_Ch;mCaRhzSZ+cUCSar8KJzNObwGA?nf+)lPsunId!ZdO zELWK)aqsR5WLFbHo(UU56((S#-9EdkkbOsXHJSJ6EKmD57JH!`Geu?16Gb*%ywb_~RGK4A|7q-sgc>?l;H%*@K`g_P+AO5nx zeqH4y@!Ig@yddf;wCi_TWX;`O(RF>SFrl`s6DLTH=WGsNA_88T+pg=oO4b_ax;|Do zF6=(n&I{Ul_uBzkX;o!i-vU|J=O)w(?fPvNPhgciH&`K6_=-0t8bTG03wvkI+@P)7 z`^htwV=}*cQs(+N7JKoR3p{~Y@*HQp)No5>G{FiJu={^JCuqkXJLOrbuIqCX>cw9! z@C2TgwI3B`4fTK9_PEKR-%8-Put#p39kk=9p7Pv4R$2J5%7SCDS6SIt`mF;`py80L zy!ul0)%>x7pA{xxkM2AxXxH^Ex-q)0&rPTo+Vxup@)!Q(dF{(m!==k8l@*Q)`_Sqc zK|8LUDciBURkA-_=K9=(dZ9h~MTGKw|;+oUX(2y9xC|yMD*W6Sz~Zb6wZR3de<=k<}|` zpARk{n3eXptm_-TZKBDcvDgcdqF-wA%k_bEeU~@&8bTE&VE?eCXV5<9TrO)M(_~#= zd6~K6SnP#%d{@e@>+5wxR-U}g5iWfB6ho-O1nk>y?-{h~`mT_Bt*-0iSnQ?kc3s~z zCr2@PV}unZG@o79=ZkJf9D%Kc7b58wwLF2hoPE_u*7dQ%abe?l*qZvN)QLDAw{Dvl zN2nJf;W*iKeYw(?;Fw<}l|CyR7dFlqJ2p5c+c84Ut3PGt&P}Kn+V$&Wp1^Ig_WX>T zy9;If!3xKPjqAgXaek5Ws_?9#;TAI2=O)w(?YPqHw>fep$Hj6D?t8O%m=%r-8&|L$ zt9j)5=p)zp0a?l6Ce#b{qkgH)6S!E8$6D!2+R6x$6^;uV{goYaE|d?aKiaYL4{{&eDeL;=8A}YoUWkOg-Ok+cx;|EzfQ>tY zodXEU`jU_3{_%~h>vI$8g?8Md?95#SIUcL!9`&8fOt8XnVdD;GS312f+wr`;55^Jd zg-H4ZK&j#6e*U%GYq6S&6^;uV_j5ZZgZsI>&4G3LafEsy5-I~bb2nbLBS&h6`Z80* z3deNc%?4513gg^kL|&XJv#@rSIS!+R5P zgnD5Nr{8e&1RjwbE3dKZ`dHz(uu%cpxxR;`W?12@>vI$8g?7}U_E+(`K2|s`Y*e!G z>-yY;dZ8UPwcWeCu8$Rt3mX-=J%ZoLcE}OTz!7v4>V-yY;dZAsvIqM0$DLLxN96%j;^1}+pg^hUz`>bfRj0a^UxgRUZ-GqAK zI@d4ZdIF=Qzp5m28K1R19%6;#!p2O9eddMtadcgun@}&bN59mGf4J(cO&Gy!raobHz$Tz;kd9dGi9Gac6a8lhnw07dLaK1ajyso+q&Fv)Y?;r0Odp6}cJ75v}WU6Y7O_)N^)SpPNvH!#Q$-2HGLOkmbsxSc??RFehP1g05m#Xgz8F_Im_Ch=AIlHdUO{l^IY_!|) z;2c@kw^gdXRx*O*SnP#%)N|#{x;_xE5}^tcu+eVEwNssSePv~?k7KbH+A)rfU)RS9 z6SzKfvNN2&TgmzfDSpMTk&^3D{`2U+0=B>-wINsxMh)iZ~W~ zp&j*{UDpR9K!hqxz(%{BzuGA4`qs*f*BvtB#j)56?WpJMx;{6d3KOu=ZrAlaDeL-* zIP3a27JH!`^_*ST=O$EP0yf(1eBvNk*LSnCu8(7}7ur$J*>!zxLKP-pqutJn7N>Q6 z9E-isj(X0n>vI#TFaaCwc3t1e;bT+2l^NkFG9%2f*bD8b`s}(s5Z@7@3KOt(23kfe zvaT;%s=oVWu8(7}SFYqh)o0iB%_o8tCSdDKx+n0wtm|7PRbNF}rROHp3+<@q?7F^n zvaWBL5LFyP6^;uV?e@9BQd!qmR;s=XdD6hK*bD8b=j^&ZH=zm>u+eUxfAp4BdLgO$ zu9mqzj>TSRM?GiP^|=XEn1GFT`yA&1S=ZNGs=oU2!zxLKP-pquoA_ zDk|&xYD?93LY`o8EcQY>>N&fv@3qTbNvl{KACSapI zc3t0PGS|nk*bD8b`eN7h)pQ6|n1GG;_;r08i@mfxeqA3cOsE~Zu1~fjn23NE+Og%a z>-v;Xh2z3TyRE6^cw{@rBgC=T3+*_9c3q#FP=yKDXt!g7Pi0*n&Z}o-G{Ldh3+*@) z?Yh3HvaS!r8i!DY3D{`2W1K&m-JgA#ob5lzsEA{+7us<}*>!zxLKP-pquq|x9+q`| zSIBjKxs2{O7JH!`J%e4>=O$EP0yf(1n6rnh(tBO{E5D3VITm}N9X*^~*XJfwVFEVV z?bvyxtn2egzuQ?x+Z>C%(2kzkuIqbR*7cPVqJu-I!USxz+c|(2WnJGwxf|4ySp$y6 zUTDX?#IEae6RI!)8|`*3L+)xh&VA72U_!n0{%F_r$(=Jou)=X+N9SbZ?wsS?&%~9;oQ#+p$f-^jdnY?RJh8NoQLEI?^c=X<5=v4c2qKUU7wp!g$dYbw{v7t zE#)}%mC3<`dLaiYGP|zNO{l_gVWZv7^-0y2vrc9Qv96C}u@~A=S=x1dZbB6%V58m6 z4odZ!HlrLOVt*c3q#FP=yKDXt#6W&&#^LjWUipCUbopi@nf} zF_m4{=O$EP0yf&?*Y$BM_Ch;Gkn!vKSYZM-i1>AVZbH4#j*)P5UEe-=CR{;g2U+2` zI>xonSeD4TzRfb)_RCWlCfEz@7}48xeQrV(CSar8J}asu>-s9lJj18*bctiJ7uqp1 zVb}G!300VYjduIYYq+fID<-oaPsmd;j>TSR#|(>I*LTV8Kz4B<Ku zva+tPiahP>By)Wni@nf}IUu{P&rPVp1Z=d&uIrQ8D3gN;^+FDvpNn1B#|p=Vt+RNZ zK;yl^;KROD|E<2%aNYardrK`}UQm4cUFI9o`kfbi3ufdg+wKWGCd3pWMhL+Qe`AKf z9^(m|+Y|In6Jn%`V6XgFt44Br0weYYeVNX$VucC*-i-XEkOs3ej{SbQe~9E@g1r`R zt>CS__-V6!o~o!Oz;g2ddh)X5UXdzf(g1>^}37nCwE+PBt zE*HUGW6NLc{r;LKoE(1(agh*r3c(5!{1qHe;66EeAINc+|BoTqD}QiF^KD3+Z=S$d zA>I>0{y)qL6R}^^L5|9DHX;WT?1f*O`pW`mJMI?Zi9NyK1m{<&!UXa~zXypef2#iM z&;u@ly|C9;jh^S^z}}U871F({3KQ6G(XTt+AV;v$fX_mD1eIVfoISU9o#W&HafuMJ zJ?6Ypg$W%0=+_;q${BK9TDdUxF2`aoTm^qUGt0?wLe9c_gur#q3KKYwqu+5~C|6Xy zlb45a?lQq%xHijW&2)17DTG|L;qgMS!UV2I{hGZe@T;65*lIVyUg#xq{hH%i=}XY_ zOqKswaTC#R-j|YdDiHx%z0g-iw_~gjcSwAkSFA9B{xP~86Xl#LCi@D%iV61Wa&Kcg z9;M`*Dk{Vh-d}MoA7P*lSeP zWOJR{9CFo$_Xu&n5UenPZ;#+QceX=3YsgW=xy1x~)qOYF^j9_qdY!>e@52fc{7nME=LtM4M=(oz2lOROuoupr=zVas5Y43r>L&y%Oz?N`Jb@qNoSH4YTpt&~UO1q|k#p+Olb8E%6M_{cxDN6J z)~+oU>i7Os^Vf|pbLy+kuLbheu9#E1V2o2=p*@yhB@qGI6PUlYScc|cLTz{zIQ5my z!2~Ob2%BTy(Kj<%KYW+j4kpxw*Al0`vN@PwB@tnBWPMV@Kd{=hCI=I0!>f@~U)dZ? zu#$+dIj)!Z&qyA;U(AHs@VeHiuWSw`SV=_K9DQX!b&>sr989PUult?)%I08#l|+Qi zF+k3#@p9aeg9)|aRp8WDHU|@|BqD4MoKrY|k%I}fbvsZ+*&IwDt}2NLo8unYSFiv2 zrpdvC+VH|&Kuu(GFu_V9!sfVAj`;_l)Cglcm{1#DIGU)UYz`(^NkrHj`{bOuqT014 z2NP<;3uh(jE1QD}RuU05$D&`~%$W4>T_y(;YQqax7wQt5g9%m=5jKa@GngDqs0}al z5~#0i4klPhMA#fopNJfpOsEYn^mfthV1kuIgv}va9&+|oCKGDItE*FAMfO#MU?mY@ zbI1`4JI6ee3AN$%ic?=jj(LP&B@tnB$e9>+&fQEV)P~n0r@pc|m|!IlQAub|K(46p z8}=H^WI}Ct-R{&^HU|@|BqD4M=^4UKUy{j$+VHx}sjqAfCRj;C*c{Tsg|$y)LT%j+ z)K@kK6NsxyBEsg7o;uvs>D!r58(!E8sIP1eCRj;C*c@`l3F|$I3AN#cqlx;;=3s)A zM1(!BqY0bz4Ey^nv`-b2YuuoqW@(Haf}D@^G2*&Mp%pIsJ6 zVS>G4>s=;TVS@LYC$P*p=3g}~l*$BqadjR`u)+i%e@{Tq#P!84Pi2C=c+3z>u)+kN z$MTkta}Bo2?2yU?d-2#OmSBYmz8*aR?HSf>?VZX5d+``9mSBYm?pHhk?cvr+4kp+O zxlu*gu^JPsFoAp^Y+s^1b$8iUOt2UBA?hoeg9%oczy%#a=k4qw`lxu)+k+V-Pln)(jul4Wux^Ubr@+^FBy{u)+k_rSkVhotj#Xc`g&|#dDdl1S?E%eQnM}Gd7TOHjNWU)K}8C#}Mp=eHeZI!2~NzV84N|IdnuX_b78b zG!}c|T!_wJF~JHGIQ}4P4!PHwxeU42n)6Cyu@}y1ouxAO+6ci46F84S*c?&;m^m4# z8FH9lFRqE?T^S-+VS?)*d2@32nqbwjDgN&Vw+d&5FZU+*X^@ZSH4_gu_FndL$9#MR zdbIRc-|cH`+C71Cp5?*Erlk7izc4Fq;-O{N7}^sk>{;$hM1WSWA-yX^^2rS3-cg}( zuQ&5I$WIA#T>gbm*G}=4tlTEw&hOSOJ1%tFMNRyBnzatI!bH+HS9EB&BC5B+H-$r=7Uxn&AIi~-zE%aw@CI5SSTZdU;;<{q(yggSIc5+nf`FChq z^J4zV3zB09_PU{CM{kAW7dbhuJyAH^GxcP~;X!S}tT6FXw%=P~)vi3d9k(>D8s6IX zy^Nn4C&v)%l|8VVcYWhfp50e&KiNM@w80PVY7& zW6$BF7=pc0Z|v#)`q~?v97kt24ZoJ&J>#oC+JsqQV$Ka+yxxV!x7zL4zNb~V>!e?O zt6oitA=vA1jo#h^|6H}zZpYFUN#Wb3H}ri|u5FkVCT=~_#XDe@r=895M$xw6_nxT} zocmHz48dOckM{D0)}C%>b3Arc%ka9^r-H3KZNscE@kv@|Z{FDSG@IkA$w}c$Yjp~3 zcq=J}V6R;(yL*2-vM|l&@GPhw-dbaBXve2*!mKdi8`8l$I^{Vh$F{lk!d-6K5c;N2 zaty&Z++R z1bbD?tndA>aAzk6<|>=lYZcD@J~hk=6Q|l#qa2t`YazrI7r|cJ*H@(+@~blRSFyrG z_sW;VZh3}oIV(&gzgCv^ZoQkI%Gf4*x0mc)CfKWY`=aIu+Pzz~P9WobIfDN@+B(b% z6W&Won6tzl!Mh7S%V;TQ$sRdNm|(BMSD#8hAZMb@G5YMc8CS`fI84q&R+zXV^<4T2 zIooXxIothT$=Uvnob61oSBm$?^nc`vvN`tEE9);QSJWuEqF7-fZOMuByX301Ir8S# z^Y@gi_9wY&nP9KlmA^Nb>J}w4v`i>8V*^V%e%6)3085e5=hNcLV=5&fS0s_PTcEq4e+N zj$?Dk9mkJ5&ew9sVTFm?TOCawCwDcQL+)z+?s8ZAQ0{6>uvhPAzfRvEcTSt5*0AdS z5pw6eNba1hFwytepXnaCJKG#`clN*P+?|&w^TB$FtN6CDN`-k z94l88WXzRnX_r(>Ot6=)bV*Z@*&ORG8J96yDzbf2k+H(WjdxazRGgkbyS<|_?vtvo znN)pDuvcuIxM%koUuCIYf0XK#6(+bw^#pQX|0Q_1<23*AFXtsc=+wKVOI%dIbt1;p zsCUsGOR&O3Y>szc|HX$Kg}<6-#^)M~y>4>0+~#0{6()F_Jb~m|n=)RWe~;fku}=;Y z?6uB0f;I;etT4gH#uHexp|tH%%(poj3Nc3r+(B7MMA&n8kGx}9T)vm;CX|MkUcoj86Rad6Y>sy_{^$Sh zlSLWG!GzlI(w@lXV1kuIgw1i;!Uz0=o80X~4kpxwm-eVO2NSF$B5aPEYEJQg`ax0% zIharzUV3M+IhbH25n*#AmAlh_@$OwF2NP<;OYd+t2NSF$B5aPo2lVjw{i3RQ&dG$@ z@X|ZA&A|jKi3pn`cWwj!Lp3^@989PUFP~G%*c`75F%5u9nSqgq-b2VS>HZI%9*#c0>qPnBc149FLH5JW`lo zuNBT1r;NFO$nl5}tT4f&7f;~Txedb8 zx+Frd!UWIa$rHs|n?f&1eKm4opA;t8i|fZ&f)ysXX7mJVejE%gkQM7#PhDG{B8~1o z(~En?_p*|B$$&?_sH5+aTD1J}8K&J67%uBkWxcjv-p>fL;wGjYc*M}2z{ej2eX>&B z|B#CStzKK@S-R$qeK&&@CXUIPZT*F@?`AN;UW;UA`-XnAobBLuGgx8b?5eWf_HR#j zwxfrvN1ZCGKe3jR3HGWlGxR&o%yx3Lmi4H3cLHy|u);*TtfB8yX{M87n0VJ03Ttbb zV6U$7q~V`B^PC(bgt$xyyp_TV6V>EhfZPgmoE+6;b?IhVjq4`Z>t1Eu z!jA4_b#&z23?|qMzqaGwOPn0|RlDWg4E_*zyk<82OB zn85LWc=~cD2fv%a1bg9}p3?bgCr2$gr+UbmYHT?xOyE3T{LxA$$8U0m{4T5Fv3HqZ zFI=0WdM$Ty@VgnTFoEl_eaC>4gWt_yg1yj7Y+I)}_}vUv+{C~)GzY(%!31peLSGr( z4t_U-6(-O>Mz`aroKwHazM3feiV61WBr6A^$3w2aki1J~-b!JGi376sJbFAT$~o0b zjv$T)6YP~MD)V1m7{4{`snIZDgk{Za_rqgY`A`wjOGn*+!ESGgCu3HHLd5WNrb zyBVx7f#Z++pw01=oFSW}m+K|JiV60@IUT*9w~%u;Nr>q}u)+k++VFbZsjnjIEF*7ou#$+dIc|~l4AY%=Gnh~tUJac3%I08# zl|+Qi;jFAN^KeY44X!Ac^+=8!E9Ir}P;3AN$X*{QE0`zk`Pl8CT5_}vU9)P~m!sd{k8t<`WAO{m_!wY)>^_9)R z1S^RMo5MMR<{rg_+VH~BM15s*Fu_V9!k$-h=M3w;mI<}tg|ia%mCeBfD~Sl3L*E|2 zzG6acc;V{8ypPSn1S^RMn?oy^eIFgl=6N6VK8Oo1jKZSxS4^K(NAuZlBGeTaI@#m|!oiQO!C_^X3Z(R+!-Z<_YK#+$`^AFu`72YsV6- zFu}(^vg+QCb^1)O7mqSx309ck^EmP@Sq9!WV1m7P>=R3{!USKBv2R*1!CpLuizQfL zg8P-&cQcq^FXWDnYqd|r+Z?PgfqWorU!pyAd)Ze^uow1WbbQVPD@xY~f5ikVOyKx~usL*Q zLhiM>Ot2Tu>FE3w6Ra?S^B9E9!EahH!CqVw#lD-t3KLuh$=5tG()?+cy;rdG(UyMv z9npK%Wd*mX*a@)_VxM8f>qx{vE}a2Cx0Tc?`i`$T9n!r-OS+A;)E&cK$c2?G6@HvcF0dCSX4v zTpqmY%Rxf?w$STux}$vPr_?4U2NUXr91niCA~i9ZfA+Vt zLtEC@F@zplckBcUU7Ek@1BJquJA^7sz~1}ey5KFDO@z2ze$}AIN`;Hx zx!2@iLcNfq&~@vBzy4T3h&Hlb^tXAX!q?tz2~{{Q>`Ml`5d7)MYC`0;PV=YcRt#^t zB;VvyEV+IXKeaLVTCq2Tm?=by*7d@BaxI|>$Aw*D`-{Q5e|t-a{?bz~{IOB^u(M`c0^1hNUzqb}*m=IfB1bg9L z`+D^kgRM{0mK;ZCrTc#w*gV|(&|LE+0ae^Y(tsC(Et1wtj@#F^^M?<$4BvakX9!i8 zfc?d)O~Jk|PY}ZUSi1l86)nQ~yIwUF3lr*vU$y^^7lZZ6U)jzRnA}BHdCzYdzH;$c zL#V=WVZXj^Q?S>dmO>P{N>-9T+A>_~gPjE&i@nhPUcVQE8Ji~Oc> zCSV_ZT+YPN4-0WZ&i4CXXc69hYLWR`0Tb$l9DAy~7`$b%C*Kpu6JnhZ2Tof;6^;x0 z+3Pn2i`ThCh+H9RooNO(q8u>V+J4+_528ckCu1ddU6bB_VFEY6(?1F6>rIHU{sy z<7FWhN-z3u$$H^>S&x|nu{$jIA zg@;buYjQB5UdS=$hc&^DGb;O!L1wpisEM znWH8L6Y7N=+lM|MZ1_}tAv{8agt+x@OQ^zeVRx6h2t8yj>6Y7N=pMUX8 za9{U!LR63%ZqVmDLY;rIgen{tc2TJl+wE^JL_4Vydo+I`H0Z`^CI=Jh1!BjPXM%Ia zbQYqB5RVEmO9)mtF76CXq_TW^ST`ZY3-LmW7eX6vh$YwycAK)RgC)E565`imZT;J- zTp3O+w!p`C8Q57LzZ5+FTW%WiMejH@Po(*uo!TOtp6NCBC{^5q_VcnX{6t&-g72>k z*SIC$jAb=0h!=i%DL7?Si+0EnC0>-fL4Ua$?0S5x884~gCZczPHqs{!lfM0W>Dv)Y z6*r;1wye6BKJhytGQO~cDonsG`_W6m3Ponh-5^Vd-O{(8mcAW1m{2dYYp*TiK_MOx zqQ2ZeSmC&^|2O}o;PdM$NYC)S5Jlu}a8&wsH=$l=*IrxZ0A$M_l)J&udo7^~$A$fN z-Is!obiY+{{3JPQ%iUmJv!)@A#a?LFUfUD+PVNRj39%~45~?r(d)V|%!Se4sDLGz} zyFs^i8-*XMe3Qw+gnA)|_S&96d+Dh=$=#r6ZA+-aabXvds_)gILCNu;^t+WWt{1NJ z)MF+G6Y7N=+G|T?DZ~-E8w`-U0V^CA_Kw{fgH=EKQgT!hVwLpmPZbWt5$c6V+G~3P z0U>S?qO#l#SmC&^cdgkFd~OxqI2@_)Eq*J)1bb55Hq-%7*y)s-XGY%gpCd3av59VPWF zD5={tSZ*CYxtO7B$0#bDQRBmKiz-dPvORkvcSZA9ZVd{x5#-?n+UH(a)TC7%fEW$L z$i7poRdbitH8??0s1evTN?rH56%e<9P<^IYN93{*GM5@Klx5e`SBC$+2>r*>H9-bHN@k=AsZob`t*aJk&9#`vc%`52| zoS-Pw5Je-@%)jB8uZHuupWtde?qws$!}Fp&rcZ?Gf2ju$89`%M&d=)3%awHvPEZtT zwE4V7-LJqI@fFjd3av5PnKJ&y07R3L@gj{j9acIuB@tSaDt*xW8j6= z>a2a8fT#myySdA9RbFEw$iwrZJ>ltUwc(cDKwxh|PCl7@YFag2gA){m1dc72H1z>u z8IbZ*1UzkkE%da&;41AylT83Jj(c#JiIO`SKj5a z{`;ze;hu8>?c5I|)Q3`2&^Q54$pUy1GuWR*dP;Z{YS0r3{SP2&R_JD3;PkXR6ZE?= zSqdJOl$sNUS`RNTrMDAF9f?9Oqg1tvhkidC#58!?`@rwKu>E&V9%q7n|1eq$#6e}b z^`=u<;!HT&qW5=6%A1D~)~HR!mL(_9rdF-VQXWPoR2cpExYaiLL1P&h#m5{RBP^vB z)>js7cGZSvd8xN7$-@b>yYJen*2+7mK8!O&*Hf}hudJne>muWI=LALJ-)TI^WXZq0 zzEc0Uidwdn<#mEQJTKbE%WYMM{8$bew_r4CB#cI7vyVn`f}&7^Mya57hPF?781cHk z$vEQ0^CCfGXD}iRzo>RFs^+n^yUe4|zAz$A&0Q#|zLDq#zght!cgyT!eU!_YpfNy` zr5tFKgwestyJzScloyF2^R}p;a^KQ597G||s8yqg7P!SY0_jZ9Sf$8k2?bD`@*zcIDM-Wl7RCC@&Ik zyf&$C>znmHgM;V`Bf?AOWYdC28%Knl2^w2AS$e^!bE#?Bv~a)6x(4M%qUAFfF^?&t zYdDC)a2_v%(rB|6*v7)i<4n+~I;_Zr_HN;TG+MTWuXGK{i^Oo~FK;+gQP*$~PvNMl zc70@R8D$)ycP8lV0E}5e8_sR?BWq?$28BnVeHr@8jYDed8V+I?ylKcj?}&BcC*zw2 zXM)~YK$%z?-hXU8Vr~A_Mc1IbNTj_Nt?n4F=o$`UJ6x+Z$2VHnbu6UlPqqDBiZem) zUQCv*LnUQm_DE~iuLX4t%8SH>S5fMN;HJ8UgV~ z^zIu`BLD2BzaaRTkNPlA#E#FA#Lp@9QSs};iTw5EiQ5yqB>i`SqEKT{?YW|VhWWoB z$b(UxUWN($&i(&>RGhdlXtu}=U$Fo0M@3Pnadh)cf#0J~O^^qp{@FKO;1}ip_oL#( zqo}C@zZGxOC}oYQ)ytz-9i9tqirNNtQvb;Uzx-|^UY*ULwz!yVk?OpJnO_gSlIc8zJC^WA#yf3y<`@^Av}zU?On{EoX#Bk|Baw>n;? zR0Ks~rZuOdMfaxl>_qy~JKV+$uc8y=;dzl5krE}sT6x%sZ|lR{E>y7S1bLhZ{M}&N z*LGC;577P_Bm(nN6xu~DMu?iVoBo0zk26v3k2PY#4XRrYRk zYnJF{-o=j{?VjVu3jAt4r)|XC<}Y@{W{cXzJ)A&0N6bip-=nt??xnK09Vl~OzgBd% zJPN-{h+ipn+`;?dCb#A;r%I#+u)F*wU8hWC(y3-aEQP!*xQKf zy|cL`Jb%B#nV=~AJARqe@wB@i$?o>lden`3cwV&K+YJ!-1$&#uu-y;XD#^64r+tjj}u0gqY z6lTINlR9dg9!g=iok<&X4f1dT?UA?J3H*Y+O~WsH5x0!X=j$4rpeWS9FOxdz!5IgN zxP5yvM%N$@&x>|=(Y6A=U~ki?)3d1C0ntp?-~>gX27YrCYib+O_I6RXhZlWH*#&nOn9!+JRS4%DM1e>(8i|{zhG}8@Y(&mY`(5Rxp)+6;4_Wi z`n3@~;rFp6X@jmo9!{W*zd`(hy^X-%U~pMY*Wd(2p$2`K*hb**ynV&%x(0c8UbL|+ z!7td`2rNrNs(#QlI6+aULEm1s5ow^jYIZA&Uc!-w=S3S!JN$yZjleQ-?*2kQOE~wM(T>_c5B~*0QK*4^b*xu@ zYJ?)iJ!s>20M>B-`%!U%#yD&m__UunzW3*Ig?%#m6=9!)o_0rHZQ;TF+v`kiRGWJ^ zf%bvy0V?*8Y#LuGmUrtJa<7&%K~YHLx*w=IdPUpH&U5>>xJCcfk_V$cvGE{DXOFYk?B?Y+Wu;~-8>hM!oL?eGEH^#Ch%V1Kkh+0%c&VE z_6K;TpLg&lL2rAUiJLLAR7Xo+54US@f})Tf?ir>!+U=Ym4@M2#I!DEp_y2xWoalUU zuIgx0a}9n}6onc`cg#~AEo4rR2cz1z2v@O<`@bI*C*qcetB%$y*WgD*QK*5oqbtBi8sxz!PT>7_v|hOeCnySOdZ&|GgFG0;34AIYtyiwW35r6R-o2#OAP+`y z0)ID-)+^WG1VtfD?_N@CkO!kUfxlo!>y>M8f})V7cQ2_m$b(Uwz>>((dgU6NpeUs2 z-OEo69pWCeIe}%OqxA~mp@=vW6or4McP}=LF%MhR%9x`=9qvJ!2r7detyksiWw+AP zkM7_EkHWvxI~|+GYc1I=&y(ppxrY;IW6j`by^dO);P%D;SSo^|kieSA(H5;7=jC=~ zyJ;7CuqMNn9_rEClhnkNT$8{=i=LALZ^Rp2- z4)5PSYHB0>sHpXdQCL1=>x>_*jcB!DmYd6!2>qz|c@Tj#y{)kkU2`P3^_);K6+ux* z&{ePz{3)STbQncz*Z2|sB=E7)pDTVOoS^qc|DB+y)aSvEib`aRqB-JdQwg40gFMaz z&Hny>2>Tpyq-ieqPa>4goIDssv$6m8qv8b3v;L`p68hEU=Rr}ZL9?cR5}`zp2cu}- z^Z$NSoS<6!rv^&sN5!LP-ZN_aXM#K!#R-}Nomztv6ooX+Pfksc2cu{%#{Yg)oS?ZT zsWtdfQ50&>OqJ9Gc`%A*?ELRX#R-~IWYoZZ27kB7-!ArnH?g57D%$&;`t5`NkRT7* zoOpa?qnI&<-c9@`f})Tve|e*b*jMBi8sxz!PV8~pB)kThe?gGPnV9$cCb2TB$=+k( zN5u(>Li+RI&BE`+cSm1MhqwoAPTU{9S%io;NvW?clYg2aVGXo z*dja<&;3G!6BLDXrY>>f^5vvoXpje^I5EOIPW0Ln{|gOHP!!Tt%v(jTk}H3qK^~0a zgm0d$V)vRkztG?WMIk+M?N(8F#*klVkO!kU@xiiHoNwLc7aE+PC|x5%yg1yg^(TPa=6RiW8?Afkw;G_O}54iJ&N?{kp`9 zHjR5Y=1ly2c4-y@M#Z0x5b}stzZfav8Z=ttOpM&SM$9$0`^9gN6BLE?n~keQg|A37 zgSk;k$urVgm^FzWBya?x5zMVb8|PNy-|2m=$ubev>YprFSPZFFT_?!HiHR^PtVyXg zV#YX}{rNAf)qj#X#^V)S!38CQEHttG}h-NfDdRc2wly1lrX$uN99HaHed= zMUwLNaBlf(t%|w^CnyRv=nb~X(jMlKWeY7MU)*0wC&aPc0ucvm_1CWKEgu+OR^uL?7q6S|jwtcN6XzknfR)~!Z>GpG zk);g;k3zd>ibYgih-ZEm*6JS~bW&PZysF0U0oo_7tQP*8dncow<9-Z=wffc66=IfW zHC=-|oIrb1-nGJiDbD!q0BiL>e?Kac23Af*P!wv=m4PyqR$u0gVmx`b$Bpml+8m!en^Kg#5eRFZ0pnHo^bPw>T#^#C^FVEoITKigk z8P@9S1bH}tcJc4gqH#l9yEFpU>dV(Q$hiXwrXnZ`HD-;D5mzqa8Y~Z(Bm5%G7CCXC zjUW%ti?(0Bb)sD#oK>8@jJr}~)e-rRw7GN*PEZtT{N=M=6s?A9$%erUx*S1Ar8JnIO{q87Mv#Z+MLV|9 z7O`+Z9v~*d>`$+qd6f=1lXMMEP!ww9nYu;nEQo6@W8pluC*)P!bJ+;;@Vsa@UJ)nW zPsH^&xK{tATM^~Z)+k+r6BLCSWmm_EBDrwQ=wX=g8(6)Faw)+^kca0*dr|UM@nJNs zE4l(@;+EQ_l$c#JbPY~W6l%Dg+Ed3Ti1jO%0pS8G_3FK- zsNAnmL)YL0MWIH})_Czed=wDnVXgk^yROQ#p_O!kJUlPjzvoUA*|zltV)AEqC8&g} zQvO)ZR0Kt##`ClZVsIN7h+eQ(-*Z$|CGPVZm3w$zwA+Uy3N1@6AWlP>Sg3myWn}!F zR0Ks~rml?=#L3m!fJg!2z__Z)_pcE;K^~qLiOZuCM8WFYVEy3lFeBavh>cDJkHWf^ zT2AoAkU3CCb*-Y@zBW(g9%q7DPA1DdSgRj>vzjvfozMyLa02a#^Ag07sC;l#xK{s@ zYgJ|2iM@J@g%cEo+SDS0_0Dk5J?2+a@+A1_1bKK~w0kZ~5aqu2fVKL#V!bWwYE<{g zQN69k35r4uYCpoPVt95h1M&5$jUW%ti*{IOf~fFmr#rM6fY`I8s^a@)k>I&_6#jjr zy;TeCD<@gWYjA?1bPan;A9j0#Gw%Sz zJAXT2_wc-E54jvKGM^d)8tH%-KCqJVzHB9#=i*WLcj{%BEMwqT8$GC!@^2LzK^{(^ zy|h8R_$@X9G`yg^%5{okk1VB>^DArwd3av5J(tIcdzW#KlnOvJEmlN{ zNoUbDI6+aULA_Vl{|bo5MT;oS(%1;{@VsbO57{DmKESK~bndqY3aGcW5(| z3CpIm3$_vD;d#-v9*Y&tDwYO~FR<2fX_qw0+0UF(+9k(7;2uJUVXbBE&X45HJ2EId7mvcf(^w7c;{`+}Al}#s@^Av}lKs|;a@}w* zq)f0@|I6JYa@xOKbPY~W6l&1Sb(19F|LwVPUk4tbDC{G|Z0r9lEk>F8i zU-%j=`Uc|OR99fF{+0aIl?j;_sJxazyUzYZQF`FieW>TCab#GlKVwo=rOO0KucOH0 zOi(#*vh;Cg0g)wlkrRs~-#mtku^E^6ckca0*d%%)7@$Y%u$7>|i1}_fgRjlXtr6MQ_HK^2vy~3b2czP(W z5`Eo9kca0*yF`O6;^=34P9dCF_F5GAP?K zK-vBp%66WMN8#V8)HYe#0`aBOBe~m78$ljUpuG|L&Ts4DK6h(jt^Vw5N90kDTyzai zP!wuVsSW*Fs115tIU?VCZX?LU^P=r2sk=i-J+Z-ZdDY}%hO!-_sB}h+h21S;*=8); zp97Jn;c|IysEr^GC(s^x!y@vX!kr_#z>3_MzEfn6+@*C5PEZtTN|fy; z!&-fvAP>)rcHSwGV#hMv)p08jzI$BdGR-TcA}9(qu4jo5eVgK$uY>a_nCL1ed)Wx` z@Vsc7TSbU-8T$c|12lT&{46$KuB>Ztf}&94?}uwd&gb2Mz_t2A-+vPIuGt9k@Vsab z%(GT3IgYz*+Slp}SgWsVaDt*xBfYj--2dAf2>V)n0c-Vjf;>Dg+WBB?aZQRZ5WApk z_qU#uPEM8tJ6L3AAZs z8%A-Uj>#-4o^uPcoIK0Gd@>53N`4NhLQn@A{DyHAx=-bGeN%_ zlcg-I)&F<=P&s*dDZQOY>PXOg3e<2A)8T2K48QYy_TM>qoC*5<|6Hpt!&-g)%*o?S zING9VcS}l#M-g(LO~#fbC(x!=E%c*cWFo0s73JMl+vo?4WndJIM&NlkMp)XyTK$fj zU6uaXUg|AN@^Auedea2oH-NSJ=ciRx=Du^W@?1O$|4!pUQ2M}HeV^YdD!Epa*9r1) z0&RL@X0oh+wfZ|@G^%e!+h`OgC<--blnO=%q3zQRM!d>xGLCrhyhza4+0V853ar() z@+h?F{h*QP0>9ewFmjh|w{5JCayb(;1_&)=AUeV5;9ekj6x#Hj6Ez&f6F86H8by>w zTZ|)+&IFBRnk?O+?Nh#X5#>Q_l&(Q}k)SucsNo=Rt-jZ`yvn?6#*tKKg2rT_goC!v z^<8~_p2T>W;>i;z-n=*Q|aYWdeps{7R=P>Ggd}=l&(eJXZL3xp& zH}0t6Alkrr90^LJC<|<3;pA~9XjC2Eu|RvbQ9v4HFAzKmZJIfN8V=$y9Mz*PkL2M| z#u0jFg5D0mSB{|#_h8f`xq>Bw#-q@tm9wbfAa=u>hPLyL$bCN<-!wQA^v(iWmZja5 zZCj7X6~4OY8k83aS~-gv4&o$StFp&8${RZt(zwT&ptm*fjs@20|Cv2f&iA#Tu0eT` zpp~j3@RGyiqPI)w8k83a{yx${3~c4Dw5V6e z?ZRSX%}G1nVEu|T)uJW~C%A_bG=~M}t-=1gugoHDLG|ROmqL{JWA^JafxM>I1@kT!gh@V?SMkJUlNFjvdDH7cohNpTt-brj5}F@^Av} z3Y9JDvXMRDsCEtOEK0pqt;4zxPeo7^YS8}VFcv;@SiK!Vr>#vU4$%qn@Vsc3*b%Mf z%7?A+Sp_hda+9UHyX0ZZ@!*+r^JTKZkFK<%otiay(Vz{~`s}YC&F}!_VAhw#PhQ?#FL-^?P})9GWYPjK2VqJjZRRj~@64iNS#{$vs`7*IKua|oCd$3+dQN6Sn@)kY5TKy^p zNT}ziKkDW2lqTP}sRo1wE8N40aG-BATC1McFtR6073YtQj)c z2=Z{kA2ilvh9SGWI3`m$V}9{!^kM6=bi;HFB6!pkAcoY7R&V6R@shmrmx&%PPg$>D z9c&}H#@ zUacA19Edu%j);a&b83q{+8YQSg-1m-1MYO2RvlKegm!3JQ=K3WCti)(FTI$)QT2Q+ zfyVj=`$VH+<+Tk{8yE;4g&I^d!29!j7HMvc>e{Id^>l(fobZP8NEi{T=Ezb9G-jTk zBc5G!*LIbuY9M$NYEaDpJJ)wwQ-A+Aq19R9suSel1U@l=g*K~!^~-}sk&j`*{K!kI zUaGKx;8CbSH3Rg7%X*8`ku9|Ca|`JNc{tGeZ?+Xnc#?B|Xj2S*zA6yMf?Q zk#Odn8pf%U&X|EHJ!8G_`r1YN=T>F|!K3h~95v47dYh!41AA)Hpfox`9!|J{M#SAX zwPMpuppp4@e{svJpEl_7Q@ulN{J#8B+IRi6HW?r41bH|y7&H!i+N$oG z`xHviF+19c3B`tKU-KR^5IhPss4jt4-@9qjJGbH5>wf!nf;^mP1y^^#U-4?1euqIL zva6R^`+bZytxTkW;8CbSb&1Jx{lG5i#$G=Sc30C0@^FIJ4A5rC>@7-Fo~Z4q-(DxE zF5-0tYK-lWsCFLP3y!M8_x$yj_72i!CcEkcc{s5IuGN5uM0NAp@<1G__CxBjCP>Tr zt(1Y_QK->;d7|3%uZlqAzFbI*t{kkTTm4vYk25juXd?Wgp26F`*HEAPx&>=pEJgxp zio*JwzPbT#s`D)t7t;r8S@)kc?0fD^&|c+Gt9i_nhWLbN+i%R)3G#3P?fefC)w<6^ z;HZu_UnDNn3ejqo&zy>&C_F0K10DAAer6Wxu@KF@`8K_$NFJUS?dC5M)t76l!pxc) zx6NW(e2CU{aW0kT;!*f_+MgWWhJP`O4M0qtXCuhN3ADFdNmP&K$9>NaeKw2Jqk=V` zd{=Z0PEZtT(Ej8m%l7AHaSn)szu5@#@Vsc7mL#e*ta*~)Jq{3|p98h}ZEIM0E*^z{ zckE(b2wG%F1hlgeBw4u0wI(^MaSmLev|tReTnpYjA?1P=od-hhG%5UfqE> zZ70aX^P+trEJ2-j7xz7{d&Dg6e;lJdY8|C(aDt*xgZ3wfR$shXTm@oE8yi6$o)_)* zhvL;afw=E^;RLft$}mg|2tBN8aDt*xgZ3vkSvJ8@odF^x#72;Z=SBMid`qawH=Lh% z^srfc?cY!HnEXuF-~>gX2JKI7vRr^xcpBe+T5&r;9-bHN)9_8Cr`vI!<$k=no4aa> zIZYbR#iQ`=v_HAYGH0(@ln3Irogfb<&~6LgcJf?;1P!wv= z{^an@mmP5CKqT7<^6{k^ZMuE7b4LJiuV9A*y0nnfQV zGA7vw^6`Y(9i944Ng!LYB+W=|D>A5C?E#g3G(o~Xopo=r#734^Qp(Z zFpE5WK3h8t?5%5Xf}&7^_CSZX0QbydZtu_5sdj=qJTKavbH%9FhTy#Lg?G*3ZMPKb ztH%R$4Ng!LYS13&Cd(iwOGW{a_lb=l56_GCCiv3dqT~*s5p~rp+}^2H^Mc{J1}7*A zHJ<(4DLv|nSyTWbx1Asl&x>{-e19;TAFgTexDF+QYlL-hTR&Zc6BLCSv9<0@Fct-FRQYDDGmI8Hnx?S$5N9`aeMT>M7;pPx+ z#Maq*`-(iy1hr9MOvd`|k8w9bv;wQL3Lb?=^7KujdNIC5J=Aazl^zrjpZ^Teq$b<+ z@dWZX6ONWlrV?pHa8ig?Y^F)&QB^>rz^z18nmJ`3YB-2=`zs2sh!Cyzv|K9pI1|)H zfmYw1CsL=?!P*F~(<+Zz3@z4wpv8Kqy*UXpIf!>h8i{nRgSEiVmvs&DI1`SR%=jfY zq21Cl}D`wjorT|s(%G`*EJl(>Bc=okAs0)PuCh&R40!!L2VS6>9q3?smF=& z+LvmhtUL;PULG3~)GkjJ=^75g)3={E`#nJOPYBR8$m2{<8wFO^6uc=(b^NuU;ft(1 z3dc*bw}MfwCF^w!2eJF zd#P(Uh!O{4MTb9qw4T?W>Kf#6CLAr9`0eAxpq4(`m98(WJgP2eT>UFfy?FYEuHhh3 z!YfOEsok~TK4sAGo|A_YrQu4CT(d>pmgEAz^FjUQhzc#bYQJB~Y#?|PW};S&$+9Cj zNjf;7otC3q9-SZ$CvXgC#Y5Pc{Z;|c*qMH-n7qPE3;(x}f#6Z7L9H5?r5Cy&d3|qB zt;hb7Izb*zgn~x9Hk;H|BP)Q$^71>xy*qV*s5iTJ@PLPKa5unlEY*C+_?hG2?*9Qrg zKUM3N_QMSXk3tPsQ9xu7xQ@f!8B zYj0h{L2R3|P?|low)Mc1KwX18&IGlCOqPQ~JBznXQ`DgqgLQ&D&IGk;;QdFHS5p3$ z2h~n{Yy^2Yfur?#;T=L7{kt-MCRPwPOE*@_PYTgBh~QE9#CXG-@reqyWG2N}#NmUH z$u8wXH10v0T1a?xDU-=k{FGS?S`wr^|7aUqqA?_Fh0$0K5;RH$ZMfJaqU54Ltw~T- znR_^aHhlvYR=A1D(zB3>TJb5iF*3@SRYkMlRV7}#PJq3f;^lU2v>K$#~Sf*2DZW@Yxfj4$J`QgiU;dQMFfvR4O(9f zE##>pY3GsLa$4_6Izb*zpzZT`t*8^x6Etq*^cI1yYs(+!1*Re>3N>hbHMEvqy{TVn z%Sid=z41Ch9-bHN-={~4>XWhObqM-ucT*3N>gAH+(yDGR$WPdoNFV)JG@C z!}FqjA^$q@BLv48dO(l9O3}}9j)}cf5fp_Qw1(Sc*&SmRv6Eet{v|r-1bKK~Bxpsp z$)r_Py+}qKz3xit1i8ix;pme%xr=z6PA2DAb@8*(OW<Utj5KC&u^9hUyPwA($&H2pA zbMYwrJFUouwK6c4;R(d=c7i;dK>OhSts=uh97&x7BNOSH4^tY|J)~=Jf}&7^R%FAv z2^dMO3B=~#Z3KCEUbH{%iWhxiai#Y#c>l4dw4ahmj?^_cK~bndE3!?Nklir01jH<1 zBgn(^qWv}~L8OhxmENI;VC;2KfbuH$WL<+36orJ}lLXQB>;xbdcd8-kO$tylnI>Dg zhvyv*^+%ie31ZOp#X!WmuTScKY`k);&u9a|qyB(C)rC@t;-B8$KumqPK(v3~SZvQ} ztJSD3!ZyR8J*&m=%zctk&ryGTU0zb!+2gv12@BS^hZD1a_OHHH_#MWvC8=1X@a&XZ z{(X6nf#6Y?iE0M;c4p~KlI73(a^k8$ogfb( zcob?-%>Yk(+MFV8_hdN|W{hwTC$NW9U~i=OL&6#-?aLrhY-Nl*$YqRy;8B=~%6Vuf zt{N#CS6?Uh&0r(Q!wGB+)O{Z%Qto5@(I!uRv3TrZIZe7@x&{$E$^t~Da?!%?FYFtX zoU%;#zdR*>ygJxG@F+Ygsu^IM`p7nEp5=jj{(FC&AP*-NfkyL|F`{8h?BgVCTr4^~ zdnZ@z(8oaVDAb^u0p8at(Ne`@X_Qyhd+G#vI8g~SrVLys3Y5j3SInMC!vA#+<=)ti z27*UTgJ(A;V7<`BVSjL&<%nqYG^aAWVS59?qwuJxW`JF8K2Mc$h8I#EWNE1r5DEc?LsdogfbEB|)P_ z%{gNHLocOpslo<=N1+DQ3?@sKL*2!5xQ`>l3h4xSIN=K#*D7og_Y3C(jp}cAO0{!# zR>ri*ZXkG60-X7?s&OLgTKp>en6m3d;cs1($+t5b2p)w;<*0E2-*1$x19~d^2B*;p z@^HcfG-{uY6Z`U|0gZq*{YBXp{gmJS2yFi zc;S=g576+r;U%Q+V-$}vkp_ZCp$63@@a6~ZN2Yy#%3pQY=mdEjZf?f%f)u ziDKoIB5+jmltto2tq|pMiOi`8io&C!b>h%V&5~2Z9t%;%^w_5NEy=_4qW$oCq6pvH z6Xr7%{caZP;zN{sb8`uvi$~$#X$?2@w&AViejvuIvJvFr1lmmwCW>XzxHjAah*zV6 zmC$@wbPY~W6l%~KZj;4y#w?xz5uL|Ikca0*J0=va)l^&?-WG_E&wEg^?>4Ng!LYS0>Pm|p_#g!cn+$xe`m=S6!V?6>r(7p@HtxdZQ= zefudC*&LBbtO2ZVXDjUW%ti}qmHH>&XioE?1~TH!f1cU9ixG$}k6 zkHWvx8g3{VV77XGAX4lEc{qXgbJ+jt^ktmo{pV@3DCO2d88x=BuE7b4LJeBO{c}Ep z0`nPkf;>Dg+BdaL;&=kiRu6>v4B79yD_xVSrXnZ`H5@Cw%fdYUia=Dh6XfA}(Ow6= z)Q$CV)_oHzyrWMnuk5Mkp=)r0qELg@aNFiHq}^6tS!XB6!}FpY1ikH&opJSmeLjN% z^BHsvPEZtTI97TG#zF6HpNq24PLPM^MY{})Geqyibrbzy{?*yupJl&+y>$&vP!wv= zI&qlw^4KhD_xUWhwG-swdC`6Y``p!BgZqBW0m7quihT9)09}I<6oneJP8`0N4)5r* z0P)mLkca0*y9w-u`1}RVDlQEDTJLwNoMyprU4s)8g&Na;t|p%fXI>hJ9Cm^{JTKb$ zVUI@NbvW0#4EzR*SB{XoxAoICI6+aULF>d}4n52R2mqp~ogfd-i?%oHR9I^t&e>Mr zT8%6=Q9he*ysp6sib4%qCk`uk;5?QTohaY46XfA}(asDzE5H1bKK~v|V85$Z{|HgT@Oe6YKh?h@9TRx&|jG3N>h* zxXIEEW+#X-DWa5}AP>)r_7GTq?w1xPPwcWa2Q!3l~&4afTP_b?;H1VlbN zK^~r$#+GsA8|-WFA)V;fwxF1o7@}|wC$Mk0^wDZDt{4){CzloD2c(Gkt%4Qq;RM=r z6`($U_pfwl*8@?+Gc`d`nCYF{TCwIWUfoWY7169gZMk$@psqn4o)>Mpo3L9?(=XE3 zr;_|WFf~C@*t`E{QiM2_4xf_gY8SEo?+J3(2LTHA@Vsc#Qwe(~oO~mBB}|cb4opo@ z6!t%Z4nzv63qI|;%M1`DGDgS;^Y|&;!}Fp|zb?v{^5gt2QPTK~ea2G0oQ~ zA^pG=ZXXT}5=H-1Wwrfqg?o5jw5gPUafVrEq;>g@$Qi#3O+`=?z6Cm7DO%J`kEPFt zYd+#|*%Z0twgC$F@VsbKX$SM$cO^>giaeCN-szu;ps3}bVQv>Ao(;k^4cTXP5v!|y zmhZavR=9`fMVm@(lO=1n#nQlvX_a$ddZZ#KYA2Da1YOmHnj%eUD4lv z?ei=yD}Rr0O+`=?&Iy>4W3xE^wH#-XgNS_zg6kcb+5ax9F<0yp&nt9-bF%M;mT#sCqTG*k&|cv$8ho)>LuEyHiny^_?l+i+#w5iJ!#Q61rasF&i!k?=#H zaiV8`aVBYu5?&!n<{q9GZEE$yOu%DfrA2rAly@^$ry?k-Bxs~8N)VFHDiMJaHtHXctD7jof#mT5*+5G5l+l<9I5DAb@aC%9HUmWgU-1C++Q{0*Z~oIso2q`-V1?X<{u(p!Xe3f401 zS|jz|D2vSJeUe*Gi-nDxUwIK@R&Pt$9YHPM|0a42p*Mb z&I)N!KSfNbfW*n%H|32{hvesD+iBdxi8ObXOKaLI!t)yv*;ieV|I%*AJ&UzA5IpKw zz*6bKfd-K5R zRARO;skd)qv8V$Q*ZZ%Pr+>#1<=$A#Z9D*o$6n!b;9|2fy=OB6 z!J|S~OqbU6ZX%?esPWi)o?PmF4rN5f<{I~KVsFLi(v;Fo#FH(k5j}C7oOO6!rD2n% z27*W3-5V;Ed)7o$Dm)N~d`rg46>GXEB}2S4?y31PR9e;6Q?$9@o6NOMmX~RSygKlP zoNt4VL4y-%uT7T%wlxuH^(Qg7Z7;dN&Wm!jUPgjP&F&N?soNWiuQ7NBm%hj=|Jfi= zUf$DR;~q`~ZU~b;Wo#m<*FoagymIon31j3%wT%RiivK%Y>T;`*ke}f1{K|#ZB1^B8 zV#4fTje9t;cUQP{ziVUB>?9H`XKxnmL)M7GJ&Xj8I<;z%^s#Uwv2i*Q|L&|TCk)*w zr->b^@$1vD!DOk!QBTn~t#2|vCuor>o&vMU*;V9(XbPb)z)>QV)}3~d!F$Q^1GJp??uw31r0?| zB;LVA>34}6>lTUV&cPb@aN?C%Cf&Arh=|5WoVLab-{aH8lxaqSN409TO!}j-hsfuH z#I{qaD6?UPnAas(;~q{NU$av3ZP-9~n~-QM_KK)m6NPo6k>F9MMz55DQxwslA`%1g z9uWtwjTLD?gL^o^>v>q?Uhq%h`F*em9d0Ce6tCxDPHK*xvRm$&@_~Q+4H`VE+Lskl z;Rr=knu|vzSL-2r-K{RC$vsZv9!~Ij9`=--wMo7;YLUFL#sCAsqj>#jvSf&lmG9SC zC{LX;z)%lzg4gp)C?o!&<|cW_Sqrt6b7vi=aStc>`N4CwV2QkMc$~bnt*?RL zQM{fvS%&|>u)m8T}0ekK%0!7-8|BDIYnsOYRuvYiKiY z!g%IW`@Bl4v1eujFG6TI~St?)m+<;!``$RExb z2_9vv&u8uNms3jrDUUwd-_S1M8pbpCIa*4Ij=w6M3uvOv_z){4jIJaK=Ioc;s+7&{ zG_-$mg_W^V`=V7uHZS~rOj{3oI)6JLosb%8+{1})nPR1*|5O%po8uk)nz^*HxWjBQ zC91K3;880VY?PvgRud65k;r?kv=ZHEj#&7jk%8b*U2?}tkG5BX5m!8_XKRWm6Tp`? zmwnSh;~q}LELkrt$Wu#1ltqmbw+kzGUCPN&Nb&DM`D_`?9 zlzs2D*0_fgk6hMCJsZ^#%c`NqyB>L!Ml0LOQ`2}E2p*NvWu3HPa~-j*2VN_`{8^Mb zO^3-@Key4ihZEM?(Nc%5b;Y#Cs8RN9cBTL80Qt?dmIi`H#Y~Hqy2aKN2gjkt!msI+ zAI0X#g*Ug=xQ7$>Gek+V+PaH2{ZXTBk&MdVzry9rajgvmk80}^C56v*7av!k#7)Hc*PbxtJQJ+dA8=2sdt6Oqt!>oc#P4x2(zOqD#pp4p zp&ba7BS(A}UaG(0sCX2=hH&QnCd++;euzbp#%sli{CQ%el+$&^qpPSfy7y7JM8>xA zo^QT}tIMPKorSpuamVBgwc5*rm-rg)IVbot4g1{fxg++s%PQ)$57zkEejd9)^3&>w zU8Q`I`4PgNXCBYR$hw6@Lg8SIdpOZ7e7!XAc^xsO9iGRhHZ$Z(9j*&6s5v>oqxccR zH!VVD%F`C4i2NP>HSXcW?=RL#gHO~E+8?N~^87jI-_(>dj`TGUJj!_HCqG?~^C#Ap zqfhiV{AxMD%NSTiSNef?Kl+Zes7YTw_`Q#;Y9U- z4bs{8TB2t(YMeNHU9OxzpL}^se*?jzjAwp5DMg;LsDS(t>T~Yl1TXX99evX$B5T?> zX$kyl`Pmlex=AWupr*+8s&6tsLYOd4h?IewRIB|Potkj@eO_5L>&m-yn z0(t0%aIxh6SOdYM_z}WdvbBrlZ?_hUDI>;e+{20aCpJn|8`l)G7NEv~5%=YDt^XE1 zcK0(7Jj!_Hmm?p^&CTz`$(cTeT8$ID4u-XTPu_@}C4!`WRf08swkD6w(z2*(;@s`N z$@~amZ`}21<#ex?NZTJy(hMF(#Esb`U2_~A3wK=3GjgitaZ zT`8yU+C>yO>!)!KCu$_cO0TtQBK=I%`1IqMeC%edxajX=Ab6DV%c7UsGou0QTzyDc0z2Ve9DqRyv^^YaStc9G~Fbn zl&&J~PDhOsZ{Nzh&UlFv-})E`9%Vf96`wxI+lqA+J)8G7wE8%~TeYw!;LLRL?^`<7 z&-`?f#?N;7x6OKMdC&R2$@~c68*bk!$PKQPmWoXb(zu5cYhG^B>0FiZJl3v>l6Pl* zT)$~$)iCU_i%!@=S`MTL%+!LyA75$H|%X7c$D$XFJCUD z;1_SZ&NlW9keHCCc&dJa$wEXoAb5X(6KH$x%A?aTa>q2B35r4@R~M6=c;5b!j7L@J z-5|ZkLLQzMuLAzvc6I*~!K2XL@#eE+BQpM|plfuS6r^zvC-~ijFW&wKf=40kRr@cy zMkYJa-L63%XX5_&Q+9&R!xhe)=R%vJ@G4*??8CvEYWuZ9f_t0^q-_MwRX=){ghDDOd!l@BH4LVAG)*_-gEBZdDp^KK*Dwh z|7K4KCwLUv>l@9o6H)g2(H`yx_i%#W8yKOt6CIog9))zExu0FbnIMldv3Y52r}Mx& z$aA4hQFwls$#w_n%#q+8X98&((cAtT`~!ZqoIsnRjQ6|`Be;hXXkUSyuWcGV?1bu6 zGVmz0b2Qsk-*_H$&&k6HJSY6S@t)I}6Tzd^qKmr z_c+Rufy2Mo=xTpT$is=1P?tO_#onV1&5 z+fL9^f>)R4LYt!SN@J#9-w*OQ6G+<#uPe6t(jCeYo{LAJJ@v>bJHelJ z?%_lPTm>X-_ngig%MwoTD762c@|T_9^*Q%&g5MkHpW90xY-MnQN8#T)D_@*6h#-$M zkuu4oraljz3vG(R^TSNWdroIg9%lk+8$oRbZ1wS6Xj2rwPsVm4_izI3Tq$|%8oZt8 zOi&aOp6XWopTS*i|2%d?OMHHttC$ID73M>vJuqk!?K-wIKl4?>?6$XIrne^ zZ9FHN27gMN35r63?xv&FhiA?`JTIOf-5dByB|CF$Q&H<4ZF(bsZASdN?Wj1xJ1QS!O<(i-v=jnl<}VPvIO%||AU`7{vG>jsY@U3aVDsr zX#9O}0&R-IBXsl!>1oGb6ndzyh6EiU&*Zotzar?^jT*eX!d%o7LYw+gNZ{XX_k$DM z<4hp^>)#;dLYktC_k&vc`|M9U^)vX@#b*rrQNMn6$>U5odUyQl@?2cxdT-_y&@#=@2T$x_c#;OZ*%k}sLx5ckftc({h%|) z+8Lv$rH>lCeeEc(oCzuckfy!}(p1Jct`+s8$m2}>XAR2he1xe9{C%K0Z>v+^0BQWY z?dsBbAi+IIa{_G}G1#eBM7fZrC|;vFY6IuiD=*to4_oK9r`@@aLmp>>`V6VBE)kfQ zqVNc*2Vg9(&_gYKBzXIpGW~agqKq1Nb?ewiSg^LF);4NTD;(=!{JZ06|J_b-4<}Fq zX&ccjeMLF_y%%n-_SO>R;!(!?LA3$awRlw27UfqL|BkJ2V{JeLd7KGH-++GS)ZQfm z^HLO^pQEMEpG5RfE1Vyp@u>bAL3&0FIx2kH(L=3N%uDTE{5!P(jrW{;IDxjK)yG?w zyp+M(fJRTzcC-NaQE?9^u=b(01nfulrGlL2$#b_EaL>60-g6qQLz-$L+j-DfANO#A z*N-r3(%z=-_zMjh9YUH)SI2LV$`bNm6cP9wa7J z4Fr!G-5Q>&f>qR_Gw}UK?Pizc(Q(nqgC7slxQ7#XPSgW{U1;Ik>X)-0O!7(RXCQb~ z`W&038^x=r+cKlZZFnp5U~=i?lJHiBdpN$9m|N0w2p zmI+k{^>1V#cocs^O_oY+ODg5+O;x89@-#fVoOrZ)qZBZtntIy}#}ns!7FSNsyrQmJ z>#1=MCszAzlswPXQ2i8q+6Q`PQSy1*RBt!yY#?}4zWf`cE1lic&@!ko_;gXFR`WvE zXE&N?+{20Anr)D3=B%YI@bf!)2^nYqT&BJZk32^-}q}Zt5jnquP+%%Jdtu zwff|i8uxIbY>#zPoosc~yOr^%o(AMr27ZyPq5ds3?%_n}<#p15I(5`eo$;s&^vtf* z&<0uKGPgDmJgUcvXlYI1x@wq$8aq6)D=A|JS$7w2t#J=0+;hfALz~o9qeh^{iWeD` za~Ee>E4j8Y5IidRW0a)Tb65M$N8-Ws4nXf5S3z(DY*)#gYk!b4IA7e$R;^Tx|xXJ@y@!u%`l;l%3|k8Q@_9;|T>Cq{KxD-HFN)%S;yIF)or zjA%SWE$3?_c+{AhtE4O?8>mmdBeDI$4-vF;YI4PolQdp_c#ejCf0mkRhZlX5`EMT9 zBD;PS*PBgEK4l8lxQ7$-V0OZ{4>i;og;8Vhr&V&ze;cYF6Z{MWkK(_17~h?}TrQ_I zR9)T2YTUz#%vquSxL-qEFb_4%Bi_iv%0#N0@ANScJc|E3ov;5yDZ8dM}3b+LhcLW^8c}CvLzj z)`lLn)Q?s1JaUg&D)+uIQ?1fytbyQB{0L#TNZ2B|%HdC!#^)9O|AL!5Tl?h1b_fi+0ylM=Cza{0L#L z+V_W|YWnQzWA|W<#_nekDLo9=HQcoaWEljUmlDYDt+gZd!eU*jH5@R|YE(bqpIAE@bN z?G)f^Ab6DV%!lQ=Dq3EYRGb6JkG*TTDCzYENu9XZCz$ps+FvLS$)fb5aGq;J6`@__VV}dpA;l$RQi>3618>zvQkhn8*hnSN%S8YDX zNbsmEv5TZ1=NhW=I-ABuF{A8K^-up`je9t8KKFbnzi(sp_#q@}zt|*B+*z)^0}W2_ zsMfGXF(O}MwfJ)+IuBec&bEkDGXw-{+{1~Q7s90UbDO9aT(CtJkv>YiI1;UH8)zhW zRDqB%$^G{xYWjLe%+0k}q*UId%Hv?xB=d0Mb=w)zk8n@5XI~`Vom(oN2f!EI`Wp!z zHQzKt%30e}O$b9`!R%RL`mOD19lv0WdpJ=Q))F=x-c-f4gfJsMU7Uz7@Lr986^ESQ zQH^2UW$~AuDz3YPwPbVC$V0Nvux|J?))gvYUo# z+{1}luyWQnz*EJQv#=Iu%D#j`mWXzuT_N^L1{0e*N`&bk1|`xp(e1=bX9I(?#^VAE3b&!Wq~Lw({{M`tmZ~UB(hjoHvN& zMn}1b`+w7V%$q%n_j+?&Iv(f85LgRas7fvMo609YJuFqb?k{5rCgzNcRc#95=b4=hl21b-^0Uvp>RBC4IA>-N( zpRk7eW8o~OFALCM3*pRxwfTH;^HM3*-$TX{Ok_P>!<}(~=Z9QckN4}B@n4SZm+r6e zX9%o?Erc&fo?gt)Pu?pgH1QYdL)^ZJ&fiI#uEyFIMGk^4?uk?0KI7479w&sH^ zI`Ws!jFG0?3uoqCOyC(D`fka7zTwliQvLb;Wn5dQVQaaLhkJ{8^;~g_9^##2bj7J_>dI#{^pYy=4QB|f#n!xoQ4jus{Q&87l^M*ch6%g|!6^ji+uLvYf|cg0Wy|gqV#+M*D+WSM>o^< zE29+tZD5jA=VbsxU@dGRJlDD;@qzzsk8^-JW=q!O_O ztd`h9*vWKzz#Hy7A_aO5l(7U82d*Y^)6Nm`!An|?R_57!JFnG}BOk~RSPNSSQT-n~ z`7Z0@q$6>GGL~Rsna38ce`Q|$(Vv##;q1eIc+gI=`Vh_#Sc|Rsthb~2df)A&o$49P zt`8H~j{^I4r+56=-~uUW@<18ac5}TX_*#jR*kzHw23rW@gL}vC0m26eEWyP1RY@Fg z%8B##(R$eK*u&TOFkh0r0~rEqVGH3jlJR@^73bzllP3nsSb_=cbAp+{I+UL{rlIut zZ8$?yZVZ%+gg}PCTG&GP-b7kep;P)YX`|l&8DAHB3Na16WJ!B6QFBfc0^gC| z_`+FZye?F9TQglV{)V$=(u`0sHh#JW?*QRE8DT2;?~4=lmN%4`Gfhq0&VKR5`84*wqlYt=t>z$sz!U@`C*Eu+)BVsh`8 ztyI88$XJ32d~cv%eJv(#mu;n5l3mt*qI%IZAT*b~`D8a!c{JQXEWyMg>u(44 zTa6J%y_^KZ9w4>=F}56mwHj_*T{0|koH%CW6d;VD9#%VxrH@b#EWyOBu(%SFPvgX} z<&$YW{yzlPx&hw{&mR;ZPA;WoZ2VSCY_Hl%UtkPk2`2Ep`TrAGE2L*VjmPc@;?xk@ zR=byOARjz`iM5BqnP*6X36o-H%^ast@yQN4m$VNoCTHf`Ne#Nd7xEB+wT9X8nn%SU z;<(L}I6d<@8F+7|)UI%bj3xF4-89=wL&TL!r)%()U_|w(%GX?yAkD8cl_>)g5qx{i zi+;i4lS#CUN(YSjc~Z9Ya&{O)Fk07JXm0ogi4z7lG;WdPnbIN}?w}O{gK~3>wG~Sj!^z zWyy;R{$hrP5{m=8_^`0*a=ptefwgd-2Cwr`AAWeKr5st{#`Jbf;E@J#wKLw}{xX%N zv+StFS~pVmJKg&}NF2H$1gshYbGsGH?WHhNV+khk3=Td3AXGq9hf#|OtcB-e*vXX5 zyIo+W#x=yF3%5GVqCmVXo7=Gj6KvnD@w=D|yJIV@gi(uUH9VrGE&Oy~+S)Nr@!Z?2i1eRa|k9pl(QU_+APcZvn0&C%!2qL2}`?$coy9)?B4`K`PmEfB%K$t@r zpUV+gi!I{>tOk={H3$U)&+Yi$;B`e;M(wgPumlr$wSzNKt`(D_BwHzqkC5>`0k_qP zkL@_`d=Js@G2Jsf_*G2I;C{qG>tYEe@G1(u9ctbi+A1IFfeEaI*LHYf0ir?K{lK>4 z-pAJCW^3KvXCU-MycfZB!ezrw2FmDHW)&u|7HgHwhnakYnT`BZzcaJETm_>rb@Vq|F=ln2`mrv-@~81bUzep-(wn;!CB z<4dJcd%80O*22&2;0yoyj4zIUCk=^rW}fIV5wL6x=e>jzYlyUrl}X?D!mqca+jcGt zfwh_jt>Z>Kb`(X4694Sc7gAD>No|L_$So>u!V^ z>!*jB3d6*~Qpfdz8b3ug|&m$NRk5JooW zDt#&Dn4BU^;Cv={qCeD1c;nk#+EAHe@|G}BZ$~nB?5UmTvx!=jT;ERkH^oqTF}E8- zU@e@NrApoZkDZWa^H%J$wHuRvg$c*$Dcp|qwqoWATE@Vp_JX`GPP|yTGecl4&8HMD zz}-eX5kZL+E$oGtE91nLf9x3oYq6PU?mxN-lcEl4itn~(GS4uPYMsitK5QwL1ko}Y z{_ZBs(L1Plc8evj7S5oAH^UsA1kFWtr{IltGS*6l>&`sTlF;k2gaHt1g zf#14-wtCS`u&Z~16W_IG&S$^`K9vo=gt@VsKuWK2e;_vuOE9q)&cFH-XhV|b(wbK> zuos3^T2CGsb!G^xg->OJOec#@!tfhOq{FSwGL~Q>zJChW@<&^8E0&hwyw_HU(bMN& zG;&}Fto43KGG`awj*Qz%37@DILg>jZ{Ca&NPph27`CREhy6>E(!RM;6M0R2;AwzD< zryX#Tu>=$NdjIWC=#HX^sez#vE z!D>$szo}4=u>=!W+_!M{jqORUb{*VT!%DE7zJy=UuRB9vEqp2)tT-zy1>e$D{GxTKJT1I8E2!Gry_$4&SGltBfU>czr64vl9rhfE5nD$y)M>PhEGH z@BPz-A+Q!cr5jG6-TZ_L_CgCU|%@kh5Hf?%CjK)g}DR7A*vmyImLpYvJ>^A$Jg-KYYw= zgi2LAGbeLnq8)hdp8fJAI?vs0@WU0mnF@X3i5^SNgQvjk${<2L1#B4|Z+Y|c-A#q0 z!|dtbmU`Tm4Bh7@e)av$=A}I6^&}xYhs9FDg*K zD3)L%2Rx~7KaVClPpTJq&z*ak3e!%q1lEcN|L%ZwBZZ8L$Ks#S?5D z4QqH4o!40f1b8MY5Ll}VFP0<_yTDQs1_YL1Vi`mM>|PEex+p*h_)C^%7xUC#f(fjJ`!VD| zLQi~{S+jy_^#pm-vT_4HhcYv&I4IB)05YXs3DB& zF;K=5Ogu8Esx0U z)I*Q2g|(i5hu-znKS>0GUVQRB zmcUwx;Ool_8AEiwzGjv=B&>RUzJ8rZ8A~vMdkpxgq3^yQS4tK@zrqC8!u=S&s(adm zZ*x1FFB}^tW7~0CO##pSf>jfUE()*=zR2!AXBXdM&_o$aFflm0t)|A=AfgVScd+?Y zLq2qR3h%XjGDBdkpXx4}L;Hfs>iLw|YWAL_6fNK#c7@AWGB}iIj3Psbal&*BE*s9- zwiQWX?XJA_kvVJ`h`{XzZ-zBINwO?@@dxhCW(cf>TLkh&6E_es>oa*dbFPfFUO@cR zvSTQr@l)K=@bw}4sigAR1Tq4muULYKi4eQpHXxMH*e&>XA;LTI*f}m3BD|QuT3aFV z>pC%%(8w>uYoYJngT8wT`Yv7#@O1Y#2Leklf!D78 z^PX$hC`@22yb{6lIkeS+|FIfi3-OiUy=|zu5!`dSFTv{v)?&*z4);70?)k8C_Z;76 zyu*R~U0wWx?+Pmp-izQ>2CqI4T>`7FgH@-?^&17qbsT%r!`c2KY#dJjZ3|Eu7mc6+6 zD>_1vV4@4qAEnJ?V`mzP8}U(1UWYAmT6vu%u-2{KKTGJ9mfBFF2@si~Is!{D(R1Zs zE%A6Ojr^3Pk=U9f2j7xcebdOJv@o?=-9d;;WsGz!FR-Uq!9tMQiRf zuNG+&aKNegwP+bjFhRd%+5_6kdmtq~N7o{4FZbj^FGtH*f(iP2<-N^h;%rK6NwXv# z?Nd45;}r=-i<$OgO=a%D9tAWBxE$fWHofwkO#=-*^3 z$#_O{CJw^)gf9XS1_YL1;xvr&8xIr7uyJ&p&uUVW^iW+O#qdoeOkgcKGYs|KN<8f7 z?)A&RYVu&58)*t{g|%=Qc#eVR+Ba%a=;cQ2fWQ(=bc1>tI6SW;Ww*tcqk>7q{p@sCiSrB%*SoGhBWB-(0PKd+~=q&XKVM6ZH3#J;~&2 z7$utAP!suzFK-$#y8@wT4S_Oxr6!ZbRp^|!0KV^>=(CtN(wiw`3BE4cW*So=!z6f_FRAkChR24bs|n#63W zENuPl%@A0Nj)Iquwvm$;h63^Jtp)MeTv@nU>a86giUbq%{y(gZ&t0oDfOZoAQeYEqHVhOGXrXdSyr75{3n+k1K zdse8qQd>%xduEbZXX!ovH>(!;Fvv`p)!I`#UnvsYKJ@p|@tMRvz!Qk9?3yGku8EN5 z>sEnKwCMb}C2t3De%=#^n+x?wuVL0gjYh6Amf-8s-;);XBsP6rfN1jCf<)G6Ck$HS zSbYp< zgrY^Sw5L-x8I#!oh}cPLa?smNSiYf&mQW=4y0mq<5jo_ne+wWyDyzv`Aa<1_uvQR^ zsJhj%h+Xw2KpclvZR54x!icDbGL~S1_7dj}S;Ta^6%bYDN0JuH`w6Kt>oEk@3V(9sf(cw6yhs1DCOH=4CA=PG!Vp-Cj@lP5b4eR#b0{P5 zycH>2=p`5qH_`S)MS=-B(jGO*C9d_G0`a9@74kZ3s1U@}UhZ6+Vp(_;#MT_>U(CG!lX3`BHoHrPfjLcx+ z=BG0fmf-8s-+vwzkiJ{40#UcU1?m2LvJlxdrvjm9(fTzC*+sgz=K}HH9W}A`pCVYT z+^Ma(BEi?CzZVzpB4dsoL6bZgA{aveC5lIbe287pzS|r0KN;q-pmWZ!~ zwdg#yw@MMQF+gbv;U#25VoQn)vLG7NVSgYOn+o z_^aCB7lkL5V+GN|%X>yl&9N4piC&*8AQuKKg|@mAQi&XdG8)EK7qJ8r_&eP2j^ZjS zvTk9NU~6-pA+Q$vz3q)ZOiApADB)*u7QyXr}mGq#HSoJY=f;|1lRUm2|eD6h;>=Uq;cvjp<&-CJeJ_= z(*Fma+eH>7WB_sWnVPHsVo5myYtdb%K~_F-8Jq~jfKO`Tk~>5AA}!^y1QShQjk>co zpQ!pJ1JMD#7q)iZEFsWr3PWHmdZqQ21;n636cB4y4I;M}&k~yaJ4s6@5=?A{Z{82I zE+D;oOap@arzR!-bA-qSUJQY?aEriKd#0($mq`&qeP~@Q!34ff@I|&#eKPjZOkq>< zTpqWm_V@i=2k(P5~E|6(<{Z5A%* zJz@#0g#R}f+4ULz0&@>`Q&1LDG=uZ42W(0 z5aHs3Q(8iiV1oWOc%DzZQz~0Z#fM zzPcHtZ|ey_xIR{sKE3bo)~^FA5Q-LoGG3fcBge-D1938{I=OZ42H#X1FJKA2F8#eC z89ZJE6M+coVoIcD2YLGj6DtskRxc={)8{F@L)$Ir}P&AHR5N1wzsCgfiOsr;zDUl(2y(7Qd1NzHx`?0+!(G(%(gJ8cvIE z(}C#Q){GRSF5>H3&a6NvT3%4b;8w{b#hMaFpmh)Lp35)1JyXCEd|mo`yJIpD<7WU7 zcT-L5^84|J_Rpz6C|cUl^=&I@nmh}L-%iHFw`xDW`C*p8T7AJP`PEi(zXdHrzk@L` ziLK5*h@2~62`1=oRo)g7q?!Z7P>5z!YFLABHmf3`Xmy7&c5d53_N|x+geg3w7O#va z`)s2HEWy{MJ+$T6MDnf|tw&K`HF*w%p&d(LEgC~w|16QrXc!2@3-EqC-4RUuLZbyN z!33S>-TEYwM{6mu*JdoqXkC{)oWv4XONnfGFQh~&L^DF_q;qx7LG<#if^V13)dm7fFfp(! z2Ki~vWFT4taj0iHX9@(CV8XX7GP==u8W2f9%m%BhfWQ(=@MY0gav>auEf6IemzvI1 z0Rl@ff$xnfbrqCxt2mwebDAZv){U~5@MCYE7ht+$ZE~wI~5gFhPHR>a>+?`REVC4|rDdYE+5OJv+Aop=jZe z2Jdr@R+E-6YJ*|aVhO%39{-R}4ZDn|Bb)H|Ga{Lp0TV5t9!1NylKs1=Rn-#ukyA%G zeth2u0ZTBk4C?W9ZW1|LnXYPfFekgo?YP! zU~tdt$pL)NfwP%e6cf#$9`m;+kwH&=!K%vD+LPx=3-}YgX9`$?2^xEeJd{khVRSuT z^Ic8apI^jpGGhs>MXzLhB$+IkPYExGTDEJR&R>{2Ra=H4!31r?75}7=8%d*q=nC_0 z7a(qxBd``;AHl0e=fvAZeDb16%sPmPEl>}=A1Q?UGYZPMzClfTH$TYNYc!D|uohnD zA=mkdn%ML|%-iOLGV3`eT0=c1JWD0FT91S>TEP?KZ}nxqpl=XEV69lN>Xc0y=~iU~ zwW^;pIq~`!Z+j|4z!FU0^5ALvT00_l+s0d|rwX`5u@-Ix$gFYmAgvxe;l0!Sg^f_U z+B2QJnlPB7^VPQxX=G8`p&Tw7)>;@JYroy+H>&~}t1v-liN8CtGgE1{$Q^zZ?62^( zuom08x1&8t?<$pqDhEah`@pLICT5Ug`vIISSe5-XogCWX$zf|@zJh*ryZKN4eDWBk z9+<#8PWVpsI5p9;|H_wE8p{%3yRu`&7Qz$#sdSP$$XvLeF+fNJtE73`Nu;+6*B-2T z_a%eONpt70wW`#ZFKSZ9sfJ*mF_ftXCh%Tcl{yNns^+c|Viyf(2&~1{!=yBZths9= z7+fbpBv^HO%ntJGMQ5%vShe8(cCyR52ZyayrQZFaCMoevg(fR|GWEa&e#%g#{+wq- zI$o+PG??nn5LgRa2pK8oB8ck|BIwm`!#t&S0IT@S9pv1tP8_xtvOmA6N%EvF!t61f zn0jEM0odPBS#AhjP)1-YhjNC%q0$L@*-i3 za7Fm3^>Qj+4ot8f7V;J1YOTi!^}c>&{3V#6);2TGCEJ%#FHTW$Bv}h%yit=UJeFXB z`jCrX=#W zH_8a+lH|Am;m5Ow41u+ux<4^I*yz^9xCK}$&4S4TDU4EkFFyI+#flQiI`x5THEJp0XcCX8LTR;>OowWM+@R%mDVanf(abc zft`MBSF&vl?AhBGFa*}3_x!};0&?K!b{fF~59Idi(SrY=P$w+G1YM*4#=L5P_v1dq z)yD7!1QS?`tw%@b?fQG7g{fbvGFD-NZL5}5MvwyiNMY%cHqw6RahIDykMr>^q2+}w zC?E^QSe4Lm^CzZ&bhx1|fhaPpgC~a06_ReNB`m=NEl-JNf@j+h_KD-7ggRGliI~7z z)VKIIYPr~A0eRAUmhkZYWN9Mwp8;|CWI|zl39W7Y$_3={swpMdT2*QQ-1D)fbA&am zhBNiR1g-hs2=9t4Y8XHOODM8Tv%(0akTBmqQL%=xJ!pEe7P0qia8u zU~3_739Ry#1BLZf?pCOWLeLrRZw5pwBO~JeVuGN5;tE4xEo>q9MTe@%x`D8skJ6L# z!KznwIi&hlm4@1`#9#CHsVwQwt_ zQe}wtl^z=*Ea+vx=!1e+J@%kOS$_ zZio;w&4?ke7OoTQ;oyEmu6Gd(-dQs_MM2Q>m8>9Ik9SLVlGpk)r)cl>6w)|=2%U`E z$XJ4j!O-gcymyd&*V_T{@_Ic|ySJ0D(Z!A-uok^i!h{{fJDU;HsrO(_(8BSkVB?I@3o!`fwkDyZ3dYfT&Gj~7%qg#lud=ctK_=Uob*b3Drs*^b2+cn zbRmmIALG|P3(;n)D-ujlA5gc?Ddf&0O3VeH%%ST0`H-by41u-S{M|7jYO<$e9$$a; zWX38?uvx|7;JGXH$>QNVU`&oMCaj?zqis^i)qW$uDigQC=!Kp&t1&l1S{mQ9v|#swRUzPv(zr zpT!VZi?!-$Z#6Nv>dX5;COMX10_UVdb{ov1Z8m!HXG7R*bxhD!7r$&JM;_9Q-va|i zkoL=+_Aqn+OaV}#6dQ0oi4U~>z0uWfwfqxG~gFqJF6KV6c?$z zA4+yTCh)rsknaOCT!J|q*c3RI$#&A(0N69rv=Vdhxqv+cvt2Vyf=Xf ztVMq-Z`!~*2s6X8|sQ^9(^iZc`pUk}r+&dR)mXE+YoctmL4UP%sl**0dKNGJ?^H>5_0Ty8Cy#J6>SA2+l=1-zgb~VpkHM#m?ZRgR>b7u z(0V9-2AnyhN^Jn6wrIJBuy{lrfnH0=BcXRq$y=co{>}Z!zG6h0E$c2s&SkR`u*VB$ zk-!~d)i> zU@s@@fpmxQA=j=d9GW+hiRxo-E$d0`06UqjpWpHuoP3!WJ#8PwPtAIS_rR!ifY`*2 zaPjcrpxiL0<>e1~3g!t70Z z^Bd8E%etf5cM&5U0y!L4rE}usots5n^z|W~OR`^U2`s?`jmkHz zX&JhB?Oh6C9nh&IF1YF zy#g`Z;zb1lYt@99FfBtD6W#_^*_5>vmS6%$epRXIK>WIYQCkKku+|ueBGWQ-QRFZn z!Yy?5z!FU0Sh4PYWIvkNk=}DmV69CMccx`*(#D;+g zT<7v;@mnB`xc@N%zo@)WQ}<%DfF+oqzgP566nkHx@$;$hthRDvhUNxD+{+V+R!b=3 zW4}ak%ph9EKG?~)bm=J?u8bD21Yh?cl;QFzQQRL-%kYNXQj2Ci#k%DPtks6r<87jN z#hjMW1?EJz5l6+ESy2L(V1jy5ueaYS{h0|8?#fI4&E)Vi#AQ#7aQB&!6LZpBtm=K`mXD26#z2?zOj(MZt z*#HRFO)P=6u!X8rGl=R}%4jPE+eT{RoJtKbVGM0mt!k2JXhmx-cN|HkEp?K9yGJkt z)}l{gW-&?PTVt9z@ED%F-c1MS=+%ASSt{iy7}|w&5TcgSFqDk_MZz1lF>EUQ%O7n%MpRa3EU16Mg%*r_!J8 z{sNX@qBandMyHEW`v(KDxS1KLy6UNPErTVn7M&~eebU99b1AWtuTEB#=*b_fMhjSi zi8esIn3Ewc89D%nD^QOQiF$HCo6!Q6V4^wHJU%!>yy8WB`#kWZHu+ph?&v#;A+Q$x zz2n(7@wz9?{m`pjn>_qbN%kExO286K(7V~L^)|8Y9XdYTZ1u>o^EKoMkD&~Kwdn5+ z-802AA86}#=%Gih+T4=N!^aEwu2HSH)fr;Il0Xi(G~|WDIG3ZIN)5981uVe?wh&@P zu+#rv1lGds2Kk~i=C%2|^lGS&c6=z7;Cf&hzTp;HjSOkfL~ee>O-nTFuv2_H+MT0X zDfv6Z#|iEnTl43;R78EVvApWLj!@bJ6ZH46L78IDfFaOUlY-31r<$g6U`;P=8JJMC zDDn3W_Ar1adbO$iOzp+o4}9IJ;DM~MVw>1;2hEQ5f~ULA{>E~|qd^RTwP;&K?9PDI zxjztLMP13rT55S~=3oI!FoAn0^j+8ijCrb(M>ZSI5Lkp(c3`M7Pl_fwg>K zE)lqFv9AXudVmk0=GDP+{!b$TOE5w2>|(5A9Xq! zkyj35N zpgg$EJswN&b%VgFL*H`611o7J$NUEI9@n-!q0|XHEY`-vJa~+J=+RFeYti5O zoASjgH&xm){u1q#`7g0_06pgd7s{}z-n(>|VzO@gntypb6KYk5F#4}6>_>iHc5 z;wS8o{tlzn>r2B_10t0WlHwqU$$KkdHL` zM|%gAyM_r|o+|Y+c#3j!XUIXCrHobhy7XyjZ-+wBWYKCU!!N>^oXMCWcP(AYV+khc z?;Z;Z#p32`fLNVVi=62eA>UrpqXMC5(JLJj3&pn0-GI2YycTKh7$JMk)DemVUzh&2 zH!c$AoS;PLauYJ6eU#j<@(Y5mg|+Da$D0<3hpu0SY>{O~wa8kBC|N9dswET&Cg|^1 zzC~hjpQ}I^S=J)Ec0|j4Q|eV96fIiD61yTX##bWb->LRmLXngwmIoG!QFg&V zIKdiq5Qw>HEJ3fUXwea@b}A6#?|B0;0EnBdXUjc@PLQyqJYje_UmRc=1H|0t7UXmD z*>Z*#OHf+T+6r^~-mV4W#}aJd;D%Me(LdMxN16ELL!h|dsLO90|MiIuPf z6SSr2TvFr9E+DdixV>?TJkEayLtrg>rMUA2qG8K(K&y}Le5pVvTC|Lley~RUR0C1Bl?iDubhvzUtxCocd|mqg>l1Rt zniJ?bEU(}hr~R|UEK&X1y2J%zdClCh^;eU6!oC77VGiG-(FVyC-x zfT&+kpB(V;l#H20R;;xcfKrFnjPokY8^3a zM;By3thXRHtY^blvnFWAxiV7A6UY9qx2M6X7T0RF939FMxbNbz0)2N`6>=zVhAcc< zsV-qxWdwAG~MYT~?MWgSY19plYlUe~W>pFPu*15`2QZpG#FoDa1z337n z68XVd{=BO>Ltrg4sFTg6EHQp(S16-zvL!Kl<1E|fHJ7mj6Gxz>?^$Py-pO5nSoxqD z$!yd`w%yZ-A+XjVAk1QSiqq!PJehM48GSp{L%zSYj`j{Jca3V%-EzL%N_z*DmWKBr zVSnW}(p6rdZ>_z9iUbq5JUA0@PkngPyMsK(ts7$%w&b_bpOR#Fx?kw;23Gn0`-jW; z)=XX%<|<ZR3&ejT^@-nzyOPzeKp9K$b?NV``DvnA%y=M-8k>=Z;2msg8d8B! zv}hJmKKK*UP6PtMH>pmF&cb)%PK0SQwiF4zF8w_;AXUs7;i{?}(Kc**0@13U0Sc0!hf7i4~5r>=!1HyfbDVZ@VPP(!-yaJ(U zxj`A!huqGLo}c&zyg2!;izKraGqu?wiUePm{-!?Udr^~s@PSzB)v$3=@3`3&2t|up z`(#p*=rxcM(V@m9Jj7SpF>kikDn){?dl<^78l5DrJ23@_{gaJJx6r0i%BDz$z*_Wv z(D+C4tI0q-fGmqop-m*)j7V+kDiTc4QA?u^F#*$n7zMG3aEO=~WkoRr)}n9lSVB~0 z=3{Er3&_j(M;FbIu>=#eJR14&_N6D1{sbSu7;A0hhas?5Ybb+8mnt`%3Phc5)yeF| zRm3~{qGc?>1Z_7OG1HxH91>rXG@p1(GaOE##RS%B1!a_!ZWi<4Y-E_*Arg1>P!-LW zf1+h9!Nl3Jh@$RvV*!XE|AW9vGfQb5BMe`ZTz>?Uq_$fWn7?-U|rNrtl zIs!{DF{&)Wt2?XM8;Gj&s%XBIs|VJi>(zn}o5d*YiM>TYJh)L=^9u+p!9?}4NVV>C z<49^%vnraOPeoo-B9_jv0nno=OJM2Xka6OD1%keQhUZx0N&*1W&UT7?Pf z>km7(S@hAKRor`}CCP4?qDg>zj<1Ea=x=X_L~)MxPhe@QhsrMdidXaBx;}&%&b`{bLo&P_9d(9Lu4MZQ8Sn$HQ60#qC_hohzedEWwu0NSryu2M4I= z+};7&>T;y3xV{{LwalT6Nk9y;r$krCHf;Iwi0GFcC1VLDlxSwrFdral!Z>dO#DsDL z)?(+9n-GHxf!XIh%syCx33i6N4H+pvpBxcqK^fSRl~CIy&$o!*+xTcOtx6pX`>TUq zO{HI(BV{bX1dSC|g&5?nmvpUtb_-$?K$Mmvuoi1o7|hgH%4TXT!34V+jBwNt|A9TE zy8Y(J*b@34OJ0Lyand1g4W?nYbOFZs)NxWBSPifQ6Kbf3aie6h!E2iRSqxUq0m7^t zfwfqxN`crgJ5CbUg)=J-CTRWsuAEK5s`HEDq=xa~GSXj;f?n|xey1{|W{&7sIe-6G2fFzjM1oddcr-}KO zynwh7^NX8dbXvMnnF8m9?KoE?n4n{Rx_`Qu*|k3q z1re6y`jV$oY&uI|Ejo|U$Y`@=lvq5zDoLnQN#6Eily-(wB$(I$W%$g`5Ywvk0U{Pw z=g^ya^5$xz83Jpud(nDXHAy>b1KH{B2xe!A3A}TLb8F$rE4ZzZJbug&W`~RkE2zis zk=w+ieXd~Dfgz5h#yqvWY~)}WOE5v-`o6Gcn`q`ipIDlfLSA2tnH;U}$q-nJwW{M= zc=B>@Aa^>^hp`G1_$hJ zU?L6bVcRWJe0ZO}n_&t2cFPz$`IWH~LtrgBx`e4a#HGdb{g1lV&Ln7|gG_$6W1b-E zfhB8y>=5IZwb8J|LC8YtaI&lH^Q(i5C78fZmJqdUXhb~1JIGZ$x-kUSV%uuNysz9; zzu|H*_+)4wRXi!!<3ayFA|glJ`MoOCqe1e;Euly-(G9$V*B|GISI+$b zZ$h+d`pU?JfV20;5_DV>zVhR^H(IL{2_~$e9$DeJ;@;4=K)i-Gi;bN9<*-K2 z83Jq3di=HOUth>*JQpbUI;Hc&DU$Mp;?Yy38p9K$&-C%~NW)u<=MEFJ9%8LLaYp7X zuxg$426Fl01UW1IlGZ9kf{Dkp9y9aAy$Pp*xB~kUtJn~^cF_rjz*@8(f2|5{4Rd=+ zs2sOW=d)BKfVLir1QWC#{WN)E&GiSts@}I2k!nLH%Rk%a zYOPWvn4nomH9hjhz3a9CvF3o9{I_9>Y!SSJA+VOV46l4~Rz4-BAA0`vIbowE6bUBc;SPRHDiDu)(KlH;e1W_SCx|qhH(>~@rSs&4{ddB zuDr3bE;gY=8p;z&lmq4x=p`-9qvT|R_e}f)6SQ?zCkw>BlP^KUetT<2JN6Chlvl#+w(R%!~D%3?y*3^%dXTR3PaTG~;;%}_z z4D`egfzdKIZ!#14!34E7=HCKwcQ*&H%2GXzG@1}CH}+9!tx_bIpeyyu4+Y|tynYbD zY5;y}e~4hMdV5a$^rH}1inPOtT7FX&yHh0P2_*^$>ma<(d7@pkoR+$l&}%6Y zOi*k0gH=yLwCGE^`$frBRp+(!P$ZaG2YojX`fk=%dLF=N$o)tRi;^23yv7h% z3zrSwD=IJ|iSYK0v%W6QsaRE>P$H&~-3H_1wgY?-v6`ti`l?7ULF>^7#$dw`C$Oq$ zeR?h#?cZ&XT>ZOx9YZNHF0G z{c2xYzG!Qa0>l%T;RalsDG%_AVhF57>!FklUbWUnWDP_boyFnK5k=L2v&`~vWA>!Jx$*KI8kerBEf_Q)Z=eHgAvrDmg7|U#-0p@ zz*?+T3m`ID`qM+6D_Sx+MRaagvVy2pG}16Gu_*_4P=7Wti0LOci>xPL2`0jUj;NO< zMmV(u;tZ_kJ!kZm>was<5Lk;^Mg8!)Os6Z5tCkG8Q4MG7a-$SUc|ysuQl)l+){S*^ zkyA!CXR@gxM*ae9AXZl2c zgQLuuw_^ybMaysr-XX>YQzCC&Z8G1gyNR4ZaiZU6ln z@;AHn;xG-X^V9|8KvH9QNsgOKl6LdkAarRFZQCTpUeNJV%2nM~OQ(EEZN(!^WlBRHI;3uhZH z{K6%`lWY6ZDySJv?o1o;wQa5eB}c6;)459>y$zwb(plGnf-=F4`w8NDX81 zkTF5~*p_OkVn=hDTk9|r&Y5t`mCk4;GX&PcS<|r7hm7CQ_dBI|UQ?LtXiQM=$8_+1 zJXz}lWt@b5we(Sf!{ zKX3+P6((?|JjCb+!CA%NTWYjEoFT9l+q&0o>XABcj)+$a*|#9@n-7X-GCP321>y5d zkJv5mD-MZ^7O(^pbXPMDJgM(zP-4=JFPvA5t602&C9oFlyYPOk_-6|xe%=1UIebpn z)c+@1+jkWSCg@n14L;-nd6ck+o#nYV>6-uG%@=$vtVP$Zzi-#ff>GNcTlYQ=mS6(E z(*(W@ApF7?l*GXsJea^*_?;&3M)k5FZDZSOGT{v#Y&-o;{nYebJ6xXb9p0+x3rgUN zh1ywEsUap5kGJkEI{J;n?9_?`*1~VT!MO%N6d#>mvKq?368t6_YZd*1WSsY+68I`2 zV-+UwJ7&6iOiHY171qM-ru(|%1LuV$@Rdpdw<3O53b(o{bu(nEH%Q3P+$?v`u@-$+ z`}?-jZkS6(JU=1^XR~i2(UGdWON9G01n(x~kHC@jGQ zehUhA;gE%NvD;*+e&H-;X27Esk9o*X1fTvO$f4K!&rN$jlvRe_HDy&}*N?sMbXUj3 zQeGI*U%TQcD+?y@`Uu`Z@IcmG>nO+8Vs`+PP}X*Kb^Z?sLm-lYpb}-rLJ4KZ!4l@M ze)zz1)beuAQIt@go#@?Eo@5|{4kEk+2m{C+q*}_87uKS`|2_|%gEEF(jgbAz7IOYv!{KBXW zC_JjMEf=+f+OU~Me^ zccosMm5YPLS9_*wa4cPwdSV{TiPP-u2j7U6u>=!e^W8K?H-bguBAPi+6(Wj>$wc z!Vh070HSvLhH0Z|ureP53^*-gv9l3e3>u8Nh5 z7&X^VgRcZ9@PAX2tlxF*P0E#l3C*z$TvA>Sv56Bc<1>ufz5O>8)h}U)GmP{VBl?$21i=U7Amb0oD-eeuy;zp;s<;uXs@D?fD`|X@K zC!Utk09J#ovmX_imLss%n;mJ~4UMCi5J(C79{t7G0&Ty-ZGyFwU8e4<)8D~6_-lKy zJuL%EFtO%l2Df%^H*u0c%LsvczPY&GZrWCuz*?vFZs$_gcM+``QerXe;ezw?w096o z8VftPCEoVpp31)3Ob#WhLzP+sddZ0i+qAs|mw^elDLc7Kh8@L0Z^l6xFJLa&p1-zJ z`Fdb2<-PqT+Dwisa0i1r+I6l4^}rHLtZ~oc__TInrF*oDRPZh39rfH(ryPN`7F^8Y zR<^Je&s?QMFx1?9Zn3uJShDG77B@S=Ml8NGPJ_z^4?WbQSB~~f4#p}>C}$4n>M;n2 zphged(-DOwnAiyAI~n>wXD;TSiGZFy@YxsHjC4)2AIHF zE8v-metSUIcg@P=Ec#~wOrF@d$dPv6c> zle>^TO({Wrec^RK*wfyQCFdhExX=G~Bd>b;YVehy@51=l;OT8o=PNA1gwQsP>+;i) zxKE;G&{0d~*C?cS5EEDne?uPpS9GRsT1z|5u>=$S7bSC>-*V(s5-npj#5u=W&FMtv zL`+~UgFRa~pG=-qyF`i0Fz@!}nrK%}EIGAh6Bppzop`JKH26voV*#r=&bviVBVo$G z#5=NqyC?M^Nv?F>rDx;r=~_*D&+)ae*4fQ#xi3~OBy%w(g5iF=zA;*RKd@xqo>=bD za97g%OnIyJLcfY>ai~HWm{6^W;Vw__Nk&`HHOd9%-9~fQ?=Ig~Sj)8C67FM|8!7Rl z1U>8JaD(~U`3lzqYmF}ZDv2)V&L4WoIp>T*+Dotm6K!F?GSjUWq5Box4&dlkD{UE= zz*?2dzGI@x1f)BF0kdzH29{ugevOzCx}4N~)r`sd{}$RC0)Zu%I0^gGYX?IJ-IuCTKfoQlxjDWQ zy@OaX1NOLfjv<8Zaq*Q@si!Z)S{u8fi0+WF1QUh@Zklepf{AWV{dT3AbRB8GyMH+X zYZ+GVrP)6wn9w~ud|%D5776VBU;CM`)5j9}rIl8Hg0!{8W$VrmvMAdp;{71r@3MQ* zOlaMy&qr!|3EtP@=LEcShW93VTkxM2H4-7QEWreR7J-xT*1nY5 z)cYiT%I?k(SnHj+t7dg2rHr^2>C$!eJgMXBAQ?+Ak#OBr<8~yZ%&MuSmE=?T3DU=x z{bek{1b)5(t1RzGPB$W@zJtdw1lAgA?50U@Qp#BHJVbu+LXzyF?-4A)1b)(kw*mJr zk!sdCCf1b0WlUhLRu9}Xw{mpPqTn4|`&jBa_p*5Iyq|z2n844UkcZqs6k9pDXlg%> zk}-j`@bfObyjgm6e4cUo32>{8D}Z~ zC^cW|7sbuWsUenLVhOCZJKA0Y-*glwYM-{VPOs-$3}_=3mPE@~f{ES+9W+%+gUWiz zr;-hvZ@aeQ*8?npwe%ae*BJi{5;s6AK(w#X2JXNbd(mwVyp@Y2muDJGUiF+>>B3R-WWN%0;pPR^xe9PoI`t+b36E0Jg!OE7WiR&|Z@ut0I7*6)*S zyPk|Ia+VsU&tV9x)qI?VW>VyMvDRu@Mp~!s#Hhtk>D=tuGL~S%IIX%SAR*?E9RZESC!K!Bx# zL$4w9-b}MKv-IAWPmxLMv`R45E z;8)s(e1D$j=FX${{N~J=IU0?mSqq87{ZqTMt?Hp4ZbXC}R9tqNbItR3VOI-*Rw;*P zcJJOCp+Ct*#J*Bh`t&K&#Il+>L*=x0`;v2oa-a=a4-+eeXFiH^M*# ziTPdb#m}18O@A_+h>*=$<<48*dvedS5@@wx=cV{$`@{AA1Bi&IaahbfKFM>a$Z!J{ zB!*PH93M9;T<@`#h+$&oUcxDKjY`tFkW3>kh z>O!mW86L*xY1Ym3k^jg)TK!jQcXg`q1}aEIee)*1V3qFrTkpE$@Z-O=YfV<`yE9t} zw8FDX?J%8pP1~7dzFxPcI?v%uMFP(iMVv_`f9bQ=Q+-ljOJ7~oa(w96V_H?t1AQJzpdMUojjc~n0HtMT89Y2*B7XR#NYa`GK^UdL!`m&KM z7?H-fKe3r56OqU==T7|Xv)%MHZaP1rzi1|NbWUx2_1a3H6`nWh@35S1BfD1$GV*3` zXkgn5T|N}QqhPq6e)14E`crF$(_yk>*^I{ijg1UckU)RxzBaa$ywc_u&&-P9mh-$p z!~E|3S9|K~YYcVc9HV~koIJDa`-|H%C0!rOT!O^$oq65Oqk8HE22$@%tno$+&K>J% zo^6nYKr5Up)oYjFLrpCI^ z3g>)vS9Exq_$Q&XXYRT&mbo1Xyb?KF(>oN9Rkok-#2<(-Fj~jYh259Jd+9w}4s~M- z9j;D4rk6)1@AUMk+0U>jKm`9=*j>J1FMVbt#j(269Z@xCp6A8gAr=Cyu!RoSz_Dk< z!MqbZ%eo9VP(h+(u_Eq4<$CFlqRGdT+OtJm=F*`Z~fYAiep&&i(*vsD9@KFawO0STd02T9J5=L%Gklv?Da?k6(rWbD&vj{>a8DH zLq3kLpCHm-$myA-B1ZzPtdSS}`*V?W|L@6}2km#oHox3e@xu&MkO)pu#l1GDk3P%0%2+u1vPhSIgy-?& z;T8g|a_d#x$usrQn|iaPZ~aSRc%dlIgwDeaRFJT)`tsjjDK;H1;rV{W7z=?`X@0HZ zp7w8VeS>#>RqB^&;?VNb`sF^61}aG4eTn+JgRj03&w3a49Gf@Bz?QTrE!{Wv_R+ss zZoPX^@4UX-A#&y{=c!X{jDhtcVZAHrvU081bv&o%O2x4TTH!sFmAK{9g>(4lp5iL6 zaGY^8ahx2kZ!d2XVR^p$g_jS*caTWrt@Q%#k+L^nui23!bzE_P0Do9wzW5}%_IVjmdPmgN74P1rc zsxN1$H}QKrch}Qts*m`ca>tWl8$DoEg&rhdeej5 z@y8xf?^2Jg7gdwj@$9pm9~hH0j*i2_WcGeRMjhK~soT(H@sY2)c-M)z4pRFpBg5o@ zK|w}D=7t6;NSuGxA-+M%&R$x5^JPGoEZbB0$ZaLi>RQs2@i~GznnbITDP*!w_IWBy z?rWfeM2(J)_#GEJnD+)P74dNZ0nD$fuRX>d*xJUv)Qu3KDn+psuya zDhv8E(|gZa3A9@CS^AJh_uHC2+7+oR#??;H&nq9OAfdhZDbP5@kWuIre zTDK#CRuyM`5)xOWo!M8Z6w%%!P(dR3=4T-j%eOI!JUznXvoC^-j`qd*Xyx3fhfdDc+$5&GsU&izRpX&X5EUdk{XSF+FHlo2?S0~v zM`g*PkOV!cjXz@~Ox>tH`UW$WcKe^YLJ9e~tS35bra9xn)wwQHrQyBhc!{S`D;m zjq933*;JK9q1*}jEH#3tAc13}zWkoKvZ!7>L2qOu&}#0%&RSU8+NO_|*(!^_)h9?o z6@dy84dpN`e|Sxkz%fSxt%_#vqQx()ZT1z8IVwopS~N_Xv8tv?)KYVMN;S8)P;)yH zXoVxIp0KF7q^&uZ;Cznv(TnphkI#0niZwkwv9k5oKe*t zFV$C?>Z_e96H!3|=SQ_ictn_-uEyh9Hrt#C^PqJ+mh}pg`_&ostL==!*=|r9Pw3qi z&CPSLSl=)?w|kJ$Ij{8`L@S&z)YVy?gUi$zRYJvq3KBRgsc(QPVvadq;T0J(rgF1R z@i$L(GJ7{l^;HcsuTVh(bHU;AWU4GKsSnP#SL1;MTH(k#TxHd|3KF>bP(D=Ti&f;MR9_*1R=Ac>e`!iZUQI>5LlJmA z#};Bu4%ZOnW1iW&NT8L~2lZ|pvv)DW@ydx=sh%w2ILD~7c5s-yuU0Z=a$09mJX3qGXrMiuR@a<&n~V;VM+XEMpJZ)l zpn?Qm?bP4QR9Cf|s;}~?$dN!RymqNylbho~nTXd9oaeD7^&4JAoKZeLwGn7#^|4)@ zgGbDB5YIJSA>+BJe!&A{jtUYuW2mcvns;xid3UB75A1imI^#GwTxC?2WKmh-R&k($ z1ZI!J^|nesxvA|Z#^%HIjG!+gwTTaE=r0xwapQP|H;dGgov5MLNKfmMA+sXnawDUW zQ>~UzK?1L$>UWS+%F99HuXvu947U(y)hK18mLax=-n|d`sMkG%9Nl@3=jDXH1}aG4 zwO!4LJ%1I$R!s5ST|CS}pjESduPs+hJbJQOKXx_JKxOjQiCX!_)xB2eTm91evm)Y_ z`V0PsdaI8L67wHS)XEO2u2=KEE6VZDTQR|y;Fe2ah`+e zhgW@7$r^LW!1}aE=dt!#xr;SrDnxA~!ckY%yL_P7`)tgud zw5s}IhBjxIQ@`Q$F=DPEpGV*E+}zs4Kn01POU==4J`wtl1<6PEmAZ`G@u%m;rluAG zt!95QM@yzYMVf6t#Zh9wSveu+&z=?6n;NJfv8?HQt#p4)FYJAnT6NJGnXBtX&zjoJ zECgD0pEh4>KULGy=;R|N?wWL!JnT8SyqSRt5`UCjr1d!+q8BboaU_hrCYL2Y?3uRH zN}$zey%uTh{uiR_9`Z50-~%~+<#vzft$Ira3KDYGhGOYJx0wYNcnkB2YnMc&24qwW^i%!i6c03k}}L(&wT*KP0mf zXcg0XnO3x3WxaVk#qs=PGUwn@(>&jQ-ONA*iHccPXc6@)>O~8XkGGvuIH!cn@Pw~z zVj2g2+d$GC7^4 zqOy8!T&-sz&?@Kp812Zd(t77*|)^}W}V-Qphd@mw$ zv?}VndUUrt^QmeEDo7kVAL}LVJ}08brwMZYmND){={p;!AW?0&kEqmz;`sg2Q(5!y zB=@P4T?|x^h^QBt3_c&hE+U$iT`Xs3{mi{? z|4;)JB$C{W(V}V<*MD10#GWnZME#Am;#;nZv=C@jZf%S=a^EwujLlApPhK^P&tpUy zs30*qGDdq|rMS6fIQ7Lb@jO>Ve@(5*ryUkEcI9$Mp0yHa zH8@p__IPSZ(?`S(L*!gF(S2X7WKcok+ghu%I>$?!#QsgXSQ{DTme(Q;RFIhMTBQ}Z zTw1^4U0>C>yie3tS<+PPHbergF3yP7>ZB~EySyuz$NRpN&96??|E@XQKm~~mx1zOL znakSX{N>%grD6X~CeN;se6`lIiA>`vg?+G$G#{o|x z6*&@UWsSU0i%401@Lo??w;l#6NZ@)-Jz-=pELD{B;yTKkciwrTZ`B(^t;}^8hN>-+WrR za`q_EV)g|6yQJd{j5coOJZ({C&D&SlLbdC@-$=E)dxHLp+TDf<5`UbVr)4Ow=_S4U z;nKVekz4Lm_gqtvBY{@fLUl&9tttmcRr6%eKf*u-iIYL|wH(bgeS>$m;e{N_$TG>jkh6FDuyKm`fB-%-0PRxJ_Q3r#PhBFC0sw_<6(sJJSgKumR>|AbYJbVz7;%3`L0u%X5@_{l!R6X- zX)BsU=h?9$M|dH9hnf@72U>kSY`IpxKt(;?J0}(%v`)zKY4uERBMnrLsQbeTZF0H_ zrjN-jH;7+)r_=kqv=V4FsPIZ{(5DsjYF-}~l5P`8lm6+>^E}c(1&P#UqO~;tlr?>P zlw+qD_2Q2Ee`=M11X|%LO1&$3vs<)%6zBd`5vU-6D>e1~kApMio}3ws)&uHUp0MDP zLhEy%;~fvkJRN#^(o~Ny@Xi!7Bk%F0TK9sL^-A7M9M@#O{H;NR$EETL6(lgn)U|e2 zxGeK?I!}{KBP;}3;mE3MRQ|3qdERuMW9s}s1qsYh^*10g9g#0eHuSi^?P1{R4r9u8 ze7V-AQbqmBP?~*y$a6@pT3p|gO+}6h5*VLaC!QN9zYBe-Z~bk!g+MFJH;1c$)?YTw z^;~bLB1Z)Yj8FAd{mXLyoAjQ=DZ3lEV#W0?-W98T$ZxL7(7nIvhZTVe67Bz8r9Hk; zS`Y3=k^jB!h8(x_x*qm(xP?F~yen3-Xu3b-=9pDp0u>}`o{G`rs*?J!UgRVH@O#qv zd8~eIKvx3^w8Fb$wdz~_K&}t2re_=2)j$Oaywg)(^EmytOuxRGzE0h9B7s(T|Ed0l z(7^9y-24)ryS6OB@yELim5HOr$xV;X>!qWITkb!Qz_Z!mdeLX1JT-2!Ue89L6`sxN z_aEm+$>dAV>4#Nbp%wbT>`}YIcFd3sjcNMcpN1JY&UojAW22tj9hoIdysWN&rwCM# zzT(D;yg&U#*%i!;0Q>*HZ*4NZ?(udQLt2D;f3gJ@=RDjvfiL z!qHTB^qq=2>F<|>{9MiQWP+Yj?2jm*)AI{_#$hE;L4wMqALjMaiA1RKVJFy%h}A=L zn#AiO36fgXJd5vn2`gAHJr$wjzUL(g?;+5N%GnaKn8Z&>S9pEYdF^{%!U_`fRD_QE zo|n9jKr1TGnvlsP{#MURD2{0Jc?l~>;8PJbmzcyV8-Z3tCvl{Ej}KPt#IQMz0C8Yl zRAwvc1**x{yY|Rgu@gl5j|bL8WwxTxq?&xs$m}D?ik%?ZN7!?h396T^FyGW_$;@_V zfJ|h9=m(iUHht`a^8@QbD=HtY^shT`9IPNgG0}1VzQVfDiptw-J~s*bc(8&5jSU^Q z=5B2>+nGQsDlcpHz$EOM$O;meW9l#I*s`4ov?6*+$D8l*!HS*e-}{#UabR6kW-ICi zs>zzW962j?f@uHoz`Cf+Ry0mjQ{XXY#ZD0IBlenk6>84AOi;aSh542^i)sOKmkFZV zj`-2^@uA#hD zCSqM^MZG{ZCFU*@s374~1o=oD568qV73s=}1X|HJk&ndOWdao>jw*tD_=vYdiaG}@ zT3m7MVc(j8>q0AQ?#}J#TV;q#))fE}R6c)rrRigwNi+^XuoV&dxrGo1uNhFmdMPF% z68kC;fmT%h=GqJx4WO51K_$L4wAH2y5=z3ACbe*1_4|lUJ-DfjOrBzNjsCnLsO|@6Jr| z9v`gOiIENO#9QN_a}evIGFwqEP)&hzmlZofw2#2mE7nD2wxV&OniA(-oPAia6GU5c zx2Bo9Oi;aSWzF5GknN}-LFIhokC;9_M6eYR$9B3*0wYHS>!p~8@b4=m(2B~}zaL`~ z7vWwF6(neEi13a1`v|n6a@pWuli+!mXHiy=z#Q|RcbPydqEl49e8AT#`@GAFov4>4 zvq|tupL3T9s+X;(7wCB4+-1d15bY!WfqQ4Hi^^<8<3u(2G7$+>>;%!)+~s+f>mr)1 zFhkWoOEY)F0?eXJ5Iww2>JXog18Pk@zJHPmRctF{w2DzXIYt~PB78eJZg=*rOUhlW zW}t!um2-ByVETAz5}ySi*ouf+$I6BJTE*u_GqoZ^1?#1LBf>Wx_NyA#g;rF4+qcCHJpsN7;~tVw*d*|#oPW3Dn-L86S>`$4_y+xwC9Jp@`2y{X<| z(}$g4#ZDy4ayvjASQnMqih6-+vYsCtIV*O8Xdl5NNNZ}Wi^^<8qe(Sc$D9aM>;%z1 z;+C0L6;xg!L1nhW3{^YmOrl1BOk~1a)8pit&&M;9C>Vg)WLgn%@nSZUuxBDGSTDsy z$Nl>X>q09kf2wFM68R-Y(>35H3iOHR_p}PJ_1J&>!LDS(Kt~}zAQlk6+1z+kGN^e zU9O90wzB5#hvr=-h~5=8D1Z;F3$3X9D0;$s;$Q^{iiwT~&Rr(Zipt5=UknXA9;_fi zV?)OS=PnayMdh567n+29PGkiM%rS@SeFRz&U2eq6_xNDNPSowa))oinE)!HQTTw62 z@xXn>ik%?Ze>|`*Dzg=hCe`H2M4YKnu@gl52>ZOtbrH>0m~ZMY8=2W&Hoz>(1ks&3 zZZ&fB8D{E3%MI=KE*)=>y@e(i16<#B+!b=GirWo8xNXA zQ9(3ACbeY^4(>!OxKCIUFiTV2)Yee;|QYbbN2IA59zyR6s= zqW#AM>!LDS(Kt~}fpeD?J3(~d++~95Wh>0L#CLZg0cKGqh^Bkzz^!5heNYSOc;Luc zu@l@2>I;&#ykY_)r=F(c)>)KiA6D!H`3OA2F@Yn98KZvT22Z2teH8CPp)&mjhx&)! zY|(N5$WgHqMEmCz5>#d@dP7G({3A!jP7v)MITBQ6D}3&!{%W>*e;)cIrrb((#X$x8 zhgQq<^vmbNzS3vKPS7(lYaIMMkqN4ot!TAO$E|(EciXJk38MYS1M8wPThU6EYVyx^ zRO|%N{sfIP?jz!ip|PRk{;fg)g6|9$f zfe8QcAS;f8tavXL9p@JM&kU>}L1k)TU>``Z6}Hen4vrSvF^-}r$ao`>w-XTN;9rte)D%27rsBFzdn&FV3GFwr`kPmAn@|?(uogmsr z*ylths9v`Ea3ipNU({8Lno48IB3+1=;nn5!WR0+!O>#9R1O>mUC(jH4(}tPYdanH zZxt#;Bayh%iRVPhDRl{FK2e+j>%=lw|(2ki>- z&&0{*d(OH6-g7cRH0==bW#T-O&;k%_MMUC08D3NKJ4jZrUg`zjqhrl0vf?<%iuXg& zac-eCOHe_A%G5&NnB#TOPOudn=a>>(#c{BL^-?)84!aNBf5W?Xh@j*CtwM!bf&`U) zggq0vE~43rGKP)^&O}!11kpaizH@*Hs+X-koQb%ni1zsSXX1zUelS73Kznq2nK<0c z#Fhavk*$c}ohpeV$Ztkj!Fs6|=(zuQkQK*4R@5r8qT}2`|4d{B2`W)0hT}NMig)r5%`NmNSV4ly)I#5w+pnBR zuobq@;j+iU(PF(+et#Ue3x)TvcnP&T#lKakP)m@YvX8LOiR6P6qS=ZvhK>i$L{{ts z(Sb9O396T^KAefTvxfGw{NFN>3F-yf+v3Z_5A6_QDSe1BXCm)O>@5{h?>HSyYZV{Xl$%^A3E8c%Y z$2lfzmY{+Jm8pgPV{RwdijH$k{ytd2da3;WIB>rQ?=K;Oj{CO?6>13*RQAtAB&f_* z^sbP6_-7(2c7kXh!Os)<-8R=nG+S9Sk@G4<UGB0kx+=M@t~)1Hw3 z&P28%f_Fyvk02{pFZBW)_a6_k;yB2PcfinbZlS*qR*;}FwJ@*`B-jdD=~T!MjC>pyR$)A%P0fNKn~F*mqfQT|~1LWegqnJVvf>>uL~{%M zeXxQAm8pgPV~%y96}Hgd2SRsL^yDGnm&Js^(y-!3LTlL$!EeTh=67r#CfJHvNM--XQNentO!4`T2i*_wn>bpx^V>HnC;lp)$Afwo6(nd4 zPlUBq6gd)TMP+(dCNN8y~_#`4MDzQW4-;%fzNzdVIVxB$l_|c!=Lfyb;PJq> zCyBpa=JDX(WrgZxf_&3)U#slDMP>r6s7%lLeT4lT3oA$j9uLlT_HJ(#zpe4_U3x#j z|H*i=qWjm2R|_Nc>eJw~Bk06{?pB@=eFBt)e){iV3u$GCeO1JRYnd@!|1c@Ag*l^XtU%;PZo? znDTh=(^H}opVngJ)Dk4vijGs+8aex51?#0UKTq~u&vQQU{k+>^9vf}5IR8IW$8D_5B{wTD@gc{2mkdXw6eE~pJn>@EapG-Xe0KxasL?qrjpM<}8hCXP<;rmP~C*IwC zh~PURA`ht*4OEbbKeR|2RW3w?)FB`JJMENF%g4%;6Cx}GTE#Y9qScyEMa(HeM5>jy zWZorfWr?O?1}aE&%>0#hs#%CQ+2Y(AR)Eo+S#U+L@)36 z?k3-OCA&n8lto;vEd*NK>$O7Ldn8zl{EU1Q%aqNTS$)f-SeY6IDoC7fq*iI428%7; z@AhrJoy0l4Ml*TfS!)Y{Rs+*SYkiKC7pc78SBviask3*hK63UZX`q6{kff`$&gaXC zY8&WVTUlCWauz*QTNb_E$U>mi#r@IRv18@Luv`?!t%8}IyYJuGtZSfx#GX5` z+P^)Di&f*uM}^^^Ir|NoCpLdv+d`mK@|Ce#{*J{($B)Rz!7T-yg{th<*4?XVpn?Rx z|5RVEYf;pha{WNLl553D#59~eb%nH znB)ES*~mNza&w7A+N(634OEbz>md2?T?h9hSs~YcQA~UEa)^aME1Kc>_o@z8`mq0Bc1&6Cz|NC<8+eTvIZuNZw$Py&*EKq+@?x07E zikqvwx)f<(v}4~yYw4-lsQ%`Fj29VxpQ+tbU!z3@iI<D(#>4Wkg}`*J!h>y(5S8*&*7h z$dN!RYvdEM-<0W7Z4%+-x>?S1Byd(zt@_^{vA@W1ZR#J921ffhZl(4-x}3MKu!ZXT z&fDTd@|lOUFCVL~b3vaV(I|bi7QVTh_|4l_=kHFF*Saqji&f-EpcS^z;Y#s*iWHd^ zh+I8~8>k@hX+*TP;6OR?h4&k7EstK8lmC7uB2?r^pp`Z9H&uR-ukt<;J$iPx%)3b7 zRYF}2>Tee>K7FXYP?2LxFme&TQY*c#yeRJd+D-p9OU2u5*R@CG#v7<0@i6TQt-!Sk z=6P=Pbc@Stvx#T!NDF~hFV**07Px}N2Ct7NwH63h;Z)-L3gZn_kXV{#nYR3+N~Vwb zxweU~ca;(s)y#kdTK(^jrP`P&l|(Y{8m|7=vqjY0f+B6{@dheL;A&8PVRcoksME8W znExQsLZB6{1|6=9-FwKpl~T(jju93Dt?I9mE2oognVKuu-Q6|DdpcSs7)K?KF#>WByer#aOKW3M3%bvL}b4> z+|s+)Z-sNO(5meU7PGux)H$(bp#1paL-9D}2m=))Fh2Di_oqi?*Qd4RSyvAWfmXOu zbGRD5IV_(>)|7=+8J*oTHZ*=;G)&V+)D&^W)pNM(!!%=KO_5>p z5cj7i$7#iP)DU_)y4J3kwLxyFTg~aH6l9=+M8`D~wd8+S7c;!;-BX`Vk+G{%Ig_?+ zY$4Fk@Br}|WF@r-IB*n2g&Iwh^_IeoaS z^=g=fK&uV?Cu%$TRTo9Q*QmMuQ_Da98YDA@4L49hVnoKN+DGBl#6>T0<3wdKI?^Tf zJ&CjsXchPG1g(9m>gK$gLQgNMw&^WS_Ep~>1Oct|sSH1-S;bE+;7P1N^mQ9+_o zM(@oUXhVvvY5P(cFk)z$Oel6B>$ z8-wMeWFrhzkXSo&uGZ+GCW5`s0ABn!Nk(7oEj^6}S_rhldv*2Auq&&j)?%SNsJ>)| z3KDgW&eh`63Q@2yol#c{o|l_*>heL^FbjcJcz5q`Iexe$)7?2MdmU?Npn}Bx6LYoF zP9d^*zkj~<&K^19@fBHqVJia_B!=9Yt~HtL6v;oORt~gp+O}-?1W-8~b{#zCcfmV3VtM9lUEhbuT z+%8I;x1JxE`JF0^)3&v*A=-QIKaw}Qt$jCSvgq1jynzZ5pZ`8oTl%_&nY%{SP*J?s zKcd>%aTWrt>dIm2{HQ60ETq25s4_h^bdIcu1OK&zMu#|UrVb! zR~9|JNp7s&!$1YI>s?YI9#rg_2qBH zevtWkbhGp>TH#f~;Trlpr`)tZLY`|i+`yR=dm4Mh;X3(bq+C65znsvvhb3|(u-DZW zp>JfDXYPc{!K&luKt?GZAyl8plsrUx<=}$IDoCRm1ZGt?=6A za3vWLCZF~Va<w0O2 zYf`^3`Du?J=k+{R0k!fI0z?%=pg6d zbk-3>BBG5aG*8LqrjLG~hsjLsf}GQfS_!mz&~agC^ZYGM!cb#gMUCKg6$dIv#BW~^ zdiPcfv#&l@L|a8{vk_=DzJJruT)(t3iLPn{PpJ{yti}TsBwk!N60bMyDoX98-o5p* zlBn@U*;m1;uNJGwOR6kE1&Q1J4#(d-+0`WStGPXOzaVD~HMb*yRyeW_*O`(jMJDB3g<_M zt8^AMik6LGe)j>npbVe-k)Am`~!)-wu;{~d1@ zIyq~YIp&AI2$OF*1vyU^uo7s6=f8Tgt>Rdx;wYs?5EUeFR#MM06!E7b@+$(b$e1x_ zYBkW}3)eNXpq14J&J390c;&>bOw3&xb5xM9=58C+syV7v zdsVA28!$`o`si?FQ^XQQ>{A3PNMIg2T>DjDZ8kFzuK;+(nXsY$fw%*on`iAlMQk() zRFJ@HqWVsCzRIGJT9GADd4&X8;T)sBO+F}0{@N?Zc_*iJ7R595&u1OtqkijT=GCF7 zFxg>9kTcUK4GmO~z^k3Z6{h-Xmf5>VpcP)b)Kdes_GxFXmhk$4^E}q1?k1Fv#pZY* zfmT)@2eVfeptdFgOAo;)P}}46z!LeH&8($BDvGODWZ=k>0KwbF0(}2d9a{3WsEma zLE_PvQ1_PfeZ;X{w6aXtIa$jm(uux5SP8URa=nWC;?~}#k6T3sYh!aK6E%JwZ=iz2 zf$_oaGYxu+esSc(b?htcj|L?~T)dS)t0pPRxvQM-W%@AA&DE}cTS!#;cD#WK5<`C~ z;hxy7m-tTgo0_lEtk<&VYarUKwi0O7cYG1I{!>qLO?_GQjn z4(=(+EF&U8P7{4j`x9t&r*2MnukRzopWZvjqfw7UqkcDhw~RFJ5bBd2@a-3U>j zC;4d5__3JTZmIm#Mxa&lm>_q{DiI=kdm^fyEGbJax-MT1d#W#Elfm-C6SX5ThQ@ zy}^~1QkJ^?TJ{~(&O)G7(@7t>wRPP^RzW@r^{g(lPyAP&o!Hhu1&I!`ZpW{R?j{O& zpK>NY&{(D%m)dzDsJVqet20|3#UDD>O(ZEvKGvw`O2ew8buPT#)IbG^zm8sskGvEv z2Ie55@ZPMl=h_o8Rh^y|0~5ff#54Iz{N!!n zV#Giq@*UeNj-4GQI}RCPA<*jd%E$4MBf5z_-lyk9(jOG--%gZ2H5zW9f<(oG%EzQ| z@s0OQ!-Zd8YHe<=6^&Dmw-9JGJ;THJ=Eb_1=U~;^AB&nt3A`6^xZ-N+qE*{Ta)XKk6(lewhimx6R5EPrUirhsz7_(l@a{!Db#9eg zZr*fJW>({Y3KBRr4%gh3ZRC}ZAZGz}SA+yw;gwp=CHq^+5oLeAMqAx?3ulx5^s1H03}GfmQ{klyKi& z)Jt@mN<@iL$z-$EvGU{}0}WJ=2>G|LyL-W2VrC=}P46BR=a%-D<9Cm+5NLIDa|!p0 zvAx7VZ(pUjbWYqj6(w_YQM*MzL85%IBJQ=Rdx=NU*wKm`fB`Z!#xW75k<(>KeLo% z@Oq_oo+W#uHF^1s7?Hv{C*o|E>)A){E$zFD=nXXHX;wbe3VpXq{F!&WfeI2so1}98 z@LP9t7Cqhiy4G0E7xQXZ3AFn8N^1AWvOUD-o5;tA9yhh?XBUVoV!VM05{;98;tqb^ zLp<3`#Ql3;i_)uF%dVqFSqQYsvO2YUd8-~`#g{}hI|Nvg2eGrIo%;!B1Dg^6vu#p%SC~v zb>$Z-awO0STd3Z1_MI=zhEgJ0 zDVD1^uNNT3zA z(BXQpZLjwBakzM*d|bmN!=BxdpL)y=E-lwmt*S4a&<==s33vY zE{ALLv_2xD!z)o}(>TjD3JGf*X{ziMC+-fDZER~eT%F;Jp|WJc7BMq#4|#jSNXuM; z=X{&}AGyOPb{DgwD2}Wh)`&9GTFc(*JVylyJewV^S$)&XFDmNtaQQwK0h+@HLHB`-3eJht*J2%w8E9M!&SZB z?;?BeS#piK;@~v{uM9ZnJ6u0HHi>i#Ys;#(F~@PnwS;>Z#Nw$i=llPF1Y$J#q_bp|NPLKM;Ftxr_d+KH60H%oFkVLhbwgV~yj5>bi^m z>$?w6m9(yVzsm5tyN9|vJ+9-fGnAhJoR53t`F!6s@o-!hCn`v!+Fr+9Wk+w3!TStg z^ypchah-RIg89`uVW%`V)vvFpGoO5%Nl4-uktLZNd2x(|K&wYbYPyp>WFO^nEz?JTcUh#l zpk`4}kich7Y6ryA%yM1FeB!&z<1A03kmz@*hI?((zG9B|$;61HGeqG}R*0cnBP|44 z-9A>sovnOdF<=G7@pjY}ZG7H8MA;UR1}b>RVr>=uCe`;Z$~dquyVkGKbgj;}u6Qc< zY#Qw?_sf!33;JuCx2*OWy!#hrpkgNuZ}8FYCx~XNe1i+T^KB(iu@l_F_Y*|3Rh>aa zwa>$51c(C_J2AG-%KspUW~<%tCAFjf%nU%FVkgqOmj4GqG+WI}QC54CZ%zOL6+7|s zn5F+g5Y1K**~@G5W99`QP_YxWUVrr;1kr4j?{sjMcYvsL|qY2Wp<)dwne!f(vIK9Ha? zTMZ6M{jR62K2Wg}eq-+SfdrM=>OjX-{v()#MzFs+xBs|UG(K(0r0x!VGHc~q6!exS z7)9Qdy{(EmczM|#B2Yo1{LQjj-=x{%)2W)&F7M-)cO6;$)ovnCL1IIlG+u&y`v}k6 zLKW=-B*IS{0sGOB-70OVdZus6M+YCY@b@ zT7?P{vp=t^Y4iWu?`ze-D;;;0G@6tr0u>~}@72}X%y3lo5f9Tf+I8h#hBzWnLE=(K z9qqu7&=4Q7U_qr_r6xvu2~?1fU216kDy%h$B4K%UMbx0rwW`UxXr(tMD}jogNLW4NKM11PYOh+=?0C350D+2~ z=v8mle-K2o)dIDu8FgZ500I>|5!-Lhe-K2oRWG%w`Lq7w00b&_BKX0){~(BFtNd!U zG<(HDE8)#{%q7gquhgoBd)hjJsMra=G57jFg34@FSgmTfr>#Cvu@in{?)8BLmD#GH zTGen*TYaEnC;Z0T>jMcYv(-?ws^Olt`as1__>H;O2NG0fE4NxLC61u_rL=KH1UvJb z*2TEc8t!v5mWz-}zlG8hgqwdY6E_-N3#H?sLzjzH^Y4W^Tt6rxyCRle@DZ#aLFFeG zmzl(GiYTUt=HKf&)`eE=J6Xf$%7;+IIz<%z$wyGDNI`=AwZEv=C$4geIQO)O^L3$_ z9;^$k=yqw{#!Rj&^u z*ou7gth7R88}P9rhAJYBB2KjN5v*XnR4#jLx%laJN=1Y#;`i29<>-)%2G)gEbbODo zLj1L#h{@!m%~e_5Bv?U$%9#eO5bG1tC}Nl*VhhZaA3rPN^??Lik&hLfSBev-QYm7A zBCaXo@>3td3f4>ITa{Ld;F;+a5v7P`Kk1^o`hTnot>}2UG|^&0RBAJhsfy@i609IW zWjcPkcLtNFuQH@_0D`T^$FV)pW*q&MW#mO&A7m1&V7*ixdp+8WgT~{r$`E1p6%uSk zKDM-8WsXM*MKmZl)6>=@SiyR!+-&VCb3Be|!N%&gS3L{O5k!Km$j3j|SDAU$QTgc6 z`l@G}Nw9+TQn`H77&EUZ6Ei4lkC}-`uod}eT|dS=Kh)o&H;yVI*d$oNdZ|3?T#R{s z+*fg&QBf~9&nP6=ihRtQ8RI>h9j^MyM|DN?EZ{o_S;2a#Jgz{j@SW%8%wU9?GXoNA zMLvqGjxlGS5anZo`d?9#U?8VqIuO$EgqRU%se_q>7lSh`#DPLdQ%|pYbWRa!mNr_b5tMDO z`VnkJHBl~PsC`BebY6rj;*pwxP{DesOc}GF`7uRMUY%AM;!v}konR|EPC4Cf8xhx3 zmLyd~=1?EO3f4ZwVvf&`W6%HSK1If~e#GNhe3=18y=`Jk(vFRy6cEu^kdDjU76Vg>7^GF=CK zdDT}D^OW^eGZT?uEAm0tVBh(XOkK~XRn6#Zq5PnN^-`Ix)V}khv?A{PUBp?oknfD* zy3mSh;x&=_y9TON^;J$)GS5L)kf1WJoM;UfWh7DmHtG3-zf($|e|qS&%ah8@Ge>yK z2UYp`o$h+I!s*>s0u>~v{IpPSFa2SHt*EA2kNSa+RJ)rOqgJ7U_0sW{`9|wbORFdr zB+!b=8IBFmpRY`t7)KNntk{V+p(FL(?Owzu#u12ME28DNK|oA?(k?Fd;g-8d8MtBj zyB?i8Z;hTZtc$m&xfgssZeEaa>91~#vq#GW)ik@%TD@0%M>CGBYqhxV<}dXl*ouhV zO~27kyli6C*tbqaO}@TbyRB;6*@lzj$cp=& z%Ao_+>2o$V4>0D%jK@3oWd6H?eax94I&JI}z1L1Y2M4_f+EHXx9`9Mp`GZze9{C{Z zJ?952cA`<`Df;aZ>_c^I6!kFHMP;_4vQc+}9(O-oVqd)@Sn(syPS$6g0)kw4eIQ|& zRzyEoH9;R(5r}s_Sn;+5h(~=V>%Z=R^ZcC;B-o1R`;R8*x1Xm85Cq-;#V^vcJ@WBdMAu+=Q^&35{Y+x#R^#= z(Rs`yJ!D6UMB<$fB-o1R$15l5$(AHfB;NU8g{+V$-)NHl@LaM);++p9*ox?#2PW!y z{-k#k<~8b_4_3$uiS!jG>94mXNhIF+K!UA^{_Dgf^KM9m2NU#;r zB{qDiFFFy3V1=xZIDKf6K4&`Q?z=dUU@M}tjQLXk!V&2FV1=xZX#VdceQ8X}0FfiX zRz#m@{H300Y9N9YvO*$b&M)=0r{O$*7Y8eLVnN->`Zl$v^W7O0kYFpKvn-vg-&+Fn z)jI;OOP|cypbwc)(|dKcUZe7#RDU*gL5e+87b{d|g4Pe$3U1Qd$5b*~Rc&L%3e~=O z6_KDaTYam_ zU*(O~`&Z?@I`H`Vu4X;bS3<>3JSn+FA2#YUTkj%4WwxSnvc+5VVbwDQAd>#DO}`ud zHlD2Z>|3QbIGNo2VS?+j6TklztzT?s&l0SQ%4|htYvkSmSN|d9`7hh`@~1C(TXmyS ztnM0e-&^L^TJwqsR;Y}`lbbR6jUQgxW*fKx$w|L9k zTK`tDLiHj+<6r*7&Hz4;U@M}juP!}25P-mXv4u3|ckgZrK(IpfB5^N$oSye(DU+!4 z)uy;#>h0J?R!FcFohzNcnWER~u+bzUk5!5**kSDsRETDR%4fR9>8Ho7KH%%C$k|_S zOL=6pcV^(a(28nWRA`t!>NEDy?^(vU0oONeM+FHg&y9@JHl3`aEY*8g^~v7) z7eeSh;fn@o_0Z}Qy?JGg3&qt@Sp>d0wH!p_@(M-_Q=lfwz+|w`Q zui_&bzUdP;wa5(b-5rk_T2Z-MrFQPva}G1xao)wqt54ji?=3Xn(hKh*k0Qz*EfU^& z-upZe*VII?LS-aqjyclS6Z+1Fw_$dIt>`$-M84;6b|0*e6%sUG`JSrT30CX`&Dy?a zWOjlTJMrz|?fQ`)ZU=}13AQ4d=6T<x9WSl z=ME4D5^O~@WvK7noxN48kQEX%OZeWz5s~ck=n5%{H?2rktRP{ik?G@``FrB7g#7MDuoV%DtF6;dJ!xwa;WJysZCRAZ+bZRzVJ<=n3-4Y+e~AJxkW`k*YKXR`YjGO^)a%eXt?p9UXOO4`NC1&!2URc^M{ z6SDU}?)S8+)3A0Gsb20yB&d9S;n4#`C(b3G1t8dph}Q!?i?)^M&t-Uuwo~^%9<{IX_xE)2v+PwmCb46`_<1DfMCT=&}cSzkm>(}ASOgYb2G$Mh0D={=LV_}M_0{762v+PwMsYj_nHy z9PR<`IU(;_6~$SC1X~fkWcEy}k9PzsWQBz1x3A(GXtv0`naGNr$i1Y9yY;VA0<;PV zwjz36ubBAKsgEZ1?z>j8LRLrwJzN*RCXzGpc>6=U2R1%fk*rujg6@jmw65sB{by^l zRXL_~jtj2xDh>&>qATaH$t&RgW8bY@aed3Uy{jcw>;&Jp`Nrcg5Ue0U<<=23+~Zm% zF zCOzIL3Oo^sz3a^qB&f_*RHhwCiFbEibM+ro>_n0}n?>w^ashmBT|}@ImFwo&BEFbV z)JjC5Lgh8lTgBU?S-p{SP4DLRcX6;{CwP~pKf#Kf;60cA1S@uecToBhtk{VZe{K`) zV`*10jXCx%5^O~@@1XQ2SRpGUct4~+!HS*Wy^8(>D|X`I>+Pb^*Lb5wonYI-?Yr&_ynz(Lp+;`X(jgRBJa>>zmQRm_-930;}hje2bp6kU)l;X zdUO1@k$f4Vr)Evjw!h%kY8wEr3qY-xaZE6pvEUqAMwwfVMUEkygp-o=Bvxg z6~^aE6SM-2wty%6@dP!n(u5sd{ipi29Mt#(dRp{6gPzZ+@rfOE-^^@NgXbB#^{ACP zqM)-~!)bz6t0jk>Z+ks+;)5w+IeIspnKHRA)Xx{Bjwl}d>FjF>b(@L(FEk7 zBj;wG+o^RJW8#$CgKM+SOZ5p_A)UW7*lp1g(&cyRhiUP-|Z6U}Vd(wzX3OD-xLJ|Ffk_ zM){^ZQ*_49*}<%3{cO7;p{*ce9aQ%yPo53=rd!af#Pc!l0^r`T#&=aSR`;)G+b-j| za;(0xe6YL4E}JWAGy(a`zZzw%Xj?bDx{tT}GXI;(ZEf4t1g(&(r3I-O%MRBJiL|n9 zgM$BjvH+#Uy9*fGVRg^>w`4Txy~B?4c%L&m<5at(ZE)*=ms^0KMiVy1{e?cUG_+#u=sZ|!dD~#`-b@<WJTkTtlyZIHJAtK83pIc~+PhfRkdDx#J@%fg^gZIW%wkv&VGyxe`J-W(h z{lI0xnDmB`bvt}%E5wfNwWEK`3i_;nC~`M}{;Ube?Ef*Nk3Ct#Q*78h#VG=w>*J}n zcmn4DkY5mZUXL*fGSB-t;=a<)1)ohkJ@UjI>knAjT~(R&ao#8PENAb_J`IkvTad3c zKCx!?ShHg{Pxe32`om!MtX7tw#wW^LHO>^h!c(jdPhJ_EGU0doZU!|zaq8pa%+w}4 z{kgpA^5DUo+w2)HYBT{Ed&|+0x@MzKgMG!zZ46D&3gs&><4nPw9Cx#F)(305jj;qZ z+AoL~4vjN+ynAvuCiZE$AvkB&sWw;CXae%{Tc)a0lPlV73EFh3W@BiAR*3O(4lp!SyF`hIg#v_|`doYr%yI?I`Q;r8Ii-!d2a1g(&MS)K9buWNbs{Iff@1+|BF zZlN{WFVZhPbG(^jeh+)pf<4=UvR%_`uBg!jY%9?Otx%3% zMvpVq=j;h%bh`AjVEadxT7nww7ev+6apt{UyF%ix310=BS`V>xPK_p*!|2R8zT~&T zX&2TDv@dOi^vCv(GtIX0zP8WAZ-Pg9x3UB^nt+Tp7M(dyzWtkEPnUl7T4{n-h;jE1 zc>ejZc7H)?U362Ph{I0V|~6w5eS@VPjCk z7$hpKKHV(tz&!x?XADiy3iO+UJI2O{5Y)g*6Tyd_VhIW1O0Ef7f!=#&Thn(aM`|(7 zAwdnSG-1}YH3fhC9IcNCL5)w$9^TGe_2Rw+1T{Xfw?=zYucNBLNDOLxV%aP0&C_*u zCx}6fPpqiW!2~V0Cm^Wti6;Fzm`2OLNgHttp+l?F0m^kgFB@+M4&aeVu^NxjObr z8&j}+hs_n^#pjB?;I~9(JF|TMmj9s~ni%^)JF{rqrUbdtC8rhQUHW1>^VVk@5)jnD z>XTR6nMu;OA=6WQIjHdo&WZ7a_FGzVa#O>#HpcM@Y7j#cpWWZiT>k*~eQYz7e_*8v zS|NRPdOLHlGS{M-paxc1KeaU-&U^oV$dx8`Jl@t+__jE~)z!J872-{*-qt*KSy3!u zR{%q#EmiB`Ru*VuTxBFAY{{vCUrjJ}e0@N}bprY$WzOyK1T~1EiK}19IL-<{6SM+7 z?ESK4@waun-CC`ItcjnlNIgyr`T{{K$XiArmgQIm+My}lq?A310#^aYJi z44KiwBqpfw3FI*GT_lTbHH<-`-DPK(Piu{fmcur9SZRV*NbkR|oEb6j znFNF$sarjoVVbZ1(Ow1Ce|#&U27Wbx+DskbJtebOR}-`X&H9gxffYU9)Psy)ow7SvbpFmve6C(z<9^KsRtKJc#J&LtnQtP- z6V&*`MRRi0`-8R+@{jffnO2bF?r|)nH9oPt%oy`zTi$u;7}WTL*^^_+)ZBWu&gT{_iEQBB8C2ex&pbX3t$D zcy*a8_U%(^Tx-V=8~9em7{%;;EWSUIVLaWZmlhLSaNE70@o^W3{B7q^xaY)<(hhDSnSoM z23DGY95<`ka`*(SKp&cZotb(&cj$FFs9_8e7%>vZ&;+eOqgBpd;LWu*SJc2t6OWGW zVHRA>Cnh>q)c6F)vG^MF39c;rFYRf%eBu4xlFb!0j6ovdm}qC`&dYMm`ogUR7_VL( zoonVc{MO1Hw~jJh3%`kNS9a#4#wU_CqBdNqG9)urpjC2$Clj2pf3=#f_&Y_qs`bm zcg7O-T2bQ@SC1WORwwiMN0=)cLld+Deao9;%)|_}M;5ud)WAv;-}E1A)<2i19Mt&4 zeI>@3IaSooT_grIK2fdRIFs7SD~ByPH9oOq?nraRrUS9p%9dOcv;tjc&^YtxOZyTK z)WAv;Wpc-vt6HmF&PX|^@reaf#+h|>)J|%IpvEWmzCO-;p7LvKu58Jv@rmc&9ci+D z{w6&5r)*PwfjbtspmjKG!_iP_1MV#-J65w@ZvRwcGgniCUxm zLLT@*t~oX5Lxz;bPu0-*rzJrlV^s$gPKtGS{t5L{Q@srrO*$dw6VP~#KjuNY~TT(T;b$bnT15n3Vr^fseRhlOim z30vpX*cghqR7(EVfwi%OjiCuzfu4QsD09=VpT-h41~stKMCGO<&Go%LI{`r}(65&r zX%2MS7)#h(=@_&EG5(=Z=Ce=zeNnB^ej#7!dv=u2Bc-uLV%bH}Y)VhLMv zYJ7t0m6-9$mO~S?0)6$OQD)0KTVrEbf*M$9g6ox-7&ce>w}~?P4zO!=(DA7&PCrO5Km@8^PYl8bhk&;J< zOFQ&6rsE>JHZ0qtpS`1FZe#WU5F%1?Xkev@$u0Vu5gR^;t&cc@R-jvM>TkwBrta<{ zxuOPECuI&W%@1w1@mNohawN=^CRjHyL=OD&xLVG_mph-sbLm{QVO6r7y^|f*iXi z^ZA_A4?HY|Wd%jIXPy;JX44OU0m`iy|Pv?qfaX^&J8)ue$ zzS6c`o*as4i8fc%_(X-r#+qvr-s4c?6N9$MZ{KGE2IXxcba z|B?@E-aAYhWtx5K%E2_6<>`qEe5G4Cbpr# ziW;9NEhn7@-LHs9IW$2l&>b$5U%LInTPNl~GxFeAvuJ0&t-(r7N1M|3Eq1k;h_EH6 zFVLD8mNeRQSnI`zBWMMhxs7in+ON*x<9OXOO$!pV5>3zw z^w|~0m=$U2{v#5D8dz!K^x9)hpF{rX1nmnltss|rWsEs=bs~ZqShb(xthA@dwTk45 z{Qy>%%pYa;Ua-N|Ap1>(NWvdjQR5RYzBkGoo1jj;BqV4BnxjN~3_X@``~b0V*%;Hc zySGzhb43lTG%+=Mv^n^Qw}y)&Xa&0Fq|v7Gy@~d(sDagp{iDspAGjSa#*QrqX0Q5Z z*D>d=`^U~?E>3rq*7(=O#5WWjpayu{93~op*f|s_$%QN|sJa z@Z6ai_|?SJ3+kBNZB9hc3UsgK4Na^4i3n<7rHR+?s$<^Ym}o~>6SM+-+wBcavymke zl!F>rX=2Z+I_8dg{_kvSUyx}9`9M-blleg+f*PMlYgX6nJcsAob$w`pR-peG-@rV* zBoRRktTgd=as%_&P2SG7Z6(zB#Ka2qOwHjZB4}lEb+n$jaepF$Ee=+zeragxZ9HUq zS6pkig@+Y2KJoWg4Ndch{e3u}pcT^P3pZwB_Q7bcjl`e^R#T=oG^LyV@;~HC6SF!r zG#R&hJCJdGkXDE{TdvjRk9p5=EI|#dG%;vb15>?@cL$jRs}2{}HPt)(VDo;<8&yo3 zS6w;iDSm{b2DB#Xm#$^{9!WHoXo6OtSx@mXv|pXWC;zNsrZn|-oo&gf@ypcg#yVzs zGjC@*j-VCjtEbd9T~GIS=%LXuXod8H<*J%agVb|_95fTH(S9Kh$*yho{^ieVK0zy_ z-+iE#Nt@*VYPQxuK4*R{^X6AuZEeQoD$;hT@rg-$tC;b#)DBCe9Mt&4>krj3mw)N4 z^liyCK`YSH(}cLde~%s-P0$MI{U=p1sp;wjT_ji3Xupt`H>hQrw)iALuhj&tK$NRn z%S_ql-%aS~+6wYp51nNmJecTo8#O-BEVY*DF;4A3M#`ZHT7fQgzYv$bo1h$;pcRO^ z_nv85Ec5q}wMP4eylRKUc_C0py7B|j!Q z0ig+6fgbuyMYFTZs|jLI11n7wm#%NBlu8ss6SM+-{BvZ~z)BN2{hGv{ZcD-+?F%xk zAoC1cd<|;9jW^Xdlka=O);Z~Tf*SbM#G=^^OxEf|b*>3ofqrmcLo>Rpf9It&kf)s0 z$h5pF_#bkmiH&bJGNqnM)Y^5fXoYx4k2#+hxw_QAN)u(zYiv>%6((pUnxGZvg2x)0 z(T{sO6FEA%wt_rtOw;4!iW;A|WqxDx^qNE`elX_8BiipIZ23DG=JgY6!Y)f*#` z7}WU0<(D@wpQI=va;-E$E6|^8sBey?dSjv;;Wq5AXIehy&V}EfSxln9BOPTl}L2AtqEFzKzic02dL40A+IXi#2mRW(LJXoXa%C$!Nz9ILT{(Ywkv92 zwg08YW^Kij?b$(kN_?%5U!N#bwXs=TlD|@5FRlCoEBb;=E6AVR+Q>Ax%R32Zb43lT z9-Q6KT;H|K|Bx$9m=7D6Hm~_lm;7AO%I5I9`X*_Z_q5OEs!XVX)q%tH%$#C(6}L6&u6WpeASq`p@gio4wamO+aYBI)@phDw#tsdso-Spay<5@%r34 zru!!Ui6X2tK`W&1FImT2J;r~|sWp&G+*;dI*qm&y6+OjYD{6dV_}Vi~FwXzdAFQbH ziDtWMnP%%15gD&EK`YSTJY37%c6XvZOKM;>^uk)^y)`9lIp`_A9Mt&4?n!5vdh7p* zj&PA&QR5RWPOoK#{&F-**scyMP0$MI-2IBlm2JCv$E$SnDrWyvhwVt6kg%ddTmOT~m`2JUfh+HdbV5JGZV-`c$a_Bck z^xJNH7bJ$T*NPg%&_w6ZktF@4&+VAbC{?b{p$#iyCg z+v^tE-|>>)g~xAq#S?d2Gb?C0d&DBG(FA1tCOv*P%$4KS(`N@wtK{1ljIOQViMfrI zeA7QG^R-43HeUGKWR56)eo1gwlc!o*jmkUA#NTp{{<_$_*HVJlwy#Oi8pI<3dCQtp z&8ew%!gAa_r&%z1!)Jj{&h4D3b+S6Tvee>_Kb*^hr6ZlSC^!NIHpVTe*V}Dm$gVg94HiuJBGIxK^ zZ-B0TF(deW%ZdA%gFXSaXq-X~~<^u-_EQW*X1HslC@v_|`d{NkU-3Y*tcEioq% zK`RimDkhmFH~70aehgY6{qGBh7Dj(}5dQH;YqVd;1^trDwr&3D1fQT4($D@n$&~#= zogIwCpaxdy50^0SE&i}Tdy2I7oJ6_OMDkZ(k2vNk)?aLGHjO7w8z;3hAd$ySFg<+lJc~ z_6!cpxZcK~MiY=%Z9c{HU-eQLqv8CX!QJ=V5J%7o#P}J>X3^rOLSp99op}peCb~x;d`)1KmNP=)WAv; ze5XFr2W_u~m41(&zjA_fydj_XeNJj%rHMYBS{~;&QZzv;(2!$dU}UiKxh5d(C~LeA?sh`kNpkPbLO5Kbo;b@`#ZvsZ?fJc-x4~t!`MJ;Gyxf9ihg})nS8J4#tY=tbLC50LB=6xU3~Qk^0e>s(;J@kL3%u~D0M(krgN!Ot+RzUkl%*!&lfqFt_nBsDYIxmUXS39`_BmxRTQfbe+@RQpDHm1_td9{<097 zi|Q>Zcx&b+`$b^Xbpa;js#@<8fNs%C%2ax z94u};-o~IWZ3RzPJy=-KWzi2|jDk^vgKyU#yijX20U5tL8U04?Jx>k}+RuG;flts1 z=_lV}3OY486voIHGdQ^EvxAnPM*C&F=vQ?wy?=0!{>4A%X0jP#3ihr&DZ|y^ znVSX$2L~6~l2fAz$hdCNuj=;teNb?9lR38JnxGY8{P93x!Qo@cVT@X7gM<7=<86IV zqy2)oQEG7iM`c4|&gPqf8XKG0Z=X@43CQ>r$>^7RFYhxbsK4YF8$%PcLX4U6Uf+gt z6~h=u7xWKymwVR6pho+Jj9-q7e(U#zw1L6iH`DC*qclM)#JD!)t%3t@RSsj+J#|p< z)2&Zff*S1?#KZgM6s*qUmoVEj=^b3N?c{u|(FA1tCS>%hx|fe180@)ei;Y2F+6tb! z%Gc}aZ{-&~2fi>k7+brR{c;vHn$X`-b*<#J)xCqe=cdOH+6poLBVXaG(wg5g?K9-e z;M`x=T7nv%$d)htMZf2TN$@K9CM$eV;}fypWS#lSz~H_<>9z(rGJu{ZbueYX`j}C| zmE+q%1A~IgFR)*)<7nU$Xfe?*bT^+iAjsJ^A;9&aMiY?vJwF%Y??E>O$Ig4s#?S<< z;0Y}zI)7mHN9o~9e>+ou;}YpHUk!~RpaHE3$cev~T6*4j>9@U6Bj&5Rpr2dbOnqNX zGL$oUSV=o`QUfb~hZkQpq|9p|?%e|VQ73dkG>!iPV!-)u5foS&5*J+8r4or>q3;95|TIqLu%&|mYT}{vm z#G^m$NNbhwd&1OczmRu6S|dHJS)v%4pcRPUzuuje_zT3;Xupt$|9y6PrQwObS*!_K zfjIZKYUxkD%aK}_Tobec@oMiM(xxZ;$}u(CFXX5DR7)RnEYVkvH9;#7-#6czmbjl& zqy0i&bWyeR*DgsELld+D@yhUO>3xQt70zl>@*Mot8pu^zoSi;=US&J3&{K3|!1aO^ zH9k>O)``mwof(Y*Ada9F=-0Q@NUwdRB60+R&wB-q%v23CItew>W*p=_|r=+*&j+IQ@+QaRjYE+}L_adYi^8L*mcH z1A~nt&avw&YGBpvf~D!N4_cKTpR1&I1_qr*U0}CsgmhQ!1_2L%HvE{P*(1>&?Z%hJ;>{U{`!+dL@fHTY_~LZ${*b+#@` zzwO;Mwj7M@ay90jK|zm7MfMvb)c6FxBoe)kTG@SY&~xQr`#lzFGy(aIL(9_N?7**p zkFGE{IA!2``*jsf&z1Wg`f^=Zj-_(P^5Z$Z?Hvm>uxfDkvh;0z*V}T?lPkxb zS%ZQ`wexK`sL=%E>>W$f&t1#<*nMbF@ZN<7<8nnS)H%Mi6TQ2;VuReJUR=`FIW^iZ zAKHD0^=e6K-22JzUlXeU#-WQigGN` zH+gVS_T7{jPBQcIYb8;Tk1n1!R7`ZE+O{*Qw=RaQb zY+BCjZ)^;R_W@UOwzqhK z8lRZZ_3pI#Wj2So`gBHvw;%hUw*B6O{tXEJ?gRG1qo)(zm|VKW*iVLMZ<<{yYeA#& z1)n}XHS?0q+tP=pUtS0~O=P5}-#0b$rDuMW7(PLbCLkl-jiNIrmtHtdVyu~6s!03N zR*YTpir*!MPar4IXoB%hvP99kl?&I5yghsCFUxHVO+bbfVwC-OYUZ2y>e<&;yTZoM1g#Kbaq+awUzU}Y7(PLb_6r&52Q5+V@Xx{W{mZlN8g;vkp$S?c z#A^Cz9Oj6ejy`$wI#Z~pOnA(wNhEvelpy~&;+dz17G3H$vI78_yjfD zFJz?WYNCiSG(juGShe`2upD8G2tkeZ3mNII9POp#OZP1=niF0tP0$K4-t0ajtPh`{ zM*D?~bXOm@NS$B5xq9)cs;r_Ke^Y@!v(_XkXe2 z>D@cd4Eu*qP@@URNO%2X=IVCAX1Px5e{y|L6SP8%rzgy`y*WAWiPb9?ekDY4NKm8w zLPoml=VParUU;3fscEmdu|yNJLX5_iyWZP&)o1g#JQ z?YQS#t0aa`P^0}qM*8)ZsCjs3O78yUMI%PtZewVIR)~TA_}>Xw`}hPk+An0JziNpd z@8_fpoKvc(ugp{0m$pJW#+6Iz;Yl2bL5iS86OfUf79k`?7Gr3FR)~S|(Ul|2RfM2M z`-O~jSB{;33{9E%^YX0r;kD8Ptq=p_ysMAzgm^-Twjn`{_6r&5u0DR3IuGY4d#yA< zE5yKj<=Pd9*=|;|1U1?(WTd-xb!DwuDI;XAy-g_XOIslwv$N|TK0%EpAS2!NkLl}H zw%8ymfOmdzeNYp$LJX`QTtD{-YP4U-NO%4G{LIwiFEdlKYkX`nsp+ui2m2*V+@Ikq zl}N{G$$j^Aoe))pc>HrmP{TVRei5ACgH6ucCd4^H3|MQ7_634gjGc2o-igZ;;x!>I z`oa;&FEpB9Joml36d^u8)HwT>h7*dkFKvZ%T(=evek3tY%}OnPR)~-5If5EZK*pV( z``+E(5~EI9`Rp&R``X6P1g#JQt>n5Lcx&rnAxaDJeh)`bqy0k0{ipkS-EBhr*!}CQ z+tSJxYhT(5>1f9@I^!3bMhj7<$Jbexg#EJLP}vOIslw<4R63-ao%bEL#YH|FuRF zkU_XF%iS%Oy`_cVf1jWg(lLsYDiF{{X(Aj>CCWy{3PZ0 zR7(9+*rPN-E5yM1!S(Zw6610q`qX!QkQ(h5^6>hz?028t$ndIAUBZz;6SP7MtZLkN zl_u76${}nS@kAlX9~fIcjKM7zvb#aF2s}454A=Uka4B)RpsQoGbBcTsloJ~U&j%& zf+w_v`yRpXM%^mJJwkjfBaqf;zmU;l+?SMZ7ve2xAs82Zf>uaJn|^LQem!Zkl;eA8 zyN%KvL5=nc87<#^>-Qfa?w1xa@wdh{h9+o*80ed~PsMK`wU=B~kv>>6B&gATA)^Ox zly4-T3h}A5ko_{VXkIDWBb1;%JTb3mG%D>mQ9}1#s}!#@Q_!x*kPe+6tbq`f&Zci^P!eD!Xgg z2dU8nWUicK7f0=11>st)ql_8aFVeA!+SLrd*5eb@XaX|VU6xoF?vWK~U)qYXbM5*{ zV#E+A2Q->sJU3F$ko_yHMGIs%O8e4QNXIoyvFodsLPA!rmY_xxkdf|YoU3Zp3Wm#? z`c~P$(!R76($N;W+x3-CP@@URNOv>m{_f*MUgM!H+cT>E}b@MXAvrG05Dq+?vkwd*UN zphgprk?vMxAz^o;v@dOibd0X996mvfCLkl-m7}9vt0m$7mG-5rkd86m)dz^hil9al zkdf}{qp#H9&9bjlD0>asm$pJW=0Vr4e1aNHK;|kcIq%ZiwSsUj!^Y4Atq=qAx$7T3 zL5=ncnX9Peyb-b+74FH{9;FFdAqG|-uAln^HQFy^uA=Ntk=?(Ny(Jq%6SP7MtfJg_ zC3{N?ySd#cOHiZzLguy&4p<=_*X;$n zzVZobv|q?bcQejKvVXN++SHY@f2DnCE2N|T@3-qKpP)t)kdf|Y&Ie>?qLZ}U4OOqO zF*HFd#6W9*-mb55%dbmG7vk%Xpho+JjC8jGXm$AKlw{ezdO>zrv@dOiboAOg?E1Z4SJbt%uv9MxCmLhVai zAsw@tYggqfT$wUjh+9H}8cje(x@%W$WdCZMSoaf3`_fiO$L#F-hfh$W3CKuy{X^Ca zcK=G&47S&5f>wxu)rafnvSx_vcxjFH3z_S#$o*ip+z)2nFPh(G%rw~9!dcfHEoT{= z5JCFHMKi+_@$bnR?o?S%cmK)})Mx_o#%pGU1a`ct2=R*CA823Nim}~OfGdO;F2s9Z zIRd$YMiY$ZPC#5HL>*aKHjuj(?MquB{hi)3jeDArD?~XVGUU!kYcv7*mz^`iGxTGH z7$s}!?sE6#6SP8l-(P1K_f!UVUbldd-3hJHej!gDFf%+6pDZ!1loj~wj?A!2Eb_6r%r$G4Wta65M$X)C0kyUdE+ETzO<~wuzx%#F_MM&AS9^Kej)Si5m%1yq}0EJJxUX_LX4I(rrO?|oYzre zTqbSm=8&L9`-O~meq2At&Ng<6(xn!)FKvbNnd9UgBYU0!Mw?jCVP@@URe48gZ zFC4Fm7fbuqzO)r%yQcu*cvZYU91~Fvj-vF-c<#(yYspc4DS5R96SA}~ZH07PL-#ae zq7bJGQBwM$)@TAU-;PMmn=ZudQiEM&55OmAg>uaJZ+1_~ z`U$a7+EjTN-?c{jg^Yf_(Z1D)`gnAMG19i;2wK4t#ufLpZ{pHg$bi=bE4uVqhM0?W(#Ey`)XG4hd?s zU&xr@+!^8DWxU!i)|F*u)4sG7(lMXA{_%vw*ePvldq_~D3CNhK-TB?;h1e`@>Z`Cv zX@XXW!8K8G9{PDlxlS!Zf*S1?GFMKKd$nwwHF#OpY9R2eQTs(YR#EQ!Re07QLQta# z$Xs{Hxa&?pWa9*c_NA>D+nv86fm0S*qY1`yE3ymLu3Xqv*6iJXULHqqbp}tkhVJ}T zt+nlf>x5_<64YqFkdf|IWNqa9Rk)@u*1ohA($RX{`758GMiY>c?p9<+b^gjHXoYn2O?UnZ#CzfSE3MIfA%k!$vg_ph zRb^>Ya<67{MPJ$qo-nSs^H(6wR0K7efQ)pvBD+=y*(0<2SK61hLORA>R}P<`MiY>^ z?vnY#U8^FTRnxw-71A+(xccx3YBT|vtEl9>u+EEd=1%+4R!GM@=-QP}P@@URTty}4 zg)OmI?gwoQP0$K4FrT~r0b-2ghy*p-FJ!KwlJhQ*9wmLSn0-(av_cH5K3qSqExopz z5beS~NR9Rjnd`2|SW?WfL=&_^46LHuc%{!@X^r*^nd`2|m}t*m`2?+4j_CO-?q6w* z_RDx~MRvy8l`VQo$uE+<2A@DVV1;yCH+R0oC#cbWAtT+*II?@5BKz&dvfrMqeQ7JC zqyF7F8K0m=6OfVaX3it-WDVM`jiCuzAqHBzJMZ&nNXU3)32L-o$Vhi9fZgs)QBgY? zt+g+0g>>{pho+JjC9wo!j_0eXoVP*v|BBKxnbr#q|tE9^~k6^7FSAh23;=NZEL!ECu7w0C#Z zXaX|!uc9Zmuzw}@gOU9!XtWh`<(~F&|4Qx$ErI+(qY1`y=dZ&1!ECu7EYiNT71D9t z+|xenWmJ&0Xi9kROpPWWVYSI}qz zGS=)qKl>@s5a^j`vXSILDlG6$?FdMk~cu0t?Lj0!ouXuGyAO`Nd zU439*bgi_h9$}r+m$pI-%vY{mh4+I+azALVE;X8fj2#5muEP7lV!0o*{X-M9LJZ8# zu74!jzhY}Aff(2cas313zzMzGVUMCOZG{+EKg6HE(i%-b#{QM-=h(k`RsQ1ql~2$L z=~xFv$1B-yFP8mwOHiZzLdHH*%y<>Ef5q{NR*dbQ_H`EjIMes8+P~r`N`mp+`Kxwf zE&J`oXQ}-w5-119U0g%=G-I3)*uQEJ-aAvH{X#~4xbr1)mZB4=lKDw$(dJ4M zv_cGwEADCEy(ote^1mgh(S9K#-JQR}=yS;ilPUkl5wt=KjJvKJJ%!jRqp0i{S%Mnv z7qZ=%vgPO?#9z`v>Z$!J_|jI0f%(JL2g-4y5S7*b6|XM+g2v1cfBs7Q(pE^vJm}h0 zC5dsFv?&?kY&odW1Z2#Eu3d%u?b$fBrG05Dq+`~0{R8`=zsvsBr)vL-t(|^BV?KBN z13O;dN(F6KsypK;% zqY22-68cje(x|=yqk&`uY7N__|*~`$rv=!3P+TD4digJ2qj1aem1T~s~jC8jGC?V&s zDoLBdnIi2=TOl3&(VbiJ32HO}8R>2%qfb+5U)l=k7=7Z;UulgdAcJr#vK!=#SFiB> zL7$)%(lI`|a`*%_+An0JyK;mjx96|4FKvZ%%m%JLKvWD*Y-x=qAcJuAaY}gpigiw3 z+6ta9tGRYnQCf-Ac{b~u8cje(x@%WqOT@eSw$3#{E5yL;96f&}tvy0eqy0igy6Ycd zkFxt$+LyLMI@S-apZf$gnt+US*U#nWSBk^mh}gS_jB}Qhaa!H&w<{tER@C?e z&WoOypcP}g{dPYF&Wk|ub!b#qVR{20{0N)xEr7$Oow6SM-2 z>*k&)`Z1`1l_pTLF+?PWCTIm3SK2)p^F~^Kn_n#&iCgCYanX^Yo8Mnv;vK} zBz|Pj8pxW!+ULXstw3Xbj2{`a2C^ow_Bk;@E6`X0#E%SG16dPT`<$4d6=Qwb zbnC<1$Cile7Be#Bpz(sB2C^nl=P?9;ID%H7aou7@h8z;qz)BOS^B4kvjiCuzfyQ-< z85wd&Py;JXpw43m05*muXayQqI%Z_ZAwdnSG=VygApqDInxGYE)L{Hr!jt5SU<26SM-2xx}s8{TS51N)wp7PE61WG-e-n|KZ1=23DHD z+;w7tR-pOrNqoD~tq=9jmWb=-b|%7H<)F307d4PIftrmW0K^fr0*&kDc3Auv)WAv; zsM#1I5xcNU#3yKlbl#E0$DjsQnm`WY#}e%eGOZwE zE{Pu*w8kee+nt!86==*Q@gsxQK-L82t`ifq0*$#Oeq_)Z$eO_1bz*{6pfUTzj|^G^ zSreGMPE61WG-froC!<K zf*M$90(Blk0I)GMK`YR>ZZRW64hd>tr3ut|3<1E#&;+eO<4VVj3^^pIft4mu=P?8T z8$%Pc0*xAUV@b#1XXMN80cP)#ldP4_t6#XIWV6C%Bc>OW{$gz5cOsuJaz#l${86*Q zDKnmycV9awLTfYu`G^oNSmLxv^=ym-vWM&wv|{X&ibqR~(-omL+Argkw8RsW&dVPl z`;{eS-_j>g4pG<>-V`=VocdmBfq~8r-lSI+Arj`Le#Uwmk(CRZzMZj-^gyN z_NA?mUZql#!WVn@lNdfhjV2&h6=I4d=5#+CY>+*;X0ng0eQ7JCAG)S~;jUF(B?gGk z6hVz9Apa)BPD|`sUl@EZr=y3+c~0$1TOqyRqnd?xzSKct>=xn&MNp#&$b~}OVTtdy z+#Fmow^Y{Ea-LfI(pE@6u%$}jnZw&kjGZ1qjV2)fB19!iY`OXAg%YEPF*HFd#Ax_q zg|Hl736Z7%>4U}WgVbohkdF!B`gyD2XQXzK zHZ?_RQTx(XNdJCsQaD~+91<^t1T~s~yhjK(UY$9qo{ce4+OPJdtr#0)WpbWRpd1`U z>6h^^q9*4(IqAIA8>Qsc$IGXDs#!mey#$kkNx5vcy>n-c5O0<|mw+ z@CjNW9b?QZBZ?%3Pf(-%LdHn+vL$i~`yoaaV`zd_h=I}7mBS~f(S9Lg+;!!sS*>-8 zjWSZ-E;F0lLMIpvoRNHU)l=kn6+L1Ac471Ycv5F^SSFEcMRGdNFU5*AEYmB1y5WPCFhY~ zAEZVTkhyY7&bwJwOMRqGJtxm0;j5@hi42YmMr4czNS}9AQifY!-6{lr@$~tSpay

}RDX)C1fxT->iTVMGEHJX6D zU5Lc%D^1V}G2YCsl970QMUD0g`3)i5`l?t~OINAYk|t<{7!O`hGc1QsP^0}qen1FU zj#rny(_*!>kUPU`r3qRg#)2jF!us$DYP4U-Zwuk-qpYm2=E=I}%COEgK`X@A_+aC( zU6m1Ht`J>9f*S1?@)tt5c2%jvij=FwdQP~$(gdv#qeRW~!v5hC)M&qulZ0^nqm8Vu zu9fT5DZFFR1g#Kb){ygUZLFPj3OIsm* z(FM)I@v28iv=0etGyyqZ2sd7pko6T}WQ6yenxGY9yY-b%pd1`UNid#UUp+1BtE;5s z1H#pkCQw?AySRpK{_qKEv|q@$(r*6vN>)ofq|RRoXBzo?x7cyqJ#Oo_f&TW9V}HTJD8{-3Pw3U{2M_l(75W?B%Pjspaxc`gDt0>VXtm9S1I@Y zRkW;1pX`2H_hh1X^6uxoTF(~#_o?*4`wq>vcNX01P0s85!=FXD1N&s3RrIYTsL=%E zI!iOcJ(*5IczV4F!(u8@IphlMoa`X@&&Zy3e$zODR*3QPwDSs!zF8+RM%6f4 zwDQ5y**O!MTY?(x7c%eFlk;8~bEN3|UPW2e+jNQ}XoVOl+4T$0Hyb6!yeEzpb!#;* z>xE%mEJ2O-3pu(oQF!^0qE%BHXJyUl6-UqtF-DztcH!cbEfVAY14oMv)!LK!(7@i7 zpho+J%)OK3yjPQt6qQ)g&)l5RH;$kcV!Uv$eBmpneJL?cZ~ITt$EEg|$2z+h)M&qu zxd)V-ch{!FMF-nHUbJadHyc9}v_g#4O_B@qx^0jc4^H@}sC0>WMH5=OYekLr3z>Ua z$$8D+I$X4R-Oojj9>}mUG(juGxTjZA;e0K4tmw#@U5YQ?HP;fsWsX!WbWK1=k*(MxF~f? zUwg9HCuqg=r2gY1#@S(vULSsMv_|`7Ja^~yo0Maklw_rH;jD{}DAt%xDl z>Myxg7wwvB32L-o$lQBR&Ko54akJD%*1Sz|1g#JQ^;G(qP7>o!sq^op&JQ#yU92_Q zFJ!bBcPIRo>TUiVV!$hf*S1?GDae|!!ldOtAR3J-I~!ij-VA{VC36h@eGM^uZ)T3*4dMJ z-#{0G8toTy^hw+b8F#;taW`X5FB?M>v_cGw;3bkOON_T=Y`;my_DRFswW3D*g^ZcO z)yLa1e{6B{hmD~LS|J8zqHhWtNQ@_Cj#?pe)QAbqEkTX;3px5^^bwf{OW$=y_Js4B z#u2na49uuQI%i0X44G>i%Ut{E6Rz!2qy0jTKHA5=YPqF|cxSD;ch@9^JafjwRG+ zzmT~mO3wS|p1+DdlNDLxEpA1|`z822TKW5Qu7jd`$US6zb)T%Su6)<6$f(f-73r`XQu1R`@(JOJOy@-B@a6CJrMnebYq`3+6;c3b~s2?%{N|BC8>-Bvo3;$Ki^M8toS{?~3KCB+?QaOG|tp zT#;#lR*3P}h?6p+E3z-8wci`A$f(hNA@lA&Id6vas0Gra&J9;&nxGY8-1$|R47Vbi zEWP$#wIZWN`-RM1isZaSGBWI!k>S#CMWzW_Ax4d#&&+TuvU_Co`At@2!@?CAHQFy^ z?zYJ31R3F4s1=zeXoVQ3G^v#lU6CD>71@YzMMjPG3z@q{vQCtd`a4;XZ4OstnxGY8 zeDO=847Vb?M`nX1N%M-PhbuB_v|q^4J+eA7^yog39_Ui@mfBBMt8 zh0NW^FA1#8toS{Mk2Q&%aieHk{hpV$u&VM#K1V_R%Ca0D>7=d zU&t6yT{(=5yC+R+oRt}_$TUGK#K3s$R%Fvwxu`O&S&FxO)D zV^g>yqelCM%%{D{c|Xd0UP9*c>EVh@6SP7M%=2zV)cO)T`vVS8@B{ zg%9^1mtM2}_wT^(Fp+DBjPz++pGqG#^~ZN)H%f^1il9alkf#dqkR>iN2V2Owz9O9K z)4sG7(m!1OLi#fc4@(T6phgprR|)a7C9=+YCFNatx|AW$y|gcFh4fla%t#-;{1jOs z*UtSlWw{WSgakF3fLt>|EO@cwKcQYh_9TOs|D_g_vwxBHnA!zZZG1mso`;?=n#-!&pL-NpDP0RfFBC_CchLH%qwr2S@5qx~`-uE484}WdEkO-)0s=Cw)sL3=d(KGKM;t*b#750ltGG#G_yjfDFJ#ntRZG-wIWe_` zw5xUUyjJ_tR!Bz+9s1SR5~E&7d>j(gXaX|Y?wyuE4Dnwqacm4t&|DYoZiOJ1g#JQ*}>w>yHqG!dJt+7~oU&60BlEQj{1e@-cIbzVk@hZRAM_6xb45U$R9zELR!?UwCI6SP8%Y0GC8xVGyP)M&qu zrwifQ?)cX$1(l`U{u;J+P0$K4mL7hk!1Y0&pho+Jyi5q!2j@P#IgoKT8|TlpFKvbN zqO8{n+*mR%Byj#*Ycv75SO_5hBrZMhkVs)Yog}%US2_A7$+I30ff?EzzwR ze1aP77c$zkBV+}TAGTe)bEgSfAqLu5WX%vmP^0}qMr(J(k&f;1yGz?0C}XeorLB;T zmgr_@pP)t)klDs;+YQiev2K8dEdzda8{--^o}fk(khzj|&zoblPAaH zo84$;PbgVv<^4CBG8K{v-8 z%dvIZ>il}?f2C-RPaGZJ-Q2tH$1qo^KYg5kTGfBCR2buj8|S}0YgNxBTBH3U{n(HjO!o+Z>x4g=&{mLftzIm7N*F_ob4c8B z@uOzquD5M#FFhy66bD~h`4o|n9<4zV)WEMM79Y(u9XhiQ!Ww^Ir3qRgedq7FrdOo3 zJNu*@Xmqa1N)DMT#*5EYiLE(i#Un27e`MsEHAQahIzB-S;%Va3A-U%FtSt#jt_fO! z&U`Z0RKI?60z&8N{4;V*gU`3xtIK%t<)AP4{phV+v#sLBn7WB4bgnc}VQsEC^=U442pdBakQw8w5hKl>ibrEhZp%Rpdy64W4uCej9vG-ao8 zy{oU4CTIovxAr4VqjP^wQ0LUZs$$KN=FNsb{tvm*MEhO2=AmWS@$&ni&K0c?uidO% zbA5$f2?%OnwW5oR?bm;If?T!y=Rxz(y$7w;k*)WcnoSp4dFZ=$+jQhADQR{4fyMVM z`#x*t+imRGY@7v#tk3fD^lvPIQ@+&r1ZhVcS>CR=#dUSF_32?vK<0Vje?XOE9hXpK*hb}{O$|Gud7wx4*%8fi{!{eO zlm~1%sDWR7s+{Sug!bhVq#bdz`WxoFs*l-nP@@URJoojy+;%Hymis^it1GcdQG@78S2}>1gU)l=kC|_cN8cjfE zx+}-LT5p(!RUa$X8p!(1ju;||gpTeLq+NY%p7WogUnV_Z%R!AMAfsL3?U8>cXoc96 zT{&94^?lKRP6KT@sDWSoHV4yP3=-OxPmp#*^IvNhci7(EmV+8iK*lJ+_dk-7jz`c6 zu_?Q9EST7?_@Wo<+Hz0>zZli39UK;EA#;$EJA?6gT~Qe3AAAS-;H_LnM*VzI=kTE61psj}>RtnrA>zqY21->*n7H zTEP=#M{xGk8ts?qc$+6N0gWaoM~R`;9xGn)^6Qye16jW-!}ot&3=%rJPmp%R-h~eo zA9`Vvjz`c6o+u+mKL>F!SmwrF{sggA@k|LzZ0|q!E^8N1U1?(;{Mxk3%%*(TB%FD8a1sDYIx(0bgNRKMiZ_ynKC#>dbEtw5tqyR)@^ z3~FGd3G@~B-1*-KT7gD?bWg(-VN0%K&`S+Ykqy0k0NaRiqs2Gv41irKt zWS$R*xmIxmtw3O`Oq?rfv|q?P4G|wh6ST51BIl|7a`1Gt_6wP(IwCP5tz8qe0>QJ~ z5dz)AmYf>x7cx(${5wG_5Ydx0VL2kX(lKZSf~VNyV^E|0Lgv|@2oZ^)30i^RY5WL* zg4>c)qy0k0X{y9yi6&?Tf^QW>VnlL9jrI$f?>fX2)c6G7p@=7_@rmfEFuz^-1jk^W zTa3hrTq|lAg9Oh;#uL=|1W#_KXSTdH27mdf*6{h6=nOtTgdm zhu#^_-C4@tPgK4@&xmUc0_y(YG9=azOxZeP~#J4EFPSZ{`k`ga-|7cfkwJ}kHv3S)WAv;$l>5; z@V>JmY-`s9tw5uEF>BFCkD`XPcKeEGn z@?&dI6SM-&m0C=lM@ApKUk58qVC>pCFYXzO@&$rckTF(1Ik-Y}O`U^g0u4s$0Y98+ zSIBx@dVGQ!(3&`YIeshAxR5b!uaR1aoPhrSt@Jg)UwVkYR`dnGw1UjLjCex({c^~q zg}9=cjwiHVO}sezio&=$w^vsav_g!`8rK!Zw8Th#Xo6NC_&XZ$xuQnyX04vw`RW8ZA8(FA0k*mNa79ziR3 zqU_4?`3>?eK7{kn;)Oh8lONpcv?Lvsl=`K zWw-1&rHJK#MiY>64S6E|_yn!siLxt4uGIO6$%kwVYP4U-XbU{2pOkbwf>ww@*~R!` zMBD7=choD^8toS{T02e~|2sh|c%tlxC;n)Vo%??4V(kku&mkY5&>Ej0?P6@HaX9On z{e5jYsL=#uo=N_9f>wx4*%9wlo|e`2$h|fOHQF!6M}2ZRDJc=5tuVUA691jIA@j7n z5ye`g3CQ}avwTVCI0UUY=JRxPQcn8^H<(2dL;HfvbJ}r45(%yG3DWLbE&1WaqT(j^ z*>X^$3CKKQ{_g~>5Sy|ij&}d8sQkiyHU>4?FXaED>`dTvD&PM} z=gjo~d42o+uGjtfyszuN&w8Kp%sGco_T)^^g&K@)Vsnv7R_1e$n;KMPUK9JD82S&w z?=m$ckuoy1nYD0Xv{@f`&I9|J$t#``V!K^~gv=E}h$oT4!g@DIvbGGonrR5XA_>Iu zNu=xvx{!&n-K*Mf4z*^!I@{EsBJ(2Vb4)oCbfE@gn;4>!ty-6NE2+r5CKgXSg@t8H z_+2Jb5K$vN**gEk-bOMPVm=v^Js}k*LE0Y2nb(I}2S?9N1VKd-h~;xdVPV-3bRiRC zyGFjmBR3uF>fE>zNO(>2AhBMG}bl#8A!zU8v32ChGq+H*;r| zp{52EnHMpi7|NNT3pE(q#5d!=Rqv)Hn;KMPUc|xI2H6pGp$219Bjtr_Q`Gdqvb8sK~sC z`P5C$1YM}X*e1#}NU&z4K4@xCk$DmG*_@mSx=@3$Od2%M` zLJh_?vFf82^&4W3n{yNunHMpi1*gFqMdn3}WrZi7awh2FGRz{L-uj}o=F^`NWiG_>`6$wM4H7b!lOSyq zFTB{^Dt)Gq7X%eaAm)=+ITLgt6JwjG;!UtJ$3AFkP?32N^LebC3A#{&u}xfG8EGB; zwYjN5Mdn4!C%AGZ=t2#~Hc{)>zcU96((k|)ip-0cPjlr=(1lElZDRGjHJMw>4zkyW z|0y$>7crkP%bB1HH5l8(1IOBAwplRB)Sx2sBIc85ITLiD24kD3`Ti8uVaPaBgNn?H zSU%wv7M2}B7iuuJYv@~wDf)kAenka&sF_>|fc?lH|oN*u%+u1A4=)Yi=ijyF1*Lb4l7wYhF``Q^#RN?775Paec znb>aEAR!eeLE0vorq0fcnL5<;ii#u<^U1fI3A#|5u}y637iLW!*2UDIBJ(2V^LRNE zbfE@gn<(&6ZL4~tcvFLl%!`;$3Fb`Dg&K?vkutu=Ggi`%Ma?>=g1qv1MYh{DNXT4H zg0xMXn$y=>GVGw4GfGq#CZjbF3=$v4W>pd#}k=5wAo6Lg^lW1A>j zbDXto`Vuo|pd#}k=F_J+6Lg^lW1C1x7-uyvw!mCFQ$b$&^TD! znHMpi?aY~=3pE(qL|nUi){3Q5%X!#oC&&+iLp(5*YrLs^{-B*1{IkXF`p&P znV<_b7~4e8n`xO(935h6P?32N^BL8g3A#{&u}zdbw_knynSJd{Mdn4!r)YB~=t2#~ zHsL=G-I zwIt9(#27i~0HFc=%R1M(5v1*r&v;{ib^7~sGjHdU#TbWtvY73`1gcYU5~OY75#6U)fN_FzIPPJ*-{QZQ^TD=Hw^Lu6vRT|*Q4pS`T8I0@1qvYx@h^ISi_ z_tob0Ex$BRnVv146yMPQ#OaQgdc>nWl%N7y5^uI09A7E_;r}4$0{z9EiSb+4xYeKn zmn1UBjEg@v+^vQr=mPy*|2gr;Pi%GR6&1K7(P8)G_%Su?r%Z#Fl_cl_{nFVL@kuwf zyVRfpmn7QmeIx#G?9u-q=mP!Q0V{q+v0W|%6}TkvT>A9*)4hKD4}vbxs|Fs3FB!kj zg`fhLB)Swn9^Y*EH!cJfCs8l;o%kKUI={#Umn7&y`|hs4#y6h$ol6ZWa7m(i_21)* zo^snts5pr~;@*o-S$y23h9u|${rf}b;)kW3a3QF`C5iFd-;1v(i4^R&G8baH81wn_ z;4M+^wLj=Rn0qbv$I!hN1acuS;*Fcjn;7-nYfy0#Z9dA32OU~N5_Eya66ybBgbP6h zE=eqX;cWc;?xkG_Do*0BpH9bL>{G^tpyDLPoIe@guwz*lf{K%_hyYeuuN8Gk6Nzes)Mwc?4K6l*+Dsa`fUdA*1i_-sxUP)r?t+Jlt zW8Jny*(gBTOt*>F13&J^xs$Z|IjN*ESMDKS^2!%wkvx@7wTQfU&(Xl zJ2!#~Tow1ndJ4wd#}Z}=-L9xOiM*dY;OV!*ZEL6EBqpt_=9xNP5P>am#h;tZ_5-5f z&~Bb#{-Ygh2ldMAA5@&gg8h-6^bN(cw)VibD+#(le|ogMr()|8!Gx(Hdxe<2Vh@9Q zW%du5moC&8weEcn-{KK`HlX4h$6F6=^yGf_kjptr5_Ex%DzMt)KI@|bmn2@E`y=^(OPk`or6WD75M;|J1NdmF^Udv+!hzi=3eyzm+ z;n*$-o>2q63LLMBU0mi{lDylT&p)ZR+Bc`i0Tc7BF`)z%^hy$OG3$M+7ygj7tO6sK z1YMx{)|k*5RN#`t=V|G_1<_(o9jGA*x}GB%U6T;cK=2Y}Pme%SsY- zf#zFd0^b(NcW$r!KAGUNfh6c+ z4Zcq%v zgP;ras0+({yVBgQiewGCK=3Vap}nFa^CE8BccHJ+F}Jy>BO5V<*TVJMnTnI} zo+;*YzjmhLB+8a3Z53j1aVaq z=#?bs0xjQd7&u;;8dTtt#4>$0sFhD#;{<9*f-cZ}=V8z|%nX1ET$11%H9!PvNP;fV zoNVVV`zGJX!Ke04{XNaJ56YMxA52IEv?LJQL}yjsx;wjPBm937 zyOLspoP1N242^+*Z#JRM@2uYN-M%?i3knI{@p7b4;l-&ypaA*i4R3B-K%U4O|erJhx2-J6Mf)2f9K zbfHF(y^kim`gUnuqyF{=)+-ZcCte#GV+blTFJeBoAD$Lbxt^6&&Px3ELb(uvF4TDa zXsd)GHSu)+pdt@j$D8j@tgMAZXUpT32JgnEp)uA!o zUK3^;f{M(Gm~W}D*ZKamYTmnR-wYw>LXA!D4@el|*{N%k)!Ws~MGtyaM7$xW$h?U8 zMvw5cAN97|>E=V;(O0U65Okr&wqFM)eC}JJYwXke$I)%|ykSd6E2+r5i20ro{iSie z@9F=%;d6S15Og8a&bveWZ#;QH6Z$`IkvTmZNk!&m?I~+s^7rb9?$!NEM;ikDg(3;| z(0*e5m>&7_dgQO@Jy+(EF0^AA=2=@u*C?l#uHIw4>3YwViX;$Y-Plj8->3K5J$eo1 z(tECxpbPET79RcOAN{tTc)gWe(pyPSz2{0r=0%Kc%zk3MtlkpO>n(Ab-gBJHjD5v^{`{KWqpIpX>JPo=ItjYaj=gsN!Ejw8N$<7Q z^j=#`|0flh7cusE`}y;$y2jr+eZ)s7OBX*h;i() zpI9%Wm(`1UIo;ID%Sq6Mb{s3mw+qu}92`X_=yid0A{ChzF^<9hC)V{kuc+5I*1MCS z3+*`OpZ~psu2DMcY+wi~GB0AB8SE$459w_;Rd2W5^!6)rNf+92?&^4KvaYd1@3n20 zn6sK8s7L}a&%xnoUG+ZLLZ3N1>HSdVk}kA^u+Ozu{pWN1f4FcK=Gl^OZUKRIoP+gu zMg<`i)&R{KHqiyglG#1g^Zs7(OwW3FE36@ASIx*8dCw)Il|G-Fag_Hfl0cmGJ^(D; z0s45@-M@4tK^JP=?huz@ufYm>4W84--0Ax0AQhPxG2d7ao`xesk#%ooe&ydvm`l2l zsY9P88TOWlBf}7V?l|w?5~)Z6G2grpo>pFO?UVHR>70LSmjqp?v3FSO41162t@o&{ z`W%?%-=nC=yomWei14&O^ z=8`V7Z`j#4!#+|s(Cg!@o?FfEAE~KG0x{n+qUR;!>RCVN`CESf*+3F>p~kH`12gP1 zP9eQrEzons-TpHU6`2FWr6q*%B2=AjZ0}-yw8eufd~w z4d&B(uFNG}XvdnhXMOebR-*S<>s|k>kBTG^V@tH(o`k(tpP#JG{+XpD=t2!_)AsCj zfZp2Q)m!@x|Lm2D%!?TNiv1R-8}b*AFKMSWK^CHGR zZ@-%gH4f<`LrwooUlMeo296l^)dbGxNAwsl7OBX*h;i()-$zzXFRO8SIqCUN2tgMz z;mBuS72zoQf?gMw{Z~a)WM0I4TUU5mxIR*c>Gi$Sf8`|!x=;g0aQo^GXM?kPn|jNC zbw@?!Ma=h<*;kN#^>)iENJ-Fz8aNYWU8UCh$eh*8$f?M@h+CH5+7NF)XtN|i)$frf196uY4FWOR)1N4)&*yFg6en^1sruOvRGb9rJ#>Iykkofq z%s;rC1YB%C@%TzTZ+G?z1QgaFff}oi{rLYuz(qQ5A>~4_zi>4ilVJKzBB(|R1|k(F z0Xm2XjC{eu1cmA#5X&)T-zztQOT=DQO@3~={?id1K5wS;dPe0^)`p#?MuYu%)bTDS zO+0l$xLUvA?ckB8peIxiOCsq)VU^tLoh%|Sa!Jqydi1}ARE2YI_=zswQtR84J{#04 z^zhh$Fg4-zuY-DMkE7WOmDZ<@Tl2Z>i6jtz`&^hRGx&Rdjk&99u0P-V`4ECG5S5FC zscHRx@)Ik!J+waVmC5U*g7|~K?`9lIKIPJ@x%bsxf27N&^_;yT0eWBQFm+=IxAwL- ztFDil-@U$*pbJEm7sJ%0pRf3PHS@3X>mUF3nqW#RSL4}2F= zg9PYbX68~2cH3`lIK39}z^L#>mMv)r|@E+2Ck$tw4~(N>_Sk1 ztKh#8s$-dR?!8Ju5x8UxNenJhS``lG*RxHRTnM_5Y2394b^aH(IRh2A8ZL`a*One~ z@0BUz?3E-IREtp29rwA6T#kb-WLo`NDK+eh;|+sos^olOj8#$_wnwn>a3qb`gNgTXgOwIrKFR}zbh-miMLj&wO2NP;fVJEj#<)BkiMsK6zOgk1Sl$E8s&H6%e7=;g}` zs+~pg3zl=fO9d`Tym2v~Dp&^h;c_DA0^Rt_0_v;QZUhy$B=N?P{A%h_2Vss^GxJ2L z;mhs0NZkUFYT)JDW=m|AFG?-!am!^5Qb7$#Y^@)uBCk8XmuhNAf-cb8%S5R;+wQp3 zpaPd9a*vNxug&MTu>D5{Q$rGTfzDeiO7(u&ZC*kJE=jEUBT}`w=s5108j_$3bnMGf z%Cpfi@-V3&mc*2S6;-Ws;V$c(xj@i`xM|)fHScZbH(8;`HCQQcr0Vu}@Y;0Qu2x2= zipTAlUWKS=bs^uq=B$=|4JxQ7iGCST>Yv;9xQs&*bb;phf_i0^uFSh!^l-$CXw~^A zw{=bhc_q=gXpEXS)a_g=3A#Z0!ei81L!IB@l?q}>+_@d2x?Xf6s5psBH)GT?OY|y5 zZ@*^blAsH8?>8%{emfjB3_%4hNgVvGl6q!@qlVcMB|#VH)1@k_q3zvjP=QMl(fcc_ zIcWv5);Y#uYDj`E&`m$6tXj`;tU*IiflCt8K8{sA9~1Ao57dwZU7(v6k5zkTi8t^E z2r6(%B7J16+WNHHwoAoHSOcr559F7W12rT;7w9g#VpZN8h$mF7bWm zKn+RI1^UkEST+6FU6-RM6}Ti(k`#i~Q$;z%7JsK6zO#rdkJ znGtU14=PT=_h(gg`3JGA0yQK-7wC?KtEfr$<1YuCXHF_`Nn%IAYU=Mgf(X=*1YMwy zUaP8#=W>2^T2_}X#5KaIsDds2a_JQnC!tnWRfh^(_z!|E&<&bYRaU~8V8R?3@}^f& z|Ms`t8S0EYR>1bgxLqFAg?4wUXE3T4>-P?5kkq|YeyvqBDa#=}&E<+rxsCqu`xS}^BujN-{Uc`gnk5apS`_83TlAsI3 z)Im|ox9ZS;5Ojf9*Px=BWc}hoP?32N=V}|J4!8LBKM1-&NB@=pKRg`gtyA|Bi@ zO3m1C@IMH;K;&N=sXFH4-CDUmsK~sCw~UQc5w+a5L@G|AU)3np^5WMny^;i7psTiz zROhd_Z6#FTlEj3_D3yHC?H+(6=mNd9a-{0r%&k{c;F83Z3Q=lN!96bHkOWx z_U+0dQm}-jVq$+@#MPTds!Kz*Wf5kTNX1E1su`sUcEw+}`Ome18q5WPF2vCjBURh> zmJ30}N$fbOx01IvXAyy3QE?Lcnnx+?ytu;>=#?bs0^RtrimG|tjV?8)z$J;^uSTgi zwr|ZM0yQK-7wC;^^Pg)Nzett%3DcAEBgS#T$xopoZy{BAql!b^ebCc zoj>Z>uFTSLZTSff|ya3&i^)s;Y}$JB|!yol}u{5%*nHRn6EVj?}0T zhPN42Z#7ZLHFC6Lx3xwE=g?rZi@Ok(=iTHLlSg>t{#)BHY{}9*)}7m0+%G(bWBz4hKXJU#vuv1 zK<9ZTRh@a!u}7I+QGrVmy{D(D{NZB%2-J`SU7$;jPE|?aj$?_bK?N>J_*AO$4G_Hw z)Q|*Spda`+Rn6Sxc7&q>mn3FiOI7{9aNfC-xe(KZxajs&)vt~7w+X2@iT{0Q3j|$=o9<6l$J*R>oNJ}xBp!{Ls{U;!h(HbI0zntz#wDhzWy*P1%~^vkw9na= zs&4q4zimrJ=0*Ja)l_w5-6fZCFqd>8eoe2z-M=~SMoGm5^PY^%g_thHM{}pD|5Z8XG7c(E;>B@ORKxG_WSBlco97_I z^go#kF4oup2D z|n{{vQNgpfd+eQr{GG{w0T05KH3d6O&W} z-!YdO%msoj#MetqQq_Jq;X+Vx63yR| zPgK<&a^AC)xe(KZxOe}Fs`v=EUQuxp8Jyz|lTx|zxNP;fVHTOgSkM^h4}nrT_fFb zr^wWx;v|~PnxyhCcl63^C6b^EblIYl)%JMtcVvNHNrEmADa9wNKD9m#Ub?1NRAgSn zPt2LDuC)>OmI5^-K^KT_ODC(QFMJYQ!;FK9%!@eB5=V_N{m=AD5_EwmP;rXta!vFK zL`V&~K)k4D0E@nN8#xu37je@WQ`C|l*0_vA5_Ey6nK?x*-RQQ?smQ#DCtR7L)}M0T zb(XoL3-PpK`r4p}SmzkHSyoh>MDs`WO#g=1KR}peB?-Df&+d|{+8kRKJaSWm3S5%- zyL+m7XsFoF12rT;7wGQuQ`N>AZoQ%cmn7a8m8urSisKb(m|jVOF3>mjq^i8_9QR($ z$f>|3iEAIEs!ESIeq%8;BtaMG!Dmy|{O28aIZX{Ja7p5xG(GY|;`|X9ha~6%J?>(v zs&&k9m($c}y#EDNyKg~rcTj#o6yKsd{g2yS94g4`B>45y;5oG<5W@vpev{P@DHupd zUeQB-e>7;`Zk8?;&~i-IDs)xTw~IS0ff|ya3pBq!8d`%2T$0$*s+($2!|gtUBA@s5f8O6&M~1*SBtaKw`DN1JaZrIv5>4WJsE5jn>)JpKNzi3# zh%b`{Z;AW;3S5#%=+;BsKIC?dBMG`d_tm``_}1a9k%!@*tS()M`Nh)ErAx(0^wCTA zK#~0}H6%e7XnwIYv<4NpB+*Q7C4Js=9O2A3BtaKw`7P7nHAn?6Nnop-@TA*ZR1$Q7 z<`+vt$3X=yNnr0<*~e|3C<(ei^NXdSHK@QP2^=MQ*K(X|&9agNU7-2J($E@I;F1K6 zb{&6m{Ay`xNP;fV{99O;jbF&*Gokb_|0p?olaupEw;|wu>Jk1L4hG^+Lr-bX@$b%aK9mLQEIpYu|QI-#_fU&ma{i(c(^fmm{2$ zpbPD{r*=?%l=BXYR1izz=~?Yvj?_+qF0^kQ&_Ue|IU7g?u_W3JYVUIXa1wNp?o&aW z)v#Te--KCRx)5XJuU`1WaW;^OlfY6~^MN=!2Wl`E2)Yns4Ne&1_Ls6$oCMZ?g}1~k zBTz#Ubb-e97Lo9e%Q&dOB?)ZDL9?Ym4N1@i8v70AUBSmhsUVgF_O75=U!VqafuIX< zO?_lYoaXr3bF&7iI0+nAf@ZI%VUAalpbK;}eH^RMPyALC=oJ;XB!QzSt^n+jcUU`f zL->NJaWWTTx^N5*p0{uK(Ys;51MB0Y;v{$l5WI9jH2t%`>a@uIy5qn%Ukt=(Z(K$E z&S^$Y0t#YD{B^c>HUt$nV(~F?R!cz@m%I|1$iG&hV zkk?7@OJ>3IL`fj#SIxNG0yP3_Q0A3A6}FXKFa$XwEeSbm=~_?nZ7lW4JLg6h}oUza0;BM7*@6M8-3k1LP z8Crvi%!~L&!g#eO(>ZUKxugs6gN4Sc%7@&3MQ)isULD9dX3k`drjJ+UIvqAKdl=d) zD#$B|gMG&9IbQV{f57Ez!gP7-v1 zKCAbrwqVmVM)sO^Tpd*f@s1X(1YEXeo5^>8?vbnmG1YMx-)5Nsj z+}0o!xFnJNbrcmR!7H+$^G6C?xccGsnY_Bot9uo4b!P}Fq@RBEz4=VwZHOT? z%o=1*m{$^r-FtO^`F4S~AtL765OeMo--aj&jxTtfW6LPKW1#wLegSjeCvogxwZ790 zGly$d)@~0asDPFPzxACnL6@mLcc>Z}P`}Lfu0#VESnls z;F5&N5*ByM|NGfO&jSxJH}(CsQ_S3|A?NpQIZYM3n%xu_tP z1jiIiVB5VkubG-Y^;`2qf0G%_)w)gJnfU7^&D6Vpf9*IAf}ny}64MqmS2HCM*gqse z7wGh9&DHI(j_170@k-{EJsjD)xk`B4d5wcyR1iy|?bv4O{6AtAu8)ZJF;Nn9fo>hy zTs0f#{7YG>AeO|5%}=N^&76OM4Hp$BaqOvPs$hNRRgsgR3+=5vPpE`3Zf_8v0+%G- zxUZRdHbR_n0wb3MU7*)5Y^FNjJ@2y4slX+P<|`jpm8UwdYh^CPbRn*P=5f{SGsmwo zW}Q=U68RfHu8#M0UZ2Zci0MLnu7s{J(0RW^Do$ccW>a-?q4OTHlb{RjN!6RGneuP8 zQ}p6Zfl}_rM~%Yx%$xQb{z3?>4?|FK64{RfHJG>O56#RO8Fcmp6}Tj^^2H~!IiJhD zmU}8_>|Iwzi0eU&!>mCna7iMeV{^T=JO5%!=0Z#tV(eY>3%eZ|s5ps;J5Q)j+c^L7 zOy)vN7vkf3A6)s9^KZPQ;v`y*e?n~xcU~1a3A)gZeWl@Dx8Js@z$J;}Cz`2>?>PPv z&@5d^&;=TM*R$_9o-8&571Kk1Uc`IKH&Z(zowIP6OS%wa@2arId0i_NCs8u)arN$L z$6sceUNIL4x)5XUS~A*g4N`FuO7DaHD!A2<1YMx9r@7DisK6zOYB7&yLr`%Nhx8|n z+#%!r{FoX? zJlkTQ2($+iQUNUq#y0WJ_#YCFKKzmef{G*%XO0;ce{T3;PtF8g$i&ztwjOvTv1{Rx zrUn(67x9~I2gg^+&l=egbfE@gyT+UK-)fYv*Emyyip-1nZ1JS{hWcxXITLiD24kC8 z-gTUJ%7V8Qa*f~qUVO#06J~w<5%*qv%Hrc@eXu>4KrSkvCBfJx8vp#7cjvW%nINc0 z0&%_6cj9;addxqL>&1S8Tv)pJ_9|nn8xU*{CZys#wS!2HWkW zOF}A6g0xKxdM`uGKAUXDK}8aXIi{Qmx=@?3y$0WDQQ6wouBjPEt<68h0&(!N%8sB*E<<}9>4SS%qrbVBiClmE zbUGei=tVr{{K@!+`rEr8*lzcVgjAdaX`8sS^$BZRM3fl^6-gitURK!=bjf9CkE7tx zWb3&tTg-9y_lM5K<7>vqyYz*#@$>a}ib1g5?iC5CI0@1=vGdkYD`xskb4;Wn3BH+y$T-rqKucV!!7qFLg`ZYczmPOu2*xr~D8cHIK+JX$_AwC(M%eMr6*CURr<3kA$9A>{6R1vwv?LId zFyk=mW9O=cn^}Xg%w&j=_2DE~5A7tf=5WZ1>Toee4c23?f(cne(t#TGM&h55arv+( z969T;M%EgnA_>{UP-4ZOn>?KLF~&G%Zmnh7LkTL#O9C<5No1{axQ6-X?Icix?ZJdp zWDT|l=AvfU*azWiIJ8?(4b%g{cGfVE*-EIe28p0vr3~$xXe}%MfCblE4{h}1e)f>r zc1fcK+wB@8q#}D5(yNra*I%#ht-nTg1YM}X*sk$Yd<*Nh8|BUMii*sOICy{DjO`j<9_w%IdQzGBRR_K8 z-rlj(tdD$?(>?KxcboOW_Fw|JsF+>}g0xM{{A8qcu4@l7zoH@u#8Jo6J^jAe<6ll_hEZ5fm~G36GE&PHZ`coyolS^-sJhT_YQxJ z>tTc11!Gu&$LlTT_ zqR-Tk-gO&#ne)ezv1L4629`24BI=g)jQymfnU}CVm_Q9Gs38f)hDfC|}eUGfh^)jC6U)cAS z*d9zE7ZvnG5{zwPdE994%MUzm<~~#;fp|ujGM+wnBmC)JU0eC)s=Bx%FnwKt<+7%q`UJRdxhjsKMAKey&!| zdws!EW;>u z!`Ev4S#wrndoY1qR6t9Du_00>j2!0e+v**2{-7cW#Gj|7`xZp=SF-E~x=@?3-78$1 z?|i-BCMZWCD#g9+rKf}Ti%u}!=>x`%h}%8TY$LPZjYxkPd%=#tCOCT6{G zA@RbuJLU&LefF_G=T1aeV9Pb9(ECQ?SuNc?Kf zFf$G+l0eKQk~2Y}p(cxO+Bx_rj|O_+)#5VMCl6Lg_AV|yH%x>UA& zkJ-Nod(RZ}@i*bJB})0$_Zq-|n;^p(t{8TN0&RM2-BbBW|k&?T3l zJ&smAdRSKr{ArGfpFLa1$KQnOJYB@s!vC8v+wB@8kc$d>B?-nh5&Ojx*7WlBZ^Be0 zfjD?sWk=8@m!Un5jfa!1>2;7QNYJvMrV~O=$qsJO_=R=uSg&l74$?BjBVon zT*IsqL+sy#sYn7bmq^Y8U2+-P1ol7dh3wU(+WXA;96dzLcK6?e0WAr}HZeE4hxKxJzzsavD%-)le8$C5sg+sruF9!ww?74$?BjBO%c z*%<4cO3l0=s7L}amq^Y8U2+-PM09?o;?5+SajYB^>GvoL~&qe3-IAF$6?BX)tl4Sc>!uDVSxu~Eg zl3;9)<3P&`i3JY#Fyo*i3B+6?{uzA_>H7w~3Ag1}1tgyk^N<(zRXJ*yO99kbbj*zXl1ZNCI&%QU1@ZiCreX zVmS%A&|YNkqY1CRUD{tm5>#Yf#B8_6(eukZ-c`+>H#Mk87izqIv{gcpnnnFJBtbN{13pFA?crGFOw*3a0urNtbkpyD4+cp09xQVxUlPabL73o5a zLc5a^N{+<)$i}9mth=a*rhbBoBoL#$gdrMiYrXEnX3u*^f0@VBkOW<*G33F%2~}Re z6Fdzyu|N}J`~(%57ctsn4N+)N#JZ%3uXykOb8Du|C0%IW^!|W^S3US}>;w0jKEyXVb1s4+zE>oS*gv3Ba3mvoK3Jzq1#Fh7C* zLXiaPO)ZuhONqiZ+`Dw05q_B@6tGH7;Ptn&hTUzfS0 z3+>oSn*8$5=J2$lgCgp7(?mf(K}8aX(f+6*8gFY|cdp(NC+K}$=8`V7V>_PKf1a)Z zVxAzVNCGk1-!#Oil$5$x^&Zt&@9Q#`bfF#l=Awh)y2i+quzJ@t;q?<#B!L+1^9`|m zXsLS5wEMiKWG?AKJB~50ABt`mp4PHY;UI#FBoL$hE1TFGRqsW;oIcf~m${@1?KoDB zZ5O7`INLw37DP~y1Y)$0H$?q?4eHg=Yp;`Di!zsVp&iHhGrxDxH9$Nh2r805jP~<} z=-s<DsE7)$I;( z8TLBwsn?*_Pe?@)h(Xxvyq?}ls_1ja3wpb95_F-xL!TxY_O=V6svxMyyok|mZ@U}x z)}E}-X!G^{A#+I=+V>7?onh~TiF%Kc1QkgjM!UTacG7$81NzK-&nHbx4N1_28oe5H z%CL_mhxC!5rY7$76I5hg#AvsVCBya6=Zc<_;OHZBNf+AN_wJEl9}|b`Bb+{71qdpV zK#X?#m{?37ML*Vat3LX8B6CR>+BfX%n_(Z@odgw0AV#}=Y%i$K2KxMA;cOstNf+90 z)ft#!pQ8%tGmbugScUxr6-gjQyM2zz(C0zD-C9lb(Lv^tF0_Y_8}>;w%avuU9eCu zFPTfa(2k>M;94z35>zCCnC*6rB6@ve&y~5P3+*`02d+6&BtbX!r+Ng!ssJr3M4z%1ZPovY1~ zgut7I7_*(vJHs1H@EK_hg_n9FyQ$gY5wVyKlblYaDm`nOBw{*S;HozLYg&5lgL^hJOYqFJ!E2gfH2ET zDo!G}S80XfyiNbD?}cm0yZ5UVjl(h!uNj?3HC#}{)L^?^g9K_&aT26$qG+Bt?|1jc znd4Q1{dv^!E+@@9mPjKL+k*+I$R5htHgW6TIPW?AA6!Wn3ajK^@0c3Q3xe&zgj7IF zg0W5f{X{jZ_#chURx))#xLUvAZBv6WGEI8vepMSn>P`}OpU`+QZbesN6H@})n?I5j-Cm1|ySLr{@< z5fAH?Pqpm{qGzSu3CT59Wlml?D1@L3H3~M(uSQ&-po!J(5BWMwTAI1B_z*)-k$DlH zXpvvFKMSHy$-$mUg(5SnUmO!c(1jY`ZqKipwF7bfn?dTtZlIK3 z9|6&7U!8>T&TnQczcMa_pbIsUS`<*8M?q{&UY4=3Yw5&yN{=-J6`2?D`;i6Jj`kBZ z(dNf$jZSv*B$gXCI)tDLHF~!ysOoM7@$KCLs#BHCiQU@{Fa#Bu7xAh5g;az1DViA2 zzI>x@Pwr2wR_5gpf-cl(_Glq>FfvsWe`MrSTfWTejrhslN~p-Zh)4fhNL4t8EpdH~ z!io2FFYA4%(37TyB;(Tl;;ps(7P&+S@Mo9GMp}+r!iL+{$cFt4*9WH@&`@ z!+A#KQd>6c^fl8i#FmaxV@b2zYRnUxb+0<^i^=G5v%Yn?K%D6n6(@1Jcpg>wXMA;` z=Q|%|T;EpTx>Cm`s7M06;@%aWR&Hd~jQor0Tl;$j6VioV?Kpg&O4{+Eu7M+a-{$qL z&0p9A6(^BZ1H^|(t*qYPl=h-0zs$^~8te|!YwYoiA5Mj-Ew%sgVOGob@U(xw&y)E4 zly=tBj}`Dr#Yx;hI!r~iyrz3qVZ+ZE)54Ri)U{X5l`s`aAg=Oan7Z`y6-^}fy^-*O zHPEW?c$exuO>WlPopup z$66~Ul@1~3LXElq+|76>2JeU4^6Hd~<3*FL;$Pl0SKCx%Uc?{#eK+G!GTwXMsm1Js z)p^EQHFvHu*Upll3pExM3sW1P$J^f0n-}zyKAvpl?)HTtsK~sCD{Ep}e-JO0y3hAZ zsiD@bPV+KlF6l!2?Q>zO%wW7rukp$82}j$#YF#_IAcUX`nS9;D)QJ<{Y2sMt?in!` zx?4%Tt{H-g%!~NgfiN}Ub-Z^l*Tvom|BLQs9sA{<5P~k$DEm{GDtH6maO?AI+l;T8 zw6JcRDsT2dDl#u(?vLSVEAlOG@K)O=t0xW)^17ym zBj}Hrz^YN8o*}5nyoj$iy;o(-!FMw@v^%qTPHY3~g_EHK zU8pf~UT!t`_i#FMNSrnNK>eU2gUpLH7))RW4ic`T$+c9)13oh^L40F^Jx^r2y#`5Ogj7gN z0x^lKxoDpSb%Sc49x|~#Kohagb&Wv=i25>w^kwkYH~~WQ_v^wgwQS(Ob3$6H;*_f-J<2p`e0rVY`1HWkP2!@ zg0WqreDiAF>*pGqBix(;g;l9Db`8eJG=E7Ub!2&@J~A9VFg+vISH=5uToXf3K@Ad! zcT6j!rvC|Ie(4Vrww%lBt$p&j5P~i<@-GXj4L?U|qE)v~GWre6?fq_EXZyJ8S7ctq z%a<2aJB#8kYx4iPE1}8*s}hGU9TY;)g&M_o6;K&4guF#)S}cp@#Kke)a8Q5Yt~-mT|jV>CDv9V+}z?<~8xfe5zm> z5P$wqtPe#*_OqA1d>52tgNW9Nlxjy1zb%1*`I@MO*S(hkmlR5-Kt;;>AVpS3O%}yE-3N zII(NDvR1u9PnsH%pbIsE_m45Ns#vOrz3ozwc@eYS-aq>FukXG6L7b@(-KeNKm{G#) zgN#w*{ar=Wzp15kudaX5Xmj`W^}XMJZWC0HR}%j`TST3E27gWU*5=PPzxiZ+?_|p+ zs7L}a_pb1?eO(^hyt+|+Z-+gh1YPJ=o0Emrf(m7I4V(w|SE=vKf6^wX$h;s(+as^i zHqM*opUIqlx0H%GaK{|s7=swQw}e`kin$LC(UPEo8YB?2-6opoJ5e9$J5n;2bRka2 zQ(Rs9ALk|65p*FFW4p%kCH1`pdfPRq$h?TTZVVBY9YGgrFb+?An`|v@ zW*MtrE2V~A*>0BAf@%>ey5l}`#zFfC-5#EH_DG(@=y%$Af2mu*LM|%ki6oNVjZpPH z_y*6{n}5zYRy4_5X2#V_5L6_AxZ$!0b!{oWm-@oDe@hcXL4qK^HRhT@j(0E(NizVp7I|Ke~Gh^}S{YDl#wP<_Qrh>Glmx z{IBJU2_-9b^Zt15pAdpB)X4i#gu3|lP4uc-+l=QQZQ=dqba}H!QIUBObAPnA_C9T% z^j57L8A8y78jNis`qU%dCb=In+b(BC$jcbScUOw3uP1%1d$soHviQwwAMrl@uuV{r z1mc6Yi>diPgE;qD;msrB9`?Squdb;f3A#|@)@#Mp$OE`GD72=W=gA`tyfYrCX9y}X zFXFK~i>qcG@FwWq>hR{y#T$6PyB12&g&NVXmQZ!m@hzc8PW~9b?5j9$>n8P04JtA( z;=z|os8egwHPPeAV!lx=Ns=svGP_@5qVaaqfgQ|mDI3-E6mY{?Rt;8T+@^1>DL>zdBwhyN(Ff(QT2yP z>iwbk%gjnQYWt>4X_?sghf!waR3w2odTwPkV;$DVp8I^Bm7lgv?EP*iK^I28esN`$ zxC_Lk2e$f7rmsp&T{Otlpd#}kZrmYO9Y4HU6Q93RG2w^gb%|Zozz~8i)ELn#R!u61 zbw2HUm5ijk|0d2V+szPEWM0I%?yaIuoW>W9dY>ATFz?Y^-rxRwA%vg{HRhD6q8dDn zx0rAEctXa;R+YVvHEU`JDl#wPCG)GOqQBv5P@^ZTN?6;ty0`XiZwNsbYJC4$6_w}h zjhcw9zBVI#ZgcOntC5DFBJ(2dSGKA;e-u~3({^l4czSv(@8RL4LkPN1qu>KoRsHq& z0@&j%cV^VP*xNf{%ZW^>$h?T(A5m3Z{CbNf4&;76;YvhbZ=o^Yh7fcilW$yAHR+8n zHBqDZ!i-ikU-S07_NpPM$h?UAuBxhL?7`n5Vyh5Ib6*M}=t2#~hDh0;mT3L_ zQw_6!>^K>zI=;Qx>>rIEtEigS-Dvg?wuh(Hz7*~Gr&*%4uE_&tSy4exB+UPC49$DRJMA5_n0B5NCNT3wH4K*$MAKoFME#m`Y1JG1NM(6FWmMN8g?_Y#-UJxF4U-{qExl{Ao@Og z#y9AmS(z`M9csowMdn3(`iE%M;wp{|`5sR3e75)9%xm6ZAp~8hvGG{6+V}^Eek;fM zYWjAlh#mGXKU8F1#5^k5>wN2!ZR+iVp#)v1!8kmv$bv`XFMdDH%5%MlnE`CA9I0y7 z{o1T^#J!t+ZPt0==two^!XDkLq`$({$e+hqKX!h7GjdTuPe>rXqUjm?c4^}7q-zOn zpG~&vKJbf~$w-1Ovvlk0?Je~yO{}ROrV98*Tiu5~Y_{E6ensX*T)kIN^cZgL{ePjqK zl0f|FYr5VMdu(tW->$^ES!<*g8cF4W#KHc~~@!VGzS?MzSU-QBE{^KTl0ip-0+ z`l?8^^2cMEi0Lrix2Snn>)?&sAp~8hk$(Zo>4>tPssoyg%MfWlU6`2=tt*%jOYpvfk zQKZbT8GpaLH*@y9mqQ4;P-EK6D0OK+h~sK^`B_cO z8ZsoKtM~QHg7b!l5Okr&@h;J-;@cp$9?VR5^xF!V7mklH1QnSV@#5Pts>wL~5{L8P zuwoIJ$Lo#_A?QL4#x{{TeW>;B3$x8WSR<^8D%j#Lv)?gBjUAU`Rm}JJ4YJUVg7I^Q z4z)_=nPUhl$V&q8NKF(z0OE}~#Wq(v(aWmT<+l)mF4Va7W2|~;)KyJvC_6R%=Uu(5 z{2Oe7ip-0+?y*>Pdkl!SU(I@CbE~JVI`vC=WiIJLyz7ft)u}ap86B3mWAljFt*vgg zB0>ndkg2{EtEPEDd@wFOepi*b@c4xM()6Wo8WM0Ip|IoAWIu|u@DR!uD-2CmC$<<#8A?QMl z-oI5=<)T2Gi^-pGCgP#YRu9_uMXAWVh=aGptzoq?&o&Ds=t2#~_O|=Z-(#(aX<_Cl z+OcpIHR=9;LbhGD2NTFe1wG-mi+09_Na??JnyS&;zK*)1_qDDU^O$>mz2B^)`t7j) zYLe~YY2!Yfq1tW8ojEz){`(Rts3D14;g!|6p7=Y<3TMuHHY{wN`S51@s)&jt5T7np zSq*Iu;um$+cl__gnS)l^e~Tjtx-gDAQ}vbC#(OpKbmwXbr}}@I`AOVhODZxi;-()| zR;}lNsQ6^?Om*VVe*^H!$JC0%GQcPduhlNZGO2`_j`luEMx_nUp^j*27@-#HztrXRbj_o!Mu zhWToLG{EZnRJy4l3A#|D?yXogufiQoe3!T{zIoa}>-*Tx4M9cbMSN2ehr&U;uyW7l zLx)CMTLyFsA?QMlnfa=yzsugz#OW&kdt`3jQC8fs&W4~O^CDiXiJ1`~&d+O;p10#T zYu3$o%?w!*bfE_KP5(LSxpCI3kJtninHMqJ^|^NU9W&EMd&FAwYJAtY&FveE^j2>4vKa>z(2}TBHcHjlk7YHff1UWf%?Em0 z{IlK=R3w2oZ>=cR`(do}>VMtmtEdKe*B#sxLePbg&#N7!PB#Uy=&uo;?=L;)t$o&> zQ&W+75y!q9r92yRX`*7W=!~()I(Uy%&2P3{NzjEFpAL#rUw#kb$sg}ai0e|{``2>& zFP*5!yomc{M5%vnV~_fyN}Y@o&p+fsD3Cm*&dq;TM z8-j|=i?~8mw7QTFN1xt@OK0?XJD>L{Z^saVF4Tyx6|GKH2T^o;zJ$-q9Z6jAT^~bG zk$Dl1m=Ucy{{-T-Gvn3mxoZ>O>T2I9k_26-F|BBfsxTJE-S<~6_Vt=LH!)X3`%0LK z%!}9;9;4nG3L>pnfrNzTZ#Al(&%R2P1YIC_maw<>$5XC0imIJ#Nd-NjE4Wu}@7?10 z;MsBBZzA3^dla`_xMt>wQp1}ak zV5Pi~s@vaoUb;ZD-L64GDySz3#x~LVf8(s4J32Ljt8RfvHSqFnv)3{&GO<0FkP2u? zFt&-eKAM*KO%;0<-dV5Pc|+~LXIQ>6N>x0b-~6pJ+r!h2)Op*J-07{%-UWtxQG*I< zNaE9vqg00<@Ha(Y<}K-)cj328pRbo8s7L~F)4Wk?-rM;!@z9U;J?X=K&s_anC_xu$ zU%V$uwV4d!_Le5TCSk>_8O@(DHK@qEh^Gv!sA`?V-z}9Y`HrV(f#O!Zx7vmfbfHGB zmn*8Ghe4DbI@gyttB&<}&xVGeBJ(0%^GBp=a}mGxz53P(PxXBDtg<&7gb;M0My=l> zRjmUc7InJpd*;`tt;0zr4M9cbMVxzlqW_S$>SmQ84t*4BFEk76MNU8r$o_Lv@DxtScj9HM>W6zm2)dAIV4X;{We;$`_d?@$)fg#pd#}k4&HW)WsI`YK5QLA(1jX|?QQqB9v`Xe)yJ8uyGu7?)H2K7 zcDYr8V0$nj74$?Bj17^}u4c0L0nasak8DT5YU=Mg_I}P7naW(Ls`jkKIcnsX=!|k< z$=;PK?ihj!YLGze`?IRL`~!#@UsOoQJ$Zolqm$`oT zyv0k}TOt*i7jbV*)W`)Q{@~B)9fm#NonI!=)Q|*SAgqB^)CWoTYohqIg`Vl>YIu9h z3MJ@5ufG0&jhzXYjph5tM_IBZ;U|?PTegImY;(?eC5B{|BucVX%9iZQF*9Z&Np>pP zN|JSkFy}dMLbj}B35lc-E%s9UpYQXW@63JXIIh3z_v`vz&wYRH@B5y2yWjVH&Y4#$ zs`C9nYILi5W!clYOJbdY4RV2qn zbB*HO4ua$1gp6QGBtdP7VsBOP?SE~A3*t(>a;kp%5#vFP@pYeEI_@$FHw* zyo#^zE0$mc9weZTE-SA}E(P&>moxS6>#)H0>PG{Nha{KDW(h`QUFfs-Ra7fSf_UW6?6q}1&f!c7+P7qqU>1lx>nf`2uj5&tug~20J3HUu zoN5zEFbhl7Hds+jm;fTC$f}I{VyZaZdfK;SjL5pswHrRHT+eAu6#co5Cs?_fv$mht zthpqZ1&_(i9#(bV0`bj^Mu7?^+c?$6-)jg)WL;>r^kMZ~f%BSpE%7DK{6SAUOE=ve zK`;v*ozfpx^WFflWJAxuvORsBayiq2G9v3jhwoPpEa>M9TeU2LU=}J-+x_adEhC+p z<8m7!u~Zo~>i3;yzoG`=saQt+{N--FR0kHfjeq!^kGgLWRb>tY| z$;E?>2P3ks(Pec{^f}(j><%VpR4g^Zm#@^2NFtO4k8YL9s-Mz8ES-@nKHt56dh>bg zy&5C3E;P@m_K14j`Q4lEiAaK3@SwIt_b)p-6Fyl1PHu5dH@{_H*uOy21y75lKLotERW$3w(bbjbIikQCp9pQ%5+H_f<3= zjL5ps7fO~*BbWsbYD;XnG1^)Gfc?%GBeE_u&qTK;m<11NyB>3gedA4C zJIX=i+uzHo+N16FCa5v~rnjt`Sy12D&&zXjQ2ohUcY2$rTY?cR2?^-Dp0a9jArP%6 zj9=elYpU<*{ez5$B$x${5|bZNi&B&(eqK2wKH>FozK`^~A{mi&p@+ZoklHvEM3)Vd zG7k1Ue=5Wi7#otPAb_Sw<}_1LDcLB{RP6 zSIRk2#Llgi1he2#d3hOCc*6!wlzwPi{o>O~JI_>WYLN57?GxA=d)>hJ#1`BOtMg1RK2k4~we-uN5yujV!BmQkwAvu@M4yk$d)jzNf>|K8 z=w7sbS`h28E`P?A9tGW)m)l1W%)(L?`>ed`y$Hn3AOEYr<@~;2@kXy0f)QC4`jI37;ONOpp>C4AGac)jIDxf^t?RqMv8HqV^M zXrw);FK*_wa8tqh1=fxAH7hyTcu0a- zAg0EqD$h@NtN-NAMfKYr9P6u4B9dShmg>H^RJGw0h!OictRH;yFJIEEuEv8ASr>Y? zKULKpj81>**`l74B`*6Gxi3Z#%!0>X$*JmrMIgE@-I9?JEbT1H+td(@$hsij`!rQO z)d#bR#}25eO0}-!G@abQ5R6Cydf4XR25P!7O+z`zcj*`Ub?W zy-Nl%etq8A?%WK@h^z}e?qaICkoKo0J|BCJ=gNt$&aP8`Mi9(Gr3%+lRqxh#8~%3P zPXZsjHo$qV#zI3dBI`moDEgLKq^@WJUB=;wZ#a)H{V;-H7CfjeF<{sTr$?j8hB)t#Qd?ZQL>XBiN!B$@+sh!7y8Svn^^&lZ5 zpd~?V2!Fn%uRB-k27MqHkp%SDbCT4yG|WTBxg;9FEL5Vl9$n@RcPb6_7!O8dUFfEh zlT`H|(OJSH8o?}hP+O0JH%B`iCaj2;5m^_SXCmtnjbIikQCp&F>7(Ax$)n8nXfkuG zy3~5P*&g>79;-IjUtzWf$HNKKVgySf32M6@^BGnWbU`v?Qo4@lm~U?z!5HjYreRQ`OO{Yt4R@cp*it{yE+3R~!!~;LZp>(0o`a)iduMZWoUauBbWtmYU}afKS{x7Iu14-jL5ps1xlx==PxY} zc|;?a1rKWLv8c~t)uHrg;b2?9a_Ywo#Gp~MP2u83ZB%sG@Vr~Txug#gT{;%gcyN|TI!>orSm<5kK zT~k$$H}S2M7W<0SJ5jl_TXdc!7?E|M|9&x5)ffoENuRv-Ox>pLfU%{Gha{K<5AHqo zvr2rdsr$t^OE4nqLUY^_t;>&czioNNi^v`GQq>pVeI4;xaXg%WJ0qa^Sz(;o5_=jw z>H8?haC3Z&{ykMyKf2j?kOsl=a6(4p5=w7Ny!PKn=ljAjL1gtmld3*=Zlm#FT@V}( zCu9V)B&aQsTJMIpRIX7jvbuaSRyFGUneph^Ek(^eu*4ka9M?}KRt|`FTgLbbTp#Je zgAve@c(6x`O8a4n-sVTXyScvA-*bI$<$S{sj7S2yS};X@@&&$2HhlPf@gKIH@9Vr{ zKm@@oc<1{vMcugx#CcUA-^ihnjsjGb)jGUK=0F4aE&U}ude5TF^*I7hsqHIv*6Kd zZmQau1Y$`uCy?$w@8sHS?*|!?b)m2BNL6{-;hD^@z8apHE4nyS&R+}4S~3gcZ|q7{ zrcvcAv)HzNAUdKw`nm!Hl4N`oV~-3w>^^_E-m^&)Y3M)3;0x?uviYcu0a- z@YtAtteV&uNA2QT%>qxq`A+c2or4U)h^!0kFEUp3st#gQwZ}a(8h@#-eQ)n3B*83r z@Qh)%;L4ZRsPWe=!HBF2&2dYVzcJdayz5FlA_W>Hsiv#!Gl18gNK*3?zc%M9j)xO) zX9Tn)s4da+=i%;q>9x(C$cQAMe`}+k8YJOaINGsj1he2xZHZAc`nUt8t}`Bt$hy!^ z^i5Ljzriz0ctj(Z1rKUVe1GU!x7|H=nJvhOtP9OO|Mmp4;6ZJP7t&*bPhJ~ou7j^E z9jhjIelgdNkM}35YNha_)Cw# zv(48AOYiAp2u5UG=*vZuRi%T+wa0+HD?B4AYzn^D>$M1iS@0;BCt1aH2NBn2VW8;t z-0rHW9Sy;VtPA4Fqe-ew08b5`U06;%_*p*JbMYBNFd_-)k2fc&l!ZTNV%V$wJ+G}P ztAAg#aRk9Ec(m3}qYkgf(~mEndLc0Gvzl&~CUp(Lh^!0!#eyXD(5xexC{S^y8j_^;?Zxw}gExi*3jME(J3RMwpNz=5&^=z)PX`C%8QJCU%X^31+sAEN zA|rxe7Annem!!H~#M8mNMGAQ@C-!lhJhaXbjL5ps2RbIHjMjKMc=GNNYV>!*-H!^y zMG(va!LyP*q7p8SaBoa6>5~yG3A4hN3TKAZ&kk^t|6XW3W^PJVS2M1fV~`re*(Is! zyV#3*skZKk_bi{>-(B2su^||N9|`Coi&NG1Q}{;JBVVq}*f;ihcX5jtvppoiEO@M( znW}b`!4u(?F>~T)-_yyBeKV&a7?E|Mb4^cGWl!R%b9$}$89y&*>L&eHDuQ4ZJo1l8 zRjr=*Lle!)Jssb*VS>Bo;zNdDMAn7AIx1C7Dh*ngT%z{UWgj5w* z7sQs1SL;7F{!}piyY7ZyMAn5qd}pd!P!aDYD*rJsBdO2H;JN4Qy|W~k1&@I_Qq`j3 zAj>6l@`^ahaxv*2;8Yl_-46+}!f zfBlckUR0xNj56!Nh^!0EbH3f%%G=f>EL~V(b zq)~3{^GB78$hy#tbWeS>^?&$u^}lEYvrviJ5_f8k$%#k3G9v3j^O(Oq!7Nmww#19o zGgR7n+r#ykKUS#*16osun$C@ zd7T0~tG^R0P|)_Gl3*4*`hJ|E3I;IizQt?vJnvMR7R+^MperM?F7$INQ&gU5AUf6j zJdl6;AHh?lUy2}@g-Z9_pQ@hSiCOn0r%d$3&A1kvy21ACjL5psmCL58H6MX^e@W56 z!JVbt+FR^9NJ%gY9@|=^s=_IlxqLQpP5h-R54nH%63kLDBI`ouYOC+OdV=UTa$m-# zPwKh38rpY|l3*4*!jF$4JLYX#|e=GQsbI()u9i$|f z1!Cn_N$S`Le1j+dC&M%5=(m=q{)i-)g{9iLF-c7v2O?jK9`OhJH*l`(tz_1m5m^`d z=j%yowAmh4|NJlGLJ`ODbgB|TFbf`SZYHUjA7Oj+>~|tQXTt}agX5ku1S7I8^yr4k zs?%ebNxr{m?!b(EC7e4Sv(FhM!7O-8)^D(uw*>Lmq)YJyd!P3e+hzA%Mr2*+rc;vD zoK)=X)n{(aSdiz8ulZm0>4zkk1&?AglhtoCK-4)Dh<`Hwr@sDs`rExE6p?kI@AxfQ z4IPf7HrS(c#`8B8`bKrIPjOgFX2Ije`DArs42T~mwv8_`xtVY5pTmp?Ba(oAGhwVM zRS2`y+pPOq-TW|@@5>n@A_!)|gXa~y?>0R$&fBT?XhSd}>q2wf5(QF6I{p4GXr9B> z@+YZ1uih~I7580ua6Ful5!96gwITfb({HM(H%FSQ+TKT#)S?16jR)(Z6349v2^oQh zB&aQMhkvAde#70Sv%LGUB=zrdd(^TnDseoVkP&!Dg4z;u=S@^Uz8HEIK6k8oW!2s0 z%upbtQK@;IvFh|-{C0H7`*W;4``JXbBSbI)4-(LIHSxV6K6$Blz0uQK2mhKk%#pQZ z7RKG($!fw5eBU7P=@ILldToMzW?6y}NkAXnm8@oL19A7j_v-gOx+FNaLVx2S31-3L zkJpmb(UsT}YxneJ{P^$Z!5x3~GXx{DF7%>TlhvNZAP!D%9{=H}zkEAG$f<#vloLkJeFWY)`f0KZsTzy%m40UrG0mBTpI+NiYi@~IqTFsNx?|VVCe0E(%4c}0=?8mPdf)QC4I{f%p*l(zNu3gUv zf?4pOw#Uc9Gx1Kx!;hHfB^fGJW%S8oyQPV#D*s)1P4~)i{S2V%_wkSaTHiT+zPkBz z8NrhD5JcYbscLp5Y>(uaV8)s*<(vz}>>Udul7MdAAyu_|6X*8YGw%0npIXNGrc)E+ zAqi$-J@U6oRl$WIS{|Gd_;1H`-`>M77=jU57kbnF6tz2uy*=k^D?DBEUh<_}e=&k! z7Cd%rOHn`l0^;<5PXfb#o#D&h+rD>ZMAn7g+8{+$uZeTX!AsA3emOPC_g3PY#zPX! zg2%6QQdDyX#N=(S28M0^#{1n0d;h_RtP9OEk==r6H8*&_JQ7JT3m(*#xT}-?Censq z6>4!t6ZGR{-cr}w{b!z$aXg$rEk?2vqz&O;K7Evv=c@yz(`Q5y&;*@BG7y3jld ztVcA0S@58?#E6PJyjuhI{-Z&UPO4h^5_7Gk2GML|XVq@U=lag8P=n%uC+F<--tn;| z7{QW|FnU*K)!_#aZ4TV)Nspc3Yc<*4e}oaCEO?A6@`B39hqp_g^)4E?`Sfhx&7ziI zMAn7ARPqJYqdJJ)W$*Qrc=wWT!YTU}KoZP?M}wwalzI|xGdfmZobkrqE51%oT7nT- z7kWdBE^28n5aSPh8vp*o<(yTG?Ar`UFbf_9JG`h8pX77uq@;4rvcK&c6GmiRXpY-$ z-oIUWw?p$r=6m$XL!L3UFzP<`=hRJdyKi@Iw6;UDRSk63|7KKO6dVSFbGPSv2TUaPe=E1hY`7@A_xe$~@Q} zhufD4)EqQB*tn4W%>hPaUFg9#I)?U=j6ic`)ZUs?1gRSR|?wods_YX z-AOa+o*GN#PJUYb{1;{du3b_!aPOi~ZXmeF5R71{NIYg3; zX9U44cr^X6z3Q=NlP2EI_%Y-6`d!_3bdMz?vMzLo`R!G&DIjXxbt1mqyG`8l&y+Fy z6>G^XRN@|LKizuOnz*lhX9-3m0nKqs+$`V3?eaqz^Gzf~maVkEY=k8#`A!b=O9&jl zYhJE8f5lhTOZDY5Wy~CUM(9D}(g*F;?ALHLcxb};jI9m2y7?Mj^kI1!kpwh8x0Ubz zt=D|gJ@KAAg>QM zfX2CN;${%jU&`fqa^j`n6TiG@JS4#^c#KHypjOsFXPK1i!;GKjUI`w0#uALky3jal zH>(2TkB)QV=X_V*J$LL0;~@!V!Gn9K{dAwIQo&7|VF^ZLU1*M5V*7LDotg=aOizs~ z3yu$by0}JRNqCLIxc;_mo6q7iZj^KWd9RV_GNKV3^_8<=J*-Dd{f2wa>sNf6yW8%S z5$OSq>#qHDr;I7<`R>L=-<<{Qy_zJLg{4Zo_XTzO5Vps%@5%<=TRqday1ngp8Ig6N z8|ePQ?j?Dz|IhR3^V5874o4Eqg2#ccI;;KzuwPvn{9s`42RpqdR@!}+5n0#h&^~QH z-36z=_Ez0~3nG-o>j(B@L-^zLv9YHAHq(oGy1ZnrAE-+WVu$A?HD@i3+MMGm>tDVf zN}Y zK+|Rc9uEIP<`%_~8?qI43)lF?*sUm<12+q4v|AUb%_0;-DoMk#(Ut zZnxmRhnqMjzAs~*Eg^zl4Ot0#NpK~jhwb4i<~<)@Zc-EHjlxg(L}P!Q)P^-mfm9FR2oIByeW2 z{@VlJ?^ZG*>q2uswx8~#&7+(nFK&$>n1xEzb_?noBftKi*&co0?x{M?&2P?EjsAGu zXb>E?9=MkRAtP84Nl=rpf1L|is5Sqny~kl)R>F8VAtR)*gdB&CT#qYfdW3nPE(ng( zBWpbvp$7>rEeZQqt&jzxK}GvKku*Ip9!|&zX?l<*ku{$hpQ7}DMkS7C64)N*sAYs6 zB)GJO@FN@R&RVzH=Bz}IaDq#PEJOyW!SQecwHU#YNP^lD6Ym`DY?+p3 z&OVGt!sw?G)fe@)gw`V}5z2!1J(BPjy?3;8spB$pb!J4?g>InNY5AYuggl}V%z_8C zSr7m2)`OjE^^(n%lMz`LntP~S^JoOK;6ZJP?M+g>$NG&n=iSE^`7_T~>a<_wFZ;RK z9vlxRP>T^Pi6p2kQ7(U5-^hu>To8;%0(wowG3xMNxTD8@6^&pPDp6ZvSo$j8;9uzicU_GJ{%z_8C zC8m8)+8I>Qz71zY)`jlAX^hJ2=l6=D5zK-Iwe=WX+v|Ma*UqPAMAn5)IX_0#Xp1LB zSdVA~v*1B(i8IaGJ0&I;G~1jJSr_{LdlJ>UDVTu`k7xw5;6ZJP_XqWIuC709dLKq) zU1**$Zci`^9@K{L>jl&Qw|&W<(}rgrAKddnaNK%mLjSXU2_sk%Nl=r>x}Jk@Lf3QB z^uTyHAtUg}PRLFs>v}%qyg$qXby11q^f2pTt_F64yQ$;WWuv(OhdUY)e$D zOW1d*bpna1(V9Dq2gmiVEwqdA418gPQ*KFBbB1FC9+FtKK2eqY6~ErvbI;|BZ7tq# z>ec+n5R6Cydd0j%b;XDK+CJ~}^Nc#x-|71Lq6mUn<}>^tQQbKM#D#=c0;@`Pb1s!X zXNW&T5m^_y_OL|NZw}@&%sRBhb9r(Xr**ZzBM4@}<4C_m^+O@ddMR~eaiGda&78(v z?CeiQWL@Zw8|!|>#ohMtAwPJU>L&)BR+cy0LlVq_$Aa34YD+!5kGiAS(ZEAfD>)C0 zYhVaQWL@a}cP6T5CZK!$yTad|sr5@awePdV<#Gs}B1}3HcZ4GX8sjnFj!}sZ;y38t z-2P&qPQ#7*hvi-~1S67w{_OA=HR5>?wa&Ko6iUwT6rE$g8!icE!Q+P#iE7e+m>2%? zg|`EnrrhI9uG`LdFe2+hZ!Vjts=R@@=Otbo>v^h1tg~Tj?FfQd@Tl-iqAI=|b09zb zVoYE}OkHQ!pEV7^h^!0U_xVJ%(TiWE*tlhg$3LR2GxgKL5d^c~(K;njZM*B7CPt;U z3M}5y-r2jkfFT%>b)n0>ov7MR1yS_p1W)eNmz}?g90d<6EFe2+hPv4NJWf5F zildw-mR&X-G9$7sbTdu#P5{wvh&Mjv+Gyv%*#RC|OJ-r5XK=H(`-ff`?IbLE+YpRM z0-EEN;Qb(;e-?kJor-(OKGUb>__`*cr)s#)nmb4t!Lv{j)R}}I2(HeEZ2r3WEj>7H zk6KOWe|{GpjIb`2klxUd>j9$1p5M&7B5GNQ9$D)lJxIu?3NxJr$^TIU_^Rwso+N<>#wqdz|jeUGzgA|6Ebou zLV9E!A2=IG4`@{4I6cgInC-y`JxF9yB|&Y8Ygb-#>!}T9J=&EXmU;g{8Xg=ECuBq};VqXcb3GR9$ns!a5FC$O4@Tf2 z32IB6%hShsykEdPznWU%O*OQv{cbpEcyK(NkP&!Dg4z&z9qciPtU|K}W_qwLDsen) zJxuSz2xv)ATaO(1`nVZA?0RsIlYa2vxb+|*BiRXhTO$9tM(+73<;*#;+rfm)WAK3^ zkE_#fpEc)Qj)xOCIvK$+C<$syggz_dv9#xtnI5E3iQ}0BAak5E0uM=0TjH6UPdJY+ zFK>E=vqc-LE`9$p*ALS0;CMJ8Bk+&}wI%Y!k9Kk`o@4%c#DqCb)zrW3Uw3Rcsi{g^ zyWf0P9M^wya@7a%d8&+d_7?uwyrX9X9+D{bZd3Kv#r^ucd*quBG7kSZ#3^#DubDZ( zh$Ntg_Gqd+jd890wS5gw{kS2{xbedx2xejZD!tfLRon<-_4ic+qbKxs1`gb62u5UG z=$56Ls)W(_259Y7<2>tzzT&j}*8Y71NiYi@n+xktp*HSudOnvDX!iay&XqCw%@$-t z)`dR4zKJ@WfZx)4==C+8Z(Fu?;*S)LAeaS@bqkxQNo9V~#MKQe1J15`&hK~DFa#sA zuJMa+qC6w;_wMpNy3KR#!C2>!Z)!&nEkjvmUwE*ID*Gh9eKz!yt%2tb7jp&;YGsJC zp@^&tz51gB)#)bgSn~GW;2E&Hpi`rO{dG!7Fbf{@M<%FNv+zy59XYoIVupV2%e%Cf z@nA&Og^oL|_vsaQi~MV+B#&>}4quBSuSO8ef`{7GSY=!V@zgI90~yB?eccBRHUuNG zE_C?u(dUO@zSFgaL=em}pH=8cv&Tp9`iI`Np3$yxuiQxWE@^uo719{5l~*s-B6Jx8 ze*7oUcA4uPxWUfxVgyS<0=n2^jn(8|Ky(^>%yYTUDBm+D?XNUTf?4qBb)vDV(QUaV zhW4ziJm;qQzL{X>sxcz#LhpDmL2W68C&JYim+{Wre$4ksLQk_El3;$NGxq=S)j0oxABS=`#c)vMzMat}c2xh@!(J6gI6~`RN z^&2k)W*lnayqm-RwPr?SUFhOBny4kM(eHNK@~`K&R^6Q5*UkoIEt!S!Ld~11#ZB>> zXDhZW3Y6^J%Sk(X*bt0J0@~A|sVW$UU!l!4WQAw6{*BaE-*_j2U=}>ij%%uxBkMkjky{a< zNo1`@SAC9>9?)1?j%N~?>%j;;NYL97hwmKio*b5DdWPK#6V&8T&+zB$1a)EXH^zhG z`ft8extj5W{)^KIIX9V}ff0B}BG0S@)vdxedV9QD<4pV;`3Adv>ZJIvyo^W!x{bc# zd{78``?|k+1b%Atrdu&_asq3vuouKO8 z#JQyAOKzb5wubK4IVwgF%z}sSTw~R=Du{&zzVIaduaY|~=5a$XBI`moUD{Z+e-FP; z-J$cAzNR{XSqWJzvo{#cnsBcf&*`JKay3jld?A|_c{tQ*R zx&3Q5l3*4*s4a15%uJQPne7=6nW}#ag>|7p+|)~zvGFIpRHafUdb;cHZL5>EXJAAU zT*99|0+GIOXkco?LBTx(?cZsT1lAl`@YwL6-sT6;y?P68@Eo{2IoSGP``6_ek#(W5 zZH~PH;=nU&0!3?mA1twqBnKlGv@7?A`t_ObGB zVJ640r#A;Kw|~H0*!`&pf?4p`e7>>TaQp{NoNc?xvwU17x4?%D48e%33you?eQ^+d zhO7@%`1J{Q(aQ1>1he3g!<(Qs{*K>B89(q{Ps-Rd(-7=jU57y76^GZc&iQ8@pM zK=U>)x${c?5tOxL7RFNsC8*V@_>GjGmpAp4+}qO~*y#sDFd_+PoV(8bgWpIwbWi(0 z#U*dLzkK&@1i>tLG|*?!<@@l9CvBVVjlU2-*zL6=*$|A#y3jZWPkal6zroRr)jy7M zyLER>2OtS%!GmXTySG1_f3#ci*a|~1BI`nP+z@_TZqNZBg1hQl9UmOG9-7eqY|p?5 zXdYcyDryp0Jp=FDr5^~6hZ8b#D*{Vvi4MC*Ioe+Y?DJA_-`YTjFW$p+3zOl(l3Qy_bSo?XmlwkcT7~kpwiytw+D#H~PjTy=FWZky-HAf8ky4 zCwcD;c}Rj0NkDVldQ@JV%hzW2NaMkX%!0@J&rb3l97qpIFd_+PjvEiZCW7=}L}tOG zYj9H5dV~>-NCKMU)cB$Sim)>hoUK ze&v@0Ba(pTxb@I`qRV}k5t#*#@=M>#IzIf8U_=to9Je0&h;n%hG9t6!@!EIqnWH(9 zU_=to9Jho%Gq^mLFe0s-`B!N#C5lKLE+TW?>xr!l=vt z#77d0NCKMUmRPILhYj_4QPz@K7{@-nq~O(%ha?!01T@F3N4h=+-_YlGSxaVN9LHv* zS(ic{l3+v<&>XiOoAtTmDSe%iwPY5?agN#i)y0s9Bp8tdG{-YNeDq*MX2ApJ%B=O! z9+?Cql7QxT=4a*OXT^xjf(Op|S=&Q@R+$7Nl7Qy8_0U_;;WlSPX2Ap3uB`n^5{yU! zn&Z|(@4F87T}EUUJaBc+IzA-9h$NsnZawr7rGE**9)pa?EO@XdiX<431T@D%*e4(8 z`w)@mEA}NA=UF0>V1#wE_d&NO*oD!X=c~x2;`vGvw_gtu%z}qJ`-FQi!u24{Pd#!y zB*84u^6V4t!3eS>!M!KaLlVpaEzdsT9*iJM5(%Flh55!96gwo@1Z!0hdkU>0b6ZtRf5Js3fjB(R;rh)fSj zFbgz3H+IP39*iJM64*{*M5c!%m<1Z2G&|&Q4@QtB32dh@BGW?>%mR%q$PU@w3u?sA z^AgSm(A*x-Jc7+Oca4GmkBnp|c#gR}!7R``<|7G4kR=H$VdmKe<BSm<57Ia3sNqtP9OE#_b7af#4AwNiZVoLi3DqdxBXYc+5u1n5b4l11lzSh~D`}J63hb4V?LAclVAi{lHeI5lK@~mB*84uS?7|$Ia}6R znKascw#o=^`n8>&o5LomnLE?!zq4ht$I-vH=Bb)c5984Fv{qN|SXp-^h>lf8C>fCi zG%9Utn7CeD#nMD0m__eQ!e4ITaCc@UPf$i=UFiLHO;n!_DVSkBq7lqOC2B+X|GoHz z`)}>{jRzyLF7)u`@2F$LI)pr;5zK-Iwe|46_p&?x#(v|$h^z~}v*`rYJ?Fb2k7xw5 z;6ZIYPA=-`R^61_l@VDNdVP+!Rja#Jggl}V%t9q<>#?tY6Svr?GRA`uSr@v_h;i!P zCwGNBq7lr32etM1__12<6Wi(-4@P8N=&Hd~)ws=>kViCvS@58?9)l9@ch@v)Zaf%~ zb)h@&8mn%0iU}AZCK|ykcu-r9jGH%u=hB`x9*oGk(9P15RK3FaLLSiwX2FBndbFCn zCD`lB-o}FwS$Bf=cxcENHGh8LkjJeEnU$tJ!U+HTk`scp>JM^dL=w=&8jn&xT=-we zBO1XhRHC-)@#(!KgRfp2VLTX-b)l;d9jcx@Un1lYjbIi$sI5n?rvCXdSS$RirTEO<~GkC+#(2E2Vwk1`&N$hy#nw)IsifDfAA&+PTv*1B(Jx=#3==|KXz42f~)`dQ#eqNi>34W_yGYM&h7 z!7O-CTaOPL4RK!IG}L%7BI`moEY?cR%(pw#`$Qv{1rKWL(WuZUr{vP>N=9T|=!qj* zstNCXyG~slopaYJ5T$QHFbkESCmI4B8J|eWh^$N9{aEw*cBw*y|5gODINr?=g|=L- ziKTjI>2p7AjGPZ4aM}$hy$jPIrFrRLCQ=Jt7EZp%S(A z=&Sduo0rxaf)QC48vEOaca9HvL?f654{Ga?UmqWJ>s>G&jL5psIHEqU_j$-88o?}h zP+O1A`Zzy+x`-nqvMw}^|DM0?2zf*!n1xEz)?<-AUv(Z_&3G^(>q6r^w)ex+A&+PT zv*1B(J-*ZD-Tp5(G#-q|y3jalZ~imJb9;hW@SwKDVSWAhy=Qyl!HBF2jq6II&+>;n zq7lr32etJGUDb>SBeE_uu8-T=6b*SqBbWsbYU>fYIvWp0WL;=n=hf!>LmtrxX2FBn zdK}SxN&3CRj0YpKE;Ra;;^#|+JfacIGTS4J2z5(#dxRpgE;RbC)nAkec|;?a1rKVw z9-lvH`b6Wwh^z~Z{&9XOdPF0b1rKWL5$axz2P3jBH2UD$5JyvWUt0e(jbIi$sIA9#eScnRR2k#Jh^z~Z`}Ul@10j!S z1he2lZ9Puu=M0rLxYS4@P8NXg+Pb zJ;5w^P+KDOR4wAk9kbxUr+6T;-c6tupXsqKp1o1y8DJ#Ay38WM^~)qoe~BzcZbk6P zUHDQ-0vcJMxt%gS{4kLb`jL?PRk#Nu*$M8`ksgd>CwQbq5{zUgc>G5ajASQxR*EDT z$xh&T5YM81C((O%?XONGu0|FklE{8;_dDe)_D<{cVaqrf$xiUPVwY;u;WMkIwDuT+ zk?aJoc9!UU>C=^Wb*^d%MzRyU23z9rdp&D?mGpoi7|Bksf3U<;t*+EcnO49MjASR+ zuUO(pm0GctcK#cSHD@F{!7j=Yb639@yJW!$Lokw^U~gxM^a{zbx%+G}1S8oAcASEYP-PvkS76(f>>#yOGiCj3sXVn4=SI8-f-pA~A!ER5q* z=MxscbK}3CV~3@;&D9Us0!Z?l_KDqNdkB=`F_gLNg&AEgTNkHQa&UX`jCtrawai@0GHy)B; z7Cdk?^9iKi*>$*DT(MK#?fEJck#(VQ&gZ)czmsd3C+_(!6O9LJ$t-x_9K$E6dM}yQ zI&RZ~bmPH@B%t{|!5)KYaj(Yhs`i`lkOZ^ffioYU2y2g3*J3+07_ml1WL=Eoo`LTs z%+(o(pg9IvOJ+gie9R}?;RGYu30#TzZX!CtEYP@0@Cki5!3eS>fom7vO++V{1sc~b zzG=vu$q^%aHQ5_d^UY{DfsPCjX7SxEyC|~-GvCoOiv+VU&R#2$V1!xdilM_gvM>)x zFbf3xt}qY3j6k#3f@Y;K!hBYYWGC2n-JW0;X!c!Ugjp&^ki}l>|G!j{VBcju#(M8w zv*|?QW3m=Bv*dlWC3Hs?t2?rE8G&Z6B`aBC>)|uCrnmN_%ZMbP*>_o@@1;*`7U*0x zf?yU1_Fb0H9oZ_~k(s4p1X=90q_-tnw7Rk?by|T44-%l+cUj_im0D?!?)*1R){X}TkeAeaS$eU~M4N0z2LGP6{SAd9_L z|KYckxYw-_;#in!XtLWrP~dM?%)EBBWJD6s?7J-S)35_+Z+?9+f?yU1_Fb0H9a)<0$Ra*nX2CnGBXe~}mZm$h zSQ(LZq1i=QkN^JrIc-RKTjRl6G7FW$Ix<&xWNEr1Gaigc0-9Zv_2{zpQd;qp8OB2r z%z_7dElcQ*EKPT0#)A=A7n*&SC3HuYt~)a0Aqi%|BdjBHbw`%2JF++#k#(WjMOhEs zk)`X7%$!SDOJ<=Gdo4@ojx1ewWX6LLNkFrUvc&Gg)zXWf>TWzF!7O-$b!4vY$kJcv zGSPT2BI`o4i?SZCOlzIKc|p4IkOZ^f!CuP}tK(ix-&5^3F!onRKm**QfLj37%A?7MDHFbgz$tw@3qWJw~c zBO8`;SIu-iyX~HbM|fB2UajQ2ExqWe#%p3u!9uoe%f^vbrll6n=Pwk`YNj zqY`^K{oE=F!7O@b5}EViWJK15?*B$BFQ2OEU;hrxpvz7$3**%ItYZA<^mEb9b>0uk zh^z};@my;!?>S>)q7lqOC2H%z*=;f+>p~}9Z0qGcXG}~qf?23UZ9R7A=hSWWd^j1A zb)grJY47DdXG}~qf?23UZ9O=j#&>z zWL@ZucXjsio--yU8o?}hP+N~e`u)cpPc=6ljL5psJx+J=@}4s$CK|ykcu-r9vN194 zW<4KHMr2*+ln1+ev*yD^BbbFs)Yjup{XS~2o)0G@vM%)DsV{qZ&lwXFjbIikQCpAR z`n_7bo)0G@vMzMq2CsX0&lwXFjbIikQCknrZj%vN7y8fq1H8QFjERXxFbkEat;e1w zc0Qbp$hy#1PYv?&E;S}58o?}7qBb5eDWUmrG9v3jCu9uw@@_jOD#0vNqP8A$^?T9F(aWbFF)`5yW}y|*-ha0Eo z!^w!OJ5GD7U6t(RQ=FKXTM;sAh4u(1VqPxje5>cf$%rJN7pzV3@@bU59z-FSg-X6L!XtN4;Qz2sPSM#)`k9~&O|St zh3n5M3c)ORP+O1IdLG%2i?1sgk#(V)CQtO*`EcWNwp<0`_$>%#p%OHo>7#eGPw!+z z)}`iMY9`^o6@e^{+cym-H(suZrTS^{b+3%by3knjtod-E^@t#tg-X<6+ruX#vMw~X zQ`UUA(DsNRn1xEz*5i4-Uv;{?))0)yy3p9)vgX4@BbWsbYU^=XA0OlOd^j1Ab)j)Y zWzC0+MlcJNsIA9DeVotM^WkJf)`iBIA!|NdG=f>EL~T9Z(dVlndOnw`w>;$tgPHjCd==pFz==pFmBI`oqijy@TE*ilsRHC*X zp{ttlU_{pCH8E>G9EjA)J;Df?#VcwS!P#vxA_-`80Fm?IvJ=e0IJI4m-1m0!t<>}3 zWJK15MkkXsA1)ffEL5Vl9-;oqcrYUCLZc(gnhzI^U=}>6t;dkid^j1Ab)nH&X3d9- zMlcJNsI3QQx5{`+wjN2kZ?CQA!^w!O3yn@cYd%~wf?23UZ9UGv zw@SqPOvk`{B$!3MzRyKlL_}=Bs+m; z8SJn8PU-y99y@qt>1t##B8lu>AO8A9a_s5Xx5Uawc0%@|e&>ju5BHd!4<~!k?1b!) z_1}}x^Wj$L`EVc@$xg_=UC$}f^WlyJhZus9?1a3V@H;E@e7HCCd^mV8lAVzEIDTin zo)33Q&xZrSNOpqvS9S{))$`$|p5I;z1S8oAc|YiP{?YT{mit;5f|2Y5?u4`E!}&w= z;o!l@tq9&b>%BzJhpRfL-6|4@WGCRwX8?ZZfSwOmddopWFp{0X`tiwx-?>xIhr6if z!*R_Kkpwi(e0(?Ice?2La4o;DmX@7h7RK?Z^9hTdJE-Tw4bb!9WJK15#<`O3Cj3rO zJs)nSo)4FuU>3%)FYrl`oe$Sm&xex{Sr-~-SH7F@J7@HKxUYt!8xPi!S*V0Plux|; z&M7?~?t48SPDUgFjq@?zP52#0&xgCeTTFU(f>{{Hal!Ed{c$oP>q6rU z&UX`fPt0E??pr+{E<3?2jN>@x6G*?aNzaG7r02uQh^z~Zb3Wfq_???eJ#n@5e7Nic zvoMZ3OFl{UJJ0C(aB+G*oQ%l2(6~zQ-Gtxyam|s~w8Jmd&Q353gnYgmeimg!)`iA( zh3_V!6U+jE^D&=nhZBs*y3n{1@!dppf>|JNmEaTlaDow87aG?tzMF_nFbf2(U3}A! zHM>prYO*(^=9|%Q0-YWr%;LLSc2VXmn)z;-Ge*K(Ox$ zBm6P~&0Y(d-eE+HjASR+cio;~7HIZeVT4&KMv%o`OL~V95lbZr_FcCpm<12^U15Y- zDn^jSUhDtARFYud73LAK9?XIV`>rs;EEOZjVz2f8Un)tk?+WvXSSn`0gMF7JbVs&Y zcVv&rNcN{{;ro^D$kKF2mL`{163}5CnR`^vhkI1dhs#bd3k3TvyHvU(OVb@$nv6iR z*OHYiaZb;N`yn_a!h-~8_Fa}(s^`P?(evSCEtv()zRME2BTLgAnOQ1Eki}k0RKyDV{C&xc#;YY{;(3k3QuzG?8gx+635;bepw?>wZp_0SzzT9rBN zYRQNspxJj>LU&|ox+9Aqm<1xNBXe~}mZm$hh)tVR_Ffo>;$tg z&R)xU=#DI1cVxzc5m^^HtRr)EN0zQTGUFi$X2FBKmi5pbS-S4Xj0YpKE_7H&=C;@K z;bQfCI9W?(VVu2|_0Szzn(oNf$VhgAeU~N7ZLt22B%qlE&0Z^#U?e-izU%e`vp}=g ciX<38mL%AB-JW0;X!cr>1S80jL{>-kf9XOPbN~PV literal 48684 zcmbuId%RUuz4o`4R|qd5nuO*nh^Yty8b@W#JynRXP*KkT6)z~l2DYG7Mkrn}QK45X zO}wNaHvth5!XvD;CLJQIlcMImG&QmCyp?pkrv+y|qw(;0#%*mspR@n^edh1^{+==B z8gtIE=NfCyf1A+qozdfmf3xeq4LhalIbR#p^?&*Iqn+g2Mw>HoaA zM{V)os_}LS%_x!f=>F06>LDL(Hy-L&e`k-{m(Sic3;UrNCDhw()sk(it2_VnOBs5u zem(msDmCDhyX7(XhjzVTddg{?g-RJcA%eVD2 z9_lx;GH`9T_fCv>s7n1B5*7AC{T|-ZvC{4BJmR4$t(zvw-d6Z|lr7xhx5xa2m5bl$ z9q~|=)=d-ZrhHiW&4>G%TI!c34k$+%Wribk;26q4Z}Xwv;21$Gp;MkDH!4AzS`}MDqFXTBlxoscE44nABJ{4RAu*%QK?^=u%ChPP}(IlqlEqZBdt=E9ZNOz zBTZ;V2|JE!*l%vDRAr9|SMQcxXlmdq$0TD50uyAL2?B@lch$0`T)l5L!zKd%cQ`D^=Mmnej*yno+`DcOxFE zvR7nte547@C}HOh5f4?_nT7F46Pi&%=RxjB)OnPuly->@*S0jTn|n{OY~|?Y{f@q= zUdH*&hfZc(Aa)5=DQ)8%m0aSMpG}5G{>F*_+N^}#USPXRL{v&gJkGnSrFq0Vi;Dle zc#vs(kCzq{KN<4RKApUEanZ8(P|xG-D>F5l*d;V_x|JlT2r}4UKp{W zIO~X|HMdnzt-WDI@lH!q9q}7yK2&sF)1yANtTgexhl=IhcdokxYSpLAFWzq+?zBo< z8xnL^zqAMT)z{`ME&lq<;ZCb|FIzZ$@{;1X9}V$ZHTjyB=J6Yr76)B^m}wRF5BzqW zwyaq4xud;(rS95u^>+KAwGbcw*2BfyT{|?Zw@Y*@`_XIBqT)AS7+lx3gYJCM!s6uJ zMtS{ELR+G=>(R6H=-V={*s{wAv#(h9z|!O7-_I@narGq6L))vL8d}BumiEvVru*?> z`FR}JX-;wQrx(_>CHt2i&keq>xOUBDUaNGEB0hNf%HqfSt;hzKBk{FEmKPJR-niPf=-$;nrTd}1MO^*Ms$%`qA61?x_u)xDT2b`; z_+GE)(ke6BZQIS6=&c2Fit6?Yn%!1`U`Be|jQw`nh`GhI+28aCZLhX4-70N6;#WUd zRjhsL&FY-1Tbhr2_K{-csYS(Ym7YtqC7^k>a|v}=>kkKX{zIAc_Nynky*u{9gaKwsy z^MIvUUVa9Lu6iWj`|aJb&ZS@f$5-Zk_n7C6E41Y5Gat&2s`qGqr1U%Ny@&EW4%@l; z3+48yH>}7%9pB`Q5_l9-=I7g*hdZs(*1A2{y4u3D2hM%ZPhOJmd)ts^?4M2dEzL*0 zaJU)yY_01tvTW6q4NLQ-Qx0?X5Bj&~v}O6fe(q>*Uun5fEf1H!}-{QI@Eo? zxW_4F+wWSqDBu3Y!OhwyRO&H$VZLCeQQqEFLR+P@+bUcQZW=Q$-+i|cX8*7*M&;1W zbMx6(O!7Rm?fR*qRq=79th}${_b_#%&dlcJ@jDDwYO*$ykavY!kqZN6}yZ4%&bC0+l;%-K}ZF?Qt|F~dIUS18n(XJWk zt+`T{SA)E~8hC`ZMEe9f-YOi=zx-fTzWKl3tl*3~_}NGDYugo7?VsBZJ(t+CsA&lZ zp6y&h>uL+r9(wN6bE3A^^-z`ir3u};+7IoY>!Ig9Jtt~wT>}4SJpE|r{K0E3tROyb zUB?~qw$_gM^H=@08WN{$?UMiU`B$qTF8Plx`A_O24SmA}oy_)-P?b*)dTN1pJxATE zPdsFY$C8-`=F^A$X)U%RhIipQ+x3tOcGRiGFC z{=s74ZXZWHn1M=()i*CGPT$tqJ8HESVyY1T>dwW*eou7u&bylNiPt+WDq7ckCE~$a zAgDsTcHM&F($*~E!HiE_{)c(Rzw9|E;-LgppttRPfAM&0IWutnG=o@)Ef>xyzWoD# zw&W93Vf&)d_Z7DcJU!yU3{*;BMqb_ni1b4VszBQ~;=zol>WJ&Vm{GL*WpiX)G2;_w zynItp`}f%qf*GGU`-{_yLw~q2LNMbKpI<$#n77m05rP??*!|~M7QJ8lG(s@r6X*Q& zvSQel?Gb_*pD6bs?zTie58b=E->&+_%;GQKzb)_yn##ae^72m|s2MGZ0jP<`pjNhpIT1KN>Bv?SGagTn9;h3FFN7S&p=Q`kNCA#udhCVxkT7=5S(TBM0^IID(!jGf#dV| znL!Dv(2vUK9*Lea1CtGZ(_bP4Gd{8PoId&fLpMbTW_)5)-Z%gI&GRD!Gd?l;slNHXT`!0b%=pCG zZGH1s|NPsv;V5CoCngOWmB+6iN>By*>gUhRy`NoY?^7s{p8Pru`-v_VF<5vS_d}79w@8$8U0W&`FU$@?v$FBy=_{4>OzB!Ly z4VdwXcgN1m<5vS_eBy;IGxJTGTDyg#M6b2>Dw+u$nfrJu%&D3|*qK2VdN5<_?jY>! zBSJ9a6Ly9ZA(-(AJBx}C%=mTeXM|Drz`AVc!N>Bwq zJ6~yopbCVYue3o>1;Wl(+90R`VdpE6eWm@N3IyjX;oen(DiHY3JANHxM(ZL@|9{R* zjEK(})I(|B4`|#ErR|w7vUi!$-Xg}mfq2iApb7-$L~(){t&6zvj109@MUVKt(pIUT zoj*i+&J63)4|Ae;Ka`*fG-fz)f*GiMBHj=6Q2MtcdL%rQpbCDUbnKZwa_zjx@u37& zAod#BEC1m3d)gqV0&)7NBlFMCyR{91Di9x6dgq@uPiupq3dDJ1`{cJQ9nc0r6^J{& z);I64@9+AD$A|WuDnrcfn-4E8Z;P-h5Z`>JZ~jEfe#bTTpb7-%D~$=NK-ig6Wb87d zbrIW{Q-ol~C+y5ALNMbKc77H4JeWxk)v)J!-qj=6&buP5Qi3Y@*?CtR1XUpHysHg@ zDiC(w)doQo2s`g;gP;n8op(j{uJ)WN5DjN!xEo<+091jnmE!kHb`Vs7DDP|K~#Df{Fi@3Z~oR{~IYvB>41XUo) z`^kBEpE@!UnbEq4%lp@Pc`rQTp#)VR%KPGZc^^LF!Hm{LT;7Mz%e(cFGlLRTfhh0k z=jA>9$nnAN2cW{Y4!o~{IR3qXs+8tkk#O%a0}r2w-;?28d3>+NHCOl!i7TK&5531u zI!z!`9;s$M99k8*105Mx^w7Gx_9twW-eV`-m|#ob!PO~fVf>!H_QNMwDQuPAm1hg_ zJucTwxrAoW5)$ct;QKnRbOJ#?t>k(*JuF+o)pXd-Z#i5-dy91dO2qdSYT+}-J#TH5 zw$}B~y62T6kt+|=tx^xIE1y&3496b&9{di;ZI!BYWfi>Rgl2q#-=4W1sv2DOoU6@H zDNbm{C-|M7>!B)L!-Pt4LNh+W?l{>zUGos!%EY%_uWU{Jq?7TzMLL=(nMy zKO5nw%8}1?q@jm?8%jTn>9z?~={Y#`;JFt2hkj|oSvyp=gzJ_-^BU}Wpe3r(*+85? zEuB+wMrMyetZng*LG{p;Jg7uJw^hDM=Rt7-wR9H77NS*cuSn58rTYODTY_^f*ZyET zXM^c}_yjAtgsKp89Z}i?8J!z)FW~&hb#ktUWDG-I(C4Sm`vOtDZoRb_rjlGxD|xj6SYUg$HTZ!&m8l7Ht!H4u%J5 z*TYxwT$?83m@xAz&b91z%(cDqE^2uyb9T1d@jtakjIH#}PQ7zX_FsK2uFvlIV&&Dt z&(8kw%a;0KE4Ng-TzEq^cGblmp&2EH^?Tkn3d^1!k&4($@-e{of<{e6d2CQWN~_UMD{ZmYfgw;7dA z1O2_L#M9lbt6l#4Gnd>pv$d{!B<-Q9^uFqO{`a$wmyWGmIJmXGsqffqm-El4ymHN$ zy6zFzLo-U~UT}%uP5pLl;Ysr=6UVmJwYQVn-&k9H&Hu?R-NWy>61o?Zc0C3*&&oEw z(7pT2{;l=tBc^A)Ztv9n+>PVw+CP`jj1o=zPR&+z+o$`8gU5NJec=t~){Z`Lr|i%p zTI<^OE0&F`H67HP?O(g30ik_TI^8O5iSB8aP}NH>O~|@DdU*GFlg2oorc2zip;8-q zQqOGrbK~pU_K~ZPuif>9G1-g@{C+5*dqHW}W8V)u)Q-9D`s|N?Dv#QAd)F2ncWUDU3-s+lXZpj9YnOL9n=Fh7y-O`eE`_s6(db_PsmHMTLAx{oi`)TLf zvhmMMsNeIA=IXTnds%kq=y7%Rc0E+3eraORr)O8z{d`u|rFlaAfdkeZ_v!_cv+un) zwyxf;hpN;sP3YA?uK?=pdZvmf0`U(5gW__;JYn5h{ zIPBA#Yvr}8dUqH9EUIlEvtV5I)%E?V?@t|5*RzDvjVATbx?{Gt*7|<)$?Bn}`>oP_ zb?gB%v+_z--F|XwUHx1SB^*`qyu0DPskJR9oKf9)WounqHRBt%*UD>Gb>&HZ&$Ycu zXeGB*+V*b8U6qyBrP{*sNhBg_BGF*m6g|}+Q-kf);F|#H9LRLcD2#3`u$KsRoW-lL-(S#N?V&IRHd^Tm(aFP zex-L-UYBY&^}e`%!l;9@^9S{*{jFb%_jxFxD(#c&p>-drd{`;3SGAt=FRoAS{$AzA z2TrSIK*^*+d-g)5;d^>aP6?osV0 zttqcpwWY;`dY4_-_rLP$%W8)`;CpCYrPGAg?b+x5Rm!B*Gn32TTs6=q5tY?6gLZ1kwv`grGP@e*&v`c8KBD8r< zl}qTXNT2VdelDSPl};0?(%FmpxgI(<(C0g)39YMin$Wp{KHo|G(ydZDO=w-6C)nd7 z@+>5+tAti^Tcx#h{-DoyatW=ggjRA1t)=q^eZEtgP!FYDLTl;#L7(-MCbX{7E}^w_ zE~3wRatW=agjRA1Rq6k@X%Efl|DX0692q4_sE5*StF)H>H>ppDN)uXFX_wGi`k!9< zDNssiU8P+Qt)>6(=nODTXkDdULTjliJ+s7b+3}4Vo?4~P99aAJ@<}n}_OJi2(X$gk z^Em?`KKk;CeAJWvGo4h0n9r{OQF(n?{`y~gdjEN8OY~_AAT|x0n|J8uKW9SgBIdIt zK>YEo#rc6JpW=By#phQ*b?VXu`KkFu&J!fuy{mN*^Qi%84}FpbRBzw4IN!6+Q0Lhh zp$F^gulqrFxo1J%e(FVDKhy)Q;xkJ?4BoUX|INZ)&8~+&4*;r7{Z{0cy}pxa6=|11 zT{}wZAf9NRn{V$mslgK+^w|O+df&A&A9UN?OubzXCG_bBO1p%r&_X`f0>q?Wugd?s z{+PE)PgRKdYzYu=T(>fRYU4t0wINh|RuoicEGeHl^nAw*+xa9Vw^hjCn>ak}3FDa0 zqXLcXEU9-we8I3L2uUss#B*fE57?|AEP3TO7Z8BX-hD!`0OrJ zdVk>D;#Xb$=XjwOpV#I4#XXcj%x5ftzJAi;;>v!fc_R@jK2Z#+0ZSJY*XI|Ram5z8 zJ=ePS_%KhG0{!Ox3yP7W#&{k$KKN8Os0QA>xOl(U(7Jku9(+9?X7^4d<3Q9>06K4}U>ySr8vXWuh7 zGIsftELABT_JgAu#8tmuRou4ZG4oUvRtgE88OGgtQ+>wD(LJ}E`yF$Z!1nlPxBIHD z1X{&PQ4e!Op;e$!S812Px8D5S)#gP9nBQI9-}{w$*v?;9xdc?7+jM5}FV$T$pJ1hs zs5YHbT-xtCLy(3CeLJJM>V3D)kNt{+>@gK(JC{;?Li`r0Dn1N2XP5VMy>-hVUQ(4|+EyZvENh z^3!Z;Ryrj33s9&?z=Ph6iLdN+&5nMM4hjBd6doktLGQ-IthcV;@d=U+3I56y9wgvF z@5V&`V{Y6rDoLja)a7qf;YZpfG~*LT|LkUSq`8Eu5c5~9s1zqO;}g4|H`5&dp$F@N z;O}5jDNbm{C(2*J6y1(mcyQ>!xgYM?kg(?^<3R!*^lnVpv#3Ldggw6+6$yCIyD?$U=MEhb_BvryB;Y~s#)Q2_ zIdn+yDL2@oB;Y}Bmw?J%Urk*lSji<&%U*>IK^oiR1U&4_A|O~P^sqA)bN(Q$-Yx+T z%!)8WQa=!^P~nT|zVJhwW*P@|=-#Mn*pn{M}{f!3;c{c@wcez z0fLo64<*VoPRMjfwJnwWA-T zLxR8Ch6f3F(7Q2Fp09R%f}}%&zo~`?33$-EF;SjVUK5{A^{IpYE0PkhnbO)4hef+G9D!0LGQ+d zJr6o`NZ9kMQIUWLy&DtueD2U8VXqTLMFJl5ZcNx~ltYJvy>c2A33$-kB{1Ub^)(<^ z$t6(BUWE-o8r$OpJnYOOAXq8%u(Jts{vfU1E&&hDchnCAE4c(RoJT30CiH%ruI9Jf zBX28+sPMg!vjX33m9DN=KmC1+OQ^g0rB^hnht^H6h}Vn~>9yyYQ6jyPTr*0f*M)0F ziS#OO%_xyxv#l8=(krkvqlB*hc0WPQD3Sg>j%Jid|5ipbO6V_1+*WBu3H{B6OK3(3 zT`%quI(BsxtED5q+ESI$>CvvWl-4!LZmX0~m9F?s6ROg6=xIV#y5=%Xs7lvdrU_N) zO3*Z+DqWwNCRC*>Xw!tMbX{+nP?fGWP7|uqb=GM@Rl34FO{hxO)~5+orH>CCiK?>O zBflocw>|vERlmW~@21>V`6{IkI%Q??`2Sy#H72M+rT8~-nnA1tzq<`RSPKMIi23be zNHF6Q{N}eYK^16z=NS^rK&3?d`&zAqm@35l?l$yb#wX(6*J>@qR3VOkQ>_`F;5V^h zt5^#JRfzfRVn{HPAiVFLwU)%@`{DS?8c~_=PpPuoBj2TJ2C)+HbuvCd6}EF#N7!>_ zpi&~fA_XdDe1fZNLJuXV0v%tyqZ!0XaFtEy!CD}wLL6Tw<9kqr?eTR>n$fz5xymMN z6>F&qaeOtEW_%*Ph6O5Se1hw8!d59k73lWGs{Dij?|M(n(hOoH;%jlBV$U)6(N$sm zoWfQy1A^G`_B$)A6Q*lg(1ot~UbafkB>k?4uIwI0&758CCaHRuOh_RjlalBRV z(A7<>3nKk_aKHO>oY2)&s7o3#KTWqEnnBDFo?hdnD#SdZ(8Bo2GgTqxQLC%o!d6iQ zf@cPJaJ^qhPz8c#IE)y+ODZI&0>SgHu3HQVUO#j#7HD3HLV_88#TKuqA;AoKt1GK` zoev3S{B=^CeS`!v@ZcII^n){;kYEM`*D!&IuUpa#2(CQBcCJVYJ#?l=6=<$!3JIOJ zkp>Z8bB7E*L9CQTOYEmHLPzFTR4Hv^+?nzS`*z^UJHz*{^G(5s z@!sEi7Pjk-*zJd^l(z3OHkHzZs`&Qf&_gpy*mp8VJd{us-{Ks4XhsS9Zt1Ksp(?(O zIwUlsgndVL#6t;H@vYjShh~(p@A{5-D4{C89X#~Vj1py~O8MUKh=&rYvT-f+(2Nr0 z9;uY?4Uc#zp(-0kJT#+(eTRHxTq&U{zI8tAhh~)MRQ6$c*_XzIs%%^h3C$?6rTi?8 zE)ejmJu{S_V4yG<^NH7DH(u~7a zkw7hu5~vu59?bYHVWrT61ZrXQ-8$g@ zZw<{j^kBws2`h#DAc0!;Q!^fnLl0(9OKHZThaGX|{Hip!heS-JEeu;_k0|4zb-6wC zh^bJQ+d~g~1~494m)k>+mX8xfx*9#KZ6b-6wCh^bJQ+d~hmVc;4l zi8I7pW#xY6zDnuzN++u_b*Vxn8++dsX+~nR4kW(PNmRyzDr~oL#Df`884s>{3VW^u zRiJGg@nA+&#v{INNmQmERAIY~BOc6%%6M?qQ`iqBr~++c?>~CYNX!-SSl3w#Jy;6_ z*G5Sk=?B-IipunZtDZs+C8z>zMnLm6|pNI!D zP$}`kF-PWS&t4z#P=YGZKltmB`HTIwN8Yu?3{*;-)uDI(;X_L!9!gLJdgSch`3KXk zi+C^tl@j~S@16hcy_pdYC8z>D>yzI3xdU4w@1ti1DkZM^NALXWm2X5ml%NXqdk6H% zw~l{ltvlNHe&g~T?+r%GqlnwXchSXEc>f%>^KHAKht{PE1dqp%U`FdA=KFaY6I6lV z(G?QRXkEk{`60oKPw*YUjR~qibL58vGf*kPw+1&Rr~=KA9}>(!r3BwI+?b#WG)I0& zFawnm9LFKSj8E`g#*GQ8Ky&1W1T#=6(fIhlaR7qX62yE%V(6hNzIl;!c#YCpRN=aW znAerigYK$A%(pOx1T#LtD^X*DD$u;HG$yD5!8bjI1T$I}F|R9)393NwD$$sr3IyLK z84}EBUBtXfG$yD5!S_#w1T$I}F|QJh393Nwjg=w6jMhcWt3+dhDiG=8Lp`Vh!K16O z2UQ^W2J*%PRUr80&5&S5>muem%Nr9^8IR~2JvF0s5%XQ0p$9WQ!FP4K1g_h6Sa;)R z-$rWUbgQgNX%i>kWvZVb_gk9KmO!O6W49k5INFs!%@-&n1xI^+W3-=607*mD1|x63Fn{ zr*#o?yGy7_Y4vjnRk1GTqVR~n7nzwD9iQ)V#;P8Ox!rA*s^CEt{lZou!x^jAMa=D? zht{P^{agYW&L*`kVs3W{RVl5lbqQoRo7B39x!omHrL_9F1TvgWYF)(K?h>j}TK!xC z8D5vPE@EzX2~{brej%Y(GS*FdXkDn3W*mCxm5eH-T>>7eQkt<#pcb!QpsB)k#xCKj zlxFM_sHIm>=M38 zX~r%A57q*$|Kl=t316i&W0!yjYk}7PycxTMuTq+^OTdG74U|UA?Jl7zr4h$lg$(b0 zXkEnI9(vGSY4vjnWOx@x>mufMmr#|`>gN*3@ZOTvMa=Ckp(>@-&n1xIJu5aSK)c&|FQ$7lM!ivLaEZSC=SyAo8v19A8scVzIk_vyp_ zw3e;H8|4{ed)NGYWa86^;7^n`?};71j` z;{;TC7NsBQupi8@B}%t_W}qKvo-texzb;kmQ{3Z}tzCAErERSp^XISnt+_@a4nINE zQkBv-T+k`9cR?_NSP8`8-t~#6AML#3Ohj+D=O9$2ewX}5m;5L7k=~iwC(c{famR6v zIDTeOmHNRWe$+DK`?cLFe41MSMT8|-4PEZr~+-{hzB#GG9KkBL_BRg;-LgppluxSU`ABNqg+jir=~|dl%NW< zjUyh+h{|}Bt3u1qqsjZ#mZ&nsR7o6PlZsmAN%BgRt6LNOuqs1P1rHlX`oWB-Oh3wX zy@}65393NbIO4&KsEkLs+Bh+;l%NWpa1{> diff --git a/zed_wrapper/urdf/zed_descr.urdf.xacro b/zed_wrapper/urdf/zed_descr.urdf.xacro index 4d218f96..69abe2dc 100644 --- a/zed_wrapper/urdf/zed_descr.urdf.xacro +++ b/zed_wrapper/urdf/zed_descr.urdf.xacro @@ -49,6 +49,9 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + + + From afcc8cf222077e17b4e458a44c06dcace51c746c Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Fri, 28 May 2021 15:27:59 +0200 Subject: [PATCH 040/199] Fix ZED2i model minor error --- zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp | 5 +++++ 1 file changed, 5 insertions(+) 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 6d35dcd0..263c88e6 100644 --- a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp @@ -669,6 +669,11 @@ void ZEDWrapperNodelet::readParameters() mZedUserCamModel = sl::MODEL::ZED2; NODELET_INFO_STREAM(" * Camera Model by param\t-> " << camera_model); } + else if (camera_model == "zed2i") + { + mZedUserCamModel = sl::MODEL::ZED2i; + NODELET_INFO_STREAM(" * Camera Model by param\t-> " << camera_model); + } else { NODELET_ERROR_STREAM("Camera model not valid: " << camera_model); From 80f1c576c6ad8b42215961604c10114dc4d68942 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Fri, 28 May 2021 15:31:16 +0200 Subject: [PATCH 041/199] Update README --- README.md | 2 -- 1 file changed, 2 deletions(-) diff --git a/README.md b/README.md index bb45dc32..940470f5 100644 --- a/README.md +++ b/README.md @@ -22,8 +22,6 @@ This package lets you use the ZED stereo camera with ROS. It outputs the camera - [ZED SDK **≥ 3.5**](https://www.stereolabs.com/developers/) and its dependency [CUDA](https://developer.nvidia.com/cuda-downloads) - [ROS Melodic](http://wiki.ros.org/melodic/Installation/Ubuntu) -*Note:* an older version of the wrapper compatible with the **SDK v2.8.x** is available [here](https://github.com/stereolabs/zed-ros-wrapper/releases/tag/v2.x) - ### Build the program The zed_ros_wrapper is a catkin package. It depends on the following ROS packages: From ef7f147dde3f4bbdd7a47ffba389ea8ab687b8c1 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Fri, 28 May 2021 15:37:59 +0200 Subject: [PATCH 042/199] Update README.md --- README.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/README.md b/README.md index 940470f5..da325693 100644 --- a/README.md +++ b/README.md @@ -18,6 +18,8 @@ This package lets you use the ZED stereo camera with ROS. It outputs the camera - [ZED SDK **≥ 3.5**](https://www.stereolabs.com/developers/) and its dependency [CUDA](https://developer.nvidia.com/cuda-downloads) - [ROS Noetic](http://wiki.ros.org/noetic/Installation/Ubuntu) +or + - Ubuntu 18.04 - [ZED SDK **≥ 3.5**](https://www.stereolabs.com/developers/) and its dependency [CUDA](https://developer.nvidia.com/cuda-downloads) - [ROS Melodic](http://wiki.ros.org/melodic/Installation/Ubuntu) From 1d5d5efb83a4be3efc12f39d59ae406c1d3dbf9a Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Tue, 15 Jun 2021 15:18:37 +0200 Subject: [PATCH 043/199] Fix sensor_msgs type for depth image in OpenNI mode --- latest_changes.md | 4 ++++ zed_nodelets/src/tools/src/sl_tools.cpp | 2 +- zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp | 4 ++-- 3 files changed, 7 insertions(+), 3 deletions(-) diff --git a/latest_changes.md b/latest_changes.md index 5522270b..97d59e08 100644 --- a/latest_changes.md +++ b/latest_changes.md @@ -1,6 +1,10 @@ LATEST CHANGES ============== +OpenNI mode fix (2021-06-15) +---------------------------- +- Fix sensor_msgs type for depth image in OpenNI mode, from `sensor_msgs::image_encodings::mono16` to `sensor_msgs::image_encodings::TYPE_16UC1`. Depth image in OpenNI mode is now compatible with the nodelet `depthimage_to_laserscan` + v3.5.x --------- - Add support for ROS Noetic diff --git a/zed_nodelets/src/tools/src/sl_tools.cpp b/zed_nodelets/src/tools/src/sl_tools.cpp index 01e04572..725c047e 100644 --- a/zed_nodelets/src/tools/src/sl_tools.cpp +++ b/zed_nodelets/src/tools/src/sl_tools.cpp @@ -238,7 +238,7 @@ void imageToROSmsg(sensor_msgs::ImagePtr imgMsgPtr, sl::Mat img, std::string fra break; case sl::MAT_TYPE::U16_C1: /**< unsigned short 1 channel.*/ - imgMsgPtr->encoding = sensor_msgs::image_encodings::MONO16; + imgMsgPtr->encoding = sensor_msgs::image_encodings::TYPE_16UC1; memcpy((uint16_t*)(&imgMsgPtr->data[0]), img.getPtr(), size); break; } 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 263c88e6..aaf067a0 100644 --- a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp @@ -129,7 +129,7 @@ void ZEDWrapperNodelet::onInit() if (mOpenniDepthMode) { - NODELET_INFO_STREAM("Openni depth mode activated -> Units: mm, Encoding: MONO16"); + NODELET_INFO_STREAM("Openni depth mode activated -> Units: mm, Encoding: TYPE_16UC1"); } depth_topic_root += "/depth_registered"; @@ -1912,7 +1912,7 @@ void ZEDWrapperNodelet::publishDepth(sensor_msgs::ImagePtr imgMsgPtr, sl::Mat de imgMsgPtr->is_bigendian = !(*(char*)&num == 1); imgMsgPtr->step = imgMsgPtr->width * sizeof(uint16_t); - imgMsgPtr->encoding = sensor_msgs::image_encodings::MONO16; + imgMsgPtr->encoding = sensor_msgs::image_encodings::TYPE_16UC1; size_t size = imgMsgPtr->step * imgMsgPtr->height; imgMsgPtr->data.resize(size); From b83c67201d37cbe44cf545f6fc80dc1648ddd75a Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Thu, 17 Jun 2021 17:00:31 +0200 Subject: [PATCH 044/199] Fix ZED2i support --- latest_changes.md | 4 +++ .../zed_nodelet/src/zed_wrapper_nodelet.cpp | 32 ++++++++++++++----- 2 files changed, 28 insertions(+), 8 deletions(-) diff --git a/latest_changes.md b/latest_changes.md index 97d59e08..9f9d02d7 100644 --- a/latest_changes.md +++ b/latest_changes.md @@ -1,6 +1,10 @@ LATEST CHANGES ============== +ZED2i support fix (2021-06-17) +------------------------------ +- Fix sensors topics pubblication for ZED2i. The support for the new camera was not complete + OpenNI mode fix (2021-06-15) ---------------------------- - Fix sensor_msgs type for depth image in OpenNI mode, from `sensor_msgs::image_encodings::mono16` to `sensor_msgs::image_encodings::TYPE_16UC1`. Depth image in OpenNI mode is now compatible with the nodelet `depthimage_to_laserscan` 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 aaf067a0..894e77d0 100644 --- a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp @@ -323,6 +323,22 @@ void ZEDWrapperNodelet::onInit() "the value of the parameter 'camera_model' to 'zed2'"); } +#if ZED_SDK_MAJOR_VERSION == 3 && ZED_SDK_MINOR_VERSION < 1 + mSlCamImuTransf = mZed.getCameraInformation().camera_imu_transform; +#else + mSlCamImuTransf = mZed.getCameraInformation().sensors_configuration.camera_imu_transform; +#endif + + NODELET_INFO("Camera-IMU Transform: \n %s", mSlCamImuTransf.getInfos().c_str()); + } + else if (mZedRealCamModel == sl::MODEL::ZED2i) + { + if (mZedUserCamModel != mZedRealCamModel) + { + NODELET_WARN("Camera model does not match user parameter. Please modify " + "the value of the parameter 'camera_model' to 'zed2i'"); + } + #if ZED_SDK_MAJOR_VERSION == 3 && ZED_SDK_MINOR_VERSION < 1 mSlCamImuTransf = mZed.getCameraInformation().camera_imu_transform; #else @@ -526,7 +542,7 @@ void ZEDWrapperNodelet::onInit() mPubImuMag = mNhNs.advertise(imu_mag_topic, 1 /*MAG_FREQ*/); NODELET_INFO_STREAM("Advertised on topic " << mPubImuMag.getTopic()); - if (mZedRealCamModel == sl::MODEL::ZED2) + if (mZedRealCamModel == sl::MODEL::ZED2 || mZedRealCamModel == sl::MODEL::ZED2i) { // IMU temperature sensor mPubImuTemp = mNhNs.advertise(imu_temp_topic, 1 /*static_cast(mSensPubRate)*/); @@ -1478,9 +1494,9 @@ void ZEDWrapperNodelet::stop_3d_mapping() bool ZEDWrapperNodelet::start_obj_detect() { - if (mZedRealCamModel != sl::MODEL::ZED2) + if (mZedRealCamModel != sl::MODEL::ZED2 && mZedRealCamModel != sl::MODEL::ZED2i) { - NODELET_ERROR_STREAM("Object detection not started. OD is available only using a ZED2 camera model"); + NODELET_ERROR_STREAM("Object detection not started. OD is available only using a ZED2/ZED2i camera model"); return false; } @@ -2963,7 +2979,7 @@ void ZEDWrapperNodelet::publishSensData(ros::Time t) uint32_t tempLeftSubNumber = 0; uint32_t tempRightSubNumber = 0; - if (mZedRealCamModel == sl::MODEL::ZED2) + if (mZedRealCamModel == sl::MODEL::ZED2 || mZedRealCamModel == sl::MODEL::ZED2i) { imu_TempSubNumber = mPubImuTemp.getNumSubscribers(); imu_MagSubNumber = mPubImuMag.getNumSubscribers(); @@ -3055,7 +3071,7 @@ void ZEDWrapperNodelet::publishSensData(ros::Time t) } // <---- Publish odometry tf only if enabled - if (mZedRealCamModel == sl::MODEL::ZED2) + if (mZedRealCamModel == sl::MODEL::ZED2 || mZedRealCamModel == sl::MODEL::ZED2i) { // Update temperatures for Diagnostic sens_data.temperature.get(sl::SensorsData::TemperatureData::SENSOR_LOCATION::ONBOARD_LEFT, mTempLeft); @@ -4240,7 +4256,7 @@ void ZEDWrapperNodelet::callback_updateDiagnostic(diagnostic_updater::Diagnostic stat.add("IMU", "Topics not subscribed"); } - if (mZedRealCamModel == sl::MODEL::ZED2) + if (mZedRealCamModel == sl::MODEL::ZED2 || mZedRealCamModel == sl::MODEL::ZED2i) { stat.addf("Left CMOS Temp.", "%.1f °C", mTempLeft); stat.addf("Right CMOS Temp.", "%.1f °C", mTempRight); @@ -4538,12 +4554,12 @@ bool ZEDWrapperNodelet::on_start_object_detection(zed_interfaces::start_object_d { NODELET_INFO("Called 'start_object_detection' service"); - if (mZedRealCamModel != sl::MODEL::ZED2) + if (mZedRealCamModel != sl::MODEL::ZED2 && mZedRealCamModel != sl::MODEL::ZED2i) { mObjDetEnabled = false; mObjDetRunning = false; - NODELET_ERROR_STREAM("Object detection not started. OD is available only using a ZED2 camera model"); + NODELET_ERROR_STREAM("Object detection not started. OD is available only using a ZED2/ZED2i camera model"); res.done = false; return res.done; } From e830e8421b6e1f39da7c505cb454ba303435c7c7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?B=C5=82a=C5=BCej=20Sowa?= Date: Wed, 21 Jul 2021 20:40:38 +0200 Subject: [PATCH 045/199] Allow publishing IMU TF without Odom TF --- .../src/zed_nodelet/src/zed_wrapper_nodelet.cpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) 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 894e77d0..a475b992 100644 --- a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp @@ -3063,14 +3063,15 @@ void ZEDWrapperNodelet::publishSensData(ros::Time t) { publishPoseFrame(mMap2OdomTransf, ts_imu); // publish the odometry Frame in map frame } - - if (mPublishImuTf && !mStaticImuFramePublished) - { - publishStaticImuFrame(); - } } // <---- Publish odometry tf only if enabled + if (mPublishImuTf && !mStaticImuFramePublished) + { + NODELET_DEBUG("Publishing static IMU TF"); + publishStaticImuFrame(); + } + if (mZedRealCamModel == sl::MODEL::ZED2 || mZedRealCamModel == sl::MODEL::ZED2i) { // Update temperatures for Diagnostic From f20b5bde9e3489f702a83c2f219cefa842c2a829 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Thu, 22 Jul 2021 16:44:27 +0200 Subject: [PATCH 046/199] Add zed-ros-interface submodule --- .gitmodules | 3 + zed-ros-interfaces | 1 + zed_interfaces/CMakeLists.txt | 77 ------------------- zed_interfaces/include/foo | 0 zed_interfaces/msg/BoundingBox2Df.msg | 6 -- zed_interfaces/msg/BoundingBox2Di.msg | 6 -- zed_interfaces/msg/BoundingBox3D.msg | 8 -- zed_interfaces/msg/Keypoint2Df.msg | 1 - zed_interfaces/msg/Keypoint2Di.msg | 1 - zed_interfaces/msg/Keypoint3D.msg | 1 - zed_interfaces/msg/Object.msg | 61 --------------- zed_interfaces/msg/ObjectsStamped.msg | 5 -- zed_interfaces/msg/RGBDSensors.msg | 16 ---- zed_interfaces/msg/Skeleton2D.msg | 21 ----- zed_interfaces/msg/Skeleton3D.msg | 21 ----- zed_interfaces/package.xml | 29 ------- zed_interfaces/srv/reset_odometry.srv | 2 - zed_interfaces/srv/reset_tracking.srv | 2 - zed_interfaces/srv/set_led_status.srv | 3 - zed_interfaces/srv/set_pose.srv | 10 --- zed_interfaces/srv/start_3d_mapping.srv | 13 ---- zed_interfaces/srv/start_object_detection.srv | 38 --------- zed_interfaces/srv/start_remote_stream.srv | 25 ------ zed_interfaces/srv/start_svo_recording.srv | 5 -- zed_interfaces/srv/stop_3d_mapping.srv | 5 -- zed_interfaces/srv/stop_object_detection.srv | 5 -- zed_interfaces/srv/stop_remote_stream.srv | 2 - zed_interfaces/srv/stop_svo_recording.srv | 3 - zed_interfaces/srv/toggle_led.srv | 2 - 29 files changed, 4 insertions(+), 368 deletions(-) create mode 100644 .gitmodules create mode 160000 zed-ros-interfaces delete mode 100644 zed_interfaces/CMakeLists.txt delete mode 100644 zed_interfaces/include/foo delete mode 100644 zed_interfaces/msg/BoundingBox2Df.msg delete mode 100644 zed_interfaces/msg/BoundingBox2Di.msg delete mode 100644 zed_interfaces/msg/BoundingBox3D.msg delete mode 100644 zed_interfaces/msg/Keypoint2Df.msg delete mode 100644 zed_interfaces/msg/Keypoint2Di.msg delete mode 100644 zed_interfaces/msg/Keypoint3D.msg delete mode 100644 zed_interfaces/msg/Object.msg delete mode 100644 zed_interfaces/msg/ObjectsStamped.msg delete mode 100644 zed_interfaces/msg/RGBDSensors.msg delete mode 100644 zed_interfaces/msg/Skeleton2D.msg delete mode 100644 zed_interfaces/msg/Skeleton3D.msg delete mode 100644 zed_interfaces/package.xml delete mode 100644 zed_interfaces/srv/reset_odometry.srv delete mode 100644 zed_interfaces/srv/reset_tracking.srv delete mode 100644 zed_interfaces/srv/set_led_status.srv delete mode 100644 zed_interfaces/srv/set_pose.srv delete mode 100644 zed_interfaces/srv/start_3d_mapping.srv delete mode 100644 zed_interfaces/srv/start_object_detection.srv delete mode 100644 zed_interfaces/srv/start_remote_stream.srv delete mode 100644 zed_interfaces/srv/start_svo_recording.srv delete mode 100644 zed_interfaces/srv/stop_3d_mapping.srv delete mode 100644 zed_interfaces/srv/stop_object_detection.srv delete mode 100644 zed_interfaces/srv/stop_remote_stream.srv delete mode 100644 zed_interfaces/srv/stop_svo_recording.srv delete mode 100644 zed_interfaces/srv/toggle_led.srv diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 00000000..d1041044 --- /dev/null +++ b/.gitmodules @@ -0,0 +1,3 @@ +[submodule "zed-ros-interfaces"] + path = zed-ros-interfaces + url = git@github.com:stereolabs/zed-ros-interfaces.git diff --git a/zed-ros-interfaces b/zed-ros-interfaces new file mode 160000 index 00000000..e58b1a39 --- /dev/null +++ b/zed-ros-interfaces @@ -0,0 +1 @@ +Subproject commit e58b1a393db9a8247b8336936ab20724b33bcf3a diff --git a/zed_interfaces/CMakeLists.txt b/zed_interfaces/CMakeLists.txt deleted file mode 100644 index 924ad018..00000000 --- a/zed_interfaces/CMakeLists.txt +++ /dev/null @@ -1,77 +0,0 @@ -cmake_minimum_required(VERSION 3.5) - -project(zed_interfaces) - -#Fix for QtCretor -#list(APPEND CMAKE_PREFIX_PATH "/opt/ros/$ENV{ROS_DISTRO}/") - -find_package(catkin REQUIRED COMPONENTS - std_msgs - geometry_msgs - sensor_msgs - #actionlib_msgs - message_generation -) - -add_message_files( - DIRECTORY msg - FILES - BoundingBox2Df.msg - BoundingBox2Di.msg - BoundingBox3D.msg - Keypoint2Df.msg - Keypoint2Di.msg - Keypoint3D.msg - Object.msg - ObjectsStamped.msg - Skeleton2D.msg - Skeleton3D.msg - RGBDSensors.msg - ) - -#add_action_files(DIRECTORY action FILES act.action) - -add_service_files( - DIRECTORY srv - FILES - set_pose.srv - reset_odometry.srv - reset_tracking.srv - start_svo_recording.srv - stop_svo_recording.srv - start_remote_stream.srv - stop_remote_stream.srv - set_led_status.srv - toggle_led.srv - start_3d_mapping.srv - stop_3d_mapping.srv - start_object_detection.srv - stop_object_detection.srv - ) - -generate_messages( - DEPENDENCIES - #actionlib_msgs - std_msgs - geometry_msgs - sensor_msgs -) - -catkin_package( - INCLUDE_DIRS include - CATKIN_DEPENDS - message_generation - std_msgs - geometry_msgs - sensor_msgs - #actionlib_msgs -) - -############################################################################### - -#Add all files in subdirectories of the project in -# a dummy_target so qtcreator have access to all files -FILE(GLOB_RECURSE all_files ${CMAKE_SOURCE_DIR}/*) -add_custom_target(all_files_${PROJECT_NAME} SOURCES ${all_files}) - - diff --git a/zed_interfaces/include/foo b/zed_interfaces/include/foo deleted file mode 100644 index e69de29b..00000000 diff --git a/zed_interfaces/msg/BoundingBox2Df.msg b/zed_interfaces/msg/BoundingBox2Df.msg deleted file mode 100644 index cd11758e..00000000 --- a/zed_interfaces/msg/BoundingBox2Df.msg +++ /dev/null @@ -1,6 +0,0 @@ -# 0 ------- 1 -# | | -# | | -# | | -# 3 ------- 2 -zed_interfaces/Keypoint2Df[4] corners diff --git a/zed_interfaces/msg/BoundingBox2Di.msg b/zed_interfaces/msg/BoundingBox2Di.msg deleted file mode 100644 index bc5fd790..00000000 --- a/zed_interfaces/msg/BoundingBox2Di.msg +++ /dev/null @@ -1,6 +0,0 @@ -# 0 ------- 1 -# | | -# | | -# | | -# 3 ------- 2 -zed_interfaces/Keypoint2Di[4] corners diff --git a/zed_interfaces/msg/BoundingBox3D.msg b/zed_interfaces/msg/BoundingBox3D.msg deleted file mode 100644 index 89b3d647..00000000 --- a/zed_interfaces/msg/BoundingBox3D.msg +++ /dev/null @@ -1,8 +0,0 @@ -# 1 ------- 2 -# /. /| -# 0 ------- 3 | -# | . | | -# | 5.......| 6 -# |. |/ -# 4 ------- 7 -zed_interfaces/Keypoint3D[8] corners diff --git a/zed_interfaces/msg/Keypoint2Df.msg b/zed_interfaces/msg/Keypoint2Df.msg deleted file mode 100644 index dbadcaf2..00000000 --- a/zed_interfaces/msg/Keypoint2Df.msg +++ /dev/null @@ -1 +0,0 @@ -float32[2] kp diff --git a/zed_interfaces/msg/Keypoint2Di.msg b/zed_interfaces/msg/Keypoint2Di.msg deleted file mode 100644 index 16a8348d..00000000 --- a/zed_interfaces/msg/Keypoint2Di.msg +++ /dev/null @@ -1 +0,0 @@ -uint32[2] kp diff --git a/zed_interfaces/msg/Keypoint3D.msg b/zed_interfaces/msg/Keypoint3D.msg deleted file mode 100644 index aa21f09f..00000000 --- a/zed_interfaces/msg/Keypoint3D.msg +++ /dev/null @@ -1 +0,0 @@ -float32[3] kp diff --git a/zed_interfaces/msg/Object.msg b/zed_interfaces/msg/Object.msg deleted file mode 100644 index df86fda5..00000000 --- a/zed_interfaces/msg/Object.msg +++ /dev/null @@ -1,61 +0,0 @@ -# Object label -string label - -# Object label ID -int16 label_id - -# Object sub -string sublabel - -# Object confidence level (1-99) -float32 confidence - -# Object centroid position -float32[3] position - -# Position covariance -float32[6] position_covariance - -# Object velocity -float32[3] velocity - -# Tracking available -bool tracking_available - -# Tracking state -# 0 -> OFF (object not valid) -# 1 -> OK -# 2 -> SEARCHING (occlusion occurred, trajectory is estimated) -int8 tracking_state - -# Action state -# 0 -> IDLE -# 2 -> MOVING -int8 action_state - -# 2D Bounding box projected to Camera image -zed_interfaces/BoundingBox2Di bounding_box_2d - -# 3D Bounding box in world frame -zed_interfaces/BoundingBox3D bounding_box_3d - -# 3D dimensions (width, height, lenght) -float32[3] dimensions_3d - -# Is skeleton available? -bool skeleton_available - -# 2D Bounding box projected to Camera image of the person head -zed_interfaces/BoundingBox2Df head_bounding_box_2d - -# 3D Bounding box in world frame of the person head -zed_interfaces/BoundingBox3D head_bounding_box_3d - -# 3D position of the centroid of the person head -float32[3] head_position - -# 2D Person skeleton projected to Camera image -zed_interfaces/Skeleton2D skeleton_2d - -# 3D Person skeleton in world frame -zed_interfaces/Skeleton3D skeleton_3d diff --git a/zed_interfaces/msg/ObjectsStamped.msg b/zed_interfaces/msg/ObjectsStamped.msg deleted file mode 100644 index 6a74cf7b..00000000 --- a/zed_interfaces/msg/ObjectsStamped.msg +++ /dev/null @@ -1,5 +0,0 @@ -# Standard Header -std_msgs/Header header - -# Array of `object_stamped` topics -zed_interfaces/Object[] objects diff --git a/zed_interfaces/msg/RGBDSensors.msg b/zed_interfaces/msg/RGBDSensors.msg deleted file mode 100644 index ffabc3f2..00000000 --- a/zed_interfaces/msg/RGBDSensors.msg +++ /dev/null @@ -1,16 +0,0 @@ -# Header -Header header - -# CameraInfo -sensor_msgs/CameraInfo rgbCameraInfo -sensor_msgs/CameraInfo depthCameraInfo - -# Raw -sensor_msgs/Image rgb -sensor_msgs/Image depth - -# IMU -sensor_msgs/Imu imu - -# Magnetometer -sensor_msgs/MagneticField mag diff --git a/zed_interfaces/msg/Skeleton2D.msg b/zed_interfaces/msg/Skeleton2D.msg deleted file mode 100644 index 2e64d28e..00000000 --- a/zed_interfaces/msg/Skeleton2D.msg +++ /dev/null @@ -1,21 +0,0 @@ -# Skeleton joints indices -# 16-14 15-17 -# \ / -# 0 -# | -# 2------1------5 -# | | | | -# | | | | -# 3 | | 6 -# | | | | -# | | | | -# 4 8 11 7 -# | | -# | | -# | | -# 9 12 -# | | -# | | -# | | -# 10 13 -zed_interfaces/Keypoint2Df[18] keypoints diff --git a/zed_interfaces/msg/Skeleton3D.msg b/zed_interfaces/msg/Skeleton3D.msg deleted file mode 100644 index 136859f0..00000000 --- a/zed_interfaces/msg/Skeleton3D.msg +++ /dev/null @@ -1,21 +0,0 @@ -# Skeleton joints indices -# 16-14 15-17 -# \ / -# 0 -# | -# 2------1------5 -# | | | | -# | | | | -# 3 | | 6 -# | | | | -# | | | | -# 4 8 11 7 -# | | -# | | -# | | -# 9 12 -# | | -# | | -# | | -# 10 13 -zed_interfaces/Keypoint3D[18] keypoints diff --git a/zed_interfaces/package.xml b/zed_interfaces/package.xml deleted file mode 100644 index 5b6cad8e..00000000 --- a/zed_interfaces/package.xml +++ /dev/null @@ -1,29 +0,0 @@ - - - zed_interfaces - 3.1.0 - zed_interfaces is a packages that defines custom messages, services and actions used by the - zed-ros-wrapper package and other packages. - - STEREOLABS - MIT - - http://stereolabs.com/ - https://github.com/stereolabs/zed-ros-wrapper - https://github.com/stereolabs/zed-ros-wrapper/issues - - catkin - - std_msgs - sensor_msgs - actionlib_msgs - geometry_msgs - message_generation - - std_msgs - sensor_msgs - actionlib_msgs - geometry_msgs - message_generation - - diff --git a/zed_interfaces/srv/reset_odometry.srv b/zed_interfaces/srv/reset_odometry.srv deleted file mode 100644 index 931e58c3..00000000 --- a/zed_interfaces/srv/reset_odometry.srv +++ /dev/null @@ -1,2 +0,0 @@ ---- -bool reset_done \ No newline at end of file diff --git a/zed_interfaces/srv/reset_tracking.srv b/zed_interfaces/srv/reset_tracking.srv deleted file mode 100644 index 931e58c3..00000000 --- a/zed_interfaces/srv/reset_tracking.srv +++ /dev/null @@ -1,2 +0,0 @@ ---- -bool reset_done \ No newline at end of file diff --git a/zed_interfaces/srv/set_led_status.srv b/zed_interfaces/srv/set_led_status.srv deleted file mode 100644 index c2f49e4b..00000000 --- a/zed_interfaces/srv/set_led_status.srv +++ /dev/null @@ -1,3 +0,0 @@ -bool led_enabled ---- -bool done diff --git a/zed_interfaces/srv/set_pose.srv b/zed_interfaces/srv/set_pose.srv deleted file mode 100644 index 60c0008e..00000000 --- a/zed_interfaces/srv/set_pose.srv +++ /dev/null @@ -1,10 +0,0 @@ -# Translation XYZ [meters] -float32 x -float32 y -float32 z -# Orientation RPY [radians] -float32 R -float32 P -float32 Y ---- -bool done diff --git a/zed_interfaces/srv/start_3d_mapping.srv b/zed_interfaces/srv/start_3d_mapping.srv deleted file mode 100644 index 857232ae..00000000 --- a/zed_interfaces/srv/start_3d_mapping.srv +++ /dev/null @@ -1,13 +0,0 @@ -# Starts 3D fused pointcloud generation, if not automatically enabled with the parameter `mapping/mapping_enabled` - -# Resolution of the fused pointcloud in meters [0.01, 0.2] -float32 resolution - -# Max mapping range in meters [0.2, 20.0] -float32 max_mapping_range - -# Frequency of the publishing of the fused colored point cloud -float32 fused_pointcloud_freq - ---- -bool done diff --git a/zed_interfaces/srv/start_object_detection.srv b/zed_interfaces/srv/start_object_detection.srv deleted file mode 100644 index 099e3176..00000000 --- a/zed_interfaces/srv/start_object_detection.srv +++ /dev/null @@ -1,38 +0,0 @@ -# Starts object detection, if not automatically enabled with the parameter `object_detection/od_enabled` - -# OD Model -# '0': MULTI_CLASS_BOX - '1': MULTI_CLASS_BOX_ACCURATE - '2': HUMAN_BODY_FAST - '3': HUMAN_BODY_ACCURATE -int8 model - -# Minimum Confidence level -float32 confidence - -# MAx detection range -float32 max_range - -# Object tracking -bool tracking - -# Body Fitting -bool sk_body_fitting - -# Detect people (valid for Multi-class model) -bool mc_people - -# Detect vehicles -bool mc_vehicles - -# Detect bags -bool mc_bag - -# Detect animals -bool mc_animal - -# Detect electronic devices -bool mc_electronics - -# Detect fruits and vegetables -bool mc_fruit_vegetable - ---- -bool done diff --git a/zed_interfaces/srv/start_remote_stream.srv b/zed_interfaces/srv/start_remote_stream.srv deleted file mode 100644 index 6741958e..00000000 --- a/zed_interfaces/srv/start_remote_stream.srv +++ /dev/null @@ -1,25 +0,0 @@ -# Defines the codec used for streaming (`0`: AVCHD [H264], `1`: HEVC [H265]) -# Note: If HEVC (H265) is used, make sure the recieving host is compatible with HEVC decoding (basically a pascal NVIDIA card). If not, prefer to use AVCHD (H264) since every compatible NVIDIA card supports AVCHD decoding -uint8 codec=0 - -# Defines the PORT the data will be streamed on. -# Note: port must be an even number. Any odd number will be rejected. -uint16 port=30000 - -# Defines the streaming BITRATE in Kbits/s -uint32 bitrate=2000 - -# Defines the GOP SIZE in frame unit. -# Note: if value is set to -1, the gop size will match 2 seconds, depending on camera fps. -# Note: The gop size determines the maximum distance between IDR/I-frames. Very high GOP size will result in slightly more efficient compression, especially on static scene. But it can result in more latency if IDR/I-frame packet are lost during streaming. -# Note: Default value is -1. Maximum allowed value is 256 (frames). -int32 gop_size=-1 - -# Enable/Disable adaptive bitrate -# Note: Bitrate will be adjusted regarding the number of packet loss during streaming. -# Note_ if activated, bitrate can vary between [bitrate/4, bitrate] -# Warning: Currently, the adaptive bitrate only works when "sending" device is a NVIDIA jetson (X1,X2,Xavier,Nano) -bool adaptative_bitrate=False ---- -bool result -string info diff --git a/zed_interfaces/srv/start_svo_recording.srv b/zed_interfaces/srv/start_svo_recording.srv deleted file mode 100644 index 4b6a3dda..00000000 --- a/zed_interfaces/srv/start_svo_recording.srv +++ /dev/null @@ -1,5 +0,0 @@ -# Full path is requiredm no relative paths -string svo_filename ---- -bool result -string info diff --git a/zed_interfaces/srv/stop_3d_mapping.srv b/zed_interfaces/srv/stop_3d_mapping.srv deleted file mode 100644 index 25872614..00000000 --- a/zed_interfaces/srv/stop_3d_mapping.srv +++ /dev/null @@ -1,5 +0,0 @@ -# Stops 3D fused pointcloud generation, if automatically enabled with the parameter `mapping/mapping_enabled` or with -# the `start_object_detection` service - ---- -bool done diff --git a/zed_interfaces/srv/stop_object_detection.srv b/zed_interfaces/srv/stop_object_detection.srv deleted file mode 100644 index 9fe28601..00000000 --- a/zed_interfaces/srv/stop_object_detection.srv +++ /dev/null @@ -1,5 +0,0 @@ -# Stops object detection, if automatically enabled with the parameter `object_detection/od_enabled` or with -# the `start_mapping` service - ---- -bool done diff --git a/zed_interfaces/srv/stop_remote_stream.srv b/zed_interfaces/srv/stop_remote_stream.srv deleted file mode 100644 index 29c919ac..00000000 --- a/zed_interfaces/srv/stop_remote_stream.srv +++ /dev/null @@ -1,2 +0,0 @@ ---- -bool done diff --git a/zed_interfaces/srv/stop_svo_recording.srv b/zed_interfaces/srv/stop_svo_recording.srv deleted file mode 100644 index 17c8551b..00000000 --- a/zed_interfaces/srv/stop_svo_recording.srv +++ /dev/null @@ -1,3 +0,0 @@ ---- -bool done -string info diff --git a/zed_interfaces/srv/toggle_led.srv b/zed_interfaces/srv/toggle_led.srv deleted file mode 100644 index 4bec2c79..00000000 --- a/zed_interfaces/srv/toggle_led.srv +++ /dev/null @@ -1,2 +0,0 @@ ---- -bool new_led_status From 2a80b0f70dfbf86bf301813bcb3072720bba42cf Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Thu, 22 Jul 2021 17:06:38 +0200 Subject: [PATCH 047/199] Update README --- zed-ros-interfaces | 2 +- zed_wrapper/README.md | 7 +++++++ 2 files changed, 8 insertions(+), 1 deletion(-) diff --git a/zed-ros-interfaces b/zed-ros-interfaces index e58b1a39..db80db3d 160000 --- a/zed-ros-interfaces +++ b/zed-ros-interfaces @@ -1 +1 @@ -Subproject commit e58b1a393db9a8247b8336936ab20724b33bcf3a +Subproject commit db80db3d206886d51df5a1d6bb971cd5b519bce3 diff --git a/zed_wrapper/README.md b/zed_wrapper/README.md index df76353c..d63221ff 100644 --- a/zed_wrapper/README.md +++ b/zed_wrapper/README.md @@ -46,6 +46,13 @@ Open a terminal, clone the repository, update the dependencies and build the pac $ catkin_make -DCMAKE_BUILD_TYPE=Release $ source ./devel/setup.bash +#### Update the repository + +To update the repository to the latest release you must use the following command to retrieve the latest commits of `zed-ros-wrapper` and of all the submodules: + + $ git checkout master # if you are not on the main branch + $ git pull --recurse-submodules + ### Run the program To launch the ZED node use: From 1b01e19f140e25ec28ca780be1c00a74e6ab191b Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Thu, 22 Jul 2021 17:12:43 +0200 Subject: [PATCH 048/199] Update README --- README.md | 23 +++++++- zed_wrapper/README.md | 120 ------------------------------------------ 2 files changed, 21 insertions(+), 122 deletions(-) delete mode 100644 zed_wrapper/README.md diff --git a/README.md b/README.md index da325693..cb5dd480 100644 --- a/README.md +++ b/README.md @@ -24,7 +24,7 @@ or - [ZED SDK **≥ 3.5**](https://www.stereolabs.com/developers/) and its dependency [CUDA](https://developer.nvidia.com/cuda-downloads) - [ROS Melodic](http://wiki.ros.org/melodic/Installation/Ubuntu) -### Build the program +### Build the repository The zed_ros_wrapper is a catkin package. It depends on the following ROS packages: @@ -55,7 +55,22 @@ Open a terminal, clone the repository, update the dependencies and build the pac $ catkin_make -DCMAKE_BUILD_TYPE=Release $ source ./devel/setup.bash -### Run the program +#### Update the repository + +To update the repository to the latest release you must use the following command to retrieve the latest commits of `zed-ros-wrapper` and of all the submodules: + + $ git checkout master # if you are not on the main branch + $ git pull --recurse-submodules + +Remember to always clean the cache of your catkin workspace before compiling with the `catkin_make` command to be sure that everything will work as expected: + + $ roscd + $ cd .. + $ rm -rf build + $ rm -rf devel + $ catkin_make -DCMAKE_BUILD_TYPE=Release + +### Run the ZED wrapper To launch the ZED node use @@ -71,6 +86,10 @@ ZED2 camera: $ roslaunch zed_wrapper zed2.launch +ZED2i camera: + + $ roslaunch zed_wrapper zed2i.launch + To select the ZED from its serial number: $ roslaunch zed_wrapper zed.launch serial_number:=1010 #replace 1010 with the actual SN diff --git a/zed_wrapper/README.md b/zed_wrapper/README.md deleted file mode 100644 index d63221ff..00000000 --- a/zed_wrapper/README.md +++ /dev/null @@ -1,120 +0,0 @@ -# Stereolabs ZED Camera - ROS Integration - -This package lets you use the ZED stereo camera with ROS. It outputs the camera left and right images, depth map, point cloud, pose information and supports the use of multiple ZED cameras. - -[More information](https://www.stereolabs.com/documentation/guides/using-zed-with-ros/introduction.html) - -## Getting started - -- First, download the latest version of the ZED SDK on [stereolabs.com](https://www.stereolabs.com/developers/) -- [Install](#build-the-program) the ZED ROS wrapper -- For more information, check out our [ROS documentation](https://www.stereolabs.com/documentation/guides/using-zed-with-ros/introduction.html). If you want to customize the wrapper, check the [ZED API documentation](https://www.stereolabs.com/developers/documentation/API/) - -### Prerequisites - -- Ubuntu 16.04 or newer (Ubuntu 18 recommended) -- [ZED SDK **≥ 3.0**](https://www.stereolabs.com/developers/) and its dependency [CUDA](https://developer.nvidia.com/cuda-downloads) -- [ROS Kinetic](http://wiki.ros.org/kinetic/Installation/Ubuntu) or [ROS Melodic](http://wiki.ros.org/melodic/Installation/Ubuntu) - -### Build the program - -The zed_ros_wrapper is a catkin package. It depends on the following ROS packages: - - - nav_msgs - - tf2_geometry_msgs - - message_runtime - - catkin - - roscpp - - stereo_msgs - - rosconsole - - robot_state_publisher - - urdf - - sensor_msgs - - image_transport - - diagnostic_updater - - dynamic_reconfigure - - tf2_ros - - message_generation - - nodelet - -Open a terminal, clone the repository, update the dependencies and build the packages: - - $ cd ~/catkin_ws/src - $ git clone https://github.com/stereolabs/zed-ros-wrapper.git - $ cd ../ - $ rosdep install --from-paths src --ignore-src -r -y - $ catkin_make -DCMAKE_BUILD_TYPE=Release - $ source ./devel/setup.bash - -#### Update the repository - -To update the repository to the latest release you must use the following command to retrieve the latest commits of `zed-ros-wrapper` and of all the submodules: - - $ git checkout master # if you are not on the main branch - $ git pull --recurse-submodules - -### Run the program - -To launch the ZED node use: - - - ZED camera: `$ roslaunch zed_wrapper zed.launch` - - ZED Mini camera: `$ roslaunch zed_wrapper zedm.launch` - - ZED2 camera: `$ roslaunch zed_wrapper zed2.launch` - - To select the ZED from its serial number: - `$ roslaunch zed_wrapper zed.launch serial_number:=1010 #replace 1010 with the actual SN` - -### Parameters -To configure the features of the ZED node you can modify the configuration YAML files in the `params` folder. -A detailed description of each parameter is available in the [official online documentation](https://www.stereolabs.com/documentation/guides/using-zed-with-ros/introduction.html). - -### SVO recording -[SVO recording](https://www.stereolabs.com/docs/video/#video-recording) can be started and stopped while the ZED node is running using the service `start_svo_recording` and the service `stop_svo_recording`. -[More information](https://www.stereolabs.com/docs/ros/zed_node/#services) - -### Object Detection -The SDK v3.0 introduces the Object Detection and Tracking module. **The Object Detection module is available only with a ZED 2 camera**. -The Object Detection can be enabled automatically when the node start setting the parameter `object_detection/od_enabled` to `true` in the file `zed2.yaml`. -The Object Detection can be enabled/disabled manually calling the services `start_object_detection` and `stop_object_detection`. - -### Spatial Mapping -The Spatial Mapping can be enabled automatically when the node start setting the parameter `mapping/mapping_enabled` to `true` in the file `common.yaml`. -The Spatial Mapping can be enabled/disabled manually calling the services `start_3d_mapping` and `stop_3d_mapping`. - -### Diagnostic -The ZED node publishes diagnostic information that can be used by the robotic system using a [diagnostic_aggregator node](http://wiki.ros.org/diagnostic_aggregator). - -With the `rqt` plugin `Runtime monitor`, it is possible to retrieve all the diagnostic information, checking that the node -is working as expected. - -### 2D mode -For robots moving on a planar surface it is possible to activate the "2D mode" (parameter `tracking/two_d_mode` in `common.yaml`). -The value of the coordinate Z for odometry and pose will have a fixed value (parameter `tracking/fixed_z_value` in `common.yaml`). -Roll and pitch and relative velocities will be fixed to zero. - -### Diagnostic -The ZED node publishes diagnostic information that can be used by the robotic system using a [diagnostic_aggregator node](http://wiki.ros.org/diagnostic_aggregator). - -With the `rqt` plugin `Runtime monitor`, it is possible to retrieve all the diagnostic information, checking that the node is working as expected: - -![](../images/rqt_diagnostic.jpg) - -A brief explanation of each field: - - - `Component`: name of the diagnostic component - - `Message`: summary of the status of the ZED node - - `HardwareID`: Model of the ZED camera and its serial number - - `Capture`: grabbing frequency (if video or depth data are subscribed) and the percentage respect to the camera frame rate - - `Processing time`: time in seconds spent to elaborate data and the time limit to achieve max frame rate - - `Depth status`: indicates if the depth processing is performed - - `Point Cloud`: point cloud publishing frequency (if there is at least a subscriber) and the percentage respect to the camera frame rate - - `Floor Detection`: if the floor detection is enabled, indicates if the floor has been detected and the camera position correctly initialized - - `Tracking status`: indicates the status of the tracking, if enabled - - `IMU`: the publishing frequency of the IMU topics, if the camera is the ZED Mini and there is at least a subscriber - - -[Detailed information](https://www.stereolabs.com/documentation/guides/using-zed-with-ros/introduction.html) - - - - From ad369f1b9bd184313226b84e6e4d98007736d828 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Thu, 22 Jul 2021 17:30:46 +0200 Subject: [PATCH 049/199] Update README --- README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index cb5dd480..7d69aacf 100644 --- a/README.md +++ b/README.md @@ -49,7 +49,7 @@ The zed_ros_wrapper is a catkin package. It depends on the following ROS package Open a terminal, clone the repository, update the dependencies and build the packages: $ cd ~/catkin_ws/src - $ git clone https://github.com/stereolabs/zed-ros-wrapper.git + $ git clone --recursive https://github.com/stereolabs/zed-ros-wrapper.git $ cd ../ $ rosdep install --from-paths src --ignore-src -r -y $ catkin_make -DCMAKE_BUILD_TYPE=Release @@ -60,7 +60,7 @@ Open a terminal, clone the repository, update the dependencies and build the pac To update the repository to the latest release you must use the following command to retrieve the latest commits of `zed-ros-wrapper` and of all the submodules: $ git checkout master # if you are not on the main branch - $ git pull --recurse-submodules + $ git pull --recurse-submodules # update recursively all the submodules Remember to always clean the cache of your catkin workspace before compiling with the `catkin_make` command to be sure that everything will work as expected: From 05498c976df2a51306b4bc0d7e0e6b3837261e26 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Thu, 22 Jul 2021 17:58:32 +0200 Subject: [PATCH 050/199] Update README --- zed-ros-interfaces | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/zed-ros-interfaces b/zed-ros-interfaces index db80db3d..99428eea 160000 --- a/zed-ros-interfaces +++ b/zed-ros-interfaces @@ -1 +1 @@ -Subproject commit db80db3d206886d51df5a1d6bb971cd5b519bce3 +Subproject commit 99428eea90577b309da7ce6048f250fc7ef2c733 From a972f15ce56fe2d7b27cd9264282b30b1ee3f20e Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Thu, 22 Jul 2021 18:20:29 +0200 Subject: [PATCH 051/199] Update latest_changes.md --- latest_changes.md | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/latest_changes.md b/latest_changes.md index 9f9d02d7..5c176c58 100644 --- a/latest_changes.md +++ b/latest_changes.md @@ -1,6 +1,10 @@ LATEST CHANGES ============== +2021-07-22 +---------- +- Moved the `zed_interfaces` folder in the new [`zed-ros-interfaces`](https://github.com/stereolabs/zed-ros-interfaces) repository. The new repository is useful to receive the topics from a ZED node on system where the `zed-ros-wrapper` repository cannot be fully installed, i.e. systems without CUDA support. For this repository nothing changes because the `zed_interfaces` folder is replaced by the `zed-ros-interfaces` git submodule to automatically satisfy all the dependencies. + ZED2i support fix (2021-06-17) ------------------------------ - Fix sensors topics pubblication for ZED2i. The support for the new camera was not complete From 800df9f5f89607eb7087ab77db7b539959b73206 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Mon, 26 Jul 2021 09:26:01 +0200 Subject: [PATCH 052/199] Update latest_changes.md --- latest_changes.md | 3 +++ 1 file changed, 3 insertions(+) diff --git a/latest_changes.md b/latest_changes.md index 9f9d02d7..901051d1 100644 --- a/latest_changes.md +++ b/latest_changes.md @@ -1,5 +1,8 @@ LATEST CHANGES ============== +2021-07-26 +---------- +- Enabled static IMU TF broadcasting even it `publish_tf` is set to false, making the two options independent. Thx to @bjsowa ZED2i support fix (2021-06-17) ------------------------------ From b50f83cc4051c3485230fa80c643ff733583ae34 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Mon, 26 Jul 2021 12:23:52 +0200 Subject: [PATCH 053/199] Create submodule_update.yml --- .github/workflows/submodule_update.yml | 32 ++++++++++++++++++++++++++ 1 file changed, 32 insertions(+) create mode 100644 .github/workflows/submodule_update.yml diff --git a/.github/workflows/submodule_update.yml b/.github/workflows/submodule_update.yml new file mode 100644 index 00000000..5c38c8ab --- /dev/null +++ b/.github/workflows/submodule_update.yml @@ -0,0 +1,32 @@ +name: Submodule update (manual) + +on: + #schedule: + # # * is a special character in YAML so you have to quote this string + # - cron: '0 12 * * *' + workflow_dispatch: + +jobs: + submodule_update: + runs-on: ubuntu-latest + + steps: + - name: Checkout repository + uses: actions/checkout@v2 + + - name: Checkout submodules + run: git submodule update --init --recursive + + # Update references + - name: Git Submodule Update + run: | + git pull --recurse-submodules + git submodule update --remote --recursive + + - uses: stefanzweifel/git-auto-commit-action@v4 + with: + commit_message: Automatic submodule update + commit_user_name: bot-stereolabs + commit_user_email: bot-stereolabs@example.org + commit_author: bot-stereolabs + status_options: '--untracked-files=no' From 1e222bed3dca29df17e44b5bcfa4b68a2f3e19cc Mon Sep 17 00:00:00 2001 From: Aymeric Dujardin Date: Mon, 26 Jul 2021 13:18:31 +0200 Subject: [PATCH 054/199] Fix submodule URL for github action --- .gitmodules | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.gitmodules b/.gitmodules index d1041044..d10030ea 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,3 +1,3 @@ [submodule "zed-ros-interfaces"] path = zed-ros-interfaces - url = git@github.com:stereolabs/zed-ros-interfaces.git + url = https://github.com/stereolabs/zed-ros-interfaces.git From 7306f060aa49acda79f6b8477c23433d08c376e3 Mon Sep 17 00:00:00 2001 From: bot-stereolabs Date: Mon, 26 Jul 2021 11:19:20 +0000 Subject: [PATCH 055/199] Automatic submodule update --- zed-ros-interfaces | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/zed-ros-interfaces b/zed-ros-interfaces index 99428eea..cbcf6682 160000 --- a/zed-ros-interfaces +++ b/zed-ros-interfaces @@ -1 +1 @@ -Subproject commit 99428eea90577b309da7ce6048f250fc7ef2c733 +Subproject commit cbcf66821b86693663efe905dc09e73994ff4ad2 From 5f31ed75b81b2db4de4df8f65828526288e6ced2 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Mon, 26 Jul 2021 14:32:52 +0200 Subject: [PATCH 056/199] Update latest_changes.md --- latest_changes.md | 3 --- 1 file changed, 3 deletions(-) diff --git a/latest_changes.md b/latest_changes.md index 7ea89156..a2214047 100644 --- a/latest_changes.md +++ b/latest_changes.md @@ -3,9 +3,6 @@ LATEST CHANGES 2021-07-26 ---------- - Enabled static IMU TF broadcasting even it `publish_tf` is set to false, making the two options independent. Thx to @bjsowa - -2021-07-22 ----------- - Moved the `zed_interfaces` folder in the new [`zed-ros-interfaces`](https://github.com/stereolabs/zed-ros-interfaces) repository. The new repository is useful to receive the topics from a ZED node on system where the `zed-ros-wrapper` repository cannot be fully installed, i.e. systems without CUDA support. For this repository nothing changes because the `zed_interfaces` folder is replaced by the `zed-ros-interfaces` git submodule to automatically satisfy all the dependencies. ZED2i support fix (2021-06-17) From ebd69f46157c98eb024134ea8f8e28f9cfc2e01f Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Mon, 26 Jul 2021 15:02:45 +0200 Subject: [PATCH 057/199] Update README.md --- README.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/README.md b/README.md index 7d69aacf..277e894c 100644 --- a/README.md +++ b/README.md @@ -6,6 +6,8 @@ This package lets you use the ZED stereo camera with ROS. It outputs the camera [More information](https://www.stereolabs.com/documentation/guides/using-zed-with-ros/introduction.html) +**Note:** The `zed_interfaces` package has been removed from this repository and moved to its own [`zed-ros-interfaces` repository](https://github.com/stereolabs/zed-ros-interfaces) for allowing better integration of the ZED Wrapper on remote ground stations that do not require the full package to be installed. To update your repository please follow the [new update instructions](https://github.com/stereolabs/zed-ros-wrapper#update-the-repository). For more information please read issue [#750](https://github.com/stereolabs/zed-ros-wrapper/issues/750). + ## Getting started - First, download the latest version of the ZED SDK on [stereolabs.com](https://www.stereolabs.com/developers/) From 3454e070ca4be8d6e6648dab208c114e5e1c667b Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Mon, 26 Jul 2021 15:53:34 +0200 Subject: [PATCH 058/199] Moved cameras meshes STL files to zed_interfaces --- zed-ros-interfaces | 2 +- zed_wrapper/urdf/models/zed.stl | Bin 48684 -> 0 bytes zed_wrapper/urdf/models/zed2.stl | Bin 48684 -> 0 bytes zed_wrapper/urdf/models/zed2i.stl | Bin 718084 -> 0 bytes zed_wrapper/urdf/models/zedm.stl | Bin 75584 -> 0 bytes zed_wrapper/urdf/zed_descr.urdf.xacro | 2 +- 6 files changed, 2 insertions(+), 2 deletions(-) delete mode 100644 zed_wrapper/urdf/models/zed.stl delete mode 100644 zed_wrapper/urdf/models/zed2.stl delete mode 100644 zed_wrapper/urdf/models/zed2i.stl delete mode 100644 zed_wrapper/urdf/models/zedm.stl diff --git a/zed-ros-interfaces b/zed-ros-interfaces index cbcf6682..294df84a 160000 --- a/zed-ros-interfaces +++ b/zed-ros-interfaces @@ -1 +1 @@ -Subproject commit cbcf66821b86693663efe905dc09e73994ff4ad2 +Subproject commit 294df84a01e7ad35d20b33cf96604c67593d38ae diff --git a/zed_wrapper/urdf/models/zed.stl b/zed_wrapper/urdf/models/zed.stl deleted file mode 100644 index 913d2cd9657a77cda8be4f1be69c8728a6cce9a5..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 48684 zcmbuId%RUuz4o`4R|qd5nuO*nh^Yty8b@W#JynRXP*KkT6)z~l2DYG7Mkrn}QK45X zO}wNaHvth5!XvD;CLJQIlcMImG&QmCyp?pkrv+y|qw(;0#%*mspR@n^edh1^{+==B z8gtIE=NfCyf1A+qozdfmf3xeq4LhalIbR#p^?&*Iqn+g2Mw>HoaA zM{V)os_}LS%_x!f=>F06>LDL(Hy-L&e`k-{m(Sic3;UrNCDhw()sk(it2_VnOBs5u zem(msDmCDhyX7(XhjzVTddg{?g-RJcA%eVD2 z9_lx;GH`9T_fCv>s7n1B5*7AC{T|-ZvC{4BJmR4$t(zvw-d6Z|lr7xhx5xa2m5bl$ z9q~|=)=d-ZrhHiW&4>G%TI!c34k$+%Wribk;26q4Z}Xwv;21$Gp;MkDH!4AzS`}MDqFXTBlxoscE44nABJ{4RAu*%QK?^=u%ChPP}(IlqlEqZBdt=E9ZNOz zBTZ;V2|JE!*l%vDRAr9|SMQcxXlmdq$0TD50uyAL2?B@lch$0`T)l5L!zKd%cQ`D^=Mmnej*yno+`DcOxFE zvR7nte547@C}HOh5f4?_nT7F46Pi&%=RxjB)OnPuly->@*S0jTn|n{OY~|?Y{f@q= zUdH*&hfZc(Aa)5=DQ)8%m0aSMpG}5G{>F*_+N^}#USPXRL{v&gJkGnSrFq0Vi;Dle zc#vs(kCzq{KN<4RKApUEanZ8(P|xG-D>F5l*d;V_x|JlT2r}4UKp{W zIO~X|HMdnzt-WDI@lH!q9q}7yK2&sF)1yANtTgexhl=IhcdokxYSpLAFWzq+?zBo< z8xnL^zqAMT)z{`ME&lq<;ZCb|FIzZ$@{;1X9}V$ZHTjyB=J6Yr76)B^m}wRF5BzqW zwyaq4xud;(rS95u^>+KAwGbcw*2BfyT{|?Zw@Y*@`_XIBqT)AS7+lx3gYJCM!s6uJ zMtS{ELR+G=>(R6H=-V={*s{wAv#(h9z|!O7-_I@narGq6L))vL8d}BumiEvVru*?> z`FR}JX-;wQrx(_>CHt2i&keq>xOUBDUaNGEB0hNf%HqfSt;hzKBk{FEmKPJR-niPf=-$;nrTd}1MO^*Ms$%`qA61?x_u)xDT2b`; z_+GE)(ke6BZQIS6=&c2Fit6?Yn%!1`U`Be|jQw`nh`GhI+28aCZLhX4-70N6;#WUd zRjhsL&FY-1Tbhr2_K{-csYS(Ym7YtqC7^k>a|v}=>kkKX{zIAc_Nynky*u{9gaKwsy z^MIvUUVa9Lu6iWj`|aJb&ZS@f$5-Zk_n7C6E41Y5Gat&2s`qGqr1U%Ny@&EW4%@l; z3+48yH>}7%9pB`Q5_l9-=I7g*hdZs(*1A2{y4u3D2hM%ZPhOJmd)ts^?4M2dEzL*0 zaJU)yY_01tvTW6q4NLQ-Qx0?X5Bj&~v}O6fe(q>*Uun5fEf1H!}-{QI@Eo? zxW_4F+wWSqDBu3Y!OhwyRO&H$VZLCeQQqEFLR+P@+bUcQZW=Q$-+i|cX8*7*M&;1W zbMx6(O!7Rm?fR*qRq=79th}${_b_#%&dlcJ@jDDwYO*$ykavY!kqZN6}yZ4%&bC0+l;%-K}ZF?Qt|F~dIUS18n(XJWk zt+`T{SA)E~8hC`ZMEe9f-YOi=zx-fTzWKl3tl*3~_}NGDYugo7?VsBZJ(t+CsA&lZ zp6y&h>uL+r9(wN6bE3A^^-z`ir3u};+7IoY>!Ig9Jtt~wT>}4SJpE|r{K0E3tROyb zUB?~qw$_gM^H=@08WN{$?UMiU`B$qTF8Plx`A_O24SmA}oy_)-P?b*)dTN1pJxATE zPdsFY$C8-`=F^A$X)U%RhIipQ+x3tOcGRiGFC z{=s74ZXZWHn1M=()i*CGPT$tqJ8HESVyY1T>dwW*eou7u&bylNiPt+WDq7ckCE~$a zAgDsTcHM&F($*~E!HiE_{)c(Rzw9|E;-LgppttRPfAM&0IWutnG=o@)Ef>xyzWoD# zw&W93Vf&)d_Z7DcJU!yU3{*;BMqb_ni1b4VszBQ~;=zol>WJ&Vm{GL*WpiX)G2;_w zynItp`}f%qf*GGU`-{_yLw~q2LNMbKpI<$#n77m05rP??*!|~M7QJ8lG(s@r6X*Q& zvSQel?Gb_*pD6bs?zTie58b=E->&+_%;GQKzb)_yn##ae^72m|s2MGZ0jP<`pjNhpIT1KN>Bv?SGagTn9;h3FFN7S&p=Q`kNCA#udhCVxkT7=5S(TBM0^IID(!jGf#dV| znL!Dv(2vUK9*Lea1CtGZ(_bP4Gd{8PoId&fLpMbTW_)5)-Z%gI&GRD!Gd?l;slNHXT`!0b%=pCG zZGH1s|NPsv;V5CoCngOWmB+6iN>By*>gUhRy`NoY?^7s{p8Pru`-v_VF<5vS_d}79w@8$8U0W&`FU$@?v$FBy=_{4>OzB!Ly z4VdwXcgN1m<5vS_eBy;IGxJTGTDyg#M6b2>Dw+u$nfrJu%&D3|*qK2VdN5<_?jY>! zBSJ9a6Ly9ZA(-(AJBx}C%=mTeXM|Drz`AVc!N>Bwq zJ6~yopbCVYue3o>1;Wl(+90R`VdpE6eWm@N3IyjX;oen(DiHY3JANHxM(ZL@|9{R* zjEK(})I(|B4`|#ErR|w7vUi!$-Xg}mfq2iApb7-$L~(){t&6zvj109@MUVKt(pIUT zoj*i+&J63)4|Ae;Ka`*fG-fz)f*GiMBHj=6Q2MtcdL%rQpbCDUbnKZwa_zjx@u37& zAod#BEC1m3d)gqV0&)7NBlFMCyR{91Di9x6dgq@uPiupq3dDJ1`{cJQ9nc0r6^J{& z);I64@9+AD$A|WuDnrcfn-4E8Z;P-h5Z`>JZ~jEfe#bTTpb7-%D~$=NK-ig6Wb87d zbrIW{Q-ol~C+y5ALNMbKc77H4JeWxk)v)J!-qj=6&buP5Qi3Y@*?CtR1XUpHysHg@ zDiC(w)doQo2s`g;gP;n8op(j{uJ)WN5DjN!xEo<+091jnmE!kHb`Vs7DDP|K~#Df{Fi@3Z~oR{~IYvB>41XUo) z`^kBEpE@!UnbEq4%lp@Pc`rQTp#)VR%KPGZc^^LF!Hm{LT;7Mz%e(cFGlLRTfhh0k z=jA>9$nnAN2cW{Y4!o~{IR3qXs+8tkk#O%a0}r2w-;?28d3>+NHCOl!i7TK&5531u zI!z!`9;s$M99k8*105Mx^w7Gx_9twW-eV`-m|#ob!PO~fVf>!H_QNMwDQuPAm1hg_ zJucTwxrAoW5)$ct;QKnRbOJ#?t>k(*JuF+o)pXd-Z#i5-dy91dO2qdSYT+}-J#TH5 zw$}B~y62T6kt+|=tx^xIE1y&3496b&9{di;ZI!BYWfi>Rgl2q#-=4W1sv2DOoU6@H zDNbm{C-|M7>!B)L!-Pt4LNh+W?l{>zUGos!%EY%_uWU{Jq?7TzMLL=(nMy zKO5nw%8}1?q@jm?8%jTn>9z?~={Y#`;JFt2hkj|oSvyp=gzJ_-^BU}Wpe3r(*+85? zEuB+wMrMyetZng*LG{p;Jg7uJw^hDM=Rt7-wR9H77NS*cuSn58rTYODTY_^f*ZyET zXM^c}_yjAtgsKp89Z}i?8J!z)FW~&hb#ktUWDG-I(C4Sm`vOtDZoRb_rjlGxD|xj6SYUg$HTZ!&m8l7Ht!H4u%J5 z*TYxwT$?83m@xAz&b91z%(cDqE^2uyb9T1d@jtakjIH#}PQ7zX_FsK2uFvlIV&&Dt z&(8kw%a;0KE4Ng-TzEq^cGblmp&2EH^?Tkn3d^1!k&4($@-e{of<{e6d2CQWN~_UMD{ZmYfgw;7dA z1O2_L#M9lbt6l#4Gnd>pv$d{!B<-Q9^uFqO{`a$wmyWGmIJmXGsqffqm-El4ymHN$ zy6zFzLo-U~UT}%uP5pLl;Ysr=6UVmJwYQVn-&k9H&Hu?R-NWy>61o?Zc0C3*&&oEw z(7pT2{;l=tBc^A)Ztv9n+>PVw+CP`jj1o=zPR&+z+o$`8gU5NJec=t~){Z`Lr|i%p zTI<^OE0&F`H67HP?O(g30ik_TI^8O5iSB8aP}NH>O~|@DdU*GFlg2oorc2zip;8-q zQqOGrbK~pU_K~ZPuif>9G1-g@{C+5*dqHW}W8V)u)Q-9D`s|N?Dv#QAd)F2ncWUDU3-s+lXZpj9YnOL9n=Fh7y-O`eE`_s6(db_PsmHMTLAx{oi`)TLf zvhmMMsNeIA=IXTnds%kq=y7%Rc0E+3eraORr)O8z{d`u|rFlaAfdkeZ_v!_cv+un) zwyxf;hpN;sP3YA?uK?=pdZvmf0`U(5gW__;JYn5h{ zIPBA#Yvr}8dUqH9EUIlEvtV5I)%E?V?@t|5*RzDvjVATbx?{Gt*7|<)$?Bn}`>oP_ zb?gB%v+_z--F|XwUHx1SB^*`qyu0DPskJR9oKf9)WounqHRBt%*UD>Gb>&HZ&$Ycu zXeGB*+V*b8U6qyBrP{*sNhBg_BGF*m6g|}+Q-kf);F|#H9LRLcD2#3`u$KsRoW-lL-(S#N?V&IRHd^Tm(aFP zex-L-UYBY&^}e`%!l;9@^9S{*{jFb%_jxFxD(#c&p>-drd{`;3SGAt=FRoAS{$AzA z2TrSIK*^*+d-g)5;d^>aP6?osV0 zttqcpwWY;`dY4_-_rLP$%W8)`;CpCYrPGAg?b+x5Rm!B*Gn32TTs6=q5tY?6gLZ1kwv`grGP@e*&v`c8KBD8r< zl}qTXNT2VdelDSPl};0?(%FmpxgI(<(C0g)39YMin$Wp{KHo|G(ydZDO=w-6C)nd7 z@+>5+tAti^Tcx#h{-DoyatW=ggjRA1t)=q^eZEtgP!FYDLTl;#L7(-MCbX{7E}^w_ zE~3wRatW=agjRA1Rq6k@X%Efl|DX0692q4_sE5*StF)H>H>ppDN)uXFX_wGi`k!9< zDNssiU8P+Qt)>6(=nODTXkDdULTjliJ+s7b+3}4Vo?4~P99aAJ@<}n}_OJi2(X$gk z^Em?`KKk;CeAJWvGo4h0n9r{OQF(n?{`y~gdjEN8OY~_AAT|x0n|J8uKW9SgBIdIt zK>YEo#rc6JpW=By#phQ*b?VXu`KkFu&J!fuy{mN*^Qi%84}FpbRBzw4IN!6+Q0Lhh zp$F^gulqrFxo1J%e(FVDKhy)Q;xkJ?4BoUX|INZ)&8~+&4*;r7{Z{0cy}pxa6=|11 zT{}wZAf9NRn{V$mslgK+^w|O+df&A&A9UN?OubzXCG_bBO1p%r&_X`f0>q?Wugd?s z{+PE)PgRKdYzYu=T(>fRYU4t0wINh|RuoicEGeHl^nAw*+xa9Vw^hjCn>ak}3FDa0 zqXLcXEU9-we8I3L2uUss#B*fE57?|AEP3TO7Z8BX-hD!`0OrJ zdVk>D;#Xb$=XjwOpV#I4#XXcj%x5ftzJAi;;>v!fc_R@jK2Z#+0ZSJY*XI|Ram5z8 zJ=ePS_%KhG0{!Ox3yP7W#&{k$KKN8Os0QA>xOl(U(7Jku9(+9?X7^4d<3Q9>06K4}U>ySr8vXWuh7 zGIsftELABT_JgAu#8tmuRou4ZG4oUvRtgE88OGgtQ+>wD(LJ}E`yF$Z!1nlPxBIHD z1X{&PQ4e!Op;e$!S812Px8D5S)#gP9nBQI9-}{w$*v?;9xdc?7+jM5}FV$T$pJ1hs zs5YHbT-xtCLy(3CeLJJM>V3D)kNt{+>@gK(JC{;?Li`r0Dn1N2XP5VMy>-hVUQ(4|+EyZvENh z^3!Z;Ryrj33s9&?z=Ph6iLdN+&5nMM4hjBd6doktLGQ-IthcV;@d=U+3I56y9wgvF z@5V&`V{Y6rDoLja)a7qf;YZpfG~*LT|LkUSq`8Eu5c5~9s1zqO;}g4|H`5&dp$F@N z;O}5jDNbm{C(2*J6y1(mcyQ>!xgYM?kg(?^<3R!*^lnVpv#3Ldggw6+6$yCIyD?$U=MEhb_BvryB;Y~s#)Q2_ zIdn+yDL2@oB;Y}Bmw?J%Urk*lSji<&%U*>IK^oiR1U&4_A|O~P^sqA)bN(Q$-Yx+T z%!)8WQa=!^P~nT|zVJhwW*P@|=-#Mn*pn{M}{f!3;c{c@wcez z0fLo64<*VoPRMjfwJnwWA-T zLxR8Ch6f3F(7Q2Fp09R%f}}%&zo~`?33$-EF;SjVUK5{A^{IpYE0PkhnbO)4hef+G9D!0LGQ+d zJr6o`NZ9kMQIUWLy&DtueD2U8VXqTLMFJl5ZcNx~ltYJvy>c2A33$-kB{1Ub^)(<^ z$t6(BUWE-o8r$OpJnYOOAXq8%u(Jts{vfU1E&&hDchnCAE4c(RoJT30CiH%ruI9Jf zBX28+sPMg!vjX33m9DN=KmC1+OQ^g0rB^hnht^H6h}Vn~>9yyYQ6jyPTr*0f*M)0F ziS#OO%_xyxv#l8=(krkvqlB*hc0WPQD3Sg>j%Jid|5ipbO6V_1+*WBu3H{B6OK3(3 zT`%quI(BsxtED5q+ESI$>CvvWl-4!LZmX0~m9F?s6ROg6=xIV#y5=%Xs7lvdrU_N) zO3*Z+DqWwNCRC*>Xw!tMbX{+nP?fGWP7|uqb=GM@Rl34FO{hxO)~5+orH>CCiK?>O zBflocw>|vERlmW~@21>V`6{IkI%Q??`2Sy#H72M+rT8~-nnA1tzq<`RSPKMIi23be zNHF6Q{N}eYK^16z=NS^rK&3?d`&zAqm@35l?l$yb#wX(6*J>@qR3VOkQ>_`F;5V^h zt5^#JRfzfRVn{HPAiVFLwU)%@`{DS?8c~_=PpPuoBj2TJ2C)+HbuvCd6}EF#N7!>_ zpi&~fA_XdDe1fZNLJuXV0v%tyqZ!0XaFtEy!CD}wLL6Tw<9kqr?eTR>n$fz5xymMN z6>F&qaeOtEW_%*Ph6O5Se1hw8!d59k73lWGs{Dij?|M(n(hOoH;%jlBV$U)6(N$sm zoWfQy1A^G`_B$)A6Q*lg(1ot~UbafkB>k?4uIwI0&758CCaHRuOh_RjlalBRV z(A7<>3nKk_aKHO>oY2)&s7o3#KTWqEnnBDFo?hdnD#SdZ(8Bo2GgTqxQLC%o!d6iQ zf@cPJaJ^qhPz8c#IE)y+ODZI&0>SgHu3HQVUO#j#7HD3HLV_88#TKuqA;AoKt1GK` zoev3S{B=^CeS`!v@ZcII^n){;kYEM`*D!&IuUpa#2(CQBcCJVYJ#?l=6=<$!3JIOJ zkp>Z8bB7E*L9CQTOYEmHLPzFTR4Hv^+?nzS`*z^UJHz*{^G(5s z@!sEi7Pjk-*zJd^l(z3OHkHzZs`&Qf&_gpy*mp8VJd{us-{Ks4XhsS9Zt1Ksp(?(O zIwUlsgndVL#6t;H@vYjShh~(p@A{5-D4{C89X#~Vj1py~O8MUKh=&rYvT-f+(2Nr0 z9;uY?4Uc#zp(-0kJT#+(eTRHxTq&U{zI8tAhh~)MRQ6$c*_XzIs%%^h3C$?6rTi?8 zE)ejmJu{S_V4yG<^NH7DH(u~7a zkw7hu5~vu59?bYHVWrT61ZrXQ-8$g@ zZw<{j^kBws2`h#DAc0!;Q!^fnLl0(9OKHZThaGX|{Hip!heS-JEeu;_k0|4zb-6wC zh^bJQ+d~g~1~494m)k>+mX8xfx*9#KZ6b-6wCh^bJQ+d~hmVc;4l zi8I7pW#xY6zDnuzN++u_b*Vxn8++dsX+~nR4kW(PNmRyzDr~oL#Df`884s>{3VW^u zRiJGg@nA+&#v{INNmQmERAIY~BOc6%%6M?qQ`iqBr~++c?>~CYNX!-SSl3w#Jy;6_ z*G5Sk=?B-IipunZtDZs+C8z>zMnLm6|pNI!D zP$}`kF-PWS&t4z#P=YGZKltmB`HTIwN8Yu?3{*;-)uDI(;X_L!9!gLJdgSch`3KXk zi+C^tl@j~S@16hcy_pdYC8z>D>yzI3xdU4w@1ti1DkZM^NALXWm2X5ml%NXqdk6H% zw~l{ltvlNHe&g~T?+r%GqlnwXchSXEc>f%>^KHAKht{PE1dqp%U`FdA=KFaY6I6lV z(G?QRXkEk{`60oKPw*YUjR~qibL58vGf*kPw+1&Rr~=KA9}>(!r3BwI+?b#WG)I0& zFawnm9LFKSj8E`g#*GQ8Ky&1W1T#=6(fIhlaR7qX62yE%V(6hNzIl;!c#YCpRN=aW znAerigYK$A%(pOx1T#LtD^X*DD$u;HG$yD5!8bjI1T$I}F|R9)393NwD$$sr3IyLK z84}EBUBtXfG$yD5!S_#w1T$I}F|QJh393Nwjg=w6jMhcWt3+dhDiG=8Lp`Vh!K16O z2UQ^W2J*%PRUr80&5&S5>muem%Nr9^8IR~2JvF0s5%XQ0p$9WQ!FP4K1g_h6Sa;)R z-$rWUbgQgNX%i>kWvZVb_gk9KmO!O6W49k5INFs!%@-&n1xI^+W3-=607*mD1|x63Fn{ zr*#o?yGy7_Y4vjnRk1GTqVR~n7nzwD9iQ)V#;P8Ox!rA*s^CEt{lZou!x^jAMa=D? zht{P^{agYW&L*`kVs3W{RVl5lbqQoRo7B39x!omHrL_9F1TvgWYF)(K?h>j}TK!xC z8D5vPE@EzX2~{brej%Y(GS*FdXkDn3W*mCxm5eH-T>>7eQkt<#pcb!QpsB)k#xCKj zlxFM_sHIm>=M38 zX~r%A57q*$|Kl=t316i&W0!yjYk}7PycxTMuTq+^OTdG74U|UA?Jl7zr4h$lg$(b0 zXkEnI9(vGSY4vjnWOx@x>mufMmr#|`>gN*3@ZOTvMa=Ckp(>@-&n1xIJu5aSK)c&|FQ$7lM!ivLaEZSC=SyAo8v19A8scVzIk_vyp_ zw3e;H8|4{ed)NGYWa86^;7^n`?};71j` z;{;TC7NsBQupi8@B}%t_W}qKvo-texzb;kmQ{3Z}tzCAErERSp^XISnt+_@a4nINE zQkBv-T+k`9cR?_NSP8`8-t~#6AML#3Ohj+D=O9$2ewX}5m;5L7k=~iwC(c{famR6v zIDTeOmHNRWe$+DK`?cLFe41MSMT8|-4PEZr~+-{hzB#GG9KkBL_BRg;-LgppluxSU`ABNqg+jir=~|dl%NW< zjUyh+h{|}Bt3u1qqsjZ#mZ&nsR7o6PlZsmAN%BgRt6LNOuqs1P1rHlX`oWB-Oh3wX zy@}65393NbIO4&KsEkLs+Bh+;l%NWpa1{> diff --git a/zed_wrapper/urdf/models/zed2.stl b/zed_wrapper/urdf/models/zed2.stl deleted file mode 100644 index 913d2cd9657a77cda8be4f1be69c8728a6cce9a5..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 48684 zcmbuId%RUuz4o`4R|qd5nuO*nh^Yty8b@W#JynRXP*KkT6)z~l2DYG7Mkrn}QK45X zO}wNaHvth5!XvD;CLJQIlcMImG&QmCyp?pkrv+y|qw(;0#%*mspR@n^edh1^{+==B z8gtIE=NfCyf1A+qozdfmf3xeq4LhalIbR#p^?&*Iqn+g2Mw>HoaA zM{V)os_}LS%_x!f=>F06>LDL(Hy-L&e`k-{m(Sic3;UrNCDhw()sk(it2_VnOBs5u zem(msDmCDhyX7(XhjzVTddg{?g-RJcA%eVD2 z9_lx;GH`9T_fCv>s7n1B5*7AC{T|-ZvC{4BJmR4$t(zvw-d6Z|lr7xhx5xa2m5bl$ z9q~|=)=d-ZrhHiW&4>G%TI!c34k$+%Wribk;26q4Z}Xwv;21$Gp;MkDH!4AzS`}MDqFXTBlxoscE44nABJ{4RAu*%QK?^=u%ChPP}(IlqlEqZBdt=E9ZNOz zBTZ;V2|JE!*l%vDRAr9|SMQcxXlmdq$0TD50uyAL2?B@lch$0`T)l5L!zKd%cQ`D^=Mmnej*yno+`DcOxFE zvR7nte547@C}HOh5f4?_nT7F46Pi&%=RxjB)OnPuly->@*S0jTn|n{OY~|?Y{f@q= zUdH*&hfZc(Aa)5=DQ)8%m0aSMpG}5G{>F*_+N^}#USPXRL{v&gJkGnSrFq0Vi;Dle zc#vs(kCzq{KN<4RKApUEanZ8(P|xG-D>F5l*d;V_x|JlT2r}4UKp{W zIO~X|HMdnzt-WDI@lH!q9q}7yK2&sF)1yANtTgexhl=IhcdokxYSpLAFWzq+?zBo< z8xnL^zqAMT)z{`ME&lq<;ZCb|FIzZ$@{;1X9}V$ZHTjyB=J6Yr76)B^m}wRF5BzqW zwyaq4xud;(rS95u^>+KAwGbcw*2BfyT{|?Zw@Y*@`_XIBqT)AS7+lx3gYJCM!s6uJ zMtS{ELR+G=>(R6H=-V={*s{wAv#(h9z|!O7-_I@narGq6L))vL8d}BumiEvVru*?> z`FR}JX-;wQrx(_>CHt2i&keq>xOUBDUaNGEB0hNf%HqfSt;hzKBk{FEmKPJR-niPf=-$;nrTd}1MO^*Ms$%`qA61?x_u)xDT2b`; z_+GE)(ke6BZQIS6=&c2Fit6?Yn%!1`U`Be|jQw`nh`GhI+28aCZLhX4-70N6;#WUd zRjhsL&FY-1Tbhr2_K{-csYS(Ym7YtqC7^k>a|v}=>kkKX{zIAc_Nynky*u{9gaKwsy z^MIvUUVa9Lu6iWj`|aJb&ZS@f$5-Zk_n7C6E41Y5Gat&2s`qGqr1U%Ny@&EW4%@l; z3+48yH>}7%9pB`Q5_l9-=I7g*hdZs(*1A2{y4u3D2hM%ZPhOJmd)ts^?4M2dEzL*0 zaJU)yY_01tvTW6q4NLQ-Qx0?X5Bj&~v}O6fe(q>*Uun5fEf1H!}-{QI@Eo? zxW_4F+wWSqDBu3Y!OhwyRO&H$VZLCeQQqEFLR+P@+bUcQZW=Q$-+i|cX8*7*M&;1W zbMx6(O!7Rm?fR*qRq=79th}${_b_#%&dlcJ@jDDwYO*$ykavY!kqZN6}yZ4%&bC0+l;%-K}ZF?Qt|F~dIUS18n(XJWk zt+`T{SA)E~8hC`ZMEe9f-YOi=zx-fTzWKl3tl*3~_}NGDYugo7?VsBZJ(t+CsA&lZ zp6y&h>uL+r9(wN6bE3A^^-z`ir3u};+7IoY>!Ig9Jtt~wT>}4SJpE|r{K0E3tROyb zUB?~qw$_gM^H=@08WN{$?UMiU`B$qTF8Plx`A_O24SmA}oy_)-P?b*)dTN1pJxATE zPdsFY$C8-`=F^A$X)U%RhIipQ+x3tOcGRiGFC z{=s74ZXZWHn1M=()i*CGPT$tqJ8HESVyY1T>dwW*eou7u&bylNiPt+WDq7ckCE~$a zAgDsTcHM&F($*~E!HiE_{)c(Rzw9|E;-LgppttRPfAM&0IWutnG=o@)Ef>xyzWoD# zw&W93Vf&)d_Z7DcJU!yU3{*;BMqb_ni1b4VszBQ~;=zol>WJ&Vm{GL*WpiX)G2;_w zynItp`}f%qf*GGU`-{_yLw~q2LNMbKpI<$#n77m05rP??*!|~M7QJ8lG(s@r6X*Q& zvSQel?Gb_*pD6bs?zTie58b=E->&+_%;GQKzb)_yn##ae^72m|s2MGZ0jP<`pjNhpIT1KN>Bv?SGagTn9;h3FFN7S&p=Q`kNCA#udhCVxkT7=5S(TBM0^IID(!jGf#dV| znL!Dv(2vUK9*Lea1CtGZ(_bP4Gd{8PoId&fLpMbTW_)5)-Z%gI&GRD!Gd?l;slNHXT`!0b%=pCG zZGH1s|NPsv;V5CoCngOWmB+6iN>By*>gUhRy`NoY?^7s{p8Pru`-v_VF<5vS_d}79w@8$8U0W&`FU$@?v$FBy=_{4>OzB!Ly z4VdwXcgN1m<5vS_eBy;IGxJTGTDyg#M6b2>Dw+u$nfrJu%&D3|*qK2VdN5<_?jY>! zBSJ9a6Ly9ZA(-(AJBx}C%=mTeXM|Drz`AVc!N>Bwq zJ6~yopbCVYue3o>1;Wl(+90R`VdpE6eWm@N3IyjX;oen(DiHY3JANHxM(ZL@|9{R* zjEK(})I(|B4`|#ErR|w7vUi!$-Xg}mfq2iApb7-$L~(){t&6zvj109@MUVKt(pIUT zoj*i+&J63)4|Ae;Ka`*fG-fz)f*GiMBHj=6Q2MtcdL%rQpbCDUbnKZwa_zjx@u37& zAod#BEC1m3d)gqV0&)7NBlFMCyR{91Di9x6dgq@uPiupq3dDJ1`{cJQ9nc0r6^J{& z);I64@9+AD$A|WuDnrcfn-4E8Z;P-h5Z`>JZ~jEfe#bTTpb7-%D~$=NK-ig6Wb87d zbrIW{Q-ol~C+y5ALNMbKc77H4JeWxk)v)J!-qj=6&buP5Qi3Y@*?CtR1XUpHysHg@ zDiC(w)doQo2s`g;gP;n8op(j{uJ)WN5DjN!xEo<+091jnmE!kHb`Vs7DDP|K~#Df{Fi@3Z~oR{~IYvB>41XUo) z`^kBEpE@!UnbEq4%lp@Pc`rQTp#)VR%KPGZc^^LF!Hm{LT;7Mz%e(cFGlLRTfhh0k z=jA>9$nnAN2cW{Y4!o~{IR3qXs+8tkk#O%a0}r2w-;?28d3>+NHCOl!i7TK&5531u zI!z!`9;s$M99k8*105Mx^w7Gx_9twW-eV`-m|#ob!PO~fVf>!H_QNMwDQuPAm1hg_ zJucTwxrAoW5)$ct;QKnRbOJ#?t>k(*JuF+o)pXd-Z#i5-dy91dO2qdSYT+}-J#TH5 zw$}B~y62T6kt+|=tx^xIE1y&3496b&9{di;ZI!BYWfi>Rgl2q#-=4W1sv2DOoU6@H zDNbm{C-|M7>!B)L!-Pt4LNh+W?l{>zUGos!%EY%_uWU{Jq?7TzMLL=(nMy zKO5nw%8}1?q@jm?8%jTn>9z?~={Y#`;JFt2hkj|oSvyp=gzJ_-^BU}Wpe3r(*+85? zEuB+wMrMyetZng*LG{p;Jg7uJw^hDM=Rt7-wR9H77NS*cuSn58rTYODTY_^f*ZyET zXM^c}_yjAtgsKp89Z}i?8J!z)FW~&hb#ktUWDG-I(C4Sm`vOtDZoRb_rjlGxD|xj6SYUg$HTZ!&m8l7Ht!H4u%J5 z*TYxwT$?83m@xAz&b91z%(cDqE^2uyb9T1d@jtakjIH#}PQ7zX_FsK2uFvlIV&&Dt z&(8kw%a;0KE4Ng-TzEq^cGblmp&2EH^?Tkn3d^1!k&4($@-e{of<{e6d2CQWN~_UMD{ZmYfgw;7dA z1O2_L#M9lbt6l#4Gnd>pv$d{!B<-Q9^uFqO{`a$wmyWGmIJmXGsqffqm-El4ymHN$ zy6zFzLo-U~UT}%uP5pLl;Ysr=6UVmJwYQVn-&k9H&Hu?R-NWy>61o?Zc0C3*&&oEw z(7pT2{;l=tBc^A)Ztv9n+>PVw+CP`jj1o=zPR&+z+o$`8gU5NJec=t~){Z`Lr|i%p zTI<^OE0&F`H67HP?O(g30ik_TI^8O5iSB8aP}NH>O~|@DdU*GFlg2oorc2zip;8-q zQqOGrbK~pU_K~ZPuif>9G1-g@{C+5*dqHW}W8V)u)Q-9D`s|N?Dv#QAd)F2ncWUDU3-s+lXZpj9YnOL9n=Fh7y-O`eE`_s6(db_PsmHMTLAx{oi`)TLf zvhmMMsNeIA=IXTnds%kq=y7%Rc0E+3eraORr)O8z{d`u|rFlaAfdkeZ_v!_cv+un) zwyxf;hpN;sP3YA?uK?=pdZvmf0`U(5gW__;JYn5h{ zIPBA#Yvr}8dUqH9EUIlEvtV5I)%E?V?@t|5*RzDvjVATbx?{Gt*7|<)$?Bn}`>oP_ zb?gB%v+_z--F|XwUHx1SB^*`qyu0DPskJR9oKf9)WounqHRBt%*UD>Gb>&HZ&$Ycu zXeGB*+V*b8U6qyBrP{*sNhBg_BGF*m6g|}+Q-kf);F|#H9LRLcD2#3`u$KsRoW-lL-(S#N?V&IRHd^Tm(aFP zex-L-UYBY&^}e`%!l;9@^9S{*{jFb%_jxFxD(#c&p>-drd{`;3SGAt=FRoAS{$AzA z2TrSIK*^*+d-g)5;d^>aP6?osV0 zttqcpwWY;`dY4_-_rLP$%W8)`;CpCYrPGAg?b+x5Rm!B*Gn32TTs6=q5tY?6gLZ1kwv`grGP@e*&v`c8KBD8r< zl}qTXNT2VdelDSPl};0?(%FmpxgI(<(C0g)39YMin$Wp{KHo|G(ydZDO=w-6C)nd7 z@+>5+tAti^Tcx#h{-DoyatW=ggjRA1t)=q^eZEtgP!FYDLTl;#L7(-MCbX{7E}^w_ zE~3wRatW=agjRA1Rq6k@X%Efl|DX0692q4_sE5*StF)H>H>ppDN)uXFX_wGi`k!9< zDNssiU8P+Qt)>6(=nODTXkDdULTjliJ+s7b+3}4Vo?4~P99aAJ@<}n}_OJi2(X$gk z^Em?`KKk;CeAJWvGo4h0n9r{OQF(n?{`y~gdjEN8OY~_AAT|x0n|J8uKW9SgBIdIt zK>YEo#rc6JpW=By#phQ*b?VXu`KkFu&J!fuy{mN*^Qi%84}FpbRBzw4IN!6+Q0Lhh zp$F^gulqrFxo1J%e(FVDKhy)Q;xkJ?4BoUX|INZ)&8~+&4*;r7{Z{0cy}pxa6=|11 zT{}wZAf9NRn{V$mslgK+^w|O+df&A&A9UN?OubzXCG_bBO1p%r&_X`f0>q?Wugd?s z{+PE)PgRKdYzYu=T(>fRYU4t0wINh|RuoicEGeHl^nAw*+xa9Vw^hjCn>ak}3FDa0 zqXLcXEU9-we8I3L2uUss#B*fE57?|AEP3TO7Z8BX-hD!`0OrJ zdVk>D;#Xb$=XjwOpV#I4#XXcj%x5ftzJAi;;>v!fc_R@jK2Z#+0ZSJY*XI|Ram5z8 zJ=ePS_%KhG0{!Ox3yP7W#&{k$KKN8Os0QA>xOl(U(7Jku9(+9?X7^4d<3Q9>06K4}U>ySr8vXWuh7 zGIsftELABT_JgAu#8tmuRou4ZG4oUvRtgE88OGgtQ+>wD(LJ}E`yF$Z!1nlPxBIHD z1X{&PQ4e!Op;e$!S812Px8D5S)#gP9nBQI9-}{w$*v?;9xdc?7+jM5}FV$T$pJ1hs zs5YHbT-xtCLy(3CeLJJM>V3D)kNt{+>@gK(JC{;?Li`r0Dn1N2XP5VMy>-hVUQ(4|+EyZvENh z^3!Z;Ryrj33s9&?z=Ph6iLdN+&5nMM4hjBd6doktLGQ-IthcV;@d=U+3I56y9wgvF z@5V&`V{Y6rDoLja)a7qf;YZpfG~*LT|LkUSq`8Eu5c5~9s1zqO;}g4|H`5&dp$F@N z;O}5jDNbm{C(2*J6y1(mcyQ>!xgYM?kg(?^<3R!*^lnVpv#3Ldggw6+6$yCIyD?$U=MEhb_BvryB;Y~s#)Q2_ zIdn+yDL2@oB;Y}Bmw?J%Urk*lSji<&%U*>IK^oiR1U&4_A|O~P^sqA)bN(Q$-Yx+T z%!)8WQa=!^P~nT|zVJhwW*P@|=-#Mn*pn{M}{f!3;c{c@wcez z0fLo64<*VoPRMjfwJnwWA-T zLxR8Ch6f3F(7Q2Fp09R%f}}%&zo~`?33$-EF;SjVUK5{A^{IpYE0PkhnbO)4hef+G9D!0LGQ+d zJr6o`NZ9kMQIUWLy&DtueD2U8VXqTLMFJl5ZcNx~ltYJvy>c2A33$-kB{1Ub^)(<^ z$t6(BUWE-o8r$OpJnYOOAXq8%u(Jts{vfU1E&&hDchnCAE4c(RoJT30CiH%ruI9Jf zBX28+sPMg!vjX33m9DN=KmC1+OQ^g0rB^hnht^H6h}Vn~>9yyYQ6jyPTr*0f*M)0F ziS#OO%_xyxv#l8=(krkvqlB*hc0WPQD3Sg>j%Jid|5ipbO6V_1+*WBu3H{B6OK3(3 zT`%quI(BsxtED5q+ESI$>CvvWl-4!LZmX0~m9F?s6ROg6=xIV#y5=%Xs7lvdrU_N) zO3*Z+DqWwNCRC*>Xw!tMbX{+nP?fGWP7|uqb=GM@Rl34FO{hxO)~5+orH>CCiK?>O zBflocw>|vERlmW~@21>V`6{IkI%Q??`2Sy#H72M+rT8~-nnA1tzq<`RSPKMIi23be zNHF6Q{N}eYK^16z=NS^rK&3?d`&zAqm@35l?l$yb#wX(6*J>@qR3VOkQ>_`F;5V^h zt5^#JRfzfRVn{HPAiVFLwU)%@`{DS?8c~_=PpPuoBj2TJ2C)+HbuvCd6}EF#N7!>_ zpi&~fA_XdDe1fZNLJuXV0v%tyqZ!0XaFtEy!CD}wLL6Tw<9kqr?eTR>n$fz5xymMN z6>F&qaeOtEW_%*Ph6O5Se1hw8!d59k73lWGs{Dij?|M(n(hOoH;%jlBV$U)6(N$sm zoWfQy1A^G`_B$)A6Q*lg(1ot~UbafkB>k?4uIwI0&758CCaHRuOh_RjlalBRV z(A7<>3nKk_aKHO>oY2)&s7o3#KTWqEnnBDFo?hdnD#SdZ(8Bo2GgTqxQLC%o!d6iQ zf@cPJaJ^qhPz8c#IE)y+ODZI&0>SgHu3HQVUO#j#7HD3HLV_88#TKuqA;AoKt1GK` zoev3S{B=^CeS`!v@ZcII^n){;kYEM`*D!&IuUpa#2(CQBcCJVYJ#?l=6=<$!3JIOJ zkp>Z8bB7E*L9CQTOYEmHLPzFTR4Hv^+?nzS`*z^UJHz*{^G(5s z@!sEi7Pjk-*zJd^l(z3OHkHzZs`&Qf&_gpy*mp8VJd{us-{Ks4XhsS9Zt1Ksp(?(O zIwUlsgndVL#6t;H@vYjShh~(p@A{5-D4{C89X#~Vj1py~O8MUKh=&rYvT-f+(2Nr0 z9;uY?4Uc#zp(-0kJT#+(eTRHxTq&U{zI8tAhh~)MRQ6$c*_XzIs%%^h3C$?6rTi?8 zE)ejmJu{S_V4yG<^NH7DH(u~7a zkw7hu5~vu59?bYHVWrT61ZrXQ-8$g@ zZw<{j^kBws2`h#DAc0!;Q!^fnLl0(9OKHZThaGX|{Hip!heS-JEeu;_k0|4zb-6wC zh^bJQ+d~g~1~494m)k>+mX8xfx*9#KZ6b-6wCh^bJQ+d~hmVc;4l zi8I7pW#xY6zDnuzN++u_b*Vxn8++dsX+~nR4kW(PNmRyzDr~oL#Df`884s>{3VW^u zRiJGg@nA+&#v{INNmQmERAIY~BOc6%%6M?qQ`iqBr~++c?>~CYNX!-SSl3w#Jy;6_ z*G5Sk=?B-IipunZtDZs+C8z>zMnLm6|pNI!D zP$}`kF-PWS&t4z#P=YGZKltmB`HTIwN8Yu?3{*;-)uDI(;X_L!9!gLJdgSch`3KXk zi+C^tl@j~S@16hcy_pdYC8z>D>yzI3xdU4w@1ti1DkZM^NALXWm2X5ml%NXqdk6H% zw~l{ltvlNHe&g~T?+r%GqlnwXchSXEc>f%>^KHAKht{PE1dqp%U`FdA=KFaY6I6lV z(G?QRXkEk{`60oKPw*YUjR~qibL58vGf*kPw+1&Rr~=KA9}>(!r3BwI+?b#WG)I0& zFawnm9LFKSj8E`g#*GQ8Ky&1W1T#=6(fIhlaR7qX62yE%V(6hNzIl;!c#YCpRN=aW znAerigYK$A%(pOx1T#LtD^X*DD$u;HG$yD5!8bjI1T$I}F|R9)393NwD$$sr3IyLK z84}EBUBtXfG$yD5!S_#w1T$I}F|QJh393Nwjg=w6jMhcWt3+dhDiG=8Lp`Vh!K16O z2UQ^W2J*%PRUr80&5&S5>muem%Nr9^8IR~2JvF0s5%XQ0p$9WQ!FP4K1g_h6Sa;)R z-$rWUbgQgNX%i>kWvZVb_gk9KmO!O6W49k5INFs!%@-&n1xI^+W3-=607*mD1|x63Fn{ zr*#o?yGy7_Y4vjnRk1GTqVR~n7nzwD9iQ)V#;P8Ox!rA*s^CEt{lZou!x^jAMa=D? zht{P^{agYW&L*`kVs3W{RVl5lbqQoRo7B39x!omHrL_9F1TvgWYF)(K?h>j}TK!xC z8D5vPE@EzX2~{brej%Y(GS*FdXkDn3W*mCxm5eH-T>>7eQkt<#pcb!QpsB)k#xCKj zlxFM_sHIm>=M38 zX~r%A57q*$|Kl=t316i&W0!yjYk}7PycxTMuTq+^OTdG74U|UA?Jl7zr4h$lg$(b0 zXkEnI9(vGSY4vjnWOx@x>mufMmr#|`>gN*3@ZOTvMa=Ckp(>@-&n1xIJu5aSK)c&|FQ$7lM!ivLaEZSC=SyAo8v19A8scVzIk_vyp_ zw3e;H8|4{ed)NGYWa86^;7^n`?};71j` z;{;TC7NsBQupi8@B}%t_W}qKvo-texzb;kmQ{3Z}tzCAErERSp^XISnt+_@a4nINE zQkBv-T+k`9cR?_NSP8`8-t~#6AML#3Ohj+D=O9$2ewX}5m;5L7k=~iwC(c{famR6v zIDTeOmHNRWe$+DK`?cLFe41MSMT8|-4PEZr~+-{hzB#GG9KkBL_BRg;-LgppluxSU`ABNqg+jir=~|dl%NW< zjUyh+h{|}Bt3u1qqsjZ#mZ&nsR7o6PlZsmAN%BgRt6LNOuqs1P1rHlX`oWB-Oh3wX zy@}65393NbIO4&KsEkLs+Bh+;l%NWpa1{> diff --git a/zed_wrapper/urdf/models/zed2i.stl b/zed_wrapper/urdf/models/zed2i.stl deleted file mode 100644 index 76a08d0cbb7d54f694d6c40420a3c25eb15cf28b..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 718084 zcmb513AokM{{N35Bn?D^s8EJzppfe9t;nw^N$93aMX59>&EsiOqzI*G64F4^rJ`DU z3rRu)4cDboDJrwd`xf6sHD=X3Y_^ZLBM!}_kZK6{_`zOG55Cbwqi zGTX$p0U{!2kV!|5vJJR@Ra^L)@tLgVT#AuE?&izfYYfTefqLO)9?Z z*VRMoY`(9ZD?09`H~9LHGupX-U3$YcE_2V#dt9T|gVR~~&nlKsJpb3TYPrnPTn~3F zf?4veHNQ8AYs>9%m%i68-FDoyDXZAJjCcRp&Yd`U9*AKl4RxoxUg@_sJ(`bD7AxIY z<32Zh?57~+P59ECP^oizWJ%`g9O|aM+BRMJtzG#DWznO- zg!Zn%gwh~>o44D&dcqy)Hy-1uq!#{_0-8#yXGU5MUTXy2vSszbGv2_^_8Q8@4)vX<3Oom6Z&WK|C=3Q}^q8b=`>MU{6S~br~0)-rHSp?r0E?&L8XWFb(Wq}aNQrz{%iD$V8{{9*KX*QoF7iGgQ@`(X)X(PQJygW^3uugQAX@s&Rl z9moFLdq}Z$8F%TJ<9dv_7amhbPIA@So|H@$4&%xa%A!Yw?{nhV{p5lV-2bh+Fj->Q zwcbODt;_h<-9y~ZbLzq4g{@QE>Azf>EIK1RAC^!SJr9?w2J!!4b4Q*!9q@SIyhS@gK2(6Bg{yff=9cmD8uk`2dv?Pnh; zwk~5gewe%D+|Mz?9T@kLYuCPQ@^H^EUs*y~^!RA`usA1PcBgX%$M;OW{oLc;LyE0? z7#{lz40q4x^n*v%FE6`=Eqf*ZH7(4$B5W2tDqTF>g}MFCMN3`7^@EawcdzuTffP$H zE_2y%ce`J4E?cq4UH0RkV`19bAGf&uBHjA(Gip)EH4v5O}>b~>b)CPreLdf4qYSd+s`l zuvzrro!z!BYq`s8%^fA4kYWkO9Ai11jt7y#NK9cQ_FG#zX=~XmzRnTar^&@2ZhUgJ z>x0pLYR;v& z6kC@upZUV8Zv=7Rp-Jw_Ff-&MltmBD7#Ag%u|YD^5N_S@huA6+BkW`_wI4UDu6D4)%l;TbD7{#44xv zgGaabN4w*&a-LjlKt4iQ^zdu2zgG8Sbsmq^`L1StJR!x_Wz4m`U9B$g_zXLM!q@?v zQZ?KUODKyT+)IRe-XM3E;XS0-x{SF;DSG$6;4u_CnFiR&TwAS0K0;aa;GQUq-O<=B z9mH;FUcDxskYejH=3eWH?Aq}796PcC*pa>Tet14Cp)7iEZx^1yYWMAOS7O&U;)$x> zLyE1-n0v+>zbpZd8#WDdTd=d-d@wxcmQWTwxO)wA$*J>qyMOG{`x{SH^t>5^+ zn6J1)ehxe2OWq3el_iu#5AL^8VD3&oAHkhIv*@8t z#%1c{b`zeEV(T(izvD73Ps~YQG3YK>j#LFPkkl3X|eY`Atf)d`jNiw@Y7RYcbOI-Hs3ux`^H- z`npqIZ}dhus^`8RlRaVHU7nDVmuT=qA9v@e(?Ia3mTb8{yZq2mo{*B4s5!Ea8{6fB zNLOxLCK3!adYmuT6hr>p+V_DHmRXQGj=G+Zl9!m2?BO11ToeRH$>E+axv32bctT2E;`>v3xQ4y!EDhIc zT+xND`-)M1BudFkl>h!g_xRjDsrk@8;d*2`PCA z&SSH$D-k`mU`}j!YXwh8$xCpqJoSlh;z%rpdH2_%@B5J`B`?AGc;GX0;z+EHxqU^4 zQ#>IhFQGZ#W!4@Xo~ly2cXD>=71_2QDBWdLoknYixn@`y>>l1_Zu)t6suzgwiU)%I z61xUVF#i0dVeW@(3omn-<{&P>il+I^5O4lc@Ik{iyqqb zyUZCNMuKP?2`RQNbr~0))WiK;f;;^VAWpunW9q^--Moh-ltmBi`dy|mh^_THrYc85 ziml7|xCPx@fwgVmaTSO^hJKjZ@K-EDdJjt|iyqqbhkO16h^HeV#nxqf{zIKy@lsd7<8mBT^2F-tPKzpe z4@)SE9@_Q0%y&4dDj-HhLW-@+xOmTwuHF4b;js(E=_PJScUxA#dssqQ^w6$9oJTnj z^&=t0)@3|tQ+xOCt3JY9!qHyy`F81_KU?bOD@!QLpU08j;4=_)LEIV%^#isp<2kS2 z=Q){g<61DRXR4k$R<5s8Fa+y9LzQ<9mOw>-$4D-`4(qB8AP|b zfskSe#=Hu{*SrLeuRv75)!p^kQtx32WzmE8DSTqm0L1Me+C)N%t;?8q^OBW+!Q&PX zCt;MdUsfP(YuPM+9!EYg*@H8$0^<5ea3u1x6iYDXs9aa|;@aUEtcTIwV^JmVVF_i? zgU?O)#DwQj7R1O%NU?Pp^XcmU`Hk?n4Mbl&wJ#2A=shfzL!-M;hS1^lK zZPU$rSVCF!;CviDG3f~6VGva!A;s2Z%vrngxBMJuFooR6#|1*Yj3pS) z!enhxUENJz1D8E4~t$FgmA!s8?m1x8+#eC3g% zNn6Wi@pbhgE;AEHRTIRxNJy~+jkk#uwM?=jye%9Udh?tgdom zV#Vfm-op~gqKA4B{FMc+?#We8OuQKhDYh=-4@UNOWu9*dkF(&h=G|(p_xJ6+hb5Fn z5A`B}SZfF=wl3pBt9rN^54V8FIq>NI>z%1ev)gzNODKyT>P1}UFPukh5H%tp#nxrq zzC?HTN7*Lu_yfcX``=6*SF@S-u!OScp%gNH zh@A^hO8>iJw)e1vvgn~+#AO!YsFs3wF%nX2UB(yR*4f>9$p!G>6VPn~W&96)Ua>j3dF0q8N=%HQ&D-Mq8 zP7sTW2SSRi%lPSUJGdohufSaLD2M}3G*7?y%q!l*63U{7dJ>7Ky!)kYejH{$p=@ zcfmEc!Q*BSGmdMU9>tnA`h-c(r;5 z-{aU?HcPLEj)fk12`ZLQJovMMwOpnLh*mhN)@!P!Y%QC`*Li-SW2ucZe;34ocLO2C z5{!8j?wd6k9yfyc4fkW=Gp~3LODKyTxgARmh@Bv|M?#9N%b0ib(V5@C<0%kNVw4mw z{+su(gtF+NUL=g&+8{>|B4iC=l zDa<|xuWsf&ETJrVs26dWvLKckLW-@+n6urzyW7L#JP;RQ7OgzHjrXvGvgn~+1idrP z{5cSpM?#9N%b2tF^u`au<0}xIS66dGzHje6ETJrVs29QC4uaT#^oVqUpys?*@Yn}pGIkl?lx&!fP!>JZi-eub3J_Z&A;s2Z%w5fz zNe$sq1n0s1Rg-~Ly@w@~MGy5N*y-b_=74x45>jkk#@wAPn0FaG9s_X>c75)VqTa(2 z%A$vQ5tlg?M^z0(uSiI-bs2Ln+AC8K9w&mBjos_k4?pp9i6xXp5A`Bpl*|HA;G;lD zv2_`9xBN`kRhX~-hx7OzyYO+Fhj|Z6D2pEIMO@}S5E&3R>wQLq&&+S;sg4hM(;&FkHVhP6TURAEBSdOq}aNQ z-)=g@oz-p?Je~$|_gNK__ia2bX=~XmzOG)xWwJs17sO?ekYWkOSDcsQ{&(MT%Uz}^ zh;ttpd)?EPALHahX>^j2TrhS?2XXNU?Ppf7W1-yYaWP;qg3(#V2e`Y}j$7 z_ppSr=%HT3WyXN`3&i?JNU?PpPq}n}yJUD3c#H(`Ve3Z{U-!PjdssqQ^iVH?=L1Bg zHjgB}i-Z(gm+^6*_jM~9)`ka1N!v2tWxukciTALCvgn~+1n-xCSakMx*)t;{#nxrq z=0W^b*j9e#)gQ#{o#kBC>Zabq63U{7dJ%l`1LBTd<=o+S10luMWnAX?p04m833v<# z@x^I3q*|3~jkk#_sv)ii<8!)yaJoudNd*P?lnlmo-9+prRJ=BZ1%o-3^y!cb< zp-4!vbs1Oa(#4(rWf6F60x|i|lIaV+Dds&ap)7i+7jc=DAf5n`jD!?hm+_~rliQs9 z63_WM5Sw19mVW1~U4AaHgtF+NUIc&F2VymdT_pk`#nxr~bEo@Vy-_c~qYsGXhw7z2 zC^N--SVCF!P%jdm^H)Kf83`%2F5`K_JGzxwSHWWuJgU^ZIbGzpvfjfI%A$wqSU}VV zar^IqkYejHZdbUYYcO*+{$gtoh+QY#>U$hp%Vz2I(6I~zu`>@r#S)5V-1gTs;TgOQ zM^)yxvMF23X7P3PBKVs-c$5KA|Mx&hu>@mYh07itQw#fc5KrRj&Mh;=dssqQ^iVGn zj%o{t%OfGh)@97QS@?`i@Zjuo;=F3P5n69}D8@NJy~+V?JF2=3WGk6gP1}UNe~l2G>U{2 zTbD6syKl$b3=httR~Eh@bz7-M-op~gqKA5sFt>LAaYrPi*t(24YxjMtIXpPGzlinY zw|AR*4@)SE9_mH#$q$HAuzvg=2`RQNW3E0oOyg%0BS74ab#VHQCf>sm%A$vQkuVY~ zfp{?zQfytuTtzP~dK)}AUwwl0{Kwumcn?b`iyrDlTxJ4@5?IgIMM8?L%b2@?aSi?t z9^8?=ja|m(9ank}ODKyT>P1}UIS?O$_%ISuY+c6O)hvFqIy~lr_z}CMwM)x-4@)SE z9_mH#cS|6agJ}ACAf(v3jJZ3z;j**gQ4hof?D__8JkEPqLRs`sFM@YJK+FSid?cjU zx{SFO{p!14Fqd#WKLh*SVSV59bBQICMGy5N_-hdm#XW>t7zpwo308C)By-<{|E8^f;nJ$MO`2jUbl% z6bSV)mSEiY${}vr_cPGrOa?Lg%qq#Ty&5HKEt|#H)r+{y3K0E3+}t}5QY^vv)6F^V zlecePh8+Ni8THOf-gv`H-op~gqDOAW(ho#|>&{EQRX-3?Y+c4<_6&AE&YS^{Q6Pp- zEt0I+c!&3}gtF+NUIhQ*5X9jrMUwL)A;s2Z-0Jy3u43g)@K_CE-L!8K)6OlJw6$y& zUso@JSro*!>E9%VMnZ}u7#Aur(A`;>|K{fj5H-6lPV_E!uJ^Envgn~+#AOzMSl(rE zqHiRm*t(2&H0kfs-HO3uK8Sn%-6hep$EDuG63U{7dJ()|0^(&5eIg;n)@9tdbzfKd zk}~jk8N?Njf0Uh^P~Cf2LRs`sFM@x=3*t-=iAYGXbs1mPqPMH{aXEO9C{_Mc*ZP_( zy@w@~MGy5Nm_

DtD^8Hxg28UB+!1_jHG^D+`YqAWkoJY3jpUs(BAfD2pEIMZ*1P z17bxaq}aNQU6Tjhsz>;*IcCA*ixp3&(x+7P9+prRJ=BZ1%oGr7L3|VmDYh=-#SeCK zbs4{KR~PqeYktN;;-TkCrN6EGj`y&Hvgn~+ z#ATL&7!IOfl|V?bbs3kb*V&b>{SZ7Hh{6wCoSy%`k>0}+%A$vQ5tr!+qCAN0p9MmS zt;_g|oKCJ`x)?lqfY@~573o+1RnU7_LRs|4?N~TpO$PDV?SYVD>oWeiOegp4Swqm{ ztO2p3%ay*zv9)ZLUJo71u?Q-bQ2c(iJ?L>bCl1F^4f|KYl&xj6__}%#{N(_MKS5Nw zBM?$7!I)R!`%ik*a+%&BZpPK!^}mtc!xGA(hk6m0c@xCet{120d=>~Pwk~7d%>%2J zz+(xB@falsD!=1BETJrVhb5Fn z5A`BpTpicsgmj-sNU?Pp^XY0ozesJoe+7>d2W(7jT2nl2YuPNmu3p4tCV^On=ls)1 zNU;QC&Jv>+od*xjK5t|8S$|4J?_mjL(L=pR*k!B(u`v=-Y+c5j?W#55cPDV(?TA@) z%`Mfuhb5Fn5A`B}=xPWlwk~7N+U*Kn4G*pc8^UVfJuIOtdZ-r(t6E$?JR!x_Wz5xQ z=A@eNm;;X)VI9mzD2pEIMO@|;5NQx~CRF!?6kC@uSJ5HgvNz!B+y$%iz#f<8Ba}rC z^&&3w9}o*b+#d-kwk~7t1}a{42|N~p_!GN~d&`~cJuIOtdZ-scCk$dSh#8TPV(T*I zu4dK7Qt)^bL|N>XJ~+3a_ppSr=%HQ&|3VtXlOU=`LW-@+n0vTGEl+^Q1`s!4pSY>f z4*v{VLRs`sFM^&EL=_Mh+#Cogwk~7tGFMgm7|;1&5bLqu9dyG>-op~gqKA4Be5(qG z4?sLpKM+!EUB=uky91Np@dk*;unYfJuSVX(63U`SZpVU7B0U+zUA+S##nxra-r&o^ zHQ+%4JxFrgFON7|%VzQQ+>V9o$5as0e+q;YOE6aVg1P|r3&aEv<&SUe3H36TVEk3F!EWK#P0-`K0OGG(3MUU8u9&d3Y!+Wv zFTzg&nx<u2tUnen!X3b;z&rbbs0bXVo!J6((mCh3B((_ zPfa~>UQzF131!hky+|MygLoV+gKLkRGt;_hM ze|2*^zFh>5+3>it+NM;q`fI(1C6q-E^&)&r`9+prRJ=BZvUA3m^1t6Y@gcMts@ydI;x-YN09v%}wJhJ?h^t^+2dJjt| ziyrDlTxLCpz98QEF%VL0UB(x#?cyp`UW<8m4T#Otiltlry3NnjmQWTwayu3t)d|y! zrEC2e2r0HM;~xigalIaRV@Vk89U2t(J&vtqv-EoCSolPB%tKJIgyKS{uR)K)r}iuy zmHTy@v$bp%Uso@JT?U9nIP=&42!s?%Fy>XL{Yryc;ab(h)lD3{Gi7VpEWWN@B+QBH zK{Wj-5K=6`n0Is4D^J1WaU9hn7$w{4PWK*`P!>JZi|`%#rs))jR3xO>x{Nt06Hlh# z!FBLajP@7nuk{|5P!>IMJC+3?=2hL4`aTj;Y+c5Dx?b+L3m$VoG{RF`^vCbKhb5Fn z5A`Bp7JUQ6A3p>_iml6-v&4HNv#?L(s&)XgPuX7!q-`ym#n;t~;O|92ylx07mSD`; zu0mOU=EZUK8fMW)&nxOZETJrVs22$<&O8ufA|b`rWz1Ro>++|=V=_GMz-ln>+~VHD z63U{7dXaEc<3P-hgcMtsF;}0y&Ch_xba>QUFe`h)d#8I3ODKyT>P7gM98J@`K#Yro z6kC@uSJ8<}*w=D(J{{}%qg4ud4@)SE9_mHHYETjD`I<;bv2_`9&#=AKN$}t-dMxUhU9m{YKmxCBrIS}e)EW!ASJ_Fp+ ztES_l0FJWuhEWWN@1fLIrCJ962mWe(R)}zS@cjZ60X%J*F2VRk&t5R zGH!lJKX>7Cmx1mH{AIUfv?{b|j?Ox{McH-PcVk`xrcO zKsRJ?a^F41-7I`hBv`IlS3>SVCF! zP%nbrD~OHP_sK3D2`RQN;}6R8cK;nT4IU#wtoql?6~BGI)q7Y%S@cjZ;xZ3`CW7X(F4R=Ri~zA?3my^ETJrVs29OsS%CNm z#IQ(6v2__=bz65=q2cB5=nZ1-;w7n?TkrQCmQWTw)Qg0{L zoR_k-Y!+WvFM_|ng2#sE)}%&u4}=s;Fy`H?yr2a{cn?b`iyrDl@OPFVwu6`u2`RQNV?JGL zySxAoJ|BfL15(}hE%Y9iP!>IMI~LAxIe5;eMM8?L%b2r7(>Y7wF#<%92mO5IJuIOt zdZ-r(Gxb-PuO54Rl_#Xwx{Nv7&AIShcyOlv0ki1RVjt%tltmBqBIqDN+y-JnB&687 z-Vby8`c@xBkB_h#oba6O-n)wyco!+eCY=%HT3Ww>YP0HQ%8 zq}aNQxf|#{Ck+o0KVn}}x#SA(VF_i?gZq`xv5W(8JoY66N(4fRt;?9ZnvZJEhX?1~ z*4SVD>w*`(hb5Fn5A`DWYg`b;K>QjBDYhjkk#`-qY;IaFU{_f#sL%oM3 zltmBqB4L!2%NpQXM?#9N%UIu^8jk9n@}IlLXFupYETJra9!EMBUfsjxK6lSWLS2fj z%Q*MDSb64uwmCcX)rbEI=Mih!EPALH3C~BLd(KWZj)W9TFxI!a;(Z@@Y@U8?YWCc# zy@w@~MGy5N!DBm!$&rv^>oV5&z=qL2c5kcHo?S(~hb5Fn5A`D9>Slx3zdI08Y+c6s z=GbrtXO!rZdSlBfKVMlwS@cjZ62|U$5KnFmgcMtsvA%0Ic%1vnu++Ex8+s2*D2pDs z9SiSyc@RSe211Ih%UIt|8^-R3#UJuLj;&?0^m^!6=#iJ8VhP3Jn{K&&T!^Fky?;Yz zYuPNmp4+i-7A*>*@4!Gvu>@m%18%s3ui)zbv}Kj|u!OScpGC2af?*KdOH-BAiF8WwYp^UL@=?-ko*hifbbw#S)D5jl{vD_3)C}1>V3n zVVYVti?6E}2_7k|gJmKi#S)D59me4~zZC2F9U}&K4@)SE9_mHH4qzVE^N)uILW-@+ zSl@aah+f#2JU;k7?_mjL(L=pR*u7RNULf&VP9UV%x{UQb$$_}^rYed36&rdFODKyT z>P5m6H5>b@#TNuZiml67->e*-k0#hBmK<5ZdssqQ^iVGnc4YIgPnZyvDB+@)63ShkB7P5=mTs zC=gO?UB>#p=kU~a?>8b*tJC`{Y%QC`*WWnOu{?=q@Ocn-3=D)6OE6aV628TJJ-%W5 z_rg!2tIyrBa1Exw1oS?L%2cVKJA$xCRKz;Ezm4e@`=!1wy}%Ln|e9maLW z+~wZcFwV!hzu}gbkdlw6cUSyHoqr;R#4OR<#wL~>IjSK#DtjdS1qL1A;`VpDJty(+ zEc1Fu$$L)xjfn6YZjm_RVF_iCUbU{3JK=MF?_gdJDa^7&?r*r|^{|ApNdK|CjjJ++ z-;t8nLkhDjp=yvODK!ypFWvxg=f!$S(Q zETP|a%YPo0P!{R!?MAo(4Tc}XLkhDjq2G4P?_mjLk^Zs&LvCa;3wY;Bigct~NECG;D0`8_P5EYf^7_e>ab3=b*HvIL*`@C#sh&)gEqBF#DG#!b_X z;UR@tme6n1&M>y|OD}`B>(68G4GodWf>=`DN zJpULTQkZ24_9EdI^70;)C6q;)y-#M?C&yR~q%g}8`W?Ld=V1wDk!BBfRCjk|#gW1+ zOQ^G?N7j)4By*C(-ksuqVO9NL))fPe?oyBRob6kuFSmsHOiM84>&@}^kodLD*6Bqg zhP*q)|5B_aly&rZv~8XK4~d*e=sav)`EB=o?S>AIy?WO5{gSC$JEm+co5fhyFr0ab zK=|Ju)R~iJ7Q0{3?A1MyCBiCs3DJSr)~!o&dfRkD?yPGG#_C@GnNSuhDGtQDr~Ny5 z*2d$LRoQiq`_F{3=%F|e^)D)vd|}5O-b0G5%UJ#HKNHHLhvGmq>AoSc?U|0=LyE1-SpDul z6Uw58;y}Fd@~A|U|MvDCQfytu>UaN{P!>HD2cqh>ec4l+4E7#UY+c6ccmJ7C7CjUP zV*VHBxP9w#yoVH9m$CZYe>oQh{{Lh54=%F|e!!9qAZZqj7 z?;*w3WvmYQp9y8rLvbL^>sd4X_K3^9hZI|vu{z{`CX_`F#ev9b+cf?0{}uNhQfytu z>X84LP!>HD2cl4?cIly2clo(piml67J@r2m%A$wjKx{j`XL?%yXT66MTbHqV>VGDb zMGwV+`0}rT>4%%1<2|ILJZ+~Jk<1@lvOOjnCGYdJS*$(2xYO7Vt?jCKF9r--v3$eA;s2Z%=@JNJS*$( z2xZYjaquXDan+;hF7F}5)@97`rj9-<>+c9<(L-_Y_zVB~Wz+xR-&-2R)@97+Mjd@t z*547zVkO1FBR=QeLyE1-n9sjD`mC(KBa}rC#ld4B=Bsv-Zt@;dY+c5j$JEhhW&ItY zEP5!8*J|R=sk8UA_8wAfUB;Y`)zRZx{SBckdMFMaasBWfQfytu#Z5<_mGyUovgn~W zc(lSgcy-eO-b0G5%b4q<-UG(d z2xZYjaq#GZed5+-32@D@E%fZUB=u8>pg(1 ztiL0aMGwWnBktS1hZI|vG57O&*B~qF?+9hlLvir90{zE<9p`%wDYh(d z2xZYjaqx(Jl=qNg>oQiS|IdW7=%F|e><4GGP5ZfBiml7|s4g|@?+9hlLviqkU8?tx zV(T(iPfcP-lB+HL^J^^?cGaw_{+#i=`fX!JZwOrGTqQ4|o;tsWYLQSD<62L)bvcEG zgiir*DdH6eM31yMytaRj)Y7zbpT0&VQ)*RQ>O*<4ni4);Hq}aNQ zIlCVDq*{dcu!OQm3_iKL8*seUyhKltp4=&pvZ6Xz63QY`e(s>Z86{F|UB=wo9r>jCC=W|0i^Ljy$JVi*4=J`T zWA0Ipd{TXshb5FnqE&~XfAgG6v2_`9uYKf`>Z3d?p)3;j+%xQN<|`?-E@Sn(`DZvw zD2v3+WrqLFyeq}lWz62^$S2iDorfiqMS>mKkx#1smQWUn&0~iD&59$%)@5w_+M~y= zC6q;??5o57X62M(>nc`Xd$dPhf>|W!-R!t?!zT?#ow*cSm$ACn{CCh2$|Aw5-nrg? zj^QE2)@7{jHNS@?ltsdJsYjo=C6q;iu2+v2_`%vpnK4#P5+! zEt|!dYw)o>q~s;kS?2e!gtAC;FLCUvD}`B>P-mIn!xG9O%{|evM~M_>Swfv7^_b_LL9k+)4}@x2f^myE zZQbqulWGz(K%92Ra%XGVEP01B=TSWiqG0nt=sYYTzuH|&VdZ=s#QEiWCZ`@4?fY{} z@I07B4_$S9k8uA2i7p@-?+=6&TbFU2jqTiZZ_R_pMi4*MxHox5{hi*!63U{7Mvu!J zkE5CmVt*v0*t(2&)^6{{*FO=n=;46{5|hW=n5=oQlz+d(63U{7o&tQU3W)h2Dn>$z zt;=|0g^up$6_>%|Sr9M&Stx(2hF{QC6q-E&0{W8WpIJSlBorf9U>vc)@59?N>_LNq>k{|fUA3H>!pd3 z^E!JEODKyTnvY%P%K-%vh1x7l{P9X4q}aNQ-#)*)TlDn+cr?a&oc36)M3=$+yoV)} zMGvhjF7qIc>d=^4i3cJf#nxqf)3rU_kg5E8pOZmw(p|NFkoT~Jvgn}|H9sN6)@3}S zQ=jOu80T^7W3^KE$D^`@vgo08-enqrcx7y@RQE_ov2__=_(=bFt@h*UUfp_W>Y{i* zETJrVXour6n|bDKmZrXm_d|-U%XrbCLGhknicxahv;yf`agS@h6O&1KF8@#oY6 z={g!$aX)D5GJdnj8`HJpQ)>xj(L;M}m-!vUToC6+LW-@+_=K9n;#~3vh_`Fpn;sNrA4@2U9_j$_ zi_kdp&p~{3eVE~-*t&DzvFX6DI49l>96;NSyY70qKA4Pmnnr&axI7+ai*4H z3C5qc9PVdr{E{+=Y0C$u+uRja14}529_ry-rXt4GBoO?md8^pEj2jFc9@nUHAjW{W z{jRX8sg}*sd4zXTOX8^JfOxt^Ab3<%EFnLxqAqg*h@Lp}L;FT2Y%QC`*Le-YJEt;?9t`DZWlx3JC3;5{s%EP8N`3Ga*E0*@atmt6Njb5BUI zbs2Le+FWHQJPu)8)xw-ud|u~#gtF+txiY*j`T)-STM&P~5(p`_E@RH9k2W6-kL@4| zVs7s+xS#j1gtF+tIX}EFN`k9F$4E%Abs2MIxH9W;cwEkz8gJ~sy?&7Qu!OSc!L=)R zL?TOs6kC@uSGeHu3_MQ6T6<4CDoZGf9$bUNwfY?Aao5;diEfdQV(T*IN*%6MFA&#b z&rmMj4@)SE9^6ZWd;SNGY7dAXoVq!BiwVYAAevcgEh)uD@!Pg9^4a!vAYOI z^&b%ZA|b`rWz3yZ7`xo{#XX$&u!OSc!M$C020sO{2z$7j&{-oZqfh7vzvNCN?u~hNqt?5 zrk8`DM|SJ`-J~%?JRv17apCkn?u9S;&DnNs$JJeaK2jj<2+AD$wxfi zD-yg`)t2Pc=@vZ>MkO!N{G?vysJ6EFt;_iE*sm zpD7@CR7>u;KfBMPLp&iRFTp$b`*eQGKZyyr=S9DK$`exZ5*!Ol`h6ddD)nkko!-CC z^#sS&s4q@jmY3iN{kBP=W#K&7K`!`lo+qT_Bk+6#f=Bh#;+z!^Mh`w8RPqvh(&!z| zV<(>4m#RPSJ*4C%_%w&};Amfhnc#j46iCps<$MT|a)`8N?D0?>-&~c0%kk zEWxRgzf-#N&W94Vmd&!eewX>Hz{JFxApW-_5b7W-Q4StUhYxeh z%5H*3nUf|a8fWfI7cBOv_Yh&TxN@@p3_E~=II3TMxig&tVHHa-uC#Ti+dcg(>_v-# z_`Ldc=`T7IPv#|*#n;s*;5)EEEC6wQ$3RH2bs5+Geuz8n#5(YJ0L1I>S4jWoof_W5 z63U{7`UICb0cYNRZH08}NJz1D8NV{=}Cv0cn?b`iyrC|T;>lD zf4q~Go*xM*wl3ojkk#vAYG<1YMt6g*mi z;ClY`WLNcs4Sr@dZu1C zNU?Pp_if!hUaN;dlv%bgQ8!*)ODKyT>Jwb%bP&tmSeR%R2`RQNRlnQfytu<9}-(=aND=s)N7W znOqZRA4@2U9_kZZ=BiUBCW>V4Og>RG%vVxuUB&}nyf4m)4M5ylzf%Vv? zWoy|izRqhHK9#uxSNB>FsYpn%1Y=(5FQ0k=9!U_ZaL*SM`_y|_LRs|SSO}lWWZ|f; z!990H10luMWy}%t(`&!NqX>vEF%q|SD4w>pY!+YVm=2%H9Ol&pu`v=-EWwx~|JlR^ z%U$Lacx=ZLHS-<(M!BhFv-mon&G4zrQV?T6d=LpKmSD^$c+#xv;4uZnpYLQPpE%US zdssqQ^xzy5K9%_n=Wz$-lKzp9V(T*IOmyh*zu{3B9(6G%HY(ZHdssqQ^x#|>K9zX^ z#0!`cn?*v3t;?7*>YkQe;gJJk$$QlkuXXR{JuIOtdT`DUpURX5F&cCG@<>Rrbs2MI z*nLGGc-#bnyYK^(dwLH`D2pCkyMjlfJcJZmmoZnk;PD`cR#-U~#G|r=vgpAzI9w~U z*7~E8V(T*IN*%5hSA)y3XQ&ykt|gR35AG$xJ?9?oRqPo$L_&(K%a}WkaL>QTC~1nl z&!9L;ETJrVa8DG*Zc`9vgIEy>DYhoVp}Hq0f*gSZ5H z>cesNv4pa;Zx3@~MVxsF5Zf@rS;f|6%zb;96H9=|!A^hV&WG|5%2Lk|=JpLZssSKg z+YtzT5@YMCPY84S2Ow6UV_84;HD_ztEWWPZ2cN!zcn`!TtRGgf1Y>nbE>o?<-o)HK z_om%*v$L)8ZvV&XRA@WQHANpKI&?+vffxwlju!%Q=nD%|9d-G;atIhEnj6vTIXN~d4kaIyEWgtF+Neh0tS3?c<$aU`VJx{Nn(8SI+Xx(Oan zgScVD_o;=~)bk#eP!>JZ@8G=*5buMS6$vS}F5@9N1Ksjo_rT+75F-lBOcguvcJE;c zWzj?Zj>}vKVj_ssBO%4sWqj5*{oKy`9)w2%#HhU&rP_>b?L91^EPANl!Mg?^nt`}G z5>jkk#>ZXV$9*)cKRj*%!9J>N_qN`{63U{7`kf<0R=G$>v2_`L)w5^xxD_75_g;ig zX2N+`LRs`szk^?i0r3uqTO%RG)@8go)jeLTF(7&snwj|Hgm6DBp)7i+-*K7hAT9(^ zA>I!uwl3q2mpu^g`K2KCkN7?@HI5QXD2pEIcU-0rh>t-mh=dedmvQk=I>)hl=&Zeo z_IpYvlN-bHVF_i?L;a4+%!0>w5TC~9LyE1-`1<$nkI&$I5I;1!D*1GLYAvBGdZ^#Q z?-hag3dHM?kYejHPM7Z(=Mvt*I)fV}PmA-FC6q-E^*iWmL0kjkp-4!vbs3+qr+u6g zPXRIK^_I!=PY<)GC6q;v+^(oZB#w`S6kC_^>I&`S+UHAFb2fYD^S3$h>+-zrS*(`a7mGevx3qb6S1kZztCFIwl*Y9`_ znP)yLT&t9=WwZFY`W=_~6C-gEh#!je_kjkk#vC!%p12Dhy+It`*pJ<`t!1JZ@3_qOxK^)$_+vvLq}aNQ`2>H}^9p!yloWmF`^1!M z>Uj@KD2pEIcks73AXZ{7c`g!CY+c5jiEdfg5FX7z^uY{w>WQ~|4@)SE9_n|(T6;Q( zQz9Y7)@95Ywa*OvF->L^Jo;g#zIkkG?_mjL(L?S?Uqm$J=E{GOa~C}fp{|#Qfytu+yRDX@LwQy zVK2HiKDCxm7CqGOxXf1|eg)Ah5>jkk#@z3Qx#VXMJF!FF8D}3$D2pEIcktV1AU+2% zW>%Qtq}aNQxg!sAB3Dk#qUo|l!z^kEWvOQfb36NwxgeSq>7TNSt;?AGN0{4Jf*6U8 z<$0_Ic?o6l_1v!L5)dzd=<8er@N(Y^$ug_Nh954jJm2 zppOzAx}xDA&e`55J$G9m)Gt|ral^Jl-L~$H(Bq5-QTfaI=}}j=N!VI8i?6HS!MlSX z_U^8qeywI8q*#J+g`z`UwStS_u?9r%$~Dt1J1_MfmQWTwa=W5lAa;UCbqa(OTbFUK z5%_mSbAN#c&-{YRE2MAvE#o~bp)7i+-*K6@K&-A-Aw4S+Qfytu`<}>gH{N}A?QkAn zPdPO`^t4h*Tgztgb@e+g^9wv)1o2cPq*#J+(RT;A^b`Lq-Pn6rLRs`szvD8W!-K1u2r0HM<4>mbj2=Am#n+TfycCbh63U{7`W=^f z55%b;-iw43TbJ?0wI7VviX-u>nd1|C6cyN*gXhh{*+UbqvL3|gtF+N ze#d1FfS3#7nMg>nbs7Ivx^sL6zX9=PwF*f+=ax_wJ=E{G%vPQUh$rK7F2&aM@$>h` zxrD?^m1`zj$Jys7B4*J;{SJOT0>o?(RpJaM#nxr~+}MtBPMiqhkKOf?Gp`A=s3nv| zkKC?^S9d#z=5eN$V(T()zNQwLO?VXkA`m<(DwdF6m)AFUZ!viXblfvVQW3s{a^5*SVCF!$nAVt~=V1wD(L?=?%WMPj7>GsjJfzsVjJZ;WYxNa~ ztFdQzJziZ)D2pEIcknOuK%5F5x{SHw2=|=laVB;$*Tzv|31!hk{f^6UhPxR= zkw{3fbs2N#6vpmPAU0t~)+vs5ODKyT>UUh`9T3YwEQ*8_TbD8SiQyS^AhNI*y&yid zmQWTw)bHT8WkGz0z3ARZNU?Ppb0-_-lKvoOR<4|85MTWRml_tSs9Ej_lDw{s%z*O&H31!hk{SMv(05KNCYx@Hs#nxr~LX8~v zUZ;)l=nP`*U4_%{4%_5CETJrVYE_CJzoKQILmx$tkvQ|IU)4?7S~iQXtKV^%r$BTE z@ogleSb{OHbg5)x^f*s|I3IU#>Vc`=!xGA(hx#4-{Vs^EATE!D6kC@u$JOsUo`Xkw z5bH1!*ALs|JuIOt?{TCnx)sDB5OYOD#nxrak^jp&{-#As5N9s@HnHLB%y1uKE-5^JZ@8EMz5PLw3iG&ne zmoay;xpPT-?2vDXvyUZ|MGy5m_-%4{+zX=5{xHKyv2_`9M;_)xj;njI(|_jcFz;GI zS@cl9<1$-7)B|z%-atsPbs4k&2y^>-5TB!Cd1GZ*4J@H7dgOLRyt)n06^&XG2r0HM zV|7R_v%1i2$$1-!qz|lVmTi^w^PjD=_};;;5&9_6p)2YJ;`)t6(tDN#Lj95@70{)+dogtF+7 z+ZDYB;sp?kA|b`rWnAOpfo}0-8{qLai1l?>rsn+lvG=fqvgo0H$7SY$xU=5M)UZfM zv2_`b$1jCnQR63gECTWA2@_M`NGYf{RbY)u5FU| zD_&hoD2pEIcfv?)TDM7Jb0nnLx{UV}crf1cRq$AT!o)jkk#*2qP5T8NLiCg#Y zPV|gVttFI2kKC^4brA1?_%ISuY+c6H#&wNz$qEpEw9QI3h_jC+ltmBqJ1)aBCvhke zQfytur+AH{3+1MG%DYh6jD0O%+JnS~iQX ztKSJnH41a$&ykQ~3C5gJKg{_S9$ag4Ft^|G$7$Zf63U{7`W=^<4T5uf^GHasbs2MI z*t|Le56%o+afWs+>OCx>EPANl$xleJbs2Mo3m!bG$FSDk8IQ^m%A$w*9hc#XQv+-5 zjggRI>oVp_9j+D6BaJ=73F&Y@ETJrVsNccg`+)d2_6+}x_d|-U%b5F;aL?ZX@eXz} z1>-2OgtF+Nekbf1x`TKu5>jkk#@spOj@=#D!|jiw-4e>8hx#3STPTjIJoa$)A|b`r zWy~F5cn0~@uE)-@YkX=gp)7i+-*FkPI1a?Sk&t5RGUiS;%q1kw#h&`+WnsRugtF+N ze#d3TN8+9pfskVBGUkpv%!!YKD2JVX)2uM>T0&X$P`~3c{Xkp>qH~;grP#WR*?)w& zo&CqT=vbb5Kdc6pP!>IMyP^#s7NaXF8rKggwk~6JNcaxAS{;+St8Yv_zOGERRl1d* zTIaEO1N^tOhz?!Rr687psI)N<>X$6Rxa9Z&uGCi(S84#-?A;s2ZT(4z6xBK8cc#t?$@9xxw0k3)wODKyT zxm^*7Uko9|)@A%`x4y1Uxi{f)A&8S7DwW!|^&RhF31!hk{SN-69*9CCN~OMzgcMts zapD1d)6|3y;K6ZK_L9l&v|;PKhb5Fn5A{3Y%*Rxk?8-($iml7|V5{D4-~oOH$@9Px zHRP7FKlUD$P!>JZ@3_oGfyfde#nxqX;&W~ZWzj?Z4!#W$9{&UJaD2|C*t(2cZR-~2lBytj99oe`#ret- z%A$w*9ha#DqBDrvalVpb>oRU}Z#Oe1VirwY6K7FND2pEIcfu3Z5QOGkDYh=-jU~Iq zx&0Cx)qDSsvGV|rs@mFc=nbLl8vhaEZ5Kh}zdj8TTb zS;3LoRsCEpc>T7r93_S&Cf2wVrEpuzQy?SsvQ$nOYxbY9rF(|w9Am$e$6;7ECb zCu<}JbS@o*<6Q69cGXyjRy8d_k0xNF#q?=fYMmXUMoLRu&~K#C&;+BP!S#-pT}95S z${=a&Pg{Z>ofkG*ezUZuqT$9UpNvsPLlcaG2G={5@JSzB;|O|mUfAftNnZ_i8pRqu z;yXNSrP0s?qo5I96`@iulUnklBk0k2VWSfDoIX`FKwSNNIp2w|w;ByiFbW!6?|9j$ zqK`;TBtehP3mX;nK;b!}f&NiiYI~V6yN!k>7zGWkckDP=Kx%s-N6@45!p6w3^s!9Q zxK%W;j%t40mqtSqjDiN&JK-^^nIq`Yd0}IOvlF>kju5p}nz%=E8wRa6s8Jvc_rb#t(XQUf5hA$tR<02KkpR$Ot^WI*_bA_})o=s~>5*DtbwX*M;b? z))HI^X#zIC2iD65F>uV_z`Z@Er085a3dh5%qF01?SctKXphpw1`OUHN){7AP8}|r2 z(BXv9&;+BP!S#-O;x5FPrab~rw6O#|IxlQ~*Q~s2AVkvLEdm8Utl-nRbQF$ry(4dO z2ywbvi$I}OmY_!yu=(w@_Fg?-qFUgEGd`oC2}VJK>m7ULLxp(B5%lQ1u=#zqR^zLD ziGj2g4;u|lFbW!6@5nxbT*rHP69cOpL66Q0o8N$Iul$o`>%60i zs*g4rnqU+(xZbg6g;DJtN6@458awojz33m6eo9HH<<2UCa8c0UddF(~{Buf5f+OhB zd13RLeC?Hgvus_;d3Sd;!6;~Oy(6Cq%UQj^00$K3d#2}VI9yeh&oml4PJv>QL@(RpF>o0UUjluyQ}4LX;O;&s@O6QjXQ z(U3995L^jqf_nCwZb1x{D=*h`il=kwC>#&3ia@j#Vx%MJ(FAOMd$YY)o8$@h>u|zo zXo6AD;Ce^i9TbiGg_zjU67=Z2u=#z@wq5O%w)?@#3IUxmA$LiwiN+5%lQ1u*2U^jVD+`D%_FN&ln9&FbW!6@7O1JScv_OphxG0&2PH4 z*Ky~Gl$4~4BaDV77zGWkcdSM|84c<=f*ze0HoqI&5*Tsbtv=dlXo6AD;Cd&PphxF5 zb_IUVHtyA(Kc{%txU-5NTog38-mzEST8JW!phxG0&F|l~M6%2PesOnK6O4ie*E^P2 zFT^@W(4+Ih<~MfR{xMDFKJ8p9(FCKQ!S#;4SGNhV$r1GEys-Hl-}bEL$&BoA*FQAD zC}?oKV~J5hyy^&gbY9r}R&YBS{9V08;49b9HNhxoaJ?gcZy-O0|JvuLJu8_jtr$6w;jFjkaN9UXY z1m`H0*e<`iFipOPKAK=<0KrNmzriy;f8f-LVZnJ5^84PJ+&}rgE&aW`&o20=)~V-` zcbw?&ZSV2YM>sxszWma-yhkYS?@q`cI4MLyN6@1Q*vQn$5HASvqY!iD|8*`MMeTz- zeiMzqhc(`8G&F(hKom5}R)5wTHx!?ewVsqe zkY9-BnplD!ofr0>EuQs89>d>~l^3E{w2*L{!6)4ZwOIJ&Z@o;Upj&wO~A%+dsfASXf`Ro?~^zLqo9%c>p(}0 zko;>W=J)M$1U))0?0v`O*Hz4QJSN1gO*Z=~x_hMwMnPlcAA{WUm@8+sT8MM*UeTlT z!fyH9Aon~P2=U&;>--t+32K5-(0E|)VArm`lvdJ5h%JtwN9TpzV9j9Hu9gZhpj#9F z0oM{W!6;~aF?@*YA64Y6778)W5%lQ1u(LZ4as8u_5Xs|v`TM#)s0l_vz4A-FG6X%EfDLV1uO>>Q z)GPlSSK&0lC~BitdfDe~z4A-F@@bFGOFdLn`J_QKq+a=lH{KjW;A&ZSaSz}7WSM+o z(n9X8)GNQ#D?`wu^TNiRUif;Z5T%9aDNk^(v=p3`CK!d|XbTOR6kR9Zzbj|;u@LgHofkG*{*hXD3$afK zX(7Hsa`ifwj>2*D%^_9W3URYsd72PcJAxifz(xMTTYN6@1Q*cd(RpEG9JKA~S|J8Xbvxu*q9zyx4UF2he^iz$UoS)tN6@45!p3-R`^SSq zNWBU^?fRf57zGW^iKIu#SxLPLj&psG9-S99XHH)Bnr$P3|6N$XS0Xq$dHpXLrj2%x zSyA&^qfN_zjpJyEzplPq#}YeATuf=0)*?lFqKQ*G26}J((9;oJzTclRpyEP9&=XCJ zn={aR?_z~o@@oZhR;4R1Oj&Ytzai*}CYC%l(EI52>Qb-P3Q@OhiZan2uPA5Mt6Lfq>JdNcw1)hmO&jhW-udf7vTX#QP&|8=*O@atSU3dcEr z@UmCQm47J2fmk0xN}={?w6|IAOKaYBe{lE0f!I+u>Z@$f7oNzN*d5Ca@Rk0xMu zdwY<#=5RTg0hAV^uv~Q)xq6*TN8xyQmN8w3KE74H#~neBCSarW*s->?5Ci4urOH#( zxpWkcqu@NrtOl>t;ZNRDPySL*i0bkLrJe+J zE**vAXbW~1XP{j9J|Q;UY6*HY0UIsms$CsKW3~{rB!7RQbS@o*<7m@%7w0=6irTho z2zoRD8!f-vpZJaSkA!$uu3G9<48bU9pf}syk27-Rr&Cr1dN_g}ofkIxxjm~zLVW*V zw%1yoqRyqGa2$2T?&AC;XEm`&w%6Mc^k@P$j@uot--P(%zu74R;t-622CA#Q4m`oG zO|w&y9YK%I3mbLU-CZFXN^iQ!UAZP01r6@cc-f2Otge^ZzSt4;=)ABoe%R;njSzQ9 z9qHkoxh5C|4UBfSUCj`pv=A>kf*ze0HpW5Qt|kics8qMMuI*}qQP9AsZTrV|x$^l! z%yMm)9-S99#&g?0-WQ^tROuqFM`?ml&~HtGCFs!v zY&^{hBkq;C&*wrESa^efT=5G=LlcaG25dW9$`)eVTQ~T(I)WaZ7dG~4-Z#5Z7ljB) z^md_iE**vA;n`9xx$@tOUGN=p1U;I7J-6*3cUDz|ST0w+S?-t4rK4~>JX=~PXI1=< zIlj*vL60V2V?Mvp>|Y%eqVJ}CDd*)W>RdVs$5B`8Z0UMAt2exTDW5xn9!=q&1iQJG$Xq%KnK%!Uzq`Yg3-P-n=+OjhWD56>uj?lVUvPa;6O4ie z=Rsa}UAgjILKJmT4cpbKi-YFW=3WEowZE24tsF!N?08rtk%9Rh2mJ&_iIuM29;n~u5 zc^+kwlY?WWHENH}3mbR(*trriFZoi4vI}nrww9I>O)v__VcSvdynE))JAxja7dG0} zSo3M$B_X6<1-42{(YbUKj&mO5W!I4_KUwTT;2THKqY2n(`H5x@hsw}YuKIx7FP%$A z;dpqqv{BA#mfYFz96^sJV56U>na>9g3sGkCKJRyViaM8$!g16UJ6l5aSs&Qv?Q{e^ znt+YtznM=hO9`=k^S+dgaR^31gYzKyE^j%je}el`E<1uAofkIhuDuTQ+Tqfhu9cRe zbLl7?=R8Q>rF@GXlL6Ms?TjgNWC&= zMUT!48{?pDSAPhRPpVrD*Ag|sC}?mVB)w}tL z6f`&wlFugO%C`yObA6B=ofkIdCAOctdgZ_9YKbNo1*$<-_WORkpCqw~VXCqlMf z{rGIt;E+xg{Ih4jm7+aYUz=g>D0aC>+bViSh~7f9nPUmgqci~fM}y=6O@o^^mGFP?wI%4$1Z=dPPtDFYh?#RQ_}Xj@7!6G@3L4?r zQgb013NhRf^ys{>(JCvNIb0nfj&2F~E(xV`=_niz&zAm>D<8cr;Cssv^k@P$Rx)-U z?i$gkda$4GPq})XOGn{&c(!z_5ZB3l`ppsaXaY8NSnX#lcL?#NJnzc#6m>2gh2y9z zcD7VWh;nn^O1Z-k^k@P$j@voI4MLQd{gziP4#6mBgl9{43b9v+DvqE>=Y@@(ZF?Oz z3i0pP{Q`&Gm1}}g(BM2szFS?0Wfo?XDW0#{qeQNzziH2}a>K zuH24lU4-~~&V|4VN6@45!bXd+XVp}QOs&et$HTLwVnWnA*e`I-5%g#RHhS;~^Nv?(A-2lXt1VAa=h9I) zj=ExJOI3vUOquyNep-HJj?kqUQv9D-5M2+x+v2yufDNsgdL=Y@^BYpnGm9w^uGh{UeTio*cdF@mJV?H4 zQi$V1d@gNEdvspd$Yk49dm&zw5$%Gst!RQ#IL>*H{5_`-?S#1E+AcjhFKmp|wtw6v zL@BA#(_J6b1f!tAd62DFm4uLbWzLEoofkIdCAOcVGW3(tW}2%dnqU+(B4(!gJ`viBtx5f8k*%m37sz4DMH=+Sv$quZeJ^9V5W~v0@TSEf7zGW^gXFuZg!r{w3vZMo z=+Sv$^L-8Zgja|mCCUbRyDQfOqo5ICt&%Ba?j|APTm6CUB2? zf|_6yG&m3PveBb{Zuv|AtxVFcTEaW?De^U zb2L=Q6hX(4+Ih=DRIkHu~T$(fD54md>T4cpbKix_T8Ttqi`J0#*WT>#PUl;#L}Gyxke z-@Ye4Q?Q3_*`3VDmj7`Tj&9`pVOrAkSUr(or~$ zx?*QbD}?x2YKc@IL(roM*f?(ADFSg;YNAxQ7=lsI2+x+#2d7I-e9{s0=)AD`u95Av zWu^bMaaXPhMnQw~Ao+w>G`^ME-X_i7D|&QZ*vMp`2Z(!QjB4thpe7gv4bFq)@9u;s zDa2EbphxG0jd9Snt7$@vl9lZJ@a&=+OjhjMTP&xO(N2 zdSx^;!6;~O9wdLEAR3)yB-rQrAU!%SY}^ss&oM9AFQd&#R~a6Z_r6>YHvj6z%Z?`Ki6$}{ba8LQ=SDD!^tV%d>tZ$V*#!QI22tB~KkDT>Qm;O2 zyc1&of+t#oo@ioKk4L-{1@Y@cxe<)wS>Y~(hz#BbUQJ+c z!2Sh~JFAd}CKv^J-}O&;=f+PBYlH}T5T%Jzzdz}1nzA5F;4;iJrze`gh-&xrqt8kc zi~`-GTraQvsjRR@=&a~LlqNb(=)$nm`?kXeFVu(gdSG|1fj3cjeyY zks9W#=s}bwPL>_(&Au=*k}zjQPc(tmYebI!;ZR*axaR_>%3AAJTw=&UZr3prXM!z}n+T=JI^dL$T=)v|KmS_!4 zFbXvKf7AW*<7m)>C{3Wo45|6u*CHf>EF`@ABO= zCXNO@h|&b+c3Zw`ABUhPn!ubfel5`iqd;ShI$(POXo4P1 zz{YX=?)mLqgCBs%*gn_OxpWk@c^wG}n&5S4f_h=1%mYLGfi^wO-wA6%+q{SN${k_; zB9~W=GvSJb8eFj?B#_XaXo9rSNcirVzWz6!2>L+KqY2nt`{YhA3YlnIV*Ayd{bjz$ z>(?Hg7dF>Exf6^+Cfb&`C*wi?siRem20c12Y_67aCm00{+Lrja&F%iYmHkG89-S99 zS4+7QjDiMjOFa0WBK`$OI~WambY9q8U*%3P3L3O6(W?Jm-;r58jRrkBFKn)_awixC z4ceAiyL^JL<-vhQgC3n1HrH3V6O4ieZA;w!?WvTxEk_y+dURgcT#@BYFbW#9E%Dy5 zir%T+QXP~>=Y`D`S?&a*kcqY>e1A0v)NMV?Xwakc!sZ$-cY;ySplyjECEf|V`r9)` zgC3n1HrH^u6O4ieZA+A!c`k6xiLOS29-S99SADq?jDiMjON^;eA^6zLW=4Y^ofkG& zeYq2if(C6%+>l-;xNTewqd||(3!AIH+zCcOgSI6So@gEXqERWML66Q0o9o2f2}VJK zwj~Pp?jFpje%#b{dURgcTqovEFbW#9E%9y1^xzvqXB!QAbY9q8S>{eK3L3O6as0~g z;P}=RjRrkBFKn(wb0-)D4cdmt;7nY5bY9wArD8>Ec2hIr5s2dP5RrlFc)oSTfcEIT zuyK7{r6wfgL@)}OXxr<+^LTUUY@ggq#RQL4&r{C?@S{K=tEBgC3n1 zHrgB4wFwD15sZQcZL9Hbx7NXZjYX>hJ0~RML@){(w5>)L83${(9%eM?(RpEGeB^37 zAt5J%QP7}mHFh1Z=>5EVl+mC^=Y`ERwILF6A{Ye?+LpK|^O9LDM;Z-!bY9q)uW)^y zkdPC>C}_~O8ehx&s_DUjMuQ%m7dGZyT%RW-C}_~O8gAZhH0aTJVPihe6@5ZN zP6VT%LEBx2tUrD|TGeRKqw~VXx`cZGaveDkjDiMjtKrsBMuQ%m7dF@Jxf6_n25n1x zE9=3`CxWK7)1&jkj##B8&tBf%&Ty;Fze(4*+ld0}IY8v4{SLPHab0`cmNLvyGM z^ys{>xlWAj=bB&?2&@1?pISzol_nTvhy_D)sG^IUN9To&l}zYU%LolkFbafT-G#;v z({?q%C=maAJ~W5%gC3oiHdl9%8qoyTgU}vRET8?HGv+$r(RpEW4Hx@7m`g{&=87zL zf>9vwoH9;-6X&ex(RpF(wOZtrYl2ZAxW0-#D@`y81lshWHr3a~67=Z2u(`gEu^O6S6bPnPY5ALHLEdZG!gWMVZm!6?ue z=i|2$dJv@vu4Fm^ys{>xyp#u&;+AEU>+R5iqfO=!q)4L$arXb!6<6md$qt3PmE141g}HqrQV=#zLD?c94SQgr@Hy~KE5G_z;z%B8hlP( z_M@vO`+n-t%|EuACFs$4VXxiO-y2(KyNo#Xh4}fu4gFspC~elYnqU+(*h0PR11l%{ zjyG!PuiyxJbY9pqrabG-n0&p|tGXXf_8mP?+MnF0q0!I;qoBdQ>1FqjvpVoqY5xL8 z(4+IhUO#q#cjMNUqH$X8RjFalGfNgGA$2A%2w>GCU5!C}?n8 z@v>_PQD9s`V2C5=(RpD{zG{%Wjz@&3DlMd{yK+r13K|?y<-Kruf^(!5ra6Khofr1q zw+?pCqn8keq=nRWPf!z#f(GXj^1TK^d?rLQN6@45!fso6h-+7;r6t}e(S^RYZC4YF zf(B~VxH@R z^ys{>PqiLuda#%MmJm`)f>N(cKi34Kput(K{OytuPb-2Rofme`p+i-@a)eLk(owt) zyTgJi+D9~46LD6oqRdM@R8)H%H_Da&`uGN;p$S|E>n`r0-7l#lclTV6ZozKS51~$v z&I=oNx^Maa*7LIS3XvdBaI@5tXo68Vj<#U;OAbjZ`Cf=hj-W^9g^d<-{oc`{QD0i( zCTWRnq{``BIts_prtR)WMIrVG(cBUAXu{}8|M;oTEYWyWi0RTolBB-tTsjKJ(Kqd0 z#uM^9rb;Vp;0Ss&0UJH|jt7>C1}Xz;$uJqGbS@o*JACd62&a2N}9E**vAsFn6Q?iS+XF$uos9YK#KV56ei>v&j*!O}uXx+~WNqo9GB zZ=c6&LM#y?!4dT6ys$Ac*ymAD+SP1nA(h<|)C8lTfw9ZBt2^Wg78c@|Bk0k2VPk}| z?dn%)iS;G=jBAOSU=%bk2HXBoTFxp{h|@9_YLCtf8zZ&tA4x*U80i1j^+D#+QOLwO zk^H5e+$$La{pqd`(xVC3oH@z2U(_Dye`)^j-lgWllPkYECV6I_L0)_LewTG$k1;I+ zHja189_Y0>JV`1;aUrg5@}2kIzjacyCz=>=aG>|*zwbC=P%!A_sWQzF^h6V1y)EBK zUf^>fN{GgW>!*1?ctJzZ6HV;sJ7Si-MBT2# zH-4ley_Ibaj-0Yq? zJ<&w#W$C6Z*ypiLh#Q&@-+*UMPc(r{g_Hk~@4CkmeAu3q2ZEkx0%yCp`~5=TtdfGk zl$w>N8G@c@0(ay{n}^p}joOu_rHtGdGz2}-1n%yXhn|yJINEN3F6Idu0?*^}xYZv; z6L_*2XFhdABWWeqNgu&=&=XCd^&pegxOkzCSG4JOhM*^!Kno3@RSjwFNdbEu^h6Wr z1@_8|$#r}vJ?b@gujq*;(9`VQ1%Y1s{=aq1S3F43DNiKWr4!UE$nqDf>F>2&oX)}y5xJ| z;IcqTN6@45!Y(C$&-q;|{C?CCAzGI^=_R!8Vl*_tC}^PH*jdI=AyS06(#jI_=)ACT zJTov`G>!}La@mt9d7OqO7zGW^AH3{ugy=3r0Y}iI^TPg0{tDY($CE;2N}DR`u3Qt0 zf(GXg@=G*AOcA1tBk0k2VHe*uz&(#M(snmX3wf!$edd~A6f`)0@Upkd6KwHAA^!;X z%<0j2VXyh?S=X+zg}7Gor@NM@2}VI9Jj?iAh>}8NID#IX7xvKB&$|9`UWoVVxA0eT zeNYpOf<}0j(Lk{Tv9!*fM-j7pcG?*$x zBWWRS8)%$dw!1hJ+ z67*;SHd_8o)A89v9_dk8(xz^e`mS^7C>%%Mw7WQu3vss)H#veHO~6JEzIJjG(bz9U zX=zhZeT;@C7zGW~6}yXbhg^AIsU^QTf*ze0HjdBg(MB|?3bCR5N$*4)f>F>wt+dzi zrJPk4X@&V@EYu#I7d9%Yy^dZ&ye751fxB|%(ox97{TcZMNg+x~E4(6Oq4sD3Hbw^f zJZ_ZhNRbg|i+h61rK6AuW0!4LyM=g0h^~&HM-#9y!r69pS%}ioruw^ofkI7bK5`839(eJ+UNS9CKv?`?$3DHk4xL#AjCY^2kFsyVPjrm z`}t!+^pK}_r#yF^OGn{2=S1@Br$Rg;#D87&p+^(2IdhWl;>EPLfAB~147zGW^gXH_Eh1g%G zbnwGGmY_%Hg}v)W`L@tJ4~j;Cx9{>V$n#@hTb&14NzkF9m+zc9+r61f!tAd63L3$cmwOOMVA z``dfc)$^DqEo75>f|_6yG&m2kt>hIUb~%C`ofme6iSjL*rd|Cg&*QPWrTx#jwyOz7 zL4)%kFZ)R$QiN!o*S1}HbY9pmZ z@$hWPFK1P3$6fxcvzDMo6R>fo-#wKk^DES=EO~-buZ)H!7zK^+Z0W2JQm_20rJiVy z&I=nY=IB>jL?cnI5Z_3T zIw|#C=h9I)&UuiR-Asr!LL7AjJ(_@x9(>i0WkusUA(Ew*)RR$1=h9I)j=ExJOIPGx zZGUQO%0rHzM-#Ad{LX|bqLCmPMW5R0wTMG73L4?r(%+)-y;RZGj-W^9g^h}8uj8~3 z`J_@y^)c6>2}VI9JX@+JXZ3~QzAM zl{qVVbY9pP2W`7Tz3M7st<)>i5;egnXmB1RpVtbJEJT~UmY_%Hg^iKg_74zJuY#p7 z*dC<`MnNMyTl!fDw5e);S%My&7dGZ4wx6F7;!$~eAIVeHxpWkchi6N!UIoAGZ3%ib z0h==?8P%4z^Ur8fG+2M{^c3y6b@S-tFDec;d!eN5Y-!4}cK(@7iw0+Gvjpc+nt+|1 zIN0kv_Fb7@%@g9mxAO%H-;(CjxpWkca~>q0h6^!rX};ilw_1W8O~78iYmisx^K+uH zQ;5cuzYo+H^N!Kb1f!r4o-Ms7#P%xR2ZoQf1U))0>gy=u|ok0F7mY_%Hh28Cf|2(jnJ zG~XNU$~D0#XmB3nWv>w8&zsVGO&mdw&I@~dmVDA+p2s>N8cCZP=AOAG7zGW^gJkzZ zh|i@>jdahP9-SBVuDa=}U2T{Aue!FY2}VJK^B^z#E77<{@}J4DZI>RM7k0HH16=?3 zOo-uf)wkYYdz2;^1r5%F3kJA;UPXuk&5HWHFKlJd1f!r4 zo-HjCqOUyfM_etTN9ToI_N4*tQ<+zU=psbnFQ$7smyY6f*eY63h)3kCp4@H;oE1Ep zpx!orN*#GmMu?Wn^7+SCm44{%2?<;WqHvt^ATRq@Ax16D=g+#;67=Z2uyLnfn6OUv z$WX5u$uqAh^+e~=Q8>Y-Vpu~>+gr4J64`W{U%3dh59G*B7-lFIOyj5<1(j>2)&6+2s+BgBJ3q&k8gO~A(SmZ=*>1B7q)bT2Iq z!6;~;R@&JTuKW(Ei4Qn}9-S99DyqGX)j~9u+MeXDToa6f2IoOu_B%rSE496mBk0k2 zVPpKT&%@QLfYdAV1U11ZXmB3nWe*dLxzeUixMxm}&I=nOoNZT=g*Yntm$(h`|1P2Oq=dURgcoH=>fUzB{tzw5#G1IK6A zP0=2FuF6jlN!!`dULlSMQPdHfM`;3f`0WH}?7n|tU`UxnpU$PDaGdiX*%1~ZONcd& zphpw1`PPD$y+?@CMW+NN4(wz!G{Gona2_PT8!p7TVp9V5JZA}dbY9qe&%w+7Scsbk zbqX9WI>l&cf>F>2&z80bu~CRm9YK%I3!867$S<-9@pGxfz~}}GjfN%|1r5%F?7doY zO=4h@Bk0k2Ve?%I`OPrV=q*2M{@R1@8x2h`3L2;@cDA%nh?YW3cLY5;FKisQ?`43f zzNBsnH8jB}XmB3nWusl4T2wdXZKpwx&I_CGYuL8?S*b+dqwdNz!6;~O9wd8xqVZbk zMBj_|+q+AT&I>#I?gxmT(x#qrPf!z#f(GY7@>}sjOdrt6_eOEsO6bvfVe>5#FMG2P zw-lS=JLcN1CKv?`&VyurCB%3krli}pOOMVAo9~@?*?1n?<*NI;9;FFJL4)%k+oQG$ z@tGs&(RpF>O%+S*m#25iRR&Ek3L4?r5?Z2EAK$;TZ1tf>=Y`F8TWs3}@%`+&p3bGC zcpbKiqFo&pqKG3nL(&BG>{}M7B~q_^!^$KEbS@o*iu*}(4+Ih<{LL&_6{LFl{QsEs+`WHqi~$_ATRs8 z5MBCr^8MxrdNct${PxbbLd=&wSfRl}qoD~#K_fg{+9|{(>4U`_L66Q0o9_YHdv%M{ zlKC>~=v+Dq$5B`8YzeLX7pWyueGEa5CSc>ZeWwU5v7@bUF$ANa!FiB9t2&CHN9Tpj zca3cS_)=-t&u{m`UP%qavR6Xo8vcJ+Pu)V&=T+ zss0*u3I%8HD-lCL4^hZevvYedb>c!HmTc9^{ ztG1;2x7>dw&|}ZDF$ANa(QRR>che^?3Q^S8$vpY0Oj zO5rqr{R`UzYtF5VAs7XX2E`urK7Dhu5S9Mv=&!ruqd@6_I}JgP&I@~O=SRK2-ug+1 z4*yB>FSxiO@aoLZVhBb-BhQnMds)c`h4}DzNB^4M^8&@+Ic5lYbY9qFZt3Q2-SM{& z^L=Ul@d+;nhE0nl7zK^VWuEkgTzguG(z`qQXKf!DXm-~*qd||(3wxihhd1%}g2{=Q zC0eKXXI%GKp!$n{#t@8xMxo-pyux2x5{Bx_g*m?%%!7{sb$L3-c_^m3en}mj{fa^&wCH{ENC?7(FE-KNswNY zt9zvRH?R7@Yw$t77=lsIXf{>eTN+b9hz9R=^k4qAtvB<5s|-Pp&I|jVchkKYNu`8% zw^y2f&W@=YhNKsWAs7XX2EPsTx|GA+O?;=LfA)YW$)gTf4SIB5*m;@{@&282jS$cD zO!J>Q)IR0pk^DwO6O4jJy(YuFRUPvSvGDzl{)5HdPZ?V!(Gc|Lys#S_8sXjic5xxv zbxZS~Seugq>*pZs>&5cKH07eyoC{;}Sf z(`Y4a8>jiJOwI89JpEJzp`)G@jYI3ldOzKATr@uZx}*Qfhf{r9d;Sv89!m7rSe%sR|6t#I-|sIUh#?q-OuvtO-n;8DRG;sDm*+8XxvynjOVFe9!rqxU!5gvk zkPy4er}?Kmy3V(C)kdSC2}VKVgOU@xvx%#P7+R#0|6Gf0KJVI~A?VS0VV7Pw(R;Ge zCLun(l&}1Fv?sb^potchN~P1f!ra_=QQ{{a+6j;y(4;g|Uofr190+YSU zl^O`~(5O`ZsJeOm)!#1`Lof;&^)J5QUCulybB04pJNXOzl+V90t*{~J(RpFNIcKsr zzQK7R1~*If@4K;}e`D*HJ)KKO;rPjhFM40hOA%uHp-%qmekL34MA@h9}I+u>39UQlHttBSpLLiEno-eUV2z6E|+N1Nr#+9#Gvt?ak z<{-K9{r1XZ2u2|j?%_RUKN8{_xx0ySch{#C_GypK3mea7UB}zjCuX*gCpbc$VD0yd z#Sn}_CbWgCXI2qnleCiY(n>B@ddLv;=)ADe#@;>9R|wS-V+ck;18w@?HC=^Jt=$mx z=)ADeSAMMYx)6U#k18uY>Qd23F$ANa@w@cRHMdU{0=>4p^xAwy78-&cofkIx{OWZd z3b98j!$zqLk36wBhF}ylP-Fay-xgw|RGGu=q>2vxcaso@rHcM2RkU5+1NN+(N9To&8vIhr6GAMNNN3_fiBw`eSpS;jTR-%lA;CD9P{=)ABoM_JhG zYSHK`Gnt2vv`_ivh&?M!FbW!&;S}C=MKrFg=;&W2v!#~~TxB%q(RpEGu9cj(sAw#f z8Clhjr*0UZULb~G6f`im8@AwI(Woo4zVY9+l{bd120c12Y|I(2O)erDd1PjJPG**^ zKFDV@G{GonV2;{#_XW}DD6`kMWcJ#+XF)^Iqw~VX40%D7LZUHTX2`2$hWz2ZSIjfl z1f!sVnRADJzlla`nS~#fS@>gZ>=UF%=Y@?q{9S)(VF-G3Uf5WRT-*JO5O2$hWt6N~Ql}k@As7V>tbHyY{7i@qvMSml ztD@5@EJ2UX3ma>y`|m#_M5?U3UXhj8?3te#4NWi#8d$@X@vRo3oUHDe$?C4$z@3Jm zN9To&wc+NTn}xU}E64`2g4}s-WemY6Xkabc`N6qDG3qq!5X++FmZJ?Z-1-GXy<4FKn#Uo3)%EL_t~6@0At( zb9>>B*?cwa-%qw~VXp2MW8nhVjy?M&R3eRmAOC}?2! zqdzyB`N+_v7l~cN&5oofkItPBxq2}XhDURJCIJ&4jo*@^AFNe@hqqoD~#f#zOTtOh-Z(nO_v zoxG)!R>slL1fxK6FDq7q9zA@rtNBiQJ}e( z6+xJ`OAn$nv8sPB@0<46FHwEaXlQ~_pu;;}iZB}VAW9Rne|_59aRz%!iZB|QU=(QX zWknE1gC0a_qTBNWyweTJ#L>_Mqd;>nD^`OZL}_CDE%J?|aWyo-DA1Sz+8r--<>or* zL6jz128QQwuQb6Z(A>+4y$*U1rHSk2W#sV8HNhy*+{=p9pa)T!D7JNsYP$B>n z^*d|pT(B9%b%NdT!o7NT!`W5YZyc+wJ<&v)o#W#4K~2DB6m0HgMP7&YL=zt^8y}~Z zL=%j{aqeZsYS4oyO}v{nF-}d?xnMI2Huth(HRy>Zy6>A5r?%@{uo(rLds(p>^h6UU zOTHLqjMBMaGYU5MvSKypi6&|nn;d7X)wy6Z3O4t$A~X`TCz@!pZE~DBLo~rC91rh! zseW!W=s}bwI^Q!TcJ32HFbZ^d$17Tc9z<#4mDDL_b{2c(nqU+)>>gP>4R{cx3Fu)q z7+-?~qd;@_E0&;#*MYpSao=h+FStH;f>9v2`xQ&jqw~T>i%AJ!rXPLfnqU+N?taA* z^ys{>(en2l!-^$ZLlcYw!QHP|f*ze0HhS>cTb9Jp&;+AEaQ7=#gC3n1HY!ow_qW8+ z&;+AEaQ7=#gC3n1HtJ(~;UjT0G{Gnk-2IBxphxG0jgjHO-G9c>&;+AEaQDk<9PrMr z^1XR%wf5+|9Itw`&AY)8`KR|;Rb~Co2tr4}#)xY7$aE{wo@fH&?&zDa=NJA5h0?pm8SPgm*r3tLtB5FxULlcYw z&E2n94SEoz39LUOYGO!36O01Q-LF^;dJv@vtfM1pdq_hQi~`NwuUHLw5Tyz12Skif zAq`D13N&}WVm0VNlqRsR5i!<=G&I2|(BVC@=;}icqBMd1nTR<!)V-n+ek%;bN8gn7=PBPJs6Cp1&G)kG zS>;493YlnI;%s&ef8GLhj0QbAFKoWal{>*GXwbIAx}O{SKUq@QXwakc8okgvUbym{ z2)sFhH^IzxL=X=*?Ch^FFt5E1=h1m#^Gz;$Ryh%jf(C6%eDGdx|DDHn1hhxzh0Qm) zawiytOtdXA2tuFKk?&ePc_mBS(T!$OOBldB^MASG{TulDj+ZfeA)~9-S99o|AoJ3)cZ+Y#f47 z(4cLed4_9OMuQ%m7dGF^ifC6c1f!rq+iG-?{!wmVUcdI}ys-IRmet6KU=%XZwi>IY zpMSQbveBSN=Y@^_Z{OI$y~>GT6f|gCjcln`|K_h_H0aTJjb7-DEokIKpxWWRE^{54 z$XG6Qw@CRG_BxzL=Y`Gpvh2OeiC`2oXxr;JA>+r^QC*D&JvuMP68pv$t|K0yqc}!| zh>RsN4j%g9X}|Vp0yf4+`^FYDav~VTu{K0x6p`_KUBMwngHbv!Y>e~vjV);8L@){( zwC%mhf9123Z&!~r8uaMAurXh;Z)`y$CxTJXplzSvGvAu|mC>L_=Y@@VmwjUk8aWY+ zf(C7?(OKq+W%~^`8uaMAurYtMZ)`y$CxTJXplvmV%Y1jn=yan&kIoAl^I-eN7Bq4q z7zGX5R^vmNw_ktnQ$~XxofkId^Y)D`XyimN3L3PnMnKjdpYP~sH0aTJVPjol-`Iji zP6VT%LECEVk#*FjkqwOoJvuLJtgq}FThPdfU=%cHTa9aEJ=pc5JB$WBIxlRj6YU#Y z(8!5k6f|gCjmfgEy>hIC(V$1?h0XV}awixC4ceA)>vNy>=)AD8Zntl2;W~067==u< zt;P=7XQ*{{iP4}(=Y@^^2m8j>XU%8U1d$YnU=%c9=QHnk$$m-AZFaxJXwakc!sdHf ziJ4_&&fs=fVhBb-12*=95;N;ns+r?X5eaC-e`8DMg3bLT*!cZo>@6*t&bbxtvi3w1 z(8Eqq1d$ObREgD6d)Z{qI< zV>L9vDA4$Qy|d$|MiS;a=s}bwP-F1-gRvT#U=(QlPTwkK&1Y!X}>V5i7xKev@}-2obf>9vwi^q?2 ztrACr9-S99*6sM)zE}-SFbV{It@+Wvc1Df0+N1Nr#{L6#abh)?OGm-RZ&uI!^KBHIxlSQ zyu@l~f>9vwWY5noxIUJkN9TpjJ(=7IMu9*Jy?$zP91VJOUfA4uiPg{qqd=giy|lA$ z91VJOUfA4uiPg{qqd=hg{Pf!7I2!cmyhhLMyjYF%QsLSbeq)u+rK4E+n(d$OG%oMH zw$?otnuKa1Jeq*bofoTdQ7U!6_qMOpxpWkcqiSP^CGyHOp`&19lt{1rP8@=sXoCAF zu^O6S6ljcg6}zp8qd^a%G{Jq8SPe}u3N%LTdnG9Mm427dCfAVl|jcN5RHQ|JW2?91VJ+3G5(X zza&;e6N~~K-u)O-wfP5G{!ziv+vl1$Yq-zw{y&w>nQWK+i%8<*#oZ$a=F$Xg9DmB} zun1A(L#tul_t6BSBCov8txrb~ybhh0df%9P)uT?0nx!8Y5)8EIY4(;h0UJ>uaM#b9 zCs_NZvRP$?*wm(Hfa=(-fz6!|YTzy`n%=Z}jVz}T(4J@l^iO6_=F=M&XMOuj-{AZw zf`*_+6R^1}vS|8{CDU>w7zJ(E`OTipxXQ<~c3j;#SmukoLG96bVRKhx(e$eR(^=bv zC>@7j6f(iCZuVsS-HNVE%6Kq%`e;?7L66Q0o4X>5ra!WzRE`9rpaJ`w*^?Q0;^vh< zx4Au-x3b@8(4+Ih=B~)1=_7kr&5>XfG+=KsdookT*IW6(e~JVb9PMB<=+Sv$b01~V z^!uvU$dO^;90^821NKO>!_uNr zhm~uWPYAR;IM8U&qw~V%9>=2TZshc{ly!+cz-n^D0jRrkBFKq5{ zESmn>&@MR=jDiO2;%0~Cmg0}BeD7Grl#{zh84Y@LUfA5@STw!iur4_gjDiO2(`JXI z$*pNCeSbCZ)one@Xwakc!sZ^wqUqh`UgbnE3L3D#Fgq-LZ)&}AaEW((ul)9m(V$1? zh0Q&VMbodBXPy(mC}_aG-R!W8tlMy9xtZsD*PQ5TH0aTJVTboi#)(GR90*211NIfO z!_sa2-7Ck`sNjEWW;3HfkIsuZe0cYxhV+kkgpPs+h}mYpWLmlFR$iZ8$G>%44WmJi zCSY^NV$t+LQcH3o7zGX3!_0n3p{y%eSGu+K?`u@bXwakc!sd>}qUjGwP0Wd46f|H@ zF#9DdYHiCZ(!0BVboJw=w$r2Y!sd>}qUkl)?8=c~6f|IWHTxyY%8k$Zwq&~h&7rf6 z20c12Z0=YrnqI}<#}M=55R8Hb?8nW1$xScp{NRTx!~Nr1S2P;*=)ADGW3g!Z)&A3l zm=K3x6f|JJ<@QT3)6Fp7Q=~mQFKzB+P_UOE@aea0# z<0}8@90^7t6YNQ5_v7!In$*Pekh^O%=+Sv$<2l*AjPxbb4Dm)Bf>F?beb(%LWY*eN zv$(XY^y=CUMY87QdQ~ao5s~J8uaMAu(@NgXnOjRQaKWgf(GmpX7}TX^>-(Ykb2d9 zW;3HfkIu{5W%n}ftzIK8p`)OI%GcEFevGTzFzN5t&-u!o=xQ|R(FAPNW4kl)>REq| z1f!q&7m1f!q$pU6gsrtd=8<20c12Y|K~e&cw`5Qgb921r68>&0a>cMjet4%KWO?!GT7D9-S99 z=3RDYVqW>22}VH!_71a`(RN4Eq&_lF{CZYTqd||(3mfxCyE9Qto_TK}4#XiC1r6A9 z&0faL@%55w%6#|TqaBO}JvuLJ%!BPdgRF6ql5!vz1r69m%+7?&+mqb9-DuFG^TNh_ z-tIHVygf&PQP6`cfys-{~<84Y@LUfA3lFzcwAZXFdvFbW#5UpG4wvL3AI)`OWGvFeZUN3Wfj^p9+gJU@m?SZWcWU_1L z+z3X2#@X7nb2LE@qHy1^`qkPI*Aa78n!uB_>)L1yULo=_3O3rCU8P17^dJf?AM4Ej z|5<4QebcTWqc!wdF$x-}7OJpyu0^mm)H> zx{iX)8BXjIq$ird*kxBHq6Rn}oic!$O zoXD2C}?n28~Z%y;hAW{t|p>2Gyxk?ps_Zv z^L9m;CrA&^L=#+1*p-A7A9Ed?S%P5IhRqflx{i!^^-2@0#~}iMIVEG3CyFKL zL6jyqtIeHY6ll)vVhMT>r3ubzb0-)Dnsd|$!n9qsA4KVKm+j3E!SQWhUODI1MKvCq zQ#5%&daBoCPTO^l!>%yN*f@Urwp6dT`D>w!qd?c&^ssmT`KEC+=s}bwI-H*nNrW^s!6?u-tm)#NP8t(OgC0a_;)zS+ zBZ-iPCKv^J{q0@7Gf&Qqqd^a%G%;i2^N~bILlcYw-Jp0k@AennH{(S-4|))#i8d|9 zMG_$mO)v`dTkW6pF8^mooa>+mQJSc_d`u(}($EB>K!0|+r&sO3S8+7xL6jz@ewPtR zgfuk4DA0vA_w}x7^nDx+dJv_F?@x}1BtjaRU=-*soBDgd)py_U7x6skL6jz@o*EKK zgfuk4DA4#_p}DX8;;sYLXW#9^vf$AbM^_^Bm3uxqX}=2zXO)m%I%5?Gdf4Y#yU@=g z2~e7V%_!LDD+jCBTpy|o01QD-G=aYPzX?WxMqlZ(pm!V%dJv@v^v(ZGFbXvKN^<^} z<7m)>C{3Vm{%?X&pwU+b-0Hpy$vh9C{3Vm{%?X&pwU;_{;)re20e(<1p4OxCKzR|L;7HygNNg2px?oR zC{3Vm{%?X&pwVkjRKxc=4_Z8;M#0ok{sz~*n!fzGs_U-184~0>0COB+UWC%nals^Vu%ME!TO;I>a{XNnf5nl)xTw^KVd~` zK~1 zR|zq|5%lQ1uyOnwQ?IT|XrDFuy{`U8x}G;0nqU+(*yj^7qX~L+Uf4L^*VL=}cRruh z_)K%ZzgPL7&ZVPpoPA!tDgDj@D?y|6O4ie`+Q<%G(nHf3meA=n|k$6tyU}R{hH-FeyE?(&;+BP!9Jgu z8BNfm^TNjQcBWqKy(x9&Q_E6)t#27>G&I2|Xs}8oW=0eA=)ACTysoKNe=g~=@?iN_ zQ>3pO4NWi#8sX{_P0*wB!p8BROuYiJy8NpiH8jB}Xs}kw?`A|1^ys{>aok?VXL7Ih zEK3bEb@xgWjDiMheqttwFBCzK&I=pI?eoZ%Xa2;mS%JgunQMYk<~l;78i?+S;Haka z8a*LwySiT5Zda)z8LsVW0yd(c!7*6A*C2|ZN9To&aNPFuOsOS%q>hv+U~35p9R&@}i4rrT33@aE8^>+E8YLC(YB{5q zU4_#GqoBdLo%}|XR8bI99YK%I3meC6-K}_~UDjTC7AcR~+O7#kL4$MD#LQ@d9-S99 zj@$8LV(SW7$D~a)la``$=_nlMTstxI#nyAOjtbGz5%g#RHjdkI@WdaGz@|Bg-7S59%fF7nLquJTod=|Myb6zmyW`5 z++Dku@xvb*YPfr42zoRD8^_0+-H+E=SE%`&Ji$~MLv$`3h2v-?b}u8Ephpw1ar{2B z`%&UbyPEr??cV+9d845TMnMDZ*zRSNy0Wk47ed_Q2zqp0*f{=++5H%QXMv%$Bw0jxR1U;I7jpM(Y-H+F+R!TY~wWM%?T1G{m_cEl48bYe5A?VS0SsyQ%-4ChMNv^g>5IPDPsPlF& zBbuN`6R>goF0=b_p;oJ;4pJ449O`E@G{GonV7#(>8PNniIxlP-Pc*w9GIA!lvDRp4 zf>Gu=LOT;6+*oVR%6W8NqbJ9IHM<`&14wdnh8P0lIijF}`Geidh$iUKd12%D9JBiY z!p(h*h9(#V4a|e=o=h}BkIoAl$L)26}xO)v@?n3uXeG9m6#1U))0Y#g`kN@mDOZcc5kLlcaG2Ikkce?$}X=)ACT z-1ZNd=_k3hfzi+eqo9Fxg6-!Z+}gkp^ys{>aoqOvQBoN$%Npmj{L{H~6pmxPW9wBk zL60V2gIjvohFRj64=R>C7r+c8QLjDiN%(RLh+Cg{<5VdJ?djuY*xdcFpURkU`5*0xCh(g-n3v>EFpBhP`S$k^kpUfgcqTk|9LM*v&X?c$2@wDc zK@Xxd!TS~>GU5@80*&vNT_V5p6CwZ@4SEoz2|oW2kr9tz6lne)Ply0uH0VK;CfM3T zL`FPq;D;0PBYLK@5kf>E&f>pCGKq(M(K!I3&dgfuk4DA4?Moe&Yypa)T!;EW?ggfuk4 zDA4?Moe&Yypa)T!;LIt4Fi%hui~`MH*NGs^^PmS&n&1pDf-oAIU=(Qnx=sXPH0VK; zCODIgAdH447zLWY#}iA?W3I!^qhVt{k9mnDGRI$BwW{N-D|Id%WoG^ES9>h+bOry1fyW%*IZ)9bM1*Huw#LlOsocTfnXGD{3;FR5)p*CSM)>^+&hUN%)QbC zqd?=ga$@H`+5=k?$n<}Foq4>K(-+3CNF_8OA!GRwDhjFIb5cT@BuPT0d7it?_cka} zicmDk)IgD;l-_fYWK2RbN6C~~PL%pR&))m#uI|0%51;ip&-$+C9rxbrytnfnu=e42 zwSTLqUiQ2 z%m#~FpHsssmSD_zh3^1^dAtgV-j{X`D2RQcu+!F^5%YkIQx`mt7nf$ zF5Z$%S;f|6%p>C8C4~KO#G*n7WwDZuVe$D#9Nz5N-a`i_r(QbOc}TH!8SC9e#pgc- zkAo4)qKB@l;`0+fPkDCbBVCi9?fb-eNU?Pp>zekzjZ0#4A%wE%q1Y2YSH3a%#d#&t zR)t#xp)6KX?1^jYHB2_EdY1E$V(T*27;|VsS@clsiMtZjlJn=(b{HD zdt%)1)roJ5_HiCkY+c5h9}i6^iyn$Sv2o$(#4$Uwore@#m$BygLlerPhhk5(Zk?5= zvwM{DkYejH)_Ud8gtF+N*b{y}&vhPBY+c4$sU4b77CjX2->Q`MLyE1-SS!v$6Uw58 zVo%_Fq;x){*t(3hay~SnEP5#R#CyY6r#6M_N{X$^SiQla31!hku_xYL_;+ezxId)W zy7pb2o>*T9VY6%}En4^1eGl@xnob>$n==bvA~c}TH!8LMwUG@&edDE7qqN4lnS zF{@d{)@97^AN>0VUp#Euvm{0pLMV%s^o<2PiZpxn?x6$I>n@q=JfzsVjQKr^Uo*@r za^pb>Wzj?5ycD0`;gf%sosvBwef>8{=OM+`Wz27i{JWP&71?}HLRs|CcT2_RPi@j- z+0xND>8&$zW2@M@{9NB(6`!AbYuy6~DVAWYw?h}7Kl#?W7t-UW8M%?IWwYeX)nM`Y zBm4dD0D{{~#S-#MJEFxW|6EuR$Li{Dk_lVOW-;b5oP+0s#pm0#C`SXk{?v0$e$Lo? zRQ~v%3%O35vn83Z3P&6fjJdYsca~g>ZayfXELLLdx9YINn_a|J-;_(|Iu9wfE@Q6k z_}y#q`8h>yJSd?odN6*%5m+Z)6xNB(LyE1-n5(`!uec8$oYBHMu>hegdMNfpSSKc} zV(T*I+HT>U{$EsOWgU!A7AqcIs zTsEh+^N?ceGUnQ@a&P}Hk+QN5MktFOioHiotP>ZV+}wFcv2_`9ZTIhm`@()4j8GOm z6nl^ESSLQ(s=f1&V(T*I+Acl5c-=!2%A$v2PdtQm;#WocI1eedE@Q6kK5bPhcpQvS z7CjVukD`l4Cra$hb{aXtzqltmB4-s3y06Wi_jnMiz6ZknA;s3^ zyu!D@=iP%d3n6Tln{f^x_F^-_-omd6qu2pPZ#$4OwjqL{`l*LMly+>FlIu9wfE@Q6x^5&?65z3;6 zV($^wi3zLNx{SHD^K&iMi5G=+VgW)~ti;&QwR;Y0b`jT!uU#_N5mIbj#$1>9HNz~- z=LaK{MUT9dj9vR!#nxr4RaEi$%NOOw6T6O1zc6EUbm~vpbzeVjTy*!$a;yGcH>Pf- zQsbgh(;Bbj=glu07u|@z*orf^HQOFHX+PTi>q)CvLh;gnj;|eOR^)lag%Qk>ckQJ^ zKvX+zdtCL6Vd<9B>ZPn=>t?Wp71xZ7j+nIs#MHyb#5-0GPG>gVbpTq8Aod12ORZvGLF?jnid!RwzIyiJ}iBMUU2R=0r7n z-v(mmny=$4W|T`0tl!)bQfytujei>z4L_?th_8;D5H}otWV&JXHU$V}(WBToqoR99 zj090@%~$c6JwHgjJ+!MMq}aNQ`yM|c`hMS75N$5MBVK&hi>cG9^(jCoiyrqj93J(o zIuS&o>lbnDtbVDkEwddV#nxp!xA@TLnxF0fG3l*IaotsQQbkLTC_pHS9;esIj_zJ{ z7l?D0ej4w7-P!>HdI%`nynE$}X z@%W!gCnoIiTP4NTWxVdK{!xcX6X5aaFVo^(PuEGzEmDwB7Cq7@_7BJElQTYyOHX|w z(PM5uw^dSXUB+WS=o3|0$TK)_@_q5A*)Jxh9PiJEC6q;vYcA;%&iQjKHpK0p+MDPw zoOj`x_k83qWbXo{*&2pyz{(doB96!u!OSc5jXB0?!leY*TwxCUY#tFQ_*=y zv2_`*f4gh+WA&5a@!TErHk&`yc}TH! z8DBZ9bM#2gFnHYk-;?ppZ3ib$c-W7-mQWTw*4FGC`LR8F<%;;C4Wp91w!PwJ11YvH zw#xDvPFN^JOcg`}-zv-ml$(Cp^tfVk$q zb@2dP?Zt8`Izoyi81r74wC!9FwLYC4FT)*m+x)r(2xZZOciNi^8i8noyY@!hwPnZF zbA%LImoe}8W7b^*;#iCfk6~okeRP`wgtF+t5#yZY9YL(X=u-=$Pp>IAJ3@-B%a~(V z`Wq6{F~Yr%5pL@7eF_lDqK6y#W()>lMo~vdv2_`93~t?g5QxX}M(P5DvgpAvf7#Qc zLF8gKc+k%Vj*w#OGUiU;sj>SVx>nEAe2Q9HwU|8br@#nhcG*LXqoK@DYh*dyj6ztkYejH<{G8ctvAABH&!x@u##y|-CtLhP!>J7 zCi2(rmsl-bj@8nT`pun(6kC@u*IE}vb>J}@E3(6}B75vDe}7m)S@huA&fkOUu#gzQl_iu#53aTSm^eDD!tW~|yYWhjt;?7z{aa5T3Xe&7 zEByk5vgn~sCeECf*G)J=imj{8g8fdMX?sLYdgsVzqbA*o*FEmv*>(R~J0NO|xoFtP zId!W%J=n$ky!QtKqJC34)rvDWfq1XPQ&FkXZ#hCrexmj60nxt?%y|(b9Ec}wofn;W zU8RV%q~s^+*BuxYf3Zo}su90Uj&53VizB4uCpJwV7~OQ#+(3-^xJPu@n2C;%lAqYV zcVIN7_d6iCRpp!1h`zXLmLsI(CuR;C6iwayD+uoSwtIi98~0n}2r2oA;rj+fo5viE zGsvU+N987U{}{H+5mNFK&)qdRx@2;35cD{E!W}O)tni#8q~s?qsW2p3`T3E7n05T3 z)psP9Izmc*V%sA_qWVYv7mn57%yIGU6&`hjl>Ee+v$CTa4}KMn)ri-|#HVCuJEF>r z?5M}sL@9+43vwr1 zc7Vrf5mfRM^#1SIcR=vE;(ok-b7wuulNr~*7hDeRztU3?g%OQ2_D0x&sPdW z8=Q~9+1ZYelAqvtdi#b%AdWcv+PKoO_c}sKeuC$`+BesM;C?)X>+1I8Qb+JASvRvo z?fe97wmo17V4g2xWrgRA;2a)gxp z1n%=yAtgV- zacp6O69TcokBN?ulAqvMdDJ~WR(m860nSr0P-|@Y{HV{{Vxa(w3 zNU;Rtj+b_cey?}TsyOo*h$C9{PG+^gD`9KdEPk$4f1G&&#Bm@twDW`%OEB(Gs%!N8 z^he+^7sU2Y+a_On`WxqA31!hEuQ&Jy#99#h0wKlLW&CuXZc)xF8F*|4aZ{iA$p+t@ zn6$NQ7C+ajKhC`R=eX3{Ab#HN2`QFfJoD-v(dU!Sz)I!|5Y2a=k(~2uZRcSLWzj>c z{x~xMTU7+anm|agbs6uwt5>w_r+VAf(v3jC=3uAALW$8$9j?asT?G z6L)_!$az>oS@h7VKh8V=VgiUMfskVBGQRrPLDB46`@&-^h@9qi<6FySI}b}Jiym6_ z$C*(eZZL!tTbFT}pR=RCPT)$v8;D{Jx~I7r7zio0F5{Co z4UdYx-2omigLrn#yQ%g2+B*+RD2pCi_4{Kr4a8@GkYejH9yxVX)Ou@Ecr1g*i^EH$ zyN+z+JS?FsdT7-jXNKWwzYfILfskVBGX8I0PE_v1i{Nn;h^j}_NH1Pi#d%mlS@h7V zKhE5`b6n~f5UZZ?gcMts@%Z9nqRu@^!6O&Mw&Sl&w_H`!d00YO^w6q5&NK$`8Hiqi zkYejHe)Z!q(ao2v$5_$}M7c*gr#F4R!i`s!P!>J3>W?%10`Yzzq}aNQYj+tN^%~U! z9-~1l{dz#UW1^1pu!OP>><#AN488#3oO*QF|1-UPAKk8qBVV(T*IXxDezaCkfhVt)PZsk1ut zD?liV9vqMT6O*nWPQ|!eDG*X@UB(=>FZhMeaYln!b*-B}oQEZpMGwv^{)q|as6CiJ z+Lq0BgcMtsF=wA5Wyix~F^C+@YIl7!r~siXdT@U9PfV78xZMy^Y+c5jMbBB33y;@8 z^up|XbGp0pu!OSc!Fk?4F_{mdxu4G+A;s2Z%+)~aC-_{AD}XjwW%Rq`h604L=)v`h ze`3-B#MvP32!s?{moZm0CogLQkB%T}VYT$&ueF_rC6q-EuDkpblU=ylD}Y!R2r0HM zW3JBXUt1d<-+<_kRbQL$PIMlYP!>J(EF{hxhP(Dj5Fc#!gcMtsG1sD%pEwR4Wk77k z>UGJ}-}uodJbkfQ^x!(!KQZCh{x*oyp7DefOEBhY`OZ^6#CWv`#8FsKd;>?#IN`iQ|ohPK&x{TQy%)X%?JiZ3e4BbS!!s^)8vRV9GeMy|@4Wc!O2Tt~c z6iYBx_Y!B)(?3r2Kf7zP>;un4Rw=r2a@_@0x<)O~QxYb211G@7;mZBEqZ6`^i^@D zC5X8lnLW-@+_|c)=qf?L4Duc)kgcM6K?tDSd=!);ly%c9o1#v~G6O*e(R&ySfP!>JZi{NPnj_zm> z3j!g<)@9tTLGP&D2i4*6K8W2lzE5l}-^h7bLRs`sFA`@;V5?pP@kJn{*t(3bSkO0m z_q!|MQ3^zxZ8H+R`?Pf)mQWTw)Qg~_2eEhSj6}~sNU?PpXX+1#>bGeHkMlvSt$IXa z<)+Tg!xGA(hk6meRYj{EkysrFDYh=-mU{<9CqL2_9>>Dtt=Fr^Lx1b)JS?FsdZ-tP zGsl4_xvqLVI1o~7UB=~J&W>u{(FPv8O7{J8Q|gR`9h`?HltmBqB5`IV&R`=D4FVy> z)@8h+&+zEKQ=7x%4iGs%y`1`At(MNi63U{7dXYF&3|sXbh+i-DgcMtsanIjJMr&TL z3y)vm@#r&0rsq|ToQEZpMGy5N_^uBguY>3o2r0HMc_5 zcOI5d7CqF9#F-aCd=H{QAf(v3j2leOjb_$393C-)7CqF9;IEKC zoC{)TAf(v3jO#ZZ6D>b!HO7+JAZFatI=$z?r<{i+ltmBqBL4m$(c~ddNU?PpPhCGI zI_t9Q;n4*i0}ktyuJT4T=V1wD(L=om{*nVkUl2zHLW-@+xM!KM(ae)i#@Id##B(*e zxgN*XvRV4vcPv8-A*fhF@#xP>)Q&SGdSR<-zELe@YuPM*u3jY0+yY_{h&}5(A;l7m zc@(-Vm<5k(K`g}4E%DG(&chPQqDNlGaxaLkI&SLE63U{7dJ#Xu zJq+T)KuEE58FSR0*K`0pzK2II%m%r?b#)$=P!>JZi^Q2fK$HP7IuKH9UB;Y!CUhMH z53aA?!92KjQ)lO431!hky-1uXi!)fv&x4MTV(T*IEV|^>KJbV@w8ebhp-6MYyoQEZpMGy5NaV86U-XFy5KuEE58FO{^^pf-8@ee#Mz^ZT5 zou!L%oQf4VHrF z6bLD{E@Q5iAMCRh;}z$RjaY@}Y#Qr4ETJrV)1I&ZBf;fI`qvUBzdL?Wvo5j!7i^Q41 zAoh-FlpGrfDVAVdb4~Z?i63%SdE$yEYbQ?{`m*z|gtF+7*RgP`+JX2p+Y?f3UB(Z5 z-6J}_^ICXx1u^9K%E@6H4@=rwHjAID7l|_iKy(L@69_4mV7#MaujsQbhrNWqxdZXt z#1hFPUOd%#SVCF!P%jc^cs`2VQ6l-@3QtI}bs5*I+b6na_et>JetdEG#>CDo7dsD2 zD2pEIMX-hg(FMe=KuEE58INt#FFJQzRd|dCF}MAliQk7@={ziwh4@)SE9_mH#n zGS%+HCeFhW%A$vQ5&XRmhu zh(G6RL0l0CDYh=-Z%!H+jhIpy9{0ne=3{@RmX$rvd00YO^iVJ2_xvOfZv{e%t;=|K z`O#6CZ;yos&tUC+<L=%HQ&eJzM4AT9}n6kC_^eOQaupZF*|jsdZKSN-&aQ)W94ODKyT z>P6yAeGp%Q_~v9!NU?PpznC>9dL`=;c+>;Yt5&mgpFO8K4@)SE9(f&05=3th{R1Jz z)@A&89sFyK$G71xwk`*8^byy&9>>u z2%ZUpCgNwfNU?PpbN0D%E}u>CeB6S0@bN9p3lPephk6lz&Z~o16bLD{E@RH3 z=ak~VkLn1Ir!b!v&A!rkSVCF!P%q+VgA*{Hzc<7aQfytuTn$XS`YL!l1dlaXWo+7V zvGcHmvgn~+1pPUPP9RSG+7nW2UB+D1yzp`jc(e!cE7n)#UOd%#SVCF!P%q+VoIN0Z zTj2>Qwk~6?&aOQFBzW|L#|W(YdTcz*d00YO^iVGnXWD@n3}Rd$q}aNQxfcEY*S|5A zv;ol)tJm{~zU;;lODKyT>P7rkT@T{>Y)?qBbs2NDydY;8Jo>_;7}o7)E$QVvETJrV zV}E@Sov^Nwo@4-(_iP2@Z`FSfO87C+DHSbBgM3*wSMNU;QCbuV}( zJga8m&< z$=qh1kYejHPJh!os(ism@OTTv&GUXvw5?boX=~Xmey&~w-#UZ17euQ-NU;Rta((+o zHyy)&^Rp7f)ZQ;7hMiW?d00YO^iVGnXPyS}5{RLJkYejHPF&nSO7|-bkEI}PySaB_ z(7^MYhb5Fn5A`B(<^>Q{Knw_k6kC_^#!3UD3g?~-k7r3tT_0U`e+}nh31!hky$HVL z1ThRm6bLD{F5~ad8ywaC;52xUIPvtO;`Wza>^v-?EPALH!Cwx5Sa#Y`amPSNv2_{8 zmu5%*HaHa?^Ffq3@x0W#*H(8PmQWTw)QkA@(E-HjKuEE58F#pNShV&|{%ei}@c4T5 z1F3ZJ%Fe?Q%A$vQ5j<%C@ivI{fskVBGVWe>M0DDY;_$#Yl73|P#?&`&mUAAKP!>JZ zi};sA;s2Z{6IW1I(*|#xIf;5$K+;*r#pA~*WH7bP!>JZi^Q23Ao_x6)YTJG zY+c49|2H~%s6C&tJOEq0a5_ z=nJB5-*eL4KA+$`ETJrVs29Qa)F7&ZnDm(^q}aNQcNWQw8l_9aV;+bn&%P*q?~Ns# zhb5FnkGziMUl9F4tZwBADYh=-`)cM!zZ{>79_Mcm&-cFA^*FYc&C=(-W7$~SYHJ_{S%)RJp|%;TbmsU zI&o$Lh%UHmm%UjoZEM*qey(1`pO3C1Hl`j4gcM6K<|r{~*%|QQ=(7f+&xYcaorfiq zMGy5N{tUhcVq+kr*t(24+Es7P-%jAT+a06mo7Yx%9+prRJ=BYMqK_e@*t(24YIiPi zDLgnEyzgfN=V1wD(L=pRoOu#Nm_HmL#nxra+2`?@wcxP`#C*(Z$@^;*Ae2Q9^&P7I4Fo?1s-VcNnTbD6c%khqx@aP6& z9oFry4Qb*$ETJrVP4_Z2JsGvv4N0c>oP9&NS|opuzv7Z3}V%!?-FIo zFLxf6P!>J%I+i&gir)2I;&qfgrBJS?FsdZ-uiTU8#!F@ca`>oR_) z#(?OrnxDagTlMDC^%753Et#~nY!*LPFM_|O;(k0+FY#<3q*#LS)h`T;I)B6UF0b9= zo?Z|wexr=@u!OScp-|i)jSa610luMW!&QY5z&@kmcgSRh`+0Sl$w9#+s?xh z%A$vQ5&T6wh*H%*N;M0F6kC_^#)c!KfyJi4BNxOk?`BfdE}!Q-ETJrVs2AaPmMzk| zL6i%G6kC_^EoY93zHM*?Jf?zp^`+wJH~zfId00YO^iVItCyKbEKrH>s6H;tl#{aAw z6;-+5ZH&8vKs1|II(_`VU%HXn63U`SUdPfN#H}E<@AiZgTbJ?q#YRVi`>tG$e*p?& z_0{EEk7H}uEPd`fmRmr)l#ifd3B@JLyonyCBZ$t}s>}ZUGPbpB7C%=n!tbeDq%Q}t zYPTn(Sb{N+LY=3st{rD`Kzxj&yW`KBQnr@O;^*o`_++$2`biKI{_=zrOEBiyT>I2? zc-#wO4X%JZi})GmR@~>81VW0f z%b26Y8xyjyPUQTtP7I)1c-i^ zKNbf`Zf~W-I&Ok`9 zbs4iaC|kNbJRSw{FuI8bd)v9O-4e>8M_$LmmCPy-gNw9xgcMtsvAP#LiTj{yx?9c7 ziQB4Fj;!+ekXdy*|JgTcf*wb-?^woxxDdp&3p}A-#uAKo9MdmaamhS<*GFP+#|?=M zslS|uC6q-E^&)us3ZibO4T(9iC#2ZAjPJO)Uv$J%OE900260izR}yVXUX`%5Y!*LP zFM{u2xc63lmXE+5K?Si z#@p`bADyzKF+6fX+*$kX#JIB_a~_sZ7CqF9U_J-&HHc1ukYejHUfXs+w6pCnc#H(` zLbW!DMETXu!xGA(M_$K5kK?PiNz@926kC^Z@1_Hzxu@O@k5M3=D0^(8XyRSxVF_i? zL%j$_Q4mL$IX1B?@`Myym+|SX21OMnJ_wIHNi@uk_WiTTd00YO^iVH?zfJ(L?27DY zZy==Dx{O2je|*MySfXUeqKA4B z^qe3TgZSeIPe`$K8Mp2?Ec$(IM|eB{qD{3qsc6gn&chPQqKA4Btgk?H0&z+pq}aNQ zw^tt?Rcdr0JZ6FD@WS%co11z#4@)SE9_mHVL4vpn#6KT_)$a^PNU;QCp3MuMZUc{du~j2*m6ZFqM*%`v^iVJ2uf(%JObUb)TbD7f z%Ffj$!Nc4i&chPQqKA4Bzg2^9e^lLazaymBx{P^uz1{m!c-({iD1p27iXWdTKq!kI zc^wN!(PJ_rQvVBt6kC@uM~N1TR={ICi2fL_`cGZ!JS?FsdZ-uiBisgzR|5kf#nxra z(QeV%ufv1mZfA_5&y@bad00YO^x$~xI~EexgLpa+Qfyu4hp~M_yY*o|_G11h_WNe% zVF`}y%%X>S5&U%uh~M*(ApZSl zlOv?qx{NuCZY%RPJUD-3V?JLWy<31#7CqF9;9n1b*jxJ8M0UaxQfytuTn+SJl!nJZ z5HqnZ=~jNV^RR@n=%HT3uVi+EI6DwhY+c4&)vT|z6duFiF$=4uS!X@wJS?FsdZ-ui z=i?0!$v{Z4bs2MY)}g};cyQI%9jm?{FQ4E%ETJrVs29PvQ6TDr7*yXAQfytuTxDkc zoDC1I$Xa3b+OFhP&chPQqKA4Bzd~*cV)ap;kYejH=9+rpPnW@itMH0gg`brE%ZJZi}sYww>{wp9z!Oqz zUB>EOu*#^BoqlxK_{8|0Z`HNRark{e{rZ|{-?7XCaXEQZc7#`=X%e8U2w$;B-a7f!5{u(fO!KUXh;=TRVrf+!UTDVAWY-vq_CA0S?9 zTs84xrAE%f63U{7dJ%lj0Ag{Is)=g@A;s2ZtX~nu_Y5GODpxddLQWUwVF_i?L%oRK z^DoL4O*9FF6kC@uf5FiIBIzu6e0So~sP^~~&chPQqKA4Bzg4p;ERAXgLW-@+Sie+? zwGTX6jV&LQTsh8pSVCF!P%nZN0EoZFmyeDQgcMtsv3}bWD`XIfhp$;Hg)@7_;L&Zu4#OnWs#{*lAaUPaX7CqF9_^nFs86Mvr2r0HM zWBq=rKf14-{zcsMqyf&u63TM>vA<*C(f#+dFXD#-p)SSNWt{h0tUOkK-F8yy#&>V@ z`w?o{EPALH@z?J2ARY;X6iYDHuey5T!iVdn_Ab8Ed00YO^iVH?Z)>qtmx0(32r0HM zWBm@S_c->ucBxU{mU14JP!>JZi}<4(f%s{gC#2ZAjP=X0{tOl?pPf2u(^@xPSwdO# zP%nbtJ;#20eL{BXrOlp@V(T*2Z_Rq5`%`06_hvV89+prRJ@Psh9;=QZW)1d)6kC_E zex25P>?(J=>v3!?o2Acv$HM*iCm%t@5{msV-EymL!B*vDH;Qd7o5j!bI+lAti~zBC zuqUKgg0X%9*Iy;Ia6U$CTI)P4p)7i+7x8y(HqJ-?&7P2A>oV4F=KArf6t0q1-&Z(UA;s2Ztl#DJ z#6P%eo4s_$;h=VbyR#nxra)qwv!WUhT4z`A7T=q}E~63U{7 zdJ%sH_hMZ#Eyoj5Y+c6son%jZi}lqfl^Qt@ODKyT>P7H$30t)g>#G;e@`Myym$80X z*%Md$b)xgIgtF+NUc?jiK-8aD$q`a)UB>z?X20J3)vtHmcx4G?(L=q6C;kL+-H(4d zLW-@+Sij!viRoB{ciVY?0YX{yP%q+-?j#Tw?(&2bTbHqZ@7doU{n1U-?)lbgTgzs- zV-=pa;5!+dk4Hel{;BmYL7 zLlMnGmgq01Bv$R;s$6ZA-4ecmL0h=|?g`QMBl&lh`8}lM-zUAPZ9lQ!!xG9O{nV$o zM~8pG-yO{FA%$6%$a{xdAohD$LRqBm*)~3^I-9?dlHWrLvn-)Ewe2VNdssqQq$@u= zF8XW2-Gz8aVU{KIrnZ6}mQWVyhprhLy)xyIJdgcbC52g*XuCW&YW3r@c|;cfW0sWs zgx>8|aH}k#EYj=#8x@VNoi4;f3bQPsce@qzu!OQm@4I<;)TZXTLOi4}%MyCGTR{&? zD2sHZhlfO^7QS_$N3K;ETSD)4+wZae=&BY8WifvAiviJ&+&2sHkdmL!o7xI`SVCE( zOP=31+PHR2As$kgWr+#%dq!0rTzMejt`aHv3BB9xfUR=J$`Z;V-Ku4`Xy2tz7UCg= zS(ebd-3oeGLRpS()j4`-*UUmZt_cdWETMP174)!#vPidEd0RB%>T!j5NMV*G^lrC; z9+prR=~`3UM-y7|H@5PRS5lZ|3BB9x0K%OQODK!<#;vzT?b@Gqpob%*Fv}7by>d&` z{%?M>oqwyOpC6q;) z^ZaA)uPVet3bQPschnW!4@)SEG}kNZK7O_k4=K#Dgx*nC(8ChSBF%Nz?1@hl;vt1u zme4!u3VK*VS){psEP37og?LC|mL<4GO;j3N2q7gup|{!<+$u{bi!|5TYc}zBg!7+s zDa^8j-fDMfLRqBQGt4Z1W+5I@m}LpQ2d|)qC6q;)y-#NBhlR`rQkZ24c47YQr}_8X z63QaY9`1ndZvTuUg;|zRXUYA@%3YnzNnZ8pZ1;v$^@IOjIO4!Ab$`zpwM}1W3H6zl zV9d{3DN0spi4R-ch!Ar{(L=E( zzPqkU@~~5r&O?f=%UJ#Hp$TQtL$N2${$I)D@+~(w4=J`TWA(d-CX_`F#hw_`|NX?q z2fI5DDYhEn4^1eG9*RA&^y^dNAK%My9#U*w#_D$uO(=^Viak;C@A|1aEk-yGDYh4LlerPhhk5>SY}ZAfe{Zm4=J`T zWA)UBCX_`F#h%!;Z)AF4^HZFM6kC_Ey6`+=Kp}*(=)rhy=;*U@wVJVtt*co5ITtbR z8}!`52xjT?{X{PJqkr>LQdY5b8FT;CpJ!zqj8GOUDRz6FyBz0Z#)yZUhZI|vG0&6w z^Q^3c5z3;6V()P*uB(C7wmJ_fwk~5{Z|dl?vJOTliyn%-2k(y}_}7zGv2_{qzEMY? zm31&eS*)bkdxZPkc}TH!8T0;EN1v5-FhW`MQ0zT!#&|V)W@G0e#nxraaZDY3R@T7? zWzj>i_jnEC?z!9BI}a(gE@O_z>gcnw4n`=89*Vt3m_M9{6kC@u=M{DISy=}oltmB4 z-s3#XgSA?Wa2`@@UB;Xr^&LP~*1-s6(L=HKSikj@`1|*AoQD)!moevgeFu=0budC% z^ib?PvY!9y$7p8r(auAPt;?9}6@3Sgbx=ZC^ib?Q?#245)ltk+~7Q<*t(3l4%T-7 zc-!hupyD2pD7J;8o(P?xkD+ojmLj1TBivkpcmiyn%-N9a%J|C4GFem@MMEYjZ>8y8*vF8}>`{{4``EKBe_?SE2zfQKcNMf$&H zW25oconEJ)hZJU6g4f&rC)EddSVCE(|Ha!j5gaS zM!ApmD#Swyvn;{;fB%zedAKuZ31yMysI>n{wFq|xEukzDEys-;GR1|iml6-v&8-<)$(v#WeH`GDAp`Hs(RABg?LD@ zbs2NE+yA8c01rzji$v4XgAU>$#nxraS$qGJ>H|D1p)3;1w)8v5v65ozGUn=I|C8zi zJS?Fs5?mAQe^Px=LRlpCecJmVSBVr`moe9N`=3-F;9&`6k(gYe=RxidDYhnc`Xd!R>tLY*+ZTOL-?KWR8%&!yP9jMcpsJcE`{76~5pUiF_X#6ya$ z%UIoOK@UqPi-he`58QK0D2oKIw+d5=y;RUciml67z38C{Ws%^0bKS3eKWF|mgA`kr zv3k)%6UrjN`~Uc_S{C9V#nxr4UbLWxC6q;i&hpTNvPf|58oJ{4LbghZt;<-Q<$jM`E|G05o5h%OaN!4gqPSHdzh~aHKA;l7mxAqNk=`- zG#^$pF$~1jKY2omC7!^3^#5dR)Zo=6@VEy=#fv*8=UnlP^AKUP=%K48&ipj8Xku~A zj>#PjJt4&sjOSJw8{OCN2#liJK+KwaO|sU`6W#YEmQWTwbQj3y>*<2C6q-E-T(Nv;UIEBoVSc!>H(rncd;>CW!UzS0oNw(#v^RLRs|C zc#OAN;VPL4qF5lL*t(3{+%`N~_QMEx{0@)zr_@e#A3e-@SVCF!(7Y07PJzcF5IqAS z#nxr~A$|iWcMkvF=Qnt81buD8DCc1bWzj=3Y96sR5K?Si#-l3?3Lf*}(P2vM)PS&6 zmQWTwv@-B}z8FOBKuEE58UM4pUpQ9vu;=fzUy<7Tls_MqP!>J3!ih6Yu~pqboD|N7 z6kC^Z&tiSTIo~&;XyW@hMbquWRbmNc(L*aW{F`cc95J_O`l>)kv2_{GZq+kfyV)RG z@2!%4JY4OTP!>J3MvXH~Mi))Y0nt1VQfytu#rAd!_u$4Euk!WXssP* zzQR`B3gXT{NU?PpAAfvTGnQCzxDUklH~Cr363Wtk_-|5IgSZRCzP6s=R#CBa z<;Pjn?|EbF`KX^JC2TF5#m{*R{WqzX!s7-IjRPUY5{!AIOE>)r9!U^i;|%V(;v45- z31!iP*Mk2hbt|r`pFwO6gcMtsF|U|?M^&t|e~wCHj!N2EHjAJ0n)ctMzK*NCiNAIo zA;l7mdF8hq+YlbNgJ_I9YR0{F3J}Vo2k%Y)O)BTX=RjN=2r0HMW8T4^KgoAtbB_8N zW63S=w00hrP!>Hn#`y0=&jhgtM2kR3v2_`9B-&JU3_LiNe1j1VbCmP2gtF+tvC@Ap z`Yz7kl_1InLW-@+m?P?4ttY`_42ZTEse6nb<~%H+EP8Ov_uq?h<}7)4?NqlwNU?Pp zb7r_WYbrcAGGL0zU9(}7^RR@n=)t+mdz|cvED=&{UB;Z@yhlS28!&Tft1O``dT_--37?#07znV(T*IO4g4h zUxPRTYw9s!^s$7p=)twN9}|~g75+Nb)PFSeBb*dlmoeAveoPz)qB>UkSN!b9T}vp7 z9_$(X*#0W6lIuZWj&h@@6kAt)f*;$fmCH%auDK*`)-|hc#d&kR~5wb%@)Lax}_W;B|j1WG$1NlaR&%`)LcF*eyYsL5o<}w zPgJThFsjt?c+3VQT5p&Xr=Muy2r2oAv7-k@ZCYFyh;MK05uY?A*AY_k6aRcWFnaV` z{xUZ`+EltU{$<;AM@Y#}9Cpi~sNZ#M1F^Zy?$r<1c*GG>@)MK48Wf$9=?a3!s`~Pr z)&2G?c7&AtL>2rV+4RnRf(OT|XZ~2?2q^`Kot7y2Vou#&!J|r0@)PSu4>4PH*^RsF zeh~IUN`9i{VcBNSajYU8D=GPjwnMYS(dC{$vSCtm;R1gKH++@t&QyM)YEArVwmat} zcE2zy`n8jh!Tp&2LJpqbFLng4tBGG9u_`~oEA*GE8J9F_d}s172GaBi!l`t`EyPWj-rdo_^U)peuATG-gWf?M(Tat z{FNvrKfw{)U%T|+Y|yl8mb-SP&s#F-z8%}6{wZD6|kwC5wvGU_=hUT|mKqMbTN zEisqybM`xa#+iKNjKoe5Ur+Lc6iYBJf!?6q|N3K0oCD(ChCS0=zrHrxdy~5Z`MdH>)+3LSVCF!P@jNbRt0eth^2v$V(T()T6Iu#_MVCG zXa|Dx`46)OIS)%HiyrC|_7hnmq}aNQi&f7K9*z8dB(xuvP!>JZC&ZajAg+J2M&jkL zA5v^x#tSM9569|D+=H*Ld?wL599>H&iyrC|@Eh3lSoKWel0ZnYbs2a5V`Mnz^Ff@j zHY+(cTqTxJ7CqD_#F?u>WM0ckJ{AZmwl3pMxjEt5MaJ5@PS@ck!fbXe6 zybj{=KuEE5UHo}&xCe)U*ifTE^5bs)K0km6S@ck!5ND1B5ra57+~-nkUB+u)8xzKo zk|1{eeN*zy(tf?W3BtJ;8g8-!JCUB-{0k{{cj2l4W>fyoc2JRjvJl*P~Up2{=_@dk(iQ#~QY z);$Ft|NC#On|=JqupY!0Q=X4)Et@58|IOWUc&r4`d8#M)M2U(e5sxA31ThI`O~F8EkGzs*Oh-NlZCCi z4Cg#9??>Dt@h|^RR@n=)rr_Kb7HKn+EY*Af(v3F2;R6b3p@mGzRfMj3pCx zHFq8d5Fv{m9Ao@bnVBH=VJw*z2r0HMV~#|-{>2xx8SeQz7!zBR@8mozp)7iEtn^Q1 z{=z8Q7Gq-5KuEE58FNJ4-nI`s&WA@gjMOjn@8>)$p)7iE%=b@aE(OsP#L7TOv2_`9 zX4rO7b~sjCh3}X($az>oS@huClmDY+c4&arkq7ABYB6 z$#e}@i6xXp53Y&)waa65E{O4gkYejH=E})myPVZN!HR5dxY{kDEP8Nl=kGxt-EAQ5 z41^S0moZm>{vON$F&Qh%YTf;PZV6@4L(f~{%n=}lfanznDYh4I zOE6~t;m7s|Kx{+D(t6tSv8`pZ__=zYIP(X#Dg|Q2Bu_}O1Y>nb_$|E?zDvx`?wI~_ z*22gtuMNMu?xxjUq88|*MEjZZUJwt1SP}^JOO{}~`@b$xt7|);$9WLM#24G9_mm!< zu(fO!KUcpKXHF>pU7{6;-^+MHiX|A=N_UOo$!pDT%H%b5a_}k9I63U`SURTr; zTh$dr_drOobs7KhPq%2&RY%o{Gd(~|YjR2Y!Y4~4Z7rL{&(-hXecQ)>m)HWLXdt9m zg7MHtdqg#JE`Z0cAYRyBA-(7QbDW1IltmBqJNUZ{Y}GIjr8as(iml7|>LtCRmbDwh z<6022$N!dEbXk4pVF_i?L;ViE%K-5fhy{U=V(T)VTeNTV(%{?RaVd!LB_B_fKBATL zu!OScp?)XMoDE_Ih%$kYV(T(4F|&X4O_u@ir~~5K@BWwSG^M@su!OScp?)XM)Ba_lyoQEZpMGy5m`-vcAo;# zXM2U@zv2F{gtDB+{;uciZr+}FBV%y}tFz#AHS@cl9gEL*$a=MApV=TFwRdXOZ(wx&Y2))fJn?; z7+b~Gm0z2|d+;4HiQBQ~n4?np3GN58___KWyki8!Js{o-gcMtsF^}{ghw*)0qe0Zc z87%q4+s?xh%A!YJSM(c*!$EWjgcMtsF|U|QLl1;oRFkYejH=16qyGmYSJGl)_c6L%cm%6V8q zS@cl96K6gEF%)CscY%;%>oVqunmr$XjG1`^M8c2M&chPQqKEn&zsh(6#Pw6!J3@-B z%a}7m$G8JLI>Uoq(TV*#6(E#F5A{27rc*vbiml6-Go1Gbb5sg*l=HBJvgo0HC(g8j zM+?lgw**3pt;?7*wLez%K#cHf2IpZ3Wzj?ZPMm2CVk(HDNBA=+#nxra6^B3PoX=mv zO6IX}l~_Vq^iaPOXSnBS5OV_|#nxram6N}Axgwi|71^KRYPW>4=%IcGJr0P8AS!P3 zca#)cmoZm>{vP}b>%^zAvaI{0zt1h9EPANl!M7jqcp1cR;XapQ>oVqg*N-LrK_sw3 zJ~oU#mQWTw^1339)R%*}@o_)GNwIYqb4BjQM9$7VvC_wkUD18;7y@Ef z8Ba*Dbs4k&@MHUQ5bRiX&hoQ?C6q;vysn6MExV${fskVBGFFEaXMS#ZVsg`$P0}xa zHY2jintIdgo;j{t)Es@3Xx|k*2cq~_P0}qt@r3#%OE8|ax?A*R|0d{hmV@|UTf_9c z+8q+Mmd)bl>UZMI`yiHsIK7T1q*#LSrY_y1>Lr%J<3kYJE~u3r*kgtBu!OSck=GT~ zz5c}HEg)+5^n?^!mvPOJcnZAuZ+KJ(F}QlA^uXO2=V1wD(L?=CoLP;nI_1Jj=@0+) zgcMts@kM|3jIO!$q&jhCEr|KEk4nFO^odDZ%VzO&^*eE94~V89z6gXAOE4~#?j5D? zsR@riK>RarbE?WORh@?=ltmBqJ8|Yc5Ir8=oT?lMDYh=-iW~Yy#Y*zuRP%f^c;dd) z>z^i_hb5Fn5A{2EG793q$L~w634|0|m+>vf4Tv5s-x?mDgJ^zP`PB1?Yn+EAltmBq zJ8|YC5Mw~R7zio0F5_ZD2Sw$gTZ0GtsEO&O&chPQqKEpOIP)buIIGUZKy45BoM4S|qi>oU$X8WxV#Qy^}9;=aT;;pkdIS@cl9 z6K6Jocn-wVfskVBGOl^|h;Yt%*B4=#keI4Fjj%A$w*ojB70 zLkCrfn^o#n096#F-@^9s;p75K=6`n0N5? z@0<*e6(G*WSkmm5s?Ng_%A$w*9rQRLHef8dE)Y^|UB(=VW;f)&MGkXR0&|q}u!OSc zp?)XMEP}_|7!%hALW-@+m?LVf>k{xtgLnrc_1eTW&chPQqKEn&KdT)FVnraN*t(24 zGc3w(2#-(U!5L>#x~cQ9gtF+NekacGy0U~6TbD6sIPbx+y%lE8)nThFp)7i+-|_R{ z-5{0)LW-@+m@~CMRvZ~F#hT&8aC9x9EPANli8C8P90g)yAf(v3jJe|Q=RC|&Da=vs zSXn|@^iaPOXa0gmGpv1%4TKb1moZmP{@UFI;*Qxzr5A^*-4e>8hx#3Vm2?M@b&S8G zq}aNQxlZ)=;H&U>0c+9E!d+_#Wzj?Z4*s42tJe=f^!V403{q@e#$3tzv4q5YtdJY_ z@Z*&wltmBqJAQUZMI3m`B@u{Urd zoD^G^G5ZfcwzL2E4?WHopZWR263U`SURT6f?K2RUeBucywk~6JNcb+}y;{jOovNoN z#$zL^oO$z%y5BGA5j900CE9mI!$8yrF(>5-^-Gpu{M#ozqNSB;p#K;FqSb9x(%=4c zdBWDRS^Ql64ti%0T|qn;2q~6e+_PKHs9lwr@aP4i^Yl~G$sKc?hb5Fn5A{2EVP^en?Ojhbs5*5)H~{M z!QplA{y`8IKmAK;>kmbewwBG}=jwOjOn(rYL2L+w6iYDv=cvBX6%{JLV!u0Tk!bs5iH+bJZ@8Ivq zK(qpJWgw*3x{S*|IUssy_r>ra@nwx7sXk{^bsm;b7CqGO;GJP0ri17f2r0HM<4+e4 zifaE|CwO4S$$hZ$h0eng%A$w*9lSRj9-N~@NU?Pp4}We*@E8wbc#R^7VPUH*p)7i+ z-@(812Jt?Ku7QwZ>oTtU+|Y2WcwHs`$w{0Uj;UaDZTmqs(Af(v3jN7yv8Lr(a@VM*gUlMPHtKAaHqKEpO zIP)Tibs+i$LW-@+xcrBs!acYG#0R$=lYBbdwU$s8J=E{SnX5o71u=b`9~q?Bx{Nm; zlM}|0D?!{b{nX@bVf3+tvgpC}wg0AQCWx^hcK_%{I4QO+oTrhKR1l+qe0Z@R6V)t6+atTLRs|4>xym$aV?1B zUh{+$TbJ?0H|4t72Y)FGq9usZua1pvEt{qN@G~d(qY8+oVpQvuDf0@MsL;yjzb+)*Abf^RR@n=#keItpd>$*Y4rtJR!x_ zWy~x8@Avpli>E-e!5#J74@J_pmd)bl>UaEoz5~SkKuEC!W8T4^w9I-5Pu$`0E=Go9 z=A7U>ETJrVsNeBp`(i^#v2_`9Bq}ku3_Pa6<4TNh|EpHnd00YO^iaR!=fR8onCJ*8 zwk~6isMRm22oH`|<1kWVjw(PXiyrEC;>-jPZ-5vS2r0HMW6likRp;MX-UWg)&I6S% zbRL#a7CqGO6eOhBx{Nu)c@JK@-(lwL7`Dn1%A$w*oj5ZQ#6%E70wKlLWz3n{A1e~) zV+GJSoDWMViyrEC{LyU+;_^U9v2_`9#o^C6*CkD`_W5SEzpgByEPANli8F&ioCu;+ zxDut;eWHLOizzVr)7_Tg$EPANl@vHEwK&%La6kC@u zSLA+79 z!T8D-@QkHNHawPs_^VS^`r6f4E1Ft1i=V6Ci8D`tII442x@;h%Sb}lq?tP*szgP&5 zg&;cb*p|BPtEZfYC6q;vysqdC5Rd+}EwwBVQfytuKi=Fodg1){;qf|%vh`m{b>91d z^RR@n=%Icm&b$obVi0WtA;s2ZJZWOT=%Si`!XpJ@;^8w=4Zi);d00YO^iaPOXBL1s z>4+JrO9COq)@5AbtNzjQZ;!0w&*0j6%~MCFizaO?o5j!7@8Dl3z#|t#(LhMC1mo{p z4~&`@Ee#J6vp(J!H~#Nv=V1wD(L?{hYnI-jQ$2 z5)I61BUQ39Ej-W^9h214Ao3+m=X=&I^0nzJado?h@jOY8wLCU2E3_qoBd{j=Vc4#I`3k1fIHXdlWr7FYL}Y z40e6+eIXW~J|5_L#P)MdFbW!6@5owBh%bfs*7b9GbY9qJFAjEN$!Z~zJIY^kd~U}p zO)v@?T<^#Zy%3%dvm8N>&I>!oLiy~@jEQ)HUtPR8SjvsNnqU+(xZaU((h*{Z5ThJH zkIoBwPQjsWY+o$IvE8|XT{qa-Kog9DMs!t#{_%qle}*kVkIoDGRPs&rR{#CLM#Wi#Co(z$dLj&r?Z`^PpR zIyr(KO~A&TzOPb&wRU8vAy06`=fjMKCKv?`u6O(hMurMPtab!FIxlRrm`~?*6OFY( zbeER+_>m<>LlcaG2HLc($k5s|ojD$e9JT~KIxlRr{9oq$Pc$%d4v{`Mr%LC?P~ON6@45!bSyX`yl#9VW~w6yI!jaMnQw?9b3I#kXrP# zBk0k2VWX0@V+m%1f2F3bvEGhXnqU+(xZbf z1f!tA^^Ux`EW~snzD}|PJvuLJtUv77eo%;ovSO*R#?A(sU=%c>tD-4FTo>Z>7E92h z^TOr|$&Vyg?Hqir!k*CB{Y5 zdWDAKdx{jG}geny{X*4v!C}>1iMIdSmQP~mn=)ABeC3pAg&#qk6YK*+RKD4u3 zazN+OQ8>=^jvvV@8r`m}4?X;_CFs!v?6+5U_wu*vD;l>6@m=2qp_1K)8VyY_3L0GR z_>sGXcuI&b96^uH3;RTy9^U2EQ$^!WA*xiK6k2n`Y@?wGMnQw?9jj49h%JtwN9Too z=>DGG@xPaf#yz5OuzK6jkU=Yqh9(#Vjp(YVq7a`{Ya5#82zqp0*rN*c@{)^xAsP<} z@#v^Rq2CT{HX52>6g0Tr@gpULNPewQ=yylZqw~W4`PSavhwp56*HQNVS$@e8JB@}W z7zGWkcl^kGqOteBS$;7`(4+IhE?ugRH~0)bgT%eU6xHzd?~H~f7zGWkcl^k`mPjH& zkIoDGzAAm4Mkyh>-8aj7z@3#Q7zGWkcWi5Ka{nwZt0U;qd13deE#Dhu?iHTLsZoUj zTio5%1f!tA^^PC8Lx`_N779H5gMEVZ=)ACZ)#>k^d0`<6)@U18>RO2=7zGWkcjS9H zg?L$r-j1M0=Y`$(kpZsl787DqrAdKmnQZ^i1f!tA^^W{aoDelCPYOKb`UgEaFYGQ! z16>~k(W38yz{{`Ney#~dL4)fZKY}r_oDkDpKc`3Mg&qEApc_j-{Be1GprjjpG{Gon zaJ?gM6$|mF5KGJ2@roXu7xtz*2e~n^j1Wa~$X`?4X2)GkFbW!6@A#3dLNpYj(;b$e zN9Tq8RM$anY|kY`xkvT{E`4Wb15Gdr8qrk|#>5j9_5{jow*);puhAPl$bDBBTFEa$ zto^QtuXE`rUWc7IvkTEe&T8~_OW>^F(FFA>)SDsS6^84WFIO&q?`1SJf$KmNG`QZ8 z_hf_^oBf->Q$;O7kIrlK({9fjju@7Q+ruMmHhwFEty zfQ=UOO5OZx<(t1mW2m&m4_+N=G&I2|XmGtFe`PAfKYbPip6G4~dURgcX!-S8))kHN zLX4C?_|FZqjfN%|1r4ru{K&0BY>_@V#u4=Bys*)OE9~np8byV;O-6=~2CXm}nqU+( zFvi#lxrh+SLVVx|dURgc7>PPhnIsx$SH)f{6!_=BW}~4AMnQw?9ea28$(VT65%lQ1 zurZ?U&pt~uN{dD_8Qbp~vD0X1f>F@mddJQmp9pb}Bk0k2VPj@k(0-0+aK@>3`*%h| z6O4ie*E{h9JvuLJ%y3rYez}fC&;+BP!S#+GDJMj8sTtmP&x0PF7d9#m`^?dHmr3pOtZOBjU=%dC-mw+g z7O8!%ID#IX7d9#<+jh~e9+Qe}oonryU=%dC-m&*8AjC>X(4+IhMg?g5AnL@$QdypG z{ah1_f(F++ex!g9uL+T>hwbO|=)AB|$=b05_3mz|sgujw(MJ=Ef(F++c8)qF#E)*g zqDSY2jk?{AiKz7JN~M2$0XvFnf>F@mddJp2^@Pac2zqp0*jQQEu^qL~Oj)t4*l%Y8 zO)v@?(N$4Uhz>&hzS|P?=)ADGLXuDWR`&}o_#h?JZB-~)d+@!J{8m5Gc2)F-5YvQc z`ne^z64C^0eh;i40nv9v|4^mQ6Fr?vN8xyMRrHn+t%Mlq2zoRDo8KHOpSTOrt4^oT zU5!r|4NWi#8eH%Ak*Pu)s@*Bn>sd?Cqw~V%cg@N>cS39}(;&3_vmybVOGn{2*E{ld zn-IU1Z4f%M!V>go0ye*$*50ev@{|d^em-C{G{GonaJ^&Cs#)$bp;CWZf*ze0HownS z-hhy^3T4g~dSpo}qoD~#L4)fZOH34^h9l_Fd13P#aOJHq(YRsBTL0}c-HnDO7zGWk zckC0Kxp=KV%MtYGys-J5x%R9so$&mmD?^NiCKv?`u6OL+J^!2MXLkfWIxlQ~%dULl zF4uu|)RMBpjfN%|1r4ru?3H7$Z>b~b(Rq!X`i;HlgWFGfURifmX$Thu4X$^rMwL^Z z_xojg<@D&hu=!2C_R1?PTI=2NhkYKJU=%dC-m!Ogh!7Xu^PorPh0X8&wP!UeOSZt; z#kO5(f>F@mdPlxHL9Tp5=4^rgxpqa5&I>#GJ;LLKcpz_?z|*dOXo6AD;Cjbu3>IR~ zIom(z(RpF-a^Fa7i85sy1d6Y;y;c*9f(F++u15*+wd?2f=)AD`9mcj@O|H`^P~DAJ znqU+(xZaUp5|As;B}DV)cD$lT=Y`F0J(hQ)gvd9de_(buJMLgb zyK$EuofkI0C)pAcK1c~{``FF~nqZW##E?Mjx4@0wWh$f-50SP|tqTErUx<5flHqi z3F%xq3dgzLv1e86;Rb;Mj-W>qu+j4EcNL@8R+b)BF@mdMBQsN9Q$m>i2BpUfm&cZ3%Z)X$Thu4X$_Wn0Q)sTqc!vCl&jjDiN&JNE9blbT_zBk0k2Ve=cit;S@jWS(`cL=%jH2G={b z4;~aEyCdk)d13QAzU`RUPinX-u77BPQPALe$JUASrG^{!r|lo~=)AD`t>AX9y(YEj z1=nje!6;~Oy<_X$u~LhEw8D-I^ys{>`90#6s3tXac{g5Zf>F@mddHqsAtA!g+VP4W zofkI0dE7q1SEbU==SERYFbW!6@7T86RfuVhphxG0&F?C=#QU;hX}`+O2AW_LG~!o9 zuY7I^dURgcTp`(SuE+Q3=bMn?#eO?FYX%UkQ7mz<{OZCM`5yXMf+GV6jzoUsrZHJU z-z*svzHfY%z`GNAB>%i&fS0M~2W!fn>XY2;y#e0VPH(Ki@%bkPcrD~TLiyxpyd^G; zwgf$zfQ?Me4UtudMWRti{$J<1qm;0-;Bza=Xr#DNZV!ojh( zhjlI;h2sMk_4m%T?JmR=IjcKgs1v?8#uD^s0(Sp5`+Kc^?kvPkA!O+$mR`U#>ouU=)r|+S=F4UUZfa zt;S{vg@pL;HA~Q=^TNK_m*4+bjL+_d3h|Wa?2xCZbLl7?$M3%sTKPXAa!d3|p-9l9 z3D`Jp&nk})-;K=@_||D?f>F@eGOf2GE=c~>LhN(|JvuM!dUNF0Rm^pSMWaNWje&gb zUTK0+(C9L`mwO)NM59~nje(kuphxG0y<=W4_dM>CXMS+Z?ZGPU32K5-&^WN7r)yW0 zCS(bHHTL#k1J_FE(RpDX`nIQQSG6W)3HcrB1WUWNs|iLyqh7Y2u7Bj0D=#O+?c;3Q zrAOz5eb0S8UH@n*#HrU_4n|y$(gdTR(Q9@O&-U{dU(XWyM~GJ)L66Q0`&zh%>A~{t z_(E)7GAQ_l+%ug^N8$M5em&fHHAo_5gbOYcQhPK38`^ff8Y{%FLd+UtM>yuvQPjp* zDKn1bUM|Gk*DZlu97UOzdKgjtNE;!J%9Wp&wxx6FC>+N<{N$@e@`*_Uxw|r61!cT4 z1U;I7jXOPe+8iND2qEKD@NsD=I+u>ZakPbMb#kutBO9e%l@a2UBk0itY_yp0msy2q zFJt<*(sm2KP{(L!f>F>wo1Xe$Wg$Ar*#4X3?%m1n3!$S0x=P_QcI*(j^ zEWs!oN8cP!vat|v$lXnnK3K~U^ys{>(Ss*Fj?Zy6%f0$Xp0|t)Mne;ff(FKw(a*LP zqLTFUcgAH2$arN4dURgcIBw4>AjClV??3{AQP9BXYKdp%tlpCUwm5SvmQU&$kQ-fBWH$ftKG+NdEkEifN-w zr7Eghakyz2uyGu1?2lE~D$9GwLM+aE#hcZ*fu}vO#3R1E`*FOp5Nn0leDr(ozan!D zK~F3(^Y`A~Cs&G8v_y%bbG>hcf(AXYME|?`cxy_RmGKH1E1ER$?vgVDK~F64*dFgc|Owe4Z6OvBaUOecg57?$$o? zy+5Ity;q<2?rZKzEKy=kUw3!i^YHmR=!qrJ2kjGFEgDbGy)l$0Y@azju>^WEp1-^o z?uh*13F~pM=!qqeX{I+u2weHcd9V2IHE!U8peL5V*={(C-_+xkPbxOo5cI?nxFgMf z+Tw^p61_<77c}UJC2)6_E~U9sP$#bpnOwGKUAz1h2yLr{Kz%A@{xg+fuW9|M-#BodhA?#hiD9!r}u(9MV(7W;W+w@tr=F~->IxF}bi zJGe5`!Cko~7zGX14}PSeXp|A6sUzsod0|f<(a$}PWFgkvmnXc)JwZ({3L2~*Cfm>Z3en+9S0CMr$I+(kF3w@OyE*UA6HajiJ(_@xmfzto z{KonkA$rPHw~?#YxpWkcqi@>Xj|Os9s{$)S&pU!1O~6JEwrAB_i0__^_|4=g>RdVs z$1$$hU7VA0R%7c#{4S25M-#Ad-0pat72?k)BVMlr1f!sV(bZnZU^%NUbs}CpN6@45 z!p693ucMg|#bl&TaaXPhMnMC!ge8JP+$_X6N6@45!p8hzpT`Cv2Fo0E{%8BlHNhxo zV79aEYO2hGFUmMF{fH&#(RpEG9<=T1P9ZS5JyhDZT}?0w8kn_h|9D;Q?j9k&aBY_! zofkIdbK5_j5JK)#@JZLBG{GonM0at9$d$h?gzwp2OOMVA8<}iB$Cy|{W}7~B?Z}`B zMnQu$ksrAtL`5N5xUqyDofkItQS5ki_WJGNRg+&K5n;Hcb}jfN%|1r6A?TKY%M>VXgL48EAx67=Z2u(6Btso9Ns zSBMEYF9$M-j?Sf{a6DQqRTSb}uFHXgA6kMQO~4*euD3g@;zBGwH!HAF?w8J`qi{T0 zEv=O+&;94Dz;;K_qY2ok=f5!fSNnu`^NYRS6?yJDmyW`5j4QTUDk*0*Y5iVrrz7am z1Z*6yZg$m*2+{J3z5dn&1f!tAI>?V4m$UlL-|Js>1U))0Y^=!abzBjm#ravGu)A_i zFbW#1gZ#)TAs!QAmm}!Wd13E)u%CM#-wSax_vH|upe7gv4c0+^32&ec2@ga2t4!G96^sJ zV57y1G@tfe6(TIrn}yQ3bQF$9t0i1Vb!oe+9YK#KV58+{Gd0{9A+DdB6*?$auXE`r z9FJB@*M(Rw#1D?3M-#BogIk)<2hRzSb>m+Dh&*?lOGn{2#uZyF6_(Lwdw8$^r6cIk z1Z*5XYd*ESO^B1yZZ{<$7zGX1L9&jLvpOHz>)jx2OM7%)*cf;1b)eS{ma)A+vb|T# zrK6CEb&wyqU(Twv^w_EHUeTio*qA@;^Qb1o5t*YZN!!x7bQF$bwzKVOp%5cwj+*A4 zIX#+yjd{?vtDl7EC!<@-T(<3Mf>F?59VFkyDp#IEh$i_gL66Q08<}kX*d)YVQUN?E zEk);zNWMo{&gy0%+MTupJ(_@xy2SSLZ9+UQv(0NVPw8Ab3df_>5}x^HA@2Rw z67*;SHa-!u<5jg@b;CQG6$uXc=tEC?vKL4(cNDu^r0ppBRL{EMwk?VTpPy_A)=`>( zjmmkh*$W>hMCVO;f~EJo8qm3P6prKW+Sy>0oYnR(@&qRwv;;kxfQ|O{wb|JoAjI1< zF9%L<2^kGdFbW#cYN>z_Yi3^#e81Te^ys{>(RPcP8t%GWM-_?g_U>h)p$SGoBU&w8 z5#l)^Iyr(KofkG%GIk&Cst^r+cr}o7W1gVSrK4~>S}o<3>!@?!)j)Pf(4z_1*kQGw zv0M|4YBN9d+BPp@G&I2|Xkc8i)zWbxo}c}p*V_^F=)ACT+|~>ygeW`fL%&Y~f>F?5 z9VFjREkwQ9ANo8idURgc*x9z%abAd82VM0JmDJBwsbBXh2zm`DVrk-JAxifz{Vbj z?dP|kB{nY-?ksIf=h9I)&N@hbi%y6}LR6S#33@aE8+XL+x8EqlGeY#9`Ju0K=_qR3 zQ50uYLx_UX8ns6g)Z1u2mB}i^r44z)umA9BD3-w0A_~WG*X?ZZk37LVa(5qc1U))0 zY&;LUs}>QWzC6K+(o%FT9fjk#ayzT-7NYQbmqR5TL60V2qs7>>La)7B+HQW)(YbUK zj)SnX^UZSQ^(Ft~j-W>qu+j1dnSD6itIqph4V9Iv*ST~Qjz_Dd@8zr($$gSuYY2KY z0UJGdh2hrGY{$ z7GkJtyY%S1urX8H{xL|1$7Pg$XP51RnqU+(qSaCpxx3wkc;aVE(4+IhMqOh2c|Rfc z$o%x?d^<8|f>F?jR!gtQS)G$=>F8UQphxG0jXPq;s|Am~65jX8roif38hF}+Pf7VH zB56B{UJ&Bk(oKPPOIw0md0~Cn5f6u__SB zWeIvT0UIj-`#!@$A?i(?9jM;0yV1}Dqo9H3Z)dehLcB9=cHph2EJ2UX3md(_z5y{) zh}{jk2X>2&&ZVPpJX$R+7UIA9-2-=sj`nB*HhPzRXW}CvmgFfISRhv)OE3z@VcRML zPjIXd3mrj^&I_CGQpmR<3URSe18=fCcb!W|;W)+>TP=b3U5F`;phpw1aooO_;WT_| zXo6ADU>#)7Ds$lmsRTVbFKoWAA>V8yXSFC#!O%zU$~D0#Xs{0QBlCoqD8#3ZphxG0 zjZD$!F`+^C&^-6dHNhxounv;%XcUbu4ZDY~zGI&`JvuLJ%qzBCEfwO=sk1|UTuamh zqo5J3mc|LuLWn$#Y}=(r=Y@^E0oy;`5aMc!RiVLoZI99fqo5J3mgWkPqvfj5zpf9` zqw~VX9kKoVJt6jfvMF?`pdA@B!6;}%tEKma*eS$sw^@Q7ofkIWZSf=j6Jm!DzZ7iX z>s&gD*I`G|cZE18M1DuGLed2F>{}KfR?5Aqp0{8~=h9I)9<7$f2r*IaRj1sRphpw1 zai{IOACrZ+f7Ceo|8!|7 zI+u>ZaXcG4JI@m0squ+j4Edop-}E#*FaAook>(or}L!d4mRwdjK~!Wn`d zO~B@RKz3xfP^f|bmOMqBOGn{2#uZyFVf6V;2pN40L60V2O)X)T@pn=iV zUWe20-gX-F=)AD`u919yqG;5X{x{KGxh5C|jcB!m9<@xy_U{})kIoAlnWE2Qz06VT z-4oOVqoBb$NdD$WG?vK7^MCpsi%+$7jtP?S|jDiO1AV2b}5NCzBEte(e(RpFxj@W*FP>5q6 zZwjo+VaF0pFbW#cYUxcOHpm<};XzB#qw~UMeFoe|6(WVhMU;iH3KM zac{(DL@e(r2R2@U7#cyfU-9CD5P;c{PE(0s9v`?yOQZG{Gp)rIwHME{>iQ)kr1i zL6jzD)*az(nD{}Iz-5?cPERa>8P)FT$DWlY7zMi3suZuuspV0P)U%=oQJUEH?+|Zj z)o_$ZJu7-*3C!pAO^(>J(gdSGUvE9oYw^go1RC@pN)s5h?OzGSYG{H{pznUaua|ql z*9kP}L6jyw{HdpRCgrOH1U<0?Mus$3j(e4YC`~X5$8$XRs<&s$rnDM{pa)T!7(J(p zS9-?hX$jLx=!qpTj-_cOsb{4LMuBe9_$BXprNwDA%vsTcC{5ID($SlFd3svHoE1H> z1XizUdQ|FJX@XIppZolIZ&2-#2{hAg#Ft-*Gf_~G{Gp)mEUORjc(i|t%kXB zdJv_FJ;&O5&$TO_mM~XNPb`7=PU6NyqoD~#fiAzHjo0qdH_;I;^*ZQ5lqO1-Y2*Fx z&->F7=B((6CHOZ@siP<~;s{28MyBJdC(8RJ>h7iz@E}SPI79pQQ5g}80*&h%-(_Zk z8HXNT2lB$kJ-j$G>$>=}q9>NX)3kpv5_?vfU=(Pyh2oQv6KK$bC{3WfP5P#50)n1c z0`1uTtxW7$X@XIp(Qi(?Ga-QnJ&4i-da!+mC00Wdi~^1RU-$d>5@^taC{19D>GkN+ z1Oz>?1V%gi=5p*=X@XIpF;*7ousVSTJ&4i-#^Xne`3VSmVhPL)_V2u6&q@=F0*!g4 zP0dgO4SEoz3Cvw{cdt!A&=X5wPE35SG{Gp)n1i?NU!FjN9z&5vQ{w)i2}XfN-4(cdL;?+Z5Tyy!cANG!NkGsOOQ2>< zJeFvJQJ_(yPA^z8fd)N@(gZ5!#G|Mt7zG;Da%9Bb1fwWDh|&aB7Ktl;dSVH#GBEBY zrTD>N!P*;Vdx+vHW#D(cQ*9jQ%EFIiZWS)433@aE8^`TmK_30ExC;EE+FiG=pV5~Phr z(y;DbgFRmi2SCuH3D{iwWK1v$nP^)gbI#VmWxF#4wMXZL&9zU)1f!6Nwk7hX)C}f7 zTGD9Hqw~V%YAIuaQP7}miQk?n9n4fLXf){2d0}(4lrh05XwbGqse5w-R~>0;H0aTJ zVRLxxUJnU=%cHTcX$EF@Y98^femv=)ADGzRH+j z6f|gC;?&Qlyjcy08V!1MUf5ibWlS&%8ni9($xzV6U=Y@^_&y{deQhEfVph4Sew3hK|$n<(fgC3n1HpVfo zgp-oeBNzn@+E!z;jJx-pXlFF&(RpEGJm%UtDJeaIQP7}mHQfARH0aTJVPjt5+BqpH zJ%UltplvlCka@65qd`W49-S99=0~o!lakUS7zGX5R%6GpqW*6?hZzlebY9q)=egQW zN=lDl6f|fXjik>GpYoa> zK2J(Yk6;uuXj_f{Nu9WTMrWfzkIoAl^&{8kNlEDujDiMjtC3&o-Hk_@8V!1MUf8IE zxuQ=>N{?U^G-z85SGOAtdURgcsOPz&PfALUU=%cHTaCQ3{v@L=4VDA^hW^AWN=Y^eSm1-fww4PmZaYcLALmVX9#hsHmh;H>D;d10gVq<(72 z>o6LcU=)Zs6_xs_Wf~1lFbV`}yVOrD z(4VRdT^mo(qw~V%`YL0BQ6SKp`y6VYK!YBg7dF>d z@fw<76bP=CGA0-W0wdqTT`399iXNR8HdjmW8k%4f2(EoHCKv?*WB%gDMkhEcdURgc zT>HdpXo687xROaFWM3vJb;g0Zj)IN(G4Z{kCzjw!CSF4mi~@~$K5;9d2T_{fN+wkU zmtn3$6O01QbxA6bl89gw2-HN0`v*NbFKn($QZ*o9&Po%E0>M>ADv^?iU=#?{kBP?; zdURgcTxG;-Xo687PzNU-Md{IbVe9or+VM&gi~@nGpP$_sBK68O!6*=1GsrKQeKsL5 z@8;gYVIz|Q@PNR4SE!Zzef|Rjy}d{OsgjoXWaWgw7$MrGSc17U0sFni1Kr;{&k>@# z5YLZHvU`1Q^`oPxZSU2Ma#ov#cp=3SybeuJuiuY9%J*{qwPHfxr%oM$qdKlP8k)d$ zAPO3MPV#+ZLL|M^Ay~!{^ys{>JAO968+p@KnQ?v=;)&YTgCFd>)vRkZ!6<03h5C_2 zt0x3LeX@FR>wZhnqw~TZ9qI2)olru?tK34=+<$BE;ab&=h9(#V4fahx(qz?yz`H`^ zbOb#*FYFFC^!M)C+)y;m$n(hGVSOOaOC5}cCKv?`jxm0ur4Szpko&fP0| zbY9qJTlaF$W2m&Nccg_(cTZ3gjDiMh2|x0PoK>L%w}yXt(zX(MbY9roU+n4HRcRq^ zlKiJ!OVk9Tpuq}9-fx$)+9>(o6CLf*d10Sf*u(XYAwoPXSKUDFS1iFO9A`BsZ$LrwqVl}UjJ6|#CKv?`R<-h*_iH8u{>#=oe7_rg=+Sv$ zugu=VjaSQrI4MN_(RPH>1fzHzc5M)pvwB3%s+}7}aaJ5fNl*_XsvpTN#4>49*%#y zu=^#+(n_YtGaqum67*;SHd;)H-NQu#y|$6GsiC#18x2h`3L0qBcK4&WoKzGeqNiAs&@c`Y##ZbuJx+FqXamDUs%o5`4@TAaSnWwZz=YZ-Wu(sQu3Qt0f(FKX`#jzeV!jZV3$;h*g^iiPK9Afo<9O1h zI=CmuTsjJwFn8H@b-T3oi9%eGxlnsF0UI-%ZCB%kI3f8T+hNu zKkkSvmn-`l>Krx%J+Z{>_I%r>_zjw|M^u!Wq`F&1acEn@t1}ER{ z2zp|P)&FRXhSw^4>)lz=6H8pH(bru^E;*~4w+QjYyXU-GQ|&XSCzjapV_(x2{0N8*Le!W#cs-suJ+TBb zWl#Q7zU!W6Rq9-29|S$I1kU!Oj!!sZpj`Q^yC)lho>&5R*+S(&c!T2k0xO6*x19HH}`kxgI@_TEV)7W_kD{3I+u>Z@o1HC)9Ov}!q(L?UlvEw&|4;LzDG&I2|Xhf@w;c`}Q9=j=gUtvqoqw~V9F~660 z_*s0q)KG|SKP-}Oj%;8wG{Gonuzv6(9ffEt#7;-hqw~Vf@j-9z@YDGHs0W1j@7CXa zd5T6u6O4ie`i-qJj>{8#zToeErbd>aN9To&<8wkYMdOGNPZ#>#yY4hJ!6<03e()nz z<*W`!D?H)|dURgcPpgO)v@?(JG_95Y2?x>j-*uUf4NT_jAwV zWg#j^3t8cwxh5C|jcAooP_BH(v73Se+%u;~=Y`#AQh(R3atN_Y@_*`Dq9zyx4b~5S z95QJ2_$zEFr;r1sff$c_w}U=%c16Zw&Ta^SvmQUUlu$RY%-{7xJuW}p7vDjos#_TD?QD(OOdwKQc#GTgR6$SbhHHP zC{4ibJg%qrQ?{K_Uo{tE>(+M4;c+j zFbW#1gZ#)+A+8Eh$`SPFys)e7@9lkdb)aZ$5@KMd&HlJSRgH!w7zGWCE4ErXEktt1 z&3-pW(4+Ih#_?m>u}5}Ph>@K)dmR!GjDiO1Ao+HDA@aVo*&FQ$dURgcxyST%*Rf29 z{(rv_NOo7Q2}VJKb&wx9D8!FKT;F2vEGymJqwW~EkJSfqlT}#viqoBb$$d5cHEwQx_w_HDNT01>DFYLF6 z_jmoHxe#aMs*hf;==)AC-{3+KVyGDMbzYyOD@xV))eVt23@jC1%x6J-xzix4l%mDj%bMo8z-+f(lDZs~{GqY2o!(;uH|DfLx- zAwn`L98UT*j$jlrMXRNGa#qLXnIDl6NPBc%*l00F_H7ak^r-J_+YQGOjKXo&L4M>0 zIjhssruxW8sy#X{Y_$A^xihczBj-gUNqSU48Q)_GM&Wq0TKYtY%R=1k2zqp0*yzDG z|5{Kqz7k@Yj3onQp3=E=6pmwDvDMO6Ar1>M*b($-0yd66m{eRez7}GvjETb&5R8Hb z>mYk~E7(!goE1GfFKmpc_BvLH#{JSlYP&1f1f!tAI>?V)7GkRqCuJ_w9-S99W(NB_ zFb}>XZK|q!g3P6(kcoAWANg3G`NA814JB2!&zv4jz{Wgi+toH9)=G3=*Ag|sC}>2h zr6Y1y1BCcR=0fe!d0}Iww*BLj5V%hdx<1HUItrOs2lqB+_Q0b za=n*&nY~cbwpuzP#J6>FhW+m>!8%G4u2)) zL4KsR5bYOb4!>2x67*;ScKcPmyvpBR6piLWtSRoqoBb$NPbCKh=>qxID#IX7xw4l<#U7e zWkqA45Iah>42@ivIjD2#C>&=UB>Qke3@Onv^pYd!(FE*IFn&kr20kJH@YG zH>c6i1f!sVam7|kV}i??SZN zHN|V~2zqp0*f&1e*Ima5A=XQq8sM&66O4ie>mWZeK!`3vyzK~jbY9rg_sAy==6T?m zj~u=UX=6Zg#N(RpDvtk=)AtMx*BTKt#5{1n>~HNhxo zunv-6wicqM5SK(pdvspd=jZox{o{Edrpr}NzQ^_`=F(Bf#5%~flGlXzsiY<7(FE+$ zzw~qce4c0o<>{T-ZAS)8FbW#cYH6YnErqDH*An#Tys&S&)X#k?^Og{8gvh>Uim!9& zC|-vhMHdS3un?v8S^{SUk0z+MCFoU_-_8`Grd)aUds>EcE**vAtb_c>vqD@E;-kAP zL60V2<9WP3X07az^$?oqoBb$$d9}v#6oFP?Hoam&I=nY|I|}6M8l0&0U58tI+u>Z@o2R) zN;KL@9~|xodNctWJ^1!6t3=}+A?DbT!DwiLQP9A+VymU;LOd=+bD7z+N9TnNqG5|K zL}RuPlXgt;TO}YEg-oo2{KzLl6q7NrvLooxd0}Hjwb!vrh@3LE=XO`F2}VI9S}oyu z%(+ttnG3Z?=Y@^=!#)p;4C7_S*&{PsEWs!oXC356UJzoc5NjPlkIoAlGn{Q#ZG~7Q z(ccQCbLl7?k5)@fh4?@S%!S&c3D}saZU1oNRanNWID%2g#5%~2ToR2Qa%Yo@TY?^) z7dGk=+s}U%;s$wouWYv?gC-aSjcBzrK?oVI!ZqDkLXXZ1o0XI7;^gfftWxvSP-ICJ zPkZqBFh4~kZL6hKLUela)6jb}Ex|fU6R@LiCwwDBnd);xGxB5$=v+Dq$5{vYksI@M z5BiVK4gGbiCFs!vY`(Q1>nPEvam&Qe!v4*Th9(#V4c0-j&mhFbToXf&^sxjzIxlR# z=io)zU9Qoa@^>R4u0^=+Sv$^UVm^zY?Ny-fW>w)#e%v zO)v@?tb=6bCB*K0*+T0aL66Q0o9|NiksU$|k)Ji6U-MI=p$SGogLRM}*(=29c~$&( z96^uH3meDndl}ye(Ro1?Z%P7!QP5x={lVWLr~K(@dGj-W^9g&lqO1J68{w5f*f32K5-=6R%6OXyJ#4rm_e=~@XZB%K#F z-y)IqhY+PC|B`;TC2E3E(1=z`9}1CQ@^>3*33_y1*nICqW&Detok0xOAO%-|9S%~WLyfZJgBZDRw1&wI6G+Btilb;5%?6w3wIxlR# z+u}zS3(-i3J7nRebLl8vhaE*P2~nZOr-6ezEP=CvM-$YuZ&~~##7ojbo|0arbLl7? zXC356W(e^_&AEYFZm|SCnt;vsXZ*-&A%2x--lBhVqoD~#L4$RWANfOwJUJ!?t_`vT zJvuLJzHuYFwL-ioZK__*iAF;cjDkkATKZOqzofP2bOb#*FYM^sJLu=zq)k0lZLZPK z1f!tAI>?Xg6r!H=!FrCMN9Tpj_kiR#4uxoLM+T#z2}VH!F?59b}*2 zSZRfw9YK%I3!85z`H{nNRz0On?UcDt=h9I)j=9TLOWTAvEpt>RnG3Z?6R`O{ll*d) z5LYFCEtv~r2}a>K>mb<|mH9kL^8eJ+67=Z2u=xg*9er@sk=(XNX@XJEU>zjiFfL~m z5TaZ`OVFe9!sa_u@;QzWJLKt2kryI$E**vA(P{~Ou$mAr&9DSLnt;v9$-dq8q`Yky za&MloPsF~l#dd7ppj#qu&^31NS#p+%B{(z4&fTTv!P7Ht3XiRq!~6T=4awbCPw=w$ z?z0wlqn{>t0onO~eyX2*tjc&VD0a3vPdZ{;y&S&wXo8vc)>u+C+nm{DTLjBgz9~Fr zU!FJudWb@%z6Hj6Eym6jV*ZBa!K%w|43~Opmm%oUd11fu!f>F?Ddw#5U_gCYE$Q)=MJh9~0(48so7=j+17j~gKD%K7MnU6^H%58OlYbCm&9P>|Z@au3y13-1A?VS0VK*#0%GvF1JN{D^qoD~#LF4#i-My8~vIsH%Q)#=oKlO$b&t?dEbY9qp7IyVYEzB)M(+(|z zh1N#Ao6g%Os0l_vW6zZ?-t^i@LR9#?Suod;qJffk+q0ra=Y`!UxwF@`Q+6R*H*Xp2 zT=Mb2)^1nKGuH&8pi$&_C$HSK3(~G;ZfzEvp5wW|r)RGjf*ze0cHzt&z4K?#O8VE9 zwmUH;@X3@@aRj5F(J|)>-pPB8iN=Nl&4RsGP6|xzeA*E7=)AD6eGZ;$7TnZ#aiDu9OVFe9!XDkLjW=Y$K_T`OZW)~S z+}gnVE59%rnqU+(*0pcrUC6ddh}k)s2Rk>|68N}4*bwySys*zFxAb1TV}lSEuC@p^ z{A^F)u@^pyBN%0_V{uFG<%u(e$oydQV2^JP1{P+YYtPDgbY9r&1~l_#qE($k}GJA?VS0VW(7V;*D=GQiv@}TLj<8 ze>w0$hiBplMnPl6+NNIbHkF0&2R085f9Bsn`(jTSf*ze0_O=0yy<&G%6XL01ErP?U zWD1u3G*=wKC}_-kyNP#g&hJt)oLs&et$7}!ltheV~Pl!ebn+N-!%^J-8(0D`8qY2paS2gnfYgSW;68WSq z`Rpe1eHJ>Gj-nkNwfXb^n?MvZy_RRC5bCTv?a_H*Ayi9@BNzn@wCNx6wG%?Mc0FtD(vyThuN@-2c2@SehM-61g^fPHYVBu2T$Yi+lab+(7e0z3 z7zGWCF~N@(3Nc4UpZYTTtS%5X1U))0Y>ZuB*4!z?E*arIkrD2Lm0!dWj560Dqv*i@ zHVAP*M$w%ziuTF0-=3B8=)ABo2EWnpgb-6@r2bh(>RfNd6O4ie#{6=d_X+W(%myQ6 zHt62@w9%kP=Y@^Aq+Gu%LX4CdXQ<3L$A;iDKqClnK{4gb|sEr6f`iS4k>d{G|I~CeC2S_KwY(j#jWYM;TSTLm zR5Fc!YT}*v$)1%a7zGVfIN5hx7mXXFT6#sQrAhm5HX8Kkys%MgC1=Vh8uO$g%k<@> z^<#S97)LM)8mR3Ceej=XRFbN1yi|RQYFG_=bY9q~84D!m5RL0EwhUgA%ChOQ%tk{K zjDiMg)OI^Bi^kJZy`Jj&x8JgJRzuLE^TI}j{6X=XM5B*X$Tu}B7Ft*7x_RcBU=%b^ zIX8XvtZ3AiDm+iyK=UF%=Y@?*|DjNlXk?N~f4Wrqt;hdmG&I2|Xkaz5dD&sn zC@rgr8|-Sr5cKH0Tv-(8cwUIjvSPVKRxE+ZN7E2GimRe)KWrBwB&(tWvMSoSh57#-CgbTjUlkQgGc9ujkV#% z&KreDk`?4%vV#2g;)*zeQP99zv~|tdLezAt)WT<$8G;_27dF>d@5?^ys{>vF9-E=K4Yuayt_%FFhPbFbW#j{kXB(KQf+OLP5x^gMPq^N;#84coO_|Z_j+4 z*PsVcnmD_9ly|k<^#mH4U=(QXWm%0!2R~5ptEmN7X%B1?n0*#}I>HN&%9d=2k}35n zF25^R8bU{LzOp-Bx<_eGEb-a66z|6-*e_8;QY^tJ9FOjJDI!ICU~6L3w87rC^VnNb zgwbFw5R8J&y{t5Z(V!=m`22c5?@YDZ5@=|GQJ}e(6|X@LqBJqRZLf4RG{Gp)+{=pB zpa)T!*tNA=I`>Kwi~`NQtauH25T%LV>b{)LGuH&8KyxoEUV|P)Y2t=|JF2#e?`^|B zoeMUjU~?}kUW1-kqW{_U3HqSU1)EW@xtA5MK~F5Pv~1f1V~NfMn^Ca2mldx;Pb?AM z*)qYHsB^()6m0Hg#cR+LOEfyvEWy~WbHQd5Z0=>nYtR!*)NS7+!5pP?!DbX}?q$Vm z&=X56d#Q1PxmM?b%_!L1%SxkRS|UBMM4zjT64VSj7i>nsj_!EHj&StE5*>>+im!dl zS!se%prbopu^RLsN)y+1KW(bB_$${0qo`r`$P#J5gD6cv57l5|4HAq3&E2nff*xK6 z^1{ZQu2?_ox{L`%f#B{}JVB4n3mYxQ3!&1Fy>d-33Iun*;t6_mUf5{)dyitp604yJ zMuFh&S3E(F&I=no_`*H&6KH6HQ6RYc6|X^$&I=nOQI$_OCD70Wqd;)?D_(;hofkGn z)ZW>DN}!<$MuFh&SG)#2IxlR@3^jNDl|Vxii~_;kuXqi5bY9q);W{Any-L;41fxK3_bXn5 z9-S99>gzPmJXJ#zi~_;kuXqi5bY9q4e?;4Ex#9IHw!bUaqdJ$4;!4Kuk)f45IOu_j zg{BpJRC{6xtasA%L316N;Hn5wu(|t{_BymDmcY6#%~+DE!CW911)IBH@f!5R5?Ftx z852`AG{Gp)-2IBzpa)T!z&bk3*q*AP2}XhD?pM49J&4i-_5;$)QK=f5U=(QXe#L9h zgD6d4Un9+2o2sD+MuCp*k?GM#dthq<`!i{3hExsa0>LQQ-2IBb4tinFK>U9LGM~Q|7I(@jDwX12Js-SYLZI z!QDaoj@Nhfr!SjhHR1@q??vra=B=>OIm<2I`SZZ=JD2K(v`6QK9eu|uN&bPDo`7H! zGSSApN=iEQeUI?Vf6X@<^ys{>`6gG!1f!rq+Y%QCy%2u&^a-OukIoC5Z*pZ!FbW#9 zEio@wKV}eo8plyj6;hV$1Zfj;V=+Sxksf>Nc3#}v(p`#uVjp$opAO?-v zA9|?LOJVKN1Z=*SWzQ--f>Fps+Y)n^O$)s>ytmPyN9TpjH@Pw<7zGX5mIyvmI#l@8 z!A64~ofkIWlOQph4Rb3;*5j{jzGP(V$1? zh0XV}GA0-W4ceA?>-nyMWmyLp4SIB5*nBT5V}eo8plylzS=I#39`9x}=+Sv$^G&Xd z2}VJKwk5jdx)eAutew%IN9TpjH@Pw<7zGX5miYe1g26L|8W;_FbY9qeFDqk$QP7}m ziM*G~1y5wDY&7W6d13QSu8avrL4&p>-u6^)3hx6#Xu=ysJJ*)HxMnQwNyN*w~1m8TmEu=j*#DZh!n*%`W?Ay;A;EtKXEPe~=)AD` zCYQVkSijKn^aw^l1NQgktuQ=6R({%}^U}6&Z2eyZqL^u@dB7_KhuQq(?9c8nmrO zmh0QSpH~ev8uaMAuu-qrH@2XW9>FMR&^8)Ljea)umC>L_=Y@^B%f7J%jr0gcL4&r{ zXf1VO!B+2{w^ys`?i`X}|a2+2eAaoShI8ovc zSw}4zT0N*ent+Yv@PM*=K<}}d0}JSZr|9#b)-iy3YlnIjnlHv@WNm7 zjRrkBFKp~T*f+M0)t_D-#PbOVMnMC1X7i4h?3a|^W%o;r20c12Y`&M3ZBFsL-;{GZ zEO7**paC2ELD}ZaC|*9@ogxy@Nc_f@&IOzMNwD$z#n@Y#H-)to?y~m8641j=Q5qs8 zmS7Z)<2TAS?S3;YVKnGLlqPTw@%MxA8k%4fX#8^8uURvvC5#3=h|&bw0{(t5UPBX% z0*zmQ%Ot;xoQ5zO^dL$T+$l;!n0uuOMuEmJ**!G7TLKMw5Tyz9P5k{}yoM$i1scDv zcVYCTw1l}1dJv@v?gv?o(q;Eod~D|Yl{yz}Mq%8=?*x7^@dKxEPjbnM-tk~X?a>5m z?gv?oyFR(8V&B&q#Sx4`Cj5Hgv!|9jjsLaXUh!n*jw`iC=f!c1`Pf@ZbFY##p`&19 zCc<9>rXf;d2}a>KeuZ&J)o=oW9-S99W>ow&U>Xf`R+?ZG2>eQA^GCKN(4a@>g^kJp ze+?L~p$SHTz^`KFTJUuO4SIB5*r;&u*MRXFnqU+N{JQ7ql&=zK(4+IhMx}<&3`ujMvZvqd?#n zUH_}JH~~SA&I=nW0Q@yzyoM$i1%i7u854{Gf#0&7ad~1U<0? zcX8r1G{Gp)u(2oee-nt}S)E!vF&zRPotI~bJ(}kIoC5`zY}mnqU+NR3B|?h7xGdqw~V%K1!-a3RXormyUvsDr(;DwFxxni6yv? z60e~NMuA2(_~riP2{hvpK?DCl7|amxc!9Z^hH6Hgrv8iF28z~-)q)o?2ob5@#Q z6bP(+f_IOIxpM8%d0}%`B<BP(F_r_oKO2b1RiAU#iA{u-~Y&*;~>CY(#;;UH{uW z!AU0yE(Z~6)Hy_T?AE~MP6#z{7v@c=y1IcOL?fg|OoW>4n%QXefJ-o0zM*Nb68 z(4z_1Xm56hrEuA0=@N{BHtZ~BPiF5O$ChWx**d&zcc!rR=)ADGD>83Nhv1pznbRQ{ zg-o!^nmw5g9dfS7mr^r)>(P=%gC3n1Huq8HO=*5COS%N3paJ`$*^?Q1qST7to+%y9 zR4iyT=+SvOCfXgA!CguwBy9BFDa=+Ol1=nhMk3&C^= zMnMC16SF5%e_P!ZBYNx(eK(`C(V$1?h0Q&Vc~fevZjdg)C}_YQYIazjt<`izuf=0R zEq>^0H0aTJVRMgT-juS1(=^XkBdfn1f!q<`;6IPsdZn=71=LU z3sh}1$Y{``^TOsH$Gj=`%e_jEU=%c9?=d?pFW=p0#eh6Z18<$}ZZzo8d0}&pW8ReF z^2|X@PCzgU8n8>79hPxbs;@XQ?P8$hiFQVV9-S99_c-QFX)A5_=X3~0K?C-6v%}JN z-NP%|mn#w+IK7_HphxG0%^iz*Q*J3+E?t6A(11PD?3eT@bo+|xy(A2JA6r zzhp_pEz5Is=@=Yd_Lv#l>Ct&%bH`%dl=7>0q)RXg8nD}${gRZzqnEGD+dDXI&`hI2 zkIoC5I~FoWl`Xe?l@RYFAQ%M=*w35&lI~-^S$6sQ;9$>&MU4hMIxlSQSV-Fqo-stP z1O%g?0sBL@UxIZ+ius-*?a_H@b1x%f0#VF#$n1VJ@AXHytM)nq+N1Nr#`W2~jHYFm zrAsghnP87IyC2>?b;`TDYc%N5d12!@*}aS_fmh1o2~Hg}Gmc;sG+!bZQbdl}L{Dx^m+ z3YlO}G`k`FyB`DAJzSxcj8{XZ z*E1UQ=)ABoj@iA8TxH9pOE3x=uV-cWwYUU_t0*r-?R&IB};r$aCbnPAT~dl?ODHLdWr)K|~`(AQ|t zqw~T>-DP(s9w?kX!6<0J-e&ePp4?Wq!h2FDZlBTFXwakc!bbgQcP8pz2&PLg3L3Cy zo4t&WMpvznU+UeBN17T9dURgc+#8rTC6{OvNQYn)G+^g2I}?LXl&Wyj*6l`v9-Wsp zyxnIQ+NER#SLvrAbQCnO0tlF$iS`|GR>&{wkNihV8iF28z{a}7?lUx%@v1;N1f!q< zd!yNzkabjfw~jIz^ys{>xi?_eQRUq_Dvn?jG+<9NI}@@VEbrEXW^AWN=Y@@RqTOc* z&YxU?Jc!q|ls-T(htX@V=V_$$}a0?< zx{iX)3MXEJo>&5NmtA=&4fD)3!6?v}QSCY^mY@ewnA@=u`u{&GO`!U)Yn)gOeO8Qu z25KU^Vu>Z_K@{#Z)*S!;XBA7>)r8V8&qJRTY(znWRc-w9poeFo3A>tz)zAcNM1jWI zz}D@GFi(&ko{1*7ny@Pg8GOuju(AZfQ5!Z}XzF#OBpRGs z4vZ%lMeUU4v+TFlnX~$;bo8v?(RrztmdNwapyfK3j>7SC)5m&a=Hz?~Z?UF?pF1{X zMb?SK%WID&VE1~kr6Sr2F`~rJMuWL@6f*sFbd2}J-*ugabn%q(Q(jxCJ+Z{AGh3x4 z%*db#*o=aG{lsYROob6j1Bmv-5>4K0la@%;U@j1hf?a;;Yu@=6XD860Czd#yxm{W! zRYMbu0)4LPD6jN*_sw`=2~Po{J+L*==eOt65&+@|MuGOHjqtAByDhI1|H0VK;CT`f;IW3W@p$SHTzIBtl(OB!31RC@pN)ySmx~3&k zH8jB}&>J=l@XkKwzTq#;^PmS&ns{?&kF-Rph9(#V8ow(v`>oUNIxzYyx_{7e_B)I| zh|(hw`rspLez)I+WP0u9NyE#t7r~BoF z(RpE`pa0(kqd=f{z4Aeq1RC_{ys*(X|8Ig(AkbHmv%Hx=gC3n1Hu~oOqwGAuq$rv< z4w4oDL2?F30usDHy4$@KB#9)EsG=YU7!VadGI%B=CqeO$u%Z$Kk-J0X?snHeP%sNf zFklX-AczPm{Oj$W+I@F+LH|6D-&aq+^{d)RJ=HxuH~&tk7YJM{V;ehv3khe5`Ns;! zg^la{-wE{sforAX_Phk!p$f-^jcfDY3H1VjYi0cXyAtG3h2z4;wfXOadV#>Ta{V`Z z6XZ~ZiAW%`m=be?xR|2vwMXjrQKA$`E2RatIORSnQ>_(PMZ58-E*Y zh^rk!`wu2GUvEQH>DMqjz0o9pp$&sV9E-isuDygO@Yo5jA92O2>zn=brXl{bx12ON zm{2d|&|bn5a1*L4s&HJ`Xusd|R}H^Q%K7^CYW}&$x|S@dm)GR5}tsYP=yKDXrEyE zs}iky=lu9{cE*WAV@wVv)C)PZmyo}K?IKj+xUkXQ*YsDr8Vt($VEv$sZjC0H989Pe za%eB%3AhPWI4*3ow=@0K(KUl}{;9UeC)c{k!GwAtN3{2G6RL1r*l7RW^j9DjR$CO* z989Pea%f-a3AhPWI4*3o+wE|+Jf!=I3H3q_?eio1Dnh8jabcs~9*;AvdgrA7k{$Zm zIp$2L7jo#m%M$?MB|;UB3mfhBylN`vZnGgLL-U<;mkISk4!sATx? z^dl+j2E`HTg-AN*;0gTRr=cN|9YPh33mfhBe!k-O!J7+9KayH=QIKP?7ut2sLB2jz zqQ4mSHVD_6g z8T+c$xC*;j^8g$dYbpKfM9mRwgO`Exmf1AobmBh*XJ1v{4k z;tL@L3Bd}-g^ly-Dl_}>%DKMDE9BfgddtZ;LcI_P=dqp3$UnEo5WhQwDjXL!+K-so zkJ`RhWQ{_M>L@V@kbRDQBb~Ia0Kh$-#trAqV;u zJC_0CFCtXoxUkXQ#LRw350~Qfi6#dV>V+KWAMIR*n^1-0!bbZUGy5Sub&Ausn;cB2 z7jmGVw{sb8LKTh+8|}@_?8iT?dZ)CIUg5h#V@wVv)C)Oqzp`@~ZbB7~3mfeoGy5U; z+7zivaOX5Rm{2d|zo3H3q_)DLzp!%e8dabcr< znVJ0n;nY4R2NUXr9H@ircDM;uI4*3o+wE|+95tND!GwAt2kJR{JlupT92Yj)?eQqt zx_8PUsYOpZ$D9fELJriW_Pn~-A^va(RX8qewA=GaD&!QWrZ(HbgnA(dYHfRcxCvD_ zE^M^h>q9F26lZK;axkG@$boT!z0PGck@A%tO&CHIj;rGYdz}xLo}s#oao%zI5+&FR zIWXR_{gsN1itx$y5z!ydMW+0{Pp(;k%>I2=r7UVj&}Sd*A?=ep9o=o7%Q-u z(ES!6A~`e`2=#)Ezc0H+zVj0yA~{rX6MB|Jh)515)C;tJk0(Mza;O3?CiFax5Rn{A zs26Dc9#4dbJ)Q^=$)O6om_Q$c-X}staxkG@p!It^mUzDG zyIcNweSRxeU@O7BohAOdy~38~zwNe#3HCy}eviiz?>>5Z%kbWfQ&?dFwtlrIvK?m2 zS#cA3XNVAH%iV-}p{>5b6b6zsD0HA~{rX6MCnP5Rn{A zs26Dcx=w_Mb)5(i$)O6on9#~8hA70bV5=8w{kl#JVUC9? zZbB=-7{cUWLcKui*L7kDlS37FF`<=g3}JFGpIE9#%89Ri zSb@z1BE{4`=&vHb3W}T1xuuw26-THSXzY$@vqp$fd1z!i+S*9#Ns1zNwA z6HllDFDB3vVGjAignEJ2Z{@@js=$j0^xBvUzc8U*p!Hih@q{YyVgh#wd}Ab@P$iCV zo&sE$z+DZ`3P9_ZZ{i76;Kc;)qIjBdVM4t?>z8lh302_51nLJomANpXUZC~MH}Qlj z@L~d04W2Gtm{2d!`sJH=LKS#1f$A*rb}*q{p!Lf)@i|n17Za#U6CV#I)C;tJ`6fPx zD)3?g)iTbQ3lr)ETECAIPpASfCNMt}y`tWc(Zs;sjdM5_Z1vJn2EKe_b9^r&md)RG zGh-b6a*mtO@7P#kgN%w!y*@uO#=&=R+yq8Q@qLMoia@|tFWCB3n%FI8#Z6$O7vCqk z3H3s|ew8LZhbr)50wdP=zMW&iRxjB4Rhsx5s<;V^yyNdt91FI3!Ny2G{yxZxo4}ky z{Jqvqs2AEXV-bHpX9YGBmk?Mngw6*&w|Gn8a!D^vd%%C> z$xNRTsJ!q^9Q`6R+I0@d6Ue;TyBWmSPi6*LVFI>(UB?sHP`KQA3H8$4*bYzN!A3Q+ zuN*(wzy7d4#0tlSjXh$&C6rsZoFUdFAk+(ybRT*GMT_*z9z9{Ke}0=4CWk5<7dFlk z`%NU-j`I@gg&cZbc>;5Po}2yWGb8=G&K@v1RN=U=^_;eU8yCdR1cZ7ahuW5y*>IqL zQnQj_RyZzfy*4jQs23usZHeJmb@I1pQQzcHh2z53-si%EdLf6}mYAK<(jQo!W^$;) zabasOdSODnkV9=tTsE*D3tDO zhbkNwwpN@MCe#Z#)V4&*+G(LG&hb!%d>_b$oSULcI`4ZA375EcUj@Muywq9VM4tSNo`B)Z8$JoRPG^;4?Mx$AyjBZtYC_MpdCg=Ofe$k<_+1MoOL7v_*ZBLluq-8@1ih@*goW1`fFo0q4V9I9|!*r@Fqj<#cn+5dYK%ma9r4^?am~f zbGGArgnA)|+BU~$YyQZ1yWbF#LlusT>qehiMu}Z6g1vA>#S-63o%ms~yZx*%0UNd5 z!G2}EHplr0^+F`IEs-X5VzHCsO%7E!E^O3xJC9Xxa-5G)FXT|$=5XpnlS37b3maA6 z&mAf|InGC@7jmd=b2xRP$)O6zg^k)Sn&W(gdLf6}HiuIunjETdT-d1XqWkK6gnA)| z+BU}ssS_(Y$3qp43mdgv^q8NIP%q?A+vX@UxgfO5Ij>aVxUf<6g&*-a$K!m2dLf6} zHplH!C$5&WofVD?8&zNQ8ayANUWlZ&%~4(I!~(g_S>d>_QT0XplJgPjg-B}K9POn} z{7(8SRyZzfRDIDt@qC1OA(GlQhxDRh^t-HZT-d1kqJ8`M2=ziFwQUZkPBb}G;kd9- z^+oSd=Ofe$In=f}oH{Xs6^;uVwVl1!N}ZVO)QNF~dLa^Qd#^24v}ba6sT2FQU111S zI4*3|C8!2GfuV9gKOdo9$Puk%xb|U%OOWauy-Hyn|y|o2b-jTCh!nS_ueM+(e6s6N9U3*G)jE z;wBoeyg&HzD@q)1vOP6v?TTD@slo(~?EbgM1grj1+sScluRoHeFDY%#5>?zp;nH^o zL))u6#G$6i$&KIrI~lQ5aT6sbj0o4`N^}34WWvg zC{t%Z@b)9S9b$ObzNwG&dnFfJt_l-nrM<>=eS@3YZE%R~WztfTKHQniikt8j>l0j6 zWR^p`_hN^Xm7hFe2vyv~ppLzRC7-_5A)0KtCgtqU-3+0Mo9IxfS8!{$LJsjqa<7yd z+P|BGU!{thm^Y#u_1Ca`@^>~r4k_t$P-Kjq?Y_T_?5g$W!d^ojc?4am1S zMppVF`S|{3CWjL2g-F=z@3o)o5NH4SCV5hBpUClm!USyel|zfpcZg~Ye@e5%Qz zvDgbaaF+bOZ>2+YDsxZrjL9d=cBtYe(7Sf`yyOrq+dq^XtW|Ui$AYb1<;2Drx@kt( zA%0p=Eal6S`;u8<0yg^Nve&%l5FPfGPWj{3LOE_iz0i*9rdqo5#=5`#tV$`Z+E!0t zh2z3T58mOrADtW*m#&-g?R#k^hsI(rM8eg)XWB`Jc<*4dlv3XgH#t;c0yg^mvo{th zu*baEi{6w0x6Cs+m{2d|Kp%5tLNSLJxukc>w$-^NhbkNwHtrH_FTL0ya#HU|d9T?K zlY( zvBCsw+`GE|Qq3V=9{+JlU~>P~ZbH34;NEq^^y&^F9gnFps)@3qLr)(nGT61|_w=E@ zQaipPV?}Kx?j7697hjRVOO-@Kv?AkQ#RP14fo}A3Z(p<`V?q_pr^G`i`uL(1S;PNL z+A_b!pDFNCg^8-tzV5+(zGy{u?Z$Ufisn9>!-|{OG-seMT9GwRU6>LKEir^DZsJYP zO}=PFw(vlYlttIhFoY^@;ydjQ@gyiE*JYs6((RGD|MeQz9M7A zO`Q66oG)6D$uT$o;H8S2=$t><7p=&cP{mEm^Je;@6&Vw%xQW@*AM!;jGA2}U6R&1J z;)_;fOsL`}R=+BL$um-sF`EyT9M_w_(t-#pHw!4DsE!fs=2;sMYb-#bZX|_ zow>-N3KKP?{b-wczGy}Ew^U@CGTu#M#Z7d5WS%ctk!3m+83NPUTpRiwj2r*u+j5HE3#XpBI|je zSvJRFFSKK?M=P>Fq$0adDl%4>fQ`N~T9H+hifpM=WNt#e(2lbtz9M6VWtyE+Oq#|R53E1e5qZOH7DzfUs3gx&7^+G$Yn`lMWPAam= zZK|iR!f|1v2ai@{XG+yg`B*A4H=$l=$JHFI$hJvE_LEd(tZ-b|=<}l$Ss|&&u9S+* zO{f>z(Z@t9vi?$$rAkG{3deitzETP*Cf)yry8{zeS6{_pxI3K}Yuhnkj?LTx*f*hzlsU=3dz}?TVYOuU&RU&vwm#jZSg?U1UZ;suczgz?Ne!{ zljFMuA8l#-TFo462P;g>t=Pg_YqK{Uy`3k}y-w4Yum9r-)0Z&8UI(Q2Y5(?guO*!8 z+#C;9nD}0L)FlHRO+c{1M8(Yd-VY0Rc7BzU!yFGL*y}Focc&eABtZ^VnApC)YHSYK z2KzMS^iMN9lCc0O?g!V3iBLi|U!Cq%qm5trInFbJo6(-W3EoqK<{PAFg ziQestn)50`L~<~}Ue!t$Gv{uEus=$E6)Q{>zWP-90l7XRL?j0j>=kZ&k-2IkL?j0* zOr&^!O#er&+6xoxRlD*R=|`n6i4c)r#W~n(QM1CPzlso%U&RU&zHSH7w@9BDAtE`L zV6Rhqw&w?X-Rg@FksPcraiZ4#^doX#i4e#k|7C`mV6T~Ff6c$H$?ya@SYhG;A!_v= z9*oby1bf}zvt+>shi^-egB2$379y*}hy*#9V6R~_sugs+cVvPbtS~WHh_XvZIyusw z%*lQB=;F25S4^xBvS@=3po3OSx&C59+n(p2FWCVb>oC{UIv zeP`F2bZ1bdeW_If_&3GbO=Z4(fz zEG_kvH>+^#|HrRl;@8s4y!Ag%Nsxno6?+}adD{EO@}vX=D^vbi?#;it<^SVXF>&~Z z72e$+HA|3#e-(Sx7`M{9sAtnC5joo@Oxqixe5#sK$~T{-xbU-TQ4<*>^-tg50 z)hgeW;8(G7v|`7C1%<6w?5}cfx#nPE=_efvI=`7{%US7PwsXPKbnErsa=3}#8=miY zsKP|!!<`EbZcrj!Mt2?hEit1zKIU)9G1&L<2p>jtJnHiVwijEGJL^mrb6&B+#Pc$` z>(ykGGv=K6@rvAQs&q00D@@!aqr2c$!)83l3KKo$JZ}5(?dBK8UQw(t z@uZx)`(C^w0l^9rBjwzEVdR}r!a3}jVOE%cz4aHn-#h`|$9b(sEot&H&RsX5USG($ zyZ!GwefF+4waRC&UfX<$Iony`xJS2NT~OrLM8Ap^Ci>UwSkU#8LQ$?E=Yrdg(GiRgtZ>|$k998Se7_RirKdiy ze}kE=S=@ndDA8Q*&Qm*8 zFa#@@ht;^8+Y|U+?#^wWx3lN0Fo8K)-6naTQ|`{QuDK%aSFsmnSTRbqdv}7|of|B; z#t^LV@!;ICN02Kre)7W=Cy2=3d(EtHMg4+MmBQX4;YadeuNv7rVz2L2$h&lf`%6vm zQpHVdY1!gG5bCA5TTFY}u7QmF6uzf$i%5b6c`t6rCS zC;nQHAcrdOVq$#DTK|DiFVGwPRlLtm3nj?GXCI%H*l+a*kBt(U=zr*0aOAcrdOVgg4thKS@~LcKs^zkR#&x&%2?ffo}vvN1#?2NUWA8hiTg zp9a}-)VrP<3+zPhMIf%YY5vkM!$ytMzht3^Sy zFG*XJoja+bH-{C_O2Gd9r$R2G%bX^y-Gq8+?!-L6o4&6Q?_3sREk`9N8Ema(ytN zUZAn3V|s>24prdA1dePB5y`=XdV$8Cj_Da9IaGlc6F9OlL?j0j>IE8mI;LlcCrHY%VJ$FU=)HjP$e}$jrCe#b0kmK>2*5$_$CWk8Uaud5Z?TF10A)vSk(Ep84`qYy% z@<-nPeS#dSXbvT?eTfNG+ywU9&rRP?kb?>J0*&KTt^6km2vy*PV?O54XOScL@4t!( zoRzc6eVrf&|0?xDKAh8w`hS;zs9s@VJ`{L$Ip8GS=qf82tBN#RSgKnDYu(RNVE!=P|Y~@pY~WehCxEhwCx%mLryK zhhDYVzL@JAKg#?nRX|{0q5se=|My?T1db;9t_%Mv^+FDu3+VYgf!puMN?Y^r1O6hH zPYGh3Hr@=v+dwbK7t>$w*V8vf-eg3(zC9#=r>JjM-tXrg@MAkzVM6V3=B>sjB*!|N z!c@i@mV!Il4~&6UmW&ytn_) zA1e4+VFGrCKl}SOn>Qy94$Vq?``#}8OQzM0Bh(9#bOpXAFlb;_-uo?m{#xUk8A26~ zyIykK=DWdH!@M~;V|Z3t@z%-yJ#V#%A=nE!bOpXAuxDsi-ky1l{I`zkU;7Tubaw)D~)LodkQ zbrb4^_UM}y>2mIVTx(9Kwj4oLI4O1?C(}R?7Wq-O!|^OZN3Rjle67Ts2AGxZ52=8f5Wo!ipp(vhg?yta9r4@ z@*Z{Gt{EfSadu>x@LO`#x(W3{d-M&SvV-Iv)wz0jpY#l@a9r3s*FWa#Xx`iYRQjur z=QRqW_i+>Ih4$#1NTp<7ebg#B?0LJ5AynbGus<3#(|4(P|NP>cv(g@H=?mX4y(q_G zFSJMBs9JGbR^G0AyM)nGv%&=Ie!tH0^)&C%PnPp)`tjc3`EoaK6Y7QQJo;wXeNr>* zUo$A&Q0_Raa9r4mdZ8U}JKDDzuj!wa_w2(Dgu6GLVhB|@F6={d=lMpN@6o5-m6aw{ zpBW8vEcViN`>y4V-)9R zx4$L*?p>og#1ZO+9Qv-cCon;d$6)E(w|47p2vs;PY}^^_yZR4HU-GluKlY617e}ZU za_Af2oBur{XC9PFXYg-)eqNZsC4j-Z=RFSKI>5IIZo zr0T=iz!0i%T-X>{*uR~yRO*s%Wc+cjoQWKZz0i)4j6K`ONDb$VaSWjf6RDj-}FjwwT^<+Hwo%9SG zi@nf}k){2+A8C@~eHn9(klu$CCSYS+`;qw@TVCl)ipaQj|66V12=ziFjF9bLBN-z7 z)kiXRt}eYOD;!tH(Id@YV zUo!LT-Z(O{=RwEz88L#V=WVPi(8xcOUQzsXqbu+011(q>s4pY#m(F) zfgC!E=LvlBMeWU-rRp2hbV~50Ki@SKQ#PKlJlz!fnNfJ3C(Bg-Rr9Vl2lHrzLruUb1e2kJL zVFEVV?O*YjG4+;|(lRsgk<3hREcQY>>bZ92ucWyNRhWQ{cKcU6GS-Yvc~z>u$7F_u zW3ks}$$@(A8S_`tyh6O<5Uel(TW56S@6flNkaAF}KB*b}ZbH4#j(YAh^HGIRHn%-nG- z_Ch=Axf{*jRdW-nFaaCw_V0cSn4Fn1SY{x{$P6UMVlT9#o(r1459cOSVFEVV?cXmc zT4hQ~ZK?V~GS|nk*bD8b=Wbh!zjNm%RAB-(+U?(ADRtY}l%Z1fJuY*79E-isj;ilo z^Y{APgepwH)){CSd0jLzrC8ffRtCH z>MJ&~pUJ_5dLaj@zE8|kfTeXlNzM_XghQypabcs~J~!BY`px7+QuW;_bA23(z0i(& zuBLgK;U-jJ0yf(1^N*B68G{)snP=KB19 z$y^`DVlT9#I$LUlFaq_>&TN)j>TSRN7XmaJbit% zODL(M5H%b^6((S#-9E3ylU-RTf+xJ09E-iQeVFr?ZX-k?R+vy5Pn11@8C^n~o$UxF zBH)E~Y7>7`W3D{`2a{w#q49Hn0cY`uAYrwJC3+=dxUkV~=a$Y^ zo|5yR)FrRTY!t_0FSMhQsbc=Nx|>ji3D{`2b7WF2~o>pwx*b2NUXr9H=axF@J~NO{l_gVWZv7SxWVq_C_+-$FbN8?Wp0;n7`TYCRAYpHrnm%VEZ+ra{@A& zm?Lw29E-hlT(Zb~NuUD}tS|vvXSqFrrBiRoIViK|U&>scn@}&bW8`(C`M!ahP=({d zM!TIIoPDfE&JY>h?UcDbj>TSR#|ZKd^Q7UiWAA1U7vfEaP=yKDXt&QMGCOX~-YetU z^71r;W3d<7F%sTpo|w1^RhWQ{cKeLwe{Tn}@r1Xw%=K|B_Ch;G^hM2+A2*>26R^>4 zpB3GD@UiS~WS*h1%=K|B_Ch;mCaRhzSZ+cUCSar8KJzNObwGA?nf+)lPsunId!ZdO zELWK)aqsR5WLFbHo(UU56((S#-9EdkkbOsXHJSJ6EKmD57JH!`Geu?16Gb*%ywb_~RGK4A|7q-sgc>?l;H%*@K`g_P+AO5nx zeqH4y@!Ig@yddf;wCi_TWX;`O(RF>SFrl`s6DLTH=WGsNA_88T+pg=oO4b_ax;|Do zF6=(n&I{Ul_uBzkX;o!i-vU|J=O)w(?fPvNPhgciH&`K6_=-0t8bTG03wvkI+@P)7 z`^htwV=}*cQs(+N7JKoR3p{~Y@*HQp)No5>G{FiJu={^JCuqkXJLOrbuIqCX>cw9! z@C2TgwI3B`4fTK9_PEKR-%8-Put#p39kk=9p7Pv4R$2J5%7SCDS6SIt`mF;`py80L zy!ul0)%>x7pA{xxkM2AxXxH^Ex-q)0&rPTo+Vxup@)!Q(dF{(m!==k8l@*Q)`_Sqc zK|8LUDciBURkA-_=K9=(dZ9h~MTGKw|;+oUX(2y9xC|yMD*W6Sz~Zb6wZR3de<=k<}|` zpARk{n3eXptm_-TZKBDcvDgcdqF-wA%k_bEeU~@&8bTE&VE?eCXV5<9TrO)M(_~#= zd6~K6SnP#%d{@e@>+5wxR-U}g5iWfB6ho-O1nk>y?-{h~`mT_Bt*-0iSnQ?kc3s~z zCr2@PV}unZG@o79=ZkJf9D%Kc7b58wwLF2hoPE_u*7dQ%abe?l*qZvN)QLDAw{Dvl zN2nJf;W*iKeYw(?;Fw<}l|CyR7dFlqJ2p5c+c84Ut3PGt&P}Kn+V$&Wp1^Ig_WX>T zy9;If!3xKPjqAgXaek5Ws_?9#;TAI2=O)w(?YPqHw>fep$Hj6D?t8O%m=%r-8&|L$ zt9j)5=p)zp0a?l6Ce#b{qkgH)6S!E8$6D!2+R6x$6^;uV{goYaE|d?aKiaYL4{{&eDeL;=8A}YoUWkOg-Ok+cx;|EzfQ>tY zodXEU`jU_3{_%~h>vI$8g?8Md?95#SIUcL!9`&8fOt8XnVdD;GS312f+wr`;55^Jd zg-H4ZK&j#6e*U%GYq6S&6^;uV_j5ZZgZsI>&4G3LafEsy5-I~bb2nbLBS&h6`Z80* z3deNc%?4513gg^kL|&XJv#@rSIS!+R5P zgnD5Nr{8e&1RjwbE3dKZ`dHz(uu%cpxxR;`W?12@>vI$8g?7}U_E+(`K2|s`Y*e!G z>-yY;dZ8UPwcWeCu8$Rt3mX-=J%ZoLcE}OTz!7v4>V-yY;dZAsvIqM0$DLLxN96%j;^1}+pg^hUz`>bfRj0a^UxgRUZ-GqAK zI@d4ZdIF=Qzp5m28K1R19%6;#!p2O9eddMtadcgun@}&bN59mGf4J(cO&Gy!raobHz$Tz;kd9dGi9Gac6a8lhnw07dLaK1ajyso+q&Fv)Y?;r0Odp6}cJ75v}WU6Y7O_)N^)SpPNvH!#Q$-2HGLOkmbsxSc??RFehP1g05m#Xgz8F_Im_Ch=AIlHdUO{l^IY_!|) z;2c@kw^gdXRx*O*SnP#%)N|#{x;_xE5}^tcu+eVEwNssSePv~?k7KbH+A)rfU)RS9 z6SzKfvNN2&TgmzfDSpMTk&^3D{`2U+0=B>-wINsxMh)iZ~W~ zp&j*{UDpR9K!hqxz(%{BzuGA4`qs*f*BvtB#j)56?WpJMx;{6d3KOu=ZrAlaDeL-* zIP3a27JH!`^_*ST=O$EP0yf(1eBvNk*LSnCu8(7}7ur$J*>!zxLKP-pqutJn7N>Q6 z9E-isj(X0n>vI#TFaaCwc3t1e;bT+2l^NkFG9%2f*bD8b`s}(s5Z@7@3KOt(23kfe zvaT;%s=oVWu8(7}SFYqh)o0iB%_o8tCSdDKx+n0wtm|7PRbNF}rROHp3+<@q?7F^n zvaWBL5LFyP6^;uV?e@9BQd!qmR;s=XdD6hK*bD8b=j^&ZH=zm>u+eUxfAp4BdLgO$ zu9mqzj>TSRM?GiP^|=XEn1GFT`yA&1S=ZNGs=oU2!zxLKP-pquoA_ zDk|&xYD?93LY`o8EcQY>>N&fv@3qTbNvl{KACSapI zc3t0PGS|nk*bD8b`eN7h)pQ6|n1GG;_;r08i@mfxeqA3cOsE~Zu1~fjn23NE+Og%a z>-v;Xh2z3TyRE6^cw{@rBgC=T3+*_9c3q#FP=yKDXt!g7Pi0*n&Z}o-G{Ldh3+*@) z?Yh3HvaS!r8i!DY3D{`2W1K&m-JgA#ob5lzsEA{+7us<}*>!zxLKP-pquq|x9+q`| zSIBjKxs2{O7JH!`J%e4>=O$EP0yf(1n6rnh(tBO{E5D3VITm}N9X*^~*XJfwVFEVV z?bvyxtn2egzuQ?x+Z>C%(2kzkuIqbR*7cPVqJu-I!USxz+c|(2WnJGwxf|4ySp$y6 zUTDX?#IEae6RI!)8|`*3L+)xh&VA72U_!n0{%F_r$(=Jou)=X+N9SbZ?wsS?&%~9;oQ#+p$f-^jdnY?RJh8NoQLEI?^c=X<5=v4c2qKUU7wp!g$dYbw{v7t zE#)}%mC3<`dLaiYGP|zNO{l_gVWZv7^-0y2vrc9Qv96C}u@~A=S=x1dZbB6%V58m6 z4odZ!HlrLOVt*c3q#FP=yKDXt#6W&&#^LjWUipCUbopi@nf} zF_m4{=O$EP0yf&?*Y$BM_Ch;Gkn!vKSYZM-i1>AVZbH4#j*)P5UEe-=CR{;g2U+2` zI>xonSeD4TzRfb)_RCWlCfEz@7}48xeQrV(CSar8J}asu>-s9lJj18*bctiJ7uqp1 zVb}G!300VYjduIYYq+fID<-oaPsmd;j>TSR#|(>I*LTV8Kz4B<Ku zva+tPiahP>By)Wni@nf}IUu{P&rPVp1Z=d&uIrQ8D3gN;^+FDvpNn1B#|p=Vt+RNZ zK;yl^;KROD|E<2%aNYardrK`}UQm4cUFI9o`kfbi3ufdg+wKWGCd3pWMhL+Qe`AKf z9^(m|+Y|In6Jn%`V6XgFt44Br0weYYeVNX$VucC*-i-XEkOs3ej{SbQe~9E@g1r`R zt>CS__-V6!o~o!Oz;g2ddh)X5UXdzf(g1>^}37nCwE+PBt zE*HUGW6NLc{r;LKoE(1(agh*r3c(5!{1qHe;66EeAINc+|BoTqD}QiF^KD3+Z=S$d zA>I>0{y)qL6R}^^L5|9DHX;WT?1f*O`pW`mJMI?Zi9NyK1m{<&!UXa~zXypef2#iM z&;u@ly|C9;jh^S^z}}U871F({3KQ6G(XTt+AV;v$fX_mD1eIVfoISU9o#W&HafuMJ zJ?6Ypg$W%0=+_;q${BK9TDdUxF2`aoTm^qUGt0?wLe9c_gur#q3KKYwqu+5~C|6Xy zlb45a?lQq%xHijW&2)17DTG|L;qgMS!UV2I{hGZe@T;65*lIVyUg#xq{hH%i=}XY_ zOqKswaTC#R-j|YdDiHx%z0g-iw_~gjcSwAkSFA9B{xP~86Xl#LCi@D%iV61Wa&Kcg z9;M`*Dk{Vh-d}MoA7P*lSeP zWOJR{9CFo$_Xu&n5UenPZ;#+QceX=3YsgW=xy1x~)qOYF^j9_qdY!>e@52fc{7nME=LtM4M=(oz2lOROuoupr=zVas5Y43r>L&y%Oz?N`Jb@qNoSH4YTpt&~UO1q|k#p+Olb8E%6M_{cxDN6J z)~+oU>i7Os^Vf|pbLy+kuLbheu9#E1V2o2=p*@yhB@qGI6PUlYScc|cLTz{zIQ5my z!2~Ob2%BTy(Kj<%KYW+j4kpxw*Al0`vN@PwB@tnBWPMV@Kd{=hCI=I0!>f@~U)dZ? zu#$+dIj)!Z&qyA;U(AHs@VeHiuWSw`SV=_K9DQX!b&>sr989PUult?)%I08#l|+Qi zF+k3#@p9aeg9)|aRp8WDHU|@|BqD4MoKrY|k%I}fbvsZ+*&IwDt}2NLo8unYSFiv2 zrpdvC+VH|&Kuu(GFu_V9!sfVAj`;_l)Cglcm{1#DIGU)UYz`(^NkrHj`{bOuqT014 z2NP<;3uh(jE1QD}RuU05$D&`~%$W4>T_y(;YQqax7wQt5g9%m=5jKa@GngDqs0}al z5~#0i4klPhMA#fopNJfpOsEYn^mfthV1kuIgv}va9&+|oCKGDItE*FAMfO#MU?mY@ zbI1`4JI6ee3AN$%ic?=jj(LP&B@tnB$e9>+&fQEV)P~n0r@pc|m|!IlQAub|K(46p z8}=H^WI}Ct-R{&^HU|@|BqD4M=^4UKUy{j$+VHx}sjqAfCRj;C*c{Tsg|$y)LT%j+ z)K@kK6NsxyBEsg7o;uvs>D!r58(!E8sIP1eCRj;C*c@`l3F|$I3AN#cqlx;;=3s)A zM1(!BqY0bz4Ey^nv`-b2YuuoqW@(Haf}D@^G2*&Mp%pIsJ6 zVS>G4>s=;TVS@LYC$P*p=3g}~l*$BqadjR`u)+i%e@{Tq#P!84Pi2C=c+3z>u)+kN z$MTkta}Bo2?2yU?d-2#OmSBYmz8*aR?HSf>?VZX5d+``9mSBYm?pHhk?cvr+4kp+O zxlu*gu^JPsFoAp^Y+s^1b$8iUOt2UBA?hoeg9%oczy%#a=k4qw`lxu)+k+V-Pln)(jul4Wux^Ubr@+^FBy{u)+k_rSkVhotj#Xc`g&|#dDdl1S?E%eQnM}Gd7TOHjNWU)K}8C#}Mp=eHeZI!2~NzV84N|IdnuX_b78b zG!}c|T!_wJF~JHGIQ}4P4!PHwxeU42n)6Cyu@}y1ouxAO+6ci46F84S*c?&;m^m4# z8FH9lFRqE?T^S-+VS?)*d2@32nqbwjDgN&Vw+d&5FZU+*X^@ZSH4_gu_FndL$9#MR zdbIRc-|cH`+C71Cp5?*Erlk7izc4Fq;-O{N7}^sk>{;$hM1WSWA-yX^^2rS3-cg}( zuQ&5I$WIA#T>gbm*G}=4tlTEw&hOSOJ1%tFMNRyBnzatI!bH+HS9EB&BC5B+H-$r=7Uxn&AIi~-zE%aw@CI5SSTZdU;;<{q(yggSIc5+nf`FChq z^J4zV3zB09_PU{CM{kAW7dbhuJyAH^GxcP~;X!S}tT6FXw%=P~)vi3d9k(>D8s6IX zy^Nn4C&v)%l|8VVcYWhfp50e&KiNM@w80PVY7& zW6$BF7=pc0Z|v#)`q~?v97kt24ZoJ&J>#oC+JsqQV$Ka+yxxV!x7zL4zNb~V>!e?O zt6oitA=vA1jo#h^|6H}zZpYFUN#Wb3H}ri|u5FkVCT=~_#XDe@r=895M$xw6_nxT} zocmHz48dOckM{D0)}C%>b3Arc%ka9^r-H3KZNscE@kv@|Z{FDSG@IkA$w}c$Yjp~3 zcq=J}V6R;(yL*2-vM|l&@GPhw-dbaBXve2*!mKdi8`8l$I^{Vh$F{lk!d-6K5c;N2 zaty&Z++R z1bbD?tndA>aAzk6<|>=lYZcD@J~hk=6Q|l#qa2t`YazrI7r|cJ*H@(+@~blRSFyrG z_sW;VZh3}oIV(&gzgCv^ZoQkI%Gf4*x0mc)CfKWY`=aIu+Pzz~P9WobIfDN@+B(b% z6W&Won6tzl!Mh7S%V;TQ$sRdNm|(BMSD#8hAZMb@G5YMc8CS`fI84q&R+zXV^<4T2 zIooXxIothT$=Uvnob61oSBm$?^nc`vvN`tEE9);QSJWuEqF7-fZOMuByX301Ir8S# z^Y@gi_9wY&nP9KlmA^Nb>J}w4v`i>8V*^V%e%6)3085e5=hNcLV=5&fS0s_PTcEq4e+N zj$?Dk9mkJ5&ew9sVTFm?TOCawCwDcQL+)z+?s8ZAQ0{6>uvhPAzfRvEcTSt5*0AdS z5pw6eNba1hFwytepXnaCJKG#`clN*P+?|&w^TB$FtN6CDN`-k z94l88WXzRnX_r(>Ot6=)bV*Z@*&ORG8J96yDzbf2k+H(WjdxazRGgkbyS<|_?vtvo znN)pDuvcuIxM%koUuCIYf0XK#6(+bw^#pQX|0Q_1<23*AFXtsc=+wKVOI%dIbt1;p zsCUsGOR&O3Y>szc|HX$Kg}<6-#^)M~y>4>0+~#0{6()F_Jb~m|n=)RWe~;fku}=;Y z?6uB0f;I;etT4gH#uHexp|tH%%(poj3Nc3r+(B7MMA&n8kGx}9T)vm;CX|MkUcoj86Rad6Y>sy_{^$Sh zlSLWG!GzlI(w@lXV1kuIgw1i;!Uz0=o80X~4kpxwm-eVO2NSF$B5aPEYEJQg`ax0% zIharzUV3M+IhbH25n*#AmAlh_@$OwF2NP<;OYd+t2NSF$B5aPo2lVjw{i3RQ&dG$@ z@X|ZA&A|jKi3pn`cWwj!Lp3^@989PUFP~G%*c`75F%5u9nSqgq-b2VS>HZI%9*#c0>qPnBc149FLH5JW`lo zuNBT1r;NFO$nl5}tT4f&7f;~Txedb8 zx+Frd!UWIa$rHs|n?f&1eKm4opA;t8i|fZ&f)ysXX7mJVejE%gkQM7#PhDG{B8~1o z(~En?_p*|B$$&?_sH5+aTD1J}8K&J67%uBkWxcjv-p>fL;wGjYc*M}2z{ej2eX>&B z|B#CStzKK@S-R$qeK&&@CXUIPZT*F@?`AN;UW;UA`-XnAobBLuGgx8b?5eWf_HR#j zwxfrvN1ZCGKe3jR3HGWlGxR&o%yx3Lmi4H3cLHy|u);*TtfB8yX{M87n0VJ03Ttbb zV6U$7q~V`B^PC(bgt$xyyp_TV6V>EhfZPgmoE+6;b?IhVjq4`Z>t1Eu z!jA4_b#&z23?|qMzqaGwOPn0|RlDWg4E_*zyk<82OB zn85LWc=~cD2fv%a1bg9}p3?bgCr2$gr+UbmYHT?xOyE3T{LxA$$8U0m{4T5Fv3HqZ zFI=0WdM$Ty@VgnTFoEl_eaC>4gWt_yg1yj7Y+I)}_}vUv+{C~)GzY(%!31peLSGr( z4t_U-6(-O>Mz`aroKwHazM3feiV61WBr6A^$3w2aki1J~-b!JGi376sJbFAT$~o0b zjv$T)6YP~MD)V1m7{4{`snIZDgk{Za_rqgY`A`wjOGn*+!ESGgCu3HHLd5WNrb zyBVx7f#Z++pw01=oFSW}m+K|JiV60@IUT*9w~%u;Nr>q}u)+k++VFbZsjnjIEF*7ou#$+dIc|~l4AY%=Gnh~tUJac3%I08# zl|+Qi;jFAN^KeY44X!Ac^+=8!E9Ir}P;3AN$X*{QE0`zk`Pl8CT5_}vU9)P~m!sd{k8t<`WAO{m_!wY)>^_9)R z1S^RMo5MMR<{rg_+VH~BM15s*Fu_V9!k$-h=M3w;mI<}tg|ia%mCeBfD~Sl3L*E|2 zzG6acc;V{8ypPSn1S^RMn?oy^eIFgl=6N6VK8Oo1jKZSxS4^K(NAuZlBGeTaI@#m|!oiQO!C_^X3Z(R+!-Z<_YK#+$`^AFu`72YsV6- zFu}(^vg+QCb^1)O7mqSx309ck^EmP@Sq9!WV1m7P>=R3{!USKBv2R*1!CpLuizQfL zg8P-&cQcq^FXWDnYqd|r+Z?PgfqWorU!pyAd)Ze^uow1WbbQVPD@xY~f5ikVOyKx~usL*Q zLhiM>Ot2Tu>FE3w6Ra?S^B9E9!EahH!CqVw#lD-t3KLuh$=5tG()?+cy;rdG(UyMv z9npK%Wd*mX*a@)_VxM8f>qx{vE}a2Cx0Tc?`i`$T9n!r-OS+A;)E&cK$c2?G6@HvcF0dCSX4v zTpqmY%Rxf?w$STux}$vPr_?4U2NUXr91niCA~i9ZfA+Vt zLtEC@F@zplckBcUU7Ek@1BJquJA^7sz~1}ey5KFDO@z2ze$}AIN`;Hx zx!2@iLcNfq&~@vBzy4T3h&Hlb^tXAX!q?tz2~{{Q>`Ml`5d7)MYC`0;PV=YcRt#^t zB;VvyEV+IXKeaLVTCq2Tm?=by*7d@BaxI|>$Aw*D`-{Q5e|t-a{?bz~{IOB^u(M`c0^1hNUzqb}*m=IfB1bg9L z`+D^kgRM{0mK;ZCrTc#w*gV|(&|LE+0ae^Y(tsC(Et1wtj@#F^^M?<$4BvakX9!i8 zfc?d)O~Jk|PY}ZUSi1l86)nQ~yIwUF3lr*vU$y^^7lZZ6U)jzRnA}BHdCzYdzH;$c zL#V=WVZXj^Q?S>dmO>P{N>-9T+A>_~gPjE&i@nhPUcVQE8Ji~Oc> zCSV_ZT+YPN4-0WZ&i4CXXc69hYLWR`0Tb$l9DAy~7`$b%C*Kpu6JnhZ2Tof;6^;x0 z+3Pn2i`ThCh+H9RooNO(q8u>V+J4+_528ckCu1ddU6bB_VFEY6(?1F6>rIHU{sy z<7FWhN-z3u$$H^>S&x|nu{$jIA zg@;buYjQB5UdS=$hc&^DGb;O!L1wpisEM znWH8L6Y7N=+lM|MZ1_}tAv{8agt+x@OQ^zeVRx6h2t8yj>6Y7N=pMUX8 za9{U!LR63%ZqVmDLY;rIgen{tc2TJl+wE^JL_4Vydo+I`H0Z`^CI=Jh1!BjPXM%Ia zbQYqB5RVEmO9)mtF76CXq_TW^ST`ZY3-LmW7eX6vh$YwycAK)RgC)E565`imZT;J- zTp3O+w!p`C8Q57LzZ5+FTW%WiMejH@Po(*uo!TOtp6NCBC{^5q_VcnX{6t&-g72>k z*SIC$jAb=0h!=i%DL7?Si+0EnC0>-fL4Ua$?0S5x884~gCZczPHqs{!lfM0W>Dv)Y z6*r;1wye6BKJhytGQO~cDonsG`_W6m3Ponh-5^Vd-O{(8mcAW1m{2dYYp*TiK_MOx zqQ2ZeSmC&^|2O}o;PdM$NYC)S5Jlu}a8&wsH=$l=*IrxZ0A$M_l)J&udo7^~$A$fN z-Is!obiY+{{3JPQ%iUmJv!)@A#a?LFUfUD+PVNRj39%~45~?r(d)V|%!Se4sDLGz} zyFs^i8-*XMe3Qw+gnA)|_S&96d+Dh=$=#r6ZA+-aabXvds_)gILCNu;^t+WWt{1NJ z)MF+G6Y7N=+G|T?DZ~-E8w`-U0V^CA_Kw{fgH=EKQgT!hVwLpmPZbWt5$c6V+G~3P z0U>S?qO#l#SmC&^cdgkFd~OxqI2@_)Eq*J)1bb55Hq-%7*y)s-XGY%gpCd3av59VPWF zD5={tSZ*CYxtO7B$0#bDQRBmKiz-dPvORkvcSZA9ZVd{x5#-?n+UH(a)TC7%fEW$L z$i7poRdbitH8??0s1evTN?rH56%e<9P<^IYN93{*GM5@Klx5e`SBC$+2>r*>H9-bHN@k=AsZob`t*aJk&9#`vc%`52| zoS-Pw5Je-@%)jB8uZHuupWtde?qws$!}Fp&rcZ?Gf2ju$89`%M&d=)3%awHvPEZtT zwE4V7-LJqI@fFjd3av5PnKJ&y07R3L@gj{j9acIuB@tSaDt*xW8j6= z>a2a8fT#myySdA9RbFEw$iwrZJ>ltUwc(cDKwxh|PCl7@YFag2gA){m1dc72H1z>u z8IbZ*1UzkkE%da&;41AylT83Jj(c#JiIO`SKj5a z{`;ze;hu8>?c5I|)Q3`2&^Q54$pUy1GuWR*dP;Z{YS0r3{SP2&R_JD3;PkXR6ZE?= zSqdJOl$sNUS`RNTrMDAF9f?9Oqg1tvhkidC#58!?`@rwKu>E&V9%q7n|1eq$#6e}b z^`=u<;!HT&qW5=6%A1D~)~HR!mL(_9rdF-VQXWPoR2cpExYaiLL1P&h#m5{RBP^vB z)>js7cGZSvd8xN7$-@b>yYJen*2+7mK8!O&*Hf}hudJne>muWI=LALJ-)TI^WXZq0 zzEc0Uidwdn<#mEQJTKbE%WYMM{8$bew_r4CB#cI7vyVn`f}&7^Mya57hPF?781cHk z$vEQ0^CCfGXD}iRzo>RFs^+n^yUe4|zAz$A&0Q#|zLDq#zght!cgyT!eU!_YpfNy` zr5tFKgwestyJzScloyF2^R}p;a^KQ597G||s8yqg7P!SY0_jZ9Sf$8k2?bD`@*zcIDM-Wl7RCC@&Ik zyf&$C>znmHgM;V`Bf?AOWYdC28%Knl2^w2AS$e^!bE#?Bv~a)6x(4M%qUAFfF^?&t zYdDC)a2_v%(rB|6*v7)i<4n+~I;_Zr_HN;TG+MTWuXGK{i^Oo~FK;+gQP*$~PvNMl zc70@R8D$)ycP8lV0E}5e8_sR?BWq?$28BnVeHr@8jYDed8V+I?ylKcj?}&BcC*zw2 zXM)~YK$%z?-hXU8Vr~A_Mc1IbNTj_Nt?n4F=o$`UJ6x+Z$2VHnbu6UlPqqDBiZem) zUQCv*LnUQm_DE~iuLX4t%8SH>S5fMN;HJ8UgV~ z^zIu`BLD2BzaaRTkNPlA#E#FA#Lp@9QSs};iTw5EiQ5yqB>i`SqEKT{?YW|VhWWoB z$b(UxUWN($&i(&>RGhdlXtu}=U$Fo0M@3Pnadh)cf#0J~O^^qp{@FKO;1}ip_oL#( zqo}C@zZGxOC}oYQ)ytz-9i9tqirNNtQvb;Uzx-|^UY*ULwz!yVk?OpJnO_gSlIc8zJC^WA#yf3y<`@^Av}zU?On{EoX#Bk|Baw>n;? zR0Ks~rZuOdMfaxl>_qy~JKV+$uc8y=;dzl5krE}sT6x%sZ|lR{E>y7S1bLhZ{M}&N z*LGC;577P_Bm(nN6xu~DMu?iVoBo0zk26v3k2PY#4XRrYRk zYnJF{-o=j{?VjVu3jAt4r)|XC<}Y@{W{cXzJ)A&0N6bip-=nt??xnK09Vl~OzgBd% zJPN-{h+ipn+`;?dCb#A;r%I#+u)F*wU8hWC(y3-aEQP!*xQKf zy|cL`Jb%B#nV=~AJARqe@wB@i$?o>lden`3cwV&K+YJ!-1$&#uu-y;XD#^64r+tjj}u0gqY z6lTINlR9dg9!g=iok<&X4f1dT?UA?J3H*Y+O~WsH5x0!X=j$4rpeWS9FOxdz!5IgN zxP5yvM%N$@&x>|=(Y6A=U~ki?)3d1C0ntp?-~>gX27YrCYib+O_I6RXhZlWH*#&nOn9!+JRS4%DM1e>(8i|{zhG}8@Y(&mY`(5Rxp)+6;4_Wi z`n3@~;rFp6X@jmo9!{W*zd`(hy^X-%U~pMY*Wd(2p$2`K*hb**ynV&%x(0c8UbL|+ z!7td`2rNrNs(#QlI6+aULEm1s5ow^jYIZA&Uc!-w=S3S!JN$yZjleQ-?*2kQOE~wM(T>_c5B~*0QK*4^b*xu@ zYJ?)iJ!s>20M>B-`%!U%#yD&m__UunzW3*Ig?%#m6=9!)o_0rHZQ;TF+v`kiRGWJ^ zf%bvy0V?*8Y#LuGmUrtJa<7&%K~YHLx*w=IdPUpH&U5>>xJCcfk_V$cvGE{DXOFYk?B?Y+Wu;~-8>hM!oL?eGEH^#Ch%V1Kkh+0%c&VE z_6K;TpLg&lL2rAUiJLLAR7Xo+54US@f})Tf?ir>!+U=Ym4@M2#I!DEp_y2xWoalUU zuIgx0a}9n}6onc`cg#~AEo4rR2cz1z2v@O<`@bI*C*qcetB%$y*WgD*QK*5oqbtBi8sxz!PT>7_v|hOeCnySOdZ&|GgFG0;34AIYtyiwW35r6R-o2#OAP+`y z0)ID-)+^WG1VtfD?_N@CkO!kUfxlo!>y>M8f})V7cQ2_m$b(Uwz>>((dgU6NpeUs2 z-OEo69pWCeIe}%OqxA~mp@=vW6or4McP}=LF%MhR%9x`=9qvJ!2r7detyksiWw+AP zkM7_EkHWvxI~|+GYc1I=&y(ppxrY;IW6j`by^dO);P%D;SSo^|kieSA(H5;7=jC=~ zyJ;7CuqMNn9_rEClhnkNT$8{=i=LALZ^Rp2- z4)5PSYHB0>sHpXdQCL1=>x>_*jcB!DmYd6!2>qz|c@Tj#y{)kkU2`P3^_);K6+ux* z&{ePz{3)STbQncz*Z2|sB=E7)pDTVOoS^qc|DB+y)aSvEib`aRqB-JdQwg40gFMaz z&Hny>2>Tpyq-ieqPa>4goIDssv$6m8qv8b3v;L`p68hEU=Rr}ZL9?cR5}`zp2cu}- z^Z$NSoS<6!rv^&sN5!LP-ZN_aXM#K!#R-}Nomztv6ooX+Pfksc2cu{%#{Yg)oS?ZT zsWtdfQ50&>OqJ9Gc`%A*?ELRX#R-~IWYoZZ27kB7-!ArnH?g57D%$&;`t5`NkRT7* zoOpa?qnI&<-c9@`f})Tve|e*b*jMBi8sxz!PV8~pB)kThe?gGPnV9$cCb2TB$=+k( zN5u(>Li+RI&BE`+cSm1MhqwoAPTU{9S%io;NvW?clYg2aVGXo z*dja<&;3G!6BLDXrY>>f^5vvoXpje^I5EOIPW0Ln{|gOHP!!Tt%v(jTk}H3qK^~0a zgm0d$V)vRkztG?WMIk+M?N(8F#*klVkO!kU@xiiHoNwLc7aE+PC|x5%yg1yg^(TPa=6RiW8?Afkw;G_O}54iJ&N?{kp`9 zHjR5Y=1ly2c4-y@M#Z0x5b}stzZfav8Z=ttOpM&SM$9$0`^9gN6BLE?n~keQg|A37 zgSk;k$urVgm^FzWBya?x5zMVb8|PNy-|2m=$ubev>YprFSPZFFT_?!HiHR^PtVyXg zV#YX}{rNAf)qj#X#^V)S!38CQEHttG}h-NfDdRc2wly1lrX$uN99HaHed= zMUwLNaBlf(t%|w^CnyRv=nb~X(jMlKWeY7MU)*0wC&aPc0ucvm_1CWKEgu+OR^uL?7q6S|jwtcN6XzknfR)~!Z>GpG zk);g;k3zd>ibYgih-ZEm*6JS~bW&PZysF0U0oo_7tQP*8dncow<9-Z=wffc66=IfW zHC=-|oIrb1-nGJiDbD!q0BiL>e?Kac23Af*P!wv=m4PyqR$u0gVmx`b$Bpml+8m!en^Kg#5eRFZ0pnHo^bPw>T#^#C^FVEoITKigk z8P@9S1bH}tcJc4gqH#l9yEFpU>dV(Q$hiXwrXnZ`HD-;D5mzqa8Y~Z(Bm5%G7CCXC zjUW%ti?(0Bb)sD#oK>8@jJr}~)e-rRw7GN*PEZtT{N=M=6s?A9$%erUx*S1Ar8JnIO{q87Mv#Z+MLV|9 z7O`+Z9v~*d>`$+qd6f=1lXMMEP!ww9nYu;nEQo6@W8pluC*)P!bJ+;;@Vsa@UJ)nW zPsH^&xK{tATM^~Z)+k+r6BLCSWmm_EBDrwQ=wX=g8(6)Faw)+^kca0*dr|UM@nJNs zE4l(@;+EQ_l$c#JbPY~W6l%Dg+Ed3Ti1jO%0pS8G_3FK- zsNAnmL)YL0MWIH})_Czed=wDnVXgk^yROQ#p_O!kJUlPjzvoUA*|zltV)AEqC8&g} zQvO)ZR0Kt##`ClZVsIN7h+eQ(-*Z$|CGPVZm3w$zwA+Uy3N1@6AWlP>Sg3myWn}!F zR0Ks~rml?=#L3m!fJg!2z__Z)_pcE;K^~qLiOZuCM8WFYVEy3lFeBavh>cDJkHWf^ zT2AoAkU3CCb*-Y@zBW(g9%q7DPA1DdSgRj>vzjvfozMyLa02a#^Ag07sC;l#xK{s@ zYgJ|2iM@J@g%cEo+SDS0_0Dk5J?2+a@+A1_1bKK~w0kZ~5aqu2fVKL#V!bWwYE<{g zQN69k35r4uYCpoPVt95h1M&5$jUW%ti*{IOf~fFmr#rM6fY`I8s^a@)k>I&_6#jjr zy;TeCD<@gWYjA?1bPan;A9j0#Gw%Sz zJAXT2_wc-E54jvKGM^d)8tH%-KCqJVzHB9#=i*WLcj{%BEMwqT8$GC!@^2LzK^{(^ zy|h8R_$@X9G`yg^%5{okk1VB>^DArwd3av5J(tIcdzW#KlnOvJEmlN{ zNoUbDI6+aULA_Vl{|bo5MT;oS(%1;{@VsbO57{DmKESK~bndqY3aGcW5(| z3CpIm3$_vD;d#-v9*Y&tDwYO~FR<2fX_qw0+0UF(+9k(7;2uJUVXbBE&X45HJ2EId7mvcf(^w7c;{`+}Al}#s@^Av}lKs|;a@}w* zq)f0@|I6JYa@xOKbPY~W6l&1Sb(19F|LwVPUk4tbDC{G|Z0r9lEk>F8i zU-%j=`Uc|OR99fF{+0aIl?j;_sJxazyUzYZQF`FieW>TCab#GlKVwo=rOO0KucOH0 zOi(#*vh;Cg0g)wlkrRs~-#mtku^E^6ckca0*d%%)7@$Y%u$7>|i1}_fgRjlXtr6MQ_HK^2vy~3b2czP(W z5`Eo9kca0*yF`O6;^=34P9dCF_F5GAP?K zK-vBp%66WMN8#V8)HYe#0`aBOBe~m78$ljUpuG|L&Ts4DK6h(jt^Vw5N90kDTyzai zP!wuVsSW*Fs115tIU?VCZX?LU^P=r2sk=i-J+Z-ZdDY}%hO!-_sB}h+h21S;*=8); zp97Jn;c|IysEr^GC(s^x!y@vX!kr_#z>3_MzEfn6+@*C5PEZtTN|fy; z!&-fvAP>)rcHSwGV#hMv)p08jzI$BdGR-TcA}9(qu4jo5eVgK$uY>a_nCL1ed)Wx` z@Vsc7TSbU-8T$c|12lT&{46$KuB>Ztf}&94?}uwd&gb2Mz_t2A-+vPIuGt9k@Vsab z%(GT3IgYz*+Slp}SgWsVaDt*xBfYj--2dAf2>V)n0c-Vjf;>Dg+WBB?aZQRZ5WApk z_qU#uPEM8tJ6L3AAZs z8%A-Uj>#-4o^uPcoIK0Gd@>53N`4NhLQn@A{DyHAx=-bGeN%_ zlcg-I)&F<=P&s*dDZQOY>PXOg3e<2A)8T2K48QYy_TM>qoC*5<|6Hpt!&-g)%*o?S zING9VcS}l#M-g(LO~#fbC(x!=E%c*cWFo0s73JMl+vo?4WndJIM&NlkMp)XyTK$fj zU6uaXUg|AN@^Auedea2oH-NSJ=ciRx=Du^W@?1O$|4!pUQ2M}HeV^YdD!Epa*9r1) z0&RL@X0oh+wfZ|@G^%e!+h`OgC<--blnO=%q3zQRM!d>xGLCrhyhza4+0V853ar() z@+h?F{h*QP0>9ewFmjh|w{5JCayb(;1_&)=AUeV5;9ekj6x#Hj6Ez&f6F86H8by>w zTZ|)+&IFBRnk?O+?Nh#X5#>Q_l&(Q}k)SucsNo=Rt-jZ`yvn?6#*tKKg2rT_goC!v z^<8~_p2T>W;>i;z-n=*Q|aYWdeps{7R=P>Ggd}=l&(eJXZL3xp& zH}0t6Alkrr90^LJC<|<3;pA~9XjC2Eu|RvbQ9v4HFAzKmZJIfN8V=$y9Mz*PkL2M| z#u0jFg5D0mSB{|#_h8f`xq>Bw#-q@tm9wbfAa=u>hPLyL$bCN<-!wQA^v(iWmZja5 zZCj7X6~4OY8k83aS~-gv4&o$StFp&8${RZt(zwT&ptm*fjs@20|Cv2f&iA#Tu0eT` zpp~j3@RGyiqPI)w8k83a{yx${3~c4Dw5V6e z?ZRSX%}G1nVEu|T)uJW~C%A_bG=~M}t-=1gugoHDLG|ROmqL{JWA^JafxM>I1@kT!gh@V?SMkJUlNFjvdDH7cohNpTt-brj5}F@^Av} z3Y9JDvXMRDsCEtOEK0pqt;4zxPeo7^YS8}VFcv;@SiK!Vr>#vU4$%qn@Vsc3*b%Mf z%7?A+Sp_hda+9UHyX0ZZ@!*+r^JTKZkFK<%otiay(Vz{~`s}YC&F}!_VAhw#PhQ?#FL-^?P})9GWYPjK2VqJjZRRj~@64iNS#{$vs`7*IKua|oCd$3+dQN6Sn@)kY5TKy^p zNT}ziKkDW2lqTP}sRo1wE8N40aG-BATC1McFtR6073YtQj)c z2=Z{kA2ilvh9SGWI3`m$V}9{!^kM6=bi;HFB6!pkAcoY7R&V6R@shmrmx&%PPg$>D z9c&}H#@ zUacA19Edu%j);a&b83q{+8YQSg-1m-1MYO2RvlKegm!3JQ=K3WCti)(FTI$)QT2Q+ zfyVj=`$VH+<+Tk{8yE;4g&I^d!29!j7HMvc>e{Id^>l(fobZP8NEi{T=Ezb9G-jTk zBc5G!*LIbuY9M$NYEaDpJJ)wwQ-A+Aq19R9suSel1U@l=g*K~!^~-}sk&j`*{K!kI zUaGKx;8CbSH3Rg7%X*8`ku9|Ca|`JNc{tGeZ?+Xnc#?B|Xj2S*zA6yMf?Q zk#Odn8pf%U&X|EHJ!8G_`r1YN=T>F|!K3h~95v47dYh!41AA)Hpfox`9!|J{M#SAX zwPMpuppp4@e{svJpEl_7Q@ulN{J#8B+IRi6HW?r41bH|y7&H!i+N$oG z`xHviF+19c3B`tKU-KR^5IhPss4jt4-@9qjJGbH5>wf!nf;^mP1y^^#U-4?1euqIL zva6R^`+bZytxTkW;8CbSb&1Jx{lG5i#$G=Sc30C0@^FIJ4A5rC>@7-Fo~Z4q-(DxE zF5-0tYK-lWsCFLP3y!M8_x$yj_72i!CcEkcc{s5IuGN5uM0NAp@<1G__CxBjCP>Tr zt(1Y_QK->;d7|3%uZlqAzFbI*t{kkTTm4vYk25juXd?Wgp26F`*HEAPx&>=pEJgxp zio*JwzPbT#s`D)t7t;r8S@)kc?0fD^&|c+Gt9i_nhWLbN+i%R)3G#3P?fefC)w<6^ z;HZu_UnDNn3ejqo&zy>&C_F0K10DAAer6Wxu@KF@`8K_$NFJUS?dC5M)t76l!pxc) zx6NW(e2CU{aW0kT;!*f_+MgWWhJP`O4M0qtXCuhN3ADFdNmP&K$9>NaeKw2Jqk=V` zd{=Z0PEZtT(Ej8m%l7AHaSn)szu5@#@Vsc7mL#e*ta*~)Jq{3|p98h}ZEIM0E*^z{ zckE(b2wG%F1hlgeBw4u0wI(^MaSmLev|tReTnpYjA?1P=od-hhG%5UfqE> zZ70aX^P+trEJ2-j7xz7{d&Dg6e;lJdY8|C(aDt*xgZ3wfR$shXTm@oE8yi6$o)_)* zhvL;afw=E^;RLft$}mg|2tBN8aDt*xgZ3vkSvJ8@odF^x#72;Z=SBMid`qawH=Lh% z^srfc?cY!HnEXuF-~>gX2JKI7vRr^xcpBe+T5&r;9-bHN)9_8Cr`vI!<$k=no4aa> zIZYbR#iQ`=v_HAYGH0(@ln3Irogfb<&~6LgcJf?;1P!wv= z{^an@mmP5CKqT7<^6{k^ZMuE7b4LJiuV9A*y0nnfQV zGA7vw^6`Y(9i944Ng!LYB+W=|D>A5C?E#g3G(o~Xopo=r#734^Qp(Z zFpE5WK3h8t?5%5Xf}&7^_CSZX0QbydZtu_5sdj=qJTKavbH%9FhTy#Lg?G*3ZMPKb ztH%R$4Ng!LYS13&Cd(iwOGW{a_lb=l56_GCCiv3dqT~*s5p~rp+}^2H^Mc{J1}7*A zHJ<(4DLv|nSyTWbx1Asl&x>{-e19;TAFgTexDF+QYlL-hTR&Zc6BLCSv9<0@Fct-FRQYDDGmI8Hnx?S$5N9`aeMT>M7;pPx+ z#Maq*`-(iy1hr9MOvd`|k8w9bv;wQL3Lb?=^7KujdNIC5J=Aazl^zrjpZ^Teq$b<+ z@dWZX6ONWlrV?pHa8ig?Y^F)&QB^>rz^z18nmJ`3YB-2=`zs2sh!Cyzv|K9pI1|)H zfmYw1CsL=?!P*F~(<+Zz3@z4wpv8Kqy*UXpIf!>h8i{nRgSEiVmvs&DI1`SR%=jfY zq21Cl}D`wjorT|s(%G`*EJl(>Bc=okAs0)PuCh&R40!!L2VS6>9q3?smF=& z+LvmhtUL;PULG3~)GkjJ=^75g)3={E`#nJOPYBR8$m2{<8wFO^6uc=(b^NuU;ft(1 z3dc*bw}MfwCF^w!2eJF zd#P(Uh!O{4MTb9qw4T?W>Kf#6CLAr9`0eAxpq4(`m98(WJgP2eT>UFfy?FYEuHhh3 z!YfOEsok~TK4sAGo|A_YrQu4CT(d>pmgEAz^FjUQhzc#bYQJB~Y#?|PW};S&$+9Cj zNjf;7otC3q9-SZ$CvXgC#Y5Pc{Z;|c*qMH-n7qPE3;(x}f#6Z7L9H5?r5Cy&d3|qB zt;hb7Izb*zgn~x9Hk;H|BP)Q$^71>xy*qV*s5iTJ@PLPKa5unlEY*C+_?hG2?*9Qrg zKUM3N_QMSXk3tPsQ9xu7xQ@f!8B zYj0h{L2R3|P?|low)Mc1KwX18&IGlCOqPQ~JBznXQ`DgqgLQ&D&IGk;;QdFHS5p3$ z2h~n{Yy^2Yfur?#;T=L7{kt-MCRPwPOE*@_PYTgBh~QE9#CXG-@reqyWG2N}#NmUH z$u8wXH10v0T1a?xDU-=k{FGS?S`wr^|7aUqqA?_Fh0$0K5;RH$ZMfJaqU54Ltw~T- znR_^aHhlvYR=A1D(zB3>TJb5iF*3@SRYkMlRV7}#PJq3f;^lU2v>K$#~Sf*2DZW@Yxfj4$J`QgiU;dQMFfvR4O(9f zE##>pY3GsLa$4_6Izb*zpzZT`t*8^x6Etq*^cI1yYs(+!1*Re>3N>hbHMEvqy{TVn z%Sid=z41Ch9-bHN-={~4>XWhObqM-ucT*3N>gAH+(yDGR$WPdoNFV)JG@C z!}FqjA^$q@BLv48dO(l9O3}}9j)}cf5fp_Qw1(Sc*&SmRv6Eet{v|r-1bKK~Bxpsp z$)r_Py+}qKz3xit1i8ix;pme%xr=z6PA2DAb@8*(OW<Utj5KC&u^9hUyPwA($&H2pA zbMYwrJFUouwK6c4;R(d=c7i;dK>OhSts=uh97&x7BNOSH4^tY|J)~=Jf}&7^R%FAv z2^dMO3B=~#Z3KCEUbH{%iWhxiai#Y#c>l4dw4ahmj?^_cK~bndE3!?Nklir01jH<1 zBgn(^qWv}~L8OhxmENI;VC;2KfbuH$WL<+36orJ}lLXQB>;xbdcd8-kO$tylnI>Dg zhvyv*^+%ie31ZOp#X!WmuTScKY`k);&u9a|qyB(C)rC@t;-B8$KumqPK(v3~SZvQ} ztJSD3!ZyR8J*&m=%zctk&ryGTU0zb!+2gv12@BS^hZD1a_OHHH_#MWvC8=1X@a&XZ z{(X6nf#6Y?iE0M;c4p~KlI73(a^k8$ogfb( zcob?-%>Yk(+MFV8_hdN|W{hwTC$NW9U~i=OL&6#-?aLrhY-Nl*$YqRy;8B=~%6Vuf zt{N#CS6?Uh&0r(Q!wGB+)O{Z%Qto5@(I!uRv3TrZIZe7@x&{$E$^t~Da?!%?FYFtX zoU%;#zdR*>ygJxG@F+Ygsu^IM`p7nEp5=jj{(FC&AP*-NfkyL|F`{8h?BgVCTr4^~ zdnZ@z(8oaVDAb^u0p8at(Ne`@X_Qyhd+G#vI8g~SrVLys3Y5j3SInMC!vA#+<=)ti z27*UTgJ(A;V7<`BVSjL&<%nqYG^aAWVS59?qwuJxW`JF8K2Mc$h8I#EWNE1r5DEc?LsdogfbEB|)P_ z%{gNHLocOpslo<=N1+DQ3?@sKL*2!5xQ`>l3h4xSIN=K#*D7og_Y3C(jp}cAO0{!# zR>ri*ZXkG60-X7?s&OLgTKp>en6m3d;cs1($+t5b2p)w;<*0E2-*1$x19~d^2B*;p z@^HcfG-{uY6Z`U|0gZq*{YBXp{gmJS2yFi zc;S=g576+r;U%Q+V-$}vkp_ZCp$63@@a6~ZN2Yy#%3pQY=mdEjZf?f%f)u ziDKoIB5+jmltto2tq|pMiOi`8io&C!b>h%V&5~2Z9t%;%^w_5NEy=_4qW$oCq6pvH z6Xr7%{caZP;zN{sb8`uvi$~$#X$?2@w&AViejvuIvJvFr1lmmwCW>XzxHjAah*zV6 zmC$@wbPY~W6l%~KZj;4y#w?xz5uL|Ikca0*J0=va)l^&?-WG_E&wEg^?>4Ng!LYS0>Pm|p_#g!cn+$xe`m=S6!V?6>r(7p@HtxdZQ= zefudC*&LBbtO2ZVXDjUW%ti}qmHH>&XioE?1~TH!f1cU9ixG$}k6 zkHWvx8g3{VV77XGAX4lEc{qXgbJ+jt^ktmo{pV@3DCO2d88x=BuE7b4LJeBO{c}Ep z0`nPkf;>Dg+BdaL;&=kiRu6>v4B79yD_xVSrXnZ`H5@Cw%fdYUia=Dh6XfA}(Ow6= z)Q$CV)_oHzyrWMnuk5Mkp=)r0qELg@aNFiHq}^6tS!XB6!}FpY1ikH&opJSmeLjN% z^BHsvPEZtTI97TG#zF6HpNq24PLPM^MY{})Geqyibrbzy{?*yupJl&+y>$&vP!wv= zI&qlw^4KhD_xUWhwG-swdC`6Y``p!BgZqBW0m7quihT9)09}I<6oneJP8`0N4)5r* z0P)mLkca0*y9w-u`1}RVDlQEDTJLwNoMyprU4s)8g&Na;t|p%fXI>hJ9Cm^{JTKb$ zVUI@NbvW0#4EzR*SB{XoxAoICI6+aULF>d}4n52R2mqp~ogfd-i?%oHR9I^t&e>Mr zT8%6=Q9he*ysp6sib4%qCk`uk;5?QTohaY46XfA}(asDzE5H1bKK~v|V85$Z{|HgT@Oe6YKh?h@9TRx&|jG3N>h* zxXIEEW+#X-DWa5}AP>)r_7GTq?w1xPPwcWa2Q!3l~&4afTP_b?;H1VlbN zK^~r$#+GsA8|-WFA)V;fwxF1o7@}|wC$Mk0^wDZDt{4){CzloD2c(Gkt%4Qq;RM=r z6`($U_pfwl*8@?+Gc`d`nCYF{TCwIWUfoWY7169gZMk$@psqn4o)>Mpo3L9?(=XE3 zr;_|WFf~C@*t`E{QiM2_4xf_gY8SEo?+J3(2LTHA@Vsc#Qwe(~oO~mBB}|cb4opo@ z6!t%Z4nzv63qI|;%M1`DGDgS;^Y|&;!}Fp|zb?v{^5gt2QPTK~ea2G0oQ~ zA^pG=ZXXT}5=H-1Wwrfqg?o5jw5gPUafVrEq;>g@$Qi#3O+`=?z6Cm7DO%J`kEPFt zYd+#|*%Z0twgC$F@VsbKX$SM$cO^>giaeCN-szu;ps3}bVQv>Ao(;k^4cTXP5v!|y zmhZavR=9`fMVm@(lO=1n#nQlvX_a$ddZZ#KYA2Da1YOmHnj%eUD4lv z?ei=yD}Rr0O+`=?&Iy>4W3xE^wH#-XgNS_zg6kcb+5ax9F<0yp&nt9-bF%M;mT#sCqTG*k&|cv$8ho)>LuEyHiny^_?l+i+#w5iJ!#Q61rasF&i!k?=#H zaiV8`aVBYu5?&!n<{q9GZEE$yOu%DfrA2rAly@^$ry?k-Bxs~8N)VFHDiMJaHtHXctD7jof#mT5*+5G5l+l<9I5DAb@aC%9HUmWgU-1C++Q{0*Z~oIso2q`-V1?X<{u(p!Xe3f401 zS|jz|D2vSJeUe*Gi-nDxUwIK@R&Pt$9YHPM|0a42p*Mb z&I)N!KSfNbfW*n%H|32{hvesD+iBdxi8ObXOKaLI!t)yv*;ieV|I%*AJ&UzA5IpKw zz*6bKfd-K5R zRARO;skd)qv8V$Q*ZZ%Pr+>#1<=$A#Z9D*o$6n!b;9|2fy=OB6 z!J|S~OqbU6ZX%?esPWi)o?PmF4rN5f<{I~KVsFLi(v;Fo#FH(k5j}C7oOO6!rD2n% z27*W3-5V;Ed)7o$Dm)N~d`rg46>GXEB}2S4?y31PR9e;6Q?$9@o6NOMmX~RSygKlP zoNt4VL4y-%uT7T%wlxuH^(Qg7Z7;dN&Wm!jUPgjP&F&N?soNWiuQ7NBm%hj=|Jfi= zUf$DR;~q`~ZU~b;Wo#m<*FoagymIon31j3%wT%RiivK%Y>T;`*ke}f1{K|#ZB1^B8 zV#4fTje9t;cUQP{ziVUB>?9H`XKxnmL)M7GJ&Xj8I<;z%^s#Uwv2i*Q|L&|TCk)*w zr->b^@$1vD!DOk!QBTn~t#2|vCuor>o&vMU*;V9(XbPb)z)>QV)}3~d!F$Q^1GJp??uw31r0?| zB;LVA>34}6>lTUV&cPb@aN?C%Cf&Arh=|5WoVLab-{aH8lxaqSN409TO!}j-hsfuH z#I{qaD6?UPnAas(;~q{NU$av3ZP-9~n~-QM_KK)m6NPo6k>F9MMz55DQxwslA`%1g z9uWtwjTLD?gL^o^>v>q?Uhq%h`F*em9d0Ce6tCxDPHK*xvRm$&@_~Q+4H`VE+Lskl z;Rr=knu|vzSL-2r-K{RC$vsZv9!~Ij9`=--wMo7;YLUFL#sCAsqj>#jvSf&lmG9SC zC{LX;z)%lzg4gp)C?o!&<|cW_Sqrt6b7vi=aStc>`N4CwV2QkMc$~bnt*?RL zQM{fvS%&|>u)m8T}0ekK%0!7-8|BDIYnsOYRuvYiKiY z!g%IW`@Bl4v1eujFG6TI~St?)m+<;!``$RExb z2_9vv&u8uNms3jrDUUwd-_S1M8pbpCIa*4Ij=w6M3uvOv_z){4jIJaK=Ioc;s+7&{ zG_-$mg_W^V`=V7uHZS~rOj{3oI)6JLosb%8+{1})nPR1*|5O%po8uk)nz^*HxWjBQ zC91K3;880VY?PvgRud65k;r?kv=ZHEj#&7jk%8b*U2?}tkG5BX5m!8_XKRWm6Tp`? zmwnSh;~q}LELkrt$Wu#1ltqmbw+kzGUCPN&Nb&DM`D_`?9 zlzs2D*0_fgk6hMCJsZ^#%c`NqyB>L!Ml0LOQ`2}E2p*NvWu3HPa~-j*2VN_`{8^Mb zO^3-@Key4ihZEM?(Nc%5b;Y#Cs8RN9cBTL80Qt?dmIi`H#Y~Hqy2aKN2gjkt!msI+ zAI0X#g*Ug=xQ7$>Gek+V+PaH2{ZXTBk&MdVzry9rajgvmk80}^C56v*7av!k#7)Hc*PbxtJQJ+dA8=2sdt6Oqt!>oc#P4x2(zOqD#pp4p zp&ba7BS(A}UaG(0sCX2=hH&QnCd++;euzbp#%sli{CQ%el+$&^qpPSfy7y7JM8>xA zo^QT}tIMPKorSpuamVBgwc5*rm-rg)IVbot4g1{fxg++s%PQ)$57zkEejd9)^3&>w zU8Q`I`4PgNXCBYR$hw6@Lg8SIdpOZ7e7!XAc^xsO9iGRhHZ$Z(9j*&6s5v>oqxccR zH!VVD%F`C4i2NP>HSXcW?=RL#gHO~E+8?N~^87jI-_(>dj`TGUJj!_HCqG?~^C#Ap zqfhiV{AxMD%NSTiSNef?Kl+Zes7YTw_`Q#;Y9U- z4bs{8TB2t(YMeNHU9OxzpL}^se*?jzjAwp5DMg;LsDS(t>T~Yl1TXX99evX$B5T?> zX$kyl`Pmlex=AWupr*+8s&6tsLYOd4h?IewRIB|Potkj@eO_5L>&m-yn z0(t0%aIxh6SOdYM_z}WdvbBrlZ?_hUDI>;e+{20aCpJn|8`l)G7NEv~5%=YDt^XE1 zcK0(7Jj!_Hmm?p^&CTz`$(cTeT8$ID4u-XTPu_@}C4!`WRf08swkD6w(z2*(;@s`N z$@~amZ`}21<#ex?NZTJy(hMF(#Esb`U2_~A3wK=3GjgitaZ zT`8yU+C>yO>!)!KCu$_cO0TtQBK=I%`1IqMeC%edxajX=Ab6DV%c7UsGou0QTzyDc0z2Ve9DqRyv^^YaStc9G~Fbn zl&&J~PDhOsZ{Nzh&UlFv-})E`9%Vf96`wxI+lqA+J)8G7wE8%~TeYw!;LLRL?^`<7 z&-`?f#?N;7x6OKMdC&R2$@~c68*bk!$PKQPmWoXb(zu5cYhG^B>0FiZJl3v>l6Pl* zT)$~$)iCU_i%!@=S`MTL%+!LyA75$H|%X7c$D$XFJCUD z;1_SZ&NlW9keHCCc&dJa$wEXoAb5X(6KH$x%A?aTa>q2B35r4@R~M6=c;5b!j7L@J z-5|ZkLLQzMuLAzvc6I*~!K2XL@#eE+BQpM|plfuS6r^zvC-~ijFW&wKf=40kRr@cy zMkYJa-L63%XX5_&Q+9&R!xhe)=R%vJ@G4*??8CvEYWuZ9f_t0^q-_MwRX=){ghDDOd!l@BH4LVAG)*_-gEBZdDp^KK*Dwh z|7K4KCwLUv>l@9o6H)g2(H`yx_i%#W8yKOt6CIog9))zExu0FbnIMldv3Y52r}Mx& z$aA4hQFwls$#w_n%#q+8X98&((cAtT`~!ZqoIsnRjQ6|`Be;hXXkUSyuWcGV?1bu6 zGVmz0b2Qsk-*_H$&&k6HJSY6S@t)I}6Tzd^qKmr z_c+Rufy2Mo=xTpT$is=1P?tO_#onV1&5 z+fL9^f>)R4LYt!SN@J#9-w*OQ6G+<#uPe6t(jCeYo{LAJJ@v>bJHelJ z?%_lPTm>X-_ngig%MwoTD762c@|T_9^*Q%&g5MkHpW90xY-MnQN8#T)D_@*6h#-$M zkuu4oraljz3vG(R^TSNWdroIg9%lk+8$oRbZ1wS6Xj2rwPsVm4_izI3Tq$|%8oZt8 zOi&aOp6XWopTS*i|2%d?OMHHttC$ID73M>vJuqk!?K-wIKl4?>?6$XIrne^ zZ9FHN27gMN35r63?xv&FhiA?`JTIOf-5dByB|CF$Q&H<4ZF(bsZASdN?Wj1xJ1QS!O<(i-v=jnl<}VPvIO%||AU`7{vG>jsY@U3aVDsr zX#9O}0&R-IBXsl!>1oGb6ndzyh6EiU&*Zotzar?^jT*eX!d%o7LYw+gNZ{XX_k$DM z<4hp^>)#;dLYktC_k&vc`|M9U^)vX@#b*rrQNMn6$>U5odUyQl@?2cxdT-_y&@#=@2T$x_c#;OZ*%k}sLx5ckftc({h%|) z+8Lv$rH>lCeeEc(oCzuckfy!}(p1Jct`+s8$m2}>XAR2he1xe9{C%K0Z>v+^0BQWY z?dsBbAi+IIa{_G}G1#eBM7fZrC|;vFY6IuiD=*to4_oK9r`@@aLmp>>`V6VBE)kfQ zqVNc*2Vg9(&_gYKBzXIpGW~agqKq1Nb?ewiSg^LF);4NTD;(=!{JZ06|J_b-4<}Fq zX&ccjeMLF_y%%n-_SO>R;!(!?LA3$awRlw27UfqL|BkJ2V{JeLd7KGH-++GS)ZQfm z^HLO^pQEMEpG5RfE1Vyp@u>bAL3&0FIx2kH(L=3N%uDTE{5!P(jrW{;IDxjK)yG?w zyp+M(fJRTzcC-NaQE?9^u=b(01nfulrGlL2$#b_EaL>60-g6qQLz-$L+j-DfANO#A z*N-r3(%z=-_zMjh9YUH)SI2LV$`bNm6cP9wa7J z4Fr!G-5Q>&f>qR_Gw}UK?Pizc(Q(nqgC7slxQ7#XPSgW{U1;Ik>X)-0O!7(RXCQb~ z`W&038^x=r+cKlZZFnp5U~=i?lJHiBdpN$9m|N0w2p zmI+k{^>1V#cocs^O_oY+ODg5+O;x89@-#fVoOrZ)qZBZtntIy}#}ns!7FSNsyrQmJ z>#1=MCszAzlswPXQ2i8q+6Q`PQSy1*RBt!yY#?}4zWf`cE1lic&@!ko_;gXFR`WvE zXE&N?+{20Anr)D3=B%YI@bf!)2^nYqT&BJZk32^-}q}Zt5jnquP+%%Jdtu zwff|i8uxIbY>#zPoosc~yOr^%o(AMr27ZyPq5ds3?%_n}<#p15I(5`eo$;s&^vtf* z&<0uKGPgDmJgUcvXlYI1x@wq$8aq6)D=A|JS$7w2t#J=0+;hfALz~o9qeh^{iWeD` za~Ee>E4j8Y5IidRW0a)Tb65M$N8-Ws4nXf5S3z(DY*)#gYk!b4IA7e$R;^Tx|xXJ@y@!u%`l;l%3|k8Q@_9;|T>Cq{KxD-HFN)%S;yIF)or zjA%SWE$3?_c+{AhtE4O?8>mmdBeDI$4-vF;YI4PolQdp_c#ejCf0mkRhZlX5`EMT9 zBD;PS*PBgEK4l8lxQ7$-V0OZ{4>i;og;8Vhr&V&ze;cYF6Z{MWkK(_17~h?}TrQ_I zR9)T2YTUz#%vquSxL-qEFb_4%Bi_iv%0#N0@ANScJc|E3ov;5yDZ8dM}3b+LhcLW^8c}CvLzj z)`lLn)Q?s1JaUg&D)+uIQ?1fytbyQB{0L#TNZ2B|%HdC!#^)9O|AL!5Tl?h1b_fi+0ylM=Cza{0L#L z+V_W|YWnQzWA|W<#_nekDLo9=HQcoaWEljUmlDYDt+gZd!eU*jH5@R|YE(bqpIAE@bN z?G)f^Ab6DV%!lQ=Dq3EYRGb6JkG*TTDCzYENu9XZCz$ps+FvLS$)fb5aGq;J6`@__VV}dpA;l$RQi>3618>zvQkhn8*hnSN%S8YDX zNbsmEv5TZ1=NhW=I-ABuF{A8K^-up`je9t8KKFbnzi(sp_#q@}zt|*B+*z)^0}W2_ zsMfGXF(O}MwfJ)+IuBec&bEkDGXw-{+{1~Q7s90UbDO9aT(CtJkv>YiI1;UH8)zhW zRDqB%$^G{xYWjLe%+0k}q*UId%Hv?xB=d0Mb=w)zk8n@5XI~`Vom(oN2f!EI`Wp!z zHQzKt%30e}O$b9`!R%RL`mOD19lv0WdpJ=Q))F=x-c-f4gfJsMU7Uz7@Lr986^ESQ zQH^2UW$~AuDz3YPwPbVC$V0Nvux|J?))gvYUo# z+{1}luyWQnz*EJQv#=Iu%D#j`mWXzuT_N^L1{0e*N`&bk1|`xp(e1=bX9I(?#^VAE3b&!Wq~Lw({{M`tmZ~UB(hjoHvN& zMn}1b`+w7V%$q%n_j+?&Iv(f85LgRas7fvMo609YJuFqb?k{5rCgzNcRc#95=b4=hl21b-^0Uvp>RBC4IA>-N( zpRk7eW8o~OFALCM3*pRxwfTH;^HM3*-$TX{Ok_P>!<}(~=Z9QckN4}B@n4SZm+r6e zX9%o?Erc&fo?gt)Pu?pgH1QYdL)^ZJ&fiI#uEyFIMGk^4?uk?0KI7479w&sH^ zI`Ws!jFG0?3uoqCOyC(D`fka7zTwliQvLb;Wn5dQVQaaLhkJ{8^;~g_9^##2bj7J_>dI#{^pYy=4QB|f#n!xoQ4jus{Q&87l^M*ch6%g|!6^ji+uLvYf|cg0Wy|gqV#+M*D+WSM>o^< zE29+tZD5jA=VbsxU@dGRJlDD;@qzzsk8^-JW=q!O_O ztd`h9*vWKzz#Hy7A_aO5l(7U82d*Y^)6Nm`!An|?R_57!JFnG}BOk~RSPNSSQT-n~ z`7Z0@q$6>GGL~Rsna38ce`Q|$(Vv##;q1eIc+gI=`Vh_#Sc|Rsthb~2df)A&o$49P zt`8H~j{^I4r+56=-~uUW@<18ac5}TX_*#jR*kzHw23rW@gL}vC0m26eEWyP1RY@Fg z%8B##(R$eK*u&TOFkh0r0~rEqVGH3jlJR@^73bzllP3nsSb_=cbAp+{I+UL{rlIut zZ8$?yZVZ%+gg}PCTG&GP-b7kep;P)YX`|l&8DAHB3Na16WJ!B6QFBfc0^gC| z_`+FZye?F9TQglV{)V$=(u`0sHh#JW?*QRE8DT2;?~4=lmN%4`Gfhq0&VKR5`84*wqlYt=t>z$sz!U@`C*Eu+)BVsh`8 ztyI88$XJ32d~cv%eJv(#mu;n5l3mt*qI%IZAT*b~`D8a!c{JQXEWyMg>u(44 zTa6J%y_^KZ9w4>=F}56mwHj_*T{0|koH%CW6d;VD9#%VxrH@b#EWyOBu(%SFPvgX} z<&$YW{yzlPx&hw{&mR;ZPA;WoZ2VSCY_Hl%UtkPk2`2Ep`TrAGE2L*VjmPc@;?xk@ zR=byOARjz`iM5BqnP*6X36o-H%^ast@yQN4m$VNoCTHf`Ne#Nd7xEB+wT9X8nn%SU z;<(L}I6d<@8F+7|)UI%bj3xF4-89=wL&TL!r)%()U_|w(%GX?yAkD8cl_>)g5qx{i zi+;i4lS#CUN(YSjc~Z9Ya&{O)Fk07JXm0ogi4z7lG;WdPnbIN}?w}O{gK~3>wG~Sj!^z zWyy;R{$hrP5{m=8_^`0*a=ptefwgd-2Cwr`AAWeKr5st{#`Jbf;E@J#wKLw}{xX%N zv+StFS~pVmJKg&}NF2H$1gshYbGsGH?WHhNV+khk3=Td3AXGq9hf#|OtcB-e*vXX5 zyIo+W#x=yF3%5GVqCmVXo7=Gj6KvnD@w=D|yJIV@gi(uUH9VrGE&Oy~+S)Nr@!Z?2i1eRa|k9pl(QU_+APcZvn0&C%!2qL2}`?$coy9)?B4`K`PmEfB%K$t@r zpUV+gi!I{>tOk={H3$U)&+Yi$;B`e;M(wgPumlr$wSzNKt`(D_BwHzqkC5>`0k_qP zkL@_`d=Js@G2Jsf_*G2I;C{qG>tYEe@G1(u9ctbi+A1IFfeEaI*LHYf0ir?K{lK>4 z-pAJCW^3KvXCU-MycfZB!ezrw2FmDHW)&u|7HgHwhnakYnT`BZzcaJETm_>rb@Vq|F=ln2`mrv-@~81bUzep-(wn;!CB z<4dJcd%80O*22&2;0yoyj4zIUCk=^rW}fIV5wL6x=e>jzYlyUrl}X?D!mqca+jcGt zfwh_jt>Z>Kb`(X4694Sc7gAD>No|L_$So>u!V^ z>!*jB3d6*~Qpfdz8b3ug|&m$NRk5JooW zDt#&Dn4BU^;Cv={qCeD1c;nk#+EAHe@|G}BZ$~nB?5UmTvx!=jT;ERkH^oqTF}E8- zU@e@NrApoZkDZWa^H%J$wHuRvg$c*$Dcp|qwqoWATE@Vp_JX`GPP|yTGecl4&8HMD zz}-eX5kZL+E$oGtE91nLf9x3oYq6PU?mxN-lcEl4itn~(GS4uPYMsitK5QwL1ko}Y z{_ZBs(L1Plc8evj7S5oAH^UsA1kFWtr{IltGS*6l>&`sTlF;k2gaHt1g zf#14-wtCS`u&Z~16W_IG&S$^`K9vo=gt@VsKuWK2e;_vuOE9q)&cFH-XhV|b(wbK> zuos3^T2CGsb!G^xg->OJOec#@!tfhOq{FSwGL~Q>zJChW@<&^8E0&hwyw_HU(bMN& zG;&}Fto43KGG`awj*Qz%37@DILg>jZ{Ca&NPph27`CREhy6>E(!RM;6M0R2;AwzD< zryX#Tu>=$NdjIWC=#HX^sez#vE z!D>$szo}4=u>=!W+_!M{jqORUb{*VT!%DE7zJy=UuRB9vEqp2)tT-zy1>e$D{GxTKJT1I8E2!Gry_$4&SGltBfU>czr64vl9rhfE5nD$y)M>PhEGH z@BPz-A+Q!cr5jG6-TZ_L_CgCU|%@kh5Hf?%CjK)g}DR7A*vmyImLpYvJ>^A$Jg-KYYw= zgi2LAGbeLnq8)hdp8fJAI?vs0@WU0mnF@X3i5^SNgQvjk${<2L1#B4|Z+Y|c-A#q0 z!|dtbmU`Tm4Bh7@e)av$=A}I6^&}xYhs9FDg*K zD3)L%2Rx~7KaVClPpTJq&z*ak3e!%q1lEcN|L%ZwBZZ8L$Ks#S?5D z4QqH4o!40f1b8MY5Ll}VFP0<_yTDQs1_YL1Vi`mM>|PEex+p*h_)C^%7xUC#f(fjJ`!VD| zLQi~{S+jy_^#pm-vT_4HhcYv&I4IB)05YXs3DB& zF;K=5Ogu8Esx0U z)I*Q2g|(i5hu-znKS>0GUVQRB zmcUwx;Ool_8AEiwzGjv=B&>RUzJ8rZ8A~vMdkpxgq3^yQS4tK@zrqC8!u=S&s(adm zZ*x1FFB}^tW7~0CO##pSf>jfUE()*=zR2!AXBXdM&_o$aFflm0t)|A=AfgVScd+?Y zLq2qR3h%XjGDBdkpXx4}L;Hfs>iLw|YWAL_6fNK#c7@AWGB}iIj3Psbal&*BE*s9- zwiQWX?XJA_kvVJ`h`{XzZ-zBINwO?@@dxhCW(cf>TLkh&6E_es>oa*dbFPfFUO@cR zvSTQr@l)K=@bw}4sigAR1Tq4muULYKi4eQpHXxMH*e&>XA;LTI*f}m3BD|QuT3aFV z>pC%%(8w>uYoYJngT8wT`Yv7#@O1Y#2Leklf!D78 z^PX$hC`@22yb{6lIkeS+|FIfi3-OiUy=|zu5!`dSFTv{v)?&*z4);70?)k8C_Z;76 zyu*R~U0wWx?+Pmp-izQ>2CqI4T>`7FgH@-?^&17qbsT%r!`c2KY#dJjZ3|Eu7mc6+6 zD>_1vV4@4qAEnJ?V`mzP8}U(1UWYAmT6vu%u-2{KKTGJ9mfBFF2@si~Is!{D(R1Zs zE%A6Ojr^3Pk=U9f2j7xcebdOJv@o?=-9d;;WsGz!FR-Uq!9tMQiRf zuNG+&aKNegwP+bjFhRd%+5_6kdmtq~N7o{4FZbj^FGtH*f(iP2<-N^h;%rK6NwXv# z?Nd45;}r=-i<$OgO=a%D9tAWBxE$fWHofwkO#=-*^3 z$#_O{CJw^)gf9XS1_YL1;xvr&8xIr7uyJ&p&uUVW^iW+O#qdoeOkgcKGYs|KN<8f7 z?)A&RYVu&58)*t{g|%=Qc#eVR+Ba%a=;cQ2fWQ(=bc1>tI6SW;Ww*tcqk>7q{p@sCiSrB%*SoGhBWB-(0PKd+~=q&XKVM6ZH3#J;~&2 z7$utAP!suzFK-$#y8@wT4S_Oxr6!ZbRp^|!0KV^>=(CtN(wiw`3BE4cW*So=!z6f_FRAkChR24bs|n#63W zENuPl%@A0Nj)Iquwvm$;h63^Jtp)MeTv@nU>a86giUbq%{y(gZ&t0oDfOZoAQeYEqHVhOGXrXdSyr75{3n+k1K zdse8qQd>%xduEbZXX!ovH>(!;Fvv`p)!I`#UnvsYKJ@p|@tMRvz!Qk9?3yGku8EN5 z>sEnKwCMb}C2t3De%=#^n+x?wuVL0gjYh6Amf-8s-;);XBsP6rfN1jCf<)G6Ck$HS zSbYp< zgrY^Sw5L-x8I#!oh}cPLa?smNSiYf&mQW=4y0mq<5jo_ne+wWyDyzv`Aa<1_uvQR^ zsJhj%h+Xw2KpclvZR54x!icDbGL~S1_7dj}S;Ta^6%bYDN0JuH`w6Kt>oEk@3V(9sf(cw6yhs1DCOH=4CA=PG!Vp-Cj@lP5b4eR#b0{P5 zycH>2=p`5qH_`S)MS=-B(jGO*C9d_G0`a9@74kZ3s1U@}UhZ6+Vp(_;#MT_>U(CG!lX3`BHoHrPfjLcx+ z=BG0fmf-8s-+vwzkiJ{40#UcU1?m2LvJlxdrvjm9(fTzC*+sgz=K}HH9W}A`pCVYT z+^Ma(BEi?CzZVzpB4dsoL6bZgA{aveC5lIbe287pzS|r0KN;q-pmWZ!~ zwdg#yw@MMQF+gbv;U#25VoQn)vLG7NVSgYOn+o z_^aCB7lkL5V+GN|%X>yl&9N4piC&*8AQuKKg|@mAQi&XdG8)EK7qJ8r_&eP2j^ZjS zvTk9NU~6-pA+Q$vz3q)ZOiApADB)*u7QyXr}mGq#HSoJY=f;|1lRUm2|eD6h;>=Uq;cvjp<&-CJeJ_= z(*Fma+eH>7WB_sWnVPHsVo5myYtdb%K~_F-8Jq~jfKO`Tk~>5AA}!^y1QShQjk>co zpQ!pJ1JMD#7q)iZEFsWr3PWHmdZqQ21;n636cB4y4I;M}&k~yaJ4s6@5=?A{Z{82I zE+D;oOap@arzR!-bA-qSUJQY?aEriKd#0($mq`&qeP~@Q!34ff@I|&#eKPjZOkq>< zTpqWm_V@i=2k(P5~E|6(<{Z5A%* zJz@#0g#R}f+4ULz0&@>`Q&1LDG=uZ42W(0 z5aHs3Q(8iiV1oWOc%DzZQz~0Z#fM zzPcHtZ|ey_xIR{sKE3bo)~^FA5Q-LoGG3fcBge-D1938{I=OZ42H#X1FJKA2F8#eC z89ZJE6M+coVoIcD2YLGj6DtskRxc={)8{F@L)$Ir}P&AHR5N1wzsCgfiOsr;zDUl(2y(7Qd1NzHx`?0+!(G(%(gJ8cvIE z(}C#Q){GRSF5>H3&a6NvT3%4b;8w{b#hMaFpmh)Lp35)1JyXCEd|mo`yJIpD<7WU7 zcT-L5^84|J_Rpz6C|cUl^=&I@nmh}L-%iHFw`xDW`C*p8T7AJP`PEi(zXdHrzk@L` ziLK5*h@2~62`1=oRo)g7q?!Z7P>5z!YFLABHmf3`Xmy7&c5d53_N|x+geg3w7O#va z`)s2HEWy{MJ+$T6MDnf|tw&K`HF*w%p&d(LEgC~w|16QrXc!2@3-EqC-4RUuLZbyN z!33S>-TEYwM{6mu*JdoqXkC{)oWv4XONnfGFQh~&L^DF_q;qx7LG<#if^V13)dm7fFfp(! z2Ki~vWFT4taj0iHX9@(CV8XX7GP==u8W2f9%m%BhfWQ(=@MY0gav>auEf6IemzvI1 z0Rl@ff$xnfbrqCxt2mwebDAZv){U~5@MCYE7ht+$ZE~wI~5gFhPHR>a>+?`REVC4|rDdYE+5OJv+Aop=jZe z2Jdr@R+E-6YJ*|aVhO%39{-R}4ZDn|Bb)H|Ga{Lp0TV5t9!1NylKs1=Rn-#ukyA%G zeth2u0ZTBk4C?W9ZW1|LnXYPfFekgo?YP! zU~tdt$pL)NfwP%e6cf#$9`m;+kwH&=!K%vD+LPx=3-}YgX9`$?2^xEeJd{khVRSuT z^Ic8apI^jpGGhs>MXzLhB$+IkPYExGTDEJR&R>{2Ra=H4!31r?75}7=8%d*q=nC_0 z7a(qxBd``;AHl0e=fvAZeDb16%sPmPEl>}=A1Q?UGYZPMzClfTH$TYNYc!D|uohnD zA=mkdn%ML|%-iOLGV3`eT0=c1JWD0FT91S>TEP?KZ}nxqpl=XEV69lN>Xc0y=~iU~ zwW^;pIq~`!Z+j|4z!FU0^5ALvT00_l+s0d|rwX`5u@-Ix$gFYmAgvxe;l0!Sg^f_U z+B2QJnlPB7^VPQxX=G8`p&Tw7)>;@JYroy+H>&~}t1v-liN8CtGgE1{$Q^zZ?62^( zuom08x1&8t?<$pqDhEah`@pLICT5Ug`vIISSe5-XogCWX$zf|@zJh*ryZKN4eDWBk z9+<#8PWVpsI5p9;|H_wE8p{%3yRu`&7Qz$#sdSP$$XvLeF+fNJtE73`Nu;+6*B-2T z_a%eONpt70wW`#ZFKSZ9sfJ*mF_ftXCh%Tcl{yNns^+c|Viyf(2&~1{!=yBZths9= z7+fbpBv^HO%ntJGMQ5%vShe8(cCyR52ZyayrQZFaCMoevg(fR|GWEa&e#%g#{+wq- zI$o+PG??nn5LgRa2pK8oB8ck|BIwm`!#t&S0IT@S9pv1tP8_xtvOmA6N%EvF!t61f zn0jEM0odPBS#AhjP)1-YhjNC%q0$L@*-i3 za7Fm3^>Qj+4ot8f7V;J1YOTi!^}c>&{3V#6);2TGCEJ%#FHTW$Bv}h%yit=UJeFXB z`jCrX=#W zH_8a+lH|Am;m5Ow41u+ux<4^I*yz^9xCK}$&4S4TDU4EkFFyI+#flQiI`x5THEJp0XcCX8LTR;>OowWM+@R%mDVanf(abc zft`MBSF&vl?AhBGFa*}3_x!};0&?K!b{fF~59Idi(SrY=P$w+G1YM*4#=L5P_v1dq z)yD7!1QS?`tw%@b?fQG7g{fbvGFD-NZL5}5MvwyiNMY%cHqw6RahIDykMr>^q2+}w zC?E^QSe4Lm^CzZ&bhx1|fhaPpgC~a06_ReNB`m=NEl-JNf@j+h_KD-7ggRGliI~7z z)VKIIYPr~A0eRAUmhkZYWN9Mwp8;|CWI|zl39W7Y$_3={swpMdT2*QQ-1D)fbA&am zhBNiR1g-hs2=9t4Y8XHOODM8Tv%(0akTBmqQL%=xJ!pEe7P0qia8u zU~3_739Ry#1BLZf?pCOWLeLrRZw5pwBO~JeVuGN5;tE4xEo>q9MTe@%x`D8skJ6L# z!KznwIi&hlm4@1`#9#CHsVwQwt_ zQe}wtl^z=*Ea+vx=!1e+J@%kOS$_ zZio;w&4?ke7OoTQ;oyEmu6Gd(-dQs_MM2Q>m8>9Ik9SLVlGpk)r)cl>6w)|=2%U`E z$XJ4j!O-gcymyd&*V_T{@_Ic|ySJ0D(Z!A-uok^i!h{{fJDU;HsrO(_(8BSkVB?I@3o!`fwkDyZ3dYfT&Gj~7%qg#lud=ctK_=Uob*b3Drs*^b2+cn zbRmmIALG|P3(;n)D-ujlA5gc?Ddf&0O3VeH%%ST0`H-by41u-S{M|7jYO<$e9$$a; zWX38?uvx|7;JGXH$>QNVU`&oMCaj?zqis^i)qW$uDigQC=!Kp&t1&l1S{mQ9v|#swRUzPv(zr zpT!VZi?!-$Z#6Nv>dX5;COMX10_UVdb{ov1Z8m!HXG7R*bxhD!7r$&JM;_9Q-va|i zkoL=+_Aqn+OaV}#6dQ0oi4U~>z0uWfwfqxG~gFqJF6KV6c?$z zA4+yTCh)rsknaOCT!J|q*c3RI$#&A(0N69rv=Vdhxqv+cvt2Vyf=Xf ztVMq-Z`!~*2s6X8|sQ^9(^iZc`pUk}r+&dR)mXE+YoctmL4UP%sl**0dKNGJ?^H>5_0Ty8Cy#J6>SA2+l=1-zgb~VpkHM#m?ZRgR>b7u z(0V9-2AnyhN^Jn6wrIJBuy{lrfnH0=BcXRq$y=co{>}Z!zG6h0E$c2s&SkR`u*VB$ zk-!~d)i> zU@s@@fpmxQA=j=d9GW+hiRxo-E$d0`06UqjpWpHuoP3!WJ#8PwPtAIS_rR!ifY`*2 zaPjcrpxiL0<>e1~3g!t70Z z^Bd8E%etf5cM&5U0y!L4rE}usots5n^z|W~OR`^U2`s?`jmkHz zX&JhB?Oh6C9nh&IF1YF zy#g`Z;zb1lYt@99FfBtD6W#_^*_5>vmS6%$epRXIK>WIYQCkKku+|ueBGWQ-QRFZn z!Yy?5z!FU0Sh4PYWIvkNk=}DmV69CMccx`*(#D;+g zT<7v;@mnB`xc@N%zo@)WQ}<%DfF+oqzgP566nkHx@$;$hthRDvhUNxD+{+V+R!b=3 zW4}ak%ph9EKG?~)bm=J?u8bD21Yh?cl;QFzQQRL-%kYNXQj2Ci#k%DPtks6r<87jN z#hjMW1?EJz5l6+ESy2L(V1jy5ueaYS{h0|8?#fI4&E)Vi#AQ#7aQB&!6LZpBtm=K`mXD26#z2?zOj(MZt z*#HRFO)P=6u!X8rGl=R}%4jPE+eT{RoJtKbVGM0mt!k2JXhmx-cN|HkEp?K9yGJkt z)}l{gW-&?PTVt9z@ED%F-c1MS=+%ASSt{iy7}|w&5TcgSFqDk_MZz1lF>EUQ%O7n%MpRa3EU16Mg%*r_!J8 z{sNX@qBandMyHEW`v(KDxS1KLy6UNPErTVn7M&~eebU99b1AWtuTEB#=*b_fMhjSi zi8esIn3Ewc89D%nD^QOQiF$HCo6!Q6V4^wHJU%!>yy8WB`#kWZHu+ph?&v#;A+Q$x zz2n(7@wz9?{m`pjn>_qbN%kExO286K(7V~L^)|8Y9XdYTZ1u>o^EKoMkD&~Kwdn5+ z-802AA86}#=%Gih+T4=N!^aEwu2HSH)fr;Il0Xi(G~|WDIG3ZIN)5981uVe?wh&@P zu+#rv1lGds2Kk~i=C%2|^lGS&c6=z7;Cf&hzTp;HjSOkfL~ee>O-nTFuv2_H+MT0X zDfv6Z#|iEnTl43;R78EVvApWLj!@bJ6ZH46L78IDfFaOUlY-31r<$g6U`;P=8JJMC zDDn3W_Ar1adbO$iOzp+o4}9IJ;DM~MVw>1;2hEQ5f~ULA{>E~|qd^RTwP;&K?9PDI zxjztLMP13rT55S~=3oI!FoAn0^j+8ijCrb(M>ZSI5Lkp(c3`M7Pl_fwg>K zE)lqFv9AXudVmk0=GDP+{!b$TOE5w2>|(5A9Xq! zkyj35N zpgg$EJswN&b%VgFL*H`611o7J$NUEI9@n-!q0|XHEY`-vJa~+J=+RFeYti5O zoASjgH&xm){u1q#`7g0_06pgd7s{}z-n(>|VzO@gntypb6KYk5F#4}6>_>iHc5 z;wS8o{tlzn>r2B_10t0WlHwqU$$KkdHL` zM|%gAyM_r|o+|Y+c#3j!XUIXCrHobhy7XyjZ-+wBWYKCU!!N>^oXMCWcP(AYV+khc z?;Z;Z#p32`fLNVVi=62eA>UrpqXMC5(JLJj3&pn0-GI2YycTKh7$JMk)DemVUzh&2 zH!c$AoS;PLauYJ6eU#j<@(Y5mg|+Da$D0<3hpu0SY>{O~wa8kBC|N9dswET&Cg|^1 zzC~hjpQ}I^S=J)Ec0|j4Q|eV96fIiD61yTX##bWb->LRmLXngwmIoG!QFg&V zIKdiq5Qw>HEJ3fUXwea@b}A6#?|B0;0EnBdXUjc@PLQyqJYje_UmRc=1H|0t7UXmD z*>Z*#OHf+T+6r^~-mV4W#}aJd;D%Me(LdMxN16ELL!h|dsLO90|MiIuPf z6SSr2TvFr9E+DdixV>?TJkEayLtrg>rMUA2qG8K(K&y}Le5pVvTC|Lley~RUR0C1Bl?iDubhvzUtxCocd|mqg>l1Rt zniJ?bEU(}hr~R|UEK&X1y2J%zdClCh^;eU6!oC77VGiG-(FVyC-x zfT&+kpB(V;l#H20R;;xcfKrFnjPokY8^3a zM;By3thXRHtY^blvnFWAxiV7A6UY9qx2M6X7T0RF939FMxbNbz0)2N`6>=zVhAcc< zsV-qxWdwAG~MYT~?MWgSY19plYlUe~W>pFPu*15`2QZpG#FoDa1z337n z68XVd{=BO>Ltrg4sFTg6EHQp(S16-zvL!Kl<1E|fHJ7mj6Gxz>?^$Py-pO5nSoxqD z$!yd`w%yZ-A+XjVAk1QSiqq!PJehM48GSp{L%zSYj`j{Jca3V%-EzL%N_z*DmWKBr zVSnW}(p6rdZ>_z9iUbq5JUA0@PkngPyMsK(ts7$%w&b_bpOR#Fx?kw;23Gn0`-jW; z)=XX%<|<ZR3&ejT^@-nzyOPzeKp9K$b?NV``DvnA%y=M-8k>=Z;2msg8d8B! zv}hJmKKK*UP6PtMH>pmF&cb)%PK0SQwiF4zF8w_;AXUs7;i{?}(Kc**0@13U0Sc0!hf7i4~5r>=!1HyfbDVZ@VPP(!-yaJ(U zxj`A!huqGLo}c&zyg2!;izKraGqu?wiUePm{-!?Udr^~s@PSzB)v$3=@3`3&2t|up z`(#p*=rxcM(V@m9Jj7SpF>kikDn){?dl<^78l5DrJ23@_{gaJJx6r0i%BDz$z*_Wv z(D+C4tI0q-fGmqop-m*)j7V+kDiTc4QA?u^F#*$n7zMG3aEO=~WkoRr)}n9lSVB~0 z=3{Er3&_j(M;FbIu>=#eJR14&_N6D1{sbSu7;A0hhas?5Ybb+8mnt`%3Phc5)yeF| zRm3~{qGc?>1Z_7OG1HxH91>rXG@p1(GaOE##RS%B1!a_!ZWi<4Y-E_*Arg1>P!-LW zf1+h9!Nl3Jh@$RvV*!XE|AW9vGfQb5BMe`ZTz>?Uq_$fWn7?-U|rNrtl zIs!{DF{&)Wt2?XM8;Gj&s%XBIs|VJi>(zn}o5d*YiM>TYJh)L=^9u+p!9?}4NVV>C z<49^%vnraOPeoo-B9_jv0nno=OJM2Xka6OD1%keQhUZx0N&*1W&UT7?Pf z>km7(S@hAKRor`}CCP4?qDg>zj<1Ea=x=X_L~)MxPhe@QhsrMdidXaBx;}&%&b`{bLo&P_9d(9Lu4MZQ8Sn$HQ60#qC_hohzedEWwu0NSryu2M4I= z+};7&>T;y3xV{{LwalT6Nk9y;r$krCHf;Iwi0GFcC1VLDlxSwrFdral!Z>dO#DsDL z)?(+9n-GHxf!XIh%syCx33i6N4H+pvpBxcqK^fSRl~CIy&$o!*+xTcOtx6pX`>TUq zO{HI(BV{bX1dSC|g&5?nmvpUtb_-$?K$Mmvuoi1o7|hgH%4TXT!34V+jBwNt|A9TE zy8Y(J*b@34OJ0Lyand1g4W?nYbOFZs)NxWBSPifQ6Kbf3aie6h!E2iRSqxUq0m7^t zfwfqxN`crgJ5CbUg)=J-CTRWsuAEK5s`HEDq=xa~GSXj;f?n|xey1{|W{&7sIe-6G2fFzjM1oddcr-}KO zynwh7^NX8dbXvMnnF8m9?KoE?n4n{Rx_`Qu*|k3q z1re6y`jV$oY&uI|Ejo|U$Y`@=lvq5zDoLnQN#6Eily-(wB$(I$W%$g`5Ywvk0U{Pw z=g^ya^5$xz83Jpud(nDXHAy>b1KH{B2xe!A3A}TLb8F$rE4ZzZJbug&W`~RkE2zis zk=w+ieXd~Dfgz5h#yqvWY~)}WOE5v-`o6Gcn`q`ipIDlfLSA2tnH;U}$q-nJwW{M= zc=B>@Aa^>^hp`G1_$hJ zU?L6bVcRWJe0ZO}n_&t2cFPz$`IWH~LtrgBx`e4a#HGdb{g1lV&Ln7|gG_$6W1b-E zfhB8y>=5IZwb8J|LC8YtaI&lH^Q(i5C78fZmJqdUXhb~1JIGZ$x-kUSV%uuNysz9; zzu|H*_+)4wRXi!!<3ayFA|glJ`MoOCqe1e;Euly-(G9$V*B|GISI+$b zZ$h+d`pU?JfV20;5_DV>zVhR^H(IL{2_~$e9$DeJ;@;4=K)i-Gi;bN9<*-K2 z83Jq3di=HOUth>*JQpbUI;Hc&DU$Mp;?Yy38p9K$&-C%~NW)u<=MEFJ9%8LLaYp7X zuxg$426Fl01UW1IlGZ9kf{Dkp9y9aAy$Pp*xB~kUtJn~^cF_rjz*@8(f2|5{4Rd=+ zs2sOW=d)BKfVLir1QWC#{WN)E&GiSts@}I2k!nLH%Rk%a zYOPWvn4nomH9hjhz3a9CvF3o9{I_9>Y!SSJA+VOV46l4~Rz4-BAA0`vIbowE6bUBc;SPRHDiDu)(KlH;e1W_SCx|qhH(>~@rSs&4{ddB zuDr3bE;gY=8p;z&lmq4x=p`-9qvT|R_e}f)6SQ?zCkw>BlP^KUetT<2JN6Chlvl#+w(R%!~D%3?y*3^%dXTR3PaTG~;;%}_z z4D`egfzdKIZ!#14!34E7=HCKwcQ*&H%2GXzG@1}CH}+9!tx_bIpeyyu4+Y|tynYbD zY5;y}e~4hMdV5a$^rH}1inPOtT7FX&yHh0P2_*^$>ma<(d7@pkoR+$l&}%6Y zOi*k0gH=yLwCGE^`$frBRp+(!P$ZaG2YojX`fk=%dLF=N$o)tRi;^23yv7h% z3zrSwD=IJ|iSYK0v%W6QsaRE>P$H&~-3H_1wgY?-v6`ti`l?7ULF>^7#$dw`C$Oq$ zeR?h#?cZ&XT>ZOx9YZNHF0G z{c2xYzG!Qa0>l%T;RalsDG%_AVhF57>!FklUbWUnWDP_boyFnK5k=L2v&`~vWA>!Jx$*KI8kerBEf_Q)Z=eHgAvrDmg7|U#-0p@ zz*?+T3m`ID`qM+6D_Sx+MRaagvVy2pG}16Gu_*_4P=7Wti0LOci>xPL2`0jUj;NO< zMmV(u;tZ_kJ!kZm>was<5Lk;^Mg8!)Os6Z5tCkG8Q4MG7a-$SUc|ysuQl)l+){S*^ zkyA!CXR@gxM*ae9AXZl2c zgQLuuw_^ybMaysr-XX>YQzCC&Z8G1gyNR4ZaiZU6ln z@;AHn;xG-X^V9|8KvH9QNsgOKl6LdkAarRFZQCTpUeNJV%2nM~OQ(EEZN(!^WlBRHI;3uhZH z{K6%`lWY6ZDySJv?o1o;wQa5eB}c6;)459>y$zwb(plGnf-=F4`w8NDX81 zkTF5~*p_OkVn=hDTk9|r&Y5t`mCk4;GX&PcS<|r7hm7CQ_dBI|UQ?LtXiQM=$8_+1 zJXz}lWt@b5we(Sf!{ zKX3+P6((?|JjCb+!CA%NTWYjEoFT9l+q&0o>XABcj)+$a*|#9@n-7X-GCP321>y5d zkJv5mD-MZ^7O(^pbXPMDJgM(zP-4=JFPvA5t602&C9oFlyYPOk_-6|xe%=1UIebpn z)c+@1+jkWSCg@n14L;-nd6ck+o#nYV>6-uG%@=$vtVP$Zzi-#ff>GNcTlYQ=mS6(E z(*(W@ApF7?l*GXsJea^*_?;&3M)k5FZDZSOGT{v#Y&-o;{nYebJ6xXb9p0+x3rgUN zh1ywEsUap5kGJkEI{J;n?9_?`*1~VT!MO%N6d#>mvKq?368t6_YZd*1WSsY+68I`2 zV-+UwJ7&6iOiHY171qM-ru(|%1LuV$@Rdpdw<3O53b(o{bu(nEH%Q3P+$?v`u@-$+ z`}?-jZkS6(JU=1^XR~i2(UGdWON9G01n(x~kHC@jGQ zehUhA;gE%NvD;*+e&H-;X27Esk9o*X1fTvO$f4K!&rN$jlvRe_HDy&}*N?sMbXUj3 zQeGI*U%TQcD+?y@`Uu`Z@IcmG>nO+8Vs`+PP}X*Kb^Z?sLm-lYpb}-rLJ4KZ!4l@M ze)zz1)beuAQIt@go#@?Eo@5|{4kEk+2m{C+q*}_87uKS`|2_|%gEEF(jgbAz7IOYv!{KBXW zC_JjMEf=+f+OU~Me^ zccosMm5YPLS9_*wa4cPwdSV{TiPP-u2j7U6u>=!e^W8K?H-bguBAPi+6(Wj>$wc z!Vh070HSvLhH0Z|ureP53^*-gv9l3e3>u8Nh5 z7&X^VgRcZ9@PAX2tlxF*P0E#l3C*z$TvA>Sv56Bc<1>ufz5O>8)h}U)GmP{VBl?$21i=U7Amb0oD-eeuy;zp;s<;uXs@D?fD`|X@K zC!Utk09J#ovmX_imLss%n;mJ~4UMCi5J(C79{t7G0&Ty-ZGyFwU8e4<)8D~6_-lKy zJuL%EFtO%l2Df%^H*u0c%LsvczPY&GZrWCuz*?vFZs$_gcM+``QerXe;ezw?w096o z8VftPCEoVpp31)3Ob#WhLzP+sddZ0i+qAs|mw^elDLc7Kh8@L0Z^l6xFJLa&p1-zJ z`Fdb2<-PqT+Dwisa0i1r+I6l4^}rHLtZ~oc__TInrF*oDRPZh39rfH(ryPN`7F^8Y zR<^Je&s?QMFx1?9Zn3uJShDG77B@S=Ml8NGPJ_z^4?WbQSB~~f4#p}>C}$4n>M;n2 zphged(-DOwnAiyAI~n>wXD;TSiGZFy@YxsHjC4)2AIHF zE8v-metSUIcg@P=Ec#~wOrF@d$dPv6c> zle>^TO({Wrec^RK*wfyQCFdhExX=G~Bd>b;YVehy@51=l;OT8o=PNA1gwQsP>+;i) zxKE;G&{0d~*C?cS5EEDne?uPpS9GRsT1z|5u>=$S7bSC>-*V(s5-npj#5u=W&FMtv zL`+~UgFRa~pG=-qyF`i0Fz@!}nrK%}EIGAh6Bppzop`JKH26voV*#r=&bviVBVo$G z#5=NqyC?M^Nv?F>rDx;r=~_*D&+)ae*4fQ#xi3~OBy%w(g5iF=zA;*RKd@xqo>=bD za97g%OnIyJLcfY>ai~HWm{6^W;Vw__Nk&`HHOd9%-9~fQ?=Ig~Sj)8C67FM|8!7Rl z1U>8JaD(~U`3lzqYmF}ZDv2)V&L4WoIp>T*+Dotm6K!F?GSjUWq5Box4&dlkD{UE= zz*?2dzGI@x1f)BF0kdzH29{ugevOzCx}4N~)r`sd{}$RC0)Zu%I0^gGYX?IJ-IuCTKfoQlxjDWQ zy@OaX1NOLfjv<8Zaq*Q@si!Z)S{u8fi0+WF1QUh@Zklepf{AWV{dT3AbRB8GyMH+X zYZ+GVrP)6wn9w~ud|%D5776VBU;CM`)5j9}rIl8Hg0!{8W$VrmvMAdp;{71r@3MQ* zOlaMy&qr!|3EtP@=LEcShW93VTkxM2H4-7QEWreR7J-xT*1nY5 z)cYiT%I?k(SnHj+t7dg2rHr^2>C$!eJgMXBAQ?+Ak#OBr<8~yZ%&MuSmE=?T3DU=x z{bek{1b)5(t1RzGPB$W@zJtdw1lAgA?50U@Qp#BHJVbu+LXzyF?-4A)1b)(kw*mJr zk!sdCCf1b0WlUhLRu9}Xw{mpPqTn4|`&jBa_p*5Iyq|z2n844UkcZqs6k9pDXlg%> zk}-j`@bfObyjgm6e4cUo32>{8D}Z~ zC^cW|7sbuWsUenLVhOCZJKA0Y-*glwYM-{VPOs-$3}_=3mPE@~f{ES+9W+%+gUWiz zr;-hvZ@aeQ*8?npwe%ae*BJi{5;s6AK(w#X2JXNbd(mwVyp@Y2muDJGUiF+>>B3R-WWN%0;pPR^xe9PoI`t+b36E0Jg!OE7WiR&|Z@ut0I7*6)*S zyPk|Ia+VsU&tV9x)qI?VW>VyMvDRu@Mp~!s#Hhtk>D=tuGL~S%IIX%SAR*?E9RZESC!K!Bx# zL$4w9-b}MKv-IAWPmxLMv`R45E z;8)s(e1D$j=FX${{N~J=IU0?mSqq87{ZqTMt?Hp4ZbXC}R9tqNbItR3VOI-*Rw;*P zcJJOCp+Ct*#J*Bh`t&K&#Il+>L*=x0`;v2oa-a=a4-+eeXFiH^M*# ziTPdb#m}18O@A_+h>*=$<<48*dvedS5@@wx=cV{$`@{AA1Bi&IaahbfKFM>a$Z!J{ zB!*PH93M9;T<@`#h+$&oUcxDKjY`tFkW3>kh z>O!mW86L*xY1Ym3k^jg)TK!jQcXg`q1}aEIee)*1V3qFrTkpE$@Z-O=YfV<`yE9t} zw8FDX?J%8pP1~7dzFxPcI?v%uMFP(iMVv_`f9bQ=Q+-ljOJ7~oa(w96V_H?t1AQJzpdMUojjc~n0HtMT89Y2*B7XR#NYa`GK^UdL!`m&KM z7?H-fKe3r56OqU==T7|Xv)%MHZaP1rzi1|NbWUx2_1a3H6`nWh@35S1BfD1$GV*3` zXkgn5T|N}QqhPq6e)14E`crF$(_yk>*^I{ijg1UckU)RxzBaa$ywc_u&&-P9mh-$p z!~E|3S9|K~YYcVc9HV~koIJDa`-|H%C0!rOT!O^$oq65Oqk8HE22$@%tno$+&K>J% zo^6nYKr5Up)oYjFLrpCI^ z3g>)vS9Exq_$Q&XXYRT&mbo1Xyb?KF(>oN9Rkok-#2<(-Fj~jYh259Jd+9w}4s~M- z9j;D4rk6)1@AUMk+0U>jKm`9=*j>J1FMVbt#j(269Z@xCp6A8gAr=Cyu!RoSz_Dk< z!MqbZ%eo9VP(h+(u_Eq4<$CFlqRGdT+OtJm=F*`Z~fYAiep&&i(*vsD9@KFawO0STd02T9J5=L%Gklv?Da?k6(rWbD&vj{>a8DH zLq3kLpCHm-$myA-B1ZzPtdSS}`*V?W|L@6}2km#oHox3e@xu&MkO)pu#l1GDk3P%0%2+u1vPhSIgy-?& z;T8g|a_d#x$usrQn|iaPZ~aSRc%dlIgwDeaRFJT)`tsjjDK;H1;rV{W7z=?`X@0HZ zp7w8VeS>#>RqB^&;?VNb`sF^61}aG4eTn+JgRj03&w3a49Gf@Bz?QTrE!{Wv_R+ss zZoPX^@4UX-A#&y{=c!X{jDhtcVZAHrvU081bv&o%O2x4TTH!sFmAK{9g>(4lp5iL6 zaGY^8ahx2kZ!d2XVR^p$g_jS*caTWrt@Q%#k+L^nui23!bzE_P0Do9wzW5}%_IVjmdPmgN74P1rc zsxN1$H}QKrch}Qts*m`ca>tWl8$DoEg&rhdeej5 z@y8xf?^2Jg7gdwj@$9pm9~hH0j*i2_WcGeRMjhK~soT(H@sY2)c-M)z4pRFpBg5o@ zK|w}D=7t6;NSuGxA-+M%&R$x5^JPGoEZbB0$ZaLi>RQs2@i~GznnbITDP*!w_IWBy z?rWfeM2(J)_#GEJnD+)P74dNZ0nD$fuRX>d*xJUv)Qu3KDn+psuya zDhv8E(|gZa3A9@CS^AJh_uHC2+7+oR#??;H&nq9OAfdhZDbP5@kWuIre zTDK#CRuyM`5)xOWo!M8Z6w%%!P(dR3=4T-j%eOI!JUznXvoC^-j`qd*Xyx3fhfdDc+$5&GsU&izRpX&X5EUdk{XSF+FHlo2?S0~v zM`g*PkOV!cjXz@~Ox>tH`UW$WcKe^YLJ9e~tS35bra9xn)wwQHrQyBhc!{S`D;m zjq933*;JK9q1*}jEH#3tAc13}zWkoKvZ!7>L2qOu&}#0%&RSU8+NO_|*(!^_)h9?o z6@dy84dpN`e|Sxkz%fSxt%_#vqQx()ZT1z8IVwopS~N_Xv8tv?)KYVMN;S8)P;)yH zXoVxIp0KF7q^&uZ;Cznv(TnphkI#0niZwkwv9k5oKe*t zFV$C?>Z_e96H!3|=SQ_ictn_-uEyh9Hrt#C^PqJ+mh}pg`_&ostL==!*=|r9Pw3qi z&CPSLSl=)?w|kJ$Ij{8`L@S&z)YVy?gUi$zRYJvq3KBRgsc(QPVvadq;T0J(rgF1R z@i$L(GJ7{l^;HcsuTVh(bHU;AWU4GKsSnP#SL1;MTH(k#TxHd|3KF>bP(D=Ti&f;MR9_*1R=Ac>e`!iZUQI>5LlJmA z#};Bu4%ZOnW1iW&NT8L~2lZ|pvv)DW@ydx=sh%w2ILD~7c5s-yuU0Z=a$09mJX3qGXrMiuR@a<&n~V;VM+XEMpJZ)l zpn?Qm?bP4QR9Cf|s;}~?$dN!RymqNylbho~nTXd9oaeD7^&4JAoKZeLwGn7#^|4)@ zgGbDB5YIJSA>+BJe!&A{jtUYuW2mcvns;xid3UB75A1imI^#GwTxC?2WKmh-R&k($ z1ZI!J^|nesxvA|Z#^%HIjG!+gwTTaE=r0xwapQP|H;dGgov5MLNKfmMA+sXnawDUW zQ>~UzK?1L$>UWS+%F99HuXvu947U(y)hK18mLax=-n|d`sMkG%9Nl@3=jDXH1}aG4 zwO!4LJ%1I$R!s5ST|CS}pjESduPs+hJbJQOKXx_JKxOjQiCX!_)xB2eTm91evm)Y_ z`V0PsdaI8L67wHS)XEO2u2=KEE6VZDTQR|y;Fe2ah`+e zhgW@7$r^LW!1}aE=dt!#xr;SrDnxA~!ckY%yL_P7`)tgud zw5s}IhBjxIQ@`Q$F=DPEpGV*E+}zs4Kn01POU==4J`wtl1<6PEmAZ`G@u%m;rluAG zt!95QM@yzYMVf6t#Zh9wSveu+&z=?6n;NJfv8?HQt#p4)FYJAnT6NJGnXBtX&zjoJ zECgD0pEh4>KULGy=;R|N?wWL!JnT8SyqSRt5`UCjr1d!+q8BboaU_hrCYL2Y?3uRH zN}$zey%uTh{uiR_9`Z50-~%~+<#vzft$Ira3KDYGhGOYJx0wYNcnkB2YnMc&24qwW^i%!i6c03k}}L(&wT*KP0mf zXcg0XnO3x3WxaVk#qs=PGUwn@(>&jQ-ONA*iHccPXc6@)>O~8XkGGvuIH!cn@Pw~z zVj2g2+d$GC7^4 zqOy8!T&-sz&?@Kp812Zd(t77*|)^}W}V-Qphd@mw$ zv?}VndUUrt^QmeEDo7kVAL}LVJ}08brwMZYmND){={p;!AW?0&kEqmz;`sg2Q(5!y zB=@P4T?|x^h^QBt3_c&hE+U$iT`Xs3{mi{? z|4;)JB$C{W(V}V<*MD10#GWnZME#Am;#;nZv=C@jZf%S=a^EwujLlApPhK^P&tpUy zs30*qGDdq|rMS6fIQ7Lb@jO>Ve@(5*ryUkEcI9$Mp0yHa zH8@p__IPSZ(?`S(L*!gF(S2X7WKcok+ghu%I>$?!#QsgXSQ{DTme(Q;RFIhMTBQ}Z zTw1^4U0>C>yie3tS<+PPHbergF3yP7>ZB~EySyuz$NRpN&96??|E@XQKm~~mx1zOL znakSX{N>%grD6X~CeN;se6`lIiA>`vg?+G$G#{o|x z6*&@UWsSU0i%401@Lo??w;l#6NZ@)-Jz-=pELD{B;yTKkciwrTZ`B(^t;}^8hN>-+WrR za`q_EV)g|6yQJd{j5coOJZ({C&D&SlLbdC@-$=E)dxHLp+TDf<5`UbVr)4Ow=_S4U z;nKVekz4Lm_gqtvBY{@fLUl&9tttmcRr6%eKf*u-iIYL|wH(bgeS>$m;e{N_$TG>jkh6FDuyKm`fB-%-0PRxJ_Q3r#PhBFC0sw_<6(sJJSgKumR>|AbYJbVz7;%3`L0u%X5@_{l!R6X- zX)BsU=h?9$M|dH9hnf@72U>kSY`IpxKt(;?J0}(%v`)zKY4uERBMnrLsQbeTZF0H_ zrjN-jH;7+)r_=kqv=V4FsPIZ{(5DsjYF-}~l5P`8lm6+>^E}c(1&P#UqO~;tlr?>P zlw+qD_2Q2Ee`=M11X|%LO1&$3vs<)%6zBd`5vU-6D>e1~kApMio}3ws)&uHUp0MDP zLhEy%;~fvkJRN#^(o~Ny@Xi!7Bk%F0TK9sL^-A7M9M@#O{H;NR$EETL6(lgn)U|e2 zxGeK?I!}{KBP;}3;mE3MRQ|3qdERuMW9s}s1qsYh^*10g9g#0eHuSi^?P1{R4r9u8 ze7V-AQbqmBP?~*y$a6@pT3p|gO+}6h5*VLaC!QN9zYBe-Z~bk!g+MFJH;1c$)?YTw z^;~bLB1Z)Yj8FAd{mXLyoAjQ=DZ3lEV#W0?-W98T$ZxL7(7nIvhZTVe67Bz8r9Hk; zS`Y3=k^jB!h8(x_x*qm(xP?F~yen3-Xu3b-=9pDp0u>}`o{G`rs*?J!UgRVH@O#qv zd8~eIKvx3^w8Fb$wdz~_K&}t2re_=2)j$Oaywg)(^EmytOuxRGzE0h9B7s(T|Ed0l z(7^9y-24)ryS6OB@yELim5HOr$xV;X>!qWITkb!Qz_Z!mdeLX1JT-2!Ue89L6`sxN z_aEm+$>dAV>4#Nbp%wbT>`}YIcFd3sjcNMcpN1JY&UojAW22tj9hoIdysWN&rwCM# zzT(D;yg&U#*%i!;0Q>*HZ*4NZ?(udQLt2D;f3gJ@=RDjvfiL z!qHTB^qq=2>F<|>{9MiQWP+Yj?2jm*)AI{_#$hE;L4wMqALjMaiA1RKVJFy%h}A=L zn#AiO36fgXJd5vn2`gAHJr$wjzUL(g?;+5N%GnaKn8Z&>S9pEYdF^{%!U_`fRD_QE zo|n9jKr1TGnvlsP{#MURD2{0Jc?l~>;8PJbmzcyV8-Z3tCvl{Ej}KPt#IQMz0C8Yl zRAwvc1**x{yY|Rgu@gl5j|bL8WwxTxq?&xs$m}D?ik%?ZN7!?h396T^FyGW_$;@_V zfJ|h9=m(iUHht`a^8@QbD=HtY^shT`9IPNgG0}1VzQVfDiptw-J~s*bc(8&5jSU^Q z=5B2>+nGQsDlcpHz$EOM$O;meW9l#I*s`4ov?6*+$D8l*!HS*e-}{#UabR6kW-ICi zs>zzW962j?f@uHoz`Cf+Ry0mjQ{XXY#ZD0IBlenk6>84AOi;aSh542^i)sOKmkFZV zj`-2^@uA#hD zCSqM^MZG{ZCFU*@s374~1o=oD568qV73s=}1X|HJk&ndOWdao>jw*tD_=vYdiaG}@ zT3m7MVc(j8>q0AQ?#}J#TV;q#))fE}R6c)rrRigwNi+^XuoV&dxrGo1uNhFmdMPF% z68kC;fmT%h=GqJx4WO51K_$L4wAH2y5=z3ACbe*1_4|lUJ-DfjOrBzNjsCnLsO|@6Jr| z9v`gOiIENO#9QN_a}evIGFwqEP)&hzmlZofw2#2mE7nD2wxV&OniA(-oPAia6GU5c zx2Bo9Oi;aSWzF5GknN}-LFIhokC;9_M6eYR$9B3*0wYHS>!p~8@b4=m(2B~}zaL`~ z7vWwF6(neEi13a1`v|n6a@pWuli+!mXHiy=z#Q|RcbPydqEl49e8AT#`@GAFov4>4 zvq|tupL3T9s+X;(7wCB4+-1d15bY!WfqQ4Hi^^<8<3u(2G7$+>>;%!)+~s+f>mr)1 zFhkWoOEY)F0?eXJ5Iww2>JXog18Pk@zJHPmRctF{w2DzXIYt~PB78eJZg=*rOUhlW zW}t!um2-ByVETAz5}ySi*ouf+$I6BJTE*u_GqoZ^1?#1LBf>Wx_NyA#g;rF4+qcCHJpsN7;~tVw*d*|#oPW3Dn-L86S>`$4_y+xwC9Jp@`2y{X<| z(}$g4#ZDy4ayvjASQnMqih6-+vYsCtIV*O8Xdl5NNNZ}Wi^^<8qe(Sc$D9aM>;%z1 z;+C0L6;xg!L1nhW3{^YmOrl1BOk~1a)8pit&&M;9C>Vg)WLgn%@nSZUuxBDGSTDsy z$Nl>X>q09kf2wFM68R-Y(>35H3iOHR_p}PJ_1J&>!LDS(Kt~}zAQlk6+1z+kGN^e zU9O90wzB5#hvr=-h~5=8D1Z;F3$3X9D0;$s;$Q^{iiwT~&Rr(Zipt5=UknXA9;_fi zV?)OS=PnayMdh567n+29PGkiM%rS@SeFRz&U2eq6_xNDNPSowa))oinE)!HQTTw62 z@xXn>ik%?Ze>|`*Dzg=hCe`H2M4YKnu@gl52>ZOtbrH>0m~ZMY8=2W&Hoz>(1ks&3 zZZ&fB8D{E3%MI=KE*)=>y@e(i16<#B+!b=GirWo8xNXA zQ9(3ACbeY^4(>!OxKCIUFiTV2)Yee;|QYbbN2IA59zyR6s= zqW#AM>!LDS(Kt~}fpeD?J3(~d++~95Wh>0L#CLZg0cKGqh^Bkzz^!5heNYSOc;Luc zu@l@2>I;&#ykY_)r=F(c)>)KiA6D!H`3OA2F@Yn98KZvT22Z2teH8CPp)&mjhx&)! zY|(N5$WgHqMEmCz5>#d@dP7G({3A!jP7v)MITBQ6D}3&!{%W>*e;)cIrrb((#X$x8 zhgQq<^vmbNzS3vKPS7(lYaIMMkqN4ot!TAO$E|(EciXJk38MYS1M8wPThU6EYVyx^ zRO|%N{sfIP?jz!ip|PRk{;fg)g6|9$f zfe8QcAS;f8tavXL9p@JM&kU>}L1k)TU>``Z6}Hen4vrSvF^-}r$ao`>w-XTN;9rte)D%27rsBFzdn&FV3GFwr`kPmAn@|?(uogmsr z*ylths9v`Ea3ipNU({8Lno48IB3+1=;nn5!WR0+!O>#9R1O>mUC(jH4(}tPYdanH zZxt#;Bayh%iRVPhDRl{FK2e+j>%=lw|(2ki>- z&&0{*d(OH6-g7cRH0==bW#T-O&;k%_MMUC08D3NKJ4jZrUg`zjqhrl0vf?<%iuXg& zac-eCOHe_A%G5&NnB#TOPOudn=a>>(#c{BL^-?)84!aNBf5W?Xh@j*CtwM!bf&`U) zggq0vE~43rGKP)^&O}!11kpaizH@*Hs+X-koQb%ni1zsSXX1zUelS73Kznq2nK<0c z#Fhavk*$c}ohpeV$Ztkj!Fs6|=(zuQkQK*4R@5r8qT}2`|4d{B2`W)0hT}NMig)r5%`NmNSV4ly)I#5w+pnBR zuobq@;j+iU(PF(+et#Ue3x)TvcnP&T#lKakP)m@YvX8LOiR6P6qS=ZvhK>i$L{{ts z(Sb9O396T^KAefTvxfGw{NFN>3F-yf+v3Z_5A6_QDSe1BXCm)O>@5{h?>HSyYZV{Xl$%^A3E8c%Y z$2lfzmY{+Jm8pgPV{RwdijH$k{ytd2da3;WIB>rQ?=K;Oj{CO?6>13*RQAtAB&f_* z^sbP6_-7(2c7kXh!Os)<-8R=nG+S9Sk@G4<UGB0kx+=M@t~)1Hw3 z&P28%f_Fyvk02{pFZBW)_a6_k;yB2PcfinbZlS*qR*;}FwJ@*`B-jdD=~T!MjC>pyR$)A%P0fNKn~F*mqfQT|~1LWegqnJVvf>>uL~{%M zeXxQAm8pgPV~%y96}Hgd2SRsL^yDGnm&Js^(y-!3LTlL$!EeTh=67r#CfJHvNM--XQNentO!4`T2i*_wn>bpx^V>HnC;lp)$Afwo6(nd4 zPlUBq6gd)TMP+(dCNN8y~_#`4MDzQW4-;%fzNzdVIVxB$l_|c!=Lfyb;PJq> zCyBpa=JDX(WrgZxf_&3)U#slDMP>r6s7%lLeT4lT3oA$j9uLlT_HJ(#zpe4_U3x#j z|H*i=qWjm2R|_Nc>eJw~Bk06{?pB@=eFBt)e){iV3u$GCeO1JRYnd@!|1c@Ag*l^XtU%;PZo? znDTh=(^H}opVngJ)Dk4vijGs+8aex51?#0UKTq~u&vQQU{k+>^9vf}5IR8IW$8D_5B{wTD@gc{2mkdXw6eE~pJn>@EapG-Xe0KxasL?qrjpM<}8hCXP<;rmP~C*IwC zh~PURA`ht*4OEbbKeR|2RW3w?)FB`JJMENF%g4%;6Cx}GTE#Y9qScyEMa(HeM5>jy zWZorfWr?O?1}aE&%>0#hs#%CQ+2Y(AR)Eo+S#U+L@)36 z?k3-OCA&n8lto;vEd*NK>$O7Ldn8zl{EU1Q%aqNTS$)f-SeY6IDoC7fq*iI428%7; z@AhrJoy0l4Ml*TfS!)Y{Rs+*SYkiKC7pc78SBviask3*hK63UZX`q6{kff`$&gaXC zY8&WVTUlCWauz*QTNb_E$U>mi#r@IRv18@Luv`?!t%8}IyYJuGtZSfx#GX5` z+P^)Di&f*uM}^^^Ir|NoCpLdv+d`mK@|Ce#{*J{($B)Rz!7T-yg{th<*4?XVpn?Rx z|5RVEYf;pha{WNLl553D#59~eb%nH znB)ES*~mNza&w7A+N(634OEbz>md2?T?h9hSs~YcQA~UEa)^aME1Kc>_o@z8`mq0Bc1&6Cz|NC<8+eTvIZuNZw$Py&*EKq+@?x07E zikqvwx)f<(v}4~yYw4-lsQ%`Fj29VxpQ+tbU!z3@iI<D(#>4Wkg}`*J!h>y(5S8*&*7h z$dN!RYvdEM-<0W7Z4%+-x>?S1Byd(zt@_^{vA@W1ZR#J921ffhZl(4-x}3MKu!ZXT z&fDTd@|lOUFCVL~b3vaV(I|bi7QVTh_|4l_=kHFF*Saqji&f-EpcS^z;Y#s*iWHd^ zh+I8~8>k@hX+*TP;6OR?h4&k7EstK8lmC7uB2?r^pp`Z9H&uR-ukt<;J$iPx%)3b7 zRYF}2>Tee>K7FXYP?2LxFme&TQY*c#yeRJd+D-p9OU2u5*R@CG#v7<0@i6TQt-!Sk z=6P=Pbc@Stvx#T!NDF~hFV**07Px}N2Ct7NwH63h;Z)-L3gZn_kXV{#nYR3+N~Vwb zxweU~ca;(s)y#kdTK(^jrP`P&l|(Y{8m|7=vqjY0f+B6{@dheL;A&8PVRcoksME8W znExQsLZB6{1|6=9-FwKpl~T(jju93Dt?I9mE2oognVKuu-Q6|DdpcSs7)K?KF#>WByer#aOKW3M3%bvL}b4> z+|s+)Z-sNO(5meU7PGux)H$(bp#1paL-9D}2m=))Fh2Di_oqi?*Qd4RSyvAWfmXOu zbGRD5IV_(>)|7=+8J*oTHZ*=;G)&V+)D&^W)pNM(!!%=KO_5>p z5cj7i$7#iP)DU_)y4J3kwLxyFTg~aH6l9=+M8`D~wd8+S7c;!;-BX`Vk+G{%Ig_?+ zY$4Fk@Br}|WF@r-IB*n2g&Iwh^_IeoaS z^=g=fK&uV?Cu%$TRTo9Q*QmMuQ_Da98YDA@4L49hVnoKN+DGBl#6>T0<3wdKI?^Tf zJ&CjsXchPG1g(9m>gK$gLQgNMw&^WS_Ep~>1Oct|sSH1-S;bE+;7P1N^mQ9+_o zM(@oUXhVvvY5P(cFk)z$Oel6B>$ z8-wMeWFrhzkXSo&uGZ+GCW5`s0ABn!Nk(7oEj^6}S_rhldv*2Auq&&j)?%SNsJ>)| z3KDgW&eh`63Q@2yol#c{o|l_*>heL^FbjcJcz5q`Iexe$)7?2MdmU?Npn}Bx6LYoF zP9d^*zkj~<&K^19@fBHqVJia_B!=9Yt~HtL6v;oORt~gp+O}-?1W-8~b{#zCcfmV3VtM9lUEhbuT z+%8I;x1JxE`JF0^)3&v*A=-QIKaw}Qt$jCSvgq1jynzZ5pZ`8oTl%_&nY%{SP*J?s zKcd>%aTWrt>dIm2{HQ60ETq25s4_h^bdIcu1OK&zMu#|UrVb! zR~9|JNp7s&!$1YI>s?YI9#rg_2qBH zevtWkbhGp>TH#f~;Trlpr`)tZLY`|i+`yR=dm4Mh;X3(bq+C65znsvvhb3|(u-DZW zp>JfDXYPc{!K&luKt?GZAyl8plsrUx<=}$IDoCRm1ZGt?=6A za3vWLCZF~Va<w0O2 zYf`^3`Du?J=k+{R0k!fI0z?%=pg6d zbk-3>BBG5aG*8LqrjLG~hsjLsf}GQfS_!mz&~agC^ZYGM!cb#gMUCKg6$dIv#BW~^ zdiPcfv#&l@L|a8{vk_=DzJJruT)(t3iLPn{PpJ{yti}TsBwk!N60bMyDoX98-o5p* zlBn@U*;m1;uNJGwOR6kE1&Q1J4#(d-+0`WStGPXOzaVD~HMb*yRyeW_*O`(jMJDB3g<_M zt8^AMik6LGe)j>npbVe-k)Am`~!)-wu;{~d1@ zIyq~YIp&AI2$OF*1vyU^uo7s6=f8Tgt>Rdx;wYs?5EUeFR#MM06!E7b@+$(b$e1x_ zYBkW}3)eNXpq14J&J390c;&>bOw3&xb5xM9=58C+syV7v zdsVA28!$`o`si?FQ^XQQ>{A3PNMIg2T>DjDZ8kFzuK;+(nXsY$fw%*on`iAlMQk() zRFJ@HqWVsCzRIGJT9GADd4&X8;T)sBO+F}0{@N?Zc_*iJ7R595&u1OtqkijT=GCF7 zFxg>9kTcUK4GmO~z^k3Z6{h-Xmf5>VpcP)b)Kdes_GxFXmhk$4^E}q1?k1Fv#pZY* zfmT)@2eVfeptdFgOAo;)P}}46z!LeH&8($BDvGODWZ=k>0KwbF0(}2d9a{3WsEma zLE_PvQ1_PfeZ;X{w6aXtIa$jm(uux5SP8URa=nWC;?~}#k6T3sYh!aK6E%JwZ=iz2 zf$_oaGYxu+esSc(b?htcj|L?~T)dS)t0pPRxvQM-W%@AA&DE}cTS!#;cD#WK5<`C~ z;hxy7m-tTgo0_lEtk<&VYarUKwi0O7cYG1I{!>qLO?_GQjn z4(=(+EF&U8P7{4j`x9t&r*2MnukRzopWZvjqfw7UqkcDhw~RFJ5bBd2@a-3U>j zC;4d5__3JTZmIm#Mxa&lm>_q{DiI=kdm^fyEGbJax-MT1d#W#Elfm-C6SX5ThQ@ zy}^~1QkJ^?TJ{~(&O)G7(@7t>wRPP^RzW@r^{g(lPyAP&o!Hhu1&I!`ZpW{R?j{O& zpK>NY&{(D%m)dzDsJVqet20|3#UDD>O(ZEvKGvw`O2ew8buPT#)IbG^zm8sskGvEv z2Ie55@ZPMl=h_o8Rh^y|0~5ff#54Iz{N!!n zV#Giq@*UeNj-4GQI}RCPA<*jd%E$4MBf5z_-lyk9(jOG--%gZ2H5zW9f<(oG%EzQ| z@s0OQ!-Zd8YHe<=6^&Dmw-9JGJ;THJ=Eb_1=U~;^AB&nt3A`6^xZ-N+qE*{Ta)XKk6(lewhimx6R5EPrUirhsz7_(l@a{!Db#9eg zZr*fJW>({Y3KBRr4%gh3ZRC}ZAZGz}SA+yw;gwp=CHq^+5oLeAMqAx?3ulx5^s1H03}GfmQ{klyKi& z)Jt@mN<@iL$z-$EvGU{}0}WJ=2>G|LyL-W2VrC=}P46BR=a%-D<9Cm+5NLIDa|!p0 zvAx7VZ(pUjbWYqj6(w_YQM*MzL85%IBJQ=Rdx=NU*wKm`fB`Z!#xW75k<(>KeLo% z@Oq_oo+W#uHF^1s7?Hv{C*o|E>)A){E$zFD=nXXHX;wbe3VpXq{F!&WfeI2so1}98 z@LP9t7Cqhiy4G0E7xQXZ3AFn8N^1AWvOUD-o5;tA9yhh?XBUVoV!VM05{;98;tqb^ zLp<3`#Ql3;i_)uF%dVqFSqQYsvO2YUd8-~`#g{}hI|Nvg2eGrIo%;!B1Dg^6vu#p%SC~v zb>$Z-awO0STd3Z1_MI=zhEgJ0 zDVD1^uNNT3zA z(BXQpZLjwBakzM*d|bmN!=BxdpL)y=E-lwmt*S4a&<==s33vY zE{ALLv_2xD!z)o}(>TjD3JGf*X{ziMC+-fDZER~eT%F;Jp|WJc7BMq#4|#jSNXuM; z=X{&}AGyOPb{DgwD2}Wh)`&9GTFc(*JVylyJewV^S$)&XFDmNtaQQwK0h+@HLHB`-3eJht*J2%w8E9M!&SZB z?;?BeS#piK;@~v{uM9ZnJ6u0HHi>i#Ys;#(F~@PnwS;>Z#Nw$i=llPF1Y$J#q_bp|NPLKM;Ftxr_d+KH60H%oFkVLhbwgV~yj5>bi^m z>$?w6m9(yVzsm5tyN9|vJ+9-fGnAhJoR53t`F!6s@o-!hCn`v!+Fr+9Wk+w3!TStg z^ypchah-RIg89`uVW%`V)vvFpGoO5%Nl4-uktLZNd2x(|K&wYbYPyp>WFO^nEz?JTcUh#l zpk`4}kich7Y6ryA%yM1FeB!&z<1A03kmz@*hI?((zG9B|$;61HGeqG}R*0cnBP|44 z-9A>sovnOdF<=G7@pjY}ZG7H8MA;UR1}b>RVr>=uCe`;Z$~dquyVkGKbgj;}u6Qc< zY#Qw?_sf!33;JuCx2*OWy!#hrpkgNuZ}8FYCx~XNe1i+T^KB(iu@l_F_Y*|3Rh>aa zwa>$51c(C_J2AG-%KspUW~<%tCAFjf%nU%FVkgqOmj4GqG+WI}QC54CZ%zOL6+7|s zn5F+g5Y1K**~@G5W99`QP_YxWUVrr;1kr4j?{sjMcYvsL|qY2Wp<)dwne!f(vIK9Ha? zTMZ6M{jR62K2Wg}eq-+SfdrM=>OjX-{v()#MzFs+xBs|UG(K(0r0x!VGHc~q6!exS z7)9Qdy{(EmczM|#B2Yo1{LQjj-=x{%)2W)&F7M-)cO6;$)ovnCL1IIlG+u&y`v}k6 zLKW=-B*IS{0sGOB-70OVdZus6M+YCY@b@ zT7?P{vp=t^Y4iWu?`ze-D;;;0G@6tr0u>~}@72}X%y3lo5f9Tf+I8h#hBzWnLE=(K z9qqu7&=4Q7U_qr_r6xvu2~?1fU216kDy%h$B4K%UMbx0rwW`UxXr(tMD}jogNLW4NKM11PYOh+=?0C350D+2~ z=v8mle-K2o)dIDu8FgZ500I>|5!-Lhe-K2oRWG%w`Lq7w00b&_BKX0){~(BFtNd!U zG<(HDE8)#{%q7gquhgoBd)hjJsMra=G57jFg34@FSgmTfr>#Cvu@in{?)8BLmD#GH zTGen*TYaEnC;Z0T>jMcYv(-?ws^Olt`as1__>H;O2NG0fE4NxLC61u_rL=KH1UvJb z*2TEc8t!v5mWz-}zlG8hgqwdY6E_-N3#H?sLzjzH^Y4W^Tt6rxyCRle@DZ#aLFFeG zmzl(GiYTUt=HKf&)`eE=J6Xf$%7;+IIz<%z$wyGDNI`=AwZEv=C$4geIQO)O^L3$_ z9;^$k=yqw{#!Rj&^u z*ou7gth7R88}P9rhAJYBB2KjN5v*XnR4#jLx%laJN=1Y#;`i29<>-)%2G)gEbbODo zLj1L#h{@!m%~e_5Bv?U$%9#eO5bG1tC}Nl*VhhZaA3rPN^??Lik&hLfSBev-QYm7A zBCaXo@>3td3f4>ITa{Ld;F;+a5v7P`Kk1^o`hTnot>}2UG|^&0RBAJhsfy@i609IW zWjcPkcLtNFuQH@_0D`T^$FV)pW*q&MW#mO&A7m1&V7*ixdp+8WgT~{r$`E1p6%uSk zKDM-8WsXM*MKmZl)6>=@SiyR!+-&VCb3Be|!N%&gS3L{O5k!Km$j3j|SDAU$QTgc6 z`l@G}Nw9+TQn`H77&EUZ6Ei4lkC}-`uod}eT|dS=Kh)o&H;yVI*d$oNdZ|3?T#R{s z+*fg&QBf~9&nP6=ihRtQ8RI>h9j^MyM|DN?EZ{o_S;2a#Jgz{j@SW%8%wU9?GXoNA zMLvqGjxlGS5anZo`d?9#U?8VqIuO$EgqRU%se_q>7lSh`#DPLdQ%|pYbWRa!mNr_b5tMDO z`VnkJHBl~PsC`BebY6rj;*pwxP{DesOc}GF`7uRMUY%AM;!v}konR|EPC4Cf8xhx3 zmLyd~=1?EO3f4ZwVvf&`W6%HSK1If~e#GNhe3=18y=`Jk(vFRy6cEu^kdDjU76Vg>7^GF=CK zdDT}D^OW^eGZT?uEAm0tVBh(XOkK~XRn6#Zq5PnN^-`Ix)V}khv?A{PUBp?oknfD* zy3mSh;x&=_y9TON^;J$)GS5L)kf1WJoM;UfWh7DmHtG3-zf($|e|qS&%ah8@Ge>yK z2UYp`o$h+I!s*>s0u>~v{IpPSFa2SHt*EA2kNSa+RJ)rOqgJ7U_0sW{`9|wbORFdr zB+!b=8IBFmpRY`t7)KNntk{V+p(FL(?Owzu#u12ME28DNK|oA?(k?Fd;g-8d8MtBj zyB?i8Z;hTZtc$m&xfgssZeEaa>91~#vq#GW)ik@%TD@0%M>CGBYqhxV<}dXl*ouhV zO~27kyli6C*tbqaO}@TbyRB;6*@lzj$cp=& z%Ao_+>2o$V4>0D%jK@3oWd6H?eax94I&JI}z1L1Y2M4_f+EHXx9`9Mp`GZze9{C{Z zJ?952cA`<`Df;aZ>_c^I6!kFHMP;_4vQc+}9(O-oVqd)@Sn(syPS$6g0)kw4eIQ|& zRzyEoH9;R(5r}s_Sn;+5h(~=V>%Z=R^ZcC;B-o1R`;R8*x1Xm85Cq-;#V^vcJ@WBdMAu+=Q^&35{Y+x#R^#= z(Rs`yJ!D6UMB<$fB-o1R$15l5$(AHfB;NU8g{+V$-)NHl@LaM);++p9*ox?#2PW!y z{-k#k<~8b_4_3$uiS!jG>94mXNhIF+K!UA^{_Dgf^KM9m2NU#;r zB{qDiFFFy3V1=xZIDKf6K4&`Q?z=dUU@M}tjQLXk!V&2FV1=xZX#VdceQ8X}0FfiX zRz#m@{H300Y9N9YvO*$b&M)=0r{O$*7Y8eLVnN->`Zl$v^W7O0kYFpKvn-vg-&+Fn z)jI;OOP|cypbwc)(|dKcUZe7#RDU*gL5e+87b{d|g4Pe$3U1Qd$5b*~Rc&L%3e~=O z6_KDaTYam_ zU*(O~`&Z?@I`H`Vu4X;bS3<>3JSn+FA2#YUTkj%4WwxSnvc+5VVbwDQAd>#DO}`ud zHlD2Z>|3QbIGNo2VS?+j6TklztzT?s&l0SQ%4|htYvkSmSN|d9`7hh`@~1C(TXmyS ztnM0e-&^L^TJwqsR;Y}`lbbR6jUQgxW*fKx$w|L9k zTK`tDLiHj+<6r*7&Hz4;U@M}juP!}25P-mXv4u3|ckgZrK(IpfB5^N$oSye(DU+!4 z)uy;#>h0J?R!FcFohzNcnWER~u+bzUk5!5**kSDsRETDR%4fR9>8Ho7KH%%C$k|_S zOL=6pcV^(a(28nWRA`t!>NEDy?^(vU0oONeM+FHg&y9@JHl3`aEY*8g^~v7) z7eeSh;fn@o_0Z}Qy?JGg3&qt@Sp>d0wH!p_@(M-_Q=lfwz+|w`Q zui_&bzUdP;wa5(b-5rk_T2Z-MrFQPva}G1xao)wqt54ji?=3Xn(hKh*k0Qz*EfU^& z-upZe*VII?LS-aqjyclS6Z+1Fw_$dIt>`$-M84;6b|0*e6%sUG`JSrT30CX`&Dy?a zWOjlTJMrz|?fQ`)ZU=}13AQ4d=6T<x9WSl z=ME4D5^O~@WvK7noxN48kQEX%OZeWz5s~ck=n5%{H?2rktRP{ik?G@``FrB7g#7MDuoV%DtF6;dJ!xwa;WJysZCRAZ+bZRzVJ<=n3-4Y+e~AJxkW`k*YKXR`YjGO^)a%eXt?p9UXOO4`NC1&!2URc^M{ z6SDU}?)S8+)3A0Gsb20yB&d9S;n4#`C(b3G1t8dph}Q!?i?)^M&t-Uuwo~^%9<{IX_xE)2v+PwmCb46`_<1DfMCT=&}cSzkm>(}ASOgYb2G$Mh0D={=LV_}M_0{762v+PwMsYj_nHy z9PR<`IU(;_6~$SC1X~fkWcEy}k9PzsWQBz1x3A(GXtv0`naGNr$i1Y9yY;VA0<;PV zwjz36ubBAKsgEZ1?z>j8LRLrwJzN*RCXzGpc>6=U2R1%fk*rujg6@jmw65sB{by^l zRXL_~jtj2xDh>&>qATaH$t&RgW8bY@aed3Uy{jcw>;&Jp`Nrcg5Ue0U<<=23+~Zm% zF zCOzIL3Oo^sz3a^qB&f_*RHhwCiFbEibM+ro>_n0}n?>w^ashmBT|}@ImFwo&BEFbV z)JjC5Lgh8lTgBU?S-p{SP4DLRcX6;{CwP~pKf#Kf;60cA1S@uecToBhtk{VZe{K`) zV`*10jXCx%5^O~@@1XQ2SRpGUct4~+!HS*Wy^8(>D|X`I>+Pb^*Lb5wonYI-?Yr&_ynz(Lp+;`X(jgRBJa>>zmQRm_-930;}hje2bp6kU)l;X zdUO1@k$f4Vr)Evjw!h%kY8wEr3qY-xaZE6pvEUqAMwwfVMUEkygp-o=Bvxg z6~^aE6SM-2wty%6@dP!n(u5sd{ipi29Mt#(dRp{6gPzZ+@rfOE-^^@NgXbB#^{ACP zqM)-~!)bz6t0jk>Z+ks+;)5w+IeIspnKHRA)Xx{Bjwl}d>FjF>b(@L(FEk7 zBj;wG+o^RJW8#$CgKM+SOZ5p_A)UW7*lp1g(&cyRhiUP-|Z6U}Vd(wzX3OD-xLJ|Ffk_ zM){^ZQ*_49*}<%3{cO7;p{*ce9aQ%yPo53=rd!af#Pc!l0^r`T#&=aSR`;)G+b-j| za;(0xe6YL4E}JWAGy(a`zZzw%Xj?bDx{tT}GXI;(ZEf4t1g(&(r3I-O%MRBJiL|n9 zgM$BjvH+#Uy9*fGVRg^>w`4Txy~B?4c%L&m<5at(ZE)*=ms^0KMiVy1{e?cUG_+#u=sZ|!dD~#`-b@<WJTkTtlyZIHJAtK83pIc~+PhfRkdDx#J@%fg^gZIW%wkv&VGyxe`J-W(h z{lI0xnDmB`bvt}%E5wfNwWEK`3i_;nC~`M}{;Ube?Ef*Nk3Ct#Q*78h#VG=w>*J}n zcmn4DkY5mZUXL*fGSB-t;=a<)1)ohkJ@UjI>knAjT~(R&ao#8PENAb_J`IkvTad3c zKCx!?ShHg{Pxe32`om!MtX7tw#wW^LHO>^h!c(jdPhJ_EGU0doZU!|zaq8pa%+w}4 z{kgpA^5DUo+w2)HYBT{Ed&|+0x@MzKgMG!zZ46D&3gs&><4nPw9Cx#F)(305jj;qZ z+AoL~4vjN+ynAvuCiZE$AvkB&sWw;CXae%{Tc)a0lPlV73EFh3W@BiAR*3O(4lp!SyF`hIg#v_|`doYr%yI?I`Q;r8Ii-!d2a1g(&MS)K9buWNbs{Iff@1+|BF zZlN{WFVZhPbG(^jeh+)pf<4=UvR%_`uBg!jY%9?Otx%3% zMvpVq=j;h%bh`AjVEadxT7nww7ev+6apt{UyF%ix310=BS`V>xPK_p*!|2R8zT~&T zX&2TDv@dOi^vCv(GtIX0zP8WAZ-Pg9x3UB^nt+Tp7M(dyzWtkEPnUl7T4{n-h;jE1 zc>ejZc7H)?U362Ph{I0V|~6w5eS@VPjCk z7$hpKKHV(tz&!x?XADiy3iO+UJI2O{5Y)g*6Tyd_VhIW1O0Ef7f!=#&Thn(aM`|(7 zAwdnSG-1}YH3fhC9IcNCL5)w$9^TGe_2Rw+1T{Xfw?=zYucNBLNDOLxV%aP0&C_*u zCx}6fPpqiW!2~V0Cm^Wti6;Fzm`2OLNgHttp+l?F0m^kgFB@+M4&aeVu^NxjObr z8&j}+hs_n^#pjB?;I~9(JF|TMmj9s~ni%^)JF{rqrUbdtC8rhQUHW1>^VVk@5)jnD z>XTR6nMu;OA=6WQIjHdo&WZ7a_FGzVa#O>#HpcM@Y7j#cpWWZiT>k*~eQYz7e_*8v zS|NRPdOLHlGS{M-paxc1KeaU-&U^oV$dx8`Jl@t+__jE~)z!J872-{*-qt*KSy3!u zR{%q#EmiB`Ru*VuTxBFAY{{vCUrjJ}e0@N}bprY$WzOyK1T~1EiK}19IL-<{6SM+7 z?ESK4@waun-CC`ItcjnlNIgyr`T{{K$XiArmgQIm+My}lq?A310#^aYJi z44KiwBqpfw3FI*GT_lTbHH<-`-DPK(Piu{fmcur9SZRV*NbkR|oEb6j znFNF$sarjoVVbZ1(Ow1Ce|#&U27Wbx+DskbJtebOR}-`X&H9gxffYU9)Psy)ow7SvbpFmve6C(z<9^KsRtKJc#J&LtnQtP- z6V&*`MRRi0`-8R+@{jffnO2bF?r|)nH9oPt%oy`zTi$u;7}WTL*^^_+)ZBWu&gT{_iEQBB8C2ex&pbX3t$D zcy*a8_U%(^Tx-V=8~9em7{%;;EWSUIVLaWZmlhLSaNE70@o^W3{B7q^xaY)<(hhDSnSoM z23DGY95<`ka`*(SKp&cZotb(&cj$FFs9_8e7%>vZ&;+eOqgBpd;LWu*SJc2t6OWGW zVHRA>Cnh>q)c6F)vG^MF39c;rFYRf%eBu4xlFb!0j6ovdm}qC`&dYMm`ogUR7_VL( zoonVc{MO1Hw~jJh3%`kNS9a#4#wU_CqBdNqG9)urpjC2$Clj2pf3=#f_&Y_qs`bm zcg7O-T2bQ@SC1WORwwiMN0=)cLld+Deao9;%)|_}M;5ud)WAv;-}E1A)<2i19Mt&4 zeI>@3IaSooT_grIK2fdRIFs7SD~ByPH9oOq?nraRrUS9p%9dOcv;tjc&^YtxOZyTK z)WAv;Wpc-vt6HmF&PX|^@reaf#+h|>)J|%IpvEWmzCO-;p7LvKu58Jv@rmc&9ci+D z{w6&5r)*PwfjbtspmjKG!_iP_1MV#-J65w@ZvRwcGgniCUxm zLLT@*t~oX5Lxz;bPu0-*rzJrlV^s$gPKtGS{t5L{Q@srrO*$dw6VP~#KjuNY~TT(T;b$bnT15n3Vr^fseRhlOim z30vpX*cghqR7(EVfwi%OjiCuzfu4QsD09=VpT-h41~stKMCGO<&Go%LI{`r}(65&r zX%2MS7)#h(=@_&EG5(=Z=Ce=zeNnB^ej#7!dv=u2Bc-uLV%bH}Y)VhLMv zYJ7t0m6-9$mO~S?0)6$OQD)0KTVrEbf*M$9g6ox-7&ce>w}~?P4zO!=(DA7&PCrO5Km@8^PYl8bhk&;J< zOFQ&6rsE>JHZ0qtpS`1FZe#WU5F%1?Xkev@$u0Vu5gR^;t&cc@R-jvM>TkwBrta<{ zxuOPECuI&W%@1w1@mNohawN=^CRjHyL=OD&xLVG_mph-sbLm{QVO6r7y^|f*iXi z^ZA_A4?HY|Wd%jIXPy;JX44OU0m`iy|Pv?qfaX^&J8)ue$ zzS6c`o*as4i8fc%_(X-r#+qvr-s4c?6N9$MZ{KGE2IXxcba z|B?@E-aAYhWtx5K%E2_6<>`qEe5G4Cbpr# ziW;9NEhn7@-LHs9IW$2l&>b$5U%LInTPNl~GxFeAvuJ0&t-(r7N1M|3Eq1k;h_EH6 zFVLD8mNeRQSnI`zBWMMhxs7in+ON*x<9OXOO$!pV5>3zw z^w|~0m=$U2{v#5D8dz!K^x9)hpF{rX1nmnltss|rWsEs=bs~ZqShb(xthA@dwTk45 z{Qy>%%pYa;Ua-N|Ap1>(NWvdjQR5RYzBkGoo1jj;BqV4BnxjN~3_X@``~b0V*%;Hc zySGzhb43lTG%+=Mv^n^Qw}y)&Xa&0Fq|v7Gy@~d(sDagp{iDspAGjSa#*QrqX0Q5Z z*D>d=`^U~?E>3rq*7(=O#5WWjpayu{93~op*f|s_$%QN|sJa z@Z6ai_|?SJ3+kBNZB9hc3UsgK4Na^4i3n<7rHR+?s$<^Ym}o~>6SM+-+wBcavymke zl!F>rX=2Z+I_8dg{_kvSUyx}9`9M-blleg+f*PMlYgX6nJcsAob$w`pR-peG-@rV* zBoRRktTgd=as%_&P2SG7Z6(zB#Ka2qOwHjZB4}lEb+n$jaepF$Ee=+zeragxZ9HUq zS6pkig@+Y2KJoWg4Ndch{e3u}pcT^P3pZwB_Q7bcjl`e^R#T=oG^LyV@;~HC6SF!r zG#R&hJCJdGkXDE{TdvjRk9p5=EI|#dG%;vb15>?@cL$jRs}2{}HPt)(VDo;<8&yo3 zS6w;iDSm{b2DB#Xm#$^{9!WHoXo6OtSx@mXv|pXWC;zNsrZn|-oo&gf@ypcg#yVzs zGjC@*j-VCjtEbd9T~GIS=%LXuXod8H<*J%agVb|_95fTH(S9Kh$*yho{^ieVK0zy_ z-+iE#Nt@*VYPQxuK4*R{^X6AuZEeQoD$;hT@rg-$tC;b#)DBCe9Mt&4>krj3mw)N4 z^liyCK`YSH(}cLde~%s-P0$MI{U=p1sp;wjT_ji3Xupt`H>hQrw)iALuhj&tK$NRn z%S_ql-%aS~+6wYp51nNmJecTo8#O-BEVY*DF;4A3M#`ZHT7fQgzYv$bo1h$;pcRO^ z_nv85Ec5q}wMP4eylRKUc_C0py7B|j!Q z0ig+6fgbuyMYFTZs|jLI11n7wm#%NBlu8ss6SM+-{BvZ~z)BN2{hGv{ZcD-+?F%xk zAoC1cd<|;9jW^Xdlka=O);Z~Tf*SbM#G=^^OxEf|b*>3ofqrmcLo>Rpf9It&kf)s0 z$h5pF_#bkmiH&bJGNqnM)Y^5fXoYx4k2#+hxw_QAN)u(zYiv>%6((pUnxGZvg2x)0 z(T{sO6FEA%wt_rtOw;4!iW;A|WqxDx^qNE`elX_8BiipIZ23DG=JgY6!Y)f*#` z7}WU0<(D@wpQI=va;-E$E6|^8sBey?dSjv;;Wq5AXIehy&V}EfSxln9BOPTl}L2AtqEFzKzic02dL40A+IXi#2mRW(LJXoXa%C$!Nz9ILT{(Ywkv92 zwg08YW^Kij?b$(kN_?%5U!N#bwXs=TlD|@5FRlCoEBb;=E6AVR+Q>Ax%R32Zb43lT z9-Q6KT;H|K|Bx$9m=7D6Hm~_lm;7AO%I5I9`X*_Z_q5OEs!XVX)q%tH%$#C(6}L6&u6WpeASq`p@gio4wamO+aYBI)@phDw#tsdso-Spay<5@%r34 zru!!Ui6X2tK`W&1FImT2J;r~|sWp&G+*;dI*qm&y6+OjYD{6dV_}Vi~FwXzdAFQbH ziDtWMnP%%15gD&EK`YSTJY37%c6XvZOKM;>^uk)^y)`9lIp`_A9Mt&4?n!5vdh7p* zj&PA&QR5RWPOoK#{&F-**scyMP0$MI-2IBlm2JCv$E$SnDrWyvhwVt6kg%ddTmOT~m`2JUfh+HdbV5JGZV-`c$a_Bck z^xJNH7bJ$T*NPg%&_w6ZktF@4&+VAbC{?b{p$#iyCg z+v^tE-|>>)g~xAq#S?d2Gb?C0d&DBG(FA1tCOv*P%$4KS(`N@wtK{1ljIOQViMfrI zeA7QG^R-43HeUGKWR56)eo1gwlc!o*jmkUA#NTp{{<_$_*HVJlwy#Oi8pI<3dCQtp z&8ew%!gAa_r&%z1!)Jj{&h4D3b+S6Tvee>_Kb*^hr6ZlSC^!NIHpVTe*V}Dm$gVg94HiuJBGIxK^ zZ-B0TF(deW%ZdA%gFXSaXq-X~~<^u-_EQW*X1HslC@v_|`d{NkU-3Y*tcEioq% zK`RimDkhmFH~70aehgY6{qGBh7Dj(}5dQH;YqVd;1^trDwr&3D1fQT4($D@n$&~#= zogIwCpaxdy50^0SE&i}Tdy2I7oJ6_OMDkZ(k2vNk)?aLGHjO7w8z;3hAd$ySFg<+lJc~ z_6!cpxZcK~MiY=%Z9c{HU-eQLqv8CX!QJ=V5J%7o#P}J>X3^rOLSp99op}peCb~x;d`)1KmNP=)WAv; ze5XFr2W_u~m41(&zjA_fydj_XeNJj%rHMYBS{~;&QZzv;(2!$dU}UiKxh5d(C~LeA?sh`kNpkPbLO5Kbo;b@`#ZvsZ?fJc-x4~t!`MJ;Gyxf9ihg})nS8J4#tY=tbLC50LB=6xU3~Qk^0e>s(;J@kL3%u~D0M(krgN!Ot+RzUkl%*!&lfqFt_nBsDYIxmUXS39`_BmxRTQfbe+@RQpDHm1_td9{<097 zi|Q>Zcx&b+`$b^Xbpa;js#@<8fNs%C%2ax z94u};-o~IWZ3RzPJy=-KWzi2|jDk^vgKyU#yijX20U5tL8U04?Jx>k}+RuG;flts1 z=_lV}3OY486voIHGdQ^EvxAnPM*C&F=vQ?wy?=0!{>4A%X0jP#3ihr&DZ|y^ znVSX$2L~6~l2fAz$hdCNuj=;teNb?9lR38JnxGY8{P93x!Qo@cVT@X7gM<7=<86IV zqy2)oQEG7iM`c4|&gPqf8XKG0Z=X@43CQ>r$>^7RFYhxbsK4YF8$%PcLX4U6Uf+gt z6~h=u7xWKymwVR6pho+Jj9-q7e(U#zw1L6iH`DC*qclM)#JD!)t%3t@RSsj+J#|p< z)2&Zff*S1?#KZgM6s*qUmoVEj=^b3N?c{u|(FA1tCS>%hx|fe180@)ei;Y2F+6tb! z%Gc}aZ{-&~2fi>k7+brR{c;vHn$X`-b*<#J)xCqe=cdOH+6poLBVXaG(wg5g?K9-e z;M`x=T7nv%$d)htMZf2TN$@K9CM$eV;}fypWS#lSz~H_<>9z(rGJu{ZbueYX`j}C| zmE+q%1A~IgFR)*)<7nU$Xfe?*bT^+iAjsJ^A;9&aMiY?vJwF%Y??E>O$Ig4s#?S<< z;0Y}zI)7mHN9o~9e>+ou;}YpHUk!~RpaHE3$cev~T6*4j>9@U6Bj&5Rpr2dbOnqNX zGL$oUSV=o`QUfb~hZkQpq|9p|?%e|VQ73dkG>!iPV!-)u5foS&5*J+8r4or>q3;95|TIqLu%&|mYT}{vm z#G^m$NNbhwd&1OczmRu6S|dHJS)v%4pcRPUzuuje_zT3;Xupt$|9y6PrQwObS*!_K zfjIZKYUxkD%aK}_Tobec@oMiM(xxZ;$}u(CFXX5DR7)RnEYVkvH9;#7-#6czmbjl& zqy0i&bWyeR*DgsELld+D@yhUO>3xQt70zl>@*Mot8pu^zoSi;=US&J3&{K3|!1aO^ zH9k>O)``mwof(Y*Ada9F=-0Q@NUwdRB60+R&wB-q%v23CItew>W*p=_|r=+*&j+IQ@+QaRjYE+}L_adYi^8L*mcH z1A~nt&avw&YGBpvf~D!N4_cKTpR1&I1_qr*U0}CsgmhQ!1_2L%HvE{P*(1>&?Z%hJ;>{U{`!+dL@fHTY_~LZ${*b+#@` zzwO;Mwj7M@ay90jK|zm7MfMvb)c6FxBoe)kTG@SY&~xQr`#lzFGy(aIL(9_N?7**p zkFGE{IA!2``*jsf&z1Wg`f^=Zj-_(P^5Z$Z?Hvm>uxfDkvh;0z*V}T?lPkxb zS%ZQ`wexK`sL=%E>>W$f&t1#<*nMbF@ZN<7<8nnS)H%Mi6TQ2;VuReJUR=`FIW^iZ zAKHD0^=e6K-22JzUlXeU#-WQigGN` zH+gVS_T7{jPBQcIYb8;Tk1n1!R7`ZE+O{*Qw=RaQb zY+BCjZ)^;R_W@UOwzqhK z8lRZZ_3pI#Wj2So`gBHvw;%hUw*B6O{tXEJ?gRG1qo)(zm|VKW*iVLMZ<<{yYeA#& z1)n}XHS?0q+tP=pUtS0~O=P5}-#0b$rDuMW7(PLbCLkl-jiNIrmtHtdVyu~6s!03N zR*YTpir*!MPar4IXoB%hvP99kl?&I5yghsCFUxHVO+bbfVwC-OYUZ2y>e<&;yTZoM1g#Kbaq+awUzU}Y7(PLb_6r&52Q5+V@Xx{W{mZlN8g;vkp$S?c z#A^Cz9Oj6ejy`$wI#Z~pOnA(wNhEvelpy~&;+dz17G3H$vI78_yjfD zFJz?WYNCiSG(juGShe`2upD8G2tkeZ3mNII9POp#OZP1=niF0tP0$K4-t0ajtPh`{ zM*D?~bXOm@NS$B5xq9)cs;r_Ke^Y@!v(_XkXe2 z>D@cd4Eu*qP@@URNO%2X=IVCAX1Px5e{y|L6SP8%rzgy`y*WAWiPb9?ekDY4NKm8w zLPoml=VParUU;3fscEmdu|yNJLX5_iyWZP&)o1g#JQ z?YQS#t0aa`P^0}qM*8)ZsCjs3O78yUMI%PtZewVIR)~TA_}>Xw`}hPk+An0JziNpd z@8_fpoKvc(ugp{0m$pJW#+6Iz;Yl2bL5iS86OfUf79k`?7Gr3FR)~S|(Ul|2RfM2M z`-O~jSB{;33{9E%^YX0r;kD8Ptq=p_ysMAzgm^-Twjn`{_6r&5u0DR3IuGY4d#yA< zE5yKj<=Pd9*=|;|1U1?(WTd-xb!DwuDI;XAy-g_XOIslwv$N|TK0%EpAS2!NkLl}H zw%8ymfOmdzeNYp$LJX`QTtD{-YP4U-NO%4G{LIwiFEdlKYkX`nsp+ui2m2*V+@Ikq zl}N{G$$j^Aoe))pc>HrmP{TVRei5ACgH6ucCd4^H3|MQ7_634gjGc2o-igZ;;x!>I z`oa;&FEpB9Joml36d^u8)HwT>h7*dkFKvZ%T(=evek3tY%}OnPR)~-5If5EZK*pV( z``+E(5~EI9`Rp&R``X6P1g#JQt>n5Lcx&rnAxaDJeh)`bqy0k0{ipkS-EBhr*!}CQ z+tSJxYhT(5>1f9@I^!3bMhj7<$Jbexg#EJLP}vOIslw<4R63-ao%bEL#YH|FuRF zkU_XF%iS%Oy`_cVf1jWg(lLsYDiF{{X(Aj>CCWy{3PZ0 zR7(9+*rPN-E5yM1!S(Zw6610q`qX!QkQ(h5^6>hz?028t$ndIAUBZz;6SP7MtZLkN zl_u76${}nS@kAlX9~fIcjKM7zvb#aF2s}454A=Uka4B)RpsQoGbBcTsloJ~U&j%& zf+w_v`yRpXM%^mJJwkjfBaqf;zmU;l+?SMZ7ve2xAs82Zf>uaJn|^LQem!Zkl;eA8 zyN%KvL5=nc87<#^>-Qfa?w1xa@wdh{h9+o*80ed~PsMK`wU=B~kv>>6B&gATA)^Ox zly4-T3h}A5ko_{VXkIDWBb1;%JTb3mG%D>mQ9}1#s}!#@Q_!x*kPe+6tbq`f&Zci^P!eD!Xgg z2dU8nWUicK7f0=11>st)ql_8aFVeA!+SLrd*5eb@XaX|VU6xoF?vWK~U)qYXbM5*{ zV#E+A2Q->sJU3F$ko_yHMGIs%O8e4QNXIoyvFodsLPA!rmY_xxkdf|YoU3Zp3Wm#? z`c~P$(!R76($N;W+x3-CP@@URNOv>m{_f*MUgM!H+cT>E}b@MXAvrG05Dq+?vkwd*UN zphgprk?vMxAz^o;v@dOibd0X996mvfCLkl-m7}9vt0m$7mG-5rkd86m)dz^hil9al zkdf}{qp#H9&9bjlD0>asm$pJW=0Vr4e1aNHK;|kcIq%ZiwSsUj!^Y4Atq=qAx$7T3 zL5=ncnX9Peyb-b+74FH{9;FFdAqG|-uAln^HQFy^uA=Ntk=?(Ny(Jq%6SP7MtfJg_ zC3{N?ySd#cOHiZzLguy&4p<=_*X;$n zzVZobv|q?bcQejKvVXN++SHY@f2DnCE2N|T@3-qKpP)t)kdf|Y&Ie>?qLZ}U4OOqO zF*HFd#6W9*-mb55%dbmG7vk%Xpho+JjC8jGXm$AKlw{ezdO>zrv@dOiboAOg?E1Z4SJbt%uv9MxCmLhVai zAsw@tYggqfT$wUjh+9H}8cje(x@%W$WdCZMSoaf3`_fiO$L#F-hfh$W3CKuy{X^Ca zcK=G&47S&5f>wxu)rafnvSx_vcxjFH3z_S#$o*ip+z)2nFPh(G%rw~9!dcfHEoT{= z5JCFHMKi+_@$bnR?o?S%cmK)})Mx_o#%pGU1a`ct2=R*CA823Nim}~OfGdO;F2s9Z zIRd$YMiY$ZPC#5HL>*aKHjuj(?MquB{hi)3jeDArD?~XVGUU!kYcv7*mz^`iGxTGH z7$s}!?sE6#6SP8l-(P1K_f!UVUbldd-3hJHej!gDFf%+6pDZ!1loj~wj?A!2Eb_6r%r$G4Wta65M$X)C0kyUdE+ETzO<~wuzx%#F_MM&AS9^Kej)Si5m%1yq}0EJJxUX_LX4I(rrO?|oYzre zTqbSm=8&L9`-O~meq2At&Ng<6(xn!)FKvbNnd9UgBYU0!Mw?jCVP@@URe48gZ zFC4Fm7fbuqzO)r%yQcu*cvZYU91~Fvj-vF-c<#(yYspc4DS5R96SA}~ZH07PL-#ae zq7bJGQBwM$)@TAU-;PMmn=ZudQiEM&55OmAg>uaJZ+1_~ z`U$a7+EjTN-?c{jg^Yf_(Z1D)`gnAMG19i;2wK4t#ufLpZ{pHg$bi=bE4uVqhM0?W(#Ey`)XG4hd?s zU&xr@+!^8DWxU!i)|F*u)4sG7(lMXA{_%vw*ePvldq_~D3CNhK-TB?;h1e`@>Z`Cv zX@XXW!8K8G9{PDlxlS!Zf*S1?GFMKKd$nwwHF#OpY9R2eQTs(YR#EQ!Re07QLQta# z$Xs{Hxa&?pWa9*c_NA>D+nv86fm0S*qY1`yE3ymLu3Xqv*6iJXULHqqbp}tkhVJ}T zt+nlf>x5_<64YqFkdf|IWNqa9Rk)@u*1ohA($RX{`758GMiY>c?p9<+b^gjHXoYn2O?UnZ#CzfSE3MIfA%k!$vg_ph zRb^>Ya<67{MPJ$qo-nSs^H(6wR0K7efQ)pvBD+=y*(0<2SK61hLORA>R}P<`MiY>^ z?vnY#U8^FTRnxw-71A+(xccx3YBT|vtEl9>u+EEd=1%+4R!GM@=-QP}P@@URTty}4 zg)OmI?gwoQP0$K4FrT~r0b-2ghy*p-FJ!KwlJhQ*9wmLSn0-(av_cH5K3qSqExopz z5beS~NR9Rjnd`2|SW?WfL=&_^46LHuc%{!@X^r*^nd`2|m}t*m`2?+4j_CO-?q6w* z_RDx~MRvy8l`VQo$uE+<2A@DVV1;yCH+R0oC#cbWAtT+*II?@5BKz&dvfrMqeQ7JC zqyF7F8K0m=6OfVaX3it-WDVM`jiCuzAqHBzJMZ&nNXU3)32L-o$Vhi9fZgs)QBgY? zt+g+0g>>{pho+JjC9wo!j_0eXoVP*v|BBKxnbr#q|tE9^~k6^7FSAh23;=NZEL!ECu7w0C#Z zXaX|!uc9Zmuzw}@gOU9!XtWh`<(~F&|4Qx$ErI+(qY1`y=dZ&1!ECu7EYiNT71D9t z+|xenWmJ&0Xi9kROpPWWVYSI}qz zGS=)qKl>@s5a^j`vXSILDlG6$?FdMk~cu0t?Lj0!ouXuGyAO`Nd zU439*bgi_h9$}r+m$pI-%vY{mh4+I+azALVE;X8fj2#5muEP7lV!0o*{X-M9LJZ8# zu74!jzhY}Aff(2cas313zzMzGVUMCOZG{+EKg6HE(i%-b#{QM-=h(k`RsQ1ql~2$L z=~xFv$1B-yFP8mwOHiZzLdHH*%y<>Ef5q{NR*dbQ_H`EjIMes8+P~r`N`mp+`Kxwf zE&J`oXQ}-w5-119U0g%=G-I3)*uQEJ-aAvH{X#~4xbr1)mZB4=lKDw$(dJ4M zv_cGwEADCEy(ote^1mgh(S9K#-JQR}=yS;ilPUkl5wt=KjJvKJJ%!jRqp0i{S%Mnv z7qZ=%vgPO?#9z`v>Z$!J_|jI0f%(JL2g-4y5S7*b6|XM+g2v1cfBs7Q(pE^vJm}h0 zC5dsFv?&?kY&odW1Z2#Eu3d%u?b$fBrG05Dq+`~0{R8`=zsvsBr)vL-t(|^BV?KBN z13O;dN(F6KsypK;% zqY22-68cje(x|=yqk&`uY7N__|*~`$rv=!3P+TD4digJ2qj1aem1T~s~jC8jGC?V&s zDoLBdnIi2=TOl3&(VbiJ32HO}8R>2%qfb+5U)l=k7=7Z;UulgdAcJr#vK!=#SFiB> zL7$)%(lI`|a`*%_+An0JyK;mjx96|4FKvZ%%m%JLKvWD*Y-x=qAcJuAaY}gpigiw3 z+6ta9tGRYnQCf-Ac{b~u8cje(x@%WqOT@eSw$3#{E5yL;96f&}tvy0eqy0igy6Ycd zkFxt$+LyLMI@S-apZf$gnt+US*U#nWSBk^mh}gS_jB}Qhaa!H&w<{tER@C?e z&WoOypcP}g{dPYF&Wk|ub!b#qVR{20{0N)xEr7$Oow6SM-2 z>*k&)`Z1`1l_pTLF+?PWCTIm3SK2)p^F~^Kn_n#&iCgCYanX^Yo8Mnv;vK} zBz|Pj8pxW!+ULXstw3Xbj2{`a2C^ow_Bk;@E6`X0#E%SG16dPT`<$4d6=Qwb zbnC<1$Cile7Be#Bpz(sB2C^nl=P?9;ID%H7aou7@h8z;qz)BOS^B4kvjiCuzfyQ-< z85wd&Py;JXpw43m05*muXayQqI%Z_ZAwdnSG=VygApqDInxGYE)L{Hr!jt5SU<26SM-2xx}s8{TS51N)wp7PE61WG-e-n|KZ1=23DHD z+;w7tR-pOrNqoD~tq=9jmWb=-b|%7H<)F307d4PIftrmW0K^fr0*&kDc3Auv)WAv; zsM#1I5xcNU#3yKlbl#E0$DjsQnm`WY#}e%eGOZwE zE{Pu*w8kee+nt!86==*Q@gsxQK-L82t`ifq0*$#Oeq_)Z$eO_1bz*{6pfUTzj|^G^ zSreGMPE61WG-froC!<K zf*M$90(Blk0I)GMK`YR>ZZRW64hd>tr3ut|3<1E#&;+eO<4VVj3^^pIft4mu=P?8T z8$%Pc0*xAUV@b#1XXMN80cP)#ldP4_t6#XIWV6C%Bc>OW{$gz5cOsuJaz#l${86*Q zDKnmycV9awLTfYu`G^oNSmLxv^=ym-vWM&wv|{X&ibqR~(-omL+Argkw8RsW&dVPl z`;{eS-_j>g4pG<>-V`=VocdmBfq~8r-lSI+Arj`Le#Uwmk(CRZzMZj-^gyN z_NA?mUZql#!WVn@lNdfhjV2&h6=I4d=5#+CY>+*;X0ng0eQ7JCAG)S~;jUF(B?gGk z6hVz9Apa)BPD|`sUl@EZr=y3+c~0$1TOqyRqnd?xzSKct>=xn&MNp#&$b~}OVTtdy z+#Fmow^Y{Ea-LfI(pE@6u%$}jnZw&kjGZ1qjV2)fB19!iY`OXAg%YEPF*HFd#Ax_q zg|Hl736Z7%>4U}WgVbohkdF!B`gyD2XQXzK zHZ?_RQTx(XNdJCsQaD~+91<^t1T~s~yhjK(UY$9qo{ce4+OPJdtr#0)WpbWRpd1`U z>6h^^q9*4(IqAIA8>Qsc$IGXDs#!mey#$kkNx5vcy>n-c5O0<|mw+ z@CjNW9b?QZBZ?%3Pf(-%LdHn+vL$i~`yoaaV`zd_h=I}7mBS~f(S9Lg+;!!sS*>-8 zjWSZ-E;F0lLMIpvoRNHU)l=kn6+L1Ac471Ycv5F^SSFEcMRGdNFU5*AEYmB1y5WPCFhY~ zAEZVTkhyY7&bwJwOMRqGJtxm0;j5@hi42YmMr4czNS}9AQifY!-6{lr@$~tSpay

}RDX)C1fxT->iTVMGEHJX6D zU5Lc%D^1V}G2YCsl970QMUD0g`3)i5`l?t~OINAYk|t<{7!O`hGc1QsP^0}qen1FU zj#rny(_*!>kUPU`r3qRg#)2jF!us$DYP4U-Zwuk-qpYm2=E=I}%COEgK`X@A_+aC( zU6m1Ht`J>9f*S1?@)tt5c2%jvij=FwdQP~$(gdv#qeRW~!v5hC)M&qulZ0^nqm8Vu zu9fT5DZFFR1g#Kb){ygUZLFPj3OIsm* z(FM)I@v28iv=0etGyyqZ2sd7pko6T}WQ6yenxGY9yY-b%pd1`UNid#UUp+1BtE;5s z1H#pkCQw?AySRpK{_qKEv|q@$(r*6vN>)ofq|RRoXBzo?x7cyqJ#Oo_f&TW9V}HTJD8{-3Pw3U{2M_l(75W?B%Pjspaxc`gDt0>VXtm9S1I@Y zRkW;1pX`2H_hh1X^6uxoTF(~#_o?*4`wq>vcNX01P0s85!=FXD1N&s3RrIYTsL=%E zI!iOcJ(*5IczV4F!(u8@IphlMoa`X@&&Zy3e$zODR*3QPwDSs!zF8+RM%6f4 zwDQ5y**O!MTY?(x7c%eFlk;8~bEN3|UPW2e+jNQ}XoVOl+4T$0Hyb6!yeEzpb!#;* z>xE%mEJ2O-3pu(oQF!^0qE%BHXJyUl6-UqtF-DztcH!cbEfVAY14oMv)!LK!(7@i7 zpho+J%)OK3yjPQt6qQ)g&)l5RH;$kcV!Uv$eBmpneJL?cZ~ITt$EEg|$2z+h)M&qu zxd)V-ch{!FMF-nHUbJadHyc9}v_g#4O_B@qx^0jc4^H@}sC0>WMH5=OYekLr3z>Ua z$$8D+I$X4R-Oojj9>}mUG(juGxTjZA;e0K4tmw#@U5YQ?HP;fsWsX!WbWK1=k*(MxF~f? zUwg9HCuqg=r2gY1#@S(vULSsMv_|`7Ja^~yo0Maklw_rH;jD{}DAt%xDl z>Myxg7wwvB32L-o$lQBR&Ko54akJD%*1Sz|1g#JQ^;G(qP7>o!sq^op&JQ#yU92_Q zFJ!bBcPIRo>TUiVV!$hf*S1?GDae|!!ldOtAR3J-I~!ij-VA{VC36h@eGM^uZ)T3*4dMJ z-#{0G8toTy^hw+b8F#;taW`X5FB?M>v_cGw;3bkOON_T=Y`;my_DRFswW3D*g^ZcO z)yLa1e{6B{hmD~LS|J8zqHhWtNQ@_Cj#?pe)QAbqEkTX;3px5^^bwf{OW$=y_Js4B z#u2na49uuQI%i0X44G>i%Ut{E6Rz!2qy0jTKHA5=YPqF|cxSD;ch@9^JafjwRG+ zzmT~mO3wS|p1+DdlNDLxEpA1|`z822TKW5Qu7jd`$US6zb)T%Su6)<6$f(f-73r`XQu1R`@(JOJOy@-B@a6CJrMnebYq`3+6;c3b~s2?%{N|BC8>-Bvo3;$Ki^M8toS{?~3KCB+?QaOG|tp zT#;#lR*3P}h?6p+E3z-8wci`A$f(hNA@lA&Id6vas0Gra&J9;&nxGY8-1$|R47Vbi zEWP$#wIZWN`-RM1isZaSGBWI!k>S#CMWzW_Ax4d#&&+TuvU_Co`At@2!@?CAHQFy^ z?zYJ31R3F4s1=zeXoVQ3G^v#lU6CD>71@YzMMjPG3z@q{vQCtd`a4;XZ4OstnxGY8 zeDO=847Vb?M`nX1N%M-PhbuB_v|q^4J+eA7^yog39_Ui@mfBBMt8 zh0NW^FA1#8toS{Mk2Q&%aieHk{hpV$u&VM#K1V_R%Ca0D>7=d zU&t6yT{(=5yC+R+oRt}_$TUGK#K3s$R%Fvwxu`O&S&FxO)D zV^g>yqelCM%%{D{c|Xd0UP9*c>EVh@6SP7M%=2zV)cO)T`vVS8@B{ zg%9^1mtM2}_wT^(Fp+DBjPz++pGqG#^~ZN)H%f^1il9alkf#dqkR>iN2V2Owz9O9K z)4sG7(m!1OLi#fc4@(T6phgprR|)a7C9=+YCFNatx|AW$y|gcFh4fla%t#-;{1jOs z*UtSlWw{WSgakF3fLt>|EO@cwKcQYh_9TOs|D_g_vwxBHnA!zZZG1mso`;?=n#-!&pL-NpDP0RfFBC_CchLH%qwr2S@5qx~`-uE484}WdEkO-)0s=Cw)sL3=d(KGKM;t*b#750ltGG#G_yjfDFJ#ntRZG-wIWe_` zw5xUUyjJ_tR!Bz+9s1SR5~E&7d>j(gXaX|Y?wyuE4Dnwqacm4t&|DYoZiOJ1g#JQ*}>w>yHqG!dJt+7~oU&60BlEQj{1e@-cIbzVk@hZRAM_6xb45U$R9zELR!?UwCI6SP8%Y0GC8xVGyP)M&qu zrwifQ?)cX$1(l`U{u;J+P0$K4mL7hk!1Y0&pho+Jyi5q!2j@P#IgoKT8|TlpFKvbN zqO8{n+*mR%Byj#*Ycv75SO_5hBrZMhkVs)Yog}%US2_A7$+I30ff?EzzwR ze1aP77c$zkBV+}TAGTe)bEgSfAqLu5WX%vmP^0}qMr(J(k&f;1yGz?0C}XeorLB;T zmgr_@pP)t)klDs;+YQiev2K8dEdzda8{--^o}fk(khzj|&zoblPAaH zo84$;PbgVv<^4CBG8K{v-8 z%dvIZ>il}?f2C-RPaGZJ-Q2tH$1qo^KYg5kTGfBCR2buj8|S}0YgNxBTBH3U{n(HjO!o+Z>x4g=&{mLftzIm7N*F_ob4c8B z@uOzquD5M#FFhy66bD~h`4o|n9<4zV)WEMM79Y(u9XhiQ!Ww^Ir3qRgedq7FrdOo3 zJNu*@Xmqa1N)DMT#*5EYiLE(i#Un27e`MsEHAQahIzB-S;%Va3A-U%FtSt#jt_fO! z&U`Z0RKI?60z&8N{4;V*gU`3xtIK%t<)AP4{phV+v#sLBn7WB4bgnc}VQsEC^=U442pdBakQw8w5hKl>ibrEhZp%Rpdy64W4uCej9vG-ao8 zy{oU4CTIovxAr4VqjP^wQ0LUZs$$KN=FNsb{tvm*MEhO2=AmWS@$&ni&K0c?uidO% zbA5$f2?%OnwW5oR?bm;If?T!y=Rxz(y$7w;k*)WcnoSp4dFZ=$+jQhADQR{4fyMVM z`#x*t+imRGY@7v#tk3fD^lvPIQ@+&r1ZhVcS>CR=#dUSF_32?vK<0Vje?XOE9hXpK*hb}{O$|Gud7wx4*%8fi{!{eO zlm~1%sDWR7s+{Sug!bhVq#bdz`WxoFs*l-nP@@URJoojy+;%Hymis^it1GcdQG@78S2}>1gU)l=kC|_cN8cjfE zx+}-LT5p(!RUa$X8p!(1ju;||gpTeLq+NY%p7WogUnV_Z%R!AMAfsL3?U8>cXoc96 zT{&94^?lKRP6KT@sDWSoHV4yP3=-OxPmp#*^IvNhci7(EmV+8iK*lJ+_dk-7jz`c6 zu_?Q9EST7?_@Wo<+Hz0>zZli39UK;EA#;$EJA?6gT~Qe3AAAS-;H_LnM*VzI=kTE61psj}>RtnrA>zqY21->*n7H zTEP=#M{xGk8ts?qc$+6N0gWaoM~R`;9xGn)^6Qye16jW-!}ot&3=%rJPmp%R-h~eo zA9`Vvjz`c6o+u+mKL>F!SmwrF{sggA@k|LzZ0|q!E^8N1U1?(;{Mxk3%%*(TB%FD8a1sDYIx(0bgNRKMiZ_ynKC#>dbEtw5tqyR)@^ z3~FGd3G@~B-1*-KT7gD?bWg(-VN0%K&`S+Ykqy0k0NaRiqs2Gv41irKt zWS$R*xmIxmtw3O`Oq?rfv|q?P4G|wh6ST51BIl|7a`1Gt_6wP(IwCP5tz8qe0>QJ~ z5dz)AmYf>x7cx(${5wG_5Ydx0VL2kX(lKZSf~VNyV^E|0Lgv|@2oZ^)30i^RY5WL* zg4>c)qy0k0X{y9yi6&?Tf^QW>VnlL9jrI$f?>fX2)c6G7p@=7_@rmfEFuz^-1jk^W zTa3hrTq|lAg9Oh;#uL=|1W#_KXSTdH27mdf*6{h6=nOtTgdm zhu#^_-C4@tPgK4@&xmUc0_y(YG9=azOxZeP~#J4EFPSZ{`k`ga-|7cfkwJ}kHv3S)WAv;$l>5; z@V>JmY-`s9tw5uEF>BFCkD`XPcKeEGn z@?&dI6SM-&m0C=lM@ApKUk58qVC>pCFYXzO@&$rckTF(1Ik-Y}O`U^g0u4s$0Y98+ zSIBx@dVGQ!(3&`YIeshAxR5b!uaR1aoPhrSt@Jg)UwVkYR`dnGw1UjLjCex({c^~q zg}9=cjwiHVO}sezio&=$w^vsav_g!`8rK!Zw8Th#Xo6NC_&XZ$xuQnyX04vw`RW8ZA8(FA0k*mNa79ziR3 zqU_4?`3>?eK7{kn;)Oh8lONpcv?Lvsl=`K zWw-1&rHJK#MiY>64S6E|_yn!siLxt4uGIO6$%kwVYP4U-XbU{2pOkbwf>ww@*~R!` zMBD7=choD^8toS{T02e~|2sh|c%tlxC;n)Vo%??4V(kku&mkY5&>Ej0?P6@HaX9On z{e5jYsL=#uo=N_9f>wx4*%9wlo|e`2$h|fOHQF!6M}2ZRDJc=5tuVUA691jIA@j7n z5ye`g3CQ}avwTVCI0UUY=JRxPQcn8^H<(2dL;HfvbJ}r45(%yG3DWLbE&1WaqT(j^ z*>X^$3CKKQ{_g~>5Sy|ij&}d8sQkiyHU>4?FXaED>`dTvD&PM} z=gjo~d42o+uGjtfyszuN&w8Kp%sGco_T)^^g&K@)Vsnv7R_1e$n;KMPUK9JD82S&w z?=m$ckuoy1nYD0Xv{@f`&I9|J$t#``V!K^~gv=E}h$oT4!g@DIvbGGonrR5XA_>Iu zNu=xvx{!&n-K*Mf4z*^!I@{EsBJ(2Vb4)oCbfE@gn;4>!ty-6NE2+r5CKgXSg@t8H z_+2Jb5K$vN**gEk-bOMPVm=v^Js}k*LE0Y2nb(I}2S?9N1VKd-h~;xdVPV-3bRiRC zyGFjmBR3uF>fE>zNO(>2AhBMG}bl#8A!zU8v32ChGq+H*;r| zp{52EnHMpi7|NNT3pE(q#5d!=Rqv)Hn;KMPUc|xI2H6pGp$219Bjtr_Q`Gdqvb8sK~sC z`P5C$1YM}X*e1#}NU&z4K4@xCk$DmG*_@mSx=@3$Od2%M` zLJh_?vFf82^&4W3n{yNunHMpi1*gFqMdn3}WrZi7awh2FGRz{L-uj}o=F^`NWiG_>`6$wM4H7b!lOSyq zFTB{^Dt)Gq7X%eaAm)=+ITLgt6JwjG;!UtJ$3AFkP?32N^LebC3A#{&u}xfG8EGB; zwYjN5Mdn4!C%AGZ=t2#~Hc{)>zcU96((k|)ip-0cPjlr=(1lElZDRGjHJMw>4zkyW z|0y$>7crkP%bB1HH5l8(1IOBAwplRB)Sx2sBIc85ITLiD24kD3`Ti8uVaPaBgNn?H zSU%wv7M2}B7iuuJYv@~wDf)kAenka&sF_>|fc?lH|oN*u%+u1A4=)Yi=ijyF1*Lb4l7wYhF``Q^#RN?775Paec znb>aEAR!eeLE0vorq0fcnL5<;ii#u<^U1fI3A#|5u}y637iLW!*2UDIBJ(2V^LRNE zbfE@gn<(&6ZL4~tcvFLl%!`;$3Fb`Dg&K?vkutu=Ggi`%Ma?>=g1qv1MYh{DNXT4H zg0xMXn$y=>GVGw4GfGq#CZjbF3=$v4W>pd#}k=5wAo6Lg^lW1A>j zbDXto`Vuo|pd#}k=F_J+6Lg^lW1C1x7-uyvw!mCFQ$b$&^TD! znHMpi?aY~=3pE(qL|nUi){3Q5%X!#oC&&+iLp(5*YrLs^{-B*1{IkXF`p&P znV<_b7~4e8n`xO(935h6P?32N^BL8g3A#{&u}zdbw_knynSJd{Mdn4!r)YB~=t2#~ zHsL=G-I zwIt9(#27i~0HFc=%R1M(5v1*r&v;{ib^7~sGjHdU#TbWtvY73`1gcYU5~OY75#6U)fN_FzIPPJ*-{QZQ^TD=Hw^Lu6vRT|*Q4pS`T8I0@1qvYx@h^ISi_ z_tob0Ex$BRnVv146yMPQ#OaQgdc>nWl%N7y5^uI09A7E_;r}4$0{z9EiSb+4xYeKn zmn1UBjEg@v+^vQr=mPy*|2gr;Pi%GR6&1K7(P8)G_%Su?r%Z#Fl_cl_{nFVL@kuwf zyVRfpmn7QmeIx#G?9u-q=mP!Q0V{q+v0W|%6}TkvT>A9*)4hKD4}vbxs|Fs3FB!kj zg`fhLB)Swn9^Y*EH!cJfCs8l;o%kKUI={#Umn7&y`|hs4#y6h$ol6ZWa7m(i_21)* zo^snts5pr~;@*o-S$y23h9u|${rf}b;)kW3a3QF`C5iFd-;1v(i4^R&G8baH81wn_ z;4M+^wLj=Rn0qbv$I!hN1acuS;*Fcjn;7-nYfy0#Z9dA32OU~N5_Eya66ybBgbP6h zE=eqX;cWc;?xkG_Do*0BpH9bL>{G^tpyDLPoIe@guwz*lf{K%_hyYeuuN8Gk6Nzes)Mwc?4K6l*+Dsa`fUdA*1i_-sxUP)r?t+Jlt zW8Jny*(gBTOt*>F13&J^xs$Z|IjN*ESMDKS^2!%wkvx@7wTQfU&(Xl zJ2!#~Tow1ndJ4wd#}Z}=-L9xOiM*dY;OV!*ZEL6EBqpt_=9xNP5P>am#h;tZ_5-5f z&~Bb#{-Ygh2ldMAA5@&gg8h-6^bN(cw)VibD+#(le|ogMr()|8!Gx(Hdxe<2Vh@9Q zW%du5moC&8weEcn-{KK`HlX4h$6F6=^yGf_kjptr5_Ex%DzMt)KI@|bmn2@E`y=^(OPk`or6WD75M;|J1NdmF^Udv+!hzi=3eyzm+ z;n*$-o>2q63LLMBU0mi{lDylT&p)ZR+Bc`i0Tc7BF`)z%^hy$OG3$M+7ygj7tO6sK z1YMx{)|k*5RN#`t=V|G_1<_(o9jGA*x}GB%U6T;cK=2Y}Pme%SsY- zf#zFd0^b(NcW$r!KAGUNfh6c+ z4Zcq%v zgP;ras0+({yVBgQiewGCK=3Vap}nFa^CE8BccHJ+F}Jy>BO5V<*TVJMnTnI} zo+;*YzjmhLB+8a3Z53j1aVaq z=#?bs0xjQd7&u;;8dTtt#4>$0sFhD#;{<9*f-cZ}=V8z|%nX1ET$11%H9!PvNP;fV zoNVVV`zGJX!Ke04{XNaJ56YMxA52IEv?LJQL}yjsx;wjPBm937 zyOLspoP1N242^+*Z#JRM@2uYN-M%?i3knI{@p7b4;l-&ypaA*i4R3B-K%U4O|erJhx2-J6Mf)2f9K zbfHF(y^kim`gUnuqyF{=)+-ZcCte#GV+blTFJeBoAD$Lbxt^6&&Px3ELb(uvF4TDa zXsd)GHSu)+pdt@j$D8j@tgMAZXUpT32JgnEp)uA!o zUK3^;f{M(Gm~W}D*ZKamYTmnR-wYw>LXA!D4@el|*{N%k)!Ws~MGtyaM7$xW$h?U8 zMvw5cAN97|>E=V;(O0U65Okr&wqFM)eC}JJYwXke$I)%|ykSd6E2+r5i20ro{iSie z@9F=%;d6S15Og8a&bveWZ#;QH6Z$`IkvTmZNk!&m?I~+s^7rb9?$!NEM;ikDg(3;| z(0*e5m>&7_dgQO@Jy+(EF0^AA=2=@u*C?l#uHIw4>3YwViX;$Y-Plj8->3K5J$eo1 z(tECxpbPET79RcOAN{tTc)gWe(pyPSz2{0r=0%Kc%zk3MtlkpO>n(Ab-gBJHjD5v^{`{KWqpIpX>JPo=ItjYaj=gsN!Ejw8N$<7Q z^j=#`|0flh7cusE`}y;$y2jr+eZ)s7OBX*h;i() zpI9%Wm(`1UIo;ID%Sq6Mb{s3mw+qu}92`X_=yid0A{ChzF^<9hC)V{kuc+5I*1MCS z3+*`OpZ~psu2DMcY+wi~GB0AB8SE$459w_;Rd2W5^!6)rNf+92?&^4KvaYd1@3n20 zn6sK8s7L}a&%xnoUG+ZLLZ3N1>HSdVk}kA^u+Ozu{pWN1f4FcK=Gl^OZUKRIoP+gu zMg<`i)&R{KHqiyglG#1g^Zs7(OwW3FE36@ASIx*8dCw)Il|G-Fag_Hfl0cmGJ^(D; z0s45@-M@4tK^JP=?huz@ufYm>4W84--0Ax0AQhPxG2d7ao`xesk#%ooe&ydvm`l2l zsY9P88TOWlBf}7V?l|w?5~)Z6G2grpo>pFO?UVHR>70LSmjqp?v3FSO41162t@o&{ z`W%?%-=nC=yomWei14&O^ z=8`V7Z`j#4!#+|s(Cg!@o?FfEAE~KG0x{n+qUR;!>RCVN`CESf*+3F>p~kH`12gP1 zP9eQrEzons-TpHU6`2FWr6q*%B2=AjZ0}-yw8eufd~w z4d&B(uFNG}XvdnhXMOebR-*S<>s|k>kBTG^V@tH(o`k(tpP#JG{+XpD=t2!_)AsCj zfZp2Q)m!@x|Lm2D%!?TNiv1R-8}b*AFKMSWK^CHGR zZ@-%gH4f<`LrwooUlMeo296l^)dbGxNAwsl7OBX*h;i()-$zzXFRO8SIqCUN2tgMz z;mBuS72zoQf?gMw{Z~a)WM0I4TUU5mxIR*c>Gi$Sf8`|!x=;g0aQo^GXM?kPn|jNC zbw@?!Ma=h<*;kN#^>)iENJ-Fz8aNYWU8UCh$eh*8$f?M@h+CH5+7NF)XtN|i)$frf196uY4FWOR)1N4)&*yFg6en^1sruOvRGb9rJ#>Iykkofq z%s;rC1YB%C@%TzTZ+G?z1QgaFff}oi{rLYuz(qQ5A>~4_zi>4ilVJKzBB(|R1|k(F z0Xm2XjC{eu1cmA#5X&)T-zztQOT=DQO@3~={?id1K5wS;dPe0^)`p#?MuYu%)bTDS zO+0l$xLUvA?ckB8peIxiOCsq)VU^tLoh%|Sa!Jqydi1}ARE2YI_=zswQtR84J{#04 z^zhh$Fg4-zuY-DMkE7WOmDZ<@Tl2Z>i6jtz`&^hRGx&Rdjk&99u0P-V`4ECG5S5FC zscHRx@)Ik!J+waVmC5U*g7|~K?`9lIKIPJ@x%bsxf27N&^_;yT0eWBQFm+=IxAwL- ztFDil-@U$*pbJEm7sJ%0pRf3PHS@3X>mUF3nqW#RSL4}2F= zg9PYbX68~2cH3`lIK39}z^L#>mMv)r|@E+2Ck$tw4~(N>_Sk1 ztKh#8s$-dR?!8Ju5x8UxNenJhS``lG*RxHRTnM_5Y2394b^aH(IRh2A8ZL`a*One~ z@0BUz?3E-IREtp29rwA6T#kb-WLo`NDK+eh;|+sos^olOj8#$_wnwn>a3qb`gNgTXgOwIrKFR}zbh-miMLj&wO2NP;fVJEj#<)BkiMsK6zOgk1Sl$E8s&H6%e7=;g}` zs+~pg3zl=fO9d`Tym2v~Dp&^h;c_DA0^Rt_0_v;QZUhy$B=N?P{A%h_2Vss^GxJ2L z;mhs0NZkUFYT)JDW=m|AFG?-!am!^5Qb7$#Y^@)uBCk8XmuhNAf-cb8%S5R;+wQp3 zpaPd9a*vNxug&MTu>D5{Q$rGTfzDeiO7(u&ZC*kJE=jEUBT}`w=s5108j_$3bnMGf z%Cpfi@-V3&mc*2S6;-Ws;V$c(xj@i`xM|)fHScZbH(8;`HCQQcr0Vu}@Y;0Qu2x2= zipTAlUWKS=bs^uq=B$=|4JxQ7iGCST>Yv;9xQs&*bb;phf_i0^uFSh!^l-$CXw~^A zw{=bhc_q=gXpEXS)a_g=3A#Z0!ei81L!IB@l?q}>+_@d2x?Xf6s5psBH)GT?OY|y5 zZ@*^blAsH8?>8%{emfjB3_%4hNgVvGl6q!@qlVcMB|#VH)1@k_q3zvjP=QMl(fcc_ zIcWv5);Y#uYDj`E&`m$6tXj`;tU*IiflCt8K8{sA9~1Ao57dwZU7(v6k5zkTi8t^E z2r6(%B7J16+WNHHwoAoHSOcr559F7W12rT;7w9g#VpZN8h$mF7bWm zKn+RI1^UkEST+6FU6-RM6}Ti(k`#i~Q$;z%7JsK6zO#rdkJ znGtU14=PT=_h(gg`3JGA0yQK-7wC?KtEfr$<1YuCXHF_`Nn%IAYU=Mgf(X=*1YMwy zUaP8#=W>2^T2_}X#5KaIsDds2a_JQnC!tnWRfh^(_z!|E&<&bYRaU~8V8R?3@}^f& z|Ms`t8S0EYR>1bgxLqFAg?4wUXE3T4>-P?5kkq|YeyvqBDa#=}&E<+rxsCqu`xS}^BujN-{Uc`gnk5apS`_83TlAsI3 z)Im|ox9ZS;5Ojf9*Px=BWc}hoP?32N=V}|J4!8LBKM1-&NB@=pKRg`gtyA|Bi@ zO3m1C@IMH;K;&N=sXFH4-CDUmsK~sCw~UQc5w+a5L@G|AU)3np^5WMny^;i7psTiz zROhd_Z6#FTlEj3_D3yHC?H+(6=mNd9a-{0r%&k{c;F83Z3Q=lN!96bHkOWx z_U+0dQm}-jVq$+@#MPTds!Kz*Wf5kTNX1E1su`sUcEw+}`Ome18q5WPF2vCjBURh> zmJ30}N$fbOx01IvXAyy3QE?Lcnnx+?ytu;>=#?bs0^RtrimG|tjV?8)z$J;^uSTgi zwr|ZM0yQK-7wC;^^Pg)Nzett%3DcAEBgS#T$xopoZy{BAql!b^ebCc zoj>Z>uFTSLZTSff|ya3&i^)s;Y}$JB|!yol}u{5%*nHRn6EVj?}0T zhPN42Z#7ZLHFC6Lx3xwE=g?rZi@Ok(=iTHLlSg>t{#)BHY{}9*)}7m0+%G(bWBz4hKXJU#vuv1 zK<9ZTRh@a!u}7I+QGrVmy{D(D{NZB%2-J`SU7$;jPE|?aj$?_bK?N>J_*AO$4G_Hw z)Q|*Spda`+Rn6Sxc7&q>mn3FiOI7{9aNfC-xe(KZxajs&)vt~7w+X2@iT{0Q3j|$=o9<6l$J*R>oNJ}xBp!{Ls{U;!h(HbI0zntz#wDhzWy*P1%~^vkw9na= zs&4q4zimrJ=0*Ja)l_w5-6fZCFqd>8eoe2z-M=~SMoGm5^PY^%g_thHM{}pD|5Z8XG7c(E;>B@ORKxG_WSBlco97_I z^go#kF4oup2D z|n{{vQNgpfd+eQr{GG{w0T05KH3d6O&W} z-!YdO%msoj#MetqQq_Jq;X+Vx63yR| zPgK<&a^AC)xe(KZxOe}Fs`v=EUQuxp8Jyz|lTx|zxNP;fVHTOgSkM^h4}nrT_fFb zr^wWx;v|~PnxyhCcl63^C6b^EblIYl)%JMtcVvNHNrEmADa9wNKD9m#Ub?1NRAgSn zPt2LDuC)>OmI5^-K^KT_ODC(QFMJYQ!;FK9%!@eB5=V_N{m=AD5_EwmP;rXta!vFK zL`V&~K)k4D0E@nN8#xu37je@WQ`C|l*0_vA5_Ey6nK?x*-RQQ?smQ#DCtR7L)}M0T zb(XoL3-PpK`r4p}SmzkHSyoh>MDs`WO#g=1KR}peB?-Df&+d|{+8kRKJaSWm3S5%- zyL+m7XsFoF12rT;7wGQuQ`N>AZoQ%cmn7a8m8urSisKb(m|jVOF3>mjq^i8_9QR($ z$f>|3iEAIEs!ESIeq%8;BtaMG!Dmy|{O28aIZX{Ja7p5xG(GY|;`|X9ha~6%J?>(v zs&&k9m($c}y#EDNyKg~rcTj#o6yKsd{g2yS94g4`B>45y;5oG<5W@vpev{P@DHupd zUeQB-e>7;`Zk8?;&~i-IDs)xTw~IS0ff|ya3pBq!8d`%2T$0$*s+($2!|gtUBA@s5f8O6&M~1*SBtaKw`DN1JaZrIv5>4WJsE5jn>)JpKNzi3# zh%b`{Z;AW;3S5#%=+;BsKIC?dBMG`d_tm``_}1a9k%!@*tS()M`Nh)ErAx(0^wCTA zK#~0}H6%e7XnwIYv<4NpB+*Q7C4Js=9O2A3BtaKw`7P7nHAn?6Nnop-@TA*ZR1$Q7 z<`+vt$3X=yNnr0<*~e|3C<(ei^NXdSHK@QP2^=MQ*K(X|&9agNU7-2J($E@I;F1K6 zb{&6m{Ay`xNP;fV{99O;jbF&*Gokb_|0p?olaupEw;|wu>Jk1L4hG^+Lr-bX@$b%aK9mLQEIpYu|QI-#_fU&ma{i(c(^fmm{2$ zpbPD{r*=?%l=BXYR1izz=~?Yvj?_+qF0^kQ&_Ue|IU7g?u_W3JYVUIXa1wNp?o&aW z)v#Te--KCRx)5XJuU`1WaW;^OlfY6~^MN=!2Wl`E2)Yns4Ne&1_Ls6$oCMZ?g}1~k zBTz#Ubb-e97Lo9e%Q&dOB?)ZDL9?Ym4N1@i8v70AUBSmhsUVgF_O75=U!VqafuIX< zO?_lYoaXr3bF&7iI0+nAf@ZI%VUAalpbK;}eH^RMPyALC=oJ;XB!QzSt^n+jcUU`f zL->NJaWWTTx^N5*p0{uK(Ys;51MB0Y;v{$l5WI9jH2t%`>a@uIy5qn%Ukt=(Z(K$E z&S^$Y0t#YD{B^c>HUt$nV(~F?R!cz@m%I|1$iG&hV zkk?7@OJ>3IL`fj#SIxNG0yP3_Q0A3A6}FXKFa$XwEeSbm=~_?nZ7lW4JLg6h}oUza0;BM7*@6M8-3k1LP z8Crvi%!~L&!g#eO(>ZUKxugs6gN4Sc%7@&3MQ)isULD9dX3k`drjJ+UIvqAKdl=d) zD#$B|gMG&9IbQV{f57Ez!gP7-v1 zKCAbrwqVmVM)sO^Tpd*f@s1X(1YEXeo5^>8?vbnmG1YMx-)5Nsj z+}0o!xFnJNbrcmR!7H+$^G6C?xccGsnY_Bot9uo4b!P}Fq@RBEz4=VwZHOT? z%o=1*m{$^r-FtO^`F4S~AtL765OeMo--aj&jxTtfW6LPKW1#wLegSjeCvogxwZ790 zGly$d)@~0asDPFPzxACnL6@mLcc>Z}P`}Lfu0#VESnls z;F5&N5*ByM|NGfO&jSxJH}(CsQ_S3|A?NpQIZYM3n%xu_tP z1jiIiVB5VkubG-Y^;`2qf0G%_)w)gJnfU7^&D6Vpf9*IAf}ny}64MqmS2HCM*gqse z7wGh9&DHI(j_170@k-{EJsjD)xk`B4d5wcyR1iy|?bv4O{6AtAu8)ZJF;Nn9fo>hy zTs0f#{7YG>AeO|5%}=N^&76OM4Hp$BaqOvPs$hNRRgsgR3+=5vPpE`3Zf_8v0+%G- zxUZRdHbR_n0wb3MU7*)5Y^FNjJ@2y4slX+P<|`jpm8UwdYh^CPbRn*P=5f{SGsmwo zW}Q=U68RfHu8#M0UZ2Zci0MLnu7s{J(0RW^Do$ccW>a-?q4OTHlb{RjN!6RGneuP8 zQ}p6Zfl}_rM~%Yx%$xQb{z3?>4?|FK64{RfHJG>O56#RO8Fcmp6}Tj^^2H~!IiJhD zmU}8_>|Iwzi0eU&!>mCna7iMeV{^T=JO5%!=0Z#tV(eY>3%eZ|s5ps;J5Q)j+c^L7 zOy)vN7vkf3A6)s9^KZPQ;v`y*e?n~xcU~1a3A)gZeWl@Dx8Js@z$J;}Cz`2>?>PPv z&@5d^&;=TM*R$_9o-8&571Kk1Uc`IKH&Z(zowIP6OS%wa@2arId0i_NCs8u)arN$L z$6sceUNIL4x)5XUS~A*g4N`FuO7DaHD!A2<1YMx9r@7DisK6zOYB7&yLr`%Nhx8|n z+#%!r{FoX? zJlkTQ2($+iQUNUq#y0WJ_#YCFKKzmef{G*%XO0;ce{T3;PtF8g$i&ztwjOvTv1{Rx zrUn(67x9~I2gg^+&l=egbfE@gyT+UK-)fYv*Emyyip-1nZ1JS{hWcxXITLiD24kC8 z-gTUJ%7V8Qa*f~qUVO#06J~w<5%*qv%Hrc@eXu>4KrSkvCBfJx8vp#7cjvW%nINc0 z0&%_6cj9;addxqL>&1S8Tv)pJ_9|nn8xU*{CZys#wS!2HWkW zOF}A6g0xKxdM`uGKAUXDK}8aXIi{Qmx=@?3y$0WDQQ6wouBjPEt<68h0&(!N%8sB*E<<}9>4SS%qrbVBiClmE zbUGei=tVr{{K@!+`rEr8*lzcVgjAdaX`8sS^$BZRM3fl^6-gitURK!=bjf9CkE7tx zWb3&tTg-9y_lM5K<7>vqyYz*#@$>a}ib1g5?iC5CI0@1=vGdkYD`xskb4;Wn3BH+y$T-rqKucV!!7qFLg`ZYczmPOu2*xr~D8cHIK+JX$_AwC(M%eMr6*CURr<3kA$9A>{6R1vwv?LId zFyk=mW9O=cn^}Xg%w&j=_2DE~5A7tf=5WZ1>Toee4c23?f(cne(t#TGM&h55arv+( z969T;M%EgnA_>{UP-4ZOn>?KLF~&G%Zmnh7LkTL#O9C<5No1{axQ6-X?Icix?ZJdp zWDT|l=AvfU*azWiIJ8?(4b%g{cGfVE*-EIe28p0vr3~$xXe}%MfCblE4{h}1e)f>r zc1fcK+wB@8q#}D5(yNra*I%#ht-nTg1YM}X*sk$Yd<*Nh8|BUMii*sOICy{DjO`j<9_w%IdQzGBRR_K8 z-rlj(tdD$?(>?KxcboOW_Fw|JsF+>}g0xM{{A8qcu4@l7zoH@u#8Jo6J^jAe<6ll_hEZ5fm~G36GE&PHZ`coyolS^-sJhT_YQxJ z>tTc11!Gu&$LlTT_ zqR-Tk-gO&#ne)ezv1L4629`24BI=g)jQymfnU}CVm_Q9Gs38f)hDfC|}eUGfh^)jC6U)cAS z*d9zE7ZvnG5{zwPdE994%MUzm<~~#;fp|ujGM+wnBmC)JU0eC)s=Bx%FnwKt<+7%q`UJRdxhjsKMAKey&!| zdws!EW;>u z!`Ev4S#wrndoY1qR6t9Du_00>j2!0e+v**2{-7cW#Gj|7`xZp=SF-E~x=@?3-78$1 z?|i-BCMZWCD#g9+rKf}Ti%u}!=>x`%h}%8TY$LPZjYxkPd%=#tCOCT6{G zA@RbuJLU&LefF_G=T1aeV9Pb9(ECQ?SuNc?Kf zFf$G+l0eKQk~2Y}p(cxO+Bx_rj|O_+)#5VMCl6Lg_AV|yH%x>UA& zkJ-Nod(RZ}@i*bJB})0$_Zq-|n;^p(t{8TN0&RM2-BbBW|k&?T3l zJ&smAdRSKr{ArGfpFLa1$KQnOJYB@s!vC8v+wB@8kc$d>B?-nh5&Ojx*7WlBZ^Be0 zfjD?sWk=8@m!Un5jfa!1>2;7QNYJvMrV~O=$qsJO_=R=uSg&l74$?BjBVon zT*IsqL+sy#sYn7bmq^Y8U2+-P1ol7dh3wU(+WXA;96dzLcK6?e0WAr}HZeE4hxKxJzzsavD%-)le8$C5sg+sruF9!ww?74$?BjBO%c z*%<4cO3l0=s7L}amq^Y8U2+-PM09?o;?5+SajYB^>GvoL~&qe3-IAF$6?BX)tl4Sc>!uDVSxu~Eg zl3;9)<3P&`i3JY#Fyo*i3B+6?{uzA_>H7w~3Ag1}1tgyk^N<(zRXJ*yO99kbbj*zXl1ZNCI&%QU1@ZiCreX zVmS%A&|YNkqY1CRUD{tm5>#Yf#B8_6(eukZ-c`+>H#Mk87izqIv{gcpnnnFJBtbN{13pFA?crGFOw*3a0urNtbkpyD4+cp09xQVxUlPabL73o5a zLc5a^N{+<)$i}9mth=a*rhbBoBoL#$gdrMiYrXEnX3u*^f0@VBkOW<*G33F%2~}Re z6Fdzyu|N}J`~(%57ctsn4N+)N#JZ%3uXykOb8Du|C0%IW^!|W^S3US}>;w0jKEyXVb1s4+zE>oS*gv3Ba3mvoK3Jzq1#Fh7C* zLXiaPO)ZuhONqiZ+`Dw05q_B@6tGH7;Ptn&hTUzfS0 z3+>oSn*8$5=J2$lgCgp7(?mf(K}8aX(f+6*8gFY|cdp(NC+K}$=8`V7V>_PKf1a)Z zVxAzVNCGk1-!#Oil$5$x^&Zt&@9Q#`bfF#l=Awh)y2i+quzJ@t;q?<#B!L+1^9`|m zXsLS5wEMiKWG?AKJB~50ABt`mp4PHY;UI#FBoL$hE1TFGRqsW;oIcf~m${@1?KoDB zZ5O7`INLw37DP~y1Y)$0H$?q?4eHg=Yp;`Di!zsVp&iHhGrxDxH9$Nh2r805jP~<} z=-s<DsE7)$I;( z8TLBwsn?*_Pe?@)h(Xxvyq?}ls_1ja3wpb95_F-xL!TxY_O=V6svxMyyok|mZ@U}x z)}E}-X!G^{A#+I=+V>7?onh~TiF%Kc1QkgjM!UTacG7$81NzK-&nHbx4N1_28oe5H z%CL_mhxC!5rY7$76I5hg#AvsVCBya6=Zc<_;OHZBNf+AN_wJEl9}|b`Bb+{71qdpV zK#X?#m{?37ML*Vat3LX8B6CR>+BfX%n_(Z@odgw0AV#}=Y%i$K2KxMA;cOstNf+90 z)ft#!pQ8%tGmbugScUxr6-gjQyM2zz(C0zD-C9lb(Lv^tF0_Y_8}>;w%avuU9eCu zFPTfa(2k>M;94z35>zCCnC*6rB6@ve&y~5P3+*`02d+6&BtbX!r+Ng!ssJr3M4z%1ZPovY1~ zgut7I7_*(vJHs1H@EK_hg_n9FyQ$gY5wVyKlblYaDm`nOBw{*S;HozLYg&5lgL^hJOYqFJ!E2gfH2ET zDo!G}S80XfyiNbD?}cm0yZ5UVjl(h!uNj?3HC#}{)L^?^g9K_&aT26$qG+Bt?|1jc znd4Q1{dv^!E+@@9mPjKL+k*+I$R5htHgW6TIPW?AA6!Wn3ajK^@0c3Q3xe&zgj7IF zg0W5f{X{jZ_#chURx))#xLUvAZBv6WGEI8vepMSn>P`}OpU`+QZbesN6H@})n?I5j-Cm1|ySLr{@< z5fAH?Pqpm{qGzSu3CT59Wlml?D1@L3H3~M(uSQ&-po!J(5BWMwTAI1B_z*)-k$DlH zXpvvFKMSHy$-$mUg(5SnUmO!c(1jY`ZqKipwF7bfn?dTtZlIK3 z9|6&7U!8>T&TnQczcMa_pbIsUS`<*8M?q{&UY4=3Yw5&yN{=-J6`2?D`;i6Jj`kBZ z(dNf$jZSv*B$gXCI)tDLHF~!ysOoM7@$KCLs#BHCiQU@{Fa#Bu7xAh5g;az1DViA2 zzI>x@Pwr2wR_5gpf-cl(_Glq>FfvsWe`MrSTfWTejrhslN~p-Zh)4fhNL4t8EpdH~ z!io2FFYA4%(37TyB;(Tl;;ps(7P&+S@Mo9GMp}+r!iL+{$cFt4*9WH@&`@ z!+A#KQd>6c^fl8i#FmaxV@b2zYRnUxb+0<^i^=G5v%Yn?K%D6n6(@1Jcpg>wXMA;` z=Q|%|T;EpTx>Cm`s7M06;@%aWR&Hd~jQor0Tl;$j6VioV?Kpg&O4{+Eu7M+a-{$qL z&0p9A6(^BZ1H^|(t*qYPl=h-0zs$^~8te|!YwYoiA5Mj-Ew%sgVOGob@U(xw&y)E4 zly=tBj}`Dr#Yx;hI!r~iyrz3qVZ+ZE)54Ri)U{X5l`s`aAg=Oan7Z`y6-^}fy^-*O zHPEW?c$exuO>WlPopup z$66~Ul@1~3LXElq+|76>2JeU4^6Hd~<3*FL;$Pl0SKCx%Uc?{#eK+G!GTwXMsm1Js z)p^EQHFvHu*Upll3pExM3sW1P$J^f0n-}zyKAvpl?)HTtsK~sCD{Ep}e-JO0y3hAZ zsiD@bPV+KlF6l!2?Q>zO%wW7rukp$82}j$#YF#_IAcUX`nS9;D)QJ<{Y2sMt?in!` zx?4%Tt{H-g%!~NgfiN}Ub-Z^l*Tvom|BLQs9sA{<5P~k$DEm{GDtH6maO?AI+l;T8 zw6JcRDsT2dDl#u(?vLSVEAlOG@K)O=t0xW)^17ym zBj}Hrz^YN8o*}5nyoj$iy;o(-!FMw@v^%qTPHY3~g_EHK zU8pf~UT!t`_i#FMNSrnNK>eU2gUpLH7))RW4ic`T$+c9)13oh^L40F^Jx^r2y#`5Ogj7gN z0x^lKxoDpSb%Sc49x|~#Kohagb&Wv=i25>w^kwkYH~~WQ_v^wgwQS(Ob3$6H;*_f-J<2p`e0rVY`1HWkP2!@ zg0WqreDiAF>*pGqBix(;g;l9Db`8eJG=E7Ub!2&@J~A9VFg+vISH=5uToXf3K@Ad! zcT6j!rvC|Ie(4Vrww%lBt$p&j5P~i<@-GXj4L?U|qE)v~GWre6?fq_EXZyJ8S7ctq z%a<2aJB#8kYx4iPE1}8*s}hGU9TY;)g&M_o6;K&4guF#)S}cp@#Kke)a8Q5Yt~-mT|jV>CDv9V+}z?<~8xfe5zm> z5P$wqtPe#*_OqA1d>52tgNW9Nlxjy1zb%1*`I@MO*S(hkmlR5-Kt;;>AVpS3O%}yE-3N zII(NDvR1u9PnsH%pbIsE_m45Ns#vOrz3ozwc@eYS-aq>FukXG6L7b@(-KeNKm{G#) zgN#w*{ar=Wzp15kudaX5Xmj`W^}XMJZWC0HR}%j`TST3E27gWU*5=PPzxiZ+?_|p+ zs7L}a_pb1?eO(^hyt+|+Z-+gh1YPJ=o0Emrf(m7I4V(w|SE=vKf6^wX$h;s(+as^i zHqM*opUIqlx0H%GaK{|s7=swQw}e`kin$LC(UPEo8YB?2-6opoJ5e9$J5n;2bRka2 zQ(Rs9ALk|65p*FFW4p%kCH1`pdfPRq$h?TTZVVBY9YGgrFb+?An`|v@ zW*MtrE2V~A*>0BAf@%>ey5l}`#zFfC-5#EH_DG(@=y%$Af2mu*LM|%ki6oNVjZpPH z_y*6{n}5zYRy4_5X2#V_5L6_AxZ$!0b!{oWm-@oDe@hcXL4qK^HRhT@j(0E(NizVp7I|Ke~Gh^}S{YDl#wP<_Qrh>Glmx z{IBJU2_-9b^Zt15pAdpB)X4i#gu3|lP4uc-+l=QQZQ=dqba}H!QIUBObAPnA_C9T% z^j57L8A8y78jNis`qU%dCb=In+b(BC$jcbScUOw3uP1%1d$soHviQwwAMrl@uuV{r z1mc6Yi>diPgE;qD;msrB9`?Squdb;f3A#|@)@#Mp$OE`GD72=W=gA`tyfYrCX9y}X zFXFK~i>qcG@FwWq>hR{y#T$6PyB12&g&NVXmQZ!m@hzc8PW~9b?5j9$>n8P04JtA( z;=z|os8egwHPPeAV!lx=Ns=svGP_@5qVaaqfgQ|mDI3-E6mY{?Rt;8T+@^1>DL>zdBwhyN(Ff(QT2yP z>iwbk%gjnQYWt>4X_?sghf!waR3w2odTwPkV;$DVp8I^Bm7lgv?EP*iK^I28esN`$ zxC_Lk2e$f7rmsp&T{Otlpd#}kZrmYO9Y4HU6Q93RG2w^gb%|Zozz~8i)ELn#R!u61 zbw2HUm5ijk|0d2V+szPEWM0I%?yaIuoW>W9dY>ATFz?Y^-rxRwA%vg{HRhD6q8dDn zx0rAEctXa;R+YVvHEU`JDl#wPCG)GOqQBv5P@^ZTN?6;ty0`XiZwNsbYJC4$6_w}h zjhcw9zBVI#ZgcOntC5DFBJ(2dSGKA;e-u~3({^l4czSv(@8RL4LkPN1qu>KoRsHq& z0@&j%cV^VP*xNf{%ZW^>$h?T(A5m3Z{CbNf4&;76;YvhbZ=o^Yh7fcilW$yAHR+8n zHBqDZ!i-ikU-S07_NpPM$h?UAuBxhL?7`n5Vyh5Ib6*M}=t2#~hDh0;mT3L_ zQw_6!>^K>zI=;Qx>>rIEtEigS-Dvg?wuh(Hz7*~Gr&*%4uE_&tSy4exB+UPC49$DRJMA5_n0B5NCNT3wH4K*$MAKoFME#m`Y1JG1NM(6FWmMN8g?_Y#-UJxF4U-{qExl{Ao@Og z#y9AmS(z`M9csowMdn3(`iE%M;wp{|`5sR3e75)9%xm6ZAp~8hvGG{6+V}^Eek;fM zYWjAlh#mGXKU8F1#5^k5>wN2!ZR+iVp#)v1!8kmv$bv`XFMdDH%5%MlnE`CA9I0y7 z{o1T^#J!t+ZPt0==two^!XDkLq`$({$e+hqKX!h7GjdTuPe>rXqUjm?c4^}7q-zOn zpG~&vKJbf~$w-1Ovvlk0?Je~yO{}ROrV98*Tiu5~Y_{E6ensX*T)kIN^cZgL{ePjqK zl0f|FYr5VMdu(tW->$^ES!<*g8cF4W#KHc~~@!VGzS?MzSU-QBE{^KTl0ip-0+ z`l?8^^2cMEi0Lrix2Snn>)?&sAp~8hk$(Zo>4>tPssoyg%MfWlU6`2=tt*%jOYpvfk zQKZbT8GpaLH*@y9mqQ4;P-EK6D0OK+h~sK^`B_cO z8ZsoKtM~QHg7b!l5Okr&@h;J-;@cp$9?VR5^xF!V7mklH1QnSV@#5Pts>wL~5{L8P zuwoIJ$Lo#_A?QL4#x{{TeW>;B3$x8WSR<^8D%j#Lv)?gBjUAU`Rm}JJ4YJUVg7I^Q z4z)_=nPUhl$V&q8NKF(z0OE}~#Wq(v(aWmT<+l)mF4Va7W2|~;)KyJvC_6R%=Uu(5 z{2Oe7ip-0+?y*>Pdkl!SU(I@CbE~JVI`vC=WiIJLyz7ft)u}ap86B3mWAljFt*vgg zB0>ndkg2{EtEPEDd@wFOepi*b@c4xM()6Wo8WM0Ip|IoAWIu|u@DR!uD-2CmC$<<#8A?QMl z-oI5=<)T2Gi^-pGCgP#YRu9_uMXAWVh=aGptzoq?&o&Ds=t2#~_O|=Z-(#(aX<_Cl z+OcpIHR=9;LbhGD2NTFe1wG-mi+09_Na??JnyS&;zK*)1_qDDU^O$>mz2B^)`t7j) zYLe~YY2!Yfq1tW8ojEz){`(Rts3D14;g!|6p7=Y<3TMuHHY{wN`S51@s)&jt5T7np zSq*Iu;um$+cl__gnS)l^e~Tjtx-gDAQ}vbC#(OpKbmwXbr}}@I`AOVhODZxi;-()| zR;}lNsQ6^?Om*VVe*^H!$JC0%GQcPduhlNZGO2`_j`luEMx_nUp^j*27@-#HztrXRbj_o!Mu zhWToLG{EZnRJy4l3A#|D?yXogufiQoe3!T{zIoa}>-*Tx4M9cbMSN2ehr&U;uyW7l zLx)CMTLyFsA?QMlnfa=yzsugz#OW&kdt`3jQC8fs&W4~O^CDiXiJ1`~&d+O;p10#T zYu3$o%?w!*bfE_KP5(LSxpCI3kJtninHMqJ^|^NU9W&EMd&FAwYJAtY&FveE^j2>4vKa>z(2}TBHcHjlk7YHff1UWf%?Em0 z{IlK=R3w2oZ>=cR`(do}>VMtmtEdKe*B#sxLePbg&#N7!PB#Uy=&uo;?=L;)t$o&> zQ&W+75y!q9r92yRX`*7W=!~()I(Uy%&2P3{NzjEFpAL#rUw#kb$sg}ai0e|{``2>& zFP*5!yomc{M5%vnV~_fyN}Y@o&p+fsD3Cm*&dq;TM z8-j|=i?~8mw7QTFN1xt@OK0?XJD>L{Z^saVF4Tyx6|GKH2T^o;zJ$-q9Z6jAT^~bG zk$Dl1m=Ucy{{-T-Gvn3mxoZ>O>T2I9k_26-F|BBfsxTJE-S<~6_Vt=LH!)X3`%0LK z%!}9;9;4nG3L>pnfrNzTZ#Al(&%R2P1YIC_maw<>$5XC0imIJ#Nd-NjE4Wu}@7?10 z;MsBBZzA3^dla`_xMt>wQp1}ak zV5Pi~s@vaoUb;ZD-L64GDySz3#x~LVf8(s4J32Ljt8RfvHSqFnv)3{&GO<0FkP2u? zFt&-eKAM*KO%;0<-dV5Pc|+~LXIQ>6N>x0b-~6pJ+r!h2)Op*J-07{%-UWtxQG*I< zNaE9vqg00<@Ha(Y<}K-)cj328pRbo8s7L~F)4Wk?-rM;!@z9U;J?X=K&s_anC_xu$ zU%V$uwV4d!_Le5TCSk>_8O@(DHK@qEh^Gv!sA`?V-z}9Y`HrV(f#O!Zx7vmfbfHGB zmn*8Ghe4DbI@gyttB&<}&xVGeBJ(0%^GBp=a}mGxz53P(PxXBDtg<&7gb;M0My=l> zRjmUc7InJpd*;`tt;0zr4M9cbMVxzlqW_S$>SmQ84t*4BFEk76MNU8r$o_Lv@DxtScj9HM>W6zm2)dAIV4X;{We;$`_d?@$)fg#pd#}k4&HW)WsI`YK5QLA(1jX|?QQqB9v`Xe)yJ8uyGu7?)H2K7 zcDYr8V0$nj74$?Bj17^}u4c0L0nasak8DT5YU=Mg_I}P7naW(Ls`jkKIcnsX=!|k< z$=;PK?ihj!YLGze`?IRL`~!#@UsOoQJ$Zolqm$`oT zyv0k}TOt*i7jbV*)W`)Q{@~B)9fm#NonI!=)Q|*SAgqB^)CWoTYohqIg`Vl>YIu9h z3MJ@5ufG0&jhzXYjph5tM_IBZ;U|?PTegImY;(?eC5B{|BucVX%9iZQF*9Z&Np>pP zN|JSkFy}dMLbj}B35lc-E%s9UpYQXW@63JXIIh3z_v`vz&wYRH@B5y2yWjVH&Y4#$ zs`C9nYILi5W!clYOJbdY4RV2qn zbB*HO4ua$1gp6QGBtdP7VsBOP?SE~A3*t(>a;kp%5#vFP@pYeEI_@$FHw* zyo#^zE0$mc9weZTE-SA}E(P&>moxS6>#)H0>PG{Nha{KDW(h`QUFfs-Ra7fSf_UW6?6q}1&f!c7+P7qqU>1lx>nf`2uj5&tug~20J3HUu zoN5zEFbhl7Hds+jm;fTC$f}I{VyZaZdfK;SjL5pswHrRHT+eAu6#co5Cs?_fv$mht zthpqZ1&_(i9#(bV0`bj^Mu7?^+c?$6-)jg)WL;>r^kMZ~f%BSpE%7DK{6SAUOE=ve zK`;v*ozfpx^WFflWJAxuvORsBayiq2G9v3jhwoPpEa>M9TeU2LU=}J-+x_adEhC+p z<8m7!u~Zo~>i3;yzoG`=saQt+{N--FR0kHfjeq!^kGgLWRb>tY| z$;E?>2P3ks(Pec{^f}(j><%VpR4g^Zm#@^2NFtO4k8YL9s-Mz8ES-@nKHt56dh>bg zy&5C3E;P@m_K14j`Q4lEiAaK3@SwIt_b)p-6Fyl1PHu5dH@{_H*uOy21y75lKLotERW$3w(bbjbIikQCp9pQ%5+H_f<3= zjL5ps7fO~*BbWsbYD;XnG1^)Gfc?%GBeE_u&qTK;m<11NyB>3gedA4C zJIX=i+uzHo+N16FCa5v~rnjt`Sy12D&&zXjQ2ohUcY2$rTY?cR2?^-Dp0a9jArP%6 zj9=elYpU<*{ez5$B$x${5|bZNi&B&(eqK2wKH>FozK`^~A{mi&p@+ZoklHvEM3)Vd zG7k1Ue=5Wi7#otPAb_Sw<}_1LDcLB{RP6 zSIRk2#Llgi1he2#d3hOCc*6!wlzwPi{o>O~JI_>WYLN57?GxA=d)>hJ#1`BOtMg1RK2k4~we-uN5yujV!BmQkwAvu@M4yk$d)jzNf>|K8 z=w7sbS`h28E`P?A9tGW)m)l1W%)(L?`>ed`y$Hn3AOEYr<@~;2@kXy0f)QC4`jI37;ONOpp>C4AGac)jIDxf^t?RqMv8HqV^M zXrw);FK*_wa8tqh1=fxAH7hyTcu0a- zAg0EqD$h@NtN-NAMfKYr9P6u4B9dShmg>H^RJGw0h!OictRH;yFJIEEuEv8ASr>Y? zKULKpj81>**`l74B`*6Gxi3Z#%!0>X$*JmrMIgE@-I9?JEbT1H+td(@$hsij`!rQO z)d#bR#}25eO0}-!G@abQ5R6Cydf4XR25P!7O+z`zcj*`Ub?W zy-Nl%etq8A?%WK@h^z}e?qaICkoKo0J|BCJ=gNt$&aP8`Mi9(Gr3%+lRqxh#8~%3P zPXZsjHo$qV#zI3dBI`moDEgLKq^@WJUB=;wZ#a)H{V;-H7CfjeF<{sTr$?j8hB)t#Qd?ZQL>XBiN!B$@+sh!7y8Svn^^&lZ5 zpd~?V2!Fn%uRB-k27MqHkp%SDbCT4yG|WTBxg;9FEL5Vl9$n@RcPb6_7!O8dUFfEh zlT`H|(OJSH8o?}hP+O0JH%B`iCaj2;5m^_SXCmtnjbIikQCp&F>7(Ax$)n8nXfkuG zy3~5P*&g>79;-IjUtzWf$HNKKVgySf32M6@^BGnWbU`v?Qo4@lm~U?z!5HjYreRQ`OO{Yt4R@cp*it{yE+3R~!!~;LZp>(0o`a)iduMZWoUauBbWtmYU}afKS{x7Iu14-jL5ps1xlx==PxY} zc|;?a1rKWLv8c~t)uHrg;b2?9a_Ywo#Gp~MP2u83ZB%sG@Vr~Txug#gT{;%gcyN|TI!>orSm<5kK zT~k$$H}S2M7W<0SJ5jl_TXdc!7?E|M|9&x5)ffoENuRv-Ox>pLfU%{Gha{K<5AHqo zvr2rdsr$t^OE4nqLUY^_t;>&czioNNi^v`GQq>pVeI4;xaXg%WJ0qa^Sz(;o5_=jw z>H8?haC3Z&{ykMyKf2j?kOsl=a6(4p5=w7Ny!PKn=ljAjL1gtmld3*=Zlm#FT@V}( zCu9V)B&aQsTJMIpRIX7jvbuaSRyFGUneph^Ek(^eu*4ka9M?}KRt|`FTgLbbTp#Je zgAve@c(6x`O8a4n-sVTXyScvA-*bI$<$S{sj7S2yS};X@@&&$2HhlPf@gKIH@9Vr{ zKm@@oc<1{vMcugx#CcUA-^ihnjsjGb)jGUK=0F4aE&U}ude5TF^*I7hsqHIv*6Kd zZmQau1Y$`uCy?$w@8sHS?*|!?b)m2BNL6{-;hD^@z8apHE4nyS&R+}4S~3gcZ|q7{ zrcvcAv)HzNAUdKw`nm!Hl4N`oV~-3w>^^_E-m^&)Y3M)3;0x?uviYcu0a- z@YtAtteV&uNA2QT%>qxq`A+c2or4U)h^!0kFEUp3st#gQwZ}a(8h@#-eQ)n3B*83r z@Qh)%;L4ZRsPWe=!HBF2&2dYVzcJdayz5FlA_W>Hsiv#!Gl18gNK*3?zc%M9j)xO) zX9Tn)s4da+=i%;q>9x(C$cQAMe`}+k8YJOaINGsj1he2xZHZAc`nUt8t}`Bt$hy!^ z^i5Ljzriz0ctj(Z1rKUVe1GU!x7|H=nJvhOtP9OO|Mmp4;6ZJP7t&*bPhJ~ou7j^E z9jhjIelgdNkM}35YNha_)Cw# zv(48AOYiAp2u5UG=*vZuRi%T+wa0+HD?B4AYzn^D>$M1iS@0;BCt1aH2NBn2VW8;t z-0rHW9Sy;VtPA4Fqe-ew08b5`U06;%_*p*JbMYBNFd_-)k2fc&l!ZTNV%V$wJ+G}P ztAAg#aRk9Ec(m3}qYkgf(~mEndLc0Gvzl&~CUp(Lh^!0!#eyXD(5xexC{S^y8j_^;?Zxw}gExi*3jME(J3RMwpNz=5&^=z)PX`C%8QJCU%X^31+sAEN zA|rxe7Annem!!H~#M8mNMGAQ@C-!lhJhaXbjL5ps2RbIHjMjKMc=GNNYV>!*-H!^y zMG(va!LyP*q7p8SaBoa6>5~yG3A4hN3TKAZ&kk^t|6XW3W^PJVS2M1fV~`re*(Is! zyV#3*skZKk_bi{>-(B2su^||N9|`Coi&NG1Q}{;JBVVq}*f;ihcX5jtvppoiEO@M( znW}b`!4u(?F>~T)-_yyBeKV&a7?E|Mb4^cGWl!R%b9$}$89y&*>L&eHDuQ4ZJo1l8 zRjr=*Lle!)Jssb*VS>Bo;zNdDMAn7AIx1C7Dh*ngT%z{UWgj5w* z7sQs1SL;7F{!}piyY7ZyMAn5qd}pd!P!aDYD*rJsBdO2H;JN4Qy|W~k1&@I_Qq`j3 zAj>6l@`^ahaxv*2;8Yl_-46+}!f zfBlckUR0xNj56!Nh^!0EbH3f%%G=f>EL~V(b zq)~3{^GB78$hy#tbWeS>^?&$u^}lEYvrviJ5_f8k$%#k3G9v3j^O(Oq!7Nmww#19o zGgR7n+r#ykKUS#*16osun$C@ zd7T0~tG^R0P|)_Gl3*4*`hJ|E3I;IizQt?vJnvMR7R+^MperM?F7$INQ&gU5AUf6j zJdl6;AHh?lUy2}@g-Z9_pQ@hSiCOn0r%d$3&A1kvy21ACjL5psmCL58H6MX^e@W56 z!JVbt+FR^9NJ%gY9@|=^s=_IlxqLQpP5h-R54nH%63kLDBI`ouYOC+OdV=UTa$m-# zPwKh38rpY|l3*4*!jF$4JLYX#|e=GQsbI()u9i$|f z1!Cn_N$S`Le1j+dC&M%5=(m=q{)i-)g{9iLF-c7v2O?jK9`OhJH*l`(tz_1m5m^`d z=j%yowAmh4|NJlGLJ`ODbgB|TFbf`SZYHUjA7Oj+>~|tQXTt}agX5ku1S7I8^yr4k zs?%ebNxr{m?!b(EC7e4Sv(FhM!7O-8)^D(uw*>Lmq)YJyd!P3e+hzA%Mr2*+rc;vD zoK)=X)n{(aSdiz8ulZm0>4zkk1&?AglhtoCK-4)Dh<`Hwr@sDs`rExE6p?kI@AxfQ z4IPf7HrS(c#`8B8`bKrIPjOgFX2Ije`DArs42T~mwv8_`xtVY5pTmp?Ba(oAGhwVM zRS2`y+pPOq-TW|@@5>n@A_!)|gXa~y?>0R$&fBT?XhSd}>q2wf5(QF6I{p4GXr9B> z@+YZ1uih~I7580ua6Ful5!96gwITfb({HM(H%FSQ+TKT#)S?16jR)(Z6349v2^oQh zB&aQMhkvAde#70Sv%LGUB=zrdd(^TnDseoVkP&!Dg4z;u=S@^Uz8HEIK6k8oW!2s0 z%upbtQK@;IvFh|-{C0H7`*W;4``JXbBSbI)4-(LIHSxV6K6$Blz0uQK2mhKk%#pQZ z7RKG($!fw5eBU7P=@ILldToMzW?6y}NkAXnm8@oL19A7j_v-gOx+FNaLVx2S31-3L zkJpmb(UsT}YxneJ{P^$Z!5x3~GXx{DF7%>TlhvNZAP!D%9{=H}zkEAG$f<#vloLkJeFWY)`f0KZsTzy%m40UrG0mBTpI+NiYi@~IqTFsNx?|VVCe0E(%4c}0=?8mPdf)QC4I{f%p*l(zNu3gUv zf?4pOw#Uc9Gx1Kx!;hHfB^fGJW%S8oyQPV#D*s)1P4~)i{S2V%_wkSaTHiT+zPkBz z8NrhD5JcYbscLp5Y>(uaV8)s*<(vz}>>Udul7MdAAyu_|6X*8YGw%0npIXNGrc)E+ zAqi$-J@U6oRl$WIS{|Gd_;1H`-`>M77=jU57kbnF6tz2uy*=k^D?DBEUh<_}e=&k! z7Cd%rOHn`l0^;<5PXfb#o#D&h+rD>ZMAn7g+8{+$uZeTX!AsA3emOPC_g3PY#zPX! zg2%6QQdDyX#N=(S28M0^#{1n0d;h_RtP9OEk==r6H8*&_JQ7JT3m(*#xT}-?Censq z6>4!t6ZGR{-cr}w{b!z$aXg$rEk?2vqz&O;K7Evv=c@yz(`Q5y&;*@BG7y3jld ztVcA0S@58?#E6PJyjuhI{-Z&UPO4h^5_7Gk2GML|XVq@U=lag8P=n%uC+F<--tn;| z7{QW|FnU*K)!_#aZ4TV)Nspc3Yc<*4e}oaCEO?A6@`B39hqp_g^)4E?`Sfhx&7ziI zMAn7ARPqJYqdJJ)W$*Qrc=wWT!YTU}KoZP?M}wwalzI|xGdfmZobkrqE51%oT7nT- z7kWdBE^28n5aSPh8vp*o<(yTG?Ar`UFbf_9JG`h8pX77uq@;4rvcK&c6GmiRXpY-$ z-oIUWw?p$r=6m$XL!L3UFzP<`=hRJdyKi@Iw6;UDRSk63|7KKO6dVSFbGPSv2TUaPe=E1hY`7@A_xe$~@Q} zhufD4)EqQB*tn4W%>hPaUFg9#I)?U=j6ic`)ZUs?1gRSR|?wods_YX z-AOa+o*GN#PJUYb{1;{du3b_!aPOi~ZXmeF5R71{NIYg3; zX9U44cr^X6z3Q=NlP2EI_%Y-6`d!_3bdMz?vMzLo`R!G&DIjXxbt1mqyG`8l&y+Fy z6>G^XRN@|LKizuOnz*lhX9-3m0nKqs+$`V3?eaqz^Gzf~maVkEY=k8#`A!b=O9&jl zYhJE8f5lhTOZDY5Wy~CUM(9D}(g*F;?ALHLcxb};jI9m2y7?Mj^kI1!kpwh8x0Ubz zt=D|gJ@KAAg>QM zfX2CN;${%jU&`fqa^j`n6TiG@JS4#^c#KHypjOsFXPK1i!;GKjUI`w0#uALky3jal zH>(2TkB)QV=X_V*J$LL0;~@!V!Gn9K{dAwIQo&7|VF^ZLU1*M5V*7LDotg=aOizs~ z3yu$by0}JRNqCLIxc;_mo6q7iZj^KWd9RV_GNKV3^_8<=J*-Dd{f2wa>sNf6yW8%S z5$OSq>#qHDr;I7<`R>L=-<<{Qy_zJLg{4Zo_XTzO5Vps%@5%<=TRqday1ngp8Ig6N z8|ePQ?j?Dz|IhR3^V5874o4Eqg2#ccI;;KzuwPvn{9s`42RpqdR@!}+5n0#h&^~QH z-36z=_Ez0~3nG-o>j(B@L-^zLv9YHAHq(oGy1ZnrAE-+WVu$A?HD@i3+MMGm>tDVf zN}Y zK+|Rc9uEIP<`%_~8?qI43)lF?*sUm<12+q4v|AUb%_0;-DoMk#(Ut zZnxmRhnqMjzAs~*Eg^zl4Ot0#NpK~jhwb4i<~<)@Zc-EHjlxg(L}P!Q)P^-mfm9FR2oIByeW2 z{@VlJ?^ZG*>q2uswx8~#&7+(nFK&$>n1xEzb_?noBftKi*&co0?x{M?&2P?EjsAGu zXb>E?9=MkRAtP84Nl=rpf1L|is5Sqny~kl)R>F8VAtR)*gdB&CT#qYfdW3nPE(ng( zBWpbvp$7>rEeZQqt&jzxK}GvKku*Ip9!|&zX?l<*ku{$hpQ7}DMkS7C64)N*sAYs6 zB)GJO@FN@R&RVzH=Bz}IaDq#PEJOyW!SQecwHU#YNP^lD6Ym`DY?+p3 z&OVGt!sw?G)fe@)gw`V}5z2!1J(BPjy?3;8spB$pb!J4?g>InNY5AYuggl}V%z_8C zSr7m2)`OjE^^(n%lMz`LntP~S^JoOK;6ZJP?M+g>$NG&n=iSE^`7_T~>a<_wFZ;RK z9vlxRP>T^Pi6p2kQ7(U5-^hu>To8;%0(wowG3xMNxTD8@6^&pPDp6ZvSo$j8;9uzicU_GJ{%z_8C zC8m8)+8I>Qz71zY)`jlAX^hJ2=l6=D5zK-Iwe=WX+v|Ma*UqPAMAn5)IX_0#Xp1LB zSdVA~v*1B(i8IaGJ0&I;G~1jJSr_{LdlJ>UDVTu`k7xw5;6ZJP_XqWIuC709dLKq) zU1**$Zci`^9@K{L>jl&Qw|&W<(}rgrAKddnaNK%mLjSXU2_sk%Nl=r>x}Jk@Lf3QB z^uTyHAtUg}PRLFs>v}%qyg$qXby11q^f2pTt_F64yQ$;WWuv(OhdUY)e$D zOW1d*bpna1(V9Dq2gmiVEwqdA418gPQ*KFBbB1FC9+FtKK2eqY6~ErvbI;|BZ7tq# z>ec+n5R6Cydd0j%b;XDK+CJ~}^Nc#x-|71Lq6mUn<}>^tQQbKM#D#=c0;@`Pb1s!X zXNW&T5m^_y_OL|NZw}@&%sRBhb9r(Xr**ZzBM4@}<4C_m^+O@ddMR~eaiGda&78(v z?CeiQWL@Zw8|!|>#ohMtAwPJU>L&)BR+cy0LlVq_$Aa34YD+!5kGiAS(ZEAfD>)C0 zYhVaQWL@a}cP6T5CZK!$yTad|sr5@awePdV<#Gs}B1}3HcZ4GX8sjnFj!}sZ;y38t z-2P&qPQ#7*hvi-~1S67w{_OA=HR5>?wa&Ko6iUwT6rE$g8!icE!Q+P#iE7e+m>2%? zg|`EnrrhI9uG`LdFe2+hZ!Vjts=R@@=Otbo>v^h1tg~Tj?FfQd@Tl-iqAI=|b09zb zVoYE}OkHQ!pEV7^h^!0U_xVJ%(TiWE*tlhg$3LR2GxgKL5d^c~(K;njZM*B7CPt;U z3M}5y-r2jkfFT%>b)n0>ov7MR1yS_p1W)eNmz}?g90d<6EFe2+hPv4NJWf5F zildw-mR&X-G9$7sbTdu#P5{wvh&Mjv+Gyv%*#RC|OJ-r5XK=H(`-ff`?IbLE+YpRM z0-EEN;Qb(;e-?kJor-(OKGUb>__`*cr)s#)nmb4t!Lv{j)R}}I2(HeEZ2r3WEj>7H zk6KOWe|{GpjIb`2klxUd>j9$1p5M&7B5GNQ9$D)lJxIu?3NxJr$^TIU_^Rwso+N<>#wqdz|jeUGzgA|6Ebou zLV9E!A2=IG4`@{4I6cgInC-y`JxF9yB|&Y8Ygb-#>!}T9J=&EXmU;g{8Xg=ECuBq};VqXcb3GR9$ns!a5FC$O4@Tf2 z32IB6%hShsykEdPznWU%O*OQv{cbpEcyK(NkP&!Dg4z&z9qciPtU|K}W_qwLDsen) zJxuSz2xv)ATaO(1`nVZA?0RsIlYa2vxb+|*BiRXhTO$9tM(+73<;*#;+rfm)WAK3^ zkE_#fpEc)Qj)xOCIvK$+C<$syggz_dv9#xtnI5E3iQ}0BAak5E0uM=0TjH6UPdJY+ zFK>E=vqc-LE`9$p*ALS0;CMJ8Bk+&}wI%Y!k9Kk`o@4%c#DqCb)zrW3Uw3Rcsi{g^ zyWf0P9M^wya@7a%d8&+d_7?uwyrX9X9+D{bZd3Kv#r^ucd*quBG7kSZ#3^#DubDZ( zh$Ntg_Gqd+jd890wS5gw{kS2{xbedx2xejZD!tfLRon<-_4ic+qbKxs1`gb62u5UG z=$56Ls)W(_259Y7<2>tzzT&j}*8Y71NiYi@n+xktp*HSudOnvDX!iay&XqCw%@$-t z)`dR4zKJ@WfZx)4==C+8Z(Fu?;*S)LAeaS@bqkxQNo9V~#MKQe1J15`&hK~DFa#sA zuJMa+qC6w;_wMpNy3KR#!C2>!Z)!&nEkjvmUwE*ID*Gh9eKz!yt%2tb7jp&;YGsJC zp@^&tz51gB)#)bgSn~GW;2E&Hpi`rO{dG!7Fbf{@M<%FNv+zy59XYoIVupV2%e%Cf z@nA&Og^oL|_vsaQi~MV+B#&>}4quBSuSO8ef`{7GSY=!V@zgI90~yB?eccBRHUuNG zE_C?u(dUO@zSFgaL=em}pH=8cv&Tp9`iI`Np3$yxuiQxWE@^uo719{5l~*s-B6Jx8 ze*7oUcA4uPxWUfxVgyS<0=n2^jn(8|Ky(^>%yYTUDBm+D?XNUTf?4qBb)vDV(QUaV zhW4ziJm;qQzL{X>sxcz#LhpDmL2W68C&JYim+{Wre$4ksLQk_El3;$NGxq=S)j0oxABS=`#c)vMzMat}c2xh@!(J6gI6~`RN z^&2k)W*lnayqm-RwPr?SUFhOBny4kM(eHNK@~`K&R^6Q5*UkoIEt!S!Ld~11#ZB>> zXDhZW3Y6^J%Sk(X*bt0J0@~A|sVW$UU!l!4WQAw6{*BaE-*_j2U=}>ij%%uxBkMkjky{a< zNo1`@SAC9>9?)1?j%N~?>%j;;NYL97hwmKio*b5DdWPK#6V&8T&+zB$1a)EXH^zhG z`ft8extj5W{)^KIIX9V}ff0B}BG0S@)vdxedV9QD<4pV;`3Adv>ZJIvyo^W!x{bc# zd{78``?|k+1b%Atrdu&_asq3vuouKO8 z#JQyAOKzb5wubK4IVwgF%z}sSTw~R=Du{&zzVIaduaY|~=5a$XBI`moUD{Z+e-FP; z-J$cAzNR{XSqWJzvo{#cnsBcf&*`JKay3jld?A|_c{tQ*R zx&3Q5l3*4*s4a15%uJQPne7=6nW}#ag>|7p+|)~zvGFIpRHafUdb;cHZL5>EXJAAU zT*99|0+GIOXkco?LBTx(?cZsT1lAl`@YwL6-sT6;y?P68@Eo{2IoSGP``6_ek#(W5 zZH~PH;=nU&0!3?mA1twqBnKlGv@7?A`t_ObGB zVJ640r#A;Kw|~H0*!`&pf?4p`e7>>TaQp{NoNc?xvwU17x4?%D48e%33you?eQ^+d zhO7@%`1J{Q(aQ1>1he3g!<(Qs{*K>B89(q{Ps-Rd(-7=jU57y76^GZc&iQ8@pM zK=U>)x${c?5tOxL7RFNsC8*V@_>GjGmpAp4+}qO~*y#sDFd_+PoV(8bgWpIwbWi(0 z#U*dLzkK&@1i>tLG|*?!<@@l9CvBVVjlU2-*zL6=*$|A#y3jZWPkal6zroRr)jy7M zyLER>2OtS%!GmXTySG1_f3#ci*a|~1BI`nP+z@_TZqNZBg1hQl9UmOG9-7eqY|p?5 zXdYcyDryp0Jp=FDr5^~6hZ8b#D*{Vvi4MC*Ioe+Y?DJA_-`YTjFW$p+3zOl(l3Qy_bSo?XmlwkcT7~kpwiytw+D#H~PjTy=FWZky-HAf8ky4 zCwcD;c}Rj0NkDVldQ@JV%hzW2NaMkX%!0@J&rb3l97qpIFd_+PjvEiZCW7=}L}tOG zYj9H5dV~>-NCKMU)cB$Sim)>hoUK ze&v@0Ba(pTxb@I`qRV}k5t#*#@=M>#IzIf8U_=to9Je0&h;n%hG9t6!@!EIqnWH(9 zU_=to9Jho%Gq^mLFe0s-`B!N#C5lKLE+TW?>xr!l=vt z#77d0NCKMUmRPILhYj_4QPz@K7{@-nq~O(%ha?!01T@F3N4h=+-_YlGSxaVN9LHv* zS(ic{l3+v<&>XiOoAtTmDSe%iwPY5?agN#i)y0s9Bp8tdG{-YNeDq*MX2ApJ%B=O! z9+?Cql7QxT=4a*OXT^xjf(Op|S=&Q@R+$7Nl7Qy8_0U_;;WlSPX2Ap3uB`n^5{yU! zn&Z|(@4F87T}EUUJaBc+IzA-9h$NsnZawr7rGE**9)pa?EO@XdiX<431T@D%*e4(8 z`w)@mEA}NA=UF0>V1#wE_d&NO*oD!X=c~x2;`vGvw_gtu%z}qJ`-FQi!u24{Pd#!y zB*84u^6V4t!3eS>!M!KaLlVpaEzdsT9*iJM5(%Flh55!96gwo@1Z!0hdkU>0b6ZtRf5Js3fjB(R;rh)fSj zFbgz3H+IP39*iJM64*{*M5c!%m<1Z2G&|&Q4@QtB32dh@BGW?>%mR%q$PU@w3u?sA z^AgSm(A*x-Jc7+Oca4GmkBnp|c#gR}!7R``<|7G4kR=H$VdmKe<BSm<57Ia3sNqtP9OE#_b7af#4AwNiZVoLi3DqdxBXYc+5u1n5b4l11lzSh~D`}J63hb4V?LAclVAi{lHeI5lK@~mB*84uS?7|$Ia}6R znKascw#o=^`n8>&o5LomnLE?!zq4ht$I-vH=Bb)c5984Fv{qN|SXp-^h>lf8C>fCi zG%9Utn7CeD#nMD0m__eQ!e4ITaCc@UPf$i=UFiLHO;n!_DVSkBq7lqOC2B+X|GoHz z`)}>{jRzyLF7)u`@2F$LI)pr;5zK-Iwe|46_p&?x#(v|$h^z~}v*`rYJ?Fb2k7xw5 z;6ZIYPA=-`R^61_l@VDNdVP+!Rja#Jggl}V%t9q<>#?tY6Svr?GRA`uSr@v_h;i!P zCwGNBq7lr32etM1__12<6Wi(-4@P8N=&Hd~)ws=>kViCvS@58?9)l9@ch@v)Zaf%~ zb)h@&8mn%0iU}AZCK|ykcu-r9jGH%u=hB`x9*oGk(9P15RK3FaLLSiwX2FBndbFCn zCD`lB-o}FwS$Bf=cxcENHGh8LkjJeEnU$tJ!U+HTk`scp>JM^dL=w=&8jn&xT=-we zBO1XhRHC-)@#(!KgRfp2VLTX-b)l;d9jcx@Un1lYjbIi$sI5n?rvCXdSS$RirTEO<~GkC+#(2E2Vwk1`&N$hy#nw)IsifDfAA&+PTv*1B(Jx=#3==|KXz42f~)`dQ#eqNi>34W_yGYM&h7 z!7O-CTaOPL4RK!IG}L%7BI`moEY?cR%(pw#`$Qv{1rKWL(WuZUr{vP>N=9T|=!qj* zstNCXyG~slopaYJ5T$QHFbkESCmI4B8J|eWh^$N9{aEw*cBw*y|5gODINr?=g|=L- ziKTjI>2p7AjGPZ4aM}$hy$jPIrFrRLCQ=Jt7EZp%S(A z=&Sduo0rxaf)QC48vEOaca9HvL?f654{Ga?UmqWJ>s>G&jL5psIHEqU_j$-88o?}h zP+O1A`Zzy+x`-nqvMw}^|DM0?2zf*!n1xEz)?<-AUv(Z_&3G^(>q6r^w)ex+A&+PT zv*1B(J-*ZD-Tp5(G#-q|y3jalZ~imJb9;hW@SwKDVSWAhy=Qyl!HBF2jq6II&+>;n zq7lr32etJGUDb>SBeE_uu8-T=6b*SqBbWsbYU>fYIvWp0WL;=n=hf!>LmtrxX2FBn zdK}SxN&3CRj0YpKE;Ra;;^#|+JfacIGTS4J2z5(#dxRpgE;RbC)nAkec|;?a1rKVw z9-lvH`b6Wwh^z~Z{&9XOdPF0b1rKWL5$axz2P3jBH2UD$5JyvWUt0e(jbIi$sIA9#eScnRR2k#Jh^z~Z`}Ul@10j!S z1he2lZ9Puu=M0rLxYS4@P8NXg+Pb zJ;5w^P+KDOR4wAk9kbxUr+6T;-c6tupXsqKp1o1y8DJ#Ay38WM^~)qoe~BzcZbk6P zUHDQ-0vcJMxt%gS{4kLb`jL?PRk#Nu*$M8`ksgd>CwQbq5{zUgc>G5ajASQxR*EDT z$xh&T5YM81C((O%?XONGu0|FklE{8;_dDe)_D<{cVaqrf$xiUPVwY;u;WMkIwDuT+ zk?aJoc9!UU>C=^Wb*^d%MzRyU23z9rdp&D?mGpoi7|Bksf3U<;t*+EcnO49MjASR+ zuUO(pm0GctcK#cSHD@F{!7j=Yb639@yJW!$Lokw^U~gxM^a{zbx%+G}1S8oAcASEYP-PvkS76(f>>#yOGiCj3sXVn4=SI8-f-pA~A!ER5q* z=MxscbK}3CV~3@;&D9Us0!Z?l_KDqNdkB=`F_gLNg&AEgTNkHQa&UX`jCtrawai@0GHy)B; z7Cdk?^9iKi*>$*DT(MK#?fEJck#(VQ&gZ)czmsd3C+_(!6O9LJ$t-x_9K$E6dM}yQ zI&RZ~bmPH@B%t{|!5)KYaj(Yhs`i`lkOZ^ffioYU2y2g3*J3+07_ml1WL=Eoo`LTs z%+(o(pg9IvOJ+gie9R}?;RGYu30#TzZX!CtEYP@0@Cki5!3eS>fom7vO++V{1sc~b zzG=vu$q^%aHQ5_d^UY{DfsPCjX7SxEyC|~-GvCoOiv+VU&R#2$V1!xdilM_gvM>)x zFbf3xt}qY3j6k#3f@Y;K!hBYYWGC2n-JW0;X!c!Ugjp&^ki}l>|G!j{VBcju#(M8w zv*|?QW3m=Bv*dlWC3Hs?t2?rE8G&Z6B`aBC>)|uCrnmN_%ZMbP*>_o@@1;*`7U*0x zf?yU1_Fb0H9oZ_~k(s4p1X=90q_-tnw7Rk?by|T44-%l+cUj_im0D?!?)*1R){X}TkeAeaS$eU~M4N0z2LGP6{SAd9_L z|KYckxYw-_;#in!XtLWrP~dM?%)EBBWJD6s?7J-S)35_+Z+?9+f?yU1_Fb0H9a)<0$Ra*nX2CnGBXe~}mZm$h zSQ(LZq1i=QkN^JrIc-RKTjRl6G7FW$Ix<&xWNEr1Gaigc0-9Zv_2{zpQd;qp8OB2r z%z_7dElcQ*EKPT0#)A=A7n*&SC3HuYt~)a0Aqi%|BdjBHbw`%2JF++#k#(WjMOhEs zk)`X7%$!SDOJ<=Gdo4@ojx1ewWX6LLNkFrUvc&Gg)zXWf>TWzF!7O-$b!4vY$kJcv zGSPT2BI`o4i?SZCOlzIKc|p4IkOZ^f!CuP}tK(ix-&5^3F!onRKm**QfLj37%A?7MDHFbgz$tw@3qWJw~c zBO8`;SIu-iyX~HbM|fB2UajQ2ExqWe#%p3u!9uoe%f^vbrll6n=Pwk`YNj zqY`^K{oE=F!7O@b5}EViWJK15?*B$BFQ2OEU;hrxpvz7$3**%ItYZA<^mEb9b>0uk zh^z};@my;!?>S>)q7lqOC2H%z*=;f+>p~}9Z0qGcXG}~qf?23UZ9R7A=hSWWd^j1A zb)grJY47DdXG}~qf?23UZ9O=j#&>z zWL@ZucXjsio--yU8o?}hP+N~e`u)cpPc=6ljL5psJx+J=@}4s$CK|ykcu-r9vN194 zW<4KHMr2*+ln1+ev*yD^BbbFs)Yjup{XS~2o)0G@vM%)DsV{qZ&lwXFjbIikQCpAR z`n_7bo)0G@vMzMq2CsX0&lwXFjbIikQCknrZj%vN7y8fq1H8QFjERXxFbkEat;e1w zc0Qbp$hy#1PYv?&E;S}58o?}7qBb5eDWUmrG9v3jCu9uw@@_jOD#0vNqP8A$^?T9F(aWbFF)`5yW}y|*-ha0Eo z!^w!OJ5GD7U6t(RQ=FKXTM;sAh4u(1VqPxje5>cf$%rJN7pzV3@@bU59z-FSg-X6L!XtN4;Qz2sPSM#)`k9~&O|St zh3n5M3c)ORP+O1IdLG%2i?1sgk#(V)CQtO*`EcWNwp<0`_$>%#p%OHo>7#eGPw!+z z)}`iMY9`^o6@e^{+cym-H(suZrTS^{b+3%by3knjtod-E^@t#tg-X<6+ruX#vMw~X zQ`UUA(DsNRn1xEz*5i4-Uv;{?))0)yy3p9)vgX4@BbWsbYU^=XA0OlOd^j1Ab)j)Y zWzC0+MlcJNsIA9DeVotM^WkJf)`iBIA!|NdG=f>EL~T9Z(dVlndOnw`w>;$tgPHjCd==pFz==pFmBI`oqijy@TE*ilsRHC*X zp{ttlU_{pCH8E>G9EjA)J;Df?#VcwS!P#vxA_-`80Fm?IvJ=e0IJI4m-1m0!t<>}3 zWJK15MkkXsA1)ffEL5Vl9-;oqcrYUCLZc(gnhzI^U=}>6t;dkid^j1Ab)nH&X3d9- zMlcJNsI3QQx5{`+wjN2kZ?CQA!^w!O3yn@cYd%~wf?23UZ9UGv zw@SqPOvk`{B$!3MzRyKlL_}=Bs+m; z8SJn8PU-y99y@qt>1t##B8lu>AO8A9a_s5Xx5Uawc0%@|e&>ju5BHd!4<~!k?1b!) z_1}}x^Wj$L`EVc@$xg_=UC$}f^WlyJhZus9?1a3V@H;E@e7HCCd^mV8lAVzEIDTin zo)33Q&xZrSNOpqvS9S{))$`$|p5I;z1S8oAc|YiP{?YT{mit;5f|2Y5?u4`E!}&w= z;o!l@tq9&b>%BzJhpRfL-6|4@WGCRwX8?ZZfSwOmddopWFp{0X`tiwx-?>xIhr6if z!*R_Kkpwi(e0(?Ice?2La4o;DmX@7h7RK?Z^9hTdJE-Tw4bb!9WJK15#<`O3Cj3rO zJs)nSo)4FuU>3%)FYrl`oe$Sm&xex{Sr-~-SH7F@J7@HKxUYt!8xPi!S*V0Plux|; z&M7?~?t48SPDUgFjq@?zP52#0&xgCeTTFU(f>{{Hal!Ed{c$oP>q6rU z&UX`fPt0E??pr+{E<3?2jN>@x6G*?aNzaG7r02uQh^z~Zb3Wfq_???eJ#n@5e7Nic zvoMZ3OFl{UJJ0C(aB+G*oQ%l2(6~zQ-Gtxyam|s~w8Jmd&Q353gnYgmeimg!)`iA( zh3_V!6U+jE^D&=nhZBs*y3n{1@!dppf>|JNmEaTlaDow87aG?tzMF_nFbf2(U3}A! zHM>prYO*(^=9|%Q0-YWr%;LLSc2VXmn)z;-Ge*K(Ox$ zBm6P~&0Y(d-eE+HjASR+cio;~7HIZeVT4&KMv%o`OL~V95lbZr_FcCpm<12^U15Y- zDn^jSUhDtARFYud73LAK9?XIV`>rs;EEOZjVz2f8Un)tk?+WvXSSn`0gMF7JbVs&Y zcVv&rNcN{{;ro^D$kKF2mL`{163}5CnR`^vhkI1dhs#bd3k3TvyHvU(OVb@$nv6iR z*OHYiaZb;N`yn_a!h-~8_Fa}(s^`P?(evSCEtv()zRME2BTLgAnOQ1Eki}k0RKyDV{C&xc#;YY{;(3k3QuzG?8gx+635;bepw?>wZp_0SzzT9rBN zYRQNspxJj>LU&|ox+9Aqm<1xNBXe~}mZm$hh)tVR_Ffo>;$tg z&R)xU=#DI1cVxzc5m^^HtRr)EN0zQTGUFi$X2FBKmi5pbS-S4Xj0YpKE_7H&=C;@K z;bQfCI9W?(VVu2|_0Szzn(oNf$VhgAeU~N7ZLt22B%qlE&0Z^#U?e-izU%e`vp}=g ciX<38mL%AB-JW0;X!cr>1S80jL{>-kf9XOPbN~PV diff --git a/zed_wrapper/urdf/models/zedm.stl b/zed_wrapper/urdf/models/zedm.stl deleted file mode 100644 index 7048bc7798f754239dcaced2a75e06f9871d6598..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 75584 zcmb5X3Aj#W7yrK%GL(!NLMkcqc#!i9rIbdLOudb09%xi4=Pi|rC<-N1#s)(;{f-!>+yVhM_~6 zTyovDS2yW?)zE8(485*(hc;b0H#z3|8!v9suDSRB^lx(o&pT`UGo6p_c+NAPSCoEc z;r-nn+Bv&4v1(u<3)wrNW|H#c)2FR0uDz@9WXPWP=5q(HxWB?N#U$*RX%C#;?eQlT zly-oYEy?r#_foUv_jFjZEKVHrT(e_eOBI(M4Z_l%cVO#Nx?VSaZb6**!ywDJu@%OB`0qdgysieX^{e#jg+N#bMjyE!6bxiHG=Y<9T16G`nEN zfamfEtMwUbdiszC0WoJ5%x~ zlV!_J+)EsNQKRzxc+|9V%=11uY4)-w5J%jrHmFhgZfVb(H?7_B`|laKG~VXc6H9yE zo#US=F0D1N1qs{EHjkrTecGoKna+PjxpC+RGtmz&xO-6PQuL2U(N`!hLVL_a=2P~( z5nufmd7DpItr2LCmB@TY%wGDz@o7(j|1iNU(~BkXb<^rY0sO#^n)%(?j;WEiKRWSKSrMgV_swK z=4_4Hntls4{YUqjNf*Z$v|u(MtiK8PE-g!YUSWfhu6=5+<#-jT-LKGUf1wv$jv9^H zxNz&uU5@UsBCmFjdA`}PlhBK91Yv2<`)Fe06|-{>6~t{k6!fCAL0H<=>y+1C^&_|b z4i0(|2}^t4MGZ<;G~Y`cHu7F*O~vGS|Ji!;@~!PxEJKfqL~ga3qcs(i=Y2e}ao3uN zBTiT?>!FLI<&ZnlS0^?lw_RAUa?xAklV={*AVK-;Dif0H$__}hEL*$hxFZIE_^pj2 ztd^Bkk|Sodsg(Zk^#SR>Q0~vk`;#kI^hx{)`S`YDlc#S#Inf3-bleezAZooaAZ@i; zLu=o}Ylmz~E{112Q0}Mi-JRqt^Ec$DYmG_X{$_vQhK@TMZv;eKYv07h<>#kb%^9Ay zgpG-Dh;}xH1Vmg*(dH9PQBTX-R!aW?--o_GE?N1XZi(%XS5+LJd}w&fL@U_PacARb z5KBSCwf0RMTd^R$aQ}ix?OJ=brk#xzAWA`4E$c}>@yhJs>31v6Pg$>Qj**s!&c;n3 z?x-|BWwmVfgf)X!fKCzp{2^z4dn|`~Nc#a}@t|2Qd`{z1pJ%Sz7tTU1*CBW)JrXd17hW z)^TUU60%`E%qQ+ckG-RHCEvzJ$k**#TKaKZi99~-hC;utblll!4x(NgM_4T@M~R2Z z7o_`DDDZ7a%eD=@(s5^_DTr4BLiJ89D@Tdbk%?DTo}Xp8$_&c#gtU%38)tyHJRq!= zm7@e^oAh@JZ(U)>+FxPgB;@g1)jup`=4(WCHb~4nr0WW+)dE`oXJYK4Tf5L-^@C;s z@%o|s*pfW&yvrJOB~hzS|IU#!4V+gbqH&~WUhl`zvh1iX z*7up4Cv-a%aU8Y$-dG&{XI|fhkr#rnHlE52DG^~~^1R|+izNKD?{_hnX)`23giyIS7| z!G;K1lIPV~ai1UgkWGt@*VO*{>a@4+ zPF{-j6<05k?bW0sT7zILR?A-9Vb8E5Mx7Mv!tV89Pvr!+!S0JnwWohLT=o`87zTdi(XOw*;B%c@@ z{|4(|X9N8&cj5P2J6Wy10YR;NV)T`Fb7(OS1&S=-LW z)cxjXZwFzuti609i))WpFgkRHzwe+QY{8nEwIKO;9RCIVrXSYV&c=Bl&Tm`EuPbSh zkXAlXvR^^2Q z&J75wW$&s*i8fuwC5}6wNovc7Bh#I`{#f{9?H!43>kmsbU3_1H^1P;v5q#OkVg^j}p(jvGcgZFCd-*VYMj#bJY>aR=0I8i4v3GRf}`VQm51z zlH-~GJWxBi;P!H*B(7>&JNe$Y-9EuMdUvgd+@0KMTw(x-BnYcTnQ?g2>y)yjkmw1o zTA;4BemK&%p)+xU^;V^F6WADt+8vI%`VEBDqD-xuPH*Jf2#Hf+qw4NX={mbR`8LXi zG)k6hTUtV*^O}aqJN{hZ$D#NkLU3o>S(xhw7b#tzFsn zygxypX4A)juv*q0>nbGP1__I3I-7 zlD*%)JU&L;fXIJDmVAR6wq;p@UMb%^?_p#j^Xf?uR*SOD#E^IcUR6Qu*25oduok~Q zr<~tESi9{~yM3<|AMUk>?cw(iY0(B-P?3j34`d>9_hdv*8|rH$Y>$#{&zk_Rg533O zNQ;E*S#Le>0c7HT(3{X}{Y+H$k+|)H^$BHrn28S{j#dXW$uirm7R&8BczuGsh_xFM zf1-AOh4z4;J^s_+nS}cJ>=Dl<*w0BX*>6F@wfRl(ihbk4pgp8Td+T~FOsLI6VifAC zF|zQ~pst$KdN-l=*tzcAgxZ51y78(i>WT#Om*q-JF=@OCi3*4u{hX2e?V&Sqf{}Bq z^t}BM$N8>yeH*M%mP>+K8bw2*4s0|)AFmkXRh0{0OsIccecQ@}vPAKD-c^YFC=fe$ zb@FXU%f`go4T+7g(H^7QJwYE-Et0UcE8DPvy6OhvKS3Xq775v-jgWW<#4#XdpoVou z;}r>xS88)Nw*L;Vjt9{r7)zujuQ;|xiR)0i%vF3+$QHGeQ;IDhasBWHY`*8p*Jf%f;iI{E!WTB<8W9ugdP(a)KQ zz76#v66%B2wcuWErf1BBJ0EPGL+QMMqxc-Z>no3%RlxvPw$)}C?Yu(f`#ZCURa zaa!`5f)_)1xGNiz^5>KvV{keq?1 zX@7(G2_C(1ts|_~G^_?jmFry+CF+Ctch1wPb@Q4;=5yN6yi4NTm+B-BK9biTf0Y)b zkDvQ=>i2-KT8v{2OCf9*qvIrW4AxoYKEi@&B&xv)ecMkvHEr7)j1%p1hM;XM_4UI8zriNSUayt z`oy_UN9J?dkUbKrY0oP`T^&8YNqRy+SS@)HC5l0`12Gac%v{p!MOkw=vxM}3dtb+V zzCXMwgIB!+!fMfz<1g-27$tUsV3xda?fRrGmvLxDCL!AM7K7-7OneP(VzubIvM);X z1yL8Z+Z%thp*hvYM1nP)-pPA1e|dY9+b zg;zU~?VAEZUfFV^L_=ia8|Z&;#4?dKG_J4~l$DVbIUZ%0@y`5q;< zAoAZZx;-59D767?$Q}vRwC6Q~jkiIZ9uQVbo13gw;|mMTzmS@e@YS zkMYNHHJ?+~j7)+x?dFf(Ao`*`8sRUl#kyj&QG#RpNr?JKL{CEf&e|iP-i2!e5O2?U zI=d_&CXP?N0HqAaa^;@3t)(zVVn^KD2gPADd4V;hLdAgmT;Y0-vcM+7!fI>YM$5tQZC zHe3})Y^1D~>`^P9xF3&ldb zWE-Ee@i(-8#Gloo%=%H}k{uCbqHlv4$Ez(8%tXqxp(|w1JMxSlI*PDbl-WOc-4-Q2 z-FU2ely5_2#Ry)MUf*23NYEibe8xm*y`Qp;5=&@e~ zZLX_E5?2M+wKhJ_8}#zRr42#6qc#t;NZ80#Bd&HiR$Y%wWLdnTQ$MGye$J~t*FPQ_ zP;HqAmMbmF-vnALDI{iJS#=ruU=IDiqZOI&0)!PH41#Mq69hqvt?1X zgyN%Kv<0n}jcMNm&uBg;QGVmIg|e;ryeNHE{|bBB!)npqyz4scg`jWzXUkF?B2+j{jJ$W-`iu*7zWyy3$PT`p2;T75q2v<65&uZb&GO zkchXS&XBf*>X5z^r9XQhj3ch4dW#a|_>bjkCZp`F%O;e&iqFOI*5Gg~v066rkch{j zS%x;A-n_`4&nauBrY}Y54r9aqVYRm3vNWMQjuPbf&z42m5{l2o;q)D0wQNiw5syPN z8Eufzm`Ja5{ZW*@rGGeHSuNVrcoikAS309OM5qp_8|~+|Tw8DZCNz`L23t^bl;X>` zVYO^b`zDxiytbqb%9?RBqvqSNTFhh3gCW6LjU4~6T%Dn83B~8yyk7q>cdeF)Vi5 ztSilGB;=vzEdVhT#3KP=wce;yE7@_|C_j#n*bHL-qvmFBn0$7U<=UB3GY)M$wds)L z?T=MN93*x(otu3>AgtD`W9ucK!4-X!m$7*Wyk+$ixJhzaPi`19*ZUeShT zH4^gB^X7sW24cUyj<8zY>%E^)?uNt$5Y;g9%tE`fTsw1W#-WYz=f9C~{ewhx5F3Ns zwOaQTznV}#4~d6BJc<$YfN8D|$}4M+Hsqn_%?5EQh;xEID6gz1QG#R1wIGlwDV3}F zRkNCCH(pH!@i2(11Hx+26OD(!=F#c6B3&H zqF$W{8zms71ccRk>!2FR4nJL15+&AvnD@l-x%LgWCwb>q<`ltyM`9%qJ*Ddl?r?}F$KVhISAt9hF;wK$(giPiAx zThuIT*jkT!CEMs9AYKP?I|!>~?OAU?>jKSjIS2;-5v6A`yKau^84z!Ruv(NgTe75(cn8ETcvOs7c!n9L zc~EmWk26C#ce&gxhunP$1k05cWomIgj}jeWV-;$4*K=;JmG!t+vh8_qqXi!UaV^@! zYFT^MTX*fB#PbzAXYu3Ey#3Si$M($OBs9-^-q{a~U3NE!k3m>1%JYNOK$M`B8}X!p z&z|j>I1$#>75Ck@v;v4nL0B!ye6qnBWl16N`7s-o;rV%v&r^9u^B{@0_c}Y%t2%hb z@(~EDMOn4Lk|cXxW_QbCJn74g!gDpA(Huph9Om$w@GPCj!?~6=?gH@!o+4W<%F@aw z@QgEu=PbSrX~hY}hi`IBKD?N3Kcw(n&1zA$k%vUxZHUzBzxZB^9l+;q+~<$Hi<)n zIh;f|bKbgtL9?ao9uJ}A}j5lTvVYMjR$V1}EE1oal6Mgh^KX)~6lURc} zT>U(pYp2(LzF;bd7pl74l@^Ja!D@hBg+#k;(+l`61D~hbGjT%kdES|y%qT!VPotmv z{X^xFu#tyEQ`iWIlwhj@4J!i5EB@ANfjcdJEN^+p>ZVcI;+GnxlUumw4X+xYNn^7E&u&C^zkvSNx7 z%-tXpeH)rtNZ1)CoNF5tH|-Q;yKh5UB-lT=8la7kFn!Rsp)+wp@p;~<#U0cKeH$v5 zgpE8T1|520`nz#WbByP|KR;hs9Q=0r&Oqj`O#gZDi^7M?1_r;W-rVrS^g&}CVb55i zA>PvKHf~^M{y`P-F6Wi7@eyp?vUb%{d&Uxt1ET$T74bgYP!OMi=v4RiuJ(*2E`wL| z;8pl**;)90(8~=rWgdj@KYTSl+4$rJrK|7-E6F@QrtyU2?CN~KJ$v^1rIWrLl`jAL z=!h17(a-N1e2cFXLCVWq3K7OIeGwsg0KY9PUK0Ym($48woHrcRIPuSqUG`{U~ z4G4N7Ey~i$Ctj()DLD_nTIZR%_-;&Vd^d)KWF9X&dQ9?+pZfUzBs*d>ei7dUzhvc^Ip2;;etu-3ABSWfuXX77 z?z=mWVr;YRQ-3CVlnV7a(l%nR40*qF z3i|oI#iJuy`fA7r#rGCQ3Hrqrd=wdPZCEWHS4=LBpv_ZOi?X!xiR0YJ;MW!NmETe$ zA(;ew6u-CTY>2R0Y!7MW6D^SKC0PIPjB1p$WFD72XCvrQX{%-JA3v|;yUnqD!f{1@?kf9iCi07$`%gN=*qYZu+gDpiuGLJunFC?&Koej(eIm`xWt3_E_ z`NSuf(Vj-e)B39Vr)StZNk}Gf1HO>J-t26Muv(O*l}~iQ=vEmmXwSq6#pG;s3<#@5 zSz0VfvLhB@4qT1dlV^Uz7Y{fBkdVyd{N4da3`cwoVjKvoMOj+;1m1Pd{eo=g8PzBW z$=0^Bf!QF3*}%6UEo(2IKtIo6Hb`0Paj%qP&V~r9MOj)bNwOpGj!ACvfb*8wj3c2; zwCkXdm@;5(C#y9pAh<%xC&uEtA3wF;kg;dt1ihNJ=Sv|XtQKWyu_VdPt1ig)8_|k) z1?@2vUkaH6f;CN<$C)Mk5}zXu0P!#gt2GaVwDO7nWIxHgHS-SN1~Z&r-yMl&FmF;+%>vg|HTx;osm(A*=<- zB-mHNFNJ)GdYTBrYEhO}KEeL+6aMU(IH9(2as1_aly5`jl8_cllI(~e+p{(s;#s1Y z9I*yH>I{6V$ZFY`Y>jx{VSg_!+;#q43#(wYv=p8c;Y+|=ExiR z|DrU%!2JNeVjLga?U?U*mz+2)RrtvD>E`b=_P>Cs6(ql($F)y~vJd>Py>ZodA-=2U zO5cDHbRIH=t4XcT`87nzJg(KUBl==pG8lx_qAaa^;!|9)%#B@t@GCB=T@tz$aW-DS z`VH4>{{9e9o>7gGkZf%`ul5760fg1E_VS5muzKx_Z|_*^ zaj%p;&c=yzhNr*W-+ePvzLSs^OOot)OD9h*eEZS~-MYuFSomdTuEOG1FP_(N?5b`T zeKoP$Hz2GQzsx)n-*wI>a!>uYTcZab?e;wgtHp2ql;P{>`2-{BHg|frZ6N5c^2!o6 zD_!J4CPuDt_=R9y71>p$BZ6%AzauOyez%zR*n{(lCta5KYieB&@XNZA?ONN}_yWXa z^jxdOulH)*9wpjW7@Bza=TYgO`ixKTtBxE2`rwO{{2C^|ZFnZWqm#|v=o~g9*i5|$3wfoPHT;s$E#pG<9i%dMYvimBq%C#}kMwAFfAHS~HC%AUz zccdk=cG+vgtDtQNo6EUkPZ=uwetoOtcZo{J;swf+~B)dtoc8&aW>ZBI;tLO*lNWyF-kbDD6Rb%n0=#uZo0As_GyUEB~F+B^F3ax#qZH^j@mmh z6{GY|v8y5yTCv&{r?Vl#YEjneHJ>;XtG;{Cf;^+E27ZZ8GLLhW5ne}$uv(O*l~16b zXLyH@XB2Hba<0h2>!=e!U{>?LpJ)kbu_VbZ^4oA#^ba!Lo}mqC@wj4g#5d*VXAeP} zTP+(?K7lzZ$B}_&bnQe!GLP#D$=Sf@lfxY4e?L)LB&3y3bjMZEoY+-SymnQRvw>^?Ehl$jYj#-dXpAQlI#dxM^WM#WgiL2_DUus z?i^gPz-sYZiX?a)l~3&Lswhs-tNiOItHtl?Nz1N-omafiFcz(7SKj? zzj3c*&k-0!eZp#4d)8Y=Jb|eHj9nG!TZ6i$VkU;yQJ;c%0)*A#w-lviuaKOLAloBX zMR7uH6A~NCyKgkAT-!FbWgYP``p3<&t0J2v_6o@ny7IDGHl`@CzE*GlH<0mb9J_kq zt~6Yoab|Ef-2X-XtfuvjUHe3dCR6(rK8UNLr?4Wk*}(l+c)i0l+*0&SUBg9fJcBb; z4xQuAWcCUvdR25quYa)%4gW>&4rW2~dXTmTh_ zHm1+YUqILzwKW|j7<0`*=dRotYY%HLN-Uo+EZy*(pSnC(cVYGzJfmy=4W386n<(W| zxyug0U6pN#(n*;w{p;vI7Ny|TcjJepTR!+xm%|$_%v!Dcp~d=X_jxKtyfSiF`iNs1 zCD&fDAY#LoOWUmm+?yPEGryBs?wVogwcj>MKJem#tktSn`eoruL+?$lIy+e!C9b}D zSo-;bvgGNzW@oJy<%`#hNzOXu*cfpnh<`ejB`f^v2&=WJbWHNPhq}aw6F>~DSD2dn z-6Y?J2->J{g#S z-15p{J|WukzPM^w`l~IIQfuow!fJK+=z(PUpBu*r4>o%KJ3Fj+T`?>@xAL4+m%e40xYqY)j7?5?r&f%(@!DbO=N2zW?f$G$gkZ)rSvJ;x z4$LeOjd!DN886D(GE8F zJc>3%W6v-U8$fth_U;_lItw<={X9-IxP4f<_Z16Lcf8dw$+Dysr?;UNZhNv8M(Q(A zyXR&Xr1qcLC~37=)3>8`8&#|AdleF$LCoGgJM|ed-fC43+PwbbhsB6L(dL(<1#d`{ zC9M{}lEqf*^k(B2@fC<1vSfYz!jvT_Gslkmt9ioG_BS>_8zN|feWlur4n83oJ7&#yZCSf3E>c?W@5)1WZbt;`Wti=)9)mv@N~f1|Fh>RgswiyF4&QsxL(zE#pE zM0?(H*qGbCEcxe7M_4V6oX_a}h(=uvJ1f0r?7_Ki4c|@1we&tzlz65HSyHiTX5+&PBC92? zoQ72DmFfCsVX0j?dETX$VeQlUz)W#%x4x3r8zo;BHbw3ZpOuUeKcKF@Y*m)oj{34% z)Up$BTM!HXneBTe8zh*08`pO!jS|~HG^$mY{dVIdpO6*_=F){z z+sBA=LA+eAFk6PTPh5+6Jg9ER7||cZU0B8N&k~fGOIy>;eL^(uI)m5&?H(^U!fG*( z?;6%3MpOc^vTRcJE%ZZME@kG@f76HgglNzE6}5ZjmPy%p0b#Y6?U&SQ5+gVpT#r@d zmlz#vxs*pE6Svi`?GvK$n`#h8qb+{x=m@Jd9oc@xJ#pg9mc!E5R9&1sWAlA}S<jlAQu3JwaHlxK|-j3B>bXR822@abe_LH4<8p(A9^T^HP+4(;_rl#fkSBqt4dTLyTk@_ZYzv0O;UIc{ zcWCAEJYV%Ton1M5 z-Y^hv2L#KK7Tuq_x89DP<{EV&iY+iW1AHN1__^Lm3AjIpFH2&+ZeMy?vcFN;BV7~R@@ zGBL?}VY*{(S2fZ1mPUZsfD!bf)e|FYAG>mj674}8j*;gH)Gp8HxdGR6TFLTw_{3x& zh(|#j2f}Jm)+#(oFpjC1pAJU;@{FFukkCp$darLhz4~xs<{RX%)w0(KQ6d4Nr0$?> zKh&=hbY}dZ_9Uc67`K}Yvw=No!Ir4(0 zxww{|M?{Ii@ahS)`O)yqu7s(jXDqfP&ua-{3W#AKtQKY2h!RJF7>R6u1g%ILTGx`$ z(*;TU z=C4@j+t9k!zSm`Kd)^@+{s-b;5LV0DixM7)%TcqZG#KRD(0Z4&o~x0NZMTNQ{E-`o zerUC+~*KOA{$($dita{Wh6NI%e`X>Q8ShkU|%A`hbkpBL!)guORO zS$FFwhxbKETsiBL;_QiMcC=ddPFgyXcV)z73s;6XCN7**IZs8{dY?C80ZMw2@DA{<@8CLucZIp1m+;mnCDK zYVF%lxg>O7j5hL#OD44L$SXQMyW!mk-BsgpUFka;KlS{v;F%R|I$ABtx-XVbocaBZ zWoLk}XX1qJ4TN#Lc6A5ehRP+OyId?OB-9=x?3p;Bdk*1u2e;?C`ZiQ93EkylNg<)y zC1KCR3EfQydtG@6az|A1~-gjZwS3y`U&QW>;B1+J!PW=;0PJ1ZY2W?MfEvP@b+`Z+> z#FAa;=T^)1^C-~|ULD=AsG2cQw{JjB%p7$t-?I3zwJ-fha+25!|iFe-GR#FGQaGUvV*-CrH{%XtG z#udwo>8W$tUY~xv`(a7@MkDuIr-x&(E4zJH5$}ahnv!_z`%&pNf8Lw0<=QtWqwhfe z`ts1k?Rd|!#nkbo+`*a=a*u8Mb}aYX=JD_i?8SC|0 zVb~)Md&Q-#7Uk{Be=X%V`=i9Auu%r>ORHdy`z6`N$O7Uh9mbEVurEK1D6eqP66r!?+n#xqlQeo@Lj!${on_Lrsnt~ZY} z4!7szogh*mu=`@pYEfn!{3dyn;GUDsf;}hgnK;3E3-_G7&efG~L*n(OTf{orFDxrp%4J?a$>WSbtw`JSyKUfU} z_nbTzgw>Kg?mrkMhQq7xkR?OVCiYC6P`-KIU}Pfm3i|{4HW)3-C1Eo$Bv!#I>_YY~2U<)$xDA5Ntu$w3MOZ9E2uaU4lO1AN) zI*1^5eH+q>dleEVBl7>CH*xPiYAO3DUv+qmjIuq<#1W|78tAd#2ZYrs>R%(nUc_1t ziC^Ir_O8thMtd;A9XnfP)XzUVy;X+&oX6igpk>CjIrr529mJtQdq|63b-SoVhHVoi zxYJ*-Z=YXRkBw@WQG3j**D#~@poea}stB(JB5$#`tY25sQsf%1LZUNl9O5GPZRkv# zVC3PRljpkH^=+`OSS|@_X%r2K*6^x6daUxQ>A<5h>K`|}+ccvrQGB@O#Ezc-V1L7A zL0(CVHWU->wKB#&pVQW{maea>V;Esq*gFYxN60%1dA#p7T^z&SK z)UeKIyduHzN-gNd_F3?X`&wQYj3v^NR~*};#Fwyvy=8OQTh_P1I9OJkP`-KIF4XSN zAf68955~cANhrr?BP0$(?KVPtl*b>>uog9cuy#3r@Hiu9eAo#JHiGu>dz7?PSBg9& zf`0DrImw=*UPMB@%i8w5dy$EQP)`^aeH+puA$w8cVC>E{13SKbirvz9Mj1z0;|f>E zJnr_K{1W?MwZxv26|o~*T&wv3zm#%MwJ1>?HddqjbFeEL-B1UAn7Zds|@ zi|z~9sD$!IV9&d_R)hUlmU3sYD8XI$nqjB3FR?op3C&)VHHVXU;-Sw<-JW5avG>`j zAgauFgw+VJtw2Y35eqi z>_dk?f3DS>YGWet@%GnC-5zcQAo_y1HXsza^*u^>Ah^F_A@)CGxth-@%N_~UG_DO$ zS35y)x4*cSJc$xjVPh+ZdZ=M$iRN?J<9N<2A?^0m`w=!OV3*1+Agq?k<^F?Ff;$#g zLzWzjY_tSr<(Q>CuLW9gIWlo8+9a-}?28g@VdGrX?#=jP9GW3*O!SI*<@Px22x2g_ z`4`txv{B**M9y9Q${~6Z>UY*23H2`A(}#_AAXcM><6810N*syEuS5TPDVB+>1&u3= zTv_RPTVZ1-h@AmpwLbp4Mn?T3Br+hHVQ;|w&>r+kqa|g{CG=`(<+d5u&%Xz;8|AOS zuAyhCtHH}J0PqUqUfKKQ6dYX3kZy){#>is zi#9ax$}2Zs9R#n|z^g$4VYTQ9$Mz_}y=A%2?6m}R(ai_7bu|t-w=t}WjZ^=BaD@*F*s-E9x1Yxx(ODmsv z6kkZ=_a^O`IKj+!drr~@_M-C%t3_E_EJ-qU8T<3aPTTMsFZ}s7bS+EbyWqN(gkquv z{7L}?_8;`)kQNDPMTu(TsugI@NuJ>q9rvOmaTsLoMaSd3>T`R(T;2b`f^qmh+$wy1 z&uUQ~6lg^W>_OK_drsOjaYFHV-pFp%3bf~>)uODJSW-wB8!4UP^?(Sc$R1;qbF=Z?xRRq@i`lZ4RQOW#ZCEWEd6Xc> ze=L_HfYv^3iW+28R~#{TrGKUXA@J>>pOE z)q6*0)IUNZ9=Xo2TuUfE`cjnsr(c-6R?Ef|B`)tiVo$HtOhy|dG$v{eb~cKi3&$&~ zMSB{rqQpb9LK`}xI7Dcsb~gAw|HZXzz3rRO>_wU_s5wgUvE-ukhQV&X)VP+7DN3xF z^;ai(rJ02^Wz9I6QJsx;W8HqKaV_St=D{dIj{ivL3}s6wK9^T_n7P(!*~mkJdrt1e zE@tOoS1Xol=U2^YzhT#n%ANi$b$e}*xDUk8fUsKIDs3(0eqK?6yK}vO-Pj7SJ6Ak% z*(1Svb9+v5C$oP+U=PBGSGL?JF(1S{>_aylyNuC>yrQgGjf6b(Jnm9?4~XJ`uv#yB zUzc(}mMFp9(vHCHc$Kl!7|XTut7aVE!;@E+x*gfR0FeUmEp{^_VYT+3@kuH7qKgvT zC+<@0^0x&$jalEVJreTJ?E!iMh(!S*uWY$dVhU`G0P!hWQRQmH(X1xg?WxB-4*vt< z?SQaa^n`mjwMRm=;PxME4C2p#(0p#ojS_70=g@-uW4ZE*vSu|B z^3d%+xC+`&2ZYsPW^fq-BpbR15fC28aXEH@GirTubFf3HI9g z=&{^i)_O%*vl z2m3j0rKaYh-AP!jjjfwx)XzhLJ?e6dpic!oO1@irv>^}iTNx1DK=cR*d1XC`5*(?q ze{HHL7#Zx0tXWO88?R=7;J7|KAgmTW(U{0M9Kn8mEFxbKjERa|ad1AT4b_6%TXtL1 zxv38xb7P{svYtc4Qgo)=kbYKekgVOc{Rq)SqbcW)&+#s zqO936N|b_lzy9{j+t{6pXBaJanIfUct!=zb1!5(3eB;hEKy=1h_|pio`*drd8Yo)UzBnW7T>F24(IV+qdzNk zd(kZifv9s+8*EQnEy|k7qQp$J$K%-Lk9$t?j68|QA=_>*I_^^WI0&m{?OAU!oiqRw!aDG?l_WZUiA_Yw%~cb8iZ!fMGL$HXYHB#68WyYTUh>V$-ioN>5)uwDYe z-3n);-L00jXT8POnLuzS!O!q#t;fA$?z(*x&jT?H+WfOxlr>wjq>$hadDp?C74VE_ z*gv?-lY<1MIwI26Mwmt7-w8w)W zE(2k;tUc>3W&;rD=cy|L8ylZIFrzp$bCQ@grdGz)Rcm-v7dh1-upuql(A*a#xF_pZ z^|vP%2X&=7Az>qzZFmJ@BlhikKd38dk&r#w2#J0mu0W4n6|}khjeAvTzxo-MyOlt! z1(6BbT)vagY)Km-u@VILQLNm^zQ@Qh zt}g?@a-~I?TAa_LL>myLsM$)WVQW3^m27+7bBO$A5D$T{TGpQR)?NDt``!6AG;jZa zU9BXO&^#X{tQKWyhh=6JcG^O0aKV+GX zd5+fEYMhHmNLJ)-Y^N0wR*SM~A)n9=l03tVlOH(U6OSM{*rIk5k|aB?4sARnJp_AB+B0!NF*%|(i2FfUEy~hjNs=QrQaZzVTZFDy!nu}A zV8b6vSe6KB$y+zqk_aOAZAeQVQj3J@kox$R7i#whyq#dRDBIeN55b(6=AiWg_gc_94Cr$9cCQe zalnoPyIdN0vbwO36Fe z&Et4{75GtjqW0+3woi$6^RTtzAw{JtV=gr>SJlFNB zk*RvGIKpb_D`2z{68LuXp1k_ym9=~J6?t%0vl4xcd-Ok>_IyX$^SW0`<-R+#I92_E z8opPuL9c2QKaeb$d6Msy?0Mer_a$?;*Iu4_Zj~wH&K{R+_0qAvSF(o&-_SGn;pOX6w_U!@C#;sf zG)AvNV#FoAas@x{N{uKtGeXeDmG?iGEIhk~Z$q@_9bLIsZqdKHQzJV%!fNUJV6+hu zFMZf2cl>2l(!JYv_ic!vjiINFPyTXvecy&?w8z7Ja+ThylJ0S&BdnHw0Zbbqk?qg6@lZO4z!SS`vc#*R-mtagC!RYc5y?y$3y~G00`jt6;wRLQt-0hQVrk^VB2&<*< zgGGtn7q`gmn0{NT$K{>;IOMPOiuTxVMl|n@I4=FUdG5Q9My6g}*~yPXzO!6?B`ZqI zo7g9J^cz*uPdDkFqLw^~$H6vtdmJ|XxKHkaORA)2wCSF*T9lb%hqr9uM;;OfR!imT z)+tWS7+521t;fA$Z=NyYB*byRgk-MALCaIk7u85xEz0`N8A}R@|5ojlI}5*O_`dB- zKMq-sdli0nea;KE-O1-#ih98G~C&3pCB8{rU-pep$a>r> z(VlnWcRh1$FJ71W@S=6TSMr^N%C+`fp0C2bWY{e>GOKaU&{(@U7+tx;vLl`k_9csJ z?VD&H>`P_|${IOoL$tHe(hye5%2DE5?B2p%r){~EH4f5-XlFxvxW%>hP4o$No#yDH z6#!*^RdSy!qB zXCv4R%CB9kWo1jaJ!Qyptks%xZ&DVN9w87e^-$HwA>%vT~HbUPAtUceZ{gYhGeqsTQ0K?qv2W z_DYFs?TaYN)i`dVznWo$A6C0BdHcGQAFW-ZMC-~~a3TVqDXkCL+b}|xEgc0YqjJ%X-i1IC~dEC?E2GQ?|5FS?$$k5MOMqMUZVu# zc;@jodums!VtQg@`rkKWlV6q`7{qbV(oxIkTU<-N^LR*PrtK>HVb`eisdax{$+d`9 zsZRwxio467yK=bSqujTU=+(%h78E?Y{jw#tEUsQy?$wVRnBZ}FOJ9m|?QUJR?AzC_ zTVk~+Q;VzRDDmOyUb)k5s^fo2&}N3!vfg^$5)i`z!fII?QR0n_Ph^+%xjtQEz+uTB zF!r%eaBoS8~1p`QJj~{*#Qu^A@`GRhlixd7IY*wl$dxI_?Or;kXN5T#G9U zUZvy{!Ct1R$AZzBUQuS>=E~c8>ufB+$k6bB`92}7pMvpx?}T=Dq*t6%xf`X8>1WV7 z?ra3>#7N}U_k4nL?ax^GS>GvhoHph^;7p4hXAdea|O?^_6c!o>+S%bllnC8tyXO507iflPFRDiO&*67~$R@KRRLi zoy}L4qzrX7f>AVSwJ1v~pIGD8yOHZbdnIh!-`VKv*3^+JVQMLN^9jy4i?PR{^@Oro zE#IrD$ld3WyK$|36N|A5{|yug<*2<9wk_-8Xy?}Lk;tv@aUz@tIeu{DT=RRq$Q8XE z$7rJ{mv~}NZT-ZFqFkkIOA9*1?(f=rB=#<@tE*+t*6Y&$n9`+Ii}k}* z@gr-xNA3*{+L z67-6`@gK{wT9#IP)Qb{U%f=KX{+V@j$2m7$+ksC^_;jJ`Sa&U}>stFH#n~wT(bXMG zf4j8~lpYbu=Nq;R>93yDM z64r*LJ?~H47hU<|OPOi7FKVA)T!%P*c%)Z`_d+d=D=*v^Z8Y=c%tN>@YPD8kT+w&o zqr^_!vur>8wrufbogxJ3Nt@2j@IIxbJ?}o;vpo9ak=ZA3&oZv1Z|Fyf%?G7&ztt|z zuEqUCOHh98)&3dYLnI-ZyK^OTxdWGHCoFV?)zVkqqeRzpPsyz){3bi^yKhpqT*{a1 z9+ctTK@y^|yFBjoRlf*N0C2A_uBGqWM~TI_1NqYCUD|HAvNW?qyD@m)$w&0b4K1#j`*+Ofq}5_p)*gOA zhSvsBV!z4#O0LD8tWV;8!*fBO7~XVHseP6@KIlx)Uv_TVv$MI=6=Ag+1~&X_g9tHg^YJUE1Usx+ zPppRtJXh0htFH9VP*?w;uIy7}TUXIIzP#yx+)GC+$xN$0Ao3L1(j4J9N-Uez57(Se z9(O=)@S96A*R~rFu|Y!n0z`?+oAk*Yl&P5;devQ-VL>}r?tDNgWoF`<;q^;b1X;p8 zt3B_+E`4&fr`ODF`0-Akuv+>ieU!NPsusDDhi=P$e_MtxE3T#@(~DE>Gfdut#y{X6&KxNuOMUp;dDK zZP`6*wJ5WnANh0(Kk|@xx>hPTyjpQ~aNin!9I_twisRUmznlacLmo}$np9k#ZS+hH zKMwg$Lc1<7j*zH(Rj=HU>vv`6?3j_|8IEd|s&^|Tp*<9=hj=D@P_Nwd?Ypz5wwjr> zTC4^69wo|Ma7ym`PTyo#ulvTgA?tCkWZUzmeBU$IYtXuEH~d*GYtMSiYai~D1E@{_i6Do0e!wRXgC#B~7?*V;RwHQWuuvwj?9`=68H zDvVxfPGlU4k2Y|PbN1WIE_?nJKMrZdBe#U-;rR#mKTlh)C~HP$9P-e`VF{~cWlOj= ze=#$|_ewKzJdUfH_0Mn}?Bck**&_v4baI5^V6>_Q%9g-ReAthk*HPAY%9`~Vhdgv~ zSi)*qIZEK(QifOcwry?w*qOoEI0*#rEycA|KXJm<)p3tp?$?!87OX4P8zV1%c|eBO zKCZ6rA27b)k<%}U)UK_cIN^D>pv`yjUbSgk%8Hg=$wL=M+aOEgTKgty;!VR(adl_g zfU;IdEJ-!(Y*aUd)v~fBTwUG0u!9Z z^%EsDUbT93i{A&e-k}ZIBXL~aK^fjvb?wop_}_vfA8xfIt|d?61a=@o?%spkwY6*O zhwY)7c6HSV#N7d*I-!=8qr`eVw;O{y6SfT~Yc)xGs%d8f_cC(4ml4<6H-Y$@%2dgyFeLe{BeWyR!(-Lr@1uEO0S>lJ0KWNA+xhQzf2VYRFrCHyD)xEE*3wQF43 z5bbPC#Z&4njW#9YT3Scv6S$L_Vjnte^TZX>Ydc0eAA`_{grG;^jy;`_tcGb{QDWU1+Bg4>h&BD69U3& zSx=&bi@hkf`lQPWtPQJW>&@vi`h#D7c&XJ|5Y#SL-g_s=+B245d#I*E8y#<+yu@nN z3AAVPN0D93kJhg5=7pN1qc) zDeE2(WnD8k;+`2t`d-Dg^fqojAsh5c{!+GHY1Qv+?Em_`dm^`5oRL>|ILePBO2`JS zbLExonD*naG3lDY**NZx*ZjI7VYL)3k4Fh|3kI%RO5bIjvb-V@uG`rjXI*e`=T}~< zyezK8HPwWd+xi|xiE+E!-9eVcRoLebb@jDuOuA-paqxfsBVo1VJCE<1kacQVul7w? zE$g8rJnw-k@5pU!y&+?J6lIP$(B+>LAPn`negT#ih%UNKr_qV>?(;7VpPvOTV~ZvxNHd0p$rVOvnXGY%bhHf{rf ztD;EcHrjmRQ)I{tWTN%Twi@G*ht39x=>ef^pq7=RM0I51SWqlinPD@r7HA!JHX4G! zwLv6u>w7*?4q3Pf*=T*Y_N<4_#(^N1zgEkZ8znl7U$ye)uP1g}hr68iHy?Z}Nbhep zU+#W;;O+?1$_KAL)@?fotHpQV_D+0VHfCkD9n-sQ1!1)YAHI2IS<6OAmK(Kk^n@Jck7wW5>H~6vA^GFP;9j<9eo#P6o{F@PG(k% z&ph=ePn5Xe@u8(-u>;t*squ+l@TOc(yfxQn-P}Sx4b__i`ER`(fgQjqotE?8LXnnz z3nfa-y=6-2biAe4-WwBnpTXL&wq2W7f{o=L0r)Tdt_4@SW)rV#5neSkab@kYy4@<0VdR3I3u-bibk-q?Y zX~996Gq!!||8DoPF?BL){#fSA{Oy|i<^YM28!Gr~I1)NT*%D)3IW)83sL}pAMMdcs zSG(U~QA=kiSAXTOOqaz~VmA0((A5~80>x}tqWlqu2VRkwydjJubK_T~qi!7S|DMl! z`1h8w((6wu##@sl{=0hbah&$VU!_~RulDJXVC2>-Syvp__O2SS=W5qRZarZfB3L8z z))CgmE5#rA-*lmk88@ve&95tZb@x8vumo!)Y>&HEyD$8)Tt=%lw;nQbw!f>ZM>m8m zXth)>O9}~kwWw-E|Lc@{v+??G>;8YVxwLFn%C_@rTr79hYSN-@W~FO$dUg9ga#wAT zPsHQcn+@8#sQdaLuclPJXRrN(aah_B@w~EaZsTBUGGknp?5oXf=3Bz^cAQt1`UAi8 z=B`Hi`Y*qlESbmc_mHvY9lr$t!fH{LRz5K!*sZ{xi4%+|+~45AfUsJWrNxpYyPko0 zHtk{d>@Tml1Dqr1)sY}nt_a%WmyV;v#LAz)z{n?~73~?tAwqTNc`t(CUJy$_SS`x7 zc0^X4xk8Oq@_mE{tpwj$5y>%Sx<^SHiY?1(-fxW9qb zqAaa^;%a0%zOv!lkXD>fjW`=3tQKWy(S~G4e1;y2YeL_KzWz%>vW+iFSS`vna><_e zvdi7vi$U)4%e>q>k%YdwPaFL9uj?O^L11LaJr?Ayv`A1(*&Y(~YcodBM9>FyCQc|O z7Y9da^mD&|s9X}#qF0h5gg=(>jP^8;tT^0wMP>_ZFjG{n2-@S$1ySNzjBxx8%lu$W zq!!Ck93oVQ_=12NMRQ|=F;V4`P`%McNYJaG&2v1%mf|=2Nl4~##YFv(uv(O*l}`kj z=-Xh%aV{bunS{P!>}-gzT9l=gPoTZCY6F=78k;tFVtrlfz(S~Ffhvsvh z>Aa?4l9@yK3OS7{*6DQ=Yi$jFfqAV?zBsoH)bcXYt2+Fb@&gZo8C&m)1 zC3_sNqQnG@aEyFEd{xDsi4&^B=zMOqDBIc%2}TmMV461AYMjqWNak@yo(D zEAq_Fb?+urSCV=Bwhz`P!Z<{*Txn63Rz7h%Mrp2j?U^{CdUIZhuv(O*#gZgDq8irh z7~g#xSKqcWq1=^BLf%FRmMbmF(uxwVBX=LhSk)V27SGHc@oYl5k|alntj=gmq!wk_&Y#a!t_XUl@rtq| z?0jy^l7}KxhuHlKVgX}*GlrvR-A4=>7ZnIPi9|n+_kbolJ`qUheX^fS(jJ5M`G=<^rG}~ z6S=J`MlOOi;_k!6ZM@OCevB6QSFI*3mc%>`@4ec{ zt(Mv#N-*+x9DB3DlEV9NpS7M*Ks$;Ef5Ru>f z=+{8;-~aIVC-d4R=#P7Qhf-XNzZj}Jy2YN~744nSFQ@G9l_>N0^CLP1HtO|xZ_n?e ztXJ|RpZK!zp$=ST_Dx!RCwU}nbUyp;AMc;L<@@)Mjw6Qm>t6whI$GgrXY*~^;*f*EM_Sl=&v>QA5HloBytKanR z2$Rqm$s+7q#Zen1tk!F@+xa&3P3R15P!^%Le;IF4Zrt+k{r<5xt&0C`;oFE3C|rkrusL*rQpDK#$6e+4ZVlS8;;1 zaNpMr_Shrs;^6=MN5X1RUhsMI{|AD(^wlSa`h;kf9F4CWkr7+uuKHCti61K9RE>^qrs=W>-q%cHGdwp#~y7{uPAqG`Ir2> z5@EGuPu@C$|MMRSj%t5YtLc|Z`H7BA_SmCs>V@qg!fMH$ymdsm1B-mGm?g?l<(SRH z+55MS*@$Z~caJFS=(jmb`o9y(C6z?lc~!6Rz&+V+wJ1+Lq|2V%4GD7m$FeLD&qUJH zE6NRTc*dUh;j9+rI}+|KG}&{+ao6_g_~FT?FJkQ~6DglNVRW)>ZAYjiN9-PT^q$;} zYsp5GAjf|!SNSS==j)yQHkWPcMcbT&v}DhE>xk()*X}hFnHjfk?XV|znU#)s|FqBj z_F!3YE!mT|j;Pk6Xb)lYmGVC?wAz!q%t}Y_fBvJ5xR$(9@1mZIeA(XvmT07Y?%G2V z$|a7dJkF8&uZ}emjyUPPw#CoAz3v68#dj4oYUdL(tJL}b>N=ayDy}dL3pTbi265p} z1VzQz1eE%dn~QQ|E4cEnMS@TeG17<-gbRvxAsRtZv`|5cV)yl+Y}+R7+6A zg`fuO4;5EgG`g^Y<~g75oNvxtXE7Y^e9w8$%)IBDGn1KW5tV}<+sOM<)ge_kyzF}yS(FASD5SN z?YI+b*P1KxE_u*vTW;U5<;T*)N^_QXiSl}RLg&1x?Yp3X8>&A{4;qxogN(JLZTrD` zwn>A?^gx*}w6Ad$~p?{n(rj5f`195;#Lw zXVFE%M^R=53Dtz25$6WCFK_R%F@wJ%YX%7j)nr8D=2A9hNTZ~YC*W9=)%+EinH>3D z+Qh{@7ojNW6^U2ts@e4%U->JtW=7QLqo@JlqokK7S_Y4YbB;Rh(>Q^$W)zgRX*_so z|MyjdkCI*_$h&YXufA65*rBwfz2Z0{`ePm9lcMw2K;Pl=LEz z?gwtrJvn9NU}>Aip06HiPv~1I)0o%zWjKRAN@ebssVPsuv5?{XOpbnbN*u$TK5;Q$L8MVWqekM^+DV~R z5Sk$})kA|(ZBfIwN-cpd5))tV%*_Mo_0$U(Ye$Wi|F`Aln)LP7Ct%s487X}f<@~H% zTPDBF`Z?`IU4;5zp4fQW`k=2XCE|=(oR9B&JHydUqtH`khy=O4WzqBCUG~O30fs}wNR~!GiLcKP8vnMn#U^=-ih>WpM5mLg8F>C)*fEm6#gp1 vmNW{DBJt?+h2dDyi^vT6e$aolr!lG>tAmry)JE6LuR$ov_dDMoCro$`VW`lD diff --git a/zed_wrapper/urdf/zed_descr.urdf.xacro b/zed_wrapper/urdf/zed_descr.urdf.xacro index 69abe2dc..3844abfc 100644 --- a/zed_wrapper/urdf/zed_descr.urdf.xacro +++ b/zed_wrapper/urdf/zed_descr.urdf.xacro @@ -67,7 +67,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - + From 05c318fb2d7868ddc991fb5398060e3b97d2c71a Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Mon, 26 Jul 2021 16:26:43 +0200 Subject: [PATCH 059/199] Delete .gitlab-ci.yml --- .gitlab-ci.yml | 8 -------- 1 file changed, 8 deletions(-) delete mode 100644 .gitlab-ci.yml diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml deleted file mode 100644 index 75b1d044..00000000 --- a/.gitlab-ci.yml +++ /dev/null @@ -1,8 +0,0 @@ -image: stereolabs/zed:3.4-ros-devel-cuda11.1-ubuntu18.04 - -build: - stage: build - before_script: - - cp $(pwd)/* /home/ros/src/ -Rf - script: - - cd /home/ros/ ; source "/opt/ros/melodic/setup.bash"; catkin_make From 1dbfc81ae85f7e4aa3c015608dae1b6070655815 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Tue, 27 Jul 2021 12:30:51 +0200 Subject: [PATCH 060/199] Add service to save 3D map as fused point cloud --- zed-ros-interfaces | 2 +- zed_nodelets/package.xml | 2 +- .../include/zed_wrapper_nodelet.hpp | 23 ++++--- .../zed_nodelet/src/zed_wrapper_nodelet.cpp | 68 ++++++++++++++++--- zed_ros/package.xml | 2 +- zed_wrapper/package.xml | 2 +- 6 files changed, 76 insertions(+), 23 deletions(-) diff --git a/zed-ros-interfaces b/zed-ros-interfaces index 294df84a..dcc55f56 160000 --- a/zed-ros-interfaces +++ b/zed-ros-interfaces @@ -1 +1 @@ -Subproject commit 294df84a01e7ad35d20b33cf96604c67593d38ae +Subproject commit dcc55f569ce1e1d53d65aedc7fa8e9d9153f1fe7 diff --git a/zed_nodelets/package.xml b/zed_nodelets/package.xml index e0d5a7f7..6b7367cf 100644 --- a/zed_nodelets/package.xml +++ b/zed_nodelets/package.xml @@ -1,7 +1,7 @@ zed_nodelets - 3.1.0 + 3.5.0 zed_nodelets is package containing all the nodelets provided by the ZED ROS wrapper. STEREOLABS 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 6459be59..8d56a004 100644 --- a/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp +++ b/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp @@ -21,10 +21,6 @@ // /////////////////////////////////////////////////////////////////////////// -#include "sl_tools.h" - -#include - #include #include #include @@ -37,12 +33,17 @@ #include #include +#include + +#include "sl_tools.h" + // Dynamic reconfiguration #include // Services #include #include +#include #include #include #include @@ -312,6 +313,10 @@ class ZEDWrapperNodelet : public nodelet::Nodelet bool on_stop_3d_mapping(zed_interfaces::stop_3d_mapping::Request& req, zed_interfaces::stop_3d_mapping::Response& res); + /*! \brief Service callback to save_3d_map service + */ + bool on_save_3d_map(zed_interfaces::save_3d_map::Request& req, zed_interfaces::save_3d_map::Response& res); + /*! \brief Service callback to start_object_detection service */ bool on_start_object_detection(zed_interfaces::start_object_detection::Request& req, @@ -454,6 +459,7 @@ class ZEDWrapperNodelet : public nodelet::Nodelet ros::ServiceServer mSrvToggleLed; ros::ServiceServer mSrvStartMapping; ros::ServiceServer mSrvStopMapping; + ros::ServiceServer mSrvSave3dMap; ros::ServiceServer mSrvStartObjDet; ros::ServiceServer mSrvStopObjDet; @@ -533,9 +539,9 @@ class ZEDWrapperNodelet : public nodelet::Nodelet double mCamMaxDepth; // Positional tracking - bool mPosTrackingEnabled=false; - bool mPosTrackingActivated=false; - bool mPosTrackingReady=false; + bool mPosTrackingEnabled = false; + bool mPosTrackingActivated = false; + bool mPosTrackingReady = false; bool mTwoDMode = false; double mFixedZValue = 0.0; bool mFloorAlignment = false; @@ -609,7 +615,7 @@ class ZEDWrapperNodelet : public nodelet::Nodelet double mCamImageResizeFactor = 1.0; double mCamDepthResizeFactor = 1.0; - // flags + // flags bool mTriggerAutoExposure = true; bool mTriggerAutoWB = true; bool mComputeDepth; @@ -678,6 +684,7 @@ class ZEDWrapperNodelet : public nodelet::Nodelet // Spatial mapping bool mMappingEnabled; bool mMappingRunning; + bool mMapSave=false; float mMappingRes = 0.1; float mMaxMappingRange = -1; double mFusedPcPubFreq = 2.0; 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 a475b992..d7a6c055 100644 --- a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp @@ -18,11 +18,11 @@ // /////////////////////////////////////////////////////////////////////////// -#include "zed_wrapper_nodelet.hpp" - #include #include +#include "zed_wrapper_nodelet.hpp" + #ifndef NDEBUG #include #endif @@ -615,6 +615,7 @@ void ZEDWrapperNodelet::onInit() mSrvStartMapping = mNhNs.advertiseService("start_3d_mapping", &ZEDWrapperNodelet::on_start_3d_mapping, this); mSrvStopMapping = mNhNs.advertiseService("stop_3d_mapping", &ZEDWrapperNodelet::on_stop_3d_mapping, this); + mSrvSave3dMap = mNhNs.advertiseService("save_3d_map", &ZEDWrapperNodelet::on_save_3d_map, this); mSrvStartObjDet = mNhNs.advertiseService("start_object_detection", &ZEDWrapperNodelet::on_start_object_detection, this); @@ -1815,7 +1816,7 @@ void ZEDWrapperNodelet::publishOdomFrame(tf2::Transform odomTransf, ros::Time t) // ----> Avoid duplicated TF publishing static ros::Time last_stamp; - if( t==last_stamp ) + if (t == last_stamp) { return; } @@ -1846,7 +1847,7 @@ void ZEDWrapperNodelet::publishOdomFrame(tf2::Transform odomTransf, ros::Time t) // Publish transformation mTransformOdomBroadcaster.sendTransform(transformStamped); - //NODELET_INFO_STREAM( "Published ODOM TF with TS: " << t ); + // NODELET_INFO_STREAM( "Published ODOM TF with TS: " << t ); } void ZEDWrapperNodelet::publishPoseFrame(tf2::Transform baseTransform, ros::Time t) @@ -1854,7 +1855,7 @@ void ZEDWrapperNodelet::publishPoseFrame(tf2::Transform baseTransform, ros::Time // ----> Avoid duplicated TF publishing static ros::Time last_stamp; - if( t==last_stamp ) + if (t == last_stamp) { return; } @@ -3478,8 +3479,9 @@ void ZEDWrapperNodelet::device_poll_thread_func() std::lock_guard lock(mPosTrkMutex); // Note: ones tracking is started it is never stopped anymore to not lose tracking information - bool computeTracking = (mPosTrackingEnabled || mPosTrackingActivated || mMappingEnabled || mObjDetEnabled || (mComputeDepth & mDepthStabilization) || - poseSubnumber > 0 || poseCovSubnumber > 0 || odomSubnumber > 0 || pathSubNumber > 0); + bool computeTracking = (mPosTrackingEnabled || mPosTrackingActivated || mMappingEnabled || mObjDetEnabled || + (mComputeDepth & mDepthStabilization) || poseSubnumber > 0 || poseCovSubnumber > 0 || + odomSubnumber > 0 || pathSubNumber > 0); // Start the tracking? if ((computeTracking) && !mPosTrackingActivated && (mDepthMode != sl::DEPTH_MODE::NONE)) @@ -3505,8 +3507,9 @@ void ZEDWrapperNodelet::device_poll_thread_func() // Detect if one of the subscriber need to have the depth information mComputeDepth = mDepthMode != sl::DEPTH_MODE::NONE && - (computeTracking || ((depthSubnumber + disparitySubnumber + cloudSubnumber + fusedCloudSubnumber + poseSubnumber + - poseCovSubnumber + odomSubnumber + confMapSubnumber + objDetSubnumber) > 0)); + (computeTracking || + ((depthSubnumber + disparitySubnumber + cloudSubnumber + fusedCloudSubnumber + poseSubnumber + + poseCovSubnumber + odomSubnumber + confMapSubnumber + objDetSubnumber) > 0)); if (mComputeDepth) { @@ -3587,7 +3590,8 @@ void ZEDWrapperNodelet::device_poll_thread_func() mPosTrackingActivated = false; - computeTracking = mPosTrackingEnabled || mDepthStabilization || poseSubnumber > 0 || poseCovSubnumber > 0 || odomSubnumber > 0; + computeTracking = mPosTrackingEnabled || mDepthStabilization || poseSubnumber > 0 || poseCovSubnumber > 0 || + odomSubnumber > 0; if (computeTracking) { // Start the tracking @@ -4381,6 +4385,7 @@ bool ZEDWrapperNodelet::on_stop_svo_recording(zed_interfaces::stop_svo_recording { res.done = false; res.info = "Recording was not active"; + NODELET_WARN_STREAM("Can't stop SVO recording. Recording was not active"); return false; } @@ -4550,6 +4555,46 @@ bool ZEDWrapperNodelet::on_stop_3d_mapping(zed_interfaces::stop_3d_mapping::Requ return res.done; } +bool ZEDWrapperNodelet::on_save_3d_map(zed_interfaces::save_3d_map::Request& req, + zed_interfaces::save_3d_map::Response& res) +{ + if (!mMappingEnabled) + { + res.result = false; + res.info = "3D Mapping was not active"; + NODELET_WARN_STREAM("Can't save 3D map. Mapping was not active"); + return false; + } + + mMapSave = true; + + std::lock_guard lock(mMappingMutex); + sl::String filename = req.map_filename.c_str(); + if (req.file_format < 0 || req.file_format > static_cast(sl::MESH_FILE_FORMAT::OBJ)) + { + res.result = false; + res.info = "File format not correct"; + NODELET_WARN_STREAM("Can't save 3D map. File format not correct"); + return false; + } + + sl::MESH_FILE_FORMAT file_format = static_cast(req.file_format); + + bool success = mFusedPC.save(filename, file_format); + + if (!success) + { + res.result = false; + res.info = "3D Map not saved"; + NODELET_ERROR_STREAM("3D Map not saved"); + return false; + } + + res.info = "3D map saved"; + res.result = true; + return true; +} + bool ZEDWrapperNodelet::on_start_object_detection(zed_interfaces::start_object_detection::Request& req, zed_interfaces::start_object_detection::Response& res) { @@ -4700,7 +4745,8 @@ void ZEDWrapperNodelet::processDetectedObjects(ros::Time t) objMsg->objects[idx].tracking_available = mObjDetTracking; objMsg->objects[idx].tracking_state = static_cast(data.tracking_state); - //NODELET_INFO_STREAM( "[" << idx << "] Tracking: " << sl::toString(static_cast(data.tracking_state))); + // NODELET_INFO_STREAM( "[" << idx << "] Tracking: " << + // sl::toString(static_cast(data.tracking_state))); objMsg->objects[idx].action_state = static_cast(data.action_state); if (data.bounding_box_2d.size() == 4) diff --git a/zed_ros/package.xml b/zed_ros/package.xml index f82d8b33..61ae8670 100644 --- a/zed_ros/package.xml +++ b/zed_ros/package.xml @@ -1,7 +1,7 @@ zed_ros - 3.1.0 + 3.5.0 Stereolabs zed-ros-wrapper support meta package STEREOLABS diff --git a/zed_wrapper/package.xml b/zed_wrapper/package.xml index 0388bfe3..d05b057c 100644 --- a/zed_wrapper/package.xml +++ b/zed_wrapper/package.xml @@ -1,7 +1,7 @@ zed_wrapper - 3.1.0 + 3.5.0 zed_wrapper is a ROS package containing the ZED wrapper node for the ZED library for interfacing with the ZED camera. The zed_wrapper_node is a simple loader for the ZEDWrapperNodelet that is the main interface between ROS and the ZED SDK. From 1aea5f9c0eb44b222b5809cf0b841a5e725a82f8 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Tue, 27 Jul 2021 14:09:05 +0200 Subject: [PATCH 061/199] update zed_interfaces --- zed-ros-interfaces | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/zed-ros-interfaces b/zed-ros-interfaces index dcc55f56..11f2ae36 160000 --- a/zed-ros-interfaces +++ b/zed-ros-interfaces @@ -1 +1 @@ -Subproject commit dcc55f569ce1e1d53d65aedc7fa8e9d9153f1fe7 +Subproject commit 11f2ae36904e5469f192a05ce383a06f0468918e From e38a7fc050271259fbaeb8195787d2f19199f2e7 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Tue, 27 Jul 2021 18:52:15 +0200 Subject: [PATCH 062/199] Add new sl_tools::resolveFilePath --- zed-ros-interfaces | 2 +- zed_nodelets/CMakeLists.txt | 1 + zed_nodelets/src/tools/include/sl_tools.h | 6 ++ zed_nodelets/src/tools/src/sl_tools.cpp | 51 ++++++++++-- .../include/zed_wrapper_nodelet.hpp | 1 + .../zed_nodelet/src/zed_wrapper_nodelet.cpp | 77 +++++++++++++------ zed_wrapper/params/common.yaml | 7 +- 7 files changed, 111 insertions(+), 34 deletions(-) diff --git a/zed-ros-interfaces b/zed-ros-interfaces index 294df84a..11f2ae36 160000 --- a/zed-ros-interfaces +++ b/zed-ros-interfaces @@ -1 +1 @@ -Subproject commit 294df84a01e7ad35d20b33cf96604c67593d38ae +Subproject commit 11f2ae36904e5469f192a05ce383a06f0468918e diff --git a/zed_nodelets/CMakeLists.txt b/zed_nodelets/CMakeLists.txt index b7ee9438..17c67fd5 100644 --- a/zed_nodelets/CMakeLists.txt +++ b/zed_nodelets/CMakeLists.txt @@ -111,6 +111,7 @@ set(LINK_LIBRARIES ${catkin_LIBRARIES} ${ZED_LIBRARIES} ${CUDA_LIBRARIES} + stdc++fs ) add_library(ZEDNodelets diff --git a/zed_nodelets/src/tools/include/sl_tools.h b/zed_nodelets/src/tools/include/sl_tools.h index e8025009..c6d86513 100644 --- a/zed_nodelets/src/tools/include/sl_tools.h +++ b/zed_nodelets/src/tools/include/sl_tools.h @@ -46,6 +46,12 @@ std::vector convertRodrigues(sl::float3 r); */ bool file_exist(const std::string& name); +/*! \brief Convert a path to absolute + * \param file_path the file path to be converted + * \return the absolute path + */ +std::string resolveFilePath(std::string file_path); + /*! \brief Get Stereolabs SDK version * \param major : major value for version * \param minor : minor value for version diff --git a/zed_nodelets/src/tools/src/sl_tools.cpp b/zed_nodelets/src/tools/src/sl_tools.cpp index 725c047e..cc3e09d0 100644 --- a/zed_nodelets/src/tools/src/sl_tools.cpp +++ b/zed_nodelets/src/tools/src/sl_tools.cpp @@ -18,15 +18,15 @@ // /////////////////////////////////////////////////////////////////////////// -#include "sl_tools.h" - +#include #include + +#include +#include // for std::experimental::filesystem::absolute #include #include -#include - -#include +#include "sl_tools.h" namespace sl_tools { @@ -135,6 +135,47 @@ bool file_exist(const std::string& name) return (stat(name.c_str(), &buffer) == 0); } +namespace fs = std::experimental::filesystem; +std::string resolveFilePath(std::string file_path) +{ + std::string abs_path = file_path; + if (file_path[0] == '~') + { + std::string home = getenv("HOME"); + file_path.erase(0, 1); + abs_path = home + file_path; + } + else if (file_path[0] == '.') + { + if (file_path[1] == '.' && file_path[2] == '/') + { + file_path.erase(0, 2); + fs::path current_path = fs::current_path(); + fs::path parent_path = current_path.parent_path(); + abs_path = parent_path.string() + file_path; + } + else if (file_path[1] == '/') + { + file_path.erase(0, 1); + fs::path current_path = fs::current_path(); + abs_path = current_path.string() + file_path; + } + else + { + std::cerr << "[sl_tools::resolveFilePath] Invalid file path '" << file_path << "' replaced with null string." + << std::endl; + return std::string(); + } + } + else if(file_path[0] != '/') + { + fs::path current_path = fs::current_path(); + abs_path = current_path.string() + "/" + file_path; + } + + return abs_path; +} + std::string getSDKVersion(int& major, int& minor, int& sub_minor) { std::string ver = sl::Camera::getSDKVersion().c_str(); 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 6459be59..dbe5bdd7 100644 --- a/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp +++ b/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp @@ -522,6 +522,7 @@ class ZEDWrapperNodelet : public nodelet::Nodelet int mZedId; int mDepthStabilization; std::string mAreaMemDbPath; + bool mSaveAreaMapOnClosing=true; std::string mSvoFilepath; std::string mRemoteStreamAddr; bool mSensTimestampSync; 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 a475b992..2ec6a1cd 100644 --- a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp @@ -18,11 +18,11 @@ // /////////////////////////////////////////////////////////////////////////// -#include "zed_wrapper_nodelet.hpp" - #include #include +#include "zed_wrapper_nodelet.hpp" + #ifndef NDEBUG #include #endif @@ -48,6 +48,12 @@ ZEDWrapperNodelet::ZEDWrapperNodelet() : Nodelet() ZEDWrapperNodelet::~ZEDWrapperNodelet() { + if (mSaveAreaMapOnClosing && mPosTrackingActivated) + { + // if(mSavePath) + // mZed.saveAreaMap() + } + if (mDevicePollThread.joinable()) { mDevicePollThread.join(); @@ -57,6 +63,8 @@ ZEDWrapperNodelet::~ZEDWrapperNodelet() { mPcThread.join(); } + + std::cerr << "ZED Nodelet destroyed" << std::endl; } void ZEDWrapperNodelet::onInit() @@ -749,6 +757,8 @@ void ZEDWrapperNodelet::readParameters() mNhNs.getParam("pos_tracking/initial_base_pose", mInitialBasePose); mNhNs.getParam("pos_tracking/area_memory_db_path", mAreaMemDbPath); + mAreaMemDbPath = sl_tools::resolveFilePath(mAreaMemDbPath); + NODELET_INFO_STREAM(" * Odometry DB path\t\t-> " << mAreaMemDbPath.c_str()); mNhNs.param("pos_tracking/area_memory", mAreaMemory, false); NODELET_INFO_STREAM(" * Spatial Memory\t\t-> " << (mAreaMemory ? "ENABLED" : "DISABLED")); @@ -1626,27 +1636,40 @@ void ZEDWrapperNodelet::start_pos_tracking() NODELET_INFO(" * Q: [%g,%g,%g,%g]", mInitialPoseSl.getOrientation().ox, mInitialPoseSl.getOrientation().oy, mInitialPoseSl.getOrientation().oz, mInitialPoseSl.getOrientation().ow); - if (mAreaMemDbPath != "" && !sl_tools::file_exist(mAreaMemDbPath)) - { - mAreaMemDbPath = ""; - NODELET_WARN("area_memory_db_path path doesn't exist or is unreachable."); - } + - // Tracking parameters - sl::PositionalTrackingParameters trackParams; + // Positional Tracking parameters + sl::PositionalTrackingParameters posTrackParams; - trackParams.area_file_path = mAreaMemDbPath.c_str(); + posTrackParams.initial_world_transform = mInitialPoseSl; + posTrackParams.enable_area_memory = mAreaMemory; mPoseSmoothing = false; // Always false. Pose Smoothing is to be enabled only for VR/AR applications - trackParams.enable_pose_smoothing = mPoseSmoothing; + posTrackParams.enable_pose_smoothing = mPoseSmoothing; + + posTrackParams.set_floor_as_origin = mFloorAlignment; + + + + if (mAreaMemDbPath != "" && !sl_tools::file_exist(mAreaMemDbPath)) + { + posTrackParams.area_file_path = ""; + NODELET_WARN_STREAM("area_memory_db_path [" << mAreaMemDbPath << "] doesn't exist or is unreachable."); + if(mSaveAreaMapOnClosing) + { + NODELET_INFO_STREAM(mAreaMemDbPath << "will be automatically created when closing the node or calling the 'save_area_map' service."); + } + } + else + { + posTrackParams.area_file_path = mAreaMemDbPath.c_str(); + } - trackParams.enable_area_memory = mAreaMemory; - trackParams.enable_imu_fusion = mImuFusion; - trackParams.initial_world_transform = mInitialPoseSl; + posTrackParams.enable_imu_fusion = mImuFusion; - trackParams.set_floor_as_origin = mFloorAlignment; + posTrackParams.set_as_static = false; - sl::ERROR_CODE err = mZed.enablePositionalTracking(trackParams); + sl::ERROR_CODE err = mZed.enablePositionalTracking(posTrackParams); if (err == sl::ERROR_CODE::SUCCESS) { @@ -1815,7 +1838,7 @@ void ZEDWrapperNodelet::publishOdomFrame(tf2::Transform odomTransf, ros::Time t) // ----> Avoid duplicated TF publishing static ros::Time last_stamp; - if( t==last_stamp ) + if (t == last_stamp) { return; } @@ -1846,7 +1869,7 @@ void ZEDWrapperNodelet::publishOdomFrame(tf2::Transform odomTransf, ros::Time t) // Publish transformation mTransformOdomBroadcaster.sendTransform(transformStamped); - //NODELET_INFO_STREAM( "Published ODOM TF with TS: " << t ); + // NODELET_INFO_STREAM( "Published ODOM TF with TS: " << t ); } void ZEDWrapperNodelet::publishPoseFrame(tf2::Transform baseTransform, ros::Time t) @@ -1854,7 +1877,7 @@ void ZEDWrapperNodelet::publishPoseFrame(tf2::Transform baseTransform, ros::Time // ----> Avoid duplicated TF publishing static ros::Time last_stamp; - if( t==last_stamp ) + if (t == last_stamp) { return; } @@ -3478,8 +3501,9 @@ void ZEDWrapperNodelet::device_poll_thread_func() std::lock_guard lock(mPosTrkMutex); // Note: ones tracking is started it is never stopped anymore to not lose tracking information - bool computeTracking = (mPosTrackingEnabled || mPosTrackingActivated || mMappingEnabled || mObjDetEnabled || (mComputeDepth & mDepthStabilization) || - poseSubnumber > 0 || poseCovSubnumber > 0 || odomSubnumber > 0 || pathSubNumber > 0); + bool computeTracking = (mPosTrackingEnabled || mPosTrackingActivated || mMappingEnabled || mObjDetEnabled || + (mComputeDepth & mDepthStabilization) || poseSubnumber > 0 || poseCovSubnumber > 0 || + odomSubnumber > 0 || pathSubNumber > 0); // Start the tracking? if ((computeTracking) && !mPosTrackingActivated && (mDepthMode != sl::DEPTH_MODE::NONE)) @@ -3505,8 +3529,9 @@ void ZEDWrapperNodelet::device_poll_thread_func() // Detect if one of the subscriber need to have the depth information mComputeDepth = mDepthMode != sl::DEPTH_MODE::NONE && - (computeTracking || ((depthSubnumber + disparitySubnumber + cloudSubnumber + fusedCloudSubnumber + poseSubnumber + - poseCovSubnumber + odomSubnumber + confMapSubnumber + objDetSubnumber) > 0)); + (computeTracking || + ((depthSubnumber + disparitySubnumber + cloudSubnumber + fusedCloudSubnumber + poseSubnumber + + poseCovSubnumber + odomSubnumber + confMapSubnumber + objDetSubnumber) > 0)); if (mComputeDepth) { @@ -3587,7 +3612,8 @@ void ZEDWrapperNodelet::device_poll_thread_func() mPosTrackingActivated = false; - computeTracking = mPosTrackingEnabled || mDepthStabilization || poseSubnumber > 0 || poseCovSubnumber > 0 || odomSubnumber > 0; + computeTracking = mPosTrackingEnabled || mDepthStabilization || poseSubnumber > 0 || poseCovSubnumber > 0 || + odomSubnumber > 0; if (computeTracking) { // Start the tracking @@ -4700,7 +4726,8 @@ void ZEDWrapperNodelet::processDetectedObjects(ros::Time t) objMsg->objects[idx].tracking_available = mObjDetTracking; objMsg->objects[idx].tracking_state = static_cast(data.tracking_state); - //NODELET_INFO_STREAM( "[" << idx << "] Tracking: " << sl::toString(static_cast(data.tracking_state))); + // NODELET_INFO_STREAM( "[" << idx << "] Tracking: " << + // sl::toString(static_cast(data.tracking_state))); objMsg->objects[idx].action_state = static_cast(data.action_state); if (data.bounding_box_2d.size() == 4) diff --git a/zed_wrapper/params/common.yaml b/zed_wrapper/params/common.yaml index 705c1943..b57b09f7 100644 --- a/zed_wrapper/params/common.yaml +++ b/zed_wrapper/params/common.yaml @@ -47,9 +47,10 @@ pos_tracking: pos_tracking_enabled: true # True to enable positional tracking from start publish_tf: true # publish `odom -> base_link` TF publish_map_tf: true # publish `map -> odom` TF - map_frame: 'map' - odometry_frame: 'odom' - area_memory_db_path: '' + map_frame: 'map' # main frame + odometry_frame: 'odom' # odometry frame + area_memory_db_path: 'zed_area_memory.area' # file loaded when the node starts to restore the "known visual features" map. + save_area_memory_db_on_exit: true # save the "known visual features" map when the node is correctly closed to the path indicated by `area_memory_db_path` area_memory: true # Enable to detect loop closure floor_alignment: false # Enable to automatically calculate camera/floor offset initial_base_pose: [0.0,0.0,0.0, 0.0,0.0,0.0] # Initial position of the `base_frame` -> [X, Y, Z, R, P, Y] From bb57f000cfe60e6cedbe8047ee5ed428e547296d Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Wed, 28 Jul 2021 12:26:24 +0200 Subject: [PATCH 063/199] New save Area Memory functions --- zed_nodelets/src/tools/src/sl_tools.cpp | 6 + .../include/zed_wrapper_nodelet.hpp | 35 +++-- .../zed_nodelet/src/zed_wrapper_nodelet.cpp | 132 +++++++++++++----- zed_wrapper/params/common.yaml | 2 +- 4 files changed, 126 insertions(+), 49 deletions(-) diff --git a/zed_nodelets/src/tools/src/sl_tools.cpp b/zed_nodelets/src/tools/src/sl_tools.cpp index cc3e09d0..4a9e3ef8 100644 --- a/zed_nodelets/src/tools/src/sl_tools.cpp +++ b/zed_nodelets/src/tools/src/sl_tools.cpp @@ -138,6 +138,12 @@ bool file_exist(const std::string& name) namespace fs = std::experimental::filesystem; std::string resolveFilePath(std::string file_path) { + if(file_path.empty()) + { + return file_path; + } + + std::string abs_path = file_path; if (file_path[0] == '~') { 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 dbe5bdd7..61e21c3e 100644 --- a/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp +++ b/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp @@ -21,10 +21,6 @@ // /////////////////////////////////////////////////////////////////////////// -#include "sl_tools.h" - -#include - #include #include #include @@ -37,6 +33,10 @@ #include #include +#include + +#include "sl_tools.h" + // Dynamic reconfiguration #include @@ -227,7 +227,7 @@ class ZEDWrapperNodelet : public nodelet::Nodelet */ void fillCamDepthInfo(sl::Camera& zed, sensor_msgs::CameraInfoPtr depth_info_msg, string frame_id); - /* \bried Check if FPS and Resolution chosen by user are correct. + /*! \brief Check if FPS and Resolution chosen by user are correct. * Modifies FPS to match correct value. */ void checkResolFps(); @@ -342,23 +342,23 @@ class ZEDWrapperNodelet : public nodelet::Nodelet */ bool getCamera2BaseTransform(); - /* \bried Start tracking + /*! \brief Start tracking */ void start_pos_tracking(); - /* \bried Start spatial mapping + /*! \brief Start spatial mapping */ bool start_3d_mapping(); - /* \bried Stop spatial mapping + /*! \brief Stop spatial mapping */ void stop_3d_mapping(); - /* \bried Start object detection + /*! \brief Start object detection */ bool start_obj_detect(); - /* \bried Stop object detection + /*! \brief Stop object detection */ void stop_obj_detect(); @@ -381,6 +381,11 @@ class ZEDWrapperNodelet : public nodelet::Nodelet */ void updateDynamicReconfigure(); + /*! \brief Save the current area map if positional tracking + * and area memory are active + */ + bool saveAreaMap(); + private: uint64_t mFrameCount = 0; @@ -522,7 +527,7 @@ class ZEDWrapperNodelet : public nodelet::Nodelet int mZedId; int mDepthStabilization; std::string mAreaMemDbPath; - bool mSaveAreaMapOnClosing=true; + bool mSaveAreaMapOnClosing = true; std::string mSvoFilepath; std::string mRemoteStreamAddr; bool mSensTimestampSync; @@ -534,9 +539,9 @@ class ZEDWrapperNodelet : public nodelet::Nodelet double mCamMaxDepth; // Positional tracking - bool mPosTrackingEnabled=false; - bool mPosTrackingActivated=false; - bool mPosTrackingReady=false; + bool mPosTrackingEnabled = false; + bool mPosTrackingActivated = false; + bool mPosTrackingReady = false; bool mTwoDMode = false; double mFixedZValue = 0.0; bool mFloorAlignment = false; @@ -610,7 +615,7 @@ class ZEDWrapperNodelet : public nodelet::Nodelet double mCamImageResizeFactor = 1.0; double mCamDepthResizeFactor = 1.0; - // flags + // flags bool mTriggerAutoExposure = true; bool mTriggerAutoWB = true; bool mComputeDepth; 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 2ec6a1cd..f3dfcfcc 100644 --- a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp @@ -48,12 +48,6 @@ ZEDWrapperNodelet::ZEDWrapperNodelet() : Nodelet() ZEDWrapperNodelet::~ZEDWrapperNodelet() { - if (mSaveAreaMapOnClosing && mPosTrackingActivated) - { - // if(mSavePath) - // mZed.saveAreaMap() - } - if (mDevicePollThread.joinable()) { mDevicePollThread.join(); @@ -758,10 +752,13 @@ void ZEDWrapperNodelet::readParameters() mNhNs.getParam("pos_tracking/area_memory_db_path", mAreaMemDbPath); mAreaMemDbPath = sl_tools::resolveFilePath(mAreaMemDbPath); - NODELET_INFO_STREAM(" * Odometry DB path\t\t-> " << mAreaMemDbPath.c_str()); + + + mNhNs.param("pos_tracking/save_area_memory_db_on_exit", mSaveAreaMapOnClosing, false); + NODELET_INFO_STREAM(" * Save Area Memory on closing\t-> " << (mSaveAreaMapOnClosing ? "ENABLED" : "DISABLED")); mNhNs.param("pos_tracking/area_memory", mAreaMemory, false); - NODELET_INFO_STREAM(" * Spatial Memory\t\t-> " << (mAreaMemory ? "ENABLED" : "DISABLED")); + NODELET_INFO_STREAM(" * Area Memory\t\t\t-> " << (mAreaMemory ? "ENABLED" : "DISABLED")); mNhNs.param("pos_tracking/imu_fusion", mImuFusion, true); NODELET_INFO_STREAM(" * IMU Fusion\t\t\t-> " << (mImuFusion ? "ENABLED" : "DISABLED")); mNhNs.param("pos_tracking/floor_alignment", mFloorAlignment, false); @@ -874,6 +871,7 @@ void ZEDWrapperNodelet::readParameters() // ----> SVO mNhNs.param("svo_file", mSvoFilepath, std::string()); + mSvoFilepath = sl_tools::resolveFilePath(mSvoFilepath); NODELET_INFO_STREAM(" * SVO input file: \t\t-> " << mSvoFilepath.c_str()); int svo_compr = 0; @@ -1636,8 +1634,6 @@ void ZEDWrapperNodelet::start_pos_tracking() NODELET_INFO(" * Q: [%g,%g,%g,%g]", mInitialPoseSl.getOrientation().ox, mInitialPoseSl.getOrientation().oy, mInitialPoseSl.getOrientation().oz, mInitialPoseSl.getOrientation().ow); - - // Positional Tracking parameters sl::PositionalTrackingParameters posTrackParams; @@ -1649,15 +1645,14 @@ void ZEDWrapperNodelet::start_pos_tracking() posTrackParams.set_floor_as_origin = mFloorAlignment; - - if (mAreaMemDbPath != "" && !sl_tools::file_exist(mAreaMemDbPath)) { posTrackParams.area_file_path = ""; - NODELET_WARN_STREAM("area_memory_db_path [" << mAreaMemDbPath << "] doesn't exist or is unreachable."); - if(mSaveAreaMapOnClosing) + NODELET_WARN_STREAM("area_memory_db_path [" << mAreaMemDbPath << "] doesn't exist or is unreachable. "); + if (mSaveAreaMapOnClosing) { - NODELET_INFO_STREAM(mAreaMemDbPath << "will be automatically created when closing the node or calling the 'save_area_map' service."); + NODELET_INFO_STREAM("The file will be automatically created when closing the node or calling the " + "'save_area_map' service if a valid Area Memory is available."); } } else @@ -1683,6 +1678,63 @@ void ZEDWrapperNodelet::start_pos_tracking() } } +bool ZEDWrapperNodelet::saveAreaMap() +{ + bool node_running = mNhNs.ok(); + if (!mZed.isOpened()) + { + if (node_running) + NODELET_WARN_STREAM("Cannot save Area Memory. The camera is closed."); + else + std::cerr << "Cannot save Area Memory. The camera is closed." << std::endl; + return false; + } + + if (mPosTrackingActivated && mAreaMemory) + { + sl::ERROR_CODE err = mZed.saveAreaMap(sl::String(mAreaMemDbPath.c_str())); + if (err != sl::ERROR_CODE::SUCCESS) + { + if (node_running) + NODELET_WARN_STREAM("Error saving positiona tracking area memory: " << sl::toString(err).c_str()); + else + std::cerr << "Error saving positiona tracking area memory: " << sl::toString(err).c_str() << std::endl; + return false; + } + + std::cerr << "Saving Area Memory file: " << mAreaMemDbPath << " "; + // NODELET_INFO_STREAM("Saving Area Memory file: " << mAreaMemDbPath); + sl::AREA_EXPORTING_STATE state; + do + { + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + state = mZed.getAreaExportState(); + if (node_running) + NODELET_INFO_STREAM("."); + else + std::cerr << "."; + } while (state == sl::AREA_EXPORTING_STATE::RUNNING); + if (!node_running) + std::cerr << std::endl; + + if (state == sl::AREA_EXPORTING_STATE::SUCCESS) + { + if (node_running) + NODELET_INFO_STREAM("Area Memory file saved correctly."); + else + std::cerr << "Area Memory file saved correctly." << std::endl; + return true; + } + + if (node_running) + NODELET_WARN_STREAM("Error saving Area Memory file: " << sl::toString(state).c_str()); + else + std::cerr << "Error saving Area Memory file: " << sl::toString(state).c_str() << std::endl; + return false; + } + return false; +} + void ZEDWrapperNodelet::publishOdom(tf2::Transform odom2baseTransf, sl::Pose& slPose, ros::Time t) { nav_msgs::OdometryPtr odomMsg = boost::make_shared(); @@ -3916,29 +3968,41 @@ void ZEDWrapperNodelet::device_poll_thread_func() // Publish the zed camera pose if someone has subscribed to if (computeTracking) { - static sl::POSITIONAL_TRACKING_STATE oldStatus; mPosTrackingStatus = mZed.getPosition(mLastZedPose, sl::REFERENCE_FRAME::WORLD); sl::Translation translation = mLastZedPose.getTranslation(); sl::Orientation quat = mLastZedPose.getOrientation(); #if 0 //#ifndef NDEBUG // Enable for TF checking - double roll, pitch, yaw; - tf2::Matrix3x3(tf2::Quaternion(quat.ox, quat.oy, quat.oz, quat.ow)).getRPY(roll, pitch, yaw); + double roll, pitch, yaw; + tf2::Matrix3x3(tf2::Quaternion(quat.ox, quat.oy, quat.oz, quat.ow)).getRPY(roll, pitch, yaw); - NODELET_DEBUG("Sensor POSE [%s -> %s] - {%.2f,%.2f,%.2f} {%.2f,%.2f,%.2f}", - mLeftCamFrameId.c_str(), mMapFrameId.c_str(), - translation.x, translation.y, translation.z, - roll * RAD2DEG, pitch * RAD2DEG, yaw * RAD2DEG); + NODELET_DEBUG("Sensor POSE [%s -> %s] - {%.2f,%.2f,%.2f} {%.2f,%.2f,%.2f}", + mLeftCamFrameId.c_str(), mMapFrameId.c_str(), + translation.x, translation.y, translation.z, + roll * RAD2DEG, pitch * RAD2DEG, yaw * RAD2DEG); - NODELET_DEBUG_STREAM("MAP -> Tracking Status: " << sl::toString(mTrackingStatus)); + NODELET_DEBUG_STREAM("MAP -> Tracking Status: " << sl::toString(mTrackingStatus)); #endif + static sl::POSITIONAL_TRACKING_STATE old_tracking_state = sl::POSITIONAL_TRACKING_STATE::OFF; if (mPosTrackingStatus == sl::POSITIONAL_TRACKING_STATE::OK || - mPosTrackingStatus == - sl::POSITIONAL_TRACKING_STATE::SEARCHING /*|| status == sl::POSITIONAL_TRACKING_STATE::FPS_TOO_LOW*/) + mPosTrackingStatus == sl::POSITIONAL_TRACKING_STATE::SEARCHING + /*|| status == sl::POSITIONAL_TRACKING_STATE::FPS_TOO_LOW*/) { + if (mPosTrackingStatus == sl::POSITIONAL_TRACKING_STATE::OK && mPosTrackingStatus != old_tracking_state) + { + NODELET_INFO_STREAM("Positional tracking -> OK [" << sl::toString(mPosTrackingStatus).c_str() << "]"); + old_tracking_state = mPosTrackingStatus; + } + if (mPosTrackingStatus == sl::POSITIONAL_TRACKING_STATE::SEARCHING && + mPosTrackingStatus != old_tracking_state) + { + NODELET_INFO_STREAM("Positional tracking -> Searching for a known position [" + << sl::toString(mPosTrackingStatus).c_str() << "]"); + old_tracking_state = mPosTrackingStatus; + } // Transform ZED pose in TF2 Transformation geometry_msgs::Transform map2sensTransf; @@ -3970,13 +4034,12 @@ void ZEDWrapperNodelet::device_poll_thread_func() } #if 0 //#ifndef NDEBUG // Enable for TF checking - double roll, pitch, yaw; - tf2::Matrix3x3(mMap2BaseTransf.getRotation()).getRPY(roll, pitch, yaw); + double roll, pitch, yaw; + tf2::Matrix3x3(mMap2BaseTransf.getRotation()).getRPY(roll, pitch, yaw); - NODELET_DEBUG("*** Base POSE [%s -> %s] - {%.3f,%.3f,%.3f} {%.3f,%.3f,%.3f}", - mMapFrameId.c_str(), mBaseFrameId.c_str(), - mMap2BaseTransf.getOrigin().x(), mMap2BaseTransf.getOrigin().y(), mMap2BaseTransf.getOrigin().z(), - roll * RAD2DEG, pitch * RAD2DEG, yaw * RAD2DEG); + NODELET_DEBUG("*** Base POSE [%s -> %s] - {%.3f,%.3f,%.3f} {%.3f,%.3f,%.3f}", mMapFrameId.c_str(), + mBaseFrameId.c_str(), mMap2BaseTransf.getOrigin().x(), mMap2BaseTransf.getOrigin().y(), + mMap2BaseTransf.getOrigin().z(), roll * RAD2DEG, pitch * RAD2DEG, yaw * RAD2DEG); #endif bool initOdom = false; @@ -4032,8 +4095,6 @@ void ZEDWrapperNodelet::device_poll_thread_func() mPosTrackingReady = true; } - - oldStatus = mPosTrackingStatus; } if (mZedRealCamModel == sl::MODEL::ZED) @@ -4157,6 +4218,11 @@ void ZEDWrapperNodelet::device_poll_thread_func() mDiagUpdater.update(); } // while loop + if (mSaveAreaMapOnClosing && mPosTrackingActivated) + { + saveAreaMap(); + } + mStopNode = true; // Stops other threads std::lock_guard lock(mCloseZedMutex); diff --git a/zed_wrapper/params/common.yaml b/zed_wrapper/params/common.yaml index b57b09f7..85ba1d65 100644 --- a/zed_wrapper/params/common.yaml +++ b/zed_wrapper/params/common.yaml @@ -50,7 +50,7 @@ pos_tracking: map_frame: 'map' # main frame odometry_frame: 'odom' # odometry frame area_memory_db_path: 'zed_area_memory.area' # file loaded when the node starts to restore the "known visual features" map. - save_area_memory_db_on_exit: true # save the "known visual features" map when the node is correctly closed to the path indicated by `area_memory_db_path` + save_area_memory_db_on_exit: false # save the "known visual features" map when the node is correctly closed to the path indicated by `area_memory_db_path` area_memory: true # Enable to detect loop closure floor_alignment: false # Enable to automatically calculate camera/floor offset initial_base_pose: [0.0,0.0,0.0, 0.0,0.0,0.0] # Initial position of the `base_frame` -> [X, Y, Z, R, P, Y] From b69fd40d402ea21a483117b4a2df25ff3a54edbf Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Wed, 28 Jul 2021 15:49:52 +0200 Subject: [PATCH 064/199] add support for `save_area_memory` service --- latest_changes.md | 8 +++ zed-ros-interfaces | 2 +- .../include/zed_wrapper_nodelet.hpp | 10 ++- .../zed_nodelet/src/zed_wrapper_nodelet.cpp | 68 +++++++++++++++---- zed_wrapper/params/common.yaml | 4 +- 5 files changed, 74 insertions(+), 18 deletions(-) diff --git a/latest_changes.md b/latest_changes.md index a2214047..9b5f847e 100644 --- a/latest_changes.md +++ b/latest_changes.md @@ -1,5 +1,13 @@ LATEST CHANGES ============== + +2021-07-28 +---------- +- New parameter `save_area_memory_db_on_exit` to force Area Memory saving when the node is closed and Area Memory is enabled and valid. +- Add service `save_Area_map` to trigger an Area Memory saving. +- New tool function to transform a relative path to absolute. + + 2021-07-26 ---------- - Enabled static IMU TF broadcasting even it `publish_tf` is set to false, making the two options independent. Thx to @bjsowa diff --git a/zed-ros-interfaces b/zed-ros-interfaces index 11f2ae36..179dd763 160000 --- a/zed-ros-interfaces +++ b/zed-ros-interfaces @@ -1 +1 @@ -Subproject commit 11f2ae36904e5469f192a05ce383a06f0468918e +Subproject commit 179dd763840f22e17d2642d6ae0c976cc83626be 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 61e21c3e..a932534e 100644 --- a/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp +++ b/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp @@ -54,6 +54,8 @@ #include #include #include +#include +#include // Topics #include @@ -322,6 +324,11 @@ class ZEDWrapperNodelet : public nodelet::Nodelet bool on_stop_object_detection(zed_interfaces::stop_object_detection::Request& req, zed_interfaces::stop_object_detection::Response& res); + /*! \brief Service callback to save_area_memory service + */ + bool on_save_area_memory(zed_interfaces::save_area_memory::Request& req, + zed_interfaces::save_area_memory::Response& res); + /*! \brief Utility to initialize the pose variables */ bool set_pose(float xt, float yt, float zt, float rr, float pr, float yr); @@ -384,7 +391,7 @@ class ZEDWrapperNodelet : public nodelet::Nodelet /*! \brief Save the current area map if positional tracking * and area memory are active */ - bool saveAreaMap(); + bool saveAreaMap(std::string file_path, std::string* out_msg=nullptr); private: uint64_t mFrameCount = 0; @@ -461,6 +468,7 @@ class ZEDWrapperNodelet : public nodelet::Nodelet ros::ServiceServer mSrvStopMapping; ros::ServiceServer mSrvStartObjDet; ros::ServiceServer mSrvStopObjDet; + ros::ServiceServer mSrvSaveAreaMemory; // ----> Topics (ONLY THOSE NOT CHANGING WHILE NODE RUNS) // Camera info 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 f3dfcfcc..7845772d 100644 --- a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp @@ -20,6 +20,7 @@ #include #include +#include #include "zed_wrapper_nodelet.hpp" @@ -622,6 +623,8 @@ void ZEDWrapperNodelet::onInit() mNhNs.advertiseService("start_object_detection", &ZEDWrapperNodelet::on_start_object_detection, this); mSrvStopObjDet = mNhNs.advertiseService("stop_object_detection", &ZEDWrapperNodelet::on_stop_object_detection, this); + mSrvSaveAreaMemory = mNhNs.advertiseService("save_area_memory", &ZEDWrapperNodelet::on_save_area_memory, this); + // Start Pointcloud thread mPcThread = std::thread(&ZEDWrapperNodelet::pointcloud_thread_func, this); @@ -754,7 +757,6 @@ void ZEDWrapperNodelet::readParameters() mAreaMemDbPath = sl_tools::resolveFilePath(mAreaMemDbPath); NODELET_INFO_STREAM(" * Odometry DB path\t\t-> " << mAreaMemDbPath.c_str()); - mNhNs.param("pos_tracking/save_area_memory_db_on_exit", mSaveAreaMapOnClosing, false); NODELET_INFO_STREAM(" * Save Area Memory on closing\t-> " << (mSaveAreaMapOnClosing ? "ENABLED" : "DISABLED")); mNhNs.param("pos_tracking/area_memory", mAreaMemory, false); @@ -1678,32 +1680,59 @@ void ZEDWrapperNodelet::start_pos_tracking() } } -bool ZEDWrapperNodelet::saveAreaMap() +bool ZEDWrapperNodelet::on_save_area_memory(zed_interfaces::save_area_memory::Request& req, + zed_interfaces::save_area_memory::Response& res) +{ + std::string file_path = sl_tools::resolveFilePath(req.area_memory_filename); + + bool ret = saveAreaMap(file_path,&res.info); + + return ret; +} + +bool ZEDWrapperNodelet::saveAreaMap(std::string file_path, std::string* out_msg) { + std::ostringstream os; + bool node_running = mNhNs.ok(); if (!mZed.isOpened()) { + os << "Cannot save Area Memory. The camera is closed."; + if (node_running) - NODELET_WARN_STREAM("Cannot save Area Memory. The camera is closed."); + NODELET_WARN_STREAM(os.str().c_str()); else - std::cerr << "Cannot save Area Memory. The camera is closed." << std::endl; + std::cerr << os.str() << std::endl; + + if (out_msg) + *out_msg = os.str(); + return false; } if (mPosTrackingActivated && mAreaMemory) { - sl::ERROR_CODE err = mZed.saveAreaMap(sl::String(mAreaMemDbPath.c_str())); + sl::ERROR_CODE err = mZed.saveAreaMap(sl::String(file_path.c_str())); if (err != sl::ERROR_CODE::SUCCESS) { + os << "Error saving positional tracking area memory: " << sl::toString(err).c_str(); + if (node_running) - NODELET_WARN_STREAM("Error saving positiona tracking area memory: " << sl::toString(err).c_str()); + NODELET_WARN_STREAM(os.str().c_str()); else - std::cerr << "Error saving positiona tracking area memory: " << sl::toString(err).c_str() << std::endl; + std::cerr << os.str() << std::endl; + + if (out_msg) + *out_msg = os.str(); + return false; } - std::cerr << "Saving Area Memory file: " << mAreaMemDbPath << " "; - // NODELET_INFO_STREAM("Saving Area Memory file: " << mAreaMemDbPath); + if (node_running) + NODELET_INFO_STREAM("Saving Area Memory file: " << file_path); + else + std::cerr << "Saving Area Memory file: " << file_path << " "; + sl::AREA_EXPORTING_STATE state; do { @@ -1719,17 +1748,28 @@ bool ZEDWrapperNodelet::saveAreaMap() if (state == sl::AREA_EXPORTING_STATE::SUCCESS) { + os << "Area Memory file saved correctly."; + if (node_running) - NODELET_INFO_STREAM("Area Memory file saved correctly."); + NODELET_INFO_STREAM(os.str().c_str()); else - std::cerr << "Area Memory file saved correctly." << std::endl; + std::cerr << os.str() << std::endl; + + if (out_msg) + *out_msg = os.str(); return true; } + os << "Error saving Area Memory file: " << sl::toString(state).c_str(); + if (node_running) - NODELET_WARN_STREAM("Error saving Area Memory file: " << sl::toString(state).c_str()); + NODELET_WARN_STREAM(os.str().c_str()); else - std::cerr << "Error saving Area Memory file: " << sl::toString(state).c_str() << std::endl; + std::cerr << os.str() << std::endl; + + if (out_msg) + *out_msg = os.str(); + return false; } return false; @@ -4220,7 +4260,7 @@ void ZEDWrapperNodelet::device_poll_thread_func() if (mSaveAreaMapOnClosing && mPosTrackingActivated) { - saveAreaMap(); + saveAreaMap(mAreaMemDbPath); } mStopNode = true; // Stops other threads diff --git a/zed_wrapper/params/common.yaml b/zed_wrapper/params/common.yaml index 85ba1d65..e1b5f95e 100644 --- a/zed_wrapper/params/common.yaml +++ b/zed_wrapper/params/common.yaml @@ -49,8 +49,8 @@ pos_tracking: publish_map_tf: true # publish `map -> odom` TF map_frame: 'map' # main frame odometry_frame: 'odom' # odometry frame - area_memory_db_path: 'zed_area_memory.area' # file loaded when the node starts to restore the "known visual features" map. - save_area_memory_db_on_exit: false # save the "known visual features" map when the node is correctly closed to the path indicated by `area_memory_db_path` + area_memory_db_path: 'zed_area_memory.area' # file loaded when the node starts to restore the "known visual features" map. + save_area_memory_db_on_exit: false # save the "known visual features" map when the node is correctly closed to the path indicated by `area_memory_db_path` area_memory: true # Enable to detect loop closure floor_alignment: false # Enable to automatically calculate camera/floor offset initial_base_pose: [0.0,0.0,0.0, 0.0,0.0,0.0] # Initial position of the `base_frame` -> [X, Y, Z, R, P, Y] From b655e70a407a2181974e634f3455553d97d11268 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Wed, 28 Jul 2021 15:53:31 +0200 Subject: [PATCH 065/199] Update zed-ros-interfaces --- zed-ros-interfaces | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/zed-ros-interfaces b/zed-ros-interfaces index 179dd763..95f47d44 160000 --- a/zed-ros-interfaces +++ b/zed-ros-interfaces @@ -1 +1 @@ -Subproject commit 179dd763840f22e17d2642d6ae0c976cc83626be +Subproject commit 95f47d4466f262bc20dadebc077df1dc4456f32d From 5e592e23f460d32ed9c6f295365628647d31f3c4 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Wed, 28 Jul 2021 15:54:38 +0200 Subject: [PATCH 066/199] Update changelog --- latest_changes.md | 1 - 1 file changed, 1 deletion(-) diff --git a/latest_changes.md b/latest_changes.md index 9b5f847e..e40edfb7 100644 --- a/latest_changes.md +++ b/latest_changes.md @@ -7,7 +7,6 @@ LATEST CHANGES - Add service `save_Area_map` to trigger an Area Memory saving. - New tool function to transform a relative path to absolute. - 2021-07-26 ---------- - Enabled static IMU TF broadcasting even it `publish_tf` is set to false, making the two options independent. Thx to @bjsowa From 5d848e5211b35e8f015daffe43f574e82641c215 Mon Sep 17 00:00:00 2001 From: Chris Iverach-Brereton Date: Tue, 17 Aug 2021 11:27:31 -0400 Subject: [PATCH 067/199] Create a macro that can be used to add multiple zed cameras to an existing URDF. Make the base-link connect to the mounting hole of the camera instead of the camera's center (makes attaching the camera to a mounting point on another URDF easier). --- zed_wrapper/urdf/zed_descr.urdf.xacro | 138 +++--------------------- zed_wrapper/urdf/zed_macro.urdf.xacro | 146 ++++++++++++++++++++++++++ 2 files changed, 162 insertions(+), 122 deletions(-) create mode 100644 zed_wrapper/urdf/zed_macro.urdf.xacro diff --git a/zed_wrapper/urdf/zed_descr.urdf.xacro b/zed_wrapper/urdf/zed_descr.urdf.xacro index 3844abfc..0ba8742e 100644 --- a/zed_wrapper/urdf/zed_descr.urdf.xacro +++ b/zed_wrapper/urdf/zed_descr.urdf.xacro @@ -19,126 +19,20 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. --> - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + diff --git a/zed_wrapper/urdf/zed_macro.urdf.xacro b/zed_wrapper/urdf/zed_macro.urdf.xacro new file mode 100644 index 00000000..ab41a527 --- /dev/null +++ b/zed_wrapper/urdf/zed_macro.urdf.xacro @@ -0,0 +1,146 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From 21b1a7fc9288ce8c328974297400d1ddd740b6d5 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Thu, 9 Sep 2021 14:59:23 +0200 Subject: [PATCH 068/199] Update question.md --- .github/ISSUE_TEMPLATE/question.md | 17 ++++++++--------- 1 file changed, 8 insertions(+), 9 deletions(-) diff --git a/.github/ISSUE_TEMPLATE/question.md b/.github/ISSUE_TEMPLATE/question.md index e66d3822..52b77960 100644 --- a/.github/ISSUE_TEMPLATE/question.md +++ b/.github/ISSUE_TEMPLATE/question.md @@ -1,10 +1,9 @@ ---- -name: Question -about: Issue a question to developers and users -title: "[Question] " -labels: question -assignees: Myzhar - ---- - +blank_issues_enabled: true +contact_links: + - name: Generic question about ZED cameras and their plugins + url: https://community.stereolabs.com/ + about: Ask a question on the Stereolabs forum + - name: Report an Hardware problem + url: mailto:support@stereolabs.com + about: Direct support contact to report hardware problems From a878797eeac128e9b9ad6072a6c428d95c75d029 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Thu, 9 Sep 2021 15:06:01 +0200 Subject: [PATCH 069/199] Update question.md --- .github/ISSUE_TEMPLATE/question.md | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/.github/ISSUE_TEMPLATE/question.md b/.github/ISSUE_TEMPLATE/question.md index 52b77960..206c8a7c 100644 --- a/.github/ISSUE_TEMPLATE/question.md +++ b/.github/ISSUE_TEMPLATE/question.md @@ -1,4 +1,5 @@ -blank_issues_enabled: true +blank_issues_enabled: false + contact_links: - name: Generic question about ZED cameras and their plugins url: https://community.stereolabs.com/ From 5e53725c802033e5a2d480d92e309bcf0c96243a Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Thu, 9 Sep 2021 15:08:01 +0200 Subject: [PATCH 070/199] Create config.yml --- .github/ISSUE_TEMPLATE/config.yml | 9 +++++++++ 1 file changed, 9 insertions(+) create mode 100644 .github/ISSUE_TEMPLATE/config.yml diff --git a/.github/ISSUE_TEMPLATE/config.yml b/.github/ISSUE_TEMPLATE/config.yml new file mode 100644 index 00000000..72ac4c3f --- /dev/null +++ b/.github/ISSUE_TEMPLATE/config.yml @@ -0,0 +1,9 @@ +blank_issues_enabled: false + +contact_links: + - name: Generic question about ZED cameras and their plugins + url: https://community.stereolabs.com/ + about: Ask a question on the Stereolabs forum + - name: Report an Hardware problem + url: mailto:support@stereolabs.com + about: Direct support contact to report hardware problems From e9655dd7b8465ff6ca121efa11b00d52caca0c73 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Thu, 9 Sep 2021 15:13:30 +0200 Subject: [PATCH 071/199] Update config.yml --- .github/ISSUE_TEMPLATE/config.yml | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/.github/ISSUE_TEMPLATE/config.yml b/.github/ISSUE_TEMPLATE/config.yml index 72ac4c3f..c92f82ab 100644 --- a/.github/ISSUE_TEMPLATE/config.yml +++ b/.github/ISSUE_TEMPLATE/config.yml @@ -5,5 +5,4 @@ contact_links: url: https://community.stereolabs.com/ about: Ask a question on the Stereolabs forum - name: Report an Hardware problem - url: mailto:support@stereolabs.com - about: Direct support contact to report hardware problems + about: Write an email to support@stereolabs.com From 8586db86c0859514b95ed6c33a6ff9e5685dab07 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Thu, 9 Sep 2021 15:15:52 +0200 Subject: [PATCH 072/199] Update config.yml --- .github/ISSUE_TEMPLATE/config.yml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/.github/ISSUE_TEMPLATE/config.yml b/.github/ISSUE_TEMPLATE/config.yml index c92f82ab..9044085d 100644 --- a/.github/ISSUE_TEMPLATE/config.yml +++ b/.github/ISSUE_TEMPLATE/config.yml @@ -5,4 +5,5 @@ contact_links: url: https://community.stereolabs.com/ about: Ask a question on the Stereolabs forum - name: Report an Hardware problem - about: Write an email to support@stereolabs.com + url: mailto:support@stereolabs.com + about: Write a direct email to the Stereolabs' support team From c0ca2862d443339cb4039d0d013daad87c9db4cb Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Thu, 9 Sep 2021 15:17:14 +0200 Subject: [PATCH 073/199] Update question.md --- .github/ISSUE_TEMPLATE/question.md | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/.github/ISSUE_TEMPLATE/question.md b/.github/ISSUE_TEMPLATE/question.md index 206c8a7c..3ee8fd9b 100644 --- a/.github/ISSUE_TEMPLATE/question.md +++ b/.github/ISSUE_TEMPLATE/question.md @@ -1,10 +1,9 @@ blank_issues_enabled: false - contact_links: - - name: Generic question about ZED cameras and their plugins - url: https://community.stereolabs.com/ - about: Ask a question on the Stereolabs forum - name: Report an Hardware problem url: mailto:support@stereolabs.com about: Direct support contact to report hardware problems + - name: Generic question about ZED cameras and their plugins + url: https://community.stereolabs.com/ + about: Ask a question on the Stereolabs forum From 8c2d9d5f1d2a1e560fe6397e5e2c9299e3363e31 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Thu, 9 Sep 2021 15:18:01 +0200 Subject: [PATCH 074/199] Delete question.md --- .github/ISSUE_TEMPLATE/question.md | 9 --------- 1 file changed, 9 deletions(-) delete mode 100644 .github/ISSUE_TEMPLATE/question.md diff --git a/.github/ISSUE_TEMPLATE/question.md b/.github/ISSUE_TEMPLATE/question.md deleted file mode 100644 index 3ee8fd9b..00000000 --- a/.github/ISSUE_TEMPLATE/question.md +++ /dev/null @@ -1,9 +0,0 @@ -blank_issues_enabled: false -contact_links: - - name: Report an Hardware problem - url: mailto:support@stereolabs.com - about: Direct support contact to report hardware problems - - name: Generic question about ZED cameras and their plugins - url: https://community.stereolabs.com/ - about: Ask a question on the Stereolabs forum - From 901589fb1cfd3eb1edf48934dbf04599b1b72707 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Thu, 9 Sep 2021 15:19:01 +0200 Subject: [PATCH 075/199] Update config.yml --- .github/ISSUE_TEMPLATE/config.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/ISSUE_TEMPLATE/config.yml b/.github/ISSUE_TEMPLATE/config.yml index 9044085d..0355b7bf 100644 --- a/.github/ISSUE_TEMPLATE/config.yml +++ b/.github/ISSUE_TEMPLATE/config.yml @@ -5,5 +5,5 @@ contact_links: url: https://community.stereolabs.com/ about: Ask a question on the Stereolabs forum - name: Report an Hardware problem - url: mailto:support@stereolabs.com + url: "mailto:support@stereolabs.com" about: Write a direct email to the Stereolabs' support team From 81a850abe7d3195cb5fa6fe58f27332b5aee10c1 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Thu, 9 Sep 2021 15:20:35 +0200 Subject: [PATCH 076/199] Update config.yml --- .github/ISSUE_TEMPLATE/config.yml | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/.github/ISSUE_TEMPLATE/config.yml b/.github/ISSUE_TEMPLATE/config.yml index 0355b7bf..01d6db14 100644 --- a/.github/ISSUE_TEMPLATE/config.yml +++ b/.github/ISSUE_TEMPLATE/config.yml @@ -1,9 +1,10 @@ blank_issues_enabled: false contact_links: - - name: Generic question about ZED cameras and their plugins - url: https://community.stereolabs.com/ - about: Ask a question on the Stereolabs forum - name: Report an Hardware problem url: "mailto:support@stereolabs.com" about: Write a direct email to the Stereolabs' support team + - name: Generic question about ZED cameras and their plugins + url: https://community.stereolabs.com/ + about: Ask a question on the Stereolabs forum + From c9325e88146c5fde5902c97679f6c7e75ef844c2 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Thu, 9 Sep 2021 15:22:34 +0200 Subject: [PATCH 077/199] Update config.yml --- .github/ISSUE_TEMPLATE/config.yml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/.github/ISSUE_TEMPLATE/config.yml b/.github/ISSUE_TEMPLATE/config.yml index 01d6db14..3c94eb6c 100644 --- a/.github/ISSUE_TEMPLATE/config.yml +++ b/.github/ISSUE_TEMPLATE/config.yml @@ -1,9 +1,9 @@ blank_issues_enabled: false contact_links: - - name: Report an Hardware problem - url: "mailto:support@stereolabs.com" - about: Write a direct email to the Stereolabs' support team + - name: Hardware problem + url: https://support.stereolabs.com/hc/en-us + about: Go to Stereolabs support center and search for a known solution or write a direct email to [support@stereolabs.com](mailto:support@stereolabs.com) - name: Generic question about ZED cameras and their plugins url: https://community.stereolabs.com/ about: Ask a question on the Stereolabs forum From 4b68ca06541cae51314af73bf0a6c2ca4b3f2369 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Thu, 9 Sep 2021 15:23:05 +0200 Subject: [PATCH 078/199] Update config.yml --- .github/ISSUE_TEMPLATE/config.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/ISSUE_TEMPLATE/config.yml b/.github/ISSUE_TEMPLATE/config.yml index 3c94eb6c..5e48905a 100644 --- a/.github/ISSUE_TEMPLATE/config.yml +++ b/.github/ISSUE_TEMPLATE/config.yml @@ -3,7 +3,7 @@ blank_issues_enabled: false contact_links: - name: Hardware problem url: https://support.stereolabs.com/hc/en-us - about: Go to Stereolabs support center and search for a known solution or write a direct email to [support@stereolabs.com](mailto:support@stereolabs.com) + about: Go to Stereolabs support center and search for a known solution or write a direct email to support@stereolabs.com - name: Generic question about ZED cameras and their plugins url: https://community.stereolabs.com/ about: Ask a question on the Stereolabs forum From e6de353f41bb7ffffb598b7c5763fdb11642c967 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Thu, 9 Sep 2021 15:36:56 +0200 Subject: [PATCH 079/199] New issue templates --- .github/ISSUE_TEMPLATE/bug_report.md | 35 ------------------- .github/ISSUE_TEMPLATE/feature_request.md | 20 ----------- .../include/zed_wrapper_nodelet.hpp | 2 +- zed_wrapper/params/common.yaml | 8 ++--- 4 files changed, 5 insertions(+), 60 deletions(-) delete mode 100644 .github/ISSUE_TEMPLATE/bug_report.md delete mode 100644 .github/ISSUE_TEMPLATE/feature_request.md diff --git a/.github/ISSUE_TEMPLATE/bug_report.md b/.github/ISSUE_TEMPLATE/bug_report.md deleted file mode 100644 index e4cbe9eb..00000000 --- a/.github/ISSUE_TEMPLATE/bug_report.md +++ /dev/null @@ -1,35 +0,0 @@ ---- -name: Bug report -about: Create a report to help us improve -title: "[BUG]" -labels: bug -assignees: Myzhar - ---- - -**Describe the bug** -A clear and concise description of what the bug is. - -**Device information:** - - OS: [e.g. Ubuntu] - - OS version: [e.g. 18.04] - - CPU: [e.g. i7, Jeston Nano, Jetson TX2] - - GPU Nvidia: [e.g. GTX1080, RTX1070] - - ZED SDK Version: [e.g. v3.1.2, v2.8.5] - - ROS Wrapper version: [e.g. v2.8.5, latest master] - -**To Reproduce** -Steps to reproduce the behavior: -1. Go to '...' -2. Click on '....' -3. Scroll down to '....' -4. See error - -**Expected behavior** -A clear and concise description of what you expected to happen. - -**Screenshots** -If applicable, add screenshots to help explain your problem. - -**Additional context** -Add any other context about the problem here. diff --git a/.github/ISSUE_TEMPLATE/feature_request.md b/.github/ISSUE_TEMPLATE/feature_request.md deleted file mode 100644 index b24084f9..00000000 --- a/.github/ISSUE_TEMPLATE/feature_request.md +++ /dev/null @@ -1,20 +0,0 @@ ---- -name: Feature request -about: Suggest an idea for this project -title: "[Feature]" -labels: Feature request -assignees: Myzhar - ---- - -**Is your feature request related to a problem? Please describe.** -A clear and concise description of what the problem is. Ex. I'm always frustrated when [...] - -**Describe the solution you'd like** -A clear and concise description of what you want to happen. - -**Describe alternatives you've considered** -A clear and concise description of any alternative solutions or features you've considered. - -**Additional context** -Add any other context or screenshots about the feature request here. 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 c2b79cb5..7de3c277 100644 --- a/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp +++ b/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp @@ -414,7 +414,7 @@ class ZEDWrapperNodelet : public nodelet::Nodelet bool mStopNode = false; - const double mSensPubRate = 400.0; + const double mSensPubRate = 400.0; // Maximum ODR for ZED2/ZED2i. You can change this to 800 for ZED-M, but it's not recommended // Publishers image_transport::CameraPublisher mPubRgb; // diff --git a/zed_wrapper/params/common.yaml b/zed_wrapper/params/common.yaml index e1b5f95e..b2d670aa 100644 --- a/zed_wrapper/params/common.yaml +++ b/zed_wrapper/params/common.yaml @@ -16,15 +16,15 @@ auto_whitebalance: true # Dynamic whitebalance_temperature: 42 # Dynamic - works only if `auto_whitebalance` is false depth_confidence: 50 # Dynamic depth_texture_conf: 100 # Dynamic -pub_frame_rate: 15.0 # Dynamic - frequency of publishing of video and depth data -point_cloud_freq: 15.0 # Dynamic - frequency of the pointcloud publishing (equal or less to `grab_frame_rate` value) +pub_frame_rate: 100.0 # Dynamic - frequency of publishing of video and depth data +point_cloud_freq: 100.0 # Dynamic - frequency of the pointcloud publishing (equal or less to `grab_frame_rate` value) general: camera_name: zed # A name for the camera (can be different from camera model and node name and can be overwritten by the launch file) zed_id: 0 serial_number: 0 - resolution: 2 # '0': HD2K, '1': HD1080, '2': HD720, '3': VGA - grab_frame_rate: 30 # Frequency of frame grabbing for internal SDK operations + resolution: 3 # '0': HD2K, '1': HD1080, '2': HD720, '3': VGA + grab_frame_rate: 100 # Frequency of frame grabbing for internal SDK operations gpu_id: -1 base_frame: 'base_link' # must be equal to the frame_id used in the URDF file verbose: false # Enable info message by the ZED SDK From fb26c137cda02a5183b6721c86e10ad1391a313e Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Thu, 9 Sep 2021 15:37:40 +0200 Subject: [PATCH 080/199] add missing files --- .github/ISSUE_TEMPLATE/1_feature_request.md | 23 ++++++ .github/ISSUE_TEMPLATE/2_bug_report.yml | 79 +++++++++++++++++++++ 2 files changed, 102 insertions(+) create mode 100644 .github/ISSUE_TEMPLATE/1_feature_request.md create mode 100644 .github/ISSUE_TEMPLATE/2_bug_report.yml diff --git a/.github/ISSUE_TEMPLATE/1_feature_request.md b/.github/ISSUE_TEMPLATE/1_feature_request.md new file mode 100644 index 00000000..696434e2 --- /dev/null +++ b/.github/ISSUE_TEMPLATE/1_feature_request.md @@ -0,0 +1,23 @@ +name: Feature request 🧭 +description: Suggest an idea for this project +labels: "needs-discussion,feature-request" +body: +- type: textarea + attributes: + label: Proposal + description: "What would you like to have as a feature" + placeholder: "A clear and concise description of what you want to happen." + validations: + required: true +- type: textarea + attributes: + label: Use-Case + description: "How would this help you?" + placeholder: "Tell us more what you'd like to achieve." + validations: + required: false +- type: textarea + id: anything-else + attributes: + label: Anything else? + description: "Let us know if you have anything else to share" diff --git a/.github/ISSUE_TEMPLATE/2_bug_report.yml b/.github/ISSUE_TEMPLATE/2_bug_report.yml new file mode 100644 index 00000000..d7cdc175 --- /dev/null +++ b/.github/ISSUE_TEMPLATE/2_bug_report.yml @@ -0,0 +1,79 @@ +name: Report a bug 🐛 +description: Create a report to help us improve +labels: "bug" +body: +- type: textarea + attributes: + label: Report + description: "What bug have you encountered?" + placeholder: "A clear and concise description of what the bug is." + validations: + required: true +- type: dropdown + attributes: + label: ZED Camera model + description: What model of ZED camera are you using? + options: + - "ZED" + - "ZED Mini" + - "ZED2" + - "ZED2i" + validations: + required: true +- type: textarea + attributes: + label: Operating System + description: "What operating system are you using?" + placeholder: "e.g. Ubuntu 20.04" + validations: + required: true +- type: textarea + attributes: + label: GPU model + description: "What GPU card are you using?" + placeholder: "e.g. Nvidia Jetson Xavier NX" + validations: + required: true +- type: textarea + attributes: + label: ZED SDK Version + description: "What ZED SDK version are you using?" + placeholder: "e.g. v3.5.3" + validations: + required: true +- type: textarea + attributes: + label: ROS Version + description: "What ROS version are you using?" + placeholder: "e.g. Melodic" + validations: + required: true +- type: textarea + attributes: + label: Expected Behavior + description: What did you expect to happen? + placeholder: What did you expect to happen? + validations: + required: true +- type: textarea + attributes: + label: Actual Behavior + description: Also tell us, what did you see is happen? + placeholder: Tell us what you see that is happening + validations: + required: true +- type: textarea + attributes: + label: Steps to Reproduce the Problem + description: "How can we reproduce this bug? Please walk us through it step by step." + value: | + 1. + 2. + 3. + validations: + required: true +- type: textarea + id: anything-else + attributes: + label: Anything else? + description: "Let us know if you have anything else to share" From 9f2e3476a7bf5a29d2e1cd0f02c5af9dc15b8880 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Thu, 9 Sep 2021 15:40:25 +0200 Subject: [PATCH 081/199] rename file to correct extension --- .github/ISSUE_TEMPLATE/1_feature_request.md | 23 --------------------- 1 file changed, 23 deletions(-) delete mode 100644 .github/ISSUE_TEMPLATE/1_feature_request.md diff --git a/.github/ISSUE_TEMPLATE/1_feature_request.md b/.github/ISSUE_TEMPLATE/1_feature_request.md deleted file mode 100644 index 696434e2..00000000 --- a/.github/ISSUE_TEMPLATE/1_feature_request.md +++ /dev/null @@ -1,23 +0,0 @@ -name: Feature request 🧭 -description: Suggest an idea for this project -labels: "needs-discussion,feature-request" -body: -- type: textarea - attributes: - label: Proposal - description: "What would you like to have as a feature" - placeholder: "A clear and concise description of what you want to happen." - validations: - required: true -- type: textarea - attributes: - label: Use-Case - description: "How would this help you?" - placeholder: "Tell us more what you'd like to achieve." - validations: - required: false -- type: textarea - id: anything-else - attributes: - label: Anything else? - description: "Let us know if you have anything else to share" From 01e0c4fb6a3af00c9fb75291d476efb0326934b5 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Thu, 9 Sep 2021 15:40:35 +0200 Subject: [PATCH 082/199] rename file to correct extension --- .github/ISSUE_TEMPLATE/1_feature_request.yml | 23 ++++++++++++++++++++ 1 file changed, 23 insertions(+) create mode 100644 .github/ISSUE_TEMPLATE/1_feature_request.yml diff --git a/.github/ISSUE_TEMPLATE/1_feature_request.yml b/.github/ISSUE_TEMPLATE/1_feature_request.yml new file mode 100644 index 00000000..696434e2 --- /dev/null +++ b/.github/ISSUE_TEMPLATE/1_feature_request.yml @@ -0,0 +1,23 @@ +name: Feature request 🧭 +description: Suggest an idea for this project +labels: "needs-discussion,feature-request" +body: +- type: textarea + attributes: + label: Proposal + description: "What would you like to have as a feature" + placeholder: "A clear and concise description of what you want to happen." + validations: + required: true +- type: textarea + attributes: + label: Use-Case + description: "How would this help you?" + placeholder: "Tell us more what you'd like to achieve." + validations: + required: false +- type: textarea + id: anything-else + attributes: + label: Anything else? + description: "Let us know if you have anything else to share" From 2eeaa0dbd47c648d0cdb200b831c456ce8957cf3 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Thu, 9 Sep 2021 15:41:28 +0200 Subject: [PATCH 083/199] Update 1_feature_request.yml --- .github/ISSUE_TEMPLATE/1_feature_request.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/ISSUE_TEMPLATE/1_feature_request.yml b/.github/ISSUE_TEMPLATE/1_feature_request.yml index 696434e2..c25c789d 100644 --- a/.github/ISSUE_TEMPLATE/1_feature_request.yml +++ b/.github/ISSUE_TEMPLATE/1_feature_request.yml @@ -1,6 +1,6 @@ name: Feature request 🧭 description: Suggest an idea for this project -labels: "needs-discussion,feature-request" +labels: "Feature request" body: - type: textarea attributes: From d20208eb5f2336e8dbefdb20c3942ad49e2cb88c Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Thu, 9 Sep 2021 17:36:53 +0200 Subject: [PATCH 084/199] New issue templates --- .github/ISSUE_TEMPLATE/1_feature_request.yml | 22 ++- .github/ISSUE_TEMPLATE/2_bug_report.yml | 162 ++++++++++--------- .github/ISSUE_TEMPLATE/config.yml | 14 +- 3 files changed, 114 insertions(+), 84 deletions(-) diff --git a/.github/ISSUE_TEMPLATE/1_feature_request.yml b/.github/ISSUE_TEMPLATE/1_feature_request.yml index c25c789d..83e360b0 100644 --- a/.github/ISSUE_TEMPLATE/1_feature_request.yml +++ b/.github/ISSUE_TEMPLATE/1_feature_request.yml @@ -2,10 +2,30 @@ name: Feature request 🧭 description: Suggest an idea for this project labels: "Feature request" body: + - type: markdown + attributes: + value: | + # Welcome 👋 + + Thanks for taking the time to fill out this feature request module. + Please fill out each section below. This info allows Stereolabs developers to correctly evaluate your request. + + Useful Links: + - Documentation: https://www.stereolabs.com/docs/ + - Stereolabs support: https://support.stereolabs.com/hc/en-us/ + - type: checkboxes + attributes: + label: Preliminary Checks + description: Please make sure that you verify each checkbox and follow the instructions for them. + options: + - label: "This issue is not a duplicate. Before opening a new issue, please search existing issues. + required: true + - label: "This issue is not a question, bug report, or anything other than a feature request directly related to this project." + required: true - type: textarea attributes: label: Proposal - description: "What would you like to have as a feature" + description: "What would you like to have as a new feature?" placeholder: "A clear and concise description of what you want to happen." validations: required: true diff --git a/.github/ISSUE_TEMPLATE/2_bug_report.yml b/.github/ISSUE_TEMPLATE/2_bug_report.yml index d7cdc175..36287d82 100644 --- a/.github/ISSUE_TEMPLATE/2_bug_report.yml +++ b/.github/ISSUE_TEMPLATE/2_bug_report.yml @@ -1,79 +1,87 @@ -name: Report a bug 🐛 -description: Create a report to help us improve +name: Bug Report 🐛 +description: Something isn't working as expected? Report your bugs here. labels: "bug" body: -- type: textarea - attributes: - label: Report - description: "What bug have you encountered?" - placeholder: "A clear and concise description of what the bug is." - validations: - required: true -- type: dropdown - attributes: - label: ZED Camera model - description: What model of ZED camera are you using? - options: - - "ZED" - - "ZED Mini" - - "ZED2" - - "ZED2i" - validations: - required: true -- type: textarea - attributes: - label: Operating System - description: "What operating system are you using?" - placeholder: "e.g. Ubuntu 20.04" - validations: - required: true -- type: textarea - attributes: - label: GPU model - description: "What GPU card are you using?" - placeholder: "e.g. Nvidia Jetson Xavier NX" - validations: - required: true -- type: textarea - attributes: - label: ZED SDK Version - description: "What ZED SDK version are you using?" - placeholder: "e.g. v3.5.3" - validations: - required: true -- type: textarea - attributes: - label: ROS Version - description: "What ROS version are you using?" - placeholder: "e.g. Melodic" - validations: - required: true -- type: textarea - attributes: - label: Expected Behavior - description: What did you expect to happen? - placeholder: What did you expect to happen? - validations: - required: true -- type: textarea - attributes: - label: Actual Behavior - description: Also tell us, what did you see is happen? - placeholder: Tell us what you see that is happening - validations: - required: true -- type: textarea - attributes: - label: Steps to Reproduce the Problem - description: "How can we reproduce this bug? Please walk us through it step by step." - value: | - 1. - 2. - 3. - validations: - required: true -- type: textarea - id: anything-else - attributes: - label: Anything else? - description: "Let us know if you have anything else to share" + - type: markdown + attributes: + value: | + # Welcome 👋 + + Thanks for taking the time to fill out this bug report. + Please fill out each section below. This info allows Stereolabs developers to diagnose (and fix!) your issue as quickly as possible. Otherwise we might need to close the issue without e.g. clear reproduction steps. + + Bug reports also shoulnd't be used for generic questions, please use the [Stereolabs Community forum](https://community.stereolabs.com/) instead. + + Useful Links: + - Documentation: https://www.stereolabs.com/docs/ + - Stereolabs support: https://support.stereolabs.com/hc/en-us/ + - type: checkboxes + attributes: + label: Preliminary Checks + description: Please make sure that you verify each checkbox and follow the instructions for them. + options: + - label: "This issue is not a duplicate. Before opening a new issue, please search existing issues. + required: true + - label: "This issue is not a question, feature request, or anything other than a bug report directly related to this project." + required: true + - type: textarea + attributes: + label: Description + description: Describe the issue that you're seeing. + placeholder: Be as precise as you can. Feel free to share screenshots, videos, or data. The more information you provide the easier will be to provide you with a fast solution. + validations: + required: true + - type: textarea + attributes: + label: Steps to Reproduce + description: Clear steps describing how to reproduce the issue. + value: | + 1. + 2. + 3. + ... + validations: + required: true + - type: textarea + attributes: + label: Expected Result + description: Describe what you expected to happen. + validations: + required: true + - type: textarea + attributes: + label: Actual Result + description: Describe what actually happened. + validations: + required: true + - type: dropdown + attributes: + label: ZED Camera model + description: What model of ZED camera are you using? + options: + - "ZED" + - "ZED Mini" + - "ZED2" + - "ZED2i" + validations: + required: true + - type: textarea + attributes: + label: Environment + render: shell + description: Useful information about your system. + placeholder: | + OS: Operating System + CPU: e.g. ARM + GPU: Nvidia Jetson Xavier NX + ZED SDK version: e.g. v3.5.3 + Other info: e.g. ROS Melodic + validations: + required: true + - type: textarea + attributes: + label: Anything else? + description: Please add any other information or comment that you think may be useful for solving the problem + placeholder: + validations: + required: false diff --git a/.github/ISSUE_TEMPLATE/config.yml b/.github/ISSUE_TEMPLATE/config.yml index 5e48905a..2b5cafee 100644 --- a/.github/ISSUE_TEMPLATE/config.yml +++ b/.github/ISSUE_TEMPLATE/config.yml @@ -1,10 +1,12 @@ blank_issues_enabled: false - contact_links: - - name: Hardware problem - url: https://support.stereolabs.com/hc/en-us - about: Go to Stereolabs support center and search for a known solution or write a direct email to support@stereolabs.com - - name: Generic question about ZED cameras and their plugins + - name: Online Documentation + url: https://www.stereolabs.com/docs/ + about: Check out the Stereolabs documentation for answers to common questions + - name: Stereolabs Community url: https://community.stereolabs.com/ - about: Ask a question on the Stereolabs forum + about: Ask questions, request features & discuss with other users and developers + - name: Stereolabs Twitter + url: https://twitter.com/Stereolabs3D + about: The official Twitter account to ask questions and get help and share your projects with us From 42dc9a0a48173d0fe31df70475d879bc1df72c21 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Thu, 9 Sep 2021 17:39:26 +0200 Subject: [PATCH 085/199] Update 1_feature_request.yml --- .github/ISSUE_TEMPLATE/1_feature_request.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/ISSUE_TEMPLATE/1_feature_request.yml b/.github/ISSUE_TEMPLATE/1_feature_request.yml index 83e360b0..f464b985 100644 --- a/.github/ISSUE_TEMPLATE/1_feature_request.yml +++ b/.github/ISSUE_TEMPLATE/1_feature_request.yml @@ -18,7 +18,7 @@ body: label: Preliminary Checks description: Please make sure that you verify each checkbox and follow the instructions for them. options: - - label: "This issue is not a duplicate. Before opening a new issue, please search existing issues. + - label: "This issue is not a duplicate. Before opening a new issue, please search existing issues." required: true - label: "This issue is not a question, bug report, or anything other than a feature request directly related to this project." required: true From 6df1e0dd88e248f79db5fc36d4ebd884bfa3b6ef Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Thu, 9 Sep 2021 17:41:14 +0200 Subject: [PATCH 086/199] Update 2_bug_report.yml --- .github/ISSUE_TEMPLATE/2_bug_report.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/ISSUE_TEMPLATE/2_bug_report.yml b/.github/ISSUE_TEMPLATE/2_bug_report.yml index 36287d82..5402edc4 100644 --- a/.github/ISSUE_TEMPLATE/2_bug_report.yml +++ b/.github/ISSUE_TEMPLATE/2_bug_report.yml @@ -20,7 +20,7 @@ body: label: Preliminary Checks description: Please make sure that you verify each checkbox and follow the instructions for them. options: - - label: "This issue is not a duplicate. Before opening a new issue, please search existing issues. + - label: "This issue is not a duplicate. Before opening a new issue, please search existing issues." required: true - label: "This issue is not a question, feature request, or anything other than a bug report directly related to this project." required: true From 1cacd1eb7f0a9d1cb5192d834e129eec5ec105e8 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Thu, 9 Sep 2021 17:41:52 +0200 Subject: [PATCH 087/199] Update 2_bug_report.yml --- .github/ISSUE_TEMPLATE/2_bug_report.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/ISSUE_TEMPLATE/2_bug_report.yml b/.github/ISSUE_TEMPLATE/2_bug_report.yml index 5402edc4..30ac21e9 100644 --- a/.github/ISSUE_TEMPLATE/2_bug_report.yml +++ b/.github/ISSUE_TEMPLATE/2_bug_report.yml @@ -75,7 +75,7 @@ body: CPU: e.g. ARM GPU: Nvidia Jetson Xavier NX ZED SDK version: e.g. v3.5.3 - Other info: e.g. ROS Melodic + Other info: e.g. ROS Melodic validations: required: true - type: textarea From e74bd0d4b3db36c931045d068892b5fb35fb0923 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Thu, 9 Sep 2021 17:44:36 +0200 Subject: [PATCH 088/199] Update 1_feature_request.yml --- .github/ISSUE_TEMPLATE/1_feature_request.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/ISSUE_TEMPLATE/1_feature_request.yml b/.github/ISSUE_TEMPLATE/1_feature_request.yml index f464b985..9e0cefaa 100644 --- a/.github/ISSUE_TEMPLATE/1_feature_request.yml +++ b/.github/ISSUE_TEMPLATE/1_feature_request.yml @@ -1,6 +1,6 @@ name: Feature request 🧭 description: Suggest an idea for this project -labels: "Feature request" +labels: "feature request" body: - type: markdown attributes: From 7da8b4ca64f9acd4663b6c67cd932c35e689a2f0 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Thu, 9 Sep 2021 17:47:49 +0200 Subject: [PATCH 089/199] Update 1_feature_request.yml --- .github/ISSUE_TEMPLATE/1_feature_request.yml | 38 ++++++++++---------- 1 file changed, 19 insertions(+), 19 deletions(-) diff --git a/.github/ISSUE_TEMPLATE/1_feature_request.yml b/.github/ISSUE_TEMPLATE/1_feature_request.yml index 9e0cefaa..89994270 100644 --- a/.github/ISSUE_TEMPLATE/1_feature_request.yml +++ b/.github/ISSUE_TEMPLATE/1_feature_request.yml @@ -22,22 +22,22 @@ body: required: true - label: "This issue is not a question, bug report, or anything other than a feature request directly related to this project." required: true -- type: textarea - attributes: - label: Proposal - description: "What would you like to have as a new feature?" - placeholder: "A clear and concise description of what you want to happen." - validations: - required: true -- type: textarea - attributes: - label: Use-Case - description: "How would this help you?" - placeholder: "Tell us more what you'd like to achieve." - validations: - required: false -- type: textarea - id: anything-else - attributes: - label: Anything else? - description: "Let us know if you have anything else to share" + - type: textarea + attributes: + label: Proposal + description: "What would you like to have as a new feature?" + placeholder: "A clear and concise description of what you want to happen." + validations: + required: true + - type: textarea + attributes: + label: Use-Case + description: "How would this help you?" + placeholder: "Tell us more what you'd like to achieve." + validations: + required: false + - type: textarea + id: anything-else + attributes: + label: Anything else? + description: "Let us know if you have anything else to share" From 900d898d64bb685b806d2c258e5426b265855855 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Thu, 9 Sep 2021 17:54:47 +0200 Subject: [PATCH 090/199] Update config.yml --- .github/ISSUE_TEMPLATE/config.yml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/.github/ISSUE_TEMPLATE/config.yml b/.github/ISSUE_TEMPLATE/config.yml index 2b5cafee..bf092435 100644 --- a/.github/ISSUE_TEMPLATE/config.yml +++ b/.github/ISSUE_TEMPLATE/config.yml @@ -2,11 +2,11 @@ blank_issues_enabled: false contact_links: - name: Online Documentation url: https://www.stereolabs.com/docs/ - about: Check out the Stereolabs documentation for answers to common questions + about: Check out the Stereolabs documentation for answers to common questions. - name: Stereolabs Community url: https://community.stereolabs.com/ - about: Ask questions, request features & discuss with other users and developers + about: Ask questions, request features & discuss with other users and developers. - name: Stereolabs Twitter url: https://twitter.com/Stereolabs3D - about: The official Twitter account to ask questions and get help and share your projects with us + about: The official Stereolabs Twitter account to ask questions, comment our products and share your projects with the ZED community. From 9f195801b09476e0e7912fce21a9ac3202e2c76a Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Thu, 9 Sep 2021 17:55:16 +0200 Subject: [PATCH 091/199] Update 1_feature_request.yml --- .github/ISSUE_TEMPLATE/1_feature_request.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/ISSUE_TEMPLATE/1_feature_request.yml b/.github/ISSUE_TEMPLATE/1_feature_request.yml index 89994270..69a987cc 100644 --- a/.github/ISSUE_TEMPLATE/1_feature_request.yml +++ b/.github/ISSUE_TEMPLATE/1_feature_request.yml @@ -1,5 +1,5 @@ name: Feature request 🧭 -description: Suggest an idea for this project +description: Suggest an idea for this project. labels: "feature request" body: - type: markdown From 8bd81e69656d222fd02db99a502f05368d773aea Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Tue, 14 Sep 2021 10:20:43 +0200 Subject: [PATCH 092/199] Create stale.yml --- .github/stale.yml | 61 +++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 61 insertions(+) create mode 100644 .github/stale.yml diff --git a/.github/stale.yml b/.github/stale.yml new file mode 100644 index 00000000..69f375e1 --- /dev/null +++ b/.github/stale.yml @@ -0,0 +1,61 @@ +# Configuration for probot-stale - https://github.com/probot/stale + +# Number of days of inactivity before an Issue or Pull Request becomes stale +daysUntilStale: 30 + +# Number of days of inactivity before an Issue or Pull Request with the stale label is closed. +# Set to false to disable. If disabled, issues still need to be closed manually, but will remain marked as stale. +daysUntilClose: 3 + +# Only issues or pull requests with all of these labels are check if stale. Defaults to `[]` (disabled) +onlyLabels: [] + +# Issues or Pull Requests with these labels will never be considered stale. Set to `[]` to disable +exemptLabels: + - pinned + +# Set to true to ignore issues in a project (defaults to false) +exemptProjects: false + +# Set to true to ignore issues in a milestone (defaults to false) +exemptMilestones: false + +# Set to true to ignore issues with an assignee (defaults to false) +exemptAssignees: false + +# Label to use when marking as stale +staleLabel: closed_for_stale + +# Comment to post when marking as stale. Set to `false` to disable +markComment: > + This issue has been automatically marked as stale because it has not had + recent activity. It will be closed in 72h if no further activity occurs. Thank you + for your contributions. + +# Comment to post when removing the stale label. +# unmarkComment: > +# Your comment here. + +# Comment to post when closing a stale Issue or Pull Request. +closeComment: > + This issue has been closed automatically because it has not had + recent activity. Please feel free to re-open in case new info + is available or further job is required. + +# Limit the number of actions per hour, from 1-30. Default is 30 +limitPerRun: 100 + +# Limit to only `issues` or `pulls` +only: issues + +# Optionally, specify configuration settings that are specific to just 'issues' or 'pulls': +# pulls: +# daysUntilStale: 30 +# markComment: > +# This pull request has been automatically marked as stale because it has not had +# recent activity. It will be closed if no further activity occurs. Thank you +# for your contributions. + +# issues: +# exemptLabels: +# - confirmed From 4126564fccb0d64bacd5820537271c19c104192e Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Tue, 14 Sep 2021 10:22:47 +0200 Subject: [PATCH 093/199] Update stale.yml --- .github/stale.yml | 1 + 1 file changed, 1 insertion(+) diff --git a/.github/stale.yml b/.github/stale.yml index 69f375e1..f16dd5ab 100644 --- a/.github/stale.yml +++ b/.github/stale.yml @@ -13,6 +13,7 @@ onlyLabels: [] # Issues or Pull Requests with these labels will never be considered stale. Set to `[]` to disable exemptLabels: - pinned + - feature_request # Set to true to ignore issues in a project (defaults to false) exemptProjects: false From bc033a2d79e1a06b7c1dd61e24319241b279871a Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Tue, 14 Sep 2021 10:54:27 +0200 Subject: [PATCH 094/199] Update stale.yml --- .github/stale.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/stale.yml b/.github/stale.yml index f16dd5ab..1f20f59a 100644 --- a/.github/stale.yml +++ b/.github/stale.yml @@ -1,7 +1,7 @@ # Configuration for probot-stale - https://github.com/probot/stale # Number of days of inactivity before an Issue or Pull Request becomes stale -daysUntilStale: 30 +daysUntilStale: 28 # Number of days of inactivity before an Issue or Pull Request with the stale label is closed. # Set to false to disable. If disabled, issues still need to be closed manually, but will remain marked as stale. From be91bcbf75108cf2a3c606e727f8ca8c1f6b62d7 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Tue, 14 Sep 2021 15:40:43 +0200 Subject: [PATCH 095/199] Update 1_feature_request.yml --- .github/ISSUE_TEMPLATE/1_feature_request.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/ISSUE_TEMPLATE/1_feature_request.yml b/.github/ISSUE_TEMPLATE/1_feature_request.yml index 69a987cc..7b161ecd 100644 --- a/.github/ISSUE_TEMPLATE/1_feature_request.yml +++ b/.github/ISSUE_TEMPLATE/1_feature_request.yml @@ -1,6 +1,6 @@ name: Feature request 🧭 description: Suggest an idea for this project. -labels: "feature request" +labels: "feature_request" body: - type: markdown attributes: From f88c1dd2a3ae106f89e038adb49de3bee87fbdb1 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Thu, 7 Oct 2021 17:28:17 +0200 Subject: [PATCH 096/199] Update config.yml --- .github/ISSUE_TEMPLATE/config.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/ISSUE_TEMPLATE/config.yml b/.github/ISSUE_TEMPLATE/config.yml index bf092435..5b74d2eb 100644 --- a/.github/ISSUE_TEMPLATE/config.yml +++ b/.github/ISSUE_TEMPLATE/config.yml @@ -3,9 +3,9 @@ contact_links: - name: Online Documentation url: https://www.stereolabs.com/docs/ about: Check out the Stereolabs documentation for answers to common questions. - - name: Stereolabs Community + - name: Stereolabs Community [Ask a question] url: https://community.stereolabs.com/ - about: Ask questions, request features & discuss with other users and developers. + about: Ask questions & discuss with other users and Stereolabs developers. - name: Stereolabs Twitter url: https://twitter.com/Stereolabs3D about: The official Stereolabs Twitter account to ask questions, comment our products and share your projects with the ZED community. From 2b949a779b6238ba5ac4d58edf8b0354ff6c3186 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Mon, 22 Nov 2021 10:27:02 +0100 Subject: [PATCH 097/199] Fix `fluid_pressure` measure units --- zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 68688df4..7901fb52 100644 --- a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp @@ -3247,7 +3247,7 @@ void ZEDWrapperNodelet::publishSensData(ros::Time t) old_ts = pressMsg->header.stamp; #endif pressMsg->header.frame_id = mBaroFrameId; - pressMsg->fluid_pressure = sens_data.barometer.pressure * 1e-2; // Pascal + pressMsg->fluid_pressure = sens_data.barometer.pressure * 1e2; // Pascal pressMsg->variance = 1.0585e-2; sensors_data_published = true; From a064324fdce71dfe1253561fca93f95d93499ce6 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Wed, 24 Nov 2021 14:15:16 +0100 Subject: [PATCH 098/199] Fix syntax errors in xacro file and improve URDF --- zed_wrapper/urdf/zed_descr.urdf.xacro | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/zed_wrapper/urdf/zed_descr.urdf.xacro b/zed_wrapper/urdf/zed_descr.urdf.xacro index 0ba8742e..d57d8ca6 100644 --- a/zed_wrapper/urdf/zed_descr.urdf.xacro +++ b/zed_wrapper/urdf/zed_descr.urdf.xacro @@ -30,9 +30,9 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - - + + - + From 038fac5b199b41bc9db5a5cb4accf680e5d099cd Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Wed, 24 Nov 2021 14:15:37 +0100 Subject: [PATCH 099/199] Improve URDF link positions --- zed_wrapper/urdf/zed_macro.urdf.xacro | 20 ++++++++++++++++---- 1 file changed, 16 insertions(+), 4 deletions(-) diff --git a/zed_wrapper/urdf/zed_macro.urdf.xacro b/zed_wrapper/urdf/zed_macro.urdf.xacro index ab41a527..81e4d188 100644 --- a/zed_wrapper/urdf/zed_macro.urdf.xacro +++ b/zed_wrapper/urdf/zed_macro.urdf.xacro @@ -33,18 +33,30 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + + + + + + + + + + + + @@ -61,7 +73,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - + @@ -76,7 +88,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - + @@ -90,7 +102,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - + @@ -106,7 +118,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - + From 6fd61f9c39f31cca2638934aec4ff59b07692f30 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Wed, 24 Nov 2021 14:24:10 +0100 Subject: [PATCH 100/199] Update latest_changes.md --- latest_changes.md | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/latest_changes.md b/latest_changes.md index e40edfb7..b0396535 100644 --- a/latest_changes.md +++ b/latest_changes.md @@ -1,6 +1,12 @@ LATEST CHANGES ============== +2021-11-24 +---------- +- Add new `_base_link` frame on the base of the camera to easily handle camera positioning on robots. Thx @civerachb-cpr +- Improve URDF by adding 3° slope for ZED and ZED2, X-offset for optical frames to correctly match the CMOS sensors position on the PCB, X-offset for mounting screw on ZED2i +- Add `zed_macro.urdf.xacro` to be included by other `xacro` file to easily integrate ZED cameras in the robot descriptions. See [PR #771](https://github.com/stereolabs/zed-ros-wrapper/pull/771) for details. Thx @civerachb-cpr + 2021-07-28 ---------- - New parameter `save_area_memory_db_on_exit` to force Area Memory saving when the node is closed and Area Memory is enabled and valid. From 5bd1400a52bb803667e467ff6afdd3c8bee48293 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Fri, 3 Dec 2021 15:42:28 +0100 Subject: [PATCH 101/199] Update README.md --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 277e894c..1394700a 100644 --- a/README.md +++ b/README.md @@ -57,7 +57,7 @@ Open a terminal, clone the repository, update the dependencies and build the pac $ catkin_make -DCMAKE_BUILD_TYPE=Release $ source ./devel/setup.bash -#### Update the repository +#### Update the local repository To update the repository to the latest release you must use the following command to retrieve the latest commits of `zed-ros-wrapper` and of all the submodules: From ab88ea16e29d135971aea4c90a600fbce5456691 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Fri, 3 Dec 2021 15:53:22 +0100 Subject: [PATCH 102/199] Fix wrong ZED, ZED2, and ZED2i height value --- zed_wrapper/urdf/zed_macro.urdf.xacro | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/zed_wrapper/urdf/zed_macro.urdf.xacro b/zed_wrapper/urdf/zed_macro.urdf.xacro index 81e4d188..8d07dbd2 100644 --- a/zed_wrapper/urdf/zed_macro.urdf.xacro +++ b/zed_wrapper/urdf/zed_macro.urdf.xacro @@ -32,7 +32,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - + @@ -46,14 +46,14 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - + - + From dcd9d972e62c2fd2bd4d3114d70194934fad364c Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Fri, 3 Dec 2021 15:54:51 +0100 Subject: [PATCH 103/199] Update latest_changes.md --- latest_changes.md | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/latest_changes.md b/latest_changes.md index b0396535..4cf3678f 100644 --- a/latest_changes.md +++ b/latest_changes.md @@ -1,6 +1,10 @@ LATEST CHANGES ============== +2021-12-03 +---------- +- Fixed wrong ZED, ZED2, and ZED2i height value. See issue [#797](https://github.com/stereolabs/zed-ros-wrapper/issues/797). Thx @zulfiz + 2021-11-24 ---------- - Add new `_base_link` frame on the base of the camera to easily handle camera positioning on robots. Thx @civerachb-cpr From 1bfdbab194cf19512ac5eb6837b0ee41a2ca8ff8 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Thu, 24 Feb 2022 11:30:09 +0100 Subject: [PATCH 104/199] SDK v3.7 support --- latest_changes.md | 8 + zed-ros-interfaces | 2 +- .../include/zed_wrapper_nodelet.hpp | 1 + .../zed_nodelet/src/zed_wrapper_nodelet.cpp | 7735 ++++++++--------- zed_wrapper/params/common.yaml | 12 +- zed_wrapper/params/zed2.yaml | 19 +- zed_wrapper/params/zed2i.yaml | 19 +- zed_wrapper/params/zedm.yaml | 15 + 8 files changed, 3635 insertions(+), 4176 deletions(-) diff --git a/latest_changes.md b/latest_changes.md index b0396535..50491add 100644 --- a/latest_changes.md +++ b/latest_changes.md @@ -1,6 +1,14 @@ LATEST CHANGES ============== +v3.7.x +--------- +- Add support for the new Neural Depth mode +- Add support for head detection model +- Add support for sport-related object class +- Add support for X_MEDIUM neural network models +- Enable AI for ZED Mini + 2021-11-24 ---------- - Add new `_base_link` frame on the base of the camera to easily handle camera positioning on robots. Thx @civerachb-cpr diff --git a/zed-ros-interfaces b/zed-ros-interfaces index 95f47d44..92bbf60f 160000 --- a/zed-ros-interfaces +++ b/zed-ros-interfaces @@ -1 +1 @@ -Subproject commit 95f47d4466f262bc20dadebc077df1dc4456f32d +Subproject commit 92bbf60f2cab850f1b0147f3eaa2213c07e58c5c 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 7de3c277..bd6f3170 100644 --- a/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp +++ b/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp @@ -717,6 +717,7 @@ class ZEDWrapperNodelet : public nodelet::Nodelet bool mObjDetAnimalsEnable = true; bool mObjDetElectronicsEnable = true; bool mObjDetFruitsEnable = true; + bool mObjDetSportsEnable = true; sl::DETECTION_MODEL mObjDetModel = sl::DETECTION_MODEL::MULTI_CLASS_BOX; 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 7901fb52..17e11b46 100644 --- a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp @@ -35,8 +35,7 @@ //#define DEBUG_SENS_TS 1 -namespace zed_nodelets -{ +namespace zed_nodelets { #ifndef DEG2RAD #define DEG2RAD 0.017453293 #define RAD2DEG 57.295777937 @@ -45,3897 +44,3459 @@ namespace zed_nodelets #define MAG_FREQ 50. #define BARO_FREQ 25. -ZEDWrapperNodelet::ZEDWrapperNodelet() : Nodelet() +ZEDWrapperNodelet::ZEDWrapperNodelet() + : Nodelet() { } ZEDWrapperNodelet::~ZEDWrapperNodelet() { - if (mDevicePollThread.joinable()) - { - mDevicePollThread.join(); - } + if (mDevicePollThread.joinable()) { + mDevicePollThread.join(); + } - if (mPcThread.joinable()) - { - mPcThread.join(); - } + if (mPcThread.joinable()) { + mPcThread.join(); + } - std::cerr << "ZED Nodelet destroyed" << std::endl; + std::cerr << "ZED Nodelet destroyed" << std::endl; } void ZEDWrapperNodelet::onInit() { - // Node handlers - mNh = getMTNodeHandle(); - mNhNs = getMTPrivateNodeHandle(); + // Node handlers + mNh = getMTNodeHandle(); + mNhNs = getMTPrivateNodeHandle(); - mStopNode = false; - mPcDataReady = false; - mRgbDepthDataRetrieved = true; + mStopNode = false; + mPcDataReady = false; + mRgbDepthDataRetrieved = true; #ifndef NDEBUG - if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug)) - { - ros::console::notifyLoggerLevelsChanged(); - } + if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug)) { + ros::console::notifyLoggerLevelsChanged(); + } #endif - NODELET_INFO("********** Starting nodelet '%s' **********", getName().c_str()); - - std::string ver = sl_tools::getSDKVersion(mVerMajor, mVerMinor, mVerSubMinor); - NODELET_INFO_STREAM("SDK version : " << ver); - - if (mVerMajor < 3) - { - NODELET_ERROR("This version of the ZED ROS Wrapper is designed to be used with ZED SDK v3.x"); - ros::shutdown(); - raise(SIGINT); - raise(SIGABRT); - exit(-1); - } - - readParameters(); - - initTransforms(); - - // Set the video topic names - std::string rgbTopicRoot = "rgb"; - std::string rightTopicRoot = "right"; - std::string leftTopicRoot = "left"; - std::string stereoTopicRoot = "stereo"; - std::string img_topic = "/image_rect_color"; - std::string img_raw_topic = "/image_raw_color"; - std::string img_gray_topic = "/image_rect_gray"; - std::string img_raw_gray_topic_ = "/image_raw_gray"; - std::string raw_suffix = "_raw"; - string left_topic = leftTopicRoot + img_topic; - string left_raw_topic = leftTopicRoot + raw_suffix + img_raw_topic; - string right_topic = rightTopicRoot + img_topic; - string right_raw_topic = rightTopicRoot + raw_suffix + img_raw_topic; - string rgb_topic = rgbTopicRoot + img_topic; - string rgb_raw_topic = rgbTopicRoot + raw_suffix + img_raw_topic; - string stereo_topic = stereoTopicRoot + img_topic; - string stereo_raw_topic = stereoTopicRoot + raw_suffix + img_raw_topic; - string left_gray_topic = leftTopicRoot + img_gray_topic; - string left_raw_gray_topic = leftTopicRoot + raw_suffix + img_raw_gray_topic_; - string right_gray_topic = rightTopicRoot + img_gray_topic; - string right_raw_gray_topic = rightTopicRoot + raw_suffix + img_raw_gray_topic_; - string rgb_gray_topic = rgbTopicRoot + img_gray_topic; - string rgb_raw_gray_topic = rgbTopicRoot + raw_suffix + img_raw_gray_topic_; - - // Set the disparity topic name - std::string disparityTopic = "disparity/disparity_image"; - - // Set the depth topic names - string depth_topic_root = "depth"; - - if (mOpenniDepthMode) - { - NODELET_INFO_STREAM("Openni depth mode activated -> Units: mm, Encoding: TYPE_16UC1"); - } - depth_topic_root += "/depth_registered"; - - string pointcloud_topic = "point_cloud/cloud_registered"; - - string pointcloud_fused_topic = "mapping/fused_cloud"; - - string object_det_topic_root = "obj_det"; - string object_det_topic = object_det_topic_root + "/objects"; - - std::string confImgRoot = "confidence"; - string conf_map_topic_name = "confidence_map"; - string conf_map_topic = confImgRoot + "/" + conf_map_topic_name; - - // Set the positional tracking topic names - std::string poseTopic = "pose"; - string pose_cov_topic; - pose_cov_topic = poseTopic + "_with_covariance"; - - std::string odometryTopic = "odom"; - string odom_path_topic = "path_odom"; - string map_path_topic = "path_map"; - - // Create camera info - mRgbCamInfoMsg.reset(new sensor_msgs::CameraInfo()); - mLeftCamInfoMsg.reset(new sensor_msgs::CameraInfo()); - mRightCamInfoMsg.reset(new sensor_msgs::CameraInfo()); - mRgbCamInfoRawMsg.reset(new sensor_msgs::CameraInfo()); - mLeftCamInfoRawMsg.reset(new sensor_msgs::CameraInfo()); - mRightCamInfoRawMsg.reset(new sensor_msgs::CameraInfo()); - mDepthCamInfoMsg.reset(new sensor_msgs::CameraInfo()); - - // Initialization transformation listener - mTfBuffer.reset(new tf2_ros::Buffer); - mTfListener.reset(new tf2_ros::TransformListener(*mTfBuffer)); - - // Try to initialize the ZED - if (!mSvoFilepath.empty() || !mRemoteStreamAddr.empty()) - { - if (!mSvoFilepath.empty()) - { - mZedParams.input.setFromSVOFile(mSvoFilepath.c_str()); - mZedParams.svo_real_time_mode = true; - - // TODO ADD PARAMETER FOR SVO REAL TIME - } - else if (!mRemoteStreamAddr.empty()) - { - std::vector configStream = sl_tools::split_string(mRemoteStreamAddr, ':'); - sl::String ip = sl::String(configStream.at(0).c_str()); - - if (configStream.size() == 2) - { - mZedParams.input.setFromStream(ip, atoi(configStream.at(1).c_str())); - } - else - { - mZedParams.input.setFromStream(ip); - } - } - - mSvoMode = true; - } - else - { - mZedParams.camera_fps = mCamFrameRate; - mZedParams.camera_resolution = static_cast(mCamResol); - - if (mZedSerialNumber == 0) - { - mZedParams.input.setFromCameraID(mZedId); - } - else - { - bool waiting_for_camera = true; - - while (waiting_for_camera) - { - // Ctrl+C check - if (!mNhNs.ok()) - { - mStopNode = true; // Stops other threads - - std::lock_guard lock(mCloseZedMutex); - NODELET_DEBUG("Closing ZED"); - mZed.close(); - - NODELET_DEBUG("ZED pool thread finished"); - return; - } + NODELET_INFO("********** Starting nodelet '%s' **********", getName().c_str()); + + std::string ver = sl_tools::getSDKVersion(mVerMajor, mVerMinor, mVerSubMinor); + NODELET_INFO_STREAM("SDK version : " << ver); + + if (mVerMajor < 3) { + NODELET_ERROR("This version of the ZED ROS Wrapper is designed to be used with ZED SDK v3.x"); + ros::shutdown(); + raise(SIGINT); + raise(SIGABRT); + exit(-1); + } + + readParameters(); + + initTransforms(); + + // Set the video topic names + std::string rgbTopicRoot = "rgb"; + std::string rightTopicRoot = "right"; + std::string leftTopicRoot = "left"; + std::string stereoTopicRoot = "stereo"; + std::string img_topic = "/image_rect_color"; + std::string img_raw_topic = "/image_raw_color"; + std::string img_gray_topic = "/image_rect_gray"; + std::string img_raw_gray_topic_ = "/image_raw_gray"; + std::string raw_suffix = "_raw"; + string left_topic = leftTopicRoot + img_topic; + string left_raw_topic = leftTopicRoot + raw_suffix + img_raw_topic; + string right_topic = rightTopicRoot + img_topic; + string right_raw_topic = rightTopicRoot + raw_suffix + img_raw_topic; + string rgb_topic = rgbTopicRoot + img_topic; + string rgb_raw_topic = rgbTopicRoot + raw_suffix + img_raw_topic; + string stereo_topic = stereoTopicRoot + img_topic; + string stereo_raw_topic = stereoTopicRoot + raw_suffix + img_raw_topic; + string left_gray_topic = leftTopicRoot + img_gray_topic; + string left_raw_gray_topic = leftTopicRoot + raw_suffix + img_raw_gray_topic_; + string right_gray_topic = rightTopicRoot + img_gray_topic; + string right_raw_gray_topic = rightTopicRoot + raw_suffix + img_raw_gray_topic_; + string rgb_gray_topic = rgbTopicRoot + img_gray_topic; + string rgb_raw_gray_topic = rgbTopicRoot + raw_suffix + img_raw_gray_topic_; + + // Set the disparity topic name + std::string disparityTopic = "disparity/disparity_image"; + + // Set the depth topic names + string depth_topic_root = "depth"; + + if (mOpenniDepthMode) { + NODELET_INFO_STREAM("Openni depth mode activated -> Units: mm, Encoding: TYPE_16UC1"); + } + depth_topic_root += "/depth_registered"; + + string pointcloud_topic = "point_cloud/cloud_registered"; - sl::DeviceProperties prop = sl_tools::getZEDFromSN(mZedSerialNumber); + string pointcloud_fused_topic = "mapping/fused_cloud"; - if (prop.id < -1 || prop.camera_state == sl::CAMERA_STATE::NOT_AVAILABLE) - { - std::string msg = "ZED SN" + to_string(mZedSerialNumber) + " not detected ! Please connect this ZED"; - NODELET_INFO_STREAM(msg.c_str()); - std::this_thread::sleep_for(std::chrono::milliseconds(2000)); + string object_det_topic_root = "obj_det"; + string object_det_topic = object_det_topic_root + "/objects"; + + std::string confImgRoot = "confidence"; + string conf_map_topic_name = "confidence_map"; + string conf_map_topic = confImgRoot + "/" + conf_map_topic_name; + + // Set the positional tracking topic names + std::string poseTopic = "pose"; + string pose_cov_topic; + pose_cov_topic = poseTopic + "_with_covariance"; + + std::string odometryTopic = "odom"; + string odom_path_topic = "path_odom"; + string map_path_topic = "path_map"; + + // Create camera info + mRgbCamInfoMsg.reset(new sensor_msgs::CameraInfo()); + mLeftCamInfoMsg.reset(new sensor_msgs::CameraInfo()); + mRightCamInfoMsg.reset(new sensor_msgs::CameraInfo()); + mRgbCamInfoRawMsg.reset(new sensor_msgs::CameraInfo()); + mLeftCamInfoRawMsg.reset(new sensor_msgs::CameraInfo()); + mRightCamInfoRawMsg.reset(new sensor_msgs::CameraInfo()); + mDepthCamInfoMsg.reset(new sensor_msgs::CameraInfo()); + + // Initialization transformation listener + mTfBuffer.reset(new tf2_ros::Buffer); + mTfListener.reset(new tf2_ros::TransformListener(*mTfBuffer)); + + // Try to initialize the ZED + if (!mSvoFilepath.empty() || !mRemoteStreamAddr.empty()) { + if (!mSvoFilepath.empty()) { + mZedParams.input.setFromSVOFile(mSvoFilepath.c_str()); + mZedParams.svo_real_time_mode = true; + + // TODO ADD PARAMETER FOR SVO REAL TIME + } else if (!mRemoteStreamAddr.empty()) { + std::vector configStream = sl_tools::split_string(mRemoteStreamAddr, ':'); + sl::String ip = sl::String(configStream.at(0).c_str()); + + if (configStream.size() == 2) { + mZedParams.input.setFromStream(ip, atoi(configStream.at(1).c_str())); + } else { + mZedParams.input.setFromStream(ip); + } } - else - { - waiting_for_camera = false; - mZedParams.input.setFromCameraID(prop.id); + + mSvoMode = true; + } else { + mZedParams.camera_fps = mCamFrameRate; + mZedParams.camera_resolution = static_cast(mCamResol); + + if (mZedSerialNumber == 0) { + mZedParams.input.setFromCameraID(mZedId); + } else { + bool waiting_for_camera = true; + + while (waiting_for_camera) { + // Ctrl+C check + if (!mNhNs.ok()) { + mStopNode = true; // Stops other threads + + std::lock_guard lock(mCloseZedMutex); + NODELET_DEBUG("Closing ZED"); + mZed.close(); + + NODELET_DEBUG("ZED pool thread finished"); + return; + } + + sl::DeviceProperties prop = sl_tools::getZEDFromSN(mZedSerialNumber); + + if (prop.id < -1 || prop.camera_state == sl::CAMERA_STATE::NOT_AVAILABLE) { + std::string msg = "ZED SN" + to_string(mZedSerialNumber) + " not detected ! Please connect this ZED"; + NODELET_INFO_STREAM(msg.c_str()); + std::this_thread::sleep_for(std::chrono::milliseconds(2000)); + } else { + waiting_for_camera = false; + mZedParams.input.setFromCameraID(prop.id); + } + } } - } } - } - mZedParams.coordinate_system = sl::COORDINATE_SYSTEM::RIGHT_HANDED_Z_UP_X_FWD; - NODELET_INFO_STREAM(" * Camera coordinate system\t-> " << sl::toString(mZedParams.coordinate_system)); + mZedParams.coordinate_system = sl::COORDINATE_SYSTEM::RIGHT_HANDED_Z_UP_X_FWD; + NODELET_INFO_STREAM(" * Camera coordinate system\t-> " << sl::toString(mZedParams.coordinate_system)); - mZedParams.coordinate_units = sl::UNIT::METER; - mZedParams.depth_mode = static_cast(mDepthMode); - mZedParams.sdk_verbose = mVerbose; - mZedParams.sdk_gpu_id = mGpuId; - mZedParams.depth_stabilization = mDepthStabilization; - mZedParams.camera_image_flip = mCameraFlip; - mZedParams.depth_minimum_distance = static_cast(mCamMinDepth); - mZedParams.depth_maximum_distance = static_cast(mCamMaxDepth); - mZedParams.camera_disable_self_calib = !mCameraSelfCalib; + mZedParams.coordinate_units = sl::UNIT::METER; + mZedParams.depth_mode = static_cast(mDepthMode); + mZedParams.sdk_verbose = mVerbose; + mZedParams.sdk_gpu_id = mGpuId; + mZedParams.depth_stabilization = mDepthStabilization; + mZedParams.camera_image_flip = mCameraFlip; + mZedParams.depth_minimum_distance = static_cast(mCamMinDepth); + mZedParams.depth_maximum_distance = static_cast(mCamMaxDepth); + mZedParams.camera_disable_self_calib = !mCameraSelfCalib; - mZedParams.enable_image_enhancement = true; // Always active + mZedParams.enable_image_enhancement = true; // Always active - mDiagUpdater.add("ZED Diagnostic", this, &ZEDWrapperNodelet::callback_updateDiagnostic); - mDiagUpdater.setHardwareID("ZED camera"); + mDiagUpdater.add("ZED Diagnostic", this, &ZEDWrapperNodelet::callback_updateDiagnostic); + mDiagUpdater.setHardwareID("ZED camera"); - mConnStatus = sl::ERROR_CODE::CAMERA_NOT_DETECTED; + mConnStatus = sl::ERROR_CODE::CAMERA_NOT_DETECTED; - NODELET_INFO_STREAM(" *** Opening " << sl::toString(mZedUserCamModel) << "..."); - while (mConnStatus != sl::ERROR_CODE::SUCCESS) - { - mConnStatus = mZed.open(mZedParams); - NODELET_INFO_STREAM("ZED connection -> " << sl::toString(mConnStatus)); - std::this_thread::sleep_for(std::chrono::milliseconds(2000)); + NODELET_INFO_STREAM(" *** Opening " << sl::toString(mZedUserCamModel) << "..."); + while (mConnStatus != sl::ERROR_CODE::SUCCESS) { + mConnStatus = mZed.open(mZedParams); + NODELET_INFO_STREAM("ZED connection -> " << sl::toString(mConnStatus)); + std::this_thread::sleep_for(std::chrono::milliseconds(2000)); - if (!mNhNs.ok()) - { - mStopNode = true; // Stops other threads + if (!mNhNs.ok()) { + mStopNode = true; // Stops other threads - std::lock_guard lock(mCloseZedMutex); - NODELET_DEBUG("Closing ZED"); - mZed.close(); + std::lock_guard lock(mCloseZedMutex); + NODELET_DEBUG("Closing ZED"); + mZed.close(); - NODELET_DEBUG("ZED pool thread finished"); - return; - } + NODELET_DEBUG("ZED pool thread finished"); + return; + } - mDiagUpdater.update(); - } - NODELET_INFO_STREAM(" ... " << sl::toString(mZedRealCamModel) << " ready"); + mDiagUpdater.update(); + } + NODELET_INFO_STREAM(" ... " << sl::toString(mZedRealCamModel) << " ready"); - // CUdevice devid; - cuCtxGetDevice(&mGpuId); + // CUdevice devid; + cuCtxGetDevice(&mGpuId); - NODELET_INFO_STREAM("ZED SDK running on GPU #" << mGpuId); + NODELET_INFO_STREAM("ZED SDK running on GPU #" << mGpuId); - // Disable AEC_AGC and Auto Whitebalance to trigger it if use set to automatic - mZed.setCameraSettings(sl::VIDEO_SETTINGS::AEC_AGC, 0); - mZed.setCameraSettings(sl::VIDEO_SETTINGS::WHITEBALANCE_AUTO, 0); + // Disable AEC_AGC and Auto Whitebalance to trigger it if use set to automatic + mZed.setCameraSettings(sl::VIDEO_SETTINGS::AEC_AGC, 0); + mZed.setCameraSettings(sl::VIDEO_SETTINGS::WHITEBALANCE_AUTO, 0); - mZedRealCamModel = mZed.getCameraInformation().camera_model; + mZedRealCamModel = mZed.getCameraInformation().camera_model; - if (mZedRealCamModel == sl::MODEL::ZED) - { - if (mZedUserCamModel != mZedRealCamModel) - { - NODELET_WARN("Camera model does not match user parameter. Please modify " - "the value of the parameter 'camera_model' to 'zed'"); - } - } - else if (mZedRealCamModel == sl::MODEL::ZED_M) - { - if (mZedUserCamModel != mZedRealCamModel) - { - NODELET_WARN("Camera model does not match user parameter. Please modify " - "the value of the parameter 'camera_model' to 'zedm'"); - } + if (mZedRealCamModel == sl::MODEL::ZED) { + if (mZedUserCamModel != mZedRealCamModel) { + NODELET_WARN("Camera model does not match user parameter. Please modify " + "the value of the parameter 'camera_model' to 'zed'"); + } + } else if (mZedRealCamModel == sl::MODEL::ZED_M) { + if (mZedUserCamModel != mZedRealCamModel) { + NODELET_WARN("Camera model does not match user parameter. Please modify " + "the value of the parameter 'camera_model' to 'zedm'"); + } #if ZED_SDK_MAJOR_VERSION == 3 && ZED_SDK_MINOR_VERSION < 1 - mSlCamImuTransf = mZed.getCameraInformation().camera_imu_transform; + mSlCamImuTransf = mZed.getCameraInformation().camera_imu_transform; #else - mSlCamImuTransf = mZed.getCameraInformation().sensors_configuration.camera_imu_transform; + mSlCamImuTransf = mZed.getCameraInformation().sensors_configuration.camera_imu_transform; #endif - NODELET_INFO("Camera-IMU Transform: \n %s", mSlCamImuTransf.getInfos().c_str()); - } - else if (mZedRealCamModel == sl::MODEL::ZED2) - { - if (mZedUserCamModel != mZedRealCamModel) - { - NODELET_WARN("Camera model does not match user parameter. Please modify " - "the value of the parameter 'camera_model' to 'zed2'"); - } + NODELET_INFO("Camera-IMU Transform: \n %s", mSlCamImuTransf.getInfos().c_str()); + } else if (mZedRealCamModel == sl::MODEL::ZED2) { + if (mZedUserCamModel != mZedRealCamModel) { + NODELET_WARN("Camera model does not match user parameter. Please modify " + "the value of the parameter 'camera_model' to 'zed2'"); + } #if ZED_SDK_MAJOR_VERSION == 3 && ZED_SDK_MINOR_VERSION < 1 - mSlCamImuTransf = mZed.getCameraInformation().camera_imu_transform; + mSlCamImuTransf = mZed.getCameraInformation().camera_imu_transform; #else - mSlCamImuTransf = mZed.getCameraInformation().sensors_configuration.camera_imu_transform; + mSlCamImuTransf = mZed.getCameraInformation().sensors_configuration.camera_imu_transform; #endif - NODELET_INFO("Camera-IMU Transform: \n %s", mSlCamImuTransf.getInfos().c_str()); - } - else if (mZedRealCamModel == sl::MODEL::ZED2i) - { - if (mZedUserCamModel != mZedRealCamModel) - { - NODELET_WARN("Camera model does not match user parameter. Please modify " - "the value of the parameter 'camera_model' to 'zed2i'"); - } + NODELET_INFO("Camera-IMU Transform: \n %s", mSlCamImuTransf.getInfos().c_str()); + } else if (mZedRealCamModel == sl::MODEL::ZED2i) { + if (mZedUserCamModel != mZedRealCamModel) { + NODELET_WARN("Camera model does not match user parameter. Please modify " + "the value of the parameter 'camera_model' to 'zed2i'"); + } #if ZED_SDK_MAJOR_VERSION == 3 && ZED_SDK_MINOR_VERSION < 1 - mSlCamImuTransf = mZed.getCameraInformation().camera_imu_transform; + mSlCamImuTransf = mZed.getCameraInformation().camera_imu_transform; #else - mSlCamImuTransf = mZed.getCameraInformation().sensors_configuration.camera_imu_transform; + mSlCamImuTransf = mZed.getCameraInformation().sensors_configuration.camera_imu_transform; #endif - NODELET_INFO("Camera-IMU Transform: \n %s", mSlCamImuTransf.getInfos().c_str()); - } + NODELET_INFO("Camera-IMU Transform: \n %s", mSlCamImuTransf.getInfos().c_str()); + } - NODELET_INFO_STREAM(" * CAMERA MODEL\t -> " << sl::toString(mZedRealCamModel).c_str()); - mZedSerialNumber = mZed.getCameraInformation().serial_number; - NODELET_INFO_STREAM(" * Serial Number -> " << mZedSerialNumber); + NODELET_INFO_STREAM(" * CAMERA MODEL\t -> " << sl::toString(mZedRealCamModel).c_str()); + mZedSerialNumber = mZed.getCameraInformation().serial_number; + NODELET_INFO_STREAM(" * Serial Number -> " << mZedSerialNumber); - if (!mSvoMode) - { + if (!mSvoMode) { #if ZED_SDK_MAJOR_VERSION == 3 && ZED_SDK_MINOR_VERSION < 1 - mCamFwVersion = mZed.getCameraInformation().camera_firmware_version; + mCamFwVersion = mZed.getCameraInformation().camera_firmware_version; #else - mCamFwVersion = mZed.getCameraInformation().camera_configuration.firmware_version; + mCamFwVersion = mZed.getCameraInformation().camera_configuration.firmware_version; #endif - NODELET_INFO_STREAM(" * Camera FW Version -> " << mCamFwVersion); - if (mZedRealCamModel != sl::MODEL::ZED) - { + NODELET_INFO_STREAM(" * Camera FW Version -> " << mCamFwVersion); + if (mZedRealCamModel != sl::MODEL::ZED) { #if ZED_SDK_MAJOR_VERSION == 3 && ZED_SDK_MINOR_VERSION < 1 - mSensFwVersion = mZed.getCameraInformation().sensors_firmware_version; + mSensFwVersion = mZed.getCameraInformation().sensors_firmware_version; #else - mSensFwVersion = mZed.getCameraInformation().sensors_configuration.firmware_version; + mSensFwVersion = mZed.getCameraInformation().sensors_configuration.firmware_version; #endif - NODELET_INFO_STREAM(" * Sensors FW Version -> " << mSensFwVersion); - } - } - else - { - NODELET_INFO_STREAM(" * Input type\t -> " << sl::toString(mZed.getCameraInformation().input_type).c_str()); - } - - // Set the IMU topic names using real camera model - string imu_topic; - string imu_topic_raw; - string imu_temp_topic; - string imu_mag_topic; - // string imu_mag_topic_raw; - string pressure_topic; - string temp_topic_root = "temperature"; - string temp_topic_left = temp_topic_root + "/left"; - string temp_topic_right = temp_topic_root + "/right"; - - if (mZedRealCamModel != sl::MODEL::ZED) - { - std::string imuTopicRoot = "imu"; - string imu_topic_name = "data"; - string imu_topic_raw_name = "data_raw"; - string imu_topic_mag_name = "mag"; - // string imu_topic_mag_raw_name = "mag_raw"; - string pressure_topic_name = "atm_press"; - imu_topic = imuTopicRoot + "/" + imu_topic_name; - imu_topic_raw = imuTopicRoot + "/" + imu_topic_raw_name; - imu_temp_topic = temp_topic_root + "/" + imuTopicRoot; - imu_mag_topic = imuTopicRoot + "/" + imu_topic_mag_name; - // imu_mag_topic_raw = imuTopicRoot + "/" + imu_topic_mag_raw_name; - pressure_topic = /*imuTopicRoot + "/" +*/ pressure_topic_name; - } - - mDiagUpdater.setHardwareIDf("%s - s/n: %d [GPU #%d]", sl::toString(mZedRealCamModel).c_str(), mZedSerialNumber, - mGpuId); - - // ----> Dynamic Reconfigure parameters - mDynRecServer = boost::make_shared>(mDynServerMutex); - dynamic_reconfigure::Server::CallbackType f; - f = boost::bind(&ZEDWrapperNodelet::callback_dynamicReconf, this, _1, _2); - mDynRecServer->setCallback(f); - // Update parameters - zed_nodelets::ZedConfig config; - mDynRecServer->getConfigDefault(config); - mDynServerMutex.lock(); - mDynRecServer->updateConfig(config); - mDynServerMutex.unlock(); - - updateDynamicReconfigure(); - // <---- Dynamic Reconfigure parameters - - // Create all the publishers - // Image publishers - image_transport::ImageTransport it_zed(mNhNs); - - mPubRgb = it_zed.advertiseCamera(rgb_topic, 1); // rgb - NODELET_INFO_STREAM("Advertised on topic " << mPubRgb.getTopic()); - NODELET_INFO_STREAM("Advertised on topic " << mPubRgb.getInfoTopic()); - mPubRawRgb = it_zed.advertiseCamera(rgb_raw_topic, 1); // rgb raw - NODELET_INFO_STREAM("Advertised on topic " << mPubRawRgb.getTopic()); - NODELET_INFO_STREAM("Advertised on topic " << mPubRawRgb.getInfoTopic()); - mPubLeft = it_zed.advertiseCamera(left_topic, 1); // left - NODELET_INFO_STREAM("Advertised on topic " << mPubLeft.getTopic()); - NODELET_INFO_STREAM("Advertised on topic " << mPubLeft.getInfoTopic()); - mPubRawLeft = it_zed.advertiseCamera(left_raw_topic, 1); // left raw - NODELET_INFO_STREAM("Advertised on topic " << mPubRawLeft.getTopic()); - NODELET_INFO_STREAM("Advertised on topic " << mPubRawLeft.getInfoTopic()); - mPubRight = it_zed.advertiseCamera(right_topic, 1); // right - NODELET_INFO_STREAM("Advertised on topic " << mPubRight.getTopic()); - NODELET_INFO_STREAM("Advertised on topic " << mPubRight.getInfoTopic()); - mPubRawRight = it_zed.advertiseCamera(right_raw_topic, 1); // right raw - NODELET_INFO_STREAM("Advertised on topic " << mPubRawRight.getTopic()); - NODELET_INFO_STREAM("Advertised on topic " << mPubRawRight.getInfoTopic()); - - mPubRgbGray = it_zed.advertiseCamera(rgb_gray_topic, 1); // rgb - NODELET_INFO_STREAM("Advertised on topic " << mPubRgbGray.getTopic()); - NODELET_INFO_STREAM("Advertised on topic " << mPubRgbGray.getInfoTopic()); - mPubRawRgbGray = it_zed.advertiseCamera(rgb_raw_gray_topic, 1); // rgb raw - NODELET_INFO_STREAM("Advertised on topic " << mPubRawRgbGray.getTopic()); - NODELET_INFO_STREAM("Advertised on topic " << mPubRawRgbGray.getInfoTopic()); - mPubLeftGray = it_zed.advertiseCamera(left_gray_topic, 1); // left - NODELET_INFO_STREAM("Advertised on topic " << mPubLeftGray.getTopic()); - NODELET_INFO_STREAM("Advertised on topic " << mPubLeftGray.getInfoTopic()); - mPubRawLeftGray = it_zed.advertiseCamera(left_raw_gray_topic, 1); // left raw - NODELET_INFO_STREAM("Advertised on topic " << mPubRawLeftGray.getTopic()); - NODELET_INFO_STREAM("Advertised on topic " << mPubRawLeftGray.getInfoTopic()); - mPubRightGray = it_zed.advertiseCamera(right_gray_topic, 1); // right - NODELET_INFO_STREAM("Advertised on topic " << mPubRightGray.getTopic()); - NODELET_INFO_STREAM("Advertised on topic " << mPubRightGray.getInfoTopic()); - mPubRawRightGray = it_zed.advertiseCamera(right_raw_gray_topic, 1); // right raw - NODELET_INFO_STREAM("Advertised on topic " << mPubRawRightGray.getTopic()); - NODELET_INFO_STREAM("Advertised on topic " << mPubRawRightGray.getInfoTopic()); - - mPubDepth = it_zed.advertiseCamera(depth_topic_root, 1); // depth - NODELET_INFO_STREAM("Advertised on topic " << mPubDepth.getTopic()); - NODELET_INFO_STREAM("Advertised on topic " << mPubDepth.getInfoTopic()); - - mPubStereo = it_zed.advertise(stereo_topic, 1); - NODELET_INFO_STREAM("Advertised on topic " << mPubStereo.getTopic()); - mPubRawStereo = it_zed.advertise(stereo_raw_topic, 1); - NODELET_INFO_STREAM("Advertised on topic " << mPubRawStereo.getTopic()); - - // Confidence Map publisher - mPubConfMap = mNhNs.advertise(conf_map_topic, 1); // confidence map - NODELET_INFO_STREAM("Advertised on topic " << mPubConfMap.getTopic()); - - // Disparity publisher - mPubDisparity = mNhNs.advertise(disparityTopic, static_cast(mVideoDepthFreq)); - NODELET_INFO_STREAM("Advertised on topic " << mPubDisparity.getTopic()); - - // PointCloud publishers - mPubCloud = mNhNs.advertise(pointcloud_topic, 1); - NODELET_INFO_STREAM("Advertised on topic " << mPubCloud.getTopic()); - - if (mMappingEnabled) - { - mPubFusedCloud = mNhNs.advertise(pointcloud_fused_topic, 1); - NODELET_INFO_STREAM("Advertised on topic " << mPubFusedCloud.getTopic() << " @ " << mFusedPcPubFreq << " Hz"); - } - - // Object detection publishers - if (mObjDetEnabled) - { - mPubObjDet = mNhNs.advertise(object_det_topic, 1); - NODELET_INFO_STREAM("Advertised on topic " << mPubObjDet.getTopic()); - } - - // Odometry and Pose publisher - mPubPose = mNhNs.advertise(poseTopic, 1); - NODELET_INFO_STREAM("Advertised on topic " << mPubPose.getTopic()); - - mPubPoseCov = mNhNs.advertise(pose_cov_topic, 1); - NODELET_INFO_STREAM("Advertised on topic " << mPubPoseCov.getTopic()); - - mPubOdom = mNhNs.advertise(odometryTopic, 1); - NODELET_INFO_STREAM("Advertised on topic " << mPubOdom.getTopic()); - - // Camera Path - if (mPathPubRate > 0) - { - mPubOdomPath = mNhNs.advertise(odom_path_topic, 1, true); - NODELET_INFO_STREAM("Advertised on topic " << mPubOdomPath.getTopic()); - mPubMapPath = mNhNs.advertise(map_path_topic, 1, true); - NODELET_INFO_STREAM("Advertised on topic " << mPubMapPath.getTopic()); - - mPathTimer = mNhNs.createTimer(ros::Duration(1.0 / mPathPubRate), &ZEDWrapperNodelet::callback_pubPath, this); - - if (mPathMaxCount != -1) - { - NODELET_DEBUG_STREAM("Path vectors reserved " << mPathMaxCount << " poses."); - mOdomPath.reserve(mPathMaxCount); - mMapPath.reserve(mPathMaxCount); - - NODELET_DEBUG_STREAM("Path vector sizes: " << mOdomPath.size() << " " << mMapPath.size()); - } - } - else - { - NODELET_INFO_STREAM("Path topics not published -> mPathPubRate: " << mPathPubRate); - } - - // Sensor publishers - if (mZedRealCamModel != sl::MODEL::ZED) - { - // IMU Publishers - mPubImu = mNhNs.advertise(imu_topic, 1 /*static_cast(mSensPubRate)*/); - NODELET_INFO_STREAM("Advertised on topic " << mPubImu.getTopic()); - mPubImuRaw = mNhNs.advertise(imu_topic_raw, 1 /*static_cast(mSensPubRate)*/); - NODELET_INFO_STREAM("Advertised on topic " << mPubImuRaw.getTopic()); - mPubImuMag = mNhNs.advertise(imu_mag_topic, 1 /*MAG_FREQ*/); - NODELET_INFO_STREAM("Advertised on topic " << mPubImuMag.getTopic()); - - if (mZedRealCamModel == sl::MODEL::ZED2 || mZedRealCamModel == sl::MODEL::ZED2i) - { - // IMU temperature sensor - mPubImuTemp = mNhNs.advertise(imu_temp_topic, 1 /*static_cast(mSensPubRate)*/); - NODELET_INFO_STREAM("Advertised on topic " << mPubImuTemp.getTopic()); - - // Atmospheric pressure - mPubPressure = mNhNs.advertise(pressure_topic, 1 /*static_cast(BARO_FREQ)*/); - NODELET_INFO_STREAM("Advertised on topic " << mPubPressure.getTopic()); - - // CMOS sensor temperatures - mPubTempL = mNhNs.advertise(temp_topic_left, 1 /*static_cast(BARO_FREQ)*/); - NODELET_INFO_STREAM("Advertised on topic " << mPubTempL.getTopic()); - mPubTempR = mNhNs.advertise(temp_topic_right, 1 /*static_cast(BARO_FREQ)*/); - NODELET_INFO_STREAM("Advertised on topic " << mPubTempR.getTopic()); - } - - // Publish camera imu transform in a latched topic - if (mZedRealCamModel != sl::MODEL::ZED) - { - string cam_imu_tr_topic = "left_cam_imu_transform"; - mPubCamImuTransf = mNhNs.advertise(cam_imu_tr_topic, 1, true); - - sl::Orientation sl_rot = mSlCamImuTransf.getOrientation(); - sl::Translation sl_tr = mSlCamImuTransf.getTranslation(); - - mCameraImuTransfMgs = boost::make_shared(); - - mCameraImuTransfMgs->rotation.x = sl_rot.ox; - mCameraImuTransfMgs->rotation.y = sl_rot.oy; - mCameraImuTransfMgs->rotation.z = sl_rot.oz; - mCameraImuTransfMgs->rotation.w = sl_rot.ow; - - mCameraImuTransfMgs->translation.x = sl_tr.x; - mCameraImuTransfMgs->translation.y = sl_tr.y; - mCameraImuTransfMgs->translation.z = sl_tr.z; - - NODELET_DEBUG("Camera-IMU Rotation: \n %s", sl_rot.getRotationMatrix().getInfos().c_str()); - NODELET_DEBUG("Camera-IMU Translation: \n %g %g %g", sl_tr.x, sl_tr.y, sl_tr.z); - - mPubCamImuTransf.publish(mCameraImuTransfMgs); - - NODELET_INFO_STREAM("Advertised on topic " << mPubCamImuTransf.getTopic() << " [LATCHED]"); - } - - if (!mSvoMode && !mSensTimestampSync) - { - mFrameTimestamp = ros::Time::now(); - mImuTimer = mNhNs.createTimer(ros::Duration(1.0 / (mSensPubRate * 1.5)), - &ZEDWrapperNodelet::callback_pubSensorsData, this); - mSensPeriodMean_usec.reset(new sl_tools::CSmartMean(mSensPubRate / 2)); - } - else - { - mSensPeriodMean_usec.reset(new sl_tools::CSmartMean(mCamFrameRate / 2)); - } - } - - // Services - mSrvSetInitPose = mNhNs.advertiseService("set_pose", &ZEDWrapperNodelet::on_set_pose, this); - mSrvResetOdometry = mNhNs.advertiseService("reset_odometry", &ZEDWrapperNodelet::on_reset_odometry, this); - mSrvResetTracking = mNhNs.advertiseService("reset_tracking", &ZEDWrapperNodelet::on_reset_tracking, this); - mSrvSvoStartRecording = - mNhNs.advertiseService("start_svo_recording", &ZEDWrapperNodelet::on_start_svo_recording, this); - mSrvSvoStopRecording = mNhNs.advertiseService("stop_svo_recording", &ZEDWrapperNodelet::on_stop_svo_recording, this); - - mSrvSetLedStatus = mNhNs.advertiseService("set_led_status", &ZEDWrapperNodelet::on_set_led_status, this); - mSrvToggleLed = mNhNs.advertiseService("toggle_led", &ZEDWrapperNodelet::on_toggle_led, this); - mSrvSvoStartStream = mNhNs.advertiseService("start_remote_stream", &ZEDWrapperNodelet::on_start_remote_stream, this); - mSrvSvoStopStream = mNhNs.advertiseService("stop_remote_stream", &ZEDWrapperNodelet::on_stop_remote_stream, this); - - mSrvStartMapping = mNhNs.advertiseService("start_3d_mapping", &ZEDWrapperNodelet::on_start_3d_mapping, this); - mSrvStopMapping = mNhNs.advertiseService("stop_3d_mapping", &ZEDWrapperNodelet::on_stop_3d_mapping, this); - mSrvSave3dMap = mNhNs.advertiseService("save_3d_map", &ZEDWrapperNodelet::on_save_3d_map, this); - - mSrvStartObjDet = - mNhNs.advertiseService("start_object_detection", &ZEDWrapperNodelet::on_start_object_detection, this); - mSrvStopObjDet = mNhNs.advertiseService("stop_object_detection", &ZEDWrapperNodelet::on_stop_object_detection, this); - - mSrvSaveAreaMemory = mNhNs.advertiseService("save_area_memory", &ZEDWrapperNodelet::on_save_area_memory, this); + NODELET_INFO_STREAM(" * Sensors FW Version -> " << mSensFwVersion); + } + } else { + NODELET_INFO_STREAM(" * Input type\t -> " << sl::toString(mZed.getCameraInformation().input_type).c_str()); + } + + // Set the IMU topic names using real camera model + string imu_topic; + string imu_topic_raw; + string imu_temp_topic; + string imu_mag_topic; + // string imu_mag_topic_raw; + string pressure_topic; + string temp_topic_root = "temperature"; + string temp_topic_left = temp_topic_root + "/left"; + string temp_topic_right = temp_topic_root + "/right"; + + if (mZedRealCamModel != sl::MODEL::ZED) { + std::string imuTopicRoot = "imu"; + string imu_topic_name = "data"; + string imu_topic_raw_name = "data_raw"; + string imu_topic_mag_name = "mag"; + // string imu_topic_mag_raw_name = "mag_raw"; + string pressure_topic_name = "atm_press"; + imu_topic = imuTopicRoot + "/" + imu_topic_name; + imu_topic_raw = imuTopicRoot + "/" + imu_topic_raw_name; + imu_temp_topic = temp_topic_root + "/" + imuTopicRoot; + imu_mag_topic = imuTopicRoot + "/" + imu_topic_mag_name; + // imu_mag_topic_raw = imuTopicRoot + "/" + imu_topic_mag_raw_name; + pressure_topic = /*imuTopicRoot + "/" +*/ pressure_topic_name; + } + + mDiagUpdater.setHardwareIDf("%s - s/n: %d [GPU #%d]", sl::toString(mZedRealCamModel).c_str(), mZedSerialNumber, + mGpuId); + + // ----> Dynamic Reconfigure parameters + mDynRecServer = boost::make_shared>(mDynServerMutex); + dynamic_reconfigure::Server::CallbackType f; + f = boost::bind(&ZEDWrapperNodelet::callback_dynamicReconf, this, _1, _2); + mDynRecServer->setCallback(f); + // Update parameters + zed_nodelets::ZedConfig config; + mDynRecServer->getConfigDefault(config); + mDynServerMutex.lock(); + mDynRecServer->updateConfig(config); + mDynServerMutex.unlock(); + + updateDynamicReconfigure(); + // <---- Dynamic Reconfigure parameters + + // Create all the publishers + // Image publishers + image_transport::ImageTransport it_zed(mNhNs); + + mPubRgb = it_zed.advertiseCamera(rgb_topic, 1); // rgb + NODELET_INFO_STREAM("Advertised on topic " << mPubRgb.getTopic()); + NODELET_INFO_STREAM("Advertised on topic " << mPubRgb.getInfoTopic()); + mPubRawRgb = it_zed.advertiseCamera(rgb_raw_topic, 1); // rgb raw + NODELET_INFO_STREAM("Advertised on topic " << mPubRawRgb.getTopic()); + NODELET_INFO_STREAM("Advertised on topic " << mPubRawRgb.getInfoTopic()); + mPubLeft = it_zed.advertiseCamera(left_topic, 1); // left + NODELET_INFO_STREAM("Advertised on topic " << mPubLeft.getTopic()); + NODELET_INFO_STREAM("Advertised on topic " << mPubLeft.getInfoTopic()); + mPubRawLeft = it_zed.advertiseCamera(left_raw_topic, 1); // left raw + NODELET_INFO_STREAM("Advertised on topic " << mPubRawLeft.getTopic()); + NODELET_INFO_STREAM("Advertised on topic " << mPubRawLeft.getInfoTopic()); + mPubRight = it_zed.advertiseCamera(right_topic, 1); // right + NODELET_INFO_STREAM("Advertised on topic " << mPubRight.getTopic()); + NODELET_INFO_STREAM("Advertised on topic " << mPubRight.getInfoTopic()); + mPubRawRight = it_zed.advertiseCamera(right_raw_topic, 1); // right raw + NODELET_INFO_STREAM("Advertised on topic " << mPubRawRight.getTopic()); + NODELET_INFO_STREAM("Advertised on topic " << mPubRawRight.getInfoTopic()); + + mPubRgbGray = it_zed.advertiseCamera(rgb_gray_topic, 1); // rgb + NODELET_INFO_STREAM("Advertised on topic " << mPubRgbGray.getTopic()); + NODELET_INFO_STREAM("Advertised on topic " << mPubRgbGray.getInfoTopic()); + mPubRawRgbGray = it_zed.advertiseCamera(rgb_raw_gray_topic, 1); // rgb raw + NODELET_INFO_STREAM("Advertised on topic " << mPubRawRgbGray.getTopic()); + NODELET_INFO_STREAM("Advertised on topic " << mPubRawRgbGray.getInfoTopic()); + mPubLeftGray = it_zed.advertiseCamera(left_gray_topic, 1); // left + NODELET_INFO_STREAM("Advertised on topic " << mPubLeftGray.getTopic()); + NODELET_INFO_STREAM("Advertised on topic " << mPubLeftGray.getInfoTopic()); + mPubRawLeftGray = it_zed.advertiseCamera(left_raw_gray_topic, 1); // left raw + NODELET_INFO_STREAM("Advertised on topic " << mPubRawLeftGray.getTopic()); + NODELET_INFO_STREAM("Advertised on topic " << mPubRawLeftGray.getInfoTopic()); + mPubRightGray = it_zed.advertiseCamera(right_gray_topic, 1); // right + NODELET_INFO_STREAM("Advertised on topic " << mPubRightGray.getTopic()); + NODELET_INFO_STREAM("Advertised on topic " << mPubRightGray.getInfoTopic()); + mPubRawRightGray = it_zed.advertiseCamera(right_raw_gray_topic, 1); // right raw + NODELET_INFO_STREAM("Advertised on topic " << mPubRawRightGray.getTopic()); + NODELET_INFO_STREAM("Advertised on topic " << mPubRawRightGray.getInfoTopic()); + + mPubDepth = it_zed.advertiseCamera(depth_topic_root, 1); // depth + NODELET_INFO_STREAM("Advertised on topic " << mPubDepth.getTopic()); + NODELET_INFO_STREAM("Advertised on topic " << mPubDepth.getInfoTopic()); + + mPubStereo = it_zed.advertise(stereo_topic, 1); + NODELET_INFO_STREAM("Advertised on topic " << mPubStereo.getTopic()); + mPubRawStereo = it_zed.advertise(stereo_raw_topic, 1); + NODELET_INFO_STREAM("Advertised on topic " << mPubRawStereo.getTopic()); + + // Confidence Map publisher + mPubConfMap = mNhNs.advertise(conf_map_topic, 1); // confidence map + NODELET_INFO_STREAM("Advertised on topic " << mPubConfMap.getTopic()); + + // Disparity publisher + mPubDisparity = mNhNs.advertise(disparityTopic, static_cast(mVideoDepthFreq)); + NODELET_INFO_STREAM("Advertised on topic " << mPubDisparity.getTopic()); + + // PointCloud publishers + mPubCloud = mNhNs.advertise(pointcloud_topic, 1); + NODELET_INFO_STREAM("Advertised on topic " << mPubCloud.getTopic()); + + if (mMappingEnabled) { + mPubFusedCloud = mNhNs.advertise(pointcloud_fused_topic, 1); + NODELET_INFO_STREAM("Advertised on topic " << mPubFusedCloud.getTopic() << " @ " << mFusedPcPubFreq << " Hz"); + } + + // Object detection publishers + if (mObjDetEnabled) { + mPubObjDet = mNhNs.advertise(object_det_topic, 1); + NODELET_INFO_STREAM("Advertised on topic " << mPubObjDet.getTopic()); + } + + // Odometry and Pose publisher + mPubPose = mNhNs.advertise(poseTopic, 1); + NODELET_INFO_STREAM("Advertised on topic " << mPubPose.getTopic()); + + mPubPoseCov = mNhNs.advertise(pose_cov_topic, 1); + NODELET_INFO_STREAM("Advertised on topic " << mPubPoseCov.getTopic()); + + mPubOdom = mNhNs.advertise(odometryTopic, 1); + NODELET_INFO_STREAM("Advertised on topic " << mPubOdom.getTopic()); + + // Camera Path + if (mPathPubRate > 0) { + mPubOdomPath = mNhNs.advertise(odom_path_topic, 1, true); + NODELET_INFO_STREAM("Advertised on topic " << mPubOdomPath.getTopic()); + mPubMapPath = mNhNs.advertise(map_path_topic, 1, true); + NODELET_INFO_STREAM("Advertised on topic " << mPubMapPath.getTopic()); + + mPathTimer = mNhNs.createTimer(ros::Duration(1.0 / mPathPubRate), &ZEDWrapperNodelet::callback_pubPath, this); + + if (mPathMaxCount != -1) { + NODELET_DEBUG_STREAM("Path vectors reserved " << mPathMaxCount << " poses."); + mOdomPath.reserve(mPathMaxCount); + mMapPath.reserve(mPathMaxCount); + + NODELET_DEBUG_STREAM("Path vector sizes: " << mOdomPath.size() << " " << mMapPath.size()); + } + } else { + NODELET_INFO_STREAM("Path topics not published -> mPathPubRate: " << mPathPubRate); + } + + // Sensor publishers + if (mZedRealCamModel != sl::MODEL::ZED) { + // IMU Publishers + mPubImu = mNhNs.advertise(imu_topic, 1 /*static_cast(mSensPubRate)*/); + NODELET_INFO_STREAM("Advertised on topic " << mPubImu.getTopic()); + mPubImuRaw = mNhNs.advertise(imu_topic_raw, 1 /*static_cast(mSensPubRate)*/); + NODELET_INFO_STREAM("Advertised on topic " << mPubImuRaw.getTopic()); + mPubImuMag = mNhNs.advertise(imu_mag_topic, 1 /*MAG_FREQ*/); + NODELET_INFO_STREAM("Advertised on topic " << mPubImuMag.getTopic()); + + if (mZedRealCamModel == sl::MODEL::ZED2 || mZedRealCamModel == sl::MODEL::ZED2i) { + // IMU temperature sensor + mPubImuTemp = mNhNs.advertise(imu_temp_topic, 1 /*static_cast(mSensPubRate)*/); + NODELET_INFO_STREAM("Advertised on topic " << mPubImuTemp.getTopic()); + + // Atmospheric pressure + mPubPressure = mNhNs.advertise(pressure_topic, 1 /*static_cast(BARO_FREQ)*/); + NODELET_INFO_STREAM("Advertised on topic " << mPubPressure.getTopic()); + + // CMOS sensor temperatures + mPubTempL = mNhNs.advertise(temp_topic_left, 1 /*static_cast(BARO_FREQ)*/); + NODELET_INFO_STREAM("Advertised on topic " << mPubTempL.getTopic()); + mPubTempR = mNhNs.advertise(temp_topic_right, 1 /*static_cast(BARO_FREQ)*/); + NODELET_INFO_STREAM("Advertised on topic " << mPubTempR.getTopic()); + } + + // Publish camera imu transform in a latched topic + if (mZedRealCamModel != sl::MODEL::ZED) { + string cam_imu_tr_topic = "left_cam_imu_transform"; + mPubCamImuTransf = mNhNs.advertise(cam_imu_tr_topic, 1, true); + + sl::Orientation sl_rot = mSlCamImuTransf.getOrientation(); + sl::Translation sl_tr = mSlCamImuTransf.getTranslation(); + + mCameraImuTransfMgs = boost::make_shared(); + + mCameraImuTransfMgs->rotation.x = sl_rot.ox; + mCameraImuTransfMgs->rotation.y = sl_rot.oy; + mCameraImuTransfMgs->rotation.z = sl_rot.oz; + mCameraImuTransfMgs->rotation.w = sl_rot.ow; + + mCameraImuTransfMgs->translation.x = sl_tr.x; + mCameraImuTransfMgs->translation.y = sl_tr.y; + mCameraImuTransfMgs->translation.z = sl_tr.z; + + NODELET_DEBUG("Camera-IMU Rotation: \n %s", sl_rot.getRotationMatrix().getInfos().c_str()); + NODELET_DEBUG("Camera-IMU Translation: \n %g %g %g", sl_tr.x, sl_tr.y, sl_tr.z); + + mPubCamImuTransf.publish(mCameraImuTransfMgs); + + NODELET_INFO_STREAM("Advertised on topic " << mPubCamImuTransf.getTopic() << " [LATCHED]"); + } + + if (!mSvoMode && !mSensTimestampSync) { + mFrameTimestamp = ros::Time::now(); + mImuTimer = mNhNs.createTimer(ros::Duration(1.0 / (mSensPubRate * 1.5)), + &ZEDWrapperNodelet::callback_pubSensorsData, this); + mSensPeriodMean_usec.reset(new sl_tools::CSmartMean(mSensPubRate / 2)); + } else { + mSensPeriodMean_usec.reset(new sl_tools::CSmartMean(mCamFrameRate / 2)); + } + } + + // Services + mSrvSetInitPose = mNhNs.advertiseService("set_pose", &ZEDWrapperNodelet::on_set_pose, this); + mSrvResetOdometry = mNhNs.advertiseService("reset_odometry", &ZEDWrapperNodelet::on_reset_odometry, this); + mSrvResetTracking = mNhNs.advertiseService("reset_tracking", &ZEDWrapperNodelet::on_reset_tracking, this); + mSrvSvoStartRecording = mNhNs.advertiseService("start_svo_recording", &ZEDWrapperNodelet::on_start_svo_recording, this); + mSrvSvoStopRecording = mNhNs.advertiseService("stop_svo_recording", &ZEDWrapperNodelet::on_stop_svo_recording, this); - // Start Pointcloud thread - mPcThread = std::thread(&ZEDWrapperNodelet::pointcloud_thread_func, this); - - // Start pool thread - mDevicePollThread = std::thread(&ZEDWrapperNodelet::device_poll_thread_func, this); - - // Start data publishing timer - mVideoDepthTimer = - mNhNs.createTimer(ros::Duration(1.0 / mVideoDepthFreq), &ZEDWrapperNodelet::callback_pubVideoDepth, this); + mSrvSetLedStatus = mNhNs.advertiseService("set_led_status", &ZEDWrapperNodelet::on_set_led_status, this); + mSrvToggleLed = mNhNs.advertiseService("toggle_led", &ZEDWrapperNodelet::on_toggle_led, this); + mSrvSvoStartStream = mNhNs.advertiseService("start_remote_stream", &ZEDWrapperNodelet::on_start_remote_stream, this); + mSrvSvoStopStream = mNhNs.advertiseService("stop_remote_stream", &ZEDWrapperNodelet::on_stop_remote_stream, this); + + mSrvStartMapping = mNhNs.advertiseService("start_3d_mapping", &ZEDWrapperNodelet::on_start_3d_mapping, this); + mSrvStopMapping = mNhNs.advertiseService("stop_3d_mapping", &ZEDWrapperNodelet::on_stop_3d_mapping, this); + mSrvSave3dMap = mNhNs.advertiseService("save_3d_map", &ZEDWrapperNodelet::on_save_3d_map, this); + + mSrvStartObjDet = mNhNs.advertiseService("start_object_detection", &ZEDWrapperNodelet::on_start_object_detection, this); + mSrvStopObjDet = mNhNs.advertiseService("stop_object_detection", &ZEDWrapperNodelet::on_stop_object_detection, this); + + mSrvSaveAreaMemory = mNhNs.advertiseService("save_area_memory", &ZEDWrapperNodelet::on_save_area_memory, this); + + // Start Pointcloud thread + mPcThread = std::thread(&ZEDWrapperNodelet::pointcloud_thread_func, this); + + // Start pool thread + mDevicePollThread = std::thread(&ZEDWrapperNodelet::device_poll_thread_func, this); + + // Start data publishing timer + mVideoDepthTimer = mNhNs.createTimer(ros::Duration(1.0 / mVideoDepthFreq), &ZEDWrapperNodelet::callback_pubVideoDepth, this); } void ZEDWrapperNodelet::readParameters() { - NODELET_INFO_STREAM("*** GENERAL PARAMETERS ***"); - - // ----> General - // Get parameters from param files - mNhNs.getParam("general/camera_name", mCameraName); - NODELET_INFO_STREAM(" * Camera Name\t\t\t-> " << mCameraName.c_str()); - int resol; - mNhNs.getParam("general/resolution", resol); - mCamResol = static_cast(resol); - NODELET_INFO_STREAM(" * Camera Resolution\t\t-> " << sl::toString(mCamResol).c_str()); - mNhNs.getParam("general/grab_frame_rate", mCamFrameRate); - checkResolFps(); - NODELET_INFO_STREAM(" * Camera Grab Framerate\t-> " << mCamFrameRate); - - mNhNs.getParam("general/gpu_id", mGpuId); - NODELET_INFO_STREAM(" * Gpu ID\t\t\t-> " << mGpuId); - mNhNs.getParam("general/zed_id", mZedId); - NODELET_INFO_STREAM(" * Camera ID\t\t\t-> " << mGpuId); - mNhNs.getParam("general/verbose", mVerbose); - NODELET_INFO_STREAM(" * Verbose\t\t\t-> " << (mVerbose ? "ENABLED" : "DISABLED")); - mNhNs.param("general/camera_flip", mCameraFlip, false); - NODELET_INFO_STREAM(" * Camera Flip\t\t\t-> " << (mCameraFlip ? "ENABLED" : "DISABLED")); - mNhNs.param("general/self_calib", mCameraSelfCalib, true); - NODELET_INFO_STREAM(" * Self calibration\t\t-> " << (mCameraSelfCalib ? "ENABLED" : "DISABLED")); - - int tmp_sn = 0; - mNhNs.getParam("general/serial_number", tmp_sn); - - if (tmp_sn > 0) - { - mZedSerialNumber = static_cast(tmp_sn); - NODELET_INFO_STREAM(" * Serial number\t\t-> " << mZedSerialNumber); - } - - string camera_model; - mNhNs.getParam("general/camera_model", camera_model); - - if (camera_model == "zed") - { - mZedUserCamModel = sl::MODEL::ZED; - NODELET_INFO_STREAM(" * Camera Model by param\t-> " << camera_model); - } - else if (camera_model == "zedm") - { - mZedUserCamModel = sl::MODEL::ZED_M; - NODELET_INFO_STREAM(" * Camera Model by param\t-> " << camera_model); - } - else if (camera_model == "zed2") - { - mZedUserCamModel = sl::MODEL::ZED2; - NODELET_INFO_STREAM(" * Camera Model by param\t-> " << camera_model); - } - else if (camera_model == "zed2i") - { - mZedUserCamModel = sl::MODEL::ZED2i; - NODELET_INFO_STREAM(" * Camera Model by param\t-> " << camera_model); - } - else - { - NODELET_ERROR_STREAM("Camera model not valid: " << camera_model); - } - // <---- General - - NODELET_INFO_STREAM("*** VIDEO PARAMETERS ***"); - - // ----> Video - mNhNs.getParam("video/img_downsample_factor", mCamImageResizeFactor); - NODELET_INFO_STREAM(" * Image resample factor\t-> " << mCamImageResizeFactor); - - mNhNs.getParam("video/extrinsic_in_camera_frame", mUseOldExtrinsic); - NODELET_INFO_STREAM(" * Extrinsic param. frame\t-> " - << (mUseOldExtrinsic ? "X RIGHT - Y DOWN - Z FWD" : "X FWD - Y LEFT - Z UP")); - // <---- Video - - NODELET_INFO_STREAM("*** DEPTH PARAMETERS ***"); - - // -----> Depth - int depth_mode; - mNhNs.getParam("depth/quality", depth_mode); - mDepthMode = static_cast(depth_mode); - NODELET_INFO_STREAM(" * Depth quality\t\t-> " << sl::toString(mDepthMode).c_str()); - int sensing_mode; - mNhNs.getParam("depth/sensing_mode", sensing_mode); - mCamSensingMode = static_cast(sensing_mode); - NODELET_INFO_STREAM(" * Depth Sensing mode\t\t-> " << sl::toString(mCamSensingMode).c_str()); - mNhNs.getParam("depth/openni_depth_mode", mOpenniDepthMode); - NODELET_INFO_STREAM(" * OpenNI mode\t\t\t-> " << (mOpenniDepthMode ? "ENABLED" : "DISABLED")); - mNhNs.getParam("depth/depth_stabilization", mDepthStabilization); - NODELET_INFO_STREAM(" * Depth Stabilization\t\t-> " << (mDepthStabilization ? "ENABLED" : "DISABLED")); - mNhNs.getParam("depth/min_depth", mCamMinDepth); - NODELET_INFO_STREAM(" * Minimum depth\t\t-> " << mCamMinDepth << " m"); - mNhNs.getParam("depth/max_depth", mCamMaxDepth); - NODELET_INFO_STREAM(" * Maximum depth\t\t-> " << mCamMaxDepth << " m"); - mNhNs.getParam("depth/depth_downsample_factor", mCamDepthResizeFactor); - NODELET_INFO_STREAM(" * Depth resample factor\t-> " << mCamDepthResizeFactor); - // <----- Depth - - NODELET_INFO_STREAM("*** POSITIONAL TRACKING PARAMETERS ***"); - - // ----> Tracking - mNhNs.param("pos_tracking/pos_tracking_enabled", mPosTrackingEnabled, true); - NODELET_INFO_STREAM(" * Positional tracking\t\t-> " << (mPosTrackingEnabled ? "ENABLED" : "DISABLED")); - 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)); - - if (mPathMaxCount < 2 && mPathMaxCount != -1) - { - mPathMaxCount = 2; - } - - mNhNs.getParam("pos_tracking/initial_base_pose", mInitialBasePose); - - mNhNs.getParam("pos_tracking/area_memory_db_path", mAreaMemDbPath); - mAreaMemDbPath = sl_tools::resolveFilePath(mAreaMemDbPath); - NODELET_INFO_STREAM(" * Odometry DB path\t\t-> " << mAreaMemDbPath.c_str()); - - mNhNs.param("pos_tracking/save_area_memory_db_on_exit", mSaveAreaMapOnClosing, false); - NODELET_INFO_STREAM(" * Save Area Memory on closing\t-> " << (mSaveAreaMapOnClosing ? "ENABLED" : "DISABLED")); - mNhNs.param("pos_tracking/area_memory", mAreaMemory, false); - NODELET_INFO_STREAM(" * Area Memory\t\t\t-> " << (mAreaMemory ? "ENABLED" : "DISABLED")); - mNhNs.param("pos_tracking/imu_fusion", mImuFusion, true); - NODELET_INFO_STREAM(" * IMU Fusion\t\t\t-> " << (mImuFusion ? "ENABLED" : "DISABLED")); - mNhNs.param("pos_tracking/floor_alignment", mFloorAlignment, false); - NODELET_INFO_STREAM(" * Floor alignment\t\t-> " << (mFloorAlignment ? "ENABLED" : "DISABLED")); - mNhNs.param("pos_tracking/init_odom_with_first_valid_pose", mInitOdomWithPose, true); - NODELET_INFO_STREAM(" * Init Odometry with first valid pose data -> " - << (mInitOdomWithPose ? "ENABLED" : "DISABLED")); - mNhNs.param("pos_tracking/two_d_mode", mTwoDMode, false); - NODELET_INFO_STREAM(" * Two D mode\t\t\t-> " << (mTwoDMode ? "ENABLED" : "DISABLED")); - mNhNs.param("pos_tracking/fixed_z_value", mFixedZValue, 0.0); - - if (mTwoDMode) - { - NODELET_INFO_STREAM(" * Fixed Z value\t\t-> " << mFixedZValue); - } - // <---- Tracking - - NODELET_INFO_STREAM("*** MAPPING PARAMETERS ***"); - - // ----> Mapping - mNhNs.param("mapping/mapping_enabled", mMappingEnabled, false); - - if (mMappingEnabled) - { - NODELET_INFO_STREAM(" * Mapping\t\t\t-> ENABLED"); - - mNhNs.getParam("mapping/resolution", mMappingRes); - NODELET_INFO_STREAM(" * Mapping resolution\t\t-> " << mMappingRes << " m"); - - mNhNs.getParam("mapping/max_mapping_range", mMaxMappingRange); - NODELET_INFO_STREAM(" * Mapping max range\t\t-> " << mMaxMappingRange << " m" - << ((mMaxMappingRange < 0.0) ? " [AUTO]" : "")); - - mNhNs.getParam("mapping/fused_pointcloud_freq", mFusedPcPubFreq); - NODELET_INFO_STREAM(" * Fused point cloud freq:\t-> " << mFusedPcPubFreq << " Hz"); - } - else - { - NODELET_INFO_STREAM(" * Mapping\t\t\t-> DISABLED"); - } - // <---- Mapping - - NODELET_INFO_STREAM("*** OBJECT DETECTION PARAMETERS ***"); - - // ----> Object Detection - mNhNs.param("object_detection/od_enabled", mObjDetEnabled, false); - - if (mObjDetEnabled) - { - NODELET_INFO_STREAM(" * Object Detection\t\t-> ENABLED"); - mNhNs.getParam("object_detection/confidence_threshold", mObjDetConfidence); - NODELET_INFO_STREAM(" * Object confidence\t\t-> " << mObjDetConfidence); - mNhNs.getParam("object_detection/object_tracking_enabled", mObjDetTracking); - NODELET_INFO_STREAM(" * Object tracking\t\t-> " << (mObjDetTracking ? "ENABLED" : "DISABLED")); - mNhNs.getParam("object_detection/max_range", mObjDetMaxRange); - if (mObjDetMaxRange > mCamMaxDepth) - { - NODELET_WARN("Detection max range cannot be major than depth max range. Automatically fixed."); - mObjDetMaxRange = mCamMaxDepth; + NODELET_INFO_STREAM("*** GENERAL PARAMETERS ***"); + + // ----> General + // Get parameters from param files + mNhNs.getParam("general/camera_name", mCameraName); + NODELET_INFO_STREAM(" * Camera Name\t\t\t-> " << mCameraName.c_str()); + int resol; + mNhNs.getParam("general/resolution", resol); + mCamResol = static_cast(resol); + NODELET_INFO_STREAM(" * Camera Resolution\t\t-> " << sl::toString(mCamResol).c_str()); + mNhNs.getParam("general/grab_frame_rate", mCamFrameRate); + checkResolFps(); + NODELET_INFO_STREAM(" * Camera Grab Framerate\t-> " << mCamFrameRate); + + mNhNs.getParam("general/gpu_id", mGpuId); + NODELET_INFO_STREAM(" * Gpu ID\t\t\t-> " << mGpuId); + mNhNs.getParam("general/zed_id", mZedId); + NODELET_INFO_STREAM(" * Camera ID\t\t\t-> " << mGpuId); + mNhNs.getParam("general/verbose", mVerbose); + NODELET_INFO_STREAM(" * Verbose\t\t\t-> " << (mVerbose ? "ENABLED" : "DISABLED")); + mNhNs.param("general/camera_flip", mCameraFlip, false); + NODELET_INFO_STREAM(" * Camera Flip\t\t\t-> " << (mCameraFlip ? "ENABLED" : "DISABLED")); + mNhNs.param("general/self_calib", mCameraSelfCalib, true); + NODELET_INFO_STREAM(" * Self calibration\t\t-> " << (mCameraSelfCalib ? "ENABLED" : "DISABLED")); + + int tmp_sn = 0; + mNhNs.getParam("general/serial_number", tmp_sn); + + if (tmp_sn > 0) { + mZedSerialNumber = static_cast(tmp_sn); + NODELET_INFO_STREAM(" * Serial number\t\t-> " << mZedSerialNumber); } - NODELET_INFO_STREAM(" * Detection max range\t\t-> " << mObjDetMaxRange); - int model; - mNhNs.getParam("object_detection/model", model); - if (model < 0 || model >= static_cast(sl::DETECTION_MODEL::LAST)) - { - NODELET_WARN("Detection model not valid. Forced to the default value"); - model = static_cast(mObjDetModel); + string camera_model; + mNhNs.getParam("general/camera_model", camera_model); + + if (camera_model == "zed") { + mZedUserCamModel = sl::MODEL::ZED; + NODELET_INFO_STREAM(" * Camera Model by param\t-> " << camera_model); + } else if (camera_model == "zedm") { + mZedUserCamModel = sl::MODEL::ZED_M; + NODELET_INFO_STREAM(" * Camera Model by param\t-> " << camera_model); + } else if (camera_model == "zed2") { + mZedUserCamModel = sl::MODEL::ZED2; + NODELET_INFO_STREAM(" * Camera Model by param\t-> " << camera_model); + } else if (camera_model == "zed2i") { + mZedUserCamModel = sl::MODEL::ZED2i; + NODELET_INFO_STREAM(" * Camera Model by param\t-> " << camera_model); + } else { + NODELET_ERROR_STREAM("Camera model not valid: " << camera_model); + } + // <---- General + + NODELET_INFO_STREAM("*** VIDEO PARAMETERS ***"); + + // ----> Video + mNhNs.getParam("video/img_downsample_factor", mCamImageResizeFactor); + NODELET_INFO_STREAM(" * Image resample factor\t-> " << mCamImageResizeFactor); + + mNhNs.getParam("video/extrinsic_in_camera_frame", mUseOldExtrinsic); + NODELET_INFO_STREAM(" * Extrinsic param. frame\t-> " + << (mUseOldExtrinsic ? "X RIGHT - Y DOWN - Z FWD" : "X FWD - Y LEFT - Z UP")); + // <---- Video + + NODELET_INFO_STREAM("*** DEPTH PARAMETERS ***"); + + // -----> Depth + int depth_mode; + mNhNs.getParam("depth/quality", depth_mode); + mDepthMode = static_cast(depth_mode); + NODELET_INFO_STREAM(" * Depth quality\t\t-> " << sl::toString(mDepthMode).c_str()); + int sensing_mode; + mNhNs.getParam("depth/sensing_mode", sensing_mode); + mCamSensingMode = static_cast(sensing_mode); + NODELET_INFO_STREAM(" * Depth Sensing mode\t\t-> " << sl::toString(mCamSensingMode).c_str()); + mNhNs.getParam("depth/openni_depth_mode", mOpenniDepthMode); + NODELET_INFO_STREAM(" * OpenNI mode\t\t\t-> " << (mOpenniDepthMode ? "ENABLED" : "DISABLED")); + mNhNs.getParam("depth/depth_stabilization", mDepthStabilization); + NODELET_INFO_STREAM(" * Depth Stabilization\t\t-> " << (mDepthStabilization ? "ENABLED" : "DISABLED")); + mNhNs.getParam("depth/min_depth", mCamMinDepth); + NODELET_INFO_STREAM(" * Minimum depth\t\t-> " << mCamMinDepth << " m"); + mNhNs.getParam("depth/max_depth", mCamMaxDepth); + NODELET_INFO_STREAM(" * Maximum depth\t\t-> " << mCamMaxDepth << " m"); + mNhNs.getParam("depth/depth_downsample_factor", mCamDepthResizeFactor); + NODELET_INFO_STREAM(" * Depth resample factor\t-> " << mCamDepthResizeFactor); + // <----- Depth + + NODELET_INFO_STREAM("*** POSITIONAL TRACKING PARAMETERS ***"); + + // ----> Tracking + mNhNs.param("pos_tracking/pos_tracking_enabled", mPosTrackingEnabled, true); + NODELET_INFO_STREAM(" * Positional tracking\t\t-> " << (mPosTrackingEnabled ? "ENABLED" : "DISABLED")); + 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)); + + if (mPathMaxCount < 2 && mPathMaxCount != -1) { + mPathMaxCount = 2; } - mObjDetModel = static_cast(model); - NODELET_INFO_STREAM(" * Detection model\t\t-> " << sl::toString(mObjDetModel)); + mNhNs.getParam("pos_tracking/initial_base_pose", mInitialBasePose); + + mNhNs.getParam("pos_tracking/area_memory_db_path", mAreaMemDbPath); + mAreaMemDbPath = sl_tools::resolveFilePath(mAreaMemDbPath); + NODELET_INFO_STREAM(" * Odometry DB path\t\t-> " << mAreaMemDbPath.c_str()); + + mNhNs.param("pos_tracking/save_area_memory_db_on_exit", mSaveAreaMapOnClosing, false); + NODELET_INFO_STREAM(" * Save Area Memory on closing\t-> " << (mSaveAreaMapOnClosing ? "ENABLED" : "DISABLED")); + mNhNs.param("pos_tracking/area_memory", mAreaMemory, false); + NODELET_INFO_STREAM(" * Area Memory\t\t\t-> " << (mAreaMemory ? "ENABLED" : "DISABLED")); + mNhNs.param("pos_tracking/imu_fusion", mImuFusion, true); + NODELET_INFO_STREAM(" * IMU Fusion\t\t\t-> " << (mImuFusion ? "ENABLED" : "DISABLED")); + mNhNs.param("pos_tracking/floor_alignment", mFloorAlignment, false); + NODELET_INFO_STREAM(" * Floor alignment\t\t-> " << (mFloorAlignment ? "ENABLED" : "DISABLED")); + mNhNs.param("pos_tracking/init_odom_with_first_valid_pose", mInitOdomWithPose, true); + NODELET_INFO_STREAM(" * Init Odometry with first valid pose data -> " + << (mInitOdomWithPose ? "ENABLED" : "DISABLED")); + mNhNs.param("pos_tracking/two_d_mode", mTwoDMode, false); + NODELET_INFO_STREAM(" * Two D mode\t\t\t-> " << (mTwoDMode ? "ENABLED" : "DISABLED")); + mNhNs.param("pos_tracking/fixed_z_value", mFixedZValue, 0.0); + + if (mTwoDMode) { + NODELET_INFO_STREAM(" * Fixed Z value\t\t-> " << mFixedZValue); + } + // <---- Tracking + + NODELET_INFO_STREAM("*** MAPPING PARAMETERS ***"); + + // ----> Mapping + mNhNs.param("mapping/mapping_enabled", mMappingEnabled, false); + + if (mMappingEnabled) { + NODELET_INFO_STREAM(" * Mapping\t\t\t-> ENABLED"); + + mNhNs.getParam("mapping/resolution", mMappingRes); + NODELET_INFO_STREAM(" * Mapping resolution\t\t-> " << mMappingRes << " m"); + + mNhNs.getParam("mapping/max_mapping_range", mMaxMappingRange); + NODELET_INFO_STREAM(" * Mapping max range\t\t-> " << mMaxMappingRange << " m" + << ((mMaxMappingRange < 0.0) ? " [AUTO]" : "")); - if (mObjDetModel == sl::DETECTION_MODEL::HUMAN_BODY_FAST || - mObjDetModel == sl::DETECTION_MODEL::HUMAN_BODY_ACCURATE) - { - mNhNs.getParam("object_detection/body_fitting", mObjDetBodyFitting); - NODELET_INFO_STREAM(" * Body fitting\t\t\t-> " << (mObjDetBodyFitting ? "ENABLED" : "DISABLED")); - } - else - { - mNhNs.getParam("object_detection/mc_people", mObjDetPeopleEnable); - NODELET_INFO_STREAM(" * Detect people\t\t-> " << (mObjDetPeopleEnable ? "ENABLED" : "DISABLED")); - mNhNs.getParam("object_detection/mc_vehicle", mObjDetVehiclesEnable); - NODELET_INFO_STREAM(" * Detect vehicles\t\t-> " << (mObjDetVehiclesEnable ? "ENABLED" : "DISABLED")); - mNhNs.getParam("object_detection/mc_bag", mObjDetBagsEnable); - NODELET_INFO_STREAM(" * Detect bags\t\t\t-> " << (mObjDetBagsEnable ? "ENABLED" : "DISABLED")); - mNhNs.getParam("object_detection/mc_animal", mObjDetAnimalsEnable); - NODELET_INFO_STREAM(" * Detect animals\t\t-> " << (mObjDetAnimalsEnable ? "ENABLED" : "DISABLED")); - mNhNs.getParam("object_detection/mc_electronics", mObjDetElectronicsEnable); - NODELET_INFO_STREAM(" * Detect electronics\t\t-> " << (mObjDetElectronicsEnable ? "ENABLED" : "DISABLED")); - mNhNs.getParam("object_detection/mc_fruit_vegetable", mObjDetFruitsEnable); - NODELET_INFO_STREAM(" * Detect fruit and vegetables\t-> " << (mObjDetFruitsEnable ? "ENABLED" : "DISABLED")); - } - } - else - { - NODELET_INFO_STREAM(" * Object Detection\t\t-> DISABLED"); - } - // <---- Object Detection - - NODELET_INFO_STREAM("*** SENSORS PARAMETERS ***"); - - // ----> Sensors - mNhNs.getParam("sensors/sensors_timestamp_sync", mSensTimestampSync); - NODELET_INFO_STREAM(" * Sensors timestamp sync\t-> " << (mSensTimestampSync ? "ENABLED" : "DISABLED")); - // <---- Sensors - - NODELET_INFO_STREAM("*** SVO PARAMETERS ***"); - - // ----> SVO - mNhNs.param("svo_file", mSvoFilepath, std::string()); - mSvoFilepath = sl_tools::resolveFilePath(mSvoFilepath); - NODELET_INFO_STREAM(" * SVO input file: \t\t-> " << mSvoFilepath.c_str()); - - int svo_compr = 0; - mNhNs.getParam("general/svo_compression", svo_compr); - - if (svo_compr >= static_cast(sl::SVO_COMPRESSION_MODE::LAST)) - { - NODELET_WARN_STREAM("The parameter `general/svo_compression` has an invalid value. Please check it in the " - "configuration file `common.yaml`"); - - svo_compr = 0; - } - - mSvoComprMode = static_cast(svo_compr); - - NODELET_INFO_STREAM(" * SVO REC compression\t\t-> " << sl::toString(mSvoComprMode)); - // <---- SVO - - // Remote Stream - mNhNs.param("stream", mRemoteStreamAddr, std::string()); - - NODELET_INFO_STREAM("*** COORDINATE FRAMES ***"); - - // ----> Coordinate frames - mNhNs.param("pos_tracking/map_frame", mMapFrameId, "map"); - mNhNs.param("pos_tracking/odometry_frame", mOdometryFrameId, "odom"); - mNhNs.param("general/base_frame", mBaseFrameId, "base_link"); - - mCameraFrameId = mCameraName + "_camera_center"; - mImuFrameId = mCameraName + "_imu_link"; - mLeftCamFrameId = mCameraName + "_left_camera_frame"; - mLeftCamOptFrameId = mCameraName + "_left_camera_optical_frame"; - mRightCamFrameId = mCameraName + "_right_camera_frame"; - mRightCamOptFrameId = mCameraName + "_right_camera_optical_frame"; - - mBaroFrameId = mCameraName + "_baro_link"; - mMagFrameId = mCameraName + "_mag_link"; - mTempLeftFrameId = mCameraName + "_temp_left_link"; - mTempRightFrameId = mCameraName + "_temp_right_link"; - - mDepthFrameId = mLeftCamFrameId; - mDepthOptFrameId = mLeftCamOptFrameId; - - // Note: Depth image frame id must match color image frame id - mCloudFrameId = mDepthOptFrameId; - mRgbFrameId = mDepthFrameId; - mRgbOptFrameId = mCloudFrameId; - mDisparityFrameId = mDepthFrameId; - mDisparityOptFrameId = mDepthOptFrameId; - mConfidenceFrameId = mDepthFrameId; - mConfidenceOptFrameId = mDepthOptFrameId; - - // Print TF frames - NODELET_INFO_STREAM(" * map_frame\t\t\t-> " << mMapFrameId); - NODELET_INFO_STREAM(" * odometry_frame\t\t-> " << mOdometryFrameId); - NODELET_INFO_STREAM(" * base_frame\t\t\t-> " << mBaseFrameId); - NODELET_INFO_STREAM(" * camera_frame\t\t\t-> " << mCameraFrameId); - NODELET_INFO_STREAM(" * imu_link\t\t\t-> " << mImuFrameId); - NODELET_INFO_STREAM(" * left_camera_frame\t\t-> " << mLeftCamFrameId); - NODELET_INFO_STREAM(" * left_camera_optical_frame\t-> " << mLeftCamOptFrameId); - NODELET_INFO_STREAM(" * right_camera_frame\t\t-> " << mRightCamFrameId); - NODELET_INFO_STREAM(" * right_camera_optical_frame\t-> " << mRightCamOptFrameId); - NODELET_INFO_STREAM(" * depth_frame\t\t\t-> " << mDepthFrameId); - NODELET_INFO_STREAM(" * depth_optical_frame\t\t-> " << mDepthOptFrameId); - NODELET_INFO_STREAM(" * disparity_frame\t\t-> " << mDisparityFrameId); - NODELET_INFO_STREAM(" * disparity_optical_frame\t-> " << mDisparityOptFrameId); - NODELET_INFO_STREAM(" * confidence_frame\t\t-> " << mConfidenceFrameId); - NODELET_INFO_STREAM(" * confidence_optical_frame\t-> " << mConfidenceOptFrameId); - // <---- Coordinate frames - - // ----> TF broadcasting - mNhNs.param("pos_tracking/publish_tf", mPublishTf, true); - NODELET_INFO_STREAM(" * Broadcast odometry TF\t-> " << (mPublishTf ? "ENABLED" : "DISABLED")); - mNhNs.param("pos_tracking/publish_map_tf", mPublishMapTf, true); - NODELET_INFO_STREAM(" * Broadcast map pose TF\t-> " - << (mPublishTf ? (mPublishMapTf ? "ENABLED" : "DISABLED") : "DISABLED")); - mNhNs.param("sensors/publish_imu_tf", mPublishImuTf, true); - NODELET_INFO_STREAM(" * Broadcast IMU pose TF\t-> " << (mPublishImuTf ? "ENABLED" : "DISABLED")); - // <---- TF broadcasting - - NODELET_INFO_STREAM("*** DYNAMIC PARAMETERS (Init. values) ***"); - - // ----> Dynamic - mNhNs.getParam("depth_confidence", mCamDepthConfidence); - NODELET_INFO_STREAM(" * [DYN] Depth confidence\t-> " << mCamDepthConfidence); - mNhNs.getParam("depth_texture_conf", mCamDepthTextureConf); - NODELET_INFO_STREAM(" * [DYN] Depth texture conf.\t-> " << mCamDepthTextureConf); - - mNhNs.getParam("pub_frame_rate", mVideoDepthFreq); - NODELET_INFO_STREAM(" * [DYN] pub_frame_rate\t\t-> " << mVideoDepthFreq << " Hz"); - mNhNs.getParam("point_cloud_freq", mPointCloudFreq); - NODELET_INFO_STREAM(" * [DYN] point_cloud_freq\t-> " << mPointCloudFreq << " Hz"); - mNhNs.getParam("brightness", mCamBrightness); - NODELET_INFO_STREAM(" * [DYN] brightness\t\t-> " << mCamBrightness); - mNhNs.getParam("contrast", mCamContrast); - NODELET_INFO_STREAM(" * [DYN] contrast\t\t-> " << mCamContrast); - mNhNs.getParam("hue", mCamHue); - NODELET_INFO_STREAM(" * [DYN] hue\t\t\t-> " << mCamHue); - mNhNs.getParam("saturation", mCamSaturation); - NODELET_INFO_STREAM(" * [DYN] saturation\t\t-> " << mCamSaturation); - mNhNs.getParam("sharpness", mCamSharpness); - NODELET_INFO_STREAM(" * [DYN] sharpness\t\t-> " << mCamSharpness); + mNhNs.getParam("mapping/fused_pointcloud_freq", mFusedPcPubFreq); + NODELET_INFO_STREAM(" * Fused point cloud freq:\t-> " << mFusedPcPubFreq << " Hz"); + } else { + NODELET_INFO_STREAM(" * Mapping\t\t\t-> DISABLED"); + } + // <---- Mapping + + NODELET_INFO_STREAM("*** OBJECT DETECTION PARAMETERS ***"); + + // ----> Object Detection + mNhNs.param("object_detection/od_enabled", mObjDetEnabled, false); + + if (mObjDetEnabled) { + NODELET_INFO_STREAM(" * Object Detection\t\t-> ENABLED"); + mNhNs.getParam("object_detection/confidence_threshold", mObjDetConfidence); + NODELET_INFO_STREAM(" * Object confidence\t\t-> " << mObjDetConfidence); + mNhNs.getParam("object_detection/object_tracking_enabled", mObjDetTracking); + NODELET_INFO_STREAM(" * Object tracking\t\t-> " << (mObjDetTracking ? "ENABLED" : "DISABLED")); + mNhNs.getParam("object_detection/max_range", mObjDetMaxRange); + if (mObjDetMaxRange > mCamMaxDepth) { + NODELET_WARN("Detection max range cannot be major than depth max range. Automatically fixed."); + mObjDetMaxRange = mCamMaxDepth; + } + NODELET_INFO_STREAM(" * Detection max range\t\t-> " << mObjDetMaxRange); + + int model; + mNhNs.getParam("object_detection/model", model); + if (model < 0 || model >= static_cast(sl::DETECTION_MODEL::LAST)) { + NODELET_WARN("Detection model not valid. Forced to the default value"); + model = static_cast(mObjDetModel); + } + mObjDetModel = static_cast(model); + + NODELET_INFO_STREAM(" * Detection model\t\t-> " << sl::toString(mObjDetModel)); + + if (mObjDetModel == sl::DETECTION_MODEL::HUMAN_BODY_ACCURATE || mObjDetModel == sl::DETECTION_MODEL::HUMAN_BODY_MEDIUM || mObjDetModel == sl::DETECTION_MODEL::HUMAN_BODY_FAST) { + mNhNs.getParam("object_detection/body_fitting", mObjDetBodyFitting); + NODELET_INFO_STREAM(" * Body fitting\t\t\t-> " << (mObjDetBodyFitting ? "ENABLED" : "DISABLED")); + } else { + mNhNs.getParam("object_detection/mc_people", mObjDetPeopleEnable); + NODELET_INFO_STREAM(" * Detect people\t\t-> " << (mObjDetPeopleEnable ? "ENABLED" : "DISABLED")); + mNhNs.getParam("object_detection/mc_vehicle", mObjDetVehiclesEnable); + NODELET_INFO_STREAM(" * Detect vehicles\t\t-> " << (mObjDetVehiclesEnable ? "ENABLED" : "DISABLED")); + mNhNs.getParam("object_detection/mc_bag", mObjDetBagsEnable); + NODELET_INFO_STREAM(" * Detect bags\t\t\t-> " << (mObjDetBagsEnable ? "ENABLED" : "DISABLED")); + mNhNs.getParam("object_detection/mc_animal", mObjDetAnimalsEnable); + NODELET_INFO_STREAM(" * Detect animals\t\t-> " << (mObjDetAnimalsEnable ? "ENABLED" : "DISABLED")); + mNhNs.getParam("object_detection/mc_electronics", mObjDetElectronicsEnable); + NODELET_INFO_STREAM(" * Detect electronics\t\t-> " << (mObjDetElectronicsEnable ? "ENABLED" : "DISABLED")); + mNhNs.getParam("object_detection/mc_fruit_vegetable", mObjDetFruitsEnable); + NODELET_INFO_STREAM(" * Detect fruit and vegetables\t-> " << (mObjDetFruitsEnable ? "ENABLED" : "DISABLED")); + mNhNs.getParam("object_detection/mc_sport", mObjDetSportsEnable); + NODELET_INFO_STREAM(" * Detect sport-related objects\t-> " << (mObjDetSportsEnable ? "ENABLED" : "DISABLED")); + } + } else if (mObjDetModel != sl::DETECTION_MODEL::PERSON_HEAD_BOX) { + NODELET_INFO_STREAM(" * Object Detection\t\t-> DISABLED"); + } + // <---- Object Detection + + NODELET_INFO_STREAM("*** SENSORS PARAMETERS ***"); + + // ----> Sensors + mNhNs.getParam("sensors/sensors_timestamp_sync", mSensTimestampSync); + NODELET_INFO_STREAM(" * Sensors timestamp sync\t-> " << (mSensTimestampSync ? "ENABLED" : "DISABLED")); + // <---- Sensors + + NODELET_INFO_STREAM("*** SVO PARAMETERS ***"); + + // ----> SVO + mNhNs.param("svo_file", mSvoFilepath, std::string()); + mSvoFilepath = sl_tools::resolveFilePath(mSvoFilepath); + NODELET_INFO_STREAM(" * SVO input file: \t\t-> " << mSvoFilepath.c_str()); + + int svo_compr = 0; + mNhNs.getParam("general/svo_compression", svo_compr); + + if (svo_compr >= static_cast(sl::SVO_COMPRESSION_MODE::LAST)) { + NODELET_WARN_STREAM("The parameter `general/svo_compression` has an invalid value. Please check it in the " + "configuration file `common.yaml`"); + + svo_compr = 0; + } + + mSvoComprMode = static_cast(svo_compr); + + NODELET_INFO_STREAM(" * SVO REC compression\t\t-> " << sl::toString(mSvoComprMode)); + // <---- SVO + + // Remote Stream + mNhNs.param("stream", mRemoteStreamAddr, std::string()); + + NODELET_INFO_STREAM("*** COORDINATE FRAMES ***"); + + // ----> Coordinate frames + mNhNs.param("pos_tracking/map_frame", mMapFrameId, "map"); + mNhNs.param("pos_tracking/odometry_frame", mOdometryFrameId, "odom"); + mNhNs.param("general/base_frame", mBaseFrameId, "base_link"); + + mCameraFrameId = mCameraName + "_camera_center"; + mImuFrameId = mCameraName + "_imu_link"; + mLeftCamFrameId = mCameraName + "_left_camera_frame"; + mLeftCamOptFrameId = mCameraName + "_left_camera_optical_frame"; + mRightCamFrameId = mCameraName + "_right_camera_frame"; + mRightCamOptFrameId = mCameraName + "_right_camera_optical_frame"; + + mBaroFrameId = mCameraName + "_baro_link"; + mMagFrameId = mCameraName + "_mag_link"; + mTempLeftFrameId = mCameraName + "_temp_left_link"; + mTempRightFrameId = mCameraName + "_temp_right_link"; + + mDepthFrameId = mLeftCamFrameId; + mDepthOptFrameId = mLeftCamOptFrameId; + + // Note: Depth image frame id must match color image frame id + mCloudFrameId = mDepthOptFrameId; + mRgbFrameId = mDepthFrameId; + mRgbOptFrameId = mCloudFrameId; + mDisparityFrameId = mDepthFrameId; + mDisparityOptFrameId = mDepthOptFrameId; + mConfidenceFrameId = mDepthFrameId; + mConfidenceOptFrameId = mDepthOptFrameId; + + // Print TF frames + NODELET_INFO_STREAM(" * map_frame\t\t\t-> " << mMapFrameId); + NODELET_INFO_STREAM(" * odometry_frame\t\t-> " << mOdometryFrameId); + NODELET_INFO_STREAM(" * base_frame\t\t\t-> " << mBaseFrameId); + NODELET_INFO_STREAM(" * camera_frame\t\t\t-> " << mCameraFrameId); + NODELET_INFO_STREAM(" * imu_link\t\t\t-> " << mImuFrameId); + NODELET_INFO_STREAM(" * left_camera_frame\t\t-> " << mLeftCamFrameId); + NODELET_INFO_STREAM(" * left_camera_optical_frame\t-> " << mLeftCamOptFrameId); + NODELET_INFO_STREAM(" * right_camera_frame\t\t-> " << mRightCamFrameId); + NODELET_INFO_STREAM(" * right_camera_optical_frame\t-> " << mRightCamOptFrameId); + NODELET_INFO_STREAM(" * depth_frame\t\t\t-> " << mDepthFrameId); + NODELET_INFO_STREAM(" * depth_optical_frame\t\t-> " << mDepthOptFrameId); + NODELET_INFO_STREAM(" * disparity_frame\t\t-> " << mDisparityFrameId); + NODELET_INFO_STREAM(" * disparity_optical_frame\t-> " << mDisparityOptFrameId); + NODELET_INFO_STREAM(" * confidence_frame\t\t-> " << mConfidenceFrameId); + NODELET_INFO_STREAM(" * confidence_optical_frame\t-> " << mConfidenceOptFrameId); + // <---- Coordinate frames + + // ----> TF broadcasting + mNhNs.param("pos_tracking/publish_tf", mPublishTf, true); + NODELET_INFO_STREAM(" * Broadcast odometry TF\t-> " << (mPublishTf ? "ENABLED" : "DISABLED")); + mNhNs.param("pos_tracking/publish_map_tf", mPublishMapTf, true); + NODELET_INFO_STREAM(" * Broadcast map pose TF\t-> " + << (mPublishTf ? (mPublishMapTf ? "ENABLED" : "DISABLED") : "DISABLED")); + mNhNs.param("sensors/publish_imu_tf", mPublishImuTf, true); + NODELET_INFO_STREAM(" * Broadcast IMU pose TF\t-> " << (mPublishImuTf ? "ENABLED" : "DISABLED")); + // <---- TF broadcasting + + NODELET_INFO_STREAM("*** DYNAMIC PARAMETERS (Init. values) ***"); + + // ----> Dynamic + mNhNs.getParam("depth_confidence", mCamDepthConfidence); + NODELET_INFO_STREAM(" * [DYN] Depth confidence\t-> " << mCamDepthConfidence); + mNhNs.getParam("depth_texture_conf", mCamDepthTextureConf); + NODELET_INFO_STREAM(" * [DYN] Depth texture conf.\t-> " << mCamDepthTextureConf); + + mNhNs.getParam("pub_frame_rate", mVideoDepthFreq); + NODELET_INFO_STREAM(" * [DYN] pub_frame_rate\t\t-> " << mVideoDepthFreq << " Hz"); + mNhNs.getParam("point_cloud_freq", mPointCloudFreq); + NODELET_INFO_STREAM(" * [DYN] point_cloud_freq\t-> " << mPointCloudFreq << " Hz"); + mNhNs.getParam("brightness", mCamBrightness); + NODELET_INFO_STREAM(" * [DYN] brightness\t\t-> " << mCamBrightness); + mNhNs.getParam("contrast", mCamContrast); + NODELET_INFO_STREAM(" * [DYN] contrast\t\t-> " << mCamContrast); + mNhNs.getParam("hue", mCamHue); + NODELET_INFO_STREAM(" * [DYN] hue\t\t\t-> " << mCamHue); + mNhNs.getParam("saturation", mCamSaturation); + NODELET_INFO_STREAM(" * [DYN] saturation\t\t-> " << mCamSaturation); + mNhNs.getParam("sharpness", mCamSharpness); + NODELET_INFO_STREAM(" * [DYN] sharpness\t\t-> " << mCamSharpness); #if (ZED_SDK_MAJOR_VERSION == 3 && ZED_SDK_MINOR_VERSION >= 1) - mNhNs.getParam("gamma", mCamGamma); - NODELET_INFO_STREAM(" * [DYN] gamma\t\t\t-> " << mCamGamma); + mNhNs.getParam("gamma", mCamGamma); + NODELET_INFO_STREAM(" * [DYN] gamma\t\t\t-> " << mCamGamma); #endif - mNhNs.getParam("auto_exposure_gain", mCamAutoExposure); - NODELET_INFO_STREAM(" * [DYN] auto_exposure_gain\t-> " << (mCamAutoExposure ? "ENABLED" : "DISABLED")); - mNhNs.getParam("gain", mCamGain); - mNhNs.getParam("exposure", mCamExposure); - if (!mCamAutoExposure) - { - NODELET_INFO_STREAM(" * [DYN] gain\t\t-> " << mCamGain); - NODELET_INFO_STREAM(" * [DYN] exposure\t\t-> " << mCamExposure); - } - mNhNs.getParam("auto_whitebalance", mCamAutoWB); - NODELET_INFO_STREAM(" * [DYN] auto_whitebalance\t-> " << (mCamAutoWB ? "ENABLED" : "DISABLED")); - mNhNs.getParam("whitebalance_temperature", mCamWB); - if (!mCamAutoWB) - { - NODELET_INFO_STREAM(" * [DYN] whitebalance_temperature\t\t-> " << mCamWB); - } - - if (mCamAutoExposure) - { - mTriggerAutoExposure = true; - } - if (mCamAutoWB) - { - mTriggerAutoWB = true; - } - // <---- Dynamic + mNhNs.getParam("auto_exposure_gain", mCamAutoExposure); + NODELET_INFO_STREAM(" * [DYN] auto_exposure_gain\t-> " << (mCamAutoExposure ? "ENABLED" : "DISABLED")); + mNhNs.getParam("gain", mCamGain); + mNhNs.getParam("exposure", mCamExposure); + if (!mCamAutoExposure) { + NODELET_INFO_STREAM(" * [DYN] gain\t\t-> " << mCamGain); + NODELET_INFO_STREAM(" * [DYN] exposure\t\t-> " << mCamExposure); + } + mNhNs.getParam("auto_whitebalance", mCamAutoWB); + NODELET_INFO_STREAM(" * [DYN] auto_whitebalance\t-> " << (mCamAutoWB ? "ENABLED" : "DISABLED")); + mNhNs.getParam("whitebalance_temperature", mCamWB); + if (!mCamAutoWB) { + NODELET_INFO_STREAM(" * [DYN] whitebalance_temperature\t\t-> " << mCamWB); + } + + if (mCamAutoExposure) { + mTriggerAutoExposure = true; + } + if (mCamAutoWB) { + mTriggerAutoWB = true; + } + // <---- Dynamic } void ZEDWrapperNodelet::checkResolFps() { - switch (mCamResol) - { + switch (mCamResol) { case sl::RESOLUTION::HD2K: - if (mCamFrameRate != 15) - { - NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD2K. Set to 15 FPS."); - mCamFrameRate = 15; - } + if (mCamFrameRate != 15) { + NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD2K. Set to 15 FPS."); + mCamFrameRate = 15; + } - break; + break; case sl::RESOLUTION::HD1080: - if (mCamFrameRate == 15 || mCamFrameRate == 30) - { - break; - } - - if (mCamFrameRate > 15 && mCamFrameRate < 30) - { - NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD1080. Set to 15 FPS."); - mCamFrameRate = 15; - } - else if (mCamFrameRate > 30) - { - NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD1080. Set to 30 FPS."); - mCamFrameRate = 30; - } - else - { - NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD1080. Set to 15 FPS."); - mCamFrameRate = 15; - } + if (mCamFrameRate == 15 || mCamFrameRate == 30) { + break; + } + + if (mCamFrameRate > 15 && mCamFrameRate < 30) { + NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD1080. Set to 15 FPS."); + mCamFrameRate = 15; + } else if (mCamFrameRate > 30) { + NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD1080. Set to 30 FPS."); + mCamFrameRate = 30; + } else { + NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD1080. Set to 15 FPS."); + mCamFrameRate = 15; + } - break; + break; case sl::RESOLUTION::HD720: - if (mCamFrameRate == 15 || mCamFrameRate == 30 || mCamFrameRate == 60) - { + if (mCamFrameRate == 15 || mCamFrameRate == 30 || mCamFrameRate == 60) { + break; + } + + if (mCamFrameRate > 15 && mCamFrameRate < 30) { + NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD720. Set to 15 FPS."); + mCamFrameRate = 15; + } else if (mCamFrameRate > 30 && mCamFrameRate < 60) { + NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD720. Set to 30 FPS."); + mCamFrameRate = 30; + } else if (mCamFrameRate > 60) { + NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD720. Set to 60 FPS."); + mCamFrameRate = 60; + } else { + NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD720. Set to 15 FPS."); + mCamFrameRate = 15; + } + break; - } - - if (mCamFrameRate > 15 && mCamFrameRate < 30) - { - NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD720. Set to 15 FPS."); - mCamFrameRate = 15; - } - else if (mCamFrameRate > 30 && mCamFrameRate < 60) - { - NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD720. Set to 30 FPS."); - mCamFrameRate = 30; - } - else if (mCamFrameRate > 60) - { - NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD720. Set to 60 FPS."); - mCamFrameRate = 60; - } - else - { - NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD720. Set to 15 FPS."); - mCamFrameRate = 15; - } - - break; case sl::RESOLUTION::VGA: - if (mCamFrameRate == 15 || mCamFrameRate == 30 || mCamFrameRate == 60 || mCamFrameRate == 100) - { + if (mCamFrameRate == 15 || mCamFrameRate == 30 || mCamFrameRate == 60 || mCamFrameRate == 100) { + break; + } + + if (mCamFrameRate > 15 && mCamFrameRate < 30) { + NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution VGA. Set to 15 FPS."); + mCamFrameRate = 15; + } else if (mCamFrameRate > 30 && mCamFrameRate < 60) { + NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution VGA. Set to 30 FPS."); + mCamFrameRate = 30; + } else if (mCamFrameRate > 60 && mCamFrameRate < 100) { + NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution VGA. Set to 60 FPS."); + mCamFrameRate = 60; + } else if (mCamFrameRate > 100) { + NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution VGA. Set to 100 FPS."); + mCamFrameRate = 100; + } else { + NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution VGA. Set to 15 FPS."); + mCamFrameRate = 15; + } + break; - } - - if (mCamFrameRate > 15 && mCamFrameRate < 30) - { - NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution VGA. Set to 15 FPS."); - mCamFrameRate = 15; - } - else if (mCamFrameRate > 30 && mCamFrameRate < 60) - { - NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution VGA. Set to 30 FPS."); - mCamFrameRate = 30; - } - else if (mCamFrameRate > 60 && mCamFrameRate < 100) - { - NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution VGA. Set to 60 FPS."); - mCamFrameRate = 60; - } - else if (mCamFrameRate > 100) - { - NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution VGA. Set to 100 FPS."); - mCamFrameRate = 100; - } - else - { - NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution VGA. Set to 15 FPS."); - mCamFrameRate = 15; - } - - break; default: - NODELET_WARN_STREAM("Invalid resolution. Set to HD720 @ 30 FPS"); - mCamResol = sl::RESOLUTION::HD720; - mCamFrameRate = 30; - } + NODELET_WARN_STREAM("Invalid resolution. Set to HD720 @ 30 FPS"); + mCamResol = sl::RESOLUTION::HD720; + mCamFrameRate = 30; + } } void ZEDWrapperNodelet::initTransforms() { - // According to REP 105 -> http://www.ros.org/reps/rep-0105.html - - // base_link <- odom <- map - // ^ | - // | | - // ------------------- - - // ----> Dynamic transforms - mOdom2BaseTransf.setIdentity(); // broadcasted if `publish_tf` is true - mMap2OdomTransf.setIdentity(); // broadcasted if `publish_map_tf` is true - mMap2BaseTransf.setIdentity(); // used internally, but not broadcasted - mMap2CameraTransf.setIdentity(); // used internally, but not broadcasted - // <---- Dynamic transforms + // According to REP 105 -> http://www.ros.org/reps/rep-0105.html + + // base_link <- odom <- map + // ^ | + // | | + // ------------------- + + // ----> Dynamic transforms + mOdom2BaseTransf.setIdentity(); // broadcasted if `publish_tf` is true + mMap2OdomTransf.setIdentity(); // broadcasted if `publish_map_tf` is true + mMap2BaseTransf.setIdentity(); // used internally, but not broadcasted + mMap2CameraTransf.setIdentity(); // used internally, but not broadcasted + // <---- Dynamic transforms } 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; - - // ----> Static transforms - // Sensor to Base link - try - { - // Save the transformation - geometry_msgs::TransformStamped c2b = - mTfBuffer->lookupTransform(mCameraFrameId, mBaseFrameId, ros::Time(0), ros::Duration(0.1)); - - // Get the TF2 transformation - tf2::fromMsg(c2b.transform, mCamera2BaseTransf); - - double roll, pitch, yaw; - tf2::Matrix3x3(mCamera2BaseTransf.getRotation()).getRPY(roll, pitch, yaw); - - NODELET_INFO("Static transform Camera Center to Base [%s -> %s]", mCameraFrameId.c_str(), mBaseFrameId.c_str()); - NODELET_INFO(" * Translation: {%.3f,%.3f,%.3f}", mCamera2BaseTransf.getOrigin().x(), - mCamera2BaseTransf.getOrigin().y(), mCamera2BaseTransf.getOrigin().z()); - NODELET_INFO(" * Rotation: {%.3f,%.3f,%.3f}", roll * RAD2DEG, pitch * RAD2DEG, yaw * RAD2DEG); - } - catch (tf2::TransformException& ex) - { - if (!first_error) - { - NODELET_DEBUG_THROTTLE(1.0, "Transform error: %s", ex.what()); - NODELET_WARN_THROTTLE(1.0, "The tf from '%s' to '%s' is not available.", mCameraFrameId.c_str(), - mBaseFrameId.c_str()); - NODELET_WARN_THROTTLE(1.0, - "Note: one of the possible cause of the problem is the absense of an instance " - "of the `robot_state_publisher` node publishing the correct static TF transformations " - "or a modified URDF not correctly reproducing the ZED " - "TF chain '%s' -> '%s' -> '%s'", - mBaseFrameId.c_str(), mCameraFrameId.c_str(), mDepthFrameId.c_str()); - first_error = false; - } - - mCamera2BaseTransf.setIdentity(); - return false; - } + NODELET_DEBUG("Getting static TF from '%s' to '%s'", mCameraFrameId.c_str(), mBaseFrameId.c_str()); + + mCamera2BaseTransfValid = false; + static bool first_error = true; + + // ----> Static transforms + // Sensor to Base link + try { + // Save the transformation + geometry_msgs::TransformStamped c2b = mTfBuffer->lookupTransform(mCameraFrameId, mBaseFrameId, ros::Time(0), ros::Duration(0.1)); + + // Get the TF2 transformation + tf2::fromMsg(c2b.transform, mCamera2BaseTransf); + + double roll, pitch, yaw; + tf2::Matrix3x3(mCamera2BaseTransf.getRotation()).getRPY(roll, pitch, yaw); + + NODELET_INFO("Static transform Camera Center to Base [%s -> %s]", mCameraFrameId.c_str(), mBaseFrameId.c_str()); + NODELET_INFO(" * Translation: {%.3f,%.3f,%.3f}", mCamera2BaseTransf.getOrigin().x(), + mCamera2BaseTransf.getOrigin().y(), mCamera2BaseTransf.getOrigin().z()); + NODELET_INFO(" * Rotation: {%.3f,%.3f,%.3f}", roll * RAD2DEG, pitch * RAD2DEG, yaw * RAD2DEG); + } catch (tf2::TransformException& ex) { + if (!first_error) { + NODELET_DEBUG_THROTTLE(1.0, "Transform error: %s", ex.what()); + NODELET_WARN_THROTTLE(1.0, "The tf from '%s' to '%s' is not available.", mCameraFrameId.c_str(), + mBaseFrameId.c_str()); + NODELET_WARN_THROTTLE(1.0, + "Note: one of the possible cause of the problem is the absense of an instance " + "of the `robot_state_publisher` node publishing the correct static TF transformations " + "or a modified URDF not correctly reproducing the ZED " + "TF chain '%s' -> '%s' -> '%s'", + mBaseFrameId.c_str(), mCameraFrameId.c_str(), mDepthFrameId.c_str()); + first_error = false; + } + + mCamera2BaseTransf.setIdentity(); + return false; + } - // <---- Static transforms - mCamera2BaseTransfValid = true; - return true; + // <---- Static transforms + mCamera2BaseTransfValid = true; + return true; } 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; - - // ----> Static transforms - // Sensor to Camera Center - try - { - // Save the transformation - geometry_msgs::TransformStamped s2c = - mTfBuffer->lookupTransform(mDepthFrameId, mCameraFrameId, ros::Time(0), ros::Duration(0.1)); - // Get the TF2 transformation - tf2::fromMsg(s2c.transform, mSensor2CameraTransf); - - double roll, pitch, yaw; - tf2::Matrix3x3(mSensor2CameraTransf.getRotation()).getRPY(roll, pitch, yaw); - - NODELET_INFO("Static transform Sensor to Camera Center [%s -> %s]", mDepthFrameId.c_str(), mCameraFrameId.c_str()); - NODELET_INFO(" * Translation: {%.3f,%.3f,%.3f}", mSensor2CameraTransf.getOrigin().x(), - mSensor2CameraTransf.getOrigin().y(), mSensor2CameraTransf.getOrigin().z()); - NODELET_INFO(" * Rotation: {%.3f,%.3f,%.3f}", roll * RAD2DEG, pitch * RAD2DEG, yaw * RAD2DEG); - } - catch (tf2::TransformException& ex) - { - if (!first_error) - { - NODELET_DEBUG_THROTTLE(1.0, "Transform error: %s", ex.what()); - NODELET_WARN_THROTTLE(1.0, "The tf from '%s' to '%s' is not available.", mDepthFrameId.c_str(), - mCameraFrameId.c_str()); - NODELET_WARN_THROTTLE(1.0, - "Note: one of the possible cause of the problem is the absense of an instance " - "of the `robot_state_publisher` node publishing the correct static TF transformations " - "or a modified URDF not correctly reproducing the ZED " - "TF chain '%s' -> '%s' -> '%s'", - mBaseFrameId.c_str(), mCameraFrameId.c_str(), mDepthFrameId.c_str()); - first_error = false; - } - - mSensor2CameraTransf.setIdentity(); - return false; - } + NODELET_DEBUG("Getting static TF from '%s' to '%s'", mDepthFrameId.c_str(), mCameraFrameId.c_str()); + + mSensor2CameraTransfValid = false; + + static bool first_error = true; + + // ----> Static transforms + // Sensor to Camera Center + try { + // Save the transformation + geometry_msgs::TransformStamped s2c = mTfBuffer->lookupTransform(mDepthFrameId, mCameraFrameId, ros::Time(0), ros::Duration(0.1)); + // Get the TF2 transformation + tf2::fromMsg(s2c.transform, mSensor2CameraTransf); - // <---- Static transforms + double roll, pitch, yaw; + tf2::Matrix3x3(mSensor2CameraTransf.getRotation()).getRPY(roll, pitch, yaw); + + NODELET_INFO("Static transform Sensor to Camera Center [%s -> %s]", mDepthFrameId.c_str(), mCameraFrameId.c_str()); + NODELET_INFO(" * Translation: {%.3f,%.3f,%.3f}", mSensor2CameraTransf.getOrigin().x(), + mSensor2CameraTransf.getOrigin().y(), mSensor2CameraTransf.getOrigin().z()); + NODELET_INFO(" * Rotation: {%.3f,%.3f,%.3f}", roll * RAD2DEG, pitch * RAD2DEG, yaw * RAD2DEG); + } catch (tf2::TransformException& ex) { + if (!first_error) { + NODELET_DEBUG_THROTTLE(1.0, "Transform error: %s", ex.what()); + NODELET_WARN_THROTTLE(1.0, "The tf from '%s' to '%s' is not available.", mDepthFrameId.c_str(), + mCameraFrameId.c_str()); + NODELET_WARN_THROTTLE(1.0, + "Note: one of the possible cause of the problem is the absense of an instance " + "of the `robot_state_publisher` node publishing the correct static TF transformations " + "or a modified URDF not correctly reproducing the ZED " + "TF chain '%s' -> '%s' -> '%s'", + mBaseFrameId.c_str(), mCameraFrameId.c_str(), mDepthFrameId.c_str()); + first_error = false; + } + + mSensor2CameraTransf.setIdentity(); + return false; + } - mSensor2CameraTransfValid = true; - return true; + // <---- Static transforms + + mSensor2CameraTransfValid = true; + return true; } 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; - - // ----> Static transforms - // Sensor to Base link - try - { - // Save the transformation - geometry_msgs::TransformStamped s2b = - mTfBuffer->lookupTransform(mDepthFrameId, mBaseFrameId, ros::Time(0), ros::Duration(0.1)); - // Get the TF2 transformation - tf2::fromMsg(s2b.transform, mSensor2BaseTransf); - - double roll, pitch, yaw; - tf2::Matrix3x3(mSensor2BaseTransf.getRotation()).getRPY(roll, pitch, yaw); - - NODELET_INFO("Static transform Sensor to Base [%s -> %s]", mDepthFrameId.c_str(), mBaseFrameId.c_str()); - NODELET_INFO(" * Translation: {%.3f,%.3f,%.3f}", mSensor2BaseTransf.getOrigin().x(), - mSensor2BaseTransf.getOrigin().y(), mSensor2BaseTransf.getOrigin().z()); - NODELET_INFO(" * Rotation: {%.3f,%.3f,%.3f}", roll * RAD2DEG, pitch * RAD2DEG, yaw * RAD2DEG); - } - catch (tf2::TransformException& ex) - { - if (!first_error) - { - NODELET_DEBUG_THROTTLE(1.0, "Transform error: %s", ex.what()); - NODELET_WARN_THROTTLE(1.0, "The tf from '%s' to '%s' is not available.", mDepthFrameId.c_str(), - mBaseFrameId.c_str()); - NODELET_WARN_THROTTLE(1.0, - "Note: one of the possible cause of the problem is the absense of an instance " - "of the `robot_state_publisher` node publishing the correct static TF transformations " - "or a modified URDF not correctly reproducing the ZED " - "TF chain '%s' -> '%s' -> '%s'", - mBaseFrameId.c_str(), mCameraFrameId.c_str(), mDepthFrameId.c_str()); - first_error = false; - } - - mSensor2BaseTransf.setIdentity(); - return false; - } + NODELET_DEBUG("Getting static TF from '%s' to '%s'", mDepthFrameId.c_str(), mBaseFrameId.c_str()); + + mSensor2BaseTransfValid = false; + static bool first_error = true; - // <---- Static transforms + // ----> Static transforms + // Sensor to Base link + try { + // Save the transformation + geometry_msgs::TransformStamped s2b = mTfBuffer->lookupTransform(mDepthFrameId, mBaseFrameId, ros::Time(0), ros::Duration(0.1)); + // Get the TF2 transformation + tf2::fromMsg(s2b.transform, mSensor2BaseTransf); - mSensor2BaseTransfValid = true; - return true; + double roll, pitch, yaw; + tf2::Matrix3x3(mSensor2BaseTransf.getRotation()).getRPY(roll, pitch, yaw); + + NODELET_INFO("Static transform Sensor to Base [%s -> %s]", mDepthFrameId.c_str(), mBaseFrameId.c_str()); + NODELET_INFO(" * Translation: {%.3f,%.3f,%.3f}", mSensor2BaseTransf.getOrigin().x(), + mSensor2BaseTransf.getOrigin().y(), mSensor2BaseTransf.getOrigin().z()); + NODELET_INFO(" * Rotation: {%.3f,%.3f,%.3f}", roll * RAD2DEG, pitch * RAD2DEG, yaw * RAD2DEG); + } catch (tf2::TransformException& ex) { + if (!first_error) { + NODELET_DEBUG_THROTTLE(1.0, "Transform error: %s", ex.what()); + NODELET_WARN_THROTTLE(1.0, "The tf from '%s' to '%s' is not available.", mDepthFrameId.c_str(), + mBaseFrameId.c_str()); + NODELET_WARN_THROTTLE(1.0, + "Note: one of the possible cause of the problem is the absense of an instance " + "of the `robot_state_publisher` node publishing the correct static TF transformations " + "or a modified URDF not correctly reproducing the ZED " + "TF chain '%s' -> '%s' -> '%s'", + mBaseFrameId.c_str(), mCameraFrameId.c_str(), mDepthFrameId.c_str()); + first_error = false; + } + + mSensor2BaseTransf.setIdentity(); + return false; + } + + // <---- Static transforms + + mSensor2BaseTransfValid = true; + return true; } bool ZEDWrapperNodelet::set_pose(float xt, float yt, float zt, float rr, float pr, float yr) { - initTransforms(); - - if (!mSensor2BaseTransfValid) - { - getSens2BaseTransform(); - } - - if (!mSensor2CameraTransfValid) - { - getSens2CameraTransform(); - } - - if (!mCamera2BaseTransfValid) - { - getCamera2BaseTransform(); - } - - // Apply Base to sensor transform - tf2::Transform initPose; - tf2::Vector3 origin(xt, yt, zt); - initPose.setOrigin(origin); - tf2::Quaternion quat; - quat.setRPY(rr, pr, yr); - initPose.setRotation(quat); - - initPose = initPose * mSensor2BaseTransf.inverse(); - - // SL pose - sl::float3 t_vec; - t_vec[0] = initPose.getOrigin().x(); - t_vec[1] = initPose.getOrigin().y(); - t_vec[2] = initPose.getOrigin().z(); - - sl::float4 q_vec; - q_vec[0] = initPose.getRotation().x(); - q_vec[1] = initPose.getRotation().y(); - q_vec[2] = initPose.getRotation().z(); - q_vec[3] = initPose.getRotation().w(); - - sl::Translation trasl(t_vec); - sl::Orientation orient(q_vec); - mInitialPoseSl.setTranslation(trasl); - mInitialPoseSl.setOrientation(orient); - - return (mSensor2BaseTransfValid & mSensor2CameraTransfValid & mCamera2BaseTransfValid); + initTransforms(); + + if (!mSensor2BaseTransfValid) { + getSens2BaseTransform(); + } + + if (!mSensor2CameraTransfValid) { + getSens2CameraTransform(); + } + + if (!mCamera2BaseTransfValid) { + getCamera2BaseTransform(); + } + + // Apply Base to sensor transform + tf2::Transform initPose; + tf2::Vector3 origin(xt, yt, zt); + initPose.setOrigin(origin); + tf2::Quaternion quat; + quat.setRPY(rr, pr, yr); + initPose.setRotation(quat); + + initPose = initPose * mSensor2BaseTransf.inverse(); + + // SL pose + sl::float3 t_vec; + t_vec[0] = initPose.getOrigin().x(); + t_vec[1] = initPose.getOrigin().y(); + t_vec[2] = initPose.getOrigin().z(); + + sl::float4 q_vec; + q_vec[0] = initPose.getRotation().x(); + q_vec[1] = initPose.getRotation().y(); + q_vec[2] = initPose.getRotation().z(); + q_vec[3] = initPose.getRotation().w(); + + sl::Translation trasl(t_vec); + sl::Orientation orient(q_vec); + mInitialPoseSl.setTranslation(trasl); + mInitialPoseSl.setOrientation(orient); + + return (mSensor2BaseTransfValid & mSensor2CameraTransfValid & mCamera2BaseTransfValid); } bool ZEDWrapperNodelet::on_set_pose(zed_interfaces::set_pose::Request& req, zed_interfaces::set_pose::Response& res) { - mInitialBasePose.resize(6); - mInitialBasePose[0] = req.x; - mInitialBasePose[1] = req.y; - mInitialBasePose[2] = req.z; - mInitialBasePose[3] = req.R; - mInitialBasePose[4] = req.P; - mInitialBasePose[5] = req.Y; - - if (!set_pose(mInitialBasePose[0], mInitialBasePose[1], mInitialBasePose[2], mInitialBasePose[3], mInitialBasePose[4], - mInitialBasePose[5])) - { - res.done = false; - return false; - } + mInitialBasePose.resize(6); + mInitialBasePose[0] = req.x; + mInitialBasePose[1] = req.y; + mInitialBasePose[2] = req.z; + mInitialBasePose[3] = req.R; + mInitialBasePose[4] = req.P; + mInitialBasePose[5] = req.Y; + + if (!set_pose(mInitialBasePose[0], mInitialBasePose[1], mInitialBasePose[2], mInitialBasePose[3], mInitialBasePose[4], + mInitialBasePose[5])) { + res.done = false; + return false; + } - std::lock_guard lock(mPosTrkMutex); + std::lock_guard lock(mPosTrkMutex); - // Disable tracking - mPosTrackingActivated = false; - mZed.disablePositionalTracking(); + // Disable tracking + mPosTrackingActivated = false; + mZed.disablePositionalTracking(); - // Restart tracking - start_pos_tracking(); + // Restart tracking + start_pos_tracking(); - res.done = true; - return true; + res.done = true; + return true; } bool ZEDWrapperNodelet::on_reset_tracking(zed_interfaces::reset_tracking::Request& req, - zed_interfaces::reset_tracking::Response& res) + zed_interfaces::reset_tracking::Response& res) { - if (!mPosTrackingActivated) - { - res.reset_done = false; - return false; - } + if (!mPosTrackingActivated) { + res.reset_done = false; + return false; + } - if (!set_pose(mInitialBasePose[0], mInitialBasePose[1], mInitialBasePose[2], mInitialBasePose[3], mInitialBasePose[4], - mInitialBasePose[5])) - { - res.reset_done = false; - return false; - } + if (!set_pose(mInitialBasePose[0], mInitialBasePose[1], mInitialBasePose[2], mInitialBasePose[3], mInitialBasePose[4], + mInitialBasePose[5])) { + res.reset_done = false; + return false; + } - std::lock_guard lock(mPosTrkMutex); + std::lock_guard lock(mPosTrkMutex); - // Disable tracking - mPosTrackingActivated = false; - mZed.disablePositionalTracking(); + // Disable tracking + mPosTrackingActivated = false; + mZed.disablePositionalTracking(); - // Restart tracking - start_pos_tracking(); + // Restart tracking + start_pos_tracking(); - res.reset_done = true; - return true; + res.reset_done = true; + return true; } bool ZEDWrapperNodelet::on_reset_odometry(zed_interfaces::reset_odometry::Request& req, - zed_interfaces::reset_odometry::Response& res) + zed_interfaces::reset_odometry::Response& res) { - mResetOdom = true; - res.reset_done = true; - return true; + mResetOdom = true; + res.reset_done = true; + return true; } bool ZEDWrapperNodelet::start_3d_mapping() { - if (!mMappingEnabled) - { - return false; - } - - NODELET_INFO_STREAM("*** Starting Spatial Mapping ***"); - - sl::SpatialMappingParameters params; - params.map_type = sl::SpatialMappingParameters::SPATIAL_MAP_TYPE::FUSED_POINT_CLOUD; - params.use_chunk_only = true; - - sl::SpatialMappingParameters spMapPar; - - float lRes = spMapPar.allowed_resolution.first; - float hRes = spMapPar.allowed_resolution.second; - - if (mMappingRes < lRes) - { - NODELET_WARN_STREAM("'mapping/resolution' value (" - << mMappingRes << " m) is lower than the allowed resolution values. Fixed automatically"); - mMappingRes = lRes; - } - if (mMappingRes > hRes) - { - NODELET_WARN_STREAM("'mapping/resolution' value (" - << mMappingRes << " m) is higher than the allowed resolution values. Fixed automatically"); - mMappingRes = hRes; - } - - params.resolution_meter = mMappingRes; - - float lRng = spMapPar.allowed_range.first; - float hRng = spMapPar.allowed_range.second; - - if (mMaxMappingRange < 0) - { - mMaxMappingRange = sl::SpatialMappingParameters::getRecommendedRange(mMappingRes, mZed); - NODELET_INFO_STREAM("Mapping: max range set to " << mMaxMappingRange << " m for a resolution of " << mMappingRes - << " m"); - } - else if (mMaxMappingRange < lRng) - { - NODELET_WARN_STREAM("'mapping/max_mapping_range_m' value (" - << mMaxMappingRange << " m) is lower than the allowed resolution values. Fixed automatically"); - mMaxMappingRange = lRng; - } - else if (mMaxMappingRange > hRng) - { - NODELET_WARN_STREAM("'mapping/max_mapping_range_m' value (" - << mMaxMappingRange << " m) is higher than the allowed resolution values. Fixed automatically"); - mMaxMappingRange = hRng; - } - - params.range_meter = mMaxMappingRange; - - sl::ERROR_CODE err = mZed.enableSpatialMapping(params); - - if (err == sl::ERROR_CODE::SUCCESS) - { - if (mPubFusedCloud.getTopic().empty()) - { - string pointcloud_fused_topic = "mapping/fused_cloud"; - mPubFusedCloud = mNhNs.advertise(pointcloud_fused_topic, 1); - NODELET_INFO_STREAM("Advertised on topic " << mPubFusedCloud.getTopic() << " @ " << mFusedPcPubFreq << " Hz"); - } - - mMappingRunning = true; - - mFusedPcTimer = - mNhNs.createTimer(ros::Duration(1.0 / mFusedPcPubFreq), &ZEDWrapperNodelet::callback_pubFusedPointCloud, this); - - NODELET_INFO_STREAM(" * Resolution: " << params.resolution_meter << " m"); - NODELET_INFO_STREAM(" * Max Mapping Range: " << params.range_meter << " m"); - NODELET_INFO_STREAM(" * Map point cloud publishing rate: " << mFusedPcPubFreq << " Hz"); + if (!mMappingEnabled) { + return false; + } - return true; - } - else - { - mMappingRunning = false; - mFusedPcTimer.stop(); + NODELET_INFO_STREAM("*** Starting Spatial Mapping ***"); - NODELET_WARN("Mapping not activated: %s", sl::toString(err).c_str()); + sl::SpatialMappingParameters params; + params.map_type = sl::SpatialMappingParameters::SPATIAL_MAP_TYPE::FUSED_POINT_CLOUD; + params.use_chunk_only = true; - return false; - } + sl::SpatialMappingParameters spMapPar; + + float lRes = spMapPar.allowed_resolution.first; + float hRes = spMapPar.allowed_resolution.second; + + if (mMappingRes < lRes) { + NODELET_WARN_STREAM("'mapping/resolution' value (" + << mMappingRes << " m) is lower than the allowed resolution values. Fixed automatically"); + mMappingRes = lRes; + } + if (mMappingRes > hRes) { + NODELET_WARN_STREAM("'mapping/resolution' value (" + << mMappingRes << " m) is higher than the allowed resolution values. Fixed automatically"); + mMappingRes = hRes; + } + + params.resolution_meter = mMappingRes; + + float lRng = spMapPar.allowed_range.first; + float hRng = spMapPar.allowed_range.second; + + if (mMaxMappingRange < 0) { + mMaxMappingRange = sl::SpatialMappingParameters::getRecommendedRange(mMappingRes, mZed); + NODELET_INFO_STREAM("Mapping: max range set to " << mMaxMappingRange << " m for a resolution of " << mMappingRes + << " m"); + } else if (mMaxMappingRange < lRng) { + NODELET_WARN_STREAM("'mapping/max_mapping_range_m' value (" + << mMaxMappingRange << " m) is lower than the allowed resolution values. Fixed automatically"); + mMaxMappingRange = lRng; + } else if (mMaxMappingRange > hRng) { + NODELET_WARN_STREAM("'mapping/max_mapping_range_m' value (" + << mMaxMappingRange << " m) is higher than the allowed resolution values. Fixed automatically"); + mMaxMappingRange = hRng; + } + + params.range_meter = mMaxMappingRange; + + sl::ERROR_CODE err = mZed.enableSpatialMapping(params); + + if (err == sl::ERROR_CODE::SUCCESS) { + if (mPubFusedCloud.getTopic().empty()) { + string pointcloud_fused_topic = "mapping/fused_cloud"; + mPubFusedCloud = mNhNs.advertise(pointcloud_fused_topic, 1); + NODELET_INFO_STREAM("Advertised on topic " << mPubFusedCloud.getTopic() << " @ " << mFusedPcPubFreq << " Hz"); + } + + mMappingRunning = true; + + mFusedPcTimer = mNhNs.createTimer(ros::Duration(1.0 / mFusedPcPubFreq), &ZEDWrapperNodelet::callback_pubFusedPointCloud, this); + + NODELET_INFO_STREAM(" * Resolution: " << params.resolution_meter << " m"); + NODELET_INFO_STREAM(" * Max Mapping Range: " << params.range_meter << " m"); + NODELET_INFO_STREAM(" * Map point cloud publishing rate: " << mFusedPcPubFreq << " Hz"); + + return true; + } else { + mMappingRunning = false; + mFusedPcTimer.stop(); + + NODELET_WARN("Mapping not activated: %s", sl::toString(err).c_str()); + + return false; + } } void ZEDWrapperNodelet::stop_3d_mapping() { - mFusedPcTimer.stop(); - mMappingRunning = false; - mMappingEnabled = false; - mZed.disableSpatialMapping(); + mFusedPcTimer.stop(); + mMappingRunning = false; + mMappingEnabled = false; + mZed.disableSpatialMapping(); - NODELET_INFO("*** Spatial Mapping stopped ***"); + NODELET_INFO("*** Spatial Mapping stopped ***"); } bool ZEDWrapperNodelet::start_obj_detect() { - if (mZedRealCamModel != sl::MODEL::ZED2 && mZedRealCamModel != sl::MODEL::ZED2i) - { - NODELET_ERROR_STREAM("Object detection not started. OD is available only using a ZED2/ZED2i camera model"); - return false; - } + if (mZedRealCamModel == sl::MODEL::ZED) { + NODELET_ERROR_STREAM("Object detection not started. OD is not available for ZED camera model"); + return false; + } - if (!mObjDetEnabled) - { - return false; - } + if (!mObjDetEnabled) { + return false; + } - if (!mCamera2BaseTransfValid || !mSensor2CameraTransfValid || !mSensor2BaseTransfValid) - { - NODELET_DEBUG("Tracking transforms not yet ready, OD starting postponed"); - return false; - } + if (!mCamera2BaseTransfValid || !mSensor2CameraTransfValid || !mSensor2BaseTransfValid) { + NODELET_DEBUG("Tracking transforms not yet ready, OD starting postponed"); + return false; + } - NODELET_INFO_STREAM("*** Starting Object Detection ***"); + NODELET_INFO_STREAM("*** Starting Object Detection ***"); - sl::ObjectDetectionParameters od_p; - od_p.enable_mask_output = false; - od_p.enable_tracking = mObjDetTracking; - od_p.image_sync = false; // Asynchronous object detection - od_p.detection_model = mObjDetModel; - od_p.enable_body_fitting = mObjDetBodyFitting; - od_p.max_range = mObjDetMaxRange; + sl::ObjectDetectionParameters od_p; + od_p.enable_mask_output = false; + od_p.enable_tracking = mObjDetTracking; + od_p.image_sync = false; // Asynchronous object detection + od_p.detection_model = mObjDetModel; + od_p.enable_body_fitting = mObjDetBodyFitting; + od_p.max_range = mObjDetMaxRange; - sl::ERROR_CODE objDetError = mZed.enableObjectDetection(od_p); + sl::ERROR_CODE objDetError = mZed.enableObjectDetection(od_p); - if (objDetError != sl::ERROR_CODE::SUCCESS) - { - NODELET_ERROR_STREAM("Object detection error: " << sl::toString(objDetError)); + if (objDetError != sl::ERROR_CODE::SUCCESS) { + NODELET_ERROR_STREAM("Object detection error: " << sl::toString(objDetError)); - mObjDetRunning = false; - return false; - } + mObjDetRunning = false; + return false; + } - if (mPubObjDet.getTopic().empty()) - { - string object_det_topic_root = "obj_det"; - string object_det_topic = object_det_topic_root + "/objects"; + if (mPubObjDet.getTopic().empty()) { + string object_det_topic_root = "obj_det"; + string object_det_topic = object_det_topic_root + "/objects"; - mPubObjDet = mNhNs.advertise(object_det_topic, 1); - NODELET_INFO_STREAM("Advertised on topic " << mPubObjDet.getTopic()); - } + mPubObjDet = mNhNs.advertise(object_det_topic, 1); + NODELET_INFO_STREAM("Advertised on topic " << mPubObjDet.getTopic()); + } - mObjDetFilter.clear(); + mObjDetFilter.clear(); - if (mObjDetModel == sl::DETECTION_MODEL::MULTI_CLASS_BOX || - mObjDetModel == sl::DETECTION_MODEL::MULTI_CLASS_BOX_ACCURATE) - { - if (mObjDetPeopleEnable) - { - mObjDetFilter.push_back(sl::OBJECT_CLASS::PERSON); - } - if (mObjDetVehiclesEnable) - { - mObjDetFilter.push_back(sl::OBJECT_CLASS::VEHICLE); - } - if (mObjDetBagsEnable) - { - mObjDetFilter.push_back(sl::OBJECT_CLASS::BAG); - } - if (mObjDetAnimalsEnable) - { - mObjDetFilter.push_back(sl::OBJECT_CLASS::ANIMAL); - } - if (mObjDetElectronicsEnable) - { - mObjDetFilter.push_back(sl::OBJECT_CLASS::ELECTRONICS); - } - if (mObjDetFruitsEnable) - { - mObjDetFilter.push_back(sl::OBJECT_CLASS::FRUIT_VEGETABLE); + if (mObjDetModel == sl::DETECTION_MODEL::MULTI_CLASS_BOX || mObjDetModel == sl::DETECTION_MODEL::MULTI_CLASS_BOX_MEDIUM || mObjDetModel == sl::DETECTION_MODEL::MULTI_CLASS_BOX_ACCURATE) { + if (mObjDetPeopleEnable) { + mObjDetFilter.push_back(sl::OBJECT_CLASS::PERSON); + } + if (mObjDetVehiclesEnable) { + mObjDetFilter.push_back(sl::OBJECT_CLASS::VEHICLE); + } + if (mObjDetBagsEnable) { + mObjDetFilter.push_back(sl::OBJECT_CLASS::BAG); + } + if (mObjDetAnimalsEnable) { + mObjDetFilter.push_back(sl::OBJECT_CLASS::ANIMAL); + } + if (mObjDetElectronicsEnable) { + mObjDetFilter.push_back(sl::OBJECT_CLASS::ELECTRONICS); + } + if (mObjDetFruitsEnable) { + mObjDetFilter.push_back(sl::OBJECT_CLASS::FRUIT_VEGETABLE); + } + if (mObjDetSportsEnable) { + mObjDetFilter.push_back(sl::OBJECT_CLASS::SPORT); + } } - } - mObjDetRunning = true; - return false; + mObjDetRunning = true; + return false; } void ZEDWrapperNodelet::stop_obj_detect() { - if (mObjDetRunning) - { - NODELET_INFO_STREAM("*** Stopping Object Detection ***"); - mObjDetRunning = false; - mObjDetEnabled = false; - mZed.disableObjectDetection(); - } + if (mObjDetRunning) { + NODELET_INFO_STREAM("*** Stopping Object Detection ***"); + mObjDetRunning = false; + mObjDetEnabled = false; + mZed.disableObjectDetection(); + } } void ZEDWrapperNodelet::start_pos_tracking() { - NODELET_INFO_STREAM("*** Starting Positional Tracking ***"); + NODELET_INFO_STREAM("*** Starting Positional Tracking ***"); - NODELET_INFO(" * Waiting for valid static transformations..."); + NODELET_INFO(" * Waiting for valid static transformations..."); - bool transformOk = false; - double elapsed = 0.0; + bool transformOk = false; + double elapsed = 0.0; - auto start = std::chrono::high_resolution_clock::now(); + auto start = std::chrono::high_resolution_clock::now(); - do - { - transformOk = set_pose(mInitialBasePose[0], mInitialBasePose[1], mInitialBasePose[2], mInitialBasePose[3], - mInitialBasePose[4], mInitialBasePose[5]); + do { + transformOk = set_pose(mInitialBasePose[0], mInitialBasePose[1], mInitialBasePose[2], mInitialBasePose[3], + mInitialBasePose[4], mInitialBasePose[5]); - elapsed = std::chrono::duration_cast(std::chrono::high_resolution_clock::now() - start) - .count(); + elapsed = std::chrono::duration_cast(std::chrono::high_resolution_clock::now() - start) + .count(); - std::this_thread::sleep_for(std::chrono::milliseconds(100)); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); - if (elapsed > 10000) - { - NODELET_WARN(" !!! Failed to get static transforms. Is the 'ROBOT STATE PUBLISHER' node correctly working? "); - break; - } + if (elapsed > 10000) { + NODELET_WARN(" !!! Failed to get static transforms. Is the 'ROBOT STATE PUBLISHER' node correctly working? "); + break; + } - } while (transformOk == false); + } while (transformOk == false); - if (transformOk) - { - NODELET_DEBUG("Time required to get valid static transforms: %g sec", elapsed / 1000.); - } + if (transformOk) { + NODELET_DEBUG("Time required to get valid static transforms: %g sec", elapsed / 1000.); + } - NODELET_INFO("Initial ZED left camera pose (ZED pos. tracking): "); - NODELET_INFO(" * T: [%g,%g,%g]", mInitialPoseSl.getTranslation().x, mInitialPoseSl.getTranslation().y, - mInitialPoseSl.getTranslation().z); - NODELET_INFO(" * Q: [%g,%g,%g,%g]", mInitialPoseSl.getOrientation().ox, mInitialPoseSl.getOrientation().oy, - mInitialPoseSl.getOrientation().oz, mInitialPoseSl.getOrientation().ow); + NODELET_INFO("Initial ZED left camera pose (ZED pos. tracking): "); + NODELET_INFO(" * T: [%g,%g,%g]", mInitialPoseSl.getTranslation().x, mInitialPoseSl.getTranslation().y, + mInitialPoseSl.getTranslation().z); + NODELET_INFO(" * Q: [%g,%g,%g,%g]", mInitialPoseSl.getOrientation().ox, mInitialPoseSl.getOrientation().oy, + mInitialPoseSl.getOrientation().oz, mInitialPoseSl.getOrientation().ow); - // Positional Tracking parameters - sl::PositionalTrackingParameters posTrackParams; + // Positional Tracking parameters + sl::PositionalTrackingParameters posTrackParams; - posTrackParams.initial_world_transform = mInitialPoseSl; - posTrackParams.enable_area_memory = mAreaMemory; + posTrackParams.initial_world_transform = mInitialPoseSl; + posTrackParams.enable_area_memory = mAreaMemory; - mPoseSmoothing = false; // Always false. Pose Smoothing is to be enabled only for VR/AR applications - posTrackParams.enable_pose_smoothing = mPoseSmoothing; + mPoseSmoothing = false; // Always false. Pose Smoothing is to be enabled only for VR/AR applications + posTrackParams.enable_pose_smoothing = mPoseSmoothing; - posTrackParams.set_floor_as_origin = mFloorAlignment; + posTrackParams.set_floor_as_origin = mFloorAlignment; - if (mAreaMemDbPath != "" && !sl_tools::file_exist(mAreaMemDbPath)) - { - posTrackParams.area_file_path = ""; - NODELET_WARN_STREAM("area_memory_db_path [" << mAreaMemDbPath << "] doesn't exist or is unreachable. "); - if (mSaveAreaMapOnClosing) - { - NODELET_INFO_STREAM("The file will be automatically created when closing the node or calling the " - "'save_area_map' service if a valid Area Memory is available."); + if (mAreaMemDbPath != "" && !sl_tools::file_exist(mAreaMemDbPath)) { + posTrackParams.area_file_path = ""; + NODELET_WARN_STREAM("area_memory_db_path [" << mAreaMemDbPath << "] doesn't exist or is unreachable. "); + if (mSaveAreaMapOnClosing) { + NODELET_INFO_STREAM("The file will be automatically created when closing the node or calling the " + "'save_area_map' service if a valid Area Memory is available."); + } + } else { + posTrackParams.area_file_path = mAreaMemDbPath.c_str(); } - } - else - { - posTrackParams.area_file_path = mAreaMemDbPath.c_str(); - } - posTrackParams.enable_imu_fusion = mImuFusion; + posTrackParams.enable_imu_fusion = mImuFusion; - posTrackParams.set_as_static = false; + posTrackParams.set_as_static = false; - sl::ERROR_CODE err = mZed.enablePositionalTracking(posTrackParams); + sl::ERROR_CODE err = mZed.enablePositionalTracking(posTrackParams); - if (err == sl::ERROR_CODE::SUCCESS) - { - mPosTrackingActivated = true; - } - else - { - mPosTrackingActivated = false; + if (err == sl::ERROR_CODE::SUCCESS) { + mPosTrackingActivated = true; + } else { + mPosTrackingActivated = false; - NODELET_WARN("Tracking not activated: %s", sl::toString(err).c_str()); - } + NODELET_WARN("Tracking not activated: %s", sl::toString(err).c_str()); + } } bool ZEDWrapperNodelet::on_save_area_memory(zed_interfaces::save_area_memory::Request& req, - zed_interfaces::save_area_memory::Response& res) + zed_interfaces::save_area_memory::Response& res) { - std::string file_path = sl_tools::resolveFilePath(req.area_memory_filename); + std::string file_path = sl_tools::resolveFilePath(req.area_memory_filename); - bool ret = saveAreaMap(file_path,&res.info); + bool ret = saveAreaMap(file_path, &res.info); - return ret; + return ret; } bool ZEDWrapperNodelet::saveAreaMap(std::string file_path, std::string* out_msg) { - std::ostringstream os; + std::ostringstream os; - bool node_running = mNhNs.ok(); - if (!mZed.isOpened()) - { - os << "Cannot save Area Memory. The camera is closed."; + bool node_running = mNhNs.ok(); + if (!mZed.isOpened()) { + os << "Cannot save Area Memory. The camera is closed."; - if (node_running) - NODELET_WARN_STREAM(os.str().c_str()); - else - std::cerr << os.str() << std::endl; - - if (out_msg) - *out_msg = os.str(); + if (node_running) + NODELET_WARN_STREAM(os.str().c_str()); + else + std::cerr << os.str() << std::endl; - return false; - } + if (out_msg) + *out_msg = os.str(); - if (mPosTrackingActivated && mAreaMemory) - { - sl::ERROR_CODE err = mZed.saveAreaMap(sl::String(file_path.c_str())); - if (err != sl::ERROR_CODE::SUCCESS) - { - os << "Error saving positional tracking area memory: " << sl::toString(err).c_str(); + return false; + } - if (node_running) - NODELET_WARN_STREAM(os.str().c_str()); - else - std::cerr << os.str() << std::endl; + if (mPosTrackingActivated && mAreaMemory) { + sl::ERROR_CODE err = mZed.saveAreaMap(sl::String(file_path.c_str())); + if (err != sl::ERROR_CODE::SUCCESS) { + os << "Error saving positional tracking area memory: " << sl::toString(err).c_str(); - if (out_msg) - *out_msg = os.str(); + if (node_running) + NODELET_WARN_STREAM(os.str().c_str()); + else + std::cerr << os.str() << std::endl; - return false; - } + if (out_msg) + *out_msg = os.str(); - if (node_running) - NODELET_INFO_STREAM("Saving Area Memory file: " << file_path); - else - std::cerr << "Saving Area Memory file: " << file_path << " "; + return false; + } - sl::AREA_EXPORTING_STATE state; - do - { - std::this_thread::sleep_for(std::chrono::milliseconds(500)); - state = mZed.getAreaExportState(); - if (node_running) - NODELET_INFO_STREAM("."); - else - std::cerr << "."; - } while (state == sl::AREA_EXPORTING_STATE::RUNNING); - if (!node_running) - std::cerr << std::endl; + if (node_running) + NODELET_INFO_STREAM("Saving Area Memory file: " << file_path); + else + std::cerr << "Saving Area Memory file: " << file_path << " "; + + sl::AREA_EXPORTING_STATE state; + do { + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + state = mZed.getAreaExportState(); + if (node_running) + NODELET_INFO_STREAM("."); + else + std::cerr << "."; + } while (state == sl::AREA_EXPORTING_STATE::RUNNING); + if (!node_running) + std::cerr << std::endl; - if (state == sl::AREA_EXPORTING_STATE::SUCCESS) - { - os << "Area Memory file saved correctly."; + if (state == sl::AREA_EXPORTING_STATE::SUCCESS) { + os << "Area Memory file saved correctly."; - if (node_running) - NODELET_INFO_STREAM(os.str().c_str()); - else - std::cerr << os.str() << std::endl; + if (node_running) + NODELET_INFO_STREAM(os.str().c_str()); + else + std::cerr << os.str() << std::endl; - if (out_msg) - *out_msg = os.str(); - return true; - } + if (out_msg) + *out_msg = os.str(); + return true; + } - os << "Error saving Area Memory file: " << sl::toString(state).c_str(); + os << "Error saving Area Memory file: " << sl::toString(state).c_str(); - if (node_running) - NODELET_WARN_STREAM(os.str().c_str()); - else - std::cerr << os.str() << std::endl; + if (node_running) + NODELET_WARN_STREAM(os.str().c_str()); + else + std::cerr << os.str() << std::endl; - if (out_msg) - *out_msg = os.str(); + if (out_msg) + *out_msg = os.str(); + return false; + } return false; - } - return false; } void ZEDWrapperNodelet::publishOdom(tf2::Transform odom2baseTransf, sl::Pose& slPose, ros::Time t) { - nav_msgs::OdometryPtr odomMsg = boost::make_shared(); - - odomMsg->header.stamp = t; - odomMsg->header.frame_id = mOdometryFrameId; // frame - odomMsg->child_frame_id = mBaseFrameId; // camera_frame - // conversion from Tranform to message - geometry_msgs::Transform base2odom = tf2::toMsg(odom2baseTransf); - // Add all value in odometry message - odomMsg->pose.pose.position.x = base2odom.translation.x; - odomMsg->pose.pose.position.y = base2odom.translation.y; - odomMsg->pose.pose.position.z = base2odom.translation.z; - odomMsg->pose.pose.orientation.x = base2odom.rotation.x; - odomMsg->pose.pose.orientation.y = base2odom.rotation.y; - odomMsg->pose.pose.orientation.z = base2odom.rotation.z; - odomMsg->pose.pose.orientation.w = base2odom.rotation.w; - - // Odometry pose covariance - - for (size_t i = 0; i < odomMsg->pose.covariance.size(); i++) - { - odomMsg->pose.covariance[i] = static_cast(slPose.pose_covariance[i]); - - if (mTwoDMode) - { - if (i == 14 || i == 21 || i == 28) - { - odomMsg->pose.covariance[i] = 1e-9; // Very low covariance if 2D mode - } - else if ((i >= 2 && i <= 4) || (i >= 8 && i <= 10) || (i >= 12 && i <= 13) || (i >= 15 && i <= 16) || - (i >= 18 && i <= 20) || (i == 22) || (i >= 24 && i <= 27)) - { - odomMsg->pose.covariance[i] = 0.0; - } - } - } - - // Publish odometry message - mPubOdom.publish(odomMsg); + nav_msgs::OdometryPtr odomMsg = boost::make_shared(); + + odomMsg->header.stamp = t; + odomMsg->header.frame_id = mOdometryFrameId; // frame + odomMsg->child_frame_id = mBaseFrameId; // camera_frame + // conversion from Tranform to message + geometry_msgs::Transform base2odom = tf2::toMsg(odom2baseTransf); + // Add all value in odometry message + odomMsg->pose.pose.position.x = base2odom.translation.x; + odomMsg->pose.pose.position.y = base2odom.translation.y; + odomMsg->pose.pose.position.z = base2odom.translation.z; + odomMsg->pose.pose.orientation.x = base2odom.rotation.x; + odomMsg->pose.pose.orientation.y = base2odom.rotation.y; + odomMsg->pose.pose.orientation.z = base2odom.rotation.z; + odomMsg->pose.pose.orientation.w = base2odom.rotation.w; + + // Odometry pose covariance + + for (size_t i = 0; i < odomMsg->pose.covariance.size(); i++) { + odomMsg->pose.covariance[i] = static_cast(slPose.pose_covariance[i]); + + if (mTwoDMode) { + if (i == 14 || i == 21 || i == 28) { + odomMsg->pose.covariance[i] = 1e-9; // Very low covariance if 2D mode + } else if ((i >= 2 && i <= 4) || (i >= 8 && i <= 10) || (i >= 12 && i <= 13) || (i >= 15 && i <= 16) || (i >= 18 && i <= 20) || (i == 22) || (i >= 24 && i <= 27)) { + odomMsg->pose.covariance[i] = 0.0; + } + } + } + + // Publish odometry message + mPubOdom.publish(odomMsg); } void ZEDWrapperNodelet::publishPose(ros::Time t) { - tf2::Transform base_pose; - base_pose.setIdentity(); - - if (mPublishMapTf) - { - base_pose = mMap2BaseTransf; - } - else if (mPublishTf) - { - base_pose = mOdom2BaseTransf; - } - - std_msgs::Header header; - header.stamp = t; - header.frame_id = mMapFrameId; - geometry_msgs::Pose pose; - - // conversion from Tranform to message - geometry_msgs::Transform base2frame = tf2::toMsg(base_pose); - - // Add all value in Pose message - pose.position.x = base2frame.translation.x; - pose.position.y = base2frame.translation.y; - pose.position.z = base2frame.translation.z; - pose.orientation.x = base2frame.rotation.x; - pose.orientation.y = base2frame.rotation.y; - pose.orientation.z = base2frame.rotation.z; - pose.orientation.w = base2frame.rotation.w; - - if (mPubPose.getNumSubscribers() > 0) - { - geometry_msgs::PoseStamped poseNoCov; - - poseNoCov.header = header; - poseNoCov.pose = pose; - - // Publish pose stamped message - mPubPose.publish(poseNoCov); - } - - if (mPubPoseCov.getNumSubscribers() > 0) - { - geometry_msgs::PoseWithCovarianceStampedPtr poseCovMsg = - boost::make_shared(); - - poseCovMsg->header = header; - poseCovMsg->pose.pose = pose; - - // Odometry pose covariance if available - for (size_t i = 0; i < poseCovMsg->pose.covariance.size(); i++) - { - poseCovMsg->pose.covariance[i] = static_cast(mLastZedPose.pose_covariance[i]); - - if (mTwoDMode) - { - if (i == 14 || i == 21 || i == 28) - { - poseCovMsg->pose.covariance[i] = 1e-9; // Very low covariance if 2D mode - } - else if ((i >= 2 && i <= 4) || (i >= 8 && i <= 10) || (i >= 12 && i <= 13) || (i >= 15 && i <= 16) || - (i >= 18 && i <= 20) || (i == 22) || (i >= 24 && i <= 27)) - { - poseCovMsg->pose.covariance[i] = 0.0; - } - } + tf2::Transform base_pose; + base_pose.setIdentity(); + + if (mPublishMapTf) { + base_pose = mMap2BaseTransf; + } else if (mPublishTf) { + base_pose = mOdom2BaseTransf; } - // Publish pose with covariance stamped message - mPubPoseCov.publish(poseCovMsg); - } -} + std_msgs::Header header; + header.stamp = t; + header.frame_id = mMapFrameId; + geometry_msgs::Pose pose; -void ZEDWrapperNodelet::publishStaticImuFrame() -{ - // Publish IMU TF as static TF - if (!mPublishImuTf) - { - return; - } - - if (mStaticImuFramePublished) - { - return; - } - - mStaticImuTransformStamped.header.stamp = ros::Time::now(); - mStaticImuTransformStamped.header.frame_id = mLeftCamFrameId; - mStaticImuTransformStamped.child_frame_id = mImuFrameId; - sl::Translation sl_tr = mSlCamImuTransf.getTranslation(); - mStaticImuTransformStamped.transform.translation.x = sl_tr.x; - mStaticImuTransformStamped.transform.translation.y = sl_tr.y; - mStaticImuTransformStamped.transform.translation.z = sl_tr.z; - sl::Orientation sl_or = mSlCamImuTransf.getOrientation(); - mStaticImuTransformStamped.transform.rotation.x = sl_or.ox; - mStaticImuTransformStamped.transform.rotation.y = sl_or.oy; - mStaticImuTransformStamped.transform.rotation.z = sl_or.oz; - mStaticImuTransformStamped.transform.rotation.w = sl_or.ow; - - // Publish transformation - mStaticTransformImuBroadcaster.sendTransform(mStaticImuTransformStamped); - - NODELET_INFO_STREAM("Published static transform '" << mImuFrameId << "' -> '" << mLeftCamFrameId << "'"); - - mStaticImuFramePublished = true; -} + // conversion from Tranform to message + geometry_msgs::Transform base2frame = tf2::toMsg(base_pose); -void ZEDWrapperNodelet::publishOdomFrame(tf2::Transform odomTransf, ros::Time t) -{ - // ----> Avoid duplicated TF publishing - static ros::Time last_stamp; - - if (t == last_stamp) - { - return; - } - last_stamp = t; - // <---- Avoid duplicated TF publishing - - if (!mSensor2BaseTransfValid) - { - getSens2BaseTransform(); - } - - if (!mSensor2CameraTransfValid) - { - getSens2CameraTransform(); - } - - if (!mCamera2BaseTransfValid) - { - getCamera2BaseTransform(); - } - - geometry_msgs::TransformStamped transformStamped; - transformStamped.header.stamp = t; - transformStamped.header.frame_id = mOdometryFrameId; - transformStamped.child_frame_id = mBaseFrameId; - // conversion from Tranform to message - transformStamped.transform = tf2::toMsg(odomTransf); - // Publish transformation - mTransformOdomBroadcaster.sendTransform(transformStamped); - - // NODELET_INFO_STREAM( "Published ODOM TF with TS: " << t ); -} + // Add all value in Pose message + pose.position.x = base2frame.translation.x; + pose.position.y = base2frame.translation.y; + pose.position.z = base2frame.translation.z; + pose.orientation.x = base2frame.rotation.x; + pose.orientation.y = base2frame.rotation.y; + pose.orientation.z = base2frame.rotation.z; + pose.orientation.w = base2frame.rotation.w; + + if (mPubPose.getNumSubscribers() > 0) { + geometry_msgs::PoseStamped poseNoCov; + + poseNoCov.header = header; + poseNoCov.pose = pose; + + // Publish pose stamped message + mPubPose.publish(poseNoCov); + } + + if (mPubPoseCov.getNumSubscribers() > 0) { + geometry_msgs::PoseWithCovarianceStampedPtr poseCovMsg = boost::make_shared(); + + poseCovMsg->header = header; + poseCovMsg->pose.pose = pose; + + // Odometry pose covariance if available + for (size_t i = 0; i < poseCovMsg->pose.covariance.size(); i++) { + poseCovMsg->pose.covariance[i] = static_cast(mLastZedPose.pose_covariance[i]); + + if (mTwoDMode) { + if (i == 14 || i == 21 || i == 28) { + poseCovMsg->pose.covariance[i] = 1e-9; // Very low covariance if 2D mode + } else if ((i >= 2 && i <= 4) || (i >= 8 && i <= 10) || (i >= 12 && i <= 13) || (i >= 15 && i <= 16) || (i >= 18 && i <= 20) || (i == 22) || (i >= 24 && i <= 27)) { + poseCovMsg->pose.covariance[i] = 0.0; + } + } + } + + // Publish pose with covariance stamped message + mPubPoseCov.publish(poseCovMsg); + } +} + +void ZEDWrapperNodelet::publishStaticImuFrame() +{ + // Publish IMU TF as static TF + if (!mPublishImuTf) { + return; + } + + if (mStaticImuFramePublished) { + return; + } + + mStaticImuTransformStamped.header.stamp = ros::Time::now(); + mStaticImuTransformStamped.header.frame_id = mLeftCamFrameId; + mStaticImuTransformStamped.child_frame_id = mImuFrameId; + sl::Translation sl_tr = mSlCamImuTransf.getTranslation(); + mStaticImuTransformStamped.transform.translation.x = sl_tr.x; + mStaticImuTransformStamped.transform.translation.y = sl_tr.y; + mStaticImuTransformStamped.transform.translation.z = sl_tr.z; + sl::Orientation sl_or = mSlCamImuTransf.getOrientation(); + mStaticImuTransformStamped.transform.rotation.x = sl_or.ox; + mStaticImuTransformStamped.transform.rotation.y = sl_or.oy; + mStaticImuTransformStamped.transform.rotation.z = sl_or.oz; + mStaticImuTransformStamped.transform.rotation.w = sl_or.ow; + + // Publish transformation + mStaticTransformImuBroadcaster.sendTransform(mStaticImuTransformStamped); + + NODELET_INFO_STREAM("Published static transform '" << mImuFrameId << "' -> '" << mLeftCamFrameId << "'"); + + mStaticImuFramePublished = true; +} + +void ZEDWrapperNodelet::publishOdomFrame(tf2::Transform odomTransf, ros::Time t) +{ + // ----> Avoid duplicated TF publishing + static ros::Time last_stamp; + + if (t == last_stamp) { + return; + } + last_stamp = t; + // <---- Avoid duplicated TF publishing + + if (!mSensor2BaseTransfValid) { + getSens2BaseTransform(); + } + + if (!mSensor2CameraTransfValid) { + getSens2CameraTransform(); + } + + if (!mCamera2BaseTransfValid) { + getCamera2BaseTransform(); + } + + geometry_msgs::TransformStamped transformStamped; + transformStamped.header.stamp = t; + transformStamped.header.frame_id = mOdometryFrameId; + transformStamped.child_frame_id = mBaseFrameId; + // conversion from Tranform to message + transformStamped.transform = tf2::toMsg(odomTransf); + // Publish transformation + mTransformOdomBroadcaster.sendTransform(transformStamped); + + // NODELET_INFO_STREAM( "Published ODOM TF with TS: " << t ); +} void ZEDWrapperNodelet::publishPoseFrame(tf2::Transform baseTransform, ros::Time t) { - // ----> Avoid duplicated TF publishing - static ros::Time last_stamp; - - if (t == last_stamp) - { - return; - } - last_stamp = t; - // <---- Avoid duplicated TF publishing - - if (!mSensor2BaseTransfValid) - { - getSens2BaseTransform(); - } - - if (!mSensor2CameraTransfValid) - { - getSens2CameraTransform(); - } - - if (!mCamera2BaseTransfValid) - { - getCamera2BaseTransform(); - } - - geometry_msgs::TransformStamped transformStamped; - transformStamped.header.stamp = t; - transformStamped.header.frame_id = mMapFrameId; - transformStamped.child_frame_id = mOdometryFrameId; - // conversion from Tranform to message - transformStamped.transform = tf2::toMsg(baseTransform); - // Publish transformation - mTransformPoseBroadcaster.sendTransform(transformStamped); - - // NODELET_INFO_STREAM( "Published POSE TF with TS: " << t ); + // ----> Avoid duplicated TF publishing + static ros::Time last_stamp; + + if (t == last_stamp) { + return; + } + last_stamp = t; + // <---- Avoid duplicated TF publishing + + if (!mSensor2BaseTransfValid) { + getSens2BaseTransform(); + } + + if (!mSensor2CameraTransfValid) { + getSens2CameraTransform(); + } + + if (!mCamera2BaseTransfValid) { + getCamera2BaseTransform(); + } + + geometry_msgs::TransformStamped transformStamped; + transformStamped.header.stamp = t; + transformStamped.header.frame_id = mMapFrameId; + transformStamped.child_frame_id = mOdometryFrameId; + // conversion from Tranform to message + transformStamped.transform = tf2::toMsg(baseTransform); + // Publish transformation + mTransformPoseBroadcaster.sendTransform(transformStamped); + + // NODELET_INFO_STREAM( "Published POSE TF with TS: " << t ); } void ZEDWrapperNodelet::publishImage(sensor_msgs::ImagePtr imgMsgPtr, sl::Mat img, - image_transport::CameraPublisher& pubImg, sensor_msgs::CameraInfoPtr camInfoMsg, - string imgFrameId, ros::Time t) + image_transport::CameraPublisher& pubImg, sensor_msgs::CameraInfoPtr camInfoMsg, + string imgFrameId, ros::Time t) { - camInfoMsg->header.stamp = t; - sl_tools::imageToROSmsg(imgMsgPtr, img, imgFrameId, t); - pubImg.publish(imgMsgPtr, camInfoMsg); + camInfoMsg->header.stamp = t; + sl_tools::imageToROSmsg(imgMsgPtr, img, imgFrameId, t); + pubImg.publish(imgMsgPtr, camInfoMsg); } void ZEDWrapperNodelet::publishDepth(sensor_msgs::ImagePtr imgMsgPtr, sl::Mat depth, ros::Time t) { - mDepthCamInfoMsg->header.stamp = t; + mDepthCamInfoMsg->header.stamp = t; - // NODELET_DEBUG_STREAM("mOpenniDepthMode: " << mOpenniDepthMode); + // NODELET_DEBUG_STREAM("mOpenniDepthMode: " << mOpenniDepthMode); - if (!mOpenniDepthMode) - { - // NODELET_INFO("Using float32"); - sl_tools::imageToROSmsg(imgMsgPtr, depth, mDepthOptFrameId, t); - mPubDepth.publish(imgMsgPtr, mDepthCamInfoMsg); + if (!mOpenniDepthMode) { + // NODELET_INFO("Using float32"); + sl_tools::imageToROSmsg(imgMsgPtr, depth, mDepthOptFrameId, t); + mPubDepth.publish(imgMsgPtr, mDepthCamInfoMsg); - return; - } + return; + } #if (ZED_SDK_MAJOR_VERSION == 3 && ZED_SDK_MINOR_VERSION < 4) - // OPENNI CONVERSION (meter -> millimeters - float32 -> uint16) - if (!imgMsgPtr) - { - imgMsgPtr = boost::make_shared(); - } + // OPENNI CONVERSION (meter -> millimeters - float32 -> uint16) + if (!imgMsgPtr) { + imgMsgPtr = boost::make_shared(); + } - imgMsgPtr->header.stamp = t; - imgMsgPtr->header.frame_id = mDepthOptFrameId; - imgMsgPtr->height = depth.getHeight(); - imgMsgPtr->width = depth.getWidth(); + imgMsgPtr->header.stamp = t; + imgMsgPtr->header.frame_id = mDepthOptFrameId; + imgMsgPtr->height = depth.getHeight(); + imgMsgPtr->width = depth.getWidth(); - int num = 1; // for endianness detection - imgMsgPtr->is_bigendian = !(*(char*)&num == 1); + int num = 1; // for endianness detection + imgMsgPtr->is_bigendian = !(*(char*)&num == 1); - imgMsgPtr->step = imgMsgPtr->width * sizeof(uint16_t); - imgMsgPtr->encoding = sensor_msgs::image_encodings::TYPE_16UC1; + imgMsgPtr->step = imgMsgPtr->width * sizeof(uint16_t); + imgMsgPtr->encoding = sensor_msgs::image_encodings::TYPE_16UC1; - size_t size = imgMsgPtr->step * imgMsgPtr->height; - imgMsgPtr->data.resize(size); + size_t size = imgMsgPtr->step * imgMsgPtr->height; + imgMsgPtr->data.resize(size); - uint16_t* data = (uint16_t*)(&imgMsgPtr->data[0]); + uint16_t* data = (uint16_t*)(&imgMsgPtr->data[0]); - int dataSize = imgMsgPtr->width * imgMsgPtr->height; - sl::float1* depthDataPtr = depth.getPtr(); + int dataSize = imgMsgPtr->width * imgMsgPtr->height; + sl::float1* depthDataPtr = depth.getPtr(); - for (int i = 0; i < dataSize; i++) - { - *(data++) = static_cast(std::round(*(depthDataPtr++) * 1000)); // in mm, rounded - } - mPubDepth.publish(imgMsgPtr, mDepthCamInfoMsg); + for (int i = 0; i < dataSize; i++) { + *(data++) = static_cast(std::round(*(depthDataPtr++) * 1000)); // in mm, rounded + } + mPubDepth.publish(imgMsgPtr, mDepthCamInfoMsg); #else - // NODELET_INFO("Using depth16"); - sl_tools::imageToROSmsg(imgMsgPtr, depth, mDepthOptFrameId, t); - mPubDepth.publish(imgMsgPtr, mDepthCamInfoMsg); + // NODELET_INFO("Using depth16"); + sl_tools::imageToROSmsg(imgMsgPtr, depth, mDepthOptFrameId, t); + mPubDepth.publish(imgMsgPtr, mDepthCamInfoMsg); #endif } void ZEDWrapperNodelet::publishDisparity(sl::Mat disparity, ros::Time t) { - sl::CameraInformation zedParam = mZed.getCameraInformation(mMatResolDepth); + sl::CameraInformation zedParam = mZed.getCameraInformation(mMatResolDepth); - sensor_msgs::ImagePtr disparityImgMsg = boost::make_shared(); - stereo_msgs::DisparityImagePtr disparityMsg = boost::make_shared(); + sensor_msgs::ImagePtr disparityImgMsg = boost::make_shared(); + stereo_msgs::DisparityImagePtr disparityMsg = boost::make_shared(); - sl_tools::imageToROSmsg(disparityImgMsg, disparity, mDisparityFrameId, t); + sl_tools::imageToROSmsg(disparityImgMsg, disparity, mDisparityFrameId, t); - disparityMsg->image = *disparityImgMsg; - disparityMsg->header = disparityMsg->image.header; + disparityMsg->image = *disparityImgMsg; + disparityMsg->header = disparityMsg->image.header; #if ZED_SDK_MAJOR_VERSION == 3 && ZED_SDK_MINOR_VERSION < 1 - disparityMsg->f = zedParam.calibration_parameters.left_cam.fx; - disparityMsg->T = zedParam.calibration_parameters.T.x; + disparityMsg->f = zedParam.calibration_parameters.left_cam.fx; + disparityMsg->T = zedParam.calibration_parameters.T.x; #else - disparityMsg->f = zedParam.camera_configuration.calibration_parameters.left_cam.fx; - disparityMsg->T = zedParam.camera_configuration.calibration_parameters.getCameraBaseline(); + disparityMsg->f = zedParam.camera_configuration.calibration_parameters.left_cam.fx; + disparityMsg->T = zedParam.camera_configuration.calibration_parameters.getCameraBaseline(); #endif - if (disparityMsg->T > 0) - { - disparityMsg->T *= -1.0f; - } + if (disparityMsg->T > 0) { + disparityMsg->T *= -1.0f; + } - disparityMsg->min_disparity = disparityMsg->f * disparityMsg->T / mZed.getInitParameters().depth_minimum_distance; - disparityMsg->max_disparity = disparityMsg->f * disparityMsg->T / mZed.getInitParameters().depth_maximum_distance; + disparityMsg->min_disparity = disparityMsg->f * disparityMsg->T / mZed.getInitParameters().depth_minimum_distance; + disparityMsg->max_disparity = disparityMsg->f * disparityMsg->T / mZed.getInitParameters().depth_maximum_distance; - mPubDisparity.publish(disparityMsg); + mPubDisparity.publish(disparityMsg); } void ZEDWrapperNodelet::pointcloud_thread_func() { - std::unique_lock lock(mPcMutex); - - while (!mStopNode) - { - while (!mPcDataReady) - { // loop to avoid spurious wakeups - if (mPcDataReadyCondVar.wait_for(lock, std::chrono::milliseconds(500)) == std::cv_status::timeout) - { - // Check thread stopping - if (mStopNode) - { - return; - } - else - { - continue; + std::unique_lock lock(mPcMutex); + + while (!mStopNode) { + while (!mPcDataReady) { // loop to avoid spurious wakeups + if (mPcDataReadyCondVar.wait_for(lock, std::chrono::milliseconds(500)) == std::cv_status::timeout) { + // Check thread stopping + if (mStopNode) { + return; + } else { + continue; + } + } } - } - } - // ----> Check publishing frequency - double pc_period_msec = 1000.0 / mPointCloudFreq; + // ----> Check publishing frequency + double pc_period_msec = 1000.0 / mPointCloudFreq; - static std::chrono::steady_clock::time_point last_time = std::chrono::steady_clock::now(); - std::chrono::steady_clock::time_point now = std::chrono::steady_clock::now(); + static std::chrono::steady_clock::time_point last_time = std::chrono::steady_clock::now(); + std::chrono::steady_clock::time_point now = std::chrono::steady_clock::now(); - double elapsed_msec = std::chrono::duration_cast(now - last_time).count(); + double elapsed_msec = std::chrono::duration_cast(now - last_time).count(); - if (elapsed_msec < pc_period_msec) - { - std::this_thread::sleep_for(std::chrono::milliseconds(static_cast(pc_period_msec - elapsed_msec))); - } - // <---- Check publishing frequency + if (elapsed_msec < pc_period_msec) { + std::this_thread::sleep_for(std::chrono::milliseconds(static_cast(pc_period_msec - elapsed_msec))); + } + // <---- Check publishing frequency - last_time = std::chrono::steady_clock::now(); - publishPointCloud(); + last_time = std::chrono::steady_clock::now(); + publishPointCloud(); - mPcDataReady = false; - } + mPcDataReady = false; + } - NODELET_DEBUG("Pointcloud thread finished"); + NODELET_DEBUG("Pointcloud thread finished"); } void ZEDWrapperNodelet::publishPointCloud() { - sensor_msgs::PointCloud2Ptr pointcloudMsg = boost::make_shared(); + sensor_msgs::PointCloud2Ptr pointcloudMsg = boost::make_shared(); - // Publish freq calculation - static std::chrono::steady_clock::time_point last_time = std::chrono::steady_clock::now(); - std::chrono::steady_clock::time_point now = std::chrono::steady_clock::now(); + // Publish freq calculation + static std::chrono::steady_clock::time_point last_time = std::chrono::steady_clock::now(); + std::chrono::steady_clock::time_point now = std::chrono::steady_clock::now(); - double elapsed_usec = std::chrono::duration_cast(now - last_time).count(); - last_time = now; + double elapsed_usec = std::chrono::duration_cast(now - last_time).count(); + last_time = now; - mPcPeriodMean_usec->addValue(elapsed_usec); + mPcPeriodMean_usec->addValue(elapsed_usec); - // Initialize Point Cloud message - // https://github.com/ros/common_msgs/blob/jade-devel/sensor_msgs/include/sensor_msgs/point_cloud2_iterator.h + // Initialize Point Cloud message + // https://github.com/ros/common_msgs/blob/jade-devel/sensor_msgs/include/sensor_msgs/point_cloud2_iterator.h - int ptsCount = mMatResolDepth.width * mMatResolDepth.height; + int ptsCount = mMatResolDepth.width * mMatResolDepth.height; - pointcloudMsg->header.stamp = mPointCloudTime; + pointcloudMsg->header.stamp = mPointCloudTime; - if (pointcloudMsg->width != mMatResolDepth.width || pointcloudMsg->height != mMatResolDepth.height) - { - pointcloudMsg->header.frame_id = mPointCloudFrameId; // Set the header values of the ROS message + if (pointcloudMsg->width != mMatResolDepth.width || pointcloudMsg->height != mMatResolDepth.height) { + pointcloudMsg->header.frame_id = mPointCloudFrameId; // Set the header values of the ROS message - pointcloudMsg->is_bigendian = false; - pointcloudMsg->is_dense = false; + pointcloudMsg->is_bigendian = false; + pointcloudMsg->is_dense = false; - pointcloudMsg->width = mMatResolDepth.width; - pointcloudMsg->height = mMatResolDepth.height; + pointcloudMsg->width = mMatResolDepth.width; + pointcloudMsg->height = mMatResolDepth.height; - sensor_msgs::PointCloud2Modifier modifier(*pointcloudMsg); - modifier.setPointCloud2Fields(4, "x", 1, sensor_msgs::PointField::FLOAT32, "y", 1, sensor_msgs::PointField::FLOAT32, - "z", 1, sensor_msgs::PointField::FLOAT32, "rgb", 1, sensor_msgs::PointField::FLOAT32); - } + sensor_msgs::PointCloud2Modifier modifier(*pointcloudMsg); + modifier.setPointCloud2Fields(4, "x", 1, sensor_msgs::PointField::FLOAT32, "y", 1, sensor_msgs::PointField::FLOAT32, + "z", 1, sensor_msgs::PointField::FLOAT32, "rgb", 1, sensor_msgs::PointField::FLOAT32); + } - // Data copy - sl::Vector4* cpu_cloud = mCloud.getPtr(); - float* ptCloudPtr = (float*)(&pointcloudMsg->data[0]); + // Data copy + sl::Vector4* cpu_cloud = mCloud.getPtr(); + float* ptCloudPtr = (float*)(&pointcloudMsg->data[0]); - // We can do a direct memcpy since data organization is the same - memcpy(ptCloudPtr, (float*)cpu_cloud, 4 * ptsCount * sizeof(float)); + // We can do a direct memcpy since data organization is the same + memcpy(ptCloudPtr, (float*)cpu_cloud, 4 * ptsCount * sizeof(float)); - // Pointcloud publishing - mPubCloud.publish(pointcloudMsg); + // Pointcloud publishing + mPubCloud.publish(pointcloudMsg); } void ZEDWrapperNodelet::callback_pubFusedPointCloud(const ros::TimerEvent& e) { - sensor_msgs::PointCloud2Ptr pointcloudFusedMsg = boost::make_shared(); + sensor_msgs::PointCloud2Ptr pointcloudFusedMsg = boost::make_shared(); - uint32_t fusedCloudSubnumber = mPubFusedCloud.getNumSubscribers(); + uint32_t fusedCloudSubnumber = mPubFusedCloud.getNumSubscribers(); - if (fusedCloudSubnumber == 0) - { - return; - } + if (fusedCloudSubnumber == 0) { + return; + } - std::lock_guard lock(mCloseZedMutex); + std::lock_guard lock(mCloseZedMutex); - if (!mZed.isOpened()) - { - return; - } + if (!mZed.isOpened()) { + return; + } - // pointcloudFusedMsg->header.stamp = t; - mZed.requestSpatialMapAsync(); + // pointcloudFusedMsg->header.stamp = t; + mZed.requestSpatialMapAsync(); - while (mZed.getSpatialMapRequestStatusAsync() == sl::ERROR_CODE::FAILURE) - { - // Mesh is still generating - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } + while (mZed.getSpatialMapRequestStatusAsync() == sl::ERROR_CODE::FAILURE) { + // Mesh is still generating + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } - sl::ERROR_CODE res = mZed.retrieveSpatialMapAsync(mFusedPC); + sl::ERROR_CODE res = mZed.retrieveSpatialMapAsync(mFusedPC); - if (res != sl::ERROR_CODE::SUCCESS) - { - NODELET_WARN_STREAM("Fused point cloud not extracted: " << sl::toString(res).c_str()); - return; - } + if (res != sl::ERROR_CODE::SUCCESS) { + NODELET_WARN_STREAM("Fused point cloud not extracted: " << sl::toString(res).c_str()); + return; + } - size_t ptsCount = mFusedPC.getNumberOfPoints(); - bool resized = false; + size_t ptsCount = mFusedPC.getNumberOfPoints(); + bool resized = false; - if (pointcloudFusedMsg->width != ptsCount || pointcloudFusedMsg->height != 1) - { - // Initialize Point Cloud message - // https://github.com/ros/common_msgs/blob/jade-devel/sensor_msgs/include/sensor_msgs/point_cloud2_iterator.h - pointcloudFusedMsg->header.frame_id = mMapFrameId; // Set the header values of the ROS message - pointcloudFusedMsg->is_bigendian = false; - pointcloudFusedMsg->is_dense = false; - pointcloudFusedMsg->width = ptsCount; - pointcloudFusedMsg->height = 1; + if (pointcloudFusedMsg->width != ptsCount || pointcloudFusedMsg->height != 1) { + // Initialize Point Cloud message + // https://github.com/ros/common_msgs/blob/jade-devel/sensor_msgs/include/sensor_msgs/point_cloud2_iterator.h + pointcloudFusedMsg->header.frame_id = mMapFrameId; // Set the header values of the ROS message + pointcloudFusedMsg->is_bigendian = false; + pointcloudFusedMsg->is_dense = false; + pointcloudFusedMsg->width = ptsCount; + pointcloudFusedMsg->height = 1; - sensor_msgs::PointCloud2Modifier modifier(*pointcloudFusedMsg); - modifier.setPointCloud2Fields(4, "x", 1, sensor_msgs::PointField::FLOAT32, "y", 1, sensor_msgs::PointField::FLOAT32, - "z", 1, sensor_msgs::PointField::FLOAT32, "rgb", 1, sensor_msgs::PointField::FLOAT32); + sensor_msgs::PointCloud2Modifier modifier(*pointcloudFusedMsg); + modifier.setPointCloud2Fields(4, "x", 1, sensor_msgs::PointField::FLOAT32, "y", 1, sensor_msgs::PointField::FLOAT32, + "z", 1, sensor_msgs::PointField::FLOAT32, "rgb", 1, sensor_msgs::PointField::FLOAT32); - resized = true; - } + 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()); + // NODELET_INFO_STREAM("Chunks: " << mFusedPC.chunks.size()); - int index = 0; - float* ptCloudPtr = (float*)(&pointcloudFusedMsg->data[0]); - int updated = 0; + int index = 0; + float* ptCloudPtr = (float*)(&pointcloudFusedMsg->data[0]); + int updated = 0; - for (int c = 0; c < mFusedPC.chunks.size(); c++) - { - if (mFusedPC.chunks[c].has_been_updated || resized) - { - updated++; + for (int c = 0; c < mFusedPC.chunks.size(); c++) { + if (mFusedPC.chunks[c].has_been_updated || resized) { + updated++; - size_t chunkSize = mFusedPC.chunks[c].vertices.size(); + size_t chunkSize = mFusedPC.chunks[c].vertices.size(); - if (chunkSize > 0) - { - float* cloud_pts = (float*)(mFusedPC.chunks[c].vertices.data()); + if (chunkSize > 0) { + float* cloud_pts = (float*)(mFusedPC.chunks[c].vertices.data()); - memcpy(ptCloudPtr, cloud_pts, 4 * chunkSize * sizeof(float)); + memcpy(ptCloudPtr, cloud_pts, 4 * chunkSize * sizeof(float)); - ptCloudPtr += 4 * chunkSize; + ptCloudPtr += 4 * chunkSize; - pointcloudFusedMsg->header.stamp = sl_tools::slTime2Ros(mFusedPC.chunks[c].timestamp); - } - } - else - { - index += mFusedPC.chunks[c].vertices.size(); + 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); + // NODELET_INFO_STREAM("Updated: " << updated); - // double elapsed_usec = std::chrono::duration_cast(end_time - start_time).count(); + // double elapsed_usec = std::chrono::duration_cast(end_time - start_time).count(); - // NODELET_INFO_STREAM("Data copy: " << elapsed_usec << " usec [" << ptsCount << "] - " << (static_cast - // (ptsCount) / elapsed_usec) << " pts/usec"); + // NODELET_INFO_STREAM("Data copy: " << elapsed_usec << " usec [" << ptsCount << "] - " << (static_cast + // (ptsCount) / elapsed_usec) << " pts/usec"); - // Pointcloud publishing - mPubFusedCloud.publish(pointcloudFusedMsg); + // Pointcloud publishing + mPubFusedCloud.publish(pointcloudFusedMsg); } void ZEDWrapperNodelet::publishCamInfo(sensor_msgs::CameraInfoPtr camInfoMsg, ros::Publisher pubCamInfo, ros::Time t) { - static int seq = 0; - camInfoMsg->header.stamp = t; - camInfoMsg->header.seq = seq; - pubCamInfo.publish(camInfoMsg); - seq++; + static int seq = 0; + camInfoMsg->header.stamp = t; + camInfoMsg->header.seq = seq; + pubCamInfo.publish(camInfoMsg); + seq++; } void ZEDWrapperNodelet::fillCamInfo(sl::Camera& zed, sensor_msgs::CameraInfoPtr leftCamInfoMsg, - sensor_msgs::CameraInfoPtr rightCamInfoMsg, string leftFrameId, string rightFrameId, - bool rawParam /*= false*/) + sensor_msgs::CameraInfoPtr rightCamInfoMsg, string leftFrameId, string rightFrameId, + bool rawParam /*= false*/) { - sl::CalibrationParameters zedParam; + sl::CalibrationParameters zedParam; #if ZED_SDK_MAJOR_VERSION == 3 && ZED_SDK_MINOR_VERSION < 1 - if (rawParam) - { - zedParam = zed.getCameraInformation(mMatResolVideo).calibration_parameters_raw; // ok - } - else - { - zedParam = zed.getCameraInformation(mMatResolVideo).calibration_parameters; // ok - } + if (rawParam) { + zedParam = zed.getCameraInformation(mMatResolVideo).calibration_parameters_raw; // ok + } else { + zedParam = zed.getCameraInformation(mMatResolVideo).calibration_parameters; // ok + } #else - if (rawParam) - { - zedParam = zed.getCameraInformation(mMatResolVideo).camera_configuration.calibration_parameters_raw; - } - else - { - zedParam = zed.getCameraInformation(mMatResolVideo).camera_configuration.calibration_parameters; - } + if (rawParam) { + zedParam = zed.getCameraInformation(mMatResolVideo).camera_configuration.calibration_parameters_raw; + } else { + zedParam = zed.getCameraInformation(mMatResolVideo).camera_configuration.calibration_parameters; + } #endif - float baseline = zedParam.getCameraBaseline(); - leftCamInfoMsg->distortion_model = sensor_msgs::distortion_models::PLUMB_BOB; - rightCamInfoMsg->distortion_model = sensor_msgs::distortion_models::PLUMB_BOB; - leftCamInfoMsg->D.resize(5); - rightCamInfoMsg->D.resize(5); - leftCamInfoMsg->D[0] = zedParam.left_cam.disto[0]; // k1 - leftCamInfoMsg->D[1] = zedParam.left_cam.disto[1]; // k2 - leftCamInfoMsg->D[2] = zedParam.left_cam.disto[4]; // k3 - leftCamInfoMsg->D[3] = zedParam.left_cam.disto[2]; // p1 - leftCamInfoMsg->D[4] = zedParam.left_cam.disto[3]; // p2 - rightCamInfoMsg->D[0] = zedParam.right_cam.disto[0]; // k1 - rightCamInfoMsg->D[1] = zedParam.right_cam.disto[1]; // k2 - rightCamInfoMsg->D[2] = zedParam.right_cam.disto[4]; // k3 - rightCamInfoMsg->D[3] = zedParam.right_cam.disto[2]; // p1 - rightCamInfoMsg->D[4] = zedParam.right_cam.disto[3]; // p2 - leftCamInfoMsg->K.fill(0.0); - rightCamInfoMsg->K.fill(0.0); - leftCamInfoMsg->K[0] = static_cast(zedParam.left_cam.fx); - leftCamInfoMsg->K[2] = static_cast(zedParam.left_cam.cx); - leftCamInfoMsg->K[4] = static_cast(zedParam.left_cam.fy); - leftCamInfoMsg->K[5] = static_cast(zedParam.left_cam.cy); - leftCamInfoMsg->K[8] = 1.0; - rightCamInfoMsg->K[0] = static_cast(zedParam.right_cam.fx); - rightCamInfoMsg->K[2] = static_cast(zedParam.right_cam.cx); - rightCamInfoMsg->K[4] = static_cast(zedParam.right_cam.fy); - rightCamInfoMsg->K[5] = static_cast(zedParam.right_cam.cy); - rightCamInfoMsg->K[8] = 1.0; - leftCamInfoMsg->R.fill(0.0); - rightCamInfoMsg->R.fill(0.0); - - for (size_t i = 0; i < 3; i++) - { - // identity - rightCamInfoMsg->R[i + i * 3] = 1; - leftCamInfoMsg->R[i + i * 3] = 1; - } + float baseline = zedParam.getCameraBaseline(); + leftCamInfoMsg->distortion_model = sensor_msgs::distortion_models::PLUMB_BOB; + rightCamInfoMsg->distortion_model = sensor_msgs::distortion_models::PLUMB_BOB; + leftCamInfoMsg->D.resize(5); + rightCamInfoMsg->D.resize(5); + leftCamInfoMsg->D[0] = zedParam.left_cam.disto[0]; // k1 + leftCamInfoMsg->D[1] = zedParam.left_cam.disto[1]; // k2 + leftCamInfoMsg->D[2] = zedParam.left_cam.disto[4]; // k3 + leftCamInfoMsg->D[3] = zedParam.left_cam.disto[2]; // p1 + leftCamInfoMsg->D[4] = zedParam.left_cam.disto[3]; // p2 + rightCamInfoMsg->D[0] = zedParam.right_cam.disto[0]; // k1 + rightCamInfoMsg->D[1] = zedParam.right_cam.disto[1]; // k2 + rightCamInfoMsg->D[2] = zedParam.right_cam.disto[4]; // k3 + rightCamInfoMsg->D[3] = zedParam.right_cam.disto[2]; // p1 + rightCamInfoMsg->D[4] = zedParam.right_cam.disto[3]; // p2 + leftCamInfoMsg->K.fill(0.0); + rightCamInfoMsg->K.fill(0.0); + leftCamInfoMsg->K[0] = static_cast(zedParam.left_cam.fx); + leftCamInfoMsg->K[2] = static_cast(zedParam.left_cam.cx); + leftCamInfoMsg->K[4] = static_cast(zedParam.left_cam.fy); + leftCamInfoMsg->K[5] = static_cast(zedParam.left_cam.cy); + leftCamInfoMsg->K[8] = 1.0; + rightCamInfoMsg->K[0] = static_cast(zedParam.right_cam.fx); + rightCamInfoMsg->K[2] = static_cast(zedParam.right_cam.cx); + rightCamInfoMsg->K[4] = static_cast(zedParam.right_cam.fy); + rightCamInfoMsg->K[5] = static_cast(zedParam.right_cam.cy); + rightCamInfoMsg->K[8] = 1.0; + leftCamInfoMsg->R.fill(0.0); + rightCamInfoMsg->R.fill(0.0); + + for (size_t i = 0; i < 3; i++) { + // identity + rightCamInfoMsg->R[i + i * 3] = 1; + leftCamInfoMsg->R[i + i * 3] = 1; + } #if ZED_SDK_MAJOR_VERSION == 3 && ZED_SDK_MINOR_VERSION < 1 - if (rawParam) - { - std::vector R_ = sl_tools::convertRodrigues(zedParam.R); - float* p = R_.data(); + if (rawParam) { + std::vector R_ = sl_tools::convertRodrigues(zedParam.R); + float* p = R_.data(); - for (int i = 0; i < 9; i++) - { - rightCamInfoMsg->R[i] = p[i]; + for (int i = 0; i < 9; i++) { + rightCamInfoMsg->R[i] = p[i]; + } } - } #else - if (rawParam) - { - if (mUseOldExtrinsic) - { // Camera frame (Z forward, Y down, X right) - - std::vector R_ = sl_tools::convertRodrigues(zedParam.R); - float* p = R_.data(); - - for (int i = 0; i < 9; i++) - { - rightCamInfoMsg->R[i] = p[i]; - } - } - else - { // ROS frame (X forward, Z up, Y left) - for (int i = 0; i < 9; i++) - { - rightCamInfoMsg->R[i] = zedParam.stereo_transform.getRotationMatrix().r[i]; - } - } - } + if (rawParam) { + if (mUseOldExtrinsic) { // Camera frame (Z forward, Y down, X right) + + std::vector R_ = sl_tools::convertRodrigues(zedParam.R); + float* p = R_.data(); + + for (int i = 0; i < 9; i++) { + rightCamInfoMsg->R[i] = p[i]; + } + } else { // ROS frame (X forward, Z up, Y left) + for (int i = 0; i < 9; i++) { + rightCamInfoMsg->R[i] = zedParam.stereo_transform.getRotationMatrix().r[i]; + } + } + } #endif - leftCamInfoMsg->P.fill(0.0); - rightCamInfoMsg->P.fill(0.0); - leftCamInfoMsg->P[0] = static_cast(zedParam.left_cam.fx); - leftCamInfoMsg->P[2] = static_cast(zedParam.left_cam.cx); - leftCamInfoMsg->P[5] = static_cast(zedParam.left_cam.fy); - leftCamInfoMsg->P[6] = static_cast(zedParam.left_cam.cy); - leftCamInfoMsg->P[10] = 1.0; - // http://docs.ros.org/api/sensor_msgs/html/msg/CameraInfo.html - rightCamInfoMsg->P[3] = static_cast(-1 * zedParam.left_cam.fx * baseline); - rightCamInfoMsg->P[0] = static_cast(zedParam.right_cam.fx); - rightCamInfoMsg->P[2] = static_cast(zedParam.right_cam.cx); - rightCamInfoMsg->P[5] = static_cast(zedParam.right_cam.fy); - rightCamInfoMsg->P[6] = static_cast(zedParam.right_cam.cy); - rightCamInfoMsg->P[10] = 1.0; - leftCamInfoMsg->width = rightCamInfoMsg->width = static_cast(mMatResolVideo.width); - leftCamInfoMsg->height = rightCamInfoMsg->height = static_cast(mMatResolVideo.height); - leftCamInfoMsg->header.frame_id = leftFrameId; - rightCamInfoMsg->header.frame_id = rightFrameId; + leftCamInfoMsg->P.fill(0.0); + rightCamInfoMsg->P.fill(0.0); + leftCamInfoMsg->P[0] = static_cast(zedParam.left_cam.fx); + leftCamInfoMsg->P[2] = static_cast(zedParam.left_cam.cx); + leftCamInfoMsg->P[5] = static_cast(zedParam.left_cam.fy); + leftCamInfoMsg->P[6] = static_cast(zedParam.left_cam.cy); + leftCamInfoMsg->P[10] = 1.0; + // http://docs.ros.org/api/sensor_msgs/html/msg/CameraInfo.html + rightCamInfoMsg->P[3] = static_cast(-1 * zedParam.left_cam.fx * baseline); + rightCamInfoMsg->P[0] = static_cast(zedParam.right_cam.fx); + rightCamInfoMsg->P[2] = static_cast(zedParam.right_cam.cx); + rightCamInfoMsg->P[5] = static_cast(zedParam.right_cam.fy); + rightCamInfoMsg->P[6] = static_cast(zedParam.right_cam.cy); + rightCamInfoMsg->P[10] = 1.0; + leftCamInfoMsg->width = rightCamInfoMsg->width = static_cast(mMatResolVideo.width); + leftCamInfoMsg->height = rightCamInfoMsg->height = static_cast(mMatResolVideo.height); + leftCamInfoMsg->header.frame_id = leftFrameId; + rightCamInfoMsg->header.frame_id = rightFrameId; } void ZEDWrapperNodelet::fillCamDepthInfo(sl::Camera& zed, sensor_msgs::CameraInfoPtr depth_info_msg, string frame_id) { - sl::CalibrationParameters zedParam; + sl::CalibrationParameters zedParam; #if ZED_SDK_MAJOR_VERSION == 3 && ZED_SDK_MINOR_VERSION < 1 - zedParam = zed.getCameraInformation(mMatResolDepth).calibration_parameters; + zedParam = zed.getCameraInformation(mMatResolDepth).calibration_parameters; #else - zedParam = zed.getCameraInformation(mMatResolDepth).camera_configuration.calibration_parameters; + zedParam = zed.getCameraInformation(mMatResolDepth).camera_configuration.calibration_parameters; #endif - 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 - depth_info_msg->D[1] = zedParam.left_cam.disto[1]; // k2 - depth_info_msg->D[2] = zedParam.left_cam.disto[4]; // k3 - depth_info_msg->D[3] = zedParam.left_cam.disto[2]; // p1 - depth_info_msg->D[4] = zedParam.left_cam.disto[3]; // p2 - depth_info_msg->K.fill(0.0); - depth_info_msg->K[0] = static_cast(zedParam.left_cam.fx); - depth_info_msg->K[2] = static_cast(zedParam.left_cam.cx); - depth_info_msg->K[4] = static_cast(zedParam.left_cam.fy); - depth_info_msg->K[5] = static_cast(zedParam.left_cam.cy); - depth_info_msg->K[8] = 1.0; - depth_info_msg->R.fill(0.0); - - for (size_t i = 0; i < 3; i++) - { - // identity - depth_info_msg->R[i + i * 3] = 1; - } - - depth_info_msg->P.fill(0.0); - depth_info_msg->P[0] = static_cast(zedParam.left_cam.fx); - depth_info_msg->P[2] = static_cast(zedParam.left_cam.cx); - depth_info_msg->P[5] = static_cast(zedParam.left_cam.fy); - depth_info_msg->P[6] = static_cast(zedParam.left_cam.cy); - depth_info_msg->P[10] = 1.0; - // http://docs.ros.org/api/sensor_msgs/html/msg/CameraInfo.html - depth_info_msg->width = static_cast(mMatResolDepth.width); - depth_info_msg->height = static_cast(mMatResolDepth.height); - depth_info_msg->header.frame_id = frame_id; + 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 + depth_info_msg->D[1] = zedParam.left_cam.disto[1]; // k2 + depth_info_msg->D[2] = zedParam.left_cam.disto[4]; // k3 + depth_info_msg->D[3] = zedParam.left_cam.disto[2]; // p1 + depth_info_msg->D[4] = zedParam.left_cam.disto[3]; // p2 + depth_info_msg->K.fill(0.0); + depth_info_msg->K[0] = static_cast(zedParam.left_cam.fx); + depth_info_msg->K[2] = static_cast(zedParam.left_cam.cx); + depth_info_msg->K[4] = static_cast(zedParam.left_cam.fy); + depth_info_msg->K[5] = static_cast(zedParam.left_cam.cy); + depth_info_msg->K[8] = 1.0; + depth_info_msg->R.fill(0.0); + + for (size_t i = 0; i < 3; i++) { + // identity + depth_info_msg->R[i + i * 3] = 1; + } + + depth_info_msg->P.fill(0.0); + depth_info_msg->P[0] = static_cast(zedParam.left_cam.fx); + depth_info_msg->P[2] = static_cast(zedParam.left_cam.cx); + depth_info_msg->P[5] = static_cast(zedParam.left_cam.fy); + depth_info_msg->P[6] = static_cast(zedParam.left_cam.cy); + depth_info_msg->P[10] = 1.0; + // http://docs.ros.org/api/sensor_msgs/html/msg/CameraInfo.html + depth_info_msg->width = static_cast(mMatResolDepth.width); + depth_info_msg->height = static_cast(mMatResolDepth.height); + depth_info_msg->header.frame_id = frame_id; } void ZEDWrapperNodelet::updateDynamicReconfigure() { - // NODELET_DEBUG_STREAM( "updateDynamicReconfigure MUTEX LOCK"); - mDynParMutex.lock(); - zed_nodelets::ZedConfig config; - - config.auto_exposure_gain = mCamAutoExposure; - config.auto_whitebalance = mCamAutoWB; - config.brightness = mCamBrightness; - config.depth_confidence = mCamDepthConfidence; - config.depth_texture_conf = mCamDepthTextureConf; - config.contrast = mCamContrast; - config.exposure = mCamExposure; - config.gain = mCamGain; - config.hue = mCamHue; - config.saturation = mCamSaturation; - config.sharpness = mCamSharpness; - config.gamma = mCamGamma; - config.whitebalance_temperature = mCamWB / 100; - config.point_cloud_freq = mPointCloudFreq; - config.pub_frame_rate = mVideoDepthFreq; - mDynParMutex.unlock(); - - mDynServerMutex.lock(); - mDynRecServer->updateConfig(config); - mDynServerMutex.unlock(); - - mUpdateDynParams = false; - - // NODELET_DEBUG_STREAM( "updateDynamicReconfigure MUTEX UNLOCK"); + // NODELET_DEBUG_STREAM( "updateDynamicReconfigure MUTEX LOCK"); + mDynParMutex.lock(); + zed_nodelets::ZedConfig config; + + config.auto_exposure_gain = mCamAutoExposure; + config.auto_whitebalance = mCamAutoWB; + config.brightness = mCamBrightness; + config.depth_confidence = mCamDepthConfidence; + config.depth_texture_conf = mCamDepthTextureConf; + config.contrast = mCamContrast; + config.exposure = mCamExposure; + config.gain = mCamGain; + config.hue = mCamHue; + config.saturation = mCamSaturation; + config.sharpness = mCamSharpness; + config.gamma = mCamGamma; + config.whitebalance_temperature = mCamWB / 100; + config.point_cloud_freq = mPointCloudFreq; + config.pub_frame_rate = mVideoDepthFreq; + mDynParMutex.unlock(); + + mDynServerMutex.lock(); + mDynRecServer->updateConfig(config); + mDynServerMutex.unlock(); + + mUpdateDynParams = false; + + // NODELET_DEBUG_STREAM( "updateDynamicReconfigure MUTEX UNLOCK"); } void ZEDWrapperNodelet::callback_dynamicReconf(zed_nodelets::ZedConfig& config, uint32_t level) { - // NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX LOCK"); - mDynParMutex.lock(); - DynParams param = static_cast(level); + // NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX LOCK"); + mDynParMutex.lock(); + DynParams param = static_cast(level); - switch (param) - { + switch (param) { case DATAPUB_FREQ: - if (config.pub_frame_rate > mCamFrameRate) - { - mVideoDepthFreq = mCamFrameRate; - NODELET_WARN_STREAM("'pub_frame_rate' cannot be major than camera grabbing framerate. Set to " - << mVideoDepthFreq); - - mUpdateDynParams = true; - } - else - { - mVideoDepthFreq = config.pub_frame_rate; - NODELET_INFO("Reconfigure Video and Depth pub. frequency: %g", mVideoDepthFreq); - } - - mVideoDepthTimer.setPeriod(ros::Duration(1.0 / mVideoDepthFreq)); - - mDynParMutex.unlock(); - // NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); - break; + if (config.pub_frame_rate > mCamFrameRate) { + mVideoDepthFreq = mCamFrameRate; + NODELET_WARN_STREAM("'pub_frame_rate' cannot be major than camera grabbing framerate. Set to " + << mVideoDepthFreq); + + mUpdateDynParams = true; + } else { + mVideoDepthFreq = config.pub_frame_rate; + NODELET_INFO("Reconfigure Video and Depth pub. frequency: %g", mVideoDepthFreq); + } + + mVideoDepthTimer.setPeriod(ros::Duration(1.0 / mVideoDepthFreq)); + + mDynParMutex.unlock(); + // NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); + break; case CONFIDENCE: - mCamDepthConfidence = config.depth_confidence; - NODELET_INFO("Reconfigure confidence threshold: %d", mCamDepthConfidence); - mDynParMutex.unlock(); - // NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); - break; + mCamDepthConfidence = config.depth_confidence; + NODELET_INFO("Reconfigure confidence threshold: %d", mCamDepthConfidence); + mDynParMutex.unlock(); + // NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); + break; case TEXTURE_CONF: - mCamDepthTextureConf = config.depth_texture_conf; - NODELET_INFO("Reconfigure texture confidence threshold: %d", mCamDepthTextureConf); - mDynParMutex.unlock(); - // NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); - break; + mCamDepthTextureConf = config.depth_texture_conf; + NODELET_INFO("Reconfigure texture confidence threshold: %d", mCamDepthTextureConf); + mDynParMutex.unlock(); + // NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); + break; case POINTCLOUD_FREQ: - if (config.point_cloud_freq > mCamFrameRate) - { - mPointCloudFreq = mCamFrameRate; - NODELET_WARN_STREAM("'point_cloud_freq' cannot be major than camera grabbing framerate. Set to " - << mPointCloudFreq); - - mUpdateDynParams = true; - } - else - { - mPointCloudFreq = config.point_cloud_freq; - NODELET_INFO("Reconfigure point cloud pub. frequency: %g", mPointCloudFreq); - } - - mDynParMutex.unlock(); - // NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); - break; + if (config.point_cloud_freq > mCamFrameRate) { + mPointCloudFreq = mCamFrameRate; + NODELET_WARN_STREAM("'point_cloud_freq' cannot be major than camera grabbing framerate. Set to " + << mPointCloudFreq); + + mUpdateDynParams = true; + } else { + mPointCloudFreq = config.point_cloud_freq; + NODELET_INFO("Reconfigure point cloud pub. frequency: %g", mPointCloudFreq); + } + + mDynParMutex.unlock(); + // NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); + break; case BRIGHTNESS: - mCamBrightness = config.brightness; - NODELET_INFO("Reconfigure image brightness: %d", mCamBrightness); - mDynParMutex.unlock(); - // NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); - break; + mCamBrightness = config.brightness; + NODELET_INFO("Reconfigure image brightness: %d", mCamBrightness); + mDynParMutex.unlock(); + // NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); + break; case CONTRAST: - mCamContrast = config.contrast; - NODELET_INFO("Reconfigure image contrast: %d", mCamContrast); - mDynParMutex.unlock(); - // NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); - break; + mCamContrast = config.contrast; + NODELET_INFO("Reconfigure image contrast: %d", mCamContrast); + mDynParMutex.unlock(); + // NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); + break; case HUE: - mCamHue = config.hue; - NODELET_INFO("Reconfigure image hue: %d", mCamHue); - mDynParMutex.unlock(); - // NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); - break; + mCamHue = config.hue; + NODELET_INFO("Reconfigure image hue: %d", mCamHue); + mDynParMutex.unlock(); + // NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); + break; case SATURATION: - mCamSaturation = config.saturation; - NODELET_INFO("Reconfigure image saturation: %d", mCamSaturation); - mDynParMutex.unlock(); - // NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); - break; + mCamSaturation = config.saturation; + NODELET_INFO("Reconfigure image saturation: %d", mCamSaturation); + mDynParMutex.unlock(); + // NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); + break; case SHARPNESS: - mCamSharpness = config.sharpness; - NODELET_INFO("Reconfigure image sharpness: %d", mCamSharpness); - mDynParMutex.unlock(); - // NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); - break; + mCamSharpness = config.sharpness; + NODELET_INFO("Reconfigure image sharpness: %d", mCamSharpness); + mDynParMutex.unlock(); + // NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); + break; case GAMMA: #if (ZED_SDK_MAJOR_VERSION == 3 && ZED_SDK_MINOR_VERSION >= 1) - mCamGamma = config.gamma; - NODELET_INFO("Reconfigure image gamma: %d", mCamGamma); - mDynParMutex.unlock(); - // NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); + mCamGamma = config.gamma; + NODELET_INFO("Reconfigure image gamma: %d", mCamGamma); + mDynParMutex.unlock(); + // NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); #else - NODELET_DEBUG_STREAM("Gamma Control is not available for SDK older that v3.1"); - mDynParMutex.unlock(); + NODELET_DEBUG_STREAM("Gamma Control is not available for SDK older that v3.1"); + mDynParMutex.unlock(); #endif - break; + break; case AUTO_EXP_GAIN: - mCamAutoExposure = config.auto_exposure_gain; - NODELET_INFO_STREAM("Reconfigure auto exposure/gain: " << mCamAutoExposure ? "ENABLED" : "DISABLED"); - if (!mCamAutoExposure) - { - mZed.setCameraSettings(sl::VIDEO_SETTINGS::AEC_AGC, 0); - mTriggerAutoExposure = false; - } - else - { - mTriggerAutoExposure = true; - } - mDynParMutex.unlock(); - // NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); - break; + mCamAutoExposure = config.auto_exposure_gain; + NODELET_INFO_STREAM("Reconfigure auto exposure/gain: " << mCamAutoExposure ? "ENABLED" : "DISABLED"); + if (!mCamAutoExposure) { + mZed.setCameraSettings(sl::VIDEO_SETTINGS::AEC_AGC, 0); + mTriggerAutoExposure = false; + } else { + mTriggerAutoExposure = true; + } + mDynParMutex.unlock(); + // NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); + break; case GAIN: - mCamGain = config.gain; - if (mCamAutoExposure) - { - NODELET_WARN("Reconfigure gain has no effect if 'auto_exposure_gain' is enabled"); - } - else - { - NODELET_INFO("Reconfigure gain: %d", mCamGain); - } - mDynParMutex.unlock(); - // NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); - break; + mCamGain = config.gain; + if (mCamAutoExposure) { + NODELET_WARN("Reconfigure gain has no effect if 'auto_exposure_gain' is enabled"); + } else { + NODELET_INFO("Reconfigure gain: %d", mCamGain); + } + mDynParMutex.unlock(); + // NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); + break; case EXPOSURE: - mCamExposure = config.exposure; - if (mCamAutoExposure) - { - NODELET_WARN("Reconfigure exposure has no effect if 'auto_exposure_gain' is enabled"); - } - else - { - NODELET_INFO("Reconfigure exposure: %d", mCamExposure); - } - mDynParMutex.unlock(); - // NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); - break; + mCamExposure = config.exposure; + if (mCamAutoExposure) { + NODELET_WARN("Reconfigure exposure has no effect if 'auto_exposure_gain' is enabled"); + } else { + NODELET_INFO("Reconfigure exposure: %d", mCamExposure); + } + mDynParMutex.unlock(); + // NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); + break; case AUTO_WB: - mCamAutoWB = config.auto_whitebalance; - NODELET_INFO_STREAM("Reconfigure auto white balance: " << mCamAutoWB ? "ENABLED" : "DISABLED"); - if (!mCamAutoWB) - { - mZed.setCameraSettings(sl::VIDEO_SETTINGS::WHITEBALANCE_AUTO, 0); - mTriggerAutoWB = false; - } - else - { - mTriggerAutoWB = true; - } - mDynParMutex.unlock(); - // NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); - break; + mCamAutoWB = config.auto_whitebalance; + NODELET_INFO_STREAM("Reconfigure auto white balance: " << mCamAutoWB ? "ENABLED" : "DISABLED"); + if (!mCamAutoWB) { + mZed.setCameraSettings(sl::VIDEO_SETTINGS::WHITEBALANCE_AUTO, 0); + mTriggerAutoWB = false; + } else { + mTriggerAutoWB = true; + } + mDynParMutex.unlock(); + // NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); + break; case WB_TEMP: - mCamWB = config.whitebalance_temperature * 100; - if (mCamAutoWB) - { - NODELET_WARN("Reconfigure white balance temperature has no effect if 'auto_whitebalance' is enabled"); - } - else - { - NODELET_INFO("Reconfigure white balance temperature: %d", mCamWB); - } - mDynParMutex.unlock(); - // NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); - break; + mCamWB = config.whitebalance_temperature * 100; + if (mCamAutoWB) { + NODELET_WARN("Reconfigure white balance temperature has no effect if 'auto_whitebalance' is enabled"); + } else { + NODELET_INFO("Reconfigure white balance temperature: %d", mCamWB); + } + mDynParMutex.unlock(); + // NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); + break; default: - NODELET_DEBUG_STREAM("dynamicReconfCallback Unknown param: " << level); - mDynParMutex.unlock(); - // NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); - } + NODELET_DEBUG_STREAM("dynamicReconfCallback Unknown param: " << level); + mDynParMutex.unlock(); + // NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); + } } void ZEDWrapperNodelet::callback_pubVideoDepth(const ros::TimerEvent& e) { - static sl::Timestamp lastZedTs = 0; // Used to calculate stable publish frequency - - uint32_t rgbSubnumber = mPubRgb.getNumSubscribers(); - uint32_t rgbRawSubnumber = mPubRawRgb.getNumSubscribers(); - uint32_t leftSubnumber = mPubLeft.getNumSubscribers(); - uint32_t leftRawSubnumber = mPubRawLeft.getNumSubscribers(); - uint32_t rightSubnumber = mPubRight.getNumSubscribers(); - uint32_t rightRawSubnumber = mPubRawRight.getNumSubscribers(); - uint32_t rgbGraySubnumber = mPubRgbGray.getNumSubscribers(); - uint32_t rgbGrayRawSubnumber = mPubRawRgbGray.getNumSubscribers(); - uint32_t leftGraySubnumber = mPubLeftGray.getNumSubscribers(); - uint32_t leftGrayRawSubnumber = mPubRawLeftGray.getNumSubscribers(); - uint32_t rightGraySubnumber = mPubRightGray.getNumSubscribers(); - uint32_t rightGrayRawSubnumber = mPubRawRightGray.getNumSubscribers(); - uint32_t depthSubnumber = mPubDepth.getNumSubscribers(); - uint32_t disparitySubnumber = mPubDisparity.getNumSubscribers(); - uint32_t confMapSubnumber = mPubConfMap.getNumSubscribers(); - uint32_t stereoSubNumber = mPubStereo.getNumSubscribers(); - uint32_t stereoRawSubNumber = mPubRawStereo.getNumSubscribers(); - - uint32_t tot_sub = rgbSubnumber + rgbRawSubnumber + leftSubnumber + leftRawSubnumber + rightSubnumber + - rightRawSubnumber + rgbGraySubnumber + rgbGrayRawSubnumber + leftGraySubnumber + - leftGrayRawSubnumber + rightGraySubnumber + rightGrayRawSubnumber + depthSubnumber + - disparitySubnumber + confMapSubnumber + stereoSubNumber + stereoRawSubNumber; - - bool retrieved = false; - - sl::Mat mat_left, mat_left_raw; - sl::Mat mat_right, mat_right_raw; - sl::Mat mat_left_gray, mat_left_raw_gray; - 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 grab_ts = 0; - - mCamDataMutex.lock(); - - // ----> Retrieve all required image data - if (rgbSubnumber + leftSubnumber + stereoSubNumber > 0) - { - mZed.retrieveImage(mat_left, sl::VIEW::LEFT, sl::MEM::CPU, mMatResolVideo); - retrieved = true; - ts_rgb = mat_left.timestamp; - grab_ts = mat_left.timestamp; - } - if (rgbRawSubnumber + leftRawSubnumber + stereoRawSubNumber > 0) - { - mZed.retrieveImage(mat_left_raw, sl::VIEW::LEFT_UNRECTIFIED, sl::MEM::CPU, mMatResolVideo); - retrieved = true; - grab_ts = mat_left_raw.timestamp; - } - if (rightSubnumber + stereoSubNumber > 0) - { - mZed.retrieveImage(mat_right, sl::VIEW::RIGHT, sl::MEM::CPU, mMatResolVideo); - retrieved = true; - grab_ts = mat_right.timestamp; - } - if (rightRawSubnumber + stereoRawSubNumber > 0) - { - mZed.retrieveImage(mat_right_raw, sl::VIEW::RIGHT_UNRECTIFIED, sl::MEM::CPU, mMatResolVideo); - retrieved = true; - grab_ts = mat_right_raw.timestamp; - } - if (rgbGraySubnumber + leftGraySubnumber > 0) - { - mZed.retrieveImage(mat_left_gray, sl::VIEW::LEFT_GRAY, sl::MEM::CPU, mMatResolVideo); - retrieved = true; - grab_ts = mat_left_gray.timestamp; - } - if (rgbGrayRawSubnumber + leftGrayRawSubnumber > 0) - { - mZed.retrieveImage(mat_left_raw_gray, sl::VIEW::LEFT_UNRECTIFIED_GRAY, sl::MEM::CPU, mMatResolVideo); - retrieved = true; - grab_ts = mat_left_raw_gray.timestamp; - } - if (rightGraySubnumber > 0) - { - mZed.retrieveImage(mat_right_gray, sl::VIEW::RIGHT_GRAY, sl::MEM::CPU, mMatResolVideo); - retrieved = true; - grab_ts = mat_right_gray.timestamp; - } - if (rightGrayRawSubnumber > 0) - { - mZed.retrieveImage(mat_right_raw_gray, sl::VIEW::RIGHT_UNRECTIFIED_GRAY, sl::MEM::CPU, mMatResolVideo); - retrieved = true; - grab_ts = mat_right_raw_gray.timestamp; - } - if (depthSubnumber > 0) - { + static sl::Timestamp lastZedTs = 0; // Used to calculate stable publish frequency + + uint32_t rgbSubnumber = mPubRgb.getNumSubscribers(); + uint32_t rgbRawSubnumber = mPubRawRgb.getNumSubscribers(); + uint32_t leftSubnumber = mPubLeft.getNumSubscribers(); + uint32_t leftRawSubnumber = mPubRawLeft.getNumSubscribers(); + uint32_t rightSubnumber = mPubRight.getNumSubscribers(); + uint32_t rightRawSubnumber = mPubRawRight.getNumSubscribers(); + uint32_t rgbGraySubnumber = mPubRgbGray.getNumSubscribers(); + uint32_t rgbGrayRawSubnumber = mPubRawRgbGray.getNumSubscribers(); + uint32_t leftGraySubnumber = mPubLeftGray.getNumSubscribers(); + uint32_t leftGrayRawSubnumber = mPubRawLeftGray.getNumSubscribers(); + uint32_t rightGraySubnumber = mPubRightGray.getNumSubscribers(); + uint32_t rightGrayRawSubnumber = mPubRawRightGray.getNumSubscribers(); + uint32_t depthSubnumber = mPubDepth.getNumSubscribers(); + uint32_t disparitySubnumber = mPubDisparity.getNumSubscribers(); + uint32_t confMapSubnumber = mPubConfMap.getNumSubscribers(); + uint32_t stereoSubNumber = mPubStereo.getNumSubscribers(); + uint32_t stereoRawSubNumber = mPubRawStereo.getNumSubscribers(); + + uint32_t tot_sub = rgbSubnumber + rgbRawSubnumber + leftSubnumber + leftRawSubnumber + rightSubnumber + rightRawSubnumber + rgbGraySubnumber + rgbGrayRawSubnumber + leftGraySubnumber + leftGrayRawSubnumber + rightGraySubnumber + rightGrayRawSubnumber + depthSubnumber + disparitySubnumber + confMapSubnumber + stereoSubNumber + stereoRawSubNumber; + + bool retrieved = false; + + sl::Mat mat_left, mat_left_raw; + sl::Mat mat_right, mat_right_raw; + sl::Mat mat_left_gray, mat_left_raw_gray; + 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 grab_ts = 0; + + mCamDataMutex.lock(); + + // ----> Retrieve all required image data + if (rgbSubnumber + leftSubnumber + stereoSubNumber > 0) { + mZed.retrieveImage(mat_left, sl::VIEW::LEFT, sl::MEM::CPU, mMatResolVideo); + retrieved = true; + ts_rgb = mat_left.timestamp; + grab_ts = mat_left.timestamp; + } + if (rgbRawSubnumber + leftRawSubnumber + stereoRawSubNumber > 0) { + mZed.retrieveImage(mat_left_raw, sl::VIEW::LEFT_UNRECTIFIED, sl::MEM::CPU, mMatResolVideo); + retrieved = true; + grab_ts = mat_left_raw.timestamp; + } + if (rightSubnumber + stereoSubNumber > 0) { + mZed.retrieveImage(mat_right, sl::VIEW::RIGHT, sl::MEM::CPU, mMatResolVideo); + retrieved = true; + grab_ts = mat_right.timestamp; + } + if (rightRawSubnumber + stereoRawSubNumber > 0) { + mZed.retrieveImage(mat_right_raw, sl::VIEW::RIGHT_UNRECTIFIED, sl::MEM::CPU, mMatResolVideo); + retrieved = true; + grab_ts = mat_right_raw.timestamp; + } + if (rgbGraySubnumber + leftGraySubnumber > 0) { + mZed.retrieveImage(mat_left_gray, sl::VIEW::LEFT_GRAY, sl::MEM::CPU, mMatResolVideo); + retrieved = true; + grab_ts = mat_left_gray.timestamp; + } + if (rgbGrayRawSubnumber + leftGrayRawSubnumber > 0) { + mZed.retrieveImage(mat_left_raw_gray, sl::VIEW::LEFT_UNRECTIFIED_GRAY, sl::MEM::CPU, mMatResolVideo); + retrieved = true; + grab_ts = mat_left_raw_gray.timestamp; + } + if (rightGraySubnumber > 0) { + mZed.retrieveImage(mat_right_gray, sl::VIEW::RIGHT_GRAY, sl::MEM::CPU, mMatResolVideo); + retrieved = true; + grab_ts = mat_right_gray.timestamp; + } + if (rightGrayRawSubnumber > 0) { + mZed.retrieveImage(mat_right_raw_gray, sl::VIEW::RIGHT_UNRECTIFIED_GRAY, sl::MEM::CPU, mMatResolVideo); + retrieved = true; + grab_ts = mat_right_raw_gray.timestamp; + } + if (depthSubnumber > 0) { #if (ZED_SDK_MAJOR_VERSION == 3 && ZED_SDK_MINOR_VERSION < 4) - mZed.retrieveMeasure(mat_depth, sl::MEASURE::DEPTH, sl::MEM::CPU, mMatResolDepth); + mZed.retrieveMeasure(mat_depth, sl::MEASURE::DEPTH, sl::MEM::CPU, mMatResolDepth); #else - if (!mOpenniDepthMode) - { - mZed.retrieveMeasure(mat_depth, sl::MEASURE::DEPTH, sl::MEM::CPU, mMatResolDepth); + if (!mOpenniDepthMode) { + mZed.retrieveMeasure(mat_depth, sl::MEASURE::DEPTH, sl::MEM::CPU, mMatResolDepth); + } else { + mZed.retrieveMeasure(mat_depth, sl::MEASURE::DEPTH_U16_MM, sl::MEM::CPU, mMatResolDepth); + } +#endif + retrieved = true; + grab_ts = mat_depth.timestamp; + + ts_depth = mat_depth.timestamp; + + if (ts_rgb.data_ns != 0 && (ts_depth.data_ns != ts_rgb.data_ns)) { + NODELET_WARN_STREAM("!!!!! DEPTH/RGB ASYNC !!!!! - Delta: " << 1e-9 * static_cast(ts_depth - ts_rgb) + << " sec"); + } } - else - { - mZed.retrieveMeasure(mat_depth, sl::MEASURE::DEPTH_U16_MM, sl::MEM::CPU, mMatResolDepth); + if (disparitySubnumber > 0) { + mZed.retrieveMeasure(mat_disp, sl::MEASURE::DISPARITY, sl::MEM::CPU, mMatResolDepth); + retrieved = true; + grab_ts = mat_disp.timestamp; + } + if (confMapSubnumber > 0) { + mZed.retrieveMeasure(mat_conf, sl::MEASURE::CONFIDENCE, sl::MEM::CPU, mMatResolDepth); + retrieved = true; + grab_ts = mat_conf.timestamp; + } + // <---- Retrieve all required image data + + // ----> Data ROS timestamp + ros::Time stamp = sl_tools::slTime2Ros(grab_ts); + if (mSvoMode) { + stamp = ros::Time::now(); + } + // <---- Data ROS timestamp + + // ----> Publish sensor data if sync is required by user or SVO + if (mZedRealCamModel != sl::MODEL::ZED) { + if (mSensTimestampSync) { + // NODELET_INFO_STREAM("tot_sub: " << tot_sub << " - retrieved: " << retrieved << " - + // grab_ts.data_ns!=lastZedTs.data_ns: " << (grab_ts.data_ns!=lastZedTs.data_ns)); + if (tot_sub > 0 && retrieved && (grab_ts.data_ns != lastZedTs.data_ns)) { + // NODELET_INFO("CALLBACK"); + publishSensData(stamp); + } + } else if (mSvoMode) { + publishSensData(stamp); + } + } + // <---- Publish sensor data if sync is required by user or SVO + + mCamDataMutex.unlock(); + + // ----> Notify grab thread that all data are synchronized and a new grab can be done + // mRgbDepthDataRetrievedCondVar.notify_one(); + // mRgbDepthDataRetrieved = true; + // <---- Notify grab thread that all data are synchronized and a new grab can be done + + if (!retrieved) { + mPublishingData = false; + lastZedTs = 0; + return; + } + mPublishingData = true; + + // ----> Check if a grab has been done before publishing the same images + if (grab_ts.data_ns == lastZedTs.data_ns) { + // Data not updated by a grab calling in the grab thread + return; + } + if (lastZedTs.data_ns != 0) { + double period_sec = static_cast(grab_ts.data_ns - lastZedTs.data_ns) / 1e9; + // NODELET_DEBUG_STREAM( "PUBLISHING PERIOD: " << period_sec << " sec @" << 1./period_sec << " Hz") ; + + mVideoDepthPeriodMean_sec->addValue(period_sec); + // NODELET_DEBUG_STREAM( "MEAN PUBLISHING PERIOD: " << mVideoDepthPeriodMean_sec->getMean() << " sec @" + // << 1./mVideoDepthPeriodMean_sec->getMean() << " Hz") ; + } + lastZedTs = grab_ts; + // <---- Check if a grab has been done before publishing the same images + + // Publish the left = rgb image if someone has subscribed to + if (leftSubnumber > 0) { + sensor_msgs::ImagePtr leftImgMsg = boost::make_shared(); + publishImage(leftImgMsg, mat_left, mPubLeft, mLeftCamInfoMsg, mLeftCamOptFrameId, stamp); + } + if (rgbSubnumber > 0) { + sensor_msgs::ImagePtr rgbImgMsg = boost::make_shared(); + publishImage(rgbImgMsg, mat_left, mPubRgb, mRgbCamInfoMsg, mDepthOptFrameId, stamp); + } + + // Publish the left = rgb GRAY image if someone has subscribed to + if (leftGraySubnumber > 0) { + sensor_msgs::ImagePtr leftGrayImgMsg = boost::make_shared(); + publishImage(leftGrayImgMsg, mat_left_gray, mPubLeftGray, mLeftCamInfoMsg, mLeftCamOptFrameId, stamp); + } + if (rgbGraySubnumber > 0) { + sensor_msgs::ImagePtr rgbGrayImgMsg = boost::make_shared(); + publishImage(rgbGrayImgMsg, mat_left_gray, mPubRgbGray, mRgbCamInfoMsg, mDepthOptFrameId, stamp); + } + + // Publish the left_raw = rgb_raw image if someone has subscribed to + if (leftRawSubnumber > 0) { + sensor_msgs::ImagePtr rawLeftImgMsg = boost::make_shared(); + publishImage(rawLeftImgMsg, mat_left_raw, mPubRawLeft, mLeftCamInfoRawMsg, mLeftCamOptFrameId, stamp); + } + if (rgbRawSubnumber > 0) { + sensor_msgs::ImagePtr rawRgbImgMsg = boost::make_shared(); + publishImage(rawRgbImgMsg, mat_left_raw, mPubRawRgb, mRgbCamInfoRawMsg, mDepthOptFrameId, stamp); + } + + // Publish the left_raw == rgb_raw GRAY image if someone has subscribed to + if (leftGrayRawSubnumber > 0) { + sensor_msgs::ImagePtr rawLeftGrayImgMsg = boost::make_shared(); + + publishImage(rawLeftGrayImgMsg, mat_left_raw_gray, mPubRawLeftGray, mLeftCamInfoRawMsg, mLeftCamOptFrameId, stamp); + } + if (rgbGrayRawSubnumber > 0) { + sensor_msgs::ImagePtr rawRgbGrayImgMsg = boost::make_shared(); + publishImage(rawRgbGrayImgMsg, mat_left_raw_gray, mPubRawRgbGray, mRgbCamInfoRawMsg, mDepthOptFrameId, stamp); + } + + // Publish the right image if someone has subscribed to + if (rightSubnumber > 0) { + sensor_msgs::ImagePtr rightImgMsg = boost::make_shared(); + publishImage(rightImgMsg, mat_right, mPubRight, mRightCamInfoMsg, mRightCamOptFrameId, stamp); + } + + // Publish the right image GRAY if someone has subscribed to + if (rightGraySubnumber > 0) { + sensor_msgs::ImagePtr rightGrayImgMsg = boost::make_shared(); + publishImage(rightGrayImgMsg, mat_right_gray, mPubRightGray, mRightCamInfoMsg, mRightCamOptFrameId, stamp); + } + + // Publish the right raw image if someone has subscribed to + if (rightRawSubnumber > 0) { + sensor_msgs::ImagePtr rawRightImgMsg = boost::make_shared(); + publishImage(rawRightImgMsg, mat_right_raw, mPubRawRight, mRightCamInfoRawMsg, mRightCamOptFrameId, stamp); + } + + // Publish the right raw image GRAY if someone has subscribed to + if (rightGrayRawSubnumber > 0) { + sensor_msgs::ImagePtr rawRightGrayImgMsg = boost::make_shared(); + publishImage(rawRightGrayImgMsg, mat_right_raw_gray, mPubRawRightGray, mRightCamInfoRawMsg, mRightCamOptFrameId, + stamp); + } + + // Stereo couple side-by-side + if (stereoSubNumber > 0) { + sensor_msgs::ImagePtr stereoImgMsg = boost::make_shared(); + sl_tools::imagesToROSmsg(stereoImgMsg, mat_left, mat_right, mCameraFrameId, stamp); + mPubStereo.publish(stereoImgMsg); + } + + // Stereo RAW couple side-by-side + if (stereoRawSubNumber > 0) { + sensor_msgs::ImagePtr rawStereoImgMsg = boost::make_shared(); + sl_tools::imagesToROSmsg(rawStereoImgMsg, mat_left_raw, mat_right_raw, mCameraFrameId, stamp); + mPubRawStereo.publish(rawStereoImgMsg); + } + + // Publish the depth image if someone has subscribed to + if (depthSubnumber > 0) { + sensor_msgs::ImagePtr depthImgMsg = boost::make_shared(); + publishDepth(depthImgMsg, mat_depth, stamp); + } + + // Publish the disparity image if someone has subscribed to + if (disparitySubnumber > 0) { + publishDisparity(mat_disp, stamp); + } + + // Publish the confidence map if someone has subscribed to + if (confMapSubnumber > 0) { + sensor_msgs::ImagePtr confMapMsg = boost::make_shared(); + sl_tools::imageToROSmsg(confMapMsg, mat_conf, mConfidenceOptFrameId, stamp); + mPubConfMap.publish(confMapMsg); } -#endif - retrieved = true; - grab_ts = mat_depth.timestamp; - - ts_depth = mat_depth.timestamp; - - if (ts_rgb.data_ns != 0 && (ts_depth.data_ns != ts_rgb.data_ns)) - { - NODELET_WARN_STREAM("!!!!! DEPTH/RGB ASYNC !!!!! - Delta: " << 1e-9 * static_cast(ts_depth - ts_rgb) - << " sec"); - } - } - if (disparitySubnumber > 0) - { - mZed.retrieveMeasure(mat_disp, sl::MEASURE::DISPARITY, sl::MEM::CPU, mMatResolDepth); - retrieved = true; - grab_ts = mat_disp.timestamp; - } - if (confMapSubnumber > 0) - { - mZed.retrieveMeasure(mat_conf, sl::MEASURE::CONFIDENCE, sl::MEM::CPU, mMatResolDepth); - retrieved = true; - grab_ts = mat_conf.timestamp; - } - // <---- Retrieve all required image data - - // ----> Data ROS timestamp - ros::Time stamp = sl_tools::slTime2Ros(grab_ts); - if (mSvoMode) - { - stamp = ros::Time::now(); - } - // <---- Data ROS timestamp - - // ----> Publish sensor data if sync is required by user or SVO - if (mZedRealCamModel != sl::MODEL::ZED) - { - if (mSensTimestampSync) - { - // NODELET_INFO_STREAM("tot_sub: " << tot_sub << " - retrieved: " << retrieved << " - - // grab_ts.data_ns!=lastZedTs.data_ns: " << (grab_ts.data_ns!=lastZedTs.data_ns)); - if (tot_sub > 0 && retrieved && (grab_ts.data_ns != lastZedTs.data_ns)) - { - // NODELET_INFO("CALLBACK"); - publishSensData(stamp); - } - } - else if (mSvoMode) - { - publishSensData(stamp); - } - } - // <---- Publish sensor data if sync is required by user or SVO - - mCamDataMutex.unlock(); - - // ----> Notify grab thread that all data are synchronized and a new grab can be done - // mRgbDepthDataRetrievedCondVar.notify_one(); - // mRgbDepthDataRetrieved = true; - // <---- Notify grab thread that all data are synchronized and a new grab can be done - - if (!retrieved) - { - mPublishingData = false; - lastZedTs = 0; - return; - } - mPublishingData = true; - - // ----> Check if a grab has been done before publishing the same images - if (grab_ts.data_ns == lastZedTs.data_ns) - { - // Data not updated by a grab calling in the grab thread - return; - } - if (lastZedTs.data_ns != 0) - { - double period_sec = static_cast(grab_ts.data_ns - lastZedTs.data_ns) / 1e9; - // NODELET_DEBUG_STREAM( "PUBLISHING PERIOD: " << period_sec << " sec @" << 1./period_sec << " Hz") ; - - mVideoDepthPeriodMean_sec->addValue(period_sec); - // NODELET_DEBUG_STREAM( "MEAN PUBLISHING PERIOD: " << mVideoDepthPeriodMean_sec->getMean() << " sec @" - // << 1./mVideoDepthPeriodMean_sec->getMean() << " Hz") ; - } - lastZedTs = grab_ts; - // <---- Check if a grab has been done before publishing the same images - - // Publish the left = rgb image if someone has subscribed to - if (leftSubnumber > 0) - { - sensor_msgs::ImagePtr leftImgMsg = boost::make_shared(); - publishImage(leftImgMsg, mat_left, mPubLeft, mLeftCamInfoMsg, mLeftCamOptFrameId, stamp); - } - if (rgbSubnumber > 0) - { - sensor_msgs::ImagePtr rgbImgMsg = boost::make_shared(); - publishImage(rgbImgMsg, mat_left, mPubRgb, mRgbCamInfoMsg, mDepthOptFrameId, stamp); - } - - // Publish the left = rgb GRAY image if someone has subscribed to - if (leftGraySubnumber > 0) - { - sensor_msgs::ImagePtr leftGrayImgMsg = boost::make_shared(); - publishImage(leftGrayImgMsg, mat_left_gray, mPubLeftGray, mLeftCamInfoMsg, mLeftCamOptFrameId, stamp); - } - if (rgbGraySubnumber > 0) - { - sensor_msgs::ImagePtr rgbGrayImgMsg = boost::make_shared(); - publishImage(rgbGrayImgMsg, mat_left_gray, mPubRgbGray, mRgbCamInfoMsg, mDepthOptFrameId, stamp); - } - - // Publish the left_raw = rgb_raw image if someone has subscribed to - if (leftRawSubnumber > 0) - { - sensor_msgs::ImagePtr rawLeftImgMsg = boost::make_shared(); - publishImage(rawLeftImgMsg, mat_left_raw, mPubRawLeft, mLeftCamInfoRawMsg, mLeftCamOptFrameId, stamp); - } - if (rgbRawSubnumber > 0) - { - sensor_msgs::ImagePtr rawRgbImgMsg = boost::make_shared(); - publishImage(rawRgbImgMsg, mat_left_raw, mPubRawRgb, mRgbCamInfoRawMsg, mDepthOptFrameId, stamp); - } - - // Publish the left_raw == rgb_raw GRAY image if someone has subscribed to - if (leftGrayRawSubnumber > 0) - { - sensor_msgs::ImagePtr rawLeftGrayImgMsg = boost::make_shared(); - - publishImage(rawLeftGrayImgMsg, mat_left_raw_gray, mPubRawLeftGray, mLeftCamInfoRawMsg, mLeftCamOptFrameId, stamp); - } - if (rgbGrayRawSubnumber > 0) - { - sensor_msgs::ImagePtr rawRgbGrayImgMsg = boost::make_shared(); - publishImage(rawRgbGrayImgMsg, mat_left_raw_gray, mPubRawRgbGray, mRgbCamInfoRawMsg, mDepthOptFrameId, stamp); - } - - // Publish the right image if someone has subscribed to - if (rightSubnumber > 0) - { - sensor_msgs::ImagePtr rightImgMsg = boost::make_shared(); - publishImage(rightImgMsg, mat_right, mPubRight, mRightCamInfoMsg, mRightCamOptFrameId, stamp); - } - - // Publish the right image GRAY if someone has subscribed to - if (rightGraySubnumber > 0) - { - sensor_msgs::ImagePtr rightGrayImgMsg = boost::make_shared(); - publishImage(rightGrayImgMsg, mat_right_gray, mPubRightGray, mRightCamInfoMsg, mRightCamOptFrameId, stamp); - } - - // Publish the right raw image if someone has subscribed to - if (rightRawSubnumber > 0) - { - sensor_msgs::ImagePtr rawRightImgMsg = boost::make_shared(); - publishImage(rawRightImgMsg, mat_right_raw, mPubRawRight, mRightCamInfoRawMsg, mRightCamOptFrameId, stamp); - } - - // Publish the right raw image GRAY if someone has subscribed to - if (rightGrayRawSubnumber > 0) - { - sensor_msgs::ImagePtr rawRightGrayImgMsg = boost::make_shared(); - publishImage(rawRightGrayImgMsg, mat_right_raw_gray, mPubRawRightGray, mRightCamInfoRawMsg, mRightCamOptFrameId, - stamp); - } - - // Stereo couple side-by-side - if (stereoSubNumber > 0) - { - sensor_msgs::ImagePtr stereoImgMsg = boost::make_shared(); - sl_tools::imagesToROSmsg(stereoImgMsg, mat_left, mat_right, mCameraFrameId, stamp); - mPubStereo.publish(stereoImgMsg); - } - - // Stereo RAW couple side-by-side - if (stereoRawSubNumber > 0) - { - sensor_msgs::ImagePtr rawStereoImgMsg = boost::make_shared(); - sl_tools::imagesToROSmsg(rawStereoImgMsg, mat_left_raw, mat_right_raw, mCameraFrameId, stamp); - mPubRawStereo.publish(rawStereoImgMsg); - } - - // Publish the depth image if someone has subscribed to - if (depthSubnumber > 0) - { - sensor_msgs::ImagePtr depthImgMsg = boost::make_shared(); - publishDepth(depthImgMsg, mat_depth, stamp); - } - - // Publish the disparity image if someone has subscribed to - if (disparitySubnumber > 0) - { - publishDisparity(mat_disp, stamp); - } - - // Publish the confidence map if someone has subscribed to - if (confMapSubnumber > 0) - { - sensor_msgs::ImagePtr confMapMsg = boost::make_shared(); - sl_tools::imageToROSmsg(confMapMsg, mat_conf, mConfidenceOptFrameId, stamp); - mPubConfMap.publish(confMapMsg); - } } void ZEDWrapperNodelet::callback_pubPath(const ros::TimerEvent& e) { - uint32_t mapPathSub = mPubMapPath.getNumSubscribers(); - uint32_t odomPathSub = mPubOdomPath.getNumSubscribers(); - - geometry_msgs::PoseStamped odomPose; - geometry_msgs::PoseStamped mapPose; - - odomPose.header.stamp = mFrameTimestamp; - odomPose.header.frame_id = mMapFrameId; // frame - // conversion from Tranform to message - geometry_msgs::Transform base2odom = tf2::toMsg(mOdom2BaseTransf); - // Add all value in Pose message - odomPose.pose.position.x = base2odom.translation.x; - odomPose.pose.position.y = base2odom.translation.y; - odomPose.pose.position.z = base2odom.translation.z; - odomPose.pose.orientation.x = base2odom.rotation.x; - odomPose.pose.orientation.y = base2odom.rotation.y; - odomPose.pose.orientation.z = base2odom.rotation.z; - odomPose.pose.orientation.w = base2odom.rotation.w; - - mapPose.header.stamp = mFrameTimestamp; - mapPose.header.frame_id = mMapFrameId; // frame - // conversion from Tranform to message - geometry_msgs::Transform base2map = tf2::toMsg(mMap2BaseTransf); - // Add all value in Pose message - mapPose.pose.position.x = base2map.translation.x; - mapPose.pose.position.y = base2map.translation.y; - mapPose.pose.position.z = base2map.translation.z; - mapPose.pose.orientation.x = base2map.rotation.x; - mapPose.pose.orientation.y = base2map.rotation.y; - mapPose.pose.orientation.z = base2map.rotation.z; - mapPose.pose.orientation.w = base2map.rotation.w; - - // Circular vector - if (mPathMaxCount != -1) - { - if (mOdomPath.size() == mPathMaxCount) - { - NODELET_DEBUG("Path vectors full: rotating "); - std::rotate(mOdomPath.begin(), mOdomPath.begin() + 1, mOdomPath.end()); - std::rotate(mMapPath.begin(), mMapPath.begin() + 1, mMapPath.end()); - - mMapPath[mPathMaxCount - 1] = mapPose; - mOdomPath[mPathMaxCount - 1] = odomPose; - } - else - { - // NODELET_DEBUG_STREAM("Path vectors adding last available poses"); - mMapPath.push_back(mapPose); - mOdomPath.push_back(odomPose); - } - } - else - { - // NODELET_DEBUG_STREAM("No limit path vectors, adding last available poses"); - mMapPath.push_back(mapPose); - mOdomPath.push_back(odomPose); - } - - if (mapPathSub > 0) - { - nav_msgs::PathPtr mapPath = boost::make_shared(); - mapPath->header.frame_id = mMapFrameId; - mapPath->header.stamp = mFrameTimestamp; - mapPath->poses = mMapPath; - - mPubMapPath.publish(mapPath); - } - - if (odomPathSub > 0) - { - nav_msgs::PathPtr odomPath = boost::make_shared(); - odomPath->header.frame_id = mMapFrameId; - odomPath->header.stamp = mFrameTimestamp; - odomPath->poses = mOdomPath; - - mPubOdomPath.publish(odomPath); - } + uint32_t mapPathSub = mPubMapPath.getNumSubscribers(); + uint32_t odomPathSub = mPubOdomPath.getNumSubscribers(); + + geometry_msgs::PoseStamped odomPose; + geometry_msgs::PoseStamped mapPose; + + odomPose.header.stamp = mFrameTimestamp; + odomPose.header.frame_id = mMapFrameId; // frame + // conversion from Tranform to message + geometry_msgs::Transform base2odom = tf2::toMsg(mOdom2BaseTransf); + // Add all value in Pose message + odomPose.pose.position.x = base2odom.translation.x; + odomPose.pose.position.y = base2odom.translation.y; + odomPose.pose.position.z = base2odom.translation.z; + odomPose.pose.orientation.x = base2odom.rotation.x; + odomPose.pose.orientation.y = base2odom.rotation.y; + odomPose.pose.orientation.z = base2odom.rotation.z; + odomPose.pose.orientation.w = base2odom.rotation.w; + + mapPose.header.stamp = mFrameTimestamp; + mapPose.header.frame_id = mMapFrameId; // frame + // conversion from Tranform to message + geometry_msgs::Transform base2map = tf2::toMsg(mMap2BaseTransf); + // Add all value in Pose message + mapPose.pose.position.x = base2map.translation.x; + mapPose.pose.position.y = base2map.translation.y; + mapPose.pose.position.z = base2map.translation.z; + mapPose.pose.orientation.x = base2map.rotation.x; + mapPose.pose.orientation.y = base2map.rotation.y; + mapPose.pose.orientation.z = base2map.rotation.z; + mapPose.pose.orientation.w = base2map.rotation.w; + + // Circular vector + if (mPathMaxCount != -1) { + if (mOdomPath.size() == mPathMaxCount) { + NODELET_DEBUG("Path vectors full: rotating "); + std::rotate(mOdomPath.begin(), mOdomPath.begin() + 1, mOdomPath.end()); + std::rotate(mMapPath.begin(), mMapPath.begin() + 1, mMapPath.end()); + + mMapPath[mPathMaxCount - 1] = mapPose; + mOdomPath[mPathMaxCount - 1] = odomPose; + } else { + // NODELET_DEBUG_STREAM("Path vectors adding last available poses"); + mMapPath.push_back(mapPose); + mOdomPath.push_back(odomPose); + } + } else { + // NODELET_DEBUG_STREAM("No limit path vectors, adding last available poses"); + mMapPath.push_back(mapPose); + mOdomPath.push_back(odomPose); + } + + if (mapPathSub > 0) { + nav_msgs::PathPtr mapPath = boost::make_shared(); + mapPath->header.frame_id = mMapFrameId; + mapPath->header.stamp = mFrameTimestamp; + mapPath->poses = mMapPath; + + mPubMapPath.publish(mapPath); + } + + if (odomPathSub > 0) { + nav_msgs::PathPtr odomPath = boost::make_shared(); + odomPath->header.frame_id = mMapFrameId; + odomPath->header.stamp = mFrameTimestamp; + odomPath->poses = mOdomPath; + + mPubOdomPath.publish(odomPath); + } } void ZEDWrapperNodelet::callback_pubSensorsData(const ros::TimerEvent& e) { - // NODELET_INFO("callback_pubSensorsData"); - std::lock_guard lock(mCloseZedMutex); + // NODELET_INFO("callback_pubSensorsData"); + std::lock_guard lock(mCloseZedMutex); - if (!mZed.isOpened()) - { - return; - } + if (!mZed.isOpened()) { + return; + } - publishSensData(); + publishSensData(); } void ZEDWrapperNodelet::publishSensData(ros::Time t) { - // NODELET_INFO("publishSensData"); - - uint32_t imu_SubNumber = mPubImu.getNumSubscribers(); - uint32_t imu_RawSubNumber = mPubImuRaw.getNumSubscribers(); - uint32_t imu_TempSubNumber = 0; - uint32_t imu_MagSubNumber = 0; - uint32_t pressSubNumber = 0; - uint32_t tempLeftSubNumber = 0; - uint32_t tempRightSubNumber = 0; - - if (mZedRealCamModel == sl::MODEL::ZED2 || mZedRealCamModel == sl::MODEL::ZED2i) - { - imu_TempSubNumber = mPubImuTemp.getNumSubscribers(); - imu_MagSubNumber = mPubImuMag.getNumSubscribers(); - pressSubNumber = mPubPressure.getNumSubscribers(); - tempLeftSubNumber = mPubTempL.getNumSubscribers(); - tempRightSubNumber = mPubTempR.getNumSubscribers(); - } - - uint32_t tot_sub = imu_SubNumber + imu_RawSubNumber + imu_TempSubNumber + imu_MagSubNumber + pressSubNumber + - tempLeftSubNumber + tempRightSubNumber; - - if (tot_sub > 0) - { - mSensPublishing = true; - } - else - { - mSensPublishing = false; - } - - bool sensors_data_published = false; - - ros::Time ts_imu; - ros::Time ts_baro; - ros::Time ts_mag; - - static ros::Time lastTs_imu = ros::Time(); - static ros::Time lastTs_baro = ros::Time(); - static ros::Time lastT_mag = ros::Time(); - - sl::SensorsData sens_data; - - if (mSvoMode || mSensTimestampSync) - { - if (mZed.getSensorsData(sens_data, sl::TIME_REFERENCE::IMAGE) != sl::ERROR_CODE::SUCCESS) - { - NODELET_DEBUG("Not retrieved sensors data in IMAGE REFERENCE TIME"); - return; - } - } - else - { - if (mZed.getSensorsData(sens_data, sl::TIME_REFERENCE::CURRENT) != sl::ERROR_CODE::SUCCESS) - { - NODELET_DEBUG("Not retrieved sensors data in CURRENT REFERENCE TIME"); - return; - } - } - - if (t != ros::Time(0)) - { - ts_imu = t; - ts_baro = t; - ts_mag = t; - } - else - { - ts_imu = sl_tools::slTime2Ros(sens_data.imu.timestamp); - ts_baro = sl_tools::slTime2Ros(sens_data.barometer.timestamp); - ts_mag = sl_tools::slTime2Ros(sens_data.magnetometer.timestamp); - } - - bool new_imu_data = ts_imu != lastTs_imu; - bool new_baro_data = ts_baro != lastTs_baro; - bool new_mag_data = ts_mag != lastT_mag; - - if (!new_imu_data && !new_baro_data && !new_mag_data) - { - NODELET_DEBUG("No updated sensors data"); - return; - } - - // ----> Publish odometry tf only if enabled - if (mPublishTf && mPosTrackingReady && new_imu_data) - { - NODELET_DEBUG("Publishing TF"); - - publishOdomFrame(mOdom2BaseTransf, ts_imu); // publish the base Frame in odometry frame - - if (mPublishMapTf) - { - publishPoseFrame(mMap2OdomTransf, ts_imu); // publish the odometry Frame in map frame - } - } - // <---- Publish odometry tf only if enabled - - if (mPublishImuTf && !mStaticImuFramePublished) - { - NODELET_DEBUG("Publishing static IMU TF"); - publishStaticImuFrame(); - } - - if (mZedRealCamModel == sl::MODEL::ZED2 || mZedRealCamModel == sl::MODEL::ZED2i) - { - // Update temperatures for Diagnostic - sens_data.temperature.get(sl::SensorsData::TemperatureData::SENSOR_LOCATION::ONBOARD_LEFT, mTempLeft); - sens_data.temperature.get(sl::SensorsData::TemperatureData::SENSOR_LOCATION::ONBOARD_RIGHT, mTempRight); - } - - if (imu_TempSubNumber > 0 && new_imu_data) - { - lastTs_imu = ts_imu; - - sensor_msgs::TemperaturePtr imuTempMsg = boost::make_shared(); - - imuTempMsg->header.stamp = ts_imu; + // NODELET_INFO("publishSensData"); + + uint32_t imu_SubNumber = mPubImu.getNumSubscribers(); + uint32_t imu_RawSubNumber = mPubImuRaw.getNumSubscribers(); + uint32_t imu_TempSubNumber = 0; + uint32_t imu_MagSubNumber = 0; + uint32_t pressSubNumber = 0; + uint32_t tempLeftSubNumber = 0; + uint32_t tempRightSubNumber = 0; + + if (mZedRealCamModel == sl::MODEL::ZED2 || mZedRealCamModel == sl::MODEL::ZED2i) { + imu_TempSubNumber = mPubImuTemp.getNumSubscribers(); + imu_MagSubNumber = mPubImuMag.getNumSubscribers(); + pressSubNumber = mPubPressure.getNumSubscribers(); + tempLeftSubNumber = mPubTempL.getNumSubscribers(); + tempRightSubNumber = mPubTempR.getNumSubscribers(); + } + + uint32_t tot_sub = imu_SubNumber + imu_RawSubNumber + imu_TempSubNumber + imu_MagSubNumber + pressSubNumber + tempLeftSubNumber + tempRightSubNumber; + + if (tot_sub > 0) { + mSensPublishing = true; + } else { + mSensPublishing = false; + } + + bool sensors_data_published = false; + + ros::Time ts_imu; + ros::Time ts_baro; + ros::Time ts_mag; + + static ros::Time lastTs_imu = ros::Time(); + static ros::Time lastTs_baro = ros::Time(); + static ros::Time lastT_mag = ros::Time(); + + sl::SensorsData sens_data; + + if (mSvoMode || mSensTimestampSync) { + if (mZed.getSensorsData(sens_data, sl::TIME_REFERENCE::IMAGE) != sl::ERROR_CODE::SUCCESS) { + NODELET_DEBUG("Not retrieved sensors data in IMAGE REFERENCE TIME"); + return; + } + } else { + if (mZed.getSensorsData(sens_data, sl::TIME_REFERENCE::CURRENT) != sl::ERROR_CODE::SUCCESS) { + NODELET_DEBUG("Not retrieved sensors data in CURRENT REFERENCE TIME"); + return; + } + } + + if (t != ros::Time(0)) { + ts_imu = t; + ts_baro = t; + ts_mag = t; + } else { + ts_imu = sl_tools::slTime2Ros(sens_data.imu.timestamp); + ts_baro = sl_tools::slTime2Ros(sens_data.barometer.timestamp); + ts_mag = sl_tools::slTime2Ros(sens_data.magnetometer.timestamp); + } + + bool new_imu_data = ts_imu != lastTs_imu; + bool new_baro_data = ts_baro != lastTs_baro; + bool new_mag_data = ts_mag != lastT_mag; + + if (!new_imu_data && !new_baro_data && !new_mag_data) { + NODELET_DEBUG("No updated sensors data"); + return; + } + + // ----> Publish odometry tf only if enabled + if (mPublishTf && mPosTrackingReady && new_imu_data) { + NODELET_DEBUG("Publishing TF"); + + publishOdomFrame(mOdom2BaseTransf, ts_imu); // publish the base Frame in odometry frame + + if (mPublishMapTf) { + publishPoseFrame(mMap2OdomTransf, ts_imu); // publish the odometry Frame in map frame + } + } + // <---- Publish odometry tf only if enabled + + if (mPublishImuTf && !mStaticImuFramePublished) { + NODELET_DEBUG("Publishing static IMU TF"); + publishStaticImuFrame(); + } + + if (mZedRealCamModel == sl::MODEL::ZED2 || mZedRealCamModel == sl::MODEL::ZED2i) { + // Update temperatures for Diagnostic + sens_data.temperature.get(sl::SensorsData::TemperatureData::SENSOR_LOCATION::ONBOARD_LEFT, mTempLeft); + sens_data.temperature.get(sl::SensorsData::TemperatureData::SENSOR_LOCATION::ONBOARD_RIGHT, mTempRight); + } + + if (imu_TempSubNumber > 0 && new_imu_data) { + lastTs_imu = ts_imu; + + sensor_msgs::TemperaturePtr imuTempMsg = boost::make_shared(); + + imuTempMsg->header.stamp = ts_imu; #ifdef DEBUG_SENS_TS - static ros::Time old_ts; - if (old_ts == imuTempMsg->header.stamp) - { - NODELET_WARN_STREAM("Publishing IMU data with old timestamp " << old_ts); + static ros::Time old_ts; + if (old_ts == imuTempMsg->header.stamp) { + NODELET_WARN_STREAM("Publishing IMU data with old timestamp " << old_ts); + } + old_ts = imuTempMsg->header.stamp; +#endif + + imuTempMsg->header.frame_id = mImuFrameId; + float imu_temp; + sens_data.temperature.get(sl::SensorsData::TemperatureData::SENSOR_LOCATION::IMU, imu_temp); + imuTempMsg->temperature = static_cast(imu_temp); + imuTempMsg->variance = 0.0; + + sensors_data_published = true; + mPubImuTemp.publish(imuTempMsg); + } else { + NODELET_DEBUG("No new IMU temp."); } - old_ts = imuTempMsg->header.stamp; + + if (sens_data.barometer.is_available && new_baro_data) { + lastTs_baro = ts_baro; + + if (pressSubNumber > 0) { + sensor_msgs::FluidPressurePtr pressMsg = boost::make_shared(); + + pressMsg->header.stamp = ts_baro; + +#ifdef DEBUG_SENS_TS + static ros::Time old_ts; + if (old_ts == pressMsg->header.stamp) { + NODELET_WARN_STREAM("Publishing BARO data with old timestamp " << old_ts); + } + old_ts = pressMsg->header.stamp; #endif + pressMsg->header.frame_id = mBaroFrameId; + pressMsg->fluid_pressure = sens_data.barometer.pressure * 1e2; // Pascal + pressMsg->variance = 1.0585e-2; - imuTempMsg->header.frame_id = mImuFrameId; - float imu_temp; - sens_data.temperature.get(sl::SensorsData::TemperatureData::SENSOR_LOCATION::IMU, imu_temp); - imuTempMsg->temperature = static_cast(imu_temp); - imuTempMsg->variance = 0.0; + sensors_data_published = true; + mPubPressure.publish(pressMsg); + } - sensors_data_published = true; - mPubImuTemp.publish(imuTempMsg); - } - else - { - NODELET_DEBUG("No new IMU temp."); - } + if (tempLeftSubNumber > 0) { + sensor_msgs::TemperaturePtr tempLeftMsg = boost::make_shared(); - if (sens_data.barometer.is_available && new_baro_data) - { - lastTs_baro = ts_baro; + tempLeftMsg->header.stamp = ts_baro; - if (pressSubNumber > 0) - { - sensor_msgs::FluidPressurePtr pressMsg = boost::make_shared(); +#ifdef DEBUG_SENS_TS + static ros::Time old_ts; + if (old_ts == tempLeftMsg->header.stamp) { + NODELET_WARN_STREAM("Publishing BARO data with old timestamp " << old_ts); + } + old_ts = tempLeftMsg->header.stamp; +#endif - pressMsg->header.stamp = ts_baro; + tempLeftMsg->header.frame_id = mTempLeftFrameId; + tempLeftMsg->temperature = static_cast(mTempLeft); + tempLeftMsg->variance = 0.0; + + sensors_data_published = true; + mPubTempL.publish(tempLeftMsg); + } + + if (tempRightSubNumber > 0) { + sensor_msgs::TemperaturePtr tempRightMsg = boost::make_shared(); + + tempRightMsg->header.stamp = ts_baro; #ifdef DEBUG_SENS_TS - static ros::Time old_ts; - if (old_ts == pressMsg->header.stamp) - { - NODELET_WARN_STREAM("Publishing BARO data with old timestamp " << old_ts); - } - old_ts = pressMsg->header.stamp; + static ros::Time old_ts; + if (old_ts == tempRightMsg->header.stamp) { + NODELET_WARN_STREAM("Publishing BARO data with old timestamp " << old_ts); + } + old_ts = tempRightMsg->header.stamp; #endif - pressMsg->header.frame_id = mBaroFrameId; - pressMsg->fluid_pressure = sens_data.barometer.pressure * 1e2; // Pascal - pressMsg->variance = 1.0585e-2; - sensors_data_published = true; - mPubPressure.publish(pressMsg); + tempRightMsg->header.frame_id = mTempRightFrameId; + tempRightMsg->temperature = static_cast(mTempRight); + tempRightMsg->variance = 0.0; + + sensors_data_published = true; + mPubTempR.publish(tempRightMsg); + } + } else { + NODELET_DEBUG("No new BAROM. DATA"); } - if (tempLeftSubNumber > 0) - { - sensor_msgs::TemperaturePtr tempLeftMsg = boost::make_shared(); + if (imu_MagSubNumber > 0) { + if (sens_data.magnetometer.is_available && new_mag_data) { + lastT_mag = ts_mag; - tempLeftMsg->header.stamp = ts_baro; + sensor_msgs::MagneticFieldPtr magMsg = boost::make_shared(); + + magMsg->header.stamp = ts_mag; #ifdef DEBUG_SENS_TS - static ros::Time old_ts; - if (old_ts == tempLeftMsg->header.stamp) - { - NODELET_WARN_STREAM("Publishing BARO data with old timestamp " << old_ts); - } - old_ts = tempLeftMsg->header.stamp; + static ros::Time old_ts; + if (old_ts == magMsg->header.stamp) { + NODELET_WARN_STREAM("Publishing MAG data with old timestamp " << old_ts); + } + old_ts = magMsg->header.stamp; #endif - tempLeftMsg->header.frame_id = mTempLeftFrameId; - tempLeftMsg->temperature = static_cast(mTempLeft); - tempLeftMsg->variance = 0.0; - - sensors_data_published = true; - mPubTempL.publish(tempLeftMsg); + magMsg->header.frame_id = mMagFrameId; + magMsg->magnetic_field.x = sens_data.magnetometer.magnetic_field_calibrated.x * 1e-6; // Tesla + magMsg->magnetic_field.y = sens_data.magnetometer.magnetic_field_calibrated.y * 1e-6; // Tesla + magMsg->magnetic_field.z = sens_data.magnetometer.magnetic_field_calibrated.z * 1e-6; // Tesla + magMsg->magnetic_field_covariance[0] = 0.039e-6; + magMsg->magnetic_field_covariance[1] = 0.0f; + magMsg->magnetic_field_covariance[2] = 0.0f; + magMsg->magnetic_field_covariance[3] = 0.0f; + magMsg->magnetic_field_covariance[4] = 0.037e-6; + magMsg->magnetic_field_covariance[5] = 0.0f; + magMsg->magnetic_field_covariance[6] = 0.0f; + magMsg->magnetic_field_covariance[7] = 0.0f; + magMsg->magnetic_field_covariance[8] = 0.047e-6; + + sensors_data_published = true; + mPubImuMag.publish(magMsg); + } + } else { + NODELET_DEBUG("No new MAG. DATA"); } - if (tempRightSubNumber > 0) - { - sensor_msgs::TemperaturePtr tempRightMsg = boost::make_shared(); + if (imu_SubNumber > 0 && new_imu_data) { + lastTs_imu = ts_imu; + + sensor_msgs::ImuPtr imuMsg = boost::make_shared(); + + imuMsg->header.stamp = ts_imu; + +#ifdef DEBUG_SENS_TS + static ros::Time old_ts; + if (old_ts == imuMsg->header.stamp) { + NODELET_WARN_STREAM("Publishing IMU data with old timestamp " << old_ts); + } else { + NODELET_INFO_STREAM("Publishing IMU data with new timestamp. Freq: " << 1. / (ts_imu.toSec() - old_ts.toSec())); + old_ts = imuMsg->header.stamp; + } +#endif + + imuMsg->header.frame_id = mImuFrameId; + + imuMsg->orientation.x = sens_data.imu.pose.getOrientation()[0]; + imuMsg->orientation.y = sens_data.imu.pose.getOrientation()[1]; + imuMsg->orientation.z = sens_data.imu.pose.getOrientation()[2]; + imuMsg->orientation.w = sens_data.imu.pose.getOrientation()[3]; + + imuMsg->angular_velocity.x = sens_data.imu.angular_velocity[0] * DEG2RAD; + imuMsg->angular_velocity.y = sens_data.imu.angular_velocity[1] * DEG2RAD; + imuMsg->angular_velocity.z = sens_data.imu.angular_velocity[2] * DEG2RAD; - tempRightMsg->header.stamp = ts_baro; + imuMsg->linear_acceleration.x = sens_data.imu.linear_acceleration[0]; + imuMsg->linear_acceleration.y = sens_data.imu.linear_acceleration[1]; + imuMsg->linear_acceleration.z = sens_data.imu.linear_acceleration[2]; -#ifdef DEBUG_SENS_TS - static ros::Time old_ts; - if (old_ts == tempRightMsg->header.stamp) - { - NODELET_WARN_STREAM("Publishing BARO data with old timestamp " << old_ts); - } - old_ts = tempRightMsg->header.stamp; -#endif + for (int i = 0; i < 3; ++i) { + int r = 0; - tempRightMsg->header.frame_id = mTempRightFrameId; - tempRightMsg->temperature = static_cast(mTempRight); - tempRightMsg->variance = 0.0; + if (i == 0) { + r = 0; + } else if (i == 1) { + r = 1; + } else { + r = 2; + } - sensors_data_published = true; - mPubTempR.publish(tempRightMsg); - } - } - else - { - NODELET_DEBUG("No new BAROM. DATA"); - } + imuMsg->orientation_covariance[i * 3 + 0] = sens_data.imu.pose_covariance.r[r * 3 + 0] * DEG2RAD * DEG2RAD; + imuMsg->orientation_covariance[i * 3 + 1] = sens_data.imu.pose_covariance.r[r * 3 + 1] * DEG2RAD * DEG2RAD; + imuMsg->orientation_covariance[i * 3 + 2] = sens_data.imu.pose_covariance.r[r * 3 + 2] * DEG2RAD * DEG2RAD; - if (imu_MagSubNumber > 0) - { - if (sens_data.magnetometer.is_available && new_mag_data) - { - lastT_mag = ts_mag; + imuMsg->linear_acceleration_covariance[i * 3 + 0] = sens_data.imu.linear_acceleration_covariance.r[r * 3 + 0]; + imuMsg->linear_acceleration_covariance[i * 3 + 1] = sens_data.imu.linear_acceleration_covariance.r[r * 3 + 1]; + imuMsg->linear_acceleration_covariance[i * 3 + 2] = sens_data.imu.linear_acceleration_covariance.r[r * 3 + 2]; - sensor_msgs::MagneticFieldPtr magMsg = boost::make_shared(); + imuMsg->angular_velocity_covariance[i * 3 + 0] = sens_data.imu.angular_velocity_covariance.r[r * 3 + 0] * DEG2RAD * DEG2RAD; + imuMsg->angular_velocity_covariance[i * 3 + 1] = sens_data.imu.angular_velocity_covariance.r[r * 3 + 1] * DEG2RAD * DEG2RAD; + imuMsg->angular_velocity_covariance[i * 3 + 2] = sens_data.imu.angular_velocity_covariance.r[r * 3 + 2] * DEG2RAD * DEG2RAD; + } - magMsg->header.stamp = ts_mag; + sensors_data_published = true; + mPubImu.publish(imuMsg); + } else { + NODELET_DEBUG("No new IMU DATA"); + } -#ifdef DEBUG_SENS_TS - static ros::Time old_ts; - if (old_ts == magMsg->header.stamp) - { - NODELET_WARN_STREAM("Publishing MAG data with old timestamp " << old_ts); - } - old_ts = magMsg->header.stamp; -#endif + if (imu_RawSubNumber > 0 && new_imu_data) { + lastTs_imu = ts_imu; + + sensor_msgs::ImuPtr imuRawMsg = boost::make_shared(); + + imuRawMsg->header.stamp = ts_imu; + imuRawMsg->header.frame_id = mImuFrameId; + imuRawMsg->angular_velocity.x = sens_data.imu.angular_velocity[0] * DEG2RAD; + imuRawMsg->angular_velocity.y = sens_data.imu.angular_velocity[1] * DEG2RAD; + imuRawMsg->angular_velocity.z = sens_data.imu.angular_velocity[2] * DEG2RAD; + imuRawMsg->linear_acceleration.x = sens_data.imu.linear_acceleration[0]; + imuRawMsg->linear_acceleration.y = sens_data.imu.linear_acceleration[1]; + imuRawMsg->linear_acceleration.z = sens_data.imu.linear_acceleration[2]; + + for (int i = 0; i < 3; ++i) { + int r = 0; + + if (i == 0) { + r = 0; + } else if (i == 1) { + r = 1; + } else { + r = 2; + } - magMsg->header.frame_id = mMagFrameId; - magMsg->magnetic_field.x = sens_data.magnetometer.magnetic_field_calibrated.x * 1e-6; // Tesla - magMsg->magnetic_field.y = sens_data.magnetometer.magnetic_field_calibrated.y * 1e-6; // Tesla - magMsg->magnetic_field.z = sens_data.magnetometer.magnetic_field_calibrated.z * 1e-6; // Tesla - magMsg->magnetic_field_covariance[0] = 0.039e-6; - magMsg->magnetic_field_covariance[1] = 0.0f; - magMsg->magnetic_field_covariance[2] = 0.0f; - magMsg->magnetic_field_covariance[3] = 0.0f; - magMsg->magnetic_field_covariance[4] = 0.037e-6; - magMsg->magnetic_field_covariance[5] = 0.0f; - magMsg->magnetic_field_covariance[6] = 0.0f; - magMsg->magnetic_field_covariance[7] = 0.0f; - magMsg->magnetic_field_covariance[8] = 0.047e-6; - - sensors_data_published = true; - mPubImuMag.publish(magMsg); - } - } - else - { - NODELET_DEBUG("No new MAG. DATA"); - } - - if (imu_SubNumber > 0 && new_imu_data) - { - lastTs_imu = ts_imu; - - sensor_msgs::ImuPtr imuMsg = boost::make_shared(); - - imuMsg->header.stamp = ts_imu; + imuRawMsg->linear_acceleration_covariance[i * 3 + 0] = sens_data.imu.linear_acceleration_covariance.r[r * 3 + 0]; + imuRawMsg->linear_acceleration_covariance[i * 3 + 1] = sens_data.imu.linear_acceleration_covariance.r[r * 3 + 1]; + imuRawMsg->linear_acceleration_covariance[i * 3 + 2] = sens_data.imu.linear_acceleration_covariance.r[r * 3 + 2]; + imuRawMsg->angular_velocity_covariance[i * 3 + 0] = sens_data.imu.angular_velocity_covariance.r[r * 3 + 0] * DEG2RAD * DEG2RAD; + imuRawMsg->angular_velocity_covariance[i * 3 + 1] = sens_data.imu.angular_velocity_covariance.r[r * 3 + 1] * DEG2RAD * DEG2RAD; + imuRawMsg->angular_velocity_covariance[i * 3 + 2] = sens_data.imu.angular_velocity_covariance.r[r * 3 + 2] * DEG2RAD * DEG2RAD; + } -#ifdef DEBUG_SENS_TS - static ros::Time old_ts; - if (old_ts == imuMsg->header.stamp) - { - NODELET_WARN_STREAM("Publishing IMU data with old timestamp " << old_ts); - } - else - { - NODELET_INFO_STREAM("Publishing IMU data with new timestamp. Freq: " << 1. / (ts_imu.toSec() - old_ts.toSec())); - old_ts = imuMsg->header.stamp; + // Orientation data is not available in "data_raw" -> See ROS REP145 + // http://www.ros.org/reps/rep-0145.html#topics + imuRawMsg->orientation_covariance[0] = -1; + sensors_data_published = true; + mPubImuRaw.publish(imuRawMsg); } -#endif - imuMsg->header.frame_id = mImuFrameId; - - imuMsg->orientation.x = sens_data.imu.pose.getOrientation()[0]; - imuMsg->orientation.y = sens_data.imu.pose.getOrientation()[1]; - imuMsg->orientation.z = sens_data.imu.pose.getOrientation()[2]; - imuMsg->orientation.w = sens_data.imu.pose.getOrientation()[3]; - - imuMsg->angular_velocity.x = sens_data.imu.angular_velocity[0] * DEG2RAD; - imuMsg->angular_velocity.y = sens_data.imu.angular_velocity[1] * DEG2RAD; - imuMsg->angular_velocity.z = sens_data.imu.angular_velocity[2] * DEG2RAD; - - imuMsg->linear_acceleration.x = sens_data.imu.linear_acceleration[0]; - imuMsg->linear_acceleration.y = sens_data.imu.linear_acceleration[1]; - imuMsg->linear_acceleration.z = sens_data.imu.linear_acceleration[2]; - - for (int i = 0; i < 3; ++i) - { - int r = 0; - - if (i == 0) - { - r = 0; - } - else if (i == 1) - { - r = 1; - } - else - { - r = 2; - } - - imuMsg->orientation_covariance[i * 3 + 0] = sens_data.imu.pose_covariance.r[r * 3 + 0] * DEG2RAD * DEG2RAD; - imuMsg->orientation_covariance[i * 3 + 1] = sens_data.imu.pose_covariance.r[r * 3 + 1] * DEG2RAD * DEG2RAD; - imuMsg->orientation_covariance[i * 3 + 2] = sens_data.imu.pose_covariance.r[r * 3 + 2] * DEG2RAD * DEG2RAD; - - imuMsg->linear_acceleration_covariance[i * 3 + 0] = sens_data.imu.linear_acceleration_covariance.r[r * 3 + 0]; - imuMsg->linear_acceleration_covariance[i * 3 + 1] = sens_data.imu.linear_acceleration_covariance.r[r * 3 + 1]; - imuMsg->linear_acceleration_covariance[i * 3 + 2] = sens_data.imu.linear_acceleration_covariance.r[r * 3 + 2]; - - imuMsg->angular_velocity_covariance[i * 3 + 0] = - sens_data.imu.angular_velocity_covariance.r[r * 3 + 0] * DEG2RAD * DEG2RAD; - imuMsg->angular_velocity_covariance[i * 3 + 1] = - sens_data.imu.angular_velocity_covariance.r[r * 3 + 1] * DEG2RAD * DEG2RAD; - imuMsg->angular_velocity_covariance[i * 3 + 2] = - sens_data.imu.angular_velocity_covariance.r[r * 3 + 2] * DEG2RAD * DEG2RAD; - } - - sensors_data_published = true; - mPubImu.publish(imuMsg); - } - else - { - NODELET_DEBUG("No new IMU DATA"); - } - - if (imu_RawSubNumber > 0 && new_imu_data) - { - lastTs_imu = ts_imu; - - sensor_msgs::ImuPtr imuRawMsg = boost::make_shared(); - - imuRawMsg->header.stamp = ts_imu; - imuRawMsg->header.frame_id = mImuFrameId; - imuRawMsg->angular_velocity.x = sens_data.imu.angular_velocity[0] * DEG2RAD; - imuRawMsg->angular_velocity.y = sens_data.imu.angular_velocity[1] * DEG2RAD; - imuRawMsg->angular_velocity.z = sens_data.imu.angular_velocity[2] * DEG2RAD; - imuRawMsg->linear_acceleration.x = sens_data.imu.linear_acceleration[0]; - imuRawMsg->linear_acceleration.y = sens_data.imu.linear_acceleration[1]; - imuRawMsg->linear_acceleration.z = sens_data.imu.linear_acceleration[2]; - - for (int i = 0; i < 3; ++i) - { - int r = 0; - - if (i == 0) - { - r = 0; - } - else if (i == 1) - { - r = 1; - } - else - { - r = 2; - } - - imuRawMsg->linear_acceleration_covariance[i * 3 + 0] = sens_data.imu.linear_acceleration_covariance.r[r * 3 + 0]; - imuRawMsg->linear_acceleration_covariance[i * 3 + 1] = sens_data.imu.linear_acceleration_covariance.r[r * 3 + 1]; - imuRawMsg->linear_acceleration_covariance[i * 3 + 2] = sens_data.imu.linear_acceleration_covariance.r[r * 3 + 2]; - imuRawMsg->angular_velocity_covariance[i * 3 + 0] = - sens_data.imu.angular_velocity_covariance.r[r * 3 + 0] * DEG2RAD * DEG2RAD; - imuRawMsg->angular_velocity_covariance[i * 3 + 1] = - sens_data.imu.angular_velocity_covariance.r[r * 3 + 1] * DEG2RAD * DEG2RAD; - imuRawMsg->angular_velocity_covariance[i * 3 + 2] = - sens_data.imu.angular_velocity_covariance.r[r * 3 + 2] * DEG2RAD * DEG2RAD; - } - - // Orientation data is not available in "data_raw" -> See ROS REP145 - // http://www.ros.org/reps/rep-0145.html#topics - imuRawMsg->orientation_covariance[0] = -1; - sensors_data_published = true; - mPubImuRaw.publish(imuRawMsg); - } - - // ----> Update Diagnostic - if (sensors_data_published) - { - // Publish freq calculation - static std::chrono::steady_clock::time_point last_time = std::chrono::steady_clock::now(); - std::chrono::steady_clock::time_point now = std::chrono::steady_clock::now(); + // ----> Update Diagnostic + if (sensors_data_published) { + // Publish freq calculation + static std::chrono::steady_clock::time_point last_time = std::chrono::steady_clock::now(); + std::chrono::steady_clock::time_point now = std::chrono::steady_clock::now(); - double elapsed_usec = std::chrono::duration_cast(now - last_time).count(); - last_time = now; + double elapsed_usec = std::chrono::duration_cast(now - last_time).count(); + last_time = now; - mSensPeriodMean_usec->addValue(elapsed_usec); - } - // <---- Update Diagnostic + mSensPeriodMean_usec->addValue(elapsed_usec); + } + // <---- Update Diagnostic } void ZEDWrapperNodelet::device_poll_thread_func() { - ros::Rate loop_rate(mCamFrameRate); + ros::Rate loop_rate(mCamFrameRate); - mRecording = false; + mRecording = false; - mElabPeriodMean_sec.reset(new sl_tools::CSmartMean(mCamFrameRate)); - mGrabPeriodMean_usec.reset(new sl_tools::CSmartMean(mCamFrameRate)); - mVideoDepthPeriodMean_sec.reset(new sl_tools::CSmartMean(mCamFrameRate)); - mPcPeriodMean_usec.reset(new sl_tools::CSmartMean(mCamFrameRate)); - mObjDetPeriodMean_msec.reset(new sl_tools::CSmartMean(mCamFrameRate)); + mElabPeriodMean_sec.reset(new sl_tools::CSmartMean(mCamFrameRate)); + mGrabPeriodMean_usec.reset(new sl_tools::CSmartMean(mCamFrameRate)); + mVideoDepthPeriodMean_sec.reset(new sl_tools::CSmartMean(mCamFrameRate)); + mPcPeriodMean_usec.reset(new sl_tools::CSmartMean(mCamFrameRate)); + mObjDetPeriodMean_msec.reset(new sl_tools::CSmartMean(mCamFrameRate)); - // Timestamp initialization - if (mSvoMode) - { - mFrameTimestamp = ros::Time::now(); - } - else - { - mFrameTimestamp = sl_tools::slTime2Ros(mZed.getTimestamp(sl::TIME_REFERENCE::CURRENT)); - } + // Timestamp initialization + if (mSvoMode) { + mFrameTimestamp = ros::Time::now(); + } else { + mFrameTimestamp = sl_tools::slTime2Ros(mZed.getTimestamp(sl::TIME_REFERENCE::CURRENT)); + } - mPrevFrameTimestamp = mFrameTimestamp; + mPrevFrameTimestamp = mFrameTimestamp; - mPosTrackingActivated = false; - mMappingRunning = false; - mRecording = false; + mPosTrackingActivated = false; + mMappingRunning = false; + mRecording = false; - // Get the parameters of the ZED images + // Get the parameters of the ZED images #if ZED_SDK_MAJOR_VERSION == 3 && ZED_SDK_MINOR_VERSION < 1 - mCamWidth = mZed.getCameraInformation().camera_resolution.width; - mCamHeight = mZed.getCameraInformation().camera_resolution.height; + mCamWidth = mZed.getCameraInformation().camera_resolution.width; + mCamHeight = mZed.getCameraInformation().camera_resolution.height; #else - mCamWidth = mZed.getCameraInformation().camera_configuration.resolution.width; - mCamHeight = mZed.getCameraInformation().camera_configuration.resolution.height; + mCamWidth = mZed.getCameraInformation().camera_configuration.resolution.width; + mCamHeight = mZed.getCameraInformation().camera_configuration.resolution.height; #endif - NODELET_DEBUG_STREAM("Camera Frame size : " << mCamWidth << "x" << mCamHeight); - int v_w = static_cast(mCamWidth * mCamImageResizeFactor); - int v_h = static_cast(mCamHeight * mCamImageResizeFactor); - mMatResolVideo = sl::Resolution(v_w, v_h); - NODELET_DEBUG_STREAM("Image Mat size : " << mMatResolVideo.width << "x" << mMatResolVideo.height); - int d_w = static_cast(mCamWidth * mCamDepthResizeFactor); - int d_h = static_cast(mCamHeight * mCamDepthResizeFactor); - mMatResolDepth = sl::Resolution(d_w, d_h); - NODELET_DEBUG_STREAM("Depth Mat size : " << mMatResolDepth.width << "x" << mMatResolDepth.height); - - // Create and fill the camera information messages - fillCamInfo(mZed, mLeftCamInfoMsg, mRightCamInfoMsg, mLeftCamOptFrameId, mRightCamOptFrameId); - fillCamInfo(mZed, mLeftCamInfoRawMsg, mRightCamInfoRawMsg, mLeftCamOptFrameId, mRightCamOptFrameId, true); - fillCamDepthInfo(mZed, mDepthCamInfoMsg, mLeftCamOptFrameId); - - // the reference camera is the Left one (next to the ZED logo) - - mRgbCamInfoMsg = mLeftCamInfoMsg; - mRgbCamInfoRawMsg = mLeftCamInfoRawMsg; - - sl::RuntimeParameters runParams; - runParams.sensing_mode = static_cast(mCamSensingMode); - - // Main loop - while (mNhNs.ok()) - { - // Check for subscribers - uint32_t rgbSubnumber = mPubRgb.getNumSubscribers(); - uint32_t rgbRawSubnumber = mPubRawRgb.getNumSubscribers(); - uint32_t leftSubnumber = mPubLeft.getNumSubscribers(); - uint32_t leftRawSubnumber = mPubRawLeft.getNumSubscribers(); - uint32_t rightSubnumber = mPubRight.getNumSubscribers(); - uint32_t rightRawSubnumber = mPubRawRight.getNumSubscribers(); - uint32_t rgbGraySubnumber = mPubRgbGray.getNumSubscribers(); - uint32_t rgbGrayRawSubnumber = mPubRawRgbGray.getNumSubscribers(); - uint32_t leftGraySubnumber = mPubLeftGray.getNumSubscribers(); - uint32_t leftGrayRawSubnumber = mPubRawLeftGray.getNumSubscribers(); - uint32_t rightGraySubnumber = mPubRightGray.getNumSubscribers(); - uint32_t rightGrayRawSubnumber = mPubRawRightGray.getNumSubscribers(); - uint32_t depthSubnumber = mPubDepth.getNumSubscribers(); - uint32_t disparitySubnumber = mPubDisparity.getNumSubscribers(); - uint32_t cloudSubnumber = mPubCloud.getNumSubscribers(); - uint32_t fusedCloudSubnumber = mPubFusedCloud.getNumSubscribers(); - uint32_t poseSubnumber = mPubPose.getNumSubscribers(); - uint32_t poseCovSubnumber = mPubPoseCov.getNumSubscribers(); - uint32_t odomSubnumber = mPubOdom.getNumSubscribers(); - uint32_t confMapSubnumber = mPubConfMap.getNumSubscribers(); - uint32_t pathSubNumber = mPubMapPath.getNumSubscribers() + mPubOdomPath.getNumSubscribers(); - uint32_t stereoSubNumber = mPubStereo.getNumSubscribers(); - uint32_t stereoRawSubNumber = mPubRawStereo.getNumSubscribers(); + NODELET_DEBUG_STREAM("Camera Frame size : " << mCamWidth << "x" << mCamHeight); + int v_w = static_cast(mCamWidth * mCamImageResizeFactor); + int v_h = static_cast(mCamHeight * mCamImageResizeFactor); + mMatResolVideo = sl::Resolution(v_w, v_h); + NODELET_DEBUG_STREAM("Image Mat size : " << mMatResolVideo.width << "x" << mMatResolVideo.height); + int d_w = static_cast(mCamWidth * mCamDepthResizeFactor); + int d_h = static_cast(mCamHeight * mCamDepthResizeFactor); + mMatResolDepth = sl::Resolution(d_w, d_h); + NODELET_DEBUG_STREAM("Depth Mat size : " << mMatResolDepth.width << "x" << mMatResolDepth.height); + + // Create and fill the camera information messages + fillCamInfo(mZed, mLeftCamInfoMsg, mRightCamInfoMsg, mLeftCamOptFrameId, mRightCamOptFrameId); + fillCamInfo(mZed, mLeftCamInfoRawMsg, mRightCamInfoRawMsg, mLeftCamOptFrameId, mRightCamOptFrameId, true); + fillCamDepthInfo(mZed, mDepthCamInfoMsg, mLeftCamOptFrameId); + + // the reference camera is the Left one (next to the ZED logo) + + mRgbCamInfoMsg = mLeftCamInfoMsg; + mRgbCamInfoRawMsg = mLeftCamInfoRawMsg; + + sl::RuntimeParameters runParams; + runParams.sensing_mode = static_cast(mCamSensingMode); + + // Main loop + while (mNhNs.ok()) { + // Check for subscribers + uint32_t rgbSubnumber = mPubRgb.getNumSubscribers(); + uint32_t rgbRawSubnumber = mPubRawRgb.getNumSubscribers(); + uint32_t leftSubnumber = mPubLeft.getNumSubscribers(); + uint32_t leftRawSubnumber = mPubRawLeft.getNumSubscribers(); + uint32_t rightSubnumber = mPubRight.getNumSubscribers(); + uint32_t rightRawSubnumber = mPubRawRight.getNumSubscribers(); + uint32_t rgbGraySubnumber = mPubRgbGray.getNumSubscribers(); + uint32_t rgbGrayRawSubnumber = mPubRawRgbGray.getNumSubscribers(); + uint32_t leftGraySubnumber = mPubLeftGray.getNumSubscribers(); + uint32_t leftGrayRawSubnumber = mPubRawLeftGray.getNumSubscribers(); + uint32_t rightGraySubnumber = mPubRightGray.getNumSubscribers(); + uint32_t rightGrayRawSubnumber = mPubRawRightGray.getNumSubscribers(); + uint32_t depthSubnumber = mPubDepth.getNumSubscribers(); + uint32_t disparitySubnumber = mPubDisparity.getNumSubscribers(); + uint32_t cloudSubnumber = mPubCloud.getNumSubscribers(); + uint32_t fusedCloudSubnumber = mPubFusedCloud.getNumSubscribers(); + uint32_t poseSubnumber = mPubPose.getNumSubscribers(); + uint32_t poseCovSubnumber = mPubPoseCov.getNumSubscribers(); + uint32_t odomSubnumber = mPubOdom.getNumSubscribers(); + uint32_t confMapSubnumber = mPubConfMap.getNumSubscribers(); + uint32_t pathSubNumber = mPubMapPath.getNumSubscribers() + mPubOdomPath.getNumSubscribers(); + uint32_t stereoSubNumber = mPubStereo.getNumSubscribers(); + uint32_t stereoRawSubNumber = mPubRawStereo.getNumSubscribers(); + + uint32_t objDetSubnumber = 0; + if (mObjDetEnabled && mObjDetRunning) { + objDetSubnumber = mPubObjDet.getNumSubscribers(); + } + + mGrabActive = mRecording || mStreaming || mMappingEnabled || mObjDetEnabled || mPosTrackingEnabled || mPosTrackingActivated || ((rgbSubnumber + rgbRawSubnumber + leftSubnumber + leftRawSubnumber + rightSubnumber + rightRawSubnumber + rgbGraySubnumber + rgbGrayRawSubnumber + leftGraySubnumber + leftGrayRawSubnumber + rightGraySubnumber + rightGrayRawSubnumber + depthSubnumber + disparitySubnumber + cloudSubnumber + poseSubnumber + poseCovSubnumber + odomSubnumber + confMapSubnumber /*+ imuSubnumber + imuRawsubnumber*/ + pathSubNumber + stereoSubNumber + stereoRawSubNumber + objDetSubnumber) > 0); + + // Run the loop only if there is some subscribers or SVO is active + if (mGrabActive) { + std::lock_guard lock(mPosTrkMutex); + + // Note: ones tracking is started it is never stopped anymore to not lose tracking information + bool computeTracking = (mPosTrackingEnabled || mPosTrackingActivated || mMappingEnabled || mObjDetEnabled || (mComputeDepth & mDepthStabilization) || poseSubnumber > 0 || poseCovSubnumber > 0 || odomSubnumber > 0 || pathSubNumber > 0); + + // Start the tracking? + if ((computeTracking) && !mPosTrackingActivated && (mDepthMode != sl::DEPTH_MODE::NONE)) { + start_pos_tracking(); + } + + // Start the mapping? + mMappingMutex.lock(); + if (mMappingEnabled && !mMappingRunning) { + start_3d_mapping(); + } + mMappingMutex.unlock(); + + // Start the object detection? + mObjDetMutex.lock(); + if (mObjDetEnabled && !mObjDetRunning) { + start_obj_detect(); + } + mObjDetMutex.unlock(); + + // Detect if one of the subscriber need to have the depth information + mComputeDepth = mDepthMode != sl::DEPTH_MODE::NONE && (computeTracking || ((depthSubnumber + disparitySubnumber + cloudSubnumber + fusedCloudSubnumber + poseSubnumber + poseCovSubnumber + odomSubnumber + confMapSubnumber + objDetSubnumber) > 0)); - uint32_t objDetSubnumber = 0; - if (mObjDetEnabled && mObjDetRunning) - { - objDetSubnumber = mPubObjDet.getNumSubscribers(); - } - - mGrabActive = - mRecording || mStreaming || mMappingEnabled || mObjDetEnabled || mPosTrackingEnabled || mPosTrackingActivated || - ((rgbSubnumber + rgbRawSubnumber + leftSubnumber + leftRawSubnumber + rightSubnumber + rightRawSubnumber + - rgbGraySubnumber + rgbGrayRawSubnumber + leftGraySubnumber + leftGrayRawSubnumber + rightGraySubnumber + - rightGrayRawSubnumber + depthSubnumber + disparitySubnumber + cloudSubnumber + poseSubnumber + - poseCovSubnumber + odomSubnumber + confMapSubnumber /*+ imuSubnumber + imuRawsubnumber*/ + pathSubNumber + - stereoSubNumber + stereoRawSubNumber + objDetSubnumber) > 0); - - // Run the loop only if there is some subscribers or SVO is active - if (mGrabActive) - { - std::lock_guard lock(mPosTrkMutex); - - // Note: ones tracking is started it is never stopped anymore to not lose tracking information - bool computeTracking = (mPosTrackingEnabled || mPosTrackingActivated || mMappingEnabled || mObjDetEnabled || - (mComputeDepth & mDepthStabilization) || poseSubnumber > 0 || poseCovSubnumber > 0 || - odomSubnumber > 0 || pathSubNumber > 0); - - // Start the tracking? - if ((computeTracking) && !mPosTrackingActivated && (mDepthMode != sl::DEPTH_MODE::NONE)) - { - start_pos_tracking(); - } - - // Start the mapping? - mMappingMutex.lock(); - if (mMappingEnabled && !mMappingRunning) - { - start_3d_mapping(); - } - mMappingMutex.unlock(); - - // Start the object detection? - mObjDetMutex.lock(); - if (mObjDetEnabled && !mObjDetRunning) - { - start_obj_detect(); - } - mObjDetMutex.unlock(); - - // Detect if one of the subscriber need to have the depth information - mComputeDepth = mDepthMode != sl::DEPTH_MODE::NONE && - (computeTracking || - ((depthSubnumber + disparitySubnumber + cloudSubnumber + fusedCloudSubnumber + poseSubnumber + - poseCovSubnumber + odomSubnumber + confMapSubnumber + objDetSubnumber) > 0)); - - if (mComputeDepth) - { - runParams.confidence_threshold = mCamDepthConfidence; + if (mComputeDepth) { + runParams.confidence_threshold = mCamDepthConfidence; #if ZED_SDK_MAJOR_VERSION == 3 && ZED_SDK_MINOR_VERSION < 2 - runParams.textureness_confidence_threshold = mCamDepthTextureConf; + runParams.textureness_confidence_threshold = mCamDepthTextureConf; #else - runParams.texture_confidence_threshold = mCamDepthTextureConf; + runParams.texture_confidence_threshold = mCamDepthTextureConf; #endif - runParams.enable_depth = true; // Ask to compute the depth - } - else - { - runParams.enable_depth = false; // Ask to not compute the depth - } + runParams.enable_depth = true; // Ask to compute the depth + } else { + runParams.enable_depth = false; // Ask to not compute the depth + } - mCamDataMutex.lock(); - mRgbDepthDataRetrieved = false; - mGrabStatus = mZed.grab(runParams); - mCamDataMutex.unlock(); + mCamDataMutex.lock(); + mRgbDepthDataRetrieved = false; + mGrabStatus = mZed.grab(runParams); + mCamDataMutex.unlock(); - std::chrono::steady_clock::time_point start_elab = std::chrono::steady_clock::now(); + std::chrono::steady_clock::time_point start_elab = std::chrono::steady_clock::now(); - // cout << toString(grab_status) << endl; - if (mGrabStatus != sl::ERROR_CODE::SUCCESS) - { - // Detect if a error occurred (for example: - // the zed have been disconnected) and - // re-initialize the ZED + // cout << toString(grab_status) << endl; + if (mGrabStatus != sl::ERROR_CODE::SUCCESS) { + // Detect if a error occurred (for example: + // the zed have been disconnected) and + // re-initialize the ZED - NODELET_INFO_STREAM_ONCE(toString(mGrabStatus)); + NODELET_INFO_STREAM_ONCE(toString(mGrabStatus)); - std::this_thread::sleep_for(std::chrono::milliseconds(1)); + std::this_thread::sleep_for(std::chrono::milliseconds(1)); - if ((ros::Time::now() - mPrevFrameTimestamp).toSec() > 5 && !mSvoMode) - { - mCloseZedMutex.lock(); - mZed.close(); - mCloseZedMutex.unlock(); - - mConnStatus = sl::ERROR_CODE::CAMERA_NOT_DETECTED; - - while (mConnStatus != sl::ERROR_CODE::SUCCESS) - { - if (!mNhNs.ok()) - { - mStopNode = true; - - std::lock_guard lock(mCloseZedMutex); - NODELET_DEBUG("Closing ZED"); - if (mRecording) - { - mRecording = false; - mZed.disableRecording(); - } - mZed.close(); - - NODELET_DEBUG("ZED pool thread finished"); - return; - } + if ((ros::Time::now() - mPrevFrameTimestamp).toSec() > 5 && !mSvoMode) { + mCloseZedMutex.lock(); + mZed.close(); + mCloseZedMutex.unlock(); - int id = sl_tools::checkCameraReady(mZedSerialNumber); + mConnStatus = sl::ERROR_CODE::CAMERA_NOT_DETECTED; - if (id >= 0) - { - mZedParams.input.setFromCameraID(id); - mConnStatus = mZed.open(mZedParams); // Try to initialize the ZED - NODELET_INFO_STREAM(toString(mConnStatus)); - } - else - { - NODELET_INFO_STREAM("Waiting for the ZED (S/N " << mZedSerialNumber << ") to be re-connected"); - } + while (mConnStatus != sl::ERROR_CODE::SUCCESS) { + if (!mNhNs.ok()) { + mStopNode = true; - mDiagUpdater.force_update(); - std::this_thread::sleep_for(std::chrono::milliseconds(2000)); - } + std::lock_guard lock(mCloseZedMutex); + NODELET_DEBUG("Closing ZED"); + if (mRecording) { + mRecording = false; + mZed.disableRecording(); + } + mZed.close(); - mPosTrackingActivated = false; + NODELET_DEBUG("ZED pool thread finished"); + return; + } - computeTracking = mPosTrackingEnabled || mDepthStabilization || poseSubnumber > 0 || poseCovSubnumber > 0 || - odomSubnumber > 0; + int id = sl_tools::checkCameraReady(mZedSerialNumber); - if (computeTracking) - { // Start the tracking - start_pos_tracking(); - } - } + if (id >= 0) { + mZedParams.input.setFromCameraID(id); + mConnStatus = mZed.open(mZedParams); // Try to initialize the ZED + NODELET_INFO_STREAM(toString(mConnStatus)); + } else { + NODELET_INFO_STREAM("Waiting for the ZED (S/N " << mZedSerialNumber << ") to be re-connected"); + } - mDiagUpdater.update(); + mDiagUpdater.force_update(); + std::this_thread::sleep_for(std::chrono::milliseconds(2000)); + } - continue; - } + mPosTrackingActivated = false; - mFrameCount++; + computeTracking = mPosTrackingEnabled || mDepthStabilization || poseSubnumber > 0 || poseCovSubnumber > 0 || odomSubnumber > 0; - // SVO recording - mRecMutex.lock(); + if (computeTracking) { // Start the tracking + start_pos_tracking(); + } + } - if (mRecording) - { - mRecStatus = mZed.getRecordingStatus(); + mDiagUpdater.update(); - if (!mRecStatus.status) - { - NODELET_ERROR_THROTTLE(1.0, "Error saving frame to SVO"); - } + continue; + } - mDiagUpdater.force_update(); - } + mFrameCount++; - mRecMutex.unlock(); + // SVO recording + mRecMutex.lock(); - // Timestamp - mPrevFrameTimestamp = mFrameTimestamp; + if (mRecording) { + mRecStatus = mZed.getRecordingStatus(); - // Publish freq calculation - static std::chrono::steady_clock::time_point last_time = std::chrono::steady_clock::now(); - std::chrono::steady_clock::time_point now = std::chrono::steady_clock::now(); + if (!mRecStatus.status) { + NODELET_ERROR_THROTTLE(1.0, "Error saving frame to SVO"); + } - double elapsed_usec = std::chrono::duration_cast(now - last_time).count(); - last_time = now; + mDiagUpdater.force_update(); + } - mGrabPeriodMean_usec->addValue(elapsed_usec); + mRecMutex.unlock(); - // NODELET_INFO_STREAM("Grab time: " << elapsed_usec / 1000 << " msec"); + // Timestamp + mPrevFrameTimestamp = mFrameTimestamp; - // Timestamp - if (mSvoMode) - { - mFrameTimestamp = ros::Time::now(); - } - else - { - mFrameTimestamp = sl_tools::slTime2Ros(mZed.getTimestamp(sl::TIME_REFERENCE::IMAGE)); - } - - ros::Time stamp = mFrameTimestamp; // Timestamp - - // ----> Camera Settings - if (!mSvoMode && mFrameCount % 5 == 0) - { - // NODELET_DEBUG_STREAM( "[" << mFrameCount << "] device_poll_thread_func MUTEX LOCK"); - mDynParMutex.lock(); - - int brightness = mZed.getCameraSettings(sl::VIDEO_SETTINGS::BRIGHTNESS); - if (brightness != mCamBrightness) - { - mZed.setCameraSettings(sl::VIDEO_SETTINGS::BRIGHTNESS, mCamBrightness); - NODELET_DEBUG_STREAM("mCamBrightness changed: " << mCamBrightness << " <- " << brightness); - mUpdateDynParams = true; - } + // Publish freq calculation + static std::chrono::steady_clock::time_point last_time = std::chrono::steady_clock::now(); + std::chrono::steady_clock::time_point now = std::chrono::steady_clock::now(); - int contrast = mZed.getCameraSettings(sl::VIDEO_SETTINGS::CONTRAST); - if (contrast != mCamContrast) - { - mZed.setCameraSettings(sl::VIDEO_SETTINGS::CONTRAST, mCamContrast); - NODELET_DEBUG_STREAM("mCamContrast changed: " << mCamContrast << " <- " << contrast); - mUpdateDynParams = true; - } + double elapsed_usec = std::chrono::duration_cast(now - last_time).count(); + last_time = now; - int hue = mZed.getCameraSettings(sl::VIDEO_SETTINGS::HUE); - if (hue != mCamHue) - { - mZed.setCameraSettings(sl::VIDEO_SETTINGS::HUE, mCamHue); - NODELET_DEBUG_STREAM("mCamHue changed: " << mCamHue << " <- " << hue); - mUpdateDynParams = true; - } + mGrabPeriodMean_usec->addValue(elapsed_usec); - int saturation = mZed.getCameraSettings(sl::VIDEO_SETTINGS::SATURATION); - if (saturation != mCamSaturation) - { - mZed.setCameraSettings(sl::VIDEO_SETTINGS::SATURATION, mCamSaturation); - NODELET_DEBUG_STREAM("mCamSaturation changed: " << mCamSaturation << " <- " << saturation); - mUpdateDynParams = true; - } + // NODELET_INFO_STREAM("Grab time: " << elapsed_usec / 1000 << " msec"); - int sharpness = mZed.getCameraSettings(sl::VIDEO_SETTINGS::SHARPNESS); - if (sharpness != mCamSharpness) - { - mZed.setCameraSettings(sl::VIDEO_SETTINGS::SHARPNESS, mCamSharpness); - NODELET_DEBUG_STREAM("mCamSharpness changed: " << mCamSharpness << " <- " << sharpness); - mUpdateDynParams = true; - } + // Timestamp + if (mSvoMode) { + mFrameTimestamp = ros::Time::now(); + } else { + mFrameTimestamp = sl_tools::slTime2Ros(mZed.getTimestamp(sl::TIME_REFERENCE::IMAGE)); + } + + ros::Time stamp = mFrameTimestamp; // Timestamp + + // ----> Camera Settings + if (!mSvoMode && mFrameCount % 5 == 0) { + // NODELET_DEBUG_STREAM( "[" << mFrameCount << "] device_poll_thread_func MUTEX LOCK"); + mDynParMutex.lock(); + + int brightness = mZed.getCameraSettings(sl::VIDEO_SETTINGS::BRIGHTNESS); + if (brightness != mCamBrightness) { + mZed.setCameraSettings(sl::VIDEO_SETTINGS::BRIGHTNESS, mCamBrightness); + NODELET_DEBUG_STREAM("mCamBrightness changed: " << mCamBrightness << " <- " << brightness); + mUpdateDynParams = true; + } + + int contrast = mZed.getCameraSettings(sl::VIDEO_SETTINGS::CONTRAST); + if (contrast != mCamContrast) { + mZed.setCameraSettings(sl::VIDEO_SETTINGS::CONTRAST, mCamContrast); + NODELET_DEBUG_STREAM("mCamContrast changed: " << mCamContrast << " <- " << contrast); + mUpdateDynParams = true; + } + + int hue = mZed.getCameraSettings(sl::VIDEO_SETTINGS::HUE); + if (hue != mCamHue) { + mZed.setCameraSettings(sl::VIDEO_SETTINGS::HUE, mCamHue); + NODELET_DEBUG_STREAM("mCamHue changed: " << mCamHue << " <- " << hue); + mUpdateDynParams = true; + } + + int saturation = mZed.getCameraSettings(sl::VIDEO_SETTINGS::SATURATION); + if (saturation != mCamSaturation) { + mZed.setCameraSettings(sl::VIDEO_SETTINGS::SATURATION, mCamSaturation); + NODELET_DEBUG_STREAM("mCamSaturation changed: " << mCamSaturation << " <- " << saturation); + mUpdateDynParams = true; + } + + int sharpness = mZed.getCameraSettings(sl::VIDEO_SETTINGS::SHARPNESS); + if (sharpness != mCamSharpness) { + mZed.setCameraSettings(sl::VIDEO_SETTINGS::SHARPNESS, mCamSharpness); + NODELET_DEBUG_STREAM("mCamSharpness changed: " << mCamSharpness << " <- " << sharpness); + mUpdateDynParams = true; + } #if (ZED_SDK_MAJOR_VERSION == 3 && ZED_SDK_MINOR_VERSION >= 1) - int gamma = mZed.getCameraSettings(sl::VIDEO_SETTINGS::GAMMA); - if (gamma != mCamGamma) - { - mZed.setCameraSettings(sl::VIDEO_SETTINGS::GAMMA, mCamGamma); - NODELET_DEBUG_STREAM("mCamGamma changed: " << mCamGamma << " <- " << gamma); - mUpdateDynParams = true; - } + int gamma = mZed.getCameraSettings(sl::VIDEO_SETTINGS::GAMMA); + if (gamma != mCamGamma) { + mZed.setCameraSettings(sl::VIDEO_SETTINGS::GAMMA, mCamGamma); + NODELET_DEBUG_STREAM("mCamGamma changed: " << mCamGamma << " <- " << gamma); + mUpdateDynParams = true; + } #endif - if (mCamAutoExposure) - { - if (mTriggerAutoExposure) - { - mZed.setCameraSettings(sl::VIDEO_SETTINGS::AEC_AGC, 1); - mTriggerAutoExposure = false; - } - } - else - { - int exposure = mZed.getCameraSettings(sl::VIDEO_SETTINGS::EXPOSURE); - if (exposure != mCamExposure) - { - mZed.setCameraSettings(sl::VIDEO_SETTINGS::EXPOSURE, mCamExposure); - NODELET_DEBUG_STREAM("mCamExposure changed: " << mCamExposure << " <- " << exposure); - mUpdateDynParams = true; - } + if (mCamAutoExposure) { + if (mTriggerAutoExposure) { + mZed.setCameraSettings(sl::VIDEO_SETTINGS::AEC_AGC, 1); + mTriggerAutoExposure = false; + } + } else { + int exposure = mZed.getCameraSettings(sl::VIDEO_SETTINGS::EXPOSURE); + if (exposure != mCamExposure) { + mZed.setCameraSettings(sl::VIDEO_SETTINGS::EXPOSURE, mCamExposure); + NODELET_DEBUG_STREAM("mCamExposure changed: " << mCamExposure << " <- " << exposure); + mUpdateDynParams = true; + } + + int gain = mZed.getCameraSettings(sl::VIDEO_SETTINGS::GAIN); + if (gain != mCamGain) { + mZed.setCameraSettings(sl::VIDEO_SETTINGS::GAIN, mCamGain); + NODELET_DEBUG_STREAM("mCamGain changed: " << mCamGain << " <- " << gain); + mUpdateDynParams = true; + } + } + + if (mCamAutoWB) { + if (mTriggerAutoWB) { + mZed.setCameraSettings(sl::VIDEO_SETTINGS::WHITEBALANCE_AUTO, 1); + mTriggerAutoWB = false; + } + } else { + int wb = mZed.getCameraSettings(sl::VIDEO_SETTINGS::WHITEBALANCE_TEMPERATURE); + if (wb != mCamWB) { + mZed.setCameraSettings(sl::VIDEO_SETTINGS::WHITEBALANCE_TEMPERATURE, mCamWB); + NODELET_DEBUG_STREAM("mCamWB changed: " << mCamWB << " <- " << wb); + mUpdateDynParams = true; + } + } + mDynParMutex.unlock(); + // NODELET_DEBUG_STREAM( "device_poll_thread_func MUTEX UNLOCK"); + } - int gain = mZed.getCameraSettings(sl::VIDEO_SETTINGS::GAIN); - if (gain != mCamGain) - { - mZed.setCameraSettings(sl::VIDEO_SETTINGS::GAIN, mCamGain); - NODELET_DEBUG_STREAM("mCamGain changed: " << mCamGain << " <- " << gain); - mUpdateDynParams = true; - } - } + if (mUpdateDynParams) { + NODELET_DEBUG("Update Dynamic Parameters"); + updateDynamicReconfigure(); + } + // <---- Camera Settings + + // Publish the point cloud if someone has subscribed to + if (cloudSubnumber > 0) { + // Run the point cloud conversion asynchronously to avoid slowing down + // all the program + // Retrieve raw pointCloud data if latest Pointcloud is ready + std::unique_lock lock(mPcMutex, std::defer_lock); + + if (lock.try_lock()) { + mZed.retrieveMeasure(mCloud, sl::MEASURE::XYZBGRA, sl::MEM::CPU, mMatResolDepth); + + mPointCloudFrameId = mDepthFrameId; + mPointCloudTime = stamp; + + // Signal Pointcloud thread that a new pointcloud is ready + mPcDataReadyCondVar.notify_one(); + mPcDataReady = true; + mPcPublishing = true; + } + } else { + mPcPublishing = false; + } - if (mCamAutoWB) - { - if (mTriggerAutoWB) - { - mZed.setCameraSettings(sl::VIDEO_SETTINGS::WHITEBALANCE_AUTO, 1); - mTriggerAutoWB = false; - } - } - else - { - int wb = mZed.getCameraSettings(sl::VIDEO_SETTINGS::WHITEBALANCE_TEMPERATURE); - if (wb != mCamWB) - { - mZed.setCameraSettings(sl::VIDEO_SETTINGS::WHITEBALANCE_TEMPERATURE, mCamWB); - NODELET_DEBUG_STREAM("mCamWB changed: " << mCamWB << " <- " << wb); - mUpdateDynParams = true; - } - } - mDynParMutex.unlock(); - // NODELET_DEBUG_STREAM( "device_poll_thread_func MUTEX UNLOCK"); - } - - if (mUpdateDynParams) - { - NODELET_DEBUG("Update Dynamic Parameters"); - updateDynamicReconfigure(); - } - // <---- Camera Settings - - // Publish the point cloud if someone has subscribed to - if (cloudSubnumber > 0) - { - // Run the point cloud conversion asynchronously to avoid slowing down - // all the program - // Retrieve raw pointCloud data if latest Pointcloud is ready - std::unique_lock lock(mPcMutex, std::defer_lock); - - if (lock.try_lock()) - { - mZed.retrieveMeasure(mCloud, sl::MEASURE::XYZBGRA, sl::MEM::CPU, mMatResolDepth); - - mPointCloudFrameId = mDepthFrameId; - mPointCloudTime = stamp; - - // Signal Pointcloud thread that a new pointcloud is ready - mPcDataReadyCondVar.notify_one(); - mPcDataReady = true; - mPcPublishing = true; - } - } - else - { - mPcPublishing = false; - } - - mObjDetMutex.lock(); - if (mObjDetRunning && objDetSubnumber > 0) - { - processDetectedObjects(stamp); - } - mObjDetMutex.unlock(); - - // Publish the odometry if someone has subscribed to - if (computeTracking) - { - if (!mSensor2BaseTransfValid) - { - getSens2BaseTransform(); - } + mObjDetMutex.lock(); + if (mObjDetRunning && objDetSubnumber > 0) { + processDetectedObjects(stamp); + } + mObjDetMutex.unlock(); - if (!mSensor2CameraTransfValid) - { - getSens2CameraTransform(); - } + // Publish the odometry if someone has subscribed to + if (computeTracking) { + if (!mSensor2BaseTransfValid) { + getSens2BaseTransform(); + } - if (!mCamera2BaseTransfValid) - { - getCamera2BaseTransform(); - } + if (!mSensor2CameraTransfValid) { + getSens2CameraTransform(); + } - if (!mInitOdomWithPose) - { - sl::Pose deltaOdom; - mPosTrackingStatus = mZed.getPosition(deltaOdom, sl::REFERENCE_FRAME::CAMERA); + if (!mCamera2BaseTransfValid) { + getCamera2BaseTransform(); + } - sl::Translation translation = deltaOdom.getTranslation(); - sl::Orientation quat = deltaOdom.getOrientation(); + if (!mInitOdomWithPose) { + sl::Pose deltaOdom; + mPosTrackingStatus = mZed.getPosition(deltaOdom, sl::REFERENCE_FRAME::CAMERA); + + sl::Translation translation = deltaOdom.getTranslation(); + sl::Orientation quat = deltaOdom.getOrientation(); #if 0 NODELET_DEBUG("delta ODOM [%s] - %.2f,%.2f,%.2f %.2f,%.2f,%.2f,%.2f", @@ -3946,43 +3507,39 @@ void ZEDWrapperNodelet::device_poll_thread_func() NODELET_DEBUG_STREAM("ODOM -> Tracking Status: " << sl::toString(mTrackingStatus)); #endif - if (mPosTrackingStatus == sl::POSITIONAL_TRACKING_STATE::OK || - mPosTrackingStatus == sl::POSITIONAL_TRACKING_STATE::SEARCHING || - mPosTrackingStatus == sl::POSITIONAL_TRACKING_STATE::FPS_TOO_LOW) - { - // Transform ZED delta odom pose in TF2 Transformation - geometry_msgs::Transform deltaTransf; - deltaTransf.translation.x = translation(0); - deltaTransf.translation.y = translation(1); - deltaTransf.translation.z = translation(2); - deltaTransf.rotation.x = quat(0); - deltaTransf.rotation.y = quat(1); - deltaTransf.rotation.z = quat(2); - deltaTransf.rotation.w = quat(3); - tf2::Transform deltaOdomTf; - tf2::fromMsg(deltaTransf, deltaOdomTf); - // delta odom from sensor to base frame - tf2::Transform deltaOdomTf_base = mSensor2BaseTransf.inverse() * deltaOdomTf * mSensor2BaseTransf; - - // Propagate Odom transform in time - mOdom2BaseTransf = mOdom2BaseTransf * deltaOdomTf_base; - - if (mTwoDMode) - { - tf2::Vector3 tr_2d = mOdom2BaseTransf.getOrigin(); - tr_2d.setZ(mFixedZValue); - mOdom2BaseTransf.setOrigin(tr_2d); - - double roll, pitch, yaw; - tf2::Matrix3x3(mOdom2BaseTransf.getRotation()).getRPY(roll, pitch, yaw); - - tf2::Quaternion quat_2d; - quat_2d.setRPY(0.0, 0.0, yaw); - - mOdom2BaseTransf.setRotation(quat_2d); - } - -#if 0 //#ifndef NDEBUG // Enable for TF checking + if (mPosTrackingStatus == sl::POSITIONAL_TRACKING_STATE::OK || mPosTrackingStatus == sl::POSITIONAL_TRACKING_STATE::SEARCHING || mPosTrackingStatus == sl::POSITIONAL_TRACKING_STATE::FPS_TOO_LOW) { + // Transform ZED delta odom pose in TF2 Transformation + geometry_msgs::Transform deltaTransf; + deltaTransf.translation.x = translation(0); + deltaTransf.translation.y = translation(1); + deltaTransf.translation.z = translation(2); + deltaTransf.rotation.x = quat(0); + deltaTransf.rotation.y = quat(1); + deltaTransf.rotation.z = quat(2); + deltaTransf.rotation.w = quat(3); + tf2::Transform deltaOdomTf; + tf2::fromMsg(deltaTransf, deltaOdomTf); + // delta odom from sensor to base frame + tf2::Transform deltaOdomTf_base = mSensor2BaseTransf.inverse() * deltaOdomTf * mSensor2BaseTransf; + + // Propagate Odom transform in time + mOdom2BaseTransf = mOdom2BaseTransf * deltaOdomTf_base; + + if (mTwoDMode) { + tf2::Vector3 tr_2d = mOdom2BaseTransf.getOrigin(); + tr_2d.setZ(mFixedZValue); + mOdom2BaseTransf.setOrigin(tr_2d); + + double roll, pitch, yaw; + tf2::Matrix3x3(mOdom2BaseTransf.getRotation()).getRPY(roll, pitch, yaw); + + tf2::Quaternion quat_2d; + quat_2d.setRPY(0.0, 0.0, yaw); + + mOdom2BaseTransf.setRotation(quat_2d); + } + +#if 0 //#ifndef NDEBUG // Enable for TF checking double roll, pitch, yaw; tf2::Matrix3x3(mOdom2BaseTransf.getRotation()).getRPY(roll, pitch, yaw); @@ -3992,31 +3549,27 @@ void ZEDWrapperNodelet::device_poll_thread_func() roll * RAD2DEG, pitch * RAD2DEG, yaw * RAD2DEG); #endif - // Publish odometry message - if (odomSubnumber > 0) - { - publishOdom(mOdom2BaseTransf, deltaOdom, stamp); + // Publish odometry message + if (odomSubnumber > 0) { + publishOdom(mOdom2BaseTransf, deltaOdom, stamp); + } + + mPosTrackingReady = true; + } + } else if (mFloorAlignment) { + NODELET_WARN_THROTTLE(5.0, "Odometry will be published as soon as the floor as been detected for the first " + "time"); + } } - mPosTrackingReady = true; - } - } - else if (mFloorAlignment) - { - NODELET_WARN_THROTTLE(5.0, "Odometry will be published as soon as the floor as been detected for the first " - "time"); - } - } - - // Publish the zed camera pose if someone has subscribed to - if (computeTracking) - { - mPosTrackingStatus = mZed.getPosition(mLastZedPose, sl::REFERENCE_FRAME::WORLD); + // Publish the zed camera pose if someone has subscribed to + if (computeTracking) { + mPosTrackingStatus = mZed.getPosition(mLastZedPose, sl::REFERENCE_FRAME::WORLD); - sl::Translation translation = mLastZedPose.getTranslation(); - sl::Orientation quat = mLastZedPose.getOrientation(); + sl::Translation translation = mLastZedPose.getTranslation(); + sl::Orientation quat = mLastZedPose.getOrientation(); -#if 0 //#ifndef NDEBUG // Enable for TF checking +#if 0 //#ifndef NDEBUG // Enable for TF checking double roll, pitch, yaw; tf2::Matrix3x3(tf2::Quaternion(quat.ox, quat.oy, quat.oz, quat.ow)).getRPY(roll, pitch, yaw); @@ -4029,54 +3582,48 @@ void ZEDWrapperNodelet::device_poll_thread_func() #endif - static sl::POSITIONAL_TRACKING_STATE old_tracking_state = sl::POSITIONAL_TRACKING_STATE::OFF; - if (mPosTrackingStatus == sl::POSITIONAL_TRACKING_STATE::OK || - mPosTrackingStatus == sl::POSITIONAL_TRACKING_STATE::SEARCHING - /*|| status == sl::POSITIONAL_TRACKING_STATE::FPS_TOO_LOW*/) - { - if (mPosTrackingStatus == sl::POSITIONAL_TRACKING_STATE::OK && mPosTrackingStatus != old_tracking_state) - { - NODELET_INFO_STREAM("Positional tracking -> OK [" << sl::toString(mPosTrackingStatus).c_str() << "]"); - old_tracking_state = mPosTrackingStatus; - } - if (mPosTrackingStatus == sl::POSITIONAL_TRACKING_STATE::SEARCHING && - mPosTrackingStatus != old_tracking_state) - { - NODELET_INFO_STREAM("Positional tracking -> Searching for a known position [" - << sl::toString(mPosTrackingStatus).c_str() << "]"); - old_tracking_state = mPosTrackingStatus; - } - // Transform ZED pose in TF2 Transformation - geometry_msgs::Transform map2sensTransf; - - map2sensTransf.translation.x = translation(0); - map2sensTransf.translation.y = translation(1); - map2sensTransf.translation.z = translation(2); - map2sensTransf.rotation.x = quat(0); - map2sensTransf.rotation.y = quat(1); - map2sensTransf.rotation.z = quat(2); - map2sensTransf.rotation.w = quat(3); - tf2::Transform map_to_sens_transf; - tf2::fromMsg(map2sensTransf, map_to_sens_transf); - - mMap2BaseTransf = map_to_sens_transf * mSensor2BaseTransf; // Base position in map frame - - if (mTwoDMode) - { - tf2::Vector3 tr_2d = mMap2BaseTransf.getOrigin(); - tr_2d.setZ(mFixedZValue); - mMap2BaseTransf.setOrigin(tr_2d); + static sl::POSITIONAL_TRACKING_STATE old_tracking_state = sl::POSITIONAL_TRACKING_STATE::OFF; + if (mPosTrackingStatus == sl::POSITIONAL_TRACKING_STATE::OK || mPosTrackingStatus == sl::POSITIONAL_TRACKING_STATE::SEARCHING + /*|| status == sl::POSITIONAL_TRACKING_STATE::FPS_TOO_LOW*/) { + if (mPosTrackingStatus == sl::POSITIONAL_TRACKING_STATE::OK && mPosTrackingStatus != old_tracking_state) { + NODELET_INFO_STREAM("Positional tracking -> OK [" << sl::toString(mPosTrackingStatus).c_str() << "]"); + old_tracking_state = mPosTrackingStatus; + } + if (mPosTrackingStatus == sl::POSITIONAL_TRACKING_STATE::SEARCHING && mPosTrackingStatus != old_tracking_state) { + NODELET_INFO_STREAM("Positional tracking -> Searching for a known position [" + << sl::toString(mPosTrackingStatus).c_str() << "]"); + old_tracking_state = mPosTrackingStatus; + } + // Transform ZED pose in TF2 Transformation + geometry_msgs::Transform map2sensTransf; + + map2sensTransf.translation.x = translation(0); + map2sensTransf.translation.y = translation(1); + map2sensTransf.translation.z = translation(2); + map2sensTransf.rotation.x = quat(0); + map2sensTransf.rotation.y = quat(1); + map2sensTransf.rotation.z = quat(2); + map2sensTransf.rotation.w = quat(3); + tf2::Transform map_to_sens_transf; + tf2::fromMsg(map2sensTransf, map_to_sens_transf); + + mMap2BaseTransf = map_to_sens_transf * mSensor2BaseTransf; // Base position in map frame + + if (mTwoDMode) { + tf2::Vector3 tr_2d = mMap2BaseTransf.getOrigin(); + tr_2d.setZ(mFixedZValue); + mMap2BaseTransf.setOrigin(tr_2d); - double roll, pitch, yaw; - tf2::Matrix3x3(mMap2BaseTransf.getRotation()).getRPY(roll, pitch, yaw); + double roll, pitch, yaw; + tf2::Matrix3x3(mMap2BaseTransf.getRotation()).getRPY(roll, pitch, yaw); - tf2::Quaternion quat_2d; - quat_2d.setRPY(0.0, 0.0, yaw); + tf2::Quaternion quat_2d; + quat_2d.setRPY(0.0, 0.0, yaw); - mMap2BaseTransf.setRotation(quat_2d); - } + mMap2BaseTransf.setRotation(quat_2d); + } -#if 0 //#ifndef NDEBUG // Enable for TF checking +#if 0 //#ifndef NDEBUG // Enable for TF checking double roll, pitch, yaw; tf2::Matrix3x3(mMap2BaseTransf.getRotation()).getRPY(roll, pitch, yaw); @@ -4085,41 +3632,34 @@ void ZEDWrapperNodelet::device_poll_thread_func() mMap2BaseTransf.getOrigin().z(), roll * RAD2DEG, pitch * RAD2DEG, yaw * RAD2DEG); #endif - bool initOdom = false; - - if (!(mFloorAlignment)) - { - initOdom = mInitOdomWithPose; - } - else - { - initOdom = (mPosTrackingStatus == sl::POSITIONAL_TRACKING_STATE::OK) & mInitOdomWithPose; - } - - if (initOdom || mResetOdom) - { - NODELET_INFO("Odometry aligned to last tracking pose"); - - // Propagate Odom transform in time - mOdom2BaseTransf = mMap2BaseTransf; - mMap2BaseTransf.setIdentity(); - - if (odomSubnumber > 0) - { - // Publish odometry message - publishOdom(mOdom2BaseTransf, mLastZedPose, mFrameTimestamp); - } + bool initOdom = false; + + if (!(mFloorAlignment)) { + initOdom = mInitOdomWithPose; + } else { + initOdom = (mPosTrackingStatus == sl::POSITIONAL_TRACKING_STATE::OK) & mInitOdomWithPose; + } - mInitOdomWithPose = false; - mResetOdom = false; - } - else - { - // Transformation from map to odometry frame - // mMap2OdomTransf = mOdom2BaseTransf.inverse() * mMap2BaseTransf; - mMap2OdomTransf = mMap2BaseTransf * mOdom2BaseTransf.inverse(); + if (initOdom || mResetOdom) { + NODELET_INFO("Odometry aligned to last tracking pose"); -#if 0 //#ifndef NDEBUG // Enable for TF checking + // Propagate Odom transform in time + mOdom2BaseTransf = mMap2BaseTransf; + mMap2BaseTransf.setIdentity(); + + if (odomSubnumber > 0) { + // Publish odometry message + publishOdom(mOdom2BaseTransf, mLastZedPose, mFrameTimestamp); + } + + mInitOdomWithPose = false; + mResetOdom = false; + } else { + // Transformation from map to odometry frame + // mMap2OdomTransf = mOdom2BaseTransf.inverse() * mMap2BaseTransf; + mMap2OdomTransf = mMap2BaseTransf * mOdom2BaseTransf.inverse(); + +#if 0 //#ifndef NDEBUG // Enable for TF checking double roll, pitch, yaw; tf2::Matrix3x3(mMap2OdomTransf.getRotation()).getRPY(roll, pitch, yaw); @@ -4128,38 +3668,34 @@ void ZEDWrapperNodelet::device_poll_thread_func() mMap2OdomTransf.getOrigin().x(), mMap2OdomTransf.getOrigin().y(), mMap2OdomTransf.getOrigin().z(), roll * RAD2DEG, pitch * RAD2DEG, yaw * RAD2DEG); #endif - } + } - // Publish Pose message - if ((poseSubnumber + poseCovSubnumber) > 0) - { - publishPose(stamp); - } + // Publish Pose message + if ((poseSubnumber + poseCovSubnumber) > 0) { + publishPose(stamp); + } - mPosTrackingReady = true; - } - } - - if (mZedRealCamModel == sl::MODEL::ZED) - { - // Publish pose tf only if enabled - if (mPublishTf) - { - // Note, the frame is published, but its values will only change if - // someone has subscribed to odom - publishOdomFrame(mOdom2BaseTransf, stamp); // publish the base Frame in odometry frame - - if (mPublishMapTf) - { - // Note, the frame is published, but its values will only change if - // someone has subscribed to map - publishPoseFrame(mMap2OdomTransf, stamp); // publish the odometry Frame in map frame - } - } - } + mPosTrackingReady = true; + } + } + + if (mZedRealCamModel == sl::MODEL::ZED) { + // Publish pose tf only if enabled + if (mPublishTf) { + // Note, the frame is published, but its values will only change if + // someone has subscribed to odom + publishOdomFrame(mOdom2BaseTransf, stamp); // publish the base Frame in odometry frame + + if (mPublishMapTf) { + // Note, the frame is published, but its values will only change if + // someone has subscribed to map + publishPoseFrame(mMap2OdomTransf, stamp); // publish the odometry Frame in map frame + } + } + } -#if 0 //#ifndef NDEBUG // Enable for TF checking - // Double check: map_to_pose must be equal to mMap2BaseTransf +#if 0 //#ifndef NDEBUG // Enable for TF checking \ + // Double check: map_to_pose must be equal to mMap2BaseTransf tf2::Transform map_to_base; @@ -4185,748 +3721,645 @@ void ZEDWrapperNodelet::device_poll_thread_func() NODELET_DEBUG("*******************************"); #endif - std::chrono::steady_clock::time_point end_elab = std::chrono::steady_clock::now(); - - 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; - - if (!loop_rate.sleep()) - { - if (mean_elab_sec > (1. / mCamFrameRate)) - { - if (++count_warn > 10) - { - NODELET_DEBUG_THROTTLE(1.0, "Working thread is not synchronized with the Camera frame 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 (" << mean_elab_sec - << " sec) than requested " - "by the FPS rate (" - << loop_rate.expectedCycleTime() - << " sec). Please consider to " - "lower the 'frame_rate' setting or to " - "reduce the power requirements reducing " - "the resolutions."); - } - - loop_rate.reset(); - } - else - { - count_warn = 0; - } - } - } - else - { - NODELET_DEBUG_THROTTLE(5.0, "No topics subscribed by users"); - - if (mZedRealCamModel == sl::MODEL::ZED || !mPublishImuTf) - { - // Publish odometry tf only if enabled - if (mPublishTf) - { - ros::Time t; - - if (mSvoMode) - { - t = ros::Time::now(); - } - else - { - t = sl_tools::slTime2Ros(mZed.getTimestamp(sl::TIME_REFERENCE::CURRENT)); - } - - publishOdomFrame(mOdom2BaseTransf, mFrameTimestamp); // publish the base Frame in odometry frame - - if (mPublishMapTf) - { - publishPoseFrame(mMap2OdomTransf, mFrameTimestamp); // publish the odometry Frame in map frame - } - - if (mPublishImuTf && !mStaticImuFramePublished) - { - publishStaticImuFrame(); - } - } - } + std::chrono::steady_clock::time_point end_elab = std::chrono::steady_clock::now(); + + 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; + + if (!loop_rate.sleep()) { + if (mean_elab_sec > (1. / mCamFrameRate)) { + if (++count_warn > 10) { + NODELET_DEBUG_THROTTLE(1.0, "Working thread is not synchronized with the Camera frame 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 (" << mean_elab_sec << " sec) than requested " + "by the FPS rate (" + << loop_rate.expectedCycleTime() << " sec). Please consider to " + "lower the 'frame_rate' setting or to " + "reduce the power requirements reducing " + "the resolutions."); + } + + loop_rate.reset(); + } else { + count_warn = 0; + } + } + } else { + NODELET_DEBUG_THROTTLE(5.0, "No topics subscribed by users"); + + if (mZedRealCamModel == sl::MODEL::ZED || !mPublishImuTf) { + // Publish odometry tf only if enabled + if (mPublishTf) { + ros::Time t; + + if (mSvoMode) { + t = ros::Time::now(); + } else { + t = sl_tools::slTime2Ros(mZed.getTimestamp(sl::TIME_REFERENCE::CURRENT)); + } + + publishOdomFrame(mOdom2BaseTransf, mFrameTimestamp); // publish the base Frame in odometry frame + + if (mPublishMapTf) { + publishPoseFrame(mMap2OdomTransf, mFrameTimestamp); // publish the odometry Frame in map frame + } + + if (mPublishImuTf && !mStaticImuFramePublished) { + publishStaticImuFrame(); + } + } + } - std::this_thread::sleep_for(std::chrono::milliseconds(10)); // No subscribers, we just wait - loop_rate.reset(); - } + std::this_thread::sleep_for(std::chrono::milliseconds(10)); // No subscribers, we just wait + loop_rate.reset(); + } - mDiagUpdater.update(); - } // while loop + mDiagUpdater.update(); + } // while loop - if (mSaveAreaMapOnClosing && mPosTrackingActivated) - { - saveAreaMap(mAreaMemDbPath); - } + if (mSaveAreaMapOnClosing && mPosTrackingActivated) { + saveAreaMap(mAreaMemDbPath); + } - mStopNode = true; // Stops other threads + mStopNode = true; // Stops other threads - std::lock_guard lock(mCloseZedMutex); - NODELET_DEBUG("Closing ZED"); + std::lock_guard lock(mCloseZedMutex); + NODELET_DEBUG("Closing ZED"); - if (mRecording) - { - mRecording = false; - mZed.disableRecording(); - } - mZed.close(); + if (mRecording) { + mRecording = false; + mZed.disableRecording(); + } + mZed.close(); - NODELET_DEBUG("ZED pool thread finished"); + NODELET_DEBUG("ZED pool thread finished"); } void ZEDWrapperNodelet::callback_updateDiagnostic(diagnostic_updater::DiagnosticStatusWrapper& stat) { - if (mConnStatus != sl::ERROR_CODE::SUCCESS) - { - stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, sl::toString(mConnStatus).c_str()); - return; - } - - if (mGrabActive) - { - if (mGrabStatus == sl::ERROR_CODE::SUCCESS /*|| mGrabStatus == sl::ERROR_CODE::NOT_A_NEW_FRAME*/) - { - stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Camera grabbing"); - - double freq = 1000000. / mGrabPeriodMean_usec->getMean(); - double freq_perc = 100. * freq / mCamFrameRate; - stat.addf("Capture", "Mean Frequency: %.1f Hz (%.1f%%)", freq, freq_perc); - - stat.addf("General Processing", "Mean Time: %.3f sec (Max. %.3f sec)", mElabPeriodMean_sec->getMean(), + if (mConnStatus != sl::ERROR_CODE::SUCCESS) { + stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, sl::toString(mConnStatus).c_str()); + return; + } + + if (mGrabActive) { + if (mGrabStatus == sl::ERROR_CODE::SUCCESS /*|| mGrabStatus == sl::ERROR_CODE::NOT_A_NEW_FRAME*/) { + stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Camera grabbing"); + + double freq = 1000000. / mGrabPeriodMean_usec->getMean(); + double freq_perc = 100. * freq / mCamFrameRate; + stat.addf("Capture", "Mean Frequency: %.1f Hz (%.1f%%)", freq, freq_perc); + + stat.addf("General Processing", "Mean Time: %.3f sec (Max. %.3f sec)", mElabPeriodMean_sec->getMean(), 1. / mCamFrameRate); - if (mPublishingData) - { - freq = 1. / mVideoDepthPeriodMean_sec->getMean(); - freq_perc = 100. * freq / mVideoDepthFreq; - stat.addf("Video/Depth Publish", "Mean Frequency: %.1f Hz (%.1f%%)", freq, freq_perc); - } - - if (mSvoMode) - { - int frame = mZed.getSVOPosition(); - int totFrames = mZed.getSVONumberOfFrames(); - double svo_perc = 100. * (static_cast(frame) / totFrames); - - stat.addf("Playing SVO", "Frame: %d/%d (%.1f%%)", frame, totFrames, svo_perc); - } - - if (mComputeDepth) - { - stat.add("Depth status", "ACTIVE"); - - if (mPcPublishing) - { - double freq = 1000000. / mPcPeriodMean_usec->getMean(); - double freq_perc = 100. * freq / mPointCloudFreq; - stat.addf("Point Cloud", "Mean Frequency: %.1f Hz (%.1f%%)", freq, freq_perc); - } - else - { - stat.add("Point Cloud", "Topic not subscribed"); - } + if (mPublishingData) { + freq = 1. / mVideoDepthPeriodMean_sec->getMean(); + freq_perc = 100. * freq / mVideoDepthFreq; + stat.addf("Video/Depth Publish", "Mean Frequency: %.1f Hz (%.1f%%)", freq, freq_perc); + } - if (mFloorAlignment) - { - if (mInitOdomWithPose) - { - stat.add("Floor Detection", "NOT INITIALIZED"); - } - else - { - stat.add("Floor Detection", "INITIALIZED"); - } - } + if (mSvoMode) { + int frame = mZed.getSVOPosition(); + int totFrames = mZed.getSVONumberOfFrames(); + double svo_perc = 100. * (static_cast(frame) / totFrames); - if (mPosTrackingActivated) - { - stat.addf("Tracking status", "%s", sl::toString(mPosTrackingStatus).c_str()); - } - else - { - stat.add("Pos. Tracking status", "INACTIVE"); + stat.addf("Playing SVO", "Frame: %d/%d (%.1f%%)", frame, totFrames, svo_perc); + } + + if (mComputeDepth) { + stat.add("Depth status", "ACTIVE"); + + if (mPcPublishing) { + double freq = 1000000. / mPcPeriodMean_usec->getMean(); + double freq_perc = 100. * freq / mPointCloudFreq; + stat.addf("Point Cloud", "Mean Frequency: %.1f Hz (%.1f%%)", freq, freq_perc); + } else { + stat.add("Point Cloud", "Topic not subscribed"); + } + + if (mFloorAlignment) { + if (mInitOdomWithPose) { + stat.add("Floor Detection", "NOT INITIALIZED"); + } else { + stat.add("Floor Detection", "INITIALIZED"); + } + } + + if (mPosTrackingActivated) { + stat.addf("Tracking status", "%s", sl::toString(mPosTrackingStatus).c_str()); + } else { + stat.add("Pos. Tracking status", "INACTIVE"); + } + + if (mObjDetRunning) { + double freq = 1000. / mObjDetPeriodMean_msec->getMean(); + double freq_perc = 100. * freq / mCamFrameRate; + stat.addf("Object detection", "Mean Frequency: %.3f Hz (%.1f%%)", freq, freq_perc); + } else { + stat.add("Object Detection", "INACTIVE"); + } + } else { + stat.add("Depth status", "INACTIVE"); + } + } else { + stat.summaryf(diagnostic_msgs::DiagnosticStatus::ERROR, "Camera error: %s", sl::toString(mGrabStatus).c_str()); } + } else { + stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Waiting for data subscriber"); + stat.add("Capture", "INACTIVE"); + } - if (mObjDetRunning) - { - double freq = 1000. / mObjDetPeriodMean_msec->getMean(); - double freq_perc = 100. * freq / mCamFrameRate; - stat.addf("Object detection", "Mean Frequency: %.3f Hz (%.1f%%)", freq, freq_perc); + if (mSensPublishing) { + double freq = 1000000. / mSensPeriodMean_usec->getMean(); + double freq_perc = 100. * freq / mSensPubRate; + stat.addf("IMU", "Mean Frequency: %.1f Hz (%.1f%%)", freq, freq_perc); + } else { + stat.add("IMU", "Topics not subscribed"); + } + + if (mZedRealCamModel == sl::MODEL::ZED2 || mZedRealCamModel == sl::MODEL::ZED2i) { + stat.addf("Left CMOS Temp.", "%.1f °C", mTempLeft); + stat.addf("Right CMOS Temp.", "%.1f °C", mTempRight); + + if (mTempLeft > 70.f || mTempRight > 70.f) { + stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "Camera temperature"); } - else - { - stat.add("Object Detection", "INACTIVE"); + } else { + stat.add("Left CMOS Temp.", "N/A"); + stat.add("Right CMOS Temp.", "N/A"); + } + + if (mRecording) { + if (!mRecStatus.status) { + if (mGrabActive) { + stat.add("SVO Recording", "ERROR"); + stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "Error adding frames to SVO file while recording. Check " + "free disk space"); + } else { + stat.add("SVO Recording", "WAITING"); + } + } else { + stat.add("SVO Recording", "ACTIVE"); + stat.addf("SVO compression time", "%g msec", mRecStatus.average_compression_time); + stat.addf("SVO compression ratio", "%.1f%%", mRecStatus.average_compression_ratio); } - } - else - { - stat.add("Depth status", "INACTIVE"); - } - } - else - { - stat.summaryf(diagnostic_msgs::DiagnosticStatus::ERROR, "Camera error: %s", sl::toString(mGrabStatus).c_str()); - } - } - else - { - stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Waiting for data subscriber"); - stat.add("Capture", "INACTIVE"); - } - - if (mSensPublishing) - { - double freq = 1000000. / mSensPeriodMean_usec->getMean(); - double freq_perc = 100. * freq / mSensPubRate; - stat.addf("IMU", "Mean Frequency: %.1f Hz (%.1f%%)", freq, freq_perc); - } - else - { - stat.add("IMU", "Topics not subscribed"); - } - - if (mZedRealCamModel == sl::MODEL::ZED2 || mZedRealCamModel == sl::MODEL::ZED2i) - { - stat.addf("Left CMOS Temp.", "%.1f °C", mTempLeft); - stat.addf("Right CMOS Temp.", "%.1f °C", mTempRight); - - if (mTempLeft > 70.f || mTempRight > 70.f) - { - stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "Camera temperature"); - } - } - else - { - stat.add("Left CMOS Temp.", "N/A"); - stat.add("Right CMOS Temp.", "N/A"); - } - - if (mRecording) - { - if (!mRecStatus.status) - { - if (mGrabActive) - { - stat.add("SVO Recording", "ERROR"); - stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "Error adding frames to SVO file while recording. Check " - "free disk space"); - } - else - { - stat.add("SVO Recording", "WAITING"); - } - } - else - { - stat.add("SVO Recording", "ACTIVE"); - stat.addf("SVO compression time", "%g msec", mRecStatus.average_compression_time); - stat.addf("SVO compression ratio", "%.1f%%", mRecStatus.average_compression_ratio); - } - } - else - { - stat.add("SVO Recording", "NOT ACTIVE"); - } + } else { + stat.add("SVO Recording", "NOT ACTIVE"); + } } bool ZEDWrapperNodelet::on_start_svo_recording(zed_interfaces::start_svo_recording::Request& req, - zed_interfaces::start_svo_recording::Response& res) + zed_interfaces::start_svo_recording::Response& res) { - std::lock_guard lock(mRecMutex); + std::lock_guard lock(mRecMutex); - if (mRecording) - { - res.result = false; - res.info = "Recording was just active"; - return false; - } + if (mRecording) { + res.result = false; + res.info = "Recording was just active"; + return false; + } - // Check filename - if (req.svo_filename.empty()) - { - req.svo_filename = "zed.svo"; - } + // Check filename + if (req.svo_filename.empty()) { + req.svo_filename = "zed.svo"; + } - sl::ERROR_CODE err; + sl::ERROR_CODE err; - sl::RecordingParameters recParams; - recParams.compression_mode = mSvoComprMode; - recParams.video_filename = req.svo_filename.c_str(); - err = mZed.enableRecording(recParams); + sl::RecordingParameters recParams; + recParams.compression_mode = mSvoComprMode; + recParams.video_filename = req.svo_filename.c_str(); + err = mZed.enableRecording(recParams); - if (err == sl::ERROR_CODE::SVO_UNSUPPORTED_COMPRESSION) - { - recParams.compression_mode = mSvoComprMode == sl::SVO_COMPRESSION_MODE::H265 ? sl::SVO_COMPRESSION_MODE::H264 : - sl::SVO_COMPRESSION_MODE::H265; + if (err == sl::ERROR_CODE::SVO_UNSUPPORTED_COMPRESSION) { + recParams.compression_mode = mSvoComprMode == sl::SVO_COMPRESSION_MODE::H265 ? sl::SVO_COMPRESSION_MODE::H264 : sl::SVO_COMPRESSION_MODE::H265; - NODELET_WARN_STREAM("The chosen " << sl::toString(mSvoComprMode).c_str() << "mode is not available. Trying " - << sl::toString(recParams.compression_mode).c_str()); + NODELET_WARN_STREAM("The chosen " << sl::toString(mSvoComprMode).c_str() << "mode is not available. Trying " + << sl::toString(recParams.compression_mode).c_str()); - err = mZed.enableRecording(recParams); + err = mZed.enableRecording(recParams); - if (err == sl::ERROR_CODE::SVO_UNSUPPORTED_COMPRESSION) - { - NODELET_WARN_STREAM(sl::toString(recParams.compression_mode).c_str() - << "not available. Trying " << sl::toString(sl::SVO_COMPRESSION_MODE::H264).c_str()); - recParams.compression_mode = sl::SVO_COMPRESSION_MODE::H264; - err = mZed.enableRecording(recParams); + if (err == sl::ERROR_CODE::SVO_UNSUPPORTED_COMPRESSION) { + NODELET_WARN_STREAM(sl::toString(recParams.compression_mode).c_str() + << "not available. Trying " << sl::toString(sl::SVO_COMPRESSION_MODE::H264).c_str()); + recParams.compression_mode = sl::SVO_COMPRESSION_MODE::H264; + err = mZed.enableRecording(recParams); - if (err == sl::ERROR_CODE::SVO_UNSUPPORTED_COMPRESSION) - { - recParams.compression_mode = sl::SVO_COMPRESSION_MODE::LOSSLESS; - err = mZed.enableRecording(recParams); - } + if (err == sl::ERROR_CODE::SVO_UNSUPPORTED_COMPRESSION) { + recParams.compression_mode = sl::SVO_COMPRESSION_MODE::LOSSLESS; + err = mZed.enableRecording(recParams); + } + } } - } - if (err != sl::ERROR_CODE::SUCCESS) - { - res.result = false; - res.info = sl::toString(err).c_str(); - mRecording = false; - return false; - } + if (err != sl::ERROR_CODE::SUCCESS) { + res.result = false; + res.info = sl::toString(err).c_str(); + mRecording = false; + return false; + } - mSvoComprMode = recParams.compression_mode; - mRecording = true; - res.info = "Recording started ("; - res.info += sl::toString(mSvoComprMode).c_str(); - res.info += ")"; - res.result = true; + mSvoComprMode = recParams.compression_mode; + mRecording = true; + res.info = "Recording started ("; + res.info += sl::toString(mSvoComprMode).c_str(); + res.info += ")"; + res.result = true; - NODELET_INFO_STREAM("SVO recording STARTED: " << req.svo_filename << " (" << sl::toString(mSvoComprMode).c_str() - << ")"); + NODELET_INFO_STREAM("SVO recording STARTED: " << req.svo_filename << " (" << sl::toString(mSvoComprMode).c_str() + << ")"); - return true; + return true; } bool ZEDWrapperNodelet::on_stop_svo_recording(zed_interfaces::stop_svo_recording::Request& req, - zed_interfaces::stop_svo_recording::Response& res) + zed_interfaces::stop_svo_recording::Response& res) { - std::lock_guard lock(mRecMutex); + std::lock_guard lock(mRecMutex); - if (!mRecording) - { - res.done = false; - res.info = "Recording was not active"; - NODELET_WARN_STREAM("Can't stop SVO recording. Recording was not active"); - return false; - } + if (!mRecording) { + res.done = false; + res.info = "Recording was not active"; + NODELET_WARN_STREAM("Can't stop SVO recording. Recording was not active"); + return false; + } - mZed.disableRecording(); - mRecording = false; - res.info = "Recording stopped"; - res.done = true; + mZed.disableRecording(); + mRecording = false; + res.info = "Recording stopped"; + res.done = true; - NODELET_INFO_STREAM("SVO recording STOPPED"); + NODELET_INFO_STREAM("SVO recording STOPPED"); - return true; + return true; } bool ZEDWrapperNodelet::on_start_remote_stream(zed_interfaces::start_remote_stream::Request& req, - zed_interfaces::start_remote_stream::Response& res) + zed_interfaces::start_remote_stream::Response& res) { - if (mStreaming) - { - res.result = false; - res.info = "SVO remote streaming was just active"; - return false; - } + if (mStreaming) { + res.result = false; + res.info = "SVO remote streaming was just active"; + return false; + } - sl::StreamingParameters params; - params.codec = static_cast(req.codec); - params.port = req.port; - params.bitrate = req.bitrate; - params.gop_size = req.gop_size; - params.adaptative_bitrate = req.adaptative_bitrate; + sl::StreamingParameters params; + params.codec = static_cast(req.codec); + params.port = req.port; + params.bitrate = req.bitrate; + params.gop_size = req.gop_size; + params.adaptative_bitrate = req.adaptative_bitrate; - if ((params.gop_size < -1) || (params.gop_size > 256)) - { - mStreaming = false; + if ((params.gop_size < -1) || (params.gop_size > 256)) { + mStreaming = false; - res.result = false; - res.info = "`gop_size` wrong ("; - res.info += params.gop_size; - res.info += "). Remote streaming not started"; + res.result = false; + res.info = "`gop_size` wrong ("; + res.info += params.gop_size; + res.info += "). Remote streaming not started"; - NODELET_ERROR_STREAM(res.info); - return false; - } + NODELET_ERROR_STREAM(res.info); + return false; + } - if (params.port % 2 != 0) - { - mStreaming = false; + if (params.port % 2 != 0) { + mStreaming = false; - res.result = false; - res.info = "`port` must be an even number. Remote streaming not started"; + res.result = false; + res.info = "`port` must be an even number. Remote streaming not started"; - NODELET_ERROR_STREAM(res.info); - return false; - } + NODELET_ERROR_STREAM(res.info); + return false; + } - sl::ERROR_CODE err = mZed.enableStreaming(params); + sl::ERROR_CODE err = mZed.enableStreaming(params); - if (err != sl::ERROR_CODE::SUCCESS) - { - mStreaming = false; + if (err != sl::ERROR_CODE::SUCCESS) { + mStreaming = false; - res.result = false; - res.info = sl::toString(err).c_str(); + res.result = false; + res.info = sl::toString(err).c_str(); - NODELET_ERROR_STREAM("Remote streaming not started (" << res.info << ")"); + NODELET_ERROR_STREAM("Remote streaming not started (" << res.info << ")"); - return false; - } + return false; + } - mStreaming = true; + mStreaming = true; - NODELET_INFO_STREAM("Remote streaming STARTED"); + NODELET_INFO_STREAM("Remote streaming STARTED"); - res.result = true; - res.info = "Remote streaming STARTED"; - return true; + res.result = true; + res.info = "Remote streaming STARTED"; + return true; } bool ZEDWrapperNodelet::on_stop_remote_stream(zed_interfaces::stop_remote_stream::Request& req, - zed_interfaces::stop_remote_stream::Response& res) + zed_interfaces::stop_remote_stream::Response& res) { - if (mStreaming) - { - mZed.disableStreaming(); - } + if (mStreaming) { + mZed.disableStreaming(); + } - mStreaming = false; - NODELET_INFO_STREAM("SVO remote streaming STOPPED"); + mStreaming = false; + NODELET_INFO_STREAM("SVO remote streaming STOPPED"); - res.done = true; - return true; + res.done = true; + return true; } bool ZEDWrapperNodelet::on_set_led_status(zed_interfaces::set_led_status::Request& req, - zed_interfaces::set_led_status::Response& res) + zed_interfaces::set_led_status::Response& res) { - if (mCamFwVersion < 1523) - { - NODELET_WARN_STREAM("To set the status of the blue LED the camera must be updated to FW 1523 or newer"); - return false; - } + if (mCamFwVersion < 1523) { + NODELET_WARN_STREAM("To set the status of the blue LED the camera must be updated to FW 1523 or newer"); + return false; + } - mZed.setCameraSettings(sl::VIDEO_SETTINGS::LED_STATUS, req.led_enabled ? 1 : 0); + mZed.setCameraSettings(sl::VIDEO_SETTINGS::LED_STATUS, req.led_enabled ? 1 : 0); - return true; + return true; } bool ZEDWrapperNodelet::on_toggle_led(zed_interfaces::toggle_led::Request& req, - zed_interfaces::toggle_led::Response& res) + zed_interfaces::toggle_led::Response& res) { - if (mCamFwVersion < 1523) - { - NODELET_WARN_STREAM("To set the status of the blue LED the camera must be updated to FW 1523 or newer"); - return false; - } + if (mCamFwVersion < 1523) { + NODELET_WARN_STREAM("To set the status of the blue LED the camera must be updated to FW 1523 or newer"); + return false; + } - int status = mZed.getCameraSettings(sl::VIDEO_SETTINGS::LED_STATUS); - int new_status = status == 0 ? 1 : 0; - mZed.setCameraSettings(sl::VIDEO_SETTINGS::LED_STATUS, new_status); + int status = mZed.getCameraSettings(sl::VIDEO_SETTINGS::LED_STATUS); + int new_status = status == 0 ? 1 : 0; + mZed.setCameraSettings(sl::VIDEO_SETTINGS::LED_STATUS, new_status); - return (new_status == 1); + return (new_status == 1); } bool ZEDWrapperNodelet::on_start_3d_mapping(zed_interfaces::start_3d_mapping::Request& req, - zed_interfaces::start_3d_mapping::Response& res) + zed_interfaces::start_3d_mapping::Response& res) { - if (mMappingEnabled && mMappingRunning) - { - NODELET_WARN_STREAM("Spatial mapping was just running"); + if (mMappingEnabled && mMappingRunning) { + NODELET_WARN_STREAM("Spatial mapping was just running"); - res.done = false; - return res.done; - } + res.done = false; + return res.done; + } - mMappingRunning = false; + mMappingRunning = false; - mMappingRes = req.resolution; - mMaxMappingRange = req.max_mapping_range; - mFusedPcPubFreq = req.fused_pointcloud_freq; + mMappingRes = req.resolution; + mMaxMappingRange = req.max_mapping_range; + mFusedPcPubFreq = req.fused_pointcloud_freq; - NODELET_DEBUG_STREAM(" * Received mapping resolution\t\t-> " << mMappingRes << " m"); - NODELET_DEBUG_STREAM(" * Received mapping max range\t-> " << mMaxMappingRange << " m"); - NODELET_DEBUG_STREAM(" * Received fused point cloud freq:\t-> " << mFusedPcPubFreq << " Hz"); + NODELET_DEBUG_STREAM(" * Received mapping resolution\t\t-> " << mMappingRes << " m"); + NODELET_DEBUG_STREAM(" * Received mapping max range\t-> " << mMaxMappingRange << " m"); + NODELET_DEBUG_STREAM(" * Received fused point cloud freq:\t-> " << mFusedPcPubFreq << " Hz"); - mMappingEnabled = true; - res.done = true; + mMappingEnabled = true; + res.done = true; - return res.done; + return res.done; } bool ZEDWrapperNodelet::on_stop_3d_mapping(zed_interfaces::stop_3d_mapping::Request& req, - zed_interfaces::stop_3d_mapping::Response& res) + zed_interfaces::stop_3d_mapping::Response& res) { - if (mMappingEnabled) - { - mPubFusedCloud.shutdown(); - mMappingMutex.lock(); - stop_3d_mapping(); - mMappingMutex.unlock(); - - res.done = true; - } - else - { - res.done = false; - } + if (mMappingEnabled) { + mPubFusedCloud.shutdown(); + mMappingMutex.lock(); + stop_3d_mapping(); + mMappingMutex.unlock(); + + res.done = true; + } else { + res.done = false; + } - return res.done; + return res.done; } bool ZEDWrapperNodelet::on_save_3d_map(zed_interfaces::save_3d_map::Request& req, - zed_interfaces::save_3d_map::Response& res) + zed_interfaces::save_3d_map::Response& res) { - if (!mMappingEnabled) - { - res.result = false; - res.info = "3D Mapping was not active"; - NODELET_WARN_STREAM("Can't save 3D map. Mapping was not active"); - return false; - } + if (!mMappingEnabled) { + res.result = false; + res.info = "3D Mapping was not active"; + NODELET_WARN_STREAM("Can't save 3D map. Mapping was not active"); + return false; + } - mMapSave = true; + mMapSave = true; - std::lock_guard lock(mMappingMutex); - sl::String filename = req.map_filename.c_str(); - if (req.file_format < 0 || req.file_format > static_cast(sl::MESH_FILE_FORMAT::OBJ)) - { - res.result = false; - res.info = "File format not correct"; - NODELET_WARN_STREAM("Can't save 3D map. File format not correct"); - return false; - } + std::lock_guard lock(mMappingMutex); + sl::String filename = req.map_filename.c_str(); + if (req.file_format < 0 || req.file_format > static_cast(sl::MESH_FILE_FORMAT::OBJ)) { + res.result = false; + res.info = "File format not correct"; + NODELET_WARN_STREAM("Can't save 3D map. File format not correct"); + return false; + } - sl::MESH_FILE_FORMAT file_format = static_cast(req.file_format); + sl::MESH_FILE_FORMAT file_format = static_cast(req.file_format); - bool success = mFusedPC.save(filename, file_format); + bool success = mFusedPC.save(filename, file_format); - if (!success) - { - res.result = false; - res.info = "3D Map not saved"; - NODELET_ERROR_STREAM("3D Map not saved"); - return false; - } + if (!success) { + res.result = false; + res.info = "3D Map not saved"; + NODELET_ERROR_STREAM("3D Map not saved"); + return false; + } - res.info = "3D map saved"; - res.result = true; - return true; + res.info = "3D map saved"; + res.result = true; + return true; } bool ZEDWrapperNodelet::on_start_object_detection(zed_interfaces::start_object_detection::Request& req, - zed_interfaces::start_object_detection::Response& res) + zed_interfaces::start_object_detection::Response& res) { - NODELET_INFO("Called 'start_object_detection' service"); + NODELET_INFO("Called 'start_object_detection' service"); + + if (mZedRealCamModel == sl::MODEL::ZED) { + mObjDetEnabled = false; + mObjDetRunning = false; + + NODELET_ERROR_STREAM("Object detection not started. OD is not available for ZED camera model"); + res.done = false; + return res.done; + } + + if (mObjDetEnabled && mObjDetRunning) { + NODELET_WARN_STREAM("Object Detection was just running"); + + res.done = false; + return res.done; + } - if (mZedRealCamModel != sl::MODEL::ZED2 && mZedRealCamModel != sl::MODEL::ZED2i) - { - mObjDetEnabled = false; mObjDetRunning = false; - NODELET_ERROR_STREAM("Object detection not started. OD is available only using a ZED2/ZED2i camera model"); - res.done = false; - return res.done; - } + mObjDetConfidence = req.confidence; + mObjDetTracking = req.tracking; + if (req.model < 0 || req.model >= static_cast(sl::DETECTION_MODEL::LAST)) { + NODELET_ERROR_STREAM("Object Detection model not valid."); + res.done = false; + return res.done; + } + mObjDetModel = static_cast(req.model); - if (mObjDetEnabled && mObjDetRunning) - { - NODELET_WARN_STREAM("Object Detection was just running"); + mObjDetMaxRange = req.max_range; + if (mObjDetMaxRange > mCamMaxDepth) { + NODELET_WARN("Detection max range cannot be major than depth max range. Automatically fixed."); + mObjDetMaxRange = mCamMaxDepth; + } + NODELET_INFO_STREAM(" * Detection max range\t\t-> " << mObjDetMaxRange); - res.done = false; - return res.done; - } + NODELET_INFO_STREAM(" * Object min. confidence\t-> " << mObjDetConfidence); + NODELET_INFO_STREAM(" * Object tracking\t\t-> " << (mObjDetTracking ? "ENABLED" : "DISABLED")); + NODELET_INFO_STREAM(" * Detection model\t\t-> " << sl::toString(mObjDetModel)); + + if (mObjDetModel == sl::DETECTION_MODEL::HUMAN_BODY_ACCURATE || mObjDetModel == sl::DETECTION_MODEL::HUMAN_BODY_MEDIUM || mObjDetModel == sl::DETECTION_MODEL::HUMAN_BODY_FAST) { + mObjDetBodyFitting = req.sk_body_fitting; + NODELET_INFO_STREAM(" * Body fitting\t\t\t-> " << (mObjDetBodyFitting ? "ENABLED" : "DISABLED")); + } else { + mObjDetPeopleEnable = req.mc_people; + NODELET_INFO_STREAM(" * Detect people\t\t-> " << (mObjDetPeopleEnable ? "ENABLED" : "DISABLED")); + mObjDetVehiclesEnable = req.mc_vehicles; + NODELET_INFO_STREAM(" * Detect vehicles\t\t-> " << (mObjDetVehiclesEnable ? "ENABLED" : "DISABLED")); + mObjDetBagsEnable = req.mc_bag; + NODELET_INFO_STREAM(" * Detect bags\t\t\t-> " << (mObjDetBagsEnable ? "ENABLED" : "DISABLED")); + mObjDetAnimalsEnable = req.mc_animal; + NODELET_INFO_STREAM(" * Detect animals\t\t-> " << (mObjDetAnimalsEnable ? "ENABLED" : "DISABLED")); + mObjDetElectronicsEnable = req.mc_electronics; + NODELET_INFO_STREAM(" * Detect electronics\t\t-> " << (mObjDetElectronicsEnable ? "ENABLED" : "DISABLED")); + mObjDetFruitsEnable = req.mc_fruit_vegetable; + NODELET_INFO_STREAM(" * Detect fruit and vegetables\t-> " << (mObjDetFruitsEnable ? "ENABLED" : "DISABLED")); + mObjDetSportsEnable = req.mc_sport; + NODELET_INFO_STREAM(" * Detect sport related objects\t-> " << (mObjDetSportsEnable ? "ENABLED" : "DISABLED")); + } - mObjDetRunning = false; + mObjDetRunning = false; + mObjDetEnabled = true; + res.done = true; - mObjDetConfidence = req.confidence; - mObjDetTracking = req.tracking; - if (req.model < 0 || req.model >= static_cast(sl::DETECTION_MODEL::LAST)) - { - NODELET_ERROR_STREAM("Object Detection model not valid."); - res.done = false; return res.done; - } - mObjDetModel = static_cast(req.model); - - mObjDetMaxRange = req.max_range; - if (mObjDetMaxRange > mCamMaxDepth) - { - NODELET_WARN("Detection max range cannot be major than depth max range. Automatically fixed."); - mObjDetMaxRange = mCamMaxDepth; - } - NODELET_INFO_STREAM(" * Detection max range\t\t-> " << mObjDetMaxRange); - - NODELET_INFO_STREAM(" * Object min. confidence\t-> " << mObjDetConfidence); - NODELET_INFO_STREAM(" * Object tracking\t\t-> " << (mObjDetTracking ? "ENABLED" : "DISABLED")); - NODELET_INFO_STREAM(" * Detection model\t\t-> " << sl::toString(mObjDetModel)); - - if (mObjDetModel == sl::DETECTION_MODEL::HUMAN_BODY_FAST || mObjDetModel == sl::DETECTION_MODEL::HUMAN_BODY_ACCURATE) - { - mObjDetBodyFitting = req.sk_body_fitting; - NODELET_INFO_STREAM(" * Body fitting\t\t\t-> " << (mObjDetBodyFitting ? "ENABLED" : "DISABLED")); - } - else - { - mObjDetPeopleEnable = req.mc_people; - NODELET_INFO_STREAM(" * Detect people\t\t-> " << (mObjDetPeopleEnable ? "ENABLED" : "DISABLED")); - mObjDetVehiclesEnable = req.mc_vehicles; - NODELET_INFO_STREAM(" * Detect vehicles\t\t-> " << (mObjDetVehiclesEnable ? "ENABLED" : "DISABLED")); - mObjDetBagsEnable = req.mc_bag; - NODELET_INFO_STREAM(" * Detect bags\t\t\t-> " << (mObjDetBagsEnable ? "ENABLED" : "DISABLED")); - mObjDetAnimalsEnable = req.mc_animal; - NODELET_INFO_STREAM(" * Detect animals\t\t-> " << (mObjDetAnimalsEnable ? "ENABLED" : "DISABLED")); - mObjDetElectronicsEnable = req.mc_electronics; - NODELET_INFO_STREAM(" * Detect electronics\t\t-> " << (mObjDetElectronicsEnable ? "ENABLED" : "DISABLED")); - mObjDetFruitsEnable = req.mc_fruit_vegetable; - NODELET_INFO_STREAM(" * Detect fruit and vegetables\t-> " << (mObjDetFruitsEnable ? "ENABLED" : "DISABLED")); - } - - mObjDetRunning = false; - mObjDetEnabled = true; - res.done = true; - - return res.done; } /*! \brief Service callback to stop_object_detection service */ bool ZEDWrapperNodelet::on_stop_object_detection(zed_interfaces::stop_object_detection::Request& req, - zed_interfaces::stop_object_detection::Response& res) + zed_interfaces::stop_object_detection::Response& res) { - if (mObjDetEnabled) - { - mObjDetMutex.lock(); - stop_obj_detect(); - mObjDetMutex.unlock(); - - res.done = true; - } - else - { - res.done = false; - } + if (mObjDetEnabled) { + mObjDetMutex.lock(); + stop_obj_detect(); + mObjDetMutex.unlock(); + + res.done = true; + } else { + res.done = false; + } - return res.done; + return res.done; } void ZEDWrapperNodelet::processDetectedObjects(ros::Time t) { - static std::chrono::steady_clock::time_point old_time = std::chrono::steady_clock::now(); - - sl::ObjectDetectionRuntimeParameters objectTracker_parameters_rt; - objectTracker_parameters_rt.detection_confidence_threshold = mObjDetConfidence; - objectTracker_parameters_rt.object_class_filter = mObjDetFilter; - - sl::Objects objects; - - sl::ERROR_CODE objDetRes = mZed.retrieveObjects(objects, objectTracker_parameters_rt); - - if (objDetRes != sl::ERROR_CODE::SUCCESS) - { - NODELET_WARN_STREAM("Object Detection error: " << sl::toString(objDetRes)); - return; - } - - if (!objects.is_new) - { - return; - } - - // ----> Diagnostic information update - std::chrono::steady_clock::time_point now = std::chrono::steady_clock::now(); - double elapsed_msec = std::chrono::duration_cast(now - old_time).count(); - mObjDetPeriodMean_msec->addValue(elapsed_msec); - old_time = now; - // <---- Diagnostic information update - - NODELET_DEBUG_STREAM("Detected " << objects.object_list.size() << " objects"); - - size_t objCount = objects.object_list.size(); - - zed_interfaces::ObjectsStampedPtr objMsg = boost::make_shared(); - objMsg->header.stamp = t; - objMsg->header.frame_id = mLeftCamFrameId; - - objMsg->objects.resize(objCount); - - size_t idx = 0; - for (auto data : objects.object_list) - { - objMsg->objects[idx].label = sl::toString(data.label).c_str(); - objMsg->objects[idx].sublabel = sl::toString(data.sublabel).c_str(); - objMsg->objects[idx].label_id = data.id; - objMsg->objects[idx].confidence = data.confidence; - - memcpy(&(objMsg->objects[idx].position[0]), &(data.position[0]), 3 * sizeof(float)); - memcpy(&(objMsg->objects[idx].position_covariance[0]), &(data.position_covariance[0]), 6 * sizeof(float)); - memcpy(&(objMsg->objects[idx].velocity[0]), &(data.velocity[0]), 3 * sizeof(float)); - - objMsg->objects[idx].tracking_available = mObjDetTracking; - objMsg->objects[idx].tracking_state = static_cast(data.tracking_state); - // NODELET_INFO_STREAM( "[" << idx << "] Tracking: " << - // sl::toString(static_cast(data.tracking_state))); - objMsg->objects[idx].action_state = static_cast(data.action_state); - - if (data.bounding_box_2d.size() == 4) - { - memcpy(&(objMsg->objects[idx].bounding_box_2d.corners[0]), &(data.bounding_box_2d[0]), 8 * sizeof(unsigned int)); - } - if (data.bounding_box.size() == 8) - { - memcpy(&(objMsg->objects[idx].bounding_box_3d.corners[0]), &(data.bounding_box[0]), 24 * sizeof(float)); - } - - memcpy(&(objMsg->objects[idx].dimensions_3d[0]), &(data.dimensions[0]), 3 * sizeof(float)); - - if (mObjDetModel == sl::DETECTION_MODEL::HUMAN_BODY_ACCURATE || - mObjDetModel == sl::DETECTION_MODEL::HUMAN_BODY_FAST) - { - objMsg->objects[idx].skeleton_available = true; - - if (data.head_bounding_box_2d.size() == 4) - { - memcpy(&(objMsg->objects[idx].head_bounding_box_2d.corners[0]), &(data.head_bounding_box_2d[0]), - 8 * sizeof(unsigned int)); - } - if (data.head_bounding_box.size() == 8) - { - memcpy(&(objMsg->objects[idx].head_bounding_box_3d.corners[0]), &(data.head_bounding_box[0]), - 24 * sizeof(float)); - } - memcpy(&(objMsg->objects[idx].head_position[0]), &(data.head_position[0]), 3 * sizeof(float)); - - if (data.keypoint_2d.size() == 18) - { - memcpy(&(objMsg->objects[idx].skeleton_2d.keypoints[0]), &(data.keypoint_2d[0]), 36 * sizeof(float)); - } - if (data.keypoint_2d.size() == 18) - { - memcpy(&(objMsg->objects[idx].skeleton_3d.keypoints[0]), &(data.keypoint[0]), 54 * sizeof(float)); - } - } - else - { - objMsg->objects[idx].skeleton_available = false; - } - - // at the end of the loop - idx++; - } - - mPubObjDet.publish(objMsg); + static std::chrono::steady_clock::time_point old_time = std::chrono::steady_clock::now(); + + sl::ObjectDetectionRuntimeParameters objectTracker_parameters_rt; + objectTracker_parameters_rt.detection_confidence_threshold = mObjDetConfidence; + objectTracker_parameters_rt.object_class_filter = mObjDetFilter; + + sl::Objects objects; + + sl::ERROR_CODE objDetRes = mZed.retrieveObjects(objects, objectTracker_parameters_rt); + + if (objDetRes != sl::ERROR_CODE::SUCCESS) { + NODELET_WARN_STREAM("Object Detection error: " << sl::toString(objDetRes)); + return; + } + + if (!objects.is_new) { + return; + } + + // ----> Diagnostic information update + std::chrono::steady_clock::time_point now = std::chrono::steady_clock::now(); + double elapsed_msec = std::chrono::duration_cast(now - old_time).count(); + mObjDetPeriodMean_msec->addValue(elapsed_msec); + old_time = now; + // <---- Diagnostic information update + + NODELET_DEBUG_STREAM("Detected " << objects.object_list.size() << " objects"); + + size_t objCount = objects.object_list.size(); + + zed_interfaces::ObjectsStampedPtr objMsg = boost::make_shared(); + objMsg->header.stamp = t; + objMsg->header.frame_id = mLeftCamFrameId; + + objMsg->objects.resize(objCount); + + size_t idx = 0; + for (auto data : objects.object_list) { + objMsg->objects[idx].label = sl::toString(data.label).c_str(); + objMsg->objects[idx].sublabel = sl::toString(data.sublabel).c_str(); + objMsg->objects[idx].label_id = data.id; + objMsg->objects[idx].confidence = data.confidence; + + memcpy(&(objMsg->objects[idx].position[0]), &(data.position[0]), 3 * sizeof(float)); + memcpy(&(objMsg->objects[idx].position_covariance[0]), &(data.position_covariance[0]), 6 * sizeof(float)); + memcpy(&(objMsg->objects[idx].velocity[0]), &(data.velocity[0]), 3 * sizeof(float)); + + objMsg->objects[idx].tracking_available = mObjDetTracking; + objMsg->objects[idx].tracking_state = static_cast(data.tracking_state); + // NODELET_INFO_STREAM( "[" << idx << "] Tracking: " << + // sl::toString(static_cast(data.tracking_state))); + objMsg->objects[idx].action_state = static_cast(data.action_state); + + if (data.bounding_box_2d.size() == 4) { + memcpy(&(objMsg->objects[idx].bounding_box_2d.corners[0]), &(data.bounding_box_2d[0]), 8 * sizeof(unsigned int)); + } + if (data.bounding_box.size() == 8) { + memcpy(&(objMsg->objects[idx].bounding_box_3d.corners[0]), &(data.bounding_box[0]), 24 * sizeof(float)); + } + + memcpy(&(objMsg->objects[idx].dimensions_3d[0]), &(data.dimensions[0]), 3 * sizeof(float)); + + if (mObjDetModel == sl::DETECTION_MODEL::HUMAN_BODY_ACCURATE || mObjDetModel == sl::DETECTION_MODEL::HUMAN_BODY_MEDIUM || mObjDetModel == sl::DETECTION_MODEL::HUMAN_BODY_FAST) { + objMsg->objects[idx].skeleton_available = true; + + if (data.head_bounding_box_2d.size() == 4) { + memcpy(&(objMsg->objects[idx].head_bounding_box_2d.corners[0]), &(data.head_bounding_box_2d[0]), + 8 * sizeof(unsigned int)); + } + if (data.head_bounding_box.size() == 8) { + memcpy(&(objMsg->objects[idx].head_bounding_box_3d.corners[0]), &(data.head_bounding_box[0]), + 24 * sizeof(float)); + } + memcpy(&(objMsg->objects[idx].head_position[0]), &(data.head_position[0]), 3 * sizeof(float)); + + if (data.keypoint_2d.size() == 18) { + memcpy(&(objMsg->objects[idx].skeleton_2d.keypoints[0]), &(data.keypoint_2d[0]), 36 * sizeof(float)); + } + if (data.keypoint_2d.size() == 18) { + memcpy(&(objMsg->objects[idx].skeleton_3d.keypoints[0]), &(data.keypoint[0]), 54 * sizeof(float)); + } + } else { + objMsg->objects[idx].skeleton_available = false; + } + + // at the end of the loop + idx++; + } + + mPubObjDet.publish(objMsg); } -} // namespace zed_nodelets +} // namespace zed_nodelets diff --git a/zed_wrapper/params/common.yaml b/zed_wrapper/params/common.yaml index b2d670aa..1088e091 100644 --- a/zed_wrapper/params/common.yaml +++ b/zed_wrapper/params/common.yaml @@ -14,17 +14,17 @@ gain: 100 # Dynamic - work exposure: 100 # Dynamic - works only if `auto_exposure_gain` is false auto_whitebalance: true # Dynamic whitebalance_temperature: 42 # Dynamic - works only if `auto_whitebalance` is false -depth_confidence: 50 # Dynamic +depth_confidence: 30 # Dynamic depth_texture_conf: 100 # Dynamic -pub_frame_rate: 100.0 # Dynamic - frequency of publishing of video and depth data -point_cloud_freq: 100.0 # Dynamic - frequency of the pointcloud publishing (equal or less to `grab_frame_rate` value) +pub_frame_rate: 15.0 # Dynamic - frequency of publishing of video and depth data +point_cloud_freq: 10.0 # Dynamic - frequency of the pointcloud publishing (equal or less to `grab_frame_rate` value) general: camera_name: zed # A name for the camera (can be different from camera model and node name and can be overwritten by the launch file) zed_id: 0 serial_number: 0 - resolution: 3 # '0': HD2K, '1': HD1080, '2': HD720, '3': VGA - grab_frame_rate: 100 # Frequency of frame grabbing for internal SDK operations + resolution: 2 # '0': HD2K, '1': HD1080, '2': HD720, '3': VGA + grab_frame_rate: 15 # Frequency of frame grabbing for internal SDK operations gpu_id: -1 base_frame: 'base_link' # must be equal to the frame_id used in the URDF file verbose: false # Enable info message by the ZED SDK @@ -37,7 +37,7 @@ video: extrinsic_in_camera_frame: true # if `false` extrinsic parameter in `camera_info` will use ROS native frame (X FORWARD, Z UP) instead of the camera frame (Z FORWARD, Y DOWN) [`true` use old behavior as for version < v3.1] depth: - quality: 1 # '0': NONE, '1': PERFORMANCE, '2': QUALITY, '3': ULTRA + quality: 4 # '0': NONE, '1': PERFORMANCE, '2': QUALITY, '3': ULTRA, '4': NEURAL sensing_mode: 0 # '0': STANDARD, '1': FILL (not use FILL for robotic applications) depth_stabilization: 1 # `0`: disabled, `1`: enabled openni_depth_mode: false # 'false': 32bit float meters, 'true': 16bit uchar millimeters diff --git a/zed_wrapper/params/zed2.yaml b/zed_wrapper/params/zed2.yaml index 3c289deb..c22c77e4 100644 --- a/zed_wrapper/params/zed2.yaml +++ b/zed_wrapper/params/zed2.yaml @@ -17,15 +17,16 @@ sensors: publish_imu_tf: true # publish `IMU -> _left_camera_frame` TF object_detection: - od_enabled: false # True to enable Object Detection [only ZED 2] - model: 3 # '0': MULTI_CLASS_BOX - '1': MULTI_CLASS_BOX_ACCURATE - '2': HUMAN_BODY_FAST - '3': HUMAN_BODY_ACCURATE + od_enabled: false # True to enable Object Detection [not available for ZED] + model: 2 # '0': MULTI_CLASS_BOX - '1': MULTI_CLASS_BOX_ACCURATE - '2': HUMAN_BODY_FAST - '3': HUMAN_BODY_ACCURATE - '4': MULTI_CLASS_BOX_MEDIUM - '5': HUMAN_BODY_MEDIUM - '6': PERSON_HEAD_BOX confidence_threshold: 50 # Minimum value of the detection confidence of an object [0,100] max_range: 15. # Maximum detection range object_tracking_enabled: true # Enable/disable the tracking of the detected objects - body_fitting: false # Enable/disable body fitting for 'HUMAN_BODY_FAST' and 'HUMAN_BODY_ACCURATE' models - mc_people: true # Enable/disable the detection of persons for 'MULTI_CLASS_BOX' and 'MULTI_CLASS_BOX_ACCURATE' models - mc_vehicle: true # Enable/disable the detection of vehicles for 'MULTI_CLASS_BOX' and 'MULTI_CLASS_BOX_ACCURATE' models - mc_bag: true # Enable/disable the detection of bags for 'MULTI_CLASS_BOX' and 'MULTI_CLASS_BOX_ACCURATE' models - mc_animal: true # Enable/disable the detection of animals for 'MULTI_CLASS_BOX' and 'MULTI_CLASS_BOX_ACCURATE' models - mc_electronics: true # Enable/disable the detection of electronic devices for 'MULTI_CLASS_BOX' and 'MULTI_CLASS_BOX_ACCURATE' models - mc_fruit_vegetable: true # Enable/disable the detection of fruits and vegetables for 'MULTI_CLASS_BOX' and 'MULTI_CLASS_BOX_ACCURATE' models + body_fitting: false # Enable/disable body fitting for 'HUMAN_BODY_X' models + mc_people: true # Enable/disable the detection of persons for 'MULTI_CLASS_BOX_X' models + mc_vehicle: true # Enable/disable the detection of vehicles for 'MULTI_CLASS_BOX_X' models + mc_bag: true # Enable/disable the detection of bags for 'MULTI_CLASS_BOX_X' models + mc_animal: true # Enable/disable the detection of animals for 'MULTI_CLASS_BOX_X' models + mc_electronics: true # Enable/disable the detection of electronic devices for 'MULTI_CLASS_BOX_X' models + mc_fruit_vegetable: true # Enable/disable the detection of fruits and vegetables for 'MULTI_CLASS_BOX_X' models + mc_sport: true # Enable/disable the detection of sport-related objects for 'MULTI_CLASS_BOX_X' models diff --git a/zed_wrapper/params/zed2i.yaml b/zed_wrapper/params/zed2i.yaml index acac6ad9..73312413 100644 --- a/zed_wrapper/params/zed2i.yaml +++ b/zed_wrapper/params/zed2i.yaml @@ -17,15 +17,16 @@ sensors: publish_imu_tf: true # publish `IMU -> _left_camera_frame` TF object_detection: - od_enabled: false # True to enable Object Detection [only ZED 2] - model: 3 # '0': MULTI_CLASS_BOX - '1': MULTI_CLASS_BOX_ACCURATE - '2': HUMAN_BODY_FAST - '3': HUMAN_BODY_ACCURATE + od_enabled: false # True to enable Object Detection [not available for ZED] + model: 1 # '0': MULTI_CLASS_BOX - '1': MULTI_CLASS_BOX_ACCURATE - '2': HUMAN_BODY_FAST - '3': HUMAN_BODY_ACCURATE - '4': MULTI_CLASS_BOX_MEDIUM - '5': HUMAN_BODY_MEDIUM - '6': PERSON_HEAD_BOX confidence_threshold: 50 # Minimum value of the detection confidence of an object [0,100] max_range: 15. # Maximum detection range object_tracking_enabled: true # Enable/disable the tracking of the detected objects - body_fitting: false # Enable/disable body fitting for 'HUMAN_BODY_FAST' and 'HUMAN_BODY_ACCURATE' models - mc_people: true # Enable/disable the detection of persons for 'MULTI_CLASS_BOX' and 'MULTI_CLASS_BOX_ACCURATE' models - mc_vehicle: true # Enable/disable the detection of vehicles for 'MULTI_CLASS_BOX' and 'MULTI_CLASS_BOX_ACCURATE' models - mc_bag: true # Enable/disable the detection of bags for 'MULTI_CLASS_BOX' and 'MULTI_CLASS_BOX_ACCURATE' models - mc_animal: true # Enable/disable the detection of animals for 'MULTI_CLASS_BOX' and 'MULTI_CLASS_BOX_ACCURATE' models - mc_electronics: true # Enable/disable the detection of electronic devices for 'MULTI_CLASS_BOX' and 'MULTI_CLASS_BOX_ACCURATE' models - mc_fruit_vegetable: true # Enable/disable the detection of fruits and vegetables for 'MULTI_CLASS_BOX' and 'MULTI_CLASS_BOX_ACCURATE' models + body_fitting: false # Enable/disable body fitting for 'HUMAN_BODY_X' models + mc_people: true # Enable/disable the detection of persons for 'MULTI_CLASS_BOX_X' models + mc_vehicle: true # Enable/disable the detection of vehicles for 'MULTI_CLASS_BOX_X' models + mc_bag: true # Enable/disable the detection of bags for 'MULTI_CLASS_BOX_X' models + mc_animal: true # Enable/disable the detection of animals for 'MULTI_CLASS_BOX_X' models + mc_electronics: true # Enable/disable the detection of electronic devices for 'MULTI_CLASS_BOX_X' models + mc_fruit_vegetable: true # Enable/disable the detection of fruits and vegetables for 'MULTI_CLASS_BOX_X' models + mc_sport: true # Enable/disable the detection of sport-related objects for 'MULTI_CLASS_BOX_X' models diff --git a/zed_wrapper/params/zedm.yaml b/zed_wrapper/params/zedm.yaml index e8e9ab3d..8f4f04fb 100644 --- a/zed_wrapper/params/zedm.yaml +++ b/zed_wrapper/params/zedm.yaml @@ -15,3 +15,18 @@ pos_tracking: sensors: sensors_timestamp_sync: false # Synchronize Sensors messages timestamp with latest received frame publish_imu_tf: true # publish `IMU -> _left_camera_frame` TF + +object_detection: + od_enabled: true # True to enable Object Detection [not available for ZED] + model: 3 # '0': MULTI_CLASS_BOX - '1': MULTI_CLASS_BOX_ACCURATE - '2': HUMAN_BODY_FAST - '3': HUMAN_BODY_ACCURATE - '4': MULTI_CLASS_BOX_MEDIUM - '5': HUMAN_BODY_MEDIUM - '6': PERSON_HEAD_BOX + confidence_threshold: 50 # Minimum value of the detection confidence of an object [0,100] + max_range: 15. # Maximum detection range + object_tracking_enabled: true # Enable/disable the tracking of the detected objects + body_fitting: false # Enable/disable body fitting for 'HUMAN_BODY_X' models + mc_people: true # Enable/disable the detection of persons for 'MULTI_CLASS_BOX_X' models + mc_vehicle: true # Enable/disable the detection of vehicles for 'MULTI_CLASS_BOX_X' models + mc_bag: true # Enable/disable the detection of bags for 'MULTI_CLASS_BOX_X' models + mc_animal: true # Enable/disable the detection of animals for 'MULTI_CLASS_BOX_X' models + mc_electronics: true # Enable/disable the detection of electronic devices for 'MULTI_CLASS_BOX_X' models + mc_fruit_vegetable: true # Enable/disable the detection of fruits and vegetables for 'MULTI_CLASS_BOX_X' models + mc_sport: true # Enable/disable the detection of sport-related objects for 'MULTI_CLASS_BOX_X' models From 845941a77d879359c3f38b36ed19b0ef720d4e61 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Thu, 24 Feb 2022 11:56:06 +0100 Subject: [PATCH 105/199] Update changelog --- latest_changes.md | 15 --------------- 1 file changed, 15 deletions(-) diff --git a/latest_changes.md b/latest_changes.md index 50491add..825c4d05 100644 --- a/latest_changes.md +++ b/latest_changes.md @@ -8,30 +8,15 @@ v3.7.x - Add support for sport-related object class - Add support for X_MEDIUM neural network models - Enable AI for ZED Mini - -2021-11-24 ----------- - Add new `_base_link` frame on the base of the camera to easily handle camera positioning on robots. Thx @civerachb-cpr - Improve URDF by adding 3° slope for ZED and ZED2, X-offset for optical frames to correctly match the CMOS sensors position on the PCB, X-offset for mounting screw on ZED2i - Add `zed_macro.urdf.xacro` to be included by other `xacro` file to easily integrate ZED cameras in the robot descriptions. See [PR #771](https://github.com/stereolabs/zed-ros-wrapper/pull/771) for details. Thx @civerachb-cpr - -2021-07-28 ----------- - New parameter `save_area_memory_db_on_exit` to force Area Memory saving when the node is closed and Area Memory is enabled and valid. - Add service `save_Area_map` to trigger an Area Memory saving. - New tool function to transform a relative path to absolute. - -2021-07-26 ----------- - Enabled static IMU TF broadcasting even it `publish_tf` is set to false, making the two options independent. Thx to @bjsowa - Moved the `zed_interfaces` folder in the new [`zed-ros-interfaces`](https://github.com/stereolabs/zed-ros-interfaces) repository. The new repository is useful to receive the topics from a ZED node on system where the `zed-ros-wrapper` repository cannot be fully installed, i.e. systems without CUDA support. For this repository nothing changes because the `zed_interfaces` folder is replaced by the `zed-ros-interfaces` git submodule to automatically satisfy all the dependencies. - -ZED2i support fix (2021-06-17) ------------------------------- - Fix sensors topics pubblication for ZED2i. The support for the new camera was not complete - -OpenNI mode fix (2021-06-15) ----------------------------- - Fix sensor_msgs type for depth image in OpenNI mode, from `sensor_msgs::image_encodings::mono16` to `sensor_msgs::image_encodings::TYPE_16UC1`. Depth image in OpenNI mode is now compatible with the nodelet `depthimage_to_laserscan` v3.5.x From be957f7e520814568d02c529731bce27c5df3db6 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Thu, 24 Feb 2022 11:56:41 +0100 Subject: [PATCH 106/199] Update README --- README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index 277e894c..52ecdf69 100644 --- a/README.md +++ b/README.md @@ -17,13 +17,13 @@ This package lets you use the ZED stereo camera with ROS. It outputs the camera ### Prerequisites - Ubuntu 20.04 -- [ZED SDK **≥ 3.5**](https://www.stereolabs.com/developers/) and its dependency [CUDA](https://developer.nvidia.com/cuda-downloads) +- [ZED SDK **≥ 3.7**](https://www.stereolabs.com/developers/) and its dependency [CUDA](https://developer.nvidia.com/cuda-downloads) - [ROS Noetic](http://wiki.ros.org/noetic/Installation/Ubuntu) or - Ubuntu 18.04 -- [ZED SDK **≥ 3.5**](https://www.stereolabs.com/developers/) and its dependency [CUDA](https://developer.nvidia.com/cuda-downloads) +- [ZED SDK **≥ 3.7**](https://www.stereolabs.com/developers/) and its dependency [CUDA](https://developer.nvidia.com/cuda-downloads) - [ROS Melodic](http://wiki.ros.org/melodic/Installation/Ubuntu) ### Build the repository From 177b4adcb5cb1d776884b624f8d6cdc078a59a18 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Fri, 4 Mar 2022 09:06:52 +0100 Subject: [PATCH 107/199] Update .gitmodules Point submodule to the `main` branch instead of the latest commit hash. Thx @matthew-reynolds --- .gitmodules | 1 + 1 file changed, 1 insertion(+) diff --git a/.gitmodules b/.gitmodules index d10030ea..718bdaee 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,3 +1,4 @@ [submodule "zed-ros-interfaces"] path = zed-ros-interfaces url = https://github.com/stereolabs/zed-ros-interfaces.git + branch = main From 1299a1c9fd454194f9a36d57d4075af185d603fe Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Fri, 4 Mar 2022 09:10:45 +0100 Subject: [PATCH 108/199] Update zed-ros-interface to latest HEAD --- zed-ros-interfaces | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/zed-ros-interfaces b/zed-ros-interfaces index 92bbf60f..d27ba2f4 160000 --- a/zed-ros-interfaces +++ b/zed-ros-interfaces @@ -1 +1 @@ -Subproject commit 92bbf60f2cab850f1b0147f3eaa2213c07e58c5c +Subproject commit d27ba2f4eb19b48f84bc705cfc6ea994f5189a8e From 1783bb42d516085adde635f769364881ddec2e1a Mon Sep 17 00:00:00 2001 From: Owen Burns Date: Mon, 14 Mar 2022 10:31:08 -0400 Subject: [PATCH 109/199] fix spelling mistake in common.yaml changed pubblication to publication --- zed_wrapper/params/common.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/zed_wrapper/params/common.yaml b/zed_wrapper/params/common.yaml index 1088e091..fb8cd5ff 100644 --- a/zed_wrapper/params/common.yaml +++ b/zed_wrapper/params/common.yaml @@ -61,7 +61,7 @@ pos_tracking: fixed_z_value: 0.00 # Value to be used for Z coordinate if `two_d_mode` is true mapping: - mapping_enabled: false # True to enable mapping and fused point cloud pubblication + mapping_enabled: false # True to enable mapping and fused point cloud publication resolution: 0.05 # maps resolution in meters [0.01f, 0.2f] max_mapping_range: -1 # maximum depth range while mapping in meters (-1 for automatic calculation) [2.0, 20.0] fused_pointcloud_freq: 1.0 # frequency of the publishing of the fused colored point cloud From d02d2032ced3e99ff0a586cd0b0e9c96f121290b Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Wed, 16 Mar 2022 11:30:37 +0100 Subject: [PATCH 110/199] fix baro and mag frame links for ZED2i --- zed_wrapper/params/zed2.yaml | 2 +- zed_wrapper/params/zed2i.yaml | 2 +- zed_wrapper/params/zedm.yaml | 4 ++-- zed_wrapper/urdf/zed_macro.urdf.xacro | 2 +- 4 files changed, 5 insertions(+), 5 deletions(-) diff --git a/zed_wrapper/params/zed2.yaml b/zed_wrapper/params/zed2.yaml index c22c77e4..82dff83f 100644 --- a/zed_wrapper/params/zed2.yaml +++ b/zed_wrapper/params/zed2.yaml @@ -18,7 +18,7 @@ sensors: object_detection: od_enabled: false # True to enable Object Detection [not available for ZED] - model: 2 # '0': MULTI_CLASS_BOX - '1': MULTI_CLASS_BOX_ACCURATE - '2': HUMAN_BODY_FAST - '3': HUMAN_BODY_ACCURATE - '4': MULTI_CLASS_BOX_MEDIUM - '5': HUMAN_BODY_MEDIUM - '6': PERSON_HEAD_BOX + model: 0 # '0': MULTI_CLASS_BOX - '1': MULTI_CLASS_BOX_ACCURATE - '2': HUMAN_BODY_FAST - '3': HUMAN_BODY_ACCURATE - '4': MULTI_CLASS_BOX_MEDIUM - '5': HUMAN_BODY_MEDIUM - '6': PERSON_HEAD_BOX confidence_threshold: 50 # Minimum value of the detection confidence of an object [0,100] max_range: 15. # Maximum detection range object_tracking_enabled: true # Enable/disable the tracking of the detected objects diff --git a/zed_wrapper/params/zed2i.yaml b/zed_wrapper/params/zed2i.yaml index 73312413..338d7066 100644 --- a/zed_wrapper/params/zed2i.yaml +++ b/zed_wrapper/params/zed2i.yaml @@ -18,7 +18,7 @@ sensors: object_detection: od_enabled: false # True to enable Object Detection [not available for ZED] - model: 1 # '0': MULTI_CLASS_BOX - '1': MULTI_CLASS_BOX_ACCURATE - '2': HUMAN_BODY_FAST - '3': HUMAN_BODY_ACCURATE - '4': MULTI_CLASS_BOX_MEDIUM - '5': HUMAN_BODY_MEDIUM - '6': PERSON_HEAD_BOX + model: 0 # '0': MULTI_CLASS_BOX - '1': MULTI_CLASS_BOX_ACCURATE - '2': HUMAN_BODY_FAST - '3': HUMAN_BODY_ACCURATE - '4': MULTI_CLASS_BOX_MEDIUM - '5': HUMAN_BODY_MEDIUM - '6': PERSON_HEAD_BOX confidence_threshold: 50 # Minimum value of the detection confidence of an object [0,100] max_range: 15. # Maximum detection range object_tracking_enabled: true # Enable/disable the tracking of the detected objects diff --git a/zed_wrapper/params/zedm.yaml b/zed_wrapper/params/zedm.yaml index 8f4f04fb..5b3db228 100644 --- a/zed_wrapper/params/zedm.yaml +++ b/zed_wrapper/params/zedm.yaml @@ -17,8 +17,8 @@ sensors: publish_imu_tf: true # publish `IMU -> _left_camera_frame` TF object_detection: - od_enabled: true # True to enable Object Detection [not available for ZED] - model: 3 # '0': MULTI_CLASS_BOX - '1': MULTI_CLASS_BOX_ACCURATE - '2': HUMAN_BODY_FAST - '3': HUMAN_BODY_ACCURATE - '4': MULTI_CLASS_BOX_MEDIUM - '5': HUMAN_BODY_MEDIUM - '6': PERSON_HEAD_BOX + od_enabled: false # True to enable Object Detection [not available for ZED] + model: 0 # '0': MULTI_CLASS_BOX - '1': MULTI_CLASS_BOX_ACCURATE - '2': HUMAN_BODY_FAST - '3': HUMAN_BODY_ACCURATE - '4': MULTI_CLASS_BOX_MEDIUM - '5': HUMAN_BODY_MEDIUM - '6': PERSON_HEAD_BOX confidence_threshold: 50 # Minimum value of the detection confidence of an object [0,100] max_range: 15. # Maximum detection range object_tracking_enabled: true # Enable/disable the tracking of the detected objects diff --git a/zed_wrapper/urdf/zed_macro.urdf.xacro b/zed_wrapper/urdf/zed_macro.urdf.xacro index 8d07dbd2..510d60e0 100644 --- a/zed_wrapper/urdf/zed_macro.urdf.xacro +++ b/zed_wrapper/urdf/zed_macro.urdf.xacro @@ -125,7 +125,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - + From 962a77216c91ff130ca3bfe1061b2a9698ee7beb Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Wed, 16 Mar 2022 11:33:44 +0100 Subject: [PATCH 111/199] Update latest_changes.md --- latest_changes.md | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/latest_changes.md b/latest_changes.md index 825c4d05..e6d51eff 100644 --- a/latest_changes.md +++ b/latest_changes.md @@ -1,6 +1,10 @@ LATEST CHANGES ============== +2022-03-16 +----------- +- Fix the frame links of barometer, magnetometer, and temperature sensors for ZED2i + v3.7.x --------- - Add support for the new Neural Depth mode From 57cc33f2fa4bff709e29c5ff20571118ac3f268f Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Mon, 28 Mar 2022 16:46:08 +0200 Subject: [PATCH 112/199] Add sensors rate parameter --- latest_changes.md | 4 + .../include/zed_wrapper_nodelet.hpp | 21 ++-- .../zed_nodelet/src/zed_wrapper_nodelet.cpp | 102 +++++++++++++----- zed_wrapper/params/zed2.yaml | 1 + zed_wrapper/params/zed2i.yaml | 1 + zed_wrapper/params/zedm.yaml | 1 + 6 files changed, 92 insertions(+), 38 deletions(-) diff --git a/latest_changes.md b/latest_changes.md index e6d51eff..512cb0b5 100644 --- a/latest_changes.md +++ b/latest_changes.md @@ -1,6 +1,10 @@ LATEST CHANGES ============== +2022-03-28 +- Add parameter `sensors/max_pub_rate` to set the maximum publishing frequency of sensors data +- Improve Sensors thread + 2022-03-16 ----------- - Fix the frame links of barometer, magnetometer, and temperature sensors for ZED2i 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 bd6f3170..14c30e00 100644 --- a/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp +++ b/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp @@ -128,10 +128,15 @@ class ZEDWrapperNodelet : public nodelet::Nodelet */ void pointcloud_thread_func(); + /*! \brief Sensors data publishing function + */ + void sensors_thread_func(); + /*! \brief Publish the pose of the camera in "Map" frame with a ros Publisher * \param t : the ros::Time to stamp the image */ - void publishPose(ros::Time t); + void + publishPose(ros::Time t); /*! \brief Publish the pose of the camera in "Odom" frame with a ros Publisher * \param base2odomTransf : Transformation representing the camera pose @@ -248,12 +253,7 @@ class ZEDWrapperNodelet : public nodelet::Nodelet * \param e : the ros::TimerEvent binded to the callback */ void callback_pubPath(const ros::TimerEvent& e); - - /*! \brief Callback to publish Sensor Data with a ROS publisher. - * \param e : the ros::TimerEvent binded to the callback - */ - void callback_pubSensorsData(const ros::TimerEvent& e); - + /*! \brief Callback to update node diagnostic status * \param stat : node status */ @@ -411,10 +411,9 @@ class ZEDWrapperNodelet : public nodelet::Nodelet ros::NodeHandle mNhNs; std::thread mDevicePollThread; std::thread mPcThread; // Point Cloud thread + std::thread mSensThread; // Sensors data thread - bool mStopNode = false; - - const double mSensPubRate = 400.0; // Maximum ODR for ZED2/ZED2i. You can change this to 800 for ZED-M, but it's not recommended + bool mStopNode = false; // Publishers image_transport::CameraPublisher mPubRgb; // @@ -454,7 +453,6 @@ class ZEDWrapperNodelet : public nodelet::Nodelet ros::Publisher mPubCamImuTransf; // Timers - ros::Timer mImuTimer; ros::Timer mPathTimer; ros::Timer mFusedPcTimer; ros::Timer mVideoDepthTimer; @@ -545,6 +543,7 @@ class ZEDWrapperNodelet : public nodelet::Nodelet std::string mSvoFilepath; std::string mRemoteStreamAddr; bool mSensTimestampSync; + double mSensPubRate = 400.0; double mPathPubRate; int mPathMaxCount; bool mVerbose; 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 17e11b46..e668885f 100644 --- a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp @@ -59,6 +59,10 @@ ZEDWrapperNodelet::~ZEDWrapperNodelet() mPcThread.join(); } + if (mSensThread.joinable()) { + mSensThread.join(); + } + std::cerr << "ZED Nodelet destroyed" << std::endl; } @@ -493,26 +497,26 @@ void ZEDWrapperNodelet::onInit() // Sensor publishers if (mZedRealCamModel != sl::MODEL::ZED) { // IMU Publishers - mPubImu = mNhNs.advertise(imu_topic, 1 /*static_cast(mSensPubRate)*/); + mPubImu = mNhNs.advertise(imu_topic, 1); NODELET_INFO_STREAM("Advertised on topic " << mPubImu.getTopic()); - mPubImuRaw = mNhNs.advertise(imu_topic_raw, 1 /*static_cast(mSensPubRate)*/); + mPubImuRaw = mNhNs.advertise(imu_topic_raw, 1); NODELET_INFO_STREAM("Advertised on topic " << mPubImuRaw.getTopic()); - mPubImuMag = mNhNs.advertise(imu_mag_topic, 1 /*MAG_FREQ*/); + mPubImuMag = mNhNs.advertise(imu_mag_topic, 1); NODELET_INFO_STREAM("Advertised on topic " << mPubImuMag.getTopic()); if (mZedRealCamModel == sl::MODEL::ZED2 || mZedRealCamModel == sl::MODEL::ZED2i) { // IMU temperature sensor - mPubImuTemp = mNhNs.advertise(imu_temp_topic, 1 /*static_cast(mSensPubRate)*/); + mPubImuTemp = mNhNs.advertise(imu_temp_topic, 1); NODELET_INFO_STREAM("Advertised on topic " << mPubImuTemp.getTopic()); // Atmospheric pressure - mPubPressure = mNhNs.advertise(pressure_topic, 1 /*static_cast(BARO_FREQ)*/); + mPubPressure = mNhNs.advertise(pressure_topic, 1); NODELET_INFO_STREAM("Advertised on topic " << mPubPressure.getTopic()); // CMOS sensor temperatures - mPubTempL = mNhNs.advertise(temp_topic_left, 1 /*static_cast(BARO_FREQ)*/); + mPubTempL = mNhNs.advertise(temp_topic_left, 1); NODELET_INFO_STREAM("Advertised on topic " << mPubTempL.getTopic()); - mPubTempR = mNhNs.advertise(temp_topic_right, 1 /*static_cast(BARO_FREQ)*/); + mPubTempR = mNhNs.advertise(temp_topic_right, 1); NODELET_INFO_STREAM("Advertised on topic " << mPubTempR.getTopic()); } @@ -545,8 +549,6 @@ void ZEDWrapperNodelet::onInit() if (!mSvoMode && !mSensTimestampSync) { mFrameTimestamp = ros::Time::now(); - mImuTimer = mNhNs.createTimer(ros::Duration(1.0 / (mSensPubRate * 1.5)), - &ZEDWrapperNodelet::callback_pubSensorsData, this); mSensPeriodMean_usec.reset(new sl_tools::CSmartMean(mSensPubRate / 2)); } else { mSensPeriodMean_usec.reset(new sl_tools::CSmartMean(mCamFrameRate / 2)); @@ -582,6 +584,9 @@ void ZEDWrapperNodelet::onInit() // Start data publishing timer mVideoDepthTimer = mNhNs.createTimer(ros::Duration(1.0 / mVideoDepthFreq), &ZEDWrapperNodelet::callback_pubVideoDepth, this); + + // Start Sensors thread + mSensThread = std::thread(&ZEDWrapperNodelet::sensors_thread_func, this); } void ZEDWrapperNodelet::readParameters() @@ -790,8 +795,22 @@ void ZEDWrapperNodelet::readParameters() NODELET_INFO_STREAM("*** SENSORS PARAMETERS ***"); // ----> Sensors - mNhNs.getParam("sensors/sensors_timestamp_sync", mSensTimestampSync); - NODELET_INFO_STREAM(" * Sensors timestamp sync\t-> " << (mSensTimestampSync ? "ENABLED" : "DISABLED")); + if (camera_model != "zed") { + mNhNs.getParam("sensors/sensors_timestamp_sync", mSensTimestampSync); + NODELET_INFO_STREAM(" * Sensors timestamp sync\t-> " << (mSensTimestampSync ? "ENABLED" : "DISABLED")); + mNhNs.getParam("sensors/max_pub_rate", mSensPubRate); + if (camera_model == "zedm") { + if (mSensPubRate > 800.) + mSensPubRate = 800.; + } else { + if (mSensPubRate > 400.) + mSensPubRate = 400.; + } + + NODELET_INFO_STREAM(" * Max sensors rate\t\t-> " << mSensPubRate); + } else { + NODELET_INFO_STREAM(" * The ZED camera has no available inertial/environmental sensors."); + } // <---- Sensors NODELET_INFO_STREAM("*** SVO PARAMETERS ***"); @@ -2763,16 +2782,44 @@ void ZEDWrapperNodelet::callback_pubPath(const ros::TimerEvent& e) } } -void ZEDWrapperNodelet::callback_pubSensorsData(const ros::TimerEvent& e) +void ZEDWrapperNodelet::sensors_thread_func() { - // NODELET_INFO("callback_pubSensorsData"); - std::lock_guard lock(mCloseZedMutex); + ros::Rate loop_rate(mSensPubRate); - if (!mZed.isOpened()) { - return; + std::chrono::steady_clock::time_point prev_usec = std::chrono::steady_clock::now(); + + int count_warn = 0; + + while (!mStopNode) { + std::chrono::steady_clock::time_point start_elab = std::chrono::steady_clock::now(); + + mCloseZedMutex.lock(); + if (!mZed.isOpened()) { + mCloseZedMutex.unlock(); + loop_rate.sleep(); + continue; + } + + publishSensData(); + mCloseZedMutex.unlock(); + + if (!loop_rate.sleep()) { + if (++count_warn > 10) { + NODELET_INFO_THROTTLE(1.0, "Sensors thread is not synchronized with the Sensors rate"); + NODELET_INFO_STREAM_THROTTLE(1.0, "Expected cycle time: " << loop_rate.expectedCycleTime() << " - Real cycle time: " << loop_rate.cycleTime()); + NODELET_WARN_STREAM_THROTTLE(10.0, "Sensors data publishing takes longer (" << loop_rate.cycleTime() << " sec) than requested by the Sensors rate (" << loop_rate.expectedCycleTime() << " sec). Please consider to " + "lower the 'max_pub_rate' setting or to " + "reduce the power requirements reducing " + "the resolutions."); + } + + loop_rate.reset(); + } else { + count_warn = 0; + } } - publishSensData(); + NODELET_DEBUG("Sensors thread finished"); } void ZEDWrapperNodelet::publishSensData(ros::Time t) @@ -2892,9 +2939,9 @@ void ZEDWrapperNodelet::publishSensData(ros::Time t) sensors_data_published = true; mPubImuTemp.publish(imuTempMsg); - } else { + } /*else { NODELET_DEBUG("No new IMU temp."); - } + }*/ if (sens_data.barometer.is_available && new_baro_data) { lastTs_baro = ts_baro; @@ -2960,9 +3007,9 @@ void ZEDWrapperNodelet::publishSensData(ros::Time t) sensors_data_published = true; mPubTempR.publish(tempRightMsg); } - } else { + } /*else { NODELET_DEBUG("No new BAROM. DATA"); - } + }*/ if (imu_MagSubNumber > 0) { if (sens_data.magnetometer.is_available && new_mag_data) { @@ -2997,9 +3044,9 @@ void ZEDWrapperNodelet::publishSensData(ros::Time t) sensors_data_published = true; mPubImuMag.publish(magMsg); } - } else { + } /*else { NODELET_DEBUG("No new MAG. DATA"); - } + }*/ if (imu_SubNumber > 0 && new_imu_data) { lastTs_imu = ts_imu; @@ -3059,9 +3106,9 @@ void ZEDWrapperNodelet::publishSensData(ros::Time t) sensors_data_published = true; mPubImu.publish(imuMsg); - } else { + } /*else { NODELET_DEBUG("No new IMU DATA"); - } + }*/ if (imu_RawSubNumber > 0 && new_imu_data) { lastTs_imu = ts_imu; @@ -3248,13 +3295,13 @@ void ZEDWrapperNodelet::device_poll_thread_func() runParams.enable_depth = false; // Ask to not compute the depth } + std::chrono::steady_clock::time_point start_elab = std::chrono::steady_clock::now(); + mCamDataMutex.lock(); mRgbDepthDataRetrieved = false; mGrabStatus = mZed.grab(runParams); mCamDataMutex.unlock(); - std::chrono::steady_clock::time_point start_elab = std::chrono::steady_clock::now(); - // cout << toString(grab_status) << endl; if (mGrabStatus != sl::ERROR_CODE::SUCCESS) { // Detect if a error occurred (for example: @@ -3793,6 +3840,7 @@ void ZEDWrapperNodelet::device_poll_thread_func() mRecording = false; mZed.disableRecording(); } + mStopNode = true; mZed.close(); NODELET_DEBUG("ZED pool thread finished"); diff --git a/zed_wrapper/params/zed2.yaml b/zed_wrapper/params/zed2.yaml index 82dff83f..8e355398 100644 --- a/zed_wrapper/params/zed2.yaml +++ b/zed_wrapper/params/zed2.yaml @@ -14,6 +14,7 @@ pos_tracking: sensors: sensors_timestamp_sync: false # Synchronize Sensors messages timestamp with latest received frame + 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: diff --git a/zed_wrapper/params/zed2i.yaml b/zed_wrapper/params/zed2i.yaml index 338d7066..a49a3981 100644 --- a/zed_wrapper/params/zed2i.yaml +++ b/zed_wrapper/params/zed2i.yaml @@ -14,6 +14,7 @@ pos_tracking: sensors: sensors_timestamp_sync: false # Synchronize Sensors messages timestamp with latest received frame + 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: diff --git a/zed_wrapper/params/zedm.yaml b/zed_wrapper/params/zedm.yaml index 5b3db228..b522f554 100644 --- a/zed_wrapper/params/zedm.yaml +++ b/zed_wrapper/params/zedm.yaml @@ -14,6 +14,7 @@ pos_tracking: sensors: sensors_timestamp_sync: false # Synchronize Sensors messages timestamp with latest received frame + max_pub_rate: 200. # max frequency of publishing of sensors data. MAX: 800. - MIN: grab rate publish_imu_tf: true # publish `IMU -> _left_camera_frame` TF object_detection: From eb4ac39aed611fadc03485858c3e826d8f85cd64 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Wed, 30 Mar 2022 10:21:57 +0200 Subject: [PATCH 113/199] Fix behavior --- .../src/zed_nodelet/src/zed_wrapper_nodelet.cpp | 17 +++++++++-------- 1 file changed, 9 insertions(+), 8 deletions(-) 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 e668885f..4b61c07b 100644 --- a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp @@ -1240,17 +1240,17 @@ bool ZEDWrapperNodelet::on_set_pose(zed_interfaces::set_pose::Request& req, zed_ mInitialBasePose[4] = req.P; mInitialBasePose[5] = req.Y; - if (!set_pose(mInitialBasePose[0], mInitialBasePose[1], mInitialBasePose[2], mInitialBasePose[3], mInitialBasePose[4], + /*if (!set_pose(mInitialBasePose[0], mInitialBasePose[1], mInitialBasePose[2], mInitialBasePose[3], mInitialBasePose[4], mInitialBasePose[5])) { res.done = false; return false; - } + }*/ std::lock_guard lock(mPosTrkMutex); // Disable tracking - mPosTrackingActivated = false; - mZed.disablePositionalTracking(); + //mPosTrackingActivated = false; + //mZed.disablePositionalTracking(); // Restart tracking start_pos_tracking(); @@ -1267,17 +1267,17 @@ bool ZEDWrapperNodelet::on_reset_tracking(zed_interfaces::reset_tracking::Reques return false; } - if (!set_pose(mInitialBasePose[0], mInitialBasePose[1], mInitialBasePose[2], mInitialBasePose[3], mInitialBasePose[4], + /*if (!set_pose(mInitialBasePose[0], mInitialBasePose[1], mInitialBasePose[2], mInitialBasePose[3], mInitialBasePose[4], mInitialBasePose[5])) { res.reset_done = false; return false; - } + }*/ std::lock_guard lock(mPosTrkMutex); // Disable tracking - mPosTrackingActivated = false; - mZed.disablePositionalTracking(); + //mPosTrackingActivated = false; + //mZed.disablePositionalTracking(); // Restart tracking start_pos_tracking(); @@ -1470,6 +1470,7 @@ void ZEDWrapperNodelet::start_pos_tracking() NODELET_INFO(" * Waiting for valid static transformations..."); + mPosTrackingReady = false; bool transformOk = false; double elapsed = 0.0; From 153fbe05c21c16d8d4534a66a065b261e8cd2e81 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Wed, 30 Mar 2022 10:34:18 +0200 Subject: [PATCH 114/199] update changelog --- latest_changes.md | 3 +++ .../zed_nodelet/src/zed_wrapper_nodelet.cpp | 22 +------------------ 2 files changed, 4 insertions(+), 21 deletions(-) diff --git a/latest_changes.md b/latest_changes.md index 512cb0b5..fad6528b 100644 --- a/latest_changes.md +++ b/latest_changes.md @@ -1,6 +1,9 @@ LATEST CHANGES ============== +2022-03-30 +- Fix wrong TP broadcasting when calling the `set_pose`, `reset_tracking`, and `reset_odometry` services + 2022-03-28 - Add parameter `sensors/max_pub_rate` to set the maximum publishing frequency of sensors data - Improve Sensors thread 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 4b61c07b..fb46cf3a 100644 --- a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp @@ -1240,18 +1240,8 @@ bool ZEDWrapperNodelet::on_set_pose(zed_interfaces::set_pose::Request& req, zed_ mInitialBasePose[4] = req.P; mInitialBasePose[5] = req.Y; - /*if (!set_pose(mInitialBasePose[0], mInitialBasePose[1], mInitialBasePose[2], mInitialBasePose[3], mInitialBasePose[4], - mInitialBasePose[5])) { - res.done = false; - return false; - }*/ - std::lock_guard lock(mPosTrkMutex); - // Disable tracking - //mPosTrackingActivated = false; - //mZed.disablePositionalTracking(); - // Restart tracking start_pos_tracking(); @@ -1267,18 +1257,8 @@ bool ZEDWrapperNodelet::on_reset_tracking(zed_interfaces::reset_tracking::Reques return false; } - /*if (!set_pose(mInitialBasePose[0], mInitialBasePose[1], mInitialBasePose[2], mInitialBasePose[3], mInitialBasePose[4], - mInitialBasePose[5])) { - res.reset_done = false; - return false; - }*/ - std::lock_guard lock(mPosTrkMutex); - // Disable tracking - //mPosTrackingActivated = false; - //mZed.disablePositionalTracking(); - // Restart tracking start_pos_tracking(); @@ -1470,7 +1450,7 @@ void ZEDWrapperNodelet::start_pos_tracking() NODELET_INFO(" * Waiting for valid static transformations..."); - mPosTrackingReady = false; + mPosTrackingReady = false; // Useful to not publish wrong TF with IMU frame broadcasting bool transformOk = false; double elapsed = 0.0; From 5d87f1fe3b499b1bb10ba1e4c541a43e2ecfb9d7 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Wed, 30 Mar 2022 14:41:10 +0200 Subject: [PATCH 115/199] Update latest_changes.md --- latest_changes.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/latest_changes.md b/latest_changes.md index fad6528b..668f0eb5 100644 --- a/latest_changes.md +++ b/latest_changes.md @@ -2,7 +2,7 @@ LATEST CHANGES ============== 2022-03-30 -- Fix wrong TP broadcasting when calling the `set_pose`, `reset_tracking`, and `reset_odometry` services +- Fix wrong TF broadcasting when calling the `set_pose`, `reset_tracking`, and `reset_odometry` services. Now the initial odometry is coherent with the new starting point. 2022-03-28 - Add parameter `sensors/max_pub_rate` to set the maximum publishing frequency of sensors data From 5301a3a350cfb4988e603972e46389a449a41ddf Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Wed, 30 Mar 2022 17:58:16 +0200 Subject: [PATCH 116/199] Handle Rviz `/clicked_point` --- zed_nodelets/CMakeLists.txt | 1 + zed_nodelets/package.xml | 1 + .../include/zed_wrapper_nodelet.hpp | 966 +++++++++--------- .../zed_nodelet/src/zed_wrapper_nodelet.cpp | 28 + zed_wrapper/params/common.yaml | 3 +- 5 files changed, 518 insertions(+), 481 deletions(-) diff --git a/zed_nodelets/CMakeLists.txt b/zed_nodelets/CMakeLists.txt index 17c67fd5..741c1b41 100644 --- a/zed_nodelets/CMakeLists.txt +++ b/zed_nodelets/CMakeLists.txt @@ -72,6 +72,7 @@ catkin_package( tf2_geometry_msgs message_runtime zed_interfaces + geometry_msgs ) ############################################################################### diff --git a/zed_nodelets/package.xml b/zed_nodelets/package.xml index 6b7367cf..8373c4a8 100644 --- a/zed_nodelets/package.xml +++ b/zed_nodelets/package.xml @@ -25,6 +25,7 @@ dynamic_reconfigure nodelet diagnostic_updater + geometry_msgs zed_interfaces 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 14c30e00..394f9e8e 100644 --- a/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp +++ b/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp @@ -23,6 +23,7 @@ #include #include +#include #include #include #include @@ -44,6 +45,7 @@ #include #include #include +#include #include #include #include @@ -55,8 +57,6 @@ #include #include #include -#include -#include // Topics #include @@ -79,94 +79,91 @@ using namespace std; -namespace zed_nodelets -{ -class ZEDWrapperNodelet : public nodelet::Nodelet -{ - typedef enum _dyn_params - { - DATAPUB_FREQ = 0, - CONFIDENCE = 1, - TEXTURE_CONF = 2, - POINTCLOUD_FREQ = 3, - BRIGHTNESS = 4, - CONTRAST = 5, - HUE = 6, - SATURATION = 7, - SHARPNESS = 8, - GAMMA = 9, - AUTO_EXP_GAIN = 10, - GAIN = 11, - EXPOSURE = 12, - AUTO_WB = 13, - WB_TEMP = 14 - } DynParams; +namespace zed_nodelets { +class ZEDWrapperNodelet : public nodelet::Nodelet { + typedef enum _dyn_params { + DATAPUB_FREQ = 0, + CONFIDENCE = 1, + TEXTURE_CONF = 2, + POINTCLOUD_FREQ = 3, + BRIGHTNESS = 4, + CONTRAST = 5, + HUE = 6, + SATURATION = 7, + SHARPNESS = 8, + GAMMA = 9, + AUTO_EXP_GAIN = 10, + GAIN = 11, + EXPOSURE = 12, + AUTO_WB = 13, + WB_TEMP = 14 + } DynParams; public: - /*! \brief Default constructor + /*! \brief Default constructor */ - ZEDWrapperNodelet(); + ZEDWrapperNodelet(); - /*! \brief \ref ZEDWrapperNodelet destructor + /*! \brief \ref ZEDWrapperNodelet destructor */ - virtual ~ZEDWrapperNodelet(); + virtual ~ZEDWrapperNodelet(); protected: - /*! \brief Initialization function called by the Nodelet base class + /*! \brief Initialization function called by the Nodelet base class */ - virtual void onInit(); + virtual void onInit(); - /*! \brief Reads parameters from the param server + /*! \brief Reads parameters from the param server */ - void readParameters(); + void readParameters(); - /*! \brief ZED camera polling thread function + /*! \brief ZED camera polling thread function */ - void device_poll_thread_func(); + void device_poll_thread_func(); - /*! \brief Pointcloud publishing function + /*! \brief Pointcloud publishing function */ - void pointcloud_thread_func(); + void pointcloud_thread_func(); - /*! \brief Sensors data publishing function + /*! \brief Sensors data publishing function */ - void sensors_thread_func(); + void sensors_thread_func(); - /*! \brief Publish the pose of the camera in "Map" frame with a ros Publisher + /*! \brief Publish the pose of the camera in "Map" frame with a ros Publisher * \param t : the ros::Time to stamp the image */ - void - publishPose(ros::Time t); + void + publishPose(ros::Time t); - /*! \brief Publish the pose of the camera in "Odom" frame with a ros Publisher + /*! \brief Publish the pose of the camera in "Odom" frame with a ros Publisher * \param base2odomTransf : Transformation representing the camera pose * from base frame to odom frame * \param slPose : latest odom pose from ZED SDK * \param t : the ros::Time to stamp the image */ - void publishOdom(tf2::Transform odom2baseTransf, sl::Pose& slPose, ros::Time t); + void publishOdom(tf2::Transform odom2baseTransf, sl::Pose& slPose, ros::Time t); - /*! \brief Publish the pose of the camera in "Map" frame as a transformation + /*! \brief Publish the pose of the camera in "Map" frame as a transformation * \param baseTransform : Transformation representing the camera pose from * odom frame to map frame * \param t : the ros::Time to stamp the image */ - void publishPoseFrame(tf2::Transform baseTransform, ros::Time t); + void publishPoseFrame(tf2::Transform baseTransform, ros::Time t); - /*! \brief Publish the odometry of the camera in "Odom" frame as a + /*! \brief Publish the odometry of the camera in "Odom" frame as a * transformation * \param odomTransf : Transformation representing the camera pose from * base frame to odom frame * \param t : the ros::Time to stamp the image */ - void publishOdomFrame(tf2::Transform odomTransf, ros::Time t); + void publishOdomFrame(tf2::Transform odomTransf, ros::Time t); - /*! + /*! * \brief Publish IMU frame once as static TF */ - void publishStaticImuFrame(); + void publishStaticImuFrame(); - /*! \brief Publish a sl::Mat image with a ros Publisher + /*! \brief Publish a sl::Mat image with a ros Publisher * \param imgMsgPtr : the image message to publish * \param img : the image to publish * \param pubImg : the publisher object to use (different image publishers @@ -176,43 +173,43 @@ class ZEDWrapperNodelet : public nodelet::Nodelet * image frames exist) * \param t : the ros::Time to stamp the image */ - void publishImage(sensor_msgs::ImagePtr imgMsgPtr, sl::Mat img, image_transport::CameraPublisher& pubImg, - sensor_msgs::CameraInfoPtr camInfoMsg, string imgFrameId, ros::Time t); + void publishImage(sensor_msgs::ImagePtr imgMsgPtr, sl::Mat img, image_transport::CameraPublisher& pubImg, + sensor_msgs::CameraInfoPtr camInfoMsg, string imgFrameId, ros::Time t); - /*! \brief Publish a sl::Mat depth image with a ros Publisher + /*! \brief Publish a sl::Mat depth image with a ros Publisher * \param imgMsgPtr : the depth image topic message to publish * \param depth : the depth image to publish * \param t : the ros::Time to stamp the depth image */ - void publishDepth(sensor_msgs::ImagePtr imgMsgPtr, sl::Mat depth, ros::Time t); + void publishDepth(sensor_msgs::ImagePtr imgMsgPtr, sl::Mat depth, ros::Time t); - /*! \brief Publish a single pointCloud with a ros Publisher + /*! \brief Publish a single pointCloud with a ros Publisher */ - void publishPointCloud(); + void publishPointCloud(); - /*! \brief Publish a fused pointCloud with a ros Publisher + /*! \brief Publish a fused pointCloud with a ros Publisher */ - void callback_pubFusedPointCloud(const ros::TimerEvent& e); + void callback_pubFusedPointCloud(const ros::TimerEvent& e); - /*! \brief Publish the informations of a camera with a ros Publisher + /*! \brief Publish the informations of a camera with a ros Publisher * \param cam_info_msg : the information message to publish * \param pub_cam_info : the publisher object to use * \param t : the ros::Time to stamp the message */ - void publishCamInfo(sensor_msgs::CameraInfoPtr camInfoMsg, ros::Publisher pubCamInfo, ros::Time t); + void publishCamInfo(sensor_msgs::CameraInfoPtr camInfoMsg, ros::Publisher pubCamInfo, ros::Time t); - /*! \brief Publish a sl::Mat disparity image with a ros Publisher + /*! \brief Publish a sl::Mat disparity image with a ros Publisher * \param disparity : the disparity image to publish * \param t : the ros::Time to stamp the depth image */ - void publishDisparity(sl::Mat disparity, ros::Time t); + void publishDisparity(sl::Mat disparity, ros::Time t); - /*! \brief Publish sensors data and TF + /*! \brief Publish sensors data and TF * \param t : the ros::Time to stamp the depth image */ - void publishSensData(ros::Time t = ros::Time(0)); + void publishSensData(ros::Time t = ros::Time(0)); - /*! \brief Get the information of the ZED cameras and store them in an + /*! \brief Get the information of the ZED cameras and store them in an * information message * \param zed : the sl::zed::Camera* pointer to an instance * \param left_cam_info_msg : the information message to fill with the left @@ -222,509 +219,518 @@ class ZEDWrapperNodelet : public nodelet::Nodelet * \param left_frame_id : the id of the reference frame of the left camera * \param right_frame_id : the id of the reference frame of the right camera */ - void fillCamInfo(sl::Camera& zed, sensor_msgs::CameraInfoPtr leftCamInfoMsg, - sensor_msgs::CameraInfoPtr rightCamInfoMsg, string leftFrameId, string rightFrameId, - bool rawParam = false); + void fillCamInfo(sl::Camera& zed, sensor_msgs::CameraInfoPtr leftCamInfoMsg, + sensor_msgs::CameraInfoPtr rightCamInfoMsg, string leftFrameId, string rightFrameId, + bool rawParam = false); - /*! \brief Get the information of the ZED cameras and store them in an + /*! \brief Get the information of the ZED cameras and store them in an * information message for depth topics * \param zed : the sl::zed::Camera* pointer to an instance * \param depth_info_msg : the information message to fill with the left * camera informations * \param frame_id : the id of the reference frame of the left camera */ - void fillCamDepthInfo(sl::Camera& zed, sensor_msgs::CameraInfoPtr depth_info_msg, string frame_id); + void fillCamDepthInfo(sl::Camera& zed, sensor_msgs::CameraInfoPtr depth_info_msg, string frame_id); - /*! \brief Check if FPS and Resolution chosen by user are correct. + /*! \brief Check if FPS and Resolution chosen by user are correct. * Modifies FPS to match correct value. */ - void checkResolFps(); + void checkResolFps(); - /*! \brief Callback to handle dynamic reconfigure events in ROS + /*! \brief Callback to handle dynamic reconfigure events in ROS */ - void callback_dynamicReconf(zed_nodelets::ZedConfig& config, uint32_t level); + void callback_dynamicReconf(zed_nodelets::ZedConfig& config, uint32_t level); - /*! \brief Callback to publish Video and Depth data + /*! \brief Callback to publish Video and Depth data * \param e : the ros::TimerEvent binded to the callback */ - void callback_pubVideoDepth(const ros::TimerEvent& e); + void callback_pubVideoDepth(const ros::TimerEvent& e); - /*! \brief Callback to publish Path data with a ROS publisher. + /*! \brief Callback to publish Path data with a ROS publisher. * \param e : the ros::TimerEvent binded to the callback */ - void callback_pubPath(const ros::TimerEvent& e); - - /*! \brief Callback to update node diagnostic status + void callback_pubPath(const ros::TimerEvent& e); + + /*! \brief Callback to update node diagnostic status * \param stat : node status */ - void callback_updateDiagnostic(diagnostic_updater::DiagnosticStatusWrapper& stat); + void callback_updateDiagnostic(diagnostic_updater::DiagnosticStatusWrapper& stat); + + /*! \brief Callback to receive geometry_msgs::PointStamped topics + * \param msg : pointer to the received message + */ + void clickedPtCallback(geometry_msgs::PointStampedConstPtr msg); - /*! \brief Service callback to reset_tracking service + /*! \brief Service callback to reset_tracking service * Tracking pose is reinitialized to the value available in the ROS Param * server */ - bool on_reset_tracking(zed_interfaces::reset_tracking::Request& req, zed_interfaces::reset_tracking::Response& res); + bool on_reset_tracking(zed_interfaces::reset_tracking::Request& req, zed_interfaces::reset_tracking::Response& res); - /*! \brief Service callback to reset_odometry service + /*! \brief Service callback to reset_odometry service * Odometry is reset to clear drift and odometry frame gets the latest * pose * from ZED tracking. */ - bool on_reset_odometry(zed_interfaces::reset_odometry::Request& req, zed_interfaces::reset_odometry::Response& res); + bool on_reset_odometry(zed_interfaces::reset_odometry::Request& req, zed_interfaces::reset_odometry::Response& res); - /*! \brief Service callback to set_pose service + /*! \brief Service callback to set_pose service * Tracking pose is set to the new values */ - bool on_set_pose(zed_interfaces::set_pose::Request& req, zed_interfaces::set_pose::Response& res); + bool on_set_pose(zed_interfaces::set_pose::Request& req, zed_interfaces::set_pose::Response& res); - /*! \brief Service callback to start_svo_recording service + /*! \brief Service callback to start_svo_recording service */ - bool on_start_svo_recording(zed_interfaces::start_svo_recording::Request& req, - zed_interfaces::start_svo_recording::Response& res); + bool on_start_svo_recording(zed_interfaces::start_svo_recording::Request& req, + zed_interfaces::start_svo_recording::Response& res); - /*! \brief Service callback to stop_svo_recording service + /*! \brief Service callback to stop_svo_recording service */ - bool on_stop_svo_recording(zed_interfaces::stop_svo_recording::Request& req, - zed_interfaces::stop_svo_recording::Response& res); + bool on_stop_svo_recording(zed_interfaces::stop_svo_recording::Request& req, + zed_interfaces::stop_svo_recording::Response& res); - /*! \brief Service callback to start_remote_stream service + /*! \brief Service callback to start_remote_stream service */ - bool on_start_remote_stream(zed_interfaces::start_remote_stream::Request& req, - zed_interfaces::start_remote_stream::Response& res); + bool on_start_remote_stream(zed_interfaces::start_remote_stream::Request& req, + zed_interfaces::start_remote_stream::Response& res); - /*! \brief Service callback to stop_remote_stream service + /*! \brief Service callback to stop_remote_stream service */ - bool on_stop_remote_stream(zed_interfaces::stop_remote_stream::Request& req, - zed_interfaces::stop_remote_stream::Response& res); + bool on_stop_remote_stream(zed_interfaces::stop_remote_stream::Request& req, + zed_interfaces::stop_remote_stream::Response& res); - /*! \brief Service callback to set_led_status service + /*! \brief Service callback to set_led_status service */ - bool on_set_led_status(zed_interfaces::set_led_status::Request& req, zed_interfaces::set_led_status::Response& res); + bool on_set_led_status(zed_interfaces::set_led_status::Request& req, zed_interfaces::set_led_status::Response& res); - /*! \brief Service callback to toggle_led service + /*! \brief Service callback to toggle_led service */ - bool on_toggle_led(zed_interfaces::toggle_led::Request& req, zed_interfaces::toggle_led::Response& res); + bool on_toggle_led(zed_interfaces::toggle_led::Request& req, zed_interfaces::toggle_led::Response& res); - /*! \brief Service callback to start_3d_mapping service + /*! \brief Service callback to start_3d_mapping service */ - bool on_start_3d_mapping(zed_interfaces::start_3d_mapping::Request& req, - zed_interfaces::start_3d_mapping::Response& res); + bool on_start_3d_mapping(zed_interfaces::start_3d_mapping::Request& req, + zed_interfaces::start_3d_mapping::Response& res); - /*! \brief Service callback to stop_3d_mapping service + /*! \brief Service callback to stop_3d_mapping service */ - bool on_stop_3d_mapping(zed_interfaces::stop_3d_mapping::Request& req, - zed_interfaces::stop_3d_mapping::Response& res); + bool on_stop_3d_mapping(zed_interfaces::stop_3d_mapping::Request& req, + zed_interfaces::stop_3d_mapping::Response& res); - /*! \brief Service callback to save_3d_map service + /*! \brief Service callback to save_3d_map service */ - bool on_save_3d_map(zed_interfaces::save_3d_map::Request& req, zed_interfaces::save_3d_map::Response& res); + bool on_save_3d_map(zed_interfaces::save_3d_map::Request& req, zed_interfaces::save_3d_map::Response& res); - /*! \brief Service callback to start_object_detection service + /*! \brief Service callback to start_object_detection service */ - bool on_start_object_detection(zed_interfaces::start_object_detection::Request& req, - zed_interfaces::start_object_detection::Response& res); + bool on_start_object_detection(zed_interfaces::start_object_detection::Request& req, + zed_interfaces::start_object_detection::Response& res); - /*! \brief Service callback to stop_object_detection service + /*! \brief Service callback to stop_object_detection service */ - bool on_stop_object_detection(zed_interfaces::stop_object_detection::Request& req, - zed_interfaces::stop_object_detection::Response& res); + bool on_stop_object_detection(zed_interfaces::stop_object_detection::Request& req, + zed_interfaces::stop_object_detection::Response& res); - /*! \brief Service callback to save_area_memory service + /*! \brief Service callback to save_area_memory service */ - bool on_save_area_memory(zed_interfaces::save_area_memory::Request& req, - zed_interfaces::save_area_memory::Response& res); + bool on_save_area_memory(zed_interfaces::save_area_memory::Request& req, + zed_interfaces::save_area_memory::Response& res); - /*! \brief Utility to initialize the pose variables + /*! \brief Utility to initialize the pose variables */ - bool set_pose(float xt, float yt, float zt, float rr, float pr, float yr); + bool set_pose(float xt, float yt, float zt, float rr, float pr, float yr); - /*! \brief Utility to initialize the most used transforms + /*! \brief Utility to initialize the most used transforms */ - void initTransforms(); + void initTransforms(); - /*! \brief Utility to initialize the static transform from Sensor to Base + /*! \brief Utility to initialize the static transform from Sensor to Base */ - bool getSens2BaseTransform(); + bool getSens2BaseTransform(); - /*! \brief Utility to initialize the static transform from Sensor to Camera + /*! \brief Utility to initialize the static transform from Sensor to Camera */ - bool getSens2CameraTransform(); + bool getSens2CameraTransform(); - /*! \brief Utility to initialize the static transform from Camera to Base + /*! \brief Utility to initialize the static transform from Camera to Base */ - bool getCamera2BaseTransform(); + bool getCamera2BaseTransform(); - /*! \brief Start tracking + /*! \brief Start tracking */ - void start_pos_tracking(); + void start_pos_tracking(); - /*! \brief Start spatial mapping + /*! \brief Start spatial mapping */ - bool start_3d_mapping(); + bool start_3d_mapping(); - /*! \brief Stop spatial mapping + /*! \brief Stop spatial mapping */ - void stop_3d_mapping(); + void stop_3d_mapping(); - /*! \brief Start object detection + /*! \brief Start object detection */ - bool start_obj_detect(); + bool start_obj_detect(); - /*! \brief Stop object detection + /*! \brief Stop object detection */ - void stop_obj_detect(); + void stop_obj_detect(); - /*! \brief Publish object detection results + /*! \brief Publish object detection results */ - void processDetectedObjects(ros::Time t); + void processDetectedObjects(ros::Time t); - /*! \brief Generates an univoque color for each object class ID + /*! \brief Generates an univoque color for each object class ID */ - inline sl::float3 generateColorClass(int idx) - { - sl::float3 clr; - clr.r = static_cast(33 + (idx * 456262)); - clr.g = static_cast(233 + (idx * 1564684)); - clr.b = static_cast(133 + (idx * 76873242)); - return clr / 255.f; - } + inline sl::float3 generateColorClass(int idx) + { + sl::float3 clr; + clr.r = static_cast(33 + (idx * 456262)); + clr.g = static_cast(233 + (idx * 1564684)); + clr.b = static_cast(133 + (idx * 76873242)); + return clr / 255.f; + } - /*! \brief Update Dynamic reconfigure parameters + /*! \brief Update Dynamic reconfigure parameters */ - void updateDynamicReconfigure(); + void updateDynamicReconfigure(); - /*! \brief Save the current area map if positional tracking + /*! \brief Save the current area map if positional tracking * and area memory are active */ - bool saveAreaMap(std::string file_path, std::string* out_msg=nullptr); + bool saveAreaMap(std::string file_path, std::string* out_msg = nullptr); private: - uint64_t mFrameCount = 0; - - // SDK version - int mVerMajor; - int mVerMinor; - int mVerSubMinor; - - // ROS - ros::NodeHandle mNh; - ros::NodeHandle mNhNs; - std::thread mDevicePollThread; - std::thread mPcThread; // Point Cloud thread - std::thread mSensThread; // Sensors data thread - - bool mStopNode = false; - - // Publishers - image_transport::CameraPublisher mPubRgb; // - image_transport::CameraPublisher mPubRawRgb; // - image_transport::CameraPublisher mPubLeft; // - image_transport::CameraPublisher mPubRawLeft; // - image_transport::CameraPublisher mPubRight; // - image_transport::CameraPublisher mPubRawRight; // - image_transport::CameraPublisher mPubDepth; // - image_transport::Publisher mPubStereo; - image_transport::Publisher mPubRawStereo; - - image_transport::CameraPublisher mPubRgbGray; - image_transport::CameraPublisher mPubRawRgbGray; - image_transport::CameraPublisher mPubLeftGray; - image_transport::CameraPublisher mPubRawLeftGray; - image_transport::CameraPublisher mPubRightGray; - image_transport::CameraPublisher mPubRawRightGray; - - ros::Publisher mPubConfMap; // - ros::Publisher mPubDisparity; // - ros::Publisher mPubCloud; - ros::Publisher mPubFusedCloud; - ros::Publisher mPubPose; - ros::Publisher mPubPoseCov; - ros::Publisher mPubOdom; - ros::Publisher mPubOdomPath; - ros::Publisher mPubMapPath; - ros::Publisher mPubImu; - ros::Publisher mPubImuRaw; - ros::Publisher mPubImuTemp; - ros::Publisher mPubImuMag; - // ros::Publisher mPubImuMagRaw; - ros::Publisher mPubPressure; - ros::Publisher mPubTempL; - ros::Publisher mPubTempR; - ros::Publisher mPubCamImuTransf; - - // Timers - ros::Timer mPathTimer; - ros::Timer mFusedPcTimer; - ros::Timer mVideoDepthTimer; - - // Services - ros::ServiceServer mSrvSetInitPose; - ros::ServiceServer mSrvResetOdometry; - ros::ServiceServer mSrvResetTracking; - ros::ServiceServer mSrvSvoStartRecording; - ros::ServiceServer mSrvSvoStopRecording; - ros::ServiceServer mSrvSvoStartStream; - ros::ServiceServer mSrvSvoStopStream; - ros::ServiceServer mSrvSetLedStatus; - ros::ServiceServer mSrvToggleLed; - ros::ServiceServer mSrvStartMapping; - ros::ServiceServer mSrvStopMapping; - ros::ServiceServer mSrvSave3dMap; - ros::ServiceServer mSrvStartObjDet; - ros::ServiceServer mSrvStopObjDet; - ros::ServiceServer mSrvSaveAreaMemory; - - // ----> Topics (ONLY THOSE NOT CHANGING WHILE NODE RUNS) - // Camera info - sensor_msgs::CameraInfoPtr mRgbCamInfoMsg; - sensor_msgs::CameraInfoPtr mLeftCamInfoMsg; - sensor_msgs::CameraInfoPtr mRightCamInfoMsg; - sensor_msgs::CameraInfoPtr mRgbCamInfoRawMsg; - sensor_msgs::CameraInfoPtr mLeftCamInfoRawMsg; - sensor_msgs::CameraInfoPtr mRightCamInfoRawMsg; - sensor_msgs::CameraInfoPtr mDepthCamInfoMsg; - - geometry_msgs::TransformPtr mCameraImuTransfMgs; - // <---- Topics - - // ROS TF - tf2_ros::TransformBroadcaster mTransformPoseBroadcaster; - tf2_ros::TransformBroadcaster mTransformOdomBroadcaster; - tf2_ros::StaticTransformBroadcaster mStaticTransformImuBroadcaster; - - std::string mRgbFrameId; - std::string mRgbOptFrameId; - - std::string mDepthFrameId; - std::string mDepthOptFrameId; - - std::string mDisparityFrameId; - std::string mDisparityOptFrameId; - - std::string mConfidenceFrameId; - std::string mConfidenceOptFrameId; - - std::string mCloudFrameId; - std::string mPointCloudFrameId; - - std::string mMapFrameId; - std::string mOdometryFrameId; - std::string mBaseFrameId; - std::string mCameraFrameId; - - std::string mRightCamFrameId; - std::string mRightCamOptFrameId; - std::string mLeftCamFrameId; - std::string mLeftCamOptFrameId; - std::string mImuFrameId; - - std::string mBaroFrameId; - std::string mMagFrameId; - std::string mTempLeftFrameId; - std::string mTempRightFrameId; - - bool mPublishTf; - bool mPublishMapTf; - bool mPublishImuTf; - bool mCameraFlip; - bool mCameraSelfCalib; - - // Launch file parameters - std::string mCameraName; - sl::RESOLUTION mCamResol; - int mCamFrameRate; - sl::DEPTH_MODE mDepthMode; - sl::SENSING_MODE mCamSensingMode; - int mGpuId; - int mZedId; - int mDepthStabilization; - std::string mAreaMemDbPath; - bool mSaveAreaMapOnClosing = true; - std::string mSvoFilepath; - std::string mRemoteStreamAddr; - bool mSensTimestampSync; - double mSensPubRate = 400.0; - double mPathPubRate; - int mPathMaxCount; - bool mVerbose; - bool mSvoMode = false; - double mCamMinDepth; - double mCamMaxDepth; - - // Positional tracking - bool mPosTrackingEnabled = false; - bool mPosTrackingActivated = false; - bool mPosTrackingReady = false; - bool mTwoDMode = false; - double mFixedZValue = 0.0; - bool mFloorAlignment = false; - bool mImuFusion = true; - bool mGrabActive = false; // Indicate if camera grabbing is active (at least one topic subscribed) - sl::ERROR_CODE mConnStatus; - sl::ERROR_CODE mGrabStatus; - sl::POSITIONAL_TRACKING_STATE mPosTrackingStatus; - bool mSensPublishing = false; - bool mPcPublishing = false; - - // Last frame time - ros::Time mPrevFrameTimestamp; - ros::Time mFrameTimestamp; - - // Positional Tracking variables - sl::Pose mLastZedPose; // Sensor to Map transform - sl::Transform mInitialPoseSl; - std::vector mInitialBasePose; - std::vector mOdomPath; - std::vector mMapPath; - - // IMU TF - tf2::Transform mLastImuPose; - - // TF Transforms - tf2::Transform mMap2OdomTransf; // Coordinates of the odometry frame in map frame - tf2::Transform mOdom2BaseTransf; // Coordinates of the base in odometry frame - tf2::Transform mMap2BaseTransf; // Coordinates of the base in map frame - tf2::Transform mMap2CameraTransf; // Coordinates of the camera in base frame - tf2::Transform mSensor2BaseTransf; // Coordinates of the base frame in sensor frame - tf2::Transform mSensor2CameraTransf; // Coordinates of the camera frame in sensor frame - tf2::Transform mCamera2BaseTransf; // Coordinates of the base frame in camera frame - - bool mSensor2BaseTransfValid = false; - bool mSensor2CameraTransfValid = false; - bool mCamera2BaseTransfValid = false; - bool mStaticImuFramePublished = false; - - // initialization Transform listener - boost::shared_ptr mTfBuffer; - boost::shared_ptr mTfListener; - - // Zed object - sl::InitParameters mZedParams; - sl::Camera mZed; - unsigned int mZedSerialNumber; - sl::MODEL mZedUserCamModel; // Camera model set by ROS Param - sl::MODEL mZedRealCamModel; // Real camera model by SDK API - unsigned int mCamFwVersion; // Camera FW version - unsigned int mSensFwVersion; // Sensors FW version - - // Dynamic Parameters - int mCamBrightness = 4; - int mCamContrast = 4; - int mCamHue = 0; - int mCamSaturation = 4; - int mCamSharpness = 3; - int mCamGamma = 1; - bool mCamAutoExposure = true; - int mCamGain = 100; - int mCamExposure = 100; - bool mCamAutoWB = true; - int mCamWB = 4200; - - int mCamDepthConfidence = 50; - int mCamDepthTextureConf = 100; - double mPointCloudFreq = 15.; - double mVideoDepthFreq = 15.; - - double mCamImageResizeFactor = 1.0; - double mCamDepthResizeFactor = 1.0; - - // flags - bool mTriggerAutoExposure = true; - bool mTriggerAutoWB = true; - bool mComputeDepth; - bool mOpenniDepthMode; // 16 bit UC data in mm else 32F in m, for more info -> http://www.ros.org/reps/rep-0118.html - bool mPoseSmoothing = false; // Always disabled. Enable only for AR/VR applications - bool mAreaMemory; - bool mInitOdomWithPose; - bool mResetOdom = false; - bool mUseOldExtrinsic = false; - bool mUpdateDynParams = false; - bool mPublishingData = false; - - // SVO recording - bool mRecording = false; - sl::RecordingStatus mRecStatus; - sl::SVO_COMPRESSION_MODE mSvoComprMode; - - // Streaming - bool mStreaming = false; - - // Mat - int mCamWidth; - int mCamHeight; - sl::Resolution mMatResolVideo; - sl::Resolution mMatResolDepth; - - // Thread Sync - std::mutex mCloseZedMutex; - std::mutex mCamDataMutex; - std::mutex mPcMutex; - std::mutex mRecMutex; - std::mutex mPosTrkMutex; - std::mutex mDynParMutex; - std::mutex mMappingMutex; - std::mutex mObjDetMutex; - std::condition_variable mPcDataReadyCondVar; - bool mPcDataReady; - std::condition_variable mRgbDepthDataRetrievedCondVar; - bool mRgbDepthDataRetrieved; - - // Point cloud variables - sl::Mat mCloud; - sl::FusedPointCloud mFusedPC; - ros::Time mPointCloudTime; - - // Dynamic reconfigure - boost::recursive_mutex mDynServerMutex; // To avoid Dynamic Reconfigure Server warning - boost::shared_ptr> mDynRecServer; - - // Diagnostic - float mTempLeft = -273.15f; - float mTempRight = -273.15f; - std::unique_ptr mElabPeriodMean_sec; - std::unique_ptr mGrabPeriodMean_usec; - std::unique_ptr mVideoDepthPeriodMean_sec; - std::unique_ptr mPcPeriodMean_usec; - std::unique_ptr mSensPeriodMean_usec; - std::unique_ptr mObjDetPeriodMean_msec; - - diagnostic_updater::Updater mDiagUpdater; // Diagnostic Updater - - // Camera IMU transform - sl::Transform mSlCamImuTransf; - geometry_msgs::TransformStamped mStaticImuTransformStamped; - - // Spatial mapping - bool mMappingEnabled; - bool mMappingRunning; - bool mMapSave=false; - float mMappingRes = 0.1; - float mMaxMappingRange = -1; - double mFusedPcPubFreq = 2.0; - - // Object Detection - bool mObjDetEnabled = false; - bool mObjDetRunning = false; - bool mObjDetTracking = true; - bool mObjDetBodyFitting = true; - float mObjDetConfidence = 50.f; - float mObjDetMaxRange = 10.f; - std::vector mObjDetFilter; - bool mObjDetPeopleEnable = true; - bool mObjDetVehiclesEnable = true; - bool mObjDetBagsEnable = true; - bool mObjDetAnimalsEnable = true; - bool mObjDetElectronicsEnable = true; - bool mObjDetFruitsEnable = true; - bool mObjDetSportsEnable = true; - - sl::DETECTION_MODEL mObjDetModel = sl::DETECTION_MODEL::MULTI_CLASS_BOX; - - ros::Publisher mPubObjDet; -}; // class ZEDROSWrapperNodelet -} // namespace zed_nodelets + uint64_t mFrameCount = 0; + + // SDK version + int mVerMajor; + int mVerMinor; + int mVerSubMinor; + + // ROS + ros::NodeHandle mNh; + ros::NodeHandle mNhNs; + std::thread mDevicePollThread; + std::thread mPcThread; // Point Cloud thread + std::thread mSensThread; // Sensors data thread + + bool mStopNode = false; + + // Publishers + image_transport::CameraPublisher mPubRgb; // + image_transport::CameraPublisher mPubRawRgb; // + image_transport::CameraPublisher mPubLeft; // + image_transport::CameraPublisher mPubRawLeft; // + image_transport::CameraPublisher mPubRight; // + image_transport::CameraPublisher mPubRawRight; // + image_transport::CameraPublisher mPubDepth; // + image_transport::Publisher mPubStereo; + image_transport::Publisher mPubRawStereo; + + image_transport::CameraPublisher mPubRgbGray; + image_transport::CameraPublisher mPubRawRgbGray; + image_transport::CameraPublisher mPubLeftGray; + image_transport::CameraPublisher mPubRawLeftGray; + image_transport::CameraPublisher mPubRightGray; + image_transport::CameraPublisher mPubRawRightGray; + + ros::Publisher mPubConfMap; // + ros::Publisher mPubDisparity; // + ros::Publisher mPubCloud; + ros::Publisher mPubFusedCloud; + ros::Publisher mPubPose; + ros::Publisher mPubPoseCov; + ros::Publisher mPubOdom; + ros::Publisher mPubOdomPath; + ros::Publisher mPubMapPath; + ros::Publisher mPubImu; + ros::Publisher mPubImuRaw; + ros::Publisher mPubImuTemp; + ros::Publisher mPubImuMag; + // ros::Publisher mPubImuMagRaw; + ros::Publisher mPubPressure; + ros::Publisher mPubTempL; + ros::Publisher mPubTempR; + ros::Publisher mPubCamImuTransf; + + // Subscribers + ros::Subscriber mClickedPtSub; + + // Timers + ros::Timer mPathTimer; + ros::Timer mFusedPcTimer; + ros::Timer mVideoDepthTimer; + + // Services + ros::ServiceServer mSrvSetInitPose; + ros::ServiceServer mSrvResetOdometry; + ros::ServiceServer mSrvResetTracking; + ros::ServiceServer mSrvSvoStartRecording; + ros::ServiceServer mSrvSvoStopRecording; + ros::ServiceServer mSrvSvoStartStream; + ros::ServiceServer mSrvSvoStopStream; + ros::ServiceServer mSrvSetLedStatus; + ros::ServiceServer mSrvToggleLed; + ros::ServiceServer mSrvStartMapping; + ros::ServiceServer mSrvStopMapping; + ros::ServiceServer mSrvSave3dMap; + ros::ServiceServer mSrvStartObjDet; + ros::ServiceServer mSrvStopObjDet; + ros::ServiceServer mSrvSaveAreaMemory; + + // ----> Topics (ONLY THOSE NOT CHANGING WHILE NODE RUNS) + // Camera info + sensor_msgs::CameraInfoPtr mRgbCamInfoMsg; + sensor_msgs::CameraInfoPtr mLeftCamInfoMsg; + sensor_msgs::CameraInfoPtr mRightCamInfoMsg; + sensor_msgs::CameraInfoPtr mRgbCamInfoRawMsg; + sensor_msgs::CameraInfoPtr mLeftCamInfoRawMsg; + sensor_msgs::CameraInfoPtr mRightCamInfoRawMsg; + sensor_msgs::CameraInfoPtr mDepthCamInfoMsg; + + geometry_msgs::TransformPtr mCameraImuTransfMgs; + // <---- Topics + + // ROS TF + tf2_ros::TransformBroadcaster mTransformPoseBroadcaster; + tf2_ros::TransformBroadcaster mTransformOdomBroadcaster; + tf2_ros::StaticTransformBroadcaster mStaticTransformImuBroadcaster; + + std::string mRgbFrameId; + std::string mRgbOptFrameId; + + std::string mDepthFrameId; + std::string mDepthOptFrameId; + + std::string mDisparityFrameId; + std::string mDisparityOptFrameId; + + std::string mConfidenceFrameId; + std::string mConfidenceOptFrameId; + + std::string mCloudFrameId; + std::string mPointCloudFrameId; + + std::string mMapFrameId; + std::string mOdometryFrameId; + std::string mBaseFrameId; + std::string mCameraFrameId; + + std::string mRightCamFrameId; + std::string mRightCamOptFrameId; + std::string mLeftCamFrameId; + std::string mLeftCamOptFrameId; + std::string mImuFrameId; + + std::string mBaroFrameId; + std::string mMagFrameId; + std::string mTempLeftFrameId; + std::string mTempRightFrameId; + + bool mPublishTf; + bool mPublishMapTf; + bool mPublishImuTf; + bool mCameraFlip; + bool mCameraSelfCalib; + + // Launch file parameters + std::string mCameraName; + sl::RESOLUTION mCamResol; + int mCamFrameRate; + sl::DEPTH_MODE mDepthMode; + sl::SENSING_MODE mCamSensingMode; + int mGpuId; + int mZedId; + int mDepthStabilization; + std::string mAreaMemDbPath; + bool mSaveAreaMapOnClosing = true; + std::string mSvoFilepath; + std::string mRemoteStreamAddr; + bool mSensTimestampSync; + double mSensPubRate = 400.0; + double mPathPubRate; + int mPathMaxCount; + bool mVerbose; + bool mSvoMode = false; + double mCamMinDepth; + double mCamMaxDepth; + std::string mClickedPtTopic; + + // Positional tracking + bool mPosTrackingEnabled = false; + bool mPosTrackingActivated = false; + bool mPosTrackingReady = false; + bool mTwoDMode = false; + double mFixedZValue = 0.0; + bool mFloorAlignment = false; + bool mImuFusion = true; + bool mGrabActive = false; // Indicate if camera grabbing is active (at least one topic subscribed) + sl::ERROR_CODE mConnStatus; + sl::ERROR_CODE mGrabStatus; + sl::POSITIONAL_TRACKING_STATE mPosTrackingStatus; + bool mSensPublishing = false; + bool mPcPublishing = false; + + // Last frame time + ros::Time mPrevFrameTimestamp; + ros::Time mFrameTimestamp; + + // Positional Tracking variables + sl::Pose mLastZedPose; // Sensor to Map transform + sl::Transform mInitialPoseSl; + std::vector mInitialBasePose; + std::vector mOdomPath; + std::vector mMapPath; + + // IMU TF + tf2::Transform mLastImuPose; + + // TF Transforms + tf2::Transform mMap2OdomTransf; // Coordinates of the odometry frame in map frame + tf2::Transform mOdom2BaseTransf; // Coordinates of the base in odometry frame + tf2::Transform mMap2BaseTransf; // Coordinates of the base in map frame + tf2::Transform mMap2CameraTransf; // Coordinates of the camera in base frame + tf2::Transform mSensor2BaseTransf; // Coordinates of the base frame in sensor frame + tf2::Transform mSensor2CameraTransf; // Coordinates of the camera frame in sensor frame + tf2::Transform mCamera2BaseTransf; // Coordinates of the base frame in camera frame + + bool mSensor2BaseTransfValid = false; + bool mSensor2CameraTransfValid = false; + bool mCamera2BaseTransfValid = false; + bool mStaticImuFramePublished = false; + + // initialization Transform listener + boost::shared_ptr mTfBuffer; + boost::shared_ptr mTfListener; + + // Zed object + sl::InitParameters mZedParams; + sl::Camera mZed; + unsigned int mZedSerialNumber; + sl::MODEL mZedUserCamModel; // Camera model set by ROS Param + sl::MODEL mZedRealCamModel; // Real camera model by SDK API + unsigned int mCamFwVersion; // Camera FW version + unsigned int mSensFwVersion; // Sensors FW version + + // Dynamic Parameters + int mCamBrightness = 4; + int mCamContrast = 4; + int mCamHue = 0; + int mCamSaturation = 4; + int mCamSharpness = 3; + int mCamGamma = 1; + bool mCamAutoExposure = true; + int mCamGain = 100; + int mCamExposure = 100; + bool mCamAutoWB = true; + int mCamWB = 4200; + + int mCamDepthConfidence = 50; + int mCamDepthTextureConf = 100; + double mPointCloudFreq = 15.; + double mVideoDepthFreq = 15.; + + double mCamImageResizeFactor = 1.0; + double mCamDepthResizeFactor = 1.0; + + // flags + bool mTriggerAutoExposure = true; + bool mTriggerAutoWB = true; + bool mComputeDepth; + bool mOpenniDepthMode; // 16 bit UC data in mm else 32F in m, for more info -> http://www.ros.org/reps/rep-0118.html + bool mPoseSmoothing = false; // Always disabled. Enable only for AR/VR applications + bool mAreaMemory; + bool mInitOdomWithPose; + bool mResetOdom = false; + bool mUseOldExtrinsic = false; + bool mUpdateDynParams = false; + bool mPublishingData = false; + + // SVO recording + bool mRecording = false; + sl::RecordingStatus mRecStatus; + sl::SVO_COMPRESSION_MODE mSvoComprMode; + + // Streaming + bool mStreaming = false; + + // Mat + int mCamWidth; + int mCamHeight; + sl::Resolution mMatResolVideo; + sl::Resolution mMatResolDepth; + + // Thread Sync + std::mutex mCloseZedMutex; + std::mutex mCamDataMutex; + std::mutex mPcMutex; + std::mutex mRecMutex; + std::mutex mPosTrkMutex; + std::mutex mDynParMutex; + std::mutex mMappingMutex; + std::mutex mObjDetMutex; + std::condition_variable mPcDataReadyCondVar; + bool mPcDataReady; + std::condition_variable mRgbDepthDataRetrievedCondVar; + bool mRgbDepthDataRetrieved; + + // Point cloud variables + sl::Mat mCloud; + sl::FusedPointCloud mFusedPC; + ros::Time mPointCloudTime; + + // Dynamic reconfigure + boost::recursive_mutex mDynServerMutex; // To avoid Dynamic Reconfigure Server warning + boost::shared_ptr> mDynRecServer; + + // Diagnostic + float mTempLeft = -273.15f; + float mTempRight = -273.15f; + std::unique_ptr mElabPeriodMean_sec; + std::unique_ptr mGrabPeriodMean_usec; + std::unique_ptr mVideoDepthPeriodMean_sec; + std::unique_ptr mPcPeriodMean_usec; + std::unique_ptr mSensPeriodMean_usec; + std::unique_ptr mObjDetPeriodMean_msec; + + diagnostic_updater::Updater mDiagUpdater; // Diagnostic Updater + + // Camera IMU transform + sl::Transform mSlCamImuTransf; + geometry_msgs::TransformStamped mStaticImuTransformStamped; + + // Spatial mapping + bool mMappingEnabled; + bool mMappingRunning; + bool mMapSave = false; + float mMappingRes = 0.1; + float mMaxMappingRange = -1; + double mFusedPcPubFreq = 2.0; + + // Object Detection + bool mObjDetEnabled = false; + bool mObjDetRunning = false; + bool mObjDetTracking = true; + bool mObjDetBodyFitting = true; + float mObjDetConfidence = 50.f; + float mObjDetMaxRange = 10.f; + std::vector mObjDetFilter; + bool mObjDetPeopleEnable = true; + bool mObjDetVehiclesEnable = true; + bool mObjDetBagsEnable = true; + bool mObjDetAnimalsEnable = true; + bool mObjDetElectronicsEnable = true; + bool mObjDetFruitsEnable = true; + bool mObjDetSportsEnable = true; + + sl::DETECTION_MODEL mObjDetModel = sl::DETECTION_MODEL::MULTI_CLASS_BOX; + + ros::Publisher mPubObjDet; +}; // class ZEDROSWrapperNodelet +} // namespace zed_nodelets #include PLUGINLIB_EXPORT_CLASS(zed_nodelets::ZEDWrapperNodelet, nodelet::Nodelet); -#endif // ZED_WRAPPER_NODELET_H +#endif // ZED_WRAPPER_NODELET_H 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 fb46cf3a..2f8a2459 100644 --- a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp @@ -555,6 +555,9 @@ void ZEDWrapperNodelet::onInit() } } + // Subscribers + mClickedPtSub = mNhNs.subscribe(mClickedPtTopic, 10, &ZEDWrapperNodelet::clickedPtCallback, this); + // Services mSrvSetInitPose = mNhNs.advertiseService("set_pose", &ZEDWrapperNodelet::on_set_pose, this); mSrvResetOdometry = mNhNs.advertiseService("reset_odometry", &ZEDWrapperNodelet::on_reset_odometry, this); @@ -738,6 +741,9 @@ void ZEDWrapperNodelet::readParameters() } else { NODELET_INFO_STREAM(" * Mapping\t\t\t-> DISABLED"); } + + mNhNs.param("mapping/clicked_point_topic", mClickedPtTopic, std::string("/clicked_point")); + NODELET_INFO_STREAM(" * Clicked point topic\t\t-> " << mClickedPtTopic.c_str()); // <---- Mapping NODELET_INFO_STREAM("*** OBJECT DETECTION PARAMETERS ***"); @@ -4391,4 +4397,26 @@ void ZEDWrapperNodelet::processDetectedObjects(ros::Time t) mPubObjDet.publish(objMsg); } +void ZEDWrapperNodelet::clickedPtCallback(geometry_msgs::PointStampedConstPtr msg) +{ + float X = msg->point.x; + float Y = msg->point.y; + float Z = msg->point.z; + NODELET_INFO_STREAM("Clicked 3D point: [" << X << "," << Y << "," << Z << "]"); + + // ----> Project the point into 2D image coordinates + sl::CalibrationParameters zedParam; + zedParam = mZed.getCameraInformation(mMatResolVideo).calibration_parameters; // ok + + float f_x = zedParam.left_cam.fx; + float f_y = zedParam.left_cam.fy; + float c_x = zedParam.left_cam.cx; + float c_y = zedParam.left_cam.cy; + + float u = (-Y / X) * f_x + c_x; + float v = (-Z / X) * f_y + c_y; + NODELET_INFO_STREAM("Clicked point image coordinates: [" << u << "," << v << "]"); + // <---- Project the point into 2D image coordinates +} + } // namespace zed_nodelets diff --git a/zed_wrapper/params/common.yaml b/zed_wrapper/params/common.yaml index fb8cd5ff..3493a2bd 100644 --- a/zed_wrapper/params/common.yaml +++ b/zed_wrapper/params/common.yaml @@ -58,12 +58,13 @@ pos_tracking: path_pub_rate: 2.0 # Camera trajectory publishing frequency path_max_count: -1 # use '-1' for unlimited path size two_d_mode: false # Force navigation on a plane. If true the Z value will be fixed to "fixed_z_value", roll and pitch to zero - fixed_z_value: 0.00 # Value to be used for Z coordinate if `two_d_mode` is true + fixed_z_value: 0.00 # Value to be used for Z coordinate if `two_d_mode` is true mapping: mapping_enabled: false # True to enable mapping and fused point cloud publication resolution: 0.05 # maps resolution in meters [0.01f, 0.2f] max_mapping_range: -1 # maximum depth range while mapping in meters (-1 for automatic calculation) [2.0, 20.0] fused_pointcloud_freq: 1.0 # frequency of the publishing of the fused colored point cloud + clicked_point_topic: '/clicked_point' # Topic published by Rviz when a point of the cloud is clicked. Used for plane detection From e4c2e0e3da446e52d3eddff21971222e8eb944aa Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Thu, 31 Mar 2022 18:56:56 +0200 Subject: [PATCH 117/199] Update common.yaml --- zed_wrapper/params/common.yaml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/zed_wrapper/params/common.yaml b/zed_wrapper/params/common.yaml index fb8cd5ff..a002f55f 100644 --- a/zed_wrapper/params/common.yaml +++ b/zed_wrapper/params/common.yaml @@ -33,15 +33,15 @@ general: camera_flip: false video: - img_downsample_factor: 1.0 # Resample factor for images [0.01,1.0] The SDK works with native image sizes, but publishes rescaled image. + img_downsample_factor: 0.5 # Resample factor for images [0.01,1.0] The SDK works with native image sizes, but publishes rescaled image. extrinsic_in_camera_frame: true # if `false` extrinsic parameter in `camera_info` will use ROS native frame (X FORWARD, Z UP) instead of the camera frame (Z FORWARD, Y DOWN) [`true` use old behavior as for version < v3.1] depth: - quality: 4 # '0': NONE, '1': PERFORMANCE, '2': QUALITY, '3': ULTRA, '4': NEURAL + quality: 1 # '0': NONE, '1': PERFORMANCE, '2': QUALITY, '3': ULTRA, '4': NEURAL sensing_mode: 0 # '0': STANDARD, '1': FILL (not use FILL for robotic applications) depth_stabilization: 1 # `0`: disabled, `1`: enabled openni_depth_mode: false # 'false': 32bit float meters, 'true': 16bit uchar millimeters - depth_downsample_factor: 1.0 # Resample factor for depth data matrices [0.01,1.0] The SDK works with native data sizes, but publishes rescaled matrices (depth map, point cloud, ...) + depth_downsample_factor: 0.5 # Resample factor for depth data matrices [0.01,1.0] The SDK works with native data sizes, but publishes rescaled matrices (depth map, point cloud, ...) pos_tracking: pos_tracking_enabled: true # True to enable positional tracking from start From 561324552544522023c905124bb2ff74d51b69fd Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Thu, 31 Mar 2022 20:04:57 +0200 Subject: [PATCH 118/199] Add plane detectiopn at hit --- zed_nodelets/CMakeLists.txt | 1 + zed_nodelets/package.xml | 1 + .../include/zed_wrapper_nodelet.hpp | 11 +- .../zed_nodelet/src/zed_wrapper_nodelet.cpp | 259 ++++++++++++++---- 4 files changed, 216 insertions(+), 56 deletions(-) diff --git a/zed_nodelets/CMakeLists.txt b/zed_nodelets/CMakeLists.txt index 741c1b41..1aef1475 100644 --- a/zed_nodelets/CMakeLists.txt +++ b/zed_nodelets/CMakeLists.txt @@ -73,6 +73,7 @@ catkin_package( message_runtime zed_interfaces geometry_msgs + visualization_msgs ) ############################################################################### diff --git a/zed_nodelets/package.xml b/zed_nodelets/package.xml index 8373c4a8..fcbf410d 100644 --- a/zed_nodelets/package.xml +++ b/zed_nodelets/package.xml @@ -26,6 +26,7 @@ nodelet diagnostic_updater geometry_msgs + visualization_msgs zed_interfaces 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 394f9e8e..12463084 100644 --- a/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp +++ b/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp @@ -33,6 +33,7 @@ #include #include #include +#include #include @@ -77,8 +78,6 @@ #include #include -using namespace std; - namespace zed_nodelets { class ZEDWrapperNodelet : public nodelet::Nodelet { typedef enum _dyn_params { @@ -174,7 +173,7 @@ class ZEDWrapperNodelet : public nodelet::Nodelet { * \param t : the ros::Time to stamp the image */ void publishImage(sensor_msgs::ImagePtr imgMsgPtr, sl::Mat img, image_transport::CameraPublisher& pubImg, - sensor_msgs::CameraInfoPtr camInfoMsg, string imgFrameId, ros::Time t); + sensor_msgs::CameraInfoPtr camInfoMsg, std::string imgFrameId, ros::Time t); /*! \brief Publish a sl::Mat depth image with a ros Publisher * \param imgMsgPtr : the depth image topic message to publish @@ -220,7 +219,7 @@ class ZEDWrapperNodelet : public nodelet::Nodelet { * \param right_frame_id : the id of the reference frame of the right camera */ void fillCamInfo(sl::Camera& zed, sensor_msgs::CameraInfoPtr leftCamInfoMsg, - sensor_msgs::CameraInfoPtr rightCamInfoMsg, string leftFrameId, string rightFrameId, + sensor_msgs::CameraInfoPtr rightCamInfoMsg, std::string leftFrameId, std::string rightFrameId, bool rawParam = false); /*! \brief Get the information of the ZED cameras and store them in an @@ -230,7 +229,7 @@ class ZEDWrapperNodelet : public nodelet::Nodelet { * camera informations * \param frame_id : the id of the reference frame of the left camera */ - void fillCamDepthInfo(sl::Camera& zed, sensor_msgs::CameraInfoPtr depth_info_msg, string frame_id); + void fillCamDepthInfo(sl::Camera& zed, sensor_msgs::CameraInfoPtr depth_info_msg, std::string frame_id); /*! \brief Check if FPS and Resolution chosen by user are correct. * Modifies FPS to match correct value. @@ -454,6 +453,8 @@ class ZEDWrapperNodelet : public nodelet::Nodelet { ros::Publisher mPubTempR; ros::Publisher mPubCamImuTransf; + ros::Publisher mPubMarker; // Publisher for Rviz markers + // Subscribers ros::Subscriber mClickedPtSub; 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 2f8a2459..46751f6e 100644 --- a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp @@ -111,51 +111,54 @@ void ZEDWrapperNodelet::onInit() std::string img_gray_topic = "/image_rect_gray"; std::string img_raw_gray_topic_ = "/image_raw_gray"; std::string raw_suffix = "_raw"; - string left_topic = leftTopicRoot + img_topic; - string left_raw_topic = leftTopicRoot + raw_suffix + img_raw_topic; - string right_topic = rightTopicRoot + img_topic; - string right_raw_topic = rightTopicRoot + raw_suffix + img_raw_topic; - string rgb_topic = rgbTopicRoot + img_topic; - string rgb_raw_topic = rgbTopicRoot + raw_suffix + img_raw_topic; - string stereo_topic = stereoTopicRoot + img_topic; - string stereo_raw_topic = stereoTopicRoot + raw_suffix + img_raw_topic; - string left_gray_topic = leftTopicRoot + img_gray_topic; - string left_raw_gray_topic = leftTopicRoot + raw_suffix + img_raw_gray_topic_; - string right_gray_topic = rightTopicRoot + img_gray_topic; - string right_raw_gray_topic = rightTopicRoot + raw_suffix + img_raw_gray_topic_; - string rgb_gray_topic = rgbTopicRoot + img_gray_topic; - string rgb_raw_gray_topic = rgbTopicRoot + raw_suffix + img_raw_gray_topic_; + std::string left_topic = leftTopicRoot + img_topic; + std::string left_raw_topic = leftTopicRoot + raw_suffix + img_raw_topic; + std::string right_topic = rightTopicRoot + img_topic; + std::string right_raw_topic = rightTopicRoot + raw_suffix + img_raw_topic; + std::string rgb_topic = rgbTopicRoot + img_topic; + std::string rgb_raw_topic = rgbTopicRoot + raw_suffix + img_raw_topic; + std::string stereo_topic = stereoTopicRoot + img_topic; + std::string stereo_raw_topic = stereoTopicRoot + raw_suffix + img_raw_topic; + std::string left_gray_topic = leftTopicRoot + img_gray_topic; + std::string left_raw_gray_topic = leftTopicRoot + raw_suffix + img_raw_gray_topic_; + std::string right_gray_topic = rightTopicRoot + img_gray_topic; + std::string right_raw_gray_topic = rightTopicRoot + raw_suffix + img_raw_gray_topic_; + std::string rgb_gray_topic = rgbTopicRoot + img_gray_topic; + std::string rgb_raw_gray_topic = rgbTopicRoot + raw_suffix + img_raw_gray_topic_; // Set the disparity topic name std::string disparityTopic = "disparity/disparity_image"; // Set the depth topic names - string depth_topic_root = "depth"; + std::string depth_topic_root = "depth"; if (mOpenniDepthMode) { NODELET_INFO_STREAM("Openni depth mode activated -> Units: mm, Encoding: TYPE_16UC1"); } depth_topic_root += "/depth_registered"; - string pointcloud_topic = "point_cloud/cloud_registered"; + std::string pointcloud_topic = "point_cloud/cloud_registered"; - string pointcloud_fused_topic = "mapping/fused_cloud"; + std::string pointcloud_fused_topic = "mapping/fused_cloud"; - string object_det_topic_root = "obj_det"; - string object_det_topic = object_det_topic_root + "/objects"; + std::string object_det_topic_root = "obj_det"; + std::string object_det_topic = object_det_topic_root + "/objects"; std::string confImgRoot = "confidence"; - string conf_map_topic_name = "confidence_map"; - string conf_map_topic = confImgRoot + "/" + conf_map_topic_name; + std::string conf_map_topic_name = "confidence_map"; + std::string conf_map_topic = confImgRoot + "/" + conf_map_topic_name; // Set the positional tracking topic names std::string poseTopic = "pose"; - string pose_cov_topic; + std::string pose_cov_topic; pose_cov_topic = poseTopic + "_with_covariance"; std::string odometryTopic = "odom"; - string odom_path_topic = "path_odom"; - string map_path_topic = "path_map"; + std::string odom_path_topic = "path_odom"; + std::string map_path_topic = "path_map"; + + // Plane markers topic + std::string marker_topic = "plane_marker"; // Create camera info mRgbCamInfoMsg.reset(new sensor_msgs::CameraInfo()); @@ -214,7 +217,7 @@ void ZEDWrapperNodelet::onInit() sl::DeviceProperties prop = sl_tools::getZEDFromSN(mZedSerialNumber); if (prop.id < -1 || prop.camera_state == sl::CAMERA_STATE::NOT_AVAILABLE) { - std::string msg = "ZED SN" + to_string(mZedSerialNumber) + " not detected ! Please connect this ZED"; + std::string msg = "ZED SN" + std::to_string(mZedSerialNumber) + " not detected ! Please connect this ZED"; NODELET_INFO_STREAM(msg.c_str()); std::this_thread::sleep_for(std::chrono::milliseconds(2000)); } else { @@ -347,23 +350,23 @@ void ZEDWrapperNodelet::onInit() } // Set the IMU topic names using real camera model - string imu_topic; - string imu_topic_raw; - string imu_temp_topic; - string imu_mag_topic; - // string imu_mag_topic_raw; - string pressure_topic; - string temp_topic_root = "temperature"; - string temp_topic_left = temp_topic_root + "/left"; - string temp_topic_right = temp_topic_root + "/right"; + std::string imu_topic; + std::string imu_topic_raw; + std::string imu_temp_topic; + std::string imu_mag_topic; + // std::string imu_mag_topic_raw; + std::string pressure_topic; + std::string temp_topic_root = "temperature"; + std::string temp_topic_left = temp_topic_root + "/left"; + std::string temp_topic_right = temp_topic_root + "/right"; if (mZedRealCamModel != sl::MODEL::ZED) { std::string imuTopicRoot = "imu"; - string imu_topic_name = "data"; - string imu_topic_raw_name = "data_raw"; - string imu_topic_mag_name = "mag"; - // string imu_topic_mag_raw_name = "mag_raw"; - string pressure_topic_name = "atm_press"; + std::string imu_topic_name = "data"; + std::string imu_topic_raw_name = "data_raw"; + std::string imu_topic_mag_name = "mag"; + // std::string imu_topic_mag_raw_name = "mag_raw"; + std::string pressure_topic_name = "atm_press"; imu_topic = imuTopicRoot + "/" + imu_topic_name; imu_topic_raw = imuTopicRoot + "/" + imu_topic_raw_name; imu_temp_topic = temp_topic_root + "/" + imuTopicRoot; @@ -474,6 +477,9 @@ void ZEDWrapperNodelet::onInit() mPubOdom = mNhNs.advertise(odometryTopic, 1); NODELET_INFO_STREAM("Advertised on topic " << mPubOdom.getTopic()); + // Rviz markers + mPubMarker = mNhNs.advertise(marker_topic, 10, true); + // Camera Path if (mPathPubRate > 0) { mPubOdomPath = mNhNs.advertise(odom_path_topic, 1, true); @@ -522,7 +528,7 @@ void ZEDWrapperNodelet::onInit() // Publish camera imu transform in a latched topic if (mZedRealCamModel != sl::MODEL::ZED) { - string cam_imu_tr_topic = "left_cam_imu_transform"; + std::string cam_imu_tr_topic = "left_cam_imu_transform"; mPubCamImuTransf = mNhNs.advertise(cam_imu_tr_topic, 1, true); sl::Orientation sl_rot = mSlCamImuTransf.getOrientation(); @@ -627,7 +633,7 @@ void ZEDWrapperNodelet::readParameters() NODELET_INFO_STREAM(" * Serial number\t\t-> " << mZedSerialNumber); } - string camera_model; + std::string camera_model; mNhNs.getParam("general/camera_model", camera_model); if (camera_model == "zed") { @@ -1333,7 +1339,7 @@ bool ZEDWrapperNodelet::start_3d_mapping() if (err == sl::ERROR_CODE::SUCCESS) { if (mPubFusedCloud.getTopic().empty()) { - string pointcloud_fused_topic = "mapping/fused_cloud"; + std::string pointcloud_fused_topic = "mapping/fused_cloud"; mPubFusedCloud = mNhNs.advertise(pointcloud_fused_topic, 1); NODELET_INFO_STREAM("Advertised on topic " << mPubFusedCloud.getTopic() << " @ " << mFusedPcPubFreq << " Hz"); } @@ -1403,8 +1409,8 @@ bool ZEDWrapperNodelet::start_obj_detect() } if (mPubObjDet.getTopic().empty()) { - string object_det_topic_root = "obj_det"; - string object_det_topic = object_det_topic_root + "/objects"; + std::string object_det_topic_root = "obj_det"; + std::string object_det_topic = object_det_topic_root + "/objects"; mPubObjDet = mNhNs.advertise(object_det_topic, 1); NODELET_INFO_STREAM("Advertised on topic " << mPubObjDet.getTopic()); @@ -1817,7 +1823,7 @@ void ZEDWrapperNodelet::publishPoseFrame(tf2::Transform baseTransform, ros::Time void ZEDWrapperNodelet::publishImage(sensor_msgs::ImagePtr imgMsgPtr, sl::Mat img, image_transport::CameraPublisher& pubImg, sensor_msgs::CameraInfoPtr camInfoMsg, - string imgFrameId, ros::Time t) + std::string imgFrameId, ros::Time t) { camInfoMsg->header.stamp = t; sl_tools::imageToROSmsg(imgMsgPtr, img, imgFrameId, t); @@ -2088,7 +2094,7 @@ void ZEDWrapperNodelet::publishCamInfo(sensor_msgs::CameraInfoPtr camInfoMsg, ro } void ZEDWrapperNodelet::fillCamInfo(sl::Camera& zed, sensor_msgs::CameraInfoPtr leftCamInfoMsg, - sensor_msgs::CameraInfoPtr rightCamInfoMsg, string leftFrameId, string rightFrameId, + sensor_msgs::CameraInfoPtr rightCamInfoMsg, std::string leftFrameId, std::string rightFrameId, bool rawParam /*= false*/) { sl::CalibrationParameters zedParam; @@ -2190,7 +2196,7 @@ void ZEDWrapperNodelet::fillCamInfo(sl::Camera& zed, sensor_msgs::CameraInfoPtr rightCamInfoMsg->header.frame_id = rightFrameId; } -void ZEDWrapperNodelet::fillCamDepthInfo(sl::Camera& zed, sensor_msgs::CameraInfoPtr depth_info_msg, string frame_id) +void ZEDWrapperNodelet::fillCamDepthInfo(sl::Camera& zed, sensor_msgs::CameraInfoPtr depth_info_msg, std::string frame_id) { sl::CalibrationParameters zedParam; @@ -2882,7 +2888,7 @@ void ZEDWrapperNodelet::publishSensData(ros::Time t) // ----> Publish odometry tf only if enabled if (mPublishTf && mPosTrackingReady && new_imu_data) { - NODELET_DEBUG("Publishing TF"); + //NODELET_DEBUG("Publishing TF"); publishOdomFrame(mOdom2BaseTransf, ts_imu); // publish the base Frame in odometry frame @@ -4402,7 +4408,36 @@ void ZEDWrapperNodelet::clickedPtCallback(geometry_msgs::PointStampedConstPtr ms float X = msg->point.x; float Y = msg->point.y; float Z = msg->point.z; - NODELET_INFO_STREAM("Clicked 3D point: [" << X << "," << Y << "," << Z << "]"); + + NODELET_INFO_STREAM("Clicked 3D point [X FW, Y LF, Z UP]: [" << X << "," << Y << "," << Z << "]"); + + // ----> Transform the point from `map` frame to `left_camera_optical_frame` + double camX, camY, camZ; + try { + // Save the transformation + geometry_msgs::TransformStamped m2o = mTfBuffer->lookupTransform(mLeftCamOptFrameId, msg->header.frame_id, ros::Time(0), ros::Duration(0.1)); + + NODELET_INFO("'%s' -> '%s': {%.3f,%.3f,%.3f} {%.3f,%.3f,%.3f,%.3f}", msg->header.frame_id.c_str(), mLeftCamOptFrameId.c_str(), + m2o.transform.translation.x, m2o.transform.translation.y, m2o.transform.translation.z, + m2o.transform.rotation.x, m2o.transform.rotation.y, m2o.transform.rotation.z, m2o.transform.rotation.w); + + // Get the TF2 transformation + geometry_msgs::PointStamped ptCam; + + tf2::doTransform(*msg, ptCam, m2o); + + camX = ptCam.point.x; + camY = ptCam.point.y; + camZ = ptCam.point.z; + + NODELET_INFO("Point in camera coordinates [Z FW, X RG, Y DW]: {%.3f,%.3f,%.3f}", camX, camY, camZ); + } catch (tf2::TransformException& ex) { + NODELET_DEBUG_THROTTLE(1.0, "Transform error: %s", ex.what()); + NODELET_WARN_THROTTLE(1.0, "The tf from '%s' to '%s' is not available.", msg->header.frame_id.c_str(), mLeftCamOptFrameId.c_str()); + + return; + } + // <---- Transform the point from `map` frame to `left_camera_optical_frame` // ----> Project the point into 2D image coordinates sl::CalibrationParameters zedParam; @@ -4413,10 +4448,132 @@ void ZEDWrapperNodelet::clickedPtCallback(geometry_msgs::PointStampedConstPtr ms float c_x = zedParam.left_cam.cx; float c_y = zedParam.left_cam.cy; - float u = (-Y / X) * f_x + c_x; - float v = (-Z / X) * f_y + c_y; + float u = (camX / camZ) * f_x + c_x; + float v = ((camY / camZ) * f_y + c_y); NODELET_INFO_STREAM("Clicked point image coordinates: [" << u << "," << v << "]"); // <---- Project the point into 2D image coordinates + + // ----> Extract plane from clicked point + sl::Plane plane; + sl::ERROR_CODE err = mZed.findPlaneAtHit(sl::uint2(u, v), plane); + if (err != sl::ERROR_CODE::SUCCESS) { + NODELET_WARN_STREAM("Error extracting plane at click: " << sl::toString(err).c_str()); + return; + } + + sl::float3 center = plane.getCenter(); + sl::float2 dims = plane.getExtents(); + + NODELET_INFO("Found plane at point [%.3f,%.3f,%.3f] -> [%.3f,%.3f,%.3f - %.3fx%.3f]", X, Y, Z, center.x, center.y, center.z, dims[0], dims[1]); + // <---- Extract plane from clicked point + + // ----> Publish a blue sphere in the clicked point + visualization_msgs::Marker pt_marker; + // Set the frame ID and timestamp. See the TF tutorials for information on these. + static int hit_pt_id = 0; + pt_marker.header.stamp = ros::Time::now(); + // Set the marker action. Options are ADD and DELETE + pt_marker.action = visualization_msgs::Marker::ADD; + pt_marker.lifetime = ros::Duration(); + + // Set the namespace and id for this marker. This serves to create a unique ID + // Any marker sent with the same namespace and id will overwrite the old one + pt_marker.ns = "plane_hit_points"; + pt_marker.id = hit_pt_id++; + pt_marker.header.frame_id = mMapFrameId; + + // Set the marker type. + pt_marker.type = visualization_msgs::Marker::SPHERE; + + // Set the pose of the marker. This is a full 6DOF pose relative to the frame/time specified in the header + pt_marker.pose.position.x = X; + pt_marker.pose.position.y = Y; + pt_marker.pose.position.z = Z; + pt_marker.pose.orientation.x = 0.0; + pt_marker.pose.orientation.y = 0.0; + pt_marker.pose.orientation.z = 0.0; + pt_marker.pose.orientation.w = 1.0; + + // Set the scale of the marker -- 1x1x1 here means 1m on a side + pt_marker.scale.x = 0.025; + pt_marker.scale.y = 0.025; + pt_marker.scale.z = 0.025; + + // Set the color -- be sure to set alpha to something non-zero! + pt_marker.color.r = 0.2f; + pt_marker.color.g = 0.1f; + pt_marker.color.b = 0.75f; + pt_marker.color.a = 0.8; + + // Publish the marker + mPubMarker.publish(pt_marker); + // ----> Publish a blue sphere in the clicked point + + // ----> Publish the plane as green mesh + visualization_msgs::Marker plane_marker; + // Set the frame ID and timestamp. See the TF tutorials for information on these. + static int plane_mesh_id = 0; + plane_marker.header.stamp = ros::Time::now(); + // Set the marker action. Options are ADD and DELETE + plane_marker.action = visualization_msgs::Marker::ADD; + plane_marker.lifetime = ros::Duration(); + + // Set the namespace and id for this marker. This serves to create a unique ID + // Any marker sent with the same namespace and id will overwrite the old one + plane_marker.ns = "plane_meshes"; + plane_marker.id = plane_mesh_id++; + plane_marker.header.frame_id = mLeftCamFrameId; + + // Set the marker type. + plane_marker.type = visualization_msgs::Marker::TRIANGLE_LIST; + + // Set the pose of the marker. This is a full 6DOF pose relative to the frame/time specified in the header + plane_marker.pose.position.x = 0; + plane_marker.pose.position.y = 0; + plane_marker.pose.position.z = 0; + plane_marker.pose.orientation.x = 0.0; + plane_marker.pose.orientation.y = 0.0; + plane_marker.pose.orientation.z = 0.0; + plane_marker.pose.orientation.w = 1.0; + + // Set the color -- be sure to set alpha to something non-zero! + plane_marker.color.r = 0.10f; + plane_marker.color.g = 0.75f; + plane_marker.color.b = 0.20f; + plane_marker.color.a = 0.75; + + // Set the scale of the marker -- 1x1x1 here means 1m on a side + plane_marker.scale.x = 1.0; + plane_marker.scale.y = 1.0; + plane_marker.scale.z = 1.0; + + sl::Mesh mesh = plane.extractMesh(); + size_t triangCount = mesh.getNumberOfTriangles(); + size_t ptCount = triangCount * 3; + plane_marker.points.resize(ptCount); + plane_marker.colors.resize(ptCount); + + size_t ptIdx = 0; + for (size_t t = 0; t < triangCount; t++) { + for (int p = 0; p < 3; p++) { + uint vIdx = mesh.triangles[t][p]; + plane_marker.points[ptIdx].x = mesh.vertices[vIdx][0]; + plane_marker.points[ptIdx].y = mesh.vertices[vIdx][1]; + plane_marker.points[ptIdx].z = mesh.vertices[vIdx][2]; + + // Set the color -- be sure to set alpha to something non-zero! + plane_marker.colors[ptIdx].r = 0.10f; + plane_marker.colors[ptIdx].g = 0.75f; + plane_marker.colors[ptIdx].b = 0.20f; + plane_marker.colors[ptIdx].a = 0.75; + + ptIdx++; + } + } + + // Publish the marker + mPubMarker.publish(plane_marker); + // <---- Publish the plane as green mesh } } // namespace zed_nodelets From 1a2450e5d0cf1dbe87cdfd1f074828b580e2d045 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Mon, 4 Apr 2022 16:04:55 +0200 Subject: [PATCH 119/199] Plublish custom PlaneStamped topic --- .../include/zed_wrapper_nodelet.hpp | 1 + .../zed_nodelet/src/zed_wrapper_nodelet.cpp | 315 ++++++++++++------ 2 files changed, 207 insertions(+), 109 deletions(-) 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 12463084..d1820703 100644 --- a/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp +++ b/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp @@ -454,6 +454,7 @@ class ZEDWrapperNodelet : public nodelet::Nodelet { ros::Publisher mPubCamImuTransf; ros::Publisher mPubMarker; // Publisher for Rviz markers + ros::Publisher mPubPlane; // Publisher for detected planes // Subscribers ros::Subscriber mClickedPtSub; 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 46751f6e..62d24267 100644 --- a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp @@ -32,6 +32,7 @@ #include "zed_interfaces/Object.h" #include "zed_interfaces/ObjectsStamped.h" +#include //#define DEBUG_SENS_TS 1 @@ -157,8 +158,9 @@ void ZEDWrapperNodelet::onInit() std::string odom_path_topic = "path_odom"; std::string map_path_topic = "path_map"; - // Plane markers topic + // Extracted plane topics std::string marker_topic = "plane_marker"; + std::string plane_topic = "plane"; // Create camera info mRgbCamInfoMsg.reset(new sensor_msgs::CameraInfo()); @@ -477,9 +479,12 @@ void ZEDWrapperNodelet::onInit() mPubOdom = mNhNs.advertise(odometryTopic, 1); NODELET_INFO_STREAM("Advertised on topic " << mPubOdom.getTopic()); - // Rviz markers + // Rviz markers publisher mPubMarker = mNhNs.advertise(marker_topic, 10, true); + // Detected planes publisher + mPubPlane = mNhNs.advertise(plane_topic, 1); + // Camera Path if (mPathPubRate > 0) { mPubOdomPath = mNhNs.advertise(odom_path_topic, 1, true); @@ -4405,6 +4410,17 @@ void ZEDWrapperNodelet::processDetectedObjects(ros::Time t) void ZEDWrapperNodelet::clickedPtCallback(geometry_msgs::PointStampedConstPtr msg) { + // ----> Check for result subscribers + uint32_t markerSubNumber = mPubMarker.getNumSubscribers(); + uint32_t planeSubNumber = mPubMarker.getNumSubscribers(); + + if ((markerSubNumber + planeSubNumber) == 0) { + return; + } + // <---- Check for result subscribers + + ros::Time ts = ros::Time::now(); + float X = msg->point.x; float Y = msg->point.y; float Z = msg->point.z; @@ -4457,123 +4473,204 @@ void ZEDWrapperNodelet::clickedPtCallback(geometry_msgs::PointStampedConstPtr ms sl::Plane plane; sl::ERROR_CODE err = mZed.findPlaneAtHit(sl::uint2(u, v), plane); if (err != sl::ERROR_CODE::SUCCESS) { - NODELET_WARN_STREAM("Error extracting plane at click: " << sl::toString(err).c_str()); + NODELET_WARN("Error extracting plane at point [%.3f,%.3f,%.3f]: %s", X, Y, Z, sl::toString(err).c_str()); return; } sl::float3 center = plane.getCenter(); sl::float2 dims = plane.getExtents(); - NODELET_INFO("Found plane at point [%.3f,%.3f,%.3f] -> [%.3f,%.3f,%.3f - %.3fx%.3f]", X, Y, Z, center.x, center.y, center.z, dims[0], dims[1]); + if (dims[0] == 0 || dims[1] == 0) { + NODELET_INFO("Plane not found at point [%.3f,%.3f,%.3f]", X, Y, Z); + } + + NODELET_INFO("Found plane at point [%.3f,%.3f,%.3f] -> Center: [%.3f,%.3f,%.3f], Dims: %.3fx%.3f", X, Y, Z, center.x, center.y, center.z, dims[0], dims[1]); // <---- Extract plane from clicked point - // ----> Publish a blue sphere in the clicked point - visualization_msgs::Marker pt_marker; - // Set the frame ID and timestamp. See the TF tutorials for information on these. - static int hit_pt_id = 0; - pt_marker.header.stamp = ros::Time::now(); - // Set the marker action. Options are ADD and DELETE - pt_marker.action = visualization_msgs::Marker::ADD; - pt_marker.lifetime = ros::Duration(); - - // Set the namespace and id for this marker. This serves to create a unique ID - // Any marker sent with the same namespace and id will overwrite the old one - pt_marker.ns = "plane_hit_points"; - pt_marker.id = hit_pt_id++; - pt_marker.header.frame_id = mMapFrameId; - - // Set the marker type. - pt_marker.type = visualization_msgs::Marker::SPHERE; - - // Set the pose of the marker. This is a full 6DOF pose relative to the frame/time specified in the header - pt_marker.pose.position.x = X; - pt_marker.pose.position.y = Y; - pt_marker.pose.position.z = Z; - pt_marker.pose.orientation.x = 0.0; - pt_marker.pose.orientation.y = 0.0; - pt_marker.pose.orientation.z = 0.0; - pt_marker.pose.orientation.w = 1.0; - - // Set the scale of the marker -- 1x1x1 here means 1m on a side - pt_marker.scale.x = 0.025; - pt_marker.scale.y = 0.025; - pt_marker.scale.z = 0.025; - - // Set the color -- be sure to set alpha to something non-zero! - pt_marker.color.r = 0.2f; - pt_marker.color.g = 0.1f; - pt_marker.color.b = 0.75f; - pt_marker.color.a = 0.8; - - // Publish the marker - mPubMarker.publish(pt_marker); - // ----> Publish a blue sphere in the clicked point - - // ----> Publish the plane as green mesh - visualization_msgs::Marker plane_marker; - // Set the frame ID and timestamp. See the TF tutorials for information on these. - static int plane_mesh_id = 0; - plane_marker.header.stamp = ros::Time::now(); - // Set the marker action. Options are ADD and DELETE - plane_marker.action = visualization_msgs::Marker::ADD; - plane_marker.lifetime = ros::Duration(); - - // Set the namespace and id for this marker. This serves to create a unique ID - // Any marker sent with the same namespace and id will overwrite the old one - plane_marker.ns = "plane_meshes"; - plane_marker.id = plane_mesh_id++; - plane_marker.header.frame_id = mLeftCamFrameId; - - // Set the marker type. - plane_marker.type = visualization_msgs::Marker::TRIANGLE_LIST; - - // Set the pose of the marker. This is a full 6DOF pose relative to the frame/time specified in the header - plane_marker.pose.position.x = 0; - plane_marker.pose.position.y = 0; - plane_marker.pose.position.z = 0; - plane_marker.pose.orientation.x = 0.0; - plane_marker.pose.orientation.y = 0.0; - plane_marker.pose.orientation.z = 0.0; - plane_marker.pose.orientation.w = 1.0; - - // Set the color -- be sure to set alpha to something non-zero! - plane_marker.color.r = 0.10f; - plane_marker.color.g = 0.75f; - plane_marker.color.b = 0.20f; - plane_marker.color.a = 0.75; - - // Set the scale of the marker -- 1x1x1 here means 1m on a side - plane_marker.scale.x = 1.0; - plane_marker.scale.y = 1.0; - plane_marker.scale.z = 1.0; - - sl::Mesh mesh = plane.extractMesh(); - size_t triangCount = mesh.getNumberOfTriangles(); - size_t ptCount = triangCount * 3; - plane_marker.points.resize(ptCount); - plane_marker.colors.resize(ptCount); - - size_t ptIdx = 0; - for (size_t t = 0; t < triangCount; t++) { - for (int p = 0; p < 3; p++) { - uint vIdx = mesh.triangles[t][p]; - plane_marker.points[ptIdx].x = mesh.vertices[vIdx][0]; - plane_marker.points[ptIdx].y = mesh.vertices[vIdx][1]; - plane_marker.points[ptIdx].z = mesh.vertices[vIdx][2]; - - // Set the color -- be sure to set alpha to something non-zero! - plane_marker.colors[ptIdx].r = 0.10f; - plane_marker.colors[ptIdx].g = 0.75f; - plane_marker.colors[ptIdx].b = 0.20f; - plane_marker.colors[ptIdx].a = 0.75; - - ptIdx++; + if (markerSubNumber > 0) { + // ----> Publish a blue sphere in the clicked point + visualization_msgs::MarkerPtr pt_marker = boost::make_shared(); + // Set the frame ID and timestamp. See the TF tutorials for information on these. + static int hit_pt_id = 0; + pt_marker->header.stamp = ts; + // Set the marker action. Options are ADD and DELETE + pt_marker->action = visualization_msgs::Marker::ADD; + pt_marker->lifetime = ros::Duration(); + + // Set the namespace and id for this marker. This serves to create a unique ID + // Any marker sent with the same namespace and id will overwrite the old one + pt_marker->ns = "plane_hit_points"; + pt_marker->id = hit_pt_id++; + pt_marker->header.frame_id = mMapFrameId; + + // Set the marker type. + pt_marker->type = visualization_msgs::Marker::SPHERE; + + // Set the pose of the marker. This is a full 6DOF pose relative to the frame/time specified in the header + pt_marker->pose.position.x = X; + pt_marker->pose.position.y = Y; + pt_marker->pose.position.z = Z; + pt_marker->pose.orientation.x = 0.0; + pt_marker->pose.orientation.y = 0.0; + pt_marker->pose.orientation.z = 0.0; + pt_marker->pose.orientation.w = 1.0; + + // Set the scale of the marker -- 1x1x1 here means 1m on a side + pt_marker->scale.x = 0.025; + pt_marker->scale.y = 0.025; + pt_marker->scale.z = 0.025; + + // Set the color -- be sure to set alpha to something non-zero! + pt_marker->color.r = 0.2f; + pt_marker->color.g = 0.1f; + pt_marker->color.b = 0.75f; + pt_marker->color.a = 0.8; + + // Publish the marker + mPubMarker.publish(pt_marker); + // ----> Publish a blue sphere in the clicked point + + // ----> Publish the plane as green mesh + visualization_msgs::MarkerPtr plane_marker = boost::make_shared(); + // Set the frame ID and timestamp. See the TF tutorials for information on these. + static int plane_mesh_id = 0; + plane_marker->header.stamp = ts; + // Set the marker action. Options are ADD and DELETE + plane_marker->action = visualization_msgs::Marker::ADD; + plane_marker->lifetime = ros::Duration(); + + // Set the namespace and id for this marker. This serves to create a unique ID + // Any marker sent with the same namespace and id will overwrite the old one + plane_marker->ns = "plane_meshes"; + plane_marker->id = plane_mesh_id++; + plane_marker->header.frame_id = mLeftCamFrameId; + + // Set the marker type. + plane_marker->type = visualization_msgs::Marker::TRIANGLE_LIST; + + // Set the pose of the marker. This is a full 6DOF pose relative to the frame/time specified in the header + plane_marker->pose.position.x = 0; + plane_marker->pose.position.y = 0; + plane_marker->pose.position.z = 0; + plane_marker->pose.orientation.x = 0.0; + plane_marker->pose.orientation.y = 0.0; + plane_marker->pose.orientation.z = 0.0; + plane_marker->pose.orientation.w = 1.0; + + // Set the color -- be sure to set alpha to something non-zero! + plane_marker->color.r = 0.10f; + plane_marker->color.g = 0.75f; + plane_marker->color.b = 0.20f; + plane_marker->color.a = 0.75; + + // Set the scale of the marker -- 1x1x1 here means 1m on a side + plane_marker->scale.x = 1.0; + plane_marker->scale.y = 1.0; + plane_marker->scale.z = 1.0; + + sl::Mesh mesh = plane.extractMesh(); + size_t triangCount = mesh.getNumberOfTriangles(); + size_t ptCount = triangCount * 3; + plane_marker->points.resize(ptCount); + plane_marker->colors.resize(ptCount); + + size_t ptIdx = 0; + for (size_t t = 0; t < triangCount; t++) { + for (int p = 0; p < 3; p++) { + uint vIdx = mesh.triangles[t][p]; + plane_marker->points[ptIdx].x = mesh.vertices[vIdx][0]; + plane_marker->points[ptIdx].y = mesh.vertices[vIdx][1]; + plane_marker->points[ptIdx].z = mesh.vertices[vIdx][2]; + + // Set the color -- be sure to set alpha to something non-zero! + plane_marker->colors[ptIdx].r = 0.10f; + plane_marker->colors[ptIdx].g = 0.75f; + plane_marker->colors[ptIdx].b = 0.20f; + plane_marker->colors[ptIdx].a = 0.75; + + ptIdx++; + } + } + + // Publish the marker + mPubMarker.publish(plane_marker); + // <---- Publish the plane as green mesh + } + + if (planeSubNumber > 0) { + // ----> Publish the plane as custom message + + zed_interfaces::PlaneStampedPtr planeMsg = boost::make_shared(); + planeMsg->header.stamp = ts; + planeMsg->header.frame_id = mLeftCamFrameId; + + // Plane equation + sl::float4 sl_coeff = plane.getPlaneEquation(); + planeMsg->coefficients.coef[0] = static_cast(sl_coeff[0]); + planeMsg->coefficients.coef[1] = static_cast(sl_coeff[1]); + planeMsg->coefficients.coef[2] = static_cast(sl_coeff[2]); + planeMsg->coefficients.coef[3] = static_cast(sl_coeff[3]); + + // Plane Normal + sl::float3 sl_normal = plane.getNormal(); + planeMsg->normal.x = sl_normal[0]; + planeMsg->normal.y = sl_normal[1]; + planeMsg->normal.z = sl_normal[2]; + + // Plane Center + sl::float3 sl_center = plane.getCenter(); + planeMsg->center.x = sl_center[0]; + planeMsg->center.y = sl_center[1]; + planeMsg->center.z = sl_center[2]; + + // Plane extents + sl::float3 sl_extents = plane.getExtents(); + planeMsg->extents[0] = sl_extents[0]; + planeMsg->extents[1] = sl_extents[1]; + + // Plane pose + sl::Pose sl_pose = plane.getPose(); + sl::Orientation sl_rot = sl_pose.getOrientation(); + sl::Translation sl_tr = sl_pose.getTranslation(); + + planeMsg->pose.rotation.x = sl_rot.ox; + planeMsg->pose.rotation.y = sl_rot.oy; + planeMsg->pose.rotation.z = sl_rot.oz; + planeMsg->pose.rotation.w = sl_rot.ow; + + planeMsg->pose.translation.x = sl_tr.x; + planeMsg->pose.translation.y = sl_tr.y; + planeMsg->pose.translation.z = sl_tr.z; + + // Plane Bounds + std::vector sl_bounds = plane.getBounds(); + planeMsg->bounds.points.resize(sl_bounds.size()); + memcpy(planeMsg->bounds.points.data(), sl_bounds.data(), 3 * sl_bounds.size()*sizeof(float)); + + // Plane mesh + sl::Mesh sl_mesh = plane.extractMesh(); + size_t triangCount = sl_mesh.triangles.size(); + size_t ptsCount = sl_mesh.vertices.size(); + planeMsg->mesh.triangles.resize(triangCount); + planeMsg->mesh.vertices.resize(ptsCount); + + // memcpy not allowed because data types are different + for (size_t i = 0; i < triangCount; i++) { + planeMsg->mesh.triangles[i].vertex_indices[0] = sl_mesh.triangles[i][0]; + planeMsg->mesh.triangles[i].vertex_indices[1] = sl_mesh.triangles[i][1]; + planeMsg->mesh.triangles[i].vertex_indices[2] = sl_mesh.triangles[i][2]; } - } - // Publish the marker - mPubMarker.publish(plane_marker); - // <---- Publish the plane as green mesh + // memcpy not allowed because data types are different + for (size_t i = 0; i < ptsCount; i++) { + planeMsg->mesh.vertices[i].x = sl_mesh.vertices[i][0]; + planeMsg->mesh.vertices[i].y = sl_mesh.vertices[i][1]; + planeMsg->mesh.vertices[i].z = sl_mesh.vertices[i][2]; + } + + mPubPlane.publish(planeMsg); + // <---- Publish the plane as custom message + } } } // namespace zed_nodelets From 1bd037cc6a6e7b444cd1007c96c809dacb555023 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Wed, 13 Apr 2022 12:48:33 +0200 Subject: [PATCH 120/199] Create stale_issues.yml --- .github/workflows/stale_issues.yml | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) create mode 100644 .github/workflows/stale_issues.yml diff --git a/.github/workflows/stale_issues.yml b/.github/workflows/stale_issues.yml new file mode 100644 index 00000000..91aead10 --- /dev/null +++ b/.github/workflows/stale_issues.yml @@ -0,0 +1,19 @@ +name: 'Stale issue handler' +on: + workflow_dispatch: + schedule: + - cron: '0 0 * * *' + +jobs: + stale: + runs-on: ubuntu-latest + steps: + - uses: actions/stale@main + id: stale + with: + stale-issue-message: 'This issue is stale because it has been open 30 days with no activity. Remove stale label or comment or this will be closed in 5 days' + days-before-stale: 30 + days-before-close: 5 + exempt-issue-labels: 'feature_request' + - name: Print outputs + run: echo ${{ join(steps.stale.outputs.*, ',') }} From 26c9b882890d3eedca3e209aefdc195eebd923b3 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Wed, 13 Apr 2022 12:57:26 +0200 Subject: [PATCH 121/199] Update stale_issues.yml --- .github/workflows/stale_issues.yml | 1 + 1 file changed, 1 insertion(+) diff --git a/.github/workflows/stale_issues.yml b/.github/workflows/stale_issues.yml index 91aead10..d627ad27 100644 --- a/.github/workflows/stale_issues.yml +++ b/.github/workflows/stale_issues.yml @@ -14,6 +14,7 @@ jobs: stale-issue-message: 'This issue is stale because it has been open 30 days with no activity. Remove stale label or comment or this will be closed in 5 days' days-before-stale: 30 days-before-close: 5 + operations-per-run: 300 exempt-issue-labels: 'feature_request' - name: Print outputs run: echo ${{ join(steps.stale.outputs.*, ',') }} From 42ae7daf5118991ae252c1a642591987a8974650 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Wed, 13 Apr 2022 14:12:15 +0200 Subject: [PATCH 122/199] Delete stale.yml --- .github/stale.yml | 62 ----------------------------------------------- 1 file changed, 62 deletions(-) delete mode 100644 .github/stale.yml diff --git a/.github/stale.yml b/.github/stale.yml deleted file mode 100644 index 1f20f59a..00000000 --- a/.github/stale.yml +++ /dev/null @@ -1,62 +0,0 @@ -# Configuration for probot-stale - https://github.com/probot/stale - -# Number of days of inactivity before an Issue or Pull Request becomes stale -daysUntilStale: 28 - -# Number of days of inactivity before an Issue or Pull Request with the stale label is closed. -# Set to false to disable. If disabled, issues still need to be closed manually, but will remain marked as stale. -daysUntilClose: 3 - -# Only issues or pull requests with all of these labels are check if stale. Defaults to `[]` (disabled) -onlyLabels: [] - -# Issues or Pull Requests with these labels will never be considered stale. Set to `[]` to disable -exemptLabels: - - pinned - - feature_request - -# Set to true to ignore issues in a project (defaults to false) -exemptProjects: false - -# Set to true to ignore issues in a milestone (defaults to false) -exemptMilestones: false - -# Set to true to ignore issues with an assignee (defaults to false) -exemptAssignees: false - -# Label to use when marking as stale -staleLabel: closed_for_stale - -# Comment to post when marking as stale. Set to `false` to disable -markComment: > - This issue has been automatically marked as stale because it has not had - recent activity. It will be closed in 72h if no further activity occurs. Thank you - for your contributions. - -# Comment to post when removing the stale label. -# unmarkComment: > -# Your comment here. - -# Comment to post when closing a stale Issue or Pull Request. -closeComment: > - This issue has been closed automatically because it has not had - recent activity. Please feel free to re-open in case new info - is available or further job is required. - -# Limit the number of actions per hour, from 1-30. Default is 30 -limitPerRun: 100 - -# Limit to only `issues` or `pulls` -only: issues - -# Optionally, specify configuration settings that are specific to just 'issues' or 'pulls': -# pulls: -# daysUntilStale: 30 -# markComment: > -# This pull request has been automatically marked as stale because it has not had -# recent activity. It will be closed if no further activity occurs. Thank you -# for your contributions. - -# issues: -# exemptLabels: -# - confirmed From 727acfd253e2d25077711e013fde2a9c9125b307 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Wed, 13 Apr 2022 14:18:01 +0200 Subject: [PATCH 123/199] Update stale_issues.yml --- .github/workflows/stale_issues.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/stale_issues.yml b/.github/workflows/stale_issues.yml index d627ad27..e5700186 100644 --- a/.github/workflows/stale_issues.yml +++ b/.github/workflows/stale_issues.yml @@ -16,5 +16,5 @@ jobs: days-before-close: 5 operations-per-run: 300 exempt-issue-labels: 'feature_request' - - name: Print outputs - run: echo ${{ join(steps.stale.outputs.*, ',') }} + exempt-pr-labels: 'feature_request' + enable-statistics: 'true' From 969b1af19b8fb33401ff0f95a1143629e40bff42 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Wed, 13 Apr 2022 14:18:23 +0200 Subject: [PATCH 124/199] Update stale_issues.yml --- .github/workflows/stale_issues.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/stale_issues.yml b/.github/workflows/stale_issues.yml index e5700186..c9164ce9 100644 --- a/.github/workflows/stale_issues.yml +++ b/.github/workflows/stale_issues.yml @@ -9,7 +9,7 @@ jobs: runs-on: ubuntu-latest steps: - uses: actions/stale@main - id: stale + id: stale_issue_and_pr with: stale-issue-message: 'This issue is stale because it has been open 30 days with no activity. Remove stale label or comment or this will be closed in 5 days' days-before-stale: 30 From 8e5adbd37fa293a0712bb741ed3d9e8088ca2e38 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Wed, 13 Apr 2022 14:22:25 +0200 Subject: [PATCH 125/199] Update stale_issues.yml --- .github/workflows/stale_issues.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/stale_issues.yml b/.github/workflows/stale_issues.yml index c9164ce9..36b42fed 100644 --- a/.github/workflows/stale_issues.yml +++ b/.github/workflows/stale_issues.yml @@ -2,14 +2,14 @@ name: 'Stale issue handler' on: workflow_dispatch: schedule: - - cron: '0 0 * * *' + - cron: '30 1 * * *' jobs: stale: runs-on: ubuntu-latest steps: - uses: actions/stale@main - id: stale_issue_and_pr + id: stale with: stale-issue-message: 'This issue is stale because it has been open 30 days with no activity. Remove stale label or comment or this will be closed in 5 days' days-before-stale: 30 From 0bd5f60475ed84c2d36b33495ae175bc150fcb14 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Wed, 13 Apr 2022 14:59:47 +0200 Subject: [PATCH 126/199] Update stale_issues.yml --- .github/workflows/stale_issues.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/stale_issues.yml b/.github/workflows/stale_issues.yml index 36b42fed..1031b509 100644 --- a/.github/workflows/stale_issues.yml +++ b/.github/workflows/stale_issues.yml @@ -2,7 +2,7 @@ name: 'Stale issue handler' on: workflow_dispatch: schedule: - - cron: '30 1 * * *' + - cron: '00 15 * * *' jobs: stale: From dcb97709afb19a735fdb4f5f2de545d6b4ce08c6 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Wed, 13 Apr 2022 16:23:09 +0200 Subject: [PATCH 127/199] Update stale_issues.yml --- .github/workflows/stale_issues.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/stale_issues.yml b/.github/workflows/stale_issues.yml index 1031b509..68fbe5b9 100644 --- a/.github/workflows/stale_issues.yml +++ b/.github/workflows/stale_issues.yml @@ -2,7 +2,7 @@ name: 'Stale issue handler' on: workflow_dispatch: schedule: - - cron: '00 15 * * *' + - cron: '30 16 * * *' jobs: stale: From bce4b385e3c6b517d4ba9a16ec02ed6b09bbb990 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Wed, 13 Apr 2022 17:54:44 +0200 Subject: [PATCH 128/199] Update stale_issues.yml --- .github/workflows/stale_issues.yml | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/.github/workflows/stale_issues.yml b/.github/workflows/stale_issues.yml index 68fbe5b9..eafc35f2 100644 --- a/.github/workflows/stale_issues.yml +++ b/.github/workflows/stale_issues.yml @@ -2,7 +2,7 @@ name: 'Stale issue handler' on: workflow_dispatch: schedule: - - cron: '30 16 * * *' + - cron: '00 00 * * *' jobs: stale: @@ -11,10 +11,13 @@ jobs: - uses: actions/stale@main id: stale with: - stale-issue-message: 'This issue is stale because it has been open 30 days with no activity. Remove stale label or comment or this will be closed in 5 days' + stale-issue-message: 'This issue is stale because it has been open 30 days with no activity. Remove stale label or comment otherwise it will be automatically closed in 5 days' + stale-pr-message: 'This PR is stale because it has been open 30 days with no activity. Remove stale label or comment otherwise it will be automatically closed in 5 days' days-before-stale: 30 days-before-close: 5 operations-per-run: 300 exempt-issue-labels: 'feature_request' exempt-pr-labels: 'feature_request' enable-statistics: 'true' + close-issue-label: 'closed_for_stale' + close-pr-label: 'closed_for_stale' From b9b056bc4409f45d8d12a0ffb8aa46b98f3073dd Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Wed, 20 Apr 2022 11:44:08 +0200 Subject: [PATCH 129/199] Update stale_issues.yml --- .github/workflows/stale_issues.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/stale_issues.yml b/.github/workflows/stale_issues.yml index eafc35f2..9e01bd59 100644 --- a/.github/workflows/stale_issues.yml +++ b/.github/workflows/stale_issues.yml @@ -15,7 +15,7 @@ jobs: stale-pr-message: 'This PR is stale because it has been open 30 days with no activity. Remove stale label or comment otherwise it will be automatically closed in 5 days' days-before-stale: 30 days-before-close: 5 - operations-per-run: 300 + operations-per-run: 1500 exempt-issue-labels: 'feature_request' exempt-pr-labels: 'feature_request' enable-statistics: 'true' From 613725462556632213c7f3bdf8307ac78867028a Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Tue, 26 Apr 2022 15:32:08 +0200 Subject: [PATCH 130/199] Minor fix --- zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp | 2 ++ 1 file changed, 2 insertions(+) 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 62d24267..e701688c 100644 --- a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp @@ -569,6 +569,8 @@ void ZEDWrapperNodelet::onInit() // Subscribers mClickedPtSub = mNhNs.subscribe(mClickedPtTopic, 10, &ZEDWrapperNodelet::clickedPtCallback, this); + NODELET_INFO_STREAM("Subscribed to topic " << mClickedPtTopic.c_str() ); + // Services mSrvSetInitPose = mNhNs.advertiseService("set_pose", &ZEDWrapperNodelet::on_set_pose, this); mSrvResetOdometry = mNhNs.advertiseService("reset_odometry", &ZEDWrapperNodelet::on_reset_odometry, this); From 555c93fe82eee068a848d193c60ff8c50acea7cb Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Tue, 26 Apr 2022 15:36:03 +0200 Subject: [PATCH 131/199] Update latest_changes.md --- latest_changes.md | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/latest_changes.md b/latest_changes.md index 668f0eb5..82889a9f 100644 --- a/latest_changes.md +++ b/latest_changes.md @@ -1,10 +1,16 @@ LATEST CHANGES ============== +2022-04-26 +---------- +- Add Plane Detection. See [ZED Documentation](https://www.stereolabs.com/docs/ros/plane_detection/) + 2022-03-30 +---------- - Fix wrong TF broadcasting when calling the `set_pose`, `reset_tracking`, and `reset_odometry` services. Now the initial odometry is coherent with the new starting point. 2022-03-28 +---------- - Add parameter `sensors/max_pub_rate` to set the maximum publishing frequency of sensors data - Improve Sensors thread From 2a9b39caaa787dded9d2bd373c60d16031628a8f Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Tue, 26 Apr 2022 16:09:44 +0200 Subject: [PATCH 132/199] =?UTF-8?q?Update=20zed-ros-interfaces=C3=B9?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- zed-ros-interfaces | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/zed-ros-interfaces b/zed-ros-interfaces index d27ba2f4..aa43a2e2 160000 --- a/zed-ros-interfaces +++ b/zed-ros-interfaces @@ -1 +1 @@ -Subproject commit d27ba2f4eb19b48f84bc705cfc6ea994f5189a8e +Subproject commit aa43a2e2ae0fa7cfd10708ba0b4159da025fb8e9 From c3d3ece3e7a2601302c46fc810e661b08e4b546a Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Tue, 26 Apr 2022 17:02:12 +0200 Subject: [PATCH 133/199] Fix plane detection issue with downsampling factor --- .../src/zed_nodelet/src/zed_wrapper_nodelet.cpp | 11 ++++++----- zed_wrapper/params/common.yaml | 2 +- 2 files changed, 7 insertions(+), 6 deletions(-) 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 e701688c..97e150bd 100644 --- a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp @@ -569,7 +569,7 @@ void ZEDWrapperNodelet::onInit() // Subscribers mClickedPtSub = mNhNs.subscribe(mClickedPtTopic, 10, &ZEDWrapperNodelet::clickedPtCallback, this); - NODELET_INFO_STREAM("Subscribed to topic " << mClickedPtTopic.c_str() ); + NODELET_INFO_STREAM("Subscribed to topic " << mClickedPtTopic.c_str()); // Services mSrvSetInitPose = mNhNs.advertiseService("set_pose", &ZEDWrapperNodelet::on_set_pose, this); @@ -4466,8 +4466,8 @@ void ZEDWrapperNodelet::clickedPtCallback(geometry_msgs::PointStampedConstPtr ms float c_x = zedParam.left_cam.cx; float c_y = zedParam.left_cam.cy; - float u = (camX / camZ) * f_x + c_x; - float v = ((camY / camZ) * f_y + c_y); + float u = ((camX / camZ) * f_x + c_x) / mCamImageResizeFactor; + float v = ((camY / camZ) * f_y + c_y) / mCamImageResizeFactor; NODELET_INFO_STREAM("Clicked point image coordinates: [" << u << "," << v << "]"); // <---- Project the point into 2D image coordinates @@ -4484,6 +4484,7 @@ void ZEDWrapperNodelet::clickedPtCallback(geometry_msgs::PointStampedConstPtr ms if (dims[0] == 0 || dims[1] == 0) { NODELET_INFO("Plane not found at point [%.3f,%.3f,%.3f]", X, Y, Z); + return; } NODELET_INFO("Found plane at point [%.3f,%.3f,%.3f] -> Center: [%.3f,%.3f,%.3f], Dims: %.3fx%.3f", X, Y, Z, center.x, center.y, center.z, dims[0], dims[1]); @@ -4647,7 +4648,7 @@ void ZEDWrapperNodelet::clickedPtCallback(geometry_msgs::PointStampedConstPtr ms // Plane Bounds std::vector sl_bounds = plane.getBounds(); planeMsg->bounds.points.resize(sl_bounds.size()); - memcpy(planeMsg->bounds.points.data(), sl_bounds.data(), 3 * sl_bounds.size()*sizeof(float)); + memcpy(planeMsg->bounds.points.data(), sl_bounds.data(), 3 * sl_bounds.size() * sizeof(float)); // Plane mesh sl::Mesh sl_mesh = plane.extractMesh(); @@ -4670,7 +4671,7 @@ void ZEDWrapperNodelet::clickedPtCallback(geometry_msgs::PointStampedConstPtr ms planeMsg->mesh.vertices[i].z = sl_mesh.vertices[i][2]; } - mPubPlane.publish(planeMsg); + mPubPlane.publish(planeMsg); // <---- Publish the plane as custom message } } diff --git a/zed_wrapper/params/common.yaml b/zed_wrapper/params/common.yaml index f2fb207d..3228e8d5 100644 --- a/zed_wrapper/params/common.yaml +++ b/zed_wrapper/params/common.yaml @@ -37,7 +37,7 @@ video: extrinsic_in_camera_frame: true # if `false` extrinsic parameter in `camera_info` will use ROS native frame (X FORWARD, Z UP) instead of the camera frame (Z FORWARD, Y DOWN) [`true` use old behavior as for version < v3.1] depth: - quality: 1 # '0': NONE, '1': PERFORMANCE, '2': QUALITY, '3': ULTRA, '4': NEURAL + quality: 4 # '0': NONE, '1': PERFORMANCE, '2': QUALITY, '3': ULTRA, '4': NEURAL sensing_mode: 0 # '0': STANDARD, '1': FILL (not use FILL for robotic applications) depth_stabilization: 1 # `0`: disabled, `1`: enabled openni_depth_mode: false # 'false': 32bit float meters, 'true': 16bit uchar millimeters From 94bc81c316b791d265807a0c026ff6c345cb3073 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Tue, 26 Apr 2022 17:48:42 +0200 Subject: [PATCH 134/199] Fix default depth mode --- zed_wrapper/params/common.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/zed_wrapper/params/common.yaml b/zed_wrapper/params/common.yaml index 3228e8d5..f2fb207d 100644 --- a/zed_wrapper/params/common.yaml +++ b/zed_wrapper/params/common.yaml @@ -37,7 +37,7 @@ video: extrinsic_in_camera_frame: true # if `false` extrinsic parameter in `camera_info` will use ROS native frame (X FORWARD, Z UP) instead of the camera frame (Z FORWARD, Y DOWN) [`true` use old behavior as for version < v3.1] depth: - quality: 4 # '0': NONE, '1': PERFORMANCE, '2': QUALITY, '3': ULTRA, '4': NEURAL + quality: 1 # '0': NONE, '1': PERFORMANCE, '2': QUALITY, '3': ULTRA, '4': NEURAL sensing_mode: 0 # '0': STANDARD, '1': FILL (not use FILL for robotic applications) depth_stabilization: 1 # `0`: disabled, `1`: enabled openni_depth_mode: false # 'false': 32bit float meters, 'true': 16bit uchar millimeters From 0225df03072d4d33e955c78b2a83b684bd75d422 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Thu, 21 Jul 2022 19:24:45 +0200 Subject: [PATCH 135/199] Fix TF timestamp in SVO mode --- latest_changes.md | 4 ++++ .../zed_nodelet/src/zed_wrapper_nodelet.cpp | 21 +++++++++++-------- zed_wrapper/params/common.yaml | 2 +- zed_wrapper/params/zed2i.yaml | 2 +- 4 files changed, 18 insertions(+), 11 deletions(-) diff --git a/latest_changes.md b/latest_changes.md index 82889a9f..d0693302 100644 --- a/latest_changes.md +++ b/latest_changes.md @@ -1,6 +1,10 @@ LATEST CHANGES ============== +2022-07-21 +---------- +- Fix TF timestamp issue in SVO mode + 2022-04-26 ---------- - Add Plane Detection. See [ZED Documentation](https://www.stereolabs.com/docs/ros/plane_detection/) 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 97e150bd..c3237336 100644 --- a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp @@ -2893,17 +2893,19 @@ void ZEDWrapperNodelet::publishSensData(ros::Time t) return; } - // ----> Publish odometry tf only if enabled - if (mPublishTf && mPosTrackingReady && new_imu_data) { - //NODELET_DEBUG("Publishing TF"); + // ----> Publish odometry tf only if enabled and not in SVO mode + if(!mSvoMode) { + if (mPublishTf && mPosTrackingReady && new_imu_data) { + //NODELET_DEBUG("Publishing TF"); - publishOdomFrame(mOdom2BaseTransf, ts_imu); // publish the base Frame in odometry frame + publishOdomFrame(mOdom2BaseTransf, ts_imu); // publish the base Frame in odometry frame - if (mPublishMapTf) { - publishPoseFrame(mMap2OdomTransf, ts_imu); // publish the odometry Frame in map frame + if (mPublishMapTf) { + publishPoseFrame(mMap2OdomTransf, ts_imu); // publish the odometry Frame in map frame + } } } - // <---- Publish odometry tf only if enabled + // <---- Publish odometry tf only if enabled and not in SVO mode if (mPublishImuTf && !mStaticImuFramePublished) { NODELET_DEBUG("Publishing static IMU TF"); @@ -3726,7 +3728,8 @@ void ZEDWrapperNodelet::device_poll_thread_func() } } - if (mZedRealCamModel == sl::MODEL::ZED) { + // Note: in SVO mode the TF must not be broadcasted at the same frequency of the IMU data to avoid timestamp issues + if (mZedRealCamModel == sl::MODEL::ZED || mSvoMode) { // Publish pose tf only if enabled if (mPublishTf) { // Note, the frame is published, but its values will only change if @@ -3797,7 +3800,7 @@ void ZEDWrapperNodelet::device_poll_thread_func() } else { NODELET_DEBUG_THROTTLE(5.0, "No topics subscribed by users"); - if (mZedRealCamModel == sl::MODEL::ZED || !mPublishImuTf) { + if (mSvoMode || mZedRealCamModel == sl::MODEL::ZED || !mPublishImuTf) { // Publish odometry tf only if enabled if (mPublishTf) { ros::Time t; diff --git a/zed_wrapper/params/common.yaml b/zed_wrapper/params/common.yaml index 3228e8d5..f2fb207d 100644 --- a/zed_wrapper/params/common.yaml +++ b/zed_wrapper/params/common.yaml @@ -37,7 +37,7 @@ video: extrinsic_in_camera_frame: true # if `false` extrinsic parameter in `camera_info` will use ROS native frame (X FORWARD, Z UP) instead of the camera frame (Z FORWARD, Y DOWN) [`true` use old behavior as for version < v3.1] depth: - quality: 4 # '0': NONE, '1': PERFORMANCE, '2': QUALITY, '3': ULTRA, '4': NEURAL + quality: 1 # '0': NONE, '1': PERFORMANCE, '2': QUALITY, '3': ULTRA, '4': NEURAL sensing_mode: 0 # '0': STANDARD, '1': FILL (not use FILL for robotic applications) depth_stabilization: 1 # `0`: disabled, `1`: enabled openni_depth_mode: false # 'false': 32bit float meters, 'true': 16bit uchar millimeters diff --git a/zed_wrapper/params/zed2i.yaml b/zed_wrapper/params/zed2i.yaml index a49a3981..12840b5e 100644 --- a/zed_wrapper/params/zed2i.yaml +++ b/zed_wrapper/params/zed2i.yaml @@ -7,7 +7,7 @@ general: depth: min_depth: 0.3 # Min: 0.2, Max: 3.0 - Default 0.7 - Note: reducing this value wil require more computational power and GPU memory - max_depth: 20.0 # Max: 40.0 + max_depth: 5.0 # Max: 40.0 pos_tracking: imu_fusion: true # enable/disable IMU fusion. When set to false, only the optical odometry will be used. From d267627cc19a08983e05e090f49d9d45ea4dc051 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Thu, 13 Oct 2022 10:11:24 +0200 Subject: [PATCH 136/199] Update zed_wrapper_nodelet.cpp --- zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 c3237336..fa8cfa90 100644 --- a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp @@ -2961,7 +2961,7 @@ void ZEDWrapperNodelet::publishSensData(ros::Time t) old_ts = pressMsg->header.stamp; #endif pressMsg->header.frame_id = mBaroFrameId; - pressMsg->fluid_pressure = sens_data.barometer.pressure * 1e2; // Pascal + pressMsg->fluid_pressure = sens_data.barometer.pressure; // Pascal pressMsg->variance = 1.0585e-2; sensors_data_published = true; From c684704cd2deed9d3587e482b5993039ca4da7c9 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Thu, 13 Oct 2022 10:13:36 +0200 Subject: [PATCH 137/199] Update latest_changes.md --- latest_changes.md | 3 +++ 1 file changed, 3 insertions(+) diff --git a/latest_changes.md b/latest_changes.md index d0693302..886761ef 100644 --- a/latest_changes.md +++ b/latest_changes.md @@ -1,6 +1,9 @@ LATEST CHANGES ============== +2022-10-13 +- Fix units for Atmospheric pressure data. Now the value is correctly published in `Pascal` according to the [topic specification](http://docs.ros.org/en/melodic/api/sensor_msgs/html/msg/FluidPressure.html). + 2022-07-21 ---------- - Fix TF timestamp issue in SVO mode From c37f65dfec547d6739d94c6109c3eb3fc7fecbeb Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Tue, 18 Oct 2022 12:47:28 +0200 Subject: [PATCH 138/199] Update changelog --- latest_changes.md => CHANGELOG.rst | 32 +++++++++--------------------- 1 file changed, 9 insertions(+), 23 deletions(-) rename latest_changes.md => CHANGELOG.rst (98%) diff --git a/latest_changes.md b/CHANGELOG.rst similarity index 98% rename from latest_changes.md rename to CHANGELOG.rst index 886761ef..5e823bce 100644 --- a/latest_changes.md +++ b/CHANGELOG.rst @@ -1,29 +1,15 @@ -LATEST CHANGES -============== +CHANGELOG +========= -2022-10-13 -- Fix units for Atmospheric pressure data. Now the value is correctly published in `Pascal` according to the [topic specification](http://docs.ros.org/en/melodic/api/sensor_msgs/html/msg/FluidPressure.html). - -2022-07-21 ----------- -- Fix TF timestamp issue in SVO mode - -2022-04-26 ----------- -- Add Plane Detection. See [ZED Documentation](https://www.stereolabs.com/docs/ros/plane_detection/) - -2022-03-30 ----------- -- Fix wrong TF broadcasting when calling the `set_pose`, `reset_tracking`, and `reset_odometry` services. Now the initial odometry is coherent with the new starting point. - -2022-03-28 ----------- +v3.8.x +------ +- Fix the frame links of barometer, magnetometer, and temperature sensors for ZED2i - Add parameter `sensors/max_pub_rate` to set the maximum publishing frequency of sensors data - Improve Sensors thread - -2022-03-16 ------------ -- Fix the frame links of barometer, magnetometer, and temperature sensors for ZED2i +- Fix wrong TF broadcasting when calling the `set_pose`, `reset_tracking`, and `reset_odometry` services. Now the initial odometry is coherent with the new starting point. +- Add Plane Detection. See [ZED Documentation](https://www.stereolabs.com/docs/ros/plane_detection/) +- Fix TF timestamp issue in SVO mode +- Fix units for Atmospheric pressure data. Now the value is correctly published in `Pascal` according to the [topic specification](http://docs.ros.org/en/melodic/api/sensor_msgs/html/msg/FluidPressure.html). v3.7.x --------- From 60e7bd741f2ed5ca5cc5c5f34d86b795d297fb0f Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Tue, 18 Oct 2022 12:49:19 +0200 Subject: [PATCH 139/199] Update README.md --- README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index e50ccf6d..8a773642 100644 --- a/README.md +++ b/README.md @@ -17,13 +17,13 @@ This package lets you use the ZED stereo camera with ROS. It outputs the camera ### Prerequisites - Ubuntu 20.04 -- [ZED SDK **≥ 3.7**](https://www.stereolabs.com/developers/) and its dependency [CUDA](https://developer.nvidia.com/cuda-downloads) +- [ZED SDK **≥ 3.8**](https://www.stereolabs.com/developers/) and its dependency [CUDA](https://developer.nvidia.com/cuda-downloads) - [ROS Noetic](http://wiki.ros.org/noetic/Installation/Ubuntu) or - Ubuntu 18.04 -- [ZED SDK **≥ 3.7**](https://www.stereolabs.com/developers/) and its dependency [CUDA](https://developer.nvidia.com/cuda-downloads) +- [ZED SDK **≥ 3.8**](https://www.stereolabs.com/developers/) and its dependency [CUDA](https://developer.nvidia.com/cuda-downloads) - [ROS Melodic](http://wiki.ros.org/melodic/Installation/Ubuntu) ### Build the repository From acf1767c6ce224771a3f839e9b3b0309dc417fec Mon Sep 17 00:00:00 2001 From: AndersonRayner Date: Wed, 4 Jan 2023 16:59:56 -0800 Subject: [PATCH 140/199] Fix general/zed_id printout Fixes the Camera ID printout on node boot --- zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 fa8cfa90..84942e1d 100644 --- a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp @@ -624,7 +624,7 @@ void ZEDWrapperNodelet::readParameters() mNhNs.getParam("general/gpu_id", mGpuId); NODELET_INFO_STREAM(" * Gpu ID\t\t\t-> " << mGpuId); mNhNs.getParam("general/zed_id", mZedId); - NODELET_INFO_STREAM(" * Camera ID\t\t\t-> " << mGpuId); + NODELET_INFO_STREAM(" * Camera ID\t\t\t-> " << mZedId); mNhNs.getParam("general/verbose", mVerbose); NODELET_INFO_STREAM(" * Verbose\t\t\t-> " << (mVerbose ? "ENABLED" : "DISABLED")); mNhNs.param("general/camera_flip", mCameraFlip, false); From 896eb1ba2e60cee77440cbed254f66ac06a1e7d7 Mon Sep 17 00:00:00 2001 From: walter Date: Fri, 31 Mar 2023 09:42:04 +0200 Subject: [PATCH 141/199] Update CMakeLists.txt files --- zed_nodelets/CMakeLists.txt | 7 +++++-- zed_ros/CMakeLists.txt | 7 ++++++- zed_wrapper/CMakeLists.txt | 3 +++ zed_wrapper/params/common.yaml | 2 +- 4 files changed, 15 insertions(+), 4 deletions(-) diff --git a/zed_nodelets/CMakeLists.txt b/zed_nodelets/CMakeLists.txt index 1aef1475..d779d65b 100644 --- a/zed_nodelets/CMakeLists.txt +++ b/zed_nodelets/CMakeLists.txt @@ -2,6 +2,9 @@ cmake_minimum_required(VERSION 3.5) project(zed_nodelets) +## Generate symbols for IDE indexer +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) + # if CMAKE_BUILD_TYPE is not specified, take 'Release' as default IF(NOT CMAKE_BUILD_TYPE) SET(CMAKE_BUILD_TYPE Release) @@ -19,8 +22,8 @@ function(checkPackage package customMessage) endif() endfunction(checkPackage) -find_package(ZED 3) -checkPackage("ZED" "ZED SDK v3.x not found, install it from:\n https://www.stereolabs.com/developers/") +find_package(ZED 4) +checkPackage("ZED" "ZED SDK v4.x not found, install it from:\n https://www.stereolabs.com/developers/") exec_program(uname ARGS -p OUTPUT_VARIABLE CMAKE_SYSTEM_NAME2) if ( CMAKE_SYSTEM_NAME2 MATCHES "aarch64" ) # Jetson TX diff --git a/zed_ros/CMakeLists.txt b/zed_ros/CMakeLists.txt index b983ecce..4f397e26 100644 --- a/zed_ros/CMakeLists.txt +++ b/zed_ros/CMakeLists.txt @@ -1,4 +1,9 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) + project(zed_ros) + +## Generate symbols for IDE indexer +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) + find_package(catkin REQUIRED) catkin_metapackage() diff --git a/zed_wrapper/CMakeLists.txt b/zed_wrapper/CMakeLists.txt index 2d2c1c56..f0b7d62d 100644 --- a/zed_wrapper/CMakeLists.txt +++ b/zed_wrapper/CMakeLists.txt @@ -2,6 +2,9 @@ cmake_minimum_required(VERSION 3.5) project(zed_wrapper) +## Generate symbols for IDE indexer +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) + # if CMAKE_BUILD_TYPE is not specified, take 'Release' as default IF(NOT CMAKE_BUILD_TYPE) SET(CMAKE_BUILD_TYPE Release) diff --git a/zed_wrapper/params/common.yaml b/zed_wrapper/params/common.yaml index f2fb207d..8d401bff 100644 --- a/zed_wrapper/params/common.yaml +++ b/zed_wrapper/params/common.yaml @@ -37,7 +37,7 @@ video: extrinsic_in_camera_frame: true # if `false` extrinsic parameter in `camera_info` will use ROS native frame (X FORWARD, Z UP) instead of the camera frame (Z FORWARD, Y DOWN) [`true` use old behavior as for version < v3.1] depth: - quality: 1 # '0': NONE, '1': PERFORMANCE, '2': QUALITY, '3': ULTRA, '4': NEURAL + quality: 3 # '0': NONE, '1': PERFORMANCE, '2': QUALITY, '3': ULTRA, '4': NEURAL sensing_mode: 0 # '0': STANDARD, '1': FILL (not use FILL for robotic applications) depth_stabilization: 1 # `0`: disabled, `1`: enabled openni_depth_mode: false # 'false': 32bit float meters, 'true': 16bit uchar millimeters From cfe905fbef2e03b7a9fa3cdcd5d042902a51ec11 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Fri, 31 Mar 2023 09:44:10 +0200 Subject: [PATCH 142/199] Update CMakeLists.txt files --- zed-ros-interfaces | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/zed-ros-interfaces b/zed-ros-interfaces index aa43a2e2..c2c171e3 160000 --- a/zed-ros-interfaces +++ b/zed-ros-interfaces @@ -1 +1 @@ -Subproject commit aa43a2e2ae0fa7cfd10708ba0b4159da025fb8e9 +Subproject commit c2c171e38b68ab85b06c5bb793662bc9afd349b2 From 8aa953b7a7b002cb8e9b54672a1bc5d13d8b2d54 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Fri, 31 Mar 2023 11:41:44 +0200 Subject: [PATCH 143/199] SDK v4 compatibility OK --- CHANGELOG.rst | 8 + zed-ros-interfaces | 2 +- .../include/zed_wrapper_nodelet.hpp | 4 +- .../zed_nodelet/src/zed_wrapper_nodelet.cpp | 447 ++++++++---------- zed_ros/CMakeLists.txt | 9 +- zed_wrapper/params/common.yaml | 2 - zed_wrapper/params/zed2.yaml | 1 - zed_wrapper/params/zed2i.yaml | 1 - zed_wrapper/params/zedm.yaml | 1 - 9 files changed, 197 insertions(+), 278 deletions(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 5e823bce..63a65d80 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -1,6 +1,14 @@ CHANGELOG ========= +2023-03-31 +---------- +- Support for ZED SDK v4.0 +- Remove parameter `object_detection.body_fitting` +- Remove parameter `depth.sensing_mode` +- Remove parameter `video.extrinsic_in_camera_frame` +- Skeleton Tracking is no more available. Migrate to ROS 2 Wrapper if you need it. + v3.8.x ------ - Fix the frame links of barometer, magnetometer, and temperature sensors for ZED2i diff --git a/zed-ros-interfaces b/zed-ros-interfaces index c2c171e3..dff089f2 160000 --- a/zed-ros-interfaces +++ b/zed-ros-interfaces @@ -1 +1 @@ -Subproject commit c2c171e38b68ab85b06c5bb793662bc9afd349b2 +Subproject commit dff089f242d4d17ef88fe95b15f155336edd6466 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 d1820703..53b743a0 100644 --- a/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp +++ b/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp @@ -541,7 +541,6 @@ class ZEDWrapperNodelet : public nodelet::Nodelet { sl::RESOLUTION mCamResol; int mCamFrameRate; sl::DEPTH_MODE mDepthMode; - sl::SENSING_MODE mCamSensingMode; int mGpuId; int mZedId; int mDepthStabilization; @@ -645,7 +644,6 @@ class ZEDWrapperNodelet : public nodelet::Nodelet { bool mAreaMemory; bool mInitOdomWithPose; bool mResetOdom = false; - bool mUseOldExtrinsic = false; bool mUpdateDynParams = false; bool mPublishingData = false; @@ -726,7 +724,7 @@ class ZEDWrapperNodelet : public nodelet::Nodelet { bool mObjDetFruitsEnable = true; bool mObjDetSportsEnable = true; - sl::DETECTION_MODEL mObjDetModel = sl::DETECTION_MODEL::MULTI_CLASS_BOX; + sl::OBJECT_DETECTION_MODEL mObjDetModel = sl::OBJECT_DETECTION_MODEL::MULTI_CLASS_BOX_MEDIUM; ros::Publisher mPubObjDet; }; // class ZEDROSWrapperNodelet 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 fa8cfa90..8c925448 100644 --- a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp @@ -292,38 +292,21 @@ void ZEDWrapperNodelet::onInit() NODELET_WARN("Camera model does not match user parameter. Please modify " "the value of the parameter 'camera_model' to 'zedm'"); } -#if ZED_SDK_MAJOR_VERSION == 3 && ZED_SDK_MINOR_VERSION < 1 - mSlCamImuTransf = mZed.getCameraInformation().camera_imu_transform; -#else mSlCamImuTransf = mZed.getCameraInformation().sensors_configuration.camera_imu_transform; -#endif - NODELET_INFO("Camera-IMU Transform: \n %s", mSlCamImuTransf.getInfos().c_str()); } else if (mZedRealCamModel == sl::MODEL::ZED2) { if (mZedUserCamModel != mZedRealCamModel) { NODELET_WARN("Camera model does not match user parameter. Please modify " "the value of the parameter 'camera_model' to 'zed2'"); } - -#if ZED_SDK_MAJOR_VERSION == 3 && ZED_SDK_MINOR_VERSION < 1 - mSlCamImuTransf = mZed.getCameraInformation().camera_imu_transform; -#else mSlCamImuTransf = mZed.getCameraInformation().sensors_configuration.camera_imu_transform; -#endif - NODELET_INFO("Camera-IMU Transform: \n %s", mSlCamImuTransf.getInfos().c_str()); } else if (mZedRealCamModel == sl::MODEL::ZED2i) { if (mZedUserCamModel != mZedRealCamModel) { NODELET_WARN("Camera model does not match user parameter. Please modify " "the value of the parameter 'camera_model' to 'zed2i'"); } - -#if ZED_SDK_MAJOR_VERSION == 3 && ZED_SDK_MINOR_VERSION < 1 - mSlCamImuTransf = mZed.getCameraInformation().camera_imu_transform; -#else mSlCamImuTransf = mZed.getCameraInformation().sensors_configuration.camera_imu_transform; -#endif - NODELET_INFO("Camera-IMU Transform: \n %s", mSlCamImuTransf.getInfos().c_str()); } @@ -332,19 +315,10 @@ void ZEDWrapperNodelet::onInit() NODELET_INFO_STREAM(" * Serial Number -> " << mZedSerialNumber); if (!mSvoMode) { -#if ZED_SDK_MAJOR_VERSION == 3 && ZED_SDK_MINOR_VERSION < 1 - mCamFwVersion = mZed.getCameraInformation().camera_firmware_version; -#else mCamFwVersion = mZed.getCameraInformation().camera_configuration.firmware_version; -#endif - NODELET_INFO_STREAM(" * Camera FW Version -> " << mCamFwVersion); if (mZedRealCamModel != sl::MODEL::ZED) { -#if ZED_SDK_MAJOR_VERSION == 3 && ZED_SDK_MINOR_VERSION < 1 - mSensFwVersion = mZed.getCameraInformation().sensors_firmware_version; -#else mSensFwVersion = mZed.getCameraInformation().sensors_configuration.firmware_version; -#endif NODELET_INFO_STREAM(" * Sensors FW Version -> " << mSensFwVersion); } } else { @@ -665,10 +639,6 @@ void ZEDWrapperNodelet::readParameters() // ----> Video mNhNs.getParam("video/img_downsample_factor", mCamImageResizeFactor); NODELET_INFO_STREAM(" * Image resample factor\t-> " << mCamImageResizeFactor); - - mNhNs.getParam("video/extrinsic_in_camera_frame", mUseOldExtrinsic); - NODELET_INFO_STREAM(" * Extrinsic param. frame\t-> " - << (mUseOldExtrinsic ? "X RIGHT - Y DOWN - Z FWD" : "X FWD - Y LEFT - Z UP")); // <---- Video NODELET_INFO_STREAM("*** DEPTH PARAMETERS ***"); @@ -678,10 +648,6 @@ void ZEDWrapperNodelet::readParameters() mNhNs.getParam("depth/quality", depth_mode); mDepthMode = static_cast(depth_mode); NODELET_INFO_STREAM(" * Depth quality\t\t-> " << sl::toString(mDepthMode).c_str()); - int sensing_mode; - mNhNs.getParam("depth/sensing_mode", sensing_mode); - mCamSensingMode = static_cast(sensing_mode); - NODELET_INFO_STREAM(" * Depth Sensing mode\t\t-> " << sl::toString(mCamSensingMode).c_str()); mNhNs.getParam("depth/openni_depth_mode", mOpenniDepthMode); NODELET_INFO_STREAM(" * OpenNI mode\t\t\t-> " << (mOpenniDepthMode ? "ENABLED" : "DISABLED")); mNhNs.getParam("depth/depth_stabilization", mDepthStabilization); @@ -779,35 +745,28 @@ void ZEDWrapperNodelet::readParameters() int model; mNhNs.getParam("object_detection/model", model); - if (model < 0 || model >= static_cast(sl::DETECTION_MODEL::LAST)) { + if (model < 0 || model >= static_cast(sl::OBJECT_DETECTION_MODEL::LAST)) { NODELET_WARN("Detection model not valid. Forced to the default value"); model = static_cast(mObjDetModel); } - mObjDetModel = static_cast(model); + mObjDetModel = static_cast(model); NODELET_INFO_STREAM(" * Detection model\t\t-> " << sl::toString(mObjDetModel)); - if (mObjDetModel == sl::DETECTION_MODEL::HUMAN_BODY_ACCURATE || mObjDetModel == sl::DETECTION_MODEL::HUMAN_BODY_MEDIUM || mObjDetModel == sl::DETECTION_MODEL::HUMAN_BODY_FAST) { - mNhNs.getParam("object_detection/body_fitting", mObjDetBodyFitting); - NODELET_INFO_STREAM(" * Body fitting\t\t\t-> " << (mObjDetBodyFitting ? "ENABLED" : "DISABLED")); - } else { - mNhNs.getParam("object_detection/mc_people", mObjDetPeopleEnable); - NODELET_INFO_STREAM(" * Detect people\t\t-> " << (mObjDetPeopleEnable ? "ENABLED" : "DISABLED")); - mNhNs.getParam("object_detection/mc_vehicle", mObjDetVehiclesEnable); - NODELET_INFO_STREAM(" * Detect vehicles\t\t-> " << (mObjDetVehiclesEnable ? "ENABLED" : "DISABLED")); - mNhNs.getParam("object_detection/mc_bag", mObjDetBagsEnable); - NODELET_INFO_STREAM(" * Detect bags\t\t\t-> " << (mObjDetBagsEnable ? "ENABLED" : "DISABLED")); - mNhNs.getParam("object_detection/mc_animal", mObjDetAnimalsEnable); - NODELET_INFO_STREAM(" * Detect animals\t\t-> " << (mObjDetAnimalsEnable ? "ENABLED" : "DISABLED")); - mNhNs.getParam("object_detection/mc_electronics", mObjDetElectronicsEnable); - NODELET_INFO_STREAM(" * Detect electronics\t\t-> " << (mObjDetElectronicsEnable ? "ENABLED" : "DISABLED")); - mNhNs.getParam("object_detection/mc_fruit_vegetable", mObjDetFruitsEnable); - NODELET_INFO_STREAM(" * Detect fruit and vegetables\t-> " << (mObjDetFruitsEnable ? "ENABLED" : "DISABLED")); - mNhNs.getParam("object_detection/mc_sport", mObjDetSportsEnable); - NODELET_INFO_STREAM(" * Detect sport-related objects\t-> " << (mObjDetSportsEnable ? "ENABLED" : "DISABLED")); - } - } else if (mObjDetModel != sl::DETECTION_MODEL::PERSON_HEAD_BOX) { - NODELET_INFO_STREAM(" * Object Detection\t\t-> DISABLED"); + mNhNs.getParam("object_detection/mc_people", mObjDetPeopleEnable); + NODELET_INFO_STREAM(" * Detect people\t\t-> " << (mObjDetPeopleEnable ? "ENABLED" : "DISABLED")); + mNhNs.getParam("object_detection/mc_vehicle", mObjDetVehiclesEnable); + NODELET_INFO_STREAM(" * Detect vehicles\t\t-> " << (mObjDetVehiclesEnable ? "ENABLED" : "DISABLED")); + mNhNs.getParam("object_detection/mc_bag", mObjDetBagsEnable); + NODELET_INFO_STREAM(" * Detect bags\t\t\t-> " << (mObjDetBagsEnable ? "ENABLED" : "DISABLED")); + mNhNs.getParam("object_detection/mc_animal", mObjDetAnimalsEnable); + NODELET_INFO_STREAM(" * Detect animals\t\t-> " << (mObjDetAnimalsEnable ? "ENABLED" : "DISABLED")); + mNhNs.getParam("object_detection/mc_electronics", mObjDetElectronicsEnable); + NODELET_INFO_STREAM(" * Detect electronics\t\t-> " << (mObjDetElectronicsEnable ? "ENABLED" : "DISABLED")); + mNhNs.getParam("object_detection/mc_fruit_vegetable", mObjDetFruitsEnable); + NODELET_INFO_STREAM(" * Detect fruit and vegetables\t-> " << (mObjDetFruitsEnable ? "ENABLED" : "DISABLED")); + mNhNs.getParam("object_detection/mc_sport", mObjDetSportsEnable); + NODELET_INFO_STREAM(" * Detect sport-related objects\t-> " << (mObjDetSportsEnable ? "ENABLED" : "DISABLED")); } // <---- Object Detection @@ -938,10 +897,8 @@ void ZEDWrapperNodelet::readParameters() NODELET_INFO_STREAM(" * [DYN] saturation\t\t-> " << mCamSaturation); mNhNs.getParam("sharpness", mCamSharpness); NODELET_INFO_STREAM(" * [DYN] sharpness\t\t-> " << mCamSharpness); -#if (ZED_SDK_MAJOR_VERSION == 3 && ZED_SDK_MINOR_VERSION >= 1) mNhNs.getParam("gamma", mCamGamma); NODELET_INFO_STREAM(" * [DYN] gamma\t\t\t-> " << mCamGamma); -#endif mNhNs.getParam("auto_exposure_gain", mCamAutoExposure); NODELET_INFO_STREAM(" * [DYN] auto_exposure_gain\t-> " << (mCamAutoExposure ? "ENABLED" : "DISABLED")); mNhNs.getParam("gain", mCamGain); @@ -1399,12 +1356,13 @@ bool ZEDWrapperNodelet::start_obj_detect() NODELET_INFO_STREAM("*** Starting Object Detection ***"); sl::ObjectDetectionParameters od_p; - od_p.enable_mask_output = false; + od_p.allow_reduced_precision_inference = false; + od_p.enable_segmentation = false; od_p.enable_tracking = mObjDetTracking; od_p.image_sync = false; // Asynchronous object detection od_p.detection_model = mObjDetModel; - od_p.enable_body_fitting = mObjDetBodyFitting; od_p.max_range = mObjDetMaxRange; + od_p.instance_module_id = 0; sl::ERROR_CODE objDetError = mZed.enableObjectDetection(od_p); @@ -1425,7 +1383,9 @@ bool ZEDWrapperNodelet::start_obj_detect() mObjDetFilter.clear(); - if (mObjDetModel == sl::DETECTION_MODEL::MULTI_CLASS_BOX || mObjDetModel == sl::DETECTION_MODEL::MULTI_CLASS_BOX_MEDIUM || mObjDetModel == sl::DETECTION_MODEL::MULTI_CLASS_BOX_ACCURATE) { + if (mObjDetModel == sl::OBJECT_DETECTION_MODEL::MULTI_CLASS_BOX_ACCURATE || + mObjDetModel == sl::OBJECT_DETECTION_MODEL::MULTI_CLASS_BOX_MEDIUM || + mObjDetModel == sl::OBJECT_DETECTION_MODEL::MULTI_CLASS_BOX_FAST) { if (mObjDetPeopleEnable) { mObjDetFilter.push_back(sl::OBJECT_CLASS::PERSON); } @@ -1851,40 +1811,9 @@ void ZEDWrapperNodelet::publishDepth(sensor_msgs::ImagePtr imgMsgPtr, sl::Mat de return; } -#if (ZED_SDK_MAJOR_VERSION == 3 && ZED_SDK_MINOR_VERSION < 4) - // OPENNI CONVERSION (meter -> millimeters - float32 -> uint16) - if (!imgMsgPtr) { - imgMsgPtr = boost::make_shared(); - } - - imgMsgPtr->header.stamp = t; - imgMsgPtr->header.frame_id = mDepthOptFrameId; - imgMsgPtr->height = depth.getHeight(); - imgMsgPtr->width = depth.getWidth(); - - int num = 1; // for endianness detection - imgMsgPtr->is_bigendian = !(*(char*)&num == 1); - - imgMsgPtr->step = imgMsgPtr->width * sizeof(uint16_t); - imgMsgPtr->encoding = sensor_msgs::image_encodings::TYPE_16UC1; - - size_t size = imgMsgPtr->step * imgMsgPtr->height; - imgMsgPtr->data.resize(size); - - uint16_t* data = (uint16_t*)(&imgMsgPtr->data[0]); - - int dataSize = imgMsgPtr->width * imgMsgPtr->height; - sl::float1* depthDataPtr = depth.getPtr(); - - for (int i = 0; i < dataSize; i++) { - *(data++) = static_cast(std::round(*(depthDataPtr++) * 1000)); // in mm, rounded - } - mPubDepth.publish(imgMsgPtr, mDepthCamInfoMsg); -#else // NODELET_INFO("Using depth16"); sl_tools::imageToROSmsg(imgMsgPtr, depth, mDepthOptFrameId, t); mPubDepth.publish(imgMsgPtr, mDepthCamInfoMsg); -#endif } void ZEDWrapperNodelet::publishDisparity(sl::Mat disparity, ros::Time t) @@ -1899,13 +1828,8 @@ void ZEDWrapperNodelet::publishDisparity(sl::Mat disparity, ros::Time t) disparityMsg->image = *disparityImgMsg; disparityMsg->header = disparityMsg->image.header; -#if ZED_SDK_MAJOR_VERSION == 3 && ZED_SDK_MINOR_VERSION < 1 - disparityMsg->f = zedParam.calibration_parameters.left_cam.fx; - disparityMsg->T = zedParam.calibration_parameters.T.x; -#else disparityMsg->f = zedParam.camera_configuration.calibration_parameters.left_cam.fx; disparityMsg->T = zedParam.camera_configuration.calibration_parameters.getCameraBaseline(); -#endif if (disparityMsg->T > 0) { disparityMsg->T *= -1.0f; @@ -2106,19 +2030,11 @@ void ZEDWrapperNodelet::fillCamInfo(sl::Camera& zed, sensor_msgs::CameraInfoPtr { sl::CalibrationParameters zedParam; -#if ZED_SDK_MAJOR_VERSION == 3 && ZED_SDK_MINOR_VERSION < 1 - if (rawParam) { - zedParam = zed.getCameraInformation(mMatResolVideo).calibration_parameters_raw; // ok - } else { - zedParam = zed.getCameraInformation(mMatResolVideo).calibration_parameters; // ok - } -#else if (rawParam) { zedParam = zed.getCameraInformation(mMatResolVideo).camera_configuration.calibration_parameters_raw; } else { zedParam = zed.getCameraInformation(mMatResolVideo).camera_configuration.calibration_parameters; } -#endif float baseline = zedParam.getCameraBaseline(); leftCamInfoMsg->distortion_model = sensor_msgs::distortion_models::PLUMB_BOB; @@ -2156,32 +2072,11 @@ void ZEDWrapperNodelet::fillCamInfo(sl::Camera& zed, sensor_msgs::CameraInfoPtr leftCamInfoMsg->R[i + i * 3] = 1; } -#if ZED_SDK_MAJOR_VERSION == 3 && ZED_SDK_MINOR_VERSION < 1 - if (rawParam) { - std::vector R_ = sl_tools::convertRodrigues(zedParam.R); - float* p = R_.data(); - + if (rawParam) { for (int i = 0; i < 9; i++) { - rightCamInfoMsg->R[i] = p[i]; - } - } -#else - if (rawParam) { - if (mUseOldExtrinsic) { // Camera frame (Z forward, Y down, X right) - - std::vector R_ = sl_tools::convertRodrigues(zedParam.R); - float* p = R_.data(); - - for (int i = 0; i < 9; i++) { - rightCamInfoMsg->R[i] = p[i]; - } - } else { // ROS frame (X forward, Z up, Y left) - for (int i = 0; i < 9; i++) { - rightCamInfoMsg->R[i] = zedParam.stereo_transform.getRotationMatrix().r[i]; - } + rightCamInfoMsg->R[i] = zedParam.stereo_transform.getRotationMatrix().r[i]; } } -#endif leftCamInfoMsg->P.fill(0.0); rightCamInfoMsg->P.fill(0.0); @@ -2207,11 +2102,7 @@ void ZEDWrapperNodelet::fillCamDepthInfo(sl::Camera& zed, sensor_msgs::CameraInf { sl::CalibrationParameters zedParam; -#if ZED_SDK_MAJOR_VERSION == 3 && ZED_SDK_MINOR_VERSION < 1 - zedParam = zed.getCameraInformation(mMatResolDepth).calibration_parameters; -#else zedParam = zed.getCameraInformation(mMatResolDepth).camera_configuration.calibration_parameters; -#endif float baseline = zedParam.getCameraBaseline(); depth_info_msg->distortion_model = sensor_msgs::distortion_models::PLUMB_BOB; @@ -2369,25 +2260,17 @@ void ZEDWrapperNodelet::callback_dynamicReconf(zed_nodelets::ZedConfig& config, break; case GAMMA: -#if (ZED_SDK_MAJOR_VERSION == 3 && ZED_SDK_MINOR_VERSION >= 1) mCamGamma = config.gamma; NODELET_INFO("Reconfigure image gamma: %d", mCamGamma); mDynParMutex.unlock(); // NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); -#else - NODELET_DEBUG_STREAM("Gamma Control is not available for SDK older that v3.1"); - mDynParMutex.unlock(); -#endif break; case AUTO_EXP_GAIN: - mCamAutoExposure = config.auto_exposure_gain; - NODELET_INFO_STREAM("Reconfigure auto exposure/gain: " << mCamAutoExposure ? "ENABLED" : "DISABLED"); - if (!mCamAutoExposure) { - mZed.setCameraSettings(sl::VIDEO_SETTINGS::AEC_AGC, 0); - mTriggerAutoExposure = false; - } else { - mTriggerAutoExposure = true; + if(config.auto_exposure_gain!=mCamAutoExposure) + { + mCamAutoExposure = config.auto_exposure_gain; + NODELET_INFO_STREAM("Reconfigure auto exposure/gain: " << mCamAutoExposure ? "ENABLED" : "DISABLED"); } mDynParMutex.unlock(); // NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); @@ -2416,13 +2299,13 @@ void ZEDWrapperNodelet::callback_dynamicReconf(zed_nodelets::ZedConfig& config, break; case AUTO_WB: - mCamAutoWB = config.auto_whitebalance; - NODELET_INFO_STREAM("Reconfigure auto white balance: " << mCamAutoWB ? "ENABLED" : "DISABLED"); - if (!mCamAutoWB) { - mZed.setCameraSettings(sl::VIDEO_SETTINGS::WHITEBALANCE_AUTO, 0); - mTriggerAutoWB = false; - } else { + if(config.auto_whitebalance!=mCamAutoExposure) + { + mCamAutoWB = config.auto_whitebalance; + NODELET_INFO_STREAM("Reconfigure auto white balance: " << mCamAutoWB ? "ENABLED" : "DISABLED"); mTriggerAutoWB = true; + } else { + mTriggerAutoWB = false; } mDynParMutex.unlock(); // NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); @@ -2527,15 +2410,11 @@ void ZEDWrapperNodelet::callback_pubVideoDepth(const ros::TimerEvent& e) grab_ts = mat_right_raw_gray.timestamp; } if (depthSubnumber > 0) { -#if (ZED_SDK_MAJOR_VERSION == 3 && ZED_SDK_MINOR_VERSION < 4) - mZed.retrieveMeasure(mat_depth, sl::MEASURE::DEPTH, sl::MEM::CPU, mMatResolDepth); -#else if (!mOpenniDepthMode) { mZed.retrieveMeasure(mat_depth, sl::MEASURE::DEPTH, sl::MEM::CPU, mMatResolDepth); } else { mZed.retrieveMeasure(mat_depth, sl::MEASURE::DEPTH_U16_MM, sl::MEM::CPU, mMatResolDepth); } -#endif retrieved = true; grab_ts = mat_depth.timestamp; @@ -3192,13 +3071,8 @@ void ZEDWrapperNodelet::device_poll_thread_func() mRecording = false; // Get the parameters of the ZED images -#if ZED_SDK_MAJOR_VERSION == 3 && ZED_SDK_MINOR_VERSION < 1 - mCamWidth = mZed.getCameraInformation().camera_resolution.width; - mCamHeight = mZed.getCameraInformation().camera_resolution.height; -#else mCamWidth = mZed.getCameraInformation().camera_configuration.resolution.width; mCamHeight = mZed.getCameraInformation().camera_configuration.resolution.height; -#endif NODELET_DEBUG_STREAM("Camera Frame size : " << mCamWidth << "x" << mCamHeight); int v_w = static_cast(mCamWidth * mCamImageResizeFactor); int v_h = static_cast(mCamHeight * mCamImageResizeFactor); @@ -3220,7 +3094,6 @@ void ZEDWrapperNodelet::device_poll_thread_func() mRgbCamInfoRawMsg = mLeftCamInfoRawMsg; sl::RuntimeParameters runParams; - runParams.sensing_mode = static_cast(mCamSensingMode); // Main loop while (mNhNs.ok()) { @@ -3287,11 +3160,7 @@ void ZEDWrapperNodelet::device_poll_thread_func() if (mComputeDepth) { runParams.confidence_threshold = mCamDepthConfidence; -#if ZED_SDK_MAJOR_VERSION == 3 && ZED_SDK_MINOR_VERSION < 2 - runParams.textureness_confidence_threshold = mCamDepthTextureConf; -#else runParams.texture_confidence_threshold = mCamDepthTextureConf; -#endif runParams.enable_depth = true; // Ask to compute the depth } else { runParams.enable_depth = false; // Ask to not compute the depth @@ -3406,85 +3275,150 @@ void ZEDWrapperNodelet::device_poll_thread_func() ros::Time stamp = mFrameTimestamp; // Timestamp // ----> Camera Settings + int value; + sl::VIDEO_SETTINGS setting; + sl::ERROR_CODE err; + if (!mSvoMode && mFrameCount % 5 == 0) { // NODELET_DEBUG_STREAM( "[" << mFrameCount << "] device_poll_thread_func MUTEX LOCK"); mDynParMutex.lock(); - int brightness = mZed.getCameraSettings(sl::VIDEO_SETTINGS::BRIGHTNESS); - if (brightness != mCamBrightness) { - mZed.setCameraSettings(sl::VIDEO_SETTINGS::BRIGHTNESS, mCamBrightness); - NODELET_DEBUG_STREAM("mCamBrightness changed: " << mCamBrightness << " <- " << brightness); + setting = sl::VIDEO_SETTINGS::BRIGHTNESS; + err = mZed.getCameraSettings(setting, value); + if (err==sl::ERROR_CODE::SUCCESS && value != mCamBrightness) { + err = mZed.setCameraSettings(setting, mCamBrightness); + } + if(err != sl::ERROR_CODE::SUCCESS) { + NODELET_WARN_STREAM( "Error setting parameter " << sl::toString(setting) << ": " << sl::toString(err)); + } else { + NODELET_DEBUG_STREAM(sl::toString(setting) << " changed: " << mCamBrightness << " <- " << value); mUpdateDynParams = true; } - int contrast = mZed.getCameraSettings(sl::VIDEO_SETTINGS::CONTRAST); - if (contrast != mCamContrast) { - mZed.setCameraSettings(sl::VIDEO_SETTINGS::CONTRAST, mCamContrast); - NODELET_DEBUG_STREAM("mCamContrast changed: " << mCamContrast << " <- " << contrast); + setting = sl::VIDEO_SETTINGS::CONTRAST; + err = mZed.getCameraSettings(setting, value); + if (err==sl::ERROR_CODE::SUCCESS && value != mCamContrast) { + err = mZed.setCameraSettings(setting, mCamContrast); + } + if(err != sl::ERROR_CODE::SUCCESS) { + NODELET_WARN_STREAM( "Error setting parameter " << sl::toString(setting) << ": " << sl::toString(err)); + } else { + NODELET_DEBUG_STREAM(sl::toString(setting) << " changed: " << mCamContrast << " <- " << value); mUpdateDynParams = true; } - int hue = mZed.getCameraSettings(sl::VIDEO_SETTINGS::HUE); - if (hue != mCamHue) { - mZed.setCameraSettings(sl::VIDEO_SETTINGS::HUE, mCamHue); - NODELET_DEBUG_STREAM("mCamHue changed: " << mCamHue << " <- " << hue); + setting = sl::VIDEO_SETTINGS::HUE; + err = mZed.getCameraSettings(setting, value); + if (err==sl::ERROR_CODE::SUCCESS && value != mCamHue) { + err = mZed.setCameraSettings(setting, mCamHue); + } + if(err != sl::ERROR_CODE::SUCCESS) { + NODELET_WARN_STREAM( "Error setting parameter " << sl::toString(setting) << ": " << sl::toString(err)); + } else { + NODELET_DEBUG_STREAM(sl::toString(setting) << " changed: " << mCamHue << " <- " << value); mUpdateDynParams = true; } - int saturation = mZed.getCameraSettings(sl::VIDEO_SETTINGS::SATURATION); - if (saturation != mCamSaturation) { - mZed.setCameraSettings(sl::VIDEO_SETTINGS::SATURATION, mCamSaturation); - NODELET_DEBUG_STREAM("mCamSaturation changed: " << mCamSaturation << " <- " << saturation); + setting = sl::VIDEO_SETTINGS::SATURATION; + err = mZed.getCameraSettings(setting, value); + if (err==sl::ERROR_CODE::SUCCESS && value != mCamSaturation) { + err = mZed.setCameraSettings(setting, mCamSaturation); + } + if(err != sl::ERROR_CODE::SUCCESS) { + NODELET_WARN_STREAM( "Error setting parameter " << sl::toString(setting) << ": " << sl::toString(err)); + } else { + NODELET_DEBUG_STREAM(sl::toString(setting) << " changed: " << mCamSaturation << " <- " << value); mUpdateDynParams = true; } - int sharpness = mZed.getCameraSettings(sl::VIDEO_SETTINGS::SHARPNESS); - if (sharpness != mCamSharpness) { - mZed.setCameraSettings(sl::VIDEO_SETTINGS::SHARPNESS, mCamSharpness); - NODELET_DEBUG_STREAM("mCamSharpness changed: " << mCamSharpness << " <- " << sharpness); + setting = sl::VIDEO_SETTINGS::SHARPNESS; + err = mZed.getCameraSettings(setting, value); + if (err==sl::ERROR_CODE::SUCCESS && value != mCamSharpness) { + err = mZed.setCameraSettings(setting, mCamSharpness); + } + if(err != sl::ERROR_CODE::SUCCESS) { + NODELET_WARN_STREAM( "Error setting parameter " << sl::toString(setting) << ": " << sl::toString(err)); + } else { + NODELET_DEBUG_STREAM(sl::toString(setting) << " changed: " << mCamSharpness << " <- " << value); mUpdateDynParams = true; } -#if (ZED_SDK_MAJOR_VERSION == 3 && ZED_SDK_MINOR_VERSION >= 1) - int gamma = mZed.getCameraSettings(sl::VIDEO_SETTINGS::GAMMA); - if (gamma != mCamGamma) { - mZed.setCameraSettings(sl::VIDEO_SETTINGS::GAMMA, mCamGamma); - NODELET_DEBUG_STREAM("mCamGamma changed: " << mCamGamma << " <- " << gamma); + setting = sl::VIDEO_SETTINGS::GAMMA; + err = mZed.getCameraSettings(setting, value); + if (err==sl::ERROR_CODE::SUCCESS && value != mCamGamma) { + err = mZed.setCameraSettings(setting, mCamGamma); + } + if(err != sl::ERROR_CODE::SUCCESS) { + NODELET_WARN_STREAM( "Error setting parameter " << sl::toString(setting) << ": " << sl::toString(err)); + } else { + NODELET_DEBUG_STREAM(sl::toString(setting) << " changed: " << mCamGamma << " <- " << value); mUpdateDynParams = true; } -#endif - if (mCamAutoExposure) { - if (mTriggerAutoExposure) { - mZed.setCameraSettings(sl::VIDEO_SETTINGS::AEC_AGC, 1); - mTriggerAutoExposure = false; + if (mTriggerAutoExposure) { + setting = sl::VIDEO_SETTINGS::AEC_AGC; + err = mZed.getCameraSettings(setting, value); + int aec_agc = (mCamAutoExposure?1:0); + if (err==sl::ERROR_CODE::SUCCESS && value != aec_agc) { + err = mZed.setCameraSettings(setting, aec_agc); } - } else { - int exposure = mZed.getCameraSettings(sl::VIDEO_SETTINGS::EXPOSURE); - if (exposure != mCamExposure) { - mZed.setCameraSettings(sl::VIDEO_SETTINGS::EXPOSURE, mCamExposure); - NODELET_DEBUG_STREAM("mCamExposure changed: " << mCamExposure << " <- " << exposure); + if(err != sl::ERROR_CODE::SUCCESS) { + NODELET_WARN_STREAM( "Error setting parameter " << sl::toString(setting) << ": " << sl::toString(err)); + } else { + NODELET_DEBUG_STREAM(sl::toString(setting) << " changed: " << aec_agc << " <- " << value); + mUpdateDynParams = true; + } + } + if (!mCamAutoExposure){ + setting = sl::VIDEO_SETTINGS::EXPOSURE; + err = mZed.getCameraSettings(setting, value); + if (err==sl::ERROR_CODE::SUCCESS && value != mCamExposure) { + err = mZed.setCameraSettings(setting, mCamExposure); + } + if(err != sl::ERROR_CODE::SUCCESS) { + NODELET_WARN_STREAM( "Error setting parameter " << sl::toString(setting) << ": " << sl::toString(err)); + } else { + NODELET_DEBUG_STREAM(sl::toString(setting) << " changed: " << mCamExposure << " <- " << value); mUpdateDynParams = true; } - int gain = mZed.getCameraSettings(sl::VIDEO_SETTINGS::GAIN); - if (gain != mCamGain) { - mZed.setCameraSettings(sl::VIDEO_SETTINGS::GAIN, mCamGain); - NODELET_DEBUG_STREAM("mCamGain changed: " << mCamGain << " <- " << gain); + setting = sl::VIDEO_SETTINGS::GAIN; + err = mZed.getCameraSettings(setting, value); + if (err==sl::ERROR_CODE::SUCCESS && value != mCamGain) { + err = mZed.setCameraSettings(setting, mCamGain); + } + if(err != sl::ERROR_CODE::SUCCESS) { + NODELET_WARN_STREAM( "Error setting parameter " << sl::toString(setting) << ": " << sl::toString(err)); + } else { + NODELET_DEBUG_STREAM(sl::toString(setting) << " changed: " << mCamGain << " <- " << value); mUpdateDynParams = true; } } - if (mCamAutoWB) { - if (mTriggerAutoWB) { - mZed.setCameraSettings(sl::VIDEO_SETTINGS::WHITEBALANCE_AUTO, 1); - mTriggerAutoWB = false; + if (mTriggerAutoWB) { + setting = sl::VIDEO_SETTINGS::WHITEBALANCE_AUTO; + err = mZed.getCameraSettings(setting, value); + int wb_auto = (mCamAutoWB?1:0); + if (err==sl::ERROR_CODE::SUCCESS && value != wb_auto) { + err = mZed.setCameraSettings(setting, wb_auto); } - } else { - int wb = mZed.getCameraSettings(sl::VIDEO_SETTINGS::WHITEBALANCE_TEMPERATURE); - if (wb != mCamWB) { - mZed.setCameraSettings(sl::VIDEO_SETTINGS::WHITEBALANCE_TEMPERATURE, mCamWB); - NODELET_DEBUG_STREAM("mCamWB changed: " << mCamWB << " <- " << wb); + if(err != sl::ERROR_CODE::SUCCESS) { + NODELET_WARN_STREAM( "Error setting parameter " << sl::toString(setting) << ": " << sl::toString(err)); + } else { + NODELET_DEBUG_STREAM(sl::toString(setting) << " changed: " << wb_auto << " <- " << value); + mUpdateDynParams = true; + } + } + if (!mCamAutoWB) { + setting = sl::VIDEO_SETTINGS::WHITEBALANCE_TEMPERATURE; + err = mZed.getCameraSettings(setting, value); + if (err==sl::ERROR_CODE::SUCCESS && value != mCamWB) { + err = mZed.setCameraSettings(setting, mCamWB); + } + if(err != sl::ERROR_CODE::SUCCESS) { + NODELET_WARN_STREAM( "Error setting parameter " << sl::toString(setting) << ": " << sl::toString(err)); + } else { + NODELET_DEBUG_STREAM(sl::toString(setting) << " changed: " << mCamWB << " <- " << value); mUpdateDynParams = true; } } @@ -4144,11 +4078,25 @@ bool ZEDWrapperNodelet::on_toggle_led(zed_interfaces::toggle_led::Request& req, return false; } - int status = mZed.getCameraSettings(sl::VIDEO_SETTINGS::LED_STATUS); + int status; + sl::ERROR_CODE err = mZed.getCameraSettings(sl::VIDEO_SETTINGS::LED_STATUS,status); + if(err!=sl::ERROR_CODE::SUCCESS) + { + NODELET_WARN_STREAM("Error getting the current status of the led: " << sl::toString(err).c_str()); + return false; + } + int new_status = status == 0 ? 1 : 0; - mZed.setCameraSettings(sl::VIDEO_SETTINGS::LED_STATUS, new_status); + err = mZed.setCameraSettings(sl::VIDEO_SETTINGS::LED_STATUS, new_status); + if(err!=sl::ERROR_CODE::SUCCESS) + { + NODELET_WARN_STREAM("Error getting the current status of the led: " << sl::toString(err).c_str()); + return false; + } - return (new_status == 1); + res.new_led_status = new_status; + + return true; } bool ZEDWrapperNodelet::on_start_3d_mapping(zed_interfaces::start_3d_mapping::Request& req, @@ -4256,12 +4204,12 @@ bool ZEDWrapperNodelet::on_start_object_detection(zed_interfaces::start_object_d mObjDetConfidence = req.confidence; mObjDetTracking = req.tracking; - if (req.model < 0 || req.model >= static_cast(sl::DETECTION_MODEL::LAST)) { + if (req.model < 0 || req.model >= static_cast(sl::OBJECT_DETECTION_MODEL::LAST)) { NODELET_ERROR_STREAM("Object Detection model not valid."); res.done = false; return res.done; } - mObjDetModel = static_cast(req.model); + mObjDetModel = static_cast(req.model); mObjDetMaxRange = req.max_range; if (mObjDetMaxRange > mCamMaxDepth) { @@ -4273,27 +4221,22 @@ bool ZEDWrapperNodelet::on_start_object_detection(zed_interfaces::start_object_d NODELET_INFO_STREAM(" * Object min. confidence\t-> " << mObjDetConfidence); NODELET_INFO_STREAM(" * Object tracking\t\t-> " << (mObjDetTracking ? "ENABLED" : "DISABLED")); NODELET_INFO_STREAM(" * Detection model\t\t-> " << sl::toString(mObjDetModel)); - - if (mObjDetModel == sl::DETECTION_MODEL::HUMAN_BODY_ACCURATE || mObjDetModel == sl::DETECTION_MODEL::HUMAN_BODY_MEDIUM || mObjDetModel == sl::DETECTION_MODEL::HUMAN_BODY_FAST) { - mObjDetBodyFitting = req.sk_body_fitting; - NODELET_INFO_STREAM(" * Body fitting\t\t\t-> " << (mObjDetBodyFitting ? "ENABLED" : "DISABLED")); - } else { - mObjDetPeopleEnable = req.mc_people; - NODELET_INFO_STREAM(" * Detect people\t\t-> " << (mObjDetPeopleEnable ? "ENABLED" : "DISABLED")); - mObjDetVehiclesEnable = req.mc_vehicles; - NODELET_INFO_STREAM(" * Detect vehicles\t\t-> " << (mObjDetVehiclesEnable ? "ENABLED" : "DISABLED")); - mObjDetBagsEnable = req.mc_bag; - NODELET_INFO_STREAM(" * Detect bags\t\t\t-> " << (mObjDetBagsEnable ? "ENABLED" : "DISABLED")); - mObjDetAnimalsEnable = req.mc_animal; - NODELET_INFO_STREAM(" * Detect animals\t\t-> " << (mObjDetAnimalsEnable ? "ENABLED" : "DISABLED")); - mObjDetElectronicsEnable = req.mc_electronics; - NODELET_INFO_STREAM(" * Detect electronics\t\t-> " << (mObjDetElectronicsEnable ? "ENABLED" : "DISABLED")); - mObjDetFruitsEnable = req.mc_fruit_vegetable; - NODELET_INFO_STREAM(" * Detect fruit and vegetables\t-> " << (mObjDetFruitsEnable ? "ENABLED" : "DISABLED")); - mObjDetSportsEnable = req.mc_sport; - NODELET_INFO_STREAM(" * Detect sport related objects\t-> " << (mObjDetSportsEnable ? "ENABLED" : "DISABLED")); - } - + + mNhNs.getParam("object_detection/mc_people", mObjDetPeopleEnable); + NODELET_INFO_STREAM(" * Detect people\t\t-> " << (mObjDetPeopleEnable ? "ENABLED" : "DISABLED")); + mNhNs.getParam("object_detection/mc_vehicle", mObjDetVehiclesEnable); + NODELET_INFO_STREAM(" * Detect vehicles\t\t-> " << (mObjDetVehiclesEnable ? "ENABLED" : "DISABLED")); + mNhNs.getParam("object_detection/mc_bag", mObjDetBagsEnable); + NODELET_INFO_STREAM(" * Detect bags\t\t\t-> " << (mObjDetBagsEnable ? "ENABLED" : "DISABLED")); + mNhNs.getParam("object_detection/mc_animal", mObjDetAnimalsEnable); + NODELET_INFO_STREAM(" * Detect animals\t\t-> " << (mObjDetAnimalsEnable ? "ENABLED" : "DISABLED")); + mNhNs.getParam("object_detection/mc_electronics", mObjDetElectronicsEnable); + NODELET_INFO_STREAM(" * Detect electronics\t\t-> " << (mObjDetElectronicsEnable ? "ENABLED" : "DISABLED")); + mNhNs.getParam("object_detection/mc_fruit_vegetable", mObjDetFruitsEnable); + NODELET_INFO_STREAM(" * Detect fruit and vegetables\t-> " << (mObjDetFruitsEnable ? "ENABLED" : "DISABLED")); + mNhNs.getParam("object_detection/mc_sport", mObjDetSportsEnable); + NODELET_INFO_STREAM(" * Detect sport-related objects\t-> " << (mObjDetSportsEnable ? "ENABLED" : "DISABLED")); + mObjDetRunning = false; mObjDetEnabled = true; res.done = true; @@ -4383,28 +4326,8 @@ void ZEDWrapperNodelet::processDetectedObjects(ros::Time t) memcpy(&(objMsg->objects[idx].dimensions_3d[0]), &(data.dimensions[0]), 3 * sizeof(float)); - if (mObjDetModel == sl::DETECTION_MODEL::HUMAN_BODY_ACCURATE || mObjDetModel == sl::DETECTION_MODEL::HUMAN_BODY_MEDIUM || mObjDetModel == sl::DETECTION_MODEL::HUMAN_BODY_FAST) { - objMsg->objects[idx].skeleton_available = true; - - if (data.head_bounding_box_2d.size() == 4) { - memcpy(&(objMsg->objects[idx].head_bounding_box_2d.corners[0]), &(data.head_bounding_box_2d[0]), - 8 * sizeof(unsigned int)); - } - if (data.head_bounding_box.size() == 8) { - memcpy(&(objMsg->objects[idx].head_bounding_box_3d.corners[0]), &(data.head_bounding_box[0]), - 24 * sizeof(float)); - } - memcpy(&(objMsg->objects[idx].head_position[0]), &(data.head_position[0]), 3 * sizeof(float)); - - if (data.keypoint_2d.size() == 18) { - memcpy(&(objMsg->objects[idx].skeleton_2d.keypoints[0]), &(data.keypoint_2d[0]), 36 * sizeof(float)); - } - if (data.keypoint_2d.size() == 18) { - memcpy(&(objMsg->objects[idx].skeleton_3d.keypoints[0]), &(data.keypoint[0]), 54 * sizeof(float)); - } - } else { - objMsg->objects[idx].skeleton_available = false; - } + // Body Detection is in a separate module in ZED SDK v4 + objMsg->objects[idx].skeleton_available = false; // at the end of the loop idx++; @@ -4462,7 +4385,7 @@ void ZEDWrapperNodelet::clickedPtCallback(geometry_msgs::PointStampedConstPtr ms // ----> Project the point into 2D image coordinates sl::CalibrationParameters zedParam; - zedParam = mZed.getCameraInformation(mMatResolVideo).calibration_parameters; // ok + zedParam = mZed.getCameraInformation(mMatResolVideo).camera_configuration.calibration_parameters; // ok float f_x = zedParam.left_cam.fx; float f_y = zedParam.left_cam.fy; diff --git a/zed_ros/CMakeLists.txt b/zed_ros/CMakeLists.txt index 4f397e26..b659e33e 100644 --- a/zed_ros/CMakeLists.txt +++ b/zed_ros/CMakeLists.txt @@ -1,9 +1,4 @@ -cmake_minimum_required(VERSION 3.5) - +cmake_minimum_required(VERSION 2.8.3) project(zed_ros) - -## Generate symbols for IDE indexer -set(CMAKE_EXPORT_COMPILE_COMMANDS ON) - find_package(catkin REQUIRED) -catkin_metapackage() +catkin_metapackage() \ No newline at end of file diff --git a/zed_wrapper/params/common.yaml b/zed_wrapper/params/common.yaml index 8d401bff..29cf21c7 100644 --- a/zed_wrapper/params/common.yaml +++ b/zed_wrapper/params/common.yaml @@ -34,11 +34,9 @@ general: video: img_downsample_factor: 0.5 # Resample factor for images [0.01,1.0] The SDK works with native image sizes, but publishes rescaled image. - extrinsic_in_camera_frame: true # if `false` extrinsic parameter in `camera_info` will use ROS native frame (X FORWARD, Z UP) instead of the camera frame (Z FORWARD, Y DOWN) [`true` use old behavior as for version < v3.1] depth: quality: 3 # '0': NONE, '1': PERFORMANCE, '2': QUALITY, '3': ULTRA, '4': NEURAL - sensing_mode: 0 # '0': STANDARD, '1': FILL (not use FILL for robotic applications) depth_stabilization: 1 # `0`: disabled, `1`: enabled openni_depth_mode: false # 'false': 32bit float meters, 'true': 16bit uchar millimeters depth_downsample_factor: 0.5 # Resample factor for depth data matrices [0.01,1.0] The SDK works with native data sizes, but publishes rescaled matrices (depth map, point cloud, ...) diff --git a/zed_wrapper/params/zed2.yaml b/zed_wrapper/params/zed2.yaml index 8e355398..c29e4a58 100644 --- a/zed_wrapper/params/zed2.yaml +++ b/zed_wrapper/params/zed2.yaml @@ -23,7 +23,6 @@ object_detection: confidence_threshold: 50 # Minimum value of the detection confidence of an object [0,100] max_range: 15. # Maximum detection range object_tracking_enabled: true # Enable/disable the tracking of the detected objects - body_fitting: false # Enable/disable body fitting for 'HUMAN_BODY_X' models mc_people: true # Enable/disable the detection of persons for 'MULTI_CLASS_BOX_X' models mc_vehicle: true # Enable/disable the detection of vehicles for 'MULTI_CLASS_BOX_X' models mc_bag: true # Enable/disable the detection of bags for 'MULTI_CLASS_BOX_X' models diff --git a/zed_wrapper/params/zed2i.yaml b/zed_wrapper/params/zed2i.yaml index 12840b5e..e43af67c 100644 --- a/zed_wrapper/params/zed2i.yaml +++ b/zed_wrapper/params/zed2i.yaml @@ -23,7 +23,6 @@ object_detection: confidence_threshold: 50 # Minimum value of the detection confidence of an object [0,100] max_range: 15. # Maximum detection range object_tracking_enabled: true # Enable/disable the tracking of the detected objects - body_fitting: false # Enable/disable body fitting for 'HUMAN_BODY_X' models mc_people: true # Enable/disable the detection of persons for 'MULTI_CLASS_BOX_X' models mc_vehicle: true # Enable/disable the detection of vehicles for 'MULTI_CLASS_BOX_X' models mc_bag: true # Enable/disable the detection of bags for 'MULTI_CLASS_BOX_X' models diff --git a/zed_wrapper/params/zedm.yaml b/zed_wrapper/params/zedm.yaml index b522f554..75fb2f17 100644 --- a/zed_wrapper/params/zedm.yaml +++ b/zed_wrapper/params/zedm.yaml @@ -23,7 +23,6 @@ object_detection: confidence_threshold: 50 # Minimum value of the detection confidence of an object [0,100] max_range: 15. # Maximum detection range object_tracking_enabled: true # Enable/disable the tracking of the detected objects - body_fitting: false # Enable/disable body fitting for 'HUMAN_BODY_X' models mc_people: true # Enable/disable the detection of persons for 'MULTI_CLASS_BOX_X' models mc_vehicle: true # Enable/disable the detection of vehicles for 'MULTI_CLASS_BOX_X' models mc_bag: true # Enable/disable the detection of bags for 'MULTI_CLASS_BOX_X' models From 0175e3222225d78155a5109f4a28c533e26de92c Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Fri, 31 Mar 2023 11:52:49 +0200 Subject: [PATCH 144/199] Add "REBOOTING" status handling --- .../zed_nodelet/src/zed_wrapper_nodelet.cpp | 79 ++++++++++--------- 1 file changed, 42 insertions(+), 37 deletions(-) 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 8c925448..c23be124 100644 --- a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp @@ -233,6 +233,7 @@ void ZEDWrapperNodelet::onInit() mZedParams.coordinate_system = sl::COORDINATE_SYSTEM::RIGHT_HANDED_Z_UP_X_FWD; NODELET_INFO_STREAM(" * Camera coordinate system\t-> " << sl::toString(mZedParams.coordinate_system)); + mZedParams.async_grab_camera_recovery = true; mZedParams.coordinate_units = sl::UNIT::METER; mZedParams.depth_mode = static_cast(mDepthMode); mZedParams.sdk_verbose = mVerbose; @@ -2299,7 +2300,7 @@ void ZEDWrapperNodelet::callback_dynamicReconf(zed_nodelets::ZedConfig& config, break; case AUTO_WB: - if(config.auto_whitebalance!=mCamAutoExposure) + if(config.auto_whitebalance!=mCamAutoWB) { mCamAutoWB = config.auto_whitebalance; NODELET_INFO_STREAM("Reconfigure auto white balance: " << mCamAutoWB ? "ENABLED" : "DISABLED"); @@ -3182,50 +3183,54 @@ void ZEDWrapperNodelet::device_poll_thread_func() NODELET_INFO_STREAM_ONCE(toString(mGrabStatus)); std::this_thread::sleep_for(std::chrono::milliseconds(1)); - - if ((ros::Time::now() - mPrevFrameTimestamp).toSec() > 5 && !mSvoMode) { - mCloseZedMutex.lock(); - mZed.close(); - mCloseZedMutex.unlock(); - - mConnStatus = sl::ERROR_CODE::CAMERA_NOT_DETECTED; - - while (mConnStatus != sl::ERROR_CODE::SUCCESS) { - if (!mNhNs.ok()) { - mStopNode = true; - - std::lock_guard lock(mCloseZedMutex); - NODELET_DEBUG("Closing ZED"); - if (mRecording) { - mRecording = false; - mZed.disableRecording(); + if(mGrabStatus!=sl::ERROR_CODE::CAMERA_REBOOTING) { + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + continue; + } else { + if ((ros::Time::now() - mPrevFrameTimestamp).toSec() > 5 && !mSvoMode) { + mCloseZedMutex.lock(); + mZed.close(); + mCloseZedMutex.unlock(); + + mConnStatus = sl::ERROR_CODE::CAMERA_NOT_DETECTED; + + while (mConnStatus != sl::ERROR_CODE::SUCCESS) { + if (!mNhNs.ok()) { + mStopNode = true; + + std::lock_guard lock(mCloseZedMutex); + NODELET_DEBUG("Closing ZED"); + if (mRecording) { + mRecording = false; + mZed.disableRecording(); + } + mZed.close(); + + NODELET_DEBUG("ZED pool thread finished"); + return; } - mZed.close(); - NODELET_DEBUG("ZED pool thread finished"); - return; - } + int id = sl_tools::checkCameraReady(mZedSerialNumber); - int id = sl_tools::checkCameraReady(mZedSerialNumber); + if (id >= 0) { + mZedParams.input.setFromCameraID(id); + mConnStatus = mZed.open(mZedParams); // Try to initialize the ZED + NODELET_INFO_STREAM(toString(mConnStatus)); + } else { + NODELET_INFO_STREAM("Waiting for the ZED (S/N " << mZedSerialNumber << ") to be re-connected"); + } - if (id >= 0) { - mZedParams.input.setFromCameraID(id); - mConnStatus = mZed.open(mZedParams); // Try to initialize the ZED - NODELET_INFO_STREAM(toString(mConnStatus)); - } else { - NODELET_INFO_STREAM("Waiting for the ZED (S/N " << mZedSerialNumber << ") to be re-connected"); + mDiagUpdater.force_update(); + std::this_thread::sleep_for(std::chrono::milliseconds(2000)); } - mDiagUpdater.force_update(); - std::this_thread::sleep_for(std::chrono::milliseconds(2000)); - } - - mPosTrackingActivated = false; + mPosTrackingActivated = false; - computeTracking = mPosTrackingEnabled || mDepthStabilization || poseSubnumber > 0 || poseCovSubnumber > 0 || odomSubnumber > 0; + computeTracking = mPosTrackingEnabled || mDepthStabilization || poseSubnumber > 0 || poseCovSubnumber > 0 || odomSubnumber > 0; - if (computeTracking) { // Start the tracking - start_pos_tracking(); + if (computeTracking) { // Start the tracking + start_pos_tracking(); + } } } From eb70f60553696de1dd4ad5bf03e5ef4d5266237f Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Fri, 31 Mar 2023 14:26:34 +0200 Subject: [PATCH 145/199] Camera Rebooting handling --- CHANGELOG.rst | 3 +- README.md | 31 +++++++++++++------ .../zed_nodelet/src/zed_wrapper_nodelet.cpp | 2 +- zed_wrapper/params/common.yaml | 19 ++++++++++++ zed_wrapper/params/zed.yaml | 2 +- zed_wrapper/params/zed2.yaml | 23 +------------- zed_wrapper/params/zed2i.yaml | 28 ++--------------- zed_wrapper/params/zedm.yaml | 28 ++--------------- zed_wrapper/params/zedx.yaml | 10 ++++++ zed_wrapper/params/zedxm.yaml | 10 ++++++ zed_wrapper/records/record_depth.sh | 2 -- zed_wrapper/records/record_stereo.sh | 2 -- 12 files changed, 72 insertions(+), 88 deletions(-) create mode 100644 zed_wrapper/params/zedx.yaml create mode 100644 zed_wrapper/params/zedxm.yaml delete mode 100644 zed_wrapper/records/record_depth.sh delete mode 100644 zed_wrapper/records/record_stereo.sh diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 63a65d80..a49df706 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -7,7 +7,8 @@ CHANGELOG - Remove parameter `object_detection.body_fitting` - Remove parameter `depth.sensing_mode` - Remove parameter `video.extrinsic_in_camera_frame` -- Skeleton Tracking is no more available. Migrate to ROS 2 Wrapper if you need it. +- Skeleton Tracking is no more available. Migrate to ROS 2 Wrapper if you need it +- `sensors` and `object_detection` parameters are now in `common.yaml` v3.8.x ------ diff --git a/README.md b/README.md index 8a773642..951e3edc 100644 --- a/README.md +++ b/README.md @@ -17,13 +17,13 @@ This package lets you use the ZED stereo camera with ROS. It outputs the camera ### Prerequisites - Ubuntu 20.04 -- [ZED SDK **≥ 3.8**](https://www.stereolabs.com/developers/) and its dependency [CUDA](https://developer.nvidia.com/cuda-downloads) +- [ZED SDK **≥ 4.0**](https://www.stereolabs.com/developers/) and its dependency [CUDA](https://developer.nvidia.com/cuda-downloads) - [ROS Noetic](http://wiki.ros.org/noetic/Installation/Ubuntu) or - Ubuntu 18.04 -- [ZED SDK **≥ 3.8**](https://www.stereolabs.com/developers/) and its dependency [CUDA](https://developer.nvidia.com/cuda-downloads) +- [ZED SDK **≥ 4.0**](https://www.stereolabs.com/developers/) and its dependency [CUDA](https://developer.nvidia.com/cuda-downloads) - [ROS Melodic](http://wiki.ros.org/melodic/Installation/Ubuntu) ### Build the repository @@ -68,8 +68,7 @@ Remember to always clean the cache of your catkin workspace before compiling wit $ roscd $ cd .. - $ rm -rf build - $ rm -rf devel + $ rm -rf build devel $ catkin_make -DCMAKE_BUILD_TYPE=Release ### Run the ZED wrapper @@ -84,20 +83,28 @@ ZED Mini camera: $ roslaunch zed_wrapper zedm.launch -ZED2 camera: +ZED 2 camera: $ roslaunch zed_wrapper zed2.launch -ZED2i camera: +ZED 2i camera: - $ roslaunch zed_wrapper zed2i.launch + $ roslaunch zed_wrapper zed2i.launch + +ZED X camera: + + $ roslaunch zed_wrapper zedx.launch + +ZED X Mini camera: + + $ roslaunch zed_wrapper zedxm.launch To select the ZED from its serial number: $ roslaunch zed_wrapper zed.launch serial_number:=1010 #replace 1010 with the actual SN ### Rviz visualization -Example launch files to start a pre-configured Rviz environment to visualize the data of ZED, ZED Mini and ZED 2 cameras are provided in the [`zed-ros-examples` repository](https://github.com/stereolabs/zed-ros-examples/tree/master/zed_display_rviz) +Example launch files to start a pre-configured Rviz environment to visualize the data of ZED, ZED Mini, ZED 2, ZED X, and ZED X Mini cameras are provided in the [`zed-ros-examples` repository](https://github.com/stereolabs/zed-ros-examples/tree/master/zed_display_rviz) ### SVO recording [SVO recording](https://www.stereolabs.com/docs/video/#video-recording) can be started and stopped while the ZED node is running using the service `start_svo_recording` and the service `stop_svo_recording`. @@ -106,14 +113,20 @@ Example launch files to start a pre-configured Rviz environment to visualize the ### Object Detection The SDK v3.0 introduces the Object Detection and Tracking module. **The Object Detection module is available only with a ZED 2 camera**. -The Object Detection can be enabled *automatically* when the node start setting the parameter `object_detection/od_enabled` to `true` in the file `zed2.yaml`. +The Object Detection can be enabled *automatically* when the node start setting the parameter `object_detection/od_enabled` to `true` in the file `common.yaml`. The Object Detection can be enabled/disabled *manually* calling the services `start_object_detection` and `stop_object_detection`. +### Body Tracking +The Body Tracking module is not available for the ZED ROS Wrapper. Please consider migrating to the [ZED ROS2 Wrapper](https://github.com/stereolabs/zed-ros2-wrapper) if you need it. + ### Spatial Mapping The Spatial Mapping can be enabled automatically when the node start setting the parameter `mapping/mapping_enabled` to `true` in the file `common.yaml`. The Spatial Mapping can be enabled/disabled manually calling the services `start_3d_mapping` and `stop_3d_mapping`. +### Geo Tracking (GNSS Fusion) +The Geo tracking module is not available for the ZED ROS Wrapper. Please consider migrating to the [ZED ROS2 Wrapper](https://github.com/stereolabs/zed-ros2-wrapper) if you need it. + ### Diagnostic The ZED node publishes diagnostic information that can be used by the robotic system using a [diagnostic_aggregator node](http://wiki.ros.org/diagnostic_aggregator). 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 c23be124..603b2851 100644 --- a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp @@ -3180,7 +3180,7 @@ void ZEDWrapperNodelet::device_poll_thread_func() // the zed have been disconnected) and // re-initialize the ZED - NODELET_INFO_STREAM_ONCE(toString(mGrabStatus)); + NODELET_INFO_STREAM_THROTTLE( 1.0, "Camera grab error: " << sl::toString(mGrabStatus).c_str()); std::this_thread::sleep_for(std::chrono::milliseconds(1)); if(mGrabStatus!=sl::ERROR_CODE::CAMERA_REBOOTING) { diff --git a/zed_wrapper/params/common.yaml b/zed_wrapper/params/common.yaml index 29cf21c7..5df7e258 100644 --- a/zed_wrapper/params/common.yaml +++ b/zed_wrapper/params/common.yaml @@ -43,6 +43,7 @@ depth: pos_tracking: pos_tracking_enabled: true # True to enable positional tracking from start + 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 publish_map_tf: true # publish `map -> odom` TF map_frame: 'map' # main frame @@ -65,4 +66,22 @@ mapping: fused_pointcloud_freq: 1.0 # frequency of the publishing of the fused colored point cloud clicked_point_topic: '/clicked_point' # Topic published by Rviz when a point of the cloud is clicked. Used for plane detection +sensors: + sensors_timestamp_sync: false # Synchronize Sensors messages timestamp with latest received frame + 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: + od_enabled: false # True to enable Object Detection [not available for ZED] + model: 0 # '0': MULTI_CLASS_BOX - '1': MULTI_CLASS_BOX_ACCURATE + confidence_threshold: 50 # Minimum value of the detection confidence of an object [0,100] + max_range: 15. # Maximum detection range + object_tracking_enabled: true # Enable/disable the tracking of the detected objects + mc_people: true # Enable/disable the detection of persons for 'MULTI_CLASS_BOX_X' models + mc_vehicle: true # Enable/disable the detection of vehicles for 'MULTI_CLASS_BOX_X' models + mc_bag: true # Enable/disable the detection of bags for 'MULTI_CLASS_BOX_X' models + mc_animal: true # Enable/disable the detection of animals for 'MULTI_CLASS_BOX_X' models + mc_electronics: true # Enable/disable the detection of electronic devices for 'MULTI_CLASS_BOX_X' models + mc_fruit_vegetable: true # Enable/disable the detection of fruits and vegetables for 'MULTI_CLASS_BOX_X' models + mc_sport: true # Enable/disable the detection of sport-related objects for 'MULTI_CLASS_BOX_X' models diff --git a/zed_wrapper/params/zed.yaml b/zed_wrapper/params/zed.yaml index 1594c7f7..50669566 100644 --- a/zed_wrapper/params/zed.yaml +++ b/zed_wrapper/params/zed.yaml @@ -6,5 +6,5 @@ general: camera_model: 'zed' depth: - min_depth: 0.7 # Min: 0.3, Max: 3.0 - Default 0.7 - Note: reducing this value wil require more computational power and GPU memory + min_depth: 0.3 # Min: 0.3, Max: 3.0 - Default 0.7 - Note: reducing this value wil require more computational power and GPU memory max_depth: 10.0 # Max: 40.0 diff --git a/zed_wrapper/params/zed2.yaml b/zed_wrapper/params/zed2.yaml index c29e4a58..92365cdd 100644 --- a/zed_wrapper/params/zed2.yaml +++ b/zed_wrapper/params/zed2.yaml @@ -8,25 +8,4 @@ general: depth: min_depth: 0.3 # Min: 0.2, Max: 3.0 - Default 0.7 - Note: reducing this value wil require more computational power and GPU memory max_depth: 20.0 # Max: 40.0 - -pos_tracking: - imu_fusion: true # enable/disable IMU fusion. When set to false, only the optical odometry will be used. - -sensors: - sensors_timestamp_sync: false # Synchronize Sensors messages timestamp with latest received frame - 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: - od_enabled: false # True to enable Object Detection [not available for ZED] - model: 0 # '0': MULTI_CLASS_BOX - '1': MULTI_CLASS_BOX_ACCURATE - '2': HUMAN_BODY_FAST - '3': HUMAN_BODY_ACCURATE - '4': MULTI_CLASS_BOX_MEDIUM - '5': HUMAN_BODY_MEDIUM - '6': PERSON_HEAD_BOX - confidence_threshold: 50 # Minimum value of the detection confidence of an object [0,100] - max_range: 15. # Maximum detection range - object_tracking_enabled: true # Enable/disable the tracking of the detected objects - mc_people: true # Enable/disable the detection of persons for 'MULTI_CLASS_BOX_X' models - mc_vehicle: true # Enable/disable the detection of vehicles for 'MULTI_CLASS_BOX_X' models - mc_bag: true # Enable/disable the detection of bags for 'MULTI_CLASS_BOX_X' models - mc_animal: true # Enable/disable the detection of animals for 'MULTI_CLASS_BOX_X' models - mc_electronics: true # Enable/disable the detection of electronic devices for 'MULTI_CLASS_BOX_X' models - mc_fruit_vegetable: true # Enable/disable the detection of fruits and vegetables for 'MULTI_CLASS_BOX_X' models - mc_sport: true # Enable/disable the detection of sport-related objects for 'MULTI_CLASS_BOX_X' models + \ No newline at end of file diff --git a/zed_wrapper/params/zed2i.yaml b/zed_wrapper/params/zed2i.yaml index e43af67c..64b3fed5 100644 --- a/zed_wrapper/params/zed2i.yaml +++ b/zed_wrapper/params/zed2i.yaml @@ -1,32 +1,10 @@ # params/zed2i.yaml -# Parameters for Stereolabs ZED2 camera +# Parameters for Stereolabs ZED 2i camera --- general: camera_model: 'zed2i' depth: - min_depth: 0.3 # Min: 0.2, Max: 3.0 - Default 0.7 - Note: reducing this value wil require more computational power and GPU memory - max_depth: 5.0 # Max: 40.0 - -pos_tracking: - imu_fusion: true # enable/disable IMU fusion. When set to false, only the optical odometry will be used. - -sensors: - sensors_timestamp_sync: false # Synchronize Sensors messages timestamp with latest received frame - 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: - od_enabled: false # True to enable Object Detection [not available for ZED] - model: 0 # '0': MULTI_CLASS_BOX - '1': MULTI_CLASS_BOX_ACCURATE - '2': HUMAN_BODY_FAST - '3': HUMAN_BODY_ACCURATE - '4': MULTI_CLASS_BOX_MEDIUM - '5': HUMAN_BODY_MEDIUM - '6': PERSON_HEAD_BOX - confidence_threshold: 50 # Minimum value of the detection confidence of an object [0,100] - max_range: 15. # Maximum detection range - object_tracking_enabled: true # Enable/disable the tracking of the detected objects - mc_people: true # Enable/disable the detection of persons for 'MULTI_CLASS_BOX_X' models - mc_vehicle: true # Enable/disable the detection of vehicles for 'MULTI_CLASS_BOX_X' models - mc_bag: true # Enable/disable the detection of bags for 'MULTI_CLASS_BOX_X' models - mc_animal: true # Enable/disable the detection of animals for 'MULTI_CLASS_BOX_X' models - mc_electronics: true # Enable/disable the detection of electronic devices for 'MULTI_CLASS_BOX_X' models - mc_fruit_vegetable: true # Enable/disable the detection of fruits and vegetables for 'MULTI_CLASS_BOX_X' models - mc_sport: true # Enable/disable the detection of sport-related objects for 'MULTI_CLASS_BOX_X' models + min_depth: 0.2 # Min: 0.2, Max: 3.0 - Default 0.7 - Note: reducing this value wil require more computational power and GPU memory + max_depth: 10.0 # Max: 40.0 diff --git a/zed_wrapper/params/zedm.yaml b/zed_wrapper/params/zedm.yaml index 75fb2f17..14dad663 100644 --- a/zed_wrapper/params/zedm.yaml +++ b/zed_wrapper/params/zedm.yaml @@ -1,4 +1,4 @@ -# params/zedm_yaml +# params/zedm.yaml # Parameters for Stereolabs ZED mini camera --- @@ -6,27 +6,5 @@ general: camera_model: 'zedm' depth: - min_depth: 0.35 # Min: 0.1, Max: 3.0 - Default 0.3 - Note: reducing this value wil require more computational power and GPU memory - max_depth: 10.0 # Max: 20.0 - -pos_tracking: - imu_fusion: true # enable/disable IMU fusion. When set to false, only the optical odometry will be used. - -sensors: - sensors_timestamp_sync: false # Synchronize Sensors messages timestamp with latest received frame - max_pub_rate: 200. # max frequency of publishing of sensors data. MAX: 800. - MIN: grab rate - publish_imu_tf: true # publish `IMU -> _left_camera_frame` TF - -object_detection: - od_enabled: false # True to enable Object Detection [not available for ZED] - model: 0 # '0': MULTI_CLASS_BOX - '1': MULTI_CLASS_BOX_ACCURATE - '2': HUMAN_BODY_FAST - '3': HUMAN_BODY_ACCURATE - '4': MULTI_CLASS_BOX_MEDIUM - '5': HUMAN_BODY_MEDIUM - '6': PERSON_HEAD_BOX - confidence_threshold: 50 # Minimum value of the detection confidence of an object [0,100] - max_range: 15. # Maximum detection range - object_tracking_enabled: true # Enable/disable the tracking of the detected objects - mc_people: true # Enable/disable the detection of persons for 'MULTI_CLASS_BOX_X' models - mc_vehicle: true # Enable/disable the detection of vehicles for 'MULTI_CLASS_BOX_X' models - mc_bag: true # Enable/disable the detection of bags for 'MULTI_CLASS_BOX_X' models - mc_animal: true # Enable/disable the detection of animals for 'MULTI_CLASS_BOX_X' models - mc_electronics: true # Enable/disable the detection of electronic devices for 'MULTI_CLASS_BOX_X' models - mc_fruit_vegetable: true # Enable/disable the detection of fruits and vegetables for 'MULTI_CLASS_BOX_X' models - mc_sport: true # Enable/disable the detection of sport-related objects for 'MULTI_CLASS_BOX_X' models + min_depth: 0.1 # Min: 0.1, Max: 3.0 - Default 0.3 - Note: reducing this value wil require more computational power and GPU memory + max_depth: 10.0 # Max: 20.0 diff --git a/zed_wrapper/params/zedx.yaml b/zed_wrapper/params/zedx.yaml new file mode 100644 index 00000000..6fa13b7a --- /dev/null +++ b/zed_wrapper/params/zedx.yaml @@ -0,0 +1,10 @@ +# params/zedx.yaml +# Parameters for Stereolabs ZED X camera +--- + +general: + camera_model: 'zedx' + +depth: + min_depth: 0.2 # Min: 0.2, Max: 3.0 - Default 0.7 - Note: reducing this value wil require more computational power and GPU memory + max_depth: 10.0 # Max: 40.0 diff --git a/zed_wrapper/params/zedxm.yaml b/zed_wrapper/params/zedxm.yaml new file mode 100644 index 00000000..9d00c811 --- /dev/null +++ b/zed_wrapper/params/zedxm.yaml @@ -0,0 +1,10 @@ +# params/zedxm.yaml +# Parameters for Stereolabs ZED X Mini camera +--- + +general: + camera_model: 'zedxm' + +depth: + min_depth: 0.1 # Min: 0.1, Max: 3.0 - Default 0.7 - Note: reducing this value wil require more computational power and GPU memory + max_depth: 10.0 # Max: 40.0 diff --git a/zed_wrapper/records/record_depth.sh b/zed_wrapper/records/record_depth.sh deleted file mode 100644 index b7f73c33..00000000 --- a/zed_wrapper/records/record_depth.sh +++ /dev/null @@ -1,2 +0,0 @@ -#!/bin/bash -rosbag record /camera/rgb/camera_info /camera/depth_registered/image_raw/compressedDepth /camera/rgb/image_rect_color/compressed /tf diff --git a/zed_wrapper/records/record_stereo.sh b/zed_wrapper/records/record_stereo.sh deleted file mode 100644 index 97d4cfcc..00000000 --- a/zed_wrapper/records/record_stereo.sh +++ /dev/null @@ -1,2 +0,0 @@ -#!/bin/bash -rosbag record /camera/left/camera_info /camera/right/camera_info /camera/left/image_raw/compressed /camera/right/image_raw/compressed /tf From 7672aedd23b5a5146e82de12c7548ed931c46ec6 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Fri, 31 Mar 2023 15:02:17 +0200 Subject: [PATCH 146/199] Copyright date update --- LICENSE | 2 +- .../include/rgbd_sensor_demux.hpp | 2 +- .../src/rgbd_sensor_demux.cpp | 2 +- .../include/rgbd_sensor_sync.hpp | 2 +- .../src/rgbd_sensor_sync.cpp | 2 +- zed_nodelets/src/tools/include/sl_tools.h | 8 +-- zed_nodelets/src/tools/src/sl_tools.cpp | 2 +- .../include/zed_wrapper_nodelet.hpp | 10 ++-- .../zed_nodelet/src/zed_wrapper_nodelet.cpp | 2 +- .../launch/include/zed_camera.launch.xml | 2 +- .../launch/include/zed_camera_nodelet.launch | 2 +- .../launch/include/zed_no_tf.launch.xml | 2 +- zed_wrapper/launch/zed.launch | 2 +- zed_wrapper/launch/zed2.launch | 2 +- zed_wrapper/launch/zed2i.launch | 2 +- zed_wrapper/launch/zed_no_tf.launch | 2 +- zed_wrapper/launch/zedm.launch | 2 +- zed_wrapper/launch/zedx.launch | 55 +++++++++++++++++++ zed_wrapper/launch/zedxm.launch | 55 +++++++++++++++++++ zed_wrapper/src/zed_wrapper_node.cpp | 3 +- zed_wrapper/urdf/include/materials.urdf.xacro | 3 +- zed_wrapper/urdf/zed_descr.urdf.xacro | 3 +- zed_wrapper/urdf/zed_macro.urdf.xacro | 3 +- 23 files changed, 138 insertions(+), 32 deletions(-) create mode 100644 zed_wrapper/launch/zedx.launch create mode 100644 zed_wrapper/launch/zedxm.launch diff --git a/LICENSE b/LICENSE index b0e047d1..93631b21 100644 --- a/LICENSE +++ b/LICENSE @@ -1,6 +1,6 @@ MIT License -Copyright (c) 2020 Stereolabs +Copyright (c) 2023 Stereolabs Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal diff --git a/zed_nodelets/src/rgbd_sensors_demux_nodelet/include/rgbd_sensor_demux.hpp b/zed_nodelets/src/rgbd_sensors_demux_nodelet/include/rgbd_sensor_demux.hpp index 581bd8c1..713e85a0 100644 --- a/zed_nodelets/src/rgbd_sensors_demux_nodelet/include/rgbd_sensor_demux.hpp +++ b/zed_nodelets/src/rgbd_sensors_demux_nodelet/include/rgbd_sensor_demux.hpp @@ -1,6 +1,6 @@ /////////////////////////////////////////////////////////////////////////// // -// Copyright (c) 2020, STEREOLABS. +// Copyright (c) 2023, STEREOLABS. // // All rights reserved. // diff --git a/zed_nodelets/src/rgbd_sensors_demux_nodelet/src/rgbd_sensor_demux.cpp b/zed_nodelets/src/rgbd_sensors_demux_nodelet/src/rgbd_sensor_demux.cpp index f4199d53..0ff9473c 100644 --- a/zed_nodelets/src/rgbd_sensors_demux_nodelet/src/rgbd_sensor_demux.cpp +++ b/zed_nodelets/src/rgbd_sensors_demux_nodelet/src/rgbd_sensor_demux.cpp @@ -1,6 +1,6 @@ /////////////////////////////////////////////////////////////////////////// // -// Copyright (c) 2020, STEREOLABS. +// Copyright (c) 2023, STEREOLABS. // // All rights reserved. // diff --git a/zed_nodelets/src/rgbd_sensors_sync_nodelet/include/rgbd_sensor_sync.hpp b/zed_nodelets/src/rgbd_sensors_sync_nodelet/include/rgbd_sensor_sync.hpp index d25c1659..0f5d4719 100644 --- a/zed_nodelets/src/rgbd_sensors_sync_nodelet/include/rgbd_sensor_sync.hpp +++ b/zed_nodelets/src/rgbd_sensors_sync_nodelet/include/rgbd_sensor_sync.hpp @@ -1,6 +1,6 @@ /////////////////////////////////////////////////////////////////////////// // -// Copyright (c) 2020, STEREOLABS. +// Copyright (c) 2023, STEREOLABS. // // All rights reserved. // diff --git a/zed_nodelets/src/rgbd_sensors_sync_nodelet/src/rgbd_sensor_sync.cpp b/zed_nodelets/src/rgbd_sensors_sync_nodelet/src/rgbd_sensor_sync.cpp index efa8dbf1..2a2046cd 100644 --- a/zed_nodelets/src/rgbd_sensors_sync_nodelet/src/rgbd_sensor_sync.cpp +++ b/zed_nodelets/src/rgbd_sensors_sync_nodelet/src/rgbd_sensor_sync.cpp @@ -1,6 +1,6 @@ /////////////////////////////////////////////////////////////////////////// // -// Copyright (c) 2020, STEREOLABS. +// Copyright (c) 2023, STEREOLABS. // // All rights reserved. // diff --git a/zed_nodelets/src/tools/include/sl_tools.h b/zed_nodelets/src/tools/include/sl_tools.h index c6d86513..a254866d 100644 --- a/zed_nodelets/src/tools/include/sl_tools.h +++ b/zed_nodelets/src/tools/include/sl_tools.h @@ -1,9 +1,6 @@ -#ifndef SL_TOOLS_H -#define SL_TOOLS_H - /////////////////////////////////////////////////////////////////////////// // -// Copyright (c) 2020, STEREOLABS. +// Copyright (c) 2023, STEREOLABS. // // All rights reserved. // @@ -21,6 +18,9 @@ // /////////////////////////////////////////////////////////////////////////// +#ifndef SL_TOOLS_H +#define SL_TOOLS_H + #include #include #include diff --git a/zed_nodelets/src/tools/src/sl_tools.cpp b/zed_nodelets/src/tools/src/sl_tools.cpp index 4a9e3ef8..97854d73 100644 --- a/zed_nodelets/src/tools/src/sl_tools.cpp +++ b/zed_nodelets/src/tools/src/sl_tools.cpp @@ -1,6 +1,6 @@ /////////////////////////////////////////////////////////////////////////// // -// Copyright (c) 2020, STEREOLABS. +// Copyright (c) 2023, STEREOLABS. // // All rights reserved. // 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 53b743a0..42c94a2c 100644 --- a/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp +++ b/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp @@ -1,9 +1,6 @@ -#ifndef ZED_WRAPPER_NODELET_H -#define ZED_WRAPPER_NODELET_H - -/////////////////////////////////////////////////////////////////////////// +/////////////////////////////////////////////////////////////////////////// // -// Copyright (c) 2020, STEREOLABS. +// Copyright (c) 2023, STEREOLABS. // // All rights reserved. // @@ -21,6 +18,9 @@ // /////////////////////////////////////////////////////////////////////////// +#ifndef ZED_WRAPPER_NODELET_H +#define ZED_WRAPPER_NODELET_H + #include #include #include 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 603b2851..948b4e95 100644 --- a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp @@ -1,6 +1,6 @@ /////////////////////////////////////////////////////////////////////////// // -// Copyright (c) 2020, STEREOLABS. +// Copyright (c) 2023, STEREOLABS. // // All rights reserved. // diff --git a/zed_wrapper/launch/include/zed_camera.launch.xml b/zed_wrapper/launch/include/zed_camera.launch.xml index f0736a92..9a50e661 100644 --- a/zed_wrapper/launch/include/zed_camera.launch.xml +++ b/zed_wrapper/launch/include/zed_camera.launch.xml @@ -1,6 +1,6 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/zed_wrapper/launch/zedxm.launch b/zed_wrapper/launch/zedxm.launch new file mode 100644 index 00000000..3d2d5c37 --- /dev/null +++ b/zed_wrapper/launch/zedxm.launch @@ -0,0 +1,55 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/zed_wrapper/src/zed_wrapper_node.cpp b/zed_wrapper/src/zed_wrapper_node.cpp index 050fb02f..25235ab0 100644 --- a/zed_wrapper/src/zed_wrapper_node.cpp +++ b/zed_wrapper/src/zed_wrapper_node.cpp @@ -1,7 +1,6 @@ // ///////////////////////////////////////////////////////////////////////// - // -// Copyright (c) 2020, STEREOLABS. +// Copyright (c) 2023, STEREOLABS. // // All rights reserved. // diff --git a/zed_wrapper/urdf/include/materials.urdf.xacro b/zed_wrapper/urdf/include/materials.urdf.xacro index 8ccb50b1..83812c46 100644 --- a/zed_wrapper/urdf/include/materials.urdf.xacro +++ b/zed_wrapper/urdf/include/materials.urdf.xacro @@ -1,7 +1,6 @@ - @@ -30,4 +29,10 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + + + + + + diff --git a/zed_wrapper/urdf/zed_descr.urdf.xacro b/zed_wrapper/urdf/zed_descr.urdf.xacro index 830c46ef..78e84c2a 100644 --- a/zed_wrapper/urdf/zed_descr.urdf.xacro +++ b/zed_wrapper/urdf/zed_descr.urdf.xacro @@ -1,20 +1,19 @@ - @@ -27,11 +26,27 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + - + + + + + + + + + + + + + diff --git a/zed_wrapper/urdf/zed_macro.urdf.xacro b/zed_wrapper/urdf/zed_macro.urdf.xacro index 28f70af0..4cc9eebb 100644 --- a/zed_wrapper/urdf/zed_macro.urdf.xacro +++ b/zed_wrapper/urdf/zed_macro.urdf.xacro @@ -1,20 +1,19 @@ - @@ -34,6 +33,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + @@ -41,6 +41,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + @@ -48,6 +49,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + @@ -55,10 +57,25 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + + + + + + + + + + + + + + + + + - - @@ -68,11 +85,10 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - - + @@ -90,41 +106,39 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - - + - + - - + - + - + @@ -153,5 +167,6 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + From 0d25a2ea27c760582b803fd694a359393adf65bf Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Mon, 3 Apr 2023 09:11:45 +0200 Subject: [PATCH 149/199] Update submodule --- zed-ros-interfaces | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/zed-ros-interfaces b/zed-ros-interfaces index dff089f2..32dcbb95 160000 --- a/zed-ros-interfaces +++ b/zed-ros-interfaces @@ -1 +1 @@ -Subproject commit dff089f242d4d17ef88fe95b15f155336edd6466 +Subproject commit 32dcbb956eef25be1738f946eb37254b82513f52 From 06947801273bdaee9850090b80a524ada19cff10 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Mon, 3 Apr 2023 09:19:03 +0200 Subject: [PATCH 150/199] Update Info --- .github/ISSUE_TEMPLATE/2_bug_report.yml | 4 ++-- CHANGELOG.rst | 1 + README.md | 6 ------ zed-ros-interfaces | 2 +- 4 files changed, 4 insertions(+), 9 deletions(-) diff --git a/.github/ISSUE_TEMPLATE/2_bug_report.yml b/.github/ISSUE_TEMPLATE/2_bug_report.yml index 30ac21e9..3deb72c7 100644 --- a/.github/ISSUE_TEMPLATE/2_bug_report.yml +++ b/.github/ISSUE_TEMPLATE/2_bug_report.yml @@ -74,8 +74,8 @@ body: OS: Operating System CPU: e.g. ARM GPU: Nvidia Jetson Xavier NX - ZED SDK version: e.g. v3.5.3 - Other info: e.g. ROS Melodic + ZED SDK version: e.g. v4.0 + Other info: e.g. ROS Noetic validations: required: true - type: textarea diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 8108e7ae..c9af60bb 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -10,6 +10,7 @@ CHANGELOG - Skeleton Tracking is no more available. Migrate to ROS 2 Wrapper if you need it - `sensors` and `object_detection` parameters are now in `common.yaml` - Move parameters `general.resolution` and `general.grab_frame_rate` to cameras yaml files to support the different configurations on ZED X and ZED X Mini. +- Remove support for ROS Melodic that reached EOL v3.8.x ------ diff --git a/README.md b/README.md index 951e3edc..f3291d1c 100644 --- a/README.md +++ b/README.md @@ -20,12 +20,6 @@ This package lets you use the ZED stereo camera with ROS. It outputs the camera - [ZED SDK **≥ 4.0**](https://www.stereolabs.com/developers/) and its dependency [CUDA](https://developer.nvidia.com/cuda-downloads) - [ROS Noetic](http://wiki.ros.org/noetic/Installation/Ubuntu) -or - -- Ubuntu 18.04 -- [ZED SDK **≥ 4.0**](https://www.stereolabs.com/developers/) and its dependency [CUDA](https://developer.nvidia.com/cuda-downloads) -- [ROS Melodic](http://wiki.ros.org/melodic/Installation/Ubuntu) - ### Build the repository The zed_ros_wrapper is a catkin package. It depends on the following ROS packages: diff --git a/zed-ros-interfaces b/zed-ros-interfaces index 32dcbb95..ee587941 160000 --- a/zed-ros-interfaces +++ b/zed-ros-interfaces @@ -1 +1 @@ -Subproject commit 32dcbb956eef25be1738f946eb37254b82513f52 +Subproject commit ee587941fcf7b3135f3c2ffc19a0462a93147ca4 From 9e81ee792fa0faccb2fa87bbfc5a24265e4e02a7 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Mon, 3 Apr 2023 10:43:09 +0200 Subject: [PATCH 151/199] Update zed_descr.urdf.xacro --- zed_wrapper/urdf/zed_descr.urdf.xacro | 12 ------------ 1 file changed, 12 deletions(-) diff --git a/zed_wrapper/urdf/zed_descr.urdf.xacro b/zed_wrapper/urdf/zed_descr.urdf.xacro index 78e84c2a..1ae2e0b8 100644 --- a/zed_wrapper/urdf/zed_descr.urdf.xacro +++ b/zed_wrapper/urdf/zed_descr.urdf.xacro @@ -37,16 +37,4 @@ - - - - - - - - - - - - From f0eb03c93922d634eff23ad02a357b11c71cf792 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Wed, 19 Apr 2023 11:03:30 +0200 Subject: [PATCH 152/199] Update zedx.launch --- zed_wrapper/launch/zedx.launch | 2 ++ 1 file changed, 2 insertions(+) diff --git a/zed_wrapper/launch/zedx.launch b/zed_wrapper/launch/zedx.launch index 605bd2be..5dc485b9 100644 --- a/zed_wrapper/launch/zedx.launch +++ b/zed_wrapper/launch/zedx.launch @@ -25,6 +25,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + @@ -43,6 +44,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + From ab7307bca176ee621e19d2402959a276af2b7caf Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Wed, 19 Apr 2023 11:03:41 +0200 Subject: [PATCH 153/199] Update zedxm.launch --- zed_wrapper/launch/zedxm.launch | 2 ++ 1 file changed, 2 insertions(+) diff --git a/zed_wrapper/launch/zedxm.launch b/zed_wrapper/launch/zedxm.launch index 3d2d5c37..adcc9e63 100644 --- a/zed_wrapper/launch/zedxm.launch +++ b/zed_wrapper/launch/zedxm.launch @@ -25,6 +25,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + @@ -43,6 +44,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + From a5a6048cdf99e6159fe81a3e44cb66d07ced2686 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Thu, 27 Apr 2023 12:51:06 +0200 Subject: [PATCH 154/199] Add 1080p for ZED X and ZED X Mini --- CHANGELOG.rst | 4 + .../include/zed_wrapper_nodelet.hpp | 5 + .../zed_nodelet/src/zed_wrapper_nodelet.cpp | 148 +++++++++++++----- zed_wrapper/params/zedx.yaml | 4 +- zed_wrapper/params/zedxm.yaml | 4 +- 5 files changed, 122 insertions(+), 43 deletions(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index c9af60bb..e27eac74 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -1,6 +1,10 @@ CHANGELOG ========= +2023-04-27 +---------- +- Add 1080p for ZED X and ZED X Mini + 2023-03-31 ---------- - Support for ZED SDK v4.0 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 42c94a2c..c1160508 100644 --- a/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp +++ b/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp @@ -231,6 +231,11 @@ class ZEDWrapperNodelet : public nodelet::Nodelet { */ void fillCamDepthInfo(sl::Camera& zed, sensor_msgs::CameraInfoPtr depth_info_msg, std::string frame_id); + /*! \brief Check if Resolution is valid for the camera model. + * Modifies Resolution to match correct value. + */ + void checkResol(); + /*! \brief Check if FPS and Resolution chosen by user are correct. * Modifies FPS to match correct value. */ 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 04c5ccff..dbb0f786 100644 --- a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp @@ -196,7 +196,7 @@ void ZEDWrapperNodelet::onInit() mSvoMode = true; } else { mZedParams.camera_fps = mCamFrameRate; - mZedParams.camera_resolution = static_cast(mCamResol); + mZedParams.camera_resolution = mCamResol; if (mZedSerialNumber == 0) { mZedParams.input.setFromCameraID(mZedId); @@ -578,12 +578,39 @@ void ZEDWrapperNodelet::readParameters() // ----> General // Get parameters from param files + std::string camera_model; + mNhNs.getParam("general/camera_model", camera_model); + + if (camera_model == "zed") { + mZedUserCamModel = sl::MODEL::ZED; + NODELET_INFO_STREAM(" * Camera Model by param\t-> " << camera_model); + } else if (camera_model == "zedm") { + mZedUserCamModel = sl::MODEL::ZED_M; + NODELET_INFO_STREAM(" * Camera Model by param\t-> " << camera_model); + } else if (camera_model == "zed2") { + mZedUserCamModel = sl::MODEL::ZED2; + NODELET_INFO_STREAM(" * Camera Model by param\t-> " << camera_model); + } else if (camera_model == "zed2i") { + mZedUserCamModel = sl::MODEL::ZED2i; + NODELET_INFO_STREAM(" * Camera Model by param\t-> " << camera_model); + } else if (camera_model == "zedx") { + mZedUserCamModel = sl::MODEL::ZED_X; + NODELET_INFO_STREAM(" * Camera Model by param\t-> " << camera_model); + } else if (camera_model == "zedxm") { + mZedUserCamModel = sl::MODEL::ZED_XM; + NODELET_INFO_STREAM(" * Camera Model by param\t-> " << camera_model); + } else { + NODELET_ERROR_STREAM("Camera model not valid: " << camera_model); + exit(EXIT_FAILURE); + } + mNhNs.getParam("general/camera_name", mCameraName); NODELET_INFO_STREAM(" * Camera Name\t\t\t-> " << mCameraName.c_str()); int resol; mNhNs.getParam("general/resolution", resol); mCamResol = static_cast(resol); - NODELET_INFO_STREAM(" * Camera Resolution\t\t-> " << sl::toString(mCamResol).c_str()); + checkResol(); + NODELET_INFO_STREAM(" * Camera Resolution\t\t-> " << sl::toString(mCamResol).c_str()); mNhNs.getParam("general/grab_frame_rate", mCamFrameRate); checkResolFps(); NODELET_INFO_STREAM(" * Camera Grab Framerate\t-> " << mCamFrameRate); @@ -605,32 +632,7 @@ void ZEDWrapperNodelet::readParameters() if (tmp_sn > 0) { mZedSerialNumber = static_cast(tmp_sn); NODELET_INFO_STREAM(" * Serial number\t\t-> " << mZedSerialNumber); - } - - std::string camera_model; - mNhNs.getParam("general/camera_model", camera_model); - - if (camera_model == "zed") { - mZedUserCamModel = sl::MODEL::ZED; - NODELET_INFO_STREAM(" * Camera Model by param\t-> " << camera_model); - } else if (camera_model == "zedm") { - mZedUserCamModel = sl::MODEL::ZED_M; - NODELET_INFO_STREAM(" * Camera Model by param\t-> " << camera_model); - } else if (camera_model == "zed2") { - mZedUserCamModel = sl::MODEL::ZED2; - NODELET_INFO_STREAM(" * Camera Model by param\t-> " << camera_model); - } else if (camera_model == "zed2i") { - mZedUserCamModel = sl::MODEL::ZED2i; - NODELET_INFO_STREAM(" * Camera Model by param\t-> " << camera_model); - } else if (camera_model == "zedx") { - mZedUserCamModel = sl::MODEL::ZED_X; - NODELET_INFO_STREAM(" * Camera Model by param\t-> " << camera_model); - } else if (camera_model == "zedxm") { - mZedUserCamModel = sl::MODEL::ZED_XM; - NODELET_INFO_STREAM(" * Camera Model by param\t-> " << camera_model); - } else { - NODELET_ERROR_STREAM("Camera model not valid: " << camera_model); - } + } // <---- General NODELET_INFO_STREAM("*** VIDEO PARAMETERS ***"); @@ -922,6 +924,54 @@ void ZEDWrapperNodelet::readParameters() // <---- Dynamic } +void ZEDWrapperNodelet::checkResol() +{ + switch (mCamResol) { + case sl::RESOLUTION::HD2K: + if (mZedUserCamModel == sl::MODEL::ZED_X || mZedUserCamModel == sl::MODEL::ZED_XM) { + NODELET_WARN("Selected a not valid resolution. Modified to 'AUTO'"); + mCamResol = sl::RESOLUTION::AUTO; + } + break; + + case sl::RESOLUTION::HD1200: + if (mZedUserCamModel != sl::MODEL::ZED_X && mZedUserCamModel != sl::MODEL::ZED_XM) { + NODELET_WARN("Selected a not valid resolution. Modified to 'AUTO'"); + mCamResol = sl::RESOLUTION::AUTO; + } + break; + + case sl::RESOLUTION::HD1080: + break; + + case sl::RESOLUTION::HD720 : + if (mZedUserCamModel == sl::MODEL::ZED_X || mZedUserCamModel == sl::MODEL::ZED_XM) { + NODELET_WARN("Selected a not valid resolution. Modified to 'AUTO'"); + mCamResol = sl::RESOLUTION::AUTO; + } + break; + + case sl::RESOLUTION::SVGA: + if (mZedUserCamModel != sl::MODEL::ZED_X && mZedUserCamModel != sl::MODEL::ZED_XM) { + NODELET_WARN("Selected a not valid resolution. Modified to 'AUTO'"); + mCamResol = sl::RESOLUTION::AUTO; + } + break; + + case sl::RESOLUTION::VGA: + if (mZedUserCamModel == sl::MODEL::ZED_X || mZedUserCamModel == sl::MODEL::ZED_XM) { + NODELET_WARN("Selected a not valid resolution. Modified to 'AUTO'"); + mCamResol = sl::RESOLUTION::AUTO; + } + break; + + default: + NODELET_WARN("Selected a not valid resolution. Modified to 'AUTO'"); + mCamResol = sl::RESOLUTION::AUTO; + break; + } +} + void ZEDWrapperNodelet::checkResolFps() { switch (mCamResol) { @@ -934,19 +984,39 @@ void ZEDWrapperNodelet::checkResolFps() break; case sl::RESOLUTION::HD1080: - if (mCamFrameRate == 15 || mCamFrameRate == 30) { - break; + if (mZedUserCamModel == sl::MODEL::ZED_X || mZedUserCamModel == sl::MODEL::ZED_XM) + { + if (mCamFrameRate == 60 || mCamFrameRate == 30) { + break; + } + + if (mCamFrameRate > 30 && mCamFrameRate < 60) { + NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD1080. Set to 30 FPS."); + mCamFrameRate = 30; + } else if (mCamFrameRate > 60) { + NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD1080. Set to 60 FPS."); + mCamFrameRate = 60; + } else { + NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD1080. Set to 30 FPS."); + mCamFrameRate = 30; + } } + else + { + if (mCamFrameRate == 15 || mCamFrameRate == 30) { + break; + } - if (mCamFrameRate > 15 && mCamFrameRate < 30) { - NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD1080. Set to 15 FPS."); - mCamFrameRate = 15; - } else if (mCamFrameRate > 30) { - NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD1080. Set to 30 FPS."); - mCamFrameRate = 30; - } else { - NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD1080. Set to 15 FPS."); - mCamFrameRate = 15; + if (mCamFrameRate > 15 && mCamFrameRate < 30) { + NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD1080. Set to 15 FPS."); + mCamFrameRate = 15; + } else if (mCamFrameRate > 30) { + NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD1080. Set to 30 FPS."); + mCamFrameRate = 30; + } else { + NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD1080. Set to 15 FPS."); + mCamFrameRate = 15; + } } break; diff --git a/zed_wrapper/params/zedx.yaml b/zed_wrapper/params/zedx.yaml index 6050d125..7a2d5794 100644 --- a/zed_wrapper/params/zedx.yaml +++ b/zed_wrapper/params/zedx.yaml @@ -4,8 +4,8 @@ general: camera_model: 'zedx' - resolution: 2 # '2': HD1200 - '4': SVGA - '6': AUTO - grab_frame_rate: 60 # 120, 60, 30 + resolution: 1 # '1': HD1080 - '2': HD1200 - '4': SVGA - '6': AUTO + grab_frame_rate: 60 # 120, 60, 30 depth: min_depth: 0.2 # Min: 0.2, Max: 3.0 - Default 0.7 - Note: reducing this value wil require more computational power and GPU memory diff --git a/zed_wrapper/params/zedxm.yaml b/zed_wrapper/params/zedxm.yaml index e125a3c7..b3d18f5a 100644 --- a/zed_wrapper/params/zedxm.yaml +++ b/zed_wrapper/params/zedxm.yaml @@ -4,8 +4,8 @@ general: camera_model: 'zedxm' - resolution: 2 # '2': HD1200 - '4': SVGA - '6': AUTO - grab_frame_rate: 60 # 120, 60, 30 + resolution: 2 # '1': HD1080 - '2': HD1200 - '4': SVGA - '6': AUTO + grab_frame_rate: 60 # 120, 60, 30 depth: min_depth: 0.1 # Min: 0.1, Max: 3.0 - Default 0.7 - Note: reducing this value wil require more computational power and GPU memory From 0a7244d1cd0acc6508f1eef6bb6d8e8290779fe4 Mon Sep 17 00:00:00 2001 From: betaBison Date: Tue, 9 May 2023 15:32:21 -0700 Subject: [PATCH 155/199] zero Rz cross covariance terms --- zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 dbb0f786..8ec5c77d 100644 --- a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp @@ -1721,7 +1721,7 @@ void ZEDWrapperNodelet::publishOdom(tf2::Transform odom2baseTransf, sl::Pose& sl if (mTwoDMode) { if (i == 14 || i == 21 || i == 28) { odomMsg->pose.covariance[i] = 1e-9; // Very low covariance if 2D mode - } else if ((i >= 2 && i <= 4) || (i >= 8 && i <= 10) || (i >= 12 && i <= 13) || (i >= 15 && i <= 16) || (i >= 18 && i <= 20) || (i == 22) || (i >= 24 && i <= 27)) { + } else if ((i >= 2 && i <= 4) || (i >= 8 && i <= 10) || (i >= 12 && i <= 13) || (i >= 15 && i <= 20) || (i >= 22 && i <= 27) || (i == 29) || (i >= 32 && i <= 34)) { odomMsg->pose.covariance[i] = 0.0; } } From d74cd5f3006b4bdf95906f59d83f8b2234e5aa12 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Wed, 5 Jul 2023 22:33:29 +0200 Subject: [PATCH 156/199] Update common.yaml --- zed_wrapper/params/common.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/zed_wrapper/params/common.yaml b/zed_wrapper/params/common.yaml index c6b039a6..de5160e0 100644 --- a/zed_wrapper/params/common.yaml +++ b/zed_wrapper/params/common.yaml @@ -35,7 +35,7 @@ video: depth: quality: 3 # '0': NONE, '1': PERFORMANCE, '2': QUALITY, '3': ULTRA, '4': NEURAL - depth_stabilization: 1 # `0`: disabled, `1`: enabled + depth_stabilization: 1 # [0-100] - 0: Disabled openni_depth_mode: false # 'false': 32bit float meters, 'true': 16bit uchar millimeters depth_downsample_factor: 0.5 # Resample factor for depth data matrices [0.01,1.0] The SDK works with native data sizes, but publishes rescaled matrices (depth map, point cloud, ...) From 8ff3c281e17bea20ae4e2d8b8b63475624f8252c Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Wed, 5 Jul 2023 22:35:43 +0200 Subject: [PATCH 157/199] Improve Depth stabilization --- zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 8ec5c77d..b79bca9a 100644 --- a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp @@ -652,7 +652,7 @@ void ZEDWrapperNodelet::readParameters() mNhNs.getParam("depth/openni_depth_mode", mOpenniDepthMode); NODELET_INFO_STREAM(" * OpenNI mode\t\t\t-> " << (mOpenniDepthMode ? "ENABLED" : "DISABLED")); mNhNs.getParam("depth/depth_stabilization", mDepthStabilization); - NODELET_INFO_STREAM(" * Depth Stabilization\t\t-> " << (mDepthStabilization ? "ENABLED" : "DISABLED")); + NODELET_INFO_STREAM(" * Depth Stabilization\t\t-> " << mDepthStabilization ); mNhNs.getParam("depth/min_depth", mCamMinDepth); NODELET_INFO_STREAM(" * Minimum depth\t\t-> " << mCamMinDepth << " m"); mNhNs.getParam("depth/max_depth", mCamMaxDepth); From 2c929baaef728a324e7266d8318356408ada1adf Mon Sep 17 00:00:00 2001 From: davideCremona Date: Wed, 19 Jul 2023 17:33:47 +0200 Subject: [PATCH 158/199] Update zed_wrapper_nodelet.cpp --- zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) 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 b79bca9a..d1dfc4de 100644 --- a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp @@ -4412,7 +4412,8 @@ void ZEDWrapperNodelet::processDetectedObjects(ros::Time t) for (auto data : objects.object_list) { objMsg->objects[idx].label = sl::toString(data.label).c_str(); objMsg->objects[idx].sublabel = sl::toString(data.sublabel).c_str(); - objMsg->objects[idx].label_id = data.id; + objMsg->objects[idx].label_id = data.raw_label; + objMsg->objects[idx].instance_id = data.id; objMsg->objects[idx].confidence = data.confidence; memcpy(&(objMsg->objects[idx].position[0]), &(data.position[0]), 3 * sizeof(float)); From 6378443bfb1c718a0d4d6c1d622b920379363704 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Thu, 20 Jul 2023 09:16:11 +0200 Subject: [PATCH 159/199] Update submodule --- zed-ros-interfaces | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/zed-ros-interfaces b/zed-ros-interfaces index ee587941..73bfaa13 160000 --- a/zed-ros-interfaces +++ b/zed-ros-interfaces @@ -1 +1 @@ -Subproject commit ee587941fcf7b3135f3c2ffc19a0462a93147ca4 +Subproject commit 73bfaa136c408afd3869d7877e4a95bbf412cc09 From 6eee00b7b03aa7394f139b95f46f1252e7577c1d Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Fri, 8 Sep 2023 14:03:18 +0200 Subject: [PATCH 160/199] Add support for "grab_compute_capping_fps" - `pub_frame_rate` now controls the `InitParameters::grab_compute_capping_fps` parameter of the ZED SDK instead of controlling the frequency of a parallel thread. It's not a Dynamic parameter anymore. - Change `general/camera_flip` parameter to string: 'AUTO', 'ON', 'OFF' - Change 'general/verbose' from bool to integer --- CHANGELOG.rst | 7 ++ zed_nodelets/cfg/Zed.cfg | 1 - .../include/zed_wrapper_nodelet.hpp | 21 +++--- .../zed_nodelet/src/zed_wrapper_nodelet.cpp | 68 ++++++++----------- zed_wrapper/cfg/Zed.cfg | 1 - zed_wrapper/params/common.yaml | 6 +- zed_wrapper/params/zed.yaml | 2 +- zed_wrapper/params/zed2.yaml | 2 +- zed_wrapper/params/zed2i.yaml | 6 +- zed_wrapper/params/zedm.yaml | 2 +- 10 files changed, 52 insertions(+), 64 deletions(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index e27eac74..7a6fa878 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -1,6 +1,13 @@ CHANGELOG ========= +2023-09-07 +---------- +- `pub_frame_rate` now controls the `InitParameters::grab_compute_capping_fps` parameter of the ZED SDK instead of controlling the frequency of a parallel thread. It's not a Dynamic parameter anymore. +- Change `general/camera_flip` parameter to string: 'AUTO', 'ON', 'OFF' +- Change 'general/verbose' from bool to integer + + 2023-04-27 ---------- - Add 1080p for ZED X and ZED X Mini diff --git a/zed_nodelets/cfg/Zed.cfg b/zed_nodelets/cfg/Zed.cfg index be86bed4..fe41f8cd 100755 --- a/zed_nodelets/cfg/Zed.cfg +++ b/zed_nodelets/cfg/Zed.cfg @@ -6,7 +6,6 @@ from dynamic_reconfigure.parameter_generator_catkin import * gen = ParameterGenerator() group_general = gen.add_group("general") -group_general.add("pub_frame_rate", double_t, 0, "Video and Depth data frequency", 15.0, 0.1, 60.0); group_depth = gen.add_group("depth") group_depth.add("depth_confidence", int_t, 1, "Depth confidence threshold", 50, 1, 100) 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 c1160508..52adb949 100644 --- a/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp +++ b/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp @@ -190,6 +190,11 @@ class ZEDWrapperNodelet : public nodelet::Nodelet { */ void callback_pubFusedPointCloud(const ros::TimerEvent& e); + /*! + * @brief Publish Color and Depth images + */ + void pubVideoDepth(); + /*! \brief Publish the informations of a camera with a ros Publisher * \param cam_info_msg : the information message to publish * \param pub_cam_info : the publisher object to use @@ -245,11 +250,6 @@ class ZEDWrapperNodelet : public nodelet::Nodelet { */ void callback_dynamicReconf(zed_nodelets::ZedConfig& config, uint32_t level); - /*! \brief Callback to publish Video and Depth data - * \param e : the ros::TimerEvent binded to the callback - */ - void callback_pubVideoDepth(const ros::TimerEvent& e); - /*! \brief Callback to publish Path data with a ROS publisher. * \param e : the ros::TimerEvent binded to the callback */ @@ -467,7 +467,6 @@ class ZEDWrapperNodelet : public nodelet::Nodelet { // Timers ros::Timer mPathTimer; ros::Timer mFusedPcTimer; - ros::Timer mVideoDepthTimer; // Services ros::ServiceServer mSrvSetInitPose; @@ -538,13 +537,14 @@ class ZEDWrapperNodelet : public nodelet::Nodelet { bool mPublishTf; bool mPublishMapTf; bool mPublishImuTf; - bool mCameraFlip; + sl::FLIP_MODE mCameraFlip; bool mCameraSelfCalib; // Launch file parameters std::string mCameraName; sl::RESOLUTION mCamResol; int mCamFrameRate; + double mVideoDepthFreq = 15.; sl::DEPTH_MODE mDepthMode; int mGpuId; int mZedId; @@ -557,7 +557,7 @@ class ZEDWrapperNodelet : public nodelet::Nodelet { double mSensPubRate = 400.0; double mPathPubRate; int mPathMaxCount; - bool mVerbose; + int mVerbose; bool mSvoMode = false; double mCamMinDepth; double mCamMaxDepth; @@ -634,8 +634,7 @@ class ZEDWrapperNodelet : public nodelet::Nodelet { int mCamDepthConfidence = 50; int mCamDepthTextureConf = 100; - double mPointCloudFreq = 15.; - double mVideoDepthFreq = 15.; + double mPointCloudFreq = 15.; double mCamImageResizeFactor = 1.0; double mCamDepthResizeFactor = 1.0; @@ -677,8 +676,6 @@ class ZEDWrapperNodelet : public nodelet::Nodelet { std::mutex mObjDetMutex; std::condition_variable mPcDataReadyCondVar; bool mPcDataReady; - std::condition_variable mRgbDepthDataRetrievedCondVar; - bool mRgbDepthDataRetrieved; // Point cloud variables sl::Mat mCloud; 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 d1dfc4de..6586f55d 100644 --- a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp @@ -71,11 +71,10 @@ void ZEDWrapperNodelet::onInit() { // Node handlers mNh = getMTNodeHandle(); - mNhNs = getMTPrivateNodeHandle(); + mNhNs = getMTPrivateNodeHandle(); mStopNode = false; mPcDataReady = false; - mRgbDepthDataRetrieved = true; #ifndef NDEBUG @@ -196,6 +195,7 @@ void ZEDWrapperNodelet::onInit() mSvoMode = true; } else { mZedParams.camera_fps = mCamFrameRate; + mZedParams.grab_compute_capping_fps = static_cast(mVideoDepthFreq); mZedParams.camera_resolution = mCamResol; if (mZedSerialNumber == 0) { @@ -415,7 +415,7 @@ void ZEDWrapperNodelet::onInit() NODELET_INFO_STREAM("Advertised on topic " << mPubConfMap.getTopic()); // Disparity publisher - mPubDisparity = mNhNs.advertise(disparityTopic, static_cast(mVideoDepthFreq)); + mPubDisparity = mNhNs.advertise(disparityTopic, 1); NODELET_INFO_STREAM("Advertised on topic " << mPubDisparity.getTopic()); // PointCloud publishers @@ -565,9 +565,6 @@ void ZEDWrapperNodelet::onInit() // Start pool thread mDevicePollThread = std::thread(&ZEDWrapperNodelet::device_poll_thread_func, this); - // Start data publishing timer - mVideoDepthTimer = mNhNs.createTimer(ros::Duration(1.0 / mVideoDepthFreq), &ZEDWrapperNodelet::callback_pubVideoDepth, this); - // Start Sensors thread mSensThread = std::thread(&ZEDWrapperNodelet::sensors_thread_func, this); } @@ -614,6 +611,12 @@ void ZEDWrapperNodelet::readParameters() mNhNs.getParam("general/grab_frame_rate", mCamFrameRate); checkResolFps(); NODELET_INFO_STREAM(" * Camera Grab Framerate\t-> " << mCamFrameRate); + mNhNs.getParam("general/pub_frame_rate", mVideoDepthFreq); + if(mVideoDepthFreq>mCamFrameRate) { + mVideoDepthFreq = mCamFrameRate; + add warning + } + NODELET_INFO_STREAM(" * Grab compute capping FPS\t-> " << mVideoDepthFreq << " Hz"); mNhNs.getParam("general/gpu_id", mGpuId); NODELET_INFO_STREAM(" * Gpu ID\t\t\t-> " << mGpuId); @@ -621,8 +624,16 @@ void ZEDWrapperNodelet::readParameters() NODELET_INFO_STREAM(" * Camera ID\t\t\t-> " << mZedId); mNhNs.getParam("general/verbose", mVerbose); NODELET_INFO_STREAM(" * Verbose\t\t\t-> " << (mVerbose ? "ENABLED" : "DISABLED")); - mNhNs.param("general/camera_flip", mCameraFlip, false); - NODELET_INFO_STREAM(" * Camera Flip\t\t\t-> " << (mCameraFlip ? "ENABLED" : "DISABLED")); + std::string flip_str; + mNhNs.getParam("general/camera_flip", flip_str); + if(flip_str=="ON") { + mCameraFlip = sl::FLIP_MODE::ON; + } else if(flip_str=="OFF") { + mCameraFlip = sl::FLIP_MODE::OFF; + } else { + mCameraFlip = sl::FLIP_MODE::AUTO; + } + NODELET_INFO_STREAM(" * Camera Flip\t\t\t-> " << sl::toString(mCameraFlip).c_str()); mNhNs.param("general/self_calib", mCameraSelfCalib, true); NODELET_INFO_STREAM(" * Self calibration\t\t-> " << (mCameraSelfCalib ? "ENABLED" : "DISABLED")); @@ -882,10 +893,7 @@ void ZEDWrapperNodelet::readParameters() mNhNs.getParam("depth_confidence", mCamDepthConfidence); NODELET_INFO_STREAM(" * [DYN] Depth confidence\t-> " << mCamDepthConfidence); mNhNs.getParam("depth_texture_conf", mCamDepthTextureConf); - NODELET_INFO_STREAM(" * [DYN] Depth texture conf.\t-> " << mCamDepthTextureConf); - - mNhNs.getParam("pub_frame_rate", mVideoDepthFreq); - NODELET_INFO_STREAM(" * [DYN] pub_frame_rate\t\t-> " << mVideoDepthFreq << " Hz"); + NODELET_INFO_STREAM(" * [DYN] Depth texture conf.\t-> " << mCamDepthTextureConf); mNhNs.getParam("point_cloud_freq", mPointCloudFreq); NODELET_INFO_STREAM(" * [DYN] point_cloud_freq\t-> " << mPointCloudFreq << " Hz"); mNhNs.getParam("brightness", mCamBrightness); @@ -2264,7 +2272,6 @@ void ZEDWrapperNodelet::updateDynamicReconfigure() config.gamma = mCamGamma; config.whitebalance_temperature = mCamWB / 100; config.point_cloud_freq = mPointCloudFreq; - config.pub_frame_rate = mVideoDepthFreq; mDynParMutex.unlock(); mDynServerMutex.lock(); @@ -2282,25 +2289,7 @@ void ZEDWrapperNodelet::callback_dynamicReconf(zed_nodelets::ZedConfig& config, mDynParMutex.lock(); DynParams param = static_cast(level); - switch (param) { - case DATAPUB_FREQ: - if (config.pub_frame_rate > mCamFrameRate) { - mVideoDepthFreq = mCamFrameRate; - NODELET_WARN_STREAM("'pub_frame_rate' cannot be major than camera grabbing framerate. Set to " - << mVideoDepthFreq); - - mUpdateDynParams = true; - } else { - mVideoDepthFreq = config.pub_frame_rate; - NODELET_INFO("Reconfigure Video and Depth pub. frequency: %g", mVideoDepthFreq); - } - - mVideoDepthTimer.setPeriod(ros::Duration(1.0 / mVideoDepthFreq)); - - mDynParMutex.unlock(); - // NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); - break; - + switch (param) { case CONFIDENCE: mCamDepthConfidence = config.depth_confidence; NODELET_INFO("Reconfigure confidence threshold: %d", mCamDepthConfidence); @@ -2436,7 +2425,7 @@ void ZEDWrapperNodelet::callback_dynamicReconf(zed_nodelets::ZedConfig& config, } } -void ZEDWrapperNodelet::callback_pubVideoDepth(const ros::TimerEvent& e) +void ZEDWrapperNodelet::pubVideoDepth() { static sl::Timestamp lastZedTs = 0; // Used to calculate stable publish frequency @@ -2472,8 +2461,6 @@ void ZEDWrapperNodelet::callback_pubVideoDepth(const ros::TimerEvent& e) sl::Timestamp ts_depth = 0; // used to check RGB/Depth sync sl::Timestamp grab_ts = 0; - mCamDataMutex.lock(); - // ----> Retrieve all required image data if (rgbSubnumber + leftSubnumber + stereoSubNumber > 0) { mZed.retrieveImage(mat_left, sl::VIEW::LEFT, sl::MEM::CPU, mMatResolVideo); @@ -2566,8 +2553,6 @@ void ZEDWrapperNodelet::callback_pubVideoDepth(const ros::TimerEvent& e) } // <---- Publish sensor data if sync is required by user or SVO - mCamDataMutex.unlock(); - // ----> Notify grab thread that all data are synchronized and a new grab can be done // mRgbDepthDataRetrievedCondVar.notify_one(); // mRgbDepthDataRetrieved = true; @@ -3278,10 +3263,8 @@ void ZEDWrapperNodelet::device_poll_thread_func() std::chrono::steady_clock::time_point start_elab = std::chrono::steady_clock::now(); - mCamDataMutex.lock(); - mRgbDepthDataRetrieved = false; + // ZED Grab mGrabStatus = mZed.grab(runParams); - mCamDataMutex.unlock(); // cout << toString(grab_status) << endl; if (mGrabStatus != sl::ERROR_CODE::SUCCESS) { @@ -3342,6 +3325,9 @@ void ZEDWrapperNodelet::device_poll_thread_func() continue; } + // Publish Color and Depth images + pubVideoDepth(); + mFrameCount++; // SVO recording @@ -3950,7 +3936,7 @@ void ZEDWrapperNodelet::callback_updateDiagnostic(diagnostic_updater::Diagnostic if (mObjDetRunning) { double freq = 1000. / mObjDetPeriodMean_msec->getMean(); - double freq_perc = 100. * freq / mCamFrameRate; + double freq_perc = 100. * freq / mVideoDepthFreq; stat.addf("Object detection", "Mean Frequency: %.3f Hz (%.1f%%)", freq, freq_perc); } else { stat.add("Object Detection", "INACTIVE"); diff --git a/zed_wrapper/cfg/Zed.cfg b/zed_wrapper/cfg/Zed.cfg index 051c419e..9a11d9fe 100755 --- a/zed_wrapper/cfg/Zed.cfg +++ b/zed_wrapper/cfg/Zed.cfg @@ -6,7 +6,6 @@ from dynamic_reconfigure.parameter_generator_catkin import * gen = ParameterGenerator() group_general = gen.add_group("general") -group_general.add("pub_frame_rate", double_t, 0, "Video and Depth data frequency", 15.0, 0.1, 60.0); group_depth = gen.add_group("depth") group_depth.add("depth_confidence", int_t, 1, "Depth confidence threshold", 50, 1, 100) diff --git a/zed_wrapper/params/common.yaml b/zed_wrapper/params/common.yaml index de5160e0..11e25c6e 100644 --- a/zed_wrapper/params/common.yaml +++ b/zed_wrapper/params/common.yaml @@ -16,7 +16,6 @@ auto_whitebalance: true # Dynamic whitebalance_temperature: 42 # Dynamic - works only if `auto_whitebalance` is false depth_confidence: 30 # Dynamic depth_texture_conf: 100 # Dynamic -pub_frame_rate: 15.0 # Dynamic - frequency of publishing of video and depth data point_cloud_freq: 10.0 # Dynamic - frequency of the pointcloud publishing (equal or less to `grab_frame_rate` value) general: @@ -25,10 +24,11 @@ general: serial_number: 0 gpu_id: -1 base_frame: 'base_link' # must be equal to the frame_id used in the URDF file - verbose: false # Enable info message by the ZED SDK + verbose: 1 # Set verbose level of the ZED SDK svo_compression: 2 # `0`: LOSSLESS, `1`: AVCHD, `2`: HEVC self_calib: true # enable/disable self calibration at starting - camera_flip: false + camera_flip: 'AUTO' # camera flip mode: 'OFF', 'ON', 'AUTO' + pub_frame_rate: 15.0 # frequency of publishing of video and depth data (see SDK API "InitParameters::grab_compute_capping_fps") video: img_downsample_factor: 0.5 # Resample factor for images [0.01,1.0] The SDK works with native image sizes, but publishes rescaled image. diff --git a/zed_wrapper/params/zed.yaml b/zed_wrapper/params/zed.yaml index 5a9a35f1..06cc54bd 100644 --- a/zed_wrapper/params/zed.yaml +++ b/zed_wrapper/params/zed.yaml @@ -5,7 +5,7 @@ general: camera_model: 'zed' resolution: 3 # '0': HD2K, '1': HD1080, '3': HD720, '5': VGA, '6': AUTO - grab_frame_rate: 15 # Frequency of frame grabbing for internal SDK operations + grab_frame_rate: 30 # Frequency of frame grabbing for internal SDK operations depth: min_depth: 0.3 # Min: 0.3, Max: 3.0 - Default 0.7 - Note: reducing this value wil require more computational power and GPU memory diff --git a/zed_wrapper/params/zed2.yaml b/zed_wrapper/params/zed2.yaml index 7f6ea2ce..e467b390 100644 --- a/zed_wrapper/params/zed2.yaml +++ b/zed_wrapper/params/zed2.yaml @@ -5,7 +5,7 @@ general: camera_model: 'zed2' resolution: 3 # '0': HD2K, '1': HD1080, '3': HD720, '5': VGA, '6': AUTO - grab_frame_rate: 15 # Frequency of frame grabbing for internal SDK operations + grab_frame_rate: 30 # Frequency of frame grabbing for internal SDK operations depth: min_depth: 0.3 # Min: 0.2, Max: 3.0 - Default 0.7 - Note: reducing this value wil require more computational power and GPU memory diff --git a/zed_wrapper/params/zed2i.yaml b/zed_wrapper/params/zed2i.yaml index 5e490e76..b50091ca 100644 --- a/zed_wrapper/params/zed2i.yaml +++ b/zed_wrapper/params/zed2i.yaml @@ -5,8 +5,8 @@ general: camera_model: 'zed2i' resolution: 3 # '0': HD2K, '1': HD1080, '3': HD720, '5': VGA, '6': AUTO - grab_frame_rate: 15 # Frequency of frame grabbing for internal SDK operations + grab_frame_rate: 30 # Frequency of frame grabbing for internal SDK operations depth: - min_depth: 0.2 # Min: 0.2, Max: 3.0 - Default 0.7 - Note: reducing this value wil require more computational power and GPU memory - max_depth: 10.0 # Max: 40.0 + min_depth: 0.3 # Min: 0.3, Max: 3.0 - Default 0.7 - Note: reducing this value wil require more computational power and GPU memory + max_depth: 10.0 # Max: 40.0 diff --git a/zed_wrapper/params/zedm.yaml b/zed_wrapper/params/zedm.yaml index 88902df1..48102e8a 100644 --- a/zed_wrapper/params/zedm.yaml +++ b/zed_wrapper/params/zedm.yaml @@ -5,7 +5,7 @@ general: camera_model: 'zedm' resolution: 3 # '0': HD2K, '1': HD1080, '3': HD720, '5': VGA, '6': AUTO - grab_frame_rate: 15 # Frequency of frame grabbing for internal SDK operations + grab_frame_rate: 30 # Frequency of frame grabbing for internal SDK operations depth: min_depth: 0.1 # Min: 0.1, Max: 3.0 - Default 0.3 - Note: reducing this value wil require more computational power and GPU memory From e4867df21571aeef6cdf97800cc05b30dc59bf5d Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Fri, 8 Sep 2023 14:06:30 +0200 Subject: [PATCH 161/199] Update CHANGELOG.rst --- CHANGELOG.rst | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index e27eac74..8843904d 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -1,12 +1,8 @@ CHANGELOG ========= -2023-04-27 ----------- -- Add 1080p for ZED X and ZED X Mini - -2023-03-31 ----------- +v4.0.5 +------ - Support for ZED SDK v4.0 - Remove parameter `object_detection.body_fitting` - Remove parameter `depth.sensing_mode` @@ -15,6 +11,7 @@ CHANGELOG - `sensors` and `object_detection` parameters are now in `common.yaml` - Move parameters `general.resolution` and `general.grab_frame_rate` to cameras yaml files to support the different configurations on ZED X and ZED X Mini. - Remove support for ROS Melodic that reached EOL +- Add 1080p for ZED X and ZED X Mini v3.8.x ------ From cdc46b203a9470e3aecf1acfa78fd73098ba7f0f Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Mon, 11 Sep 2023 14:47:45 +0200 Subject: [PATCH 162/199] Add Positional Tracking parameters * Add `pos_tracking/set_gravity_as_origin` parameter * Add `pos_tracking/pos_tracking_mode` parameter. --- CHANGELOG.rst | 9 +- .../include/zed_wrapper_nodelet.hpp | 4 + .../zed_nodelet/src/zed_wrapper_nodelet.cpp | 91 ++++++++++++------- zed_wrapper/params/common.yaml | 2 + 4 files changed, 70 insertions(+), 36 deletions(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 17e333d8..3cb064f1 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -1,11 +1,18 @@ CHANGELOG ========= +2023-09-08 +---------- +- Add `pos_tracking/set_gravity_as_origin` parameter. If 'true' align the positional tracking world to imu gravity measurement. Keep the yaw from the user initial pose. +- Add `pos_tracking/pos_tracking_mode` parameter. Matches the ZED SDK setting: 'QUALITY', 'STANDARD' +- Fix the warning `Elaboration takes longer [...]` + + 2023-09-07 ---------- - `pub_frame_rate` now controls the `InitParameters::grab_compute_capping_fps` parameter of the ZED SDK instead of controlling the frequency of a parallel thread. It's not a Dynamic parameter anymore. - Change `general/camera_flip` parameter to string: 'AUTO', 'ON', 'OFF' -- Change 'general/verbose' from bool to integer +- Change `general/verbose` from bool to integer v4.0.5 ------ 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 52adb949..11404241 100644 --- a/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp +++ b/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp @@ -565,12 +565,16 @@ class ZEDWrapperNodelet : public nodelet::Nodelet { // Positional tracking bool mPosTrackingEnabled = false; + sl::POSITIONAL_TRACKING_MODE mPosTrkMode = sl::POSITIONAL_TRACKING_MODE::QUALITY; bool mPosTrackingActivated = false; bool mPosTrackingReady = false; bool mTwoDMode = false; double mFixedZValue = 0.0; bool mFloorAlignment = false; bool mImuFusion = true; + bool mSetGravityAsOrigin = false; + + // Flags bool mGrabActive = false; // Indicate if camera grabbing is active (at least one topic subscribed) sl::ERROR_CODE mConnStatus; sl::ERROR_CODE mGrabStatus; 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 6586f55d..7e784037 100644 --- a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp @@ -614,7 +614,7 @@ void ZEDWrapperNodelet::readParameters() mNhNs.getParam("general/pub_frame_rate", mVideoDepthFreq); if(mVideoDepthFreq>mCamFrameRate) { mVideoDepthFreq = mCamFrameRate; - add warning + NODELET_WARN_STREAM("'general/pub_frame_rate' cannot be bigger than 'general/grab_frame_rate'. Forced to the correct value: " << mVideoDepthFreq); } NODELET_INFO_STREAM(" * Grab compute capping FPS\t-> " << mVideoDepthFreq << " Hz"); @@ -673,10 +673,27 @@ void ZEDWrapperNodelet::readParameters() // <----- Depth NODELET_INFO_STREAM("*** POSITIONAL TRACKING PARAMETERS ***"); - // ----> Tracking mNhNs.param("pos_tracking/pos_tracking_enabled", mPosTrackingEnabled, true); NODELET_INFO_STREAM(" * Positional tracking\t\t-> " << (mPosTrackingEnabled ? "ENABLED" : "DISABLED")); + + std::string pos_trk_mode; + mNhNs.getParam("pos_tracking/pos_tracking_mode", pos_trk_mode); + if (pos_trk_mode == "QUALITY") { + mPosTrkMode = sl::POSITIONAL_TRACKING_MODE::QUALITY; + } else if (pos_trk_mode == "STANDARD") { + mPosTrkMode = sl::POSITIONAL_TRACKING_MODE::STANDARD; + } else { + NODELET_WARN_STREAM( + "'pos_tracking/pos_tracking_mode' not valid ('" << pos_trk_mode << + "'). Using default value."); + mPosTrkMode = sl::POSITIONAL_TRACKING_MODE::QUALITY; + } + NODELET_INFO_STREAM( " * Positional tracking mode: " << sl::toString(mPosTrkMode).c_str()); + + mNhNs.getParam("pos_tracking/set_gravity_as_origin", mSetGravityAsOrigin); + NODELET_INFO_STREAM(" * Set gravity as origin\t\t-> " << (mSetGravityAsOrigin ? "ENABLED" : "DISABLED")); + 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); @@ -893,9 +910,16 @@ void ZEDWrapperNodelet::readParameters() mNhNs.getParam("depth_confidence", mCamDepthConfidence); NODELET_INFO_STREAM(" * [DYN] Depth confidence\t-> " << mCamDepthConfidence); mNhNs.getParam("depth_texture_conf", mCamDepthTextureConf); - NODELET_INFO_STREAM(" * [DYN] Depth texture conf.\t-> " << mCamDepthTextureConf); + NODELET_INFO_STREAM(" * [DYN] Depth texture conf.\t-> " << mCamDepthTextureConf); + mNhNs.getParam("point_cloud_freq", mPointCloudFreq); + if(mPointCloudFreq>mVideoDepthFreq) { + mPointCloudFreq=mVideoDepthFreq; + NODELET_WARN_STREAM("'point_cloud_freq' cannot be major than 'general/pub_frame_rate'. Set to " + << mPointCloudFreq); + } NODELET_INFO_STREAM(" * [DYN] point_cloud_freq\t-> " << mPointCloudFreq << " Hz"); + mNhNs.getParam("brightness", mCamBrightness); NODELET_INFO_STREAM(" * [DYN] brightness\t\t-> " << mCamBrightness); mNhNs.getParam("contrast", mCamContrast); @@ -1601,6 +1625,8 @@ void ZEDWrapperNodelet::start_pos_tracking() posTrackParams.enable_imu_fusion = mImuFusion; posTrackParams.set_as_static = false; + posTrackParams.set_gravity_as_origin = mSetGravityAsOrigin; + posTrackParams.mode = mPosTrkMode; sl::ERROR_CODE err = mZed.enablePositionalTracking(posTrackParams); @@ -2305,9 +2331,9 @@ void ZEDWrapperNodelet::callback_dynamicReconf(zed_nodelets::ZedConfig& config, break; case POINTCLOUD_FREQ: - if (config.point_cloud_freq > mCamFrameRate) { - mPointCloudFreq = mCamFrameRate; - NODELET_WARN_STREAM("'point_cloud_freq' cannot be major than camera grabbing framerate. Set to " + if (config.point_cloud_freq > mVideoDepthFreq) { + mPointCloudFreq = mVideoDepthFreq; + NODELET_WARN_STREAM("'point_cloud_freq' cannot be major than Video/Depth publishing framerate. Set to " << mPointCloudFreq); mUpdateDynParams = true; @@ -3142,7 +3168,7 @@ void ZEDWrapperNodelet::publishSensData(ros::Time t) void ZEDWrapperNodelet::device_poll_thread_func() { - ros::Rate loop_rate(mCamFrameRate); + ros::Rate loop_rate(mVideoDepthFreq); mRecording = false; @@ -3158,7 +3184,6 @@ void ZEDWrapperNodelet::device_poll_thread_func() } else { mFrameTimestamp = sl_tools::slTime2Ros(mZed.getTimestamp(sl::TIME_REFERENCE::CURRENT)); } - mPrevFrameTimestamp = mFrameTimestamp; mPosTrackingActivated = false; @@ -3325,12 +3350,22 @@ void ZEDWrapperNodelet::device_poll_thread_func() continue; } + mFrameCount++; + + // ----> Timestamp + if (mSvoMode) { + mFrameTimestamp = ros::Time::now(); + } else { + mFrameTimestamp = sl_tools::slTime2Ros(mZed.getTimestamp(sl::TIME_REFERENCE::IMAGE)); + } + mPrevFrameTimestamp = mFrameTimestamp; + ros::Time stamp = mFrameTimestamp; // Fix processing Timestamp + // <---- Timestamp + // Publish Color and Depth images pubVideoDepth(); - mFrameCount++; - - // SVO recording + // ----> SVO recording mRecMutex.lock(); if (mRecording) { @@ -3342,13 +3377,10 @@ void ZEDWrapperNodelet::device_poll_thread_func() mDiagUpdater.force_update(); } - mRecMutex.unlock(); + // <---- SVO recording - // Timestamp - mPrevFrameTimestamp = mFrameTimestamp; - - // Publish freq calculation + // ----> Publish freq calculation static std::chrono::steady_clock::time_point last_time = std::chrono::steady_clock::now(); std::chrono::steady_clock::time_point now = std::chrono::steady_clock::now(); @@ -3356,17 +3388,8 @@ void ZEDWrapperNodelet::device_poll_thread_func() last_time = now; mGrabPeriodMean_usec->addValue(elapsed_usec); - // NODELET_INFO_STREAM("Grab time: " << elapsed_usec / 1000 << " msec"); - - // Timestamp - if (mSvoMode) { - mFrameTimestamp = ros::Time::now(); - } else { - mFrameTimestamp = sl_tools::slTime2Ros(mZed.getTimestamp(sl::TIME_REFERENCE::IMAGE)); - } - - ros::Time stamp = mFrameTimestamp; // Timestamp + // <---- Publish freq calculation // ----> Camera Settings int value; @@ -3808,16 +3831,14 @@ void ZEDWrapperNodelet::device_poll_thread_func() static int count_warn = 0; if (!loop_rate.sleep()) { - if (mean_elab_sec > (1. / mCamFrameRate)) { + if (mean_elab_sec > (1. / mVideoDepthFreq)) { if (++count_warn > 10) { - NODELET_DEBUG_THROTTLE(1.0, "Working thread is not synchronized with the Camera frame rate"); + 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 (" << mean_elab_sec << " sec) than requested " - "by the FPS rate (" + NODELET_WARN_STREAM_THROTTLE(10.0, "Elaboration takes longer (" << mean_elab_sec << " sec) than requested by the FPS rate (" << loop_rate.expectedCycleTime() << " sec). Please consider to " - "lower the 'frame_rate' setting or to " - "reduce the power requirements reducing " - "the resolutions."); + "lower the 'general/pub_frame_rate' setting or to " + "reduce the power requirements by reducing the camera resolutions."); } loop_rate.reset(); @@ -3871,7 +3892,7 @@ void ZEDWrapperNodelet::device_poll_thread_func() mRecording = false; mZed.disableRecording(); } - mStopNode = true; + mZed.close(); NODELET_DEBUG("ZED pool thread finished"); @@ -3893,7 +3914,7 @@ void ZEDWrapperNodelet::callback_updateDiagnostic(diagnostic_updater::Diagnostic stat.addf("Capture", "Mean Frequency: %.1f Hz (%.1f%%)", freq, freq_perc); stat.addf("General Processing", "Mean Time: %.3f sec (Max. %.3f sec)", mElabPeriodMean_sec->getMean(), - 1. / mCamFrameRate); + 1. / mVideoDepthFreq); if (mPublishingData) { freq = 1. / mVideoDepthPeriodMean_sec->getMean(); diff --git a/zed_wrapper/params/common.yaml b/zed_wrapper/params/common.yaml index 11e25c6e..8b166b89 100644 --- a/zed_wrapper/params/common.yaml +++ b/zed_wrapper/params/common.yaml @@ -41,6 +41,8 @@ depth: pos_tracking: pos_tracking_enabled: true # True to enable positional tracking from start + pos_tracking_mode: 'STANDARD' # Matches the ZED SDK setting: 'QUALITY', 'STANDARD' + 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 publish_map_tf: true # publish `map -> odom` TF From 88c001cb1849040adaa96f88ef641b1c15ff58f7 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Mon, 11 Sep 2023 16:08:10 +0200 Subject: [PATCH 163/199] Improve NO DEPTH mode --- CHANGELOG.rst | 5 + .../include/zed_wrapper_nodelet.hpp | 5 +- .../zed_nodelet/src/zed_wrapper_nodelet.cpp | 540 ++++++++++-------- zed_wrapper/params/common.yaml | 2 +- 4 files changed, 310 insertions(+), 242 deletions(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 3cb064f1..8da8a49b 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -1,6 +1,11 @@ CHANGELOG ========= +2023-09-10 +---------- +- Change the parameter 'depth/quality' to 'depth/depth_mode' and replace numeric values with strings +- Improve the behavior of the "NO DEPTH" mode + 2023-09-08 ---------- - Add `pos_tracking/set_gravity_as_origin` parameter. If 'true' align the positional tracking world to imu gravity measurement. Keep the yaw from the user initial pose. 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 11404241..1242d945 100644 --- a/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp +++ b/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp @@ -545,7 +545,7 @@ class ZEDWrapperNodelet : public nodelet::Nodelet { sl::RESOLUTION mCamResol; int mCamFrameRate; double mVideoDepthFreq = 15.; - sl::DEPTH_MODE mDepthMode; + sl::DEPTH_MODE mDepthMode = sl::DEPTH_MODE::ULTRA; int mGpuId; int mZedId; int mDepthStabilization; @@ -573,7 +573,7 @@ class ZEDWrapperNodelet : public nodelet::Nodelet { bool mFloorAlignment = false; bool mImuFusion = true; bool mSetGravityAsOrigin = false; - + // Flags bool mGrabActive = false; // Indicate if camera grabbing is active (at least one topic subscribed) sl::ERROR_CODE mConnStatus; @@ -581,6 +581,7 @@ class ZEDWrapperNodelet : public nodelet::Nodelet { sl::POSITIONAL_TRACKING_STATE mPosTrackingStatus; bool mSensPublishing = false; bool mPcPublishing = false; + bool mDepthDisabled = false; // Last frame time ros::Time mPrevFrameTimestamp; 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 7e784037..d4bc0d56 100644 --- a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp @@ -401,72 +401,75 @@ void ZEDWrapperNodelet::onInit() NODELET_INFO_STREAM("Advertised on topic " << mPubRawRightGray.getTopic()); NODELET_INFO_STREAM("Advertised on topic " << mPubRawRightGray.getInfoTopic()); - mPubDepth = it_zed.advertiseCamera(depth_topic_root, 1); // depth - NODELET_INFO_STREAM("Advertised on topic " << mPubDepth.getTopic()); - NODELET_INFO_STREAM("Advertised on topic " << mPubDepth.getInfoTopic()); - mPubStereo = it_zed.advertise(stereo_topic, 1); NODELET_INFO_STREAM("Advertised on topic " << mPubStereo.getTopic()); mPubRawStereo = it_zed.advertise(stereo_raw_topic, 1); - NODELET_INFO_STREAM("Advertised on topic " << mPubRawStereo.getTopic()); - - // Confidence Map publisher - mPubConfMap = mNhNs.advertise(conf_map_topic, 1); // confidence map - NODELET_INFO_STREAM("Advertised on topic " << mPubConfMap.getTopic()); - - // Disparity publisher - mPubDisparity = mNhNs.advertise(disparityTopic, 1); - NODELET_INFO_STREAM("Advertised on topic " << mPubDisparity.getTopic()); - - // PointCloud publishers - mPubCloud = mNhNs.advertise(pointcloud_topic, 1); - NODELET_INFO_STREAM("Advertised on topic " << mPubCloud.getTopic()); - - if (mMappingEnabled) { - mPubFusedCloud = mNhNs.advertise(pointcloud_fused_topic, 1); - NODELET_INFO_STREAM("Advertised on topic " << mPubFusedCloud.getTopic() << " @ " << mFusedPcPubFreq << " Hz"); - } - - // Object detection publishers - if (mObjDetEnabled) { - mPubObjDet = mNhNs.advertise(object_det_topic, 1); - NODELET_INFO_STREAM("Advertised on topic " << mPubObjDet.getTopic()); - } + NODELET_INFO_STREAM("Advertised on topic " << mPubRawStereo.getTopic()); + + // Detected planes publisher + mPubPlane = mNhNs.advertise(plane_topic, 1); + + if (!mDepthDisabled) { + mPubDepth = it_zed.advertiseCamera(depth_topic_root, 1); // depth + NODELET_INFO_STREAM("Advertised on topic " << mPubDepth.getTopic()); + NODELET_INFO_STREAM("Advertised on topic " << mPubDepth.getInfoTopic()); + + // Confidence Map publisher + mPubConfMap = mNhNs.advertise(conf_map_topic, 1); // confidence map + NODELET_INFO_STREAM("Advertised on topic " << mPubConfMap.getTopic()); + + // Disparity publisher + mPubDisparity = mNhNs.advertise(disparityTopic, 1); + NODELET_INFO_STREAM("Advertised on topic " << mPubDisparity.getTopic()); + + // PointCloud publishers + mPubCloud = mNhNs.advertise(pointcloud_topic, 1); + NODELET_INFO_STREAM("Advertised on topic " << mPubCloud.getTopic()); + - // Odometry and Pose publisher - mPubPose = mNhNs.advertise(poseTopic, 1); - NODELET_INFO_STREAM("Advertised on topic " << mPubPose.getTopic()); + if (mMappingEnabled) { + mPubFusedCloud = mNhNs.advertise(pointcloud_fused_topic, 1); + NODELET_INFO_STREAM("Advertised on topic " << mPubFusedCloud.getTopic() << " @ " << mFusedPcPubFreq << " Hz"); + } - mPubPoseCov = mNhNs.advertise(pose_cov_topic, 1); - NODELET_INFO_STREAM("Advertised on topic " << mPubPoseCov.getTopic()); + // Object detection publishers + if (mObjDetEnabled) { + mPubObjDet = mNhNs.advertise(object_det_topic, 1); + NODELET_INFO_STREAM("Advertised on topic " << mPubObjDet.getTopic()); + } - mPubOdom = mNhNs.advertise(odometryTopic, 1); - NODELET_INFO_STREAM("Advertised on topic " << mPubOdom.getTopic()); + // Odometry and Pose publisher + mPubPose = mNhNs.advertise(poseTopic, 1); + NODELET_INFO_STREAM("Advertised on topic " << mPubPose.getTopic()); - // Rviz markers publisher - mPubMarker = mNhNs.advertise(marker_topic, 10, true); + mPubPoseCov = mNhNs.advertise(pose_cov_topic, 1); + NODELET_INFO_STREAM("Advertised on topic " << mPubPoseCov.getTopic()); - // Detected planes publisher - mPubPlane = mNhNs.advertise(plane_topic, 1); + mPubOdom = mNhNs.advertise(odometryTopic, 1); + NODELET_INFO_STREAM("Advertised on topic " << mPubOdom.getTopic()); - // Camera Path - if (mPathPubRate > 0) { - mPubOdomPath = mNhNs.advertise(odom_path_topic, 1, true); - NODELET_INFO_STREAM("Advertised on topic " << mPubOdomPath.getTopic()); - mPubMapPath = mNhNs.advertise(map_path_topic, 1, true); - NODELET_INFO_STREAM("Advertised on topic " << mPubMapPath.getTopic()); + // Rviz markers publisher + mPubMarker = mNhNs.advertise(marker_topic, 10, true); + + // Camera Path + if (mPathPubRate > 0) { + mPubOdomPath = mNhNs.advertise(odom_path_topic, 1, true); + NODELET_INFO_STREAM("Advertised on topic " << mPubOdomPath.getTopic()); + mPubMapPath = mNhNs.advertise(map_path_topic, 1, true); + NODELET_INFO_STREAM("Advertised on topic " << mPubMapPath.getTopic()); - mPathTimer = mNhNs.createTimer(ros::Duration(1.0 / mPathPubRate), &ZEDWrapperNodelet::callback_pubPath, this); + mPathTimer = mNhNs.createTimer(ros::Duration(1.0 / mPathPubRate), &ZEDWrapperNodelet::callback_pubPath, this); - if (mPathMaxCount != -1) { - NODELET_DEBUG_STREAM("Path vectors reserved " << mPathMaxCount << " poses."); - mOdomPath.reserve(mPathMaxCount); - mMapPath.reserve(mPathMaxCount); + if (mPathMaxCount != -1) { + NODELET_DEBUG_STREAM("Path vectors reserved " << mPathMaxCount << " poses."); + mOdomPath.reserve(mPathMaxCount); + mMapPath.reserve(mPathMaxCount); - NODELET_DEBUG_STREAM("Path vector sizes: " << mOdomPath.size() << " " << mMapPath.size()); + NODELET_DEBUG_STREAM("Path vector sizes: " << mOdomPath.size() << " " << mMapPath.size()); + } + } else { + NODELET_INFO_STREAM("Path topics not published -> mPathPubRate: " << mPathPubRate); } - } else { - NODELET_INFO_STREAM("Path topics not published -> mPathPubRate: " << mPathPubRate); } // Sensor publishers @@ -535,13 +538,24 @@ void ZEDWrapperNodelet::onInit() // Subscribers mClickedPtSub = mNhNs.subscribe(mClickedPtTopic, 10, &ZEDWrapperNodelet::clickedPtCallback, this); - NODELET_INFO_STREAM("Subscribed to topic " << mClickedPtTopic.c_str()); // Services - mSrvSetInitPose = mNhNs.advertiseService("set_pose", &ZEDWrapperNodelet::on_set_pose, this); - mSrvResetOdometry = mNhNs.advertiseService("reset_odometry", &ZEDWrapperNodelet::on_reset_odometry, this); - mSrvResetTracking = mNhNs.advertiseService("reset_tracking", &ZEDWrapperNodelet::on_reset_tracking, this); + if (!mDepthDisabled) { + mSrvSetInitPose = mNhNs.advertiseService("set_pose", &ZEDWrapperNodelet::on_set_pose, this); + mSrvResetOdometry = mNhNs.advertiseService("reset_odometry", &ZEDWrapperNodelet::on_reset_odometry, this); + mSrvResetTracking = mNhNs.advertiseService("reset_tracking", &ZEDWrapperNodelet::on_reset_tracking, this); + + mSrvSaveAreaMemory = mNhNs.advertiseService("save_area_memory", &ZEDWrapperNodelet::on_save_area_memory, this); + + mSrvStartMapping = mNhNs.advertiseService("start_3d_mapping", &ZEDWrapperNodelet::on_start_3d_mapping, this); + mSrvStopMapping = mNhNs.advertiseService("stop_3d_mapping", &ZEDWrapperNodelet::on_stop_3d_mapping, this); + mSrvSave3dMap = mNhNs.advertiseService("save_3d_map", &ZEDWrapperNodelet::on_save_3d_map, this); + + mSrvStartObjDet = mNhNs.advertiseService("start_object_detection", &ZEDWrapperNodelet::on_start_object_detection, this); + mSrvStopObjDet = mNhNs.advertiseService("stop_object_detection", &ZEDWrapperNodelet::on_stop_object_detection, this); + } + mSrvSvoStartRecording = mNhNs.advertiseService("start_svo_recording", &ZEDWrapperNodelet::on_start_svo_recording, this); mSrvSvoStopRecording = mNhNs.advertiseService("stop_svo_recording", &ZEDWrapperNodelet::on_stop_svo_recording, this); @@ -549,32 +563,25 @@ void ZEDWrapperNodelet::onInit() mSrvToggleLed = mNhNs.advertiseService("toggle_led", &ZEDWrapperNodelet::on_toggle_led, this); mSrvSvoStartStream = mNhNs.advertiseService("start_remote_stream", &ZEDWrapperNodelet::on_start_remote_stream, this); mSrvSvoStopStream = mNhNs.advertiseService("stop_remote_stream", &ZEDWrapperNodelet::on_stop_remote_stream, this); - - mSrvStartMapping = mNhNs.advertiseService("start_3d_mapping", &ZEDWrapperNodelet::on_start_3d_mapping, this); - mSrvStopMapping = mNhNs.advertiseService("stop_3d_mapping", &ZEDWrapperNodelet::on_stop_3d_mapping, this); - mSrvSave3dMap = mNhNs.advertiseService("save_3d_map", &ZEDWrapperNodelet::on_save_3d_map, this); - - mSrvStartObjDet = mNhNs.advertiseService("start_object_detection", &ZEDWrapperNodelet::on_start_object_detection, this); - mSrvStopObjDet = mNhNs.advertiseService("stop_object_detection", &ZEDWrapperNodelet::on_stop_object_detection, this); - - mSrvSaveAreaMemory = mNhNs.advertiseService("save_area_memory", &ZEDWrapperNodelet::on_save_area_memory, this); - - // Start Pointcloud thread - mPcThread = std::thread(&ZEDWrapperNodelet::pointcloud_thread_func, this); + + if (!mDepthDisabled) { + // Start Pointcloud thread + mPcThread = std::thread(&ZEDWrapperNodelet::pointcloud_thread_func, this); + } // Start pool thread mDevicePollThread = std::thread(&ZEDWrapperNodelet::device_poll_thread_func, this); // Start Sensors thread mSensThread = std::thread(&ZEDWrapperNodelet::sensors_thread_func, this); + + NODELET_INFO("ZED Node started"); } void ZEDWrapperNodelet::readParameters() { - NODELET_INFO_STREAM("*** GENERAL PARAMETERS ***"); - // ----> General - // Get parameters from param files + NODELET_INFO_STREAM("*** GENERAL PARAMETERS ***"); std::string camera_model; mNhNs.getParam("general/camera_model", camera_model); @@ -656,146 +663,182 @@ void ZEDWrapperNodelet::readParameters() NODELET_INFO_STREAM("*** DEPTH PARAMETERS ***"); // -----> Depth - int depth_mode; - mNhNs.getParam("depth/quality", depth_mode); - mDepthMode = static_cast(depth_mode); - NODELET_INFO_STREAM(" * Depth quality\t\t-> " << sl::toString(mDepthMode).c_str()); - mNhNs.getParam("depth/openni_depth_mode", mOpenniDepthMode); - NODELET_INFO_STREAM(" * OpenNI mode\t\t\t-> " << (mOpenniDepthMode ? "ENABLED" : "DISABLED")); - mNhNs.getParam("depth/depth_stabilization", mDepthStabilization); - NODELET_INFO_STREAM(" * Depth Stabilization\t\t-> " << mDepthStabilization ); - mNhNs.getParam("depth/min_depth", mCamMinDepth); - NODELET_INFO_STREAM(" * Minimum depth\t\t-> " << mCamMinDepth << " m"); - mNhNs.getParam("depth/max_depth", mCamMaxDepth); - NODELET_INFO_STREAM(" * Maximum depth\t\t-> " << mCamMaxDepth << " m"); - mNhNs.getParam("depth/depth_downsample_factor", mCamDepthResizeFactor); - NODELET_INFO_STREAM(" * Depth resample factor\t-> " << mCamDepthResizeFactor); - // <----- Depth + std::string depth_mode_str = sl::toString(mDepthMode).c_str(); + mNhNs.getParam("depth/depth_mode", depth_mode_str); - NODELET_INFO_STREAM("*** POSITIONAL TRACKING PARAMETERS ***"); - // ----> Tracking - mNhNs.param("pos_tracking/pos_tracking_enabled", mPosTrackingEnabled, true); - NODELET_INFO_STREAM(" * Positional tracking\t\t-> " << (mPosTrackingEnabled ? "ENABLED" : "DISABLED")); - - std::string pos_trk_mode; - mNhNs.getParam("pos_tracking/pos_tracking_mode", pos_trk_mode); - if (pos_trk_mode == "QUALITY") { - mPosTrkMode = sl::POSITIONAL_TRACKING_MODE::QUALITY; - } else if (pos_trk_mode == "STANDARD") { - mPosTrkMode = sl::POSITIONAL_TRACKING_MODE::STANDARD; - } else { - NODELET_WARN_STREAM( - "'pos_tracking/pos_tracking_mode' not valid ('" << pos_trk_mode << - "'). Using default value."); - mPosTrkMode = sl::POSITIONAL_TRACKING_MODE::QUALITY; + bool matched = false; + for (int mode = static_cast(sl::DEPTH_MODE::NONE); + mode < static_cast(sl::DEPTH_MODE::LAST); ++mode) + { + std::string test_str = sl::toString(static_cast(mode)).c_str(); + std::replace(test_str.begin(), test_str.end(), ' ', '_'); // Replace spaces with underscores to match the YAML setting + if (test_str == depth_mode_str) { + matched = true; + mDepthMode = static_cast(mode); + break; + } } - NODELET_INFO_STREAM( " * Positional tracking mode: " << sl::toString(mPosTrkMode).c_str()); - mNhNs.getParam("pos_tracking/set_gravity_as_origin", mSetGravityAsOrigin); - NODELET_INFO_STREAM(" * Set gravity as origin\t\t-> " << (mSetGravityAsOrigin ? "ENABLED" : "DISABLED")); - - 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)); + if (!matched) { + ROS_WARN_STREAM( + "The parameter 'depth.depth_mode' contains a not valid string. Please check it in 'common.yaml'."); + ROS_WARN("Using default DEPTH_MODE."); + mDepthMode = sl::DEPTH_MODE::ULTRA; + } - if (mPathMaxCount < 2 && mPathMaxCount != -1) { - mPathMaxCount = 2; + if (mDepthMode == sl::DEPTH_MODE::NONE) { + mDepthDisabled = true; + mDepthStabilization = 0; + ROS_INFO_STREAM(" * Depth mode: " << sl::toString(mDepthMode).c_str() << " - DEPTH DISABLED"); + } else { + mDepthDisabled = false; + ROS_INFO_STREAM(" * Depth mode: " << sl::toString( + mDepthMode).c_str() << " [" << static_cast(mDepthMode) << "]"); + } + + if(!mDepthDisabled) { + mNhNs.getParam("depth/openni_depth_mode", mOpenniDepthMode); + NODELET_INFO_STREAM(" * OpenNI mode\t\t\t-> " << (mOpenniDepthMode ? "ENABLED" : "DISABLED")); + mNhNs.getParam("depth/depth_stabilization", mDepthStabilization); + NODELET_INFO_STREAM(" * Depth Stabilization\t\t-> " << mDepthStabilization ); + mNhNs.getParam("depth/min_depth", mCamMinDepth); + NODELET_INFO_STREAM(" * Minimum depth\t\t-> " << mCamMinDepth << " m"); + mNhNs.getParam("depth/max_depth", mCamMaxDepth); + NODELET_INFO_STREAM(" * Maximum depth\t\t-> " << mCamMaxDepth << " m"); + mNhNs.getParam("depth/depth_downsample_factor", mCamDepthResizeFactor); + NODELET_INFO_STREAM(" * Depth resample factor\t-> " << mCamDepthResizeFactor); } + // <----- Depth + + // ----> Positional Tracking + if(!mDepthDisabled) { + NODELET_INFO_STREAM("*** POSITIONAL TRACKING PARAMETERS ***"); + + mNhNs.param("pos_tracking/pos_tracking_enabled", mPosTrackingEnabled, true); + NODELET_INFO_STREAM(" * Positional tracking\t\t-> " << (mPosTrackingEnabled ? "ENABLED" : "DISABLED")); + + std::string pos_trk_mode; + mNhNs.getParam("pos_tracking/pos_tracking_mode", pos_trk_mode); + if (pos_trk_mode == "QUALITY") { + mPosTrkMode = sl::POSITIONAL_TRACKING_MODE::QUALITY; + } else if (pos_trk_mode == "STANDARD") { + mPosTrkMode = sl::POSITIONAL_TRACKING_MODE::STANDARD; + } else { + NODELET_WARN_STREAM( + "'pos_tracking/pos_tracking_mode' not valid ('" << pos_trk_mode << + "'). Using default value."); + mPosTrkMode = sl::POSITIONAL_TRACKING_MODE::QUALITY; + } + NODELET_INFO_STREAM( " * Positional tracking mode: " << sl::toString(mPosTrkMode).c_str()); - mNhNs.getParam("pos_tracking/initial_base_pose", mInitialBasePose); + mNhNs.getParam("pos_tracking/set_gravity_as_origin", mSetGravityAsOrigin); + NODELET_INFO_STREAM(" * Set gravity as origin\t\t-> " << (mSetGravityAsOrigin ? "ENABLED" : "DISABLED")); - mNhNs.getParam("pos_tracking/area_memory_db_path", mAreaMemDbPath); - mAreaMemDbPath = sl_tools::resolveFilePath(mAreaMemDbPath); - NODELET_INFO_STREAM(" * Odometry DB path\t\t-> " << mAreaMemDbPath.c_str()); + 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)); - mNhNs.param("pos_tracking/save_area_memory_db_on_exit", mSaveAreaMapOnClosing, false); - NODELET_INFO_STREAM(" * Save Area Memory on closing\t-> " << (mSaveAreaMapOnClosing ? "ENABLED" : "DISABLED")); - mNhNs.param("pos_tracking/area_memory", mAreaMemory, false); - NODELET_INFO_STREAM(" * Area Memory\t\t\t-> " << (mAreaMemory ? "ENABLED" : "DISABLED")); - mNhNs.param("pos_tracking/imu_fusion", mImuFusion, true); - NODELET_INFO_STREAM(" * IMU Fusion\t\t\t-> " << (mImuFusion ? "ENABLED" : "DISABLED")); - mNhNs.param("pos_tracking/floor_alignment", mFloorAlignment, false); - NODELET_INFO_STREAM(" * Floor alignment\t\t-> " << (mFloorAlignment ? "ENABLED" : "DISABLED")); - mNhNs.param("pos_tracking/init_odom_with_first_valid_pose", mInitOdomWithPose, true); - NODELET_INFO_STREAM(" * Init Odometry with first valid pose data -> " - << (mInitOdomWithPose ? "ENABLED" : "DISABLED")); - mNhNs.param("pos_tracking/two_d_mode", mTwoDMode, false); - NODELET_INFO_STREAM(" * Two D mode\t\t\t-> " << (mTwoDMode ? "ENABLED" : "DISABLED")); - mNhNs.param("pos_tracking/fixed_z_value", mFixedZValue, 0.0); + if (mPathMaxCount < 2 && mPathMaxCount != -1) { + mPathMaxCount = 2; + } - if (mTwoDMode) { - NODELET_INFO_STREAM(" * Fixed Z value\t\t-> " << mFixedZValue); - } - // <---- Tracking + mNhNs.getParam("pos_tracking/initial_base_pose", mInitialBasePose); + + mNhNs.getParam("pos_tracking/area_memory_db_path", mAreaMemDbPath); + mAreaMemDbPath = sl_tools::resolveFilePath(mAreaMemDbPath); + NODELET_INFO_STREAM(" * Odometry DB path\t\t-> " << mAreaMemDbPath.c_str()); + + mNhNs.param("pos_tracking/save_area_memory_db_on_exit", mSaveAreaMapOnClosing, false); + NODELET_INFO_STREAM(" * Save Area Memory on closing\t-> " << (mSaveAreaMapOnClosing ? "ENABLED" : "DISABLED")); + mNhNs.param("pos_tracking/area_memory", mAreaMemory, false); + NODELET_INFO_STREAM(" * Area Memory\t\t\t-> " << (mAreaMemory ? "ENABLED" : "DISABLED")); + mNhNs.param("pos_tracking/imu_fusion", mImuFusion, true); + NODELET_INFO_STREAM(" * IMU Fusion\t\t\t-> " << (mImuFusion ? "ENABLED" : "DISABLED")); + mNhNs.param("pos_tracking/floor_alignment", mFloorAlignment, false); + NODELET_INFO_STREAM(" * Floor alignment\t\t-> " << (mFloorAlignment ? "ENABLED" : "DISABLED")); + mNhNs.param("pos_tracking/init_odom_with_first_valid_pose", mInitOdomWithPose, true); + NODELET_INFO_STREAM(" * Init Odometry with first valid pose data -> " + << (mInitOdomWithPose ? "ENABLED" : "DISABLED")); + mNhNs.param("pos_tracking/two_d_mode", mTwoDMode, false); + NODELET_INFO_STREAM(" * Two D mode\t\t\t-> " << (mTwoDMode ? "ENABLED" : "DISABLED")); + mNhNs.param("pos_tracking/fixed_z_value", mFixedZValue, 0.0); - NODELET_INFO_STREAM("*** MAPPING PARAMETERS ***"); + if (mTwoDMode) { + NODELET_INFO_STREAM(" * Fixed Z value\t\t-> " << mFixedZValue); + } + } + // <---- Positional Tracking // ----> Mapping - mNhNs.param("mapping/mapping_enabled", mMappingEnabled, false); + NODELET_INFO_STREAM("*** MAPPING PARAMETERS ***"); + if(!mDepthDisabled) { + mNhNs.param("mapping/mapping_enabled", mMappingEnabled, false); - if (mMappingEnabled) { - NODELET_INFO_STREAM(" * Mapping\t\t\t-> ENABLED"); + if (mMappingEnabled) { + NODELET_INFO_STREAM(" * Mapping\t\t\t-> ENABLED"); - mNhNs.getParam("mapping/resolution", mMappingRes); - NODELET_INFO_STREAM(" * Mapping resolution\t\t-> " << mMappingRes << " m"); + mNhNs.getParam("mapping/resolution", mMappingRes); + NODELET_INFO_STREAM(" * Mapping resolution\t\t-> " << mMappingRes << " m"); - mNhNs.getParam("mapping/max_mapping_range", mMaxMappingRange); - NODELET_INFO_STREAM(" * Mapping max range\t\t-> " << mMaxMappingRange << " m" - << ((mMaxMappingRange < 0.0) ? " [AUTO]" : "")); + mNhNs.getParam("mapping/max_mapping_range", mMaxMappingRange); + NODELET_INFO_STREAM(" * Mapping max range\t\t-> " << mMaxMappingRange << " m" + << ((mMaxMappingRange < 0.0) ? " [AUTO]" : "")); - mNhNs.getParam("mapping/fused_pointcloud_freq", mFusedPcPubFreq); - NODELET_INFO_STREAM(" * Fused point cloud freq:\t-> " << mFusedPcPubFreq << " Hz"); - } else { - NODELET_INFO_STREAM(" * Mapping\t\t\t-> DISABLED"); + mNhNs.getParam("mapping/fused_pointcloud_freq", mFusedPcPubFreq); + NODELET_INFO_STREAM(" * Fused point cloud freq:\t-> " << mFusedPcPubFreq << " Hz"); + } else { + NODELET_INFO_STREAM(" * Mapping\t\t\t-> DISABLED"); + } } - mNhNs.param("mapping/clicked_point_topic", mClickedPtTopic, std::string("/clicked_point")); NODELET_INFO_STREAM(" * Clicked point topic\t\t-> " << mClickedPtTopic.c_str()); // <---- Mapping - NODELET_INFO_STREAM("*** OBJECT DETECTION PARAMETERS ***"); - // ----> Object Detection - mNhNs.param("object_detection/od_enabled", mObjDetEnabled, false); - - if (mObjDetEnabled) { - NODELET_INFO_STREAM(" * Object Detection\t\t-> ENABLED"); - mNhNs.getParam("object_detection/confidence_threshold", mObjDetConfidence); - NODELET_INFO_STREAM(" * Object confidence\t\t-> " << mObjDetConfidence); - mNhNs.getParam("object_detection/object_tracking_enabled", mObjDetTracking); - NODELET_INFO_STREAM(" * Object tracking\t\t-> " << (mObjDetTracking ? "ENABLED" : "DISABLED")); - mNhNs.getParam("object_detection/max_range", mObjDetMaxRange); - if (mObjDetMaxRange > mCamMaxDepth) { - NODELET_WARN("Detection max range cannot be major than depth max range. Automatically fixed."); - mObjDetMaxRange = mCamMaxDepth; - } - NODELET_INFO_STREAM(" * Detection max range\t\t-> " << mObjDetMaxRange); + if(!mDepthDisabled) { + NODELET_INFO_STREAM("*** OBJECT DETECTION PARAMETERS ***"); + + mNhNs.param("object_detection/od_enabled", mObjDetEnabled, false); + + if (mObjDetEnabled) { + NODELET_INFO_STREAM(" * Object Detection\t\t-> ENABLED"); + mNhNs.getParam("object_detection/confidence_threshold", mObjDetConfidence); + NODELET_INFO_STREAM(" * Object confidence\t\t-> " << mObjDetConfidence); + mNhNs.getParam("object_detection/object_tracking_enabled", mObjDetTracking); + NODELET_INFO_STREAM(" * Object tracking\t\t-> " << (mObjDetTracking ? "ENABLED" : "DISABLED")); + mNhNs.getParam("object_detection/max_range", mObjDetMaxRange); + if (mObjDetMaxRange > mCamMaxDepth) { + NODELET_WARN("Detection max range cannot be major than depth max range. Automatically fixed."); + mObjDetMaxRange = mCamMaxDepth; + } + NODELET_INFO_STREAM(" * Detection max range\t\t-> " << mObjDetMaxRange); - int model; - mNhNs.getParam("object_detection/model", model); - if (model < 0 || model >= static_cast(sl::OBJECT_DETECTION_MODEL::LAST)) { - NODELET_WARN("Detection model not valid. Forced to the default value"); - model = static_cast(mObjDetModel); + int model; + mNhNs.getParam("object_detection/model", model); + if (model < 0 || model >= static_cast(sl::OBJECT_DETECTION_MODEL::LAST)) { + NODELET_WARN("Detection model not valid. Forced to the default value"); + model = static_cast(mObjDetModel); + } + mObjDetModel = static_cast(model); + + NODELET_INFO_STREAM(" * Detection model\t\t-> " << sl::toString(mObjDetModel)); + + mNhNs.getParam("object_detection/mc_people", mObjDetPeopleEnable); + NODELET_INFO_STREAM(" * Detect people\t\t-> " << (mObjDetPeopleEnable ? "ENABLED" : "DISABLED")); + mNhNs.getParam("object_detection/mc_vehicle", mObjDetVehiclesEnable); + NODELET_INFO_STREAM(" * Detect vehicles\t\t-> " << (mObjDetVehiclesEnable ? "ENABLED" : "DISABLED")); + mNhNs.getParam("object_detection/mc_bag", mObjDetBagsEnable); + NODELET_INFO_STREAM(" * Detect bags\t\t\t-> " << (mObjDetBagsEnable ? "ENABLED" : "DISABLED")); + mNhNs.getParam("object_detection/mc_animal", mObjDetAnimalsEnable); + NODELET_INFO_STREAM(" * Detect animals\t\t-> " << (mObjDetAnimalsEnable ? "ENABLED" : "DISABLED")); + mNhNs.getParam("object_detection/mc_electronics", mObjDetElectronicsEnable); + NODELET_INFO_STREAM(" * Detect electronics\t\t-> " << (mObjDetElectronicsEnable ? "ENABLED" : "DISABLED")); + mNhNs.getParam("object_detection/mc_fruit_vegetable", mObjDetFruitsEnable); + NODELET_INFO_STREAM(" * Detect fruit and vegetables\t-> " << (mObjDetFruitsEnable ? "ENABLED" : "DISABLED")); + mNhNs.getParam("object_detection/mc_sport", mObjDetSportsEnable); + NODELET_INFO_STREAM(" * Detect sport-related objects\t-> " << (mObjDetSportsEnable ? "ENABLED" : "DISABLED")); } - mObjDetModel = static_cast(model); - - NODELET_INFO_STREAM(" * Detection model\t\t-> " << sl::toString(mObjDetModel)); - - mNhNs.getParam("object_detection/mc_people", mObjDetPeopleEnable); - NODELET_INFO_STREAM(" * Detect people\t\t-> " << (mObjDetPeopleEnable ? "ENABLED" : "DISABLED")); - mNhNs.getParam("object_detection/mc_vehicle", mObjDetVehiclesEnable); - NODELET_INFO_STREAM(" * Detect vehicles\t\t-> " << (mObjDetVehiclesEnable ? "ENABLED" : "DISABLED")); - mNhNs.getParam("object_detection/mc_bag", mObjDetBagsEnable); - NODELET_INFO_STREAM(" * Detect bags\t\t\t-> " << (mObjDetBagsEnable ? "ENABLED" : "DISABLED")); - mNhNs.getParam("object_detection/mc_animal", mObjDetAnimalsEnable); - NODELET_INFO_STREAM(" * Detect animals\t\t-> " << (mObjDetAnimalsEnable ? "ENABLED" : "DISABLED")); - mNhNs.getParam("object_detection/mc_electronics", mObjDetElectronicsEnable); - NODELET_INFO_STREAM(" * Detect electronics\t\t-> " << (mObjDetElectronicsEnable ? "ENABLED" : "DISABLED")); - mNhNs.getParam("object_detection/mc_fruit_vegetable", mObjDetFruitsEnable); - NODELET_INFO_STREAM(" * Detect fruit and vegetables\t-> " << (mObjDetFruitsEnable ? "ENABLED" : "DISABLED")); - mNhNs.getParam("object_detection/mc_sport", mObjDetSportsEnable); - NODELET_INFO_STREAM(" * Detect sport-related objects\t-> " << (mObjDetSportsEnable ? "ENABLED" : "DISABLED")); } // <---- Object Detection @@ -877,49 +920,40 @@ void ZEDWrapperNodelet::readParameters() mConfidenceOptFrameId = mDepthOptFrameId; // Print TF frames - NODELET_INFO_STREAM(" * map_frame\t\t\t-> " << mMapFrameId); - NODELET_INFO_STREAM(" * odometry_frame\t\t-> " << mOdometryFrameId); - NODELET_INFO_STREAM(" * base_frame\t\t\t-> " << mBaseFrameId); NODELET_INFO_STREAM(" * camera_frame\t\t\t-> " << mCameraFrameId); NODELET_INFO_STREAM(" * imu_link\t\t\t-> " << mImuFrameId); NODELET_INFO_STREAM(" * left_camera_frame\t\t-> " << mLeftCamFrameId); NODELET_INFO_STREAM(" * left_camera_optical_frame\t-> " << mLeftCamOptFrameId); NODELET_INFO_STREAM(" * right_camera_frame\t\t-> " << mRightCamFrameId); NODELET_INFO_STREAM(" * right_camera_optical_frame\t-> " << mRightCamOptFrameId); - NODELET_INFO_STREAM(" * depth_frame\t\t\t-> " << mDepthFrameId); - NODELET_INFO_STREAM(" * depth_optical_frame\t\t-> " << mDepthOptFrameId); - NODELET_INFO_STREAM(" * disparity_frame\t\t-> " << mDisparityFrameId); - NODELET_INFO_STREAM(" * disparity_optical_frame\t-> " << mDisparityOptFrameId); - NODELET_INFO_STREAM(" * confidence_frame\t\t-> " << mConfidenceFrameId); - NODELET_INFO_STREAM(" * confidence_optical_frame\t-> " << mConfidenceOptFrameId); + + if(!mDepthDisabled) { + NODELET_INFO_STREAM(" * map_frame\t\t\t-> " << mMapFrameId); + NODELET_INFO_STREAM(" * odometry_frame\t\t-> " << mOdometryFrameId); + NODELET_INFO_STREAM(" * base_frame\t\t\t-> " << mBaseFrameId); + NODELET_INFO_STREAM(" * depth_frame\t\t\t-> " << mDepthFrameId); + NODELET_INFO_STREAM(" * depth_optical_frame\t\t-> " << mDepthOptFrameId); + NODELET_INFO_STREAM(" * disparity_frame\t\t-> " << mDisparityFrameId); + NODELET_INFO_STREAM(" * disparity_optical_frame\t-> " << mDisparityOptFrameId); + NODELET_INFO_STREAM(" * confidence_frame\t\t-> " << mConfidenceFrameId); + NODELET_INFO_STREAM(" * confidence_optical_frame\t-> " << mConfidenceOptFrameId); + } // <---- Coordinate frames // ----> TF broadcasting - mNhNs.param("pos_tracking/publish_tf", mPublishTf, true); - NODELET_INFO_STREAM(" * Broadcast odometry TF\t-> " << (mPublishTf ? "ENABLED" : "DISABLED")); - mNhNs.param("pos_tracking/publish_map_tf", mPublishMapTf, true); - NODELET_INFO_STREAM(" * Broadcast map pose TF\t-> " - << (mPublishTf ? (mPublishMapTf ? "ENABLED" : "DISABLED") : "DISABLED")); - mNhNs.param("sensors/publish_imu_tf", mPublishImuTf, true); - NODELET_INFO_STREAM(" * Broadcast IMU pose TF\t-> " << (mPublishImuTf ? "ENABLED" : "DISABLED")); - // <---- TF broadcasting - - NODELET_INFO_STREAM("*** DYNAMIC PARAMETERS (Init. values) ***"); - - // ----> Dynamic - mNhNs.getParam("depth_confidence", mCamDepthConfidence); - NODELET_INFO_STREAM(" * [DYN] Depth confidence\t-> " << mCamDepthConfidence); - mNhNs.getParam("depth_texture_conf", mCamDepthTextureConf); - NODELET_INFO_STREAM(" * [DYN] Depth texture conf.\t-> " << mCamDepthTextureConf); - - mNhNs.getParam("point_cloud_freq", mPointCloudFreq); - if(mPointCloudFreq>mVideoDepthFreq) { - mPointCloudFreq=mVideoDepthFreq; - NODELET_WARN_STREAM("'point_cloud_freq' cannot be major than 'general/pub_frame_rate'. Set to " - << mPointCloudFreq); + if(!mDepthDisabled) { + mNhNs.param("pos_tracking/publish_tf", mPublishTf, true); + NODELET_INFO_STREAM(" * Broadcast odometry TF\t-> " << (mPublishTf ? "ENABLED" : "DISABLED")); + mNhNs.param("pos_tracking/publish_map_tf", mPublishMapTf, true); + NODELET_INFO_STREAM(" * Broadcast map pose TF\t-> " + << (mPublishTf ? (mPublishMapTf ? "ENABLED" : "DISABLED") : "DISABLED")); + mNhNs.param("sensors/publish_imu_tf", mPublishImuTf, true); + NODELET_INFO_STREAM(" * Broadcast IMU pose TF\t-> " << (mPublishImuTf ? "ENABLED" : "DISABLED")); } - NODELET_INFO_STREAM(" * [DYN] point_cloud_freq\t-> " << mPointCloudFreq << " Hz"); + // <---- TF broadcasting + // ----> Dynamic + NODELET_INFO_STREAM("*** DYNAMIC PARAMETERS (Init. values) ***"); mNhNs.getParam("brightness", mCamBrightness); NODELET_INFO_STREAM(" * [DYN] brightness\t\t-> " << mCamBrightness); mNhNs.getParam("contrast", mCamContrast); @@ -953,6 +987,21 @@ void ZEDWrapperNodelet::readParameters() if (mCamAutoWB) { mTriggerAutoWB = true; } + + if(!mDepthDisabled) { + mNhNs.getParam("depth_confidence", mCamDepthConfidence); + NODELET_INFO_STREAM(" * [DYN] Depth confidence\t-> " << mCamDepthConfidence); + mNhNs.getParam("depth_texture_conf", mCamDepthTextureConf); + NODELET_INFO_STREAM(" * [DYN] Depth texture conf.\t-> " << mCamDepthTextureConf); + + mNhNs.getParam("point_cloud_freq", mPointCloudFreq); + if(mPointCloudFreq>mVideoDepthFreq) { + mPointCloudFreq=mVideoDepthFreq; + NODELET_WARN_STREAM("'point_cloud_freq' cannot be major than 'general/pub_frame_rate'. Set to " + << mPointCloudFreq); + } + NODELET_INFO_STREAM(" * [DYN] point_cloud_freq\t-> " << mPointCloudFreq << " Hz"); + } // <---- Dynamic } @@ -3216,7 +3265,7 @@ void ZEDWrapperNodelet::device_poll_thread_func() sl::RuntimeParameters runParams; // Main loop - while (mNhNs.ok()) { + while (mNhNs.ok()) { // Check for subscribers uint32_t rgbSubnumber = mPubRgb.getNumSubscribers(); uint32_t rgbRawSubnumber = mPubRawRgb.getNumSubscribers(); @@ -3230,23 +3279,34 @@ void ZEDWrapperNodelet::device_poll_thread_func() uint32_t leftGrayRawSubnumber = mPubRawLeftGray.getNumSubscribers(); uint32_t rightGraySubnumber = mPubRightGray.getNumSubscribers(); uint32_t rightGrayRawSubnumber = mPubRawRightGray.getNumSubscribers(); - uint32_t depthSubnumber = mPubDepth.getNumSubscribers(); - uint32_t disparitySubnumber = mPubDisparity.getNumSubscribers(); - uint32_t cloudSubnumber = mPubCloud.getNumSubscribers(); - uint32_t fusedCloudSubnumber = mPubFusedCloud.getNumSubscribers(); - uint32_t poseSubnumber = mPubPose.getNumSubscribers(); - uint32_t poseCovSubnumber = mPubPoseCov.getNumSubscribers(); - uint32_t odomSubnumber = mPubOdom.getNumSubscribers(); - uint32_t confMapSubnumber = mPubConfMap.getNumSubscribers(); - uint32_t pathSubNumber = mPubMapPath.getNumSubscribers() + mPubOdomPath.getNumSubscribers(); - uint32_t stereoSubNumber = mPubStereo.getNumSubscribers(); - uint32_t stereoRawSubNumber = mPubRawStereo.getNumSubscribers(); - + uint32_t depthSubnumber = 0; uint32_t objDetSubnumber = 0; - if (mObjDetEnabled && mObjDetRunning) { - objDetSubnumber = mPubObjDet.getNumSubscribers(); + uint32_t disparitySubnumber = 0; + uint32_t cloudSubnumber = 0; + uint32_t fusedCloudSubnumber = 0; + uint32_t poseSubnumber = 0; + uint32_t poseCovSubnumber = 0; + uint32_t odomSubnumber = 0; + uint32_t confMapSubnumber = 0; + uint32_t pathSubNumber = 0; + if(!mDepthDisabled) { + depthSubnumber = mPubDepth.getNumSubscribers(); + disparitySubnumber = mPubDisparity.getNumSubscribers(); + cloudSubnumber = mPubCloud.getNumSubscribers(); + fusedCloudSubnumber = mPubFusedCloud.getNumSubscribers(); + poseSubnumber = mPubPose.getNumSubscribers(); + poseCovSubnumber = mPubPoseCov.getNumSubscribers(); + odomSubnumber = mPubOdom.getNumSubscribers(); + confMapSubnumber = mPubConfMap.getNumSubscribers(); + pathSubNumber = mPubMapPath.getNumSubscribers() + mPubOdomPath.getNumSubscribers(); + + if (mObjDetEnabled && mObjDetRunning) { + objDetSubnumber = mPubObjDet.getNumSubscribers(); + } } - + uint32_t stereoSubNumber = mPubStereo.getNumSubscribers(); + uint32_t stereoRawSubNumber = mPubRawStereo.getNumSubscribers(); + mGrabActive = mRecording || mStreaming || mMappingEnabled || mObjDetEnabled || mPosTrackingEnabled || mPosTrackingActivated || ((rgbSubnumber + rgbRawSubnumber + leftSubnumber + leftRawSubnumber + rightSubnumber + rightRawSubnumber + rgbGraySubnumber + rgbGrayRawSubnumber + leftGraySubnumber + leftGrayRawSubnumber + rightGraySubnumber + rightGrayRawSubnumber + depthSubnumber + disparitySubnumber + cloudSubnumber + poseSubnumber + poseCovSubnumber + odomSubnumber + confMapSubnumber /*+ imuSubnumber + imuRawsubnumber*/ + pathSubNumber + stereoSubNumber + stereoRawSubNumber + objDetSubnumber) > 0); // Run the loop only if there is some subscribers or SVO is active @@ -3254,10 +3314,12 @@ void ZEDWrapperNodelet::device_poll_thread_func() std::lock_guard lock(mPosTrkMutex); // Note: ones tracking is started it is never stopped anymore to not lose tracking information - bool computeTracking = (mPosTrackingEnabled || mPosTrackingActivated || mMappingEnabled || mObjDetEnabled || (mComputeDepth & mDepthStabilization) || poseSubnumber > 0 || poseCovSubnumber > 0 || odomSubnumber > 0 || pathSubNumber > 0); + bool computeTracking = !mDepthDisabled && + (mPosTrackingEnabled || mPosTrackingActivated || mMappingEnabled || mObjDetEnabled || (mComputeDepth & mDepthStabilization) || + poseSubnumber > 0 || poseCovSubnumber > 0 || odomSubnumber > 0 || pathSubNumber > 0); // Start the tracking? - if ((computeTracking) && !mPosTrackingActivated && (mDepthMode != sl::DEPTH_MODE::NONE)) { + if ((computeTracking) && !mPosTrackingActivated && !mDepthDisabled) { start_pos_tracking(); } @@ -3276,7 +3338,7 @@ void ZEDWrapperNodelet::device_poll_thread_func() mObjDetMutex.unlock(); // Detect if one of the subscriber need to have the depth information - mComputeDepth = mDepthMode != sl::DEPTH_MODE::NONE && (computeTracking || ((depthSubnumber + disparitySubnumber + cloudSubnumber + fusedCloudSubnumber + poseSubnumber + poseCovSubnumber + odomSubnumber + confMapSubnumber + objDetSubnumber) > 0)); + mComputeDepth = !mDepthDisabled && (computeTracking || ((depthSubnumber + disparitySubnumber + cloudSubnumber + fusedCloudSubnumber + poseSubnumber + poseCovSubnumber + odomSubnumber + confMapSubnumber + objDetSubnumber) > 0)); if (mComputeDepth) { runParams.confidence_threshold = mCamDepthConfidence; diff --git a/zed_wrapper/params/common.yaml b/zed_wrapper/params/common.yaml index 8b166b89..e651a7e0 100644 --- a/zed_wrapper/params/common.yaml +++ b/zed_wrapper/params/common.yaml @@ -34,7 +34,7 @@ video: img_downsample_factor: 0.5 # Resample factor for images [0.01,1.0] The SDK works with native image sizes, but publishes rescaled image. depth: - quality: 3 # '0': NONE, '1': PERFORMANCE, '2': QUALITY, '3': ULTRA, '4': NEURAL + depth_mode: 'NONE' # 'NONE', 'PERFORMANCE', 'QUALITY', 'ULTRA', 'NEURAL' depth_stabilization: 1 # [0-100] - 0: Disabled openni_depth_mode: false # 'false': 32bit float meters, 'true': 16bit uchar millimeters depth_downsample_factor: 0.5 # Resample factor for depth data matrices [0.01,1.0] The SDK works with native data sizes, but publishes rescaled matrices (depth map, point cloud, ...) From 9d12c4344be9c6b573c19fd6db5309e993e77617 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Mon, 11 Sep 2023 18:08:06 +0200 Subject: [PATCH 164/199] [WIP] Custom rescale factor --- CHANGELOG.rst | 2 ++ zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp | 3 +-- zed_wrapper/params/common.yaml | 4 ++-- 3 files changed, 5 insertions(+), 4 deletions(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 8da8a49b..731f050d 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -5,6 +5,8 @@ CHANGELOG ---------- - Change the parameter 'depth/quality' to 'depth/depth_mode' and replace numeric values with strings - Improve the behavior of the "NO DEPTH" mode +- Remove the parameters 'depth_downsample_factor' and 'img_downsample_factor' +- Add the parameter 'pub_resolution' and 'pub_downscale_factor' 2023-09-08 ---------- 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 1242d945..2d78238e 100644 --- a/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp +++ b/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp @@ -641,8 +641,7 @@ class ZEDWrapperNodelet : public nodelet::Nodelet { int mCamDepthTextureConf = 100; double mPointCloudFreq = 15.; - double mCamImageResizeFactor = 1.0; - double mCamDepthResizeFactor = 1.0; + double mCustomDownscaleFactor = 0.5; // flags bool mTriggerAutoExposure = true; diff --git a/zed_wrapper/params/common.yaml b/zed_wrapper/params/common.yaml index e651a7e0..413cecfb 100644 --- a/zed_wrapper/params/common.yaml +++ b/zed_wrapper/params/common.yaml @@ -28,16 +28,16 @@ general: svo_compression: 2 # `0`: LOSSLESS, `1`: AVCHD, `2`: HEVC self_calib: true # enable/disable self calibration at starting camera_flip: 'AUTO' # camera flip mode: 'OFF', 'ON', 'AUTO' + pub_resolution: 'CUSTOM' # The resolution used for output. 'NATIVE' to use the same `general.grab_resolution` - `CUSTOM` to apply the `general.pub_downscale_factor` downscale factory to reduce bandwidth in transmission + pub_downscale_factor: 2.0 # rescale factor used to rescale image before publishing when 'pub_resolution' is 'CUSTOM' pub_frame_rate: 15.0 # frequency of publishing of video and depth data (see SDK API "InitParameters::grab_compute_capping_fps") video: - img_downsample_factor: 0.5 # Resample factor for images [0.01,1.0] The SDK works with native image sizes, but publishes rescaled image. depth: depth_mode: 'NONE' # 'NONE', 'PERFORMANCE', 'QUALITY', 'ULTRA', 'NEURAL' depth_stabilization: 1 # [0-100] - 0: Disabled openni_depth_mode: false # 'false': 32bit float meters, 'true': 16bit uchar millimeters - depth_downsample_factor: 0.5 # Resample factor for depth data matrices [0.01,1.0] The SDK works with native data sizes, but publishes rescaled matrices (depth map, point cloud, ...) pos_tracking: pos_tracking_enabled: true # True to enable positional tracking from start From 5601dc7e18ba2e0e7ef0606868bc4f676ff3cb1f Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Tue, 12 Sep 2023 12:06:12 +0200 Subject: [PATCH 165/199] 'pub_resolution' and 'pub_downscale_factor' completed --- .../include/zed_wrapper_nodelet.hpp | 15 +- .../zed_nodelet/src/zed_wrapper_nodelet.cpp | 149 +++++++++++------- zed_wrapper/params/common.yaml | 8 +- 3 files changed, 103 insertions(+), 69 deletions(-) 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 2d78238e..3a8be1eb 100644 --- a/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp +++ b/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp @@ -79,6 +79,13 @@ #include namespace zed_nodelets { + +typedef enum +{ + NATIVE, //!< Same camera grab resolution + CUSTOM //!< Custom Rescale Factor +} PubRes; + class ZEDWrapperNodelet : public nodelet::Nodelet { typedef enum _dyn_params { DATAPUB_FREQ = 0, @@ -544,7 +551,7 @@ class ZEDWrapperNodelet : public nodelet::Nodelet { std::string mCameraName; sl::RESOLUTION mCamResol; int mCamFrameRate; - double mVideoDepthFreq = 15.; + double mPubFrameRate = 15.; sl::DEPTH_MODE mDepthMode = sl::DEPTH_MODE::ULTRA; int mGpuId; int mZedId; @@ -641,7 +648,8 @@ class ZEDWrapperNodelet : public nodelet::Nodelet { int mCamDepthTextureConf = 100; double mPointCloudFreq = 15.; - double mCustomDownscaleFactor = 0.5; + PubRes mPubResolution = PubRes::NATIVE; // Use native grab resolution by default + double mCustomDownscaleFactor = 1.0; // Used to rescale data with user factor // flags bool mTriggerAutoExposure = true; @@ -666,8 +674,7 @@ class ZEDWrapperNodelet : public nodelet::Nodelet { // Mat int mCamWidth; int mCamHeight; - sl::Resolution mMatResolVideo; - sl::Resolution mMatResolDepth; + sl::Resolution mMatResol; // Thread Sync std::mutex mCloseZedMutex; 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 d4bc0d56..c2fa364f 100644 --- a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp @@ -195,7 +195,7 @@ void ZEDWrapperNodelet::onInit() mSvoMode = true; } else { mZedParams.camera_fps = mCamFrameRate; - mZedParams.grab_compute_capping_fps = static_cast(mVideoDepthFreq); + mZedParams.grab_compute_capping_fps = static_cast(mPubFrameRate); mZedParams.camera_resolution = mCamResol; if (mZedSerialNumber == 0) { @@ -618,12 +618,33 @@ void ZEDWrapperNodelet::readParameters() mNhNs.getParam("general/grab_frame_rate", mCamFrameRate); checkResolFps(); NODELET_INFO_STREAM(" * Camera Grab Framerate\t-> " << mCamFrameRate); - mNhNs.getParam("general/pub_frame_rate", mVideoDepthFreq); - if(mVideoDepthFreq>mCamFrameRate) { - mVideoDepthFreq = mCamFrameRate; - NODELET_WARN_STREAM("'general/pub_frame_rate' cannot be bigger than 'general/grab_frame_rate'. Forced to the correct value: " << mVideoDepthFreq); + mNhNs.getParam("general/pub_frame_rate", mPubFrameRate); + if(mPubFrameRate>mCamFrameRate) { + mPubFrameRate = mCamFrameRate; + NODELET_WARN_STREAM("'general/pub_frame_rate' cannot be bigger than 'general/grab_frame_rate'. Forced to the correct value: " << mPubFrameRate); + } + NODELET_INFO_STREAM(" * Grab compute capping FPS\t-> " << mPubFrameRate << " Hz"); + + + std::string out_resol = "CUSTOM"; + mNhNs.getParam("general/pub_resolution", out_resol); + if (out_resol == "NATIVE") { + mPubResolution = PubRes::NATIVE; + } else if (out_resol == "CUSTOM") { + mPubResolution = PubRes::CUSTOM; + } else { + ROS_WARN("Not valid 'general/pub_resolution' value: '%s'. Using default setting instead.", out_resol.c_str()); + out_resol = "CUSTOM"; + mPubResolution = PubRes::CUSTOM; + } + ROS_INFO_STREAM(" * Publishing resolution: " << out_resol.c_str()); + + if (mPubResolution == PubRes::CUSTOM) { + mNhNs.getParam( "general/pub_downscale_factor", mCustomDownscaleFactor ); + NODELET_INFO_STREAM(" * Publishing downscale factor: " << mCustomDownscaleFactor ); + } else { + mCustomDownscaleFactor = 1.0; } - NODELET_INFO_STREAM(" * Grab compute capping FPS\t-> " << mVideoDepthFreq << " Hz"); mNhNs.getParam("general/gpu_id", mGpuId); NODELET_INFO_STREAM(" * Gpu ID\t\t\t-> " << mGpuId); @@ -653,16 +674,15 @@ void ZEDWrapperNodelet::readParameters() } // <---- General - NODELET_INFO_STREAM("*** VIDEO PARAMETERS ***"); - // ----> Video - mNhNs.getParam("video/img_downsample_factor", mCamImageResizeFactor); - NODELET_INFO_STREAM(" * Image resample factor\t-> " << mCamImageResizeFactor); - // <---- Video - NODELET_INFO_STREAM("*** DEPTH PARAMETERS ***"); + // ----> Video + //NODELET_INFO_STREAM("*** VIDEO PARAMETERS ***"); + // Note: there are no "static" video parameters + // <---- Video // -----> Depth + NODELET_INFO_STREAM("*** DEPTH PARAMETERS ***"); std::string depth_mode_str = sl::toString(mDepthMode).c_str(); mNhNs.getParam("depth/depth_mode", depth_mode_str); @@ -705,8 +725,6 @@ void ZEDWrapperNodelet::readParameters() NODELET_INFO_STREAM(" * Minimum depth\t\t-> " << mCamMinDepth << " m"); mNhNs.getParam("depth/max_depth", mCamMaxDepth); NODELET_INFO_STREAM(" * Maximum depth\t\t-> " << mCamMaxDepth << " m"); - mNhNs.getParam("depth/depth_downsample_factor", mCamDepthResizeFactor); - NODELET_INFO_STREAM(" * Depth resample factor\t-> " << mCamDepthResizeFactor); } // <----- Depth @@ -995,8 +1013,8 @@ void ZEDWrapperNodelet::readParameters() NODELET_INFO_STREAM(" * [DYN] Depth texture conf.\t-> " << mCamDepthTextureConf); mNhNs.getParam("point_cloud_freq", mPointCloudFreq); - if(mPointCloudFreq>mVideoDepthFreq) { - mPointCloudFreq=mVideoDepthFreq; + if(mPointCloudFreq>mPubFrameRate) { + mPointCloudFreq=mPubFrameRate; NODELET_WARN_STREAM("'point_cloud_freq' cannot be major than 'general/pub_frame_rate'. Set to " << mPointCloudFreq); } @@ -2008,7 +2026,7 @@ void ZEDWrapperNodelet::publishDepth(sensor_msgs::ImagePtr imgMsgPtr, sl::Mat de void ZEDWrapperNodelet::publishDisparity(sl::Mat disparity, ros::Time t) { - sl::CameraInformation zedParam = mZed.getCameraInformation(mMatResolDepth); + sl::CameraInformation zedParam = mZed.getCameraInformation(mMatResol); sensor_msgs::ImagePtr disparityImgMsg = boost::make_shared(); stereo_msgs::DisparityImagePtr disparityMsg = boost::make_shared(); @@ -2085,18 +2103,18 @@ void ZEDWrapperNodelet::publishPointCloud() // Initialize Point Cloud message // https://github.com/ros/common_msgs/blob/jade-devel/sensor_msgs/include/sensor_msgs/point_cloud2_iterator.h - int ptsCount = mMatResolDepth.width * mMatResolDepth.height; + int ptsCount = mMatResol.width * mMatResol.height; pointcloudMsg->header.stamp = mPointCloudTime; - if (pointcloudMsg->width != mMatResolDepth.width || pointcloudMsg->height != mMatResolDepth.height) { + if (pointcloudMsg->width != mMatResol.width || pointcloudMsg->height != mMatResol.height) { pointcloudMsg->header.frame_id = mPointCloudFrameId; // Set the header values of the ROS message pointcloudMsg->is_bigendian = false; pointcloudMsg->is_dense = false; - pointcloudMsg->width = mMatResolDepth.width; - pointcloudMsg->height = mMatResolDepth.height; + pointcloudMsg->width = mMatResol.width; + pointcloudMsg->height = mMatResol.height; sensor_msgs::PointCloud2Modifier modifier(*pointcloudMsg); modifier.setPointCloud2Fields(4, "x", 1, sensor_msgs::PointField::FLOAT32, "y", 1, sensor_msgs::PointField::FLOAT32, @@ -2221,9 +2239,9 @@ void ZEDWrapperNodelet::fillCamInfo(sl::Camera& zed, sensor_msgs::CameraInfoPtr sl::CalibrationParameters zedParam; if (rawParam) { - zedParam = zed.getCameraInformation(mMatResolVideo).camera_configuration.calibration_parameters_raw; + zedParam = zed.getCameraInformation(mMatResol).camera_configuration.calibration_parameters_raw; } else { - zedParam = zed.getCameraInformation(mMatResolVideo).camera_configuration.calibration_parameters; + zedParam = zed.getCameraInformation(mMatResol).camera_configuration.calibration_parameters; } float baseline = zedParam.getCameraBaseline(); @@ -2282,8 +2300,8 @@ void ZEDWrapperNodelet::fillCamInfo(sl::Camera& zed, sensor_msgs::CameraInfoPtr rightCamInfoMsg->P[5] = static_cast(zedParam.right_cam.fy); rightCamInfoMsg->P[6] = static_cast(zedParam.right_cam.cy); rightCamInfoMsg->P[10] = 1.0; - leftCamInfoMsg->width = rightCamInfoMsg->width = static_cast(mMatResolVideo.width); - leftCamInfoMsg->height = rightCamInfoMsg->height = static_cast(mMatResolVideo.height); + leftCamInfoMsg->width = rightCamInfoMsg->width = static_cast(mMatResol.width); + leftCamInfoMsg->height = rightCamInfoMsg->height = static_cast(mMatResol.height); leftCamInfoMsg->header.frame_id = leftFrameId; rightCamInfoMsg->header.frame_id = rightFrameId; } @@ -2292,7 +2310,7 @@ void ZEDWrapperNodelet::fillCamDepthInfo(sl::Camera& zed, sensor_msgs::CameraInf { sl::CalibrationParameters zedParam; - zedParam = zed.getCameraInformation(mMatResolDepth).camera_configuration.calibration_parameters; + zedParam = zed.getCameraInformation(mMatResol).camera_configuration.calibration_parameters; float baseline = zedParam.getCameraBaseline(); depth_info_msg->distortion_model = sensor_msgs::distortion_models::PLUMB_BOB; @@ -2322,8 +2340,8 @@ void ZEDWrapperNodelet::fillCamDepthInfo(sl::Camera& zed, sensor_msgs::CameraInf depth_info_msg->P[6] = static_cast(zedParam.left_cam.cy); depth_info_msg->P[10] = 1.0; // http://docs.ros.org/api/sensor_msgs/html/msg/CameraInfo.html - depth_info_msg->width = static_cast(mMatResolDepth.width); - depth_info_msg->height = static_cast(mMatResolDepth.height); + depth_info_msg->width = static_cast(mMatResol.width); + depth_info_msg->height = static_cast(mMatResol.height); depth_info_msg->header.frame_id = frame_id; } @@ -2380,8 +2398,8 @@ void ZEDWrapperNodelet::callback_dynamicReconf(zed_nodelets::ZedConfig& config, break; case POINTCLOUD_FREQ: - if (config.point_cloud_freq > mVideoDepthFreq) { - mPointCloudFreq = mVideoDepthFreq; + if (config.point_cloud_freq > mPubFrameRate) { + mPointCloudFreq = mPubFrameRate; NODELET_WARN_STREAM("'point_cloud_freq' cannot be major than Video/Depth publishing framerate. Set to " << mPointCloudFreq); @@ -2538,51 +2556,51 @@ void ZEDWrapperNodelet::pubVideoDepth() // ----> Retrieve all required image data if (rgbSubnumber + leftSubnumber + stereoSubNumber > 0) { - mZed.retrieveImage(mat_left, sl::VIEW::LEFT, sl::MEM::CPU, mMatResolVideo); + mZed.retrieveImage(mat_left, sl::VIEW::LEFT, sl::MEM::CPU, mMatResol); retrieved = true; ts_rgb = mat_left.timestamp; grab_ts = mat_left.timestamp; } if (rgbRawSubnumber + leftRawSubnumber + stereoRawSubNumber > 0) { - mZed.retrieveImage(mat_left_raw, sl::VIEW::LEFT_UNRECTIFIED, sl::MEM::CPU, mMatResolVideo); + mZed.retrieveImage(mat_left_raw, sl::VIEW::LEFT_UNRECTIFIED, sl::MEM::CPU, mMatResol); retrieved = true; grab_ts = mat_left_raw.timestamp; } if (rightSubnumber + stereoSubNumber > 0) { - mZed.retrieveImage(mat_right, sl::VIEW::RIGHT, sl::MEM::CPU, mMatResolVideo); + mZed.retrieveImage(mat_right, sl::VIEW::RIGHT, sl::MEM::CPU, mMatResol); retrieved = true; grab_ts = mat_right.timestamp; } if (rightRawSubnumber + stereoRawSubNumber > 0) { - mZed.retrieveImage(mat_right_raw, sl::VIEW::RIGHT_UNRECTIFIED, sl::MEM::CPU, mMatResolVideo); + mZed.retrieveImage(mat_right_raw, sl::VIEW::RIGHT_UNRECTIFIED, sl::MEM::CPU, mMatResol); retrieved = true; grab_ts = mat_right_raw.timestamp; } if (rgbGraySubnumber + leftGraySubnumber > 0) { - mZed.retrieveImage(mat_left_gray, sl::VIEW::LEFT_GRAY, sl::MEM::CPU, mMatResolVideo); + mZed.retrieveImage(mat_left_gray, sl::VIEW::LEFT_GRAY, sl::MEM::CPU, mMatResol); retrieved = true; grab_ts = mat_left_gray.timestamp; } if (rgbGrayRawSubnumber + leftGrayRawSubnumber > 0) { - mZed.retrieveImage(mat_left_raw_gray, sl::VIEW::LEFT_UNRECTIFIED_GRAY, sl::MEM::CPU, mMatResolVideo); + mZed.retrieveImage(mat_left_raw_gray, sl::VIEW::LEFT_UNRECTIFIED_GRAY, sl::MEM::CPU, mMatResol); retrieved = true; grab_ts = mat_left_raw_gray.timestamp; } if (rightGraySubnumber > 0) { - mZed.retrieveImage(mat_right_gray, sl::VIEW::RIGHT_GRAY, sl::MEM::CPU, mMatResolVideo); + mZed.retrieveImage(mat_right_gray, sl::VIEW::RIGHT_GRAY, sl::MEM::CPU, mMatResol); retrieved = true; grab_ts = mat_right_gray.timestamp; } if (rightGrayRawSubnumber > 0) { - mZed.retrieveImage(mat_right_raw_gray, sl::VIEW::RIGHT_UNRECTIFIED_GRAY, sl::MEM::CPU, mMatResolVideo); + mZed.retrieveImage(mat_right_raw_gray, sl::VIEW::RIGHT_UNRECTIFIED_GRAY, sl::MEM::CPU, mMatResol); retrieved = true; grab_ts = mat_right_raw_gray.timestamp; } if (depthSubnumber > 0) { if (!mOpenniDepthMode) { - mZed.retrieveMeasure(mat_depth, sl::MEASURE::DEPTH, sl::MEM::CPU, mMatResolDepth); + mZed.retrieveMeasure(mat_depth, sl::MEASURE::DEPTH, sl::MEM::CPU, mMatResol); } else { - mZed.retrieveMeasure(mat_depth, sl::MEASURE::DEPTH_U16_MM, sl::MEM::CPU, mMatResolDepth); + mZed.retrieveMeasure(mat_depth, sl::MEASURE::DEPTH_U16_MM, sl::MEM::CPU, mMatResol); } retrieved = true; grab_ts = mat_depth.timestamp; @@ -2595,12 +2613,12 @@ void ZEDWrapperNodelet::pubVideoDepth() } } if (disparitySubnumber > 0) { - mZed.retrieveMeasure(mat_disp, sl::MEASURE::DISPARITY, sl::MEM::CPU, mMatResolDepth); + mZed.retrieveMeasure(mat_disp, sl::MEASURE::DISPARITY, sl::MEM::CPU, mMatResol); retrieved = true; grab_ts = mat_disp.timestamp; } if (confMapSubnumber > 0) { - mZed.retrieveMeasure(mat_conf, sl::MEASURE::CONFIDENCE, sl::MEM::CPU, mMatResolDepth); + mZed.retrieveMeasure(mat_conf, sl::MEASURE::CONFIDENCE, sl::MEM::CPU, mMatResol); retrieved = true; grab_ts = mat_conf.timestamp; } @@ -3217,7 +3235,7 @@ void ZEDWrapperNodelet::publishSensData(ros::Time t) void ZEDWrapperNodelet::device_poll_thread_func() { - ros::Rate loop_rate(mVideoDepthFreq); + ros::Rate loop_rate(mPubFrameRate); mRecording = false; @@ -3242,15 +3260,22 @@ void ZEDWrapperNodelet::device_poll_thread_func() // Get the parameters of the ZED images mCamWidth = mZed.getCameraInformation().camera_configuration.resolution.width; mCamHeight = mZed.getCameraInformation().camera_configuration.resolution.height; - NODELET_DEBUG_STREAM("Camera Frame size : " << mCamWidth << "x" << mCamHeight); - int v_w = static_cast(mCamWidth * mCamImageResizeFactor); - int v_h = static_cast(mCamHeight * mCamImageResizeFactor); - mMatResolVideo = sl::Resolution(v_w, v_h); - NODELET_DEBUG_STREAM("Image Mat size : " << mMatResolVideo.width << "x" << mMatResolVideo.height); - int d_w = static_cast(mCamWidth * mCamDepthResizeFactor); - int d_h = static_cast(mCamHeight * mCamDepthResizeFactor); - mMatResolDepth = sl::Resolution(d_w, d_h); - NODELET_DEBUG_STREAM("Depth Mat size : " << mMatResolDepth.width << "x" << mMatResolDepth.height); + NODELET_DEBUG_STREAM("Original Camera grab frame size -> " << mCamWidth << "x" << mCamHeight); + int pub_w, pub_h; + pub_w = static_cast(std::round(mCamWidth / mCustomDownscaleFactor)); + pub_h = static_cast(std::round(mCamHeight / mCustomDownscaleFactor)); + + if (pub_w > mCamWidth || pub_h > mCamHeight) { + NODELET_WARN_STREAM("The publishing resolution (" << + pub_w << "x" << pub_h << + ") cannot be higher than the grabbing resolution (" << + mCamWidth << "x" << mCamHeight << + "). Using grab resolution for output messages."); + pub_w = mCamWidth; + pub_h = mCamHeight; + } + mMatResol = sl::Resolution(pub_w, pub_h); + NODELET_DEBUG_STREAM("Publishing frame size -> " << mMatResol.width << "x" << mMatResol.height); // Create and fill the camera information messages fillCamInfo(mZed, mLeftCamInfoMsg, mRightCamInfoMsg, mLeftCamOptFrameId, mRightCamOptFrameId); @@ -3619,7 +3644,7 @@ void ZEDWrapperNodelet::device_poll_thread_func() std::unique_lock lock(mPcMutex, std::defer_lock); if (lock.try_lock()) { - mZed.retrieveMeasure(mCloud, sl::MEASURE::XYZBGRA, sl::MEM::CPU, mMatResolDepth); + mZed.retrieveMeasure(mCloud, sl::MEASURE::XYZBGRA, sl::MEM::CPU, mMatResol); mPointCloudFrameId = mDepthFrameId; mPointCloudTime = stamp; @@ -3893,7 +3918,7 @@ void ZEDWrapperNodelet::device_poll_thread_func() static int count_warn = 0; if (!loop_rate.sleep()) { - if (mean_elab_sec > (1. / mVideoDepthFreq)) { + 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); @@ -3972,15 +3997,15 @@ void ZEDWrapperNodelet::callback_updateDiagnostic(diagnostic_updater::Diagnostic stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Camera grabbing"); double freq = 1000000. / mGrabPeriodMean_usec->getMean(); - double freq_perc = 100. * freq / mCamFrameRate; + double freq_perc = 100. * freq / mPubFrameRate; stat.addf("Capture", "Mean Frequency: %.1f Hz (%.1f%%)", freq, freq_perc); stat.addf("General Processing", "Mean Time: %.3f sec (Max. %.3f sec)", mElabPeriodMean_sec->getMean(), - 1. / mVideoDepthFreq); + 1. / mPubFrameRate); if (mPublishingData) { freq = 1. / mVideoDepthPeriodMean_sec->getMean(); - freq_perc = 100. * freq / mVideoDepthFreq; + freq_perc = 100. * freq / mPubFrameRate; stat.addf("Video/Depth Publish", "Mean Frequency: %.1f Hz (%.1f%%)", freq, freq_perc); } @@ -4019,7 +4044,7 @@ void ZEDWrapperNodelet::callback_updateDiagnostic(diagnostic_updater::Diagnostic if (mObjDetRunning) { double freq = 1000. / mObjDetPeriodMean_msec->getMean(); - double freq_perc = 100. * freq / mVideoDepthFreq; + double freq_perc = 100. * freq / mPubFrameRate; stat.addf("Object detection", "Mean Frequency: %.3f Hz (%.1f%%)", freq, freq_perc); } else { stat.add("Object Detection", "INACTIVE"); @@ -4563,15 +4588,17 @@ void ZEDWrapperNodelet::clickedPtCallback(geometry_msgs::PointStampedConstPtr ms // ----> Project the point into 2D image coordinates sl::CalibrationParameters zedParam; - zedParam = mZed.getCameraInformation(mMatResolVideo).camera_configuration.calibration_parameters; // ok + zedParam = mZed.getCameraInformation(mMatResol).camera_configuration.calibration_parameters; // ok float f_x = zedParam.left_cam.fx; float f_y = zedParam.left_cam.fy; float c_x = zedParam.left_cam.cx; float c_y = zedParam.left_cam.cy; - float u = ((camX / camZ) * f_x + c_x) / mCamImageResizeFactor; - float v = ((camY / camZ) * f_y + c_y) / mCamImageResizeFactor; + float out_scale_factor = mMatResol.width / mCamWidth; + + float u = ((camX / camZ) * f_x + c_x) / out_scale_factor; + float v = ((camY / camZ) * f_y + c_y) / out_scale_factor; NODELET_INFO_STREAM("Clicked point image coordinates: [" << u << "," << v << "]"); // <---- Project the point into 2D image coordinates diff --git a/zed_wrapper/params/common.yaml b/zed_wrapper/params/common.yaml index 413cecfb..eef54dd6 100644 --- a/zed_wrapper/params/common.yaml +++ b/zed_wrapper/params/common.yaml @@ -28,14 +28,14 @@ general: svo_compression: 2 # `0`: LOSSLESS, `1`: AVCHD, `2`: HEVC self_calib: true # enable/disable self calibration at starting camera_flip: 'AUTO' # camera flip mode: 'OFF', 'ON', 'AUTO' - pub_resolution: 'CUSTOM' # The resolution used for output. 'NATIVE' to use the same `general.grab_resolution` - `CUSTOM` to apply the `general.pub_downscale_factor` downscale factory to reduce bandwidth in transmission - pub_downscale_factor: 2.0 # rescale factor used to rescale image before publishing when 'pub_resolution' is 'CUSTOM' + pub_resolution: 'CUSTOM' # The resolution used for output. 'NATIVE' to use the same `general.grab_resolution` - `CUSTOM` to apply the `general.pub_downscale_factor` downscale factory to reduce bandwidth in transmission + pub_downscale_factor: 2.0 # rescale factor used to rescale image before publishing when 'pub_resolution' is 'CUSTOM' pub_frame_rate: 15.0 # frequency of publishing of video and depth data (see SDK API "InitParameters::grab_compute_capping_fps") -video: +#video: depth: - depth_mode: 'NONE' # 'NONE', 'PERFORMANCE', 'QUALITY', 'ULTRA', 'NEURAL' + depth_mode: 'ULTRA' # 'NONE', 'PERFORMANCE', 'QUALITY', 'ULTRA', 'NEURAL' depth_stabilization: 1 # [0-100] - 0: Disabled openni_depth_mode: false # 'false': 32bit float meters, 'true': 16bit uchar millimeters From d9126cbd0f6bdb9dd5c28a511144aea9799a12b0 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Tue, 12 Sep 2023 14:39:24 +0200 Subject: [PATCH 166/199] Change `resolution` to `grab_resolution` --- CHANGELOG.rst | 4 + zed_nodelets/src/tools/include/sl_tools.h | 20 ++ zed_nodelets/src/tools/src/sl_tools.cpp | 38 +++ .../include/zed_wrapper_nodelet.hpp | 6 +- .../zed_nodelet/src/zed_wrapper_nodelet.cpp | 231 +++++++++--------- zed_wrapper/params/zed.yaml | 2 +- zed_wrapper/params/zed2.yaml | 2 +- zed_wrapper/params/zed2i.yaml | 2 +- zed_wrapper/params/zedm.yaml | 2 +- zed_wrapper/params/zedx.yaml | 4 +- zed_wrapper/params/zedxm.yaml | 4 +- 11 files changed, 185 insertions(+), 130 deletions(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 731f050d..76fc0563 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -1,6 +1,10 @@ CHANGELOG ========= +2023-09-12 +---------- +- Change the parameter 'general/resolution' to 'general/grab_resolution' and replace numeric values with strings + 2023-09-10 ---------- - Change the parameter 'depth/quality' to 'depth/depth_mode' and replace numeric values with strings diff --git a/zed_nodelets/src/tools/include/sl_tools.h b/zed_nodelets/src/tools/include/sl_tools.h index 2dd123f2..a222da59 100644 --- a/zed_nodelets/src/tools/include/sl_tools.h +++ b/zed_nodelets/src/tools/include/sl_tools.h @@ -52,6 +52,26 @@ std::string getSDKVersion(int& major, int& minor, int& sub_minor); */ ros::Time slTime2Ros(sl::Timestamp t); +/*! \brief check if ZED + * \param camModel the model to check + */ +bool isZED(sl::MODEL camModel); + +/*! \brief check if ZED Mini + * \param camModel the model to check + */ +bool isZEDM(sl::MODEL camModel); + +/*! \brief check if ZED2 or ZED2i + * \param camModel the model to check + */ +bool isZED2OrZED2i(sl::MODEL camModel); + +/*! \brief check if ZED-X or ZED-X Mini + * \param camModel the model to check + */ +bool isZEDX(sl::MODEL camModel); + /*! \brief sl::Mat to ros message conversion * \param imgMsgPtr : the image topic message to publish * \param img : the image to publish diff --git a/zed_nodelets/src/tools/src/sl_tools.cpp b/zed_nodelets/src/tools/src/sl_tools.cpp index 3857cdb0..2e47ef45 100644 --- a/zed_nodelets/src/tools/src/sl_tools.cpp +++ b/zed_nodelets/src/tools/src/sl_tools.cpp @@ -121,6 +121,44 @@ ros::Time slTime2Ros(sl::Timestamp t) return ros::Time(sec, nsec); } +bool isZED(sl::MODEL camModel) +{ + if (camModel == sl::MODEL::ZED) { + return true; + } + return false; +} + +bool isZEDM(sl::MODEL camModel) +{ + if (camModel == sl::MODEL::ZED_M) { + return true; + } + return false; +} + +bool isZED2OrZED2i(sl::MODEL camModel) +{ + if (camModel == sl::MODEL::ZED2) { + return true; + } + if (camModel == sl::MODEL::ZED2i) { + return true; + } + return false; +} + +bool isZEDX(sl::MODEL camModel) +{ + if (camModel == sl::MODEL::ZED_X) { + return true; + } + if (camModel == sl::MODEL::ZED_XM) { + return true; + } + return false; +} + void imageToROSmsg(sensor_msgs::ImagePtr imgMsgPtr, sl::Mat img, std::string frameId, ros::Time t) { if (!imgMsgPtr) 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 3a8be1eb..99b00679 100644 --- a/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp +++ b/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp @@ -243,11 +243,7 @@ class ZEDWrapperNodelet : public nodelet::Nodelet { */ void fillCamDepthInfo(sl::Camera& zed, sensor_msgs::CameraInfoPtr depth_info_msg, std::string frame_id); - /*! \brief Check if Resolution is valid for the camera model. - * Modifies Resolution to match correct value. - */ - void checkResol(); - + /*! \brief Check if FPS and Resolution chosen by user are correct. * Modifies FPS to match correct value. */ 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 c2fa364f..64228443 100644 --- a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp @@ -46,7 +46,7 @@ namespace zed_nodelets { #define BARO_FREQ 25. ZEDWrapperNodelet::ZEDWrapperNodelet() - : Nodelet() + : Nodelet() { } @@ -87,7 +87,7 @@ void ZEDWrapperNodelet::onInit() NODELET_INFO("********** Starting nodelet '%s' **********", getName().c_str()); std::string ver = sl_tools::getSDKVersion(mVerMajor, mVerMinor, mVerSubMinor); - NODELET_INFO_STREAM("SDK version : " << ver); + NODELET_INFO_STREAM("SDK version: " << ver); if (mVerMajor < 3) { NODELET_ERROR("This version of the ZED ROS Wrapper is designed to be used with ZED SDK v3.x"); @@ -180,6 +180,8 @@ void ZEDWrapperNodelet::onInit() mZedParams.input.setFromSVOFile(mSvoFilepath.c_str()); mZedParams.svo_real_time_mode = true; + NODELET_INFO_STREAM( "Node input: SVO - " << mSvoFilepath.c_str()); + // TODO ADD PARAMETER FOR SVO REAL TIME } else if (!mRemoteStreamAddr.empty()) { std::vector configStream = sl_tools::split_string(mRemoteStreamAddr, ':'); @@ -190,6 +192,8 @@ void ZEDWrapperNodelet::onInit() } else { mZedParams.input.setFromStream(ip); } + + NODELET_INFO_STREAM( "Node input: NETWORK STREAM - " << mRemoteStreamAddr.c_str()); } mSvoMode = true; @@ -200,8 +204,10 @@ void ZEDWrapperNodelet::onInit() if (mZedSerialNumber == 0) { mZedParams.input.setFromCameraID(mZedId); + NODELET_INFO_STREAM( "Node input: LIVE CAMERA with ID " << mZedId); } else { mZedParams.input.setFromSerialNumber(mZedSerialNumber); + NODELET_INFO_STREAM( "Node input: LIVE CAMERA with SN " << mZedSerialNumber); } } @@ -229,7 +235,7 @@ void ZEDWrapperNodelet::onInit() NODELET_INFO_STREAM(" *** Opening " << sl::toString(mZedUserCamModel) << "..."); while (mConnStatus != sl::ERROR_CODE::SUCCESS) { mConnStatus = mZed.open(mZedParams); - NODELET_INFO_STREAM("ZED connection -> " << sl::toString(mConnStatus)); + NODELET_INFO_STREAM("ZED connection: " << sl::toString(mConnStatus)); std::this_thread::sleep_for(std::chrono::milliseconds(2000)); if (!mNhNs.ok()) { @@ -300,19 +306,19 @@ void ZEDWrapperNodelet::onInit() NODELET_INFO("Camera-IMU Transform: \n %s", mSlCamImuTransf.getInfos().c_str()); } - NODELET_INFO_STREAM(" * CAMERA MODEL\t -> " << sl::toString(mZedRealCamModel).c_str()); + NODELET_INFO_STREAM(" * CAMERA MODEL\t-> " << sl::toString(mZedRealCamModel).c_str()); mZedSerialNumber = mZed.getCameraInformation().serial_number; - NODELET_INFO_STREAM(" * Serial Number -> " << mZedSerialNumber); + NODELET_INFO_STREAM(" * Serial Number: " << mZedSerialNumber); if (!mSvoMode) { mCamFwVersion = mZed.getCameraInformation().camera_configuration.firmware_version; - NODELET_INFO_STREAM(" * Camera FW Version -> " << mCamFwVersion); + NODELET_INFO_STREAM(" * Camera FW Version: " << mCamFwVersion); if (mZedRealCamModel != sl::MODEL::ZED) { mSensFwVersion = mZed.getCameraInformation().sensors_configuration.firmware_version; - NODELET_INFO_STREAM(" * Sensors FW Version -> " << mSensFwVersion); + NODELET_INFO_STREAM(" * Sensors FW Version: " << mSensFwVersion); } } else { - NODELET_INFO_STREAM(" * Input type\t -> " << sl::toString(mZed.getCameraInformation().input_type).c_str()); + NODELET_INFO_STREAM(" * Input type\t-> " << sl::toString(mZed.getCameraInformation().input_type).c_str()); } // Set the IMU topic names using real camera model @@ -610,11 +616,38 @@ void ZEDWrapperNodelet::readParameters() mNhNs.getParam("general/camera_name", mCameraName); NODELET_INFO_STREAM(" * Camera Name\t\t\t-> " << mCameraName.c_str()); - int resol; - mNhNs.getParam("general/resolution", resol); - mCamResol = static_cast(resol); - checkResol(); - NODELET_INFO_STREAM(" * Camera Resolution\t\t-> " << sl::toString(mCamResol).c_str()); + + std::string resol="AUTO"; + mNhNs.getParam("general/grab_resolution", resol); + if (resol == "AUTO") { + mCamResol = sl::RESOLUTION::AUTO; + } else if (sl_tools::isZEDX(mZedUserCamModel)) { + if (resol == "HD1200") { + mCamResol = sl::RESOLUTION::HD1200; + } else if (resol == "HD1080") { + mCamResol = sl::RESOLUTION::HD1080; + } else if (resol == "SVGA") { + mCamResol = sl::RESOLUTION::SVGA; + } else { + NODELET_WARN("Not valid 'general.grab_resolution' value: '%s'. Using 'AUTO' setting.", resol.c_str()); + mCamResol = sl::RESOLUTION::AUTO; + } + } else { + if (resol == "HD2K") { + mCamResol = sl::RESOLUTION::HD2K; + } else if (resol == "HD1080") { + mCamResol = sl::RESOLUTION::HD1080; + } else if (resol == "HD720") { + mCamResol = sl::RESOLUTION::HD720; + } else if (resol == "VGA") { + mCamResol = sl::RESOLUTION::VGA; + } else { + NODELET_WARN("Not valid 'general.grab_resolution' value: '%s'. Using 'AUTO' setting.", resol.c_str()); + mCamResol = sl::RESOLUTION::AUTO; + } + } + NODELET_INFO_STREAM(" * Camera resolution\t\t-> " << sl::toString(mCamResol).c_str()); + mNhNs.getParam("general/grab_frame_rate", mCamFrameRate); checkResolFps(); NODELET_INFO_STREAM(" * Camera Grab Framerate\t-> " << mCamFrameRate); @@ -633,15 +666,15 @@ void ZEDWrapperNodelet::readParameters() } else if (out_resol == "CUSTOM") { mPubResolution = PubRes::CUSTOM; } else { - ROS_WARN("Not valid 'general/pub_resolution' value: '%s'. Using default setting instead.", out_resol.c_str()); + NODELET_WARN("Not valid 'general/pub_resolution' value: '%s'. Using default setting instead.", out_resol.c_str()); out_resol = "CUSTOM"; mPubResolution = PubRes::CUSTOM; } - ROS_INFO_STREAM(" * Publishing resolution: " << out_resol.c_str()); + NODELET_INFO_STREAM(" * Publishing resolution\t-> " << out_resol.c_str()); if (mPubResolution == PubRes::CUSTOM) { mNhNs.getParam( "general/pub_downscale_factor", mCustomDownscaleFactor ); - NODELET_INFO_STREAM(" * Publishing downscale factor: " << mCustomDownscaleFactor ); + NODELET_INFO_STREAM(" * Publishing downscale factor\t-> " << mCustomDownscaleFactor ); } else { mCustomDownscaleFactor = 1.0; } @@ -651,7 +684,7 @@ void ZEDWrapperNodelet::readParameters() mNhNs.getParam("general/zed_id", mZedId); NODELET_INFO_STREAM(" * Camera ID\t\t\t-> " << mZedId); mNhNs.getParam("general/verbose", mVerbose); - NODELET_INFO_STREAM(" * Verbose\t\t\t-> " << (mVerbose ? "ENABLED" : "DISABLED")); + NODELET_INFO_STREAM(" * Verbose\t\t\t-> " << (mVerbose ? "ENABLED": "DISABLED")); std::string flip_str; mNhNs.getParam("general/camera_flip", flip_str); if(flip_str=="ON") { @@ -663,7 +696,7 @@ void ZEDWrapperNodelet::readParameters() } NODELET_INFO_STREAM(" * Camera Flip\t\t\t-> " << sl::toString(mCameraFlip).c_str()); mNhNs.param("general/self_calib", mCameraSelfCalib, true); - NODELET_INFO_STREAM(" * Self calibration\t\t-> " << (mCameraSelfCalib ? "ENABLED" : "DISABLED")); + NODELET_INFO_STREAM(" * Self calibration\t\t-> " << (mCameraSelfCalib ? "ENABLED": "DISABLED")); int tmp_sn = 0; mNhNs.getParam("general/serial_number", tmp_sn); @@ -700,25 +733,25 @@ void ZEDWrapperNodelet::readParameters() } if (!matched) { - ROS_WARN_STREAM( + NODELET_WARN_STREAM( "The parameter 'depth.depth_mode' contains a not valid string. Please check it in 'common.yaml'."); - ROS_WARN("Using default DEPTH_MODE."); + NODELET_WARN("Using default DEPTH_MODE."); mDepthMode = sl::DEPTH_MODE::ULTRA; } if (mDepthMode == sl::DEPTH_MODE::NONE) { mDepthDisabled = true; mDepthStabilization = 0; - ROS_INFO_STREAM(" * Depth mode: " << sl::toString(mDepthMode).c_str() << " - DEPTH DISABLED"); + NODELET_INFO_STREAM(" * Depth mode\t\t\t> " << sl::toString(mDepthMode).c_str() << " - DEPTH DISABLED"); } else { mDepthDisabled = false; - ROS_INFO_STREAM(" * Depth mode: " << sl::toString( + NODELET_INFO_STREAM(" * Depth mode\t\t\t--> " << sl::toString( mDepthMode).c_str() << " [" << static_cast(mDepthMode) << "]"); } if(!mDepthDisabled) { mNhNs.getParam("depth/openni_depth_mode", mOpenniDepthMode); - NODELET_INFO_STREAM(" * OpenNI mode\t\t\t-> " << (mOpenniDepthMode ? "ENABLED" : "DISABLED")); + NODELET_INFO_STREAM(" * OpenNI mode\t\t\t-> " << (mOpenniDepthMode ? "ENABLED": "DISABLED")); mNhNs.getParam("depth/depth_stabilization", mDepthStabilization); NODELET_INFO_STREAM(" * Depth Stabilization\t\t-> " << mDepthStabilization ); mNhNs.getParam("depth/min_depth", mCamMinDepth); @@ -733,7 +766,7 @@ void ZEDWrapperNodelet::readParameters() NODELET_INFO_STREAM("*** POSITIONAL TRACKING PARAMETERS ***"); mNhNs.param("pos_tracking/pos_tracking_enabled", mPosTrackingEnabled, true); - NODELET_INFO_STREAM(" * Positional tracking\t\t-> " << (mPosTrackingEnabled ? "ENABLED" : "DISABLED")); + NODELET_INFO_STREAM(" * Positional tracking\t\t-> " << (mPosTrackingEnabled ? "ENABLED": "DISABLED")); std::string pos_trk_mode; mNhNs.getParam("pos_tracking/pos_tracking_mode", pos_trk_mode); @@ -747,15 +780,15 @@ void ZEDWrapperNodelet::readParameters() "'). Using default value."); mPosTrkMode = sl::POSITIONAL_TRACKING_MODE::QUALITY; } - NODELET_INFO_STREAM( " * Positional tracking mode: " << sl::toString(mPosTrkMode).c_str()); + NODELET_INFO_STREAM( " * Positional tracking mode\t:" << sl::toString(mPosTrkMode).c_str()); mNhNs.getParam("pos_tracking/set_gravity_as_origin", mSetGravityAsOrigin); - NODELET_INFO_STREAM(" * Set gravity as origin\t\t-> " << (mSetGravityAsOrigin ? "ENABLED" : "DISABLED")); + NODELET_INFO_STREAM(" * Set gravity as origin\t-> " << (mSetGravityAsOrigin ? "ENABLED": "DISABLED")); 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) { mPathMaxCount = 2; @@ -768,18 +801,18 @@ void ZEDWrapperNodelet::readParameters() NODELET_INFO_STREAM(" * Odometry DB path\t\t-> " << mAreaMemDbPath.c_str()); mNhNs.param("pos_tracking/save_area_memory_db_on_exit", mSaveAreaMapOnClosing, false); - NODELET_INFO_STREAM(" * Save Area Memory on closing\t-> " << (mSaveAreaMapOnClosing ? "ENABLED" : "DISABLED")); + NODELET_INFO_STREAM(" * Save Area Memory on closing\t-> " << (mSaveAreaMapOnClosing ? "ENABLED": "DISABLED")); mNhNs.param("pos_tracking/area_memory", mAreaMemory, false); - NODELET_INFO_STREAM(" * Area Memory\t\t\t-> " << (mAreaMemory ? "ENABLED" : "DISABLED")); + NODELET_INFO_STREAM(" * Area Memory\t\t\t-> " << (mAreaMemory ? "ENABLED": "DISABLED")); mNhNs.param("pos_tracking/imu_fusion", mImuFusion, true); - NODELET_INFO_STREAM(" * IMU Fusion\t\t\t-> " << (mImuFusion ? "ENABLED" : "DISABLED")); + NODELET_INFO_STREAM(" * IMU Fusion\t\t\t-> " << (mImuFusion ? "ENABLED": "DISABLED")); mNhNs.param("pos_tracking/floor_alignment", mFloorAlignment, false); - NODELET_INFO_STREAM(" * Floor alignment\t\t-> " << (mFloorAlignment ? "ENABLED" : "DISABLED")); + NODELET_INFO_STREAM(" * Floor alignment\t\t-> " << (mFloorAlignment ? "ENABLED": "DISABLED")); mNhNs.param("pos_tracking/init_odom_with_first_valid_pose", mInitOdomWithPose, true); - NODELET_INFO_STREAM(" * Init Odometry with first valid pose data -> " - << (mInitOdomWithPose ? "ENABLED" : "DISABLED")); + NODELET_INFO_STREAM(" * Init Odometry with first valid pose data: " + << (mInitOdomWithPose ? "ENABLED": "DISABLED")); mNhNs.param("pos_tracking/two_d_mode", mTwoDMode, false); - NODELET_INFO_STREAM(" * Two D mode\t\t\t-> " << (mTwoDMode ? "ENABLED" : "DISABLED")); + NODELET_INFO_STREAM(" * Two D mode\t\t\t-> " << (mTwoDMode ? "ENABLED": "DISABLED")); mNhNs.param("pos_tracking/fixed_z_value", mFixedZValue, 0.0); if (mTwoDMode) { @@ -801,7 +834,7 @@ void ZEDWrapperNodelet::readParameters() mNhNs.getParam("mapping/max_mapping_range", mMaxMappingRange); NODELET_INFO_STREAM(" * Mapping max range\t\t-> " << mMaxMappingRange << " m" - << ((mMaxMappingRange < 0.0) ? " [AUTO]" : "")); + << ((mMaxMappingRange < 0.0) ? " [AUTO]": "")); mNhNs.getParam("mapping/fused_pointcloud_freq", mFusedPcPubFreq); NODELET_INFO_STREAM(" * Fused point cloud freq:\t-> " << mFusedPcPubFreq << " Hz"); @@ -824,7 +857,7 @@ void ZEDWrapperNodelet::readParameters() mNhNs.getParam("object_detection/confidence_threshold", mObjDetConfidence); NODELET_INFO_STREAM(" * Object confidence\t\t-> " << mObjDetConfidence); mNhNs.getParam("object_detection/object_tracking_enabled", mObjDetTracking); - NODELET_INFO_STREAM(" * Object tracking\t\t-> " << (mObjDetTracking ? "ENABLED" : "DISABLED")); + NODELET_INFO_STREAM(" * Object tracking\t\t-> " << (mObjDetTracking ? "ENABLED": "DISABLED")); mNhNs.getParam("object_detection/max_range", mObjDetMaxRange); if (mObjDetMaxRange > mCamMaxDepth) { NODELET_WARN("Detection max range cannot be major than depth max range. Automatically fixed."); @@ -843,19 +876,19 @@ void ZEDWrapperNodelet::readParameters() NODELET_INFO_STREAM(" * Detection model\t\t-> " << sl::toString(mObjDetModel)); mNhNs.getParam("object_detection/mc_people", mObjDetPeopleEnable); - NODELET_INFO_STREAM(" * Detect people\t\t-> " << (mObjDetPeopleEnable ? "ENABLED" : "DISABLED")); + NODELET_INFO_STREAM(" * Detect people\t\t-> " << (mObjDetPeopleEnable ? "ENABLED": "DISABLED")); mNhNs.getParam("object_detection/mc_vehicle", mObjDetVehiclesEnable); - NODELET_INFO_STREAM(" * Detect vehicles\t\t-> " << (mObjDetVehiclesEnable ? "ENABLED" : "DISABLED")); + NODELET_INFO_STREAM(" * Detect vehicles\t\t-> " << (mObjDetVehiclesEnable ? "ENABLED": "DISABLED")); mNhNs.getParam("object_detection/mc_bag", mObjDetBagsEnable); - NODELET_INFO_STREAM(" * Detect bags\t\t\t-> " << (mObjDetBagsEnable ? "ENABLED" : "DISABLED")); + NODELET_INFO_STREAM(" * Detect bags\t\t\t-> " << (mObjDetBagsEnable ? "ENABLED": "DISABLED")); mNhNs.getParam("object_detection/mc_animal", mObjDetAnimalsEnable); - NODELET_INFO_STREAM(" * Detect animals\t\t-> " << (mObjDetAnimalsEnable ? "ENABLED" : "DISABLED")); + NODELET_INFO_STREAM(" * Detect animals\t\t-> " << (mObjDetAnimalsEnable ? "ENABLED": "DISABLED")); mNhNs.getParam("object_detection/mc_electronics", mObjDetElectronicsEnable); - NODELET_INFO_STREAM(" * Detect electronics\t\t-> " << (mObjDetElectronicsEnable ? "ENABLED" : "DISABLED")); + NODELET_INFO_STREAM(" * Detect electronics\t\t-> " << (mObjDetElectronicsEnable ? "ENABLED": "DISABLED")); mNhNs.getParam("object_detection/mc_fruit_vegetable", mObjDetFruitsEnable); - NODELET_INFO_STREAM(" * Detect fruit and vegetables\t-> " << (mObjDetFruitsEnable ? "ENABLED" : "DISABLED")); + NODELET_INFO_STREAM(" * Detect fruit and vegetables\t-> " << (mObjDetFruitsEnable ? "ENABLED": "DISABLED")); mNhNs.getParam("object_detection/mc_sport", mObjDetSportsEnable); - NODELET_INFO_STREAM(" * Detect sport-related objects\t-> " << (mObjDetSportsEnable ? "ENABLED" : "DISABLED")); + NODELET_INFO_STREAM(" * Detect sport-related objects\t-> " << (mObjDetSportsEnable ? "ENABLED": "DISABLED")); } } // <---- Object Detection @@ -865,7 +898,7 @@ void ZEDWrapperNodelet::readParameters() // ----> Sensors if (camera_model != "zed") { mNhNs.getParam("sensors/sensors_timestamp_sync", mSensTimestampSync); - NODELET_INFO_STREAM(" * Sensors timestamp sync\t-> " << (mSensTimestampSync ? "ENABLED" : "DISABLED")); + NODELET_INFO_STREAM(" * Sensors timestamp sync\t-> " << (mSensTimestampSync ? "ENABLED": "DISABLED")); mNhNs.getParam("sensors/max_pub_rate", mSensPubRate); if (camera_model == "zedm") { if (mSensPubRate > 800.) @@ -961,12 +994,12 @@ void ZEDWrapperNodelet::readParameters() // ----> TF broadcasting if(!mDepthDisabled) { mNhNs.param("pos_tracking/publish_tf", mPublishTf, true); - NODELET_INFO_STREAM(" * Broadcast odometry TF\t-> " << (mPublishTf ? "ENABLED" : "DISABLED")); + NODELET_INFO_STREAM(" * Broadcast odometry TF\t-> " << (mPublishTf ? "ENABLED": "DISABLED")); mNhNs.param("pos_tracking/publish_map_tf", mPublishMapTf, true); NODELET_INFO_STREAM(" * Broadcast map pose TF\t-> " - << (mPublishTf ? (mPublishMapTf ? "ENABLED" : "DISABLED") : "DISABLED")); + << (mPublishTf ? (mPublishMapTf ? "ENABLED": "DISABLED"): "DISABLED")); mNhNs.param("sensors/publish_imu_tf", mPublishImuTf, true); - NODELET_INFO_STREAM(" * Broadcast IMU pose TF\t-> " << (mPublishImuTf ? "ENABLED" : "DISABLED")); + NODELET_INFO_STREAM(" * Broadcast IMU pose TF\t-> " << (mPublishImuTf ? "ENABLED": "DISABLED")); } // <---- TF broadcasting @@ -985,7 +1018,7 @@ void ZEDWrapperNodelet::readParameters() mNhNs.getParam("gamma", mCamGamma); NODELET_INFO_STREAM(" * [DYN] gamma\t\t\t-> " << mCamGamma); mNhNs.getParam("auto_exposure_gain", mCamAutoExposure); - NODELET_INFO_STREAM(" * [DYN] auto_exposure_gain\t-> " << (mCamAutoExposure ? "ENABLED" : "DISABLED")); + NODELET_INFO_STREAM(" * [DYN] auto_exposure_gain\t-> " << (mCamAutoExposure ? "ENABLED": "DISABLED")); mNhNs.getParam("gain", mCamGain); mNhNs.getParam("exposure", mCamExposure); if (!mCamAutoExposure) { @@ -993,7 +1026,7 @@ void ZEDWrapperNodelet::readParameters() NODELET_INFO_STREAM(" * [DYN] exposure\t\t-> " << mCamExposure); } mNhNs.getParam("auto_whitebalance", mCamAutoWB); - NODELET_INFO_STREAM(" * [DYN] auto_whitebalance\t-> " << (mCamAutoWB ? "ENABLED" : "DISABLED")); + NODELET_INFO_STREAM(" * [DYN] auto_whitebalance\t-> " << (mCamAutoWB ? "ENABLED": "DISABLED")); mNhNs.getParam("whitebalance_temperature", mCamWB); if (!mCamAutoWB) { NODELET_INFO_STREAM(" * [DYN] whitebalance_temperature\t\t-> " << mCamWB); @@ -1023,54 +1056,6 @@ void ZEDWrapperNodelet::readParameters() // <---- Dynamic } -void ZEDWrapperNodelet::checkResol() -{ - switch (mCamResol) { - case sl::RESOLUTION::HD2K: - if (mZedUserCamModel == sl::MODEL::ZED_X || mZedUserCamModel == sl::MODEL::ZED_XM) { - NODELET_WARN("Selected a not valid resolution. Modified to 'AUTO'"); - mCamResol = sl::RESOLUTION::AUTO; - } - break; - - case sl::RESOLUTION::HD1200: - if (mZedUserCamModel != sl::MODEL::ZED_X && mZedUserCamModel != sl::MODEL::ZED_XM) { - NODELET_WARN("Selected a not valid resolution. Modified to 'AUTO'"); - mCamResol = sl::RESOLUTION::AUTO; - } - break; - - case sl::RESOLUTION::HD1080: - break; - - case sl::RESOLUTION::HD720 : - if (mZedUserCamModel == sl::MODEL::ZED_X || mZedUserCamModel == sl::MODEL::ZED_XM) { - NODELET_WARN("Selected a not valid resolution. Modified to 'AUTO'"); - mCamResol = sl::RESOLUTION::AUTO; - } - break; - - case sl::RESOLUTION::SVGA: - if (mZedUserCamModel != sl::MODEL::ZED_X && mZedUserCamModel != sl::MODEL::ZED_XM) { - NODELET_WARN("Selected a not valid resolution. Modified to 'AUTO'"); - mCamResol = sl::RESOLUTION::AUTO; - } - break; - - case sl::RESOLUTION::VGA: - if (mZedUserCamModel == sl::MODEL::ZED_X || mZedUserCamModel == sl::MODEL::ZED_XM) { - NODELET_WARN("Selected a not valid resolution. Modified to 'AUTO'"); - mCamResol = sl::RESOLUTION::AUTO; - } - break; - - default: - NODELET_WARN("Selected a not valid resolution. Modified to 'AUTO'"); - mCamResol = sl::RESOLUTION::AUTO; - break; - } -} - void ZEDWrapperNodelet::checkResolFps() { switch (mCamResol) { @@ -2459,7 +2444,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"); @@ -2491,7 +2476,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 { mTriggerAutoWB = false; @@ -2623,7 +2608,7 @@ void ZEDWrapperNodelet::pubVideoDepth() grab_ts = mat_conf.timestamp; } // <---- Retrieve all required image data - +mSvoMode // ----> Data ROS timestamp ros::Time stamp = sl_tools::slTime2Ros(grab_ts); if (mSvoMode) { @@ -3260,7 +3245,7 @@ void ZEDWrapperNodelet::device_poll_thread_func() // Get the parameters of the ZED images mCamWidth = mZed.getCameraInformation().camera_configuration.resolution.width; mCamHeight = mZed.getCameraInformation().camera_configuration.resolution.height; - NODELET_DEBUG_STREAM("Original Camera grab frame size -> " << mCamWidth << "x" << mCamHeight); + NODELET_DEBUG_STREAM("Original Camera grab frame size: " << mCamWidth << "x" << mCamHeight); int pub_w, pub_h; pub_w = static_cast(std::round(mCamWidth / mCustomDownscaleFactor)); pub_h = static_cast(std::round(mCamHeight / mCustomDownscaleFactor)); @@ -3275,7 +3260,7 @@ void ZEDWrapperNodelet::device_poll_thread_func() pub_h = mCamHeight; } mMatResol = sl::Resolution(pub_w, pub_h); - NODELET_DEBUG_STREAM("Publishing frame size -> " << mMatResol.width << "x" << mMatResol.height); + NODELET_DEBUG_STREAM("Publishing frame size: " << mMatResol.width << "x" << mMatResol.height); // Create and fill the camera information messages fillCamInfo(mZed, mLeftCamInfoMsg, mRightCamInfoMsg, mLeftCamOptFrameId, mRightCamOptFrameId); @@ -3338,7 +3323,7 @@ void ZEDWrapperNodelet::device_poll_thread_func() if (mGrabActive) { std::lock_guard lock(mPosTrkMutex); - // Note: ones tracking is started it is never stopped anymore to not lose tracking information + // Note: once tracking is started it is never stopped anymore to not lose tracking information bool computeTracking = !mDepthDisabled && (mPosTrackingEnabled || mPosTrackingActivated || mMappingEnabled || mObjDetEnabled || (mComputeDepth & mDepthStabilization) || poseSubnumber > 0 || poseCovSubnumber > 0 || odomSubnumber > 0 || pathSubNumber > 0); @@ -3390,6 +3375,14 @@ void ZEDWrapperNodelet::device_poll_thread_func() if(mGrabStatus!=sl::ERROR_CODE::CAMERA_REBOOTING) { std::this_thread::sleep_for(std::chrono::milliseconds(50)); continue; + } else if (mSvoMode && mGrabStatus == sl::ERROR_CODE::END_OF_SVOFILE_REACHED) { + NODELET_WARN("SVO reached the end. The node will be stopped."); + mZed.close(); + exit(EXIT_SUCCESS); + } else if (!mRemoteStreamAddr.empty()) { + NODELET_ERROR("Remote stream problem. The node will be stopped."); + mZed.close(); + exit(EXIT_FAILURE); } else { if ((ros::Time::now() - mPrevFrameTimestamp).toSec() > 5 && !mSvoMode) { mCloseZedMutex.lock(); @@ -3424,9 +3417,13 @@ void ZEDWrapperNodelet::device_poll_thread_func() mPosTrackingActivated = false; - computeTracking = mPosTrackingEnabled || mDepthStabilization || poseSubnumber > 0 || poseCovSubnumber > 0 || odomSubnumber > 0; + // Note: once tracking is started it is never stopped anymore to not lose tracking information + bool computeTracking = !mDepthDisabled && + (mPosTrackingEnabled || mPosTrackingActivated || mMappingEnabled || mObjDetEnabled || (mComputeDepth & mDepthStabilization) || + poseSubnumber > 0 || poseCovSubnumber > 0 || odomSubnumber > 0 || pathSubNumber > 0); - if (computeTracking) { // Start the tracking + // Start the tracking? + if ((computeTracking) && !mPosTrackingActivated && !mDepthDisabled) { start_pos_tracking(); } } @@ -4123,7 +4120,7 @@ bool ZEDWrapperNodelet::on_start_svo_recording(zed_interfaces::start_svo_recordi err = mZed.enableRecording(recParams); if (err == sl::ERROR_CODE::SVO_UNSUPPORTED_COMPRESSION) { - recParams.compression_mode = mSvoComprMode == sl::SVO_COMPRESSION_MODE::H265 ? sl::SVO_COMPRESSION_MODE::H264 : sl::SVO_COMPRESSION_MODE::H265; + recParams.compression_mode = mSvoComprMode == sl::SVO_COMPRESSION_MODE::H265 ? sl::SVO_COMPRESSION_MODE::H264: sl::SVO_COMPRESSION_MODE::H265; NODELET_WARN_STREAM("The chosen " << sl::toString(mSvoComprMode).c_str() << "mode is not available. Trying " << sl::toString(recParams.compression_mode).c_str()); @@ -4267,7 +4264,7 @@ bool ZEDWrapperNodelet::on_set_led_status(zed_interfaces::set_led_status::Reques return false; } - mZed.setCameraSettings(sl::VIDEO_SETTINGS::LED_STATUS, req.led_enabled ? 1 : 0); + mZed.setCameraSettings(sl::VIDEO_SETTINGS::LED_STATUS, req.led_enabled ? 1: 0); return true; } @@ -4288,7 +4285,7 @@ bool ZEDWrapperNodelet::on_toggle_led(zed_interfaces::toggle_led::Request& req, return false; } - int new_status = status == 0 ? 1 : 0; + int new_status = status == 0 ? 1: 0; err = mZed.setCameraSettings(sl::VIDEO_SETTINGS::LED_STATUS, new_status); if(err!=sl::ERROR_CODE::SUCCESS) { @@ -4421,23 +4418,23 @@ bool ZEDWrapperNodelet::on_start_object_detection(zed_interfaces::start_object_d NODELET_INFO_STREAM(" * Detection max range\t\t-> " << mObjDetMaxRange); NODELET_INFO_STREAM(" * Object min. confidence\t-> " << mObjDetConfidence); - NODELET_INFO_STREAM(" * Object tracking\t\t-> " << (mObjDetTracking ? "ENABLED" : "DISABLED")); + NODELET_INFO_STREAM(" * Object tracking\t\t-> " << (mObjDetTracking ? "ENABLED": "DISABLED")); NODELET_INFO_STREAM(" * Detection model\t\t-> " << sl::toString(mObjDetModel)); mNhNs.getParam("object_detection/mc_people", mObjDetPeopleEnable); - NODELET_INFO_STREAM(" * Detect people\t\t-> " << (mObjDetPeopleEnable ? "ENABLED" : "DISABLED")); + NODELET_INFO_STREAM(" * Detect people\t\t-> " << (mObjDetPeopleEnable ? "ENABLED": "DISABLED")); mNhNs.getParam("object_detection/mc_vehicle", mObjDetVehiclesEnable); - NODELET_INFO_STREAM(" * Detect vehicles\t\t-> " << (mObjDetVehiclesEnable ? "ENABLED" : "DISABLED")); + NODELET_INFO_STREAM(" * Detect vehicles\t\t-> " << (mObjDetVehiclesEnable ? "ENABLED": "DISABLED")); mNhNs.getParam("object_detection/mc_bag", mObjDetBagsEnable); - NODELET_INFO_STREAM(" * Detect bags\t\t\t-> " << (mObjDetBagsEnable ? "ENABLED" : "DISABLED")); + NODELET_INFO_STREAM(" * Detect bags\t\t\t-> " << (mObjDetBagsEnable ? "ENABLED": "DISABLED")); mNhNs.getParam("object_detection/mc_animal", mObjDetAnimalsEnable); - NODELET_INFO_STREAM(" * Detect animals\t\t-> " << (mObjDetAnimalsEnable ? "ENABLED" : "DISABLED")); + NODELET_INFO_STREAM(" * Detect animals\t\t-> " << (mObjDetAnimalsEnable ? "ENABLED": "DISABLED")); mNhNs.getParam("object_detection/mc_electronics", mObjDetElectronicsEnable); - NODELET_INFO_STREAM(" * Detect electronics\t\t-> " << (mObjDetElectronicsEnable ? "ENABLED" : "DISABLED")); + NODELET_INFO_STREAM(" * Detect electronics\t\t-> " << (mObjDetElectronicsEnable ? "ENABLED": "DISABLED")); mNhNs.getParam("object_detection/mc_fruit_vegetable", mObjDetFruitsEnable); - NODELET_INFO_STREAM(" * Detect fruit and vegetables\t-> " << (mObjDetFruitsEnable ? "ENABLED" : "DISABLED")); + NODELET_INFO_STREAM(" * Detect fruit and vegetables\t-> " << (mObjDetFruitsEnable ? "ENABLED": "DISABLED")); mNhNs.getParam("object_detection/mc_sport", mObjDetSportsEnable); - NODELET_INFO_STREAM(" * Detect sport-related objects\t-> " << (mObjDetSportsEnable ? "ENABLED" : "DISABLED")); + NODELET_INFO_STREAM(" * Detect sport-related objects\t-> " << (mObjDetSportsEnable ? "ENABLED": "DISABLED")); mObjDetRunning = false; mObjDetEnabled = true; @@ -4503,7 +4500,7 @@ void ZEDWrapperNodelet::processDetectedObjects(ros::Time t) objMsg->objects.resize(objCount); size_t idx = 0; - for (auto data : objects.object_list) { + for (auto data: objects.object_list) { objMsg->objects[idx].label = sl::toString(data.label).c_str(); objMsg->objects[idx].sublabel = sl::toString(data.sublabel).c_str(); objMsg->objects[idx].label_id = data.raw_label; diff --git a/zed_wrapper/params/zed.yaml b/zed_wrapper/params/zed.yaml index 06cc54bd..0dada5cd 100644 --- a/zed_wrapper/params/zed.yaml +++ b/zed_wrapper/params/zed.yaml @@ -4,7 +4,7 @@ general: camera_model: 'zed' - resolution: 3 # '0': HD2K, '1': HD1080, '3': HD720, '5': VGA, '6': AUTO + grab_resolution: 'HD720' # 'HD2K', 'HD1080', 'HD720', 'VGA', 'AUTO' grab_frame_rate: 30 # Frequency of frame grabbing for internal SDK operations depth: diff --git a/zed_wrapper/params/zed2.yaml b/zed_wrapper/params/zed2.yaml index e467b390..d4d1e83b 100644 --- a/zed_wrapper/params/zed2.yaml +++ b/zed_wrapper/params/zed2.yaml @@ -4,7 +4,7 @@ general: camera_model: 'zed2' - resolution: 3 # '0': HD2K, '1': HD1080, '3': HD720, '5': VGA, '6': AUTO + grab_resolution: 'HD720' # 'HD2K', 'HD1080', 'HD720', 'VGA', 'AUTO' grab_frame_rate: 30 # Frequency of frame grabbing for internal SDK operations depth: diff --git a/zed_wrapper/params/zed2i.yaml b/zed_wrapper/params/zed2i.yaml index b50091ca..08510fa4 100644 --- a/zed_wrapper/params/zed2i.yaml +++ b/zed_wrapper/params/zed2i.yaml @@ -4,7 +4,7 @@ general: camera_model: 'zed2i' - resolution: 3 # '0': HD2K, '1': HD1080, '3': HD720, '5': VGA, '6': AUTO + grab_resolution: 'HD720' # 'HD2K', 'HD1080', 'HD720', 'VGA', 'AUTO' grab_frame_rate: 30 # Frequency of frame grabbing for internal SDK operations depth: diff --git a/zed_wrapper/params/zedm.yaml b/zed_wrapper/params/zedm.yaml index 48102e8a..a6fde8fa 100644 --- a/zed_wrapper/params/zedm.yaml +++ b/zed_wrapper/params/zedm.yaml @@ -4,7 +4,7 @@ general: camera_model: 'zedm' - resolution: 3 # '0': HD2K, '1': HD1080, '3': HD720, '5': VGA, '6': AUTO + grab_resolution: 'HD720' # 'HD2K', 'HD1080', 'HD720', 'VGA', 'AUTO' grab_frame_rate: 30 # Frequency of frame grabbing for internal SDK operations depth: diff --git a/zed_wrapper/params/zedx.yaml b/zed_wrapper/params/zedx.yaml index 7a2d5794..e2129585 100644 --- a/zed_wrapper/params/zedx.yaml +++ b/zed_wrapper/params/zedx.yaml @@ -4,8 +4,8 @@ general: camera_model: 'zedx' - resolution: 1 # '1': HD1080 - '2': HD1200 - '4': SVGA - '6': AUTO - grab_frame_rate: 60 # 120, 60, 30 + grab_resolution: 'HD1200' # HD1080, HD1200, SVGA, AUTO + grab_frame_rate: 60 # 120, 60, 30, 15 depth: min_depth: 0.2 # Min: 0.2, Max: 3.0 - Default 0.7 - Note: reducing this value wil require more computational power and GPU memory diff --git a/zed_wrapper/params/zedxm.yaml b/zed_wrapper/params/zedxm.yaml index b3d18f5a..dd01ce84 100644 --- a/zed_wrapper/params/zedxm.yaml +++ b/zed_wrapper/params/zedxm.yaml @@ -4,8 +4,8 @@ general: camera_model: 'zedxm' - resolution: 2 # '1': HD1080 - '2': HD1200 - '4': SVGA - '6': AUTO - grab_frame_rate: 60 # 120, 60, 30 + grab_resolution: 'HD1200' # HD1080, HD1200, SVGA, AUTO + grab_frame_rate: 60 # 120, 60, 30, 15 depth: min_depth: 0.1 # Min: 0.1, Max: 3.0 - Default 0.7 - Note: reducing this value wil require more computational power and GPU memory From 81e9307d3bc544f85c491c7e19662a4d5500e796 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Tue, 12 Sep 2023 16:07:49 +0200 Subject: [PATCH 167/199] SVO Realtime parameter and other fixes --- CHANGELOG.rst | 2 + .../include/zed_wrapper_nodelet.hpp | 3 +- .../zed_nodelet/src/zed_wrapper_nodelet.cpp | 54 +++++++++++-------- zed_wrapper/params/common.yaml | 31 +++++------ zed_wrapper/params/zed2.yaml | 4 +- zed_wrapper/params/zedm.yaml | 4 +- zed_wrapper/params/zedx.yaml | 8 +-- zed_wrapper/params/zedxm.yaml | 8 +-- 8 files changed, 63 insertions(+), 51 deletions(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 76fc0563..d5e9a4e3 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -4,6 +4,8 @@ CHANGELOG 2023-09-12 ---------- - Change the parameter 'general/resolution' to 'general/grab_resolution' and replace numeric values with strings +- Add 'general.svo_realtime' parameter +- Change 'general/verbose' to 'general/sdk_verbose' 2023-09-10 ---------- 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 99b00679..d3b4fd76 100644 --- a/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp +++ b/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp @@ -555,12 +555,13 @@ class ZEDWrapperNodelet : public nodelet::Nodelet { std::string mAreaMemDbPath; bool mSaveAreaMapOnClosing = true; std::string mSvoFilepath; + bool mSvoRealtime = true; std::string mRemoteStreamAddr; bool mSensTimestampSync; double mSensPubRate = 400.0; double mPathPubRate; int mPathMaxCount; - int mVerbose; + int mSdkVerbose=1; bool mSvoMode = false; double mCamMinDepth; double mCamMaxDepth; 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 64228443..93a3cffb 100644 --- a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp @@ -174,15 +174,18 @@ void ZEDWrapperNodelet::onInit() mTfBuffer.reset(new tf2_ros::Buffer); mTfListener.reset(new tf2_ros::TransformListener(*mTfBuffer)); + mZedParams.coordinate_system = sl::COORDINATE_SYSTEM::RIGHT_HANDED_Z_UP_X_FWD; + NODELET_INFO_STREAM(" * Camera coordinate system\t-> " << sl::toString(mZedParams.coordinate_system)); + // Try to initialize the ZED + std::stringstream ss; // Input mode info for logging if (!mSvoFilepath.empty() || !mRemoteStreamAddr.empty()) { if (!mSvoFilepath.empty()) { mZedParams.input.setFromSVOFile(mSvoFilepath.c_str()); - mZedParams.svo_real_time_mode = true; - - NODELET_INFO_STREAM( "Node input: SVO - " << mSvoFilepath.c_str()); + mZedParams.svo_real_time_mode = mSvoRealtime; - // TODO ADD PARAMETER FOR SVO REAL TIME + // Input mode info for logging + ss << "SVO - " << mSvoFilepath.c_str(); } else if (!mRemoteStreamAddr.empty()) { std::vector configStream = sl_tools::split_string(mRemoteStreamAddr, ':'); sl::String ip = sl::String(configStream.at(0).c_str()); @@ -193,7 +196,8 @@ void ZEDWrapperNodelet::onInit() mZedParams.input.setFromStream(ip); } - NODELET_INFO_STREAM( "Node input: NETWORK STREAM - " << mRemoteStreamAddr.c_str()); + // Input mode info for logging + ss << "NETWORK STREAM - " << mRemoteStreamAddr.c_str(); } mSvoMode = true; @@ -204,20 +208,21 @@ void ZEDWrapperNodelet::onInit() if (mZedSerialNumber == 0) { mZedParams.input.setFromCameraID(mZedId); - NODELET_INFO_STREAM( "Node input: LIVE CAMERA with ID " << mZedId); + + // Input mode info for logging + ss << "LIVE CAMERA with ID " << mZedId; } else { mZedParams.input.setFromSerialNumber(mZedSerialNumber); - NODELET_INFO_STREAM( "Node input: LIVE CAMERA with SN " << mZedSerialNumber); + + // Input mode info for logging + ss << "LIVE CAMERA with SN " << mZedSerialNumber; } } - mZedParams.coordinate_system = sl::COORDINATE_SYSTEM::RIGHT_HANDED_Z_UP_X_FWD; - NODELET_INFO_STREAM(" * Camera coordinate system\t-> " << sl::toString(mZedParams.coordinate_system)); - mZedParams.async_grab_camera_recovery = true; mZedParams.coordinate_units = sl::UNIT::METER; mZedParams.depth_mode = static_cast(mDepthMode); - mZedParams.sdk_verbose = mVerbose; + mZedParams.sdk_verbose = mSdkVerbose; mZedParams.sdk_gpu_id = mGpuId; mZedParams.depth_stabilization = mDepthStabilization; mZedParams.camera_image_flip = mCameraFlip; @@ -232,7 +237,7 @@ void ZEDWrapperNodelet::onInit() mConnStatus = sl::ERROR_CODE::CAMERA_NOT_DETECTED; - NODELET_INFO_STREAM(" *** Opening " << sl::toString(mZedUserCamModel) << "..."); + NODELET_INFO_STREAM(" *** Opening " << sl::toString(mZedUserCamModel) << " - " << ss.str().c_str() << " ***"); while (mConnStatus != sl::ERROR_CODE::SUCCESS) { mConnStatus = mZed.open(mZedParams); NODELET_INFO_STREAM("ZED connection: " << sl::toString(mConnStatus)); @@ -317,8 +322,6 @@ void ZEDWrapperNodelet::onInit() mSensFwVersion = mZed.getCameraInformation().sensors_configuration.firmware_version; NODELET_INFO_STREAM(" * Sensors FW Version: " << mSensFwVersion); } - } else { - NODELET_INFO_STREAM(" * Input type\t-> " << sl::toString(mZed.getCameraInformation().input_type).c_str()); } // Set the IMU topic names using real camera model @@ -683,8 +686,8 @@ void ZEDWrapperNodelet::readParameters() NODELET_INFO_STREAM(" * Gpu ID\t\t\t-> " << mGpuId); mNhNs.getParam("general/zed_id", mZedId); NODELET_INFO_STREAM(" * Camera ID\t\t\t-> " << mZedId); - mNhNs.getParam("general/verbose", mVerbose); - NODELET_INFO_STREAM(" * Verbose\t\t\t-> " << (mVerbose ? "ENABLED": "DISABLED")); + mNhNs.getParam("general/sdk_verbose", mSdkVerbose); + NODELET_INFO_STREAM(" * SDK Verbose level\t\t-> " << mSdkVerbose ); std::string flip_str; mNhNs.getParam("general/camera_flip", flip_str); if(flip_str=="ON") { @@ -742,10 +745,10 @@ void ZEDWrapperNodelet::readParameters() if (mDepthMode == sl::DEPTH_MODE::NONE) { mDepthDisabled = true; mDepthStabilization = 0; - NODELET_INFO_STREAM(" * Depth mode\t\t\t> " << sl::toString(mDepthMode).c_str() << " - DEPTH DISABLED"); + NODELET_INFO_STREAM(" * Depth mode\t\t\t-> " << sl::toString(mDepthMode).c_str() << " - DEPTH DISABLED"); } else { mDepthDisabled = false; - NODELET_INFO_STREAM(" * Depth mode\t\t\t--> " << sl::toString( + NODELET_INFO_STREAM(" * Depth mode\t\t\t-> " << sl::toString( mDepthMode).c_str() << " [" << static_cast(mDepthMode) << "]"); } @@ -780,7 +783,7 @@ void ZEDWrapperNodelet::readParameters() "'). Using default value."); mPosTrkMode = sl::POSITIONAL_TRACKING_MODE::QUALITY; } - NODELET_INFO_STREAM( " * Positional tracking mode\t:" << sl::toString(mPosTrkMode).c_str()); + NODELET_INFO_STREAM( " * Positional tracking mode\t-> " << sl::toString(mPosTrkMode).c_str()); mNhNs.getParam("pos_tracking/set_gravity_as_origin", mSetGravityAsOrigin); NODELET_INFO_STREAM(" * Set gravity as origin\t-> " << (mSetGravityAsOrigin ? "ENABLED": "DISABLED")); @@ -800,7 +803,7 @@ void ZEDWrapperNodelet::readParameters() mAreaMemDbPath = sl_tools::resolveFilePath(mAreaMemDbPath); NODELET_INFO_STREAM(" * Odometry DB path\t\t-> " << mAreaMemDbPath.c_str()); - mNhNs.param("pos_tracking/save_area_memory_db_on_exit", mSaveAreaMapOnClosing, false); + mNhNs.param("pos_tracking/save_area_memory_db_on_exit", mSaveAreaMapOnClosing,AUTO_WB false); NODELET_INFO_STREAM(" * Save Area Memory on closing\t-> " << (mSaveAreaMapOnClosing ? "ENABLED": "DISABLED")); mNhNs.param("pos_tracking/area_memory", mAreaMemory, false); NODELET_INFO_STREAM(" * Area Memory\t\t\t-> " << (mAreaMemory ? "ENABLED": "DISABLED")); @@ -812,10 +815,10 @@ void ZEDWrapperNodelet::readParameters() NODELET_INFO_STREAM(" * Init Odometry with first valid pose data: " << (mInitOdomWithPose ? "ENABLED": "DISABLED")); mNhNs.param("pos_tracking/two_d_mode", mTwoDMode, false); - NODELET_INFO_STREAM(" * Two D mode\t\t\t-> " << (mTwoDMode ? "ENABLED": "DISABLED")); - mNhNs.param("pos_tracking/fixed_z_value", mFixedZValue, 0.0); + NODELET_INFO_STREAM(" * Force 2D mode\t\t\t-> " << (mTwoDMode ? "ENABLED": "DISABLED")); if (mTwoDMode) { + mNhNs.param("pos_tracking/fixed_z_value", mFixedZValue, 0.0); NODELET_INFO_STREAM(" * Fixed Z value\t\t-> " << mFixedZValue); } } @@ -921,6 +924,11 @@ void ZEDWrapperNodelet::readParameters() mSvoFilepath = sl_tools::resolveFilePath(mSvoFilepath); NODELET_INFO_STREAM(" * SVO input file: \t\t-> " << mSvoFilepath.c_str()); + if(!mSvoFilepath.empty()) { + mNhNs.getParam("general/svo_realtime", mSvoRealtime); + NODELET_INFO_STREAM(" * SVO realtime mode\t\t-> " << (mSvoRealtime ? "ENABLED": "DISABLED")); + } + int svo_compr = 0; mNhNs.getParam("general/svo_compression", svo_compr); @@ -2608,7 +2616,7 @@ void ZEDWrapperNodelet::pubVideoDepth() grab_ts = mat_conf.timestamp; } // <---- Retrieve all required image data -mSvoMode + // ----> Data ROS timestamp ros::Time stamp = sl_tools::slTime2Ros(grab_ts); if (mSvoMode) { diff --git a/zed_wrapper/params/common.yaml b/zed_wrapper/params/common.yaml index eef54dd6..0dcfd9be 100644 --- a/zed_wrapper/params/common.yaml +++ b/zed_wrapper/params/common.yaml @@ -3,20 +3,20 @@ --- # Dynamic parameters cannot have a namespace -brightness: 4 # Dynamic -contrast: 4 # Dynamic -hue: 0 # Dynamic -saturation: 4 # Dynamic -sharpness: 4 # Dynamic -gamma: 8 # Dynamic - Requires SDK >=v3.1 -auto_exposure_gain: true # Dynamic -gain: 100 # Dynamic - works only if `auto_exposure_gain` is false -exposure: 100 # Dynamic - works only if `auto_exposure_gain` is false -auto_whitebalance: true # Dynamic -whitebalance_temperature: 42 # Dynamic - works only if `auto_whitebalance` is false -depth_confidence: 30 # Dynamic -depth_texture_conf: 100 # Dynamic -point_cloud_freq: 10.0 # Dynamic - frequency of the pointcloud publishing (equal or less to `grab_frame_rate` value) +brightness: 4 # Dynamic +contrast: 4 # Dynamic +hue: 0 # Dynamic +saturation: 4 # Dynamic +sharpness: 4 # Dynamic +gamma: 8 # Dynamic - Requires SDK >=v3.1 +auto_exposure_gain: true # Dynamic +gain: 100 # Dynamic - works only if `auto_exposure_gain` is false +exposure: 100 # Dynamic - works only if `auto_exposure_gain` is false +auto_whitebalance: true # Dynamic +whitebalance_temperature: 42 # Dynamic - works only if `auto_whitebalance` is false +depth_confidence: 30 # Dynamic +depth_texture_conf: 100 # Dynamic +point_cloud_freq: 10.0 # Dynamic - frequency of the pointcloud publishing (equal or less to `grab_frame_rate` value) general: camera_name: zed # A name for the camera (can be different from camera model and node name and can be overwritten by the launch file) @@ -24,13 +24,14 @@ general: serial_number: 0 gpu_id: -1 base_frame: 'base_link' # must be equal to the frame_id used in the URDF file - verbose: 1 # Set verbose level of the ZED SDK + sdk_verbose: 1 # Set verbose level of the ZED SDK svo_compression: 2 # `0`: LOSSLESS, `1`: AVCHD, `2`: HEVC self_calib: true # enable/disable self calibration at starting camera_flip: 'AUTO' # camera flip mode: 'OFF', 'ON', 'AUTO' pub_resolution: 'CUSTOM' # The resolution used for output. 'NATIVE' to use the same `general.grab_resolution` - `CUSTOM` to apply the `general.pub_downscale_factor` downscale factory to reduce bandwidth in transmission pub_downscale_factor: 2.0 # rescale factor used to rescale image before publishing when 'pub_resolution' is 'CUSTOM' pub_frame_rate: 15.0 # frequency of publishing of video and depth data (see SDK API "InitParameters::grab_compute_capping_fps") + svo_realtime: true # if true the input SVO will be played trying to respect the original framerate eventually skipping frames, otherwise every frame will be processed respecting the `pub_frame_rate` setting #video: diff --git a/zed_wrapper/params/zed2.yaml b/zed_wrapper/params/zed2.yaml index d4d1e83b..6f3cfeda 100644 --- a/zed_wrapper/params/zed2.yaml +++ b/zed_wrapper/params/zed2.yaml @@ -8,6 +8,6 @@ general: grab_frame_rate: 30 # Frequency of frame grabbing for internal SDK operations depth: - min_depth: 0.3 # Min: 0.2, Max: 3.0 - Default 0.7 - Note: reducing this value wil require more computational power and GPU memory - max_depth: 20.0 # Max: 40.0 + min_depth: 0.3 # Min: 0.2, Max: 3.0 - Default 0.7 - Note: reducing this value wil require more computational power and GPU memory + max_depth: 20.0 # Max: 40.0 \ No newline at end of file diff --git a/zed_wrapper/params/zedm.yaml b/zed_wrapper/params/zedm.yaml index a6fde8fa..ca3f66c2 100644 --- a/zed_wrapper/params/zedm.yaml +++ b/zed_wrapper/params/zedm.yaml @@ -8,5 +8,5 @@ general: grab_frame_rate: 30 # Frequency of frame grabbing for internal SDK operations depth: - min_depth: 0.1 # Min: 0.1, Max: 3.0 - Default 0.3 - Note: reducing this value wil require more computational power and GPU memory - max_depth: 10.0 # Max: 20.0 + min_depth: 0.1 # Min: 0.1, Max: 3.0 - Default 0.3 - Note: reducing this value wil require more computational power and GPU memory + max_depth: 10.0 # Max: 20.0 diff --git a/zed_wrapper/params/zedx.yaml b/zed_wrapper/params/zedx.yaml index e2129585..7d364165 100644 --- a/zed_wrapper/params/zedx.yaml +++ b/zed_wrapper/params/zedx.yaml @@ -4,10 +4,10 @@ general: camera_model: 'zedx' - grab_resolution: 'HD1200' # HD1080, HD1200, SVGA, AUTO - grab_frame_rate: 60 # 120, 60, 30, 15 + grab_resolution: 'HD1200' # HD1080, HD1200, SVGA, AUTO + grab_frame_rate: 60 # 120, 60, 30, 15 depth: - min_depth: 0.2 # Min: 0.2, Max: 3.0 - Default 0.7 - Note: reducing this value wil require more computational power and GPU memory - max_depth: 10.0 # Max: 40.0 + min_depth: 0.2 # Min: 0.2, Max: 3.0 - Default 0.7 - Note: reducing this value wil require more computational power and GPU memory + max_depth: 10.0 # Max: 40.0 diff --git a/zed_wrapper/params/zedxm.yaml b/zed_wrapper/params/zedxm.yaml index dd01ce84..dd931d84 100644 --- a/zed_wrapper/params/zedxm.yaml +++ b/zed_wrapper/params/zedxm.yaml @@ -4,9 +4,9 @@ general: camera_model: 'zedxm' - grab_resolution: 'HD1200' # HD1080, HD1200, SVGA, AUTO - grab_frame_rate: 60 # 120, 60, 30, 15 + grab_resolution: 'HD1200' # HD1080, HD1200, SVGA, AUTO + grab_frame_rate: 60 # 120, 60, 30, 15 depth: - min_depth: 0.1 # Min: 0.1, Max: 3.0 - Default 0.7 - Note: reducing this value wil require more computational power and GPU memory - max_depth: 10.0 # Max: 40.0 + min_depth: 0.1 # Min: 0.1, Max: 3.0 - Default 0.7 - Note: reducing this value wil require more computational power and GPU memory + max_depth: 10.0 # Max: 40.0 From 713071cf7b0884b5fa200c0cd3e87410c6705724 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Tue, 12 Sep 2023 16:56:34 +0200 Subject: [PATCH 168/199] - Add 'general/region_of_interest' parameter --- CHANGELOG.rst | 1 + zed_nodelets/src/tools/include/sl_tools.h | 13 ++ zed_nodelets/src/tools/src/sl_tools.cpp | 133 ++++++++++++++++++ .../include/zed_wrapper_nodelet.hpp | 8 ++ .../zed_nodelet/src/zed_wrapper_nodelet.cpp | 123 +++++++++++++++- zed_wrapper/params/common.yaml | 5 + 6 files changed, 276 insertions(+), 7 deletions(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index d5e9a4e3..4a10e200 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -6,6 +6,7 @@ CHANGELOG - Change the parameter 'general/resolution' to 'general/grab_resolution' and replace numeric values with strings - Add 'general.svo_realtime' parameter - Change 'general/verbose' to 'general/sdk_verbose' +- Add 'general/region_of_interest' parameter 2023-09-10 ---------- diff --git a/zed_nodelets/src/tools/include/sl_tools.h b/zed_nodelets/src/tools/include/sl_tools.h index a222da59..b0c38188 100644 --- a/zed_nodelets/src/tools/include/sl_tools.h +++ b/zed_nodelets/src/tools/include/sl_tools.h @@ -72,6 +72,19 @@ bool isZED2OrZED2i(sl::MODEL camModel); */ bool isZEDX(sl::MODEL camModel); +/*! \brief Creates an sl::Mat containing a ROI from a polygon + * \param poly the ROI polygon. Coordinates must be normalized from 0.0 to 1.0 + * \param out_roi the `sl::Mat` containing the ROI + */ +bool generateROI(const std::vector & poly, sl::Mat & out_roi); + +/*! \brief Parse a vector of vector of floats from a string. + * \param input + * \param error_return + * Syntax is [[1.0, 2.0], [3.3, 4.4, 5.5], ...] */ +std::vector> parseStringVector( + const std::string & input, std::string & error_return); + /*! \brief sl::Mat to ros message conversion * \param imgMsgPtr : the image topic message to publish * \param img : the image to publish diff --git a/zed_nodelets/src/tools/src/sl_tools.cpp b/zed_nodelets/src/tools/src/sl_tools.cpp index 2e47ef45..d913f6d4 100644 --- a/zed_nodelets/src/tools/src/sl_tools.cpp +++ b/zed_nodelets/src/tools/src/sl_tools.cpp @@ -351,6 +351,139 @@ std::vector split_string(const std::string& s, char seperator) return output; } +inline bool contains(std::vector & poly, sl::float2 test) +{ + int i, j; + bool c = false; + const int nvert = poly.size(); + for (i = 0, j = nvert - 1; i < nvert; j = i++) { + if ( + ((poly[i].y > test.y) != (poly[j].y > test.y)) && + (test.x < + (poly[j].x - poly[i].x) * (test.y - poly[i].y) / (poly[j].y - poly[i].y) + poly[i].x)) + { + c = !c; + } + } + return c; +} + +bool generateROI(const std::vector & poly, sl::Mat & out_roi) +{ + if (poly.size() < 3) { + out_roi = sl::Mat(); + return false; + } + + // Set each pixel to valid + // std::cerr << "Setting ROI mask to full valid" << std::endl; + out_roi.setTo(255, sl::MEM::CPU); + + // ----> De-normalize coordinates + size_t w = out_roi.getWidth(); + size_t h = out_roi.getHeight(); + + // std::cerr << "De-normalize coordinates" << std::endl; + // std::cerr << "Image resolution: " << w << "x" << h << std::endl; + std::vector poly_img; + size_t idx = 0; + for (auto & it : poly) { + sl::float2 pt; + pt.x = it.x * w; + pt.y = it.y * h; + + if (pt.x >= w) { + pt.x = (w - 1); + } + if (pt.y >= h) { + pt.y = (h - 1); + } + + poly_img.push_back(pt); + + ++idx; + } + // <---- De-normalize coordinates + + // ----> Unset ROI pixels outside the polygon + //std::cerr << "Unset ROI pixels outside the polygon" << std::endl; + //std::cerr << "Set mask" << std::endl; + for (int v = 0; v < h; v++) { + for (int u = 0; u < w; u++) { + if (!contains(poly_img, sl::float2(u, v))) { + out_roi.setValue(u, v, 0, sl::MEM::CPU); + } + } + } + // std::cerr << "Mask ready" << std::endl; + // std::cerr << "ROI resolution: " << w << "x" << h << std::endl; + // <---- Unset ROI pixels outside the polygon + + return true; +} + +std::vector> parseStringVector( + const std::string & input, std::string & error_return) +{ + std::vector> result; + + std::stringstream input_ss(input); + int depth = 0; + std::vector current_vector; + while (!!input_ss && !input_ss.eof()) { + switch (input_ss.peek()) { + case EOF: + break; + case '[': + depth++; + if (depth > 2) { + error_return = "Array depth greater than 2"; + return result; + } + input_ss.get(); + current_vector.clear(); + break; + case ']': + depth--; + if (depth < 0) { + error_return = "More close ] than open ["; + return result; + } + input_ss.get(); + if (depth == 1) { + result.push_back(current_vector); + } + break; + case ',': + case ' ': + case '\t': + input_ss.get(); + break; + default: // All other characters should be part of the numbers. + if (depth != 2) { + std::stringstream err_ss; + err_ss << "Numbers at depth other than 2. Char was '" << char(input_ss.peek()) << "'."; + error_return = err_ss.str(); + return result; + } + float value; + input_ss >> value; + if (!!input_ss) { + current_vector.push_back(value); + } + break; + } + } + + if (depth != 0) { + error_return = "Unterminated vector string."; + } else { + error_return = ""; + } + + return result; +} + CSmartMean::CSmartMean(int winSize) { mValCount = 0; 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 d3b4fd76..3be9f439 100644 --- a/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp +++ b/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp @@ -249,6 +249,13 @@ class ZEDWrapperNodelet : public nodelet::Nodelet { */ void checkResolFps(); + // ----> Region of Interest + std::string getRoiParam(std::string paramName, std::vector> & outVal); + std::string parseRoiPoly( + const std::vector> & in_poly, std::vector & out_poly); + void resetRoi(); + // <---- Region of Interest + /*! \brief Callback to handle dynamic reconfigure events in ROS */ void callback_dynamicReconf(zed_nodelets::ZedConfig& config, uint32_t level); @@ -562,6 +569,7 @@ class ZEDWrapperNodelet : public nodelet::Nodelet { double mPathPubRate; int mPathMaxCount; int mSdkVerbose=1; + std::vector> mRoiParam; bool mSvoMode = false; double mCamMinDepth; double mCamMaxDepth; 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 93a3cffb..0d7966a8 100644 --- a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp @@ -707,15 +707,16 @@ void ZEDWrapperNodelet::readParameters() if (tmp_sn > 0) { mZedSerialNumber = static_cast(tmp_sn); NODELET_INFO_STREAM(" * Serial number\t\t-> " << mZedSerialNumber); - } - // <---- General - + } + std::string parsed_str = getRoiParam("general/region_of_interest", mRoiParam); + NODELET_INFO_STREAM(" * Region of interest\t\t-> " << parsed_str.c_str()); + // <---- General - // ----> Video + // ----> Video //NODELET_INFO_STREAM("*** VIDEO PARAMETERS ***"); // Note: there are no "static" video parameters - // <---- Video + // <---- Video // -----> Depth NODELET_INFO_STREAM("*** DEPTH PARAMETERS ***"); @@ -802,8 +803,8 @@ void ZEDWrapperNodelet::readParameters() mNhNs.getParam("pos_tracking/area_memory_db_path", mAreaMemDbPath); mAreaMemDbPath = sl_tools::resolveFilePath(mAreaMemDbPath); NODELET_INFO_STREAM(" * Odometry DB path\t\t-> " << mAreaMemDbPath.c_str()); - - mNhNs.param("pos_tracking/save_area_memory_db_on_exit", mSaveAreaMapOnClosing,AUTO_WB false); + + mNhNs.param("pos_tracking/save_area_memory_db_on_exit", mSaveAreaMapOnClosing, false); NODELET_INFO_STREAM(" * Save Area Memory on closing\t-> " << (mSaveAreaMapOnClosing ? "ENABLED": "DISABLED")); mNhNs.param("pos_tracking/area_memory", mAreaMemory, false); NODELET_INFO_STREAM(" * Area Memory\t\t\t-> " << (mAreaMemory ? "ENABLED": "DISABLED")); @@ -1064,6 +1065,91 @@ void ZEDWrapperNodelet::readParameters() // <---- Dynamic } +std::string ZEDWrapperNodelet::getRoiParam(std::string paramName, std::vector> & outVal) +{ + outVal.clear(); + + std::string roi_param_str = ""; + + mNhNs.getParam("general/region_of_interest", roi_param_str); + + if (roi_param_str.empty()) { + return std::string(); + } + + std::string error; + outVal = sl_tools::parseStringVector(roi_param_str, error); + + if (error != "") { + NODELET_WARN_STREAM("Error parsing " << paramName << " parameter: " << error.c_str()); + NODELET_WARN_STREAM(" " << paramName << " string was " << roi_param_str.c_str()); + + outVal.clear(); + } + + return roi_param_str; +} + +std::string ZEDWrapperNodelet::parseRoiPoly( + const std::vector> & in_poly, + std::vector & out_poly) +{ + out_poly.clear(); + + std::string ss; + ss = "["; + + size_t poly_size = in_poly.size(); + + if (poly_size < 3) { + if (poly_size != 0) { + NODELET_WARN_STREAM("A vector with " << + poly_size << + " points is not enough to create a polygon to set a Region " + "of Interest."); + return std::string(); + } + } else { + for (size_t i; i < poly_size; ++i) { + if (in_poly[i].size() != 2) { + NODELET_WARN_STREAM("The component with index '" << i << + "' of the ROI vector " + "has not the correct size."); + out_poly.clear(); + return std::string(); + } else if (in_poly[i][0] < 0.0 || in_poly[i][1] < 0.0 || in_poly[i][0] > 1.0 || + in_poly[i][1] > 1.0) + { + NODELET_WARN_STREAM("The component with index '" << i << + "' of the ROI vector " + "is not a " + "valid normalized point: [" << + in_poly[i][0] << "," << in_poly[i][1] << + "]."); + out_poly.clear(); + return std::string(); + } else { + sl::float2 pt; + pt.x = in_poly[i][0]; + pt.y = in_poly[i][1]; + out_poly.push_back(pt); + ss += "["; + ss += std::to_string(pt.x); + ss += ","; + ss += std::to_string(pt.y); + ss += "]"; + } + + if (i != poly_size - 1) { + ss += ","; + } + } + } + ss += "]"; + + return ss; +} + void ZEDWrapperNodelet::checkResolFps() { switch (mCamResol) { @@ -3270,6 +3356,29 @@ void ZEDWrapperNodelet::device_poll_thread_func() mMatResol = sl::Resolution(pub_w, pub_h); NODELET_DEBUG_STREAM("Publishing frame size: " << mMatResol.width << "x" << mMatResol.height); + // ----> Set Region of Interest + if (!mRoiParam.empty()) { + NODELET_INFO("*** Setting ROI ***"); + sl::Resolution resol(mCamWidth, mCamHeight); + std::vector sl_poly; + std::string log_msg = parseRoiPoly(mRoiParam, sl_poly); + + // Create ROI mask + sl::Mat roi_mask(resol, sl::MAT_TYPE::U8_C1, sl::MEM::CPU); + + if (!sl_tools::generateROI(sl_poly, roi_mask)) { + NODELET_WARN(" * Error generating the region of interest image mask."); + } else { + sl::ERROR_CODE err = mZed.setRegionOfInterest(roi_mask); + if (err != sl::ERROR_CODE::SUCCESS) { + NODELET_WARN_STREAM(" * Error while setting ZED SDK region of interest: " << sl::toString(err).c_str()); + } else { + NODELET_INFO(" * Region of Interest correctly set."); + } + } + } + // <---- Set Region of Interest + // Create and fill the camera information messages fillCamInfo(mZed, mLeftCamInfoMsg, mRightCamInfoMsg, mLeftCamOptFrameId, mRightCamOptFrameId); fillCamInfo(mZed, mLeftCamInfoRawMsg, mRightCamInfoRawMsg, mLeftCamOptFrameId, mRightCamOptFrameId, true); diff --git a/zed_wrapper/params/common.yaml b/zed_wrapper/params/common.yaml index 0dcfd9be..e98bad15 100644 --- a/zed_wrapper/params/common.yaml +++ b/zed_wrapper/params/common.yaml @@ -32,6 +32,11 @@ general: pub_downscale_factor: 2.0 # rescale factor used to rescale image before publishing when 'pub_resolution' is 'CUSTOM' pub_frame_rate: 15.0 # frequency of publishing of video and depth data (see SDK API "InitParameters::grab_compute_capping_fps") svo_realtime: true # if true the input SVO will be played trying to respect the original framerate eventually skipping frames, otherwise every frame will be processed respecting the `pub_frame_rate` setting + #region_of_interest: '[]' # A polygon defining the ROI where the ZED SDK perform the processing ignoring the rest. Coordinates must be normalized to '1.0' to be resolution independent. + region_of_interest: '[[0.25,0.33],[0.75,0.33],[0.75,0.5],[0.5,0.75],[0.25,0.5]]' # A polygon defining the ROI where the ZED SDK perform the processing ignoring the rest. Coordinates must be normalized to '1.0' to be resolution independent. + #region_of_interest: '[[0.25,0.25],[0.75,0.25],[0.75,0.75],[0.25,0.75]]' # A polygon defining the ROI where the ZED SDK perform the processing ignoring the rest. Coordinates must be normalized to '1.0' to be resolution independent. + #region_of_interest: '[[0.5,0.25],[0.75,0.5],[0.5,0.75],[0.25,0.5]]' # A polygon defining the ROI where the ZED SDK perform the processing ignoring the rest. Coordinates must be normalized to '1.0' to be resolution independent. + #video: From fb9c8263d78748628b52d26137e5d245c0bf9407 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Wed, 13 Sep 2023 11:17:24 +0200 Subject: [PATCH 169/199] Rework params reading --- CHANGELOG.rst | 3 +- .../include/zed_wrapper_nodelet.hpp | 32 ++ .../zed_nodelet/src/zed_wrapper_nodelet.cpp | 354 +++++++++++------- zed_wrapper/params/common.yaml | 17 +- 4 files changed, 253 insertions(+), 153 deletions(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 4a10e200..6bd26187 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -21,13 +21,14 @@ CHANGELOG - Add `pos_tracking/pos_tracking_mode` parameter. Matches the ZED SDK setting: 'QUALITY', 'STANDARD' - Fix the warning `Elaboration takes longer [...]` - 2023-09-07 ---------- - `pub_frame_rate` now controls the `InitParameters::grab_compute_capping_fps` parameter of the ZED SDK instead of controlling the frequency of a parallel thread. It's not a Dynamic parameter anymore. - Change `general/camera_flip` parameter to string: 'AUTO', 'ON', 'OFF' - Change `general/verbose` from bool to integer + + v4.0.5 ------ - Support for ZED SDK v4.0 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 3be9f439..00dc5a72 100644 --- a/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp +++ b/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp @@ -123,6 +123,38 @@ class ZEDWrapperNodelet : public nodelet::Nodelet { */ void readParameters(); + /*! \brief Reads general parameters from the param server + */ + void readGeneralParams(); + + /*! \brief Reads depth parameters from the param server + */ + void readDepthParams(); + + /*! \brief Reads positional tracking parameters from the param server + */ + void readPosTrkParams(); + + /*! \brief Reads spatial mapping parameters from the param server + */ + void readMappingParams(); + + /*! \brief Reads object detection parameters from the param server + */ + void readObjDetParams(); + + /*! \brief Reads sensors parameters from the param server + */ + void readSensParams(); + + /*! \brief Reads SVO parameters from the param server + */ + void readSvoParams(); + + /*! \brief Reads dynamic parameters from the param server + */ + void readDynParams(); + /*! \brief ZED camera polling thread function */ void device_poll_thread_func(); 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 0d7966a8..3e93a2fb 100644 --- a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp @@ -368,94 +368,96 @@ void ZEDWrapperNodelet::onInit() updateDynamicReconfigure(); // <---- Dynamic Reconfigure parameters - // Create all the publishers + // ----> Publishers + NODELET_INFO( "*** PUBLISHERS ***"); + // Image publishers image_transport::ImageTransport it_zed(mNhNs); mPubRgb = it_zed.advertiseCamera(rgb_topic, 1); // rgb - NODELET_INFO_STREAM("Advertised on topic " << mPubRgb.getTopic()); - NODELET_INFO_STREAM("Advertised on topic " << mPubRgb.getInfoTopic()); + NODELET_INFO_STREAM(" * Advertised on topic " << mPubRgb.getTopic()); + NODELET_INFO_STREAM(" * Advertised on topic " << mPubRgb.getInfoTopic()); mPubRawRgb = it_zed.advertiseCamera(rgb_raw_topic, 1); // rgb raw - NODELET_INFO_STREAM("Advertised on topic " << mPubRawRgb.getTopic()); - NODELET_INFO_STREAM("Advertised on topic " << mPubRawRgb.getInfoTopic()); + NODELET_INFO_STREAM(" * Advertised on topic " << mPubRawRgb.getTopic()); + NODELET_INFO_STREAM(" * Advertised on topic " << mPubRawRgb.getInfoTopic()); mPubLeft = it_zed.advertiseCamera(left_topic, 1); // left - NODELET_INFO_STREAM("Advertised on topic " << mPubLeft.getTopic()); - NODELET_INFO_STREAM("Advertised on topic " << mPubLeft.getInfoTopic()); + NODELET_INFO_STREAM(" * Advertised on topic " << mPubLeft.getTopic()); + NODELET_INFO_STREAM(" * Advertised on topic " << mPubLeft.getInfoTopic()); mPubRawLeft = it_zed.advertiseCamera(left_raw_topic, 1); // left raw - NODELET_INFO_STREAM("Advertised on topic " << mPubRawLeft.getTopic()); - NODELET_INFO_STREAM("Advertised on topic " << mPubRawLeft.getInfoTopic()); + NODELET_INFO_STREAM(" * Advertised on topic " << mPubRawLeft.getTopic()); + NODELET_INFO_STREAM(" * Advertised on topic " << mPubRawLeft.getInfoTopic()); mPubRight = it_zed.advertiseCamera(right_topic, 1); // right - NODELET_INFO_STREAM("Advertised on topic " << mPubRight.getTopic()); - NODELET_INFO_STREAM("Advertised on topic " << mPubRight.getInfoTopic()); + NODELET_INFO_STREAM(" * Advertised on topic " << mPubRight.getTopic()); + NODELET_INFO_STREAM(" * Advertised on topic " << mPubRight.getInfoTopic()); mPubRawRight = it_zed.advertiseCamera(right_raw_topic, 1); // right raw - NODELET_INFO_STREAM("Advertised on topic " << mPubRawRight.getTopic()); - NODELET_INFO_STREAM("Advertised on topic " << mPubRawRight.getInfoTopic()); + NODELET_INFO_STREAM(" * Advertised on topic " << mPubRawRight.getTopic()); + NODELET_INFO_STREAM(" * Advertised on topic " << mPubRawRight.getInfoTopic()); mPubRgbGray = it_zed.advertiseCamera(rgb_gray_topic, 1); // rgb - NODELET_INFO_STREAM("Advertised on topic " << mPubRgbGray.getTopic()); - NODELET_INFO_STREAM("Advertised on topic " << mPubRgbGray.getInfoTopic()); + NODELET_INFO_STREAM(" * Advertised on topic " << mPubRgbGray.getTopic()); + NODELET_INFO_STREAM(" * Advertised on topic " << mPubRgbGray.getInfoTopic()); mPubRawRgbGray = it_zed.advertiseCamera(rgb_raw_gray_topic, 1); // rgb raw - NODELET_INFO_STREAM("Advertised on topic " << mPubRawRgbGray.getTopic()); - NODELET_INFO_STREAM("Advertised on topic " << mPubRawRgbGray.getInfoTopic()); + NODELET_INFO_STREAM(" * Advertised on topic " << mPubRawRgbGray.getTopic()); + NODELET_INFO_STREAM(" * Advertised on topic " << mPubRawRgbGray.getInfoTopic()); mPubLeftGray = it_zed.advertiseCamera(left_gray_topic, 1); // left - NODELET_INFO_STREAM("Advertised on topic " << mPubLeftGray.getTopic()); - NODELET_INFO_STREAM("Advertised on topic " << mPubLeftGray.getInfoTopic()); + NODELET_INFO_STREAM(" * Advertised on topic " << mPubLeftGray.getTopic()); + NODELET_INFO_STREAM(" * Advertised on topic " << mPubLeftGray.getInfoTopic()); mPubRawLeftGray = it_zed.advertiseCamera(left_raw_gray_topic, 1); // left raw - NODELET_INFO_STREAM("Advertised on topic " << mPubRawLeftGray.getTopic()); - NODELET_INFO_STREAM("Advertised on topic " << mPubRawLeftGray.getInfoTopic()); + NODELET_INFO_STREAM(" * Advertised on topic " << mPubRawLeftGray.getTopic()); + NODELET_INFO_STREAM(" * Advertised on topic " << mPubRawLeftGray.getInfoTopic()); mPubRightGray = it_zed.advertiseCamera(right_gray_topic, 1); // right - NODELET_INFO_STREAM("Advertised on topic " << mPubRightGray.getTopic()); - NODELET_INFO_STREAM("Advertised on topic " << mPubRightGray.getInfoTopic()); + NODELET_INFO_STREAM(" * Advertised on topic " << mPubRightGray.getTopic()); + NODELET_INFO_STREAM(" * Advertised on topic " << mPubRightGray.getInfoTopic()); mPubRawRightGray = it_zed.advertiseCamera(right_raw_gray_topic, 1); // right raw - NODELET_INFO_STREAM("Advertised on topic " << mPubRawRightGray.getTopic()); - NODELET_INFO_STREAM("Advertised on topic " << mPubRawRightGray.getInfoTopic()); + NODELET_INFO_STREAM(" * Advertised on topic " << mPubRawRightGray.getTopic()); + NODELET_INFO_STREAM(" * Advertised on topic " << mPubRawRightGray.getInfoTopic()); mPubStereo = it_zed.advertise(stereo_topic, 1); - NODELET_INFO_STREAM("Advertised on topic " << mPubStereo.getTopic()); + NODELET_INFO_STREAM(" * Advertised on topic " << mPubStereo.getTopic()); mPubRawStereo = it_zed.advertise(stereo_raw_topic, 1); - NODELET_INFO_STREAM("Advertised on topic " << mPubRawStereo.getTopic()); + NODELET_INFO_STREAM(" * Advertised on topic " << mPubRawStereo.getTopic()); // Detected planes publisher mPubPlane = mNhNs.advertise(plane_topic, 1); if (!mDepthDisabled) { mPubDepth = it_zed.advertiseCamera(depth_topic_root, 1); // depth - NODELET_INFO_STREAM("Advertised on topic " << mPubDepth.getTopic()); - NODELET_INFO_STREAM("Advertised on topic " << mPubDepth.getInfoTopic()); + NODELET_INFO_STREAM(" * Advertised on topic " << mPubDepth.getTopic()); + NODELET_INFO_STREAM(" * Advertised on topic " << mPubDepth.getInfoTopic()); // Confidence Map publisher mPubConfMap = mNhNs.advertise(conf_map_topic, 1); // confidence map - NODELET_INFO_STREAM("Advertised on topic " << mPubConfMap.getTopic()); + NODELET_INFO_STREAM(" * Advertised on topic " << mPubConfMap.getTopic()); // Disparity publisher mPubDisparity = mNhNs.advertise(disparityTopic, 1); - NODELET_INFO_STREAM("Advertised on topic " << mPubDisparity.getTopic()); + NODELET_INFO_STREAM(" * Advertised on topic " << mPubDisparity.getTopic()); // PointCloud publishers mPubCloud = mNhNs.advertise(pointcloud_topic, 1); - NODELET_INFO_STREAM("Advertised on topic " << mPubCloud.getTopic()); + NODELET_INFO_STREAM(" * Advertised on topic " << mPubCloud.getTopic()); if (mMappingEnabled) { mPubFusedCloud = mNhNs.advertise(pointcloud_fused_topic, 1); - NODELET_INFO_STREAM("Advertised on topic " << mPubFusedCloud.getTopic() << " @ " << mFusedPcPubFreq << " Hz"); + NODELET_INFO_STREAM(" * Advertised on topic " << mPubFusedCloud.getTopic() << " @ " << mFusedPcPubFreq << " Hz"); } // Object detection publishers if (mObjDetEnabled) { mPubObjDet = mNhNs.advertise(object_det_topic, 1); - NODELET_INFO_STREAM("Advertised on topic " << mPubObjDet.getTopic()); + NODELET_INFO_STREAM(" * Advertised on topic " << mPubObjDet.getTopic()); } // Odometry and Pose publisher mPubPose = mNhNs.advertise(poseTopic, 1); - NODELET_INFO_STREAM("Advertised on topic " << mPubPose.getTopic()); + NODELET_INFO_STREAM(" * Advertised on topic " << mPubPose.getTopic()); mPubPoseCov = mNhNs.advertise(pose_cov_topic, 1); - NODELET_INFO_STREAM("Advertised on topic " << mPubPoseCov.getTopic()); + NODELET_INFO_STREAM(" * Advertised on topic " << mPubPoseCov.getTopic()); mPubOdom = mNhNs.advertise(odometryTopic, 1); - NODELET_INFO_STREAM("Advertised on topic " << mPubOdom.getTopic()); + NODELET_INFO_STREAM(" * Advertised on topic " << mPubOdom.getTopic()); // Rviz markers publisher mPubMarker = mNhNs.advertise(marker_topic, 10, true); @@ -463,9 +465,9 @@ void ZEDWrapperNodelet::onInit() // Camera Path if (mPathPubRate > 0) { mPubOdomPath = mNhNs.advertise(odom_path_topic, 1, true); - NODELET_INFO_STREAM("Advertised on topic " << mPubOdomPath.getTopic()); + NODELET_INFO_STREAM(" * Advertised on topic " << mPubOdomPath.getTopic()); mPubMapPath = mNhNs.advertise(map_path_topic, 1, true); - NODELET_INFO_STREAM("Advertised on topic " << mPubMapPath.getTopic()); + NODELET_INFO_STREAM(" * Advertised on topic " << mPubMapPath.getTopic()); mPathTimer = mNhNs.createTimer(ros::Duration(1.0 / mPathPubRate), &ZEDWrapperNodelet::callback_pubPath, this); @@ -482,32 +484,32 @@ void ZEDWrapperNodelet::onInit() } // Sensor publishers - if (mZedRealCamModel != sl::MODEL::ZED) { + if (!sl_tools::isZED(mZedRealCamModel)) { // IMU Publishers mPubImu = mNhNs.advertise(imu_topic, 1); - NODELET_INFO_STREAM("Advertised on topic " << mPubImu.getTopic()); + NODELET_INFO_STREAM(" * Advertised on topic " << mPubImu.getTopic()); mPubImuRaw = mNhNs.advertise(imu_topic_raw, 1); - NODELET_INFO_STREAM("Advertised on topic " << mPubImuRaw.getTopic()); + NODELET_INFO_STREAM(" * Advertised on topic " << mPubImuRaw.getTopic()); if(mZedRealCamModel != sl::MODEL::ZED_M) { // IMU temperature sensor mPubImuTemp = mNhNs.advertise(imu_temp_topic, 1); - NODELET_INFO_STREAM("Advertised on topic " << mPubImuTemp.getTopic()); + NODELET_INFO_STREAM(" * Advertised on topic " << mPubImuTemp.getTopic()); } if (mZedRealCamModel == sl::MODEL::ZED2 || mZedRealCamModel == sl::MODEL::ZED2i) { mPubImuMag = mNhNs.advertise(imu_mag_topic, 1); - NODELET_INFO_STREAM("Advertised on topic " << mPubImuMag.getTopic()); + NODELET_INFO_STREAM(" * Advertised on topic " << mPubImuMag.getTopic()); // Atmospheric pressure mPubPressure = mNhNs.advertise(pressure_topic, 1); - NODELET_INFO_STREAM("Advertised on topic " << mPubPressure.getTopic()); + NODELET_INFO_STREAM(" * Advertised on topic " << mPubPressure.getTopic()); // CMOS sensor temperatures mPubTempL = mNhNs.advertise(temp_topic_left, 1); - NODELET_INFO_STREAM("Advertised on topic " << mPubTempL.getTopic()); + NODELET_INFO_STREAM(" * Advertised on topic " << mPubTempL.getTopic()); mPubTempR = mNhNs.advertise(temp_topic_right, 1); - NODELET_INFO_STREAM("Advertised on topic " << mPubTempR.getTopic()); + NODELET_INFO_STREAM(" * Advertised on topic " << mPubTempR.getTopic()); } // Publish camera imu transform in a latched topic @@ -534,7 +536,7 @@ void ZEDWrapperNodelet::onInit() mPubCamImuTransf.publish(mCameraImuTransfMgs); - NODELET_INFO_STREAM("Advertised on topic " << mPubCamImuTransf.getTopic() << " [LATCHED]"); + NODELET_INFO_STREAM(" * Advertised on topic " << mPubCamImuTransf.getTopic() << " [LATCHED]"); } if (!mSvoMode && !mSensTimestampSync) { @@ -544,35 +546,57 @@ void ZEDWrapperNodelet::onInit() mSensPeriodMean_usec.reset(new sl_tools::CSmartMean(mCamFrameRate / 2)); } } + // <---- Publishers - // Subscribers + // ----> Subscribers + NODELET_INFO( "*** SUBSCRIBERS ***"); mClickedPtSub = mNhNs.subscribe(mClickedPtTopic, 10, &ZEDWrapperNodelet::clickedPtCallback, this); - NODELET_INFO_STREAM("Subscribed to topic " << mClickedPtTopic.c_str()); + NODELET_INFO_STREAM(" * Subscribed to topic " << mClickedPtTopic.c_str()); + // <---- Subscribers - // Services + // ----> Services + NODELET_INFO( "*** SERVICES ***"); if (!mDepthDisabled) { mSrvSetInitPose = mNhNs.advertiseService("set_pose", &ZEDWrapperNodelet::on_set_pose, this); + NODELET_INFO_STREAM(" * Advertised on service " << mSrvSetInitPose.getService().c_str()); mSrvResetOdometry = mNhNs.advertiseService("reset_odometry", &ZEDWrapperNodelet::on_reset_odometry, this); + NODELET_INFO_STREAM(" * Advertised on service " << mSrvResetOdometry.getService().c_str()); mSrvResetTracking = mNhNs.advertiseService("reset_tracking", &ZEDWrapperNodelet::on_reset_tracking, this); + NODELET_INFO_STREAM(" * Advertised on service " << mSrvResetTracking.getService().c_str()); mSrvSaveAreaMemory = mNhNs.advertiseService("save_area_memory", &ZEDWrapperNodelet::on_save_area_memory, this); + NODELET_INFO_STREAM(" * Advertised on service " << mSrvSaveAreaMemory.getService().c_str()); mSrvStartMapping = mNhNs.advertiseService("start_3d_mapping", &ZEDWrapperNodelet::on_start_3d_mapping, this); + NODELET_INFO_STREAM(" * Advertised on service " << mSrvStartMapping.getService().c_str()); mSrvStopMapping = mNhNs.advertiseService("stop_3d_mapping", &ZEDWrapperNodelet::on_stop_3d_mapping, this); + NODELET_INFO_STREAM(" * Advertised on service " << mSrvStopMapping.getService().c_str()); mSrvSave3dMap = mNhNs.advertiseService("save_3d_map", &ZEDWrapperNodelet::on_save_3d_map, this); + NODELET_INFO_STREAM(" * Advertised on service " << mSrvSave3dMap.getService().c_str()); mSrvStartObjDet = mNhNs.advertiseService("start_object_detection", &ZEDWrapperNodelet::on_start_object_detection, this); + NODELET_INFO_STREAM(" * Advertised on service " << mSrvStartObjDet.getService().c_str()); mSrvStopObjDet = mNhNs.advertiseService("stop_object_detection", &ZEDWrapperNodelet::on_stop_object_detection, this); + NODELET_INFO_STREAM(" * Advertised on service " << mSrvStopObjDet.getService().c_str()); } mSrvSvoStartRecording = mNhNs.advertiseService("start_svo_recording", &ZEDWrapperNodelet::on_start_svo_recording, this); + NODELET_INFO_STREAM(" * Advertised on service " << mSrvSvoStartRecording.getService().c_str()); mSrvSvoStopRecording = mNhNs.advertiseService("stop_svo_recording", &ZEDWrapperNodelet::on_stop_svo_recording, this); + NODELET_INFO_STREAM(" * Advertised on service " << mSrvSvoStopRecording.getService().c_str()); mSrvSetLedStatus = mNhNs.advertiseService("set_led_status", &ZEDWrapperNodelet::on_set_led_status, this); + NODELET_INFO_STREAM(" * Advertised on service " << mSrvSetLedStatus.getService().c_str()); mSrvToggleLed = mNhNs.advertiseService("toggle_led", &ZEDWrapperNodelet::on_toggle_led, this); + NODELET_INFO_STREAM(" * Advertised on service " << mSrvToggleLed.getService().c_str()); + mSrvSvoStartStream = mNhNs.advertiseService("start_remote_stream", &ZEDWrapperNodelet::on_start_remote_stream, this); + NODELET_INFO_STREAM(" * Advertised on service " << mSrvSvoStartStream.getService().c_str()); mSrvSvoStopStream = mNhNs.advertiseService("stop_remote_stream", &ZEDWrapperNodelet::on_stop_remote_stream, this); + NODELET_INFO_STREAM(" * Advertised on service " << mSrvSvoStopStream.getService().c_str()); + // <---- Services + // ----> Threads if (!mDepthDisabled) { // Start Pointcloud thread mPcThread = std::thread(&ZEDWrapperNodelet::pointcloud_thread_func, this); @@ -583,14 +607,15 @@ void ZEDWrapperNodelet::onInit() // Start Sensors thread mSensThread = std::thread(&ZEDWrapperNodelet::sensors_thread_func, this); + // <---- Threads - NODELET_INFO("ZED Node started"); + NODELET_INFO("+++ ZED Node started +++"); } -void ZEDWrapperNodelet::readParameters() +void ZEDWrapperNodelet::readGeneralParams() { - // ----> General NODELET_INFO_STREAM("*** GENERAL PARAMETERS ***"); + std::string camera_model; mNhNs.getParam("general/camera_model", camera_model); @@ -711,15 +736,12 @@ void ZEDWrapperNodelet::readParameters() std::string parsed_str = getRoiParam("general/region_of_interest", mRoiParam); NODELET_INFO_STREAM(" * Region of interest\t\t-> " << parsed_str.c_str()); - // <---- General - - // ----> Video - //NODELET_INFO_STREAM("*** VIDEO PARAMETERS ***"); - // Note: there are no "static" video parameters - // <---- Video +} - // -----> Depth +void ZEDWrapperNodelet::readDepthParams() +{ NODELET_INFO_STREAM("*** DEPTH PARAMETERS ***"); + std::string depth_mode_str = sl::toString(mDepthMode).c_str(); mNhNs.getParam("depth/depth_mode", depth_mode_str); @@ -763,12 +785,13 @@ void ZEDWrapperNodelet::readParameters() mNhNs.getParam("depth/max_depth", mCamMaxDepth); NODELET_INFO_STREAM(" * Maximum depth\t\t-> " << mCamMaxDepth << " m"); } - // <----- Depth +} - // ----> Positional Tracking +void ZEDWrapperNodelet::readPosTrkParams() +{ if(!mDepthDisabled) { NODELET_INFO_STREAM("*** POSITIONAL TRACKING PARAMETERS ***"); - + mNhNs.param("pos_tracking/pos_tracking_enabled", mPosTrackingEnabled, true); NODELET_INFO_STREAM(" * Positional tracking\t\t-> " << (mPosTrackingEnabled ? "ENABLED": "DISABLED")); @@ -813,20 +836,28 @@ void ZEDWrapperNodelet::readParameters() mNhNs.param("pos_tracking/floor_alignment", mFloorAlignment, false); NODELET_INFO_STREAM(" * Floor alignment\t\t-> " << (mFloorAlignment ? "ENABLED": "DISABLED")); mNhNs.param("pos_tracking/init_odom_with_first_valid_pose", mInitOdomWithPose, true); - NODELET_INFO_STREAM(" * Init Odometry with first valid pose data: " + NODELET_INFO_STREAM(" * Init Odometry with first valid pose data -> " << (mInitOdomWithPose ? "ENABLED": "DISABLED")); mNhNs.param("pos_tracking/two_d_mode", mTwoDMode, false); - NODELET_INFO_STREAM(" * Force 2D mode\t\t\t-> " << (mTwoDMode ? "ENABLED": "DISABLED")); + NODELET_INFO_STREAM(" * Force 2D mode\t\t-> " << (mTwoDMode ? "ENABLED": "DISABLED")); if (mTwoDMode) { mNhNs.param("pos_tracking/fixed_z_value", mFixedZValue, 0.0); NODELET_INFO_STREAM(" * Fixed Z value\t\t-> " << mFixedZValue); } + + mNhNs.param("pos_tracking/publish_tf", mPublishTf, true); + NODELET_INFO_STREAM(" * Broadcast odometry TF\t-> " << (mPublishTf ? "ENABLED": "DISABLED")); + mNhNs.param("pos_tracking/publish_map_tf", mPublishMapTf, true); + NODELET_INFO_STREAM(" * Broadcast map pose TF\t-> " + << (mPublishTf ? (mPublishMapTf ? "ENABLED": "DISABLED"): "DISABLED")); } - // <---- Positional Tracking +} - // ----> Mapping +void ZEDWrapperNodelet::readMappingParams() +{ NODELET_INFO_STREAM("*** MAPPING PARAMETERS ***"); + if(!mDepthDisabled) { mNhNs.param("mapping/mapping_enabled", mMappingEnabled, false); @@ -848,9 +879,10 @@ void ZEDWrapperNodelet::readParameters() } mNhNs.param("mapping/clicked_point_topic", mClickedPtTopic, std::string("/clicked_point")); NODELET_INFO_STREAM(" * Clicked point topic\t\t-> " << mClickedPtTopic.c_str()); - // <---- Mapping +} - // ----> Object Detection +void ZEDWrapperNodelet::readObjDetParams() +{ if(!mDepthDisabled) { NODELET_INFO_STREAM("*** OBJECT DETECTION PARAMETERS ***"); @@ -895,16 +927,17 @@ void ZEDWrapperNodelet::readParameters() NODELET_INFO_STREAM(" * Detect sport-related objects\t-> " << (mObjDetSportsEnable ? "ENABLED": "DISABLED")); } } - // <---- Object Detection +} +void ZEDWrapperNodelet::readSensParams() +{ NODELET_INFO_STREAM("*** SENSORS PARAMETERS ***"); - // ----> Sensors - if (camera_model != "zed") { + if (!sl_tools::isZED(mZedUserCamModel)) { mNhNs.getParam("sensors/sensors_timestamp_sync", mSensTimestampSync); NODELET_INFO_STREAM(" * Sensors timestamp sync\t-> " << (mSensTimestampSync ? "ENABLED": "DISABLED")); mNhNs.getParam("sensors/max_pub_rate", mSensPubRate); - if (camera_model == "zedm") { + if (!sl_tools::isZEDM(mZedUserCamModel)) { if (mSensPubRate > 800.) mSensPubRate = 800.; } else { @@ -913,14 +946,18 @@ void ZEDWrapperNodelet::readParameters() } NODELET_INFO_STREAM(" * Max sensors rate\t\t-> " << mSensPubRate); + + mNhNs.param("sensors/publish_imu_tf", mPublishImuTf, true); + NODELET_INFO_STREAM(" * Broadcast IMU pose TF\t-> " << (mPublishImuTf ? "ENABLED": "DISABLED")); } else { NODELET_INFO_STREAM(" * The ZED camera has no available inertial/environmental sensors."); } - // <---- Sensors +} +void ZEDWrapperNodelet::readSvoParams() +{ NODELET_INFO_STREAM("*** SVO PARAMETERS ***"); - // ----> SVO mNhNs.param("svo_file", mSvoFilepath, std::string()); mSvoFilepath = sl_tools::resolveFilePath(mSvoFilepath); NODELET_INFO_STREAM(" * SVO input file: \t\t-> " << mSvoFilepath.c_str()); @@ -943,14 +980,107 @@ void ZEDWrapperNodelet::readParameters() mSvoComprMode = static_cast(svo_compr); NODELET_INFO_STREAM(" * SVO REC compression\t\t-> " << sl::toString(mSvoComprMode)); +} + +void ZEDWrapperNodelet::readDynParams() +{ + NODELET_INFO_STREAM("*** DYNAMIC PARAMETERS (Init. values) ***"); + + mNhNs.getParam("brightness", mCamBrightness); + NODELET_INFO_STREAM(" * [DYN] brightness\t\t-> " << mCamBrightness); + mNhNs.getParam("contrast", mCamContrast); + NODELET_INFO_STREAM(" * [DYN] contrast\t\t-> " << mCamContrast); + mNhNs.getParam("hue", mCamHue); + NODELET_INFO_STREAM(" * [DYN] hue\t\t\t-> " << mCamHue); + mNhNs.getParam("saturation", mCamSaturation); + NODELET_INFO_STREAM(" * [DYN] saturation\t\t-> " << mCamSaturation); + mNhNs.getParam("sharpness", mCamSharpness); + NODELET_INFO_STREAM(" * [DYN] sharpness\t\t-> " << mCamSharpness); + mNhNs.getParam("gamma", mCamGamma); + NODELET_INFO_STREAM(" * [DYN] gamma\t\t\t-> " << mCamGamma); + mNhNs.getParam("auto_exposure_gain", mCamAutoExposure); + NODELET_INFO_STREAM(" * [DYN] auto_exposure_gain\t-> " << (mCamAutoExposure ? "ENABLED": "DISABLED")); + mNhNs.getParam("gain", mCamGain); + mNhNs.getParam("exposure", mCamExposure); + if (!mCamAutoExposure) { + NODELET_INFO_STREAM(" * [DYN] gain\t\t-> " << mCamGain); + NODELET_INFO_STREAM(" * [DYN] exposure\t\t-> " << mCamExposure); + } + mNhNs.getParam("auto_whitebalance", mCamAutoWB); + NODELET_INFO_STREAM(" * [DYN] auto_whitebalance\t-> " << (mCamAutoWB ? "ENABLED": "DISABLED")); + mNhNs.getParam("whitebalance_temperature", mCamWB); + if (!mCamAutoWB) { + NODELET_INFO_STREAM(" * [DYN] whitebalance_temperature\t\t-> " << mCamWB); + } + + if (mCamAutoExposure) { + mTriggerAutoExposure = true; + } + if (mCamAutoWB) { + mTriggerAutoWB = true; + } + + if(!mDepthDisabled) { + mNhNs.getParam("depth_confidence", mCamDepthConfidence); + NODELET_INFO_STREAM(" * [DYN] Depth confidence\t-> " << mCamDepthConfidence); + mNhNs.getParam("depth_texture_conf", mCamDepthTextureConf); + NODELET_INFO_STREAM(" * [DYN] Depth texture conf.\t-> " << mCamDepthTextureConf); + + mNhNs.getParam("point_cloud_freq", mPointCloudFreq); + if(mPointCloudFreq>mPubFrameRate) { + mPointCloudFreq=mPubFrameRate; + NODELET_WARN_STREAM("'point_cloud_freq' cannot be major than 'general/pub_frame_rate'. Set to " + << mPointCloudFreq); + } + NODELET_INFO_STREAM(" * [DYN] point_cloud_freq\t-> " << mPointCloudFreq << " Hz"); + } +} + +void ZEDWrapperNodelet::readParameters() +{ + // ----> General + readGeneralParams(); + // <---- General + + // ----> Video + //NODELET_INFO_STREAM("*** VIDEO PARAMETERS ***"); + // Note: there are no "static" video parameters + // <---- Video + + // -----> Depth + readDepthParams(); + // <----- Depth + + // ----> Dynamic + void readDynParams(); + // <---- Dynamic + + // ----> Positional Tracking + readPosTrkParams(); + // <---- Positional Tracking + + // ----> Mapping + readMappingParams(); + // <---- Mapping + + // ----> Object Detection + readObjDetParams(); + // <---- Object Detection + + // ----> Sensors + readSensParams(); + // <---- Sensors + + // ----> SVO + readSvoParams(); // <---- SVO // Remote Stream - mNhNs.param("stream", mRemoteStreamAddr, std::string()); + mNhNs.param("stream", mRemoteStreamAddr, std::string()); + // ----> Coordinate frames NODELET_INFO_STREAM("*** COORDINATE FRAMES ***"); - // ----> Coordinate frames mNhNs.param("pos_tracking/map_frame", mMapFrameId, "map"); mNhNs.param("pos_tracking/odometry_frame", mOdometryFrameId, "odom"); mNhNs.param("general/base_frame", mBaseFrameId, "base_link"); @@ -999,70 +1129,6 @@ void ZEDWrapperNodelet::readParameters() NODELET_INFO_STREAM(" * confidence_optical_frame\t-> " << mConfidenceOptFrameId); } // <---- Coordinate frames - - // ----> TF broadcasting - if(!mDepthDisabled) { - mNhNs.param("pos_tracking/publish_tf", mPublishTf, true); - NODELET_INFO_STREAM(" * Broadcast odometry TF\t-> " << (mPublishTf ? "ENABLED": "DISABLED")); - mNhNs.param("pos_tracking/publish_map_tf", mPublishMapTf, true); - NODELET_INFO_STREAM(" * Broadcast map pose TF\t-> " - << (mPublishTf ? (mPublishMapTf ? "ENABLED": "DISABLED"): "DISABLED")); - mNhNs.param("sensors/publish_imu_tf", mPublishImuTf, true); - NODELET_INFO_STREAM(" * Broadcast IMU pose TF\t-> " << (mPublishImuTf ? "ENABLED": "DISABLED")); - } - // <---- TF broadcasting - - // ----> Dynamic - NODELET_INFO_STREAM("*** DYNAMIC PARAMETERS (Init. values) ***"); - mNhNs.getParam("brightness", mCamBrightness); - NODELET_INFO_STREAM(" * [DYN] brightness\t\t-> " << mCamBrightness); - mNhNs.getParam("contrast", mCamContrast); - NODELET_INFO_STREAM(" * [DYN] contrast\t\t-> " << mCamContrast); - mNhNs.getParam("hue", mCamHue); - NODELET_INFO_STREAM(" * [DYN] hue\t\t\t-> " << mCamHue); - mNhNs.getParam("saturation", mCamSaturation); - NODELET_INFO_STREAM(" * [DYN] saturation\t\t-> " << mCamSaturation); - mNhNs.getParam("sharpness", mCamSharpness); - NODELET_INFO_STREAM(" * [DYN] sharpness\t\t-> " << mCamSharpness); - mNhNs.getParam("gamma", mCamGamma); - NODELET_INFO_STREAM(" * [DYN] gamma\t\t\t-> " << mCamGamma); - mNhNs.getParam("auto_exposure_gain", mCamAutoExposure); - NODELET_INFO_STREAM(" * [DYN] auto_exposure_gain\t-> " << (mCamAutoExposure ? "ENABLED": "DISABLED")); - mNhNs.getParam("gain", mCamGain); - mNhNs.getParam("exposure", mCamExposure); - if (!mCamAutoExposure) { - NODELET_INFO_STREAM(" * [DYN] gain\t\t-> " << mCamGain); - NODELET_INFO_STREAM(" * [DYN] exposure\t\t-> " << mCamExposure); - } - mNhNs.getParam("auto_whitebalance", mCamAutoWB); - NODELET_INFO_STREAM(" * [DYN] auto_whitebalance\t-> " << (mCamAutoWB ? "ENABLED": "DISABLED")); - mNhNs.getParam("whitebalance_temperature", mCamWB); - if (!mCamAutoWB) { - NODELET_INFO_STREAM(" * [DYN] whitebalance_temperature\t\t-> " << mCamWB); - } - - if (mCamAutoExposure) { - mTriggerAutoExposure = true; - } - if (mCamAutoWB) { - mTriggerAutoWB = true; - } - - if(!mDepthDisabled) { - mNhNs.getParam("depth_confidence", mCamDepthConfidence); - NODELET_INFO_STREAM(" * [DYN] Depth confidence\t-> " << mCamDepthConfidence); - mNhNs.getParam("depth_texture_conf", mCamDepthTextureConf); - NODELET_INFO_STREAM(" * [DYN] Depth texture conf.\t-> " << mCamDepthTextureConf); - - mNhNs.getParam("point_cloud_freq", mPointCloudFreq); - if(mPointCloudFreq>mPubFrameRate) { - mPointCloudFreq=mPubFrameRate; - NODELET_WARN_STREAM("'point_cloud_freq' cannot be major than 'general/pub_frame_rate'. Set to " - << mPointCloudFreq); - } - NODELET_INFO_STREAM(" * [DYN] point_cloud_freq\t-> " << mPointCloudFreq << " Hz"); - } - // <---- Dynamic } std::string ZEDWrapperNodelet::getRoiParam(std::string paramName, std::vector> & outVal) @@ -1590,7 +1656,7 @@ bool ZEDWrapperNodelet::start_3d_mapping() if (mPubFusedCloud.getTopic().empty()) { std::string pointcloud_fused_topic = "mapping/fused_cloud"; mPubFusedCloud = mNhNs.advertise(pointcloud_fused_topic, 1); - NODELET_INFO_STREAM("Advertised on topic " << mPubFusedCloud.getTopic() << " @ " << mFusedPcPubFreq << " Hz"); + NODELET_INFO_STREAM(" * Advertised on topic " << mPubFusedCloud.getTopic() << " @ " << mFusedPcPubFreq << " Hz"); } mMappingRunning = true; @@ -1663,7 +1729,7 @@ bool ZEDWrapperNodelet::start_obj_detect() std::string object_det_topic = object_det_topic_root + "/objects"; mPubObjDet = mNhNs.advertise(object_det_topic, 1); - NODELET_INFO_STREAM("Advertised on topic " << mPubObjDet.getTopic()); + NODELET_INFO_STREAM(" * Advertised on topic " << mPubObjDet.getTopic()); } mObjDetFilter.clear(); diff --git a/zed_wrapper/params/common.yaml b/zed_wrapper/params/common.yaml index e98bad15..93f8fbcd 100644 --- a/zed_wrapper/params/common.yaml +++ b/zed_wrapper/params/common.yaml @@ -14,7 +14,7 @@ gain: 100 # Dynamic - work exposure: 100 # Dynamic - works only if `auto_exposure_gain` is false auto_whitebalance: true # Dynamic whitebalance_temperature: 42 # Dynamic - works only if `auto_whitebalance` is false -depth_confidence: 30 # Dynamic +depth_confidence: 50 # Dynamic depth_texture_conf: 100 # Dynamic point_cloud_freq: 10.0 # Dynamic - frequency of the pointcloud publishing (equal or less to `grab_frame_rate` value) @@ -24,7 +24,7 @@ general: serial_number: 0 gpu_id: -1 base_frame: 'base_link' # must be equal to the frame_id used in the URDF file - sdk_verbose: 1 # Set verbose level of the ZED SDK + sdk_verbose: 1 # Set verbose level of the ZED SDK svo_compression: 2 # `0`: LOSSLESS, `1`: AVCHD, `2`: HEVC self_calib: true # enable/disable self calibration at starting camera_flip: 'AUTO' # camera flip mode: 'OFF', 'ON', 'AUTO' @@ -32,22 +32,21 @@ general: pub_downscale_factor: 2.0 # rescale factor used to rescale image before publishing when 'pub_resolution' is 'CUSTOM' pub_frame_rate: 15.0 # frequency of publishing of video and depth data (see SDK API "InitParameters::grab_compute_capping_fps") svo_realtime: true # if true the input SVO will be played trying to respect the original framerate eventually skipping frames, otherwise every frame will be processed respecting the `pub_frame_rate` setting - #region_of_interest: '[]' # A polygon defining the ROI where the ZED SDK perform the processing ignoring the rest. Coordinates must be normalized to '1.0' to be resolution independent. - region_of_interest: '[[0.25,0.33],[0.75,0.33],[0.75,0.5],[0.5,0.75],[0.25,0.5]]' # A polygon defining the ROI where the ZED SDK perform the processing ignoring the rest. Coordinates must be normalized to '1.0' to be resolution independent. + region_of_interest: '[]' # A polygon defining the ROI where the ZED SDK perform the processing ignoring the rest. Coordinates must be normalized to '1.0' to be resolution independent. + #region_of_interest: '[[0.25,0.33],[0.75,0.33],[0.75,0.5],[0.5,0.75],[0.25,0.5]]' # A polygon defining the ROI where the ZED SDK perform the processing ignoring the rest. Coordinates must be normalized to '1.0' to be resolution independent. #region_of_interest: '[[0.25,0.25],[0.75,0.25],[0.75,0.75],[0.25,0.75]]' # A polygon defining the ROI where the ZED SDK perform the processing ignoring the rest. Coordinates must be normalized to '1.0' to be resolution independent. #region_of_interest: '[[0.5,0.25],[0.75,0.5],[0.5,0.75],[0.25,0.5]]' # A polygon defining the ROI where the ZED SDK perform the processing ignoring the rest. Coordinates must be normalized to '1.0' to be resolution independent. - #video: depth: depth_mode: 'ULTRA' # 'NONE', 'PERFORMANCE', 'QUALITY', 'ULTRA', 'NEURAL' depth_stabilization: 1 # [0-100] - 0: Disabled - openni_depth_mode: false # 'false': 32bit float meters, 'true': 16bit uchar millimeters + 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' + pos_tracking_mode: 'STANDARD' # Matches the ZED SDK setting: 'QUALITY', 'STANDARD' -> Use 'QUALITY' with 'ULTRA' depth mode for improved outdoors performance 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 @@ -63,7 +62,9 @@ pos_tracking: path_pub_rate: 2.0 # Camera trajectory publishing frequency path_max_count: -1 # use '-1' for unlimited path size two_d_mode: false # Force navigation on a plane. If true the Z value will be fixed to "fixed_z_value", roll and pitch to zero - fixed_z_value: 0.00 # Value to be used for Z coordinate if `two_d_mode` is true + fixed_z_value: 0.00 # Value to be used for Z coordinate if `two_d_mode` is true + depth_min_range: 0.0 # Set this value for removing fixed zones of the robot in the FoV of the camerafrom the visual odometry evaluation + set_as_static: false # If 'true' the camera will be static and not move in the environment mapping: mapping_enabled: false # True to enable mapping and fused point cloud publication From 1d36f548495b3180b48c34408d5d269219aee2d0 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Wed, 13 Sep 2023 11:23:28 +0200 Subject: [PATCH 170/199] Add .clang-format --- .clang-format | 65 +++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 65 insertions(+) create mode 100644 .clang-format diff --git a/.clang-format b/.clang-format new file mode 100644 index 00000000..817fd6d6 --- /dev/null +++ b/.clang-format @@ -0,0 +1,65 @@ +--- +BasedOnStyle: Google +AccessModifierOffset: -2 +ConstructorInitializerIndentWidth: 2 +AlignEscapedNewlinesLeft: false +AlignTrailingComments: true +AllowAllParametersOfDeclarationOnNextLine: false +AllowShortIfStatementsOnASingleLine: false +AllowShortLoopsOnASingleLine: false +AllowShortFunctionsOnASingleLine: None +AlwaysBreakTemplateDeclarations: true +AlwaysBreakBeforeMultilineStrings: true +BreakBeforeBinaryOperators: false +BreakBeforeTernaryOperators: false +BreakConstructorInitializersBeforeComma: true +BinPackParameters: true +ColumnLimit: 120 +ConstructorInitializerAllOnOneLineOrOnePerLine: true +DerivePointerBinding: false +PointerBindsToType: true +ExperimentalAutoDetectBinPacking: false +IndentCaseLabels: true +MaxEmptyLinesToKeep: 1 +NamespaceIndentation: None +ObjCSpaceBeforeProtocolList: true +PenaltyBreakBeforeFirstCallParameter: 19 +PenaltyBreakComment: 60 +PenaltyBreakString: 1 +PenaltyBreakFirstLessLess: 1000 +PenaltyExcessCharacter: 1000 +PenaltyReturnTypeOnItsOwnLine: 90 +SpacesBeforeTrailingComments: 2 +Cpp11BracedListStyle: false +Standard: Auto +IndentWidth: 2 +TabWidth: 2 +UseTab: Never +IndentFunctionDeclarationAfterType: false +SpacesInParentheses: false +SpacesInAngles: false +SpaceInEmptyParentheses: false +SpacesInCStyleCastParentheses: false +SpaceAfterControlStatementKeyword: true +SpaceBeforeAssignmentOperators: true +ContinuationIndentWidth: 4 +SortIncludes: false +SpaceAfterCStyleCast: false + +# Configure each individual brace in BraceWrapping +BreakBeforeBraces: Custom + +# Control of individual brace wrapping cases +BraceWrapping: { + AfterClass: 'true' + AfterControlStatement: 'true' + AfterEnum : 'true' + AfterFunction : 'true' + AfterNamespace : 'true' + AfterStruct : 'true' + AfterUnion : 'true' + BeforeCatch : 'true' + BeforeElse : 'true' + IndentBraces : 'false' +} +... From ac61ca6775f76aafacf70d12dfa2ba6755017ec3 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Wed, 13 Sep 2023 11:28:19 +0200 Subject: [PATCH 171/199] Code refactoring --- .../src/rgbd_sensor_demux.cpp | 2 +- .../src/rgbd_sensor_sync.cpp | 97 +- zed_nodelets/src/tools/include/sl_tools.h | 5 +- zed_nodelets/src/tools/src/sl_tools.cpp | 92 +- .../include/zed_wrapper_nodelet.hpp | 1019 +- .../zed_nodelet/src/zed_wrapper_nodelet.cpp | 8496 +++++++++-------- 6 files changed, 5219 insertions(+), 4492 deletions(-) diff --git a/zed_nodelets/src/rgbd_sensors_demux_nodelet/src/rgbd_sensor_demux.cpp b/zed_nodelets/src/rgbd_sensors_demux_nodelet/src/rgbd_sensor_demux.cpp index 0ff9473c..f024d56c 100644 --- a/zed_nodelets/src/rgbd_sensors_demux_nodelet/src/rgbd_sensor_demux.cpp +++ b/zed_nodelets/src/rgbd_sensors_demux_nodelet/src/rgbd_sensor_demux.cpp @@ -55,7 +55,7 @@ void RgbdSensorsDemuxNodelet::onInit() NODELET_INFO_STREAM(" * Subscribed to topic: " << mSub.getTopic().c_str()); } -void RgbdSensorsDemuxNodelet::msgCallback(const zed_interfaces::RGBDSensorsPtr &msg) +void RgbdSensorsDemuxNodelet::msgCallback(const zed_interfaces::RGBDSensorsPtr& msg) { if (!msg->rgb.header.stamp.isZero()) { diff --git a/zed_nodelets/src/rgbd_sensors_sync_nodelet/src/rgbd_sensor_sync.cpp b/zed_nodelets/src/rgbd_sensors_sync_nodelet/src/rgbd_sensor_sync.cpp index 2a2046cd..b2b329b6 100644 --- a/zed_nodelets/src/rgbd_sensors_sync_nodelet/src/rgbd_sensor_sync.cpp +++ b/zed_nodelets/src/rgbd_sensors_sync_nodelet/src/rgbd_sensor_sync.cpp @@ -209,10 +209,10 @@ void RgbdSensorsSyncNodelet::readParameters() NODELET_INFO(" * sub_mag -> %s", mUseMag ? "true" : "false"); } -void RgbdSensorsSyncNodelet::callbackRGBD(const sensor_msgs::ImageConstPtr &rgb, - const sensor_msgs::ImageConstPtr &depth, - const sensor_msgs::CameraInfoConstPtr &rgbCameraInfo, - const sensor_msgs::CameraInfoConstPtr &depthCameraInfo) +void RgbdSensorsSyncNodelet::callbackRGBD(const sensor_msgs::ImageConstPtr& rgb, + const sensor_msgs::ImageConstPtr& depth, + const sensor_msgs::CameraInfoConstPtr& rgbCameraInfo, + const sensor_msgs::CameraInfoConstPtr& depthCameraInfo) { // ----> Frequency calculation static std::chrono::steady_clock::time_point last_time = std::chrono::steady_clock::now(); @@ -251,20 +251,22 @@ void RgbdSensorsSyncNodelet::callbackRGBD(const sensor_msgs::ImageConstPtr &rgb, if (rgbStamp != rgb->header.stamp.toSec()) { - NODELET_ERROR("Input stamps changed between the beginning and the end of the callback! Make " - "sure the node publishing the topics doesn't override the same data after publishing them. A " - "solution is to use this node within another nodelet manager. "); - NODELET_ERROR("Stamps: " - "rgb=%f->%f", - rgbStamp, rgb->header.stamp.toSec()); + NODELET_ERROR( + "Input stamps changed between the beginning and the end of the callback! Make " + "sure the node publishing the topics doesn't override the same data after publishing them. A " + "solution is to use this node within another nodelet manager. "); + NODELET_ERROR( + "Stamps: " + "rgb=%f->%f", + rgbStamp, rgb->header.stamp.toSec()); } } -void RgbdSensorsSyncNodelet::callbackRGBDIMU(const sensor_msgs::ImageConstPtr &rgb, - const sensor_msgs::ImageConstPtr &depth, - const sensor_msgs::CameraInfoConstPtr &rgbCameraInfo, - const sensor_msgs::CameraInfoConstPtr &depthCameraInfo, - const sensor_msgs::ImuConstPtr &imu) +void RgbdSensorsSyncNodelet::callbackRGBDIMU(const sensor_msgs::ImageConstPtr& rgb, + const sensor_msgs::ImageConstPtr& depth, + const sensor_msgs::CameraInfoConstPtr& rgbCameraInfo, + const sensor_msgs::CameraInfoConstPtr& depthCameraInfo, + const sensor_msgs::ImuConstPtr& imu) { // ----> Frequency calculation static std::chrono::steady_clock::time_point last_time = std::chrono::steady_clock::now(); @@ -309,20 +311,22 @@ void RgbdSensorsSyncNodelet::callbackRGBDIMU(const sensor_msgs::ImageConstPtr &r if (rgbStamp != rgb->header.stamp.toSec() || imuStamp != imu->header.stamp.toSec()) { - NODELET_ERROR("Input stamps changed between the beginning and the end of the callback! Make " - "sure the node publishing the topics doesn't override the same data after publishing them. A " - "solution is to use this node within another nodelet manager. "); - NODELET_ERROR("Stamps: " - "rgb=%f->%f IMU=%f->%f", - rgbStamp, rgb->header.stamp.toSec(), imuStamp, imu->header.stamp.toSec()); + NODELET_ERROR( + "Input stamps changed between the beginning and the end of the callback! Make " + "sure the node publishing the topics doesn't override the same data after publishing them. A " + "solution is to use this node within another nodelet manager. "); + NODELET_ERROR( + "Stamps: " + "rgb=%f->%f IMU=%f->%f", + rgbStamp, rgb->header.stamp.toSec(), imuStamp, imu->header.stamp.toSec()); } } -void RgbdSensorsSyncNodelet::callbackRGBDMag(const sensor_msgs::ImageConstPtr &rgb, - const sensor_msgs::ImageConstPtr &depth, - const sensor_msgs::CameraInfoConstPtr &rgbCameraInfo, - const sensor_msgs::CameraInfoConstPtr &depthCameraInfo, - const sensor_msgs::MagneticFieldConstPtr &mag) +void RgbdSensorsSyncNodelet::callbackRGBDMag(const sensor_msgs::ImageConstPtr& rgb, + const sensor_msgs::ImageConstPtr& depth, + const sensor_msgs::CameraInfoConstPtr& rgbCameraInfo, + const sensor_msgs::CameraInfoConstPtr& depthCameraInfo, + const sensor_msgs::MagneticFieldConstPtr& mag) { // ----> Frequency calculation static std::chrono::steady_clock::time_point last_time = std::chrono::steady_clock::now(); @@ -367,21 +371,23 @@ void RgbdSensorsSyncNodelet::callbackRGBDMag(const sensor_msgs::ImageConstPtr &r if (rgbStamp != rgb->header.stamp.toSec() || magStamp != mag->header.stamp.toSec()) { - NODELET_ERROR("Input stamps changed between the beginning and the end of the callback! Make " - "sure the node publishing the topics doesn't override the same data after publishing them. A " - "solution is to use this node within another nodelet manager. "); - NODELET_ERROR("Stamps: " - "rgb=%f->%f MAG=%f->%f", - rgbStamp, rgb->header.stamp.toSec(), magStamp, mag->header.stamp.toSec()); + NODELET_ERROR( + "Input stamps changed between the beginning and the end of the callback! Make " + "sure the node publishing the topics doesn't override the same data after publishing them. A " + "solution is to use this node within another nodelet manager. "); + NODELET_ERROR( + "Stamps: " + "rgb=%f->%f MAG=%f->%f", + rgbStamp, rgb->header.stamp.toSec(), magStamp, mag->header.stamp.toSec()); } } -void RgbdSensorsSyncNodelet::callbackFull(const sensor_msgs::ImageConstPtr &rgb, - const sensor_msgs::ImageConstPtr &depth, - const sensor_msgs::CameraInfoConstPtr &rgbCameraInfo, - const sensor_msgs::CameraInfoConstPtr &depthCameraInfo, - const sensor_msgs::ImuConstPtr &imu, - const sensor_msgs::MagneticFieldConstPtr &mag) +void RgbdSensorsSyncNodelet::callbackFull(const sensor_msgs::ImageConstPtr& rgb, + const sensor_msgs::ImageConstPtr& depth, + const sensor_msgs::CameraInfoConstPtr& rgbCameraInfo, + const sensor_msgs::CameraInfoConstPtr& depthCameraInfo, + const sensor_msgs::ImuConstPtr& imu, + const sensor_msgs::MagneticFieldConstPtr& mag) { // ----> Frequency calculation static std::chrono::steady_clock::time_point last_time = std::chrono::steady_clock::now(); @@ -431,13 +437,14 @@ void RgbdSensorsSyncNodelet::callbackFull(const sensor_msgs::ImageConstPtr &rgb, if (rgbStamp != rgb->header.stamp.toSec() || imuStamp != imu->header.stamp.toSec() || magStamp != mag->header.stamp.toSec()) { - NODELET_ERROR("Input stamps changed between the beginning and the end of the callback! Make " - "sure the node publishing the topics doesn't override the same data after publishing them. A " - "solution is to use this node within another nodelet manager. "); - NODELET_ERROR("Stamps: " - "rgb=%f->%f IMU=%f->%f MAG=%f->%f", - rgbStamp, rgb->header.stamp.toSec(), imuStamp, imu->header.stamp.toSec(), magStamp, - mag->header.stamp.toSec()); + NODELET_ERROR( + "Input stamps changed between the beginning and the end of the callback! Make " + "sure the node publishing the topics doesn't override the same data after publishing them. A " + "solution is to use this node within another nodelet manager. "); + NODELET_ERROR( + "Stamps: " + "rgb=%f->%f IMU=%f->%f MAG=%f->%f", + rgbStamp, rgb->header.stamp.toSec(), imuStamp, imu->header.stamp.toSec(), magStamp, mag->header.stamp.toSec()); } } diff --git a/zed_nodelets/src/tools/include/sl_tools.h b/zed_nodelets/src/tools/include/sl_tools.h index b0c38188..7e2e649f 100644 --- a/zed_nodelets/src/tools/include/sl_tools.h +++ b/zed_nodelets/src/tools/include/sl_tools.h @@ -76,14 +76,13 @@ bool isZEDX(sl::MODEL camModel); * \param poly the ROI polygon. Coordinates must be normalized from 0.0 to 1.0 * \param out_roi the `sl::Mat` containing the ROI */ -bool generateROI(const std::vector & poly, sl::Mat & out_roi); +bool generateROI(const std::vector& poly, sl::Mat& out_roi); /*! \brief Parse a vector of vector of floats from a string. * \param input * \param error_return * Syntax is [[1.0, 2.0], [3.3, 4.4, 5.5], ...] */ -std::vector> parseStringVector( - const std::string & input, std::string & error_return); +std::vector> parseStringVector(const std::string& input, std::string& error_return); /*! \brief sl::Mat to ros message conversion * \param imgMsgPtr : the image topic message to publish diff --git a/zed_nodelets/src/tools/src/sl_tools.cpp b/zed_nodelets/src/tools/src/sl_tools.cpp index d913f6d4..6687bbb6 100644 --- a/zed_nodelets/src/tools/src/sl_tools.cpp +++ b/zed_nodelets/src/tools/src/sl_tools.cpp @@ -39,12 +39,11 @@ bool file_exist(const std::string& name) namespace fs = std::experimental::filesystem; std::string resolveFilePath(std::string file_path) { - if(file_path.empty()) + if (file_path.empty()) { return file_path; } - std::string abs_path = file_path; if (file_path[0] == '~') { @@ -74,7 +73,7 @@ std::string resolveFilePath(std::string file_path) return std::string(); } } - else if(file_path[0] != '/') + else if (file_path[0] != '/') { fs::path current_path = fs::current_path(); abs_path = current_path.string() + "/" + file_path; @@ -123,7 +122,8 @@ ros::Time slTime2Ros(sl::Timestamp t) bool isZED(sl::MODEL camModel) { - if (camModel == sl::MODEL::ZED) { + if (camModel == sl::MODEL::ZED) + { return true; } return false; @@ -131,7 +131,8 @@ bool isZED(sl::MODEL camModel) bool isZEDM(sl::MODEL camModel) { - if (camModel == sl::MODEL::ZED_M) { + if (camModel == sl::MODEL::ZED_M) + { return true; } return false; @@ -139,10 +140,12 @@ bool isZEDM(sl::MODEL camModel) bool isZED2OrZED2i(sl::MODEL camModel) { - if (camModel == sl::MODEL::ZED2) { + if (camModel == sl::MODEL::ZED2) + { return true; } - if (camModel == sl::MODEL::ZED2i) { + if (camModel == sl::MODEL::ZED2i) + { return true; } return false; @@ -150,10 +153,12 @@ bool isZED2OrZED2i(sl::MODEL camModel) bool isZEDX(sl::MODEL camModel) { - if (camModel == sl::MODEL::ZED_X) { + if (camModel == sl::MODEL::ZED_X) + { return true; } - if (camModel == sl::MODEL::ZED_XM) { + if (camModel == sl::MODEL::ZED_XM) + { return true; } return false; @@ -351,16 +356,15 @@ std::vector split_string(const std::string& s, char seperator) return output; } -inline bool contains(std::vector & poly, sl::float2 test) +inline bool contains(std::vector& poly, sl::float2 test) { int i, j; bool c = false; const int nvert = poly.size(); - for (i = 0, j = nvert - 1; i < nvert; j = i++) { - if ( - ((poly[i].y > test.y) != (poly[j].y > test.y)) && - (test.x < - (poly[j].x - poly[i].x) * (test.y - poly[i].y) / (poly[j].y - poly[i].y) + poly[i].x)) + for (i = 0, j = nvert - 1; i < nvert; j = i++) + { + if (((poly[i].y > test.y) != (poly[j].y > test.y)) && + (test.x < (poly[j].x - poly[i].x) * (test.y - poly[i].y) / (poly[j].y - poly[i].y) + poly[i].x)) { c = !c; } @@ -368,9 +372,10 @@ inline bool contains(std::vector & poly, sl::float2 test) return c; } -bool generateROI(const std::vector & poly, sl::Mat & out_roi) +bool generateROI(const std::vector& poly, sl::Mat& out_roi) { - if (poly.size() < 3) { + if (poly.size() < 3) + { out_roi = sl::Mat(); return false; } @@ -387,15 +392,18 @@ bool generateROI(const std::vector & poly, sl::Mat & out_roi) // std::cerr << "Image resolution: " << w << "x" << h << std::endl; std::vector poly_img; size_t idx = 0; - for (auto & it : poly) { + for (auto& it : poly) + { sl::float2 pt; pt.x = it.x * w; pt.y = it.y * h; - if (pt.x >= w) { + if (pt.x >= w) + { pt.x = (w - 1); } - if (pt.y >= h) { + if (pt.y >= h) + { pt.y = (h - 1); } @@ -406,11 +414,14 @@ bool generateROI(const std::vector & poly, sl::Mat & out_roi) // <---- De-normalize coordinates // ----> Unset ROI pixels outside the polygon - //std::cerr << "Unset ROI pixels outside the polygon" << std::endl; - //std::cerr << "Set mask" << std::endl; - for (int v = 0; v < h; v++) { - for (int u = 0; u < w; u++) { - if (!contains(poly_img, sl::float2(u, v))) { + // std::cerr << "Unset ROI pixels outside the polygon" << std::endl; + // std::cerr << "Set mask" << std::endl; + for (int v = 0; v < h; v++) + { + for (int u = 0; u < w; u++) + { + if (!contains(poly_img, sl::float2(u, v))) + { out_roi.setValue(u, v, 0, sl::MEM::CPU); } } @@ -422,21 +433,23 @@ bool generateROI(const std::vector & poly, sl::Mat & out_roi) return true; } -std::vector> parseStringVector( - const std::string & input, std::string & error_return) +std::vector> parseStringVector(const std::string& input, std::string& error_return) { std::vector> result; std::stringstream input_ss(input); int depth = 0; std::vector current_vector; - while (!!input_ss && !input_ss.eof()) { - switch (input_ss.peek()) { + while (!!input_ss && !input_ss.eof()) + { + switch (input_ss.peek()) + { case EOF: break; case '[': depth++; - if (depth > 2) { + if (depth > 2) + { error_return = "Array depth greater than 2"; return result; } @@ -445,12 +458,14 @@ std::vector> parseStringVector( break; case ']': depth--; - if (depth < 0) { + if (depth < 0) + { error_return = "More close ] than open ["; return result; } input_ss.get(); - if (depth == 1) { + if (depth == 1) + { result.push_back(current_vector); } break; @@ -460,7 +475,8 @@ std::vector> parseStringVector( input_ss.get(); break; default: // All other characters should be part of the numbers. - if (depth != 2) { + if (depth != 2) + { std::stringstream err_ss; err_ss << "Numbers at depth other than 2. Char was '" << char(input_ss.peek()) << "'."; error_return = err_ss.str(); @@ -468,16 +484,20 @@ std::vector> parseStringVector( } float value; input_ss >> value; - if (!!input_ss) { + if (!!input_ss) + { current_vector.push_back(value); } break; } } - if (depth != 0) { + if (depth != 0) + { error_return = "Unterminated vector string."; - } else { + } + else + { error_return = ""; } 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 00dc5a72..73354370 100644 --- a/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp +++ b/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp @@ -78,130 +78,131 @@ #include #include -namespace zed_nodelets { - +namespace zed_nodelets +{ typedef enum { NATIVE, //!< Same camera grab resolution CUSTOM //!< Custom Rescale Factor } PubRes; -class ZEDWrapperNodelet : public nodelet::Nodelet { - typedef enum _dyn_params { - DATAPUB_FREQ = 0, - CONFIDENCE = 1, - TEXTURE_CONF = 2, - POINTCLOUD_FREQ = 3, - BRIGHTNESS = 4, - CONTRAST = 5, - HUE = 6, - SATURATION = 7, - SHARPNESS = 8, - GAMMA = 9, - AUTO_EXP_GAIN = 10, - GAIN = 11, - EXPOSURE = 12, - AUTO_WB = 13, - WB_TEMP = 14 - } DynParams; +class ZEDWrapperNodelet : public nodelet::Nodelet +{ + typedef enum _dyn_params + { + DATAPUB_FREQ = 0, + CONFIDENCE = 1, + TEXTURE_CONF = 2, + POINTCLOUD_FREQ = 3, + BRIGHTNESS = 4, + CONTRAST = 5, + HUE = 6, + SATURATION = 7, + SHARPNESS = 8, + GAMMA = 9, + AUTO_EXP_GAIN = 10, + GAIN = 11, + EXPOSURE = 12, + AUTO_WB = 13, + WB_TEMP = 14 + } DynParams; public: - /*! \brief Default constructor + /*! \brief Default constructor */ - ZEDWrapperNodelet(); + ZEDWrapperNodelet(); - /*! \brief \ref ZEDWrapperNodelet destructor + /*! \brief \ref ZEDWrapperNodelet destructor */ - virtual ~ZEDWrapperNodelet(); + virtual ~ZEDWrapperNodelet(); protected: - /*! \brief Initialization function called by the Nodelet base class + /*! \brief Initialization function called by the Nodelet base class */ - virtual void onInit(); + virtual void onInit(); - /*! \brief Reads parameters from the param server + /*! \brief Reads parameters from the param server */ - void readParameters(); + void readParameters(); - /*! \brief Reads general parameters from the param server + /*! \brief Reads general parameters from the param server */ - void readGeneralParams(); + void readGeneralParams(); - /*! \brief Reads depth parameters from the param server + /*! \brief Reads depth parameters from the param server */ - void readDepthParams(); + void readDepthParams(); - /*! \brief Reads positional tracking parameters from the param server + /*! \brief Reads positional tracking parameters from the param server */ - void readPosTrkParams(); + void readPosTrkParams(); - /*! \brief Reads spatial mapping parameters from the param server + /*! \brief Reads spatial mapping parameters from the param server */ - void readMappingParams(); + void readMappingParams(); - /*! \brief Reads object detection parameters from the param server + /*! \brief Reads object detection parameters from the param server */ - void readObjDetParams(); + void readObjDetParams(); - /*! \brief Reads sensors parameters from the param server + /*! \brief Reads sensors parameters from the param server */ - void readSensParams(); + void readSensParams(); - /*! \brief Reads SVO parameters from the param server + /*! \brief Reads SVO parameters from the param server */ - void readSvoParams(); + void readSvoParams(); - /*! \brief Reads dynamic parameters from the param server + /*! \brief Reads dynamic parameters from the param server */ - void readDynParams(); + void readDynParams(); - /*! \brief ZED camera polling thread function + /*! \brief ZED camera polling thread function */ - void device_poll_thread_func(); + void device_poll_thread_func(); - /*! \brief Pointcloud publishing function + /*! \brief Pointcloud publishing function */ - void pointcloud_thread_func(); + void pointcloud_thread_func(); - /*! \brief Sensors data publishing function + /*! \brief Sensors data publishing function */ - void sensors_thread_func(); + void sensors_thread_func(); - /*! \brief Publish the pose of the camera in "Map" frame with a ros Publisher + /*! \brief Publish the pose of the camera in "Map" frame with a ros Publisher * \param t : the ros::Time to stamp the image */ - void - publishPose(ros::Time t); + void publishPose(ros::Time t); - /*! \brief Publish the pose of the camera in "Odom" frame with a ros Publisher + /*! \brief Publish the pose of the camera in "Odom" frame with a ros Publisher * \param base2odomTransf : Transformation representing the camera pose * from base frame to odom frame * \param slPose : latest odom pose from ZED SDK * \param t : the ros::Time to stamp the image */ - void publishOdom(tf2::Transform odom2baseTransf, sl::Pose& slPose, ros::Time t); + void publishOdom(tf2::Transform odom2baseTransf, sl::Pose& slPose, ros::Time t); - /*! \brief Publish the pose of the camera in "Map" frame as a transformation + /*! \brief Publish the pose of the camera in "Map" frame as a transformation * \param baseTransform : Transformation representing the camera pose from * odom frame to map frame * \param t : the ros::Time to stamp the image */ - void publishPoseFrame(tf2::Transform baseTransform, ros::Time t); + void publishPoseFrame(tf2::Transform baseTransform, ros::Time t); - /*! \brief Publish the odometry of the camera in "Odom" frame as a + /*! \brief Publish the odometry of the camera in "Odom" frame as a * transformation * \param odomTransf : Transformation representing the camera pose from * base frame to odom frame * \param t : the ros::Time to stamp the image */ - void publishOdomFrame(tf2::Transform odomTransf, ros::Time t); + void publishOdomFrame(tf2::Transform odomTransf, ros::Time t); - /*! + /*! * \brief Publish IMU frame once as static TF */ - void publishStaticImuFrame(); + void publishStaticImuFrame(); - /*! \brief Publish a sl::Mat image with a ros Publisher + /*! \brief Publish a sl::Mat image with a ros Publisher * \param imgMsgPtr : the image message to publish * \param img : the image to publish * \param pubImg : the publisher object to use (different image publishers @@ -211,48 +212,48 @@ class ZEDWrapperNodelet : public nodelet::Nodelet { * image frames exist) * \param t : the ros::Time to stamp the image */ - void publishImage(sensor_msgs::ImagePtr imgMsgPtr, sl::Mat img, image_transport::CameraPublisher& pubImg, - sensor_msgs::CameraInfoPtr camInfoMsg, std::string imgFrameId, ros::Time t); + void publishImage(sensor_msgs::ImagePtr imgMsgPtr, sl::Mat img, image_transport::CameraPublisher& pubImg, + sensor_msgs::CameraInfoPtr camInfoMsg, std::string imgFrameId, ros::Time t); - /*! \brief Publish a sl::Mat depth image with a ros Publisher + /*! \brief Publish a sl::Mat depth image with a ros Publisher * \param imgMsgPtr : the depth image topic message to publish * \param depth : the depth image to publish * \param t : the ros::Time to stamp the depth image */ - void publishDepth(sensor_msgs::ImagePtr imgMsgPtr, sl::Mat depth, ros::Time t); + void publishDepth(sensor_msgs::ImagePtr imgMsgPtr, sl::Mat depth, ros::Time t); - /*! \brief Publish a single pointCloud with a ros Publisher + /*! \brief Publish a single pointCloud with a ros Publisher */ - void publishPointCloud(); + void publishPointCloud(); - /*! \brief Publish a fused pointCloud with a ros Publisher + /*! \brief Publish a fused pointCloud with a ros Publisher */ - void callback_pubFusedPointCloud(const ros::TimerEvent& e); + void callback_pubFusedPointCloud(const ros::TimerEvent& e); - /*! - * @brief Publish Color and Depth images - */ - void pubVideoDepth(); + /*! + * @brief Publish Color and Depth images + */ + void pubVideoDepth(); - /*! \brief Publish the informations of a camera with a ros Publisher + /*! \brief Publish the informations of a camera with a ros Publisher * \param cam_info_msg : the information message to publish * \param pub_cam_info : the publisher object to use * \param t : the ros::Time to stamp the message */ - void publishCamInfo(sensor_msgs::CameraInfoPtr camInfoMsg, ros::Publisher pubCamInfo, ros::Time t); + void publishCamInfo(sensor_msgs::CameraInfoPtr camInfoMsg, ros::Publisher pubCamInfo, ros::Time t); - /*! \brief Publish a sl::Mat disparity image with a ros Publisher + /*! \brief Publish a sl::Mat disparity image with a ros Publisher * \param disparity : the disparity image to publish * \param t : the ros::Time to stamp the depth image */ - void publishDisparity(sl::Mat disparity, ros::Time t); + void publishDisparity(sl::Mat disparity, ros::Time t); - /*! \brief Publish sensors data and TF + /*! \brief Publish sensors data and TF * \param t : the ros::Time to stamp the depth image */ - void publishSensData(ros::Time t = ros::Time(0)); + void publishSensData(ros::Time t = ros::Time(0)); - /*! \brief Get the information of the ZED cameras and store them in an + /*! \brief Get the information of the ZED cameras and store them in an * information message * \param zed : the sl::zed::Camera* pointer to an instance * \param left_cam_info_msg : the information message to fill with the left @@ -262,525 +263,523 @@ class ZEDWrapperNodelet : public nodelet::Nodelet { * \param left_frame_id : the id of the reference frame of the left camera * \param right_frame_id : the id of the reference frame of the right camera */ - void fillCamInfo(sl::Camera& zed, sensor_msgs::CameraInfoPtr leftCamInfoMsg, - sensor_msgs::CameraInfoPtr rightCamInfoMsg, std::string leftFrameId, std::string rightFrameId, - bool rawParam = false); + void fillCamInfo(sl::Camera& zed, sensor_msgs::CameraInfoPtr leftCamInfoMsg, + sensor_msgs::CameraInfoPtr rightCamInfoMsg, std::string leftFrameId, std::string rightFrameId, + bool rawParam = false); - /*! \brief Get the information of the ZED cameras and store them in an + /*! \brief Get the information of the ZED cameras and store them in an * information message for depth topics * \param zed : the sl::zed::Camera* pointer to an instance * \param depth_info_msg : the information message to fill with the left * camera informations * \param frame_id : the id of the reference frame of the left camera */ - void fillCamDepthInfo(sl::Camera& zed, sensor_msgs::CameraInfoPtr depth_info_msg, std::string frame_id); + void fillCamDepthInfo(sl::Camera& zed, sensor_msgs::CameraInfoPtr depth_info_msg, std::string frame_id); - - /*! \brief Check if FPS and Resolution chosen by user are correct. + /*! \brief Check if FPS and Resolution chosen by user are correct. * Modifies FPS to match correct value. */ - void checkResolFps(); + void checkResolFps(); - // ----> Region of Interest - std::string getRoiParam(std::string paramName, std::vector> & outVal); - std::string parseRoiPoly( - const std::vector> & in_poly, std::vector & out_poly); - void resetRoi(); - // <---- Region of Interest + // ----> Region of Interest + std::string getRoiParam(std::string paramName, std::vector>& outVal); + std::string parseRoiPoly(const std::vector>& in_poly, std::vector& out_poly); + void resetRoi(); + // <---- Region of Interest - /*! \brief Callback to handle dynamic reconfigure events in ROS + /*! \brief Callback to handle dynamic reconfigure events in ROS */ - void callback_dynamicReconf(zed_nodelets::ZedConfig& config, uint32_t level); + void callback_dynamicReconf(zed_nodelets::ZedConfig& config, uint32_t level); - /*! \brief Callback to publish Path data with a ROS publisher. + /*! \brief Callback to publish Path data with a ROS publisher. * \param e : the ros::TimerEvent binded to the callback */ - void callback_pubPath(const ros::TimerEvent& e); + void callback_pubPath(const ros::TimerEvent& e); - /*! \brief Callback to update node diagnostic status + /*! \brief Callback to update node diagnostic status * \param stat : node status */ - void callback_updateDiagnostic(diagnostic_updater::DiagnosticStatusWrapper& stat); + void callback_updateDiagnostic(diagnostic_updater::DiagnosticStatusWrapper& stat); - /*! \brief Callback to receive geometry_msgs::PointStamped topics + /*! \brief Callback to receive geometry_msgs::PointStamped topics * \param msg : pointer to the received message */ - void clickedPtCallback(geometry_msgs::PointStampedConstPtr msg); + void clickedPtCallback(geometry_msgs::PointStampedConstPtr msg); - /*! \brief Service callback to reset_tracking service + /*! \brief Service callback to reset_tracking service * Tracking pose is reinitialized to the value available in the ROS Param * server */ - bool on_reset_tracking(zed_interfaces::reset_tracking::Request& req, zed_interfaces::reset_tracking::Response& res); + bool on_reset_tracking(zed_interfaces::reset_tracking::Request& req, zed_interfaces::reset_tracking::Response& res); - /*! \brief Service callback to reset_odometry service + /*! \brief Service callback to reset_odometry service * Odometry is reset to clear drift and odometry frame gets the latest * pose * from ZED tracking. */ - bool on_reset_odometry(zed_interfaces::reset_odometry::Request& req, zed_interfaces::reset_odometry::Response& res); + bool on_reset_odometry(zed_interfaces::reset_odometry::Request& req, zed_interfaces::reset_odometry::Response& res); - /*! \brief Service callback to set_pose service + /*! \brief Service callback to set_pose service * Tracking pose is set to the new values */ - bool on_set_pose(zed_interfaces::set_pose::Request& req, zed_interfaces::set_pose::Response& res); + bool on_set_pose(zed_interfaces::set_pose::Request& req, zed_interfaces::set_pose::Response& res); - /*! \brief Service callback to start_svo_recording service + /*! \brief Service callback to start_svo_recording service */ - bool on_start_svo_recording(zed_interfaces::start_svo_recording::Request& req, - zed_interfaces::start_svo_recording::Response& res); + bool on_start_svo_recording(zed_interfaces::start_svo_recording::Request& req, + zed_interfaces::start_svo_recording::Response& res); - /*! \brief Service callback to stop_svo_recording service + /*! \brief Service callback to stop_svo_recording service */ - bool on_stop_svo_recording(zed_interfaces::stop_svo_recording::Request& req, - zed_interfaces::stop_svo_recording::Response& res); + bool on_stop_svo_recording(zed_interfaces::stop_svo_recording::Request& req, + zed_interfaces::stop_svo_recording::Response& res); - /*! \brief Service callback to start_remote_stream service + /*! \brief Service callback to start_remote_stream service */ - bool on_start_remote_stream(zed_interfaces::start_remote_stream::Request& req, - zed_interfaces::start_remote_stream::Response& res); + bool on_start_remote_stream(zed_interfaces::start_remote_stream::Request& req, + zed_interfaces::start_remote_stream::Response& res); - /*! \brief Service callback to stop_remote_stream service + /*! \brief Service callback to stop_remote_stream service */ - bool on_stop_remote_stream(zed_interfaces::stop_remote_stream::Request& req, - zed_interfaces::stop_remote_stream::Response& res); + bool on_stop_remote_stream(zed_interfaces::stop_remote_stream::Request& req, + zed_interfaces::stop_remote_stream::Response& res); - /*! \brief Service callback to set_led_status service + /*! \brief Service callback to set_led_status service */ - bool on_set_led_status(zed_interfaces::set_led_status::Request& req, zed_interfaces::set_led_status::Response& res); + bool on_set_led_status(zed_interfaces::set_led_status::Request& req, zed_interfaces::set_led_status::Response& res); - /*! \brief Service callback to toggle_led service + /*! \brief Service callback to toggle_led service */ - bool on_toggle_led(zed_interfaces::toggle_led::Request& req, zed_interfaces::toggle_led::Response& res); + bool on_toggle_led(zed_interfaces::toggle_led::Request& req, zed_interfaces::toggle_led::Response& res); - /*! \brief Service callback to start_3d_mapping service + /*! \brief Service callback to start_3d_mapping service */ - bool on_start_3d_mapping(zed_interfaces::start_3d_mapping::Request& req, - zed_interfaces::start_3d_mapping::Response& res); + bool on_start_3d_mapping(zed_interfaces::start_3d_mapping::Request& req, + zed_interfaces::start_3d_mapping::Response& res); - /*! \brief Service callback to stop_3d_mapping service + /*! \brief Service callback to stop_3d_mapping service */ - bool on_stop_3d_mapping(zed_interfaces::stop_3d_mapping::Request& req, - zed_interfaces::stop_3d_mapping::Response& res); + bool on_stop_3d_mapping(zed_interfaces::stop_3d_mapping::Request& req, + zed_interfaces::stop_3d_mapping::Response& res); - /*! \brief Service callback to save_3d_map service + /*! \brief Service callback to save_3d_map service */ - bool on_save_3d_map(zed_interfaces::save_3d_map::Request& req, zed_interfaces::save_3d_map::Response& res); + bool on_save_3d_map(zed_interfaces::save_3d_map::Request& req, zed_interfaces::save_3d_map::Response& res); - /*! \brief Service callback to start_object_detection service + /*! \brief Service callback to start_object_detection service */ - bool on_start_object_detection(zed_interfaces::start_object_detection::Request& req, - zed_interfaces::start_object_detection::Response& res); + bool on_start_object_detection(zed_interfaces::start_object_detection::Request& req, + zed_interfaces::start_object_detection::Response& res); - /*! \brief Service callback to stop_object_detection service + /*! \brief Service callback to stop_object_detection service */ - bool on_stop_object_detection(zed_interfaces::stop_object_detection::Request& req, - zed_interfaces::stop_object_detection::Response& res); + bool on_stop_object_detection(zed_interfaces::stop_object_detection::Request& req, + zed_interfaces::stop_object_detection::Response& res); - /*! \brief Service callback to save_area_memory service + /*! \brief Service callback to save_area_memory service */ - bool on_save_area_memory(zed_interfaces::save_area_memory::Request& req, - zed_interfaces::save_area_memory::Response& res); + bool on_save_area_memory(zed_interfaces::save_area_memory::Request& req, + zed_interfaces::save_area_memory::Response& res); - /*! \brief Utility to initialize the pose variables + /*! \brief Utility to initialize the pose variables */ - bool set_pose(float xt, float yt, float zt, float rr, float pr, float yr); + bool set_pose(float xt, float yt, float zt, float rr, float pr, float yr); - /*! \brief Utility to initialize the most used transforms + /*! \brief Utility to initialize the most used transforms */ - void initTransforms(); + void initTransforms(); - /*! \brief Utility to initialize the static transform from Sensor to Base + /*! \brief Utility to initialize the static transform from Sensor to Base */ - bool getSens2BaseTransform(); + bool getSens2BaseTransform(); - /*! \brief Utility to initialize the static transform from Sensor to Camera + /*! \brief Utility to initialize the static transform from Sensor to Camera */ - bool getSens2CameraTransform(); + bool getSens2CameraTransform(); - /*! \brief Utility to initialize the static transform from Camera to Base + /*! \brief Utility to initialize the static transform from Camera to Base */ - bool getCamera2BaseTransform(); + bool getCamera2BaseTransform(); - /*! \brief Start tracking + /*! \brief Start tracking */ - void start_pos_tracking(); + void start_pos_tracking(); - /*! \brief Start spatial mapping + /*! \brief Start spatial mapping */ - bool start_3d_mapping(); + bool start_3d_mapping(); - /*! \brief Stop spatial mapping + /*! \brief Stop spatial mapping */ - void stop_3d_mapping(); + void stop_3d_mapping(); - /*! \brief Start object detection + /*! \brief Start object detection */ - bool start_obj_detect(); + bool start_obj_detect(); - /*! \brief Stop object detection + /*! \brief Stop object detection */ - void stop_obj_detect(); + void stop_obj_detect(); - /*! \brief Publish object detection results + /*! \brief Publish object detection results */ - void processDetectedObjects(ros::Time t); + void processDetectedObjects(ros::Time t); - /*! \brief Generates an univoque color for each object class ID + /*! \brief Generates an univoque color for each object class ID */ - inline sl::float3 generateColorClass(int idx) - { - sl::float3 clr; - clr.r = static_cast(33 + (idx * 456262)); - clr.g = static_cast(233 + (idx * 1564684)); - clr.b = static_cast(133 + (idx * 76873242)); - return clr / 255.f; - } + inline sl::float3 generateColorClass(int idx) + { + sl::float3 clr; + clr.r = static_cast(33 + (idx * 456262)); + clr.g = static_cast(233 + (idx * 1564684)); + clr.b = static_cast(133 + (idx * 76873242)); + return clr / 255.f; + } - /*! \brief Update Dynamic reconfigure parameters + /*! \brief Update Dynamic reconfigure parameters */ - void updateDynamicReconfigure(); + void updateDynamicReconfigure(); - /*! \brief Save the current area map if positional tracking + /*! \brief Save the current area map if positional tracking * and area memory are active */ - bool saveAreaMap(std::string file_path, std::string* out_msg = nullptr); + bool saveAreaMap(std::string file_path, std::string* out_msg = nullptr); private: - uint64_t mFrameCount = 0; - - // SDK version - int mVerMajor; - int mVerMinor; - int mVerSubMinor; - - // ROS - ros::NodeHandle mNh; - ros::NodeHandle mNhNs; - std::thread mDevicePollThread; - std::thread mPcThread; // Point Cloud thread - std::thread mSensThread; // Sensors data thread - - bool mStopNode = false; - - // Publishers - image_transport::CameraPublisher mPubRgb; // - image_transport::CameraPublisher mPubRawRgb; // - image_transport::CameraPublisher mPubLeft; // - image_transport::CameraPublisher mPubRawLeft; // - image_transport::CameraPublisher mPubRight; // - image_transport::CameraPublisher mPubRawRight; // - image_transport::CameraPublisher mPubDepth; // - image_transport::Publisher mPubStereo; - image_transport::Publisher mPubRawStereo; - - image_transport::CameraPublisher mPubRgbGray; - image_transport::CameraPublisher mPubRawRgbGray; - image_transport::CameraPublisher mPubLeftGray; - image_transport::CameraPublisher mPubRawLeftGray; - image_transport::CameraPublisher mPubRightGray; - image_transport::CameraPublisher mPubRawRightGray; - - ros::Publisher mPubConfMap; // - ros::Publisher mPubDisparity; // - ros::Publisher mPubCloud; - ros::Publisher mPubFusedCloud; - ros::Publisher mPubPose; - ros::Publisher mPubPoseCov; - ros::Publisher mPubOdom; - ros::Publisher mPubOdomPath; - ros::Publisher mPubMapPath; - ros::Publisher mPubImu; - ros::Publisher mPubImuRaw; - ros::Publisher mPubImuTemp; - ros::Publisher mPubImuMag; - // ros::Publisher mPubImuMagRaw; - ros::Publisher mPubPressure; - ros::Publisher mPubTempL; - ros::Publisher mPubTempR; - ros::Publisher mPubCamImuTransf; - - ros::Publisher mPubMarker; // Publisher for Rviz markers - ros::Publisher mPubPlane; // Publisher for detected planes - - // Subscribers - ros::Subscriber mClickedPtSub; - - // Timers - ros::Timer mPathTimer; - ros::Timer mFusedPcTimer; - - // Services - ros::ServiceServer mSrvSetInitPose; - ros::ServiceServer mSrvResetOdometry; - ros::ServiceServer mSrvResetTracking; - ros::ServiceServer mSrvSvoStartRecording; - ros::ServiceServer mSrvSvoStopRecording; - ros::ServiceServer mSrvSvoStartStream; - ros::ServiceServer mSrvSvoStopStream; - ros::ServiceServer mSrvSetLedStatus; - ros::ServiceServer mSrvToggleLed; - ros::ServiceServer mSrvStartMapping; - ros::ServiceServer mSrvStopMapping; - ros::ServiceServer mSrvSave3dMap; - ros::ServiceServer mSrvStartObjDet; - ros::ServiceServer mSrvStopObjDet; - ros::ServiceServer mSrvSaveAreaMemory; - - // ----> Topics (ONLY THOSE NOT CHANGING WHILE NODE RUNS) - // Camera info - sensor_msgs::CameraInfoPtr mRgbCamInfoMsg; - sensor_msgs::CameraInfoPtr mLeftCamInfoMsg; - sensor_msgs::CameraInfoPtr mRightCamInfoMsg; - sensor_msgs::CameraInfoPtr mRgbCamInfoRawMsg; - sensor_msgs::CameraInfoPtr mLeftCamInfoRawMsg; - sensor_msgs::CameraInfoPtr mRightCamInfoRawMsg; - sensor_msgs::CameraInfoPtr mDepthCamInfoMsg; - - geometry_msgs::TransformPtr mCameraImuTransfMgs; - // <---- Topics - - // ROS TF - tf2_ros::TransformBroadcaster mTransformPoseBroadcaster; - tf2_ros::TransformBroadcaster mTransformOdomBroadcaster; - tf2_ros::StaticTransformBroadcaster mStaticTransformImuBroadcaster; - - std::string mRgbFrameId; - std::string mRgbOptFrameId; - - std::string mDepthFrameId; - std::string mDepthOptFrameId; - - std::string mDisparityFrameId; - std::string mDisparityOptFrameId; - - std::string mConfidenceFrameId; - std::string mConfidenceOptFrameId; - - std::string mCloudFrameId; - std::string mPointCloudFrameId; - - std::string mMapFrameId; - std::string mOdometryFrameId; - std::string mBaseFrameId; - std::string mCameraFrameId; - - std::string mRightCamFrameId; - std::string mRightCamOptFrameId; - std::string mLeftCamFrameId; - std::string mLeftCamOptFrameId; - std::string mImuFrameId; - - std::string mBaroFrameId; - std::string mMagFrameId; - std::string mTempLeftFrameId; - std::string mTempRightFrameId; - - bool mPublishTf; - bool mPublishMapTf; - bool mPublishImuTf; - sl::FLIP_MODE mCameraFlip; - bool mCameraSelfCalib; - - // Launch file parameters - std::string mCameraName; - sl::RESOLUTION mCamResol; - int mCamFrameRate; - double mPubFrameRate = 15.; - sl::DEPTH_MODE mDepthMode = sl::DEPTH_MODE::ULTRA; - int mGpuId; - int mZedId; - int mDepthStabilization; - std::string mAreaMemDbPath; - bool mSaveAreaMapOnClosing = true; - std::string mSvoFilepath; - bool mSvoRealtime = true; - std::string mRemoteStreamAddr; - bool mSensTimestampSync; - double mSensPubRate = 400.0; - double mPathPubRate; - int mPathMaxCount; - int mSdkVerbose=1; - std::vector> mRoiParam; - bool mSvoMode = false; - double mCamMinDepth; - double mCamMaxDepth; - std::string mClickedPtTopic; - - // Positional tracking - bool mPosTrackingEnabled = false; - sl::POSITIONAL_TRACKING_MODE mPosTrkMode = sl::POSITIONAL_TRACKING_MODE::QUALITY; - bool mPosTrackingActivated = false; - bool mPosTrackingReady = false; - bool mTwoDMode = false; - double mFixedZValue = 0.0; - bool mFloorAlignment = false; - bool mImuFusion = true; - bool mSetGravityAsOrigin = false; - - // Flags - bool mGrabActive = false; // Indicate if camera grabbing is active (at least one topic subscribed) - sl::ERROR_CODE mConnStatus; - sl::ERROR_CODE mGrabStatus; - sl::POSITIONAL_TRACKING_STATE mPosTrackingStatus; - bool mSensPublishing = false; - bool mPcPublishing = false; - bool mDepthDisabled = false; - - // Last frame time - ros::Time mPrevFrameTimestamp; - ros::Time mFrameTimestamp; - - // Positional Tracking variables - sl::Pose mLastZedPose; // Sensor to Map transform - sl::Transform mInitialPoseSl; - std::vector mInitialBasePose; - std::vector mOdomPath; - std::vector mMapPath; - - // IMU TF - tf2::Transform mLastImuPose; - - // TF Transforms - tf2::Transform mMap2OdomTransf; // Coordinates of the odometry frame in map frame - tf2::Transform mOdom2BaseTransf; // Coordinates of the base in odometry frame - tf2::Transform mMap2BaseTransf; // Coordinates of the base in map frame - tf2::Transform mMap2CameraTransf; // Coordinates of the camera in base frame - tf2::Transform mSensor2BaseTransf; // Coordinates of the base frame in sensor frame - tf2::Transform mSensor2CameraTransf; // Coordinates of the camera frame in sensor frame - tf2::Transform mCamera2BaseTransf; // Coordinates of the base frame in camera frame - - bool mSensor2BaseTransfValid = false; - bool mSensor2CameraTransfValid = false; - bool mCamera2BaseTransfValid = false; - bool mStaticImuFramePublished = false; - - // initialization Transform listener - boost::shared_ptr mTfBuffer; - boost::shared_ptr mTfListener; - - // Zed object - sl::InitParameters mZedParams; - sl::Camera mZed; - unsigned int mZedSerialNumber; - sl::MODEL mZedUserCamModel; // Camera model set by ROS Param - sl::MODEL mZedRealCamModel; // Real camera model by SDK API - unsigned int mCamFwVersion; // Camera FW version - unsigned int mSensFwVersion; // Sensors FW version - - // Dynamic Parameters - int mCamBrightness = 4; - int mCamContrast = 4; - int mCamHue = 0; - int mCamSaturation = 4; - int mCamSharpness = 3; - int mCamGamma = 1; - bool mCamAutoExposure = true; - int mCamGain = 100; - int mCamExposure = 100; - bool mCamAutoWB = true; - int mCamWB = 4200; - - int mCamDepthConfidence = 50; - int mCamDepthTextureConf = 100; - double mPointCloudFreq = 15.; - - PubRes mPubResolution = PubRes::NATIVE; // Use native grab resolution by default - double mCustomDownscaleFactor = 1.0; // Used to rescale data with user factor - - // flags - bool mTriggerAutoExposure = true; - bool mTriggerAutoWB = true; - bool mComputeDepth; - bool mOpenniDepthMode; // 16 bit UC data in mm else 32F in m, for more info -> http://www.ros.org/reps/rep-0118.html - bool mPoseSmoothing = false; // Always disabled. Enable only for AR/VR applications - bool mAreaMemory; - bool mInitOdomWithPose; - bool mResetOdom = false; - bool mUpdateDynParams = false; - bool mPublishingData = false; - - // SVO recording - bool mRecording = false; - sl::RecordingStatus mRecStatus; - sl::SVO_COMPRESSION_MODE mSvoComprMode; - - // Streaming - bool mStreaming = false; - - // Mat - int mCamWidth; - int mCamHeight; - sl::Resolution mMatResol; - - // Thread Sync - std::mutex mCloseZedMutex; - std::mutex mCamDataMutex; - std::mutex mPcMutex; - std::mutex mRecMutex; - std::mutex mPosTrkMutex; - std::mutex mDynParMutex; - std::mutex mMappingMutex; - std::mutex mObjDetMutex; - std::condition_variable mPcDataReadyCondVar; - bool mPcDataReady; - - // Point cloud variables - sl::Mat mCloud; - sl::FusedPointCloud mFusedPC; - ros::Time mPointCloudTime; - - // Dynamic reconfigure - boost::recursive_mutex mDynServerMutex; // To avoid Dynamic Reconfigure Server warning - boost::shared_ptr> mDynRecServer; - - // Diagnostic - float mTempLeft = -273.15f; - float mTempRight = -273.15f; - std::unique_ptr mElabPeriodMean_sec; - std::unique_ptr mGrabPeriodMean_usec; - std::unique_ptr mVideoDepthPeriodMean_sec; - std::unique_ptr mPcPeriodMean_usec; - std::unique_ptr mSensPeriodMean_usec; - std::unique_ptr mObjDetPeriodMean_msec; - - diagnostic_updater::Updater mDiagUpdater; // Diagnostic Updater - - // Camera IMU transform - sl::Transform mSlCamImuTransf; - geometry_msgs::TransformStamped mStaticImuTransformStamped; - - // Spatial mapping - bool mMappingEnabled; - bool mMappingRunning; - bool mMapSave = false; - float mMappingRes = 0.1; - float mMaxMappingRange = -1; - double mFusedPcPubFreq = 2.0; - - // Object Detection - bool mObjDetEnabled = false; - bool mObjDetRunning = false; - bool mObjDetTracking = true; - bool mObjDetBodyFitting = true; - float mObjDetConfidence = 50.f; - float mObjDetMaxRange = 10.f; - std::vector mObjDetFilter; - bool mObjDetPeopleEnable = true; - bool mObjDetVehiclesEnable = true; - bool mObjDetBagsEnable = true; - bool mObjDetAnimalsEnable = true; - bool mObjDetElectronicsEnable = true; - bool mObjDetFruitsEnable = true; - bool mObjDetSportsEnable = true; - - sl::OBJECT_DETECTION_MODEL mObjDetModel = sl::OBJECT_DETECTION_MODEL::MULTI_CLASS_BOX_MEDIUM; - - ros::Publisher mPubObjDet; -}; // class ZEDROSWrapperNodelet -} // namespace zed_nodelets + uint64_t mFrameCount = 0; + + // SDK version + int mVerMajor; + int mVerMinor; + int mVerSubMinor; + + // ROS + ros::NodeHandle mNh; + ros::NodeHandle mNhNs; + std::thread mDevicePollThread; + std::thread mPcThread; // Point Cloud thread + std::thread mSensThread; // Sensors data thread + + bool mStopNode = false; + + // Publishers + image_transport::CameraPublisher mPubRgb; // + image_transport::CameraPublisher mPubRawRgb; // + image_transport::CameraPublisher mPubLeft; // + image_transport::CameraPublisher mPubRawLeft; // + image_transport::CameraPublisher mPubRight; // + image_transport::CameraPublisher mPubRawRight; // + image_transport::CameraPublisher mPubDepth; // + image_transport::Publisher mPubStereo; + image_transport::Publisher mPubRawStereo; + + image_transport::CameraPublisher mPubRgbGray; + image_transport::CameraPublisher mPubRawRgbGray; + image_transport::CameraPublisher mPubLeftGray; + image_transport::CameraPublisher mPubRawLeftGray; + image_transport::CameraPublisher mPubRightGray; + image_transport::CameraPublisher mPubRawRightGray; + + ros::Publisher mPubConfMap; // + ros::Publisher mPubDisparity; // + ros::Publisher mPubCloud; + ros::Publisher mPubFusedCloud; + ros::Publisher mPubPose; + ros::Publisher mPubPoseCov; + ros::Publisher mPubOdom; + ros::Publisher mPubOdomPath; + ros::Publisher mPubMapPath; + ros::Publisher mPubImu; + ros::Publisher mPubImuRaw; + ros::Publisher mPubImuTemp; + ros::Publisher mPubImuMag; + // ros::Publisher mPubImuMagRaw; + ros::Publisher mPubPressure; + ros::Publisher mPubTempL; + ros::Publisher mPubTempR; + ros::Publisher mPubCamImuTransf; + + ros::Publisher mPubMarker; // Publisher for Rviz markers + ros::Publisher mPubPlane; // Publisher for detected planes + + // Subscribers + ros::Subscriber mClickedPtSub; + + // Timers + ros::Timer mPathTimer; + ros::Timer mFusedPcTimer; + + // Services + ros::ServiceServer mSrvSetInitPose; + ros::ServiceServer mSrvResetOdometry; + ros::ServiceServer mSrvResetTracking; + ros::ServiceServer mSrvSvoStartRecording; + ros::ServiceServer mSrvSvoStopRecording; + ros::ServiceServer mSrvSvoStartStream; + ros::ServiceServer mSrvSvoStopStream; + ros::ServiceServer mSrvSetLedStatus; + ros::ServiceServer mSrvToggleLed; + ros::ServiceServer mSrvStartMapping; + ros::ServiceServer mSrvStopMapping; + ros::ServiceServer mSrvSave3dMap; + ros::ServiceServer mSrvStartObjDet; + ros::ServiceServer mSrvStopObjDet; + ros::ServiceServer mSrvSaveAreaMemory; + + // ----> Topics (ONLY THOSE NOT CHANGING WHILE NODE RUNS) + // Camera info + sensor_msgs::CameraInfoPtr mRgbCamInfoMsg; + sensor_msgs::CameraInfoPtr mLeftCamInfoMsg; + sensor_msgs::CameraInfoPtr mRightCamInfoMsg; + sensor_msgs::CameraInfoPtr mRgbCamInfoRawMsg; + sensor_msgs::CameraInfoPtr mLeftCamInfoRawMsg; + sensor_msgs::CameraInfoPtr mRightCamInfoRawMsg; + sensor_msgs::CameraInfoPtr mDepthCamInfoMsg; + + geometry_msgs::TransformPtr mCameraImuTransfMgs; + // <---- Topics + + // ROS TF + tf2_ros::TransformBroadcaster mTransformPoseBroadcaster; + tf2_ros::TransformBroadcaster mTransformOdomBroadcaster; + tf2_ros::StaticTransformBroadcaster mStaticTransformImuBroadcaster; + + std::string mRgbFrameId; + std::string mRgbOptFrameId; + + std::string mDepthFrameId; + std::string mDepthOptFrameId; + + std::string mDisparityFrameId; + std::string mDisparityOptFrameId; + + std::string mConfidenceFrameId; + std::string mConfidenceOptFrameId; + + std::string mCloudFrameId; + std::string mPointCloudFrameId; + + std::string mMapFrameId; + std::string mOdometryFrameId; + std::string mBaseFrameId; + std::string mCameraFrameId; + + std::string mRightCamFrameId; + std::string mRightCamOptFrameId; + std::string mLeftCamFrameId; + std::string mLeftCamOptFrameId; + std::string mImuFrameId; + + std::string mBaroFrameId; + std::string mMagFrameId; + std::string mTempLeftFrameId; + std::string mTempRightFrameId; + + bool mPublishTf; + bool mPublishMapTf; + bool mPublishImuTf; + sl::FLIP_MODE mCameraFlip; + bool mCameraSelfCalib; + + // Launch file parameters + std::string mCameraName; + sl::RESOLUTION mCamResol; + int mCamFrameRate; + double mPubFrameRate = 15.; + sl::DEPTH_MODE mDepthMode = sl::DEPTH_MODE::ULTRA; + int mGpuId; + int mZedId; + int mDepthStabilization; + std::string mAreaMemDbPath; + bool mSaveAreaMapOnClosing = true; + std::string mSvoFilepath; + bool mSvoRealtime = true; + std::string mRemoteStreamAddr; + bool mSensTimestampSync; + double mSensPubRate = 400.0; + double mPathPubRate; + int mPathMaxCount; + int mSdkVerbose = 1; + std::vector> mRoiParam; + bool mSvoMode = false; + double mCamMinDepth; + double mCamMaxDepth; + std::string mClickedPtTopic; + + // Positional tracking + bool mPosTrackingEnabled = false; + sl::POSITIONAL_TRACKING_MODE mPosTrkMode = sl::POSITIONAL_TRACKING_MODE::QUALITY; + bool mPosTrackingActivated = false; + bool mPosTrackingReady = false; + bool mTwoDMode = false; + double mFixedZValue = 0.0; + bool mFloorAlignment = false; + bool mImuFusion = true; + bool mSetGravityAsOrigin = false; + + // Flags + bool mGrabActive = false; // Indicate if camera grabbing is active (at least one topic subscribed) + sl::ERROR_CODE mConnStatus; + sl::ERROR_CODE mGrabStatus; + sl::POSITIONAL_TRACKING_STATE mPosTrackingStatus; + bool mSensPublishing = false; + bool mPcPublishing = false; + bool mDepthDisabled = false; + + // Last frame time + ros::Time mPrevFrameTimestamp; + ros::Time mFrameTimestamp; + + // Positional Tracking variables + sl::Pose mLastZedPose; // Sensor to Map transform + sl::Transform mInitialPoseSl; + std::vector mInitialBasePose; + std::vector mOdomPath; + std::vector mMapPath; + + // IMU TF + tf2::Transform mLastImuPose; + + // TF Transforms + tf2::Transform mMap2OdomTransf; // Coordinates of the odometry frame in map frame + tf2::Transform mOdom2BaseTransf; // Coordinates of the base in odometry frame + tf2::Transform mMap2BaseTransf; // Coordinates of the base in map frame + tf2::Transform mMap2CameraTransf; // Coordinates of the camera in base frame + tf2::Transform mSensor2BaseTransf; // Coordinates of the base frame in sensor frame + tf2::Transform mSensor2CameraTransf; // Coordinates of the camera frame in sensor frame + tf2::Transform mCamera2BaseTransf; // Coordinates of the base frame in camera frame + + bool mSensor2BaseTransfValid = false; + bool mSensor2CameraTransfValid = false; + bool mCamera2BaseTransfValid = false; + bool mStaticImuFramePublished = false; + + // initialization Transform listener + boost::shared_ptr mTfBuffer; + boost::shared_ptr mTfListener; + + // Zed object + sl::InitParameters mZedParams; + sl::Camera mZed; + unsigned int mZedSerialNumber; + sl::MODEL mZedUserCamModel; // Camera model set by ROS Param + sl::MODEL mZedRealCamModel; // Real camera model by SDK API + unsigned int mCamFwVersion; // Camera FW version + unsigned int mSensFwVersion; // Sensors FW version + + // Dynamic Parameters + int mCamBrightness = 4; + int mCamContrast = 4; + int mCamHue = 0; + int mCamSaturation = 4; + int mCamSharpness = 3; + int mCamGamma = 1; + bool mCamAutoExposure = true; + int mCamGain = 100; + int mCamExposure = 100; + bool mCamAutoWB = true; + int mCamWB = 4200; + + int mCamDepthConfidence = 50; + int mCamDepthTextureConf = 100; + double mPointCloudFreq = 15.; + + PubRes mPubResolution = PubRes::NATIVE; // Use native grab resolution by default + double mCustomDownscaleFactor = 1.0; // Used to rescale data with user factor + + // flags + bool mTriggerAutoExposure = true; + bool mTriggerAutoWB = true; + bool mComputeDepth; + bool mOpenniDepthMode; // 16 bit UC data in mm else 32F in m, for more info -> http://www.ros.org/reps/rep-0118.html + bool mPoseSmoothing = false; // Always disabled. Enable only for AR/VR applications + bool mAreaMemory; + bool mInitOdomWithPose; + bool mResetOdom = false; + bool mUpdateDynParams = false; + bool mPublishingData = false; + + // SVO recording + bool mRecording = false; + sl::RecordingStatus mRecStatus; + sl::SVO_COMPRESSION_MODE mSvoComprMode; + + // Streaming + bool mStreaming = false; + + // Mat + int mCamWidth; + int mCamHeight; + sl::Resolution mMatResol; + + // Thread Sync + std::mutex mCloseZedMutex; + std::mutex mCamDataMutex; + std::mutex mPcMutex; + std::mutex mRecMutex; + std::mutex mPosTrkMutex; + std::mutex mDynParMutex; + std::mutex mMappingMutex; + std::mutex mObjDetMutex; + std::condition_variable mPcDataReadyCondVar; + bool mPcDataReady; + + // Point cloud variables + sl::Mat mCloud; + sl::FusedPointCloud mFusedPC; + ros::Time mPointCloudTime; + + // Dynamic reconfigure + boost::recursive_mutex mDynServerMutex; // To avoid Dynamic Reconfigure Server warning + boost::shared_ptr> mDynRecServer; + + // Diagnostic + float mTempLeft = -273.15f; + float mTempRight = -273.15f; + std::unique_ptr mElabPeriodMean_sec; + std::unique_ptr mGrabPeriodMean_usec; + std::unique_ptr mVideoDepthPeriodMean_sec; + std::unique_ptr mPcPeriodMean_usec; + std::unique_ptr mSensPeriodMean_usec; + std::unique_ptr mObjDetPeriodMean_msec; + + diagnostic_updater::Updater mDiagUpdater; // Diagnostic Updater + + // Camera IMU transform + sl::Transform mSlCamImuTransf; + geometry_msgs::TransformStamped mStaticImuTransformStamped; + + // Spatial mapping + bool mMappingEnabled; + bool mMappingRunning; + bool mMapSave = false; + float mMappingRes = 0.1; + float mMaxMappingRange = -1; + double mFusedPcPubFreq = 2.0; + + // Object Detection + bool mObjDetEnabled = false; + bool mObjDetRunning = false; + bool mObjDetTracking = true; + bool mObjDetBodyFitting = true; + float mObjDetConfidence = 50.f; + float mObjDetMaxRange = 10.f; + std::vector mObjDetFilter; + bool mObjDetPeopleEnable = true; + bool mObjDetVehiclesEnable = true; + bool mObjDetBagsEnable = true; + bool mObjDetAnimalsEnable = true; + bool mObjDetElectronicsEnable = true; + bool mObjDetFruitsEnable = true; + bool mObjDetSportsEnable = true; + + sl::OBJECT_DETECTION_MODEL mObjDetModel = sl::OBJECT_DETECTION_MODEL::MULTI_CLASS_BOX_MEDIUM; + + ros::Publisher mPubObjDet; +}; // class ZEDROSWrapperNodelet +} // namespace zed_nodelets #include PLUGINLIB_EXPORT_CLASS(zed_nodelets::ZEDWrapperNodelet, nodelet::Nodelet); -#endif // ZED_WRAPPER_NODELET_H +#endif // ZED_WRAPPER_NODELET_H 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 3e93a2fb..717357e8 100644 --- a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp @@ -36,7 +36,8 @@ //#define DEBUG_SENS_TS 1 -namespace zed_nodelets { +namespace zed_nodelets +{ #ifndef DEG2RAD #define DEG2RAD 0.017453293 #define RAD2DEG 57.295777937 @@ -45,1093 +46,1244 @@ namespace zed_nodelets { #define MAG_FREQ 50. #define BARO_FREQ 25. -ZEDWrapperNodelet::ZEDWrapperNodelet() - : Nodelet() +ZEDWrapperNodelet::ZEDWrapperNodelet() : Nodelet() { } ZEDWrapperNodelet::~ZEDWrapperNodelet() { - if (mDevicePollThread.joinable()) { - mDevicePollThread.join(); - } + if (mDevicePollThread.joinable()) + { + mDevicePollThread.join(); + } - if (mPcThread.joinable()) { - mPcThread.join(); - } + if (mPcThread.joinable()) + { + mPcThread.join(); + } - if (mSensThread.joinable()) { - mSensThread.join(); - } + if (mSensThread.joinable()) + { + mSensThread.join(); + } - std::cerr << "ZED Nodelet destroyed" << std::endl; + std::cerr << "ZED Nodelet destroyed" << std::endl; } void ZEDWrapperNodelet::onInit() { - // Node handlers - mNh = getMTNodeHandle(); + // Node handlers + mNh = getMTNodeHandle(); - mNhNs = getMTPrivateNodeHandle(); - mStopNode = false; - mPcDataReady = false; + mNhNs = getMTPrivateNodeHandle(); + mStopNode = false; + mPcDataReady = false; #ifndef NDEBUG - if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug)) { - ros::console::notifyLoggerLevelsChanged(); - } + if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug)) + { + ros::console::notifyLoggerLevelsChanged(); + } #endif - NODELET_INFO("********** Starting nodelet '%s' **********", getName().c_str()); + NODELET_INFO("********** Starting nodelet '%s' **********", getName().c_str()); - std::string ver = sl_tools::getSDKVersion(mVerMajor, mVerMinor, mVerSubMinor); - NODELET_INFO_STREAM("SDK version: " << ver); + std::string ver = sl_tools::getSDKVersion(mVerMajor, mVerMinor, mVerSubMinor); + NODELET_INFO_STREAM("SDK version: " << ver); - if (mVerMajor < 3) { - NODELET_ERROR("This version of the ZED ROS Wrapper is designed to be used with ZED SDK v3.x"); - ros::shutdown(); - raise(SIGINT); - raise(SIGABRT); - exit(-1); - } + if (mVerMajor < 3) + { + NODELET_ERROR("This version of the ZED ROS Wrapper is designed to be used with ZED SDK v3.x"); + ros::shutdown(); + raise(SIGINT); + raise(SIGABRT); + exit(-1); + } - readParameters(); - - initTransforms(); - - // Set the video topic names - std::string rgbTopicRoot = "rgb"; - std::string rightTopicRoot = "right"; - std::string leftTopicRoot = "left"; - std::string stereoTopicRoot = "stereo"; - std::string img_topic = "/image_rect_color"; - std::string img_raw_topic = "/image_raw_color"; - std::string img_gray_topic = "/image_rect_gray"; - std::string img_raw_gray_topic_ = "/image_raw_gray"; - std::string raw_suffix = "_raw"; - std::string left_topic = leftTopicRoot + img_topic; - std::string left_raw_topic = leftTopicRoot + raw_suffix + img_raw_topic; - std::string right_topic = rightTopicRoot + img_topic; - std::string right_raw_topic = rightTopicRoot + raw_suffix + img_raw_topic; - std::string rgb_topic = rgbTopicRoot + img_topic; - std::string rgb_raw_topic = rgbTopicRoot + raw_suffix + img_raw_topic; - std::string stereo_topic = stereoTopicRoot + img_topic; - std::string stereo_raw_topic = stereoTopicRoot + raw_suffix + img_raw_topic; - std::string left_gray_topic = leftTopicRoot + img_gray_topic; - std::string left_raw_gray_topic = leftTopicRoot + raw_suffix + img_raw_gray_topic_; - std::string right_gray_topic = rightTopicRoot + img_gray_topic; - std::string right_raw_gray_topic = rightTopicRoot + raw_suffix + img_raw_gray_topic_; - std::string rgb_gray_topic = rgbTopicRoot + img_gray_topic; - std::string rgb_raw_gray_topic = rgbTopicRoot + raw_suffix + img_raw_gray_topic_; - - // Set the disparity topic name - std::string disparityTopic = "disparity/disparity_image"; - - // Set the depth topic names - std::string depth_topic_root = "depth"; - - if (mOpenniDepthMode) { - NODELET_INFO_STREAM("Openni depth mode activated -> Units: mm, Encoding: TYPE_16UC1"); - } - depth_topic_root += "/depth_registered"; + readParameters(); + + initTransforms(); + + // Set the video topic names + std::string rgbTopicRoot = "rgb"; + std::string rightTopicRoot = "right"; + std::string leftTopicRoot = "left"; + std::string stereoTopicRoot = "stereo"; + std::string img_topic = "/image_rect_color"; + std::string img_raw_topic = "/image_raw_color"; + std::string img_gray_topic = "/image_rect_gray"; + std::string img_raw_gray_topic_ = "/image_raw_gray"; + std::string raw_suffix = "_raw"; + std::string left_topic = leftTopicRoot + img_topic; + std::string left_raw_topic = leftTopicRoot + raw_suffix + img_raw_topic; + std::string right_topic = rightTopicRoot + img_topic; + std::string right_raw_topic = rightTopicRoot + raw_suffix + img_raw_topic; + std::string rgb_topic = rgbTopicRoot + img_topic; + std::string rgb_raw_topic = rgbTopicRoot + raw_suffix + img_raw_topic; + std::string stereo_topic = stereoTopicRoot + img_topic; + std::string stereo_raw_topic = stereoTopicRoot + raw_suffix + img_raw_topic; + std::string left_gray_topic = leftTopicRoot + img_gray_topic; + std::string left_raw_gray_topic = leftTopicRoot + raw_suffix + img_raw_gray_topic_; + std::string right_gray_topic = rightTopicRoot + img_gray_topic; + std::string right_raw_gray_topic = rightTopicRoot + raw_suffix + img_raw_gray_topic_; + std::string rgb_gray_topic = rgbTopicRoot + img_gray_topic; + std::string rgb_raw_gray_topic = rgbTopicRoot + raw_suffix + img_raw_gray_topic_; + + // Set the disparity topic name + std::string disparityTopic = "disparity/disparity_image"; + + // Set the depth topic names + std::string depth_topic_root = "depth"; + + if (mOpenniDepthMode) + { + NODELET_INFO_STREAM("Openni depth mode activated -> Units: mm, Encoding: TYPE_16UC1"); + } + depth_topic_root += "/depth_registered"; - std::string pointcloud_topic = "point_cloud/cloud_registered"; + std::string pointcloud_topic = "point_cloud/cloud_registered"; - std::string pointcloud_fused_topic = "mapping/fused_cloud"; + std::string pointcloud_fused_topic = "mapping/fused_cloud"; - std::string object_det_topic_root = "obj_det"; - std::string object_det_topic = object_det_topic_root + "/objects"; + std::string object_det_topic_root = "obj_det"; + std::string object_det_topic = object_det_topic_root + "/objects"; - std::string confImgRoot = "confidence"; - std::string conf_map_topic_name = "confidence_map"; - std::string conf_map_topic = confImgRoot + "/" + conf_map_topic_name; - - // Set the positional tracking topic names - std::string poseTopic = "pose"; - std::string pose_cov_topic; - pose_cov_topic = poseTopic + "_with_covariance"; - - std::string odometryTopic = "odom"; - std::string odom_path_topic = "path_odom"; - std::string map_path_topic = "path_map"; - - // Extracted plane topics - std::string marker_topic = "plane_marker"; - std::string plane_topic = "plane"; - - // Create camera info - mRgbCamInfoMsg.reset(new sensor_msgs::CameraInfo()); - mLeftCamInfoMsg.reset(new sensor_msgs::CameraInfo()); - mRightCamInfoMsg.reset(new sensor_msgs::CameraInfo()); - mRgbCamInfoRawMsg.reset(new sensor_msgs::CameraInfo()); - mLeftCamInfoRawMsg.reset(new sensor_msgs::CameraInfo()); - mRightCamInfoRawMsg.reset(new sensor_msgs::CameraInfo()); - mDepthCamInfoMsg.reset(new sensor_msgs::CameraInfo()); - - // Initialization transformation listener - mTfBuffer.reset(new tf2_ros::Buffer); - mTfListener.reset(new tf2_ros::TransformListener(*mTfBuffer)); - - mZedParams.coordinate_system = sl::COORDINATE_SYSTEM::RIGHT_HANDED_Z_UP_X_FWD; - NODELET_INFO_STREAM(" * Camera coordinate system\t-> " << sl::toString(mZedParams.coordinate_system)); - - // Try to initialize the ZED - std::stringstream ss; // Input mode info for logging - if (!mSvoFilepath.empty() || !mRemoteStreamAddr.empty()) { - if (!mSvoFilepath.empty()) { - mZedParams.input.setFromSVOFile(mSvoFilepath.c_str()); - mZedParams.svo_real_time_mode = mSvoRealtime; - - // Input mode info for logging - ss << "SVO - " << mSvoFilepath.c_str(); - } else if (!mRemoteStreamAddr.empty()) { - std::vector configStream = sl_tools::split_string(mRemoteStreamAddr, ':'); - sl::String ip = sl::String(configStream.at(0).c_str()); - - if (configStream.size() == 2) { - mZedParams.input.setFromStream(ip, atoi(configStream.at(1).c_str())); - } else { - mZedParams.input.setFromStream(ip); - } + std::string confImgRoot = "confidence"; + std::string conf_map_topic_name = "confidence_map"; + std::string conf_map_topic = confImgRoot + "/" + conf_map_topic_name; - // Input mode info for logging - ss << "NETWORK STREAM - " << mRemoteStreamAddr.c_str(); - } + // Set the positional tracking topic names + std::string poseTopic = "pose"; + std::string pose_cov_topic; + pose_cov_topic = poseTopic + "_with_covariance"; - mSvoMode = true; - } else { - mZedParams.camera_fps = mCamFrameRate; - mZedParams.grab_compute_capping_fps = static_cast(mPubFrameRate); - mZedParams.camera_resolution = mCamResol; - - if (mZedSerialNumber == 0) { - mZedParams.input.setFromCameraID(mZedId); - - // Input mode info for logging - ss << "LIVE CAMERA with ID " << mZedId; - } else { - mZedParams.input.setFromSerialNumber(mZedSerialNumber); - - // Input mode info for logging - ss << "LIVE CAMERA with SN " << mZedSerialNumber; - } + std::string odometryTopic = "odom"; + std::string odom_path_topic = "path_odom"; + std::string map_path_topic = "path_map"; + + // Extracted plane topics + std::string marker_topic = "plane_marker"; + std::string plane_topic = "plane"; + + // Create camera info + mRgbCamInfoMsg.reset(new sensor_msgs::CameraInfo()); + mLeftCamInfoMsg.reset(new sensor_msgs::CameraInfo()); + mRightCamInfoMsg.reset(new sensor_msgs::CameraInfo()); + mRgbCamInfoRawMsg.reset(new sensor_msgs::CameraInfo()); + mLeftCamInfoRawMsg.reset(new sensor_msgs::CameraInfo()); + mRightCamInfoRawMsg.reset(new sensor_msgs::CameraInfo()); + mDepthCamInfoMsg.reset(new sensor_msgs::CameraInfo()); + + // Initialization transformation listener + mTfBuffer.reset(new tf2_ros::Buffer); + mTfListener.reset(new tf2_ros::TransformListener(*mTfBuffer)); + + mZedParams.coordinate_system = sl::COORDINATE_SYSTEM::RIGHT_HANDED_Z_UP_X_FWD; + NODELET_INFO_STREAM(" * Camera coordinate system\t-> " << sl::toString(mZedParams.coordinate_system)); + + // Try to initialize the ZED + std::stringstream ss; // Input mode info for logging + if (!mSvoFilepath.empty() || !mRemoteStreamAddr.empty()) + { + if (!mSvoFilepath.empty()) + { + mZedParams.input.setFromSVOFile(mSvoFilepath.c_str()); + mZedParams.svo_real_time_mode = mSvoRealtime; + + // Input mode info for logging + ss << "SVO - " << mSvoFilepath.c_str(); + } + else if (!mRemoteStreamAddr.empty()) + { + std::vector configStream = sl_tools::split_string(mRemoteStreamAddr, ':'); + sl::String ip = sl::String(configStream.at(0).c_str()); + + if (configStream.size() == 2) + { + mZedParams.input.setFromStream(ip, atoi(configStream.at(1).c_str())); + } + else + { + mZedParams.input.setFromStream(ip); + } + + // Input mode info for logging + ss << "NETWORK STREAM - " << mRemoteStreamAddr.c_str(); + } + + mSvoMode = true; + } + else + { + mZedParams.camera_fps = mCamFrameRate; + mZedParams.grab_compute_capping_fps = static_cast(mPubFrameRate); + mZedParams.camera_resolution = mCamResol; + + if (mZedSerialNumber == 0) + { + mZedParams.input.setFromCameraID(mZedId); + + // Input mode info for logging + ss << "LIVE CAMERA with ID " << mZedId; } + else + { + mZedParams.input.setFromSerialNumber(mZedSerialNumber); - mZedParams.async_grab_camera_recovery = true; - mZedParams.coordinate_units = sl::UNIT::METER; - mZedParams.depth_mode = static_cast(mDepthMode); - mZedParams.sdk_verbose = mSdkVerbose; - mZedParams.sdk_gpu_id = mGpuId; - mZedParams.depth_stabilization = mDepthStabilization; - mZedParams.camera_image_flip = mCameraFlip; - mZedParams.depth_minimum_distance = static_cast(mCamMinDepth); - mZedParams.depth_maximum_distance = static_cast(mCamMaxDepth); - mZedParams.camera_disable_self_calib = !mCameraSelfCalib; + // Input mode info for logging + ss << "LIVE CAMERA with SN " << mZedSerialNumber; + } + } - mZedParams.enable_image_enhancement = true; // Always active + mZedParams.async_grab_camera_recovery = true; + mZedParams.coordinate_units = sl::UNIT::METER; + mZedParams.depth_mode = static_cast(mDepthMode); + mZedParams.sdk_verbose = mSdkVerbose; + mZedParams.sdk_gpu_id = mGpuId; + mZedParams.depth_stabilization = mDepthStabilization; + mZedParams.camera_image_flip = mCameraFlip; + mZedParams.depth_minimum_distance = static_cast(mCamMinDepth); + mZedParams.depth_maximum_distance = static_cast(mCamMaxDepth); + mZedParams.camera_disable_self_calib = !mCameraSelfCalib; - mDiagUpdater.add("ZED Diagnostic", this, &ZEDWrapperNodelet::callback_updateDiagnostic); - mDiagUpdater.setHardwareID("ZED camera"); + mZedParams.enable_image_enhancement = true; // Always active - mConnStatus = sl::ERROR_CODE::CAMERA_NOT_DETECTED; + mDiagUpdater.add("ZED Diagnostic", this, &ZEDWrapperNodelet::callback_updateDiagnostic); + mDiagUpdater.setHardwareID("ZED camera"); - NODELET_INFO_STREAM(" *** Opening " << sl::toString(mZedUserCamModel) << " - " << ss.str().c_str() << " ***"); - while (mConnStatus != sl::ERROR_CODE::SUCCESS) { - mConnStatus = mZed.open(mZedParams); - NODELET_INFO_STREAM("ZED connection: " << sl::toString(mConnStatus)); - std::this_thread::sleep_for(std::chrono::milliseconds(2000)); + mConnStatus = sl::ERROR_CODE::CAMERA_NOT_DETECTED; - if (!mNhNs.ok()) { - mStopNode = true; // Stops other threads + NODELET_INFO_STREAM(" *** Opening " << sl::toString(mZedUserCamModel) << " - " << ss.str().c_str() << " ***"); + while (mConnStatus != sl::ERROR_CODE::SUCCESS) + { + mConnStatus = mZed.open(mZedParams); + NODELET_INFO_STREAM("ZED connection: " << sl::toString(mConnStatus)); + std::this_thread::sleep_for(std::chrono::milliseconds(2000)); - std::lock_guard lock(mCloseZedMutex); - NODELET_DEBUG("Closing ZED"); - mZed.close(); + if (!mNhNs.ok()) + { + mStopNode = true; // Stops other threads - NODELET_DEBUG("ZED pool thread finished"); - return; - } + std::lock_guard lock(mCloseZedMutex); + NODELET_DEBUG("Closing ZED"); + mZed.close(); - mDiagUpdater.update(); + NODELET_DEBUG("ZED pool thread finished"); + return; } - NODELET_INFO_STREAM(" ... " << sl::toString(mZedRealCamModel) << " ready"); - // CUdevice devid; - cuCtxGetDevice(&mGpuId); + mDiagUpdater.update(); + } + NODELET_INFO_STREAM(" ... " << sl::toString(mZedRealCamModel) << " ready"); + + // CUdevice devid; + cuCtxGetDevice(&mGpuId); - NODELET_INFO_STREAM("ZED SDK running on GPU #" << mGpuId); + NODELET_INFO_STREAM("ZED SDK running on GPU #" << mGpuId); - // Disable AEC_AGC and Auto Whitebalance to trigger it if use set to automatic - mZed.setCameraSettings(sl::VIDEO_SETTINGS::AEC_AGC, 0); - mZed.setCameraSettings(sl::VIDEO_SETTINGS::WHITEBALANCE_AUTO, 0); + // Disable AEC_AGC and Auto Whitebalance to trigger it if use set to automatic + mZed.setCameraSettings(sl::VIDEO_SETTINGS::AEC_AGC, 0); + mZed.setCameraSettings(sl::VIDEO_SETTINGS::WHITEBALANCE_AUTO, 0); - mZedRealCamModel = mZed.getCameraInformation().camera_model; + mZedRealCamModel = mZed.getCameraInformation().camera_model; - if (mZedRealCamModel == sl::MODEL::ZED) { - if (mZedUserCamModel != mZedRealCamModel) { - NODELET_WARN("Camera model does not match user parameter. Please modify " - "the value of the parameter 'camera_model' to 'zed'"); - } - } else if (mZedRealCamModel == sl::MODEL::ZED_M) { - if (mZedUserCamModel != mZedRealCamModel) { - NODELET_WARN("Camera model does not match user parameter. Please modify " - "the value of the parameter 'camera_model' to 'zedm'"); - } - mSlCamImuTransf = mZed.getCameraInformation().sensors_configuration.camera_imu_transform; - NODELET_INFO("Camera-IMU Transform: \n %s", mSlCamImuTransf.getInfos().c_str()); - } else if (mZedRealCamModel == sl::MODEL::ZED2) { - if (mZedUserCamModel != mZedRealCamModel) { - NODELET_WARN("Camera model does not match user parameter. Please modify " - "the value of the parameter 'camera_model' to 'zed2'"); - } - mSlCamImuTransf = mZed.getCameraInformation().sensors_configuration.camera_imu_transform; - NODELET_INFO("Camera-IMU Transform: \n %s", mSlCamImuTransf.getInfos().c_str()); - } else if (mZedRealCamModel == sl::MODEL::ZED2i) { - if (mZedUserCamModel != mZedRealCamModel) { - NODELET_WARN("Camera model does not match user parameter. Please modify " - "the value of the parameter 'camera_model' to 'zed2i'"); - } - mSlCamImuTransf = mZed.getCameraInformation().sensors_configuration.camera_imu_transform; - NODELET_INFO("Camera-IMU Transform: \n %s", mSlCamImuTransf.getInfos().c_str()); - } else if (mZedRealCamModel == sl::MODEL::ZED_X) { - if (mZedUserCamModel != mZedRealCamModel) { - NODELET_WARN("Camera model does not match user parameter. Please modify " - "the value of the parameter 'camera_model' to 'zedx'"); - } - mSlCamImuTransf = mZed.getCameraInformation().sensors_configuration.camera_imu_transform; - NODELET_INFO("Camera-IMU Transform: \n %s", mSlCamImuTransf.getInfos().c_str()); - } else if (mZedRealCamModel == sl::MODEL::ZED_XM) { - if (mZedUserCamModel != mZedRealCamModel) { - NODELET_WARN("Camera model does not match user parameter. Please modify " - "the value of the parameter 'camera_model' to 'zedxm'"); - } - mSlCamImuTransf = mZed.getCameraInformation().sensors_configuration.camera_imu_transform; - NODELET_INFO("Camera-IMU Transform: \n %s", mSlCamImuTransf.getInfos().c_str()); + if (mZedRealCamModel == sl::MODEL::ZED) + { + if (mZedUserCamModel != mZedRealCamModel) + { + NODELET_WARN( + "Camera model does not match user parameter. Please modify " + "the value of the parameter 'camera_model' to 'zed'"); + } + } + else if (mZedRealCamModel == sl::MODEL::ZED_M) + { + if (mZedUserCamModel != mZedRealCamModel) + { + NODELET_WARN( + "Camera model does not match user parameter. Please modify " + "the value of the parameter 'camera_model' to 'zedm'"); + } + mSlCamImuTransf = mZed.getCameraInformation().sensors_configuration.camera_imu_transform; + NODELET_INFO("Camera-IMU Transform: \n %s", mSlCamImuTransf.getInfos().c_str()); + } + else if (mZedRealCamModel == sl::MODEL::ZED2) + { + if (mZedUserCamModel != mZedRealCamModel) + { + NODELET_WARN( + "Camera model does not match user parameter. Please modify " + "the value of the parameter 'camera_model' to 'zed2'"); + } + mSlCamImuTransf = mZed.getCameraInformation().sensors_configuration.camera_imu_transform; + NODELET_INFO("Camera-IMU Transform: \n %s", mSlCamImuTransf.getInfos().c_str()); + } + else if (mZedRealCamModel == sl::MODEL::ZED2i) + { + if (mZedUserCamModel != mZedRealCamModel) + { + NODELET_WARN( + "Camera model does not match user parameter. Please modify " + "the value of the parameter 'camera_model' to 'zed2i'"); + } + mSlCamImuTransf = mZed.getCameraInformation().sensors_configuration.camera_imu_transform; + NODELET_INFO("Camera-IMU Transform: \n %s", mSlCamImuTransf.getInfos().c_str()); + } + else if (mZedRealCamModel == sl::MODEL::ZED_X) + { + if (mZedUserCamModel != mZedRealCamModel) + { + NODELET_WARN( + "Camera model does not match user parameter. Please modify " + "the value of the parameter 'camera_model' to 'zedx'"); } + mSlCamImuTransf = mZed.getCameraInformation().sensors_configuration.camera_imu_transform; + NODELET_INFO("Camera-IMU Transform: \n %s", mSlCamImuTransf.getInfos().c_str()); + } + else if (mZedRealCamModel == sl::MODEL::ZED_XM) + { + if (mZedUserCamModel != mZedRealCamModel) + { + NODELET_WARN( + "Camera model does not match user parameter. Please modify " + "the value of the parameter 'camera_model' to 'zedxm'"); + } + mSlCamImuTransf = mZed.getCameraInformation().sensors_configuration.camera_imu_transform; + NODELET_INFO("Camera-IMU Transform: \n %s", mSlCamImuTransf.getInfos().c_str()); + } - NODELET_INFO_STREAM(" * CAMERA MODEL\t-> " << sl::toString(mZedRealCamModel).c_str()); - mZedSerialNumber = mZed.getCameraInformation().serial_number; - NODELET_INFO_STREAM(" * Serial Number: " << mZedSerialNumber); + NODELET_INFO_STREAM(" * CAMERA MODEL\t-> " << sl::toString(mZedRealCamModel).c_str()); + mZedSerialNumber = mZed.getCameraInformation().serial_number; + NODELET_INFO_STREAM(" * Serial Number: " << mZedSerialNumber); - if (!mSvoMode) { - mCamFwVersion = mZed.getCameraInformation().camera_configuration.firmware_version; - NODELET_INFO_STREAM(" * Camera FW Version: " << mCamFwVersion); - if (mZedRealCamModel != sl::MODEL::ZED) { - mSensFwVersion = mZed.getCameraInformation().sensors_configuration.firmware_version; - NODELET_INFO_STREAM(" * Sensors FW Version: " << mSensFwVersion); - } + if (!mSvoMode) + { + mCamFwVersion = mZed.getCameraInformation().camera_configuration.firmware_version; + NODELET_INFO_STREAM(" * Camera FW Version: " << mCamFwVersion); + if (mZedRealCamModel != sl::MODEL::ZED) + { + mSensFwVersion = mZed.getCameraInformation().sensors_configuration.firmware_version; + NODELET_INFO_STREAM(" * Sensors FW Version: " << mSensFwVersion); } + } + + // Set the IMU topic names using real camera model + std::string imu_topic; + std::string imu_topic_raw; + std::string imu_temp_topic; + std::string imu_mag_topic; + // std::string imu_mag_topic_raw; + std::string pressure_topic; + std::string temp_topic_root = "temperature"; + std::string temp_topic_left = temp_topic_root + "/left"; + std::string temp_topic_right = temp_topic_root + "/right"; + + if (mZedRealCamModel != sl::MODEL::ZED) + { + std::string imuTopicRoot = "imu"; + std::string imu_topic_name = "data"; + std::string imu_topic_raw_name = "data_raw"; + std::string imu_topic_mag_name = "mag"; + // std::string imu_topic_mag_raw_name = "mag_raw"; + std::string pressure_topic_name = "atm_press"; + imu_topic = imuTopicRoot + "/" + imu_topic_name; + imu_topic_raw = imuTopicRoot + "/" + imu_topic_raw_name; + imu_temp_topic = temp_topic_root + "/" + imuTopicRoot; + imu_mag_topic = imuTopicRoot + "/" + imu_topic_mag_name; + // imu_mag_topic_raw = imuTopicRoot + "/" + imu_topic_mag_raw_name; + pressure_topic = /*imuTopicRoot + "/" +*/ pressure_topic_name; + } - // Set the IMU topic names using real camera model - std::string imu_topic; - std::string imu_topic_raw; - std::string imu_temp_topic; - std::string imu_mag_topic; - // std::string imu_mag_topic_raw; - std::string pressure_topic; - std::string temp_topic_root = "temperature"; - std::string temp_topic_left = temp_topic_root + "/left"; - std::string temp_topic_right = temp_topic_root + "/right"; - - if (mZedRealCamModel != sl::MODEL::ZED) { - std::string imuTopicRoot = "imu"; - std::string imu_topic_name = "data"; - std::string imu_topic_raw_name = "data_raw"; - std::string imu_topic_mag_name = "mag"; - // std::string imu_topic_mag_raw_name = "mag_raw"; - std::string pressure_topic_name = "atm_press"; - imu_topic = imuTopicRoot + "/" + imu_topic_name; - imu_topic_raw = imuTopicRoot + "/" + imu_topic_raw_name; - imu_temp_topic = temp_topic_root + "/" + imuTopicRoot; - imu_mag_topic = imuTopicRoot + "/" + imu_topic_mag_name; - // imu_mag_topic_raw = imuTopicRoot + "/" + imu_topic_mag_raw_name; - pressure_topic = /*imuTopicRoot + "/" +*/ pressure_topic_name; + mDiagUpdater.setHardwareIDf("%s - s/n: %d [GPU #%d]", sl::toString(mZedRealCamModel).c_str(), mZedSerialNumber, + mGpuId); + + // ----> Dynamic Reconfigure parameters + mDynRecServer = boost::make_shared>(mDynServerMutex); + dynamic_reconfigure::Server::CallbackType f; + f = boost::bind(&ZEDWrapperNodelet::callback_dynamicReconf, this, _1, _2); + mDynRecServer->setCallback(f); + // Update parameters + zed_nodelets::ZedConfig config; + mDynRecServer->getConfigDefault(config); + mDynServerMutex.lock(); + mDynRecServer->updateConfig(config); + mDynServerMutex.unlock(); + + updateDynamicReconfigure(); + // <---- Dynamic Reconfigure parameters + + // ----> Publishers + NODELET_INFO("*** PUBLISHERS ***"); + + // Image publishers + image_transport::ImageTransport it_zed(mNhNs); + + mPubRgb = it_zed.advertiseCamera(rgb_topic, 1); // rgb + NODELET_INFO_STREAM(" * Advertised on topic " << mPubRgb.getTopic()); + NODELET_INFO_STREAM(" * Advertised on topic " << mPubRgb.getInfoTopic()); + mPubRawRgb = it_zed.advertiseCamera(rgb_raw_topic, 1); // rgb raw + NODELET_INFO_STREAM(" * Advertised on topic " << mPubRawRgb.getTopic()); + NODELET_INFO_STREAM(" * Advertised on topic " << mPubRawRgb.getInfoTopic()); + mPubLeft = it_zed.advertiseCamera(left_topic, 1); // left + NODELET_INFO_STREAM(" * Advertised on topic " << mPubLeft.getTopic()); + NODELET_INFO_STREAM(" * Advertised on topic " << mPubLeft.getInfoTopic()); + mPubRawLeft = it_zed.advertiseCamera(left_raw_topic, 1); // left raw + NODELET_INFO_STREAM(" * Advertised on topic " << mPubRawLeft.getTopic()); + NODELET_INFO_STREAM(" * Advertised on topic " << mPubRawLeft.getInfoTopic()); + mPubRight = it_zed.advertiseCamera(right_topic, 1); // right + NODELET_INFO_STREAM(" * Advertised on topic " << mPubRight.getTopic()); + NODELET_INFO_STREAM(" * Advertised on topic " << mPubRight.getInfoTopic()); + mPubRawRight = it_zed.advertiseCamera(right_raw_topic, 1); // right raw + NODELET_INFO_STREAM(" * Advertised on topic " << mPubRawRight.getTopic()); + NODELET_INFO_STREAM(" * Advertised on topic " << mPubRawRight.getInfoTopic()); + + mPubRgbGray = it_zed.advertiseCamera(rgb_gray_topic, 1); // rgb + NODELET_INFO_STREAM(" * Advertised on topic " << mPubRgbGray.getTopic()); + NODELET_INFO_STREAM(" * Advertised on topic " << mPubRgbGray.getInfoTopic()); + mPubRawRgbGray = it_zed.advertiseCamera(rgb_raw_gray_topic, 1); // rgb raw + NODELET_INFO_STREAM(" * Advertised on topic " << mPubRawRgbGray.getTopic()); + NODELET_INFO_STREAM(" * Advertised on topic " << mPubRawRgbGray.getInfoTopic()); + mPubLeftGray = it_zed.advertiseCamera(left_gray_topic, 1); // left + NODELET_INFO_STREAM(" * Advertised on topic " << mPubLeftGray.getTopic()); + NODELET_INFO_STREAM(" * Advertised on topic " << mPubLeftGray.getInfoTopic()); + mPubRawLeftGray = it_zed.advertiseCamera(left_raw_gray_topic, 1); // left raw + NODELET_INFO_STREAM(" * Advertised on topic " << mPubRawLeftGray.getTopic()); + NODELET_INFO_STREAM(" * Advertised on topic " << mPubRawLeftGray.getInfoTopic()); + mPubRightGray = it_zed.advertiseCamera(right_gray_topic, 1); // right + NODELET_INFO_STREAM(" * Advertised on topic " << mPubRightGray.getTopic()); + NODELET_INFO_STREAM(" * Advertised on topic " << mPubRightGray.getInfoTopic()); + mPubRawRightGray = it_zed.advertiseCamera(right_raw_gray_topic, 1); // right raw + NODELET_INFO_STREAM(" * Advertised on topic " << mPubRawRightGray.getTopic()); + NODELET_INFO_STREAM(" * Advertised on topic " << mPubRawRightGray.getInfoTopic()); + + mPubStereo = it_zed.advertise(stereo_topic, 1); + NODELET_INFO_STREAM(" * Advertised on topic " << mPubStereo.getTopic()); + mPubRawStereo = it_zed.advertise(stereo_raw_topic, 1); + NODELET_INFO_STREAM(" * Advertised on topic " << mPubRawStereo.getTopic()); + + // Detected planes publisher + mPubPlane = mNhNs.advertise(plane_topic, 1); + + if (!mDepthDisabled) + { + mPubDepth = it_zed.advertiseCamera(depth_topic_root, 1); // depth + NODELET_INFO_STREAM(" * Advertised on topic " << mPubDepth.getTopic()); + NODELET_INFO_STREAM(" * Advertised on topic " << mPubDepth.getInfoTopic()); + + // Confidence Map publisher + mPubConfMap = mNhNs.advertise(conf_map_topic, 1); // confidence map + NODELET_INFO_STREAM(" * Advertised on topic " << mPubConfMap.getTopic()); + + // Disparity publisher + mPubDisparity = mNhNs.advertise(disparityTopic, 1); + NODELET_INFO_STREAM(" * Advertised on topic " << mPubDisparity.getTopic()); + + // PointCloud publishers + mPubCloud = mNhNs.advertise(pointcloud_topic, 1); + NODELET_INFO_STREAM(" * Advertised on topic " << mPubCloud.getTopic()); + + if (mMappingEnabled) + { + mPubFusedCloud = mNhNs.advertise(pointcloud_fused_topic, 1); + NODELET_INFO_STREAM(" * Advertised on topic " << mPubFusedCloud.getTopic() << " @ " << mFusedPcPubFreq << " Hz"); } - mDiagUpdater.setHardwareIDf("%s - s/n: %d [GPU #%d]", sl::toString(mZedRealCamModel).c_str(), mZedSerialNumber, - mGpuId); - - // ----> Dynamic Reconfigure parameters - mDynRecServer = boost::make_shared>(mDynServerMutex); - dynamic_reconfigure::Server::CallbackType f; - f = boost::bind(&ZEDWrapperNodelet::callback_dynamicReconf, this, _1, _2); - mDynRecServer->setCallback(f); - // Update parameters - zed_nodelets::ZedConfig config; - mDynRecServer->getConfigDefault(config); - mDynServerMutex.lock(); - mDynRecServer->updateConfig(config); - mDynServerMutex.unlock(); - - updateDynamicReconfigure(); - // <---- Dynamic Reconfigure parameters - - // ----> Publishers - NODELET_INFO( "*** PUBLISHERS ***"); - - // Image publishers - image_transport::ImageTransport it_zed(mNhNs); - - mPubRgb = it_zed.advertiseCamera(rgb_topic, 1); // rgb - NODELET_INFO_STREAM(" * Advertised on topic " << mPubRgb.getTopic()); - NODELET_INFO_STREAM(" * Advertised on topic " << mPubRgb.getInfoTopic()); - mPubRawRgb = it_zed.advertiseCamera(rgb_raw_topic, 1); // rgb raw - NODELET_INFO_STREAM(" * Advertised on topic " << mPubRawRgb.getTopic()); - NODELET_INFO_STREAM(" * Advertised on topic " << mPubRawRgb.getInfoTopic()); - mPubLeft = it_zed.advertiseCamera(left_topic, 1); // left - NODELET_INFO_STREAM(" * Advertised on topic " << mPubLeft.getTopic()); - NODELET_INFO_STREAM(" * Advertised on topic " << mPubLeft.getInfoTopic()); - mPubRawLeft = it_zed.advertiseCamera(left_raw_topic, 1); // left raw - NODELET_INFO_STREAM(" * Advertised on topic " << mPubRawLeft.getTopic()); - NODELET_INFO_STREAM(" * Advertised on topic " << mPubRawLeft.getInfoTopic()); - mPubRight = it_zed.advertiseCamera(right_topic, 1); // right - NODELET_INFO_STREAM(" * Advertised on topic " << mPubRight.getTopic()); - NODELET_INFO_STREAM(" * Advertised on topic " << mPubRight.getInfoTopic()); - mPubRawRight = it_zed.advertiseCamera(right_raw_topic, 1); // right raw - NODELET_INFO_STREAM(" * Advertised on topic " << mPubRawRight.getTopic()); - NODELET_INFO_STREAM(" * Advertised on topic " << mPubRawRight.getInfoTopic()); - - mPubRgbGray = it_zed.advertiseCamera(rgb_gray_topic, 1); // rgb - NODELET_INFO_STREAM(" * Advertised on topic " << mPubRgbGray.getTopic()); - NODELET_INFO_STREAM(" * Advertised on topic " << mPubRgbGray.getInfoTopic()); - mPubRawRgbGray = it_zed.advertiseCamera(rgb_raw_gray_topic, 1); // rgb raw - NODELET_INFO_STREAM(" * Advertised on topic " << mPubRawRgbGray.getTopic()); - NODELET_INFO_STREAM(" * Advertised on topic " << mPubRawRgbGray.getInfoTopic()); - mPubLeftGray = it_zed.advertiseCamera(left_gray_topic, 1); // left - NODELET_INFO_STREAM(" * Advertised on topic " << mPubLeftGray.getTopic()); - NODELET_INFO_STREAM(" * Advertised on topic " << mPubLeftGray.getInfoTopic()); - mPubRawLeftGray = it_zed.advertiseCamera(left_raw_gray_topic, 1); // left raw - NODELET_INFO_STREAM(" * Advertised on topic " << mPubRawLeftGray.getTopic()); - NODELET_INFO_STREAM(" * Advertised on topic " << mPubRawLeftGray.getInfoTopic()); - mPubRightGray = it_zed.advertiseCamera(right_gray_topic, 1); // right - NODELET_INFO_STREAM(" * Advertised on topic " << mPubRightGray.getTopic()); - NODELET_INFO_STREAM(" * Advertised on topic " << mPubRightGray.getInfoTopic()); - mPubRawRightGray = it_zed.advertiseCamera(right_raw_gray_topic, 1); // right raw - NODELET_INFO_STREAM(" * Advertised on topic " << mPubRawRightGray.getTopic()); - NODELET_INFO_STREAM(" * Advertised on topic " << mPubRawRightGray.getInfoTopic()); - - mPubStereo = it_zed.advertise(stereo_topic, 1); - NODELET_INFO_STREAM(" * Advertised on topic " << mPubStereo.getTopic()); - mPubRawStereo = it_zed.advertise(stereo_raw_topic, 1); - NODELET_INFO_STREAM(" * Advertised on topic " << mPubRawStereo.getTopic()); - - // Detected planes publisher - mPubPlane = mNhNs.advertise(plane_topic, 1); - - if (!mDepthDisabled) { - mPubDepth = it_zed.advertiseCamera(depth_topic_root, 1); // depth - NODELET_INFO_STREAM(" * Advertised on topic " << mPubDepth.getTopic()); - NODELET_INFO_STREAM(" * Advertised on topic " << mPubDepth.getInfoTopic()); - - // Confidence Map publisher - mPubConfMap = mNhNs.advertise(conf_map_topic, 1); // confidence map - NODELET_INFO_STREAM(" * Advertised on topic " << mPubConfMap.getTopic()); - - // Disparity publisher - mPubDisparity = mNhNs.advertise(disparityTopic, 1); - NODELET_INFO_STREAM(" * Advertised on topic " << mPubDisparity.getTopic()); - - // PointCloud publishers - mPubCloud = mNhNs.advertise(pointcloud_topic, 1); - NODELET_INFO_STREAM(" * Advertised on topic " << mPubCloud.getTopic()); - - - if (mMappingEnabled) { - mPubFusedCloud = mNhNs.advertise(pointcloud_fused_topic, 1); - NODELET_INFO_STREAM(" * Advertised on topic " << mPubFusedCloud.getTopic() << " @ " << mFusedPcPubFreq << " Hz"); - } + // Object detection publishers + if (mObjDetEnabled) + { + mPubObjDet = mNhNs.advertise(object_det_topic, 1); + NODELET_INFO_STREAM(" * Advertised on topic " << mPubObjDet.getTopic()); + } - // Object detection publishers - if (mObjDetEnabled) { - mPubObjDet = mNhNs.advertise(object_det_topic, 1); - NODELET_INFO_STREAM(" * Advertised on topic " << mPubObjDet.getTopic()); - } + // Odometry and Pose publisher + mPubPose = mNhNs.advertise(poseTopic, 1); + NODELET_INFO_STREAM(" * Advertised on topic " << mPubPose.getTopic()); - // Odometry and Pose publisher - mPubPose = mNhNs.advertise(poseTopic, 1); - NODELET_INFO_STREAM(" * Advertised on topic " << mPubPose.getTopic()); + mPubPoseCov = mNhNs.advertise(pose_cov_topic, 1); + NODELET_INFO_STREAM(" * Advertised on topic " << mPubPoseCov.getTopic()); - mPubPoseCov = mNhNs.advertise(pose_cov_topic, 1); - NODELET_INFO_STREAM(" * Advertised on topic " << mPubPoseCov.getTopic()); + mPubOdom = mNhNs.advertise(odometryTopic, 1); + NODELET_INFO_STREAM(" * Advertised on topic " << mPubOdom.getTopic()); - mPubOdom = mNhNs.advertise(odometryTopic, 1); - NODELET_INFO_STREAM(" * Advertised on topic " << mPubOdom.getTopic()); + // Rviz markers publisher + mPubMarker = mNhNs.advertise(marker_topic, 10, true); - // Rviz markers publisher - mPubMarker = mNhNs.advertise(marker_topic, 10, true); - - // Camera Path - if (mPathPubRate > 0) { - mPubOdomPath = mNhNs.advertise(odom_path_topic, 1, true); - NODELET_INFO_STREAM(" * Advertised on topic " << mPubOdomPath.getTopic()); - mPubMapPath = mNhNs.advertise(map_path_topic, 1, true); - NODELET_INFO_STREAM(" * Advertised on topic " << mPubMapPath.getTopic()); + // Camera Path + if (mPathPubRate > 0) + { + mPubOdomPath = mNhNs.advertise(odom_path_topic, 1, true); + NODELET_INFO_STREAM(" * Advertised on topic " << mPubOdomPath.getTopic()); + mPubMapPath = mNhNs.advertise(map_path_topic, 1, true); + NODELET_INFO_STREAM(" * Advertised on topic " << mPubMapPath.getTopic()); - mPathTimer = mNhNs.createTimer(ros::Duration(1.0 / mPathPubRate), &ZEDWrapperNodelet::callback_pubPath, this); + mPathTimer = mNhNs.createTimer(ros::Duration(1.0 / mPathPubRate), &ZEDWrapperNodelet::callback_pubPath, this); - if (mPathMaxCount != -1) { - NODELET_DEBUG_STREAM("Path vectors reserved " << mPathMaxCount << " poses."); - mOdomPath.reserve(mPathMaxCount); - mMapPath.reserve(mPathMaxCount); + if (mPathMaxCount != -1) + { + NODELET_DEBUG_STREAM("Path vectors reserved " << mPathMaxCount << " poses."); + mOdomPath.reserve(mPathMaxCount); + mMapPath.reserve(mPathMaxCount); - NODELET_DEBUG_STREAM("Path vector sizes: " << mOdomPath.size() << " " << mMapPath.size()); - } - } else { - NODELET_INFO_STREAM("Path topics not published -> mPathPubRate: " << mPathPubRate); - } + NODELET_DEBUG_STREAM("Path vector sizes: " << mOdomPath.size() << " " << mMapPath.size()); + } + } + else + { + NODELET_INFO_STREAM("Path topics not published -> mPathPubRate: " << mPathPubRate); } + } - // Sensor publishers - if (!sl_tools::isZED(mZedRealCamModel)) { - // IMU Publishers - mPubImu = mNhNs.advertise(imu_topic, 1); - NODELET_INFO_STREAM(" * Advertised on topic " << mPubImu.getTopic()); - mPubImuRaw = mNhNs.advertise(imu_topic_raw, 1); - NODELET_INFO_STREAM(" * Advertised on topic " << mPubImuRaw.getTopic()); - - if(mZedRealCamModel != sl::MODEL::ZED_M) { - // IMU temperature sensor - mPubImuTemp = mNhNs.advertise(imu_temp_topic, 1); - NODELET_INFO_STREAM(" * Advertised on topic " << mPubImuTemp.getTopic()); - } + // Sensor publishers + if (!sl_tools::isZED(mZedRealCamModel)) + { + // IMU Publishers + mPubImu = mNhNs.advertise(imu_topic, 1); + NODELET_INFO_STREAM(" * Advertised on topic " << mPubImu.getTopic()); + mPubImuRaw = mNhNs.advertise(imu_topic_raw, 1); + NODELET_INFO_STREAM(" * Advertised on topic " << mPubImuRaw.getTopic()); - if (mZedRealCamModel == sl::MODEL::ZED2 || mZedRealCamModel == sl::MODEL::ZED2i) { - mPubImuMag = mNhNs.advertise(imu_mag_topic, 1); - NODELET_INFO_STREAM(" * Advertised on topic " << mPubImuMag.getTopic()); + if (mZedRealCamModel != sl::MODEL::ZED_M) + { + // IMU temperature sensor + mPubImuTemp = mNhNs.advertise(imu_temp_topic, 1); + NODELET_INFO_STREAM(" * Advertised on topic " << mPubImuTemp.getTopic()); + } - // Atmospheric pressure - mPubPressure = mNhNs.advertise(pressure_topic, 1); - NODELET_INFO_STREAM(" * Advertised on topic " << mPubPressure.getTopic()); + if (mZedRealCamModel == sl::MODEL::ZED2 || mZedRealCamModel == sl::MODEL::ZED2i) + { + mPubImuMag = mNhNs.advertise(imu_mag_topic, 1); + NODELET_INFO_STREAM(" * Advertised on topic " << mPubImuMag.getTopic()); - // CMOS sensor temperatures - mPubTempL = mNhNs.advertise(temp_topic_left, 1); - NODELET_INFO_STREAM(" * Advertised on topic " << mPubTempL.getTopic()); - mPubTempR = mNhNs.advertise(temp_topic_right, 1); - NODELET_INFO_STREAM(" * Advertised on topic " << mPubTempR.getTopic()); - } + // Atmospheric pressure + mPubPressure = mNhNs.advertise(pressure_topic, 1); + NODELET_INFO_STREAM(" * Advertised on topic " << mPubPressure.getTopic()); - // Publish camera imu transform in a latched topic - if (mZedRealCamModel != sl::MODEL::ZED) { - std::string cam_imu_tr_topic = "left_cam_imu_transform"; - mPubCamImuTransf = mNhNs.advertise(cam_imu_tr_topic, 1, true); + // CMOS sensor temperatures + mPubTempL = mNhNs.advertise(temp_topic_left, 1); + NODELET_INFO_STREAM(" * Advertised on topic " << mPubTempL.getTopic()); + mPubTempR = mNhNs.advertise(temp_topic_right, 1); + NODELET_INFO_STREAM(" * Advertised on topic " << mPubTempR.getTopic()); + } - sl::Orientation sl_rot = mSlCamImuTransf.getOrientation(); - sl::Translation sl_tr = mSlCamImuTransf.getTranslation(); + // Publish camera imu transform in a latched topic + if (mZedRealCamModel != sl::MODEL::ZED) + { + std::string cam_imu_tr_topic = "left_cam_imu_transform"; + mPubCamImuTransf = mNhNs.advertise(cam_imu_tr_topic, 1, true); - mCameraImuTransfMgs = boost::make_shared(); + sl::Orientation sl_rot = mSlCamImuTransf.getOrientation(); + sl::Translation sl_tr = mSlCamImuTransf.getTranslation(); - mCameraImuTransfMgs->rotation.x = sl_rot.ox; - mCameraImuTransfMgs->rotation.y = sl_rot.oy; - mCameraImuTransfMgs->rotation.z = sl_rot.oz; - mCameraImuTransfMgs->rotation.w = sl_rot.ow; + mCameraImuTransfMgs = boost::make_shared(); - mCameraImuTransfMgs->translation.x = sl_tr.x; - mCameraImuTransfMgs->translation.y = sl_tr.y; - mCameraImuTransfMgs->translation.z = sl_tr.z; + mCameraImuTransfMgs->rotation.x = sl_rot.ox; + mCameraImuTransfMgs->rotation.y = sl_rot.oy; + mCameraImuTransfMgs->rotation.z = sl_rot.oz; + mCameraImuTransfMgs->rotation.w = sl_rot.ow; - NODELET_DEBUG("Camera-IMU Rotation: \n %s", sl_rot.getRotationMatrix().getInfos().c_str()); - NODELET_DEBUG("Camera-IMU Translation: \n %g %g %g", sl_tr.x, sl_tr.y, sl_tr.z); + mCameraImuTransfMgs->translation.x = sl_tr.x; + mCameraImuTransfMgs->translation.y = sl_tr.y; + mCameraImuTransfMgs->translation.z = sl_tr.z; - mPubCamImuTransf.publish(mCameraImuTransfMgs); + NODELET_DEBUG("Camera-IMU Rotation: \n %s", sl_rot.getRotationMatrix().getInfos().c_str()); + NODELET_DEBUG("Camera-IMU Translation: \n %g %g %g", sl_tr.x, sl_tr.y, sl_tr.z); - NODELET_INFO_STREAM(" * Advertised on topic " << mPubCamImuTransf.getTopic() << " [LATCHED]"); - } + mPubCamImuTransf.publish(mCameraImuTransfMgs); - if (!mSvoMode && !mSensTimestampSync) { - mFrameTimestamp = ros::Time::now(); - mSensPeriodMean_usec.reset(new sl_tools::CSmartMean(mSensPubRate / 2)); - } else { - mSensPeriodMean_usec.reset(new sl_tools::CSmartMean(mCamFrameRate / 2)); - } + NODELET_INFO_STREAM(" * Advertised on topic " << mPubCamImuTransf.getTopic() << " [LATCHED]"); } - // <---- Publishers - - // ----> Subscribers - NODELET_INFO( "*** SUBSCRIBERS ***"); - mClickedPtSub = mNhNs.subscribe(mClickedPtTopic, 10, &ZEDWrapperNodelet::clickedPtCallback, this); - NODELET_INFO_STREAM(" * Subscribed to topic " << mClickedPtTopic.c_str()); - // <---- Subscribers - - // ----> Services - NODELET_INFO( "*** SERVICES ***"); - if (!mDepthDisabled) { - mSrvSetInitPose = mNhNs.advertiseService("set_pose", &ZEDWrapperNodelet::on_set_pose, this); - NODELET_INFO_STREAM(" * Advertised on service " << mSrvSetInitPose.getService().c_str()); - mSrvResetOdometry = mNhNs.advertiseService("reset_odometry", &ZEDWrapperNodelet::on_reset_odometry, this); - NODELET_INFO_STREAM(" * Advertised on service " << mSrvResetOdometry.getService().c_str()); - mSrvResetTracking = mNhNs.advertiseService("reset_tracking", &ZEDWrapperNodelet::on_reset_tracking, this); - NODELET_INFO_STREAM(" * Advertised on service " << mSrvResetTracking.getService().c_str()); - - mSrvSaveAreaMemory = mNhNs.advertiseService("save_area_memory", &ZEDWrapperNodelet::on_save_area_memory, this); - NODELET_INFO_STREAM(" * Advertised on service " << mSrvSaveAreaMemory.getService().c_str()); - - mSrvStartMapping = mNhNs.advertiseService("start_3d_mapping", &ZEDWrapperNodelet::on_start_3d_mapping, this); - NODELET_INFO_STREAM(" * Advertised on service " << mSrvStartMapping.getService().c_str()); - mSrvStopMapping = mNhNs.advertiseService("stop_3d_mapping", &ZEDWrapperNodelet::on_stop_3d_mapping, this); - NODELET_INFO_STREAM(" * Advertised on service " << mSrvStopMapping.getService().c_str()); - mSrvSave3dMap = mNhNs.advertiseService("save_3d_map", &ZEDWrapperNodelet::on_save_3d_map, this); - NODELET_INFO_STREAM(" * Advertised on service " << mSrvSave3dMap.getService().c_str()); - - mSrvStartObjDet = mNhNs.advertiseService("start_object_detection", &ZEDWrapperNodelet::on_start_object_detection, this); - NODELET_INFO_STREAM(" * Advertised on service " << mSrvStartObjDet.getService().c_str()); - mSrvStopObjDet = mNhNs.advertiseService("stop_object_detection", &ZEDWrapperNodelet::on_stop_object_detection, this); - NODELET_INFO_STREAM(" * Advertised on service " << mSrvStopObjDet.getService().c_str()); + + if (!mSvoMode && !mSensTimestampSync) + { + mFrameTimestamp = ros::Time::now(); + mSensPeriodMean_usec.reset(new sl_tools::CSmartMean(mSensPubRate / 2)); } - - mSrvSvoStartRecording = mNhNs.advertiseService("start_svo_recording", &ZEDWrapperNodelet::on_start_svo_recording, this); - NODELET_INFO_STREAM(" * Advertised on service " << mSrvSvoStartRecording.getService().c_str()); - mSrvSvoStopRecording = mNhNs.advertiseService("stop_svo_recording", &ZEDWrapperNodelet::on_stop_svo_recording, this); - NODELET_INFO_STREAM(" * Advertised on service " << mSrvSvoStopRecording.getService().c_str()); - - mSrvSetLedStatus = mNhNs.advertiseService("set_led_status", &ZEDWrapperNodelet::on_set_led_status, this); - NODELET_INFO_STREAM(" * Advertised on service " << mSrvSetLedStatus.getService().c_str()); - mSrvToggleLed = mNhNs.advertiseService("toggle_led", &ZEDWrapperNodelet::on_toggle_led, this); - NODELET_INFO_STREAM(" * Advertised on service " << mSrvToggleLed.getService().c_str()); - - mSrvSvoStartStream = mNhNs.advertiseService("start_remote_stream", &ZEDWrapperNodelet::on_start_remote_stream, this); - NODELET_INFO_STREAM(" * Advertised on service " << mSrvSvoStartStream.getService().c_str()); - mSrvSvoStopStream = mNhNs.advertiseService("stop_remote_stream", &ZEDWrapperNodelet::on_stop_remote_stream, this); - NODELET_INFO_STREAM(" * Advertised on service " << mSrvSvoStopStream.getService().c_str()); - // <---- Services - - // ----> Threads - if (!mDepthDisabled) { - // Start Pointcloud thread - mPcThread = std::thread(&ZEDWrapperNodelet::pointcloud_thread_func, this); + else + { + mSensPeriodMean_usec.reset(new sl_tools::CSmartMean(mCamFrameRate / 2)); } + } + // <---- Publishers + + // ----> Subscribers + NODELET_INFO("*** SUBSCRIBERS ***"); + mClickedPtSub = mNhNs.subscribe(mClickedPtTopic, 10, &ZEDWrapperNodelet::clickedPtCallback, this); + NODELET_INFO_STREAM(" * Subscribed to topic " << mClickedPtTopic.c_str()); + // <---- Subscribers + + // ----> Services + NODELET_INFO("*** SERVICES ***"); + if (!mDepthDisabled) + { + mSrvSetInitPose = mNhNs.advertiseService("set_pose", &ZEDWrapperNodelet::on_set_pose, this); + NODELET_INFO_STREAM(" * Advertised on service " << mSrvSetInitPose.getService().c_str()); + mSrvResetOdometry = mNhNs.advertiseService("reset_odometry", &ZEDWrapperNodelet::on_reset_odometry, this); + NODELET_INFO_STREAM(" * Advertised on service " << mSrvResetOdometry.getService().c_str()); + mSrvResetTracking = mNhNs.advertiseService("reset_tracking", &ZEDWrapperNodelet::on_reset_tracking, this); + NODELET_INFO_STREAM(" * Advertised on service " << mSrvResetTracking.getService().c_str()); + + mSrvSaveAreaMemory = mNhNs.advertiseService("save_area_memory", &ZEDWrapperNodelet::on_save_area_memory, this); + NODELET_INFO_STREAM(" * Advertised on service " << mSrvSaveAreaMemory.getService().c_str()); + + mSrvStartMapping = mNhNs.advertiseService("start_3d_mapping", &ZEDWrapperNodelet::on_start_3d_mapping, this); + NODELET_INFO_STREAM(" * Advertised on service " << mSrvStartMapping.getService().c_str()); + mSrvStopMapping = mNhNs.advertiseService("stop_3d_mapping", &ZEDWrapperNodelet::on_stop_3d_mapping, this); + NODELET_INFO_STREAM(" * Advertised on service " << mSrvStopMapping.getService().c_str()); + mSrvSave3dMap = mNhNs.advertiseService("save_3d_map", &ZEDWrapperNodelet::on_save_3d_map, this); + NODELET_INFO_STREAM(" * Advertised on service " << mSrvSave3dMap.getService().c_str()); + + mSrvStartObjDet = + mNhNs.advertiseService("start_object_detection", &ZEDWrapperNodelet::on_start_object_detection, this); + NODELET_INFO_STREAM(" * Advertised on service " << mSrvStartObjDet.getService().c_str()); + mSrvStopObjDet = + mNhNs.advertiseService("stop_object_detection", &ZEDWrapperNodelet::on_stop_object_detection, this); + NODELET_INFO_STREAM(" * Advertised on service " << mSrvStopObjDet.getService().c_str()); + } - // Start pool thread - mDevicePollThread = std::thread(&ZEDWrapperNodelet::device_poll_thread_func, this); + mSrvSvoStartRecording = + mNhNs.advertiseService("start_svo_recording", &ZEDWrapperNodelet::on_start_svo_recording, this); + NODELET_INFO_STREAM(" * Advertised on service " << mSrvSvoStartRecording.getService().c_str()); + mSrvSvoStopRecording = mNhNs.advertiseService("stop_svo_recording", &ZEDWrapperNodelet::on_stop_svo_recording, this); + NODELET_INFO_STREAM(" * Advertised on service " << mSrvSvoStopRecording.getService().c_str()); + + mSrvSetLedStatus = mNhNs.advertiseService("set_led_status", &ZEDWrapperNodelet::on_set_led_status, this); + NODELET_INFO_STREAM(" * Advertised on service " << mSrvSetLedStatus.getService().c_str()); + mSrvToggleLed = mNhNs.advertiseService("toggle_led", &ZEDWrapperNodelet::on_toggle_led, this); + NODELET_INFO_STREAM(" * Advertised on service " << mSrvToggleLed.getService().c_str()); + + mSrvSvoStartStream = mNhNs.advertiseService("start_remote_stream", &ZEDWrapperNodelet::on_start_remote_stream, this); + NODELET_INFO_STREAM(" * Advertised on service " << mSrvSvoStartStream.getService().c_str()); + mSrvSvoStopStream = mNhNs.advertiseService("stop_remote_stream", &ZEDWrapperNodelet::on_stop_remote_stream, this); + NODELET_INFO_STREAM(" * Advertised on service " << mSrvSvoStopStream.getService().c_str()); + // <---- Services + + // ----> Threads + if (!mDepthDisabled) + { + // Start Pointcloud thread + mPcThread = std::thread(&ZEDWrapperNodelet::pointcloud_thread_func, this); + } + + // Start pool thread + mDevicePollThread = std::thread(&ZEDWrapperNodelet::device_poll_thread_func, this); - // Start Sensors thread - mSensThread = std::thread(&ZEDWrapperNodelet::sensors_thread_func, this); - // <---- Threads + // Start Sensors thread + mSensThread = std::thread(&ZEDWrapperNodelet::sensors_thread_func, this); + // <---- Threads - NODELET_INFO("+++ ZED Node started +++"); + NODELET_INFO("+++ ZED Node started +++"); } void ZEDWrapperNodelet::readGeneralParams() { - NODELET_INFO_STREAM("*** GENERAL PARAMETERS ***"); - - std::string camera_model; - mNhNs.getParam("general/camera_model", camera_model); - - if (camera_model == "zed") { - mZedUserCamModel = sl::MODEL::ZED; - NODELET_INFO_STREAM(" * Camera Model by param\t-> " << camera_model); - } else if (camera_model == "zedm") { - mZedUserCamModel = sl::MODEL::ZED_M; - NODELET_INFO_STREAM(" * Camera Model by param\t-> " << camera_model); - } else if (camera_model == "zed2") { - mZedUserCamModel = sl::MODEL::ZED2; - NODELET_INFO_STREAM(" * Camera Model by param\t-> " << camera_model); - } else if (camera_model == "zed2i") { - mZedUserCamModel = sl::MODEL::ZED2i; - NODELET_INFO_STREAM(" * Camera Model by param\t-> " << camera_model); - } else if (camera_model == "zedx") { - mZedUserCamModel = sl::MODEL::ZED_X; - NODELET_INFO_STREAM(" * Camera Model by param\t-> " << camera_model); - } else if (camera_model == "zedxm") { - mZedUserCamModel = sl::MODEL::ZED_XM; - NODELET_INFO_STREAM(" * Camera Model by param\t-> " << camera_model); - } else { - NODELET_ERROR_STREAM("Camera model not valid: " << camera_model); - exit(EXIT_FAILURE); - } + NODELET_INFO_STREAM("*** GENERAL PARAMETERS ***"); - mNhNs.getParam("general/camera_name", mCameraName); - NODELET_INFO_STREAM(" * Camera Name\t\t\t-> " << mCameraName.c_str()); - - std::string resol="AUTO"; - mNhNs.getParam("general/grab_resolution", resol); - if (resol == "AUTO") { - mCamResol = sl::RESOLUTION::AUTO; - } else if (sl_tools::isZEDX(mZedUserCamModel)) { - if (resol == "HD1200") { - mCamResol = sl::RESOLUTION::HD1200; - } else if (resol == "HD1080") { - mCamResol = sl::RESOLUTION::HD1080; - } else if (resol == "SVGA") { - mCamResol = sl::RESOLUTION::SVGA; - } else { - NODELET_WARN("Not valid 'general.grab_resolution' value: '%s'. Using 'AUTO' setting.", resol.c_str()); - mCamResol = sl::RESOLUTION::AUTO; - } - } else { - if (resol == "HD2K") { - mCamResol = sl::RESOLUTION::HD2K; - } else if (resol == "HD1080") { - mCamResol = sl::RESOLUTION::HD1080; - } else if (resol == "HD720") { - mCamResol = sl::RESOLUTION::HD720; - } else if (resol == "VGA") { - mCamResol = sl::RESOLUTION::VGA; - } else { - NODELET_WARN("Not valid 'general.grab_resolution' value: '%s'. Using 'AUTO' setting.", resol.c_str()); - mCamResol = sl::RESOLUTION::AUTO; - } + std::string camera_model; + mNhNs.getParam("general/camera_model", camera_model); + + if (camera_model == "zed") + { + mZedUserCamModel = sl::MODEL::ZED; + NODELET_INFO_STREAM(" * Camera Model by param\t-> " << camera_model); + } + else if (camera_model == "zedm") + { + mZedUserCamModel = sl::MODEL::ZED_M; + NODELET_INFO_STREAM(" * Camera Model by param\t-> " << camera_model); + } + else if (camera_model == "zed2") + { + mZedUserCamModel = sl::MODEL::ZED2; + NODELET_INFO_STREAM(" * Camera Model by param\t-> " << camera_model); + } + else if (camera_model == "zed2i") + { + mZedUserCamModel = sl::MODEL::ZED2i; + NODELET_INFO_STREAM(" * Camera Model by param\t-> " << camera_model); + } + else if (camera_model == "zedx") + { + mZedUserCamModel = sl::MODEL::ZED_X; + NODELET_INFO_STREAM(" * Camera Model by param\t-> " << camera_model); + } + else if (camera_model == "zedxm") + { + mZedUserCamModel = sl::MODEL::ZED_XM; + NODELET_INFO_STREAM(" * Camera Model by param\t-> " << camera_model); + } + else + { + NODELET_ERROR_STREAM("Camera model not valid: " << camera_model); + exit(EXIT_FAILURE); + } + + mNhNs.getParam("general/camera_name", mCameraName); + NODELET_INFO_STREAM(" * Camera Name\t\t\t-> " << mCameraName.c_str()); + + std::string resol = "AUTO"; + mNhNs.getParam("general/grab_resolution", resol); + if (resol == "AUTO") + { + mCamResol = sl::RESOLUTION::AUTO; + } + else if (sl_tools::isZEDX(mZedUserCamModel)) + { + if (resol == "HD1200") + { + mCamResol = sl::RESOLUTION::HD1200; } - NODELET_INFO_STREAM(" * Camera resolution\t\t-> " << sl::toString(mCamResol).c_str()); - - mNhNs.getParam("general/grab_frame_rate", mCamFrameRate); - checkResolFps(); - NODELET_INFO_STREAM(" * Camera Grab Framerate\t-> " << mCamFrameRate); - mNhNs.getParam("general/pub_frame_rate", mPubFrameRate); - if(mPubFrameRate>mCamFrameRate) { - mPubFrameRate = mCamFrameRate; - NODELET_WARN_STREAM("'general/pub_frame_rate' cannot be bigger than 'general/grab_frame_rate'. Forced to the correct value: " << mPubFrameRate); + else if (resol == "HD1080") + { + mCamResol = sl::RESOLUTION::HD1080; } - NODELET_INFO_STREAM(" * Grab compute capping FPS\t-> " << mPubFrameRate << " Hz"); - - - std::string out_resol = "CUSTOM"; - mNhNs.getParam("general/pub_resolution", out_resol); - if (out_resol == "NATIVE") { - mPubResolution = PubRes::NATIVE; - } else if (out_resol == "CUSTOM") { - mPubResolution = PubRes::CUSTOM; - } else { - NODELET_WARN("Not valid 'general/pub_resolution' value: '%s'. Using default setting instead.", out_resol.c_str()); - out_resol = "CUSTOM"; - mPubResolution = PubRes::CUSTOM; + else if (resol == "SVGA") + { + mCamResol = sl::RESOLUTION::SVGA; } - NODELET_INFO_STREAM(" * Publishing resolution\t-> " << out_resol.c_str()); - - if (mPubResolution == PubRes::CUSTOM) { - mNhNs.getParam( "general/pub_downscale_factor", mCustomDownscaleFactor ); - NODELET_INFO_STREAM(" * Publishing downscale factor\t-> " << mCustomDownscaleFactor ); - } else { - mCustomDownscaleFactor = 1.0; + else + { + NODELET_WARN("Not valid 'general.grab_resolution' value: '%s'. Using 'AUTO' setting.", resol.c_str()); + mCamResol = sl::RESOLUTION::AUTO; } - - mNhNs.getParam("general/gpu_id", mGpuId); - NODELET_INFO_STREAM(" * Gpu ID\t\t\t-> " << mGpuId); - mNhNs.getParam("general/zed_id", mZedId); - NODELET_INFO_STREAM(" * Camera ID\t\t\t-> " << mZedId); - mNhNs.getParam("general/sdk_verbose", mSdkVerbose); - NODELET_INFO_STREAM(" * SDK Verbose level\t\t-> " << mSdkVerbose ); - std::string flip_str; - mNhNs.getParam("general/camera_flip", flip_str); - if(flip_str=="ON") { - mCameraFlip = sl::FLIP_MODE::ON; - } else if(flip_str=="OFF") { - mCameraFlip = sl::FLIP_MODE::OFF; - } else { - mCameraFlip = sl::FLIP_MODE::AUTO; + } + else + { + if (resol == "HD2K") + { + mCamResol = sl::RESOLUTION::HD2K; } - NODELET_INFO_STREAM(" * Camera Flip\t\t\t-> " << sl::toString(mCameraFlip).c_str()); - mNhNs.param("general/self_calib", mCameraSelfCalib, true); - NODELET_INFO_STREAM(" * Self calibration\t\t-> " << (mCameraSelfCalib ? "ENABLED": "DISABLED")); + else if (resol == "HD1080") + { + mCamResol = sl::RESOLUTION::HD1080; + } + else if (resol == "HD720") + { + mCamResol = sl::RESOLUTION::HD720; + } + else if (resol == "VGA") + { + mCamResol = sl::RESOLUTION::VGA; + } + else + { + NODELET_WARN("Not valid 'general.grab_resolution' value: '%s'. Using 'AUTO' setting.", resol.c_str()); + mCamResol = sl::RESOLUTION::AUTO; + } + } + NODELET_INFO_STREAM(" * Camera resolution\t\t-> " << sl::toString(mCamResol).c_str()); + + mNhNs.getParam("general/grab_frame_rate", mCamFrameRate); + checkResolFps(); + NODELET_INFO_STREAM(" * Camera Grab Framerate\t-> " << mCamFrameRate); + mNhNs.getParam("general/pub_frame_rate", mPubFrameRate); + if (mPubFrameRate > mCamFrameRate) + { + mPubFrameRate = mCamFrameRate; + NODELET_WARN_STREAM( + "'general/pub_frame_rate' cannot be bigger than 'general/grab_frame_rate'. Forced to the correct value: " + << mPubFrameRate); + } + NODELET_INFO_STREAM(" * Grab compute capping FPS\t-> " << mPubFrameRate << " Hz"); - int tmp_sn = 0; - mNhNs.getParam("general/serial_number", tmp_sn); + std::string out_resol = "CUSTOM"; + mNhNs.getParam("general/pub_resolution", out_resol); + if (out_resol == "NATIVE") + { + mPubResolution = PubRes::NATIVE; + } + else if (out_resol == "CUSTOM") + { + mPubResolution = PubRes::CUSTOM; + } + else + { + NODELET_WARN("Not valid 'general/pub_resolution' value: '%s'. Using default setting instead.", out_resol.c_str()); + out_resol = "CUSTOM"; + mPubResolution = PubRes::CUSTOM; + } + NODELET_INFO_STREAM(" * Publishing resolution\t-> " << out_resol.c_str()); - if (tmp_sn > 0) { - mZedSerialNumber = static_cast(tmp_sn); - NODELET_INFO_STREAM(" * Serial number\t\t-> " << mZedSerialNumber); - } + if (mPubResolution == PubRes::CUSTOM) + { + mNhNs.getParam("general/pub_downscale_factor", mCustomDownscaleFactor); + NODELET_INFO_STREAM(" * Publishing downscale factor\t-> " << mCustomDownscaleFactor); + } + else + { + mCustomDownscaleFactor = 1.0; + } - std::string parsed_str = getRoiParam("general/region_of_interest", mRoiParam); - NODELET_INFO_STREAM(" * Region of interest\t\t-> " << parsed_str.c_str()); + mNhNs.getParam("general/gpu_id", mGpuId); + NODELET_INFO_STREAM(" * Gpu ID\t\t\t-> " << mGpuId); + mNhNs.getParam("general/zed_id", mZedId); + NODELET_INFO_STREAM(" * Camera ID\t\t\t-> " << mZedId); + mNhNs.getParam("general/sdk_verbose", mSdkVerbose); + NODELET_INFO_STREAM(" * SDK Verbose level\t\t-> " << mSdkVerbose); + std::string flip_str; + mNhNs.getParam("general/camera_flip", flip_str); + if (flip_str == "ON") + { + mCameraFlip = sl::FLIP_MODE::ON; + } + else if (flip_str == "OFF") + { + mCameraFlip = sl::FLIP_MODE::OFF; + } + else + { + mCameraFlip = sl::FLIP_MODE::AUTO; + } + NODELET_INFO_STREAM(" * Camera Flip\t\t\t-> " << sl::toString(mCameraFlip).c_str()); + mNhNs.param("general/self_calib", mCameraSelfCalib, true); + NODELET_INFO_STREAM(" * Self calibration\t\t-> " << (mCameraSelfCalib ? "ENABLED" : "DISABLED")); + + int tmp_sn = 0; + mNhNs.getParam("general/serial_number", tmp_sn); + + if (tmp_sn > 0) + { + mZedSerialNumber = static_cast(tmp_sn); + NODELET_INFO_STREAM(" * Serial number\t\t-> " << mZedSerialNumber); + } + + std::string parsed_str = getRoiParam("general/region_of_interest", mRoiParam); + NODELET_INFO_STREAM(" * Region of interest\t\t-> " << parsed_str.c_str()); } void ZEDWrapperNodelet::readDepthParams() { - NODELET_INFO_STREAM("*** DEPTH PARAMETERS ***"); - - std::string depth_mode_str = sl::toString(mDepthMode).c_str(); - mNhNs.getParam("depth/depth_mode", depth_mode_str); - - bool matched = false; - for (int mode = static_cast(sl::DEPTH_MODE::NONE); - mode < static_cast(sl::DEPTH_MODE::LAST); ++mode) + NODELET_INFO_STREAM("*** DEPTH PARAMETERS ***"); + + std::string depth_mode_str = sl::toString(mDepthMode).c_str(); + mNhNs.getParam("depth/depth_mode", depth_mode_str); + + bool matched = false; + for (int mode = static_cast(sl::DEPTH_MODE::NONE); mode < static_cast(sl::DEPTH_MODE::LAST); ++mode) + { + std::string test_str = sl::toString(static_cast(mode)).c_str(); + std::replace(test_str.begin(), test_str.end(), ' ', + '_'); // Replace spaces with underscores to match the YAML setting + if (test_str == depth_mode_str) { - std::string test_str = sl::toString(static_cast(mode)).c_str(); - std::replace(test_str.begin(), test_str.end(), ' ', '_'); // Replace spaces with underscores to match the YAML setting - if (test_str == depth_mode_str) { - matched = true; - mDepthMode = static_cast(mode); - break; - } + matched = true; + mDepthMode = static_cast(mode); + break; } + } - if (!matched) { - NODELET_WARN_STREAM( - "The parameter 'depth.depth_mode' contains a not valid string. Please check it in 'common.yaml'."); - NODELET_WARN("Using default DEPTH_MODE."); - mDepthMode = sl::DEPTH_MODE::ULTRA; - } + if (!matched) + { + NODELET_WARN_STREAM( + "The parameter 'depth.depth_mode' contains a not valid string. Please check it in 'common.yaml'."); + NODELET_WARN("Using default DEPTH_MODE."); + mDepthMode = sl::DEPTH_MODE::ULTRA; + } - if (mDepthMode == sl::DEPTH_MODE::NONE) { - mDepthDisabled = true; - mDepthStabilization = 0; - NODELET_INFO_STREAM(" * Depth mode\t\t\t-> " << sl::toString(mDepthMode).c_str() << " - DEPTH DISABLED"); - } else { - mDepthDisabled = false; - NODELET_INFO_STREAM(" * Depth mode\t\t\t-> " << sl::toString( - mDepthMode).c_str() << " [" << static_cast(mDepthMode) << "]"); - } + if (mDepthMode == sl::DEPTH_MODE::NONE) + { + mDepthDisabled = true; + mDepthStabilization = 0; + NODELET_INFO_STREAM(" * Depth mode\t\t\t-> " << sl::toString(mDepthMode).c_str() << " - DEPTH DISABLED"); + } + else + { + mDepthDisabled = false; + NODELET_INFO_STREAM(" * Depth mode\t\t\t-> " << sl::toString(mDepthMode).c_str() << " [" + << static_cast(mDepthMode) << "]"); + } - if(!mDepthDisabled) { - mNhNs.getParam("depth/openni_depth_mode", mOpenniDepthMode); - NODELET_INFO_STREAM(" * OpenNI mode\t\t\t-> " << (mOpenniDepthMode ? "ENABLED": "DISABLED")); - mNhNs.getParam("depth/depth_stabilization", mDepthStabilization); - NODELET_INFO_STREAM(" * Depth Stabilization\t\t-> " << mDepthStabilization ); - mNhNs.getParam("depth/min_depth", mCamMinDepth); - NODELET_INFO_STREAM(" * Minimum depth\t\t-> " << mCamMinDepth << " m"); - mNhNs.getParam("depth/max_depth", mCamMaxDepth); - NODELET_INFO_STREAM(" * Maximum depth\t\t-> " << mCamMaxDepth << " m"); - } + if (!mDepthDisabled) + { + mNhNs.getParam("depth/openni_depth_mode", mOpenniDepthMode); + NODELET_INFO_STREAM(" * OpenNI mode\t\t\t-> " << (mOpenniDepthMode ? "ENABLED" : "DISABLED")); + mNhNs.getParam("depth/depth_stabilization", mDepthStabilization); + NODELET_INFO_STREAM(" * Depth Stabilization\t\t-> " << mDepthStabilization); + mNhNs.getParam("depth/min_depth", mCamMinDepth); + NODELET_INFO_STREAM(" * Minimum depth\t\t-> " << mCamMinDepth << " m"); + mNhNs.getParam("depth/max_depth", mCamMaxDepth); + NODELET_INFO_STREAM(" * Maximum depth\t\t-> " << mCamMaxDepth << " m"); + } } void ZEDWrapperNodelet::readPosTrkParams() { - if(!mDepthDisabled) { - NODELET_INFO_STREAM("*** POSITIONAL TRACKING PARAMETERS ***"); - - mNhNs.param("pos_tracking/pos_tracking_enabled", mPosTrackingEnabled, true); - NODELET_INFO_STREAM(" * Positional tracking\t\t-> " << (mPosTrackingEnabled ? "ENABLED": "DISABLED")); - - std::string pos_trk_mode; - mNhNs.getParam("pos_tracking/pos_tracking_mode", pos_trk_mode); - if (pos_trk_mode == "QUALITY") { - mPosTrkMode = sl::POSITIONAL_TRACKING_MODE::QUALITY; - } else if (pos_trk_mode == "STANDARD") { - mPosTrkMode = sl::POSITIONAL_TRACKING_MODE::STANDARD; - } else { - NODELET_WARN_STREAM( - "'pos_tracking/pos_tracking_mode' not valid ('" << pos_trk_mode << - "'). Using default value."); - mPosTrkMode = sl::POSITIONAL_TRACKING_MODE::QUALITY; - } - NODELET_INFO_STREAM( " * Positional tracking mode\t-> " << sl::toString(mPosTrkMode).c_str()); + if (!mDepthDisabled) + { + NODELET_INFO_STREAM("*** POSITIONAL TRACKING PARAMETERS ***"); - mNhNs.getParam("pos_tracking/set_gravity_as_origin", mSetGravityAsOrigin); - NODELET_INFO_STREAM(" * Set gravity as origin\t-> " << (mSetGravityAsOrigin ? "ENABLED": "DISABLED")); + mNhNs.param("pos_tracking/pos_tracking_enabled", mPosTrackingEnabled, true); + NODELET_INFO_STREAM(" * Positional tracking\t\t-> " << (mPosTrackingEnabled ? "ENABLED" : "DISABLED")); + + std::string pos_trk_mode; + mNhNs.getParam("pos_tracking/pos_tracking_mode", pos_trk_mode); + if (pos_trk_mode == "QUALITY") + { + mPosTrkMode = sl::POSITIONAL_TRACKING_MODE::QUALITY; + } + else if (pos_trk_mode == "STANDARD") + { + mPosTrkMode = sl::POSITIONAL_TRACKING_MODE::STANDARD; + } + else + { + NODELET_WARN_STREAM("'pos_tracking/pos_tracking_mode' not valid ('" << pos_trk_mode + << "'). Using default value."); + mPosTrkMode = sl::POSITIONAL_TRACKING_MODE::QUALITY; + } + NODELET_INFO_STREAM(" * Positional tracking mode\t-> " << sl::toString(mPosTrkMode).c_str()); - 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)); + mNhNs.getParam("pos_tracking/set_gravity_as_origin", mSetGravityAsOrigin); + NODELET_INFO_STREAM(" * Set gravity as origin\t-> " << (mSetGravityAsOrigin ? "ENABLED" : "DISABLED")); - if (mPathMaxCount < 2 && mPathMaxCount != -1) { - mPathMaxCount = 2; - } + 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)); - mNhNs.getParam("pos_tracking/initial_base_pose", mInitialBasePose); - - mNhNs.getParam("pos_tracking/area_memory_db_path", mAreaMemDbPath); - mAreaMemDbPath = sl_tools::resolveFilePath(mAreaMemDbPath); - NODELET_INFO_STREAM(" * Odometry DB path\t\t-> " << mAreaMemDbPath.c_str()); - - mNhNs.param("pos_tracking/save_area_memory_db_on_exit", mSaveAreaMapOnClosing, false); - NODELET_INFO_STREAM(" * Save Area Memory on closing\t-> " << (mSaveAreaMapOnClosing ? "ENABLED": "DISABLED")); - mNhNs.param("pos_tracking/area_memory", mAreaMemory, false); - NODELET_INFO_STREAM(" * Area Memory\t\t\t-> " << (mAreaMemory ? "ENABLED": "DISABLED")); - mNhNs.param("pos_tracking/imu_fusion", mImuFusion, true); - NODELET_INFO_STREAM(" * IMU Fusion\t\t\t-> " << (mImuFusion ? "ENABLED": "DISABLED")); - mNhNs.param("pos_tracking/floor_alignment", mFloorAlignment, false); - NODELET_INFO_STREAM(" * Floor alignment\t\t-> " << (mFloorAlignment ? "ENABLED": "DISABLED")); - mNhNs.param("pos_tracking/init_odom_with_first_valid_pose", mInitOdomWithPose, true); - NODELET_INFO_STREAM(" * Init Odometry with first valid pose data -> " - << (mInitOdomWithPose ? "ENABLED": "DISABLED")); - mNhNs.param("pos_tracking/two_d_mode", mTwoDMode, false); - NODELET_INFO_STREAM(" * Force 2D mode\t\t-> " << (mTwoDMode ? "ENABLED": "DISABLED")); - - if (mTwoDMode) { - mNhNs.param("pos_tracking/fixed_z_value", mFixedZValue, 0.0); - NODELET_INFO_STREAM(" * Fixed Z value\t\t-> " << mFixedZValue); - } - - mNhNs.param("pos_tracking/publish_tf", mPublishTf, true); - NODELET_INFO_STREAM(" * Broadcast odometry TF\t-> " << (mPublishTf ? "ENABLED": "DISABLED")); - mNhNs.param("pos_tracking/publish_map_tf", mPublishMapTf, true); - NODELET_INFO_STREAM(" * Broadcast map pose TF\t-> " - << (mPublishTf ? (mPublishMapTf ? "ENABLED": "DISABLED"): "DISABLED")); + if (mPathMaxCount < 2 && mPathMaxCount != -1) + { + mPathMaxCount = 2; + } + + mNhNs.getParam("pos_tracking/initial_base_pose", mInitialBasePose); + + mNhNs.getParam("pos_tracking/area_memory_db_path", mAreaMemDbPath); + mAreaMemDbPath = sl_tools::resolveFilePath(mAreaMemDbPath); + NODELET_INFO_STREAM(" * Odometry DB path\t\t-> " << mAreaMemDbPath.c_str()); + + mNhNs.param("pos_tracking/save_area_memory_db_on_exit", mSaveAreaMapOnClosing, false); + NODELET_INFO_STREAM(" * Save Area Memory on closing\t-> " << (mSaveAreaMapOnClosing ? "ENABLED" : "DISABLED")); + mNhNs.param("pos_tracking/area_memory", mAreaMemory, false); + NODELET_INFO_STREAM(" * Area Memory\t\t\t-> " << (mAreaMemory ? "ENABLED" : "DISABLED")); + mNhNs.param("pos_tracking/imu_fusion", mImuFusion, true); + NODELET_INFO_STREAM(" * IMU Fusion\t\t\t-> " << (mImuFusion ? "ENABLED" : "DISABLED")); + mNhNs.param("pos_tracking/floor_alignment", mFloorAlignment, false); + NODELET_INFO_STREAM(" * Floor alignment\t\t-> " << (mFloorAlignment ? "ENABLED" : "DISABLED")); + mNhNs.param("pos_tracking/init_odom_with_first_valid_pose", mInitOdomWithPose, true); + NODELET_INFO_STREAM(" * Init Odometry with first valid pose data -> " + << (mInitOdomWithPose ? "ENABLED" : "DISABLED")); + mNhNs.param("pos_tracking/two_d_mode", mTwoDMode, false); + NODELET_INFO_STREAM(" * Force 2D mode\t\t-> " << (mTwoDMode ? "ENABLED" : "DISABLED")); + + if (mTwoDMode) + { + mNhNs.param("pos_tracking/fixed_z_value", mFixedZValue, 0.0); + NODELET_INFO_STREAM(" * Fixed Z value\t\t-> " << mFixedZValue); } + + mNhNs.param("pos_tracking/publish_tf", mPublishTf, true); + NODELET_INFO_STREAM(" * Broadcast odometry TF\t-> " << (mPublishTf ? "ENABLED" : "DISABLED")); + mNhNs.param("pos_tracking/publish_map_tf", mPublishMapTf, true); + NODELET_INFO_STREAM(" * Broadcast map pose TF\t-> " + << (mPublishTf ? (mPublishMapTf ? "ENABLED" : "DISABLED") : "DISABLED")); + } } void ZEDWrapperNodelet::readMappingParams() { - NODELET_INFO_STREAM("*** MAPPING PARAMETERS ***"); + NODELET_INFO_STREAM("*** MAPPING PARAMETERS ***"); - if(!mDepthDisabled) { - mNhNs.param("mapping/mapping_enabled", mMappingEnabled, false); + if (!mDepthDisabled) + { + mNhNs.param("mapping/mapping_enabled", mMappingEnabled, false); - if (mMappingEnabled) { - NODELET_INFO_STREAM(" * Mapping\t\t\t-> ENABLED"); + if (mMappingEnabled) + { + NODELET_INFO_STREAM(" * Mapping\t\t\t-> ENABLED"); - mNhNs.getParam("mapping/resolution", mMappingRes); - NODELET_INFO_STREAM(" * Mapping resolution\t\t-> " << mMappingRes << " m"); + mNhNs.getParam("mapping/resolution", mMappingRes); + NODELET_INFO_STREAM(" * Mapping resolution\t\t-> " << mMappingRes << " m"); - mNhNs.getParam("mapping/max_mapping_range", mMaxMappingRange); - NODELET_INFO_STREAM(" * Mapping max range\t\t-> " << mMaxMappingRange << " m" - << ((mMaxMappingRange < 0.0) ? " [AUTO]": "")); + mNhNs.getParam("mapping/max_mapping_range", mMaxMappingRange); + NODELET_INFO_STREAM(" * Mapping max range\t\t-> " << mMaxMappingRange << " m" + << ((mMaxMappingRange < 0.0) ? " [AUTO]" : "")); - mNhNs.getParam("mapping/fused_pointcloud_freq", mFusedPcPubFreq); - NODELET_INFO_STREAM(" * Fused point cloud freq:\t-> " << mFusedPcPubFreq << " Hz"); - } else { - NODELET_INFO_STREAM(" * Mapping\t\t\t-> DISABLED"); - } + mNhNs.getParam("mapping/fused_pointcloud_freq", mFusedPcPubFreq); + NODELET_INFO_STREAM(" * Fused point cloud freq:\t-> " << mFusedPcPubFreq << " Hz"); + } + else + { + NODELET_INFO_STREAM(" * Mapping\t\t\t-> DISABLED"); } - mNhNs.param("mapping/clicked_point_topic", mClickedPtTopic, std::string("/clicked_point")); - NODELET_INFO_STREAM(" * Clicked point topic\t\t-> " << mClickedPtTopic.c_str()); + } + mNhNs.param("mapping/clicked_point_topic", mClickedPtTopic, std::string("/clicked_point")); + NODELET_INFO_STREAM(" * Clicked point topic\t\t-> " << mClickedPtTopic.c_str()); } void ZEDWrapperNodelet::readObjDetParams() { - if(!mDepthDisabled) { - NODELET_INFO_STREAM("*** OBJECT DETECTION PARAMETERS ***"); - - mNhNs.param("object_detection/od_enabled", mObjDetEnabled, false); - - if (mObjDetEnabled) { - NODELET_INFO_STREAM(" * Object Detection\t\t-> ENABLED"); - mNhNs.getParam("object_detection/confidence_threshold", mObjDetConfidence); - NODELET_INFO_STREAM(" * Object confidence\t\t-> " << mObjDetConfidence); - mNhNs.getParam("object_detection/object_tracking_enabled", mObjDetTracking); - NODELET_INFO_STREAM(" * Object tracking\t\t-> " << (mObjDetTracking ? "ENABLED": "DISABLED")); - mNhNs.getParam("object_detection/max_range", mObjDetMaxRange); - if (mObjDetMaxRange > mCamMaxDepth) { - NODELET_WARN("Detection max range cannot be major than depth max range. Automatically fixed."); - mObjDetMaxRange = mCamMaxDepth; - } - NODELET_INFO_STREAM(" * Detection max range\t\t-> " << mObjDetMaxRange); + if (!mDepthDisabled) + { + NODELET_INFO_STREAM("*** OBJECT DETECTION PARAMETERS ***"); - int model; - mNhNs.getParam("object_detection/model", model); - if (model < 0 || model >= static_cast(sl::OBJECT_DETECTION_MODEL::LAST)) { - NODELET_WARN("Detection model not valid. Forced to the default value"); - model = static_cast(mObjDetModel); - } - mObjDetModel = static_cast(model); - - NODELET_INFO_STREAM(" * Detection model\t\t-> " << sl::toString(mObjDetModel)); - - mNhNs.getParam("object_detection/mc_people", mObjDetPeopleEnable); - NODELET_INFO_STREAM(" * Detect people\t\t-> " << (mObjDetPeopleEnable ? "ENABLED": "DISABLED")); - mNhNs.getParam("object_detection/mc_vehicle", mObjDetVehiclesEnable); - NODELET_INFO_STREAM(" * Detect vehicles\t\t-> " << (mObjDetVehiclesEnable ? "ENABLED": "DISABLED")); - mNhNs.getParam("object_detection/mc_bag", mObjDetBagsEnable); - NODELET_INFO_STREAM(" * Detect bags\t\t\t-> " << (mObjDetBagsEnable ? "ENABLED": "DISABLED")); - mNhNs.getParam("object_detection/mc_animal", mObjDetAnimalsEnable); - NODELET_INFO_STREAM(" * Detect animals\t\t-> " << (mObjDetAnimalsEnable ? "ENABLED": "DISABLED")); - mNhNs.getParam("object_detection/mc_electronics", mObjDetElectronicsEnable); - NODELET_INFO_STREAM(" * Detect electronics\t\t-> " << (mObjDetElectronicsEnable ? "ENABLED": "DISABLED")); - mNhNs.getParam("object_detection/mc_fruit_vegetable", mObjDetFruitsEnable); - NODELET_INFO_STREAM(" * Detect fruit and vegetables\t-> " << (mObjDetFruitsEnable ? "ENABLED": "DISABLED")); - mNhNs.getParam("object_detection/mc_sport", mObjDetSportsEnable); - NODELET_INFO_STREAM(" * Detect sport-related objects\t-> " << (mObjDetSportsEnable ? "ENABLED": "DISABLED")); - } + mNhNs.param("object_detection/od_enabled", mObjDetEnabled, false); + + if (mObjDetEnabled) + { + NODELET_INFO_STREAM(" * Object Detection\t\t-> ENABLED"); + mNhNs.getParam("object_detection/confidence_threshold", mObjDetConfidence); + NODELET_INFO_STREAM(" * Object confidence\t\t-> " << mObjDetConfidence); + mNhNs.getParam("object_detection/object_tracking_enabled", mObjDetTracking); + NODELET_INFO_STREAM(" * Object tracking\t\t-> " << (mObjDetTracking ? "ENABLED" : "DISABLED")); + mNhNs.getParam("object_detection/max_range", mObjDetMaxRange); + if (mObjDetMaxRange > mCamMaxDepth) + { + NODELET_WARN("Detection max range cannot be major than depth max range. Automatically fixed."); + mObjDetMaxRange = mCamMaxDepth; + } + NODELET_INFO_STREAM(" * Detection max range\t\t-> " << mObjDetMaxRange); + + int model; + mNhNs.getParam("object_detection/model", model); + if (model < 0 || model >= static_cast(sl::OBJECT_DETECTION_MODEL::LAST)) + { + NODELET_WARN("Detection model not valid. Forced to the default value"); + model = static_cast(mObjDetModel); + } + mObjDetModel = static_cast(model); + + NODELET_INFO_STREAM(" * Detection model\t\t-> " << sl::toString(mObjDetModel)); + + mNhNs.getParam("object_detection/mc_people", mObjDetPeopleEnable); + NODELET_INFO_STREAM(" * Detect people\t\t-> " << (mObjDetPeopleEnable ? "ENABLED" : "DISABLED")); + mNhNs.getParam("object_detection/mc_vehicle", mObjDetVehiclesEnable); + NODELET_INFO_STREAM(" * Detect vehicles\t\t-> " << (mObjDetVehiclesEnable ? "ENABLED" : "DISABLED")); + mNhNs.getParam("object_detection/mc_bag", mObjDetBagsEnable); + NODELET_INFO_STREAM(" * Detect bags\t\t\t-> " << (mObjDetBagsEnable ? "ENABLED" : "DISABLED")); + mNhNs.getParam("object_detection/mc_animal", mObjDetAnimalsEnable); + NODELET_INFO_STREAM(" * Detect animals\t\t-> " << (mObjDetAnimalsEnable ? "ENABLED" : "DISABLED")); + mNhNs.getParam("object_detection/mc_electronics", mObjDetElectronicsEnable); + NODELET_INFO_STREAM(" * Detect electronics\t\t-> " << (mObjDetElectronicsEnable ? "ENABLED" : "DISABLED")); + mNhNs.getParam("object_detection/mc_fruit_vegetable", mObjDetFruitsEnable); + NODELET_INFO_STREAM(" * Detect fruit and vegetables\t-> " << (mObjDetFruitsEnable ? "ENABLED" : "DISABLED")); + mNhNs.getParam("object_detection/mc_sport", mObjDetSportsEnable); + NODELET_INFO_STREAM(" * Detect sport-related objects\t-> " << (mObjDetSportsEnable ? "ENABLED" : "DISABLED")); } + } } void ZEDWrapperNodelet::readSensParams() { - NODELET_INFO_STREAM("*** SENSORS PARAMETERS ***"); - - if (!sl_tools::isZED(mZedUserCamModel)) { - mNhNs.getParam("sensors/sensors_timestamp_sync", mSensTimestampSync); - NODELET_INFO_STREAM(" * Sensors timestamp sync\t-> " << (mSensTimestampSync ? "ENABLED": "DISABLED")); - mNhNs.getParam("sensors/max_pub_rate", mSensPubRate); - if (!sl_tools::isZEDM(mZedUserCamModel)) { - if (mSensPubRate > 800.) - mSensPubRate = 800.; - } else { - if (mSensPubRate > 400.) - mSensPubRate = 400.; - } + NODELET_INFO_STREAM("*** SENSORS PARAMETERS ***"); + + if (!sl_tools::isZED(mZedUserCamModel)) + { + mNhNs.getParam("sensors/sensors_timestamp_sync", mSensTimestampSync); + NODELET_INFO_STREAM(" * Sensors timestamp sync\t-> " << (mSensTimestampSync ? "ENABLED" : "DISABLED")); + mNhNs.getParam("sensors/max_pub_rate", mSensPubRate); + if (!sl_tools::isZEDM(mZedUserCamModel)) + { + if (mSensPubRate > 800.) + mSensPubRate = 800.; + } + else + { + if (mSensPubRate > 400.) + mSensPubRate = 400.; + } - NODELET_INFO_STREAM(" * Max sensors rate\t\t-> " << mSensPubRate); + NODELET_INFO_STREAM(" * Max sensors rate\t\t-> " << mSensPubRate); - mNhNs.param("sensors/publish_imu_tf", mPublishImuTf, true); - NODELET_INFO_STREAM(" * Broadcast IMU pose TF\t-> " << (mPublishImuTf ? "ENABLED": "DISABLED")); - } else { - NODELET_INFO_STREAM(" * The ZED camera has no available inertial/environmental sensors."); - } + mNhNs.param("sensors/publish_imu_tf", mPublishImuTf, true); + NODELET_INFO_STREAM(" * Broadcast IMU pose TF\t-> " << (mPublishImuTf ? "ENABLED" : "DISABLED")); + } + else + { + NODELET_INFO_STREAM(" * The ZED camera has no available inertial/environmental sensors."); + } } void ZEDWrapperNodelet::readSvoParams() { - NODELET_INFO_STREAM("*** SVO PARAMETERS ***"); + NODELET_INFO_STREAM("*** SVO PARAMETERS ***"); - mNhNs.param("svo_file", mSvoFilepath, std::string()); - mSvoFilepath = sl_tools::resolveFilePath(mSvoFilepath); - NODELET_INFO_STREAM(" * SVO input file: \t\t-> " << mSvoFilepath.c_str()); + mNhNs.param("svo_file", mSvoFilepath, std::string()); + mSvoFilepath = sl_tools::resolveFilePath(mSvoFilepath); + NODELET_INFO_STREAM(" * SVO input file: \t\t-> " << mSvoFilepath.c_str()); - if(!mSvoFilepath.empty()) { - mNhNs.getParam("general/svo_realtime", mSvoRealtime); - NODELET_INFO_STREAM(" * SVO realtime mode\t\t-> " << (mSvoRealtime ? "ENABLED": "DISABLED")); - } + if (!mSvoFilepath.empty()) + { + mNhNs.getParam("general/svo_realtime", mSvoRealtime); + NODELET_INFO_STREAM(" * SVO realtime mode\t\t-> " << (mSvoRealtime ? "ENABLED" : "DISABLED")); + } - int svo_compr = 0; - mNhNs.getParam("general/svo_compression", svo_compr); + int svo_compr = 0; + mNhNs.getParam("general/svo_compression", svo_compr); - if (svo_compr >= static_cast(sl::SVO_COMPRESSION_MODE::LAST)) { - NODELET_WARN_STREAM("The parameter `general/svo_compression` has an invalid value. Please check it in the " - "configuration file `common.yaml`"); + if (svo_compr >= static_cast(sl::SVO_COMPRESSION_MODE::LAST)) + { + NODELET_WARN_STREAM( + "The parameter `general/svo_compression` has an invalid value. Please check it in the " + "configuration file `common.yaml`"); - svo_compr = 0; - } + svo_compr = 0; + } - mSvoComprMode = static_cast(svo_compr); + mSvoComprMode = static_cast(svo_compr); - NODELET_INFO_STREAM(" * SVO REC compression\t\t-> " << sl::toString(mSvoComprMode)); + NODELET_INFO_STREAM(" * SVO REC compression\t\t-> " << sl::toString(mSvoComprMode)); } void ZEDWrapperNodelet::readDynParams() { - NODELET_INFO_STREAM("*** DYNAMIC PARAMETERS (Init. values) ***"); - - mNhNs.getParam("brightness", mCamBrightness); - NODELET_INFO_STREAM(" * [DYN] brightness\t\t-> " << mCamBrightness); - mNhNs.getParam("contrast", mCamContrast); - NODELET_INFO_STREAM(" * [DYN] contrast\t\t-> " << mCamContrast); - mNhNs.getParam("hue", mCamHue); - NODELET_INFO_STREAM(" * [DYN] hue\t\t\t-> " << mCamHue); - mNhNs.getParam("saturation", mCamSaturation); - NODELET_INFO_STREAM(" * [DYN] saturation\t\t-> " << mCamSaturation); - mNhNs.getParam("sharpness", mCamSharpness); - NODELET_INFO_STREAM(" * [DYN] sharpness\t\t-> " << mCamSharpness); - mNhNs.getParam("gamma", mCamGamma); - NODELET_INFO_STREAM(" * [DYN] gamma\t\t\t-> " << mCamGamma); - mNhNs.getParam("auto_exposure_gain", mCamAutoExposure); - NODELET_INFO_STREAM(" * [DYN] auto_exposure_gain\t-> " << (mCamAutoExposure ? "ENABLED": "DISABLED")); - mNhNs.getParam("gain", mCamGain); - mNhNs.getParam("exposure", mCamExposure); - if (!mCamAutoExposure) { - NODELET_INFO_STREAM(" * [DYN] gain\t\t-> " << mCamGain); - NODELET_INFO_STREAM(" * [DYN] exposure\t\t-> " << mCamExposure); - } - mNhNs.getParam("auto_whitebalance", mCamAutoWB); - NODELET_INFO_STREAM(" * [DYN] auto_whitebalance\t-> " << (mCamAutoWB ? "ENABLED": "DISABLED")); - mNhNs.getParam("whitebalance_temperature", mCamWB); - if (!mCamAutoWB) { - NODELET_INFO_STREAM(" * [DYN] whitebalance_temperature\t\t-> " << mCamWB); - } + NODELET_INFO_STREAM("*** DYNAMIC PARAMETERS (Init. values) ***"); + + mNhNs.getParam("brightness", mCamBrightness); + NODELET_INFO_STREAM(" * [DYN] brightness\t\t-> " << mCamBrightness); + mNhNs.getParam("contrast", mCamContrast); + NODELET_INFO_STREAM(" * [DYN] contrast\t\t-> " << mCamContrast); + mNhNs.getParam("hue", mCamHue); + NODELET_INFO_STREAM(" * [DYN] hue\t\t\t-> " << mCamHue); + mNhNs.getParam("saturation", mCamSaturation); + NODELET_INFO_STREAM(" * [DYN] saturation\t\t-> " << mCamSaturation); + mNhNs.getParam("sharpness", mCamSharpness); + NODELET_INFO_STREAM(" * [DYN] sharpness\t\t-> " << mCamSharpness); + mNhNs.getParam("gamma", mCamGamma); + NODELET_INFO_STREAM(" * [DYN] gamma\t\t\t-> " << mCamGamma); + mNhNs.getParam("auto_exposure_gain", mCamAutoExposure); + NODELET_INFO_STREAM(" * [DYN] auto_exposure_gain\t-> " << (mCamAutoExposure ? "ENABLED" : "DISABLED")); + mNhNs.getParam("gain", mCamGain); + mNhNs.getParam("exposure", mCamExposure); + if (!mCamAutoExposure) + { + NODELET_INFO_STREAM(" * [DYN] gain\t\t-> " << mCamGain); + NODELET_INFO_STREAM(" * [DYN] exposure\t\t-> " << mCamExposure); + } + mNhNs.getParam("auto_whitebalance", mCamAutoWB); + NODELET_INFO_STREAM(" * [DYN] auto_whitebalance\t-> " << (mCamAutoWB ? "ENABLED" : "DISABLED")); + mNhNs.getParam("whitebalance_temperature", mCamWB); + if (!mCamAutoWB) + { + NODELET_INFO_STREAM(" * [DYN] whitebalance_temperature\t\t-> " << mCamWB); + } - if (mCamAutoExposure) { - mTriggerAutoExposure = true; - } - if (mCamAutoWB) { - mTriggerAutoWB = true; - } + if (mCamAutoExposure) + { + mTriggerAutoExposure = true; + } + if (mCamAutoWB) + { + mTriggerAutoWB = true; + } - if(!mDepthDisabled) { - mNhNs.getParam("depth_confidence", mCamDepthConfidence); - NODELET_INFO_STREAM(" * [DYN] Depth confidence\t-> " << mCamDepthConfidence); - mNhNs.getParam("depth_texture_conf", mCamDepthTextureConf); - NODELET_INFO_STREAM(" * [DYN] Depth texture conf.\t-> " << mCamDepthTextureConf); - - mNhNs.getParam("point_cloud_freq", mPointCloudFreq); - if(mPointCloudFreq>mPubFrameRate) { - mPointCloudFreq=mPubFrameRate; - NODELET_WARN_STREAM("'point_cloud_freq' cannot be major than 'general/pub_frame_rate'. Set to " - << mPointCloudFreq); - } - NODELET_INFO_STREAM(" * [DYN] point_cloud_freq\t-> " << mPointCloudFreq << " Hz"); + if (!mDepthDisabled) + { + mNhNs.getParam("depth_confidence", mCamDepthConfidence); + NODELET_INFO_STREAM(" * [DYN] Depth confidence\t-> " << mCamDepthConfidence); + mNhNs.getParam("depth_texture_conf", mCamDepthTextureConf); + NODELET_INFO_STREAM(" * [DYN] Depth texture conf.\t-> " << mCamDepthTextureConf); + + mNhNs.getParam("point_cloud_freq", mPointCloudFreq); + if (mPointCloudFreq > mPubFrameRate) + { + mPointCloudFreq = mPubFrameRate; + NODELET_WARN_STREAM("'point_cloud_freq' cannot be major than 'general/pub_frame_rate'. Set to " + << mPointCloudFreq); } + NODELET_INFO_STREAM(" * [DYN] point_cloud_freq\t-> " << mPointCloudFreq << " Hz"); + } } void ZEDWrapperNodelet::readParameters() { - // ----> General - readGeneralParams(); - // <---- General - - // ----> Video - //NODELET_INFO_STREAM("*** VIDEO PARAMETERS ***"); - // Note: there are no "static" video parameters - // <---- Video - - // -----> Depth - readDepthParams(); - // <----- Depth - - // ----> Dynamic - void readDynParams(); - // <---- Dynamic - - // ----> Positional Tracking - readPosTrkParams(); - // <---- Positional Tracking - - // ----> Mapping - readMappingParams(); - // <---- Mapping - - // ----> Object Detection - readObjDetParams(); - // <---- Object Detection - - // ----> Sensors - readSensParams(); - // <---- Sensors - - // ----> SVO - readSvoParams(); - // <---- SVO - - // Remote Stream - mNhNs.param("stream", mRemoteStreamAddr, std::string()); - - // ----> Coordinate frames - NODELET_INFO_STREAM("*** COORDINATE FRAMES ***"); - - mNhNs.param("pos_tracking/map_frame", mMapFrameId, "map"); - mNhNs.param("pos_tracking/odometry_frame", mOdometryFrameId, "odom"); - mNhNs.param("general/base_frame", mBaseFrameId, "base_link"); - - mCameraFrameId = mCameraName + "_camera_center"; - mImuFrameId = mCameraName + "_imu_link"; - mLeftCamFrameId = mCameraName + "_left_camera_frame"; - mLeftCamOptFrameId = mCameraName + "_left_camera_optical_frame"; - mRightCamFrameId = mCameraName + "_right_camera_frame"; - mRightCamOptFrameId = mCameraName + "_right_camera_optical_frame"; - - mBaroFrameId = mCameraName + "_baro_link"; - mMagFrameId = mCameraName + "_mag_link"; - mTempLeftFrameId = mCameraName + "_temp_left_link"; - mTempRightFrameId = mCameraName + "_temp_right_link"; - - mDepthFrameId = mLeftCamFrameId; - mDepthOptFrameId = mLeftCamOptFrameId; - - // Note: Depth image frame id must match color image frame id - mCloudFrameId = mDepthOptFrameId; - mRgbFrameId = mDepthFrameId; - mRgbOptFrameId = mCloudFrameId; - mDisparityFrameId = mDepthFrameId; - mDisparityOptFrameId = mDepthOptFrameId; - mConfidenceFrameId = mDepthFrameId; - mConfidenceOptFrameId = mDepthOptFrameId; - - // Print TF frames - NODELET_INFO_STREAM(" * camera_frame\t\t\t-> " << mCameraFrameId); - NODELET_INFO_STREAM(" * imu_link\t\t\t-> " << mImuFrameId); - NODELET_INFO_STREAM(" * left_camera_frame\t\t-> " << mLeftCamFrameId); - NODELET_INFO_STREAM(" * left_camera_optical_frame\t-> " << mLeftCamOptFrameId); - NODELET_INFO_STREAM(" * right_camera_frame\t\t-> " << mRightCamFrameId); - NODELET_INFO_STREAM(" * right_camera_optical_frame\t-> " << mRightCamOptFrameId); - - if(!mDepthDisabled) { - NODELET_INFO_STREAM(" * map_frame\t\t\t-> " << mMapFrameId); - NODELET_INFO_STREAM(" * odometry_frame\t\t-> " << mOdometryFrameId); - NODELET_INFO_STREAM(" * base_frame\t\t\t-> " << mBaseFrameId); - NODELET_INFO_STREAM(" * depth_frame\t\t\t-> " << mDepthFrameId); - NODELET_INFO_STREAM(" * depth_optical_frame\t\t-> " << mDepthOptFrameId); - NODELET_INFO_STREAM(" * disparity_frame\t\t-> " << mDisparityFrameId); - NODELET_INFO_STREAM(" * disparity_optical_frame\t-> " << mDisparityOptFrameId); - NODELET_INFO_STREAM(" * confidence_frame\t\t-> " << mConfidenceFrameId); - NODELET_INFO_STREAM(" * confidence_optical_frame\t-> " << mConfidenceOptFrameId); - } - // <---- Coordinate frames + // ----> General + readGeneralParams(); + // <---- General + + // ----> Video + // NODELET_INFO_STREAM("*** VIDEO PARAMETERS ***"); + // Note: there are no "static" video parameters + // <---- Video + + // -----> Depth + readDepthParams(); + // <----- Depth + + // ----> Dynamic + void readDynParams(); + // <---- Dynamic + + // ----> Positional Tracking + readPosTrkParams(); + // <---- Positional Tracking + + // ----> Mapping + readMappingParams(); + // <---- Mapping + + // ----> Object Detection + readObjDetParams(); + // <---- Object Detection + + // ----> Sensors + readSensParams(); + // <---- Sensors + + // ----> SVO + readSvoParams(); + // <---- SVO + + // Remote Stream + mNhNs.param("stream", mRemoteStreamAddr, std::string()); + + // ----> Coordinate frames + NODELET_INFO_STREAM("*** COORDINATE FRAMES ***"); + + mNhNs.param("pos_tracking/map_frame", mMapFrameId, "map"); + mNhNs.param("pos_tracking/odometry_frame", mOdometryFrameId, "odom"); + mNhNs.param("general/base_frame", mBaseFrameId, "base_link"); + + mCameraFrameId = mCameraName + "_camera_center"; + mImuFrameId = mCameraName + "_imu_link"; + mLeftCamFrameId = mCameraName + "_left_camera_frame"; + mLeftCamOptFrameId = mCameraName + "_left_camera_optical_frame"; + mRightCamFrameId = mCameraName + "_right_camera_frame"; + mRightCamOptFrameId = mCameraName + "_right_camera_optical_frame"; + + mBaroFrameId = mCameraName + "_baro_link"; + mMagFrameId = mCameraName + "_mag_link"; + mTempLeftFrameId = mCameraName + "_temp_left_link"; + mTempRightFrameId = mCameraName + "_temp_right_link"; + + mDepthFrameId = mLeftCamFrameId; + mDepthOptFrameId = mLeftCamOptFrameId; + + // Note: Depth image frame id must match color image frame id + mCloudFrameId = mDepthOptFrameId; + mRgbFrameId = mDepthFrameId; + mRgbOptFrameId = mCloudFrameId; + mDisparityFrameId = mDepthFrameId; + mDisparityOptFrameId = mDepthOptFrameId; + mConfidenceFrameId = mDepthFrameId; + mConfidenceOptFrameId = mDepthOptFrameId; + + // Print TF frames + NODELET_INFO_STREAM(" * camera_frame\t\t\t-> " << mCameraFrameId); + NODELET_INFO_STREAM(" * imu_link\t\t\t-> " << mImuFrameId); + NODELET_INFO_STREAM(" * left_camera_frame\t\t-> " << mLeftCamFrameId); + NODELET_INFO_STREAM(" * left_camera_optical_frame\t-> " << mLeftCamOptFrameId); + NODELET_INFO_STREAM(" * right_camera_frame\t\t-> " << mRightCamFrameId); + NODELET_INFO_STREAM(" * right_camera_optical_frame\t-> " << mRightCamOptFrameId); + + if (!mDepthDisabled) + { + NODELET_INFO_STREAM(" * map_frame\t\t\t-> " << mMapFrameId); + NODELET_INFO_STREAM(" * odometry_frame\t\t-> " << mOdometryFrameId); + NODELET_INFO_STREAM(" * base_frame\t\t\t-> " << mBaseFrameId); + NODELET_INFO_STREAM(" * depth_frame\t\t\t-> " << mDepthFrameId); + NODELET_INFO_STREAM(" * depth_optical_frame\t\t-> " << mDepthOptFrameId); + NODELET_INFO_STREAM(" * disparity_frame\t\t-> " << mDisparityFrameId); + NODELET_INFO_STREAM(" * disparity_optical_frame\t-> " << mDisparityOptFrameId); + NODELET_INFO_STREAM(" * confidence_frame\t\t-> " << mConfidenceFrameId); + NODELET_INFO_STREAM(" * confidence_optical_frame\t-> " << mConfidenceOptFrameId); + } + // <---- Coordinate frames } -std::string ZEDWrapperNodelet::getRoiParam(std::string paramName, std::vector> & outVal) +std::string ZEDWrapperNodelet::getRoiParam(std::string paramName, std::vector>& outVal) { outVal.clear(); @@ -1139,14 +1291,16 @@ std::string ZEDWrapperNodelet::getRoiParam(std::string paramName, std::vector> & in_poly, - std::vector & out_poly) +std::string ZEDWrapperNodelet::parseRoiPoly(const std::vector>& in_poly, + std::vector& out_poly) { out_poly.clear(); @@ -1167,34 +1320,40 @@ std::string ZEDWrapperNodelet::parseRoiPoly( size_t poly_size = in_poly.size(); - if (poly_size < 3) { - if (poly_size != 0) { - NODELET_WARN_STREAM("A vector with " << - poly_size << - " points is not enough to create a polygon to set a Region " - "of Interest."); + if (poly_size < 3) + { + if (poly_size != 0) + { + NODELET_WARN_STREAM("A vector with " << poly_size + << " points is not enough to create a polygon to set a Region " + "of Interest."); return std::string(); } - } else { - for (size_t i; i < poly_size; ++i) { - if (in_poly[i].size() != 2) { - NODELET_WARN_STREAM("The component with index '" << i << - "' of the ROI vector " - "has not the correct size."); + } + else + { + for (size_t i; i < poly_size; ++i) + { + if (in_poly[i].size() != 2) + { + NODELET_WARN_STREAM("The component with index '" << i + << "' of the ROI vector " + "has not the correct size."); out_poly.clear(); return std::string(); - } else if (in_poly[i][0] < 0.0 || in_poly[i][1] < 0.0 || in_poly[i][0] > 1.0 || - in_poly[i][1] > 1.0) + } + else if (in_poly[i][0] < 0.0 || in_poly[i][1] < 0.0 || in_poly[i][0] > 1.0 || in_poly[i][1] > 1.0) { - NODELET_WARN_STREAM("The component with index '" << i << - "' of the ROI vector " - "is not a " - "valid normalized point: [" << - in_poly[i][0] << "," << in_poly[i][1] << - "]."); + NODELET_WARN_STREAM("The component with index '" << i + << "' of the ROI vector " + "is not a " + "valid normalized point: [" + << in_poly[i][0] << "," << in_poly[i][1] << "]."); out_poly.clear(); return std::string(); - } else { + } + else + { sl::float2 pt; pt.x = in_poly[i][0]; pt.y = in_poly[i][1]; @@ -1206,7 +1365,8 @@ std::string ZEDWrapperNodelet::parseRoiPoly( ss += "]"; } - if (i != poly_size - 1) { + if (i != poly_size - 1) + { ss += ","; } } @@ -1218,2652 +1378,3055 @@ std::string ZEDWrapperNodelet::parseRoiPoly( void ZEDWrapperNodelet::checkResolFps() { - switch (mCamResol) { + switch (mCamResol) + { case sl::RESOLUTION::HD2K: - if (mCamFrameRate != 15) { - NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD2K. Set to 15 FPS."); - mCamFrameRate = 15; - } + if (mCamFrameRate != 15) + { + NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD2K. Set to 15 FPS."); + mCamFrameRate = 15; + } - break; + break; case sl::RESOLUTION::HD1080: - if (mZedUserCamModel == sl::MODEL::ZED_X || mZedUserCamModel == sl::MODEL::ZED_XM) + if (mZedUserCamModel == sl::MODEL::ZED_X || mZedUserCamModel == sl::MODEL::ZED_XM) + { + if (mCamFrameRate == 60 || mCamFrameRate == 30) { - if (mCamFrameRate == 60 || mCamFrameRate == 30) { - break; - } + break; + } - if (mCamFrameRate > 30 && mCamFrameRate < 60) { - NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD1080. Set to 30 FPS."); - mCamFrameRate = 30; - } else if (mCamFrameRate > 60) { - NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD1080. Set to 60 FPS."); - mCamFrameRate = 60; - } else { - NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD1080. Set to 30 FPS."); - mCamFrameRate = 30; - } + if (mCamFrameRate > 30 && mCamFrameRate < 60) + { + NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD1080. Set to 30 FPS."); + mCamFrameRate = 30; + } + else if (mCamFrameRate > 60) + { + NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD1080. Set to 60 FPS."); + mCamFrameRate = 60; } else { - if (mCamFrameRate == 15 || mCamFrameRate == 30) { - break; - } - - if (mCamFrameRate > 15 && mCamFrameRate < 30) { - NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD1080. Set to 15 FPS."); - mCamFrameRate = 15; - } else if (mCamFrameRate > 30) { - NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD1080. Set to 30 FPS."); - mCamFrameRate = 30; - } else { - NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD1080. Set to 15 FPS."); - mCamFrameRate = 15; - } + NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD1080. Set to 30 FPS."); + mCamFrameRate = 30; } - - break; - - case sl::RESOLUTION::HD720: - if (mCamFrameRate == 15 || mCamFrameRate == 30 || mCamFrameRate == 60) { - break; + } + else + { + if (mCamFrameRate == 15 || mCamFrameRate == 30) + { + break; } - if (mCamFrameRate > 15 && mCamFrameRate < 30) { - NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD720. Set to 15 FPS."); - mCamFrameRate = 15; - } else if (mCamFrameRate > 30 && mCamFrameRate < 60) { - NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD720. Set to 30 FPS."); - mCamFrameRate = 30; - } else if (mCamFrameRate > 60) { - NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD720. Set to 60 FPS."); - mCamFrameRate = 60; - } else { - NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD720. Set to 15 FPS."); - mCamFrameRate = 15; + if (mCamFrameRate > 15 && mCamFrameRate < 30) + { + NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD1080. Set to 15 FPS."); + mCamFrameRate = 15; } - - break; - - case sl::RESOLUTION::VGA: - if (mCamFrameRate == 15 || mCamFrameRate == 30 || mCamFrameRate == 60 || mCamFrameRate == 100) { - break; + else if (mCamFrameRate > 30) + { + NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD1080. Set to 30 FPS."); + mCamFrameRate = 30; } - - if (mCamFrameRate > 15 && mCamFrameRate < 30) { - NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution VGA. Set to 15 FPS."); - mCamFrameRate = 15; - } else if (mCamFrameRate > 30 && mCamFrameRate < 60) { - NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution VGA. Set to 30 FPS."); - mCamFrameRate = 30; - } else if (mCamFrameRate > 60 && mCamFrameRate < 100) { - NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution VGA. Set to 60 FPS."); - mCamFrameRate = 60; - } else if (mCamFrameRate > 100) { - NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution VGA. Set to 100 FPS."); - mCamFrameRate = 100; - } else { - NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution VGA. Set to 15 FPS."); - mCamFrameRate = 15; + else + { + NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD1080. Set to 15 FPS."); + mCamFrameRate = 15; } + } + + break; + case sl::RESOLUTION::HD720: + if (mCamFrameRate == 15 || mCamFrameRate == 30 || mCamFrameRate == 60) + { break; + } - case sl::RESOLUTION::HD1200: - if (mCamFrameRate == 60 || mCamFrameRate == 30) { - break; - } + if (mCamFrameRate > 15 && mCamFrameRate < 30) + { + NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD720. Set to 15 FPS."); + mCamFrameRate = 15; + } + else if (mCamFrameRate > 30 && mCamFrameRate < 60) + { + NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD720. Set to 30 FPS."); + mCamFrameRate = 30; + } + else if (mCamFrameRate > 60) + { + NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD720. Set to 60 FPS."); + mCamFrameRate = 60; + } + else + { + NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD720. Set to 15 FPS."); + mCamFrameRate = 15; + } - if (mCamFrameRate > 30 && mCamFrameRate < 60) { - NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD1200. Set to 30 FPS."); - mCamFrameRate = 30; - } else if (mCamFrameRate > 60) { - NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD1200. Set to 60 FPS."); - mCamFrameRate = 60; - } else { - NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD1200. Set to 30 FPS."); - mCamFrameRate = 30; - } + break; + case sl::RESOLUTION::VGA: + if (mCamFrameRate == 15 || mCamFrameRate == 30 || mCamFrameRate == 60 || mCamFrameRate == 100) + { break; + } - case sl::RESOLUTION::SVGA: - if (mCamFrameRate == 120 || mCamFrameRate == 60) { - break; - } + if (mCamFrameRate > 15 && mCamFrameRate < 30) + { + NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution VGA. Set to 15 FPS."); + mCamFrameRate = 15; + } + else if (mCamFrameRate > 30 && mCamFrameRate < 60) + { + NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution VGA. Set to 30 FPS."); + mCamFrameRate = 30; + } + else if (mCamFrameRate > 60 && mCamFrameRate < 100) + { + NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution VGA. Set to 60 FPS."); + mCamFrameRate = 60; + } + else if (mCamFrameRate > 100) + { + NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution VGA. Set to 100 FPS."); + mCamFrameRate = 100; + } + else + { + NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution VGA. Set to 15 FPS."); + mCamFrameRate = 15; + } - if (mCamFrameRate > 60 && mCamFrameRate < 120) { - NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution SVGA. Set to 60 FPS."); - mCamFrameRate = 60; - } else if (mCamFrameRate > 120) { - NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution SVGA. Set to 120 FPS."); - mCamFrameRate = 120; - } else { - NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution SVGA. Set to 60 FPS."); - mCamFrameRate = 60; - } + break; + case sl::RESOLUTION::HD1200: + if (mCamFrameRate == 60 || mCamFrameRate == 30) + { break; + } - case sl::RESOLUTION::AUTO: + if (mCamFrameRate > 30 && mCamFrameRate < 60) + { + NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD1200. Set to 30 FPS."); + mCamFrameRate = 30; + } + else if (mCamFrameRate > 60) + { + NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD1200. Set to 60 FPS."); + mCamFrameRate = 60; + } + else + { + NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD1200. Set to 30 FPS."); + mCamFrameRate = 30; + } + + break; + + case sl::RESOLUTION::SVGA: + if (mCamFrameRate == 120 || mCamFrameRate == 60) + { break; - + } + + if (mCamFrameRate > 60 && mCamFrameRate < 120) + { + NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution SVGA. Set to 60 FPS."); + mCamFrameRate = 60; + } + else if (mCamFrameRate > 120) + { + NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution SVGA. Set to 120 FPS."); + mCamFrameRate = 120; + } + else + { + NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution SVGA. Set to 60 FPS."); + mCamFrameRate = 60; + } + + break; + + case sl::RESOLUTION::AUTO: + break; + default: - NODELET_WARN_STREAM("Invalid resolution. Set to AUTO with default frame rate"); - mCamResol = sl::RESOLUTION::AUTO; - } + NODELET_WARN_STREAM("Invalid resolution. Set to AUTO with default frame rate"); + mCamResol = sl::RESOLUTION::AUTO; + } } void ZEDWrapperNodelet::initTransforms() { - // According to REP 105 -> http://www.ros.org/reps/rep-0105.html - - // base_link <- odom <- map - // ^ | - // | | - // ------------------- - - // ----> Dynamic transforms - mOdom2BaseTransf.setIdentity(); // broadcasted if `publish_tf` is true - mMap2OdomTransf.setIdentity(); // broadcasted if `publish_map_tf` is true - mMap2BaseTransf.setIdentity(); // used internally, but not broadcasted - mMap2CameraTransf.setIdentity(); // used internally, but not broadcasted - // <---- Dynamic transforms + // According to REP 105 -> http://www.ros.org/reps/rep-0105.html + + // base_link <- odom <- map + // ^ | + // | | + // ------------------- + + // ----> Dynamic transforms + mOdom2BaseTransf.setIdentity(); // broadcasted if `publish_tf` is true + mMap2OdomTransf.setIdentity(); // broadcasted if `publish_map_tf` is true + mMap2BaseTransf.setIdentity(); // used internally, but not broadcasted + mMap2CameraTransf.setIdentity(); // used internally, but not broadcasted + // <---- Dynamic transforms } bool ZEDWrapperNodelet::getCamera2BaseTransform() { - NODELET_DEBUG("Getting static TF from '%s' to '%s'", mCameraFrameId.c_str(), mBaseFrameId.c_str()); + 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 bool first_error = true; - // ----> Static transforms - // Sensor to Base link - try { - // Save the transformation - geometry_msgs::TransformStamped c2b = mTfBuffer->lookupTransform(mCameraFrameId, mBaseFrameId, ros::Time(0), ros::Duration(0.1)); + // ----> Static transforms + // Sensor to Base link + try + { + // Save the transformation + geometry_msgs::TransformStamped c2b = + mTfBuffer->lookupTransform(mCameraFrameId, mBaseFrameId, ros::Time(0), ros::Duration(0.1)); - // Get the TF2 transformation - tf2::fromMsg(c2b.transform, mCamera2BaseTransf); + // Get the TF2 transformation + tf2::fromMsg(c2b.transform, mCamera2BaseTransf); - double roll, pitch, yaw; - tf2::Matrix3x3(mCamera2BaseTransf.getRotation()).getRPY(roll, pitch, yaw); - - NODELET_INFO("Static transform Camera Center to Base [%s -> %s]", mCameraFrameId.c_str(), mBaseFrameId.c_str()); - NODELET_INFO(" * Translation: {%.3f,%.3f,%.3f}", mCamera2BaseTransf.getOrigin().x(), - mCamera2BaseTransf.getOrigin().y(), mCamera2BaseTransf.getOrigin().z()); - NODELET_INFO(" * Rotation: {%.3f,%.3f,%.3f}", roll * RAD2DEG, pitch * RAD2DEG, yaw * RAD2DEG); - } catch (tf2::TransformException& ex) { - if (!first_error) { - NODELET_DEBUG_THROTTLE(1.0, "Transform error: %s", ex.what()); - NODELET_WARN_THROTTLE(1.0, "The tf from '%s' to '%s' is not available.", mCameraFrameId.c_str(), - mBaseFrameId.c_str()); - NODELET_WARN_THROTTLE(1.0, - "Note: one of the possible cause of the problem is the absense of an instance " - "of the `robot_state_publisher` node publishing the correct static TF transformations " - "or a modified URDF not correctly reproducing the ZED " - "TF chain '%s' -> '%s' -> '%s'", - mBaseFrameId.c_str(), mCameraFrameId.c_str(), mDepthFrameId.c_str()); - first_error = false; - } + double roll, pitch, yaw; + tf2::Matrix3x3(mCamera2BaseTransf.getRotation()).getRPY(roll, pitch, yaw); - mCamera2BaseTransf.setIdentity(); - return false; - } + NODELET_INFO("Static transform Camera Center to Base [%s -> %s]", mCameraFrameId.c_str(), mBaseFrameId.c_str()); + NODELET_INFO(" * Translation: {%.3f,%.3f,%.3f}", mCamera2BaseTransf.getOrigin().x(), + mCamera2BaseTransf.getOrigin().y(), mCamera2BaseTransf.getOrigin().z()); + NODELET_INFO(" * Rotation: {%.3f,%.3f,%.3f}", roll * RAD2DEG, pitch * RAD2DEG, yaw * RAD2DEG); + } + catch (tf2::TransformException& ex) + { + if (!first_error) + { + NODELET_DEBUG_THROTTLE(1.0, "Transform error: %s", ex.what()); + NODELET_WARN_THROTTLE(1.0, "The tf from '%s' to '%s' is not available.", mCameraFrameId.c_str(), + mBaseFrameId.c_str()); + NODELET_WARN_THROTTLE(1.0, + "Note: one of the possible cause of the problem is the absense of an instance " + "of the `robot_state_publisher` node publishing the correct static TF transformations " + "or a modified URDF not correctly reproducing the ZED " + "TF chain '%s' -> '%s' -> '%s'", + mBaseFrameId.c_str(), mCameraFrameId.c_str(), mDepthFrameId.c_str()); + first_error = false; + } + + mCamera2BaseTransf.setIdentity(); + return false; + } - // <---- Static transforms - mCamera2BaseTransfValid = true; - return true; + // <---- Static transforms + mCamera2BaseTransfValid = true; + return true; } bool ZEDWrapperNodelet::getSens2CameraTransform() { - NODELET_DEBUG("Getting static TF from '%s' to '%s'", mDepthFrameId.c_str(), mCameraFrameId.c_str()); + NODELET_DEBUG("Getting static TF from '%s' to '%s'", mDepthFrameId.c_str(), mCameraFrameId.c_str()); - mSensor2CameraTransfValid = false; + mSensor2CameraTransfValid = false; - static bool first_error = true; + static bool first_error = true; - // ----> Static transforms - // Sensor to Camera Center - try { - // Save the transformation - geometry_msgs::TransformStamped s2c = mTfBuffer->lookupTransform(mDepthFrameId, mCameraFrameId, ros::Time(0), ros::Duration(0.1)); - // Get the TF2 transformation - tf2::fromMsg(s2c.transform, mSensor2CameraTransf); + // ----> Static transforms + // Sensor to Camera Center + try + { + // Save the transformation + geometry_msgs::TransformStamped s2c = + mTfBuffer->lookupTransform(mDepthFrameId, mCameraFrameId, ros::Time(0), ros::Duration(0.1)); + // Get the TF2 transformation + tf2::fromMsg(s2c.transform, mSensor2CameraTransf); - double roll, pitch, yaw; - tf2::Matrix3x3(mSensor2CameraTransf.getRotation()).getRPY(roll, pitch, yaw); - - NODELET_INFO("Static transform Sensor to Camera Center [%s -> %s]", mDepthFrameId.c_str(), mCameraFrameId.c_str()); - NODELET_INFO(" * Translation: {%.3f,%.3f,%.3f}", mSensor2CameraTransf.getOrigin().x(), - mSensor2CameraTransf.getOrigin().y(), mSensor2CameraTransf.getOrigin().z()); - NODELET_INFO(" * Rotation: {%.3f,%.3f,%.3f}", roll * RAD2DEG, pitch * RAD2DEG, yaw * RAD2DEG); - } catch (tf2::TransformException& ex) { - if (!first_error) { - NODELET_DEBUG_THROTTLE(1.0, "Transform error: %s", ex.what()); - NODELET_WARN_THROTTLE(1.0, "The tf from '%s' to '%s' is not available.", mDepthFrameId.c_str(), - mCameraFrameId.c_str()); - NODELET_WARN_THROTTLE(1.0, - "Note: one of the possible cause of the problem is the absense of an instance " - "of the `robot_state_publisher` node publishing the correct static TF transformations " - "or a modified URDF not correctly reproducing the ZED " - "TF chain '%s' -> '%s' -> '%s'", - mBaseFrameId.c_str(), mCameraFrameId.c_str(), mDepthFrameId.c_str()); - first_error = false; - } + double roll, pitch, yaw; + tf2::Matrix3x3(mSensor2CameraTransf.getRotation()).getRPY(roll, pitch, yaw); - mSensor2CameraTransf.setIdentity(); - return false; - } + NODELET_INFO("Static transform Sensor to Camera Center [%s -> %s]", mDepthFrameId.c_str(), mCameraFrameId.c_str()); + NODELET_INFO(" * Translation: {%.3f,%.3f,%.3f}", mSensor2CameraTransf.getOrigin().x(), + mSensor2CameraTransf.getOrigin().y(), mSensor2CameraTransf.getOrigin().z()); + NODELET_INFO(" * Rotation: {%.3f,%.3f,%.3f}", roll * RAD2DEG, pitch * RAD2DEG, yaw * RAD2DEG); + } + catch (tf2::TransformException& ex) + { + if (!first_error) + { + NODELET_DEBUG_THROTTLE(1.0, "Transform error: %s", ex.what()); + NODELET_WARN_THROTTLE(1.0, "The tf from '%s' to '%s' is not available.", mDepthFrameId.c_str(), + mCameraFrameId.c_str()); + NODELET_WARN_THROTTLE(1.0, + "Note: one of the possible cause of the problem is the absense of an instance " + "of the `robot_state_publisher` node publishing the correct static TF transformations " + "or a modified URDF not correctly reproducing the ZED " + "TF chain '%s' -> '%s' -> '%s'", + mBaseFrameId.c_str(), mCameraFrameId.c_str(), mDepthFrameId.c_str()); + first_error = false; + } + + mSensor2CameraTransf.setIdentity(); + return false; + } - // <---- Static transforms + // <---- Static transforms - mSensor2CameraTransfValid = true; - return true; + mSensor2CameraTransfValid = true; + return true; } 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; - - // ----> Static transforms - // Sensor to Base link - try { - // Save the transformation - geometry_msgs::TransformStamped s2b = mTfBuffer->lookupTransform(mDepthFrameId, mBaseFrameId, ros::Time(0), ros::Duration(0.1)); - // Get the TF2 transformation - tf2::fromMsg(s2b.transform, mSensor2BaseTransf); - - double roll, pitch, yaw; - tf2::Matrix3x3(mSensor2BaseTransf.getRotation()).getRPY(roll, pitch, yaw); - - NODELET_INFO("Static transform Sensor to Base [%s -> %s]", mDepthFrameId.c_str(), mBaseFrameId.c_str()); - NODELET_INFO(" * Translation: {%.3f,%.3f,%.3f}", mSensor2BaseTransf.getOrigin().x(), - mSensor2BaseTransf.getOrigin().y(), mSensor2BaseTransf.getOrigin().z()); - NODELET_INFO(" * Rotation: {%.3f,%.3f,%.3f}", roll * RAD2DEG, pitch * RAD2DEG, yaw * RAD2DEG); - } catch (tf2::TransformException& ex) { - if (!first_error) { - NODELET_DEBUG_THROTTLE(1.0, "Transform error: %s", ex.what()); - NODELET_WARN_THROTTLE(1.0, "The tf from '%s' to '%s' is not available.", mDepthFrameId.c_str(), - mBaseFrameId.c_str()); - NODELET_WARN_THROTTLE(1.0, - "Note: one of the possible cause of the problem is the absense of an instance " - "of the `robot_state_publisher` node publishing the correct static TF transformations " - "or a modified URDF not correctly reproducing the ZED " - "TF chain '%s' -> '%s' -> '%s'", - mBaseFrameId.c_str(), mCameraFrameId.c_str(), mDepthFrameId.c_str()); - first_error = false; - } - - mSensor2BaseTransf.setIdentity(); - return false; - } + NODELET_DEBUG("Getting static TF from '%s' to '%s'", mDepthFrameId.c_str(), mBaseFrameId.c_str()); + + mSensor2BaseTransfValid = false; + static bool first_error = true; + + // ----> Static transforms + // Sensor to Base link + try + { + // Save the transformation + geometry_msgs::TransformStamped s2b = + mTfBuffer->lookupTransform(mDepthFrameId, mBaseFrameId, ros::Time(0), ros::Duration(0.1)); + // Get the TF2 transformation + tf2::fromMsg(s2b.transform, mSensor2BaseTransf); + + double roll, pitch, yaw; + tf2::Matrix3x3(mSensor2BaseTransf.getRotation()).getRPY(roll, pitch, yaw); + + NODELET_INFO("Static transform Sensor to Base [%s -> %s]", mDepthFrameId.c_str(), mBaseFrameId.c_str()); + NODELET_INFO(" * Translation: {%.3f,%.3f,%.3f}", mSensor2BaseTransf.getOrigin().x(), + mSensor2BaseTransf.getOrigin().y(), mSensor2BaseTransf.getOrigin().z()); + NODELET_INFO(" * Rotation: {%.3f,%.3f,%.3f}", roll * RAD2DEG, pitch * RAD2DEG, yaw * RAD2DEG); + } + catch (tf2::TransformException& ex) + { + if (!first_error) + { + NODELET_DEBUG_THROTTLE(1.0, "Transform error: %s", ex.what()); + NODELET_WARN_THROTTLE(1.0, "The tf from '%s' to '%s' is not available.", mDepthFrameId.c_str(), + mBaseFrameId.c_str()); + NODELET_WARN_THROTTLE(1.0, + "Note: one of the possible cause of the problem is the absense of an instance " + "of the `robot_state_publisher` node publishing the correct static TF transformations " + "or a modified URDF not correctly reproducing the ZED " + "TF chain '%s' -> '%s' -> '%s'", + mBaseFrameId.c_str(), mCameraFrameId.c_str(), mDepthFrameId.c_str()); + first_error = false; + } + + mSensor2BaseTransf.setIdentity(); + return false; + } - // <---- Static transforms + // <---- Static transforms - mSensor2BaseTransfValid = true; - return true; + mSensor2BaseTransfValid = true; + return true; } bool ZEDWrapperNodelet::set_pose(float xt, float yt, float zt, float rr, float pr, float yr) { - initTransforms(); + initTransforms(); - if (!mSensor2BaseTransfValid) { - getSens2BaseTransform(); - } + if (!mSensor2BaseTransfValid) + { + getSens2BaseTransform(); + } - if (!mSensor2CameraTransfValid) { - getSens2CameraTransform(); - } + if (!mSensor2CameraTransfValid) + { + getSens2CameraTransform(); + } - if (!mCamera2BaseTransfValid) { - getCamera2BaseTransform(); - } + if (!mCamera2BaseTransfValid) + { + getCamera2BaseTransform(); + } - // Apply Base to sensor transform - tf2::Transform initPose; - tf2::Vector3 origin(xt, yt, zt); - initPose.setOrigin(origin); - tf2::Quaternion quat; - quat.setRPY(rr, pr, yr); - initPose.setRotation(quat); - - initPose = initPose * mSensor2BaseTransf.inverse(); - - // SL pose - sl::float3 t_vec; - t_vec[0] = initPose.getOrigin().x(); - t_vec[1] = initPose.getOrigin().y(); - t_vec[2] = initPose.getOrigin().z(); - - sl::float4 q_vec; - q_vec[0] = initPose.getRotation().x(); - q_vec[1] = initPose.getRotation().y(); - q_vec[2] = initPose.getRotation().z(); - q_vec[3] = initPose.getRotation().w(); - - sl::Translation trasl(t_vec); - sl::Orientation orient(q_vec); - mInitialPoseSl.setTranslation(trasl); - mInitialPoseSl.setOrientation(orient); - - return (mSensor2BaseTransfValid & mSensor2CameraTransfValid & mCamera2BaseTransfValid); + // Apply Base to sensor transform + tf2::Transform initPose; + tf2::Vector3 origin(xt, yt, zt); + initPose.setOrigin(origin); + tf2::Quaternion quat; + quat.setRPY(rr, pr, yr); + initPose.setRotation(quat); + + initPose = initPose * mSensor2BaseTransf.inverse(); + + // SL pose + sl::float3 t_vec; + t_vec[0] = initPose.getOrigin().x(); + t_vec[1] = initPose.getOrigin().y(); + t_vec[2] = initPose.getOrigin().z(); + + sl::float4 q_vec; + q_vec[0] = initPose.getRotation().x(); + q_vec[1] = initPose.getRotation().y(); + q_vec[2] = initPose.getRotation().z(); + q_vec[3] = initPose.getRotation().w(); + + sl::Translation trasl(t_vec); + sl::Orientation orient(q_vec); + mInitialPoseSl.setTranslation(trasl); + mInitialPoseSl.setOrientation(orient); + + return (mSensor2BaseTransfValid & mSensor2CameraTransfValid & mCamera2BaseTransfValid); } bool ZEDWrapperNodelet::on_set_pose(zed_interfaces::set_pose::Request& req, zed_interfaces::set_pose::Response& res) { - mInitialBasePose.resize(6); - mInitialBasePose[0] = req.x; - mInitialBasePose[1] = req.y; - mInitialBasePose[2] = req.z; - mInitialBasePose[3] = req.R; - mInitialBasePose[4] = req.P; - mInitialBasePose[5] = req.Y; + mInitialBasePose.resize(6); + mInitialBasePose[0] = req.x; + mInitialBasePose[1] = req.y; + mInitialBasePose[2] = req.z; + mInitialBasePose[3] = req.R; + mInitialBasePose[4] = req.P; + mInitialBasePose[5] = req.Y; - std::lock_guard lock(mPosTrkMutex); + std::lock_guard lock(mPosTrkMutex); - // Restart tracking - start_pos_tracking(); + // Restart tracking + start_pos_tracking(); - res.done = true; - return true; + res.done = true; + return true; } bool ZEDWrapperNodelet::on_reset_tracking(zed_interfaces::reset_tracking::Request& req, - zed_interfaces::reset_tracking::Response& res) + zed_interfaces::reset_tracking::Response& res) { - if (!mPosTrackingActivated) { - res.reset_done = false; - return false; - } + if (!mPosTrackingActivated) + { + res.reset_done = false; + return false; + } - std::lock_guard lock(mPosTrkMutex); + std::lock_guard lock(mPosTrkMutex); - // Restart tracking - start_pos_tracking(); + // Restart tracking + start_pos_tracking(); - res.reset_done = true; - return true; + res.reset_done = true; + return true; } bool ZEDWrapperNodelet::on_reset_odometry(zed_interfaces::reset_odometry::Request& req, - zed_interfaces::reset_odometry::Response& res) + zed_interfaces::reset_odometry::Response& res) { - mResetOdom = true; - res.reset_done = true; - return true; + mResetOdom = true; + res.reset_done = true; + return true; } bool ZEDWrapperNodelet::start_3d_mapping() { - if (!mMappingEnabled) { - return false; - } + if (!mMappingEnabled) + { + return false; + } - NODELET_INFO_STREAM("*** Starting Spatial Mapping ***"); + NODELET_INFO_STREAM("*** Starting Spatial Mapping ***"); - sl::SpatialMappingParameters params; - params.map_type = sl::SpatialMappingParameters::SPATIAL_MAP_TYPE::FUSED_POINT_CLOUD; - params.use_chunk_only = true; + sl::SpatialMappingParameters params; + params.map_type = sl::SpatialMappingParameters::SPATIAL_MAP_TYPE::FUSED_POINT_CLOUD; + params.use_chunk_only = true; - sl::SpatialMappingParameters spMapPar; + sl::SpatialMappingParameters spMapPar; - float lRes = spMapPar.allowed_resolution.first; - float hRes = spMapPar.allowed_resolution.second; + float lRes = spMapPar.allowed_resolution.first; + float hRes = spMapPar.allowed_resolution.second; - if (mMappingRes < lRes) { - NODELET_WARN_STREAM("'mapping/resolution' value (" - << mMappingRes << " m) is lower than the allowed resolution values. Fixed automatically"); - mMappingRes = lRes; - } - if (mMappingRes > hRes) { - NODELET_WARN_STREAM("'mapping/resolution' value (" - << mMappingRes << " m) is higher than the allowed resolution values. Fixed automatically"); - mMappingRes = hRes; - } + if (mMappingRes < lRes) + { + NODELET_WARN_STREAM("'mapping/resolution' value (" + << mMappingRes << " m) is lower than the allowed resolution values. Fixed automatically"); + mMappingRes = lRes; + } + if (mMappingRes > hRes) + { + NODELET_WARN_STREAM("'mapping/resolution' value (" + << mMappingRes << " m) is higher than the allowed resolution values. Fixed automatically"); + mMappingRes = hRes; + } - params.resolution_meter = mMappingRes; - - float lRng = spMapPar.allowed_range.first; - float hRng = spMapPar.allowed_range.second; - - if (mMaxMappingRange < 0) { - mMaxMappingRange = sl::SpatialMappingParameters::getRecommendedRange(mMappingRes, mZed); - NODELET_INFO_STREAM("Mapping: max range set to " << mMaxMappingRange << " m for a resolution of " << mMappingRes - << " m"); - } else if (mMaxMappingRange < lRng) { - NODELET_WARN_STREAM("'mapping/max_mapping_range_m' value (" - << mMaxMappingRange << " m) is lower than the allowed resolution values. Fixed automatically"); - mMaxMappingRange = lRng; - } else if (mMaxMappingRange > hRng) { - NODELET_WARN_STREAM("'mapping/max_mapping_range_m' value (" - << mMaxMappingRange << " m) is higher than the allowed resolution values. Fixed automatically"); - mMaxMappingRange = hRng; - } + params.resolution_meter = mMappingRes; + + float lRng = spMapPar.allowed_range.first; + float hRng = spMapPar.allowed_range.second; - params.range_meter = mMaxMappingRange; + if (mMaxMappingRange < 0) + { + mMaxMappingRange = sl::SpatialMappingParameters::getRecommendedRange(mMappingRes, mZed); + NODELET_INFO_STREAM("Mapping: max range set to " << mMaxMappingRange << " m for a resolution of " << mMappingRes + << " m"); + } + else if (mMaxMappingRange < lRng) + { + NODELET_WARN_STREAM("'mapping/max_mapping_range_m' value (" + << mMaxMappingRange << " m) is lower than the allowed resolution values. Fixed automatically"); + mMaxMappingRange = lRng; + } + else if (mMaxMappingRange > hRng) + { + NODELET_WARN_STREAM("'mapping/max_mapping_range_m' value (" + << mMaxMappingRange << " m) is higher than the allowed resolution values. Fixed automatically"); + mMaxMappingRange = hRng; + } - sl::ERROR_CODE err = mZed.enableSpatialMapping(params); + params.range_meter = mMaxMappingRange; - if (err == sl::ERROR_CODE::SUCCESS) { - if (mPubFusedCloud.getTopic().empty()) { - std::string pointcloud_fused_topic = "mapping/fused_cloud"; - mPubFusedCloud = mNhNs.advertise(pointcloud_fused_topic, 1); - NODELET_INFO_STREAM(" * Advertised on topic " << mPubFusedCloud.getTopic() << " @ " << mFusedPcPubFreq << " Hz"); - } + sl::ERROR_CODE err = mZed.enableSpatialMapping(params); - mMappingRunning = true; + if (err == sl::ERROR_CODE::SUCCESS) + { + if (mPubFusedCloud.getTopic().empty()) + { + std::string pointcloud_fused_topic = "mapping/fused_cloud"; + mPubFusedCloud = mNhNs.advertise(pointcloud_fused_topic, 1); + NODELET_INFO_STREAM(" * Advertised on topic " << mPubFusedCloud.getTopic() << " @ " << mFusedPcPubFreq << " Hz"); + } - mFusedPcTimer = mNhNs.createTimer(ros::Duration(1.0 / mFusedPcPubFreq), &ZEDWrapperNodelet::callback_pubFusedPointCloud, this); + mMappingRunning = true; - NODELET_INFO_STREAM(" * Resolution: " << params.resolution_meter << " m"); - NODELET_INFO_STREAM(" * Max Mapping Range: " << params.range_meter << " m"); - NODELET_INFO_STREAM(" * Map point cloud publishing rate: " << mFusedPcPubFreq << " Hz"); + mFusedPcTimer = + mNhNs.createTimer(ros::Duration(1.0 / mFusedPcPubFreq), &ZEDWrapperNodelet::callback_pubFusedPointCloud, this); - return true; - } else { - mMappingRunning = false; - mFusedPcTimer.stop(); + NODELET_INFO_STREAM(" * Resolution: " << params.resolution_meter << " m"); + NODELET_INFO_STREAM(" * Max Mapping Range: " << params.range_meter << " m"); + NODELET_INFO_STREAM(" * Map point cloud publishing rate: " << mFusedPcPubFreq << " Hz"); + + return true; + } + else + { + mMappingRunning = false; + mFusedPcTimer.stop(); - NODELET_WARN("Mapping not activated: %s", sl::toString(err).c_str()); + NODELET_WARN("Mapping not activated: %s", sl::toString(err).c_str()); - return false; - } + return false; + } } void ZEDWrapperNodelet::stop_3d_mapping() { - mFusedPcTimer.stop(); - mMappingRunning = false; - mMappingEnabled = false; - mZed.disableSpatialMapping(); + mFusedPcTimer.stop(); + mMappingRunning = false; + mMappingEnabled = false; + mZed.disableSpatialMapping(); - NODELET_INFO("*** Spatial Mapping stopped ***"); + NODELET_INFO("*** Spatial Mapping stopped ***"); } bool ZEDWrapperNodelet::start_obj_detect() { - if (mZedRealCamModel == sl::MODEL::ZED) { - NODELET_ERROR_STREAM("Object detection not started. OD is not available for ZED camera model"); - return false; - } + if (mZedRealCamModel == sl::MODEL::ZED) + { + NODELET_ERROR_STREAM("Object detection not started. OD is not available for ZED camera model"); + return false; + } - if (!mObjDetEnabled) { - return false; - } + if (!mObjDetEnabled) + { + return false; + } - if (!mCamera2BaseTransfValid || !mSensor2CameraTransfValid || !mSensor2BaseTransfValid) { - NODELET_DEBUG("Tracking transforms not yet ready, OD starting postponed"); - return false; - } + if (!mCamera2BaseTransfValid || !mSensor2CameraTransfValid || !mSensor2BaseTransfValid) + { + NODELET_DEBUG("Tracking transforms not yet ready, OD starting postponed"); + return false; + } - NODELET_INFO_STREAM("*** Starting Object Detection ***"); + NODELET_INFO_STREAM("*** Starting Object Detection ***"); - sl::ObjectDetectionParameters od_p; - od_p.allow_reduced_precision_inference = false; - od_p.enable_segmentation = false; - od_p.enable_tracking = mObjDetTracking; - od_p.image_sync = false; // Asynchronous object detection - od_p.detection_model = mObjDetModel; - od_p.max_range = mObjDetMaxRange; - od_p.instance_module_id = 0; + sl::ObjectDetectionParameters od_p; + od_p.allow_reduced_precision_inference = false; + od_p.enable_segmentation = false; + od_p.enable_tracking = mObjDetTracking; + od_p.image_sync = false; // Asynchronous object detection + od_p.detection_model = mObjDetModel; + od_p.max_range = mObjDetMaxRange; + od_p.instance_module_id = 0; - sl::ERROR_CODE objDetError = mZed.enableObjectDetection(od_p); + sl::ERROR_CODE objDetError = mZed.enableObjectDetection(od_p); - if (objDetError != sl::ERROR_CODE::SUCCESS) { - NODELET_ERROR_STREAM("Object detection error: " << sl::toString(objDetError)); + if (objDetError != sl::ERROR_CODE::SUCCESS) + { + NODELET_ERROR_STREAM("Object detection error: " << sl::toString(objDetError)); - mObjDetRunning = false; - return false; - } + mObjDetRunning = false; + return false; + } - if (mPubObjDet.getTopic().empty()) { - std::string object_det_topic_root = "obj_det"; - std::string object_det_topic = object_det_topic_root + "/objects"; + if (mPubObjDet.getTopic().empty()) + { + std::string object_det_topic_root = "obj_det"; + std::string object_det_topic = object_det_topic_root + "/objects"; - mPubObjDet = mNhNs.advertise(object_det_topic, 1); - NODELET_INFO_STREAM(" * Advertised on topic " << mPubObjDet.getTopic()); - } + mPubObjDet = mNhNs.advertise(object_det_topic, 1); + NODELET_INFO_STREAM(" * Advertised on topic " << mPubObjDet.getTopic()); + } - mObjDetFilter.clear(); + mObjDetFilter.clear(); - if (mObjDetModel == sl::OBJECT_DETECTION_MODEL::MULTI_CLASS_BOX_ACCURATE || - mObjDetModel == sl::OBJECT_DETECTION_MODEL::MULTI_CLASS_BOX_MEDIUM || - mObjDetModel == sl::OBJECT_DETECTION_MODEL::MULTI_CLASS_BOX_FAST) { - if (mObjDetPeopleEnable) { - mObjDetFilter.push_back(sl::OBJECT_CLASS::PERSON); - } - if (mObjDetVehiclesEnable) { - mObjDetFilter.push_back(sl::OBJECT_CLASS::VEHICLE); - } - if (mObjDetBagsEnable) { - mObjDetFilter.push_back(sl::OBJECT_CLASS::BAG); - } - if (mObjDetAnimalsEnable) { - mObjDetFilter.push_back(sl::OBJECT_CLASS::ANIMAL); - } - if (mObjDetElectronicsEnable) { - mObjDetFilter.push_back(sl::OBJECT_CLASS::ELECTRONICS); - } - if (mObjDetFruitsEnable) { - mObjDetFilter.push_back(sl::OBJECT_CLASS::FRUIT_VEGETABLE); - } - if (mObjDetSportsEnable) { - mObjDetFilter.push_back(sl::OBJECT_CLASS::SPORT); - } + if (mObjDetModel == sl::OBJECT_DETECTION_MODEL::MULTI_CLASS_BOX_ACCURATE || + mObjDetModel == sl::OBJECT_DETECTION_MODEL::MULTI_CLASS_BOX_MEDIUM || + mObjDetModel == sl::OBJECT_DETECTION_MODEL::MULTI_CLASS_BOX_FAST) + { + if (mObjDetPeopleEnable) + { + mObjDetFilter.push_back(sl::OBJECT_CLASS::PERSON); + } + if (mObjDetVehiclesEnable) + { + mObjDetFilter.push_back(sl::OBJECT_CLASS::VEHICLE); + } + if (mObjDetBagsEnable) + { + mObjDetFilter.push_back(sl::OBJECT_CLASS::BAG); + } + if (mObjDetAnimalsEnable) + { + mObjDetFilter.push_back(sl::OBJECT_CLASS::ANIMAL); + } + if (mObjDetElectronicsEnable) + { + mObjDetFilter.push_back(sl::OBJECT_CLASS::ELECTRONICS); + } + if (mObjDetFruitsEnable) + { + mObjDetFilter.push_back(sl::OBJECT_CLASS::FRUIT_VEGETABLE); + } + if (mObjDetSportsEnable) + { + mObjDetFilter.push_back(sl::OBJECT_CLASS::SPORT); } + } - mObjDetRunning = true; - return false; + mObjDetRunning = true; + return false; } void ZEDWrapperNodelet::stop_obj_detect() { - if (mObjDetRunning) { - NODELET_INFO_STREAM("*** Stopping Object Detection ***"); - mObjDetRunning = false; - mObjDetEnabled = false; - mZed.disableObjectDetection(); - } + if (mObjDetRunning) + { + NODELET_INFO_STREAM("*** Stopping Object Detection ***"); + mObjDetRunning = false; + mObjDetEnabled = false; + mZed.disableObjectDetection(); + } } void ZEDWrapperNodelet::start_pos_tracking() { - NODELET_INFO_STREAM("*** Starting Positional Tracking ***"); + NODELET_INFO_STREAM("*** Starting Positional Tracking ***"); - NODELET_INFO(" * Waiting for valid static transformations..."); + NODELET_INFO(" * Waiting for valid static transformations..."); - mPosTrackingReady = false; // Useful to not publish wrong TF with IMU frame broadcasting - bool transformOk = false; - double elapsed = 0.0; + mPosTrackingReady = false; // Useful to not publish wrong TF with IMU frame broadcasting + bool transformOk = false; + double elapsed = 0.0; - auto start = std::chrono::high_resolution_clock::now(); + auto start = std::chrono::high_resolution_clock::now(); - do { - transformOk = set_pose(mInitialBasePose[0], mInitialBasePose[1], mInitialBasePose[2], mInitialBasePose[3], - mInitialBasePose[4], mInitialBasePose[5]); + do + { + transformOk = set_pose(mInitialBasePose[0], mInitialBasePose[1], mInitialBasePose[2], mInitialBasePose[3], + mInitialBasePose[4], mInitialBasePose[5]); - elapsed = std::chrono::duration_cast(std::chrono::high_resolution_clock::now() - start) - .count(); + elapsed = std::chrono::duration_cast(std::chrono::high_resolution_clock::now() - start) + .count(); - std::this_thread::sleep_for(std::chrono::milliseconds(100)); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); - if (elapsed > 10000) { - NODELET_WARN(" !!! Failed to get static transforms. Is the 'ROBOT STATE PUBLISHER' node correctly working? "); - break; - } + if (elapsed > 10000) + { + NODELET_WARN(" !!! Failed to get static transforms. Is the 'ROBOT STATE PUBLISHER' node correctly working? "); + break; + } - } while (transformOk == false); + } while (transformOk == false); - if (transformOk) { - NODELET_DEBUG("Time required to get valid static transforms: %g sec", elapsed / 1000.); - } + if (transformOk) + { + NODELET_DEBUG("Time required to get valid static transforms: %g sec", elapsed / 1000.); + } - NODELET_INFO("Initial ZED left camera pose (ZED pos. tracking): "); - NODELET_INFO(" * T: [%g,%g,%g]", mInitialPoseSl.getTranslation().x, mInitialPoseSl.getTranslation().y, - mInitialPoseSl.getTranslation().z); - NODELET_INFO(" * Q: [%g,%g,%g,%g]", mInitialPoseSl.getOrientation().ox, mInitialPoseSl.getOrientation().oy, - mInitialPoseSl.getOrientation().oz, mInitialPoseSl.getOrientation().ow); + NODELET_INFO("Initial ZED left camera pose (ZED pos. tracking): "); + NODELET_INFO(" * T: [%g,%g,%g]", mInitialPoseSl.getTranslation().x, mInitialPoseSl.getTranslation().y, + mInitialPoseSl.getTranslation().z); + NODELET_INFO(" * Q: [%g,%g,%g,%g]", mInitialPoseSl.getOrientation().ox, mInitialPoseSl.getOrientation().oy, + mInitialPoseSl.getOrientation().oz, mInitialPoseSl.getOrientation().ow); - // Positional Tracking parameters - sl::PositionalTrackingParameters posTrackParams; + // Positional Tracking parameters + sl::PositionalTrackingParameters posTrackParams; - posTrackParams.initial_world_transform = mInitialPoseSl; - posTrackParams.enable_area_memory = mAreaMemory; + posTrackParams.initial_world_transform = mInitialPoseSl; + posTrackParams.enable_area_memory = mAreaMemory; - mPoseSmoothing = false; // Always false. Pose Smoothing is to be enabled only for VR/AR applications - posTrackParams.enable_pose_smoothing = mPoseSmoothing; + mPoseSmoothing = false; // Always false. Pose Smoothing is to be enabled only for VR/AR applications + posTrackParams.enable_pose_smoothing = mPoseSmoothing; - posTrackParams.set_floor_as_origin = mFloorAlignment; + posTrackParams.set_floor_as_origin = mFloorAlignment; - if (mAreaMemDbPath != "" && !sl_tools::file_exist(mAreaMemDbPath)) { - posTrackParams.area_file_path = ""; - NODELET_WARN_STREAM("area_memory_db_path [" << mAreaMemDbPath << "] doesn't exist or is unreachable. "); - if (mSaveAreaMapOnClosing) { - NODELET_INFO_STREAM("The file will be automatically created when closing the node or calling the " - "'save_area_map' service if a valid Area Memory is available."); - } - } else { - posTrackParams.area_file_path = mAreaMemDbPath.c_str(); + if (mAreaMemDbPath != "" && !sl_tools::file_exist(mAreaMemDbPath)) + { + posTrackParams.area_file_path = ""; + NODELET_WARN_STREAM("area_memory_db_path [" << mAreaMemDbPath << "] doesn't exist or is unreachable. "); + if (mSaveAreaMapOnClosing) + { + NODELET_INFO_STREAM( + "The file will be automatically created when closing the node or calling the " + "'save_area_map' service if a valid Area Memory is available."); } + } + else + { + posTrackParams.area_file_path = mAreaMemDbPath.c_str(); + } - posTrackParams.enable_imu_fusion = mImuFusion; + posTrackParams.enable_imu_fusion = mImuFusion; - posTrackParams.set_as_static = false; - posTrackParams.set_gravity_as_origin = mSetGravityAsOrigin; - posTrackParams.mode = mPosTrkMode; + posTrackParams.set_as_static = false; + posTrackParams.set_gravity_as_origin = mSetGravityAsOrigin; + posTrackParams.mode = mPosTrkMode; - sl::ERROR_CODE err = mZed.enablePositionalTracking(posTrackParams); + sl::ERROR_CODE err = mZed.enablePositionalTracking(posTrackParams); - if (err == sl::ERROR_CODE::SUCCESS) { - mPosTrackingActivated = true; - } else { - mPosTrackingActivated = false; + if (err == sl::ERROR_CODE::SUCCESS) + { + mPosTrackingActivated = true; + } + else + { + mPosTrackingActivated = false; - NODELET_WARN("Tracking not activated: %s", sl::toString(err).c_str()); - } + NODELET_WARN("Tracking not activated: %s", sl::toString(err).c_str()); + } } bool ZEDWrapperNodelet::on_save_area_memory(zed_interfaces::save_area_memory::Request& req, - zed_interfaces::save_area_memory::Response& res) + zed_interfaces::save_area_memory::Response& res) { - std::string file_path = sl_tools::resolveFilePath(req.area_memory_filename); + std::string file_path = sl_tools::resolveFilePath(req.area_memory_filename); - bool ret = saveAreaMap(file_path, &res.info); + bool ret = saveAreaMap(file_path, &res.info); - return ret; + return ret; } bool ZEDWrapperNodelet::saveAreaMap(std::string file_path, std::string* out_msg) { - std::ostringstream os; - - bool node_running = mNhNs.ok(); - if (!mZed.isOpened()) { - os << "Cannot save Area Memory. The camera is closed."; + std::ostringstream os; - if (node_running) - NODELET_WARN_STREAM(os.str().c_str()); - else - std::cerr << os.str() << std::endl; + bool node_running = mNhNs.ok(); + if (!mZed.isOpened()) + { + os << "Cannot save Area Memory. The camera is closed."; - if (out_msg) - *out_msg = os.str(); + if (node_running) + NODELET_WARN_STREAM(os.str().c_str()); + else + std::cerr << os.str() << std::endl; - return false; - } + if (out_msg) + *out_msg = os.str(); - if (mPosTrackingActivated && mAreaMemory) { - sl::ERROR_CODE err = mZed.saveAreaMap(sl::String(file_path.c_str())); - if (err != sl::ERROR_CODE::SUCCESS) { - os << "Error saving positional tracking area memory: " << sl::toString(err).c_str(); + return false; + } - if (node_running) - NODELET_WARN_STREAM(os.str().c_str()); - else - std::cerr << os.str() << std::endl; + if (mPosTrackingActivated && mAreaMemory) + { + sl::ERROR_CODE err = mZed.saveAreaMap(sl::String(file_path.c_str())); + if (err != sl::ERROR_CODE::SUCCESS) + { + os << "Error saving positional tracking area memory: " << sl::toString(err).c_str(); - if (out_msg) - *out_msg = os.str(); + if (node_running) + NODELET_WARN_STREAM(os.str().c_str()); + else + std::cerr << os.str() << std::endl; - return false; - } + if (out_msg) + *out_msg = os.str(); - if (node_running) - NODELET_INFO_STREAM("Saving Area Memory file: " << file_path); - else - std::cerr << "Saving Area Memory file: " << file_path << " "; - - sl::AREA_EXPORTING_STATE state; - do { - std::this_thread::sleep_for(std::chrono::milliseconds(500)); - state = mZed.getAreaExportState(); - if (node_running) - NODELET_INFO_STREAM("."); - else - std::cerr << "."; - } while (state == sl::AREA_EXPORTING_STATE::RUNNING); - if (!node_running) - std::cerr << std::endl; - - if (state == sl::AREA_EXPORTING_STATE::SUCCESS) { - os << "Area Memory file saved correctly."; - - if (node_running) - NODELET_INFO_STREAM(os.str().c_str()); - else - std::cerr << os.str() << std::endl; - - if (out_msg) - *out_msg = os.str(); - return true; - } + return false; + } - os << "Error saving Area Memory file: " << sl::toString(state).c_str(); + if (node_running) + NODELET_INFO_STREAM("Saving Area Memory file: " << file_path); + else + std::cerr << "Saving Area Memory file: " << file_path << " "; - if (node_running) - NODELET_WARN_STREAM(os.str().c_str()); - else - std::cerr << os.str() << std::endl; + sl::AREA_EXPORTING_STATE state; + do + { + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + state = mZed.getAreaExportState(); + if (node_running) + NODELET_INFO_STREAM("."); + else + std::cerr << "."; + } while (state == sl::AREA_EXPORTING_STATE::RUNNING); + if (!node_running) + std::cerr << std::endl; + + if (state == sl::AREA_EXPORTING_STATE::SUCCESS) + { + os << "Area Memory file saved correctly."; - if (out_msg) - *out_msg = os.str(); + if (node_running) + NODELET_INFO_STREAM(os.str().c_str()); + else + std::cerr << os.str() << std::endl; - return false; + if (out_msg) + *out_msg = os.str(); + return true; } + + os << "Error saving Area Memory file: " << sl::toString(state).c_str(); + + if (node_running) + NODELET_WARN_STREAM(os.str().c_str()); + else + std::cerr << os.str() << std::endl; + + if (out_msg) + *out_msg = os.str(); + return false; + } + return false; } void ZEDWrapperNodelet::publishOdom(tf2::Transform odom2baseTransf, sl::Pose& slPose, ros::Time t) { - nav_msgs::OdometryPtr odomMsg = boost::make_shared(); - - odomMsg->header.stamp = t; - odomMsg->header.frame_id = mOdometryFrameId; // frame - odomMsg->child_frame_id = mBaseFrameId; // camera_frame - // conversion from Tranform to message - geometry_msgs::Transform base2odom = tf2::toMsg(odom2baseTransf); - // Add all value in odometry message - odomMsg->pose.pose.position.x = base2odom.translation.x; - odomMsg->pose.pose.position.y = base2odom.translation.y; - odomMsg->pose.pose.position.z = base2odom.translation.z; - odomMsg->pose.pose.orientation.x = base2odom.rotation.x; - odomMsg->pose.pose.orientation.y = base2odom.rotation.y; - odomMsg->pose.pose.orientation.z = base2odom.rotation.z; - odomMsg->pose.pose.orientation.w = base2odom.rotation.w; - - // Odometry pose covariance - - for (size_t i = 0; i < odomMsg->pose.covariance.size(); i++) { - odomMsg->pose.covariance[i] = static_cast(slPose.pose_covariance[i]); - - if (mTwoDMode) { - if (i == 14 || i == 21 || i == 28) { - odomMsg->pose.covariance[i] = 1e-9; // Very low covariance if 2D mode - } else if ((i >= 2 && i <= 4) || (i >= 8 && i <= 10) || (i >= 12 && i <= 13) || (i >= 15 && i <= 20) || (i >= 22 && i <= 27) || (i == 29) || (i >= 32 && i <= 34)) { - odomMsg->pose.covariance[i] = 0.0; - } - } + nav_msgs::OdometryPtr odomMsg = boost::make_shared(); + + odomMsg->header.stamp = t; + odomMsg->header.frame_id = mOdometryFrameId; // frame + odomMsg->child_frame_id = mBaseFrameId; // camera_frame + // conversion from Tranform to message + geometry_msgs::Transform base2odom = tf2::toMsg(odom2baseTransf); + // Add all value in odometry message + odomMsg->pose.pose.position.x = base2odom.translation.x; + odomMsg->pose.pose.position.y = base2odom.translation.y; + odomMsg->pose.pose.position.z = base2odom.translation.z; + odomMsg->pose.pose.orientation.x = base2odom.rotation.x; + odomMsg->pose.pose.orientation.y = base2odom.rotation.y; + odomMsg->pose.pose.orientation.z = base2odom.rotation.z; + odomMsg->pose.pose.orientation.w = base2odom.rotation.w; + + // Odometry pose covariance + + for (size_t i = 0; i < odomMsg->pose.covariance.size(); i++) + { + odomMsg->pose.covariance[i] = static_cast(slPose.pose_covariance[i]); + + if (mTwoDMode) + { + if (i == 14 || i == 21 || i == 28) + { + odomMsg->pose.covariance[i] = 1e-9; // Very low covariance if 2D mode + } + else if ((i >= 2 && i <= 4) || (i >= 8 && i <= 10) || (i >= 12 && i <= 13) || (i >= 15 && i <= 20) || + (i >= 22 && i <= 27) || (i == 29) || (i >= 32 && i <= 34)) + { + odomMsg->pose.covariance[i] = 0.0; + } } + } - // Publish odometry message - mPubOdom.publish(odomMsg); + // Publish odometry message + mPubOdom.publish(odomMsg); } void ZEDWrapperNodelet::publishPose(ros::Time t) { - tf2::Transform base_pose; - base_pose.setIdentity(); + tf2::Transform base_pose; + base_pose.setIdentity(); - if (mPublishMapTf) { - base_pose = mMap2BaseTransf; - } else if (mPublishTf) { - base_pose = mOdom2BaseTransf; - } + if (mPublishMapTf) + { + base_pose = mMap2BaseTransf; + } + else if (mPublishTf) + { + base_pose = mOdom2BaseTransf; + } - std_msgs::Header header; - header.stamp = t; - header.frame_id = mMapFrameId; - geometry_msgs::Pose pose; + std_msgs::Header header; + header.stamp = t; + header.frame_id = mMapFrameId; + geometry_msgs::Pose pose; - // conversion from Tranform to message - geometry_msgs::Transform base2frame = tf2::toMsg(base_pose); + // conversion from Tranform to message + geometry_msgs::Transform base2frame = tf2::toMsg(base_pose); - // Add all value in Pose message - pose.position.x = base2frame.translation.x; - pose.position.y = base2frame.translation.y; - pose.position.z = base2frame.translation.z; - pose.orientation.x = base2frame.rotation.x; - pose.orientation.y = base2frame.rotation.y; - pose.orientation.z = base2frame.rotation.z; - pose.orientation.w = base2frame.rotation.w; + // Add all value in Pose message + pose.position.x = base2frame.translation.x; + pose.position.y = base2frame.translation.y; + pose.position.z = base2frame.translation.z; + pose.orientation.x = base2frame.rotation.x; + pose.orientation.y = base2frame.rotation.y; + pose.orientation.z = base2frame.rotation.z; + pose.orientation.w = base2frame.rotation.w; - if (mPubPose.getNumSubscribers() > 0) { - geometry_msgs::PoseStamped poseNoCov; + if (mPubPose.getNumSubscribers() > 0) + { + geometry_msgs::PoseStamped poseNoCov; - poseNoCov.header = header; - poseNoCov.pose = pose; + poseNoCov.header = header; + poseNoCov.pose = pose; - // Publish pose stamped message - mPubPose.publish(poseNoCov); - } + // Publish pose stamped message + mPubPose.publish(poseNoCov); + } - if (mPubPoseCov.getNumSubscribers() > 0) { - geometry_msgs::PoseWithCovarianceStampedPtr poseCovMsg = boost::make_shared(); + if (mPubPoseCov.getNumSubscribers() > 0) + { + geometry_msgs::PoseWithCovarianceStampedPtr poseCovMsg = + boost::make_shared(); - poseCovMsg->header = header; - poseCovMsg->pose.pose = pose; + poseCovMsg->header = header; + poseCovMsg->pose.pose = pose; - // Odometry pose covariance if available - for (size_t i = 0; i < poseCovMsg->pose.covariance.size(); i++) { - poseCovMsg->pose.covariance[i] = static_cast(mLastZedPose.pose_covariance[i]); + // Odometry pose covariance if available + for (size_t i = 0; i < poseCovMsg->pose.covariance.size(); i++) + { + poseCovMsg->pose.covariance[i] = static_cast(mLastZedPose.pose_covariance[i]); - if (mTwoDMode) { - if (i == 14 || i == 21 || i == 28) { - poseCovMsg->pose.covariance[i] = 1e-9; // Very low covariance if 2D mode - } else if ((i >= 2 && i <= 4) || (i >= 8 && i <= 10) || (i >= 12 && i <= 13) || (i >= 15 && i <= 16) || (i >= 18 && i <= 20) || (i == 22) || (i >= 24 && i <= 27)) { - poseCovMsg->pose.covariance[i] = 0.0; - } - } + if (mTwoDMode) + { + if (i == 14 || i == 21 || i == 28) + { + poseCovMsg->pose.covariance[i] = 1e-9; // Very low covariance if 2D mode } - - // Publish pose with covariance stamped message - mPubPoseCov.publish(poseCovMsg); + else if ((i >= 2 && i <= 4) || (i >= 8 && i <= 10) || (i >= 12 && i <= 13) || (i >= 15 && i <= 16) || + (i >= 18 && i <= 20) || (i == 22) || (i >= 24 && i <= 27)) + { + poseCovMsg->pose.covariance[i] = 0.0; + } + } } + + // Publish pose with covariance stamped message + mPubPoseCov.publish(poseCovMsg); + } } void ZEDWrapperNodelet::publishStaticImuFrame() { - // Publish IMU TF as static TF - if (!mPublishImuTf) { - return; - } + // Publish IMU TF as static TF + if (!mPublishImuTf) + { + return; + } - if (mStaticImuFramePublished) { - return; - } + if (mStaticImuFramePublished) + { + return; + } - mStaticImuTransformStamped.header.stamp = ros::Time::now(); - mStaticImuTransformStamped.header.frame_id = mLeftCamFrameId; - mStaticImuTransformStamped.child_frame_id = mImuFrameId; - sl::Translation sl_tr = mSlCamImuTransf.getTranslation(); - mStaticImuTransformStamped.transform.translation.x = sl_tr.x; - mStaticImuTransformStamped.transform.translation.y = sl_tr.y; - mStaticImuTransformStamped.transform.translation.z = sl_tr.z; - sl::Orientation sl_or = mSlCamImuTransf.getOrientation(); - mStaticImuTransformStamped.transform.rotation.x = sl_or.ox; - mStaticImuTransformStamped.transform.rotation.y = sl_or.oy; - mStaticImuTransformStamped.transform.rotation.z = sl_or.oz; - mStaticImuTransformStamped.transform.rotation.w = sl_or.ow; - - // Publish transformation - mStaticTransformImuBroadcaster.sendTransform(mStaticImuTransformStamped); - - NODELET_INFO_STREAM("Published static transform '" << mImuFrameId << "' -> '" << mLeftCamFrameId << "'"); - - mStaticImuFramePublished = true; + mStaticImuTransformStamped.header.stamp = ros::Time::now(); + mStaticImuTransformStamped.header.frame_id = mLeftCamFrameId; + mStaticImuTransformStamped.child_frame_id = mImuFrameId; + sl::Translation sl_tr = mSlCamImuTransf.getTranslation(); + mStaticImuTransformStamped.transform.translation.x = sl_tr.x; + mStaticImuTransformStamped.transform.translation.y = sl_tr.y; + mStaticImuTransformStamped.transform.translation.z = sl_tr.z; + sl::Orientation sl_or = mSlCamImuTransf.getOrientation(); + mStaticImuTransformStamped.transform.rotation.x = sl_or.ox; + mStaticImuTransformStamped.transform.rotation.y = sl_or.oy; + mStaticImuTransformStamped.transform.rotation.z = sl_or.oz; + mStaticImuTransformStamped.transform.rotation.w = sl_or.ow; + + // Publish transformation + mStaticTransformImuBroadcaster.sendTransform(mStaticImuTransformStamped); + + NODELET_INFO_STREAM("Published static transform '" << mImuFrameId << "' -> '" << mLeftCamFrameId << "'"); + + mStaticImuFramePublished = true; } void ZEDWrapperNodelet::publishOdomFrame(tf2::Transform odomTransf, ros::Time t) { - // ----> Avoid duplicated TF publishing - static ros::Time last_stamp; + // ----> Avoid duplicated TF publishing + static ros::Time last_stamp; - if (t == last_stamp) { - return; - } - last_stamp = t; - // <---- Avoid duplicated TF publishing + if (t == last_stamp) + { + return; + } + last_stamp = t; + // <---- Avoid duplicated TF publishing - if (!mSensor2BaseTransfValid) { - getSens2BaseTransform(); - } + if (!mSensor2BaseTransfValid) + { + getSens2BaseTransform(); + } - if (!mSensor2CameraTransfValid) { - getSens2CameraTransform(); - } + if (!mSensor2CameraTransfValid) + { + getSens2CameraTransform(); + } - if (!mCamera2BaseTransfValid) { - getCamera2BaseTransform(); - } + if (!mCamera2BaseTransfValid) + { + getCamera2BaseTransform(); + } - geometry_msgs::TransformStamped transformStamped; - transformStamped.header.stamp = t; - transformStamped.header.frame_id = mOdometryFrameId; - transformStamped.child_frame_id = mBaseFrameId; - // conversion from Tranform to message - transformStamped.transform = tf2::toMsg(odomTransf); - // Publish transformation - mTransformOdomBroadcaster.sendTransform(transformStamped); + geometry_msgs::TransformStamped transformStamped; + transformStamped.header.stamp = t; + transformStamped.header.frame_id = mOdometryFrameId; + transformStamped.child_frame_id = mBaseFrameId; + // conversion from Tranform to message + transformStamped.transform = tf2::toMsg(odomTransf); + // Publish transformation + mTransformOdomBroadcaster.sendTransform(transformStamped); - // NODELET_INFO_STREAM( "Published ODOM TF with TS: " << t ); + // NODELET_INFO_STREAM( "Published ODOM TF with TS: " << t ); } void ZEDWrapperNodelet::publishPoseFrame(tf2::Transform baseTransform, ros::Time t) { - // ----> Avoid duplicated TF publishing - static ros::Time last_stamp; + // ----> Avoid duplicated TF publishing + static ros::Time last_stamp; - if (t == last_stamp) { - return; - } - last_stamp = t; - // <---- Avoid duplicated TF publishing + if (t == last_stamp) + { + return; + } + last_stamp = t; + // <---- Avoid duplicated TF publishing - if (!mSensor2BaseTransfValid) { - getSens2BaseTransform(); - } + if (!mSensor2BaseTransfValid) + { + getSens2BaseTransform(); + } - if (!mSensor2CameraTransfValid) { - getSens2CameraTransform(); - } + if (!mSensor2CameraTransfValid) + { + getSens2CameraTransform(); + } - if (!mCamera2BaseTransfValid) { - getCamera2BaseTransform(); - } + if (!mCamera2BaseTransfValid) + { + getCamera2BaseTransform(); + } - geometry_msgs::TransformStamped transformStamped; - transformStamped.header.stamp = t; - transformStamped.header.frame_id = mMapFrameId; - transformStamped.child_frame_id = mOdometryFrameId; - // conversion from Tranform to message - transformStamped.transform = tf2::toMsg(baseTransform); - // Publish transformation - mTransformPoseBroadcaster.sendTransform(transformStamped); + geometry_msgs::TransformStamped transformStamped; + transformStamped.header.stamp = t; + transformStamped.header.frame_id = mMapFrameId; + transformStamped.child_frame_id = mOdometryFrameId; + // conversion from Tranform to message + transformStamped.transform = tf2::toMsg(baseTransform); + // Publish transformation + mTransformPoseBroadcaster.sendTransform(transformStamped); - // NODELET_INFO_STREAM( "Published POSE TF with TS: " << t ); + // NODELET_INFO_STREAM( "Published POSE TF with TS: " << t ); } void ZEDWrapperNodelet::publishImage(sensor_msgs::ImagePtr imgMsgPtr, sl::Mat img, - image_transport::CameraPublisher& pubImg, sensor_msgs::CameraInfoPtr camInfoMsg, - std::string imgFrameId, ros::Time t) + image_transport::CameraPublisher& pubImg, sensor_msgs::CameraInfoPtr camInfoMsg, + std::string imgFrameId, ros::Time t) { - camInfoMsg->header.stamp = t; - sl_tools::imageToROSmsg(imgMsgPtr, img, imgFrameId, t); - pubImg.publish(imgMsgPtr, camInfoMsg); + camInfoMsg->header.stamp = t; + sl_tools::imageToROSmsg(imgMsgPtr, img, imgFrameId, t); + pubImg.publish(imgMsgPtr, camInfoMsg); } void ZEDWrapperNodelet::publishDepth(sensor_msgs::ImagePtr imgMsgPtr, sl::Mat depth, ros::Time t) { - mDepthCamInfoMsg->header.stamp = t; - - // NODELET_DEBUG_STREAM("mOpenniDepthMode: " << mOpenniDepthMode); + mDepthCamInfoMsg->header.stamp = t; - if (!mOpenniDepthMode) { - // NODELET_INFO("Using float32"); - sl_tools::imageToROSmsg(imgMsgPtr, depth, mDepthOptFrameId, t); - mPubDepth.publish(imgMsgPtr, mDepthCamInfoMsg); - - return; - } + // NODELET_DEBUG_STREAM("mOpenniDepthMode: " << mOpenniDepthMode); - // NODELET_INFO("Using depth16"); + if (!mOpenniDepthMode) + { + // NODELET_INFO("Using float32"); sl_tools::imageToROSmsg(imgMsgPtr, depth, mDepthOptFrameId, t); mPubDepth.publish(imgMsgPtr, mDepthCamInfoMsg); + + return; + } + + // NODELET_INFO("Using depth16"); + sl_tools::imageToROSmsg(imgMsgPtr, depth, mDepthOptFrameId, t); + mPubDepth.publish(imgMsgPtr, mDepthCamInfoMsg); } void ZEDWrapperNodelet::publishDisparity(sl::Mat disparity, ros::Time t) { - sl::CameraInformation zedParam = mZed.getCameraInformation(mMatResol); + sl::CameraInformation zedParam = mZed.getCameraInformation(mMatResol); - sensor_msgs::ImagePtr disparityImgMsg = boost::make_shared(); - stereo_msgs::DisparityImagePtr disparityMsg = boost::make_shared(); + sensor_msgs::ImagePtr disparityImgMsg = boost::make_shared(); + stereo_msgs::DisparityImagePtr disparityMsg = boost::make_shared(); - sl_tools::imageToROSmsg(disparityImgMsg, disparity, mDisparityFrameId, t); + sl_tools::imageToROSmsg(disparityImgMsg, disparity, mDisparityFrameId, t); - disparityMsg->image = *disparityImgMsg; - disparityMsg->header = disparityMsg->image.header; + disparityMsg->image = *disparityImgMsg; + disparityMsg->header = disparityMsg->image.header; - disparityMsg->f = zedParam.camera_configuration.calibration_parameters.left_cam.fx; - disparityMsg->T = zedParam.camera_configuration.calibration_parameters.getCameraBaseline(); + disparityMsg->f = zedParam.camera_configuration.calibration_parameters.left_cam.fx; + disparityMsg->T = zedParam.camera_configuration.calibration_parameters.getCameraBaseline(); - if (disparityMsg->T > 0) { - disparityMsg->T *= -1.0f; - } + if (disparityMsg->T > 0) + { + disparityMsg->T *= -1.0f; + } - disparityMsg->min_disparity = disparityMsg->f * disparityMsg->T / mZed.getInitParameters().depth_minimum_distance; - disparityMsg->max_disparity = disparityMsg->f * disparityMsg->T / mZed.getInitParameters().depth_maximum_distance; + disparityMsg->min_disparity = disparityMsg->f * disparityMsg->T / mZed.getInitParameters().depth_minimum_distance; + disparityMsg->max_disparity = disparityMsg->f * disparityMsg->T / mZed.getInitParameters().depth_maximum_distance; - mPubDisparity.publish(disparityMsg); + mPubDisparity.publish(disparityMsg); } void ZEDWrapperNodelet::pointcloud_thread_func() { - std::unique_lock lock(mPcMutex); - - while (!mStopNode) { - while (!mPcDataReady) { // loop to avoid spurious wakeups - if (mPcDataReadyCondVar.wait_for(lock, std::chrono::milliseconds(500)) == std::cv_status::timeout) { - // Check thread stopping - if (mStopNode) { - return; - } else { - continue; - } - } - } + std::unique_lock lock(mPcMutex); - // ----> Check publishing frequency - double pc_period_msec = 1000.0 / mPointCloudFreq; - - static std::chrono::steady_clock::time_point last_time = std::chrono::steady_clock::now(); - std::chrono::steady_clock::time_point now = std::chrono::steady_clock::now(); + while (!mStopNode) + { + while (!mPcDataReady) + { // loop to avoid spurious wakeups + if (mPcDataReadyCondVar.wait_for(lock, std::chrono::milliseconds(500)) == std::cv_status::timeout) + { + // Check thread stopping + if (mStopNode) + { + return; + } + else + { + continue; + } + } + } - double elapsed_msec = std::chrono::duration_cast(now - last_time).count(); + // ----> Check publishing frequency + double pc_period_msec = 1000.0 / mPointCloudFreq; - if (elapsed_msec < pc_period_msec) { - std::this_thread::sleep_for(std::chrono::milliseconds(static_cast(pc_period_msec - elapsed_msec))); - } - // <---- Check publishing frequency + static std::chrono::steady_clock::time_point last_time = std::chrono::steady_clock::now(); + std::chrono::steady_clock::time_point now = std::chrono::steady_clock::now(); - last_time = std::chrono::steady_clock::now(); - publishPointCloud(); + double elapsed_msec = std::chrono::duration_cast(now - last_time).count(); - mPcDataReady = false; + if (elapsed_msec < pc_period_msec) + { + std::this_thread::sleep_for(std::chrono::milliseconds(static_cast(pc_period_msec - elapsed_msec))); } + // <---- Check publishing frequency + + last_time = std::chrono::steady_clock::now(); + publishPointCloud(); + + mPcDataReady = false; + } - NODELET_DEBUG("Pointcloud thread finished"); + NODELET_DEBUG("Pointcloud thread finished"); } void ZEDWrapperNodelet::publishPointCloud() { - sensor_msgs::PointCloud2Ptr pointcloudMsg = boost::make_shared(); + sensor_msgs::PointCloud2Ptr pointcloudMsg = boost::make_shared(); - // Publish freq calculation - static std::chrono::steady_clock::time_point last_time = std::chrono::steady_clock::now(); - std::chrono::steady_clock::time_point now = std::chrono::steady_clock::now(); + // Publish freq calculation + static std::chrono::steady_clock::time_point last_time = std::chrono::steady_clock::now(); + std::chrono::steady_clock::time_point now = std::chrono::steady_clock::now(); - double elapsed_usec = std::chrono::duration_cast(now - last_time).count(); - last_time = now; + double elapsed_usec = std::chrono::duration_cast(now - last_time).count(); + last_time = now; - mPcPeriodMean_usec->addValue(elapsed_usec); + mPcPeriodMean_usec->addValue(elapsed_usec); - // Initialize Point Cloud message - // https://github.com/ros/common_msgs/blob/jade-devel/sensor_msgs/include/sensor_msgs/point_cloud2_iterator.h + // Initialize Point Cloud message + // https://github.com/ros/common_msgs/blob/jade-devel/sensor_msgs/include/sensor_msgs/point_cloud2_iterator.h - int ptsCount = mMatResol.width * mMatResol.height; + int ptsCount = mMatResol.width * mMatResol.height; - pointcloudMsg->header.stamp = mPointCloudTime; + pointcloudMsg->header.stamp = mPointCloudTime; - if (pointcloudMsg->width != mMatResol.width || pointcloudMsg->height != mMatResol.height) { - pointcloudMsg->header.frame_id = mPointCloudFrameId; // Set the header values of the ROS message + if (pointcloudMsg->width != mMatResol.width || pointcloudMsg->height != mMatResol.height) + { + pointcloudMsg->header.frame_id = mPointCloudFrameId; // Set the header values of the ROS message - pointcloudMsg->is_bigendian = false; - pointcloudMsg->is_dense = false; + pointcloudMsg->is_bigendian = false; + pointcloudMsg->is_dense = false; - pointcloudMsg->width = mMatResol.width; - pointcloudMsg->height = mMatResol.height; + pointcloudMsg->width = mMatResol.width; + pointcloudMsg->height = mMatResol.height; - sensor_msgs::PointCloud2Modifier modifier(*pointcloudMsg); - modifier.setPointCloud2Fields(4, "x", 1, sensor_msgs::PointField::FLOAT32, "y", 1, sensor_msgs::PointField::FLOAT32, - "z", 1, sensor_msgs::PointField::FLOAT32, "rgb", 1, sensor_msgs::PointField::FLOAT32); - } + sensor_msgs::PointCloud2Modifier modifier(*pointcloudMsg); + modifier.setPointCloud2Fields(4, "x", 1, sensor_msgs::PointField::FLOAT32, "y", 1, sensor_msgs::PointField::FLOAT32, + "z", 1, sensor_msgs::PointField::FLOAT32, "rgb", 1, sensor_msgs::PointField::FLOAT32); + } - // Data copy - sl::Vector4* cpu_cloud = mCloud.getPtr(); - float* ptCloudPtr = (float*)(&pointcloudMsg->data[0]); + // Data copy + sl::Vector4* cpu_cloud = mCloud.getPtr(); + float* ptCloudPtr = (float*)(&pointcloudMsg->data[0]); - // We can do a direct memcpy since data organization is the same - memcpy(ptCloudPtr, (float*)cpu_cloud, 4 * ptsCount * sizeof(float)); + // We can do a direct memcpy since data organization is the same + memcpy(ptCloudPtr, (float*)cpu_cloud, 4 * ptsCount * sizeof(float)); - // Pointcloud publishing - mPubCloud.publish(pointcloudMsg); + // Pointcloud publishing + mPubCloud.publish(pointcloudMsg); } void ZEDWrapperNodelet::callback_pubFusedPointCloud(const ros::TimerEvent& e) { - sensor_msgs::PointCloud2Ptr pointcloudFusedMsg = boost::make_shared(); + sensor_msgs::PointCloud2Ptr pointcloudFusedMsg = boost::make_shared(); - uint32_t fusedCloudSubnumber = mPubFusedCloud.getNumSubscribers(); + uint32_t fusedCloudSubnumber = mPubFusedCloud.getNumSubscribers(); - if (fusedCloudSubnumber == 0) { - return; - } + if (fusedCloudSubnumber == 0) + { + return; + } - std::lock_guard lock(mCloseZedMutex); + std::lock_guard lock(mCloseZedMutex); - if (!mZed.isOpened()) { - return; - } + if (!mZed.isOpened()) + { + return; + } - // pointcloudFusedMsg->header.stamp = t; - mZed.requestSpatialMapAsync(); + // pointcloudFusedMsg->header.stamp = t; + mZed.requestSpatialMapAsync(); - while (mZed.getSpatialMapRequestStatusAsync() == sl::ERROR_CODE::FAILURE) { - // Mesh is still generating - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } + while (mZed.getSpatialMapRequestStatusAsync() == sl::ERROR_CODE::FAILURE) + { + // Mesh is still generating + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } - sl::ERROR_CODE res = mZed.retrieveSpatialMapAsync(mFusedPC); + sl::ERROR_CODE res = mZed.retrieveSpatialMapAsync(mFusedPC); - if (res != sl::ERROR_CODE::SUCCESS) { - NODELET_WARN_STREAM("Fused point cloud not extracted: " << sl::toString(res).c_str()); - return; - } + if (res != sl::ERROR_CODE::SUCCESS) + { + NODELET_WARN_STREAM("Fused point cloud not extracted: " << sl::toString(res).c_str()); + return; + } - size_t ptsCount = mFusedPC.getNumberOfPoints(); - bool resized = false; + size_t ptsCount = mFusedPC.getNumberOfPoints(); + bool resized = false; - if (pointcloudFusedMsg->width != ptsCount || pointcloudFusedMsg->height != 1) { - // Initialize Point Cloud message - // https://github.com/ros/common_msgs/blob/jade-devel/sensor_msgs/include/sensor_msgs/point_cloud2_iterator.h - pointcloudFusedMsg->header.frame_id = mMapFrameId; // Set the header values of the ROS message - pointcloudFusedMsg->is_bigendian = false; - pointcloudFusedMsg->is_dense = false; - pointcloudFusedMsg->width = ptsCount; - pointcloudFusedMsg->height = 1; + if (pointcloudFusedMsg->width != ptsCount || pointcloudFusedMsg->height != 1) + { + // Initialize Point Cloud message + // https://github.com/ros/common_msgs/blob/jade-devel/sensor_msgs/include/sensor_msgs/point_cloud2_iterator.h + pointcloudFusedMsg->header.frame_id = mMapFrameId; // Set the header values of the ROS message + pointcloudFusedMsg->is_bigendian = false; + pointcloudFusedMsg->is_dense = false; + pointcloudFusedMsg->width = ptsCount; + pointcloudFusedMsg->height = 1; - sensor_msgs::PointCloud2Modifier modifier(*pointcloudFusedMsg); - modifier.setPointCloud2Fields(4, "x", 1, sensor_msgs::PointField::FLOAT32, "y", 1, sensor_msgs::PointField::FLOAT32, - "z", 1, sensor_msgs::PointField::FLOAT32, "rgb", 1, sensor_msgs::PointField::FLOAT32); + sensor_msgs::PointCloud2Modifier modifier(*pointcloudFusedMsg); + modifier.setPointCloud2Fields(4, "x", 1, sensor_msgs::PointField::FLOAT32, "y", 1, sensor_msgs::PointField::FLOAT32, + "z", 1, sensor_msgs::PointField::FLOAT32, "rgb", 1, sensor_msgs::PointField::FLOAT32); - resized = true; - } + 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()); + // NODELET_INFO_STREAM("Chunks: " << mFusedPC.chunks.size()); - int index = 0; - float* ptCloudPtr = (float*)(&pointcloudFusedMsg->data[0]); - int updated = 0; + int index = 0; + float* ptCloudPtr = (float*)(&pointcloudFusedMsg->data[0]); + int updated = 0; - for (int c = 0; c < mFusedPC.chunks.size(); c++) { - if (mFusedPC.chunks[c].has_been_updated || resized) { - updated++; + for (int c = 0; c < mFusedPC.chunks.size(); c++) + { + if (mFusedPC.chunks[c].has_been_updated || resized) + { + updated++; - size_t chunkSize = mFusedPC.chunks[c].vertices.size(); + size_t chunkSize = mFusedPC.chunks[c].vertices.size(); - if (chunkSize > 0) { - float* cloud_pts = (float*)(mFusedPC.chunks[c].vertices.data()); + if (chunkSize > 0) + { + float* cloud_pts = (float*)(mFusedPC.chunks[c].vertices.data()); - memcpy(ptCloudPtr, cloud_pts, 4 * chunkSize * sizeof(float)); + memcpy(ptCloudPtr, cloud_pts, 4 * chunkSize * sizeof(float)); - ptCloudPtr += 4 * chunkSize; + ptCloudPtr += 4 * chunkSize; - pointcloudFusedMsg->header.stamp = sl_tools::slTime2Ros(mFusedPC.chunks[c].timestamp); - } - } else { - index += mFusedPC.chunks[c].vertices.size(); - } + 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); + // NODELET_INFO_STREAM("Updated: " << updated); - // double elapsed_usec = std::chrono::duration_cast(end_time - start_time).count(); + // double elapsed_usec = std::chrono::duration_cast(end_time - start_time).count(); - // NODELET_INFO_STREAM("Data copy: " << elapsed_usec << " usec [" << ptsCount << "] - " << (static_cast - // (ptsCount) / elapsed_usec) << " pts/usec"); + // NODELET_INFO_STREAM("Data copy: " << elapsed_usec << " usec [" << ptsCount << "] - " << (static_cast + // (ptsCount) / elapsed_usec) << " pts/usec"); - // Pointcloud publishing - mPubFusedCloud.publish(pointcloudFusedMsg); + // Pointcloud publishing + mPubFusedCloud.publish(pointcloudFusedMsg); } void ZEDWrapperNodelet::publishCamInfo(sensor_msgs::CameraInfoPtr camInfoMsg, ros::Publisher pubCamInfo, ros::Time t) { - static int seq = 0; - camInfoMsg->header.stamp = t; - camInfoMsg->header.seq = seq; - pubCamInfo.publish(camInfoMsg); - seq++; + static int seq = 0; + camInfoMsg->header.stamp = t; + camInfoMsg->header.seq = seq; + pubCamInfo.publish(camInfoMsg); + seq++; } void ZEDWrapperNodelet::fillCamInfo(sl::Camera& zed, sensor_msgs::CameraInfoPtr leftCamInfoMsg, - sensor_msgs::CameraInfoPtr rightCamInfoMsg, std::string leftFrameId, std::string rightFrameId, - bool rawParam /*= false*/) + sensor_msgs::CameraInfoPtr rightCamInfoMsg, std::string leftFrameId, + std::string rightFrameId, bool rawParam /*= false*/) { - sl::CalibrationParameters zedParam; + sl::CalibrationParameters zedParam; - if (rawParam) { - zedParam = zed.getCameraInformation(mMatResol).camera_configuration.calibration_parameters_raw; - } else { - zedParam = zed.getCameraInformation(mMatResol).camera_configuration.calibration_parameters; - } + if (rawParam) + { + zedParam = zed.getCameraInformation(mMatResol).camera_configuration.calibration_parameters_raw; + } + else + { + zedParam = zed.getCameraInformation(mMatResol).camera_configuration.calibration_parameters; + } - float baseline = zedParam.getCameraBaseline(); - leftCamInfoMsg->distortion_model = sensor_msgs::distortion_models::PLUMB_BOB; - rightCamInfoMsg->distortion_model = sensor_msgs::distortion_models::PLUMB_BOB; - leftCamInfoMsg->D.resize(5); - rightCamInfoMsg->D.resize(5); - leftCamInfoMsg->D[0] = zedParam.left_cam.disto[0]; // k1 - leftCamInfoMsg->D[1] = zedParam.left_cam.disto[1]; // k2 - leftCamInfoMsg->D[2] = zedParam.left_cam.disto[4]; // k3 - leftCamInfoMsg->D[3] = zedParam.left_cam.disto[2]; // p1 - leftCamInfoMsg->D[4] = zedParam.left_cam.disto[3]; // p2 - rightCamInfoMsg->D[0] = zedParam.right_cam.disto[0]; // k1 - rightCamInfoMsg->D[1] = zedParam.right_cam.disto[1]; // k2 - rightCamInfoMsg->D[2] = zedParam.right_cam.disto[4]; // k3 - rightCamInfoMsg->D[3] = zedParam.right_cam.disto[2]; // p1 - rightCamInfoMsg->D[4] = zedParam.right_cam.disto[3]; // p2 - leftCamInfoMsg->K.fill(0.0); - rightCamInfoMsg->K.fill(0.0); - leftCamInfoMsg->K[0] = static_cast(zedParam.left_cam.fx); - leftCamInfoMsg->K[2] = static_cast(zedParam.left_cam.cx); - leftCamInfoMsg->K[4] = static_cast(zedParam.left_cam.fy); - leftCamInfoMsg->K[5] = static_cast(zedParam.left_cam.cy); - leftCamInfoMsg->K[8] = 1.0; - rightCamInfoMsg->K[0] = static_cast(zedParam.right_cam.fx); - rightCamInfoMsg->K[2] = static_cast(zedParam.right_cam.cx); - rightCamInfoMsg->K[4] = static_cast(zedParam.right_cam.fy); - rightCamInfoMsg->K[5] = static_cast(zedParam.right_cam.cy); - rightCamInfoMsg->K[8] = 1.0; - leftCamInfoMsg->R.fill(0.0); - rightCamInfoMsg->R.fill(0.0); - - for (size_t i = 0; i < 3; i++) { - // identity - rightCamInfoMsg->R[i + i * 3] = 1; - leftCamInfoMsg->R[i + i * 3] = 1; - } + float baseline = zedParam.getCameraBaseline(); + leftCamInfoMsg->distortion_model = sensor_msgs::distortion_models::PLUMB_BOB; + rightCamInfoMsg->distortion_model = sensor_msgs::distortion_models::PLUMB_BOB; + leftCamInfoMsg->D.resize(5); + rightCamInfoMsg->D.resize(5); + leftCamInfoMsg->D[0] = zedParam.left_cam.disto[0]; // k1 + leftCamInfoMsg->D[1] = zedParam.left_cam.disto[1]; // k2 + leftCamInfoMsg->D[2] = zedParam.left_cam.disto[4]; // k3 + leftCamInfoMsg->D[3] = zedParam.left_cam.disto[2]; // p1 + leftCamInfoMsg->D[4] = zedParam.left_cam.disto[3]; // p2 + rightCamInfoMsg->D[0] = zedParam.right_cam.disto[0]; // k1 + rightCamInfoMsg->D[1] = zedParam.right_cam.disto[1]; // k2 + rightCamInfoMsg->D[2] = zedParam.right_cam.disto[4]; // k3 + rightCamInfoMsg->D[3] = zedParam.right_cam.disto[2]; // p1 + rightCamInfoMsg->D[4] = zedParam.right_cam.disto[3]; // p2 + leftCamInfoMsg->K.fill(0.0); + rightCamInfoMsg->K.fill(0.0); + leftCamInfoMsg->K[0] = static_cast(zedParam.left_cam.fx); + leftCamInfoMsg->K[2] = static_cast(zedParam.left_cam.cx); + leftCamInfoMsg->K[4] = static_cast(zedParam.left_cam.fy); + leftCamInfoMsg->K[5] = static_cast(zedParam.left_cam.cy); + leftCamInfoMsg->K[8] = 1.0; + rightCamInfoMsg->K[0] = static_cast(zedParam.right_cam.fx); + rightCamInfoMsg->K[2] = static_cast(zedParam.right_cam.cx); + rightCamInfoMsg->K[4] = static_cast(zedParam.right_cam.fy); + rightCamInfoMsg->K[5] = static_cast(zedParam.right_cam.cy); + rightCamInfoMsg->K[8] = 1.0; + leftCamInfoMsg->R.fill(0.0); + rightCamInfoMsg->R.fill(0.0); + + for (size_t i = 0; i < 3; i++) + { + // identity + rightCamInfoMsg->R[i + i * 3] = 1; + leftCamInfoMsg->R[i + i * 3] = 1; + } - if (rawParam) { - for (int i = 0; i < 9; i++) { - rightCamInfoMsg->R[i] = zedParam.stereo_transform.getRotationMatrix().r[i]; - } + if (rawParam) + { + for (int i = 0; i < 9; i++) + { + rightCamInfoMsg->R[i] = zedParam.stereo_transform.getRotationMatrix().r[i]; } + } - leftCamInfoMsg->P.fill(0.0); - rightCamInfoMsg->P.fill(0.0); - leftCamInfoMsg->P[0] = static_cast(zedParam.left_cam.fx); - leftCamInfoMsg->P[2] = static_cast(zedParam.left_cam.cx); - leftCamInfoMsg->P[5] = static_cast(zedParam.left_cam.fy); - leftCamInfoMsg->P[6] = static_cast(zedParam.left_cam.cy); - leftCamInfoMsg->P[10] = 1.0; - // http://docs.ros.org/api/sensor_msgs/html/msg/CameraInfo.html - rightCamInfoMsg->P[3] = static_cast(-1 * zedParam.left_cam.fx * baseline); - rightCamInfoMsg->P[0] = static_cast(zedParam.right_cam.fx); - rightCamInfoMsg->P[2] = static_cast(zedParam.right_cam.cx); - rightCamInfoMsg->P[5] = static_cast(zedParam.right_cam.fy); - rightCamInfoMsg->P[6] = static_cast(zedParam.right_cam.cy); - rightCamInfoMsg->P[10] = 1.0; - leftCamInfoMsg->width = rightCamInfoMsg->width = static_cast(mMatResol.width); - leftCamInfoMsg->height = rightCamInfoMsg->height = static_cast(mMatResol.height); - leftCamInfoMsg->header.frame_id = leftFrameId; - rightCamInfoMsg->header.frame_id = rightFrameId; + leftCamInfoMsg->P.fill(0.0); + rightCamInfoMsg->P.fill(0.0); + leftCamInfoMsg->P[0] = static_cast(zedParam.left_cam.fx); + leftCamInfoMsg->P[2] = static_cast(zedParam.left_cam.cx); + leftCamInfoMsg->P[5] = static_cast(zedParam.left_cam.fy); + leftCamInfoMsg->P[6] = static_cast(zedParam.left_cam.cy); + leftCamInfoMsg->P[10] = 1.0; + // http://docs.ros.org/api/sensor_msgs/html/msg/CameraInfo.html + rightCamInfoMsg->P[3] = static_cast(-1 * zedParam.left_cam.fx * baseline); + rightCamInfoMsg->P[0] = static_cast(zedParam.right_cam.fx); + rightCamInfoMsg->P[2] = static_cast(zedParam.right_cam.cx); + rightCamInfoMsg->P[5] = static_cast(zedParam.right_cam.fy); + rightCamInfoMsg->P[6] = static_cast(zedParam.right_cam.cy); + rightCamInfoMsg->P[10] = 1.0; + leftCamInfoMsg->width = rightCamInfoMsg->width = static_cast(mMatResol.width); + leftCamInfoMsg->height = rightCamInfoMsg->height = static_cast(mMatResol.height); + leftCamInfoMsg->header.frame_id = leftFrameId; + rightCamInfoMsg->header.frame_id = rightFrameId; } -void ZEDWrapperNodelet::fillCamDepthInfo(sl::Camera& zed, sensor_msgs::CameraInfoPtr depth_info_msg, std::string frame_id) +void ZEDWrapperNodelet::fillCamDepthInfo(sl::Camera& zed, sensor_msgs::CameraInfoPtr depth_info_msg, + std::string frame_id) { - sl::CalibrationParameters zedParam; - - 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 - depth_info_msg->D[1] = zedParam.left_cam.disto[1]; // k2 - depth_info_msg->D[2] = zedParam.left_cam.disto[4]; // k3 - depth_info_msg->D[3] = zedParam.left_cam.disto[2]; // p1 - depth_info_msg->D[4] = zedParam.left_cam.disto[3]; // p2 - depth_info_msg->K.fill(0.0); - depth_info_msg->K[0] = static_cast(zedParam.left_cam.fx); - depth_info_msg->K[2] = static_cast(zedParam.left_cam.cx); - depth_info_msg->K[4] = static_cast(zedParam.left_cam.fy); - depth_info_msg->K[5] = static_cast(zedParam.left_cam.cy); - depth_info_msg->K[8] = 1.0; - depth_info_msg->R.fill(0.0); - - for (size_t i = 0; i < 3; i++) { - // identity - depth_info_msg->R[i + i * 3] = 1; - } + sl::CalibrationParameters zedParam; + + 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 + depth_info_msg->D[1] = zedParam.left_cam.disto[1]; // k2 + depth_info_msg->D[2] = zedParam.left_cam.disto[4]; // k3 + depth_info_msg->D[3] = zedParam.left_cam.disto[2]; // p1 + depth_info_msg->D[4] = zedParam.left_cam.disto[3]; // p2 + depth_info_msg->K.fill(0.0); + depth_info_msg->K[0] = static_cast(zedParam.left_cam.fx); + depth_info_msg->K[2] = static_cast(zedParam.left_cam.cx); + depth_info_msg->K[4] = static_cast(zedParam.left_cam.fy); + depth_info_msg->K[5] = static_cast(zedParam.left_cam.cy); + depth_info_msg->K[8] = 1.0; + depth_info_msg->R.fill(0.0); + + for (size_t i = 0; i < 3; i++) + { + // identity + depth_info_msg->R[i + i * 3] = 1; + } - depth_info_msg->P.fill(0.0); - depth_info_msg->P[0] = static_cast(zedParam.left_cam.fx); - depth_info_msg->P[2] = static_cast(zedParam.left_cam.cx); - depth_info_msg->P[5] = static_cast(zedParam.left_cam.fy); - depth_info_msg->P[6] = static_cast(zedParam.left_cam.cy); - depth_info_msg->P[10] = 1.0; - // http://docs.ros.org/api/sensor_msgs/html/msg/CameraInfo.html - depth_info_msg->width = static_cast(mMatResol.width); - depth_info_msg->height = static_cast(mMatResol.height); - depth_info_msg->header.frame_id = frame_id; + depth_info_msg->P.fill(0.0); + depth_info_msg->P[0] = static_cast(zedParam.left_cam.fx); + depth_info_msg->P[2] = static_cast(zedParam.left_cam.cx); + depth_info_msg->P[5] = static_cast(zedParam.left_cam.fy); + depth_info_msg->P[6] = static_cast(zedParam.left_cam.cy); + depth_info_msg->P[10] = 1.0; + // http://docs.ros.org/api/sensor_msgs/html/msg/CameraInfo.html + depth_info_msg->width = static_cast(mMatResol.width); + depth_info_msg->height = static_cast(mMatResol.height); + depth_info_msg->header.frame_id = frame_id; } void ZEDWrapperNodelet::updateDynamicReconfigure() { - // NODELET_DEBUG_STREAM( "updateDynamicReconfigure MUTEX LOCK"); - mDynParMutex.lock(); - zed_nodelets::ZedConfig config; - - config.auto_exposure_gain = mCamAutoExposure; - config.auto_whitebalance = mCamAutoWB; - config.brightness = mCamBrightness; - config.depth_confidence = mCamDepthConfidence; - config.depth_texture_conf = mCamDepthTextureConf; - config.contrast = mCamContrast; - config.exposure = mCamExposure; - config.gain = mCamGain; - config.hue = mCamHue; - config.saturation = mCamSaturation; - config.sharpness = mCamSharpness; - config.gamma = mCamGamma; - config.whitebalance_temperature = mCamWB / 100; - config.point_cloud_freq = mPointCloudFreq; - mDynParMutex.unlock(); - - mDynServerMutex.lock(); - mDynRecServer->updateConfig(config); - mDynServerMutex.unlock(); - - mUpdateDynParams = false; - - // NODELET_DEBUG_STREAM( "updateDynamicReconfigure MUTEX UNLOCK"); + // NODELET_DEBUG_STREAM( "updateDynamicReconfigure MUTEX LOCK"); + mDynParMutex.lock(); + zed_nodelets::ZedConfig config; + + config.auto_exposure_gain = mCamAutoExposure; + config.auto_whitebalance = mCamAutoWB; + config.brightness = mCamBrightness; + config.depth_confidence = mCamDepthConfidence; + config.depth_texture_conf = mCamDepthTextureConf; + config.contrast = mCamContrast; + config.exposure = mCamExposure; + config.gain = mCamGain; + config.hue = mCamHue; + config.saturation = mCamSaturation; + config.sharpness = mCamSharpness; + config.gamma = mCamGamma; + config.whitebalance_temperature = mCamWB / 100; + config.point_cloud_freq = mPointCloudFreq; + mDynParMutex.unlock(); + + mDynServerMutex.lock(); + mDynRecServer->updateConfig(config); + mDynServerMutex.unlock(); + + mUpdateDynParams = false; + + // NODELET_DEBUG_STREAM( "updateDynamicReconfigure MUTEX UNLOCK"); } void ZEDWrapperNodelet::callback_dynamicReconf(zed_nodelets::ZedConfig& config, uint32_t level) { - // NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX LOCK"); - mDynParMutex.lock(); - DynParams param = static_cast(level); + // NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX LOCK"); + mDynParMutex.lock(); + DynParams param = static_cast(level); - switch (param) { + switch (param) + { case CONFIDENCE: - mCamDepthConfidence = config.depth_confidence; - NODELET_INFO("Reconfigure confidence threshold: %d", mCamDepthConfidence); - mDynParMutex.unlock(); - // NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); - break; + mCamDepthConfidence = config.depth_confidence; + NODELET_INFO("Reconfigure confidence threshold: %d", mCamDepthConfidence); + mDynParMutex.unlock(); + // NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); + break; case TEXTURE_CONF: - mCamDepthTextureConf = config.depth_texture_conf; - NODELET_INFO("Reconfigure texture confidence threshold: %d", mCamDepthTextureConf); - mDynParMutex.unlock(); - // NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); - break; + mCamDepthTextureConf = config.depth_texture_conf; + NODELET_INFO("Reconfigure texture confidence threshold: %d", mCamDepthTextureConf); + mDynParMutex.unlock(); + // NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); + break; case POINTCLOUD_FREQ: - if (config.point_cloud_freq > mPubFrameRate) { - mPointCloudFreq = mPubFrameRate; - NODELET_WARN_STREAM("'point_cloud_freq' cannot be major than Video/Depth publishing framerate. Set to " - << mPointCloudFreq); + if (config.point_cloud_freq > mPubFrameRate) + { + mPointCloudFreq = mPubFrameRate; + NODELET_WARN_STREAM("'point_cloud_freq' cannot be major than Video/Depth publishing framerate. Set to " + << mPointCloudFreq); - mUpdateDynParams = true; - } else { - mPointCloudFreq = config.point_cloud_freq; - NODELET_INFO("Reconfigure point cloud pub. frequency: %g", mPointCloudFreq); - } + mUpdateDynParams = true; + } + else + { + mPointCloudFreq = config.point_cloud_freq; + NODELET_INFO("Reconfigure point cloud pub. frequency: %g", mPointCloudFreq); + } - mDynParMutex.unlock(); - // NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); - break; + mDynParMutex.unlock(); + // NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); + break; case BRIGHTNESS: - mCamBrightness = config.brightness; - NODELET_INFO("Reconfigure image brightness: %d", mCamBrightness); - mDynParMutex.unlock(); - // NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); - break; + mCamBrightness = config.brightness; + NODELET_INFO("Reconfigure image brightness: %d", mCamBrightness); + mDynParMutex.unlock(); + // NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); + break; case CONTRAST: - mCamContrast = config.contrast; - NODELET_INFO("Reconfigure image contrast: %d", mCamContrast); - mDynParMutex.unlock(); - // NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); - break; + mCamContrast = config.contrast; + NODELET_INFO("Reconfigure image contrast: %d", mCamContrast); + mDynParMutex.unlock(); + // NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); + break; case HUE: - mCamHue = config.hue; - NODELET_INFO("Reconfigure image hue: %d", mCamHue); - mDynParMutex.unlock(); - // NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); - break; + mCamHue = config.hue; + NODELET_INFO("Reconfigure image hue: %d", mCamHue); + mDynParMutex.unlock(); + // NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); + break; case SATURATION: - mCamSaturation = config.saturation; - NODELET_INFO("Reconfigure image saturation: %d", mCamSaturation); - mDynParMutex.unlock(); - // NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); - break; + mCamSaturation = config.saturation; + NODELET_INFO("Reconfigure image saturation: %d", mCamSaturation); + mDynParMutex.unlock(); + // NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); + break; case SHARPNESS: - mCamSharpness = config.sharpness; - NODELET_INFO("Reconfigure image sharpness: %d", mCamSharpness); - mDynParMutex.unlock(); - // NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); - break; + mCamSharpness = config.sharpness; + NODELET_INFO("Reconfigure image sharpness: %d", mCamSharpness); + mDynParMutex.unlock(); + // NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); + break; case GAMMA: - mCamGamma = config.gamma; - NODELET_INFO("Reconfigure image gamma: %d", mCamGamma); - mDynParMutex.unlock(); - // NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); - break; + mCamGamma = config.gamma; + NODELET_INFO("Reconfigure image gamma: %d", mCamGamma); + mDynParMutex.unlock(); + // NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); + break; case AUTO_EXP_GAIN: - if(config.auto_exposure_gain!=mCamAutoExposure) - { - mCamAutoExposure = config.auto_exposure_gain; - NODELET_INFO_STREAM("Reconfigure auto exposure/gain: " << mCamAutoExposure ? "ENABLED": "DISABLED"); - } - mDynParMutex.unlock(); - // NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); - break; + if (config.auto_exposure_gain != mCamAutoExposure) + { + mCamAutoExposure = config.auto_exposure_gain; + NODELET_INFO_STREAM("Reconfigure auto exposure/gain: " << mCamAutoExposure ? "ENABLED" : "DISABLED"); + } + mDynParMutex.unlock(); + // NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); + break; case GAIN: - mCamGain = config.gain; - if (mCamAutoExposure) { - NODELET_WARN("Reconfigure gain has no effect if 'auto_exposure_gain' is enabled"); - } else { - NODELET_INFO("Reconfigure gain: %d", mCamGain); - } - mDynParMutex.unlock(); - // NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); - break; + mCamGain = config.gain; + if (mCamAutoExposure) + { + NODELET_WARN("Reconfigure gain has no effect if 'auto_exposure_gain' is enabled"); + } + else + { + NODELET_INFO("Reconfigure gain: %d", mCamGain); + } + mDynParMutex.unlock(); + // NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); + break; case EXPOSURE: - mCamExposure = config.exposure; - if (mCamAutoExposure) { - NODELET_WARN("Reconfigure exposure has no effect if 'auto_exposure_gain' is enabled"); - } else { - NODELET_INFO("Reconfigure exposure: %d", mCamExposure); - } - mDynParMutex.unlock(); - // NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); - break; + mCamExposure = config.exposure; + if (mCamAutoExposure) + { + NODELET_WARN("Reconfigure exposure has no effect if 'auto_exposure_gain' is enabled"); + } + else + { + NODELET_INFO("Reconfigure exposure: %d", mCamExposure); + } + mDynParMutex.unlock(); + // NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); + break; case AUTO_WB: - if(config.auto_whitebalance!=mCamAutoWB) - { - mCamAutoWB = config.auto_whitebalance; - NODELET_INFO_STREAM("Reconfigure auto white balance: " << mCamAutoWB ? "ENABLED": "DISABLED"); - mTriggerAutoWB = true; - } else { - mTriggerAutoWB = false; - } - mDynParMutex.unlock(); - // NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); - break; + if (config.auto_whitebalance != mCamAutoWB) + { + mCamAutoWB = config.auto_whitebalance; + NODELET_INFO_STREAM("Reconfigure auto white balance: " << mCamAutoWB ? "ENABLED" : "DISABLED"); + mTriggerAutoWB = true; + } + else + { + mTriggerAutoWB = false; + } + mDynParMutex.unlock(); + // NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); + break; case WB_TEMP: - mCamWB = config.whitebalance_temperature * 100; - if (mCamAutoWB) { - NODELET_WARN("Reconfigure white balance temperature has no effect if 'auto_whitebalance' is enabled"); - } else { - NODELET_INFO("Reconfigure white balance temperature: %d", mCamWB); - } - mDynParMutex.unlock(); - // NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); - break; + mCamWB = config.whitebalance_temperature * 100; + if (mCamAutoWB) + { + NODELET_WARN("Reconfigure white balance temperature has no effect if 'auto_whitebalance' is enabled"); + } + else + { + NODELET_INFO("Reconfigure white balance temperature: %d", mCamWB); + } + mDynParMutex.unlock(); + // NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); + break; default: - NODELET_DEBUG_STREAM("dynamicReconfCallback Unknown param: " << level); - mDynParMutex.unlock(); - // NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); - } + NODELET_DEBUG_STREAM("dynamicReconfCallback Unknown param: " << level); + mDynParMutex.unlock(); + // NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK"); + } } void ZEDWrapperNodelet::pubVideoDepth() { - static sl::Timestamp lastZedTs = 0; // Used to calculate stable publish frequency - - uint32_t rgbSubnumber = mPubRgb.getNumSubscribers(); - uint32_t rgbRawSubnumber = mPubRawRgb.getNumSubscribers(); - uint32_t leftSubnumber = mPubLeft.getNumSubscribers(); - uint32_t leftRawSubnumber = mPubRawLeft.getNumSubscribers(); - uint32_t rightSubnumber = mPubRight.getNumSubscribers(); - uint32_t rightRawSubnumber = mPubRawRight.getNumSubscribers(); - uint32_t rgbGraySubnumber = mPubRgbGray.getNumSubscribers(); - uint32_t rgbGrayRawSubnumber = mPubRawRgbGray.getNumSubscribers(); - uint32_t leftGraySubnumber = mPubLeftGray.getNumSubscribers(); - uint32_t leftGrayRawSubnumber = mPubRawLeftGray.getNumSubscribers(); - uint32_t rightGraySubnumber = mPubRightGray.getNumSubscribers(); - uint32_t rightGrayRawSubnumber = mPubRawRightGray.getNumSubscribers(); - uint32_t depthSubnumber = mPubDepth.getNumSubscribers(); - uint32_t disparitySubnumber = mPubDisparity.getNumSubscribers(); - uint32_t confMapSubnumber = mPubConfMap.getNumSubscribers(); - uint32_t stereoSubNumber = mPubStereo.getNumSubscribers(); - uint32_t stereoRawSubNumber = mPubRawStereo.getNumSubscribers(); - - uint32_t tot_sub = rgbSubnumber + rgbRawSubnumber + leftSubnumber + leftRawSubnumber + rightSubnumber + rightRawSubnumber + rgbGraySubnumber + rgbGrayRawSubnumber + leftGraySubnumber + leftGrayRawSubnumber + rightGraySubnumber + rightGrayRawSubnumber + depthSubnumber + disparitySubnumber + confMapSubnumber + stereoSubNumber + stereoRawSubNumber; - - bool retrieved = false; - - sl::Mat mat_left, mat_left_raw; - sl::Mat mat_right, mat_right_raw; - sl::Mat mat_left_gray, mat_left_raw_gray; - 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 grab_ts = 0; - - // ----> Retrieve all required image data - if (rgbSubnumber + leftSubnumber + stereoSubNumber > 0) { - mZed.retrieveImage(mat_left, sl::VIEW::LEFT, sl::MEM::CPU, mMatResol); - retrieved = true; - ts_rgb = mat_left.timestamp; - grab_ts = mat_left.timestamp; - } - if (rgbRawSubnumber + leftRawSubnumber + stereoRawSubNumber > 0) { - mZed.retrieveImage(mat_left_raw, sl::VIEW::LEFT_UNRECTIFIED, sl::MEM::CPU, mMatResol); - retrieved = true; - grab_ts = mat_left_raw.timestamp; - } - if (rightSubnumber + stereoSubNumber > 0) { - mZed.retrieveImage(mat_right, sl::VIEW::RIGHT, sl::MEM::CPU, mMatResol); - retrieved = true; - grab_ts = mat_right.timestamp; - } - if (rightRawSubnumber + stereoRawSubNumber > 0) { - mZed.retrieveImage(mat_right_raw, sl::VIEW::RIGHT_UNRECTIFIED, sl::MEM::CPU, mMatResol); - retrieved = true; - grab_ts = mat_right_raw.timestamp; - } - if (rgbGraySubnumber + leftGraySubnumber > 0) { - mZed.retrieveImage(mat_left_gray, sl::VIEW::LEFT_GRAY, sl::MEM::CPU, mMatResol); - retrieved = true; - grab_ts = mat_left_gray.timestamp; - } - if (rgbGrayRawSubnumber + leftGrayRawSubnumber > 0) { - mZed.retrieveImage(mat_left_raw_gray, sl::VIEW::LEFT_UNRECTIFIED_GRAY, sl::MEM::CPU, mMatResol); - retrieved = true; - grab_ts = mat_left_raw_gray.timestamp; - } - if (rightGraySubnumber > 0) { - mZed.retrieveImage(mat_right_gray, sl::VIEW::RIGHT_GRAY, sl::MEM::CPU, mMatResol); - retrieved = true; - grab_ts = mat_right_gray.timestamp; + static sl::Timestamp lastZedTs = 0; // Used to calculate stable publish frequency + + uint32_t rgbSubnumber = mPubRgb.getNumSubscribers(); + uint32_t rgbRawSubnumber = mPubRawRgb.getNumSubscribers(); + uint32_t leftSubnumber = mPubLeft.getNumSubscribers(); + uint32_t leftRawSubnumber = mPubRawLeft.getNumSubscribers(); + uint32_t rightSubnumber = mPubRight.getNumSubscribers(); + uint32_t rightRawSubnumber = mPubRawRight.getNumSubscribers(); + uint32_t rgbGraySubnumber = mPubRgbGray.getNumSubscribers(); + uint32_t rgbGrayRawSubnumber = mPubRawRgbGray.getNumSubscribers(); + uint32_t leftGraySubnumber = mPubLeftGray.getNumSubscribers(); + uint32_t leftGrayRawSubnumber = mPubRawLeftGray.getNumSubscribers(); + uint32_t rightGraySubnumber = mPubRightGray.getNumSubscribers(); + uint32_t rightGrayRawSubnumber = mPubRawRightGray.getNumSubscribers(); + uint32_t depthSubnumber = mPubDepth.getNumSubscribers(); + uint32_t disparitySubnumber = mPubDisparity.getNumSubscribers(); + uint32_t confMapSubnumber = mPubConfMap.getNumSubscribers(); + uint32_t stereoSubNumber = mPubStereo.getNumSubscribers(); + uint32_t stereoRawSubNumber = mPubRawStereo.getNumSubscribers(); + + uint32_t tot_sub = rgbSubnumber + rgbRawSubnumber + leftSubnumber + leftRawSubnumber + rightSubnumber + + rightRawSubnumber + rgbGraySubnumber + rgbGrayRawSubnumber + leftGraySubnumber + + leftGrayRawSubnumber + rightGraySubnumber + rightGrayRawSubnumber + depthSubnumber + + disparitySubnumber + confMapSubnumber + stereoSubNumber + stereoRawSubNumber; + + bool retrieved = false; + + sl::Mat mat_left, mat_left_raw; + sl::Mat mat_right, mat_right_raw; + sl::Mat mat_left_gray, mat_left_raw_gray; + 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 grab_ts = 0; + + // ----> Retrieve all required image data + if (rgbSubnumber + leftSubnumber + stereoSubNumber > 0) + { + mZed.retrieveImage(mat_left, sl::VIEW::LEFT, sl::MEM::CPU, mMatResol); + retrieved = true; + ts_rgb = mat_left.timestamp; + grab_ts = mat_left.timestamp; + } + if (rgbRawSubnumber + leftRawSubnumber + stereoRawSubNumber > 0) + { + mZed.retrieveImage(mat_left_raw, sl::VIEW::LEFT_UNRECTIFIED, sl::MEM::CPU, mMatResol); + retrieved = true; + grab_ts = mat_left_raw.timestamp; + } + if (rightSubnumber + stereoSubNumber > 0) + { + mZed.retrieveImage(mat_right, sl::VIEW::RIGHT, sl::MEM::CPU, mMatResol); + retrieved = true; + grab_ts = mat_right.timestamp; + } + if (rightRawSubnumber + stereoRawSubNumber > 0) + { + mZed.retrieveImage(mat_right_raw, sl::VIEW::RIGHT_UNRECTIFIED, sl::MEM::CPU, mMatResol); + retrieved = true; + grab_ts = mat_right_raw.timestamp; + } + if (rgbGraySubnumber + leftGraySubnumber > 0) + { + mZed.retrieveImage(mat_left_gray, sl::VIEW::LEFT_GRAY, sl::MEM::CPU, mMatResol); + retrieved = true; + grab_ts = mat_left_gray.timestamp; + } + if (rgbGrayRawSubnumber + leftGrayRawSubnumber > 0) + { + mZed.retrieveImage(mat_left_raw_gray, sl::VIEW::LEFT_UNRECTIFIED_GRAY, sl::MEM::CPU, mMatResol); + retrieved = true; + grab_ts = mat_left_raw_gray.timestamp; + } + if (rightGraySubnumber > 0) + { + mZed.retrieveImage(mat_right_gray, sl::VIEW::RIGHT_GRAY, sl::MEM::CPU, mMatResol); + retrieved = true; + grab_ts = mat_right_gray.timestamp; + } + if (rightGrayRawSubnumber > 0) + { + mZed.retrieveImage(mat_right_raw_gray, sl::VIEW::RIGHT_UNRECTIFIED_GRAY, sl::MEM::CPU, mMatResol); + retrieved = true; + grab_ts = mat_right_raw_gray.timestamp; + } + if (depthSubnumber > 0) + { + if (!mOpenniDepthMode) + { + mZed.retrieveMeasure(mat_depth, sl::MEASURE::DEPTH, sl::MEM::CPU, mMatResol); } - if (rightGrayRawSubnumber > 0) { - mZed.retrieveImage(mat_right_raw_gray, sl::VIEW::RIGHT_UNRECTIFIED_GRAY, sl::MEM::CPU, mMatResol); - retrieved = true; - grab_ts = mat_right_raw_gray.timestamp; + else + { + mZed.retrieveMeasure(mat_depth, sl::MEASURE::DEPTH_U16_MM, sl::MEM::CPU, mMatResol); } - if (depthSubnumber > 0) { - if (!mOpenniDepthMode) { - mZed.retrieveMeasure(mat_depth, sl::MEASURE::DEPTH, sl::MEM::CPU, mMatResol); - } else { - mZed.retrieveMeasure(mat_depth, sl::MEASURE::DEPTH_U16_MM, sl::MEM::CPU, mMatResol); - } - retrieved = true; - grab_ts = mat_depth.timestamp; - - ts_depth = mat_depth.timestamp; + retrieved = true; + grab_ts = mat_depth.timestamp; - if (ts_rgb.data_ns != 0 && (ts_depth.data_ns != ts_rgb.data_ns)) { - NODELET_WARN_STREAM("!!!!! DEPTH/RGB ASYNC !!!!! - Delta: " << 1e-9 * static_cast(ts_depth - ts_rgb) - << " sec"); - } - } - if (disparitySubnumber > 0) { - mZed.retrieveMeasure(mat_disp, sl::MEASURE::DISPARITY, sl::MEM::CPU, mMatResol); - retrieved = true; - grab_ts = mat_disp.timestamp; - } - if (confMapSubnumber > 0) { - mZed.retrieveMeasure(mat_conf, sl::MEASURE::CONFIDENCE, sl::MEM::CPU, mMatResol); - retrieved = true; - grab_ts = mat_conf.timestamp; - } - // <---- Retrieve all required image data + ts_depth = mat_depth.timestamp; - // ----> Data ROS timestamp - ros::Time stamp = sl_tools::slTime2Ros(grab_ts); - if (mSvoMode) { - stamp = ros::Time::now(); - } - // <---- Data ROS timestamp - - // ----> Publish sensor data if sync is required by user or SVO - if (mZedRealCamModel != sl::MODEL::ZED) { - if (mSensTimestampSync) { - // NODELET_INFO_STREAM("tot_sub: " << tot_sub << " - retrieved: " << retrieved << " - - // grab_ts.data_ns!=lastZedTs.data_ns: " << (grab_ts.data_ns!=lastZedTs.data_ns)); - if (tot_sub > 0 && retrieved && (grab_ts.data_ns != lastZedTs.data_ns)) { - // NODELET_INFO("CALLBACK"); - publishSensData(stamp); - } - } else if (mSvoMode) { - publishSensData(stamp); - } + if (ts_rgb.data_ns != 0 && (ts_depth.data_ns != ts_rgb.data_ns)) + { + NODELET_WARN_STREAM("!!!!! DEPTH/RGB ASYNC !!!!! - Delta: " << 1e-9 * static_cast(ts_depth - ts_rgb) + << " sec"); } - // <---- Publish sensor data if sync is required by user or SVO - - // ----> Notify grab thread that all data are synchronized and a new grab can be done - // mRgbDepthDataRetrievedCondVar.notify_one(); - // mRgbDepthDataRetrieved = true; - // <---- Notify grab thread that all data are synchronized and a new grab can be done + } + if (disparitySubnumber > 0) + { + mZed.retrieveMeasure(mat_disp, sl::MEASURE::DISPARITY, sl::MEM::CPU, mMatResol); + retrieved = true; + grab_ts = mat_disp.timestamp; + } + if (confMapSubnumber > 0) + { + mZed.retrieveMeasure(mat_conf, sl::MEASURE::CONFIDENCE, sl::MEM::CPU, mMatResol); + retrieved = true; + grab_ts = mat_conf.timestamp; + } + // <---- Retrieve all required image data - if (!retrieved) { - mPublishingData = false; - lastZedTs = 0; - return; - } - mPublishingData = true; + // ----> Data ROS timestamp + ros::Time stamp = sl_tools::slTime2Ros(grab_ts); + if (mSvoMode) + { + stamp = ros::Time::now(); + } + // <---- Data ROS timestamp - // ----> Check if a grab has been done before publishing the same images - if (grab_ts.data_ns == lastZedTs.data_ns) { - // Data not updated by a grab calling in the grab thread - return; + // ----> Publish sensor data if sync is required by user or SVO + if (mZedRealCamModel != sl::MODEL::ZED) + { + if (mSensTimestampSync) + { + // NODELET_INFO_STREAM("tot_sub: " << tot_sub << " - retrieved: " << retrieved << " - + // grab_ts.data_ns!=lastZedTs.data_ns: " << (grab_ts.data_ns!=lastZedTs.data_ns)); + if (tot_sub > 0 && retrieved && (grab_ts.data_ns != lastZedTs.data_ns)) + { + // NODELET_INFO("CALLBACK"); + publishSensData(stamp); + } } - if (lastZedTs.data_ns != 0) { - double period_sec = static_cast(grab_ts.data_ns - lastZedTs.data_ns) / 1e9; - // NODELET_DEBUG_STREAM( "PUBLISHING PERIOD: " << period_sec << " sec @" << 1./period_sec << " Hz") ; - - mVideoDepthPeriodMean_sec->addValue(period_sec); - // NODELET_DEBUG_STREAM( "MEAN PUBLISHING PERIOD: " << mVideoDepthPeriodMean_sec->getMean() << " sec @" - // << 1./mVideoDepthPeriodMean_sec->getMean() << " Hz") ; + else if (mSvoMode) + { + publishSensData(stamp); } - lastZedTs = grab_ts; - // <---- Check if a grab has been done before publishing the same images + } + // <---- Publish sensor data if sync is required by user or SVO + + // ----> Notify grab thread that all data are synchronized and a new grab can be done + // mRgbDepthDataRetrievedCondVar.notify_one(); + // mRgbDepthDataRetrieved = true; + // <---- Notify grab thread that all data are synchronized and a new grab can be done + + if (!retrieved) + { + mPublishingData = false; + lastZedTs = 0; + return; + } + mPublishingData = true; - // Publish the left = rgb image if someone has subscribed to - if (leftSubnumber > 0) { - sensor_msgs::ImagePtr leftImgMsg = boost::make_shared(); - publishImage(leftImgMsg, mat_left, mPubLeft, mLeftCamInfoMsg, mLeftCamOptFrameId, stamp); - } - if (rgbSubnumber > 0) { - sensor_msgs::ImagePtr rgbImgMsg = boost::make_shared(); - publishImage(rgbImgMsg, mat_left, mPubRgb, mRgbCamInfoMsg, mDepthOptFrameId, stamp); - } + // ----> Check if a grab has been done before publishing the same images + if (grab_ts.data_ns == lastZedTs.data_ns) + { + // Data not updated by a grab calling in the grab thread + return; + } + if (lastZedTs.data_ns != 0) + { + double period_sec = static_cast(grab_ts.data_ns - lastZedTs.data_ns) / 1e9; + // NODELET_DEBUG_STREAM( "PUBLISHING PERIOD: " << period_sec << " sec @" << 1./period_sec << " Hz") ; + + mVideoDepthPeriodMean_sec->addValue(period_sec); + // NODELET_DEBUG_STREAM( "MEAN PUBLISHING PERIOD: " << mVideoDepthPeriodMean_sec->getMean() << " sec @" + // << 1./mVideoDepthPeriodMean_sec->getMean() << " Hz") ; + } + lastZedTs = grab_ts; + // <---- Check if a grab has been done before publishing the same images + + // Publish the left = rgb image if someone has subscribed to + if (leftSubnumber > 0) + { + sensor_msgs::ImagePtr leftImgMsg = boost::make_shared(); + publishImage(leftImgMsg, mat_left, mPubLeft, mLeftCamInfoMsg, mLeftCamOptFrameId, stamp); + } + if (rgbSubnumber > 0) + { + sensor_msgs::ImagePtr rgbImgMsg = boost::make_shared(); + publishImage(rgbImgMsg, mat_left, mPubRgb, mRgbCamInfoMsg, mDepthOptFrameId, stamp); + } - // Publish the left = rgb GRAY image if someone has subscribed to - if (leftGraySubnumber > 0) { - sensor_msgs::ImagePtr leftGrayImgMsg = boost::make_shared(); - publishImage(leftGrayImgMsg, mat_left_gray, mPubLeftGray, mLeftCamInfoMsg, mLeftCamOptFrameId, stamp); - } - if (rgbGraySubnumber > 0) { - sensor_msgs::ImagePtr rgbGrayImgMsg = boost::make_shared(); - publishImage(rgbGrayImgMsg, mat_left_gray, mPubRgbGray, mRgbCamInfoMsg, mDepthOptFrameId, stamp); - } + // Publish the left = rgb GRAY image if someone has subscribed to + if (leftGraySubnumber > 0) + { + sensor_msgs::ImagePtr leftGrayImgMsg = boost::make_shared(); + publishImage(leftGrayImgMsg, mat_left_gray, mPubLeftGray, mLeftCamInfoMsg, mLeftCamOptFrameId, stamp); + } + if (rgbGraySubnumber > 0) + { + sensor_msgs::ImagePtr rgbGrayImgMsg = boost::make_shared(); + publishImage(rgbGrayImgMsg, mat_left_gray, mPubRgbGray, mRgbCamInfoMsg, mDepthOptFrameId, stamp); + } - // Publish the left_raw = rgb_raw image if someone has subscribed to - if (leftRawSubnumber > 0) { - sensor_msgs::ImagePtr rawLeftImgMsg = boost::make_shared(); - publishImage(rawLeftImgMsg, mat_left_raw, mPubRawLeft, mLeftCamInfoRawMsg, mLeftCamOptFrameId, stamp); - } - if (rgbRawSubnumber > 0) { - sensor_msgs::ImagePtr rawRgbImgMsg = boost::make_shared(); - publishImage(rawRgbImgMsg, mat_left_raw, mPubRawRgb, mRgbCamInfoRawMsg, mDepthOptFrameId, stamp); - } + // Publish the left_raw = rgb_raw image if someone has subscribed to + if (leftRawSubnumber > 0) + { + sensor_msgs::ImagePtr rawLeftImgMsg = boost::make_shared(); + publishImage(rawLeftImgMsg, mat_left_raw, mPubRawLeft, mLeftCamInfoRawMsg, mLeftCamOptFrameId, stamp); + } + if (rgbRawSubnumber > 0) + { + sensor_msgs::ImagePtr rawRgbImgMsg = boost::make_shared(); + publishImage(rawRgbImgMsg, mat_left_raw, mPubRawRgb, mRgbCamInfoRawMsg, mDepthOptFrameId, stamp); + } - // Publish the left_raw == rgb_raw GRAY image if someone has subscribed to - if (leftGrayRawSubnumber > 0) { - sensor_msgs::ImagePtr rawLeftGrayImgMsg = boost::make_shared(); + // Publish the left_raw == rgb_raw GRAY image if someone has subscribed to + if (leftGrayRawSubnumber > 0) + { + sensor_msgs::ImagePtr rawLeftGrayImgMsg = boost::make_shared(); - publishImage(rawLeftGrayImgMsg, mat_left_raw_gray, mPubRawLeftGray, mLeftCamInfoRawMsg, mLeftCamOptFrameId, stamp); - } - if (rgbGrayRawSubnumber > 0) { - sensor_msgs::ImagePtr rawRgbGrayImgMsg = boost::make_shared(); - publishImage(rawRgbGrayImgMsg, mat_left_raw_gray, mPubRawRgbGray, mRgbCamInfoRawMsg, mDepthOptFrameId, stamp); - } + publishImage(rawLeftGrayImgMsg, mat_left_raw_gray, mPubRawLeftGray, mLeftCamInfoRawMsg, mLeftCamOptFrameId, stamp); + } + if (rgbGrayRawSubnumber > 0) + { + sensor_msgs::ImagePtr rawRgbGrayImgMsg = boost::make_shared(); + publishImage(rawRgbGrayImgMsg, mat_left_raw_gray, mPubRawRgbGray, mRgbCamInfoRawMsg, mDepthOptFrameId, stamp); + } - // Publish the right image if someone has subscribed to - if (rightSubnumber > 0) { - sensor_msgs::ImagePtr rightImgMsg = boost::make_shared(); - publishImage(rightImgMsg, mat_right, mPubRight, mRightCamInfoMsg, mRightCamOptFrameId, stamp); - } + // Publish the right image if someone has subscribed to + if (rightSubnumber > 0) + { + sensor_msgs::ImagePtr rightImgMsg = boost::make_shared(); + publishImage(rightImgMsg, mat_right, mPubRight, mRightCamInfoMsg, mRightCamOptFrameId, stamp); + } - // Publish the right image GRAY if someone has subscribed to - if (rightGraySubnumber > 0) { - sensor_msgs::ImagePtr rightGrayImgMsg = boost::make_shared(); - publishImage(rightGrayImgMsg, mat_right_gray, mPubRightGray, mRightCamInfoMsg, mRightCamOptFrameId, stamp); - } + // Publish the right image GRAY if someone has subscribed to + if (rightGraySubnumber > 0) + { + sensor_msgs::ImagePtr rightGrayImgMsg = boost::make_shared(); + publishImage(rightGrayImgMsg, mat_right_gray, mPubRightGray, mRightCamInfoMsg, mRightCamOptFrameId, stamp); + } - // Publish the right raw image if someone has subscribed to - if (rightRawSubnumber > 0) { - sensor_msgs::ImagePtr rawRightImgMsg = boost::make_shared(); - publishImage(rawRightImgMsg, mat_right_raw, mPubRawRight, mRightCamInfoRawMsg, mRightCamOptFrameId, stamp); - } + // Publish the right raw image if someone has subscribed to + if (rightRawSubnumber > 0) + { + sensor_msgs::ImagePtr rawRightImgMsg = boost::make_shared(); + publishImage(rawRightImgMsg, mat_right_raw, mPubRawRight, mRightCamInfoRawMsg, mRightCamOptFrameId, stamp); + } - // Publish the right raw image GRAY if someone has subscribed to - if (rightGrayRawSubnumber > 0) { - sensor_msgs::ImagePtr rawRightGrayImgMsg = boost::make_shared(); - publishImage(rawRightGrayImgMsg, mat_right_raw_gray, mPubRawRightGray, mRightCamInfoRawMsg, mRightCamOptFrameId, - stamp); - } + // Publish the right raw image GRAY if someone has subscribed to + if (rightGrayRawSubnumber > 0) + { + sensor_msgs::ImagePtr rawRightGrayImgMsg = boost::make_shared(); + publishImage(rawRightGrayImgMsg, mat_right_raw_gray, mPubRawRightGray, mRightCamInfoRawMsg, mRightCamOptFrameId, + stamp); + } - // Stereo couple side-by-side - if (stereoSubNumber > 0) { - sensor_msgs::ImagePtr stereoImgMsg = boost::make_shared(); - sl_tools::imagesToROSmsg(stereoImgMsg, mat_left, mat_right, mCameraFrameId, stamp); - mPubStereo.publish(stereoImgMsg); - } + // Stereo couple side-by-side + if (stereoSubNumber > 0) + { + sensor_msgs::ImagePtr stereoImgMsg = boost::make_shared(); + sl_tools::imagesToROSmsg(stereoImgMsg, mat_left, mat_right, mCameraFrameId, stamp); + mPubStereo.publish(stereoImgMsg); + } - // Stereo RAW couple side-by-side - if (stereoRawSubNumber > 0) { - sensor_msgs::ImagePtr rawStereoImgMsg = boost::make_shared(); - sl_tools::imagesToROSmsg(rawStereoImgMsg, mat_left_raw, mat_right_raw, mCameraFrameId, stamp); - mPubRawStereo.publish(rawStereoImgMsg); - } + // Stereo RAW couple side-by-side + if (stereoRawSubNumber > 0) + { + sensor_msgs::ImagePtr rawStereoImgMsg = boost::make_shared(); + sl_tools::imagesToROSmsg(rawStereoImgMsg, mat_left_raw, mat_right_raw, mCameraFrameId, stamp); + mPubRawStereo.publish(rawStereoImgMsg); + } - // Publish the depth image if someone has subscribed to - if (depthSubnumber > 0) { - sensor_msgs::ImagePtr depthImgMsg = boost::make_shared(); - publishDepth(depthImgMsg, mat_depth, stamp); - } + // Publish the depth image if someone has subscribed to + if (depthSubnumber > 0) + { + sensor_msgs::ImagePtr depthImgMsg = boost::make_shared(); + publishDepth(depthImgMsg, mat_depth, stamp); + } - // Publish the disparity image if someone has subscribed to - if (disparitySubnumber > 0) { - publishDisparity(mat_disp, stamp); - } + // Publish the disparity image if someone has subscribed to + if (disparitySubnumber > 0) + { + publishDisparity(mat_disp, stamp); + } - // Publish the confidence map if someone has subscribed to - if (confMapSubnumber > 0) { - sensor_msgs::ImagePtr confMapMsg = boost::make_shared(); - sl_tools::imageToROSmsg(confMapMsg, mat_conf, mConfidenceOptFrameId, stamp); - mPubConfMap.publish(confMapMsg); - } + // Publish the confidence map if someone has subscribed to + if (confMapSubnumber > 0) + { + sensor_msgs::ImagePtr confMapMsg = boost::make_shared(); + sl_tools::imageToROSmsg(confMapMsg, mat_conf, mConfidenceOptFrameId, stamp); + mPubConfMap.publish(confMapMsg); + } } void ZEDWrapperNodelet::callback_pubPath(const ros::TimerEvent& e) { - uint32_t mapPathSub = mPubMapPath.getNumSubscribers(); - uint32_t odomPathSub = mPubOdomPath.getNumSubscribers(); - - geometry_msgs::PoseStamped odomPose; - geometry_msgs::PoseStamped mapPose; - - odomPose.header.stamp = mFrameTimestamp; - odomPose.header.frame_id = mMapFrameId; // frame - // conversion from Tranform to message - geometry_msgs::Transform base2odom = tf2::toMsg(mOdom2BaseTransf); - // Add all value in Pose message - odomPose.pose.position.x = base2odom.translation.x; - odomPose.pose.position.y = base2odom.translation.y; - odomPose.pose.position.z = base2odom.translation.z; - odomPose.pose.orientation.x = base2odom.rotation.x; - odomPose.pose.orientation.y = base2odom.rotation.y; - odomPose.pose.orientation.z = base2odom.rotation.z; - odomPose.pose.orientation.w = base2odom.rotation.w; - - mapPose.header.stamp = mFrameTimestamp; - mapPose.header.frame_id = mMapFrameId; // frame - // conversion from Tranform to message - geometry_msgs::Transform base2map = tf2::toMsg(mMap2BaseTransf); - // Add all value in Pose message - mapPose.pose.position.x = base2map.translation.x; - mapPose.pose.position.y = base2map.translation.y; - mapPose.pose.position.z = base2map.translation.z; - mapPose.pose.orientation.x = base2map.rotation.x; - mapPose.pose.orientation.y = base2map.rotation.y; - mapPose.pose.orientation.z = base2map.rotation.z; - mapPose.pose.orientation.w = base2map.rotation.w; - - // Circular vector - if (mPathMaxCount != -1) { - if (mOdomPath.size() == mPathMaxCount) { - NODELET_DEBUG("Path vectors full: rotating "); - std::rotate(mOdomPath.begin(), mOdomPath.begin() + 1, mOdomPath.end()); - std::rotate(mMapPath.begin(), mMapPath.begin() + 1, mMapPath.end()); - - mMapPath[mPathMaxCount - 1] = mapPose; - mOdomPath[mPathMaxCount - 1] = odomPose; - } else { - // NODELET_DEBUG_STREAM("Path vectors adding last available poses"); - mMapPath.push_back(mapPose); - mOdomPath.push_back(odomPose); - } - } else { - // NODELET_DEBUG_STREAM("No limit path vectors, adding last available poses"); - mMapPath.push_back(mapPose); - mOdomPath.push_back(odomPose); + uint32_t mapPathSub = mPubMapPath.getNumSubscribers(); + uint32_t odomPathSub = mPubOdomPath.getNumSubscribers(); + + geometry_msgs::PoseStamped odomPose; + geometry_msgs::PoseStamped mapPose; + + odomPose.header.stamp = mFrameTimestamp; + odomPose.header.frame_id = mMapFrameId; // frame + // conversion from Tranform to message + geometry_msgs::Transform base2odom = tf2::toMsg(mOdom2BaseTransf); + // Add all value in Pose message + odomPose.pose.position.x = base2odom.translation.x; + odomPose.pose.position.y = base2odom.translation.y; + odomPose.pose.position.z = base2odom.translation.z; + odomPose.pose.orientation.x = base2odom.rotation.x; + odomPose.pose.orientation.y = base2odom.rotation.y; + odomPose.pose.orientation.z = base2odom.rotation.z; + odomPose.pose.orientation.w = base2odom.rotation.w; + + mapPose.header.stamp = mFrameTimestamp; + mapPose.header.frame_id = mMapFrameId; // frame + // conversion from Tranform to message + geometry_msgs::Transform base2map = tf2::toMsg(mMap2BaseTransf); + // Add all value in Pose message + mapPose.pose.position.x = base2map.translation.x; + mapPose.pose.position.y = base2map.translation.y; + mapPose.pose.position.z = base2map.translation.z; + mapPose.pose.orientation.x = base2map.rotation.x; + mapPose.pose.orientation.y = base2map.rotation.y; + mapPose.pose.orientation.z = base2map.rotation.z; + mapPose.pose.orientation.w = base2map.rotation.w; + + // Circular vector + if (mPathMaxCount != -1) + { + if (mOdomPath.size() == mPathMaxCount) + { + NODELET_DEBUG("Path vectors full: rotating "); + std::rotate(mOdomPath.begin(), mOdomPath.begin() + 1, mOdomPath.end()); + std::rotate(mMapPath.begin(), mMapPath.begin() + 1, mMapPath.end()); + + mMapPath[mPathMaxCount - 1] = mapPose; + mOdomPath[mPathMaxCount - 1] = odomPose; + } + else + { + // NODELET_DEBUG_STREAM("Path vectors adding last available poses"); + mMapPath.push_back(mapPose); + mOdomPath.push_back(odomPose); } + } + else + { + // NODELET_DEBUG_STREAM("No limit path vectors, adding last available poses"); + mMapPath.push_back(mapPose); + mOdomPath.push_back(odomPose); + } - if (mapPathSub > 0) { - nav_msgs::PathPtr mapPath = boost::make_shared(); - mapPath->header.frame_id = mMapFrameId; - mapPath->header.stamp = mFrameTimestamp; - mapPath->poses = mMapPath; + if (mapPathSub > 0) + { + nav_msgs::PathPtr mapPath = boost::make_shared(); + mapPath->header.frame_id = mMapFrameId; + mapPath->header.stamp = mFrameTimestamp; + mapPath->poses = mMapPath; - mPubMapPath.publish(mapPath); - } + mPubMapPath.publish(mapPath); + } - if (odomPathSub > 0) { - nav_msgs::PathPtr odomPath = boost::make_shared(); - odomPath->header.frame_id = mMapFrameId; - odomPath->header.stamp = mFrameTimestamp; - odomPath->poses = mOdomPath; + if (odomPathSub > 0) + { + nav_msgs::PathPtr odomPath = boost::make_shared(); + odomPath->header.frame_id = mMapFrameId; + odomPath->header.stamp = mFrameTimestamp; + odomPath->poses = mOdomPath; - mPubOdomPath.publish(odomPath); - } + mPubOdomPath.publish(odomPath); + } } void ZEDWrapperNodelet::sensors_thread_func() { - ros::Rate loop_rate(mSensPubRate); + ros::Rate loop_rate(mSensPubRate); - std::chrono::steady_clock::time_point prev_usec = std::chrono::steady_clock::now(); + std::chrono::steady_clock::time_point prev_usec = std::chrono::steady_clock::now(); - int count_warn = 0; + int count_warn = 0; - while (!mStopNode) { - std::chrono::steady_clock::time_point start_elab = std::chrono::steady_clock::now(); + while (!mStopNode) + { + std::chrono::steady_clock::time_point start_elab = std::chrono::steady_clock::now(); - mCloseZedMutex.lock(); - if (!mZed.isOpened()) { - mCloseZedMutex.unlock(); - loop_rate.sleep(); - continue; - } + mCloseZedMutex.lock(); + if (!mZed.isOpened()) + { + mCloseZedMutex.unlock(); + loop_rate.sleep(); + continue; + } - publishSensData(); - mCloseZedMutex.unlock(); - - if (!loop_rate.sleep()) { - if (++count_warn > 10) { - NODELET_INFO_THROTTLE(1.0, "Sensors thread is not synchronized with the Sensors rate"); - NODELET_INFO_STREAM_THROTTLE(1.0, "Expected cycle time: " << loop_rate.expectedCycleTime() << " - Real cycle time: " << loop_rate.cycleTime()); - NODELET_WARN_STREAM_THROTTLE(10.0, "Sensors data publishing takes longer (" << loop_rate.cycleTime() << " sec) than requested by the Sensors rate (" << loop_rate.expectedCycleTime() << " sec). Please consider to " - "lower the 'max_pub_rate' setting or to " - "reduce the power requirements reducing " - "the resolutions."); - } + publishSensData(); + mCloseZedMutex.unlock(); - loop_rate.reset(); - } else { - count_warn = 0; - } + if (!loop_rate.sleep()) + { + if (++count_warn > 10) + { + NODELET_INFO_THROTTLE(1.0, "Sensors thread is not synchronized with the Sensors rate"); + NODELET_INFO_STREAM_THROTTLE(1.0, "Expected cycle time: " << loop_rate.expectedCycleTime() + << " - Real cycle time: " << loop_rate.cycleTime()); + NODELET_WARN_STREAM_THROTTLE(10.0, "Sensors data publishing takes longer (" + << loop_rate.cycleTime() << " sec) than requested by the Sensors rate (" + << loop_rate.expectedCycleTime() + << " sec). Please consider to " + "lower the 'max_pub_rate' setting or to " + "reduce the power requirements reducing " + "the resolutions."); + } + + loop_rate.reset(); } + else + { + count_warn = 0; + } + } - NODELET_DEBUG("Sensors thread finished"); + NODELET_DEBUG("Sensors thread finished"); } void ZEDWrapperNodelet::publishSensData(ros::Time t) { - // NODELET_INFO("publishSensData"); - - uint32_t imu_SubNumber = mPubImu.getNumSubscribers(); - uint32_t imu_RawSubNumber = mPubImuRaw.getNumSubscribers(); - uint32_t imu_TempSubNumber = 0; - uint32_t imu_MagSubNumber = 0; - uint32_t pressSubNumber = 0; - uint32_t tempLeftSubNumber = 0; - uint32_t tempRightSubNumber = 0; - - if (mZedRealCamModel != sl::MODEL::ZED_M) { - imu_TempSubNumber = mPubImuTemp.getNumSubscribers(); - } + // NODELET_INFO("publishSensData"); + + uint32_t imu_SubNumber = mPubImu.getNumSubscribers(); + uint32_t imu_RawSubNumber = mPubImuRaw.getNumSubscribers(); + uint32_t imu_TempSubNumber = 0; + uint32_t imu_MagSubNumber = 0; + uint32_t pressSubNumber = 0; + uint32_t tempLeftSubNumber = 0; + uint32_t tempRightSubNumber = 0; + + if (mZedRealCamModel != sl::MODEL::ZED_M) + { + imu_TempSubNumber = mPubImuTemp.getNumSubscribers(); + } - if (mZedRealCamModel == sl::MODEL::ZED2 || mZedRealCamModel == sl::MODEL::ZED2i) { - imu_MagSubNumber = mPubImuMag.getNumSubscribers(); - pressSubNumber = mPubPressure.getNumSubscribers(); - tempLeftSubNumber = mPubTempL.getNumSubscribers(); - tempRightSubNumber = mPubTempR.getNumSubscribers(); - } + if (mZedRealCamModel == sl::MODEL::ZED2 || mZedRealCamModel == sl::MODEL::ZED2i) + { + imu_MagSubNumber = mPubImuMag.getNumSubscribers(); + pressSubNumber = mPubPressure.getNumSubscribers(); + tempLeftSubNumber = mPubTempL.getNumSubscribers(); + tempRightSubNumber = mPubTempR.getNumSubscribers(); + } - uint32_t tot_sub = imu_SubNumber + imu_RawSubNumber + imu_TempSubNumber + imu_MagSubNumber + pressSubNumber + tempLeftSubNumber + tempRightSubNumber; + uint32_t tot_sub = imu_SubNumber + imu_RawSubNumber + imu_TempSubNumber + imu_MagSubNumber + pressSubNumber + + tempLeftSubNumber + tempRightSubNumber; - if (tot_sub > 0) { - mSensPublishing = true; - } else { - mSensPublishing = false; - } + if (tot_sub > 0) + { + mSensPublishing = true; + } + else + { + mSensPublishing = false; + } - bool sensors_data_published = false; + bool sensors_data_published = false; - ros::Time ts_imu; - ros::Time ts_baro; - ros::Time ts_mag; + ros::Time ts_imu; + ros::Time ts_baro; + ros::Time ts_mag; - static ros::Time lastTs_imu = ros::Time(); - static ros::Time lastTs_baro = ros::Time(); - static ros::Time lastT_mag = ros::Time(); + static ros::Time lastTs_imu = ros::Time(); + static ros::Time lastTs_baro = ros::Time(); + static ros::Time lastT_mag = ros::Time(); - sl::SensorsData sens_data; + sl::SensorsData sens_data; - if (mSvoMode || mSensTimestampSync) { - if (mZed.getSensorsData(sens_data, sl::TIME_REFERENCE::IMAGE) != sl::ERROR_CODE::SUCCESS) { - NODELET_DEBUG("Not retrieved sensors data in IMAGE REFERENCE TIME"); - return; - } - } else { - if (mZed.getSensorsData(sens_data, sl::TIME_REFERENCE::CURRENT) != sl::ERROR_CODE::SUCCESS) { - NODELET_DEBUG("Not retrieved sensors data in CURRENT REFERENCE TIME"); - return; - } + if (mSvoMode || mSensTimestampSync) + { + if (mZed.getSensorsData(sens_data, sl::TIME_REFERENCE::IMAGE) != sl::ERROR_CODE::SUCCESS) + { + NODELET_DEBUG("Not retrieved sensors data in IMAGE REFERENCE TIME"); + return; } - - if (t != ros::Time(0)) { - ts_imu = t; - ts_baro = t; - ts_mag = t; - } else { - ts_imu = sl_tools::slTime2Ros(sens_data.imu.timestamp); - ts_baro = sl_tools::slTime2Ros(sens_data.barometer.timestamp); - ts_mag = sl_tools::slTime2Ros(sens_data.magnetometer.timestamp); + } + else + { + if (mZed.getSensorsData(sens_data, sl::TIME_REFERENCE::CURRENT) != sl::ERROR_CODE::SUCCESS) + { + NODELET_DEBUG("Not retrieved sensors data in CURRENT REFERENCE TIME"); + return; } + } - bool new_imu_data = ts_imu != lastTs_imu; - bool new_baro_data = ts_baro != lastTs_baro; - bool new_mag_data = ts_mag != lastT_mag; + if (t != ros::Time(0)) + { + ts_imu = t; + ts_baro = t; + ts_mag = t; + } + else + { + ts_imu = sl_tools::slTime2Ros(sens_data.imu.timestamp); + ts_baro = sl_tools::slTime2Ros(sens_data.barometer.timestamp); + ts_mag = sl_tools::slTime2Ros(sens_data.magnetometer.timestamp); + } - if (!new_imu_data && !new_baro_data && !new_mag_data) { - NODELET_DEBUG("No updated sensors data"); - return; - } + bool new_imu_data = ts_imu != lastTs_imu; + bool new_baro_data = ts_baro != lastTs_baro; + bool new_mag_data = ts_mag != lastT_mag; - // ----> Publish odometry tf only if enabled and not in SVO mode - if(!mSvoMode) { - if (mPublishTf && mPosTrackingReady && new_imu_data) { - //NODELET_DEBUG("Publishing TF"); + if (!new_imu_data && !new_baro_data && !new_mag_data) + { + NODELET_DEBUG("No updated sensors data"); + return; + } - publishOdomFrame(mOdom2BaseTransf, ts_imu); // publish the base Frame in odometry frame + // ----> Publish odometry tf only if enabled and not in SVO mode + if (!mSvoMode) + { + if (mPublishTf && mPosTrackingReady && new_imu_data) + { + // NODELET_DEBUG("Publishing TF"); - if (mPublishMapTf) { - publishPoseFrame(mMap2OdomTransf, ts_imu); // publish the odometry Frame in map frame - } - } - } - // <---- Publish odometry tf only if enabled and not in SVO mode + publishOdomFrame(mOdom2BaseTransf, ts_imu); // publish the base Frame in odometry frame - if (mPublishImuTf && !mStaticImuFramePublished) { - NODELET_DEBUG("Publishing static IMU TF"); - publishStaticImuFrame(); + if (mPublishMapTf) + { + publishPoseFrame(mMap2OdomTransf, ts_imu); // publish the odometry Frame in map frame + } } + } + // <---- Publish odometry tf only if enabled and not in SVO mode - if (mZedRealCamModel == sl::MODEL::ZED2 || mZedRealCamModel == sl::MODEL::ZED2i) { - // Update temperatures for Diagnostic - sens_data.temperature.get(sl::SensorsData::TemperatureData::SENSOR_LOCATION::ONBOARD_LEFT, mTempLeft); - sens_data.temperature.get(sl::SensorsData::TemperatureData::SENSOR_LOCATION::ONBOARD_RIGHT, mTempRight); - } + if (mPublishImuTf && !mStaticImuFramePublished) + { + NODELET_DEBUG("Publishing static IMU TF"); + publishStaticImuFrame(); + } - if (imu_TempSubNumber > 0 && new_imu_data) { - lastTs_imu = ts_imu; + if (mZedRealCamModel == sl::MODEL::ZED2 || mZedRealCamModel == sl::MODEL::ZED2i) + { + // Update temperatures for Diagnostic + sens_data.temperature.get(sl::SensorsData::TemperatureData::SENSOR_LOCATION::ONBOARD_LEFT, mTempLeft); + sens_data.temperature.get(sl::SensorsData::TemperatureData::SENSOR_LOCATION::ONBOARD_RIGHT, mTempRight); + } + + if (imu_TempSubNumber > 0 && new_imu_data) + { + lastTs_imu = ts_imu; - sensor_msgs::TemperaturePtr imuTempMsg = boost::make_shared(); + sensor_msgs::TemperaturePtr imuTempMsg = boost::make_shared(); - imuTempMsg->header.stamp = ts_imu; + imuTempMsg->header.stamp = ts_imu; #ifdef DEBUG_SENS_TS - static ros::Time old_ts; - if (old_ts == imuTempMsg->header.stamp) { - NODELET_WARN_STREAM("Publishing IMU data with old timestamp " << old_ts); - } - old_ts = imuTempMsg->header.stamp; + static ros::Time old_ts; + if (old_ts == imuTempMsg->header.stamp) + { + NODELET_WARN_STREAM("Publishing IMU data with old timestamp " << old_ts); + } + old_ts = imuTempMsg->header.stamp; #endif - imuTempMsg->header.frame_id = mImuFrameId; - float imu_temp; - sens_data.temperature.get(sl::SensorsData::TemperatureData::SENSOR_LOCATION::IMU, imu_temp); - imuTempMsg->temperature = static_cast(imu_temp); - imuTempMsg->variance = 0.0; + imuTempMsg->header.frame_id = mImuFrameId; + float imu_temp; + sens_data.temperature.get(sl::SensorsData::TemperatureData::SENSOR_LOCATION::IMU, imu_temp); + imuTempMsg->temperature = static_cast(imu_temp); + imuTempMsg->variance = 0.0; - sensors_data_published = true; - mPubImuTemp.publish(imuTempMsg); - } /*else { - NODELET_DEBUG("No new IMU temp."); - }*/ + sensors_data_published = true; + mPubImuTemp.publish(imuTempMsg); + } /*else { + NODELET_DEBUG("No new IMU temp."); + }*/ - if (sens_data.barometer.is_available && new_baro_data) { - lastTs_baro = ts_baro; + if (sens_data.barometer.is_available && new_baro_data) + { + lastTs_baro = ts_baro; - if (pressSubNumber > 0) { - sensor_msgs::FluidPressurePtr pressMsg = boost::make_shared(); + if (pressSubNumber > 0) + { + sensor_msgs::FluidPressurePtr pressMsg = boost::make_shared(); - pressMsg->header.stamp = ts_baro; + pressMsg->header.stamp = ts_baro; #ifdef DEBUG_SENS_TS - static ros::Time old_ts; - if (old_ts == pressMsg->header.stamp) { - NODELET_WARN_STREAM("Publishing BARO data with old timestamp " << old_ts); - } - old_ts = pressMsg->header.stamp; + static ros::Time old_ts; + if (old_ts == pressMsg->header.stamp) + { + NODELET_WARN_STREAM("Publishing BARO data with old timestamp " << old_ts); + } + old_ts = pressMsg->header.stamp; #endif - pressMsg->header.frame_id = mBaroFrameId; - pressMsg->fluid_pressure = sens_data.barometer.pressure; // Pascal - pressMsg->variance = 1.0585e-2; + pressMsg->header.frame_id = mBaroFrameId; + pressMsg->fluid_pressure = sens_data.barometer.pressure; // Pascal + pressMsg->variance = 1.0585e-2; - sensors_data_published = true; - mPubPressure.publish(pressMsg); - } + sensors_data_published = true; + mPubPressure.publish(pressMsg); + } - if (tempLeftSubNumber > 0) { - sensor_msgs::TemperaturePtr tempLeftMsg = boost::make_shared(); + if (tempLeftSubNumber > 0) + { + sensor_msgs::TemperaturePtr tempLeftMsg = boost::make_shared(); - tempLeftMsg->header.stamp = ts_baro; + tempLeftMsg->header.stamp = ts_baro; #ifdef DEBUG_SENS_TS - static ros::Time old_ts; - if (old_ts == tempLeftMsg->header.stamp) { - NODELET_WARN_STREAM("Publishing BARO data with old timestamp " << old_ts); - } - old_ts = tempLeftMsg->header.stamp; + static ros::Time old_ts; + if (old_ts == tempLeftMsg->header.stamp) + { + NODELET_WARN_STREAM("Publishing BARO data with old timestamp " << old_ts); + } + old_ts = tempLeftMsg->header.stamp; #endif - tempLeftMsg->header.frame_id = mTempLeftFrameId; - tempLeftMsg->temperature = static_cast(mTempLeft); - tempLeftMsg->variance = 0.0; + tempLeftMsg->header.frame_id = mTempLeftFrameId; + tempLeftMsg->temperature = static_cast(mTempLeft); + tempLeftMsg->variance = 0.0; - sensors_data_published = true; - mPubTempL.publish(tempLeftMsg); - } + sensors_data_published = true; + mPubTempL.publish(tempLeftMsg); + } - if (tempRightSubNumber > 0) { - sensor_msgs::TemperaturePtr tempRightMsg = boost::make_shared(); + if (tempRightSubNumber > 0) + { + sensor_msgs::TemperaturePtr tempRightMsg = boost::make_shared(); - tempRightMsg->header.stamp = ts_baro; + tempRightMsg->header.stamp = ts_baro; #ifdef DEBUG_SENS_TS - static ros::Time old_ts; - if (old_ts == tempRightMsg->header.stamp) { - NODELET_WARN_STREAM("Publishing BARO data with old timestamp " << old_ts); - } - old_ts = tempRightMsg->header.stamp; + static ros::Time old_ts; + if (old_ts == tempRightMsg->header.stamp) + { + NODELET_WARN_STREAM("Publishing BARO data with old timestamp " << old_ts); + } + old_ts = tempRightMsg->header.stamp; #endif - tempRightMsg->header.frame_id = mTempRightFrameId; - tempRightMsg->temperature = static_cast(mTempRight); - tempRightMsg->variance = 0.0; + tempRightMsg->header.frame_id = mTempRightFrameId; + tempRightMsg->temperature = static_cast(mTempRight); + tempRightMsg->variance = 0.0; - sensors_data_published = true; - mPubTempR.publish(tempRightMsg); - } - } /*else { - NODELET_DEBUG("No new BAROM. DATA"); - }*/ + sensors_data_published = true; + mPubTempR.publish(tempRightMsg); + } + } /*else { + NODELET_DEBUG("No new BAROM. DATA"); + }*/ - if (imu_MagSubNumber > 0) { - if (sens_data.magnetometer.is_available && new_mag_data) { - lastT_mag = ts_mag; + if (imu_MagSubNumber > 0) + { + if (sens_data.magnetometer.is_available && new_mag_data) + { + lastT_mag = ts_mag; - sensor_msgs::MagneticFieldPtr magMsg = boost::make_shared(); + sensor_msgs::MagneticFieldPtr magMsg = boost::make_shared(); - magMsg->header.stamp = ts_mag; + magMsg->header.stamp = ts_mag; #ifdef DEBUG_SENS_TS - static ros::Time old_ts; - if (old_ts == magMsg->header.stamp) { - NODELET_WARN_STREAM("Publishing MAG data with old timestamp " << old_ts); - } - old_ts = magMsg->header.stamp; + static ros::Time old_ts; + if (old_ts == magMsg->header.stamp) + { + NODELET_WARN_STREAM("Publishing MAG data with old timestamp " << old_ts); + } + old_ts = magMsg->header.stamp; #endif - magMsg->header.frame_id = mMagFrameId; - magMsg->magnetic_field.x = sens_data.magnetometer.magnetic_field_calibrated.x * 1e-6; // Tesla - magMsg->magnetic_field.y = sens_data.magnetometer.magnetic_field_calibrated.y * 1e-6; // Tesla - magMsg->magnetic_field.z = sens_data.magnetometer.magnetic_field_calibrated.z * 1e-6; // Tesla - magMsg->magnetic_field_covariance[0] = 0.039e-6; - magMsg->magnetic_field_covariance[1] = 0.0f; - magMsg->magnetic_field_covariance[2] = 0.0f; - magMsg->magnetic_field_covariance[3] = 0.0f; - magMsg->magnetic_field_covariance[4] = 0.037e-6; - magMsg->magnetic_field_covariance[5] = 0.0f; - magMsg->magnetic_field_covariance[6] = 0.0f; - magMsg->magnetic_field_covariance[7] = 0.0f; - magMsg->magnetic_field_covariance[8] = 0.047e-6; - - sensors_data_published = true; - mPubImuMag.publish(magMsg); - } - } /*else { - NODELET_DEBUG("No new MAG. DATA"); - }*/ + magMsg->header.frame_id = mMagFrameId; + magMsg->magnetic_field.x = sens_data.magnetometer.magnetic_field_calibrated.x * 1e-6; // Tesla + magMsg->magnetic_field.y = sens_data.magnetometer.magnetic_field_calibrated.y * 1e-6; // Tesla + magMsg->magnetic_field.z = sens_data.magnetometer.magnetic_field_calibrated.z * 1e-6; // Tesla + magMsg->magnetic_field_covariance[0] = 0.039e-6; + magMsg->magnetic_field_covariance[1] = 0.0f; + magMsg->magnetic_field_covariance[2] = 0.0f; + magMsg->magnetic_field_covariance[3] = 0.0f; + magMsg->magnetic_field_covariance[4] = 0.037e-6; + magMsg->magnetic_field_covariance[5] = 0.0f; + magMsg->magnetic_field_covariance[6] = 0.0f; + magMsg->magnetic_field_covariance[7] = 0.0f; + magMsg->magnetic_field_covariance[8] = 0.047e-6; + + sensors_data_published = true; + mPubImuMag.publish(magMsg); + } + } /*else { + NODELET_DEBUG("No new MAG. DATA"); + }*/ - if (imu_SubNumber > 0 && new_imu_data) { - lastTs_imu = ts_imu; + if (imu_SubNumber > 0 && new_imu_data) + { + lastTs_imu = ts_imu; - sensor_msgs::ImuPtr imuMsg = boost::make_shared(); + sensor_msgs::ImuPtr imuMsg = boost::make_shared(); - imuMsg->header.stamp = ts_imu; + imuMsg->header.stamp = ts_imu; #ifdef DEBUG_SENS_TS - static ros::Time old_ts; - if (old_ts == imuMsg->header.stamp) { - NODELET_WARN_STREAM("Publishing IMU data with old timestamp " << old_ts); - } else { - NODELET_INFO_STREAM("Publishing IMU data with new timestamp. Freq: " << 1. / (ts_imu.toSec() - old_ts.toSec())); - old_ts = imuMsg->header.stamp; - } + static ros::Time old_ts; + if (old_ts == imuMsg->header.stamp) + { + NODELET_WARN_STREAM("Publishing IMU data with old timestamp " << old_ts); + } + else + { + NODELET_INFO_STREAM("Publishing IMU data with new timestamp. Freq: " << 1. / (ts_imu.toSec() - old_ts.toSec())); + old_ts = imuMsg->header.stamp; + } #endif - imuMsg->header.frame_id = mImuFrameId; + imuMsg->header.frame_id = mImuFrameId; - imuMsg->orientation.x = sens_data.imu.pose.getOrientation()[0]; - imuMsg->orientation.y = sens_data.imu.pose.getOrientation()[1]; - imuMsg->orientation.z = sens_data.imu.pose.getOrientation()[2]; - imuMsg->orientation.w = sens_data.imu.pose.getOrientation()[3]; + imuMsg->orientation.x = sens_data.imu.pose.getOrientation()[0]; + imuMsg->orientation.y = sens_data.imu.pose.getOrientation()[1]; + imuMsg->orientation.z = sens_data.imu.pose.getOrientation()[2]; + imuMsg->orientation.w = sens_data.imu.pose.getOrientation()[3]; - imuMsg->angular_velocity.x = sens_data.imu.angular_velocity[0] * DEG2RAD; - imuMsg->angular_velocity.y = sens_data.imu.angular_velocity[1] * DEG2RAD; - imuMsg->angular_velocity.z = sens_data.imu.angular_velocity[2] * DEG2RAD; + imuMsg->angular_velocity.x = sens_data.imu.angular_velocity[0] * DEG2RAD; + imuMsg->angular_velocity.y = sens_data.imu.angular_velocity[1] * DEG2RAD; + imuMsg->angular_velocity.z = sens_data.imu.angular_velocity[2] * DEG2RAD; - imuMsg->linear_acceleration.x = sens_data.imu.linear_acceleration[0]; - imuMsg->linear_acceleration.y = sens_data.imu.linear_acceleration[1]; - imuMsg->linear_acceleration.z = sens_data.imu.linear_acceleration[2]; + imuMsg->linear_acceleration.x = sens_data.imu.linear_acceleration[0]; + imuMsg->linear_acceleration.y = sens_data.imu.linear_acceleration[1]; + imuMsg->linear_acceleration.z = sens_data.imu.linear_acceleration[2]; - for (int i = 0; i < 3; ++i) { - int r = 0; + for (int i = 0; i < 3; ++i) + { + int r = 0; - if (i == 0) { - r = 0; - } else if (i == 1) { - r = 1; - } else { - r = 2; - } + if (i == 0) + { + r = 0; + } + else if (i == 1) + { + r = 1; + } + else + { + r = 2; + } - imuMsg->orientation_covariance[i * 3 + 0] = sens_data.imu.pose_covariance.r[r * 3 + 0] * DEG2RAD * DEG2RAD; - imuMsg->orientation_covariance[i * 3 + 1] = sens_data.imu.pose_covariance.r[r * 3 + 1] * DEG2RAD * DEG2RAD; - imuMsg->orientation_covariance[i * 3 + 2] = sens_data.imu.pose_covariance.r[r * 3 + 2] * DEG2RAD * DEG2RAD; + imuMsg->orientation_covariance[i * 3 + 0] = sens_data.imu.pose_covariance.r[r * 3 + 0] * DEG2RAD * DEG2RAD; + imuMsg->orientation_covariance[i * 3 + 1] = sens_data.imu.pose_covariance.r[r * 3 + 1] * DEG2RAD * DEG2RAD; + imuMsg->orientation_covariance[i * 3 + 2] = sens_data.imu.pose_covariance.r[r * 3 + 2] * DEG2RAD * DEG2RAD; - imuMsg->linear_acceleration_covariance[i * 3 + 0] = sens_data.imu.linear_acceleration_covariance.r[r * 3 + 0]; - imuMsg->linear_acceleration_covariance[i * 3 + 1] = sens_data.imu.linear_acceleration_covariance.r[r * 3 + 1]; - imuMsg->linear_acceleration_covariance[i * 3 + 2] = sens_data.imu.linear_acceleration_covariance.r[r * 3 + 2]; + imuMsg->linear_acceleration_covariance[i * 3 + 0] = sens_data.imu.linear_acceleration_covariance.r[r * 3 + 0]; + imuMsg->linear_acceleration_covariance[i * 3 + 1] = sens_data.imu.linear_acceleration_covariance.r[r * 3 + 1]; + imuMsg->linear_acceleration_covariance[i * 3 + 2] = sens_data.imu.linear_acceleration_covariance.r[r * 3 + 2]; - imuMsg->angular_velocity_covariance[i * 3 + 0] = sens_data.imu.angular_velocity_covariance.r[r * 3 + 0] * DEG2RAD * DEG2RAD; - imuMsg->angular_velocity_covariance[i * 3 + 1] = sens_data.imu.angular_velocity_covariance.r[r * 3 + 1] * DEG2RAD * DEG2RAD; - imuMsg->angular_velocity_covariance[i * 3 + 2] = sens_data.imu.angular_velocity_covariance.r[r * 3 + 2] * DEG2RAD * DEG2RAD; - } + imuMsg->angular_velocity_covariance[i * 3 + 0] = + sens_data.imu.angular_velocity_covariance.r[r * 3 + 0] * DEG2RAD * DEG2RAD; + imuMsg->angular_velocity_covariance[i * 3 + 1] = + sens_data.imu.angular_velocity_covariance.r[r * 3 + 1] * DEG2RAD * DEG2RAD; + imuMsg->angular_velocity_covariance[i * 3 + 2] = + sens_data.imu.angular_velocity_covariance.r[r * 3 + 2] * DEG2RAD * DEG2RAD; + } - sensors_data_published = true; - mPubImu.publish(imuMsg); - } /*else { - NODELET_DEBUG("No new IMU DATA"); - }*/ - - if (imu_RawSubNumber > 0 && new_imu_data) { - lastTs_imu = ts_imu; - - sensor_msgs::ImuPtr imuRawMsg = boost::make_shared(); - - imuRawMsg->header.stamp = ts_imu; - imuRawMsg->header.frame_id = mImuFrameId; - imuRawMsg->angular_velocity.x = sens_data.imu.angular_velocity[0] * DEG2RAD; - imuRawMsg->angular_velocity.y = sens_data.imu.angular_velocity[1] * DEG2RAD; - imuRawMsg->angular_velocity.z = sens_data.imu.angular_velocity[2] * DEG2RAD; - imuRawMsg->linear_acceleration.x = sens_data.imu.linear_acceleration[0]; - imuRawMsg->linear_acceleration.y = sens_data.imu.linear_acceleration[1]; - imuRawMsg->linear_acceleration.z = sens_data.imu.linear_acceleration[2]; - - for (int i = 0; i < 3; ++i) { - int r = 0; - - if (i == 0) { - r = 0; - } else if (i == 1) { - r = 1; - } else { - r = 2; - } + sensors_data_published = true; + mPubImu.publish(imuMsg); + } /*else { + NODELET_DEBUG("No new IMU DATA"); + }*/ - imuRawMsg->linear_acceleration_covariance[i * 3 + 0] = sens_data.imu.linear_acceleration_covariance.r[r * 3 + 0]; - imuRawMsg->linear_acceleration_covariance[i * 3 + 1] = sens_data.imu.linear_acceleration_covariance.r[r * 3 + 1]; - imuRawMsg->linear_acceleration_covariance[i * 3 + 2] = sens_data.imu.linear_acceleration_covariance.r[r * 3 + 2]; - imuRawMsg->angular_velocity_covariance[i * 3 + 0] = sens_data.imu.angular_velocity_covariance.r[r * 3 + 0] * DEG2RAD * DEG2RAD; - imuRawMsg->angular_velocity_covariance[i * 3 + 1] = sens_data.imu.angular_velocity_covariance.r[r * 3 + 1] * DEG2RAD * DEG2RAD; - imuRawMsg->angular_velocity_covariance[i * 3 + 2] = sens_data.imu.angular_velocity_covariance.r[r * 3 + 2] * DEG2RAD * DEG2RAD; - } + if (imu_RawSubNumber > 0 && new_imu_data) + { + lastTs_imu = ts_imu; - // Orientation data is not available in "data_raw" -> See ROS REP145 - // http://www.ros.org/reps/rep-0145.html#topics - imuRawMsg->orientation_covariance[0] = -1; - sensors_data_published = true; - mPubImuRaw.publish(imuRawMsg); - } + sensor_msgs::ImuPtr imuRawMsg = boost::make_shared(); + + imuRawMsg->header.stamp = ts_imu; + imuRawMsg->header.frame_id = mImuFrameId; + imuRawMsg->angular_velocity.x = sens_data.imu.angular_velocity[0] * DEG2RAD; + imuRawMsg->angular_velocity.y = sens_data.imu.angular_velocity[1] * DEG2RAD; + imuRawMsg->angular_velocity.z = sens_data.imu.angular_velocity[2] * DEG2RAD; + imuRawMsg->linear_acceleration.x = sens_data.imu.linear_acceleration[0]; + imuRawMsg->linear_acceleration.y = sens_data.imu.linear_acceleration[1]; + imuRawMsg->linear_acceleration.z = sens_data.imu.linear_acceleration[2]; - // ----> Update Diagnostic - if (sensors_data_published) { - // Publish freq calculation - static std::chrono::steady_clock::time_point last_time = std::chrono::steady_clock::now(); - std::chrono::steady_clock::time_point now = std::chrono::steady_clock::now(); + for (int i = 0; i < 3; ++i) + { + int r = 0; - double elapsed_usec = std::chrono::duration_cast(now - last_time).count(); - last_time = now; + if (i == 0) + { + r = 0; + } + else if (i == 1) + { + r = 1; + } + else + { + r = 2; + } - mSensPeriodMean_usec->addValue(elapsed_usec); - } - // <---- Update Diagnostic + imuRawMsg->linear_acceleration_covariance[i * 3 + 0] = sens_data.imu.linear_acceleration_covariance.r[r * 3 + 0]; + imuRawMsg->linear_acceleration_covariance[i * 3 + 1] = sens_data.imu.linear_acceleration_covariance.r[r * 3 + 1]; + imuRawMsg->linear_acceleration_covariance[i * 3 + 2] = sens_data.imu.linear_acceleration_covariance.r[r * 3 + 2]; + imuRawMsg->angular_velocity_covariance[i * 3 + 0] = + sens_data.imu.angular_velocity_covariance.r[r * 3 + 0] * DEG2RAD * DEG2RAD; + imuRawMsg->angular_velocity_covariance[i * 3 + 1] = + sens_data.imu.angular_velocity_covariance.r[r * 3 + 1] * DEG2RAD * DEG2RAD; + imuRawMsg->angular_velocity_covariance[i * 3 + 2] = + sens_data.imu.angular_velocity_covariance.r[r * 3 + 2] * DEG2RAD * DEG2RAD; + } + + // Orientation data is not available in "data_raw" -> See ROS REP145 + // http://www.ros.org/reps/rep-0145.html#topics + imuRawMsg->orientation_covariance[0] = -1; + sensors_data_published = true; + mPubImuRaw.publish(imuRawMsg); + } + + // ----> Update Diagnostic + if (sensors_data_published) + { + // Publish freq calculation + static std::chrono::steady_clock::time_point last_time = std::chrono::steady_clock::now(); + std::chrono::steady_clock::time_point now = std::chrono::steady_clock::now(); + + double elapsed_usec = std::chrono::duration_cast(now - last_time).count(); + last_time = now; + + mSensPeriodMean_usec->addValue(elapsed_usec); + } + // <---- Update Diagnostic } void ZEDWrapperNodelet::device_poll_thread_func() { - ros::Rate loop_rate(mPubFrameRate); + ros::Rate loop_rate(mPubFrameRate); - mRecording = false; + mRecording = false; - mElabPeriodMean_sec.reset(new sl_tools::CSmartMean(mCamFrameRate)); - mGrabPeriodMean_usec.reset(new sl_tools::CSmartMean(mCamFrameRate)); - mVideoDepthPeriodMean_sec.reset(new sl_tools::CSmartMean(mCamFrameRate)); - mPcPeriodMean_usec.reset(new sl_tools::CSmartMean(mCamFrameRate)); - mObjDetPeriodMean_msec.reset(new sl_tools::CSmartMean(mCamFrameRate)); + mElabPeriodMean_sec.reset(new sl_tools::CSmartMean(mCamFrameRate)); + mGrabPeriodMean_usec.reset(new sl_tools::CSmartMean(mCamFrameRate)); + mVideoDepthPeriodMean_sec.reset(new sl_tools::CSmartMean(mCamFrameRate)); + mPcPeriodMean_usec.reset(new sl_tools::CSmartMean(mCamFrameRate)); + mObjDetPeriodMean_msec.reset(new sl_tools::CSmartMean(mCamFrameRate)); - // Timestamp initialization - if (mSvoMode) { - mFrameTimestamp = ros::Time::now(); - } else { - mFrameTimestamp = sl_tools::slTime2Ros(mZed.getTimestamp(sl::TIME_REFERENCE::CURRENT)); + // Timestamp initialization + if (mSvoMode) + { + mFrameTimestamp = ros::Time::now(); + } + else + { + mFrameTimestamp = sl_tools::slTime2Ros(mZed.getTimestamp(sl::TIME_REFERENCE::CURRENT)); + } + mPrevFrameTimestamp = mFrameTimestamp; + + mPosTrackingActivated = false; + mMappingRunning = false; + mRecording = false; + + // Get the parameters of the ZED images + mCamWidth = mZed.getCameraInformation().camera_configuration.resolution.width; + mCamHeight = mZed.getCameraInformation().camera_configuration.resolution.height; + NODELET_DEBUG_STREAM("Original Camera grab frame size: " << mCamWidth << "x" << mCamHeight); + int pub_w, pub_h; + pub_w = static_cast(std::round(mCamWidth / mCustomDownscaleFactor)); + pub_h = static_cast(std::round(mCamHeight / mCustomDownscaleFactor)); + + if (pub_w > mCamWidth || pub_h > mCamHeight) + { + NODELET_WARN_STREAM("The publishing resolution (" + << pub_w << "x" << pub_h << ") cannot be higher than the grabbing resolution (" << mCamWidth + << "x" << mCamHeight << "). Using grab resolution for output messages."); + pub_w = mCamWidth; + pub_h = mCamHeight; + } + mMatResol = sl::Resolution(pub_w, pub_h); + NODELET_DEBUG_STREAM("Publishing frame size: " << mMatResol.width << "x" << mMatResol.height); + + // ----> Set Region of Interest + if (!mRoiParam.empty()) + { + NODELET_INFO("*** Setting ROI ***"); + sl::Resolution resol(mCamWidth, mCamHeight); + std::vector sl_poly; + std::string log_msg = parseRoiPoly(mRoiParam, sl_poly); + + // Create ROI mask + sl::Mat roi_mask(resol, sl::MAT_TYPE::U8_C1, sl::MEM::CPU); + + if (!sl_tools::generateROI(sl_poly, roi_mask)) + { + NODELET_WARN(" * Error generating the region of interest image mask."); } - mPrevFrameTimestamp = mFrameTimestamp; + else + { + sl::ERROR_CODE err = mZed.setRegionOfInterest(roi_mask); + if (err != sl::ERROR_CODE::SUCCESS) + { + NODELET_WARN_STREAM(" * Error while setting ZED SDK region of interest: " << sl::toString(err).c_str()); + } + else + { + NODELET_INFO(" * Region of Interest correctly set."); + } + } + } + // <---- Set Region of Interest - mPosTrackingActivated = false; - mMappingRunning = false; - mRecording = false; + // Create and fill the camera information messages + fillCamInfo(mZed, mLeftCamInfoMsg, mRightCamInfoMsg, mLeftCamOptFrameId, mRightCamOptFrameId); + fillCamInfo(mZed, mLeftCamInfoRawMsg, mRightCamInfoRawMsg, mLeftCamOptFrameId, mRightCamOptFrameId, true); + fillCamDepthInfo(mZed, mDepthCamInfoMsg, mLeftCamOptFrameId); + + // the reference camera is the Left one (next to the ZED logo) + + mRgbCamInfoMsg = mLeftCamInfoMsg; + mRgbCamInfoRawMsg = mLeftCamInfoRawMsg; - // Get the parameters of the ZED images - mCamWidth = mZed.getCameraInformation().camera_configuration.resolution.width; - mCamHeight = mZed.getCameraInformation().camera_configuration.resolution.height; - NODELET_DEBUG_STREAM("Original Camera grab frame size: " << mCamWidth << "x" << mCamHeight); - int pub_w, pub_h; - pub_w = static_cast(std::round(mCamWidth / mCustomDownscaleFactor)); - pub_h = static_cast(std::round(mCamHeight / mCustomDownscaleFactor)); - - if (pub_w > mCamWidth || pub_h > mCamHeight) { - NODELET_WARN_STREAM("The publishing resolution (" << - pub_w << "x" << pub_h << - ") cannot be higher than the grabbing resolution (" << - mCamWidth << "x" << mCamHeight << - "). Using grab resolution for output messages."); - pub_w = mCamWidth; - pub_h = mCamHeight; + sl::RuntimeParameters runParams; + + // Main loop + while (mNhNs.ok()) + { + // Check for subscribers + uint32_t rgbSubnumber = mPubRgb.getNumSubscribers(); + uint32_t rgbRawSubnumber = mPubRawRgb.getNumSubscribers(); + uint32_t leftSubnumber = mPubLeft.getNumSubscribers(); + uint32_t leftRawSubnumber = mPubRawLeft.getNumSubscribers(); + uint32_t rightSubnumber = mPubRight.getNumSubscribers(); + uint32_t rightRawSubnumber = mPubRawRight.getNumSubscribers(); + uint32_t rgbGraySubnumber = mPubRgbGray.getNumSubscribers(); + uint32_t rgbGrayRawSubnumber = mPubRawRgbGray.getNumSubscribers(); + uint32_t leftGraySubnumber = mPubLeftGray.getNumSubscribers(); + uint32_t leftGrayRawSubnumber = mPubRawLeftGray.getNumSubscribers(); + uint32_t rightGraySubnumber = mPubRightGray.getNumSubscribers(); + uint32_t rightGrayRawSubnumber = mPubRawRightGray.getNumSubscribers(); + uint32_t depthSubnumber = 0; + uint32_t objDetSubnumber = 0; + uint32_t disparitySubnumber = 0; + uint32_t cloudSubnumber = 0; + uint32_t fusedCloudSubnumber = 0; + uint32_t poseSubnumber = 0; + uint32_t poseCovSubnumber = 0; + uint32_t odomSubnumber = 0; + uint32_t confMapSubnumber = 0; + uint32_t pathSubNumber = 0; + if (!mDepthDisabled) + { + depthSubnumber = mPubDepth.getNumSubscribers(); + disparitySubnumber = mPubDisparity.getNumSubscribers(); + cloudSubnumber = mPubCloud.getNumSubscribers(); + fusedCloudSubnumber = mPubFusedCloud.getNumSubscribers(); + poseSubnumber = mPubPose.getNumSubscribers(); + poseCovSubnumber = mPubPoseCov.getNumSubscribers(); + odomSubnumber = mPubOdom.getNumSubscribers(); + confMapSubnumber = mPubConfMap.getNumSubscribers(); + pathSubNumber = mPubMapPath.getNumSubscribers() + mPubOdomPath.getNumSubscribers(); + + if (mObjDetEnabled && mObjDetRunning) + { + objDetSubnumber = mPubObjDet.getNumSubscribers(); + } } - mMatResol = sl::Resolution(pub_w, pub_h); - NODELET_DEBUG_STREAM("Publishing frame size: " << mMatResol.width << "x" << mMatResol.height); - - // ----> Set Region of Interest - if (!mRoiParam.empty()) { - NODELET_INFO("*** Setting ROI ***"); - sl::Resolution resol(mCamWidth, mCamHeight); - std::vector sl_poly; - std::string log_msg = parseRoiPoly(mRoiParam, sl_poly); - - // Create ROI mask - sl::Mat roi_mask(resol, sl::MAT_TYPE::U8_C1, sl::MEM::CPU); - - if (!sl_tools::generateROI(sl_poly, roi_mask)) { - NODELET_WARN(" * Error generating the region of interest image mask."); - } else { - sl::ERROR_CODE err = mZed.setRegionOfInterest(roi_mask); - if (err != sl::ERROR_CODE::SUCCESS) { - NODELET_WARN_STREAM(" * Error while setting ZED SDK region of interest: " << sl::toString(err).c_str()); - } else { - NODELET_INFO(" * Region of Interest correctly set."); + uint32_t stereoSubNumber = mPubStereo.getNumSubscribers(); + uint32_t stereoRawSubNumber = mPubRawStereo.getNumSubscribers(); + + mGrabActive = + mRecording || mStreaming || mMappingEnabled || mObjDetEnabled || mPosTrackingEnabled || mPosTrackingActivated || + ((rgbSubnumber + rgbRawSubnumber + leftSubnumber + leftRawSubnumber + rightSubnumber + rightRawSubnumber + + rgbGraySubnumber + rgbGrayRawSubnumber + leftGraySubnumber + leftGrayRawSubnumber + rightGraySubnumber + + rightGrayRawSubnumber + depthSubnumber + disparitySubnumber + cloudSubnumber + poseSubnumber + + poseCovSubnumber + odomSubnumber + confMapSubnumber /*+ imuSubnumber + imuRawsubnumber*/ + pathSubNumber + + stereoSubNumber + stereoRawSubNumber + objDetSubnumber) > 0); + + // Run the loop only if there is some subscribers or SVO is active + if (mGrabActive) + { + std::lock_guard lock(mPosTrkMutex); + + // Note: once tracking is started it is never stopped anymore to not lose tracking information + bool computeTracking = + !mDepthDisabled && (mPosTrackingEnabled || mPosTrackingActivated || mMappingEnabled || mObjDetEnabled || + (mComputeDepth & mDepthStabilization) || poseSubnumber > 0 || poseCovSubnumber > 0 || + odomSubnumber > 0 || pathSubNumber > 0); + + // Start the tracking? + if ((computeTracking) && !mPosTrackingActivated && !mDepthDisabled) + { + start_pos_tracking(); + } + + // Start the mapping? + mMappingMutex.lock(); + if (mMappingEnabled && !mMappingRunning) + { + start_3d_mapping(); + } + mMappingMutex.unlock(); + + // Start the object detection? + mObjDetMutex.lock(); + if (mObjDetEnabled && !mObjDetRunning) + { + start_obj_detect(); + } + mObjDetMutex.unlock(); + + // Detect if one of the subscriber need to have the depth information + mComputeDepth = !mDepthDisabled && + (computeTracking || + ((depthSubnumber + disparitySubnumber + cloudSubnumber + fusedCloudSubnumber + poseSubnumber + + poseCovSubnumber + odomSubnumber + confMapSubnumber + objDetSubnumber) > 0)); + + if (mComputeDepth) + { + runParams.confidence_threshold = mCamDepthConfidence; + runParams.texture_confidence_threshold = mCamDepthTextureConf; + runParams.enable_depth = true; // Ask to compute the depth + } + else + { + runParams.enable_depth = false; // Ask to not compute the depth + } + + std::chrono::steady_clock::time_point start_elab = std::chrono::steady_clock::now(); + + // ZED Grab + mGrabStatus = mZed.grab(runParams); + + // cout << toString(grab_status) << endl; + if (mGrabStatus != sl::ERROR_CODE::SUCCESS) + { + // Detect if a error occurred (for example: + // the zed have been disconnected) and + // re-initialize the ZED + + NODELET_INFO_STREAM_THROTTLE(1.0, "Camera grab error: " << sl::toString(mGrabStatus).c_str()); + + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + if (mGrabStatus != sl::ERROR_CODE::CAMERA_REBOOTING) + { + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + continue; } + else if (mSvoMode && mGrabStatus == sl::ERROR_CODE::END_OF_SVOFILE_REACHED) + { + NODELET_WARN("SVO reached the end. The node will be stopped."); + mZed.close(); + exit(EXIT_SUCCESS); } - } - // <---- Set Region of Interest - - // Create and fill the camera information messages - fillCamInfo(mZed, mLeftCamInfoMsg, mRightCamInfoMsg, mLeftCamOptFrameId, mRightCamOptFrameId); - fillCamInfo(mZed, mLeftCamInfoRawMsg, mRightCamInfoRawMsg, mLeftCamOptFrameId, mRightCamOptFrameId, true); - fillCamDepthInfo(mZed, mDepthCamInfoMsg, mLeftCamOptFrameId); - - // the reference camera is the Left one (next to the ZED logo) - - mRgbCamInfoMsg = mLeftCamInfoMsg; - mRgbCamInfoRawMsg = mLeftCamInfoRawMsg; - - sl::RuntimeParameters runParams; - - // Main loop - while (mNhNs.ok()) { - // Check for subscribers - uint32_t rgbSubnumber = mPubRgb.getNumSubscribers(); - uint32_t rgbRawSubnumber = mPubRawRgb.getNumSubscribers(); - uint32_t leftSubnumber = mPubLeft.getNumSubscribers(); - uint32_t leftRawSubnumber = mPubRawLeft.getNumSubscribers(); - uint32_t rightSubnumber = mPubRight.getNumSubscribers(); - uint32_t rightRawSubnumber = mPubRawRight.getNumSubscribers(); - uint32_t rgbGraySubnumber = mPubRgbGray.getNumSubscribers(); - uint32_t rgbGrayRawSubnumber = mPubRawRgbGray.getNumSubscribers(); - uint32_t leftGraySubnumber = mPubLeftGray.getNumSubscribers(); - uint32_t leftGrayRawSubnumber = mPubRawLeftGray.getNumSubscribers(); - uint32_t rightGraySubnumber = mPubRightGray.getNumSubscribers(); - uint32_t rightGrayRawSubnumber = mPubRawRightGray.getNumSubscribers(); - uint32_t depthSubnumber = 0; - uint32_t objDetSubnumber = 0; - uint32_t disparitySubnumber = 0; - uint32_t cloudSubnumber = 0; - uint32_t fusedCloudSubnumber = 0; - uint32_t poseSubnumber = 0; - uint32_t poseCovSubnumber = 0; - uint32_t odomSubnumber = 0; - uint32_t confMapSubnumber = 0; - uint32_t pathSubNumber = 0; - if(!mDepthDisabled) { - depthSubnumber = mPubDepth.getNumSubscribers(); - disparitySubnumber = mPubDisparity.getNumSubscribers(); - cloudSubnumber = mPubCloud.getNumSubscribers(); - fusedCloudSubnumber = mPubFusedCloud.getNumSubscribers(); - poseSubnumber = mPubPose.getNumSubscribers(); - poseCovSubnumber = mPubPoseCov.getNumSubscribers(); - odomSubnumber = mPubOdom.getNumSubscribers(); - confMapSubnumber = mPubConfMap.getNumSubscribers(); - pathSubNumber = mPubMapPath.getNumSubscribers() + mPubOdomPath.getNumSubscribers(); - - if (mObjDetEnabled && mObjDetRunning) { - objDetSubnumber = mPubObjDet.getNumSubscribers(); - } + else if (!mRemoteStreamAddr.empty()) + { + NODELET_ERROR("Remote stream problem. The node will be stopped."); + mZed.close(); + exit(EXIT_FAILURE); } - uint32_t stereoSubNumber = mPubStereo.getNumSubscribers(); - uint32_t stereoRawSubNumber = mPubRawStereo.getNumSubscribers(); - - mGrabActive = mRecording || mStreaming || mMappingEnabled || mObjDetEnabled || mPosTrackingEnabled || mPosTrackingActivated || ((rgbSubnumber + rgbRawSubnumber + leftSubnumber + leftRawSubnumber + rightSubnumber + rightRawSubnumber + rgbGraySubnumber + rgbGrayRawSubnumber + leftGraySubnumber + leftGrayRawSubnumber + rightGraySubnumber + rightGrayRawSubnumber + depthSubnumber + disparitySubnumber + cloudSubnumber + poseSubnumber + poseCovSubnumber + odomSubnumber + confMapSubnumber /*+ imuSubnumber + imuRawsubnumber*/ + pathSubNumber + stereoSubNumber + stereoRawSubNumber + objDetSubnumber) > 0); - - // Run the loop only if there is some subscribers or SVO is active - if (mGrabActive) { - std::lock_guard lock(mPosTrkMutex); + else + { + if ((ros::Time::now() - mPrevFrameTimestamp).toSec() > 5 && !mSvoMode) + { + mCloseZedMutex.lock(); + mZed.close(); + mCloseZedMutex.unlock(); - // Note: once tracking is started it is never stopped anymore to not lose tracking information - bool computeTracking = !mDepthDisabled && - (mPosTrackingEnabled || mPosTrackingActivated || mMappingEnabled || mObjDetEnabled || (mComputeDepth & mDepthStabilization) || - poseSubnumber > 0 || poseCovSubnumber > 0 || odomSubnumber > 0 || pathSubNumber > 0); + mConnStatus = sl::ERROR_CODE::CAMERA_NOT_DETECTED; - // Start the tracking? - if ((computeTracking) && !mPosTrackingActivated && !mDepthDisabled) { - start_pos_tracking(); - } + while (mConnStatus != sl::ERROR_CODE::SUCCESS) + { + if (!mNhNs.ok()) + { + mStopNode = true; - // Start the mapping? - mMappingMutex.lock(); - if (mMappingEnabled && !mMappingRunning) { - start_3d_mapping(); - } - mMappingMutex.unlock(); + std::lock_guard lock(mCloseZedMutex); + NODELET_DEBUG("Closing ZED"); + if (mRecording) + { + mRecording = false; + mZed.disableRecording(); + } + mZed.close(); - // Start the object detection? - mObjDetMutex.lock(); - if (mObjDetEnabled && !mObjDetRunning) { - start_obj_detect(); - } - mObjDetMutex.unlock(); + NODELET_DEBUG("ZED pool thread finished"); + return; + } - // Detect if one of the subscriber need to have the depth information - mComputeDepth = !mDepthDisabled && (computeTracking || ((depthSubnumber + disparitySubnumber + cloudSubnumber + fusedCloudSubnumber + poseSubnumber + poseCovSubnumber + odomSubnumber + confMapSubnumber + objDetSubnumber) > 0)); + mZedParams.input.setFromSerialNumber(mZedSerialNumber); + mConnStatus = mZed.open(mZedParams); // Try to initialize the ZED + NODELET_INFO_STREAM("Connection status: " << toString(mConnStatus)); - if (mComputeDepth) { - runParams.confidence_threshold = mCamDepthConfidence; - runParams.texture_confidence_threshold = mCamDepthTextureConf; - runParams.enable_depth = true; // Ask to compute the depth - } else { - runParams.enable_depth = false; // Ask to not compute the depth + mDiagUpdater.force_update(); + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); } - std::chrono::steady_clock::time_point start_elab = std::chrono::steady_clock::now(); - - // ZED Grab - mGrabStatus = mZed.grab(runParams); - - // cout << toString(grab_status) << endl; - if (mGrabStatus != sl::ERROR_CODE::SUCCESS) { - // Detect if a error occurred (for example: - // the zed have been disconnected) and - // re-initialize the ZED - - NODELET_INFO_STREAM_THROTTLE( 1.0, "Camera grab error: " << sl::toString(mGrabStatus).c_str()); - - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - if(mGrabStatus!=sl::ERROR_CODE::CAMERA_REBOOTING) { - std::this_thread::sleep_for(std::chrono::milliseconds(50)); - continue; - } else if (mSvoMode && mGrabStatus == sl::ERROR_CODE::END_OF_SVOFILE_REACHED) { - NODELET_WARN("SVO reached the end. The node will be stopped."); - mZed.close(); - exit(EXIT_SUCCESS); - } else if (!mRemoteStreamAddr.empty()) { - NODELET_ERROR("Remote stream problem. The node will be stopped."); - mZed.close(); - exit(EXIT_FAILURE); - } else { - if ((ros::Time::now() - mPrevFrameTimestamp).toSec() > 5 && !mSvoMode) { - mCloseZedMutex.lock(); - mZed.close(); - mCloseZedMutex.unlock(); - - mConnStatus = sl::ERROR_CODE::CAMERA_NOT_DETECTED; - - while (mConnStatus != sl::ERROR_CODE::SUCCESS) { - if (!mNhNs.ok()) { - mStopNode = true; - - std::lock_guard lock(mCloseZedMutex); - NODELET_DEBUG("Closing ZED"); - if (mRecording) { - mRecording = false; - mZed.disableRecording(); - } - mZed.close(); - - NODELET_DEBUG("ZED pool thread finished"); - return; - } - - mZedParams.input.setFromSerialNumber(mZedSerialNumber); - mConnStatus = mZed.open(mZedParams); // Try to initialize the ZED - NODELET_INFO_STREAM("Connection status: " << toString(mConnStatus)); - - mDiagUpdater.force_update(); - std::this_thread::sleep_for(std::chrono::milliseconds(1000)); - } - - mPosTrackingActivated = false; - - // Note: once tracking is started it is never stopped anymore to not lose tracking information - bool computeTracking = !mDepthDisabled && - (mPosTrackingEnabled || mPosTrackingActivated || mMappingEnabled || mObjDetEnabled || (mComputeDepth & mDepthStabilization) || - poseSubnumber > 0 || poseCovSubnumber > 0 || odomSubnumber > 0 || pathSubNumber > 0); - - // Start the tracking? - if ((computeTracking) && !mPosTrackingActivated && !mDepthDisabled) { - start_pos_tracking(); - } - } - } + mPosTrackingActivated = false; - mDiagUpdater.update(); + // Note: once tracking is started it is never stopped anymore to not lose tracking information + bool computeTracking = + !mDepthDisabled && (mPosTrackingEnabled || mPosTrackingActivated || mMappingEnabled || mObjDetEnabled || + (mComputeDepth & mDepthStabilization) || poseSubnumber > 0 || + poseCovSubnumber > 0 || odomSubnumber > 0 || pathSubNumber > 0); - continue; + // Start the tracking? + if ((computeTracking) && !mPosTrackingActivated && !mDepthDisabled) + { + start_pos_tracking(); } + } + } - mFrameCount++; + mDiagUpdater.update(); - // ----> Timestamp - if (mSvoMode) { - mFrameTimestamp = ros::Time::now(); - } else { - mFrameTimestamp = sl_tools::slTime2Ros(mZed.getTimestamp(sl::TIME_REFERENCE::IMAGE)); - } - mPrevFrameTimestamp = mFrameTimestamp; - ros::Time stamp = mFrameTimestamp; // Fix processing Timestamp - // <---- Timestamp + continue; + } - // Publish Color and Depth images - pubVideoDepth(); + mFrameCount++; - // ----> SVO recording - mRecMutex.lock(); + // ----> Timestamp + if (mSvoMode) + { + mFrameTimestamp = ros::Time::now(); + } + else + { + mFrameTimestamp = sl_tools::slTime2Ros(mZed.getTimestamp(sl::TIME_REFERENCE::IMAGE)); + } + mPrevFrameTimestamp = mFrameTimestamp; + ros::Time stamp = mFrameTimestamp; // Fix processing Timestamp + // <---- Timestamp - if (mRecording) { - mRecStatus = mZed.getRecordingStatus(); + // Publish Color and Depth images + pubVideoDepth(); - if (!mRecStatus.status) { - NODELET_ERROR_THROTTLE(1.0, "Error saving frame to SVO"); - } + // ----> SVO recording + mRecMutex.lock(); - mDiagUpdater.force_update(); - } - mRecMutex.unlock(); - // <---- SVO recording + if (mRecording) + { + mRecStatus = mZed.getRecordingStatus(); - // ----> Publish freq calculation - static std::chrono::steady_clock::time_point last_time = std::chrono::steady_clock::now(); - std::chrono::steady_clock::time_point now = std::chrono::steady_clock::now(); + if (!mRecStatus.status) + { + NODELET_ERROR_THROTTLE(1.0, "Error saving frame to SVO"); + } - double elapsed_usec = std::chrono::duration_cast(now - last_time).count(); - last_time = now; + mDiagUpdater.force_update(); + } + mRecMutex.unlock(); + // <---- SVO recording - mGrabPeriodMean_usec->addValue(elapsed_usec); - // NODELET_INFO_STREAM("Grab time: " << elapsed_usec / 1000 << " msec"); - // <---- Publish freq calculation + // ----> Publish freq calculation + static std::chrono::steady_clock::time_point last_time = std::chrono::steady_clock::now(); + std::chrono::steady_clock::time_point now = std::chrono::steady_clock::now(); - // ----> Camera Settings - int value; - sl::VIDEO_SETTINGS setting; - sl::ERROR_CODE err; + double elapsed_usec = std::chrono::duration_cast(now - last_time).count(); + last_time = now; - if (!mSvoMode && mFrameCount % 5 == 0) { - // NODELET_DEBUG_STREAM( "[" << mFrameCount << "] device_poll_thread_func MUTEX LOCK"); - mDynParMutex.lock(); + mGrabPeriodMean_usec->addValue(elapsed_usec); + // NODELET_INFO_STREAM("Grab time: " << elapsed_usec / 1000 << " msec"); + // <---- Publish freq calculation - setting = sl::VIDEO_SETTINGS::BRIGHTNESS; - err = mZed.getCameraSettings(setting, value); - if (err==sl::ERROR_CODE::SUCCESS && value != mCamBrightness) { - err = mZed.setCameraSettings(setting, mCamBrightness); - } - if(err != sl::ERROR_CODE::SUCCESS) { - NODELET_WARN_STREAM( "Error setting parameter " << sl::toString(setting) << ": " << sl::toString(err)); - } else { - NODELET_DEBUG_STREAM(sl::toString(setting) << " changed: " << mCamBrightness << " <- " << value); - mUpdateDynParams = true; - } + // ----> Camera Settings + int value; + sl::VIDEO_SETTINGS setting; + sl::ERROR_CODE err; - setting = sl::VIDEO_SETTINGS::CONTRAST; - err = mZed.getCameraSettings(setting, value); - if (err==sl::ERROR_CODE::SUCCESS && value != mCamContrast) { - err = mZed.setCameraSettings(setting, mCamContrast); - } - if(err != sl::ERROR_CODE::SUCCESS) { - NODELET_WARN_STREAM( "Error setting parameter " << sl::toString(setting) << ": " << sl::toString(err)); - } else { - NODELET_DEBUG_STREAM(sl::toString(setting) << " changed: " << mCamContrast << " <- " << value); - mUpdateDynParams = true; - } + if (!mSvoMode && mFrameCount % 5 == 0) + { + // NODELET_DEBUG_STREAM( "[" << mFrameCount << "] device_poll_thread_func MUTEX LOCK"); + mDynParMutex.lock(); - setting = sl::VIDEO_SETTINGS::HUE; - err = mZed.getCameraSettings(setting, value); - if (err==sl::ERROR_CODE::SUCCESS && value != mCamHue) { - err = mZed.setCameraSettings(setting, mCamHue); - } - if(err != sl::ERROR_CODE::SUCCESS) { - NODELET_WARN_STREAM( "Error setting parameter " << sl::toString(setting) << ": " << sl::toString(err)); - } else { - NODELET_DEBUG_STREAM(sl::toString(setting) << " changed: " << mCamHue << " <- " << value); - mUpdateDynParams = true; - } + setting = sl::VIDEO_SETTINGS::BRIGHTNESS; + err = mZed.getCameraSettings(setting, value); + if (err == sl::ERROR_CODE::SUCCESS && value != mCamBrightness) + { + err = mZed.setCameraSettings(setting, mCamBrightness); + } + if (err != sl::ERROR_CODE::SUCCESS) + { + NODELET_WARN_STREAM("Error setting parameter " << sl::toString(setting) << ": " << sl::toString(err)); + } + else + { + NODELET_DEBUG_STREAM(sl::toString(setting) << " changed: " << mCamBrightness << " <- " << value); + mUpdateDynParams = true; + } - setting = sl::VIDEO_SETTINGS::SATURATION; - err = mZed.getCameraSettings(setting, value); - if (err==sl::ERROR_CODE::SUCCESS && value != mCamSaturation) { - err = mZed.setCameraSettings(setting, mCamSaturation); - } - if(err != sl::ERROR_CODE::SUCCESS) { - NODELET_WARN_STREAM( "Error setting parameter " << sl::toString(setting) << ": " << sl::toString(err)); - } else { - NODELET_DEBUG_STREAM(sl::toString(setting) << " changed: " << mCamSaturation << " <- " << value); - mUpdateDynParams = true; - } + setting = sl::VIDEO_SETTINGS::CONTRAST; + err = mZed.getCameraSettings(setting, value); + if (err == sl::ERROR_CODE::SUCCESS && value != mCamContrast) + { + err = mZed.setCameraSettings(setting, mCamContrast); + } + if (err != sl::ERROR_CODE::SUCCESS) + { + NODELET_WARN_STREAM("Error setting parameter " << sl::toString(setting) << ": " << sl::toString(err)); + } + else + { + NODELET_DEBUG_STREAM(sl::toString(setting) << " changed: " << mCamContrast << " <- " << value); + mUpdateDynParams = true; + } - setting = sl::VIDEO_SETTINGS::SHARPNESS; - err = mZed.getCameraSettings(setting, value); - if (err==sl::ERROR_CODE::SUCCESS && value != mCamSharpness) { - err = mZed.setCameraSettings(setting, mCamSharpness); - } - if(err != sl::ERROR_CODE::SUCCESS) { - NODELET_WARN_STREAM( "Error setting parameter " << sl::toString(setting) << ": " << sl::toString(err)); - } else { - NODELET_DEBUG_STREAM(sl::toString(setting) << " changed: " << mCamSharpness << " <- " << value); - mUpdateDynParams = true; - } + setting = sl::VIDEO_SETTINGS::HUE; + err = mZed.getCameraSettings(setting, value); + if (err == sl::ERROR_CODE::SUCCESS && value != mCamHue) + { + err = mZed.setCameraSettings(setting, mCamHue); + } + if (err != sl::ERROR_CODE::SUCCESS) + { + NODELET_WARN_STREAM("Error setting parameter " << sl::toString(setting) << ": " << sl::toString(err)); + } + else + { + NODELET_DEBUG_STREAM(sl::toString(setting) << " changed: " << mCamHue << " <- " << value); + mUpdateDynParams = true; + } - setting = sl::VIDEO_SETTINGS::GAMMA; - err = mZed.getCameraSettings(setting, value); - if (err==sl::ERROR_CODE::SUCCESS && value != mCamGamma) { - err = mZed.setCameraSettings(setting, mCamGamma); - } - if(err != sl::ERROR_CODE::SUCCESS) { - NODELET_WARN_STREAM( "Error setting parameter " << sl::toString(setting) << ": " << sl::toString(err)); - } else { - NODELET_DEBUG_STREAM(sl::toString(setting) << " changed: " << mCamGamma << " <- " << value); - mUpdateDynParams = true; - } + setting = sl::VIDEO_SETTINGS::SATURATION; + err = mZed.getCameraSettings(setting, value); + if (err == sl::ERROR_CODE::SUCCESS && value != mCamSaturation) + { + err = mZed.setCameraSettings(setting, mCamSaturation); + } + if (err != sl::ERROR_CODE::SUCCESS) + { + NODELET_WARN_STREAM("Error setting parameter " << sl::toString(setting) << ": " << sl::toString(err)); + } + else + { + NODELET_DEBUG_STREAM(sl::toString(setting) << " changed: " << mCamSaturation << " <- " << value); + mUpdateDynParams = true; + } - if (mTriggerAutoExposure) { - setting = sl::VIDEO_SETTINGS::AEC_AGC; - err = mZed.getCameraSettings(setting, value); - int aec_agc = (mCamAutoExposure?1:0); - if (err==sl::ERROR_CODE::SUCCESS && value != aec_agc) { - err = mZed.setCameraSettings(setting, aec_agc); - } - if(err != sl::ERROR_CODE::SUCCESS) { - NODELET_WARN_STREAM( "Error setting parameter " << sl::toString(setting) << ": " << sl::toString(err)); - } else { - NODELET_DEBUG_STREAM(sl::toString(setting) << " changed: " << aec_agc << " <- " << value); - mUpdateDynParams = true; - } - } - if (!mCamAutoExposure){ - setting = sl::VIDEO_SETTINGS::EXPOSURE; - err = mZed.getCameraSettings(setting, value); - if (err==sl::ERROR_CODE::SUCCESS && value != mCamExposure) { - err = mZed.setCameraSettings(setting, mCamExposure); - } - if(err != sl::ERROR_CODE::SUCCESS) { - NODELET_WARN_STREAM( "Error setting parameter " << sl::toString(setting) << ": " << sl::toString(err)); - } else { - NODELET_DEBUG_STREAM(sl::toString(setting) << " changed: " << mCamExposure << " <- " << value); - mUpdateDynParams = true; - } - - setting = sl::VIDEO_SETTINGS::GAIN; - err = mZed.getCameraSettings(setting, value); - if (err==sl::ERROR_CODE::SUCCESS && value != mCamGain) { - err = mZed.setCameraSettings(setting, mCamGain); - } - if(err != sl::ERROR_CODE::SUCCESS) { - NODELET_WARN_STREAM( "Error setting parameter " << sl::toString(setting) << ": " << sl::toString(err)); - } else { - NODELET_DEBUG_STREAM(sl::toString(setting) << " changed: " << mCamGain << " <- " << value); - mUpdateDynParams = true; - } - } + setting = sl::VIDEO_SETTINGS::SHARPNESS; + err = mZed.getCameraSettings(setting, value); + if (err == sl::ERROR_CODE::SUCCESS && value != mCamSharpness) + { + err = mZed.setCameraSettings(setting, mCamSharpness); + } + if (err != sl::ERROR_CODE::SUCCESS) + { + NODELET_WARN_STREAM("Error setting parameter " << sl::toString(setting) << ": " << sl::toString(err)); + } + else + { + NODELET_DEBUG_STREAM(sl::toString(setting) << " changed: " << mCamSharpness << " <- " << value); + mUpdateDynParams = true; + } - if (mTriggerAutoWB) { - setting = sl::VIDEO_SETTINGS::WHITEBALANCE_AUTO; - err = mZed.getCameraSettings(setting, value); - int wb_auto = (mCamAutoWB?1:0); - if (err==sl::ERROR_CODE::SUCCESS && value != wb_auto) { - err = mZed.setCameraSettings(setting, wb_auto); - } - if(err != sl::ERROR_CODE::SUCCESS) { - NODELET_WARN_STREAM( "Error setting parameter " << sl::toString(setting) << ": " << sl::toString(err)); - } else { - NODELET_DEBUG_STREAM(sl::toString(setting) << " changed: " << wb_auto << " <- " << value); - mUpdateDynParams = true; - } - } - if (!mCamAutoWB) { - setting = sl::VIDEO_SETTINGS::WHITEBALANCE_TEMPERATURE; - err = mZed.getCameraSettings(setting, value); - if (err==sl::ERROR_CODE::SUCCESS && value != mCamWB) { - err = mZed.setCameraSettings(setting, mCamWB); - } - if(err != sl::ERROR_CODE::SUCCESS) { - NODELET_WARN_STREAM( "Error setting parameter " << sl::toString(setting) << ": " << sl::toString(err)); - } else { - NODELET_DEBUG_STREAM(sl::toString(setting) << " changed: " << mCamWB << " <- " << value); - mUpdateDynParams = true; - } - } - mDynParMutex.unlock(); - // NODELET_DEBUG_STREAM( "device_poll_thread_func MUTEX UNLOCK"); - } + setting = sl::VIDEO_SETTINGS::GAMMA; + err = mZed.getCameraSettings(setting, value); + if (err == sl::ERROR_CODE::SUCCESS && value != mCamGamma) + { + err = mZed.setCameraSettings(setting, mCamGamma); + } + if (err != sl::ERROR_CODE::SUCCESS) + { + NODELET_WARN_STREAM("Error setting parameter " << sl::toString(setting) << ": " << sl::toString(err)); + } + else + { + NODELET_DEBUG_STREAM(sl::toString(setting) << " changed: " << mCamGamma << " <- " << value); + mUpdateDynParams = true; + } - if (mUpdateDynParams) { - NODELET_DEBUG("Update Dynamic Parameters"); - updateDynamicReconfigure(); - } - // <---- Camera Settings + if (mTriggerAutoExposure) + { + setting = sl::VIDEO_SETTINGS::AEC_AGC; + err = mZed.getCameraSettings(setting, value); + int aec_agc = (mCamAutoExposure ? 1 : 0); + if (err == sl::ERROR_CODE::SUCCESS && value != aec_agc) + { + err = mZed.setCameraSettings(setting, aec_agc); + } + if (err != sl::ERROR_CODE::SUCCESS) + { + NODELET_WARN_STREAM("Error setting parameter " << sl::toString(setting) << ": " << sl::toString(err)); + } + else + { + NODELET_DEBUG_STREAM(sl::toString(setting) << " changed: " << aec_agc << " <- " << value); + mUpdateDynParams = true; + } + } + if (!mCamAutoExposure) + { + setting = sl::VIDEO_SETTINGS::EXPOSURE; + err = mZed.getCameraSettings(setting, value); + if (err == sl::ERROR_CODE::SUCCESS && value != mCamExposure) + { + err = mZed.setCameraSettings(setting, mCamExposure); + } + if (err != sl::ERROR_CODE::SUCCESS) + { + NODELET_WARN_STREAM("Error setting parameter " << sl::toString(setting) << ": " << sl::toString(err)); + } + else + { + NODELET_DEBUG_STREAM(sl::toString(setting) << " changed: " << mCamExposure << " <- " << value); + mUpdateDynParams = true; + } + + setting = sl::VIDEO_SETTINGS::GAIN; + err = mZed.getCameraSettings(setting, value); + if (err == sl::ERROR_CODE::SUCCESS && value != mCamGain) + { + err = mZed.setCameraSettings(setting, mCamGain); + } + if (err != sl::ERROR_CODE::SUCCESS) + { + NODELET_WARN_STREAM("Error setting parameter " << sl::toString(setting) << ": " << sl::toString(err)); + } + else + { + NODELET_DEBUG_STREAM(sl::toString(setting) << " changed: " << mCamGain << " <- " << value); + mUpdateDynParams = true; + } + } - // Publish the point cloud if someone has subscribed to - if (cloudSubnumber > 0) { - // Run the point cloud conversion asynchronously to avoid slowing down - // all the program - // Retrieve raw pointCloud data if latest Pointcloud is ready - std::unique_lock lock(mPcMutex, std::defer_lock); + if (mTriggerAutoWB) + { + setting = sl::VIDEO_SETTINGS::WHITEBALANCE_AUTO; + err = mZed.getCameraSettings(setting, value); + int wb_auto = (mCamAutoWB ? 1 : 0); + if (err == sl::ERROR_CODE::SUCCESS && value != wb_auto) + { + err = mZed.setCameraSettings(setting, wb_auto); + } + if (err != sl::ERROR_CODE::SUCCESS) + { + NODELET_WARN_STREAM("Error setting parameter " << sl::toString(setting) << ": " << sl::toString(err)); + } + else + { + NODELET_DEBUG_STREAM(sl::toString(setting) << " changed: " << wb_auto << " <- " << value); + mUpdateDynParams = true; + } + } + if (!mCamAutoWB) + { + setting = sl::VIDEO_SETTINGS::WHITEBALANCE_TEMPERATURE; + err = mZed.getCameraSettings(setting, value); + if (err == sl::ERROR_CODE::SUCCESS && value != mCamWB) + { + err = mZed.setCameraSettings(setting, mCamWB); + } + if (err != sl::ERROR_CODE::SUCCESS) + { + NODELET_WARN_STREAM("Error setting parameter " << sl::toString(setting) << ": " << sl::toString(err)); + } + else + { + NODELET_DEBUG_STREAM(sl::toString(setting) << " changed: " << mCamWB << " <- " << value); + mUpdateDynParams = true; + } + } + mDynParMutex.unlock(); + // NODELET_DEBUG_STREAM( "device_poll_thread_func MUTEX UNLOCK"); + } - if (lock.try_lock()) { - mZed.retrieveMeasure(mCloud, sl::MEASURE::XYZBGRA, sl::MEM::CPU, mMatResol); + if (mUpdateDynParams) + { + NODELET_DEBUG("Update Dynamic Parameters"); + updateDynamicReconfigure(); + } + // <---- Camera Settings - mPointCloudFrameId = mDepthFrameId; - mPointCloudTime = stamp; + // Publish the point cloud if someone has subscribed to + if (cloudSubnumber > 0) + { + // Run the point cloud conversion asynchronously to avoid slowing down + // all the program + // Retrieve raw pointCloud data if latest Pointcloud is ready + std::unique_lock lock(mPcMutex, std::defer_lock); - // Signal Pointcloud thread that a new pointcloud is ready - mPcDataReadyCondVar.notify_one(); - mPcDataReady = true; - mPcPublishing = true; - } - } else { - mPcPublishing = false; - } + if (lock.try_lock()) + { + mZed.retrieveMeasure(mCloud, sl::MEASURE::XYZBGRA, sl::MEM::CPU, mMatResol); - mObjDetMutex.lock(); - if (mObjDetRunning && objDetSubnumber > 0) { - processDetectedObjects(stamp); - } - mObjDetMutex.unlock(); + mPointCloudFrameId = mDepthFrameId; + mPointCloudTime = stamp; - // Publish the odometry if someone has subscribed to - if (computeTracking) { - if (!mSensor2BaseTransfValid) { - getSens2BaseTransform(); - } + // Signal Pointcloud thread that a new pointcloud is ready + mPcDataReadyCondVar.notify_one(); + mPcDataReady = true; + mPcPublishing = true; + } + } + else + { + mPcPublishing = false; + } - if (!mSensor2CameraTransfValid) { - getSens2CameraTransform(); - } + mObjDetMutex.lock(); + if (mObjDetRunning && objDetSubnumber > 0) + { + processDetectedObjects(stamp); + } + mObjDetMutex.unlock(); - if (!mCamera2BaseTransfValid) { - getCamera2BaseTransform(); - } + // Publish the odometry if someone has subscribed to + if (computeTracking) + { + if (!mSensor2BaseTransfValid) + { + getSens2BaseTransform(); + } - if (!mInitOdomWithPose) { - sl::Pose deltaOdom; - mPosTrackingStatus = mZed.getPosition(deltaOdom, sl::REFERENCE_FRAME::CAMERA); + if (!mSensor2CameraTransfValid) + { + getSens2CameraTransform(); + } - sl::Translation translation = deltaOdom.getTranslation(); - sl::Orientation quat = deltaOdom.getOrientation(); + if (!mCamera2BaseTransfValid) + { + getCamera2BaseTransform(); + } + + if (!mInitOdomWithPose) + { + sl::Pose deltaOdom; + mPosTrackingStatus = mZed.getPosition(deltaOdom, sl::REFERENCE_FRAME::CAMERA); + + sl::Translation translation = deltaOdom.getTranslation(); + sl::Orientation quat = deltaOdom.getOrientation(); #if 0 NODELET_DEBUG("delta ODOM [%s] - %.2f,%.2f,%.2f %.2f,%.2f,%.2f,%.2f", @@ -3874,39 +4437,43 @@ void ZEDWrapperNodelet::device_poll_thread_func() NODELET_DEBUG_STREAM("ODOM -> Tracking Status: " << sl::toString(mTrackingStatus)); #endif - if (mPosTrackingStatus == sl::POSITIONAL_TRACKING_STATE::OK || mPosTrackingStatus == sl::POSITIONAL_TRACKING_STATE::SEARCHING || mPosTrackingStatus == sl::POSITIONAL_TRACKING_STATE::FPS_TOO_LOW) { - // Transform ZED delta odom pose in TF2 Transformation - geometry_msgs::Transform deltaTransf; - deltaTransf.translation.x = translation(0); - deltaTransf.translation.y = translation(1); - deltaTransf.translation.z = translation(2); - deltaTransf.rotation.x = quat(0); - deltaTransf.rotation.y = quat(1); - deltaTransf.rotation.z = quat(2); - deltaTransf.rotation.w = quat(3); - tf2::Transform deltaOdomTf; - tf2::fromMsg(deltaTransf, deltaOdomTf); - // delta odom from sensor to base frame - tf2::Transform deltaOdomTf_base = mSensor2BaseTransf.inverse() * deltaOdomTf * mSensor2BaseTransf; - - // Propagate Odom transform in time - mOdom2BaseTransf = mOdom2BaseTransf * deltaOdomTf_base; - - if (mTwoDMode) { - tf2::Vector3 tr_2d = mOdom2BaseTransf.getOrigin(); - tr_2d.setZ(mFixedZValue); - mOdom2BaseTransf.setOrigin(tr_2d); - - double roll, pitch, yaw; - tf2::Matrix3x3(mOdom2BaseTransf.getRotation()).getRPY(roll, pitch, yaw); - - tf2::Quaternion quat_2d; - quat_2d.setRPY(0.0, 0.0, yaw); - - mOdom2BaseTransf.setRotation(quat_2d); - } - -#if 0 //#ifndef NDEBUG // Enable for TF checking + if (mPosTrackingStatus == sl::POSITIONAL_TRACKING_STATE::OK || + mPosTrackingStatus == sl::POSITIONAL_TRACKING_STATE::SEARCHING || + mPosTrackingStatus == sl::POSITIONAL_TRACKING_STATE::FPS_TOO_LOW) + { + // Transform ZED delta odom pose in TF2 Transformation + geometry_msgs::Transform deltaTransf; + deltaTransf.translation.x = translation(0); + deltaTransf.translation.y = translation(1); + deltaTransf.translation.z = translation(2); + deltaTransf.rotation.x = quat(0); + deltaTransf.rotation.y = quat(1); + deltaTransf.rotation.z = quat(2); + deltaTransf.rotation.w = quat(3); + tf2::Transform deltaOdomTf; + tf2::fromMsg(deltaTransf, deltaOdomTf); + // delta odom from sensor to base frame + tf2::Transform deltaOdomTf_base = mSensor2BaseTransf.inverse() * deltaOdomTf * mSensor2BaseTransf; + + // Propagate Odom transform in time + mOdom2BaseTransf = mOdom2BaseTransf * deltaOdomTf_base; + + if (mTwoDMode) + { + tf2::Vector3 tr_2d = mOdom2BaseTransf.getOrigin(); + tr_2d.setZ(mFixedZValue); + mOdom2BaseTransf.setOrigin(tr_2d); + + double roll, pitch, yaw; + tf2::Matrix3x3(mOdom2BaseTransf.getRotation()).getRPY(roll, pitch, yaw); + + tf2::Quaternion quat_2d; + quat_2d.setRPY(0.0, 0.0, yaw); + + mOdom2BaseTransf.setRotation(quat_2d); + } + +#if 0 //#ifndef NDEBUG // Enable for TF checking double roll, pitch, yaw; tf2::Matrix3x3(mOdom2BaseTransf.getRotation()).getRPY(roll, pitch, yaw); @@ -3916,27 +4483,32 @@ void ZEDWrapperNodelet::device_poll_thread_func() roll * RAD2DEG, pitch * RAD2DEG, yaw * RAD2DEG); #endif - // Publish odometry message - if (odomSubnumber > 0) { - publishOdom(mOdom2BaseTransf, deltaOdom, stamp); - } - - mPosTrackingReady = true; - } - } else if (mFloorAlignment) { - NODELET_WARN_THROTTLE(5.0, "Odometry will be published as soon as the floor as been detected for the first " - "time"); - } + // Publish odometry message + if (odomSubnumber > 0) + { + publishOdom(mOdom2BaseTransf, deltaOdom, stamp); } - // Publish the zed camera pose if someone has subscribed to - if (computeTracking) { - mPosTrackingStatus = mZed.getPosition(mLastZedPose, sl::REFERENCE_FRAME::WORLD); + mPosTrackingReady = true; + } + } + else if (mFloorAlignment) + { + NODELET_WARN_THROTTLE(5.0, + "Odometry will be published as soon as the floor as been detected for the first " + "time"); + } + } + + // Publish the zed camera pose if someone has subscribed to + if (computeTracking) + { + mPosTrackingStatus = mZed.getPosition(mLastZedPose, sl::REFERENCE_FRAME::WORLD); - sl::Translation translation = mLastZedPose.getTranslation(); - sl::Orientation quat = mLastZedPose.getOrientation(); + sl::Translation translation = mLastZedPose.getTranslation(); + sl::Orientation quat = mLastZedPose.getOrientation(); -#if 0 //#ifndef NDEBUG // Enable for TF checking +#if 0 //#ifndef NDEBUG // Enable for TF checking double roll, pitch, yaw; tf2::Matrix3x3(tf2::Quaternion(quat.ox, quat.oy, quat.oz, quat.ow)).getRPY(roll, pitch, yaw); @@ -3949,48 +4521,54 @@ void ZEDWrapperNodelet::device_poll_thread_func() #endif - static sl::POSITIONAL_TRACKING_STATE old_tracking_state = sl::POSITIONAL_TRACKING_STATE::OFF; - if (mPosTrackingStatus == sl::POSITIONAL_TRACKING_STATE::OK || mPosTrackingStatus == sl::POSITIONAL_TRACKING_STATE::SEARCHING - /*|| status == sl::POSITIONAL_TRACKING_STATE::FPS_TOO_LOW*/) { - if (mPosTrackingStatus == sl::POSITIONAL_TRACKING_STATE::OK && mPosTrackingStatus != old_tracking_state) { - NODELET_INFO_STREAM("Positional tracking -> OK [" << sl::toString(mPosTrackingStatus).c_str() << "]"); - old_tracking_state = mPosTrackingStatus; - } - if (mPosTrackingStatus == sl::POSITIONAL_TRACKING_STATE::SEARCHING && mPosTrackingStatus != old_tracking_state) { - NODELET_INFO_STREAM("Positional tracking -> Searching for a known position [" - << sl::toString(mPosTrackingStatus).c_str() << "]"); - old_tracking_state = mPosTrackingStatus; - } - // Transform ZED pose in TF2 Transformation - geometry_msgs::Transform map2sensTransf; - - map2sensTransf.translation.x = translation(0); - map2sensTransf.translation.y = translation(1); - map2sensTransf.translation.z = translation(2); - map2sensTransf.rotation.x = quat(0); - map2sensTransf.rotation.y = quat(1); - map2sensTransf.rotation.z = quat(2); - map2sensTransf.rotation.w = quat(3); - tf2::Transform map_to_sens_transf; - tf2::fromMsg(map2sensTransf, map_to_sens_transf); - - mMap2BaseTransf = map_to_sens_transf * mSensor2BaseTransf; // Base position in map frame - - if (mTwoDMode) { - tf2::Vector3 tr_2d = mMap2BaseTransf.getOrigin(); - tr_2d.setZ(mFixedZValue); - mMap2BaseTransf.setOrigin(tr_2d); + static sl::POSITIONAL_TRACKING_STATE old_tracking_state = sl::POSITIONAL_TRACKING_STATE::OFF; + if (mPosTrackingStatus == sl::POSITIONAL_TRACKING_STATE::OK || + mPosTrackingStatus == sl::POSITIONAL_TRACKING_STATE::SEARCHING + /*|| status == sl::POSITIONAL_TRACKING_STATE::FPS_TOO_LOW*/) + { + if (mPosTrackingStatus == sl::POSITIONAL_TRACKING_STATE::OK && mPosTrackingStatus != old_tracking_state) + { + NODELET_INFO_STREAM("Positional tracking -> OK [" << sl::toString(mPosTrackingStatus).c_str() << "]"); + old_tracking_state = mPosTrackingStatus; + } + if (mPosTrackingStatus == sl::POSITIONAL_TRACKING_STATE::SEARCHING && + mPosTrackingStatus != old_tracking_state) + { + NODELET_INFO_STREAM("Positional tracking -> Searching for a known position [" + << sl::toString(mPosTrackingStatus).c_str() << "]"); + old_tracking_state = mPosTrackingStatus; + } + // Transform ZED pose in TF2 Transformation + geometry_msgs::Transform map2sensTransf; + + map2sensTransf.translation.x = translation(0); + map2sensTransf.translation.y = translation(1); + map2sensTransf.translation.z = translation(2); + map2sensTransf.rotation.x = quat(0); + map2sensTransf.rotation.y = quat(1); + map2sensTransf.rotation.z = quat(2); + map2sensTransf.rotation.w = quat(3); + tf2::Transform map_to_sens_transf; + tf2::fromMsg(map2sensTransf, map_to_sens_transf); + + mMap2BaseTransf = map_to_sens_transf * mSensor2BaseTransf; // Base position in map frame + + if (mTwoDMode) + { + tf2::Vector3 tr_2d = mMap2BaseTransf.getOrigin(); + tr_2d.setZ(mFixedZValue); + mMap2BaseTransf.setOrigin(tr_2d); - double roll, pitch, yaw; - tf2::Matrix3x3(mMap2BaseTransf.getRotation()).getRPY(roll, pitch, yaw); + double roll, pitch, yaw; + tf2::Matrix3x3(mMap2BaseTransf.getRotation()).getRPY(roll, pitch, yaw); - tf2::Quaternion quat_2d; - quat_2d.setRPY(0.0, 0.0, yaw); + tf2::Quaternion quat_2d; + quat_2d.setRPY(0.0, 0.0, yaw); - mMap2BaseTransf.setRotation(quat_2d); - } + mMap2BaseTransf.setRotation(quat_2d); + } -#if 0 //#ifndef NDEBUG // Enable for TF checking +#if 0 //#ifndef NDEBUG // Enable for TF checking double roll, pitch, yaw; tf2::Matrix3x3(mMap2BaseTransf.getRotation()).getRPY(roll, pitch, yaw); @@ -3999,34 +4577,41 @@ void ZEDWrapperNodelet::device_poll_thread_func() mMap2BaseTransf.getOrigin().z(), roll * RAD2DEG, pitch * RAD2DEG, yaw * RAD2DEG); #endif - bool initOdom = false; - - if (!(mFloorAlignment)) { - initOdom = mInitOdomWithPose; - } else { - initOdom = (mPosTrackingStatus == sl::POSITIONAL_TRACKING_STATE::OK) & mInitOdomWithPose; - } - - if (initOdom || mResetOdom) { - NODELET_INFO("Odometry aligned to last tracking pose"); - - // Propagate Odom transform in time - mOdom2BaseTransf = mMap2BaseTransf; - mMap2BaseTransf.setIdentity(); - - if (odomSubnumber > 0) { - // Publish odometry message - publishOdom(mOdom2BaseTransf, mLastZedPose, mFrameTimestamp); - } + bool initOdom = false; + + if (!(mFloorAlignment)) + { + initOdom = mInitOdomWithPose; + } + else + { + initOdom = (mPosTrackingStatus == sl::POSITIONAL_TRACKING_STATE::OK) & mInitOdomWithPose; + } + + if (initOdom || mResetOdom) + { + NODELET_INFO("Odometry aligned to last tracking pose"); + + // Propagate Odom transform in time + mOdom2BaseTransf = mMap2BaseTransf; + mMap2BaseTransf.setIdentity(); + + if (odomSubnumber > 0) + { + // Publish odometry message + publishOdom(mOdom2BaseTransf, mLastZedPose, mFrameTimestamp); + } - mInitOdomWithPose = false; - mResetOdom = false; - } else { - // Transformation from map to odometry frame - // mMap2OdomTransf = mOdom2BaseTransf.inverse() * mMap2BaseTransf; - mMap2OdomTransf = mMap2BaseTransf * mOdom2BaseTransf.inverse(); + mInitOdomWithPose = false; + mResetOdom = false; + } + else + { + // Transformation from map to odometry frame + // mMap2OdomTransf = mOdom2BaseTransf.inverse() * mMap2BaseTransf; + mMap2OdomTransf = mMap2BaseTransf * mOdom2BaseTransf.inverse(); -#if 0 //#ifndef NDEBUG // Enable for TF checking +#if 0 //#ifndef NDEBUG // Enable for TF checking double roll, pitch, yaw; tf2::Matrix3x3(mMap2OdomTransf.getRotation()).getRPY(roll, pitch, yaw); @@ -4035,34 +4620,39 @@ void ZEDWrapperNodelet::device_poll_thread_func() mMap2OdomTransf.getOrigin().x(), mMap2OdomTransf.getOrigin().y(), mMap2OdomTransf.getOrigin().z(), roll * RAD2DEG, pitch * RAD2DEG, yaw * RAD2DEG); #endif - } + } - // Publish Pose message - if ((poseSubnumber + poseCovSubnumber) > 0) { - publishPose(stamp); - } + // Publish Pose message + if ((poseSubnumber + poseCovSubnumber) > 0) + { + publishPose(stamp); + } - mPosTrackingReady = true; - } - } + mPosTrackingReady = true; + } + } - // Note: in SVO mode the TF must not be broadcasted at the same frequency of the IMU data to avoid timestamp issues - if (mZedRealCamModel == sl::MODEL::ZED || mSvoMode) { - // Publish pose tf only if enabled - if (mPublishTf) { - // Note, the frame is published, but its values will only change if - // someone has subscribed to odom - publishOdomFrame(mOdom2BaseTransf, stamp); // publish the base Frame in odometry frame - - if (mPublishMapTf) { - // Note, the frame is published, but its values will only change if - // someone has subscribed to map - publishPoseFrame(mMap2OdomTransf, stamp); // publish the odometry Frame in map frame - } - } - } + // Note: in SVO mode the TF must not be broadcasted at the same frequency of the IMU data to avoid timestamp + // issues + if (mZedRealCamModel == sl::MODEL::ZED || mSvoMode) + { + // Publish pose tf only if enabled + if (mPublishTf) + { + // Note, the frame is published, but its values will only change if + // someone has subscribed to odom + publishOdomFrame(mOdom2BaseTransf, stamp); // publish the base Frame in odometry frame + + if (mPublishMapTf) + { + // Note, the frame is published, but its values will only change if + // someone has subscribed to map + publishPoseFrame(mMap2OdomTransf, stamp); // publish the odometry Frame in map frame + } + } + } -#if 0 //#ifndef NDEBUG // Enable for TF checking \ +#if 0 //#ifndef NDEBUG // Enable for TF checking \ // Double check: map_to_pose must be equal to mMap2BaseTransf tf2::Transform map_to_base; @@ -4089,902 +4679,1014 @@ void ZEDWrapperNodelet::device_poll_thread_func() NODELET_DEBUG("*******************************"); #endif - std::chrono::steady_clock::time_point end_elab = std::chrono::steady_clock::now(); - - double elab_usec = std::chrono::duration_cast(end_elab - start_elab).count(); - - double mean_elab_sec = mElabPeriodMean_sec->addValue(elab_usec / 1000000.); + std::chrono::steady_clock::time_point end_elab = std::chrono::steady_clock::now(); - static int count_warn = 0; + double elab_usec = std::chrono::duration_cast(end_elab - start_elab).count(); - if (!loop_rate.sleep()) { - 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 (" << mean_elab_sec << " sec) than requested by the FPS rate (" - << loop_rate.expectedCycleTime() << " sec). Please consider to " - "lower the 'general/pub_frame_rate' setting or to " - "reduce the power requirements by reducing the camera resolutions."); - } + double mean_elab_sec = mElabPeriodMean_sec->addValue(elab_usec / 1000000.); - loop_rate.reset(); - } else { - count_warn = 0; - } - } - } else { - NODELET_DEBUG_THROTTLE(5.0, "No topics subscribed by users"); - - if (mSvoMode || mZedRealCamModel == sl::MODEL::ZED || !mPublishImuTf) { - // Publish odometry tf only if enabled - if (mPublishTf) { - ros::Time t; - - if (mSvoMode) { - t = ros::Time::now(); - } else { - t = sl_tools::slTime2Ros(mZed.getTimestamp(sl::TIME_REFERENCE::CURRENT)); - } + static int count_warn = 0; - publishOdomFrame(mOdom2BaseTransf, mFrameTimestamp); // publish the base Frame in odometry frame + if (!loop_rate.sleep()) + { + 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 (" + << mean_elab_sec << " sec) than requested by the FPS rate (" + << loop_rate.expectedCycleTime() + << " sec). Please consider to " + "lower the 'general/pub_frame_rate' setting or to " + "reduce the power requirements by reducing the camera " + "resolutions."); + } + + loop_rate.reset(); + } + else + { + count_warn = 0; + } + } + } + else + { + NODELET_DEBUG_THROTTLE(5.0, "No topics subscribed by users"); - if (mPublishMapTf) { - publishPoseFrame(mMap2OdomTransf, mFrameTimestamp); // publish the odometry Frame in map frame - } + if (mSvoMode || mZedRealCamModel == sl::MODEL::ZED || !mPublishImuTf) + { + // Publish odometry tf only if enabled + if (mPublishTf) + { + ros::Time t; + + if (mSvoMode) + { + t = ros::Time::now(); + } + else + { + t = sl_tools::slTime2Ros(mZed.getTimestamp(sl::TIME_REFERENCE::CURRENT)); + } + + publishOdomFrame(mOdom2BaseTransf, mFrameTimestamp); // publish the base Frame in odometry frame + + if (mPublishMapTf) + { + publishPoseFrame(mMap2OdomTransf, mFrameTimestamp); // publish the odometry Frame in map frame + } + + if (mPublishImuTf && !mStaticImuFramePublished) + { + publishStaticImuFrame(); + } + } + } - if (mPublishImuTf && !mStaticImuFramePublished) { - publishStaticImuFrame(); - } - } - } + std::this_thread::sleep_for(std::chrono::milliseconds(10)); // No subscribers, we just wait + loop_rate.reset(); + } - std::this_thread::sleep_for(std::chrono::milliseconds(10)); // No subscribers, we just wait - loop_rate.reset(); - } + mDiagUpdater.update(); + } // while loop - mDiagUpdater.update(); - } // while loop + if (mSaveAreaMapOnClosing && mPosTrackingActivated) + { + saveAreaMap(mAreaMemDbPath); + } - if (mSaveAreaMapOnClosing && mPosTrackingActivated) { - saveAreaMap(mAreaMemDbPath); - } + mStopNode = true; // Stops other threads - mStopNode = true; // Stops other threads + std::lock_guard lock(mCloseZedMutex); + NODELET_DEBUG("Closing ZED"); - std::lock_guard lock(mCloseZedMutex); - NODELET_DEBUG("Closing ZED"); + if (mRecording) + { + mRecording = false; + mZed.disableRecording(); + } - if (mRecording) { - mRecording = false; - mZed.disableRecording(); - } - - mZed.close(); + mZed.close(); - NODELET_DEBUG("ZED pool thread finished"); + NODELET_DEBUG("ZED pool thread finished"); } void ZEDWrapperNodelet::callback_updateDiagnostic(diagnostic_updater::DiagnosticStatusWrapper& stat) { - if (mConnStatus != sl::ERROR_CODE::SUCCESS) { - stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, sl::toString(mConnStatus).c_str()); - return; - } + if (mConnStatus != sl::ERROR_CODE::SUCCESS) + { + stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, sl::toString(mConnStatus).c_str()); + return; + } - if (mGrabActive) { - if (mGrabStatus == sl::ERROR_CODE::SUCCESS /*|| mGrabStatus == sl::ERROR_CODE::NOT_A_NEW_FRAME*/) { - stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Camera grabbing"); + if (mGrabActive) + { + if (mGrabStatus == sl::ERROR_CODE::SUCCESS /*|| mGrabStatus == sl::ERROR_CODE::NOT_A_NEW_FRAME*/) + { + stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Camera grabbing"); - double freq = 1000000. / mGrabPeriodMean_usec->getMean(); - double freq_perc = 100. * freq / mPubFrameRate; - stat.addf("Capture", "Mean Frequency: %.1f Hz (%.1f%%)", freq, freq_perc); + double freq = 1000000. / mGrabPeriodMean_usec->getMean(); + double freq_perc = 100. * freq / mPubFrameRate; + stat.addf("Capture", "Mean Frequency: %.1f Hz (%.1f%%)", freq, freq_perc); - stat.addf("General Processing", "Mean Time: %.3f sec (Max. %.3f sec)", mElabPeriodMean_sec->getMean(), + stat.addf("General Processing", "Mean Time: %.3f sec (Max. %.3f sec)", mElabPeriodMean_sec->getMean(), 1. / mPubFrameRate); - if (mPublishingData) { - freq = 1. / mVideoDepthPeriodMean_sec->getMean(); - freq_perc = 100. * freq / mPubFrameRate; - stat.addf("Video/Depth Publish", "Mean Frequency: %.1f Hz (%.1f%%)", freq, freq_perc); - } + if (mPublishingData) + { + freq = 1. / mVideoDepthPeriodMean_sec->getMean(); + freq_perc = 100. * freq / mPubFrameRate; + stat.addf("Video/Depth Publish", "Mean Frequency: %.1f Hz (%.1f%%)", freq, freq_perc); + } - if (mSvoMode) { - int frame = mZed.getSVOPosition(); - int totFrames = mZed.getSVONumberOfFrames(); - double svo_perc = 100. * (static_cast(frame) / totFrames); + if (mSvoMode) + { + int frame = mZed.getSVOPosition(); + int totFrames = mZed.getSVONumberOfFrames(); + double svo_perc = 100. * (static_cast(frame) / totFrames); - stat.addf("Playing SVO", "Frame: %d/%d (%.1f%%)", frame, totFrames, svo_perc); - } + stat.addf("Playing SVO", "Frame: %d/%d (%.1f%%)", frame, totFrames, svo_perc); + } - if (mComputeDepth) { - stat.add("Depth status", "ACTIVE"); + if (mComputeDepth) + { + stat.add("Depth status", "ACTIVE"); - if (mPcPublishing) { - double freq = 1000000. / mPcPeriodMean_usec->getMean(); - double freq_perc = 100. * freq / mPointCloudFreq; - stat.addf("Point Cloud", "Mean Frequency: %.1f Hz (%.1f%%)", freq, freq_perc); - } else { - stat.add("Point Cloud", "Topic not subscribed"); - } + if (mPcPublishing) + { + double freq = 1000000. / mPcPeriodMean_usec->getMean(); + double freq_perc = 100. * freq / mPointCloudFreq; + stat.addf("Point Cloud", "Mean Frequency: %.1f Hz (%.1f%%)", freq, freq_perc); + } + else + { + stat.add("Point Cloud", "Topic not subscribed"); + } - if (mFloorAlignment) { - if (mInitOdomWithPose) { - stat.add("Floor Detection", "NOT INITIALIZED"); - } else { - stat.add("Floor Detection", "INITIALIZED"); - } - } + if (mFloorAlignment) + { + if (mInitOdomWithPose) + { + stat.add("Floor Detection", "NOT INITIALIZED"); + } + else + { + stat.add("Floor Detection", "INITIALIZED"); + } + } - if (mPosTrackingActivated) { - stat.addf("Tracking status", "%s", sl::toString(mPosTrackingStatus).c_str()); - } else { - stat.add("Pos. Tracking status", "INACTIVE"); - } + if (mPosTrackingActivated) + { + stat.addf("Tracking status", "%s", sl::toString(mPosTrackingStatus).c_str()); + } + else + { + stat.add("Pos. Tracking status", "INACTIVE"); + } - if (mObjDetRunning) { - double freq = 1000. / mObjDetPeriodMean_msec->getMean(); - double freq_perc = 100. * freq / mPubFrameRate; - stat.addf("Object detection", "Mean Frequency: %.3f Hz (%.1f%%)", freq, freq_perc); - } else { - stat.add("Object Detection", "INACTIVE"); - } - } else { - stat.add("Depth status", "INACTIVE"); - } - } else { - stat.summaryf(diagnostic_msgs::DiagnosticStatus::ERROR, "Camera error: %s", sl::toString(mGrabStatus).c_str()); + if (mObjDetRunning) + { + double freq = 1000. / mObjDetPeriodMean_msec->getMean(); + double freq_perc = 100. * freq / mPubFrameRate; + stat.addf("Object detection", "Mean Frequency: %.3f Hz (%.1f%%)", freq, freq_perc); } - } else { - stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Waiting for data subscriber"); - stat.add("Capture", "INACTIVE"); + else + { + stat.add("Object Detection", "INACTIVE"); + } + } + else + { + stat.add("Depth status", "INACTIVE"); + } } - - if (mSensPublishing) { - double freq = 1000000. / mSensPeriodMean_usec->getMean(); - double freq_perc = 100. * freq / mSensPubRate; - stat.addf("IMU", "Mean Frequency: %.1f Hz (%.1f%%)", freq, freq_perc); - } else { - stat.add("IMU", "Topics not subscribed"); + else + { + stat.summaryf(diagnostic_msgs::DiagnosticStatus::ERROR, "Camera error: %s", sl::toString(mGrabStatus).c_str()); } + } + else + { + stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Waiting for data subscriber"); + stat.add("Capture", "INACTIVE"); + } + + if (mSensPublishing) + { + double freq = 1000000. / mSensPeriodMean_usec->getMean(); + double freq_perc = 100. * freq / mSensPubRate; + stat.addf("IMU", "Mean Frequency: %.1f Hz (%.1f%%)", freq, freq_perc); + } + else + { + stat.add("IMU", "Topics not subscribed"); + } - if (mZedRealCamModel == sl::MODEL::ZED2 || mZedRealCamModel == sl::MODEL::ZED2i) { - stat.addf("Left CMOS Temp.", "%.1f °C", mTempLeft); - stat.addf("Right CMOS Temp.", "%.1f °C", mTempRight); + if (mZedRealCamModel == sl::MODEL::ZED2 || mZedRealCamModel == sl::MODEL::ZED2i) + { + stat.addf("Left CMOS Temp.", "%.1f °C", mTempLeft); + stat.addf("Right CMOS Temp.", "%.1f °C", mTempRight); - if (mTempLeft > 70.f || mTempRight > 70.f) { - stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "Camera temperature"); - } - } else { - stat.add("Left CMOS Temp.", "N/A"); - stat.add("Right CMOS Temp.", "N/A"); + if (mTempLeft > 70.f || mTempRight > 70.f) + { + stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "Camera temperature"); } + } + else + { + stat.add("Left CMOS Temp.", "N/A"); + stat.add("Right CMOS Temp.", "N/A"); + } - if (mRecording) { - if (!mRecStatus.status) { - if (mGrabActive) { - stat.add("SVO Recording", "ERROR"); - stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "Error adding frames to SVO file while recording. Check " - "free disk space"); - } else { - stat.add("SVO Recording", "WAITING"); - } - } else { - stat.add("SVO Recording", "ACTIVE"); - stat.addf("SVO compression time", "%g msec", mRecStatus.average_compression_time); - stat.addf("SVO compression ratio", "%.1f%%", mRecStatus.average_compression_ratio); - } - } else { - stat.add("SVO Recording", "NOT ACTIVE"); + if (mRecording) + { + if (!mRecStatus.status) + { + if (mGrabActive) + { + stat.add("SVO Recording", "ERROR"); + stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, + "Error adding frames to SVO file while recording. Check " + "free disk space"); + } + else + { + stat.add("SVO Recording", "WAITING"); + } } + else + { + stat.add("SVO Recording", "ACTIVE"); + stat.addf("SVO compression time", "%g msec", mRecStatus.average_compression_time); + stat.addf("SVO compression ratio", "%.1f%%", mRecStatus.average_compression_ratio); + } + } + else + { + stat.add("SVO Recording", "NOT ACTIVE"); + } } bool ZEDWrapperNodelet::on_start_svo_recording(zed_interfaces::start_svo_recording::Request& req, - zed_interfaces::start_svo_recording::Response& res) + zed_interfaces::start_svo_recording::Response& res) { - std::lock_guard lock(mRecMutex); + std::lock_guard lock(mRecMutex); - if (mRecording) { - res.result = false; - res.info = "Recording was just active"; - return false; - } + if (mRecording) + { + res.result = false; + res.info = "Recording was just active"; + return false; + } - // Check filename - if (req.svo_filename.empty()) { - req.svo_filename = "zed.svo"; - } + // Check filename + if (req.svo_filename.empty()) + { + req.svo_filename = "zed.svo"; + } - sl::ERROR_CODE err; + sl::ERROR_CODE err; - sl::RecordingParameters recParams; - recParams.compression_mode = mSvoComprMode; - recParams.video_filename = req.svo_filename.c_str(); - err = mZed.enableRecording(recParams); + sl::RecordingParameters recParams; + recParams.compression_mode = mSvoComprMode; + recParams.video_filename = req.svo_filename.c_str(); + err = mZed.enableRecording(recParams); - if (err == sl::ERROR_CODE::SVO_UNSUPPORTED_COMPRESSION) { - recParams.compression_mode = mSvoComprMode == sl::SVO_COMPRESSION_MODE::H265 ? sl::SVO_COMPRESSION_MODE::H264: sl::SVO_COMPRESSION_MODE::H265; + if (err == sl::ERROR_CODE::SVO_UNSUPPORTED_COMPRESSION) + { + recParams.compression_mode = mSvoComprMode == sl::SVO_COMPRESSION_MODE::H265 ? sl::SVO_COMPRESSION_MODE::H264 : + sl::SVO_COMPRESSION_MODE::H265; - NODELET_WARN_STREAM("The chosen " << sl::toString(mSvoComprMode).c_str() << "mode is not available. Trying " - << sl::toString(recParams.compression_mode).c_str()); + NODELET_WARN_STREAM("The chosen " << sl::toString(mSvoComprMode).c_str() << "mode is not available. Trying " + << sl::toString(recParams.compression_mode).c_str()); - err = mZed.enableRecording(recParams); + err = mZed.enableRecording(recParams); - if (err == sl::ERROR_CODE::SVO_UNSUPPORTED_COMPRESSION) { - NODELET_WARN_STREAM(sl::toString(recParams.compression_mode).c_str() - << "not available. Trying " << sl::toString(sl::SVO_COMPRESSION_MODE::H264).c_str()); - recParams.compression_mode = sl::SVO_COMPRESSION_MODE::H264; - err = mZed.enableRecording(recParams); + if (err == sl::ERROR_CODE::SVO_UNSUPPORTED_COMPRESSION) + { + NODELET_WARN_STREAM(sl::toString(recParams.compression_mode).c_str() + << "not available. Trying " << sl::toString(sl::SVO_COMPRESSION_MODE::H264).c_str()); + recParams.compression_mode = sl::SVO_COMPRESSION_MODE::H264; + err = mZed.enableRecording(recParams); - if (err == sl::ERROR_CODE::SVO_UNSUPPORTED_COMPRESSION) { - recParams.compression_mode = sl::SVO_COMPRESSION_MODE::LOSSLESS; - err = mZed.enableRecording(recParams); - } - } + if (err == sl::ERROR_CODE::SVO_UNSUPPORTED_COMPRESSION) + { + recParams.compression_mode = sl::SVO_COMPRESSION_MODE::LOSSLESS; + err = mZed.enableRecording(recParams); + } } + } - if (err != sl::ERROR_CODE::SUCCESS) { - res.result = false; - res.info = sl::toString(err).c_str(); - mRecording = false; - return false; - } + if (err != sl::ERROR_CODE::SUCCESS) + { + res.result = false; + res.info = sl::toString(err).c_str(); + mRecording = false; + return false; + } - mSvoComprMode = recParams.compression_mode; - mRecording = true; - res.info = "Recording started ("; - res.info += sl::toString(mSvoComprMode).c_str(); - res.info += ")"; - res.result = true; + mSvoComprMode = recParams.compression_mode; + mRecording = true; + res.info = "Recording started ("; + res.info += sl::toString(mSvoComprMode).c_str(); + res.info += ")"; + res.result = true; - NODELET_INFO_STREAM("SVO recording STARTED: " << req.svo_filename << " (" << sl::toString(mSvoComprMode).c_str() - << ")"); + NODELET_INFO_STREAM("SVO recording STARTED: " << req.svo_filename << " (" << sl::toString(mSvoComprMode).c_str() + << ")"); - return true; + return true; } bool ZEDWrapperNodelet::on_stop_svo_recording(zed_interfaces::stop_svo_recording::Request& req, - zed_interfaces::stop_svo_recording::Response& res) + zed_interfaces::stop_svo_recording::Response& res) { - std::lock_guard lock(mRecMutex); + std::lock_guard lock(mRecMutex); - if (!mRecording) { - res.done = false; - res.info = "Recording was not active"; - NODELET_WARN_STREAM("Can't stop SVO recording. Recording was not active"); - return false; - } + if (!mRecording) + { + res.done = false; + res.info = "Recording was not active"; + NODELET_WARN_STREAM("Can't stop SVO recording. Recording was not active"); + return false; + } - mZed.disableRecording(); - mRecording = false; - res.info = "Recording stopped"; - res.done = true; + mZed.disableRecording(); + mRecording = false; + res.info = "Recording stopped"; + res.done = true; - NODELET_INFO_STREAM("SVO recording STOPPED"); + NODELET_INFO_STREAM("SVO recording STOPPED"); - return true; + return true; } bool ZEDWrapperNodelet::on_start_remote_stream(zed_interfaces::start_remote_stream::Request& req, - zed_interfaces::start_remote_stream::Response& res) + zed_interfaces::start_remote_stream::Response& res) { - if (mStreaming) { - res.result = false; - res.info = "SVO remote streaming was just active"; - return false; - } + if (mStreaming) + { + res.result = false; + res.info = "SVO remote streaming was just active"; + return false; + } - sl::StreamingParameters params; - params.codec = static_cast(req.codec); - params.port = req.port; - params.bitrate = req.bitrate; - params.gop_size = req.gop_size; - params.adaptative_bitrate = req.adaptative_bitrate; + sl::StreamingParameters params; + params.codec = static_cast(req.codec); + params.port = req.port; + params.bitrate = req.bitrate; + params.gop_size = req.gop_size; + params.adaptative_bitrate = req.adaptative_bitrate; - if ((params.gop_size < -1) || (params.gop_size > 256)) { - mStreaming = false; + if ((params.gop_size < -1) || (params.gop_size > 256)) + { + mStreaming = false; - res.result = false; - res.info = "`gop_size` wrong ("; - res.info += params.gop_size; - res.info += "). Remote streaming not started"; + res.result = false; + res.info = "`gop_size` wrong ("; + res.info += params.gop_size; + res.info += "). Remote streaming not started"; - NODELET_ERROR_STREAM(res.info); - return false; - } + NODELET_ERROR_STREAM(res.info); + return false; + } - if (params.port % 2 != 0) { - mStreaming = false; + if (params.port % 2 != 0) + { + mStreaming = false; - res.result = false; - res.info = "`port` must be an even number. Remote streaming not started"; + res.result = false; + res.info = "`port` must be an even number. Remote streaming not started"; - NODELET_ERROR_STREAM(res.info); - return false; - } + NODELET_ERROR_STREAM(res.info); + return false; + } - sl::ERROR_CODE err = mZed.enableStreaming(params); + sl::ERROR_CODE err = mZed.enableStreaming(params); - if (err != sl::ERROR_CODE::SUCCESS) { - mStreaming = false; + if (err != sl::ERROR_CODE::SUCCESS) + { + mStreaming = false; - res.result = false; - res.info = sl::toString(err).c_str(); + res.result = false; + res.info = sl::toString(err).c_str(); - NODELET_ERROR_STREAM("Remote streaming not started (" << res.info << ")"); + NODELET_ERROR_STREAM("Remote streaming not started (" << res.info << ")"); - return false; - } + return false; + } - mStreaming = true; + mStreaming = true; - NODELET_INFO_STREAM("Remote streaming STARTED"); + NODELET_INFO_STREAM("Remote streaming STARTED"); - res.result = true; - res.info = "Remote streaming STARTED"; - return true; + res.result = true; + res.info = "Remote streaming STARTED"; + return true; } bool ZEDWrapperNodelet::on_stop_remote_stream(zed_interfaces::stop_remote_stream::Request& req, - zed_interfaces::stop_remote_stream::Response& res) + zed_interfaces::stop_remote_stream::Response& res) { - if (mStreaming) { - mZed.disableStreaming(); - } + if (mStreaming) + { + mZed.disableStreaming(); + } - mStreaming = false; - NODELET_INFO_STREAM("SVO remote streaming STOPPED"); + mStreaming = false; + NODELET_INFO_STREAM("SVO remote streaming STOPPED"); - res.done = true; - return true; + res.done = true; + return true; } bool ZEDWrapperNodelet::on_set_led_status(zed_interfaces::set_led_status::Request& req, - zed_interfaces::set_led_status::Response& res) + zed_interfaces::set_led_status::Response& res) { - if (mCamFwVersion < 1523) { - NODELET_WARN_STREAM("To set the status of the blue LED the camera must be updated to FW 1523 or newer"); - return false; - } + if (mCamFwVersion < 1523) + { + NODELET_WARN_STREAM("To set the status of the blue LED the camera must be updated to FW 1523 or newer"); + return false; + } - mZed.setCameraSettings(sl::VIDEO_SETTINGS::LED_STATUS, req.led_enabled ? 1: 0); + mZed.setCameraSettings(sl::VIDEO_SETTINGS::LED_STATUS, req.led_enabled ? 1 : 0); - return true; + return true; } bool ZEDWrapperNodelet::on_toggle_led(zed_interfaces::toggle_led::Request& req, - zed_interfaces::toggle_led::Response& res) + zed_interfaces::toggle_led::Response& res) { - if (mCamFwVersion < 1523) { - NODELET_WARN_STREAM("To set the status of the blue LED the camera must be updated to FW 1523 or newer"); - return false; - } + if (mCamFwVersion < 1523) + { + NODELET_WARN_STREAM("To set the status of the blue LED the camera must be updated to FW 1523 or newer"); + return false; + } - int status; - sl::ERROR_CODE err = mZed.getCameraSettings(sl::VIDEO_SETTINGS::LED_STATUS,status); - if(err!=sl::ERROR_CODE::SUCCESS) - { - NODELET_WARN_STREAM("Error getting the current status of the led: " << sl::toString(err).c_str()); - return false; - } + int status; + sl::ERROR_CODE err = mZed.getCameraSettings(sl::VIDEO_SETTINGS::LED_STATUS, status); + if (err != sl::ERROR_CODE::SUCCESS) + { + NODELET_WARN_STREAM("Error getting the current status of the led: " << sl::toString(err).c_str()); + return false; + } - int new_status = status == 0 ? 1: 0; - err = mZed.setCameraSettings(sl::VIDEO_SETTINGS::LED_STATUS, new_status); - if(err!=sl::ERROR_CODE::SUCCESS) - { - NODELET_WARN_STREAM("Error getting the current status of the led: " << sl::toString(err).c_str()); - return false; - } + int new_status = status == 0 ? 1 : 0; + err = mZed.setCameraSettings(sl::VIDEO_SETTINGS::LED_STATUS, new_status); + if (err != sl::ERROR_CODE::SUCCESS) + { + NODELET_WARN_STREAM("Error getting the current status of the led: " << sl::toString(err).c_str()); + return false; + } - res.new_led_status = new_status; + res.new_led_status = new_status; - return true; + return true; } bool ZEDWrapperNodelet::on_start_3d_mapping(zed_interfaces::start_3d_mapping::Request& req, - zed_interfaces::start_3d_mapping::Response& res) + zed_interfaces::start_3d_mapping::Response& res) { - if (mMappingEnabled && mMappingRunning) { - NODELET_WARN_STREAM("Spatial mapping was just running"); + if (mMappingEnabled && mMappingRunning) + { + NODELET_WARN_STREAM("Spatial mapping was just running"); - res.done = false; - return res.done; - } + res.done = false; + return res.done; + } - mMappingRunning = false; + mMappingRunning = false; - mMappingRes = req.resolution; - mMaxMappingRange = req.max_mapping_range; - mFusedPcPubFreq = req.fused_pointcloud_freq; + mMappingRes = req.resolution; + mMaxMappingRange = req.max_mapping_range; + mFusedPcPubFreq = req.fused_pointcloud_freq; - NODELET_DEBUG_STREAM(" * Received mapping resolution\t\t-> " << mMappingRes << " m"); - NODELET_DEBUG_STREAM(" * Received mapping max range\t-> " << mMaxMappingRange << " m"); - NODELET_DEBUG_STREAM(" * Received fused point cloud freq:\t-> " << mFusedPcPubFreq << " Hz"); + NODELET_DEBUG_STREAM(" * Received mapping resolution\t\t-> " << mMappingRes << " m"); + NODELET_DEBUG_STREAM(" * Received mapping max range\t-> " << mMaxMappingRange << " m"); + NODELET_DEBUG_STREAM(" * Received fused point cloud freq:\t-> " << mFusedPcPubFreq << " Hz"); - mMappingEnabled = true; - res.done = true; + mMappingEnabled = true; + res.done = true; - return res.done; + return res.done; } bool ZEDWrapperNodelet::on_stop_3d_mapping(zed_interfaces::stop_3d_mapping::Request& req, - zed_interfaces::stop_3d_mapping::Response& res) + zed_interfaces::stop_3d_mapping::Response& res) { - if (mMappingEnabled) { - mPubFusedCloud.shutdown(); - mMappingMutex.lock(); - stop_3d_mapping(); - mMappingMutex.unlock(); - - res.done = true; - } else { - res.done = false; - } + if (mMappingEnabled) + { + mPubFusedCloud.shutdown(); + mMappingMutex.lock(); + stop_3d_mapping(); + mMappingMutex.unlock(); - return res.done; + res.done = true; + } + else + { + res.done = false; + } + + return res.done; } bool ZEDWrapperNodelet::on_save_3d_map(zed_interfaces::save_3d_map::Request& req, - zed_interfaces::save_3d_map::Response& res) + zed_interfaces::save_3d_map::Response& res) { - if (!mMappingEnabled) { - res.result = false; - res.info = "3D Mapping was not active"; - NODELET_WARN_STREAM("Can't save 3D map. Mapping was not active"); - return false; - } + if (!mMappingEnabled) + { + res.result = false; + res.info = "3D Mapping was not active"; + NODELET_WARN_STREAM("Can't save 3D map. Mapping was not active"); + return false; + } - mMapSave = true; + mMapSave = true; - std::lock_guard lock(mMappingMutex); - sl::String filename = req.map_filename.c_str(); - if (req.file_format < 0 || req.file_format > static_cast(sl::MESH_FILE_FORMAT::OBJ)) { - res.result = false; - res.info = "File format not correct"; - NODELET_WARN_STREAM("Can't save 3D map. File format not correct"); - return false; - } + std::lock_guard lock(mMappingMutex); + sl::String filename = req.map_filename.c_str(); + if (req.file_format < 0 || req.file_format > static_cast(sl::MESH_FILE_FORMAT::OBJ)) + { + res.result = false; + res.info = "File format not correct"; + NODELET_WARN_STREAM("Can't save 3D map. File format not correct"); + return false; + } - sl::MESH_FILE_FORMAT file_format = static_cast(req.file_format); + sl::MESH_FILE_FORMAT file_format = static_cast(req.file_format); - bool success = mFusedPC.save(filename, file_format); + bool success = mFusedPC.save(filename, file_format); - if (!success) { - res.result = false; - res.info = "3D Map not saved"; - NODELET_ERROR_STREAM("3D Map not saved"); - return false; - } + if (!success) + { + res.result = false; + res.info = "3D Map not saved"; + NODELET_ERROR_STREAM("3D Map not saved"); + return false; + } - res.info = "3D map saved"; - res.result = true; - return true; + res.info = "3D map saved"; + res.result = true; + return true; } bool ZEDWrapperNodelet::on_start_object_detection(zed_interfaces::start_object_detection::Request& req, - zed_interfaces::start_object_detection::Response& res) + zed_interfaces::start_object_detection::Response& res) { - NODELET_INFO("Called 'start_object_detection' service"); - - if (mZedRealCamModel == sl::MODEL::ZED) { - mObjDetEnabled = false; - mObjDetRunning = false; - - NODELET_ERROR_STREAM("Object detection not started. OD is not available for ZED camera model"); - res.done = false; - return res.done; - } + NODELET_INFO("Called 'start_object_detection' service"); - if (mObjDetEnabled && mObjDetRunning) { - NODELET_WARN_STREAM("Object Detection was just running"); + if (mZedRealCamModel == sl::MODEL::ZED) + { + mObjDetEnabled = false; + mObjDetRunning = false; - res.done = false; - return res.done; - } + NODELET_ERROR_STREAM("Object detection not started. OD is not available for ZED camera model"); + res.done = false; + return res.done; + } - mObjDetRunning = false; + if (mObjDetEnabled && mObjDetRunning) + { + NODELET_WARN_STREAM("Object Detection was just running"); - mObjDetConfidence = req.confidence; - mObjDetTracking = req.tracking; - if (req.model < 0 || req.model >= static_cast(sl::OBJECT_DETECTION_MODEL::LAST)) { - NODELET_ERROR_STREAM("Object Detection model not valid."); - res.done = false; - return res.done; - } - mObjDetModel = static_cast(req.model); + res.done = false; + return res.done; + } - mObjDetMaxRange = req.max_range; - if (mObjDetMaxRange > mCamMaxDepth) { - NODELET_WARN("Detection max range cannot be major than depth max range. Automatically fixed."); - mObjDetMaxRange = mCamMaxDepth; - } - NODELET_INFO_STREAM(" * Detection max range\t\t-> " << mObjDetMaxRange); - - NODELET_INFO_STREAM(" * Object min. confidence\t-> " << mObjDetConfidence); - NODELET_INFO_STREAM(" * Object tracking\t\t-> " << (mObjDetTracking ? "ENABLED": "DISABLED")); - NODELET_INFO_STREAM(" * Detection model\t\t-> " << sl::toString(mObjDetModel)); - - mNhNs.getParam("object_detection/mc_people", mObjDetPeopleEnable); - NODELET_INFO_STREAM(" * Detect people\t\t-> " << (mObjDetPeopleEnable ? "ENABLED": "DISABLED")); - mNhNs.getParam("object_detection/mc_vehicle", mObjDetVehiclesEnable); - NODELET_INFO_STREAM(" * Detect vehicles\t\t-> " << (mObjDetVehiclesEnable ? "ENABLED": "DISABLED")); - mNhNs.getParam("object_detection/mc_bag", mObjDetBagsEnable); - NODELET_INFO_STREAM(" * Detect bags\t\t\t-> " << (mObjDetBagsEnable ? "ENABLED": "DISABLED")); - mNhNs.getParam("object_detection/mc_animal", mObjDetAnimalsEnable); - NODELET_INFO_STREAM(" * Detect animals\t\t-> " << (mObjDetAnimalsEnable ? "ENABLED": "DISABLED")); - mNhNs.getParam("object_detection/mc_electronics", mObjDetElectronicsEnable); - NODELET_INFO_STREAM(" * Detect electronics\t\t-> " << (mObjDetElectronicsEnable ? "ENABLED": "DISABLED")); - mNhNs.getParam("object_detection/mc_fruit_vegetable", mObjDetFruitsEnable); - NODELET_INFO_STREAM(" * Detect fruit and vegetables\t-> " << (mObjDetFruitsEnable ? "ENABLED": "DISABLED")); - mNhNs.getParam("object_detection/mc_sport", mObjDetSportsEnable); - NODELET_INFO_STREAM(" * Detect sport-related objects\t-> " << (mObjDetSportsEnable ? "ENABLED": "DISABLED")); - - mObjDetRunning = false; - mObjDetEnabled = true; - res.done = true; + mObjDetRunning = false; + mObjDetConfidence = req.confidence; + mObjDetTracking = req.tracking; + if (req.model < 0 || req.model >= static_cast(sl::OBJECT_DETECTION_MODEL::LAST)) + { + NODELET_ERROR_STREAM("Object Detection model not valid."); + res.done = false; return res.done; + } + mObjDetModel = static_cast(req.model); + + mObjDetMaxRange = req.max_range; + if (mObjDetMaxRange > mCamMaxDepth) + { + NODELET_WARN("Detection max range cannot be major than depth max range. Automatically fixed."); + mObjDetMaxRange = mCamMaxDepth; + } + NODELET_INFO_STREAM(" * Detection max range\t\t-> " << mObjDetMaxRange); + + NODELET_INFO_STREAM(" * Object min. confidence\t-> " << mObjDetConfidence); + NODELET_INFO_STREAM(" * Object tracking\t\t-> " << (mObjDetTracking ? "ENABLED" : "DISABLED")); + NODELET_INFO_STREAM(" * Detection model\t\t-> " << sl::toString(mObjDetModel)); + + mNhNs.getParam("object_detection/mc_people", mObjDetPeopleEnable); + NODELET_INFO_STREAM(" * Detect people\t\t-> " << (mObjDetPeopleEnable ? "ENABLED" : "DISABLED")); + mNhNs.getParam("object_detection/mc_vehicle", mObjDetVehiclesEnable); + NODELET_INFO_STREAM(" * Detect vehicles\t\t-> " << (mObjDetVehiclesEnable ? "ENABLED" : "DISABLED")); + mNhNs.getParam("object_detection/mc_bag", mObjDetBagsEnable); + NODELET_INFO_STREAM(" * Detect bags\t\t\t-> " << (mObjDetBagsEnable ? "ENABLED" : "DISABLED")); + mNhNs.getParam("object_detection/mc_animal", mObjDetAnimalsEnable); + NODELET_INFO_STREAM(" * Detect animals\t\t-> " << (mObjDetAnimalsEnable ? "ENABLED" : "DISABLED")); + mNhNs.getParam("object_detection/mc_electronics", mObjDetElectronicsEnable); + NODELET_INFO_STREAM(" * Detect electronics\t\t-> " << (mObjDetElectronicsEnable ? "ENABLED" : "DISABLED")); + mNhNs.getParam("object_detection/mc_fruit_vegetable", mObjDetFruitsEnable); + NODELET_INFO_STREAM(" * Detect fruit and vegetables\t-> " << (mObjDetFruitsEnable ? "ENABLED" : "DISABLED")); + mNhNs.getParam("object_detection/mc_sport", mObjDetSportsEnable); + NODELET_INFO_STREAM(" * Detect sport-related objects\t-> " << (mObjDetSportsEnable ? "ENABLED" : "DISABLED")); + + mObjDetRunning = false; + mObjDetEnabled = true; + res.done = true; + + return res.done; } /*! \brief Service callback to stop_object_detection service */ bool ZEDWrapperNodelet::on_stop_object_detection(zed_interfaces::stop_object_detection::Request& req, - zed_interfaces::stop_object_detection::Response& res) + zed_interfaces::stop_object_detection::Response& res) { - if (mObjDetEnabled) { - mObjDetMutex.lock(); - stop_obj_detect(); - mObjDetMutex.unlock(); - - res.done = true; - } else { - res.done = false; - } + if (mObjDetEnabled) + { + mObjDetMutex.lock(); + stop_obj_detect(); + mObjDetMutex.unlock(); - return res.done; + res.done = true; + } + else + { + res.done = false; + } + + return res.done; } void ZEDWrapperNodelet::processDetectedObjects(ros::Time t) { - static std::chrono::steady_clock::time_point old_time = std::chrono::steady_clock::now(); + static std::chrono::steady_clock::time_point old_time = std::chrono::steady_clock::now(); - sl::ObjectDetectionRuntimeParameters objectTracker_parameters_rt; - objectTracker_parameters_rt.detection_confidence_threshold = mObjDetConfidence; - objectTracker_parameters_rt.object_class_filter = mObjDetFilter; + sl::ObjectDetectionRuntimeParameters objectTracker_parameters_rt; + objectTracker_parameters_rt.detection_confidence_threshold = mObjDetConfidence; + objectTracker_parameters_rt.object_class_filter = mObjDetFilter; - sl::Objects objects; + sl::Objects objects; - sl::ERROR_CODE objDetRes = mZed.retrieveObjects(objects, objectTracker_parameters_rt); + sl::ERROR_CODE objDetRes = mZed.retrieveObjects(objects, objectTracker_parameters_rt); - if (objDetRes != sl::ERROR_CODE::SUCCESS) { - NODELET_WARN_STREAM("Object Detection error: " << sl::toString(objDetRes)); - return; - } + if (objDetRes != sl::ERROR_CODE::SUCCESS) + { + NODELET_WARN_STREAM("Object Detection error: " << sl::toString(objDetRes)); + return; + } - if (!objects.is_new) { - return; - } + if (!objects.is_new) + { + return; + } - // ----> Diagnostic information update - std::chrono::steady_clock::time_point now = std::chrono::steady_clock::now(); - double elapsed_msec = std::chrono::duration_cast(now - old_time).count(); - mObjDetPeriodMean_msec->addValue(elapsed_msec); - old_time = now; - // <---- Diagnostic information update + // ----> Diagnostic information update + std::chrono::steady_clock::time_point now = std::chrono::steady_clock::now(); + double elapsed_msec = std::chrono::duration_cast(now - old_time).count(); + mObjDetPeriodMean_msec->addValue(elapsed_msec); + old_time = now; + // <---- Diagnostic information update - NODELET_DEBUG_STREAM("Detected " << objects.object_list.size() << " objects"); + NODELET_DEBUG_STREAM("Detected " << objects.object_list.size() << " objects"); - size_t objCount = objects.object_list.size(); + size_t objCount = objects.object_list.size(); - zed_interfaces::ObjectsStampedPtr objMsg = boost::make_shared(); - objMsg->header.stamp = t; - objMsg->header.frame_id = mLeftCamFrameId; + zed_interfaces::ObjectsStampedPtr objMsg = boost::make_shared(); + objMsg->header.stamp = t; + objMsg->header.frame_id = mLeftCamFrameId; - objMsg->objects.resize(objCount); + objMsg->objects.resize(objCount); - size_t idx = 0; - for (auto data: objects.object_list) { - objMsg->objects[idx].label = sl::toString(data.label).c_str(); - objMsg->objects[idx].sublabel = sl::toString(data.sublabel).c_str(); - objMsg->objects[idx].label_id = data.raw_label; - objMsg->objects[idx].instance_id = data.id; - objMsg->objects[idx].confidence = data.confidence; + size_t idx = 0; + for (auto data : objects.object_list) + { + objMsg->objects[idx].label = sl::toString(data.label).c_str(); + objMsg->objects[idx].sublabel = sl::toString(data.sublabel).c_str(); + objMsg->objects[idx].label_id = data.raw_label; + objMsg->objects[idx].instance_id = data.id; + objMsg->objects[idx].confidence = data.confidence; - memcpy(&(objMsg->objects[idx].position[0]), &(data.position[0]), 3 * sizeof(float)); - memcpy(&(objMsg->objects[idx].position_covariance[0]), &(data.position_covariance[0]), 6 * sizeof(float)); - memcpy(&(objMsg->objects[idx].velocity[0]), &(data.velocity[0]), 3 * sizeof(float)); + memcpy(&(objMsg->objects[idx].position[0]), &(data.position[0]), 3 * sizeof(float)); + memcpy(&(objMsg->objects[idx].position_covariance[0]), &(data.position_covariance[0]), 6 * sizeof(float)); + memcpy(&(objMsg->objects[idx].velocity[0]), &(data.velocity[0]), 3 * sizeof(float)); - objMsg->objects[idx].tracking_available = mObjDetTracking; - objMsg->objects[idx].tracking_state = static_cast(data.tracking_state); - // NODELET_INFO_STREAM( "[" << idx << "] Tracking: " << - // sl::toString(static_cast(data.tracking_state))); - objMsg->objects[idx].action_state = static_cast(data.action_state); + objMsg->objects[idx].tracking_available = mObjDetTracking; + objMsg->objects[idx].tracking_state = static_cast(data.tracking_state); + // NODELET_INFO_STREAM( "[" << idx << "] Tracking: " << + // sl::toString(static_cast(data.tracking_state))); + objMsg->objects[idx].action_state = static_cast(data.action_state); - if (data.bounding_box_2d.size() == 4) { - memcpy(&(objMsg->objects[idx].bounding_box_2d.corners[0]), &(data.bounding_box_2d[0]), 8 * sizeof(unsigned int)); - } - if (data.bounding_box.size() == 8) { - memcpy(&(objMsg->objects[idx].bounding_box_3d.corners[0]), &(data.bounding_box[0]), 24 * sizeof(float)); - } + if (data.bounding_box_2d.size() == 4) + { + memcpy(&(objMsg->objects[idx].bounding_box_2d.corners[0]), &(data.bounding_box_2d[0]), 8 * sizeof(unsigned int)); + } + if (data.bounding_box.size() == 8) + { + memcpy(&(objMsg->objects[idx].bounding_box_3d.corners[0]), &(data.bounding_box[0]), 24 * sizeof(float)); + } - memcpy(&(objMsg->objects[idx].dimensions_3d[0]), &(data.dimensions[0]), 3 * sizeof(float)); + memcpy(&(objMsg->objects[idx].dimensions_3d[0]), &(data.dimensions[0]), 3 * sizeof(float)); - // Body Detection is in a separate module in ZED SDK v4 - objMsg->objects[idx].skeleton_available = false; + // Body Detection is in a separate module in ZED SDK v4 + objMsg->objects[idx].skeleton_available = false; - // at the end of the loop - idx++; - } + // at the end of the loop + idx++; + } - mPubObjDet.publish(objMsg); + mPubObjDet.publish(objMsg); } void ZEDWrapperNodelet::clickedPtCallback(geometry_msgs::PointStampedConstPtr msg) { - // ----> Check for result subscribers - uint32_t markerSubNumber = mPubMarker.getNumSubscribers(); - uint32_t planeSubNumber = mPubMarker.getNumSubscribers(); + // ----> Check for result subscribers + uint32_t markerSubNumber = mPubMarker.getNumSubscribers(); + uint32_t planeSubNumber = mPubMarker.getNumSubscribers(); - if ((markerSubNumber + planeSubNumber) == 0) { - return; - } - // <---- Check for result subscribers - - ros::Time ts = ros::Time::now(); + if ((markerSubNumber + planeSubNumber) == 0) + { + return; + } + // <---- Check for result subscribers - float X = msg->point.x; - float Y = msg->point.y; - float Z = msg->point.z; + ros::Time ts = ros::Time::now(); - NODELET_INFO_STREAM("Clicked 3D point [X FW, Y LF, Z UP]: [" << X << "," << Y << "," << Z << "]"); + float X = msg->point.x; + float Y = msg->point.y; + float Z = msg->point.z; - // ----> Transform the point from `map` frame to `left_camera_optical_frame` - double camX, camY, camZ; - try { - // Save the transformation - geometry_msgs::TransformStamped m2o = mTfBuffer->lookupTransform(mLeftCamOptFrameId, msg->header.frame_id, ros::Time(0), ros::Duration(0.1)); + NODELET_INFO_STREAM("Clicked 3D point [X FW, Y LF, Z UP]: [" << X << "," << Y << "," << Z << "]"); - NODELET_INFO("'%s' -> '%s': {%.3f,%.3f,%.3f} {%.3f,%.3f,%.3f,%.3f}", msg->header.frame_id.c_str(), mLeftCamOptFrameId.c_str(), - m2o.transform.translation.x, m2o.transform.translation.y, m2o.transform.translation.z, - m2o.transform.rotation.x, m2o.transform.rotation.y, m2o.transform.rotation.z, m2o.transform.rotation.w); + // ----> Transform the point from `map` frame to `left_camera_optical_frame` + double camX, camY, camZ; + try + { + // Save the transformation + geometry_msgs::TransformStamped m2o = + mTfBuffer->lookupTransform(mLeftCamOptFrameId, msg->header.frame_id, ros::Time(0), ros::Duration(0.1)); - // Get the TF2 transformation - geometry_msgs::PointStamped ptCam; + NODELET_INFO("'%s' -> '%s': {%.3f,%.3f,%.3f} {%.3f,%.3f,%.3f,%.3f}", msg->header.frame_id.c_str(), + mLeftCamOptFrameId.c_str(), m2o.transform.translation.x, m2o.transform.translation.y, + m2o.transform.translation.z, m2o.transform.rotation.x, m2o.transform.rotation.y, + m2o.transform.rotation.z, m2o.transform.rotation.w); - tf2::doTransform(*msg, ptCam, m2o); + // Get the TF2 transformation + geometry_msgs::PointStamped ptCam; - camX = ptCam.point.x; - camY = ptCam.point.y; - camZ = ptCam.point.z; + tf2::doTransform(*msg, ptCam, m2o); - NODELET_INFO("Point in camera coordinates [Z FW, X RG, Y DW]: {%.3f,%.3f,%.3f}", camX, camY, camZ); - } catch (tf2::TransformException& ex) { - NODELET_DEBUG_THROTTLE(1.0, "Transform error: %s", ex.what()); - NODELET_WARN_THROTTLE(1.0, "The tf from '%s' to '%s' is not available.", msg->header.frame_id.c_str(), mLeftCamOptFrameId.c_str()); + camX = ptCam.point.x; + camY = ptCam.point.y; + camZ = ptCam.point.z; - return; - } - // <---- Transform the point from `map` frame to `left_camera_optical_frame` - - // ----> Project the point into 2D image coordinates - sl::CalibrationParameters zedParam; - zedParam = mZed.getCameraInformation(mMatResol).camera_configuration.calibration_parameters; // ok - - float f_x = zedParam.left_cam.fx; - float f_y = zedParam.left_cam.fy; - float c_x = zedParam.left_cam.cx; - float c_y = zedParam.left_cam.cy; - - float out_scale_factor = mMatResol.width / mCamWidth; - - float u = ((camX / camZ) * f_x + c_x) / out_scale_factor; - float v = ((camY / camZ) * f_y + c_y) / out_scale_factor; - NODELET_INFO_STREAM("Clicked point image coordinates: [" << u << "," << v << "]"); - // <---- Project the point into 2D image coordinates - - // ----> Extract plane from clicked point - sl::Plane plane; - sl::ERROR_CODE err = mZed.findPlaneAtHit(sl::uint2(u, v), plane); - if (err != sl::ERROR_CODE::SUCCESS) { - NODELET_WARN("Error extracting plane at point [%.3f,%.3f,%.3f]: %s", X, Y, Z, sl::toString(err).c_str()); - return; - } + NODELET_INFO("Point in camera coordinates [Z FW, X RG, Y DW]: {%.3f,%.3f,%.3f}", camX, camY, camZ); + } + catch (tf2::TransformException& ex) + { + NODELET_DEBUG_THROTTLE(1.0, "Transform error: %s", ex.what()); + NODELET_WARN_THROTTLE(1.0, "The tf from '%s' to '%s' is not available.", msg->header.frame_id.c_str(), + mLeftCamOptFrameId.c_str()); - sl::float3 center = plane.getCenter(); - sl::float2 dims = plane.getExtents(); + return; + } + // <---- Transform the point from `map` frame to `left_camera_optical_frame` + + // ----> Project the point into 2D image coordinates + sl::CalibrationParameters zedParam; + zedParam = mZed.getCameraInformation(mMatResol).camera_configuration.calibration_parameters; // ok + + float f_x = zedParam.left_cam.fx; + float f_y = zedParam.left_cam.fy; + float c_x = zedParam.left_cam.cx; + float c_y = zedParam.left_cam.cy; + + float out_scale_factor = mMatResol.width / mCamWidth; + + float u = ((camX / camZ) * f_x + c_x) / out_scale_factor; + float v = ((camY / camZ) * f_y + c_y) / out_scale_factor; + NODELET_INFO_STREAM("Clicked point image coordinates: [" << u << "," << v << "]"); + // <---- Project the point into 2D image coordinates + + // ----> Extract plane from clicked point + sl::Plane plane; + sl::ERROR_CODE err = mZed.findPlaneAtHit(sl::uint2(u, v), plane); + if (err != sl::ERROR_CODE::SUCCESS) + { + NODELET_WARN("Error extracting plane at point [%.3f,%.3f,%.3f]: %s", X, Y, Z, sl::toString(err).c_str()); + return; + } - if (dims[0] == 0 || dims[1] == 0) { - NODELET_INFO("Plane not found at point [%.3f,%.3f,%.3f]", X, Y, Z); - return; - } + sl::float3 center = plane.getCenter(); + sl::float2 dims = plane.getExtents(); - NODELET_INFO("Found plane at point [%.3f,%.3f,%.3f] -> Center: [%.3f,%.3f,%.3f], Dims: %.3fx%.3f", X, Y, Z, center.x, center.y, center.z, dims[0], dims[1]); - // <---- Extract plane from clicked point - - if (markerSubNumber > 0) { - // ----> Publish a blue sphere in the clicked point - visualization_msgs::MarkerPtr pt_marker = boost::make_shared(); - // Set the frame ID and timestamp. See the TF tutorials for information on these. - static int hit_pt_id = 0; - pt_marker->header.stamp = ts; - // Set the marker action. Options are ADD and DELETE - pt_marker->action = visualization_msgs::Marker::ADD; - pt_marker->lifetime = ros::Duration(); - - // Set the namespace and id for this marker. This serves to create a unique ID - // Any marker sent with the same namespace and id will overwrite the old one - pt_marker->ns = "plane_hit_points"; - pt_marker->id = hit_pt_id++; - pt_marker->header.frame_id = mMapFrameId; - - // Set the marker type. - pt_marker->type = visualization_msgs::Marker::SPHERE; - - // Set the pose of the marker. This is a full 6DOF pose relative to the frame/time specified in the header - pt_marker->pose.position.x = X; - pt_marker->pose.position.y = Y; - pt_marker->pose.position.z = Z; - pt_marker->pose.orientation.x = 0.0; - pt_marker->pose.orientation.y = 0.0; - pt_marker->pose.orientation.z = 0.0; - pt_marker->pose.orientation.w = 1.0; - - // Set the scale of the marker -- 1x1x1 here means 1m on a side - pt_marker->scale.x = 0.025; - pt_marker->scale.y = 0.025; - pt_marker->scale.z = 0.025; + if (dims[0] == 0 || dims[1] == 0) + { + NODELET_INFO("Plane not found at point [%.3f,%.3f,%.3f]", X, Y, Z); + return; + } - // Set the color -- be sure to set alpha to something non-zero! - pt_marker->color.r = 0.2f; - pt_marker->color.g = 0.1f; - pt_marker->color.b = 0.75f; - pt_marker->color.a = 0.8; - - // Publish the marker - mPubMarker.publish(pt_marker); - // ----> Publish a blue sphere in the clicked point - - // ----> Publish the plane as green mesh - visualization_msgs::MarkerPtr plane_marker = boost::make_shared(); - // Set the frame ID and timestamp. See the TF tutorials for information on these. - static int plane_mesh_id = 0; - plane_marker->header.stamp = ts; - // Set the marker action. Options are ADD and DELETE - plane_marker->action = visualization_msgs::Marker::ADD; - plane_marker->lifetime = ros::Duration(); - - // Set the namespace and id for this marker. This serves to create a unique ID - // Any marker sent with the same namespace and id will overwrite the old one - plane_marker->ns = "plane_meshes"; - plane_marker->id = plane_mesh_id++; - plane_marker->header.frame_id = mLeftCamFrameId; - - // Set the marker type. - plane_marker->type = visualization_msgs::Marker::TRIANGLE_LIST; - - // Set the pose of the marker. This is a full 6DOF pose relative to the frame/time specified in the header - plane_marker->pose.position.x = 0; - plane_marker->pose.position.y = 0; - plane_marker->pose.position.z = 0; - plane_marker->pose.orientation.x = 0.0; - plane_marker->pose.orientation.y = 0.0; - plane_marker->pose.orientation.z = 0.0; - plane_marker->pose.orientation.w = 1.0; + NODELET_INFO("Found plane at point [%.3f,%.3f,%.3f] -> Center: [%.3f,%.3f,%.3f], Dims: %.3fx%.3f", X, Y, Z, center.x, + center.y, center.z, dims[0], dims[1]); + // <---- Extract plane from clicked point + + if (markerSubNumber > 0) + { + // ----> Publish a blue sphere in the clicked point + visualization_msgs::MarkerPtr pt_marker = boost::make_shared(); + // Set the frame ID and timestamp. See the TF tutorials for information on these. + static int hit_pt_id = 0; + pt_marker->header.stamp = ts; + // Set the marker action. Options are ADD and DELETE + pt_marker->action = visualization_msgs::Marker::ADD; + pt_marker->lifetime = ros::Duration(); + + // Set the namespace and id for this marker. This serves to create a unique ID + // Any marker sent with the same namespace and id will overwrite the old one + pt_marker->ns = "plane_hit_points"; + pt_marker->id = hit_pt_id++; + pt_marker->header.frame_id = mMapFrameId; + + // Set the marker type. + pt_marker->type = visualization_msgs::Marker::SPHERE; + + // Set the pose of the marker. This is a full 6DOF pose relative to the frame/time specified in the header + pt_marker->pose.position.x = X; + pt_marker->pose.position.y = Y; + pt_marker->pose.position.z = Z; + pt_marker->pose.orientation.x = 0.0; + pt_marker->pose.orientation.y = 0.0; + pt_marker->pose.orientation.z = 0.0; + pt_marker->pose.orientation.w = 1.0; + + // Set the scale of the marker -- 1x1x1 here means 1m on a side + pt_marker->scale.x = 0.025; + pt_marker->scale.y = 0.025; + pt_marker->scale.z = 0.025; + + // Set the color -- be sure to set alpha to something non-zero! + pt_marker->color.r = 0.2f; + pt_marker->color.g = 0.1f; + pt_marker->color.b = 0.75f; + pt_marker->color.a = 0.8; + + // Publish the marker + mPubMarker.publish(pt_marker); + // ----> Publish a blue sphere in the clicked point + + // ----> Publish the plane as green mesh + visualization_msgs::MarkerPtr plane_marker = boost::make_shared(); + // Set the frame ID and timestamp. See the TF tutorials for information on these. + static int plane_mesh_id = 0; + plane_marker->header.stamp = ts; + // Set the marker action. Options are ADD and DELETE + plane_marker->action = visualization_msgs::Marker::ADD; + plane_marker->lifetime = ros::Duration(); + + // Set the namespace and id for this marker. This serves to create a unique ID + // Any marker sent with the same namespace and id will overwrite the old one + plane_marker->ns = "plane_meshes"; + plane_marker->id = plane_mesh_id++; + plane_marker->header.frame_id = mLeftCamFrameId; + + // Set the marker type. + plane_marker->type = visualization_msgs::Marker::TRIANGLE_LIST; + + // Set the pose of the marker. This is a full 6DOF pose relative to the frame/time specified in the header + plane_marker->pose.position.x = 0; + plane_marker->pose.position.y = 0; + plane_marker->pose.position.z = 0; + plane_marker->pose.orientation.x = 0.0; + plane_marker->pose.orientation.y = 0.0; + plane_marker->pose.orientation.z = 0.0; + plane_marker->pose.orientation.w = 1.0; + + // Set the color -- be sure to set alpha to something non-zero! + plane_marker->color.r = 0.10f; + plane_marker->color.g = 0.75f; + plane_marker->color.b = 0.20f; + plane_marker->color.a = 0.75; + + // Set the scale of the marker -- 1x1x1 here means 1m on a side + plane_marker->scale.x = 1.0; + plane_marker->scale.y = 1.0; + plane_marker->scale.z = 1.0; + + sl::Mesh mesh = plane.extractMesh(); + size_t triangCount = mesh.getNumberOfTriangles(); + size_t ptCount = triangCount * 3; + plane_marker->points.resize(ptCount); + plane_marker->colors.resize(ptCount); + + size_t ptIdx = 0; + for (size_t t = 0; t < triangCount; t++) + { + for (int p = 0; p < 3; p++) + { + uint vIdx = mesh.triangles[t][p]; + plane_marker->points[ptIdx].x = mesh.vertices[vIdx][0]; + plane_marker->points[ptIdx].y = mesh.vertices[vIdx][1]; + plane_marker->points[ptIdx].z = mesh.vertices[vIdx][2]; // Set the color -- be sure to set alpha to something non-zero! - plane_marker->color.r = 0.10f; - plane_marker->color.g = 0.75f; - plane_marker->color.b = 0.20f; - plane_marker->color.a = 0.75; - - // Set the scale of the marker -- 1x1x1 here means 1m on a side - plane_marker->scale.x = 1.0; - plane_marker->scale.y = 1.0; - plane_marker->scale.z = 1.0; - - sl::Mesh mesh = plane.extractMesh(); - size_t triangCount = mesh.getNumberOfTriangles(); - size_t ptCount = triangCount * 3; - plane_marker->points.resize(ptCount); - plane_marker->colors.resize(ptCount); - - size_t ptIdx = 0; - for (size_t t = 0; t < triangCount; t++) { - for (int p = 0; p < 3; p++) { - uint vIdx = mesh.triangles[t][p]; - plane_marker->points[ptIdx].x = mesh.vertices[vIdx][0]; - plane_marker->points[ptIdx].y = mesh.vertices[vIdx][1]; - plane_marker->points[ptIdx].z = mesh.vertices[vIdx][2]; - - // Set the color -- be sure to set alpha to something non-zero! - plane_marker->colors[ptIdx].r = 0.10f; - plane_marker->colors[ptIdx].g = 0.75f; - plane_marker->colors[ptIdx].b = 0.20f; - plane_marker->colors[ptIdx].a = 0.75; - - ptIdx++; - } - } + plane_marker->colors[ptIdx].r = 0.10f; + plane_marker->colors[ptIdx].g = 0.75f; + plane_marker->colors[ptIdx].b = 0.20f; + plane_marker->colors[ptIdx].a = 0.75; - // Publish the marker - mPubMarker.publish(plane_marker); - // <---- Publish the plane as green mesh + ptIdx++; + } } - if (planeSubNumber > 0) { - // ----> Publish the plane as custom message - - zed_interfaces::PlaneStampedPtr planeMsg = boost::make_shared(); - planeMsg->header.stamp = ts; - planeMsg->header.frame_id = mLeftCamFrameId; - - // Plane equation - sl::float4 sl_coeff = plane.getPlaneEquation(); - planeMsg->coefficients.coef[0] = static_cast(sl_coeff[0]); - planeMsg->coefficients.coef[1] = static_cast(sl_coeff[1]); - planeMsg->coefficients.coef[2] = static_cast(sl_coeff[2]); - planeMsg->coefficients.coef[3] = static_cast(sl_coeff[3]); - - // Plane Normal - sl::float3 sl_normal = plane.getNormal(); - planeMsg->normal.x = sl_normal[0]; - planeMsg->normal.y = sl_normal[1]; - planeMsg->normal.z = sl_normal[2]; - - // Plane Center - sl::float3 sl_center = plane.getCenter(); - planeMsg->center.x = sl_center[0]; - planeMsg->center.y = sl_center[1]; - planeMsg->center.z = sl_center[2]; - - // Plane extents - sl::float3 sl_extents = plane.getExtents(); - planeMsg->extents[0] = sl_extents[0]; - planeMsg->extents[1] = sl_extents[1]; - - // Plane pose - sl::Pose sl_pose = plane.getPose(); - sl::Orientation sl_rot = sl_pose.getOrientation(); - sl::Translation sl_tr = sl_pose.getTranslation(); - - planeMsg->pose.rotation.x = sl_rot.ox; - planeMsg->pose.rotation.y = sl_rot.oy; - planeMsg->pose.rotation.z = sl_rot.oz; - planeMsg->pose.rotation.w = sl_rot.ow; - - planeMsg->pose.translation.x = sl_tr.x; - planeMsg->pose.translation.y = sl_tr.y; - planeMsg->pose.translation.z = sl_tr.z; - - // Plane Bounds - std::vector sl_bounds = plane.getBounds(); - planeMsg->bounds.points.resize(sl_bounds.size()); - memcpy(planeMsg->bounds.points.data(), sl_bounds.data(), 3 * sl_bounds.size() * sizeof(float)); - - // Plane mesh - sl::Mesh sl_mesh = plane.extractMesh(); - size_t triangCount = sl_mesh.triangles.size(); - size_t ptsCount = sl_mesh.vertices.size(); - planeMsg->mesh.triangles.resize(triangCount); - planeMsg->mesh.vertices.resize(ptsCount); - - // memcpy not allowed because data types are different - for (size_t i = 0; i < triangCount; i++) { - planeMsg->mesh.triangles[i].vertex_indices[0] = sl_mesh.triangles[i][0]; - planeMsg->mesh.triangles[i].vertex_indices[1] = sl_mesh.triangles[i][1]; - planeMsg->mesh.triangles[i].vertex_indices[2] = sl_mesh.triangles[i][2]; - } + // Publish the marker + mPubMarker.publish(plane_marker); + // <---- Publish the plane as green mesh + } - // memcpy not allowed because data types are different - for (size_t i = 0; i < ptsCount; i++) { - planeMsg->mesh.vertices[i].x = sl_mesh.vertices[i][0]; - planeMsg->mesh.vertices[i].y = sl_mesh.vertices[i][1]; - planeMsg->mesh.vertices[i].z = sl_mesh.vertices[i][2]; - } + if (planeSubNumber > 0) + { + // ----> Publish the plane as custom message + + zed_interfaces::PlaneStampedPtr planeMsg = boost::make_shared(); + planeMsg->header.stamp = ts; + planeMsg->header.frame_id = mLeftCamFrameId; + + // Plane equation + sl::float4 sl_coeff = plane.getPlaneEquation(); + planeMsg->coefficients.coef[0] = static_cast(sl_coeff[0]); + planeMsg->coefficients.coef[1] = static_cast(sl_coeff[1]); + planeMsg->coefficients.coef[2] = static_cast(sl_coeff[2]); + planeMsg->coefficients.coef[3] = static_cast(sl_coeff[3]); + + // Plane Normal + sl::float3 sl_normal = plane.getNormal(); + planeMsg->normal.x = sl_normal[0]; + planeMsg->normal.y = sl_normal[1]; + planeMsg->normal.z = sl_normal[2]; + + // Plane Center + sl::float3 sl_center = plane.getCenter(); + planeMsg->center.x = sl_center[0]; + planeMsg->center.y = sl_center[1]; + planeMsg->center.z = sl_center[2]; + + // Plane extents + sl::float3 sl_extents = plane.getExtents(); + planeMsg->extents[0] = sl_extents[0]; + planeMsg->extents[1] = sl_extents[1]; + + // Plane pose + sl::Pose sl_pose = plane.getPose(); + sl::Orientation sl_rot = sl_pose.getOrientation(); + sl::Translation sl_tr = sl_pose.getTranslation(); + + planeMsg->pose.rotation.x = sl_rot.ox; + planeMsg->pose.rotation.y = sl_rot.oy; + planeMsg->pose.rotation.z = sl_rot.oz; + planeMsg->pose.rotation.w = sl_rot.ow; + + planeMsg->pose.translation.x = sl_tr.x; + planeMsg->pose.translation.y = sl_tr.y; + planeMsg->pose.translation.z = sl_tr.z; + + // Plane Bounds + std::vector sl_bounds = plane.getBounds(); + planeMsg->bounds.points.resize(sl_bounds.size()); + memcpy(planeMsg->bounds.points.data(), sl_bounds.data(), 3 * sl_bounds.size() * sizeof(float)); + + // Plane mesh + sl::Mesh sl_mesh = plane.extractMesh(); + size_t triangCount = sl_mesh.triangles.size(); + size_t ptsCount = sl_mesh.vertices.size(); + planeMsg->mesh.triangles.resize(triangCount); + planeMsg->mesh.vertices.resize(ptsCount); + + // memcpy not allowed because data types are different + for (size_t i = 0; i < triangCount; i++) + { + planeMsg->mesh.triangles[i].vertex_indices[0] = sl_mesh.triangles[i][0]; + planeMsg->mesh.triangles[i].vertex_indices[1] = sl_mesh.triangles[i][1]; + planeMsg->mesh.triangles[i].vertex_indices[2] = sl_mesh.triangles[i][2]; + } - mPubPlane.publish(planeMsg); - // <---- Publish the plane as custom message + // memcpy not allowed because data types are different + for (size_t i = 0; i < ptsCount; i++) + { + planeMsg->mesh.vertices[i].x = sl_mesh.vertices[i][0]; + planeMsg->mesh.vertices[i].y = sl_mesh.vertices[i][1]; + planeMsg->mesh.vertices[i].z = sl_mesh.vertices[i][2]; } + + mPubPlane.publish(planeMsg); + // <---- Publish the plane as custom message + } } -} // namespace zed_nodelets +} // namespace zed_nodelets From b5da35c2eadbf51033b1eaa50866b661de9fbe3a Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Wed, 13 Sep 2023 11:58:16 +0200 Subject: [PATCH 172/199] Add `set_roi` and `reset_roi` services --- CHANGELOG.rst | 6 + zed-ros-interfaces | 2 +- .../include/zed_wrapper_nodelet.hpp | 16 ++ .../zed_nodelet/src/zed_wrapper_nodelet.cpp | 143 ++++++++++++++++-- 4 files changed, 150 insertions(+), 17 deletions(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 6bd26187..1a1efe29 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -1,6 +1,12 @@ CHANGELOG ========= +2023-09-13 +---------- +- Add ROS '.clang-format' +- Code refactoring +- Add `set_roi` and `reset_roi` services + 2023-09-12 ---------- - Change the parameter 'general/resolution' to 'general/grab_resolution' and replace numeric values with strings diff --git a/zed-ros-interfaces b/zed-ros-interfaces index 73bfaa13..206b7e2d 160000 --- a/zed-ros-interfaces +++ b/zed-ros-interfaces @@ -1 +1 @@ -Subproject commit 73bfaa136c408afd3869d7877e4a95bbf412cc09 +Subproject commit 206b7e2d30505176f079168fbed45ed2dc916000 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 73354370..925b5bde 100644 --- a/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp +++ b/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp @@ -58,6 +58,8 @@ #include #include #include +#include +#include // Topics #include @@ -121,6 +123,10 @@ class ZEDWrapperNodelet : public nodelet::Nodelet */ virtual void onInit(); + /*! \brief Initialize services + */ + void initServices(); + /*! \brief Reads parameters from the param server */ void readParameters(); @@ -344,6 +350,14 @@ class ZEDWrapperNodelet : public nodelet::Nodelet bool on_stop_remote_stream(zed_interfaces::stop_remote_stream::Request& req, zed_interfaces::stop_remote_stream::Response& res); + /*! \brief Service callback to set_roi service + */ + bool on_set_roi(zed_interfaces::set_roi::Request& req, zed_interfaces::set_roi::Response& res); + + /*! \brief Service callback to reset_roi service + */ + bool on_reset_roi(zed_interfaces::reset_roi::Request& req, zed_interfaces::reset_roi::Response& res); + /*! \brief Service callback to set_led_status service */ bool on_set_led_status(zed_interfaces::set_led_status::Request& req, zed_interfaces::set_led_status::Response& res); @@ -525,6 +539,8 @@ class ZEDWrapperNodelet : public nodelet::Nodelet ros::ServiceServer mSrvStartObjDet; ros::ServiceServer mSrvStopObjDet; ros::ServiceServer mSrvSaveAreaMemory; + ros::ServiceServer mSrvSetRoi; + ros::ServiceServer mSrvResetRoi; // ----> Topics (ONLY THOSE NOT CHANGING WHILE NODE RUNS) // Camera info 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 717357e8..913ec771 100644 --- a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp @@ -614,6 +614,28 @@ void ZEDWrapperNodelet::onInit() // <---- Subscribers // ----> Services + initServices(); + // <---- Services + + // ----> Threads + if (!mDepthDisabled) + { + // Start Pointcloud thread + mPcThread = std::thread(&ZEDWrapperNodelet::pointcloud_thread_func, this); + } + + // Start pool thread + mDevicePollThread = std::thread(&ZEDWrapperNodelet::device_poll_thread_func, this); + + // Start Sensors thread + mSensThread = std::thread(&ZEDWrapperNodelet::sensors_thread_func, this); + // <---- Threads + + NODELET_INFO("+++ ZED Node started +++"); +} + +void ZEDWrapperNodelet::initServices() +{ NODELET_INFO("*** SERVICES ***"); if (!mDepthDisabled) { @@ -657,23 +679,11 @@ void ZEDWrapperNodelet::onInit() NODELET_INFO_STREAM(" * Advertised on service " << mSrvSvoStartStream.getService().c_str()); mSrvSvoStopStream = mNhNs.advertiseService("stop_remote_stream", &ZEDWrapperNodelet::on_stop_remote_stream, this); NODELET_INFO_STREAM(" * Advertised on service " << mSrvSvoStopStream.getService().c_str()); - // <---- Services - - // ----> Threads - if (!mDepthDisabled) - { - // Start Pointcloud thread - mPcThread = std::thread(&ZEDWrapperNodelet::pointcloud_thread_func, this); - } - - // Start pool thread - mDevicePollThread = std::thread(&ZEDWrapperNodelet::device_poll_thread_func, this); - - // Start Sensors thread - mSensThread = std::thread(&ZEDWrapperNodelet::sensors_thread_func, this); - // <---- Threads - NODELET_INFO("+++ ZED Node started +++"); + mSrvSetRoi = mNhNs.advertiseService("set_roi", &ZEDWrapperNodelet::on_set_roi, this); + NODELET_INFO_STREAM(" * Advertised on service " << mSrvSetRoi.getService().c_str()); + mSrvResetRoi = mNhNs.advertiseService("reset_roi", &ZEDWrapperNodelet::on_reset_roi, this); + NODELET_INFO_STREAM(" * Advertised on service " << mSrvSetRoi.getService().c_str()); } void ZEDWrapperNodelet::readGeneralParams() @@ -5148,6 +5158,107 @@ bool ZEDWrapperNodelet::on_toggle_led(zed_interfaces::toggle_led::Request& req, return true; } +bool ZEDWrapperNodelet::on_set_roi(zed_interfaces::set_roi::Request& req, zed_interfaces::set_roi::Response& res) +{ + NODELET_INFO("** Set ROI service called **"); + NODELET_INFO_STREAM(" * ROI string: " << req.roi.c_str()); + + if (req.roi == "") + { + std::string err_msg = + "Error while setting ZED SDK region of interest: a vector of normalized points describing a " + "polygon is required. e.g. '[[0.5,0.25],[0.75,0.5],[0.5,0.75],[0.25,0.5]]'"; + + NODELET_WARN_STREAM(" * " << err_msg); + + res.message = err_msg; + res.success = false; + return false; + } + + std::string error; + std::vector> parsed_poly = sl_tools::parseStringVector(req.roi, error); + + if (error != "") + { + std::string err_msg = "Error while setting ZED SDK region of interest: "; + err_msg += error; + + NODELET_WARN_STREAM(" * " << err_msg); + + res.message = err_msg; + res.success = false; + return false; + } + + // ----> Set Region of Interest + // Create mask + NODELET_INFO(" * Setting ROI"); + std::vector 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); + if (!sl_tools::generateROI(sl_poly, roi_mask)) + { + std::string err_msg = "Error generating the region of interest image mask. "; + err_msg += error; + + NODELET_WARN_STREAM(" * " << err_msg); + + res.message = err_msg; + res.success = false; + return false; + } + else + { + sl::ERROR_CODE err = mZed.setRegionOfInterest(roi_mask); + if (err != sl::ERROR_CODE::SUCCESS) + { + std::string err_msg = "Error while setting ZED SDK region of interest: "; + err_msg += sl::toString(err).c_str(); + + NODELET_WARN_STREAM(" * " << err_msg); + + res.message = err_msg; + res.success = false; + return false; + } + else + { + NODELET_INFO(" * Region of Interest correctly set."); + + res.message = "Region of Interest correctly set."; + res.success = true; + return true; + } + } + // <---- Set Region of Interest +} + +bool ZEDWrapperNodelet::on_reset_roi(zed_interfaces::reset_roi::Request& req, zed_interfaces::reset_roi::Response& res) +{ + NODELET_INFO("** Reset ROI service called **"); + + sl::Mat empty_roi; + sl::ERROR_CODE err = mZed.setRegionOfInterest(empty_roi); + + if (err != sl::ERROR_CODE::SUCCESS) + { + std::string err_msg = " * Error while resetting ZED SDK region of interest: "; + err_msg += sl::toString(err); + NODELET_WARN_STREAM(" * Error while resetting ZED SDK region of interest: " << err_msg); + res.reset_done = false; + return false; + } + else + { + NODELET_INFO(" * Region of Interest correctly reset."); + res.reset_done = true; + return true; + } +} + bool ZEDWrapperNodelet::on_start_3d_mapping(zed_interfaces::start_3d_mapping::Request& req, zed_interfaces::start_3d_mapping::Response& res) { From b6ea6332fed7415ee5201765ac45c9c180e036ac Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Wed, 13 Sep 2023 18:33:06 +0200 Subject: [PATCH 173/199] Add parameter 'pos_tracking/depth_min_range' Add parameter 'pos_tracking/set_as_static' --- CHANGELOG.rst | 148 +++++++++--------- .../include/zed_wrapper_nodelet.hpp | 21 +-- .../zed_nodelet/src/zed_wrapper_nodelet.cpp | 53 ++++--- zed_wrapper/params/common.yaml | 2 +- 4 files changed, 118 insertions(+), 106 deletions(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 1a1efe29..00ccb922 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -5,7 +5,9 @@ CHANGELOG ---------- - Add ROS '.clang-format' - Code refactoring -- Add `set_roi` and `reset_roi` services +- Add 'set_roi' and 'reset_roi' services +- Add parameter 'pos_tracking/depth_min_range' +- Add parameter 'pos_tracking/set_as_static' 2023-09-12 ---------- @@ -23,39 +25,39 @@ CHANGELOG 2023-09-08 ---------- -- Add `pos_tracking/set_gravity_as_origin` parameter. If 'true' align the positional tracking world to imu gravity measurement. Keep the yaw from the user initial pose. -- Add `pos_tracking/pos_tracking_mode` parameter. Matches the ZED SDK setting: 'QUALITY', 'STANDARD' -- Fix the warning `Elaboration takes longer [...]` +- Add 'pos_tracking/set_gravity_as_origin' parameter. If 'true' align the positional tracking world to imu gravity measurement. Keep the yaw from the user initial pose. +- Add 'pos_tracking/pos_tracking_mode' parameter. Matches the ZED SDK setting: 'QUALITY', 'STANDARD' +- Fix the warning 'Elaboration takes longer [...]' 2023-09-07 ---------- -- `pub_frame_rate` now controls the `InitParameters::grab_compute_capping_fps` parameter of the ZED SDK instead of controlling the frequency of a parallel thread. It's not a Dynamic parameter anymore. -- Change `general/camera_flip` parameter to string: 'AUTO', 'ON', 'OFF' -- Change `general/verbose` from bool to integer +- 'pub_frame_rate' now controls the 'InitParameters::grab_compute_capping_fps' parameter of the ZED SDK instead of controlling the frequency of a parallel thread. It's not a Dynamic parameter anymore. +- Change 'general/camera_flip' parameter to string: 'AUTO', 'ON', 'OFF' +- Change 'general/verbose' from bool to integer v4.0.5 ------ - Support for ZED SDK v4.0 -- Remove parameter `object_detection.body_fitting` -- Remove parameter `depth.sensing_mode` -- Remove parameter `video.extrinsic_in_camera_frame` +- Remove parameter 'object_detection.body_fitting' +- Remove parameter 'depth.sensing_mode' +- Remove parameter 'video.extrinsic_in_camera_frame' - Skeleton Tracking is no more available. Migrate to ROS 2 Wrapper if you need it -- `sensors` and `object_detection` parameters are now in `common.yaml` -- Move parameters `general.resolution` and `general.grab_frame_rate` to cameras yaml files to support the different configurations on ZED X and ZED X Mini. +- 'sensors' and 'object_detection' parameters are now in 'common.yaml' +- Move parameters 'general.resolution' and 'general.grab_frame_rate' to cameras yaml files to support the different configurations on ZED X and ZED X Mini. - Remove support for ROS Melodic that reached EOL - Add 1080p for ZED X and ZED X Mini v3.8.x ------ - Fix the frame links of barometer, magnetometer, and temperature sensors for ZED2i -- Add parameter `sensors/max_pub_rate` to set the maximum publishing frequency of sensors data +- Add parameter 'sensors/max_pub_rate' to set the maximum publishing frequency of sensors data - Improve Sensors thread -- Fix wrong TF broadcasting when calling the `set_pose`, `reset_tracking`, and `reset_odometry` services. Now the initial odometry is coherent with the new starting point. +- Fix wrong TF broadcasting when calling the 'set_pose', 'reset_tracking', and 'reset_odometry' services. Now the initial odometry is coherent with the new starting point. - Add Plane Detection. See [ZED Documentation](https://www.stereolabs.com/docs/ros/plane_detection/) - Fix TF timestamp issue in SVO mode -- Fix units for Atmospheric pressure data. Now the value is correctly published in `Pascal` according to the [topic specification](http://docs.ros.org/en/melodic/api/sensor_msgs/html/msg/FluidPressure.html). +- Fix units for Atmospheric pressure data. Now the value is correctly published in 'Pascal' according to the [topic specification](http://docs.ros.org/en/melodic/api/sensor_msgs/html/msg/FluidPressure.html). v3.7.x --------- @@ -64,23 +66,23 @@ v3.7.x - Add support for sport-related object class - Add support for X_MEDIUM neural network models - Enable AI for ZED Mini -- Add new `_base_link` frame on the base of the camera to easily handle camera positioning on robots. Thx @civerachb-cpr +- Add new '_base_link' frame on the base of the camera to easily handle camera positioning on robots. Thx @civerachb-cpr - Improve URDF by adding 3° slope for ZED and ZED2, X-offset for optical frames to correctly match the CMOS sensors position on the PCB, X-offset for mounting screw on ZED2i -- Add `zed_macro.urdf.xacro` to be included by other `xacro` file to easily integrate ZED cameras in the robot descriptions. See [PR #771](https://github.com/stereolabs/zed-ros-wrapper/pull/771) for details. Thx @civerachb-cpr -- New parameter `save_area_memory_db_on_exit` to force Area Memory saving when the node is closed and Area Memory is enabled and valid. -- Add service `save_Area_map` to trigger an Area Memory saving. +- Add 'zed_macro.urdf.xacro' to be included by other 'xacro' file to easily integrate ZED cameras in the robot descriptions. See [PR #771](https://github.com/stereolabs/zed-ros-wrapper/pull/771) for details. Thx @civerachb-cpr +- New parameter 'save_area_memory_db_on_exit' to force Area Memory saving when the node is closed and Area Memory is enabled and valid. +- Add service 'save_Area_map' to trigger an Area Memory saving. - New tool function to transform a relative path to absolute. -- Enabled static IMU TF broadcasting even it `publish_tf` is set to false, making the two options independent. Thx to @bjsowa -- Moved the `zed_interfaces` folder in the new [`zed-ros-interfaces`](https://github.com/stereolabs/zed-ros-interfaces) repository. The new repository is useful to receive the topics from a ZED node on system where the `zed-ros-wrapper` repository cannot be fully installed, i.e. systems without CUDA support. For this repository nothing changes because the `zed_interfaces` folder is replaced by the `zed-ros-interfaces` git submodule to automatically satisfy all the dependencies. +- Enabled static IMU TF broadcasting even it 'publish_tf' is set to false, making the two options independent. Thx to @bjsowa +- Moved the 'zed_interfaces' folder in the new ['zed-ros-interfaces'](https://github.com/stereolabs/zed-ros-interfaces) repository. The new repository is useful to receive the topics from a ZED node on system where the 'zed-ros-wrapper' repository cannot be fully installed, i.e. systems without CUDA support. For this repository nothing changes because the 'zed_interfaces' folder is replaced by the 'zed-ros-interfaces' git submodule to automatically satisfy all the dependencies. - Fix sensors topics pubblication for ZED2i. The support for the new camera was not complete -- Fix sensor_msgs type for depth image in OpenNI mode, from `sensor_msgs::image_encodings::mono16` to `sensor_msgs::image_encodings::TYPE_16UC1`. Depth image in OpenNI mode is now compatible with the nodelet `depthimage_to_laserscan` +- Fix sensor_msgs type for depth image in OpenNI mode, from 'sensor_msgs::image_encodings::mono16' to 'sensor_msgs::image_encodings::TYPE_16UC1'. Depth image in OpenNI mode is now compatible with the nodelet 'depthimage_to_laserscan' v3.5.x --------- - Add support for ROS Noetic - Add support for SDK v3.5 - Add support for the new ZED 2i -- Add new parameter `pos_tracking/pos_tracking_enabled` to enable positional tracking from start even if not required by any subscribed topic. This is useful, for example, to keep the TF always updated. +- Add new parameter 'pos_tracking/pos_tracking_enabled' to enable positional tracking from start even if not required by any subscribed topic. This is useful, for example, to keep the TF always updated. - Add new example to start multiple ZED Nodelets inside the same nodelet manager - Fixed issue #690 @@ -90,17 +92,17 @@ v3.4.x - Fix issue #660: detected objects topic not published if depth computation not active - Improved support for ZED Object Detection - Add Skeleton Tracking support -- New Rviz plugin for Object Detection in `zed-ros-examples` -- New parameters and name changing to fit the new OD features, also the `start_object_detection` service has been modified to match the new features: - - new `model` parameter to choose the AI model - - new `max_range` parameter to limit the detection range - - new `sk_body_fitting` parameter to enable Skeleton fitting for skeleton AI models - - `people` -> `mc_people` to indicate that it is related to multiclass AI models - - `vehicles`-> `mc_vehicles` to indicate that it is related to multiclass AI models - - new `mc_bag` parameter to enable bags detection with multiclass AI models - - new `mc_animal` parameter to enable animals detection with multiclass AI models - - new `mc_electronics` parameter to enable electronic devices detection with multiclass AI models - - new `mc_fruit_vegetable` parameter to enable fruits and vegetables detection with multiclass AI models +- New Rviz plugin for Object Detection in 'zed-ros-examples' +- New parameters and name changing to fit the new OD features, also the 'start_object_detection' service has been modified to match the new features: + - new 'model' parameter to choose the AI model + - new 'max_range' parameter to limit the detection range + - new 'sk_body_fitting' parameter to enable Skeleton fitting for skeleton AI models + - 'people' -> 'mc_people' to indicate that it is related to multiclass AI models + - 'vehicles'-> 'mc_vehicles' to indicate that it is related to multiclass AI models + - new 'mc_bag' parameter to enable bags detection with multiclass AI models + - new 'mc_animal' parameter to enable animals detection with multiclass AI models + - new 'mc_electronics' parameter to enable electronic devices detection with multiclass AI models + - new 'mc_fruit_vegetable' parameter to enable fruits and vegetables detection with multiclass AI models RGB/Depth sync fix #629 (2020-11-02) ------------------------------- @@ -110,13 +112,13 @@ RGB/Depth sync fix #629 (2020-11-02) ASYNC Object Detection (2020-09-18) ----------------------------------- - Object Detection now runs asynchronously respect to data grabbing and Object Detected data are published only when available not affecting the frequency of the publishing of the other data types -- Depth OpenNI topic name changed from `depth/depth_raw_registered` to `depth/depth_registered` +- Depth OpenNI topic name changed from 'depth/depth_raw_registered' to 'depth/depth_registered' IMU timestamp fix (2020-08-25) ------------------------------ -- Added new parameter `sensors/publish_imu_tf` to enable/disable IMU TF broadcasting +- Added new parameter 'sensors/publish_imu_tf' to enable/disable IMU TF broadcasting - Fixed duplicated IMU timestamp issue (see ticket #577) -- Fixed problem with IMU TF in Rviz: `odom` and `zed_camera_center` TFs are now published at the same frequency of the IMU TF, if available) +- Fixed problem with IMU TF in Rviz: 'odom' and 'zed_camera_center' TFs are now published at the same frequency of the IMU TF, if available) - IMU TF is now published once as static TF even if the IMU topic is not subscribed Timestamp fix (2020-06-03) @@ -125,65 +127,65 @@ Timestamp fix (2020-06-03) IMU fix (2020-05-24) -------------------- -- Fix issue with IMU frame link when `publish_tf` and `publish_map_tf` are disabled +- Fix issue with IMU frame link when 'publish_tf' and 'publish_map_tf' are disabled New package: zed_nodelets (2020-03-20) --------------------------------------- -- Added the new `zed_interfaces/RGBDSensors` custom topic that contains RGB, Depth, IMU and Magnetometer synchronized topics -- Added a new package `zed_nodelets` that contains the main `zed_nodelets/ZEDWrapperNodelet` and new nodelets -- Added a new nodelet `zed_nodelets/RgbdSensorsSyncNodelet` that subscribes to RGB, Depth, IMU and Magnetometer topics and republish them in a single synchronized message -- Added a new nodelet `zed_nodelets/RgbdSensorsDemuxNodelet` that subscribes to RGBDSensors and republish RGB, Depth, IMU and Magnetometer as single topics -- Renamed `zed_interfaces/objects` to `zed_interfaces/Objects` -- Renamed `zed_interfaces/object_stamped` to `zed_interfaces/ObjectStamped` -- Reorganized the `zed_wrapper/launch` folder adding the `include` folder -- New online documentation to explain in details the new `zed_nodelets` package: https://www.stereolabs.com/docs/ros/zed_nodelets/ +- Added the new 'zed_interfaces/RGBDSensors' custom topic that contains RGB, Depth, IMU and Magnetometer synchronized topics +- Added a new package 'zed_nodelets' that contains the main 'zed_nodelets/ZEDWrapperNodelet' and new nodelets +- Added a new nodelet 'zed_nodelets/RgbdSensorsSyncNodelet' that subscribes to RGB, Depth, IMU and Magnetometer topics and republish them in a single synchronized message +- Added a new nodelet 'zed_nodelets/RgbdSensorsDemuxNodelet' that subscribes to RGBDSensors and republish RGB, Depth, IMU and Magnetometer as single topics +- Renamed 'zed_interfaces/objects' to 'zed_interfaces/Objects' +- Renamed 'zed_interfaces/object_stamped' to 'zed_interfaces/ObjectStamped' +- Reorganized the 'zed_wrapper/launch' folder adding the 'include' folder +- New online documentation to explain in details the new 'zed_nodelets' package: https://www.stereolabs.com/docs/ros/zed_nodelets/ v3.1 ----- -- Added new package `zed_interfaces` with isolated declarations of custom messages, services and actions -- Removed not used `world_frame` parameter -- Removed the`publish_pose_covariance` parameter, now covariance for pose and odometry is always published -- Removed `_m` from parameters `mapping/resolution_m` and `mapping/max_mapping_range_m` -- Renamed the parameter `depth_resample_factor` to `depth_downsample_factor` -- Renamed the parameter `img_resample_factor` to `img_downsample_factor` -- Renamed the parameter `odometry_db` to `area_memory_db_path` -- Renamed the parameter `frame_rate` to `grab_frame_rate` -- Added new dynamic parameter `pub_frame_rate` to reduce Video and Depth publishing frequency respect to grabbing frame rate [`grab_frame_rate`] -- Added new dynamic parameter `gamma` for Gamma Control -- Added new dynamic parameter `depth_texture_conf` to filter depth according to textureness information +- Added new package 'zed_interfaces' with isolated declarations of custom messages, services and actions +- Removed not used 'world_frame' parameter +- Removed the'publish_pose_covariance' parameter, now covariance for pose and odometry is always published +- Removed '_m' from parameters 'mapping/resolution_m' and 'mapping/max_mapping_range_m' +- Renamed the parameter 'depth_resample_factor' to 'depth_downsample_factor' +- Renamed the parameter 'img_resample_factor' to 'img_downsample_factor' +- Renamed the parameter 'odometry_db' to 'area_memory_db_path' +- Renamed the parameter 'frame_rate' to 'grab_frame_rate' +- Added new dynamic parameter 'pub_frame_rate' to reduce Video and Depth publishing frequency respect to grabbing frame rate ['grab_frame_rate'] +- Added new dynamic parameter 'gamma' for Gamma Control +- Added new dynamic parameter 'depth_texture_conf' to filter depth according to textureness information - Added new TF frames for all the sensors available on ZED2 - Added publishers for gray images -- Added publisher for Camera to IMU transform: `///camera_imu_transform` -- Default value for `depth_confidence` changed from 100 to 50 -- Added `base_frame` as launch parameter to propagate the value of the parameter in the Xacro description +- Added publisher for Camera to IMU transform: '///camera_imu_transform' +- Default value for 'depth_confidence' changed from 100 to 50 +- Added 'base_frame' as launch parameter to propagate the value of the parameter in the Xacro description Bug fix (2020-03-06) -------------------- -- Fix default value for dynamic parameters not set from `common.yaml` +- Fix default value for dynamic parameters not set from 'common.yaml' XACRO and more (2020-01-31) --------------------------- - Added xacro support for parametric URDF - Removed redundant URDFs and added a single parametric URDF based on xacro - Fixed auto white balance at node start (thanks to @kjaget) -- Removed `fixed_covariance` and `fixed_cov_value` parameters (not required anymore) -- Removed `sens_pub_rate` parameter -- Removed `confidence_image` message -- Removed `color_enhancement` parameter, always ON by default +- Removed 'fixed_covariance' and 'fixed_cov_value' parameters (not required anymore) +- Removed 'sens_pub_rate' parameter +- Removed 'confidence_image' message +- Removed 'color_enhancement' parameter, always ON by default - Mapping does not use presets for resolution, but a float value in range [0.01,0.2] -- Added new parameter `max_mapping_range_m` for mapping depth range (set to `-1` for auto calculation) -- Moved "multi-camera" launch file in [`zed-ros-examples`](https://github.com/stereolabs/zed-ros-examples/tree/master/examples/zed_multicamera_example) +- Added new parameter 'max_mapping_range_m' for mapping depth range (set to '-1' for auto calculation) +- Moved "multi-camera" launch file in ['zed-ros-examples'](https://github.com/stereolabs/zed-ros-examples/tree/master/examples/zed_multicamera_example) - Added current GPU ID to Diagnostic information -- The `confidence` dynamic parameter is now called `depth_confidence` -- Removed dynamic parametes `map_resize_factor` -- Added new parameter `video/img_resample_factor` -- Added new parameter `depth/map_resample_factor` +- The 'confidence' dynamic parameter is now called 'depth_confidence' +- Removed dynamic parametes 'map_resize_factor' +- Added new parameter 'video/img_resample_factor' +- Added new parameter 'depth/map_resample_factor' - Updated the names for the parameters of the Object Detection module [only ZED2] SDK v3.0 (2020-01-27) --------------------- -- Added a new repository [`zed-ros-examples`](https://github.com/stereolabs/zed-ros-examples) to keep separated the main ZED Wrapper node from Examples and Tutorials. A clean robot installation is now allowed +- Added a new repository ['zed-ros-examples'](https://github.com/stereolabs/zed-ros-examples) to keep separated the main ZED Wrapper node from Examples and Tutorials. A clean robot installation is now allowed - ZED 2 support - Color enhancement support - Max range is not a dynamic parameter anymore @@ -191,7 +193,7 @@ SDK v3.0 (2020-01-27) - New service to start/stop mapping - Support for Object Detection (only ZED2) - Advanced support for on-board sensors (only ZED-M and ZED2) -- New tutorials, see [`zed-ros-examples`](https://github.com/stereolabs/zed-ros-examples) +- New tutorials, see ['zed-ros-examples'](https://github.com/stereolabs/zed-ros-examples) 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 925b5bde..318e164c 100644 --- a/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp +++ b/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp @@ -575,9 +575,9 @@ class ZEDWrapperNodelet : public nodelet::Nodelet std::string mCloudFrameId; std::string mPointCloudFrameId; - std::string mMapFrameId; - std::string mOdometryFrameId; - std::string mBaseFrameId; + std::string mMapFrameId = "map"; + std::string mOdometryFrameId = "odom"; + std::string mBaseFrameId = "base_link"; std::string mCameraFrameId; std::string mRightCamFrameId; @@ -591,12 +591,6 @@ class ZEDWrapperNodelet : public nodelet::Nodelet std::string mTempLeftFrameId; std::string mTempRightFrameId; - bool mPublishTf; - bool mPublishMapTf; - bool mPublishImuTf; - sl::FLIP_MODE mCameraFlip; - bool mCameraSelfCalib; - // Launch file parameters std::string mCameraName; sl::RESOLUTION mCamResol; @@ -620,7 +614,7 @@ class ZEDWrapperNodelet : public nodelet::Nodelet bool mSvoMode = false; double mCamMinDepth; double mCamMaxDepth; - std::string mClickedPtTopic; + std::string mClickedPtTopic = "/clicked_point"; // Positional tracking bool mPosTrackingEnabled = false; @@ -632,6 +626,13 @@ class ZEDWrapperNodelet : public nodelet::Nodelet bool mFloorAlignment = false; bool mImuFusion = true; bool mSetGravityAsOrigin = false; + bool mPublishTf; + bool mPublishMapTf; + bool mPublishImuTf; + sl::FLIP_MODE mCameraFlip; + bool mCameraSelfCalib; + bool mIsStatic = false; + double mPosTrkMinDepth = 0.0; // Flags bool mGrabActive = false; // Indicate if camera grabbing is active (at least one topic subscribed) 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 913ec771..64d6f9a2 100644 --- a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp @@ -846,7 +846,7 @@ void ZEDWrapperNodelet::readGeneralParams() mCameraFlip = sl::FLIP_MODE::AUTO; } NODELET_INFO_STREAM(" * Camera Flip\t\t\t-> " << sl::toString(mCameraFlip).c_str()); - mNhNs.param("general/self_calib", mCameraSelfCalib, true); + mNhNs.getParam("general/self_calib", mCameraSelfCalib); NODELET_INFO_STREAM(" * Self calibration\t\t-> " << (mCameraSelfCalib ? "ENABLED" : "DISABLED")); int tmp_sn = 0; @@ -923,7 +923,7 @@ void ZEDWrapperNodelet::readPosTrkParams() { NODELET_INFO_STREAM("*** POSITIONAL TRACKING PARAMETERS ***"); - mNhNs.param("pos_tracking/pos_tracking_enabled", mPosTrackingEnabled, true); + mNhNs.getParam("pos_tracking/pos_tracking_enabled", mPosTrackingEnabled); NODELET_INFO_STREAM(" * Positional tracking\t\t-> " << (mPosTrackingEnabled ? "ENABLED" : "DISABLED")); std::string pos_trk_mode; @@ -964,31 +964,39 @@ void ZEDWrapperNodelet::readPosTrkParams() mAreaMemDbPath = sl_tools::resolveFilePath(mAreaMemDbPath); NODELET_INFO_STREAM(" * Odometry DB path\t\t-> " << mAreaMemDbPath.c_str()); - mNhNs.param("pos_tracking/save_area_memory_db_on_exit", mSaveAreaMapOnClosing, false); + mNhNs.getParam("pos_tracking/save_area_memory_db_on_exit", mSaveAreaMapOnClosing); NODELET_INFO_STREAM(" * Save Area Memory on closing\t-> " << (mSaveAreaMapOnClosing ? "ENABLED" : "DISABLED")); - mNhNs.param("pos_tracking/area_memory", mAreaMemory, false); + mNhNs.getParam("pos_tracking/area_memory", mAreaMemory); NODELET_INFO_STREAM(" * Area Memory\t\t\t-> " << (mAreaMemory ? "ENABLED" : "DISABLED")); - mNhNs.param("pos_tracking/imu_fusion", mImuFusion, true); + mNhNs.getParam("pos_tracking/imu_fusion", mImuFusion); NODELET_INFO_STREAM(" * IMU Fusion\t\t\t-> " << (mImuFusion ? "ENABLED" : "DISABLED")); - mNhNs.param("pos_tracking/floor_alignment", mFloorAlignment, false); + mNhNs.getParam("pos_tracking/floor_alignment", mFloorAlignment); NODELET_INFO_STREAM(" * Floor alignment\t\t-> " << (mFloorAlignment ? "ENABLED" : "DISABLED")); - mNhNs.param("pos_tracking/init_odom_with_first_valid_pose", mInitOdomWithPose, true); + mNhNs.getParam("pos_tracking/init_odom_with_first_valid_pose", mInitOdomWithPose); NODELET_INFO_STREAM(" * Init Odometry with first valid pose data -> " << (mInitOdomWithPose ? "ENABLED" : "DISABLED")); - mNhNs.param("pos_tracking/two_d_mode", mTwoDMode, false); + mNhNs.getParam("pos_tracking/two_d_mode", mTwoDMode); NODELET_INFO_STREAM(" * Force 2D mode\t\t-> " << (mTwoDMode ? "ENABLED" : "DISABLED")); if (mTwoDMode) { - mNhNs.param("pos_tracking/fixed_z_value", mFixedZValue, 0.0); + mNhNs.getParam("pos_tracking/fixed_z_value", mFixedZValue); NODELET_INFO_STREAM(" * Fixed Z value\t\t-> " << mFixedZValue); } - mNhNs.param("pos_tracking/publish_tf", mPublishTf, true); + mNhNs.getParam("pos_tracking/publish_tf", mPublishTf); NODELET_INFO_STREAM(" * Broadcast odometry TF\t-> " << (mPublishTf ? "ENABLED" : "DISABLED")); - mNhNs.param("pos_tracking/publish_map_tf", mPublishMapTf, true); + mNhNs.getParam("pos_tracking/publish_map_tf", mPublishMapTf); NODELET_INFO_STREAM(" * Broadcast map pose TF\t-> " << (mPublishTf ? (mPublishMapTf ? "ENABLED" : "DISABLED") : "DISABLED")); + + mNhNs.getParam("pos_tracking/set_as_static", mIsStatic); + 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; } } @@ -998,7 +1006,7 @@ void ZEDWrapperNodelet::readMappingParams() if (!mDepthDisabled) { - mNhNs.param("mapping/mapping_enabled", mMappingEnabled, false); + mNhNs.getParam("mapping/mapping_enabled", mMappingEnabled); if (mMappingEnabled) { @@ -1019,7 +1027,7 @@ void ZEDWrapperNodelet::readMappingParams() NODELET_INFO_STREAM(" * Mapping\t\t\t-> DISABLED"); } } - mNhNs.param("mapping/clicked_point_topic", mClickedPtTopic, std::string("/clicked_point")); + mNhNs.getParam("mapping/clicked_point_topic",mClickedPtTopic); NODELET_INFO_STREAM(" * Clicked point topic\t\t-> " << mClickedPtTopic.c_str()); } @@ -1029,7 +1037,7 @@ void ZEDWrapperNodelet::readObjDetParams() { NODELET_INFO_STREAM("*** OBJECT DETECTION PARAMETERS ***"); - mNhNs.param("object_detection/od_enabled", mObjDetEnabled, false); + mNhNs.getParam("object_detection/od_enabled", mObjDetEnabled); if (mObjDetEnabled) { @@ -1097,7 +1105,7 @@ void ZEDWrapperNodelet::readSensParams() NODELET_INFO_STREAM(" * Max sensors rate\t\t-> " << mSensPubRate); - mNhNs.param("sensors/publish_imu_tf", mPublishImuTf, true); + mNhNs.getParam("sensors/publish_imu_tf", mPublishImuTf); NODELET_INFO_STREAM(" * Broadcast IMU pose TF\t-> " << (mPublishImuTf ? "ENABLED" : "DISABLED")); } else @@ -1110,7 +1118,7 @@ void ZEDWrapperNodelet::readSvoParams() { NODELET_INFO_STREAM("*** SVO PARAMETERS ***"); - mNhNs.param("svo_file", mSvoFilepath, std::string()); + mNhNs.getParam("svo_file", mSvoFilepath); mSvoFilepath = sl_tools::resolveFilePath(mSvoFilepath); NODELET_INFO_STREAM(" * SVO input file: \t\t-> " << mSvoFilepath.c_str()); @@ -1237,14 +1245,14 @@ void ZEDWrapperNodelet::readParameters() // <---- SVO // Remote Stream - mNhNs.param("stream", mRemoteStreamAddr, std::string()); + mNhNs.getParam("stream", mRemoteStreamAddr); // ----> Coordinate frames NODELET_INFO_STREAM("*** COORDINATE FRAMES ***"); - mNhNs.param("pos_tracking/map_frame", mMapFrameId, "map"); - mNhNs.param("pos_tracking/odometry_frame", mOdometryFrameId, "odom"); - mNhNs.param("general/base_frame", mBaseFrameId, "base_link"); + mNhNs.getParam("pos_tracking/map_frame", mMapFrameId); + mNhNs.getParam("pos_tracking/odometry_frame", mOdometryFrameId); + mNhNs.getParam("general/base_frame", mBaseFrameId); mCameraFrameId = mCameraName + "_camera_center"; mImuFrameId = mCameraName + "_imu_link"; @@ -2083,10 +2091,11 @@ void ZEDWrapperNodelet::start_pos_tracking() mPoseSmoothing = false; // Always false. Pose Smoothing is to be enabled only for VR/AR applications posTrackParams.enable_pose_smoothing = mPoseSmoothing; - posTrackParams.set_floor_as_origin = mFloorAlignment; + posTrackParams.set_as_static = mIsStatic; + posTrackParams.depth_min_range = static_cast(mPosTrkMinDepth); - if (mAreaMemDbPath != "" && !sl_tools::file_exist(mAreaMemDbPath)) + if (mAreaMemDbPath != "" && !sl_tools::file_exist(mAreaMemDbPath)) { posTrackParams.area_file_path = ""; NODELET_WARN_STREAM("area_memory_db_path [" << mAreaMemDbPath << "] doesn't exist or is unreachable. "); diff --git a/zed_wrapper/params/common.yaml b/zed_wrapper/params/common.yaml index 93f8fbcd..0982426e 100644 --- a/zed_wrapper/params/common.yaml +++ b/zed_wrapper/params/common.yaml @@ -53,7 +53,7 @@ pos_tracking: publish_map_tf: true # publish `map -> odom` TF map_frame: 'map' # main frame odometry_frame: 'odom' # odometry frame - area_memory_db_path: 'zed_area_memory.area' # file loaded when the node starts to restore the "known visual features" map. + area_memory_db_path: '' # file loaded when the node starts to restore the "known visual features" map. save_area_memory_db_on_exit: false # save the "known visual features" map when the node is correctly closed to the path indicated by `area_memory_db_path` area_memory: true # Enable to detect loop closure floor_alignment: false # Enable to automatically calculate camera/floor offset From 6863f2a0b4c6b335a80d038f3bd03226660803f8 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Wed, 13 Sep 2023 18:42:20 +0200 Subject: [PATCH 174/199] Code cleaning: Add 'processCameraSettings' func --- .../include/zed_wrapper_nodelet.hpp | 4 + .../zed_nodelet/src/zed_wrapper_nodelet.cpp | 432 +++++++++--------- 2 files changed, 226 insertions(+), 210 deletions(-) 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 318e164c..327c2e13 100644 --- a/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp +++ b/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp @@ -439,6 +439,10 @@ class ZEDWrapperNodelet : public nodelet::Nodelet */ void processDetectedObjects(ros::Time t); + /*! \brief Process camera settings + */ + void processCameraSettings(); + /*! \brief Generates an univoque color for each object class ID */ inline sl::float3 generateColorClass(int idx) 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 64d6f9a2..7f377b49 100644 --- a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp @@ -1027,7 +1027,7 @@ void ZEDWrapperNodelet::readMappingParams() NODELET_INFO_STREAM(" * Mapping\t\t\t-> DISABLED"); } } - mNhNs.getParam("mapping/clicked_point_topic",mClickedPtTopic); + mNhNs.getParam("mapping/clicked_point_topic", mClickedPtTopic); NODELET_INFO_STREAM(" * Clicked point topic\t\t-> " << mClickedPtTopic.c_str()); } @@ -2046,9 +2046,16 @@ void ZEDWrapperNodelet::start_pos_tracking() { NODELET_INFO_STREAM("*** Starting Positional Tracking ***"); + mPosTrackingReady = false; // Useful to not publish wrong TF with IMU frame broadcasting + + if (mDepthDisabled) + { + NODELET_WARN("Cannot start Positional Tracking if `depth.depth_mode` is set to 'NONE'"); + return; + } + NODELET_INFO(" * Waiting for valid static transformations..."); - mPosTrackingReady = false; // Useful to not publish wrong TF with IMU frame broadcasting bool transformOk = false; double elapsed = 0.0; @@ -2062,7 +2069,7 @@ void ZEDWrapperNodelet::start_pos_tracking() elapsed = std::chrono::duration_cast(std::chrono::high_resolution_clock::now() - start) .count(); - std::this_thread::sleep_for(std::chrono::milliseconds(100)); + std::this_thread::sleep_for(std::chrono::milliseconds(500)); if (elapsed > 10000) { @@ -2095,7 +2102,7 @@ void ZEDWrapperNodelet::start_pos_tracking() posTrackParams.set_as_static = mIsStatic; posTrackParams.depth_min_range = static_cast(mPosTrkMinDepth); - if (mAreaMemDbPath != "" && !sl_tools::file_exist(mAreaMemDbPath)) + if (mAreaMemDbPath != "" && !sl_tools::file_exist(mAreaMemDbPath)) { posTrackParams.area_file_path = ""; NODELET_WARN_STREAM("area_memory_db_path [" << mAreaMemDbPath << "] doesn't exist or is unreachable. "); @@ -2127,7 +2134,7 @@ void ZEDWrapperNodelet::start_pos_tracking() { mPosTrackingActivated = false; - NODELET_WARN("Tracking not activated: %s", sl::toString(err).c_str()); + NODELET_WARN("Pos. Tracking not started: %s", sl::toString(err).c_str()); } } @@ -4181,211 +4188,7 @@ void ZEDWrapperNodelet::device_poll_thread_func() // <---- Publish freq calculation // ----> Camera Settings - int value; - sl::VIDEO_SETTINGS setting; - sl::ERROR_CODE err; - - if (!mSvoMode && mFrameCount % 5 == 0) - { - // NODELET_DEBUG_STREAM( "[" << mFrameCount << "] device_poll_thread_func MUTEX LOCK"); - mDynParMutex.lock(); - - setting = sl::VIDEO_SETTINGS::BRIGHTNESS; - err = mZed.getCameraSettings(setting, value); - if (err == sl::ERROR_CODE::SUCCESS && value != mCamBrightness) - { - err = mZed.setCameraSettings(setting, mCamBrightness); - } - if (err != sl::ERROR_CODE::SUCCESS) - { - NODELET_WARN_STREAM("Error setting parameter " << sl::toString(setting) << ": " << sl::toString(err)); - } - else - { - NODELET_DEBUG_STREAM(sl::toString(setting) << " changed: " << mCamBrightness << " <- " << value); - mUpdateDynParams = true; - } - - setting = sl::VIDEO_SETTINGS::CONTRAST; - err = mZed.getCameraSettings(setting, value); - if (err == sl::ERROR_CODE::SUCCESS && value != mCamContrast) - { - err = mZed.setCameraSettings(setting, mCamContrast); - } - if (err != sl::ERROR_CODE::SUCCESS) - { - NODELET_WARN_STREAM("Error setting parameter " << sl::toString(setting) << ": " << sl::toString(err)); - } - else - { - NODELET_DEBUG_STREAM(sl::toString(setting) << " changed: " << mCamContrast << " <- " << value); - mUpdateDynParams = true; - } - - setting = sl::VIDEO_SETTINGS::HUE; - err = mZed.getCameraSettings(setting, value); - if (err == sl::ERROR_CODE::SUCCESS && value != mCamHue) - { - err = mZed.setCameraSettings(setting, mCamHue); - } - if (err != sl::ERROR_CODE::SUCCESS) - { - NODELET_WARN_STREAM("Error setting parameter " << sl::toString(setting) << ": " << sl::toString(err)); - } - else - { - NODELET_DEBUG_STREAM(sl::toString(setting) << " changed: " << mCamHue << " <- " << value); - mUpdateDynParams = true; - } - - setting = sl::VIDEO_SETTINGS::SATURATION; - err = mZed.getCameraSettings(setting, value); - if (err == sl::ERROR_CODE::SUCCESS && value != mCamSaturation) - { - err = mZed.setCameraSettings(setting, mCamSaturation); - } - if (err != sl::ERROR_CODE::SUCCESS) - { - NODELET_WARN_STREAM("Error setting parameter " << sl::toString(setting) << ": " << sl::toString(err)); - } - else - { - NODELET_DEBUG_STREAM(sl::toString(setting) << " changed: " << mCamSaturation << " <- " << value); - mUpdateDynParams = true; - } - - setting = sl::VIDEO_SETTINGS::SHARPNESS; - err = mZed.getCameraSettings(setting, value); - if (err == sl::ERROR_CODE::SUCCESS && value != mCamSharpness) - { - err = mZed.setCameraSettings(setting, mCamSharpness); - } - if (err != sl::ERROR_CODE::SUCCESS) - { - NODELET_WARN_STREAM("Error setting parameter " << sl::toString(setting) << ": " << sl::toString(err)); - } - else - { - NODELET_DEBUG_STREAM(sl::toString(setting) << " changed: " << mCamSharpness << " <- " << value); - mUpdateDynParams = true; - } - - setting = sl::VIDEO_SETTINGS::GAMMA; - err = mZed.getCameraSettings(setting, value); - if (err == sl::ERROR_CODE::SUCCESS && value != mCamGamma) - { - err = mZed.setCameraSettings(setting, mCamGamma); - } - if (err != sl::ERROR_CODE::SUCCESS) - { - NODELET_WARN_STREAM("Error setting parameter " << sl::toString(setting) << ": " << sl::toString(err)); - } - else - { - NODELET_DEBUG_STREAM(sl::toString(setting) << " changed: " << mCamGamma << " <- " << value); - mUpdateDynParams = true; - } - - if (mTriggerAutoExposure) - { - setting = sl::VIDEO_SETTINGS::AEC_AGC; - err = mZed.getCameraSettings(setting, value); - int aec_agc = (mCamAutoExposure ? 1 : 0); - if (err == sl::ERROR_CODE::SUCCESS && value != aec_agc) - { - err = mZed.setCameraSettings(setting, aec_agc); - } - if (err != sl::ERROR_CODE::SUCCESS) - { - NODELET_WARN_STREAM("Error setting parameter " << sl::toString(setting) << ": " << sl::toString(err)); - } - else - { - NODELET_DEBUG_STREAM(sl::toString(setting) << " changed: " << aec_agc << " <- " << value); - mUpdateDynParams = true; - } - } - if (!mCamAutoExposure) - { - setting = sl::VIDEO_SETTINGS::EXPOSURE; - err = mZed.getCameraSettings(setting, value); - if (err == sl::ERROR_CODE::SUCCESS && value != mCamExposure) - { - err = mZed.setCameraSettings(setting, mCamExposure); - } - if (err != sl::ERROR_CODE::SUCCESS) - { - NODELET_WARN_STREAM("Error setting parameter " << sl::toString(setting) << ": " << sl::toString(err)); - } - else - { - NODELET_DEBUG_STREAM(sl::toString(setting) << " changed: " << mCamExposure << " <- " << value); - mUpdateDynParams = true; - } - - setting = sl::VIDEO_SETTINGS::GAIN; - err = mZed.getCameraSettings(setting, value); - if (err == sl::ERROR_CODE::SUCCESS && value != mCamGain) - { - err = mZed.setCameraSettings(setting, mCamGain); - } - if (err != sl::ERROR_CODE::SUCCESS) - { - NODELET_WARN_STREAM("Error setting parameter " << sl::toString(setting) << ": " << sl::toString(err)); - } - else - { - NODELET_DEBUG_STREAM(sl::toString(setting) << " changed: " << mCamGain << " <- " << value); - mUpdateDynParams = true; - } - } - - if (mTriggerAutoWB) - { - setting = sl::VIDEO_SETTINGS::WHITEBALANCE_AUTO; - err = mZed.getCameraSettings(setting, value); - int wb_auto = (mCamAutoWB ? 1 : 0); - if (err == sl::ERROR_CODE::SUCCESS && value != wb_auto) - { - err = mZed.setCameraSettings(setting, wb_auto); - } - if (err != sl::ERROR_CODE::SUCCESS) - { - NODELET_WARN_STREAM("Error setting parameter " << sl::toString(setting) << ": " << sl::toString(err)); - } - else - { - NODELET_DEBUG_STREAM(sl::toString(setting) << " changed: " << wb_auto << " <- " << value); - mUpdateDynParams = true; - } - } - if (!mCamAutoWB) - { - setting = sl::VIDEO_SETTINGS::WHITEBALANCE_TEMPERATURE; - err = mZed.getCameraSettings(setting, value); - if (err == sl::ERROR_CODE::SUCCESS && value != mCamWB) - { - err = mZed.setCameraSettings(setting, mCamWB); - } - if (err != sl::ERROR_CODE::SUCCESS) - { - NODELET_WARN_STREAM("Error setting parameter " << sl::toString(setting) << ": " << sl::toString(err)); - } - else - { - NODELET_DEBUG_STREAM(sl::toString(setting) << " changed: " << mCamWB << " <- " << value); - mUpdateDynParams = true; - } - } - mDynParMutex.unlock(); - // NODELET_DEBUG_STREAM( "device_poll_thread_func MUTEX UNLOCK"); - } - - if (mUpdateDynParams) - { - NODELET_DEBUG("Update Dynamic Parameters"); - updateDynamicReconfigure(); - } + processCameraSettings(); // <---- Camera Settings // Publish the point cloud if someone has subscribed to @@ -4794,6 +4597,215 @@ void ZEDWrapperNodelet::device_poll_thread_func() NODELET_DEBUG("ZED pool thread finished"); } +void ZEDWrapperNodelet::processCameraSettings() +{ + int value; + sl::VIDEO_SETTINGS setting; + sl::ERROR_CODE err; + + if (!mSvoMode && mFrameCount % 5 == 0) + { + // NODELET_DEBUG_STREAM( "[" << mFrameCount << "] device_poll_thread_func MUTEX LOCK"); + mDynParMutex.lock(); + + setting = sl::VIDEO_SETTINGS::BRIGHTNESS; + err = mZed.getCameraSettings(setting, value); + if (err == sl::ERROR_CODE::SUCCESS && value != mCamBrightness) + { + err = mZed.setCameraSettings(setting, mCamBrightness); + } + if (err != sl::ERROR_CODE::SUCCESS) + { + NODELET_WARN_STREAM("Error setting parameter " << sl::toString(setting) << ": " << sl::toString(err)); + } + else + { + NODELET_DEBUG_STREAM(sl::toString(setting) << " changed: " << mCamBrightness << " <- " << value); + mUpdateDynParams = true; + } + + setting = sl::VIDEO_SETTINGS::CONTRAST; + err = mZed.getCameraSettings(setting, value); + if (err == sl::ERROR_CODE::SUCCESS && value != mCamContrast) + { + err = mZed.setCameraSettings(setting, mCamContrast); + } + if (err != sl::ERROR_CODE::SUCCESS) + { + NODELET_WARN_STREAM("Error setting parameter " << sl::toString(setting) << ": " << sl::toString(err)); + } + else + { + NODELET_DEBUG_STREAM(sl::toString(setting) << " changed: " << mCamContrast << " <- " << value); + mUpdateDynParams = true; + } + + setting = sl::VIDEO_SETTINGS::HUE; + err = mZed.getCameraSettings(setting, value); + if (err == sl::ERROR_CODE::SUCCESS && value != mCamHue) + { + err = mZed.setCameraSettings(setting, mCamHue); + } + if (err != sl::ERROR_CODE::SUCCESS) + { + NODELET_WARN_STREAM("Error setting parameter " << sl::toString(setting) << ": " << sl::toString(err)); + } + else + { + NODELET_DEBUG_STREAM(sl::toString(setting) << " changed: " << mCamHue << " <- " << value); + mUpdateDynParams = true; + } + + setting = sl::VIDEO_SETTINGS::SATURATION; + err = mZed.getCameraSettings(setting, value); + if (err == sl::ERROR_CODE::SUCCESS && value != mCamSaturation) + { + err = mZed.setCameraSettings(setting, mCamSaturation); + } + if (err != sl::ERROR_CODE::SUCCESS) + { + NODELET_WARN_STREAM("Error setting parameter " << sl::toString(setting) << ": " << sl::toString(err)); + } + else + { + NODELET_DEBUG_STREAM(sl::toString(setting) << " changed: " << mCamSaturation << " <- " << value); + mUpdateDynParams = true; + } + + setting = sl::VIDEO_SETTINGS::SHARPNESS; + err = mZed.getCameraSettings(setting, value); + if (err == sl::ERROR_CODE::SUCCESS && value != mCamSharpness) + { + err = mZed.setCameraSettings(setting, mCamSharpness); + } + if (err != sl::ERROR_CODE::SUCCESS) + { + NODELET_WARN_STREAM("Error setting parameter " << sl::toString(setting) << ": " << sl::toString(err)); + } + else + { + NODELET_DEBUG_STREAM(sl::toString(setting) << " changed: " << mCamSharpness << " <- " << value); + mUpdateDynParams = true; + } + + setting = sl::VIDEO_SETTINGS::GAMMA; + err = mZed.getCameraSettings(setting, value); + if (err == sl::ERROR_CODE::SUCCESS && value != mCamGamma) + { + err = mZed.setCameraSettings(setting, mCamGamma); + } + if (err != sl::ERROR_CODE::SUCCESS) + { + NODELET_WARN_STREAM("Error setting parameter " << sl::toString(setting) << ": " << sl::toString(err)); + } + else + { + NODELET_DEBUG_STREAM(sl::toString(setting) << " changed: " << mCamGamma << " <- " << value); + mUpdateDynParams = true; + } + + if (mTriggerAutoExposure) + { + setting = sl::VIDEO_SETTINGS::AEC_AGC; + err = mZed.getCameraSettings(setting, value); + int aec_agc = (mCamAutoExposure ? 1 : 0); + if (err == sl::ERROR_CODE::SUCCESS && value != aec_agc) + { + err = mZed.setCameraSettings(setting, aec_agc); + } + if (err != sl::ERROR_CODE::SUCCESS) + { + NODELET_WARN_STREAM("Error setting parameter " << sl::toString(setting) << ": " << sl::toString(err)); + } + else + { + NODELET_DEBUG_STREAM(sl::toString(setting) << " changed: " << aec_agc << " <- " << value); + mUpdateDynParams = true; + } + } + if (!mCamAutoExposure) + { + setting = sl::VIDEO_SETTINGS::EXPOSURE; + err = mZed.getCameraSettings(setting, value); + if (err == sl::ERROR_CODE::SUCCESS && value != mCamExposure) + { + err = mZed.setCameraSettings(setting, mCamExposure); + } + if (err != sl::ERROR_CODE::SUCCESS) + { + NODELET_WARN_STREAM("Error setting parameter " << sl::toString(setting) << ": " << sl::toString(err)); + } + else + { + NODELET_DEBUG_STREAM(sl::toString(setting) << " changed: " << mCamExposure << " <- " << value); + mUpdateDynParams = true; + } + + setting = sl::VIDEO_SETTINGS::GAIN; + err = mZed.getCameraSettings(setting, value); + if (err == sl::ERROR_CODE::SUCCESS && value != mCamGain) + { + err = mZed.setCameraSettings(setting, mCamGain); + } + if (err != sl::ERROR_CODE::SUCCESS) + { + NODELET_WARN_STREAM("Error setting parameter " << sl::toString(setting) << ": " << sl::toString(err)); + } + else + { + NODELET_DEBUG_STREAM(sl::toString(setting) << " changed: " << mCamGain << " <- " << value); + mUpdateDynParams = true; + } + } + + if (mTriggerAutoWB) + { + setting = sl::VIDEO_SETTINGS::WHITEBALANCE_AUTO; + err = mZed.getCameraSettings(setting, value); + int wb_auto = (mCamAutoWB ? 1 : 0); + if (err == sl::ERROR_CODE::SUCCESS && value != wb_auto) + { + err = mZed.setCameraSettings(setting, wb_auto); + } + if (err != sl::ERROR_CODE::SUCCESS) + { + NODELET_WARN_STREAM("Error setting parameter " << sl::toString(setting) << ": " << sl::toString(err)); + } + else + { + NODELET_DEBUG_STREAM(sl::toString(setting) << " changed: " << wb_auto << " <- " << value); + mUpdateDynParams = true; + } + } + if (!mCamAutoWB) + { + setting = sl::VIDEO_SETTINGS::WHITEBALANCE_TEMPERATURE; + err = mZed.getCameraSettings(setting, value); + if (err == sl::ERROR_CODE::SUCCESS && value != mCamWB) + { + err = mZed.setCameraSettings(setting, mCamWB); + } + if (err != sl::ERROR_CODE::SUCCESS) + { + NODELET_WARN_STREAM("Error setting parameter " << sl::toString(setting) << ": " << sl::toString(err)); + } + else + { + NODELET_DEBUG_STREAM(sl::toString(setting) << " changed: " << mCamWB << " <- " << value); + mUpdateDynParams = true; + } + } + mDynParMutex.unlock(); + // NODELET_DEBUG_STREAM( "device_poll_thread_func MUTEX UNLOCK"); + } + + if (mUpdateDynParams) + { + NODELET_DEBUG("Update Dynamic Parameters"); + updateDynamicReconfigure(); + } +} + void ZEDWrapperNodelet::callback_updateDiagnostic(diagnostic_updater::DiagnosticStatusWrapper& stat) { if (mConnStatus != sl::ERROR_CODE::SUCCESS) From 5df31033a19e4e8e90e8f7140b8e85689b9155a9 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Thu, 14 Sep 2023 16:40:36 +0200 Subject: [PATCH 175/199] Positional tracking aligned to ROS 2 code --- CHANGELOG.rst | 4 + zed-ros-interfaces | 2 +- .../include/zed_wrapper_nodelet.hpp | 58 +- .../zed_nodelet/src/zed_wrapper_nodelet.cpp | 809 +++++++++--------- 4 files changed, 448 insertions(+), 425 deletions(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 00ccb922..fd9d3df5 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -1,6 +1,10 @@ CHANGELOG ========= +2023-09-14 +---------- +- Add pose and odometry status publishers + 2023-09-13 ---------- - Add ROS '.clang-format' diff --git a/zed-ros-interfaces b/zed-ros-interfaces index 206b7e2d..12f55da8 160000 --- a/zed-ros-interfaces +++ b/zed-ros-interfaces @@ -1 +1 @@ -Subproject commit 206b7e2d30505176f079168fbed45ed2dc916000 +Subproject commit 12f55da885dd3f0f68c168d0bccd33c4e892328d 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 327c2e13..a15799cb 100644 --- a/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp +++ b/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp @@ -175,6 +175,22 @@ class ZEDWrapperNodelet : public nodelet::Nodelet */ void sensors_thread_func(); + /*! \brief Publish odometry status message + */ + void publishPoseStatus(); + + /*! \brief Publish odometry status message + */ + void publishOdomStatus(); + + /*! \brief Process odometry information + */ + void processOdometry(); + + /*! \brief Process pose information + */ + void processPose(); + /*! \brief Publish the pose of the camera in "Map" frame with a ros Publisher * \param t : the ros::Time to stamp the image */ @@ -188,20 +204,20 @@ class ZEDWrapperNodelet : public nodelet::Nodelet */ void publishOdom(tf2::Transform odom2baseTransf, sl::Pose& slPose, ros::Time t); - /*! \brief Publish the pose of the camera in "Map" frame as a transformation - * \param baseTransform : Transformation representing the camera pose from - * odom frame to map frame + /*! \brief Publish the odom -> base_link TF + * \param t : the ros::Time to stamp the image + */ + void publishTFs(ros::Time t); + + /*! \brief Publish the odom -> base_link TF * \param t : the ros::Time to stamp the image */ - void publishPoseFrame(tf2::Transform baseTransform, ros::Time t); + void publishOdomTF(ros::Time t); - /*! \brief Publish the odometry of the camera in "Odom" frame as a - * transformation - * \param odomTransf : Transformation representing the camera pose from - * base frame to odom frame + /*! \brief Publish the map -> odom TF * \param t : the ros::Time to stamp the image */ - void publishOdomFrame(tf2::Transform odomTransf, ros::Time t); + void publishPoseTF(ros::Time t); /*! * \brief Publish IMU frame once as static TF @@ -520,6 +536,9 @@ class ZEDWrapperNodelet : public nodelet::Nodelet ros::Publisher mPubMarker; // Publisher for Rviz markers ros::Publisher mPubPlane; // Publisher for detected planes + ros::Publisher mPubPoseStatus; + ros::Publisher mPubOdomStatus; + // Subscribers ros::Subscriber mClickedPtSub; @@ -560,9 +579,8 @@ class ZEDWrapperNodelet : public nodelet::Nodelet // <---- Topics // ROS TF - tf2_ros::TransformBroadcaster mTransformPoseBroadcaster; - tf2_ros::TransformBroadcaster mTransformOdomBroadcaster; - tf2_ros::StaticTransformBroadcaster mStaticTransformImuBroadcaster; + tf2_ros::TransformBroadcaster mTfBroadcaster; + tf2_ros::StaticTransformBroadcaster mStaticTfBroadcaster; std::string mRgbFrameId; std::string mRgbOptFrameId; @@ -580,7 +598,7 @@ class ZEDWrapperNodelet : public nodelet::Nodelet std::string mPointCloudFrameId; std::string mMapFrameId = "map"; - std::string mOdometryFrameId = "odom"; + std::string mOdomFrameId = "odom"; std::string mBaseFrameId = "base_link"; std::string mCameraFrameId; @@ -623,15 +641,16 @@ class ZEDWrapperNodelet : public nodelet::Nodelet // Positional tracking bool mPosTrackingEnabled = false; sl::POSITIONAL_TRACKING_MODE mPosTrkMode = sl::POSITIONAL_TRACKING_MODE::QUALITY; - bool mPosTrackingActivated = false; bool mPosTrackingReady = false; + bool mPosTrackingStarted = false; + bool mPosTrackingRequired = false; bool mTwoDMode = false; double mFixedZValue = 0.0; bool mFloorAlignment = false; bool mImuFusion = true; bool mSetGravityAsOrigin = false; - bool mPublishTf; - bool mPublishMapTf; + bool mPublishTF; + bool mPublishMapTF; bool mPublishImuTf; sl::FLIP_MODE mCameraFlip; bool mCameraSelfCalib; @@ -642,7 +661,8 @@ class ZEDWrapperNodelet : public nodelet::Nodelet bool mGrabActive = false; // Indicate if camera grabbing is active (at least one topic subscribed) sl::ERROR_CODE mConnStatus; sl::ERROR_CODE mGrabStatus; - sl::POSITIONAL_TRACKING_STATE mPosTrackingStatus; + sl::POSITIONAL_TRACKING_STATE mPosTrackingStatusWorld; + sl::POSITIONAL_TRACKING_STATE mPosTrackingStatusCamera; bool mSensPublishing = false; bool mPcPublishing = false; bool mDepthDisabled = false; @@ -657,6 +677,8 @@ class ZEDWrapperNodelet : public nodelet::Nodelet std::vector mInitialBasePose; std::vector mOdomPath; std::vector mMapPath; + ros::Time mLastTs_odom; + ros::Time mLastTs_pose; // IMU TF tf2::Transform mLastImuPose; @@ -716,7 +738,6 @@ class ZEDWrapperNodelet : public nodelet::Nodelet bool mPoseSmoothing = false; // Always disabled. Enable only for AR/VR applications bool mAreaMemory; bool mInitOdomWithPose; - bool mResetOdom = false; bool mUpdateDynParams = false; bool mPublishingData = false; @@ -739,6 +760,7 @@ class ZEDWrapperNodelet : public nodelet::Nodelet std::mutex mPcMutex; std::mutex mRecMutex; std::mutex mPosTrkMutex; + std::mutex mOdomMutex; std::mutex mDynParMutex; std::mutex mMappingMutex; std::mutex mObjDetMutex; 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 7f377b49..c1de1166 100644 --- a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp @@ -30,9 +30,10 @@ #include #endif -#include "zed_interfaces/Object.h" -#include "zed_interfaces/ObjectsStamped.h" +#include +#include #include +#include //#define DEBUG_SENS_TS 1 @@ -163,6 +164,9 @@ void ZEDWrapperNodelet::onInit() std::string odom_path_topic = "path_odom"; std::string map_path_topic = "path_map"; + std::string odomStatusTopic = odometryTopic + "/status"; + std::string poseStatusTopic = poseTopic + "/status"; + // Extracted plane topics std::string marker_topic = "plane_marker"; std::string plane_topic = "plane"; @@ -500,13 +504,17 @@ void ZEDWrapperNodelet::onInit() // Odometry and Pose publisher mPubPose = mNhNs.advertise(poseTopic, 1); NODELET_INFO_STREAM(" * Advertised on topic " << mPubPose.getTopic()); - mPubPoseCov = mNhNs.advertise(pose_cov_topic, 1); NODELET_INFO_STREAM(" * Advertised on topic " << mPubPoseCov.getTopic()); mPubOdom = mNhNs.advertise(odometryTopic, 1); NODELET_INFO_STREAM(" * Advertised on topic " << mPubOdom.getTopic()); + mPubOdomStatus = mNhNs.advertise(odomStatusTopic, 1); + NODELET_INFO_STREAM(" * Advertised on topic " << mPubOdomStatus.getTopic()); + mPubPoseStatus = mNhNs.advertise(poseStatusTopic, 1); + NODELET_INFO_STREAM(" * Advertised on topic " << mPubPoseStatus.getTopic()); + // Rviz markers publisher mPubMarker = mNhNs.advertise(marker_topic, 10, true); @@ -984,11 +992,11 @@ void ZEDWrapperNodelet::readPosTrkParams() NODELET_INFO_STREAM(" * Fixed Z value\t\t-> " << mFixedZValue); } - mNhNs.getParam("pos_tracking/publish_tf", mPublishTf); - NODELET_INFO_STREAM(" * Broadcast odometry TF\t-> " << (mPublishTf ? "ENABLED" : "DISABLED")); - mNhNs.getParam("pos_tracking/publish_map_tf", mPublishMapTf); + mNhNs.getParam("pos_tracking/publish_tf", mPublishTF); + NODELET_INFO_STREAM(" * Broadcast odometry TF\t-> " << (mPublishTF ? "ENABLED" : "DISABLED")); + mNhNs.getParam("pos_tracking/publish_map_tf", mPublishMapTF); NODELET_INFO_STREAM(" * Broadcast map pose TF\t-> " - << (mPublishTf ? (mPublishMapTf ? "ENABLED" : "DISABLED") : "DISABLED")); + << (mPublishTF ? (mPublishMapTF ? "ENABLED" : "DISABLED") : "DISABLED")); mNhNs.getParam("pos_tracking/set_as_static", mIsStatic); NODELET_INFO_STREAM(" * Camera is static\t\t-> " << (mIsStatic ? "ENABLED" : "DISABLED")); @@ -1251,7 +1259,7 @@ void ZEDWrapperNodelet::readParameters() NODELET_INFO_STREAM("*** COORDINATE FRAMES ***"); mNhNs.getParam("pos_tracking/map_frame", mMapFrameId); - mNhNs.getParam("pos_tracking/odometry_frame", mOdometryFrameId); + mNhNs.getParam("pos_tracking/odometry_frame", mOdomFrameId); mNhNs.getParam("general/base_frame", mBaseFrameId); mCameraFrameId = mCameraName + "_camera_center"; @@ -1289,7 +1297,7 @@ void ZEDWrapperNodelet::readParameters() if (!mDepthDisabled) { NODELET_INFO_STREAM(" * map_frame\t\t\t-> " << mMapFrameId); - NODELET_INFO_STREAM(" * odometry_frame\t\t-> " << mOdometryFrameId); + NODELET_INFO_STREAM(" * odometry_frame\t\t-> " << mOdomFrameId); NODELET_INFO_STREAM(" * base_frame\t\t\t-> " << mBaseFrameId); NODELET_INFO_STREAM(" * depth_frame\t\t\t-> " << mDepthFrameId); NODELET_INFO_STREAM(" * depth_optical_frame\t\t-> " << mDepthOptFrameId); @@ -1819,7 +1827,7 @@ bool ZEDWrapperNodelet::on_set_pose(zed_interfaces::set_pose::Request& req, zed_ bool ZEDWrapperNodelet::on_reset_tracking(zed_interfaces::reset_tracking::Request& req, zed_interfaces::reset_tracking::Response& res) { - if (!mPosTrackingActivated) + if (!mPosTrackingStarted) { res.reset_done = false; return false; @@ -1837,7 +1845,10 @@ bool ZEDWrapperNodelet::on_reset_tracking(zed_interfaces::reset_tracking::Reques bool ZEDWrapperNodelet::on_reset_odometry(zed_interfaces::reset_odometry::Request& req, zed_interfaces::reset_odometry::Response& res) { - mResetOdom = true; + std::lock_guard lock(mOdomMutex); + mOdom2BaseTransf.setIdentity(); + mOdomPath.clear(); + res.reset_done = true; return true; } @@ -2128,11 +2139,11 @@ void ZEDWrapperNodelet::start_pos_tracking() if (err == sl::ERROR_CODE::SUCCESS) { - mPosTrackingActivated = true; + mPosTrackingStarted = true; } else { - mPosTrackingActivated = false; + mPosTrackingStarted = false; NODELET_WARN("Pos. Tracking not started: %s", sl::toString(err).c_str()); } @@ -2168,7 +2179,7 @@ bool ZEDWrapperNodelet::saveAreaMap(std::string file_path, std::string* out_msg) return false; } - if (mPosTrackingActivated && mAreaMemory) + if (mPosTrackingStarted && mAreaMemory) { sl::ERROR_CODE err = mZed.saveAreaMap(sl::String(file_path.c_str())); if (err != sl::ERROR_CODE::SUCCESS) @@ -2238,8 +2249,8 @@ void ZEDWrapperNodelet::publishOdom(tf2::Transform odom2baseTransf, sl::Pose& sl nav_msgs::OdometryPtr odomMsg = boost::make_shared(); odomMsg->header.stamp = t; - odomMsg->header.frame_id = mOdometryFrameId; // frame - odomMsg->child_frame_id = mBaseFrameId; // camera_frame + odomMsg->header.frame_id = mOdomFrameId; // frame + odomMsg->child_frame_id = mBaseFrameId; // camera_frame // conversion from Tranform to message geometry_msgs::Transform base2odom = tf2::toMsg(odom2baseTransf); // Add all value in odometry message @@ -2280,11 +2291,11 @@ void ZEDWrapperNodelet::publishPose(ros::Time t) tf2::Transform base_pose; base_pose.setIdentity(); - if (mPublishMapTf) + if (mPublishMapTF) { base_pose = mMap2BaseTransf; } - else if (mPublishTf) + else if (mPublishTF) { base_pose = mOdom2BaseTransf; } @@ -2376,91 +2387,13 @@ void ZEDWrapperNodelet::publishStaticImuFrame() mStaticImuTransformStamped.transform.rotation.w = sl_or.ow; // Publish transformation - mStaticTransformImuBroadcaster.sendTransform(mStaticImuTransformStamped); + mStaticTfBroadcaster.sendTransform(mStaticImuTransformStamped); NODELET_INFO_STREAM("Published static transform '" << mImuFrameId << "' -> '" << mLeftCamFrameId << "'"); mStaticImuFramePublished = true; } -void ZEDWrapperNodelet::publishOdomFrame(tf2::Transform odomTransf, ros::Time t) -{ - // ----> Avoid duplicated TF publishing - static ros::Time last_stamp; - - if (t == last_stamp) - { - return; - } - last_stamp = t; - // <---- Avoid duplicated TF publishing - - if (!mSensor2BaseTransfValid) - { - getSens2BaseTransform(); - } - - if (!mSensor2CameraTransfValid) - { - getSens2CameraTransform(); - } - - if (!mCamera2BaseTransfValid) - { - getCamera2BaseTransform(); - } - - geometry_msgs::TransformStamped transformStamped; - transformStamped.header.stamp = t; - transformStamped.header.frame_id = mOdometryFrameId; - transformStamped.child_frame_id = mBaseFrameId; - // conversion from Tranform to message - transformStamped.transform = tf2::toMsg(odomTransf); - // Publish transformation - mTransformOdomBroadcaster.sendTransform(transformStamped); - - // NODELET_INFO_STREAM( "Published ODOM TF with TS: " << t ); -} - -void ZEDWrapperNodelet::publishPoseFrame(tf2::Transform baseTransform, ros::Time t) -{ - // ----> Avoid duplicated TF publishing - static ros::Time last_stamp; - - if (t == last_stamp) - { - return; - } - last_stamp = t; - // <---- Avoid duplicated TF publishing - - if (!mSensor2BaseTransfValid) - { - getSens2BaseTransform(); - } - - if (!mSensor2CameraTransfValid) - { - getSens2CameraTransform(); - } - - if (!mCamera2BaseTransfValid) - { - getCamera2BaseTransform(); - } - - geometry_msgs::TransformStamped transformStamped; - transformStamped.header.stamp = t; - transformStamped.header.frame_id = mMapFrameId; - transformStamped.child_frame_id = mOdometryFrameId; - // conversion from Tranform to message - transformStamped.transform = tf2::toMsg(baseTransform); - // Publish transformation - mTransformPoseBroadcaster.sendTransform(transformStamped); - - // NODELET_INFO_STREAM( "Published POSE TF with TS: " << t ); -} - void ZEDWrapperNodelet::publishImage(sensor_msgs::ImagePtr imgMsgPtr, sl::Mat img, image_transport::CameraPublisher& pubImg, sensor_msgs::CameraInfoPtr camInfoMsg, std::string imgFrameId, ros::Time t) @@ -3545,23 +3478,6 @@ void ZEDWrapperNodelet::publishSensData(ros::Time t) return; } - // ----> Publish odometry tf only if enabled and not in SVO mode - if (!mSvoMode) - { - if (mPublishTf && mPosTrackingReady && new_imu_data) - { - // NODELET_DEBUG("Publishing TF"); - - publishOdomFrame(mOdom2BaseTransf, ts_imu); // publish the base Frame in odometry frame - - if (mPublishMapTf) - { - publishPoseFrame(mMap2OdomTransf, ts_imu); // publish the odometry Frame in map frame - } - } - } - // <---- Publish odometry tf only if enabled and not in SVO mode - if (mPublishImuTf && !mStaticImuFramePublished) { NODELET_DEBUG("Publishing static IMU TF"); @@ -3882,7 +3798,7 @@ void ZEDWrapperNodelet::device_poll_thread_func() } mPrevFrameTimestamp = mFrameTimestamp; - mPosTrackingActivated = false; + mPosTrackingStarted = false; mMappingRunning = false; mRecording = false; @@ -3994,7 +3910,7 @@ void ZEDWrapperNodelet::device_poll_thread_func() uint32_t stereoRawSubNumber = mPubRawStereo.getNumSubscribers(); mGrabActive = - mRecording || mStreaming || mMappingEnabled || mObjDetEnabled || mPosTrackingEnabled || mPosTrackingActivated || + mRecording || mStreaming || mMappingEnabled || mObjDetEnabled || mPosTrackingEnabled || mPosTrackingStarted || ((rgbSubnumber + rgbRawSubnumber + leftSubnumber + leftRawSubnumber + rightSubnumber + rightRawSubnumber + rgbGraySubnumber + rgbGrayRawSubnumber + leftGraySubnumber + leftGrayRawSubnumber + rightGraySubnumber + rightGrayRawSubnumber + depthSubnumber + disparitySubnumber + cloudSubnumber + poseSubnumber + @@ -4007,13 +3923,13 @@ void ZEDWrapperNodelet::device_poll_thread_func() std::lock_guard lock(mPosTrkMutex); // Note: once tracking is started it is never stopped anymore to not lose tracking information - bool computeTracking = - !mDepthDisabled && (mPosTrackingEnabled || mPosTrackingActivated || mMappingEnabled || mObjDetEnabled || + mPosTrackingRequired = + !mDepthDisabled && (mPosTrackingEnabled || mPosTrackingStarted || mMappingEnabled || mObjDetEnabled || (mComputeDepth & mDepthStabilization) || poseSubnumber > 0 || poseCovSubnumber > 0 || odomSubnumber > 0 || pathSubNumber > 0); // Start the tracking? - if ((computeTracking) && !mPosTrackingActivated && !mDepthDisabled) + if ((mPosTrackingRequired) && !mPosTrackingStarted && !mDepthDisabled) { start_pos_tracking(); } @@ -4036,7 +3952,7 @@ void ZEDWrapperNodelet::device_poll_thread_func() // Detect if one of the subscriber need to have the depth information mComputeDepth = !mDepthDisabled && - (computeTracking || + (mPosTrackingRequired || ((depthSubnumber + disparitySubnumber + cloudSubnumber + fusedCloudSubnumber + poseSubnumber + poseCovSubnumber + odomSubnumber + confMapSubnumber + objDetSubnumber) > 0)); @@ -4120,16 +4036,10 @@ void ZEDWrapperNodelet::device_poll_thread_func() std::this_thread::sleep_for(std::chrono::milliseconds(1000)); } - mPosTrackingActivated = false; - - // Note: once tracking is started it is never stopped anymore to not lose tracking information - bool computeTracking = - !mDepthDisabled && (mPosTrackingEnabled || mPosTrackingActivated || mMappingEnabled || mObjDetEnabled || - (mComputeDepth & mDepthStabilization) || poseSubnumber > 0 || - poseCovSubnumber > 0 || odomSubnumber > 0 || pathSubNumber > 0); + mPosTrackingStarted = false; // Start the tracking? - if ((computeTracking) && !mPosTrackingActivated && !mDepthDisabled) + if ((mPosTrackingRequired) && !mPosTrackingStarted && !mDepthDisabled) { start_pos_tracking(); } @@ -4217,262 +4127,29 @@ void ZEDWrapperNodelet::device_poll_thread_func() mPcPublishing = false; } + // ----> Object Detection mObjDetMutex.lock(); if (mObjDetRunning && objDetSubnumber > 0) { processDetectedObjects(stamp); } mObjDetMutex.unlock(); + // <---- Object Detection - // Publish the odometry if someone has subscribed to - if (computeTracking) + // ----> Process Positional Tracking + if (!mDepthDisabled) { - if (!mSensor2BaseTransfValid) - { - getSens2BaseTransform(); - } - - if (!mSensor2CameraTransfValid) - { - getSens2CameraTransform(); - } - - if (!mCamera2BaseTransfValid) + if (mPosTrackingStarted) { - getCamera2BaseTransform(); + processOdometry(); + processPose(); } - if (!mInitOdomWithPose) - { - sl::Pose deltaOdom; - mPosTrackingStatus = mZed.getPosition(deltaOdom, sl::REFERENCE_FRAME::CAMERA); - - sl::Translation translation = deltaOdom.getTranslation(); - sl::Orientation quat = deltaOdom.getOrientation(); - -#if 0 - NODELET_DEBUG("delta ODOM [%s] - %.2f,%.2f,%.2f %.2f,%.2f,%.2f,%.2f", - sl::toString(mTrackingStatus).c_str(), - translation(0), translation(1), translation(2), - quat(0), quat(1), quat(2), quat(3)); - - NODELET_DEBUG_STREAM("ODOM -> Tracking Status: " << sl::toString(mTrackingStatus)); -#endif - - if (mPosTrackingStatus == sl::POSITIONAL_TRACKING_STATE::OK || - mPosTrackingStatus == sl::POSITIONAL_TRACKING_STATE::SEARCHING || - mPosTrackingStatus == sl::POSITIONAL_TRACKING_STATE::FPS_TOO_LOW) - { - // Transform ZED delta odom pose in TF2 Transformation - geometry_msgs::Transform deltaTransf; - deltaTransf.translation.x = translation(0); - deltaTransf.translation.y = translation(1); - deltaTransf.translation.z = translation(2); - deltaTransf.rotation.x = quat(0); - deltaTransf.rotation.y = quat(1); - deltaTransf.rotation.z = quat(2); - deltaTransf.rotation.w = quat(3); - tf2::Transform deltaOdomTf; - tf2::fromMsg(deltaTransf, deltaOdomTf); - // delta odom from sensor to base frame - tf2::Transform deltaOdomTf_base = mSensor2BaseTransf.inverse() * deltaOdomTf * mSensor2BaseTransf; - - // Propagate Odom transform in time - mOdom2BaseTransf = mOdom2BaseTransf * deltaOdomTf_base; - - if (mTwoDMode) - { - tf2::Vector3 tr_2d = mOdom2BaseTransf.getOrigin(); - tr_2d.setZ(mFixedZValue); - mOdom2BaseTransf.setOrigin(tr_2d); - - double roll, pitch, yaw; - tf2::Matrix3x3(mOdom2BaseTransf.getRotation()).getRPY(roll, pitch, yaw); - - tf2::Quaternion quat_2d; - quat_2d.setRPY(0.0, 0.0, yaw); - - mOdom2BaseTransf.setRotation(quat_2d); - } - -#if 0 //#ifndef NDEBUG // Enable for TF checking - double roll, pitch, yaw; - tf2::Matrix3x3(mOdom2BaseTransf.getRotation()).getRPY(roll, pitch, yaw); - - NODELET_DEBUG("+++ Odometry [%s -> %s] - {%.3f,%.3f,%.3f} {%.3f,%.3f,%.3f}", - mOdometryFrameId.c_str(), mBaseFrameId.c_str(), - mOdom2BaseTransf.getOrigin().x(), mOdom2BaseTransf.getOrigin().y(), mOdom2BaseTransf.getOrigin().z(), - roll * RAD2DEG, pitch * RAD2DEG, yaw * RAD2DEG); -#endif - - // Publish odometry message - if (odomSubnumber > 0) - { - publishOdom(mOdom2BaseTransf, deltaOdom, stamp); - } - - mPosTrackingReady = true; - } - } - else if (mFloorAlignment) - { - NODELET_WARN_THROTTLE(5.0, - "Odometry will be published as soon as the floor as been detected for the first " - "time"); - } - } - - // Publish the zed camera pose if someone has subscribed to - if (computeTracking) - { - mPosTrackingStatus = mZed.getPosition(mLastZedPose, sl::REFERENCE_FRAME::WORLD); - - sl::Translation translation = mLastZedPose.getTranslation(); - sl::Orientation quat = mLastZedPose.getOrientation(); - -#if 0 //#ifndef NDEBUG // Enable for TF checking - double roll, pitch, yaw; - tf2::Matrix3x3(tf2::Quaternion(quat.ox, quat.oy, quat.oz, quat.ow)).getRPY(roll, pitch, yaw); - - NODELET_DEBUG("Sensor POSE [%s -> %s] - {%.2f,%.2f,%.2f} {%.2f,%.2f,%.2f}", - mLeftCamFrameId.c_str(), mMapFrameId.c_str(), - translation.x, translation.y, translation.z, - roll * RAD2DEG, pitch * RAD2DEG, yaw * RAD2DEG); - - NODELET_DEBUG_STREAM("MAP -> Tracking Status: " << sl::toString(mTrackingStatus)); - -#endif - - static sl::POSITIONAL_TRACKING_STATE old_tracking_state = sl::POSITIONAL_TRACKING_STATE::OFF; - if (mPosTrackingStatus == sl::POSITIONAL_TRACKING_STATE::OK || - mPosTrackingStatus == sl::POSITIONAL_TRACKING_STATE::SEARCHING - /*|| status == sl::POSITIONAL_TRACKING_STATE::FPS_TOO_LOW*/) - { - if (mPosTrackingStatus == sl::POSITIONAL_TRACKING_STATE::OK && mPosTrackingStatus != old_tracking_state) - { - NODELET_INFO_STREAM("Positional tracking -> OK [" << sl::toString(mPosTrackingStatus).c_str() << "]"); - old_tracking_state = mPosTrackingStatus; - } - if (mPosTrackingStatus == sl::POSITIONAL_TRACKING_STATE::SEARCHING && - mPosTrackingStatus != old_tracking_state) - { - NODELET_INFO_STREAM("Positional tracking -> Searching for a known position [" - << sl::toString(mPosTrackingStatus).c_str() << "]"); - old_tracking_state = mPosTrackingStatus; - } - // Transform ZED pose in TF2 Transformation - geometry_msgs::Transform map2sensTransf; - - map2sensTransf.translation.x = translation(0); - map2sensTransf.translation.y = translation(1); - map2sensTransf.translation.z = translation(2); - map2sensTransf.rotation.x = quat(0); - map2sensTransf.rotation.y = quat(1); - map2sensTransf.rotation.z = quat(2); - map2sensTransf.rotation.w = quat(3); - tf2::Transform map_to_sens_transf; - tf2::fromMsg(map2sensTransf, map_to_sens_transf); - - mMap2BaseTransf = map_to_sens_transf * mSensor2BaseTransf; // Base position in map frame - - if (mTwoDMode) - { - tf2::Vector3 tr_2d = mMap2BaseTransf.getOrigin(); - tr_2d.setZ(mFixedZValue); - mMap2BaseTransf.setOrigin(tr_2d); - - double roll, pitch, yaw; - tf2::Matrix3x3(mMap2BaseTransf.getRotation()).getRPY(roll, pitch, yaw); - - tf2::Quaternion quat_2d; - quat_2d.setRPY(0.0, 0.0, yaw); - - mMap2BaseTransf.setRotation(quat_2d); - } - -#if 0 //#ifndef NDEBUG // Enable for TF checking - double roll, pitch, yaw; - tf2::Matrix3x3(mMap2BaseTransf.getRotation()).getRPY(roll, pitch, yaw); - - NODELET_DEBUG("*** Base POSE [%s -> %s] - {%.3f,%.3f,%.3f} {%.3f,%.3f,%.3f}", mMapFrameId.c_str(), - mBaseFrameId.c_str(), mMap2BaseTransf.getOrigin().x(), mMap2BaseTransf.getOrigin().y(), - mMap2BaseTransf.getOrigin().z(), roll * RAD2DEG, pitch * RAD2DEG, yaw * RAD2DEG); -#endif - - bool initOdom = false; - - if (!(mFloorAlignment)) - { - initOdom = mInitOdomWithPose; - } - else - { - initOdom = (mPosTrackingStatus == sl::POSITIONAL_TRACKING_STATE::OK) & mInitOdomWithPose; - } - - if (initOdom || mResetOdom) - { - NODELET_INFO("Odometry aligned to last tracking pose"); - - // Propagate Odom transform in time - mOdom2BaseTransf = mMap2BaseTransf; - mMap2BaseTransf.setIdentity(); - - if (odomSubnumber > 0) - { - // Publish odometry message - publishOdom(mOdom2BaseTransf, mLastZedPose, mFrameTimestamp); - } - - mInitOdomWithPose = false; - mResetOdom = false; - } - else - { - // Transformation from map to odometry frame - // mMap2OdomTransf = mOdom2BaseTransf.inverse() * mMap2BaseTransf; - mMap2OdomTransf = mMap2BaseTransf * mOdom2BaseTransf.inverse(); - -#if 0 //#ifndef NDEBUG // Enable for TF checking - double roll, pitch, yaw; - tf2::Matrix3x3(mMap2OdomTransf.getRotation()).getRPY(roll, pitch, yaw); - - NODELET_DEBUG("+++ Diff [%s -> %s] - {%.3f,%.3f,%.3f} {%.3f,%.3f,%.3f}", - mMapFrameId.c_str(), mOdometryFrameId.c_str(), - mMap2OdomTransf.getOrigin().x(), mMap2OdomTransf.getOrigin().y(), mMap2OdomTransf.getOrigin().z(), - roll * RAD2DEG, pitch * RAD2DEG, yaw * RAD2DEG); -#endif - } - - // Publish Pose message - if ((poseSubnumber + poseCovSubnumber) > 0) - { - publishPose(stamp); - } - - mPosTrackingReady = true; - } - } - - // Note: in SVO mode the TF must not be broadcasted at the same frequency of the IMU data to avoid timestamp - // issues - if (mZedRealCamModel == sl::MODEL::ZED || mSvoMode) - { - // Publish pose tf only if enabled - if (mPublishTf) - { - // Note, the frame is published, but its values will only change if - // someone has subscribed to odom - publishOdomFrame(mOdom2BaseTransf, stamp); // publish the base Frame in odometry frame - - if (mPublishMapTf) - { - // Note, the frame is published, but its values will only change if - // someone has subscribed to map - publishPoseFrame(mMap2OdomTransf, stamp); // publish the odometry Frame in map frame - } - } + // Publish `odom` and `map` TFs at the grab frequency + // RCLCPP_INFO(get_logger(), "Publishing TF -> threadFunc_zedGrab"); + publishTFs(mFrameTimestamp); } + // <---- Process Positional Tracking #if 0 //#ifndef NDEBUG // Enable for TF checking \ // Double check: map_to_pose must be equal to mMap2BaseTransf @@ -4539,36 +4216,6 @@ void ZEDWrapperNodelet::device_poll_thread_func() { NODELET_DEBUG_THROTTLE(5.0, "No topics subscribed by users"); - if (mSvoMode || mZedRealCamModel == sl::MODEL::ZED || !mPublishImuTf) - { - // Publish odometry tf only if enabled - if (mPublishTf) - { - ros::Time t; - - if (mSvoMode) - { - t = ros::Time::now(); - } - else - { - t = sl_tools::slTime2Ros(mZed.getTimestamp(sl::TIME_REFERENCE::CURRENT)); - } - - publishOdomFrame(mOdom2BaseTransf, mFrameTimestamp); // publish the base Frame in odometry frame - - if (mPublishMapTf) - { - publishPoseFrame(mMap2OdomTransf, mFrameTimestamp); // publish the odometry Frame in map frame - } - - if (mPublishImuTf && !mStaticImuFramePublished) - { - publishStaticImuFrame(); - } - } - } - std::this_thread::sleep_for(std::chrono::milliseconds(10)); // No subscribers, we just wait loop_rate.reset(); } @@ -4576,7 +4223,7 @@ void ZEDWrapperNodelet::device_poll_thread_func() mDiagUpdater.update(); } // while loop - if (mSaveAreaMapOnClosing && mPosTrackingActivated) + if (mSaveAreaMapOnClosing && mPosTrackingStarted) { saveAreaMap(mAreaMemDbPath); } @@ -4595,7 +4242,7 @@ void ZEDWrapperNodelet::device_poll_thread_func() mZed.close(); NODELET_DEBUG("ZED pool thread finished"); -} +} // namespace zed_nodelets void ZEDWrapperNodelet::processCameraSettings() { @@ -4870,9 +4517,10 @@ void ZEDWrapperNodelet::callback_updateDiagnostic(diagnostic_updater::Diagnostic } } - if (mPosTrackingActivated) + if (mPosTrackingStarted) { - stat.addf("Tracking status", "%s", sl::toString(mPosTrackingStatus).c_str()); + stat.addf("Pos. Tracking status [Pose]", "%s", sl::toString(mPosTrackingStatusWorld).c_str()); + stat.addf("Pos. Tracking status [Odometry]", "%s", sl::toString(mPosTrackingStatusCamera).c_str()); } else { @@ -5821,4 +5469,353 @@ void ZEDWrapperNodelet::clickedPtCallback(geometry_msgs::PointStampedConstPtr ms } } +void ZEDWrapperNodelet::publishPoseStatus() +{ + size_t statusSub = mPubPoseStatus.getNumSubscribers(); + + if (statusSub > 0) + { + zed_interfaces::PosTrackStatusPtr msg = boost::make_shared(); + msg->status = static_cast(mPosTrackingStatusWorld); + + mPubPoseStatus.publish(msg); + } +} + +void ZEDWrapperNodelet::publishOdomStatus() +{ + size_t statusSub = mPubOdomStatus.getNumSubscribers(); + + if (statusSub > 0) + { + zed_interfaces::PosTrackStatusPtr msg = boost::make_shared(); + msg->status = static_cast(mPosTrackingStatusCamera); + + mPubPoseStatus.publish(msg); + } +} + +void ZEDWrapperNodelet::processOdometry() +{ + if (!mSensor2BaseTransfValid) + { + getSens2BaseTransform(); + } + + if (!mSensor2CameraTransfValid) + { + getSens2CameraTransform(); + } + + if (!mCamera2BaseTransfValid) + { + getCamera2BaseTransform(); + } + + if (!mInitOdomWithPose) + { + sl::Pose deltaOdom; + + mPosTrackingStatusCamera = mZed.getPosition(deltaOdom, sl::REFERENCE_FRAME::CAMERA); + publishOdomStatus(); + + NODELET_DEBUG("delta ODOM [%s]:\n%s", sl::toString(mPosTrackingStatusCamera).c_str(), + deltaOdom.pose_data.getInfos().c_str()); + + if (mPosTrackingStatusCamera == sl::POSITIONAL_TRACKING_STATE::OK || + mPosTrackingStatusCamera == sl::POSITIONAL_TRACKING_STATE::SEARCHING || + mPosTrackingStatusCamera == sl::POSITIONAL_TRACKING_STATE::FPS_TOO_LOW) + { + sl::Translation translation = deltaOdom.getTranslation(); + sl::Orientation quat = deltaOdom.getOrientation(); + + // Transform ZED delta odom pose in TF2 Transformation + tf2::Transform deltaOdomTf; + deltaOdomTf.setOrigin(tf2::Vector3(translation(0), translation(1), translation(2))); + // w at the end in the constructor + deltaOdomTf.setRotation(tf2::Quaternion(quat(0), quat(1), quat(2), quat(3))); + + // delta odom from sensor to base frame + tf2::Transform deltaOdomTf_base = mSensor2BaseTransf.inverse() * deltaOdomTf * mSensor2BaseTransf; + // TODO(Walter) Use tf2::transform instead? + + // Propagate Odom transform in time + mOdom2BaseTransf = mOdom2BaseTransf * deltaOdomTf_base; + + if (mTwoDMode) + { + tf2::Vector3 tr_2d = mOdom2BaseTransf.getOrigin(); + tr_2d.setZ(mFixedZValue); + mOdom2BaseTransf.setOrigin(tr_2d); + + double roll, pitch, yaw; + tf2::Matrix3x3(mOdom2BaseTransf.getRotation()).getRPY(roll, pitch, yaw); + + tf2::Quaternion quat_2d; + quat_2d.setRPY(0.0, 0.0, yaw); + + mOdom2BaseTransf.setRotation(quat_2d); + } + + double roll, pitch, yaw; + tf2::Matrix3x3(mOdom2BaseTransf.getRotation()).getRPY(roll, pitch, yaw); + + NODELET_DEBUG("+++ Odometry [%s -> %s] - {%.3f,%.3f,%.3f} {%.3f,%.3f,%.3f}", mOdomFrameId.c_str(), + mBaseFrameId.c_str(), mOdom2BaseTransf.getOrigin().x(), mOdom2BaseTransf.getOrigin().y(), + mOdom2BaseTransf.getOrigin().z(), roll * RAD2DEG, pitch * RAD2DEG, yaw * RAD2DEG); + + // Publish odometry message + publishOdom(mOdom2BaseTransf, deltaOdom, mFrameTimestamp); + mPosTrackingReady = true; + } + } + else if (mFloorAlignment) + { + NODELET_DEBUG_STREAM_THROTTLE(5.0, + "Odometry will be published as soon as the floor as " + "been detected for the first time"); + } +} + +void ZEDWrapperNodelet::processPose() +{ + if (!mSensor2BaseTransfValid) + { + getSens2BaseTransform(); + } + + if (!mSensor2CameraTransfValid) + { + getSens2CameraTransform(); + } + + if (!mCamera2BaseTransfValid) + { + getCamera2BaseTransform(); + } + + mPosTrackingStatusWorld = mZed.getPosition(mLastZedPose, sl::REFERENCE_FRAME::WORLD); + + publishPoseStatus(); + + sl::Translation translation = mLastZedPose.getTranslation(); + sl::Orientation quat = mLastZedPose.getOrientation(); + + if (quat.sum() == 0) + { + return; + } + + NODELET_DEBUG_STREAM("MAP -> Tracking Status: " << sl::toString(mPosTrackingStatusWorld).c_str()); + NODELET_DEBUG("Sensor POSE [%s -> %s]:\n%s}", mLeftCamFrameId.c_str(), mMapFrameId.c_str(), + mLastZedPose.pose_data.getInfos().c_str()); + + if (mPosTrackingStatusWorld == sl::POSITIONAL_TRACKING_STATE::OK || + mPosTrackingStatusWorld == sl::POSITIONAL_TRACKING_STATE::SEARCHING) + { + double roll, pitch, yaw; + tf2::Matrix3x3(tf2::Quaternion(quat.ox, quat.oy, quat.oz, quat.ow)).getRPY(roll, pitch, yaw); + + tf2::Transform map_to_sens_transf; + map_to_sens_transf.setOrigin(tf2::Vector3(translation(0), translation(1), translation(2))); + map_to_sens_transf.setRotation(tf2::Quaternion(quat(0), quat(1), quat(2), quat(3))); + + mMap2BaseTransf = map_to_sens_transf * mSensor2BaseTransf; // Base position in map frame + + if (mTwoDMode) + { + tf2::Vector3 tr_2d = mMap2BaseTransf.getOrigin(); + tr_2d.setZ(mFixedZValue); + mMap2BaseTransf.setOrigin(tr_2d); + + double roll, pitch, yaw; + tf2::Matrix3x3(mMap2BaseTransf.getRotation()).getRPY(roll, pitch, yaw); + + tf2::Quaternion quat_2d; + quat_2d.setRPY(0.0, 0.0, yaw); + + mMap2BaseTransf.setRotation(quat_2d); + } + + // double roll, pitch, yaw; + tf2::Matrix3x3(mMap2BaseTransf.getRotation()).getRPY(roll, pitch, yaw); + + NODELET_DEBUG("*** Base POSE [%s -> %s] - {%.3f,%.3f,%.3f} {%.3f,%.3f,%.3f}", mMapFrameId.c_str(), + mBaseFrameId.c_str(), mMap2BaseTransf.getOrigin().x(), mMap2BaseTransf.getOrigin().y(), + mMap2BaseTransf.getOrigin().z(), roll * RAD2DEG, pitch * RAD2DEG, yaw * RAD2DEG); + + bool initOdom = false; + + if (!(mFloorAlignment)) + { + initOdom = mInitOdomWithPose; + } + else + { + initOdom = mInitOdomWithPose & (mPosTrackingStatusWorld == sl::POSITIONAL_TRACKING_STATE::OK); + } + + if (initOdom) + { + NODELET_INFO("Odometry aligned to last tracking pose"); + + // Propagate Odom transform in time + mOdom2BaseTransf = mMap2BaseTransf; + mMap2BaseTransf.setIdentity(); + + double roll, pitch, yaw; + tf2::Matrix3x3(mOdom2BaseTransf.getRotation()).getRPY(roll, pitch, yaw); + + NODELET_INFO(" * Initial odometry [%s -> %s] - {%.3f,%.3f,%.3f} {%.3f,%.3f,%.3f}", mOdomFrameId.c_str(), + mBaseFrameId.c_str(), mOdom2BaseTransf.getOrigin().x(), mOdom2BaseTransf.getOrigin().y(), + mOdom2BaseTransf.getOrigin().z(), roll * RAD2DEG, pitch * RAD2DEG, yaw * RAD2DEG); + + mInitOdomWithPose = false; + } + else + { + // Transformation from map to odometry frame + mMap2OdomTransf = mMap2BaseTransf * mOdom2BaseTransf.inverse(); + + double roll, pitch, yaw; + tf2::Matrix3x3(mMap2OdomTransf.getRotation()).getRPY(roll, pitch, yaw); + + NODELET_DEBUG("+++ Diff [%s -> %s] - {%.3f,%.3f,%.3f} {%.3f,%.3f,%.3f}", mMapFrameId.c_str(), + mOdomFrameId.c_str(), mMap2OdomTransf.getOrigin().x(), mMap2OdomTransf.getOrigin().y(), + mMap2OdomTransf.getOrigin().z(), roll * RAD2DEG, pitch * RAD2DEG, yaw * RAD2DEG); + } + + // Publish Pose message + publishPose(mFrameTimestamp); + mPosTrackingReady = true; + } +} + +void ZEDWrapperNodelet::publishTFs(ros::Time t) +{ + // DEBUG_STREAM_PT("publishTFs"); + + // RCLCPP_INFO_STREAM(get_logger(), "publishTFs - t type:" << + // t.get_clock_type()); + + if (!mPosTrackingReady) + { + return; + } + + if (t.isZero()) + { + NODELET_DEBUG("Time zero: not publishing TFs"); + return; + } + + // Publish pose tf only if enabled + if (mDepthMode != sl::DEPTH_MODE::NONE && mPublishTF) + { + publishOdomTF(t); // publish the base Frame in odometry frame + + if (mPublishMapTF) + { + publishPoseTF(t); // publish the odometry Frame in map frame + } + } +} + +void ZEDWrapperNodelet::publishOdomTF(ros::Time t) +{ + // DEBUG_STREAM_PT("publishOdomTF"); + + // ----> Avoid duplicated TF publishing + if (t == mLastTs_odom) + { + return; + } + mLastTs_odom = t; + // <---- Avoid duplicated TF publishing + + if (!mSensor2BaseTransfValid) + { + getSens2BaseTransform(); + } + + if (!mSensor2CameraTransfValid) + { + getSens2CameraTransform(); + } + + if (!mCamera2BaseTransfValid) + { + getCamera2BaseTransform(); + } + + geometry_msgs::TransformStamped transformStamped; + + transformStamped.header.stamp = t; + + // RCLCPP_INFO_STREAM(get_logger(), "Odom TS: " << transformStamped.header.stamp); + + transformStamped.header.frame_id = mOdomFrameId; + transformStamped.child_frame_id = mBaseFrameId; + // conversion from Tranform to message + tf2::Vector3 translation = mOdom2BaseTransf.getOrigin(); + tf2::Quaternion quat = mOdom2BaseTransf.getRotation(); + transformStamped.transform.translation.x = translation.x(); + transformStamped.transform.translation.y = translation.y(); + transformStamped.transform.translation.z = translation.z(); + transformStamped.transform.rotation.x = quat.x(); + transformStamped.transform.rotation.y = quat.y(); + transformStamped.transform.rotation.z = quat.z(); + transformStamped.transform.rotation.w = quat.w(); + + // Publish transformation + mTfBroadcaster.sendTransform(transformStamped); +} + +void ZEDWrapperNodelet::publishPoseTF(ros::Time t) +{ + // DEBUG_STREAM_PT("publishPoseTF"); + + // ----> Avoid duplicated TF publishing + if (t == mLastTs_pose) + { + return; + } + mLastTs_pose = t; + // <---- Avoid duplicated TF publishing + + if (!mSensor2BaseTransfValid) + { + getSens2BaseTransform(); + } + + if (!mSensor2CameraTransfValid) + { + getSens2CameraTransform(); + } + + if (!mCamera2BaseTransfValid) + { + getCamera2BaseTransform(); + } + + geometry_msgs::TransformStamped transformStamped; + + transformStamped.header.stamp = t; + transformStamped.header.frame_id = mMapFrameId; + transformStamped.child_frame_id = mOdomFrameId; + // conversion from Tranform to message + tf2::Vector3 translation = mMap2OdomTransf.getOrigin(); + tf2::Quaternion quat = mMap2OdomTransf.getRotation(); + transformStamped.transform.translation.x = translation.x(); + transformStamped.transform.translation.y = translation.y(); + transformStamped.transform.translation.z = translation.z(); + transformStamped.transform.rotation.x = quat.x(); + transformStamped.transform.rotation.y = quat.y(); + transformStamped.transform.rotation.z = quat.z(); + transformStamped.transform.rotation.w = quat.w(); + + // Publish transformation + mTfBroadcaster.sendTransform(transformStamped); +} + } // namespace zed_nodelets From ca9a5dde11917f43661f340ecf7742d6269c4423 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Thu, 14 Sep 2023 17:19:32 +0200 Subject: [PATCH 176/199] Add odomMutex --- .../include/zed_wrapper_nodelet.hpp | 1 - .../zed_nodelet/src/zed_wrapper_nodelet.cpp | 55 ++++++------------- 2 files changed, 17 insertions(+), 39 deletions(-) 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 a15799cb..35202f2f 100644 --- a/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp +++ b/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp @@ -687,7 +687,6 @@ class ZEDWrapperNodelet : public nodelet::Nodelet tf2::Transform mMap2OdomTransf; // Coordinates of the odometry frame in map frame tf2::Transform mOdom2BaseTransf; // Coordinates of the base in odometry frame tf2::Transform mMap2BaseTransf; // Coordinates of the base in map frame - tf2::Transform mMap2CameraTransf; // Coordinates of the camera in base frame tf2::Transform mSensor2BaseTransf; // Coordinates of the base frame in sensor frame tf2::Transform mSensor2CameraTransf; // Coordinates of the camera frame in sensor frame tf2::Transform mCamera2BaseTransf; // Coordinates of the base frame in camera frame 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 c1de1166..a0c647be 100644 --- a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp @@ -1598,7 +1598,6 @@ void ZEDWrapperNodelet::initTransforms() mOdom2BaseTransf.setIdentity(); // broadcasted if `publish_tf` is true mMap2OdomTransf.setIdentity(); // broadcasted if `publish_map_tf` is true mMap2BaseTransf.setIdentity(); // used internally, but not broadcasted - mMap2CameraTransf.setIdentity(); // used internally, but not broadcasted // <---- Dynamic transforms } @@ -1848,7 +1847,7 @@ bool ZEDWrapperNodelet::on_reset_odometry(zed_interfaces::reset_odometry::Reques std::lock_guard lock(mOdomMutex); mOdom2BaseTransf.setIdentity(); mOdomPath.clear(); - + res.reset_done = true; return true; } @@ -3271,6 +3270,7 @@ void ZEDWrapperNodelet::callback_pubPath(const ros::TimerEvent& e) // conversion from Tranform to message geometry_msgs::Transform base2odom = tf2::toMsg(mOdom2BaseTransf); // Add all value in Pose message + mOdomMutex.lock(); odomPose.pose.position.x = base2odom.translation.x; odomPose.pose.position.y = base2odom.translation.y; odomPose.pose.position.z = base2odom.translation.z; @@ -3278,6 +3278,7 @@ void ZEDWrapperNodelet::callback_pubPath(const ros::TimerEvent& e) odomPose.pose.orientation.y = base2odom.rotation.y; odomPose.pose.orientation.z = base2odom.rotation.z; odomPose.pose.orientation.w = base2odom.rotation.w; + mOdomMutex.unlock(); mapPose.header.stamp = mFrameTimestamp; mapPose.header.frame_id = mMapFrameId; // frame @@ -3293,6 +3294,7 @@ void ZEDWrapperNodelet::callback_pubPath(const ros::TimerEvent& e) mapPose.pose.orientation.w = base2map.rotation.w; // Circular vector + std::lock_guard lock(mOdomMutex); if (mPathMaxCount != -1) { if (mOdomPath.size() == mPathMaxCount) @@ -5540,6 +5542,7 @@ void ZEDWrapperNodelet::processOdometry() // TODO(Walter) Use tf2::transform instead? // Propagate Odom transform in time + mOdomMutex.lock(); mOdom2BaseTransf = mOdom2BaseTransf * deltaOdomTf_base; if (mTwoDMode) @@ -5567,6 +5570,8 @@ void ZEDWrapperNodelet::processOdometry() // Publish odometry message publishOdom(mOdom2BaseTransf, deltaOdom, mFrameTimestamp); mPosTrackingReady = true; + + mOdomMutex.unlock(); } } else if (mFloorAlignment) @@ -5646,44 +5651,16 @@ void ZEDWrapperNodelet::processPose() bool initOdom = false; - if (!(mFloorAlignment)) - { - initOdom = mInitOdomWithPose; - } - else - { - initOdom = mInitOdomWithPose & (mPosTrackingStatusWorld == sl::POSITIONAL_TRACKING_STATE::OK); - } - - if (initOdom) - { - NODELET_INFO("Odometry aligned to last tracking pose"); - - // Propagate Odom transform in time - mOdom2BaseTransf = mMap2BaseTransf; - mMap2BaseTransf.setIdentity(); - - double roll, pitch, yaw; - tf2::Matrix3x3(mOdom2BaseTransf.getRotation()).getRPY(roll, pitch, yaw); + // Transformation from map to odometry frame + mOdomMutex.lock(); + mMap2OdomTransf = mMap2BaseTransf * mOdom2BaseTransf.inverse(); + mOdomMutex.unlock(); - NODELET_INFO(" * Initial odometry [%s -> %s] - {%.3f,%.3f,%.3f} {%.3f,%.3f,%.3f}", mOdomFrameId.c_str(), - mBaseFrameId.c_str(), mOdom2BaseTransf.getOrigin().x(), mOdom2BaseTransf.getOrigin().y(), - mOdom2BaseTransf.getOrigin().z(), roll * RAD2DEG, pitch * RAD2DEG, yaw * RAD2DEG); + tf2::Matrix3x3(mMap2OdomTransf.getRotation()).getRPY(roll, pitch, yaw); - mInitOdomWithPose = false; - } - else - { - // Transformation from map to odometry frame - mMap2OdomTransf = mMap2BaseTransf * mOdom2BaseTransf.inverse(); - - double roll, pitch, yaw; - tf2::Matrix3x3(mMap2OdomTransf.getRotation()).getRPY(roll, pitch, yaw); - - NODELET_DEBUG("+++ Diff [%s -> %s] - {%.3f,%.3f,%.3f} {%.3f,%.3f,%.3f}", mMapFrameId.c_str(), - mOdomFrameId.c_str(), mMap2OdomTransf.getOrigin().x(), mMap2OdomTransf.getOrigin().y(), - mMap2OdomTransf.getOrigin().z(), roll * RAD2DEG, pitch * RAD2DEG, yaw * RAD2DEG); - } + NODELET_DEBUG("+++ Diff [%s -> %s] - {%.3f,%.3f,%.3f} {%.3f,%.3f,%.3f}", mMapFrameId.c_str(), mOdomFrameId.c_str(), + mMap2OdomTransf.getOrigin().x(), mMap2OdomTransf.getOrigin().y(), mMap2OdomTransf.getOrigin().z(), + roll * RAD2DEG, pitch * RAD2DEG, yaw * RAD2DEG); // Publish Pose message publishPose(mFrameTimestamp); @@ -5757,8 +5734,10 @@ void ZEDWrapperNodelet::publishOdomTF(ros::Time t) transformStamped.header.frame_id = mOdomFrameId; transformStamped.child_frame_id = mBaseFrameId; // conversion from Tranform to message + mOdomMutex.lock(); tf2::Vector3 translation = mOdom2BaseTransf.getOrigin(); tf2::Quaternion quat = mOdom2BaseTransf.getRotation(); + mOdomMutex.unlock(); transformStamped.transform.translation.x = translation.x(); transformStamped.transform.translation.y = translation.y(); transformStamped.transform.translation.z = translation.z(); From ad8362cbbd68d6e548e377228682e96f0328c00c Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Fri, 15 Sep 2023 10:51:19 +0200 Subject: [PATCH 177/199] Improve odometry behavior --- .../include/zed_wrapper_nodelet.hpp | 3 +- .../zed_nodelet/src/zed_wrapper_nodelet.cpp | 274 +++++++++--------- 2 files changed, 133 insertions(+), 144 deletions(-) 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 35202f2f..5fffa5b6 100644 --- a/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp +++ b/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp @@ -192,9 +192,8 @@ class ZEDWrapperNodelet : public nodelet::Nodelet void processPose(); /*! \brief Publish the pose of the camera in "Map" frame with a ros Publisher - * \param t : the ros::Time to stamp the image */ - void publishPose(ros::Time t); + void publishPose(); /*! \brief Publish the pose of the camera in "Odom" frame with a ros Publisher * \param base2odomTransf : Transformation representing the camera pose 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 a0c647be..8191e126 100644 --- a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp @@ -1595,10 +1595,10 @@ void ZEDWrapperNodelet::initTransforms() // ------------------- // ----> Dynamic transforms - mOdom2BaseTransf.setIdentity(); // broadcasted if `publish_tf` is true - mMap2OdomTransf.setIdentity(); // broadcasted if `publish_map_tf` is true - mMap2BaseTransf.setIdentity(); // used internally, but not broadcasted - // <---- Dynamic transforms + mOdom2BaseTransf.setIdentity(); // broadcasted if `publish_tf` is true + mMap2OdomTransf.setIdentity(); // broadcasted if `publish_map_tf` is true + mMap2BaseTransf.setIdentity(); // used internally, but not broadcasted + // <---- Dynamic transforms } bool ZEDWrapperNodelet::getCamera2BaseTransform() @@ -2245,117 +2245,108 @@ bool ZEDWrapperNodelet::saveAreaMap(std::string file_path, std::string* out_msg) void ZEDWrapperNodelet::publishOdom(tf2::Transform odom2baseTransf, sl::Pose& slPose, ros::Time t) { - nav_msgs::OdometryPtr odomMsg = boost::make_shared(); + size_t odomSub = mPubOdom.getNumSubscribers(); - odomMsg->header.stamp = t; - odomMsg->header.frame_id = mOdomFrameId; // frame - odomMsg->child_frame_id = mBaseFrameId; // camera_frame - // conversion from Tranform to message - geometry_msgs::Transform base2odom = tf2::toMsg(odom2baseTransf); - // Add all value in odometry message - odomMsg->pose.pose.position.x = base2odom.translation.x; - odomMsg->pose.pose.position.y = base2odom.translation.y; - odomMsg->pose.pose.position.z = base2odom.translation.z; - odomMsg->pose.pose.orientation.x = base2odom.rotation.x; - odomMsg->pose.pose.orientation.y = base2odom.rotation.y; - odomMsg->pose.pose.orientation.z = base2odom.rotation.z; - odomMsg->pose.pose.orientation.w = base2odom.rotation.w; + if (odomSub) + { + nav_msgs::OdometryPtr odomMsg = boost::make_shared(); - // Odometry pose covariance + odomMsg->header.stamp = t; + odomMsg->header.frame_id = mOdomFrameId; // frame + odomMsg->child_frame_id = mBaseFrameId; // camera_frame - for (size_t i = 0; i < odomMsg->pose.covariance.size(); i++) - { - odomMsg->pose.covariance[i] = static_cast(slPose.pose_covariance[i]); + // Add all value in odometry message + odomMsg->pose.pose.position.x = odom2baseTransf.getOrigin().x(); + odomMsg->pose.pose.position.y = odom2baseTransf.getOrigin().y(); + odomMsg->pose.pose.position.z = odom2baseTransf.getOrigin().z(); + odomMsg->pose.pose.orientation.x = odom2baseTransf.getRotation().x(); + odomMsg->pose.pose.orientation.y = odom2baseTransf.getRotation().y(); + odomMsg->pose.pose.orientation.z = odom2baseTransf.getRotation().z(); + odomMsg->pose.pose.orientation.w = odom2baseTransf.getRotation().w(); - if (mTwoDMode) + // Odometry pose covariance + for (size_t i = 0; i < odomMsg->pose.covariance.size(); i++) { - if (i == 14 || i == 21 || i == 28) - { - odomMsg->pose.covariance[i] = 1e-9; // Very low covariance if 2D mode - } - else if ((i >= 2 && i <= 4) || (i >= 8 && i <= 10) || (i >= 12 && i <= 13) || (i >= 15 && i <= 20) || - (i >= 22 && i <= 27) || (i == 29) || (i >= 32 && i <= 34)) + odomMsg->pose.covariance[i] = static_cast(slPose.pose_covariance[i]); + + if (mTwoDMode) { - odomMsg->pose.covariance[i] = 0.0; + if ((i >= 2 && i <= 4) || (i >= 8 && i <= 10) || (i >= 12 && i <= 29) || (i >= 32 && i <= 34)) + { + odomMsg->pose.covariance[i] = 1e-9; // Very low covariance if 2D mode + } } } - } - // Publish odometry message - mPubOdom.publish(odomMsg); + // Publish odometry message + NODELET_DEBUG("Publishing ODOM message"); + mPubOdom.publish(odomMsg); + } } -void ZEDWrapperNodelet::publishPose(ros::Time t) +void ZEDWrapperNodelet::publishPose() { + size_t poseSub = mPubPose.getNumSubscribers(); + size_t poseCovSub = mPubPoseCov.getNumSubscribers(); + tf2::Transform base_pose; base_pose.setIdentity(); - if (mPublishMapTF) - { - base_pose = mMap2BaseTransf; - } - else if (mPublishTF) - { - base_pose = mOdom2BaseTransf; - } + base_pose = mMap2BaseTransf; std_msgs::Header header; - header.stamp = t; - header.frame_id = mMapFrameId; - geometry_msgs::Pose pose; + header.stamp = mFrameTimestamp; + header.frame_id = mMapFrameId; // frame - // conversion from Tranform to message - geometry_msgs::Transform base2frame = tf2::toMsg(base_pose); + geometry_msgs::Pose pose; // Add all value in Pose message - pose.position.x = base2frame.translation.x; - pose.position.y = base2frame.translation.y; - pose.position.z = base2frame.translation.z; - pose.orientation.x = base2frame.rotation.x; - pose.orientation.y = base2frame.rotation.y; - pose.orientation.z = base2frame.rotation.z; - pose.orientation.w = base2frame.rotation.w; + pose.position.x = mMap2BaseTransf.getOrigin().x(); + pose.position.y = mMap2BaseTransf.getOrigin().y(); + pose.position.z = mMap2BaseTransf.getOrigin().z(); + pose.orientation.x = mMap2BaseTransf.getRotation().x(); + pose.orientation.y = mMap2BaseTransf.getRotation().y(); + pose.orientation.z = mMap2BaseTransf.getRotation().z(); + pose.orientation.w = mMap2BaseTransf.getRotation().w(); - if (mPubPose.getNumSubscribers() > 0) + if (poseSub > 0) { - geometry_msgs::PoseStamped poseNoCov; + geometry_msgs::PoseStampedPtr poseNoCov = boost::make_shared(); - poseNoCov.header = header; - poseNoCov.pose = pose; + poseNoCov->header = header; + poseNoCov->pose = pose; // Publish pose stamped message + NODELET_DEBUG("Publishing POSE NO COV message"); mPubPose.publish(poseNoCov); } - if (mPubPoseCov.getNumSubscribers() > 0) + if (poseCovSub > 0) { - geometry_msgs::PoseWithCovarianceStampedPtr poseCovMsg = + geometry_msgs::PoseWithCovarianceStampedPtr poseCov = boost::make_shared(); - poseCovMsg->header = header; - poseCovMsg->pose.pose = pose; + poseCov->header = header; + poseCov->pose.pose = pose; // Odometry pose covariance if available - for (size_t i = 0; i < poseCovMsg->pose.covariance.size(); i++) + + for (size_t i = 0; i < poseCov->pose.covariance.size(); i++) { - poseCovMsg->pose.covariance[i] = static_cast(mLastZedPose.pose_covariance[i]); + poseCov->pose.covariance[i] = static_cast(mLastZedPose.pose_covariance[i]); if (mTwoDMode) { - if (i == 14 || i == 21 || i == 28) + if ((i >= 2 && i <= 4) || (i >= 8 && i <= 10) || (i >= 12 && i <= 29) || (i >= 32 && i <= 34)) { - poseCovMsg->pose.covariance[i] = 1e-9; // Very low covariance if 2D mode - } - else if ((i >= 2 && i <= 4) || (i >= 8 && i <= 10) || (i >= 12 && i <= 13) || (i >= 15 && i <= 16) || - (i >= 18 && i <= 20) || (i == 22) || (i >= 24 && i <= 27)) - { - poseCovMsg->pose.covariance[i] = 0.0; + poseCov->pose.covariance[i] = 1e-9; // Very low covariance if 2D mode } } } // Publish pose with covariance stamped message - mPubPoseCov.publish(poseCovMsg); + NODELET_DEBUG("Publishing POSE COV message"); + mPubPoseCov.publish(std::move(poseCov)); } } @@ -5514,71 +5505,63 @@ void ZEDWrapperNodelet::processOdometry() getCamera2BaseTransform(); } - if (!mInitOdomWithPose) - { - sl::Pose deltaOdom; - - mPosTrackingStatusCamera = mZed.getPosition(deltaOdom, sl::REFERENCE_FRAME::CAMERA); - publishOdomStatus(); - - NODELET_DEBUG("delta ODOM [%s]:\n%s", sl::toString(mPosTrackingStatusCamera).c_str(), - deltaOdom.pose_data.getInfos().c_str()); + // if (!mInitOdomWithPose) { + sl::Pose deltaOdom; - if (mPosTrackingStatusCamera == sl::POSITIONAL_TRACKING_STATE::OK || - mPosTrackingStatusCamera == sl::POSITIONAL_TRACKING_STATE::SEARCHING || - mPosTrackingStatusCamera == sl::POSITIONAL_TRACKING_STATE::FPS_TOO_LOW) - { - sl::Translation translation = deltaOdom.getTranslation(); - sl::Orientation quat = deltaOdom.getOrientation(); + mPosTrackingStatusCamera = mZed.getPosition(deltaOdom, sl::REFERENCE_FRAME::CAMERA); - // Transform ZED delta odom pose in TF2 Transformation - tf2::Transform deltaOdomTf; - deltaOdomTf.setOrigin(tf2::Vector3(translation(0), translation(1), translation(2))); - // w at the end in the constructor - deltaOdomTf.setRotation(tf2::Quaternion(quat(0), quat(1), quat(2), quat(3))); + publishOdomStatus(); - // delta odom from sensor to base frame - tf2::Transform deltaOdomTf_base = mSensor2BaseTransf.inverse() * deltaOdomTf * mSensor2BaseTransf; - // TODO(Walter) Use tf2::transform instead? + NODELET_DEBUG("delta ODOM [%s]:\n%s", sl::toString(mPosTrackingStatusCamera).c_str(), + deltaOdom.pose_data.getInfos().c_str()); - // Propagate Odom transform in time - mOdomMutex.lock(); - mOdom2BaseTransf = mOdom2BaseTransf * deltaOdomTf_base; + if (mPosTrackingStatusCamera == sl::POSITIONAL_TRACKING_STATE::OK || + mPosTrackingStatusCamera == sl::POSITIONAL_TRACKING_STATE::SEARCHING || + mPosTrackingStatusCamera == sl::POSITIONAL_TRACKING_STATE::FPS_TOO_LOW) + { + sl::Translation translation = deltaOdom.getTranslation(); + sl::Orientation quat = deltaOdom.getOrientation(); - if (mTwoDMode) - { - tf2::Vector3 tr_2d = mOdom2BaseTransf.getOrigin(); - tr_2d.setZ(mFixedZValue); - mOdom2BaseTransf.setOrigin(tr_2d); + // Transform ZED delta odom pose in TF2 Transformation + tf2::Transform deltaOdomTf; + deltaOdomTf.setOrigin(tf2::Vector3(translation.x, translation.y, translation.z)); + // w at the end in the constructor + deltaOdomTf.setRotation(tf2::Quaternion(quat.x, quat.y, quat.z, quat.w)); - double roll, pitch, yaw; - tf2::Matrix3x3(mOdom2BaseTransf.getRotation()).getRPY(roll, pitch, yaw); + // delta odom from sensor to base frame + tf2::Transform deltaOdomTf_base = mSensor2BaseTransf.inverse() * deltaOdomTf * mSensor2BaseTransf; - tf2::Quaternion quat_2d; - quat_2d.setRPY(0.0, 0.0, yaw); + // Propagate Odom transform in time + mOdomMutex.lock(); // + mOdom2BaseTransf = mOdom2BaseTransf * deltaOdomTf_base; - mOdom2BaseTransf.setRotation(quat_2d); - } + if (mTwoDMode) + { + tf2::Vector3 tr_2d = mOdom2BaseTransf.getOrigin(); + tr_2d.setZ(mFixedZValue); + mOdom2BaseTransf.setOrigin(tr_2d); double roll, pitch, yaw; tf2::Matrix3x3(mOdom2BaseTransf.getRotation()).getRPY(roll, pitch, yaw); - NODELET_DEBUG("+++ Odometry [%s -> %s] - {%.3f,%.3f,%.3f} {%.3f,%.3f,%.3f}", mOdomFrameId.c_str(), - mBaseFrameId.c_str(), mOdom2BaseTransf.getOrigin().x(), mOdom2BaseTransf.getOrigin().y(), - mOdom2BaseTransf.getOrigin().z(), roll * RAD2DEG, pitch * RAD2DEG, yaw * RAD2DEG); - - // Publish odometry message - publishOdom(mOdom2BaseTransf, deltaOdom, mFrameTimestamp); - mPosTrackingReady = true; + tf2::Quaternion quat_2d; + quat_2d.setRPY(0.0, 0.0, yaw); - mOdomMutex.unlock(); + mOdom2BaseTransf.setRotation(quat_2d); } - } - else if (mFloorAlignment) - { - NODELET_DEBUG_STREAM_THROTTLE(5.0, - "Odometry will be published as soon as the floor as " - "been detected for the first time"); + + double roll, pitch, yaw; + tf2::Matrix3x3(mOdom2BaseTransf.getRotation()).getRPY(roll, pitch, yaw); + + NODELET_DEBUG("+++ Odometry [%s -> %s] - {%.3f,%.3f,%.3f} {%.3f,%.3f,%.3f}", mOdomFrameId.c_str(), + mBaseFrameId.c_str(), mOdom2BaseTransf.getOrigin().x(), mOdom2BaseTransf.getOrigin().y(), + mOdom2BaseTransf.getOrigin().z(), roll * RAD2DEG, pitch * RAD2DEG, yaw * RAD2DEG); + + // Publish odometry message + publishOdom(mOdom2BaseTransf, deltaOdom, mFrameTimestamp); + mPosTrackingReady = true; + + mOdomMutex.unlock(); // } } @@ -5601,6 +5584,8 @@ void ZEDWrapperNodelet::processPose() mPosTrackingStatusWorld = mZed.getPosition(mLastZedPose, sl::REFERENCE_FRAME::WORLD); + NODELET_DEBUG_STREAM("ZED Pose: " << mLastZedPose.pose_data.getInfos()); + publishPoseStatus(); sl::Translation translation = mLastZedPose.getTranslation(); @@ -5612,11 +5597,11 @@ void ZEDWrapperNodelet::processPose() } NODELET_DEBUG_STREAM("MAP -> Tracking Status: " << sl::toString(mPosTrackingStatusWorld).c_str()); + NODELET_DEBUG("Sensor POSE [%s -> %s]:\n%s}", mLeftCamFrameId.c_str(), mMapFrameId.c_str(), mLastZedPose.pose_data.getInfos().c_str()); - if (mPosTrackingStatusWorld == sl::POSITIONAL_TRACKING_STATE::OK || - mPosTrackingStatusWorld == sl::POSITIONAL_TRACKING_STATE::SEARCHING) + if (mPosTrackingStatusWorld == sl::POSITIONAL_TRACKING_STATE::OK) { double roll, pitch, yaw; tf2::Matrix3x3(tf2::Quaternion(quat.ox, quat.oy, quat.oz, quat.ow)).getRPY(roll, pitch, yaw); @@ -5625,7 +5610,8 @@ void ZEDWrapperNodelet::processPose() map_to_sens_transf.setOrigin(tf2::Vector3(translation(0), translation(1), translation(2))); map_to_sens_transf.setRotation(tf2::Quaternion(quat(0), quat(1), quat(2), quat(3))); - mMap2BaseTransf = map_to_sens_transf * mSensor2BaseTransf; // Base position in map frame + mMap2BaseTransf = + mSensor2BaseTransf.inverse() * map_to_sens_transf * mSensor2BaseTransf; // Base position in map frame if (mTwoDMode) { @@ -5652,27 +5638,29 @@ void ZEDWrapperNodelet::processPose() bool initOdom = false; // Transformation from map to odometry frame - mOdomMutex.lock(); + mOdomMutex.lock(); // mMap2OdomTransf = mMap2BaseTransf * mOdom2BaseTransf.inverse(); - mOdomMutex.unlock(); + mOdomMutex.unlock(); // + // double roll, pitch, yaw; tf2::Matrix3x3(mMap2OdomTransf.getRotation()).getRPY(roll, pitch, yaw); NODELET_DEBUG("+++ Diff [%s -> %s] - {%.3f,%.3f,%.3f} {%.3f,%.3f,%.3f}", mMapFrameId.c_str(), mOdomFrameId.c_str(), mMap2OdomTransf.getOrigin().x(), mMap2OdomTransf.getOrigin().y(), mMap2OdomTransf.getOrigin().z(), roll * RAD2DEG, pitch * RAD2DEG, yaw * RAD2DEG); + //} // Publish Pose message - publishPose(mFrameTimestamp); + publishPose(); mPosTrackingReady = true; } } void ZEDWrapperNodelet::publishTFs(ros::Time t) { - // DEBUG_STREAM_PT("publishTFs"); + // NODELET_DEBUG("publishTFs"); - // RCLCPP_INFO_STREAM(get_logger(), "publishTFs - t type:" << + // NODELET_DEBUG_STREAM("publishTFs - t type:" << // t.get_clock_type()); if (!mPosTrackingReady) @@ -5700,14 +5688,16 @@ void ZEDWrapperNodelet::publishTFs(ros::Time t) void ZEDWrapperNodelet::publishOdomTF(ros::Time t) { - // DEBUG_STREAM_PT("publishOdomTF"); + // NODELET_DEBUG("publishOdomTF"); // ----> Avoid duplicated TF publishing - if (t == mLastTs_odom) + static ros::Time last_stamp; + + if (t == last_stamp) { return; } - mLastTs_odom = t; + last_stamp = t; // <---- Avoid duplicated TF publishing if (!mSensor2BaseTransfValid) @@ -5726,18 +5716,17 @@ void ZEDWrapperNodelet::publishOdomTF(ros::Time t) } geometry_msgs::TransformStamped transformStamped; - transformStamped.header.stamp = t; - // RCLCPP_INFO_STREAM(get_logger(), "Odom TS: " << transformStamped.header.stamp); + // NODELET_DEBUG_STREAM(get_logger(), "Odom TS: " << transformStamped.header.stamp); transformStamped.header.frame_id = mOdomFrameId; transformStamped.child_frame_id = mBaseFrameId; // conversion from Tranform to message - mOdomMutex.lock(); + mOdomMutex.lock(); // tf2::Vector3 translation = mOdom2BaseTransf.getOrigin(); tf2::Quaternion quat = mOdom2BaseTransf.getRotation(); - mOdomMutex.unlock(); + mOdomMutex.unlock(); // transformStamped.transform.translation.x = translation.x(); transformStamped.transform.translation.y = translation.y(); transformStamped.transform.translation.z = translation.z(); @@ -5752,14 +5741,16 @@ void ZEDWrapperNodelet::publishOdomTF(ros::Time t) void ZEDWrapperNodelet::publishPoseTF(ros::Time t) { - // DEBUG_STREAM_PT("publishPoseTF"); + // NODELET_DEBUG("publishPoseTF"); // ----> Avoid duplicated TF publishing - if (t == mLastTs_pose) + static ros::Time last_stamp; + + if (t == last_stamp) { return; } - mLastTs_pose = t; + last_stamp = t; // <---- Avoid duplicated TF publishing if (!mSensor2BaseTransfValid) @@ -5778,7 +5769,6 @@ void ZEDWrapperNodelet::publishPoseTF(ros::Time t) } geometry_msgs::TransformStamped transformStamped; - transformStamped.header.stamp = t; transformStamped.header.frame_id = mMapFrameId; transformStamped.child_frame_id = mOdomFrameId; From 50405402c6345d8012d0bdfe552a361aa1ec38e3 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Fri, 15 Sep 2023 10:58:58 +0200 Subject: [PATCH 178/199] Fix Path publishers --- .../zed_nodelet/src/zed_wrapper_nodelet.cpp | 72 +++++++++---------- 1 file changed, 34 insertions(+), 38 deletions(-) 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 8191e126..ede33eea 100644 --- a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp @@ -3251,41 +3251,35 @@ void ZEDWrapperNodelet::pubVideoDepth() void ZEDWrapperNodelet::callback_pubPath(const ros::TimerEvent& e) { uint32_t mapPathSub = mPubMapPath.getNumSubscribers(); - uint32_t odomPathSub = mPubOdomPath.getNumSubscribers(); + uint32_t odomPathSub = mPubOdomPath.getNumSubscribers(); geometry_msgs::PoseStamped odomPose; geometry_msgs::PoseStamped mapPose; odomPose.header.stamp = mFrameTimestamp; - odomPose.header.frame_id = mMapFrameId; // frame - // conversion from Tranform to message - geometry_msgs::Transform base2odom = tf2::toMsg(mOdom2BaseTransf); - // Add all value in Pose message - mOdomMutex.lock(); - odomPose.pose.position.x = base2odom.translation.x; - odomPose.pose.position.y = base2odom.translation.y; - odomPose.pose.position.z = base2odom.translation.z; - odomPose.pose.orientation.x = base2odom.rotation.x; - odomPose.pose.orientation.y = base2odom.rotation.y; - odomPose.pose.orientation.z = base2odom.rotation.z; - odomPose.pose.orientation.w = base2odom.rotation.w; - mOdomMutex.unlock(); + odomPose.header.frame_id = mMapFrameId; // map_frame + mOdomMutex.lock(); // + odomPose.pose.position.x = mOdom2BaseTransf.getOrigin().x(); + odomPose.pose.position.y = mOdom2BaseTransf.getOrigin().y(); + odomPose.pose.position.z = mOdom2BaseTransf.getOrigin().z(); + odomPose.pose.orientation.x = mOdom2BaseTransf.getRotation().x(); + odomPose.pose.orientation.y = mOdom2BaseTransf.getRotation().y(); + odomPose.pose.orientation.z = mOdom2BaseTransf.getRotation().z(); + odomPose.pose.orientation.w = mOdom2BaseTransf.getRotation().w(); + mOdomMutex.unlock(); // mapPose.header.stamp = mFrameTimestamp; - mapPose.header.frame_id = mMapFrameId; // frame - // conversion from Tranform to message - geometry_msgs::Transform base2map = tf2::toMsg(mMap2BaseTransf); - // Add all value in Pose message - mapPose.pose.position.x = base2map.translation.x; - mapPose.pose.position.y = base2map.translation.y; - mapPose.pose.position.z = base2map.translation.z; - mapPose.pose.orientation.x = base2map.rotation.x; - mapPose.pose.orientation.y = base2map.rotation.y; - mapPose.pose.orientation.z = base2map.rotation.z; - mapPose.pose.orientation.w = base2map.rotation.w; + mapPose.header.frame_id = mMapFrameId; // map_frame + mapPose.pose.position.x = mMap2BaseTransf.getOrigin().x(); + mapPose.pose.position.y = mMap2BaseTransf.getOrigin().y(); + mapPose.pose.position.z = mMap2BaseTransf.getOrigin().z(); + mapPose.pose.orientation.x = mMap2BaseTransf.getRotation().x(); + mapPose.pose.orientation.y = mMap2BaseTransf.getRotation().y(); + mapPose.pose.orientation.z = mMap2BaseTransf.getRotation().z(); + mapPose.pose.orientation.w = mMap2BaseTransf.getRotation().w(); // Circular vector - std::lock_guard lock(mOdomMutex); + std::lock_guard lock(mOdomMutex); // if (mPathMaxCount != -1) { if (mOdomPath.size() == mPathMaxCount) @@ -3299,36 +3293,38 @@ void ZEDWrapperNodelet::callback_pubPath(const ros::TimerEvent& e) } else { - // NODELET_DEBUG_STREAM("Path vectors adding last available poses"); + // NODELET_DEBUG( "Path vectors adding last available poses"); mMapPath.push_back(mapPose); mOdomPath.push_back(odomPose); } } else { - // NODELET_DEBUG_STREAM("No limit path vectors, adding last available poses"); + // NODELET_DEBUG( "No limit path vectors, adding last available poses"); mMapPath.push_back(mapPose); mOdomPath.push_back(odomPose); } if (mapPathSub > 0) { - nav_msgs::PathPtr mapPath = boost::make_shared(); - mapPath->header.frame_id = mMapFrameId; - mapPath->header.stamp = mFrameTimestamp; - mapPath->poses = mMapPath; + nav_msgs::PathPtr mapPathMsg = boost::make_shared(); + mapPathMsg->header.frame_id = mMapFrameId; + mapPathMsg->header.stamp = mFrameTimestamp; + mapPathMsg->poses = mMapPath; - mPubMapPath.publish(mapPath); + NODELET_DEBUG("Publishing MAP PATH message"); + mPubMapPath.publish(mapPathMsg); } if (odomPathSub > 0) { - nav_msgs::PathPtr odomPath = boost::make_shared(); - odomPath->header.frame_id = mMapFrameId; - odomPath->header.stamp = mFrameTimestamp; - odomPath->poses = mOdomPath; + nav_msgs::PathPtr odomPathMsg = boost::make_shared(); + odomPathMsg->header.frame_id = mOdomFrameId; + odomPathMsg->header.stamp = mFrameTimestamp; + odomPathMsg->poses = mOdomPath; - mPubOdomPath.publish(odomPath); + NODELET_DEBUG("Publishing ODOM PATH message"); + mPubOdomPath.publish(odomPathMsg); } } From ad834b5955de2d1b16703ca913e94c9451c53992 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Fri, 15 Sep 2023 11:51:58 +0200 Subject: [PATCH 179/199] Code cleaning --- .../include/zed_wrapper_nodelet.hpp | 4 ++ .../zed_nodelet/src/zed_wrapper_nodelet.cpp | 54 ++++++++++--------- 2 files changed, 32 insertions(+), 26 deletions(-) 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 5fffa5b6..de7cadb7 100644 --- a/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp +++ b/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp @@ -458,6 +458,10 @@ class ZEDWrapperNodelet : public nodelet::Nodelet */ void processCameraSettings(); + /*! \brief Process point cloud + */ + void processPointcloud(); + /*! \brief Generates an univoque color for each object class ID */ inline sl::float3 generateColorClass(int idx) 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 ede33eea..ed15db65 100644 --- a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp @@ -4060,61 +4060,42 @@ void ZEDWrapperNodelet::device_poll_thread_func() // ----> SVO recording mRecMutex.lock(); - if (mRecording) { mRecStatus = mZed.getRecordingStatus(); - if (!mRecStatus.status) { NODELET_ERROR_THROTTLE(1.0, "Error saving frame to SVO"); } - mDiagUpdater.force_update(); } mRecMutex.unlock(); // <---- SVO recording - // ----> Publish freq calculation + // ----> Grab freq calculation static std::chrono::steady_clock::time_point last_time = std::chrono::steady_clock::now(); std::chrono::steady_clock::time_point now = std::chrono::steady_clock::now(); - double elapsed_usec = std::chrono::duration_cast(now - last_time).count(); last_time = now; - mGrabPeriodMean_usec->addValue(elapsed_usec); // NODELET_INFO_STREAM("Grab time: " << elapsed_usec / 1000 << " msec"); - // <---- Publish freq calculation + // <---- Grab freq calculation // ----> Camera Settings processCameraSettings(); // <---- Camera Settings - // Publish the point cloud if someone has subscribed to - if (cloudSubnumber > 0) - { - // Run the point cloud conversion asynchronously to avoid slowing down - // all the program - // Retrieve raw pointCloud data if latest Pointcloud is ready - std::unique_lock lock(mPcMutex, std::defer_lock); - - if (lock.try_lock()) - { - mZed.retrieveMeasure(mCloud, sl::MEASURE::XYZBGRA, sl::MEM::CPU, mMatResol); + // ----> Point Cloud - mPointCloudFrameId = mDepthFrameId; - mPointCloudTime = stamp; - - // Signal Pointcloud thread that a new pointcloud is ready - mPcDataReadyCondVar.notify_one(); - mPcDataReady = true; - mPcPublishing = true; - } + if (!mDepthDisabled && cloudSubnumber > 0) + { + processPointcloud(); } else { mPcPublishing = false; } + // <---- Point Cloud // ----> Object Detection mObjDetMutex.lock(); @@ -4233,6 +4214,27 @@ void ZEDWrapperNodelet::device_poll_thread_func() NODELET_DEBUG("ZED pool thread finished"); } // namespace zed_nodelets +void ZEDWrapperNodelet::processPointcloud() +{ + // Run the point cloud conversion asynchronously to avoid slowing down + // all the program + // Retrieve raw pointCloud data if latest Pointcloud is ready + std::unique_lock lock(mPcMutex, std::defer_lock); + + if (lock.try_lock()) + { + mZed.retrieveMeasure(mCloud, sl::MEASURE::XYZBGRA, sl::MEM::CPU, mMatResol); + + mPointCloudFrameId = mDepthFrameId; + mPointCloudTime = stamp; + + // Signal Pointcloud thread that a new pointcloud is ready + mPcDataReadyCondVar.notify_one(); + mPcDataReady = true; + mPcPublishing = true; + } +} + void ZEDWrapperNodelet::processCameraSettings() { int value; From ce86801dac1e9b81e99f67f903a4c34c98bb466b Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Fri, 15 Sep 2023 14:34:54 +0200 Subject: [PATCH 180/199] Minor fixes --- CHANGELOG.rst | 3 +- README.md | 35 ++++++------ .../include/zed_wrapper_nodelet.hpp | 3 +- .../zed_nodelet/src/zed_wrapper_nodelet.cpp | 54 +++++++++---------- 4 files changed, 47 insertions(+), 48 deletions(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index fd9d3df5..68fc1b60 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -1,9 +1,10 @@ CHANGELOG ========= -2023-09-14 +2023-09-15 ---------- - Add pose and odometry status publishers +- Improve odometry and pose publishing and TF broadcasting 2023-09-13 ---------- diff --git a/README.md b/README.md index f3291d1c..16fcbc35 100644 --- a/README.md +++ b/README.md @@ -17,30 +17,27 @@ This package lets you use the ZED stereo camera with ROS. It outputs the camera ### Prerequisites - Ubuntu 20.04 -- [ZED SDK **≥ 4.0**](https://www.stereolabs.com/developers/) and its dependency [CUDA](https://developer.nvidia.com/cuda-downloads) +- [ZED SDK **≥ 4.0.6**](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 The zed_ros_wrapper is a catkin package. It depends on the following ROS packages: - - nav_msgs - - tf2_geometry_msgs - - message_runtime - - catkin - - roscpp - - stereo_msgs - - rosconsole - - robot_state_publisher - - urdf - - sensor_msgs - - image_transport - - roslint - - diagnostic_updater - - dynamic_reconfigure - - tf2_ros - - message_generation - - nodelet + - roscpp +- image_transport +- rosconsole +- sensor_msgs +- stereo_msgs +- std_msgs +- message_filters +- tf2_ros +- nodelet +- tf2_geometry_msgs +- message_generation +- diagnostic_updater +- dynamic_reconfigure +- zed_interfaces Open a terminal, clone the repository, update the dependencies and build the packages: @@ -93,7 +90,7 @@ ZED X Mini camera: $ roslaunch zed_wrapper zedxm.launch - To select the ZED from its serial number: + To select the camera from its serial number: $ roslaunch zed_wrapper zed.launch serial_number:=1010 #replace 1010 with the actual SN 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 de7cadb7..2768026b 100644 --- a/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp +++ b/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp @@ -459,8 +459,9 @@ class ZEDWrapperNodelet : public nodelet::Nodelet void processCameraSettings(); /*! \brief Process point cloud + * \param ts Frame timestamp */ - void processPointcloud(); + void processPointcloud(ros::Time ts); /*! \brief Generates an univoque color for each object class ID */ 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 ed15db65..c778a47f 100644 --- a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp @@ -2965,11 +2965,17 @@ void ZEDWrapperNodelet::pubVideoDepth() uint32_t leftGrayRawSubnumber = mPubRawLeftGray.getNumSubscribers(); uint32_t rightGraySubnumber = mPubRightGray.getNumSubscribers(); uint32_t rightGrayRawSubnumber = mPubRawRightGray.getNumSubscribers(); - uint32_t depthSubnumber = mPubDepth.getNumSubscribers(); - uint32_t disparitySubnumber = mPubDisparity.getNumSubscribers(); - uint32_t confMapSubnumber = mPubConfMap.getNumSubscribers(); uint32_t stereoSubNumber = mPubStereo.getNumSubscribers(); uint32_t stereoRawSubNumber = mPubRawStereo.getNumSubscribers(); + uint32_t depthSubnumber = 0; + uint32_t disparitySubnumber = 0; + uint32_t confMapSubnumber = 0; + if (!mDepthDisabled) + { + depthSubnumber = mPubDepth.getNumSubscribers(); + disparitySubnumber = mPubDisparity.getNumSubscribers(); + confMapSubnumber = mPubConfMap.getNumSubscribers(); + } uint32_t tot_sub = rgbSubnumber + rgbRawSubnumber + leftSubnumber + leftRawSubnumber + rightSubnumber + rightRawSubnumber + rgbGraySubnumber + rgbGrayRawSubnumber + leftGraySubnumber + @@ -3101,11 +3107,6 @@ void ZEDWrapperNodelet::pubVideoDepth() } // <---- Publish sensor data if sync is required by user or SVO - // ----> Notify grab thread that all data are synchronized and a new grab can be done - // mRgbDepthDataRetrievedCondVar.notify_one(); - // mRgbDepthDataRetrieved = true; - // <---- Notify grab thread that all data are synchronized and a new grab can be done - if (!retrieved) { mPublishingData = false; @@ -3251,7 +3252,7 @@ void ZEDWrapperNodelet::pubVideoDepth() void ZEDWrapperNodelet::callback_pubPath(const ros::TimerEvent& e) { uint32_t mapPathSub = mPubMapPath.getNumSubscribers(); - uint32_t odomPathSub = mPubOdomPath.getNumSubscribers(); + uint32_t odomPathSub = mPubOdomPath.getNumSubscribers(); geometry_msgs::PoseStamped odomPose; geometry_msgs::PoseStamped mapPose; @@ -3846,7 +3847,6 @@ void ZEDWrapperNodelet::device_poll_thread_func() fillCamDepthInfo(mZed, mDepthCamInfoMsg, mLeftCamOptFrameId); // the reference camera is the Left one (next to the ZED logo) - mRgbCamInfoMsg = mLeftCamInfoMsg; mRgbCamInfoRawMsg = mLeftCamInfoRawMsg; @@ -3855,7 +3855,7 @@ void ZEDWrapperNodelet::device_poll_thread_func() // Main loop while (mNhNs.ok()) { - // Check for subscribers + // ----> Count subscribers uint32_t rgbSubnumber = mPubRgb.getNumSubscribers(); uint32_t rgbRawSubnumber = mPubRawRgb.getNumSubscribers(); uint32_t leftSubnumber = mPubLeft.getNumSubscribers(); @@ -3897,14 +3897,15 @@ void ZEDWrapperNodelet::device_poll_thread_func() } uint32_t stereoSubNumber = mPubStereo.getNumSubscribers(); uint32_t stereoRawSubNumber = mPubRawStereo.getNumSubscribers(); + // <---- Count subscribers - mGrabActive = - mRecording || mStreaming || mMappingEnabled || mObjDetEnabled || mPosTrackingEnabled || mPosTrackingStarted || - ((rgbSubnumber + rgbRawSubnumber + leftSubnumber + leftRawSubnumber + rightSubnumber + rightRawSubnumber + - rgbGraySubnumber + rgbGrayRawSubnumber + leftGraySubnumber + leftGrayRawSubnumber + rightGraySubnumber + - rightGrayRawSubnumber + depthSubnumber + disparitySubnumber + cloudSubnumber + poseSubnumber + - poseCovSubnumber + odomSubnumber + confMapSubnumber /*+ imuSubnumber + imuRawsubnumber*/ + pathSubNumber + - stereoSubNumber + stereoRawSubNumber + objDetSubnumber) > 0); + mGrabActive = mRecording || mStreaming || mMappingEnabled || mObjDetEnabled || mPosTrackingEnabled || + mPosTrackingStarted || + ((rgbSubnumber + rgbRawSubnumber + leftSubnumber + leftRawSubnumber + rightSubnumber + + rightRawSubnumber + rgbGraySubnumber + rgbGrayRawSubnumber + leftGraySubnumber + + leftGrayRawSubnumber + rightGraySubnumber + rightGrayRawSubnumber + depthSubnumber + + disparitySubnumber + cloudSubnumber + poseSubnumber + poseCovSubnumber + odomSubnumber + + confMapSubnumber + pathSubNumber + stereoSubNumber + stereoRawSubNumber + objDetSubnumber) > 0); // Run the loop only if there is some subscribers or SVO is active if (mGrabActive) @@ -3918,7 +3919,7 @@ void ZEDWrapperNodelet::device_poll_thread_func() odomSubnumber > 0 || pathSubNumber > 0); // Start the tracking? - if ((mPosTrackingRequired) && !mPosTrackingStarted && !mDepthDisabled) + if (mPosTrackingRequired && !mPosTrackingStarted) { start_pos_tracking(); } @@ -3945,6 +3946,7 @@ void ZEDWrapperNodelet::device_poll_thread_func() ((depthSubnumber + disparitySubnumber + cloudSubnumber + fusedCloudSubnumber + poseSubnumber + poseCovSubnumber + odomSubnumber + confMapSubnumber + objDetSubnumber) > 0)); + // ----> Depth runtime parameters if (mComputeDepth) { runParams.confidence_threshold = mCamDepthConfidence; @@ -3955,6 +3957,7 @@ void ZEDWrapperNodelet::device_poll_thread_func() { runParams.enable_depth = false; // Ask to not compute the depth } + // <---- Depth runtime parameters std::chrono::steady_clock::time_point start_elab = std::chrono::steady_clock::now(); @@ -3964,9 +3967,7 @@ void ZEDWrapperNodelet::device_poll_thread_func() // cout << toString(grab_status) << endl; if (mGrabStatus != sl::ERROR_CODE::SUCCESS) { - // Detect if a error occurred (for example: - // the zed have been disconnected) and - // re-initialize the ZED + // Detect if a error occurred (for example: the zed have been disconnected) and re-initialize the ZED NODELET_INFO_STREAM_THROTTLE(1.0, "Camera grab error: " << sl::toString(mGrabStatus).c_str()); @@ -4086,10 +4087,9 @@ void ZEDWrapperNodelet::device_poll_thread_func() // <---- Camera Settings // ----> Point Cloud - if (!mDepthDisabled && cloudSubnumber > 0) { - processPointcloud(); + processPointcloud(mFrameTimestamp); } else { @@ -4184,7 +4184,7 @@ void ZEDWrapperNodelet::device_poll_thread_func() } else { - NODELET_DEBUG_THROTTLE(5.0, "No topics subscribed by users"); + NODELET_DEBUG_THROTTLE(5.0, "No processing required. Waiting for subscribers or modules activation."); std::this_thread::sleep_for(std::chrono::milliseconds(10)); // No subscribers, we just wait loop_rate.reset(); @@ -4214,7 +4214,7 @@ void ZEDWrapperNodelet::device_poll_thread_func() NODELET_DEBUG("ZED pool thread finished"); } // namespace zed_nodelets -void ZEDWrapperNodelet::processPointcloud() +void ZEDWrapperNodelet::processPointcloud(ros::Time ts) { // Run the point cloud conversion asynchronously to avoid slowing down // all the program @@ -4226,7 +4226,7 @@ void ZEDWrapperNodelet::processPointcloud() mZed.retrieveMeasure(mCloud, sl::MEASURE::XYZBGRA, sl::MEM::CPU, mMatResol); mPointCloudFrameId = mDepthFrameId; - mPointCloudTime = stamp; + mPointCloudTime = ts; // Signal Pointcloud thread that a new pointcloud is ready mPcDataReadyCondVar.notify_one(); From aebf2c3593e9b32b2c0bca78293de622ff5f23af Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Wed, 20 Sep 2023 14:39:13 +0200 Subject: [PATCH 181/199] Fix `set_as_static` behavior --- .../zed_nodelet/src/zed_wrapper_nodelet.cpp | 50 +++++++++++++++---- 1 file changed, 40 insertions(+), 10 deletions(-) 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 c778a47f..90da3015 100644 --- a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp @@ -53,6 +53,8 @@ ZEDWrapperNodelet::ZEDWrapperNodelet() : Nodelet() ZEDWrapperNodelet::~ZEDWrapperNodelet() { + //std::cerr << "Destroying " << getName() << std::endl; + if (mDevicePollThread.joinable()) { mDevicePollThread.join(); @@ -68,7 +70,14 @@ ZEDWrapperNodelet::~ZEDWrapperNodelet() mSensThread.join(); } - std::cerr << "ZED Nodelet destroyed" << std::endl; + if (mZed.isOpened()) + { + mZed.close(); + } + + std::cerr << "ZED " << mZedSerialNumber << " closed." << std::endl; + + //std::cerr << "ZED Nodelet '" << getName().c_str() << "' correctly stopped" << std::endl; } void ZEDWrapperNodelet::onInit() @@ -189,6 +198,7 @@ void ZEDWrapperNodelet::onInit() // Try to initialize the ZED std::stringstream ss; // Input mode info for logging + std::string cam_id; if (!mSvoFilepath.empty() || !mRemoteStreamAddr.empty()) { if (!mSvoFilepath.empty()) @@ -198,6 +208,7 @@ void ZEDWrapperNodelet::onInit() // Input mode info for logging ss << "SVO - " << mSvoFilepath.c_str(); + cam_id = ss.str(); } else if (!mRemoteStreamAddr.empty()) { @@ -215,6 +226,7 @@ void ZEDWrapperNodelet::onInit() // Input mode info for logging ss << "NETWORK STREAM - " << mRemoteStreamAddr.c_str(); + cam_id = ss.str(); } mSvoMode = true; @@ -231,6 +243,7 @@ void ZEDWrapperNodelet::onInit() // Input mode info for logging ss << "LIVE CAMERA with ID " << mZedId; + cam_id = ss.str(); } else { @@ -238,6 +251,7 @@ void ZEDWrapperNodelet::onInit() // Input mode info for logging ss << "LIVE CAMERA with SN " << mZedSerialNumber; + cam_id = ss.str(); } } @@ -251,6 +265,7 @@ void ZEDWrapperNodelet::onInit() mZedParams.depth_minimum_distance = static_cast(mCamMinDepth); mZedParams.depth_maximum_distance = static_cast(mCamMaxDepth); mZedParams.camera_disable_self_calib = !mCameraSelfCalib; + mZedParams.open_timeout_sec = 10.0f; mZedParams.enable_image_enhancement = true; // Always active @@ -263,16 +278,25 @@ void ZEDWrapperNodelet::onInit() while (mConnStatus != sl::ERROR_CODE::SUCCESS) { mConnStatus = mZed.open(mZedParams); - NODELET_INFO_STREAM("ZED connection: " << sl::toString(mConnStatus)); + NODELET_INFO_STREAM("ZED connection [" << cam_id.c_str() << "]: " << sl::toString(mConnStatus)); std::this_thread::sleep_for(std::chrono::milliseconds(2000)); if (!mNhNs.ok()) { - mStopNode = true; // Stops other threads + mStopNode = true; std::lock_guard lock(mCloseZedMutex); - NODELET_DEBUG("Closing ZED"); - mZed.close(); + NODELET_INFO_STREAM("Closing ZED " << mZedSerialNumber << "..."); + if (mRecording) + { + mRecording = false; + mZed.disableRecording(); + } + if (mZed.isOpened()) + { + mZed.close(); + } + NODELET_INFO_STREAM("... ZED " << mZedSerialNumber << " closed."); NODELET_DEBUG("ZED pool thread finished"); return; @@ -2109,7 +2133,6 @@ void ZEDWrapperNodelet::start_pos_tracking() mPoseSmoothing = false; // Always false. Pose Smoothing is to be enabled only for VR/AR applications posTrackParams.enable_pose_smoothing = mPoseSmoothing; posTrackParams.set_floor_as_origin = mFloorAlignment; - posTrackParams.set_as_static = mIsStatic; posTrackParams.depth_min_range = static_cast(mPosTrkMinDepth); if (mAreaMemDbPath != "" && !sl_tools::file_exist(mAreaMemDbPath)) @@ -2130,7 +2153,7 @@ void ZEDWrapperNodelet::start_pos_tracking() posTrackParams.enable_imu_fusion = mImuFusion; - posTrackParams.set_as_static = false; + posTrackParams.set_as_static = mIsStatic; posTrackParams.set_gravity_as_origin = mSetGravityAsOrigin; posTrackParams.mode = mPosTrkMode; @@ -3994,7 +4017,10 @@ void ZEDWrapperNodelet::device_poll_thread_func() if ((ros::Time::now() - mPrevFrameTimestamp).toSec() > 5 && !mSvoMode) { mCloseZedMutex.lock(); - mZed.close(); + if (mZed.isOpened()) + { + mZed.close(); + } mCloseZedMutex.unlock(); mConnStatus = sl::ERROR_CODE::CAMERA_NOT_DETECTED; @@ -4006,13 +4032,17 @@ void ZEDWrapperNodelet::device_poll_thread_func() mStopNode = true; std::lock_guard lock(mCloseZedMutex); - NODELET_DEBUG("Closing ZED"); + NODELET_INFO_STREAM("Closing ZED " << mZedSerialNumber << "..."); if (mRecording) { mRecording = false; mZed.disableRecording(); } - mZed.close(); + if (mZed.isOpened()) + { + mZed.close(); + } + NODELET_INFO_STREAM("... ZED " << mZedSerialNumber << " closed."); NODELET_DEBUG("ZED pool thread finished"); return; From acb2750ce52e969726b4ae1f1c659155e00a866c Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Fri, 22 Sep 2023 12:35:43 +0200 Subject: [PATCH 182/199] [WIP] Update OD module --- .../zed_nodelet/src/zed_wrapper_nodelet.cpp | 33 ++++++++++++++----- zed_wrapper/params/common.yaml | 3 +- zed_wrapper/params/zed.yaml | 2 +- zed_wrapper/params/zed2.yaml | 2 +- zed_wrapper/params/zed2i.yaml | 2 +- zed_wrapper/params/zedm.yaml | 2 +- zed_wrapper/params/zedx.yaml | 2 +- zed_wrapper/params/zedxm.yaml | 2 +- 8 files changed, 32 insertions(+), 16 deletions(-) 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 90da3015..bf8b21d6 100644 --- a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp @@ -1086,16 +1086,33 @@ void ZEDWrapperNodelet::readObjDetParams() } NODELET_INFO_STREAM(" * Detection max range\t\t-> " << mObjDetMaxRange); - int model; - mNhNs.getParam("object_detection/model", model); - if (model < 0 || model >= static_cast(sl::OBJECT_DETECTION_MODEL::LAST)) + std::string model_str; + mNhNs.getParam("object_detection/model", model_str); + + NODELET_DEBUG_STREAM(" 'object_detection.model': " << model_str.c_str()); + + bool matched = false; + for (int idx = static_cast(sl::OBJECT_DETECTION_MODEL::MULTI_CLASS_BOX_FAST); + idx < static_cast(sl::OBJECT_DETECTION_MODEL::CUSTOM_BOX_OBJECTS); idx++) + { + sl::OBJECT_DETECTION_MODEL test_model = static_cast(idx); + std::string test_model_str = sl::toString(test_model).c_str(); + std::replace(test_model_str.begin(), test_model_str.end(), ' ', + '_'); // Replace spaces with underscores to match the YAML setting + // NODELETDEBUG(" Comparing '%s' to '%s'", test_model_str.c_str(), model_str.c_str()); + if (model_str == test_model_str) + { + mObjDetModel = test_model; + matched = true; + break; + } + } + if (!matched) { - NODELET_WARN("Detection model not valid. Forced to the default value"); - model = static_cast(mObjDetModel); + NODELET_WARN_STREAM( "The value of the parameter 'object_detection.model' is not valid: '" + << model_str << "'. Using the default value."); } - mObjDetModel = static_cast(model); - - NODELET_INFO_STREAM(" * Detection model\t\t-> " << sl::toString(mObjDetModel)); + NODELET_INFO_STREAM(" * Object Det. model:\t" << sl::toString(mObjDetModel).c_str()); mNhNs.getParam("object_detection/mc_people", mObjDetPeopleEnable); NODELET_INFO_STREAM(" * Detect people\t\t-> " << (mObjDetPeopleEnable ? "ENABLED" : "DISABLED")); diff --git a/zed_wrapper/params/common.yaml b/zed_wrapper/params/common.yaml index 0982426e..9ad141a9 100644 --- a/zed_wrapper/params/common.yaml +++ b/zed_wrapper/params/common.yaml @@ -80,8 +80,7 @@ sensors: object_detection: od_enabled: false # True to enable Object Detection [not available for ZED] - model: 0 # '0': MULTI_CLASS_BOX - '1': MULTI_CLASS_BOX_ACCURATE - confidence_threshold: 50 # Minimum value of the detection confidence of an object [0,100] + model: 'MULTI_CLASS_BOX_ACCURATE' # 'MULTI_CLASS_BOX_FAST', 'MULTI_CLASS_BOX_MEDIUM', 'MULTI_CLASS_BOX_ACCURATE', 'PERSON_HEAD_BOX_FAST', 'PERSON_HEAD_BOX_ACCURATE' max_range: 15. # Maximum detection range object_tracking_enabled: true # Enable/disable the tracking of the detected objects mc_people: true # Enable/disable the detection of persons for 'MULTI_CLASS_BOX_X' models diff --git a/zed_wrapper/params/zed.yaml b/zed_wrapper/params/zed.yaml index 0dada5cd..15212700 100644 --- a/zed_wrapper/params/zed.yaml +++ b/zed_wrapper/params/zed.yaml @@ -9,4 +9,4 @@ general: depth: min_depth: 0.3 # Min: 0.3, Max: 3.0 - Default 0.7 - Note: reducing this value wil require more computational power and GPU memory - max_depth: 10.0 # Max: 40.0 + max_depth: 15.0 # Max: 40.0 diff --git a/zed_wrapper/params/zed2.yaml b/zed_wrapper/params/zed2.yaml index 6f3cfeda..e6f18f53 100644 --- a/zed_wrapper/params/zed2.yaml +++ b/zed_wrapper/params/zed2.yaml @@ -9,5 +9,5 @@ general: depth: min_depth: 0.3 # Min: 0.2, Max: 3.0 - Default 0.7 - Note: reducing this value wil require more computational power and GPU memory - max_depth: 20.0 # Max: 40.0 + max_depth: 15.0 # Max: 40.0 \ No newline at end of file diff --git a/zed_wrapper/params/zed2i.yaml b/zed_wrapper/params/zed2i.yaml index 08510fa4..2742f370 100644 --- a/zed_wrapper/params/zed2i.yaml +++ b/zed_wrapper/params/zed2i.yaml @@ -9,4 +9,4 @@ general: depth: min_depth: 0.3 # Min: 0.3, Max: 3.0 - Default 0.7 - Note: reducing this value wil require more computational power and GPU memory - max_depth: 10.0 # Max: 40.0 + max_depth: 15.0 # Max: 40.0 diff --git a/zed_wrapper/params/zedm.yaml b/zed_wrapper/params/zedm.yaml index ca3f66c2..f328af8e 100644 --- a/zed_wrapper/params/zedm.yaml +++ b/zed_wrapper/params/zedm.yaml @@ -9,4 +9,4 @@ general: depth: min_depth: 0.1 # Min: 0.1, Max: 3.0 - Default 0.3 - Note: reducing this value wil require more computational power and GPU memory - max_depth: 10.0 # Max: 20.0 + max_depth: 15.0 # Max: 20.0 diff --git a/zed_wrapper/params/zedx.yaml b/zed_wrapper/params/zedx.yaml index 7d364165..9778857c 100644 --- a/zed_wrapper/params/zedx.yaml +++ b/zed_wrapper/params/zedx.yaml @@ -9,5 +9,5 @@ general: depth: min_depth: 0.2 # Min: 0.2, Max: 3.0 - Default 0.7 - Note: reducing this value wil require more computational power and GPU memory - max_depth: 10.0 # Max: 40.0 + max_depth: 15.0 # Max: 40.0 diff --git a/zed_wrapper/params/zedxm.yaml b/zed_wrapper/params/zedxm.yaml index dd931d84..f3e91fac 100644 --- a/zed_wrapper/params/zedxm.yaml +++ b/zed_wrapper/params/zedxm.yaml @@ -9,4 +9,4 @@ general: depth: min_depth: 0.1 # Min: 0.1, Max: 3.0 - Default 0.7 - Note: reducing this value wil require more computational power and GPU memory - max_depth: 10.0 # Max: 40.0 + max_depth: 15.0 # Max: 40.0 From 25ab3e9a5cae71ad62f4c34f264b32f9beacea1c Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Fri, 22 Sep 2023 14:54:52 +0200 Subject: [PATCH 183/199] Rework and fix OD module --- CHANGELOG.rst | 8 + README.md | 3 +- zed_nodelets/CMakeLists.txt | 1 + .../include/zed_wrapper_nodelet.hpp | 21 +- .../zed_nodelet/src/zed_wrapper_nodelet.cpp | 350 +++++++++--------- zed_wrapper/params/common.yaml | 25 +- 6 files changed, 202 insertions(+), 206 deletions(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 68fc1b60..d7fd4390 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -1,6 +1,14 @@ CHANGELOG ========= +2023-09-15 +---------- +- Remove start_object_detection and stop_object_detection services +- Add enable_object_detection service that takes a bool as parameter to enable/disable the OD module (same behavior as in ROS 2 Wrapper) +- Add parameter 'object_detection/allow_reduced_precision_inference' +- Add parameter 'object_detection/prediction_timeout' + + 2023-09-15 ---------- - Add pose and odometry status publishers diff --git a/README.md b/README.md index 16fcbc35..c89700f6 100644 --- a/README.md +++ b/README.md @@ -30,6 +30,7 @@ The zed_ros_wrapper is a catkin package. It depends on the following ROS package - sensor_msgs - stereo_msgs - std_msgs +- std_srvs - message_filters - tf2_ros - nodelet @@ -106,7 +107,7 @@ The SDK v3.0 introduces the Object Detection and Tracking module. **The Object D The Object Detection can be enabled *automatically* when the node start setting the parameter `object_detection/od_enabled` to `true` in the file `common.yaml`. -The Object Detection can be enabled/disabled *manually* calling the services `start_object_detection` and `stop_object_detection`. +The Object Detection can be enabled/disabled *manually* calling the service `enable_object_detection`. ### Body Tracking The Body Tracking module is not available for the ZED ROS Wrapper. Please consider migrating to the [ZED ROS2 Wrapper](https://github.com/stereolabs/zed-ros2-wrapper) if you need it. diff --git a/zed_nodelets/CMakeLists.txt b/zed_nodelets/CMakeLists.txt index d779d65b..811a7aaf 100644 --- a/zed_nodelets/CMakeLists.txt +++ b/zed_nodelets/CMakeLists.txt @@ -48,6 +48,7 @@ find_package(catkin REQUIRED COMPONENTS sensor_msgs stereo_msgs std_msgs + std_srvs message_filters tf2_ros nodelet 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 2768026b..8d1cb5af 100644 --- a/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp +++ b/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp @@ -34,6 +34,7 @@ #include #include #include +#include #include @@ -50,11 +51,9 @@ #include #include #include -#include #include #include #include -#include #include #include #include @@ -395,15 +394,9 @@ class ZEDWrapperNodelet : public nodelet::Nodelet */ bool on_save_3d_map(zed_interfaces::save_3d_map::Request& req, zed_interfaces::save_3d_map::Response& res); - /*! \brief Service callback to start_object_detection service + /*! \brief Service callback to enable_object_detection service */ - bool on_start_object_detection(zed_interfaces::start_object_detection::Request& req, - zed_interfaces::start_object_detection::Response& res); - - /*! \brief Service callback to stop_object_detection service - */ - bool on_stop_object_detection(zed_interfaces::stop_object_detection::Request& req, - zed_interfaces::stop_object_detection::Response& res); + bool on_enable_object_detection(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res); /*! \brief Service callback to save_area_memory service */ @@ -563,8 +556,7 @@ class ZEDWrapperNodelet : public nodelet::Nodelet ros::ServiceServer mSrvStartMapping; ros::ServiceServer mSrvStopMapping; ros::ServiceServer mSrvSave3dMap; - ros::ServiceServer mSrvStartObjDet; - ros::ServiceServer mSrvStopObjDet; + ros::ServiceServer mSrvEnableObjDet; ros::ServiceServer mSrvSaveAreaMemory; ros::ServiceServer mSrvSetRoi; ros::ServiceServer mSrvResetRoi; @@ -807,9 +799,11 @@ class ZEDWrapperNodelet : public nodelet::Nodelet bool mObjDetEnabled = false; bool mObjDetRunning = false; bool mObjDetTracking = true; + bool mObjDetReducedPrecision = false; bool mObjDetBodyFitting = true; float mObjDetConfidence = 50.f; float mObjDetMaxRange = 10.f; + double mObjDetPredTimeout = 0.5; std::vector mObjDetFilter; bool mObjDetPeopleEnable = true; bool mObjDetVehiclesEnable = true; @@ -817,9 +811,10 @@ class ZEDWrapperNodelet : public nodelet::Nodelet bool mObjDetAnimalsEnable = true; bool mObjDetElectronicsEnable = true; bool mObjDetFruitsEnable = true; - bool mObjDetSportsEnable = true; + bool mObjDetSportEnable = true; sl::OBJECT_DETECTION_MODEL mObjDetModel = sl::OBJECT_DETECTION_MODEL::MULTI_CLASS_BOX_MEDIUM; + sl::OBJECT_FILTERING_MODE mObjFilterMode = sl::OBJECT_FILTERING_MODE::NMS3D; ros::Publisher mPubObjDet; }; // class ZEDROSWrapperNodelet 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 bf8b21d6..6f8387d6 100644 --- a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp @@ -53,7 +53,7 @@ ZEDWrapperNodelet::ZEDWrapperNodelet() : Nodelet() ZEDWrapperNodelet::~ZEDWrapperNodelet() { - //std::cerr << "Destroying " << getName() << std::endl; + // std::cerr << "Destroying " << getName() << std::endl; if (mDevicePollThread.joinable()) { @@ -77,7 +77,7 @@ ZEDWrapperNodelet::~ZEDWrapperNodelet() std::cerr << "ZED " << mZedSerialNumber << " closed." << std::endl; - //std::cerr << "ZED Nodelet '" << getName().c_str() << "' correctly stopped" << std::endl; + // std::cerr << "ZED Nodelet '" << getName().c_str() << "' correctly stopped" << std::endl; } void ZEDWrapperNodelet::onInit() @@ -688,12 +688,9 @@ void ZEDWrapperNodelet::initServices() mSrvSave3dMap = mNhNs.advertiseService("save_3d_map", &ZEDWrapperNodelet::on_save_3d_map, this); NODELET_INFO_STREAM(" * Advertised on service " << mSrvSave3dMap.getService().c_str()); - mSrvStartObjDet = - mNhNs.advertiseService("start_object_detection", &ZEDWrapperNodelet::on_start_object_detection, this); - NODELET_INFO_STREAM(" * Advertised on service " << mSrvStartObjDet.getService().c_str()); - mSrvStopObjDet = - mNhNs.advertiseService("stop_object_detection", &ZEDWrapperNodelet::on_stop_object_detection, this); - NODELET_INFO_STREAM(" * Advertised on service " << mSrvStopObjDet.getService().c_str()); + mSrvEnableObjDet = + mNhNs.advertiseService("enable_object_detection", &ZEDWrapperNodelet::on_enable_object_detection, this); + NODELET_INFO_STREAM(" * Advertised on service " << mSrvEnableObjDet.getService().c_str()); } mSrvSvoStartRecording = @@ -1070,65 +1067,66 @@ void ZEDWrapperNodelet::readObjDetParams() NODELET_INFO_STREAM("*** OBJECT DETECTION PARAMETERS ***"); mNhNs.getParam("object_detection/od_enabled", mObjDetEnabled); - - if (mObjDetEnabled) - { - NODELET_INFO_STREAM(" * Object Detection\t\t-> ENABLED"); - mNhNs.getParam("object_detection/confidence_threshold", mObjDetConfidence); - NODELET_INFO_STREAM(" * Object confidence\t\t-> " << mObjDetConfidence); - mNhNs.getParam("object_detection/object_tracking_enabled", mObjDetTracking); - NODELET_INFO_STREAM(" * Object tracking\t\t-> " << (mObjDetTracking ? "ENABLED" : "DISABLED")); - mNhNs.getParam("object_detection/max_range", mObjDetMaxRange); - if (mObjDetMaxRange > mCamMaxDepth) - { - NODELET_WARN("Detection max range cannot be major than depth max range. Automatically fixed."); - mObjDetMaxRange = mCamMaxDepth; - } - NODELET_INFO_STREAM(" * Detection max range\t\t-> " << mObjDetMaxRange); - - std::string model_str; - mNhNs.getParam("object_detection/model", model_str); - - NODELET_DEBUG_STREAM(" 'object_detection.model': " << model_str.c_str()); - - bool matched = false; - for (int idx = static_cast(sl::OBJECT_DETECTION_MODEL::MULTI_CLASS_BOX_FAST); - idx < static_cast(sl::OBJECT_DETECTION_MODEL::CUSTOM_BOX_OBJECTS); idx++) + NODELET_INFO_STREAM(" * Object Detection\t\t-> " << (mObjDetEnabled ? "ENABLED" : "DISABLED")); + mNhNs.getParam("object_detection/confidence_threshold", mObjDetConfidence); + NODELET_INFO_STREAM(" * Object confidence\t\t-> " << mObjDetConfidence); + mNhNs.getParam("object_detection/object_tracking_enabled", mObjDetTracking); + NODELET_INFO_STREAM(" * Object tracking\t\t-> " << (mObjDetTracking ? "ENABLED" : "DISABLED")); + mNhNs.getParam("object_detection/max_range", mObjDetMaxRange); + if (mObjDetMaxRange > mCamMaxDepth) + { + NODELET_WARN("Detection max range cannot be major than depth max range. Automatically fixed."); + mObjDetMaxRange = mCamMaxDepth; + } + NODELET_INFO_STREAM(" * Detection max range\t\t-> " << mObjDetMaxRange); + + mNhNs.getParam("object_detection/allow_reduced_precision_inference", mObjDetReducedPrecision); + NODELET_INFO_STREAM(" * Allow reduced precision\t-> " << (mObjDetReducedPrecision ? "ENABLED" : "DISABLED")); + mNhNs.getParam("object_detection/prediction_timeout", mObjDetPredTimeout); + NODELET_INFO_STREAM(" * Prediction Timeout\t\t-> " << mObjDetPredTimeout); + + std::string model_str; + mNhNs.getParam("object_detection/model", model_str); + + NODELET_DEBUG_STREAM(" 'object_detection.model': " << model_str.c_str()); + + bool matched = false; + for (int idx = static_cast(sl::OBJECT_DETECTION_MODEL::MULTI_CLASS_BOX_FAST); + idx < static_cast(sl::OBJECT_DETECTION_MODEL::CUSTOM_BOX_OBJECTS); idx++) + { + sl::OBJECT_DETECTION_MODEL test_model = static_cast(idx); + std::string test_model_str = sl::toString(test_model).c_str(); + std::replace(test_model_str.begin(), test_model_str.end(), ' ', + '_'); // Replace spaces with underscores to match the YAML setting + // NODELETDEBUG(" Comparing '%s' to '%s'", test_model_str.c_str(), model_str.c_str()); + if (model_str == test_model_str) { - sl::OBJECT_DETECTION_MODEL test_model = static_cast(idx); - std::string test_model_str = sl::toString(test_model).c_str(); - std::replace(test_model_str.begin(), test_model_str.end(), ' ', - '_'); // Replace spaces with underscores to match the YAML setting - // NODELETDEBUG(" Comparing '%s' to '%s'", test_model_str.c_str(), model_str.c_str()); - if (model_str == test_model_str) - { - mObjDetModel = test_model; - matched = true; - break; - } - } - if (!matched) - { - NODELET_WARN_STREAM( "The value of the parameter 'object_detection.model' is not valid: '" - << model_str << "'. Using the default value."); + mObjDetModel = test_model; + matched = true; + break; } - NODELET_INFO_STREAM(" * Object Det. model:\t" << sl::toString(mObjDetModel).c_str()); - - mNhNs.getParam("object_detection/mc_people", mObjDetPeopleEnable); - NODELET_INFO_STREAM(" * Detect people\t\t-> " << (mObjDetPeopleEnable ? "ENABLED" : "DISABLED")); - mNhNs.getParam("object_detection/mc_vehicle", mObjDetVehiclesEnable); - NODELET_INFO_STREAM(" * Detect vehicles\t\t-> " << (mObjDetVehiclesEnable ? "ENABLED" : "DISABLED")); - mNhNs.getParam("object_detection/mc_bag", mObjDetBagsEnable); - NODELET_INFO_STREAM(" * Detect bags\t\t\t-> " << (mObjDetBagsEnable ? "ENABLED" : "DISABLED")); - mNhNs.getParam("object_detection/mc_animal", mObjDetAnimalsEnable); - NODELET_INFO_STREAM(" * Detect animals\t\t-> " << (mObjDetAnimalsEnable ? "ENABLED" : "DISABLED")); - mNhNs.getParam("object_detection/mc_electronics", mObjDetElectronicsEnable); - NODELET_INFO_STREAM(" * Detect electronics\t\t-> " << (mObjDetElectronicsEnable ? "ENABLED" : "DISABLED")); - mNhNs.getParam("object_detection/mc_fruit_vegetable", mObjDetFruitsEnable); - NODELET_INFO_STREAM(" * Detect fruit and vegetables\t-> " << (mObjDetFruitsEnable ? "ENABLED" : "DISABLED")); - mNhNs.getParam("object_detection/mc_sport", mObjDetSportsEnable); - NODELET_INFO_STREAM(" * Detect sport-related objects\t-> " << (mObjDetSportsEnable ? "ENABLED" : "DISABLED")); } + if (!matched) + { + NODELET_WARN_STREAM("The value of the parameter 'object_detection.model' is not valid: '" + << model_str << "'. Using the default value."); + } + NODELET_INFO_STREAM(" * Object Det. model\t\t->" << sl::toString(mObjDetModel).c_str()); + + mNhNs.getParam("object_detection/mc_people", mObjDetPeopleEnable); + NODELET_INFO_STREAM(" * Detect people\t\t-> " << (mObjDetPeopleEnable ? "ENABLED" : "DISABLED")); + mNhNs.getParam("object_detection/mc_vehicle", mObjDetVehiclesEnable); + NODELET_INFO_STREAM(" * Detect vehicles\t\t-> " << (mObjDetVehiclesEnable ? "ENABLED" : "DISABLED")); + mNhNs.getParam("object_detection/mc_bag", mObjDetBagsEnable); + NODELET_INFO_STREAM(" * Detect bags\t\t\t-> " << (mObjDetBagsEnable ? "ENABLED" : "DISABLED")); + mNhNs.getParam("object_detection/mc_animal", mObjDetAnimalsEnable); + NODELET_INFO_STREAM(" * Detect animals\t\t-> " << (mObjDetAnimalsEnable ? "ENABLED" : "DISABLED")); + mNhNs.getParam("object_detection/mc_electronics", mObjDetElectronicsEnable); + NODELET_INFO_STREAM(" * Detect electronics\t\t-> " << (mObjDetElectronicsEnable ? "ENABLED" : "DISABLED")); + mNhNs.getParam("object_detection/mc_fruit_vegetable", mObjDetFruitsEnable); + NODELET_INFO_STREAM(" * Detect fruit and vegetables\t-> " << (mObjDetFruitsEnable ? "ENABLED" : "DISABLED")); + mNhNs.getParam("object_detection/mc_sport", mObjDetSportEnable); + NODELET_INFO_STREAM(" * Detect sport-related objects\t-> " << (mObjDetSportEnable ? "ENABLED" : "DISABLED")); } } @@ -2001,6 +1999,12 @@ bool ZEDWrapperNodelet::start_obj_detect() return false; } + if (mDepthDisabled) + { + NODELET_WARN("Cannot start Object Detection if `depth/depth_mode` is set to `NONE`"); + return false; + } + if (!mObjDetEnabled) { return false; @@ -2015,13 +2019,44 @@ bool ZEDWrapperNodelet::start_obj_detect() NODELET_INFO_STREAM("*** Starting Object Detection ***"); sl::ObjectDetectionParameters od_p; - od_p.allow_reduced_precision_inference = false; od_p.enable_segmentation = false; od_p.enable_tracking = mObjDetTracking; - od_p.image_sync = false; // Asynchronous object detection + od_p.image_sync = true; od_p.detection_model = mObjDetModel; + od_p.filtering_mode = mObjFilterMode; + od_p.prediction_timeout_s = mObjDetPredTimeout; + od_p.allow_reduced_precision_inference = mObjDetReducedPrecision; od_p.max_range = mObjDetMaxRange; - od_p.instance_module_id = 0; + + mObjDetFilter.clear(); + if (mObjDetPeopleEnable) + { + mObjDetFilter.push_back(sl::OBJECT_CLASS::PERSON); + } + if (mObjDetVehiclesEnable) + { + mObjDetFilter.push_back(sl::OBJECT_CLASS::VEHICLE); + } + if (mObjDetBagsEnable) + { + mObjDetFilter.push_back(sl::OBJECT_CLASS::BAG); + } + if (mObjDetAnimalsEnable) + { + mObjDetFilter.push_back(sl::OBJECT_CLASS::ANIMAL); + } + if (mObjDetElectronicsEnable) + { + mObjDetFilter.push_back(sl::OBJECT_CLASS::ELECTRONICS); + } + if (mObjDetFruitsEnable) + { + mObjDetFilter.push_back(sl::OBJECT_CLASS::FRUIT_VEGETABLE); + } + if (mObjDetSportEnable) + { + mObjDetFilter.push_back(sl::OBJECT_CLASS::SPORT); + } sl::ERROR_CODE objDetError = mZed.enableObjectDetection(od_p); @@ -2042,42 +2077,6 @@ bool ZEDWrapperNodelet::start_obj_detect() NODELET_INFO_STREAM(" * Advertised on topic " << mPubObjDet.getTopic()); } - mObjDetFilter.clear(); - - if (mObjDetModel == sl::OBJECT_DETECTION_MODEL::MULTI_CLASS_BOX_ACCURATE || - mObjDetModel == sl::OBJECT_DETECTION_MODEL::MULTI_CLASS_BOX_MEDIUM || - mObjDetModel == sl::OBJECT_DETECTION_MODEL::MULTI_CLASS_BOX_FAST) - { - if (mObjDetPeopleEnable) - { - mObjDetFilter.push_back(sl::OBJECT_CLASS::PERSON); - } - if (mObjDetVehiclesEnable) - { - mObjDetFilter.push_back(sl::OBJECT_CLASS::VEHICLE); - } - if (mObjDetBagsEnable) - { - mObjDetFilter.push_back(sl::OBJECT_CLASS::BAG); - } - if (mObjDetAnimalsEnable) - { - mObjDetFilter.push_back(sl::OBJECT_CLASS::ANIMAL); - } - if (mObjDetElectronicsEnable) - { - mObjDetFilter.push_back(sl::OBJECT_CLASS::ELECTRONICS); - } - if (mObjDetFruitsEnable) - { - mObjDetFilter.push_back(sl::OBJECT_CLASS::FRUIT_VEGETABLE); - } - if (mObjDetSportsEnable) - { - mObjDetFilter.push_back(sl::OBJECT_CLASS::SPORT); - } - } - mObjDetRunning = true; return false; } @@ -2086,10 +2085,24 @@ void ZEDWrapperNodelet::stop_obj_detect() { if (mObjDetRunning) { - NODELET_INFO_STREAM("*** Stopping Object Detection ***"); + NODELET_INFO("*** Stopping Object Detection ***"); mObjDetRunning = false; mObjDetEnabled = false; mZed.disableObjectDetection(); + + // ----> Send an empty message to indicate that no more objects are tracked + // (e.g clean Rviz2) + zed_interfaces::ObjectsStampedPtr objMsg = boost::make_shared(); + + objMsg->header.stamp = mFrameTimestamp; + objMsg->header.frame_id = mLeftCamFrameId; + + objMsg->objects.clear(); + + NODELET_DEBUG_STREAM("Publishing EMPTY OBJ message " << mPubObjDet.getTopic().c_str()); + mPubObjDet.publish(objMsg); + // <---- Send an empty message to indicate that no more objects are tracked + // (e.g clean Rviz2) } } @@ -3973,12 +3986,15 @@ void ZEDWrapperNodelet::device_poll_thread_func() mMappingMutex.unlock(); // Start the object detection? - mObjDetMutex.lock(); - if (mObjDetEnabled && !mObjDetRunning) + if (!mDepthDisabled) { - start_obj_detect(); + mObjDetMutex.lock(); + if (mObjDetEnabled && !mObjDetRunning) + { + start_obj_detect(); + } + mObjDetMutex.unlock(); } - mObjDetMutex.unlock(); // Detect if one of the subscriber need to have the depth information mComputeDepth = !mDepthDisabled && @@ -4656,7 +4672,7 @@ bool ZEDWrapperNodelet::on_start_svo_recording(zed_interfaces::start_svo_recordi if (mRecording) { res.result = false; - res.info = "Recording was just active"; + res.info = "Recording was already active"; return false; } @@ -4971,7 +4987,7 @@ bool ZEDWrapperNodelet::on_start_3d_mapping(zed_interfaces::start_3d_mapping::Re { if (mMappingEnabled && mMappingRunning) { - NODELET_WARN_STREAM("Spatial mapping was just running"); + NODELET_WARN_STREAM("Spatial mapping was already running"); res.done = false; return res.done; @@ -5053,10 +5069,11 @@ bool ZEDWrapperNodelet::on_save_3d_map(zed_interfaces::save_3d_map::Request& req return true; } -bool ZEDWrapperNodelet::on_start_object_detection(zed_interfaces::start_object_detection::Request& req, - zed_interfaces::start_object_detection::Response& res) +bool ZEDWrapperNodelet::on_enable_object_detection(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res) { - NODELET_INFO("Called 'start_object_detection' service"); + NODELET_INFO("Called 'enable_object_detection' service"); + + std::lock_guard lock(mObjDetMutex); if (mZedRealCamModel == sl::MODEL::ZED) { @@ -5064,83 +5081,56 @@ bool ZEDWrapperNodelet::on_start_object_detection(zed_interfaces::start_object_d mObjDetRunning = false; NODELET_ERROR_STREAM("Object detection not started. OD is not available for ZED camera model"); - res.done = false; - return res.done; + res.message = "OObject detection not started. OD is not available for ZED camera model"; + res.success = false; + return res.success; } - if (mObjDetEnabled && mObjDetRunning) + if (req.data) { - NODELET_WARN_STREAM("Object Detection was just running"); - - res.done = false; - return res.done; - } + NODELET_INFO("Starting Object Detection"); + // Start + if (mObjDetEnabled && mObjDetRunning) + { + NODELET_WARN("Object Detection is already running"); + res.message = "Object Detection is already running"; + res.success = false; + return res.success; + } - mObjDetRunning = false; + mObjDetEnabled = true; - mObjDetConfidence = req.confidence; - mObjDetTracking = req.tracking; - if (req.model < 0 || req.model >= static_cast(sl::OBJECT_DETECTION_MODEL::LAST)) - { - NODELET_ERROR_STREAM("Object Detection model not valid."); - res.done = false; - return res.done; + if (start_obj_detect()) + { + res.message = "Object Detection started"; + res.success = true; + return res.success; + } + else + { + res.message = "Error occurred starting Object Detection. See log for more info"; + res.success = false; + return res.success; + } } - mObjDetModel = static_cast(req.model); - - mObjDetMaxRange = req.max_range; - if (mObjDetMaxRange > mCamMaxDepth) - { - NODELET_WARN("Detection max range cannot be major than depth max range. Automatically fixed."); - mObjDetMaxRange = mCamMaxDepth; - } - NODELET_INFO_STREAM(" * Detection max range\t\t-> " << mObjDetMaxRange); - - NODELET_INFO_STREAM(" * Object min. confidence\t-> " << mObjDetConfidence); - NODELET_INFO_STREAM(" * Object tracking\t\t-> " << (mObjDetTracking ? "ENABLED" : "DISABLED")); - NODELET_INFO_STREAM(" * Detection model\t\t-> " << sl::toString(mObjDetModel)); - - mNhNs.getParam("object_detection/mc_people", mObjDetPeopleEnable); - NODELET_INFO_STREAM(" * Detect people\t\t-> " << (mObjDetPeopleEnable ? "ENABLED" : "DISABLED")); - mNhNs.getParam("object_detection/mc_vehicle", mObjDetVehiclesEnable); - NODELET_INFO_STREAM(" * Detect vehicles\t\t-> " << (mObjDetVehiclesEnable ? "ENABLED" : "DISABLED")); - mNhNs.getParam("object_detection/mc_bag", mObjDetBagsEnable); - NODELET_INFO_STREAM(" * Detect bags\t\t\t-> " << (mObjDetBagsEnable ? "ENABLED" : "DISABLED")); - mNhNs.getParam("object_detection/mc_animal", mObjDetAnimalsEnable); - NODELET_INFO_STREAM(" * Detect animals\t\t-> " << (mObjDetAnimalsEnable ? "ENABLED" : "DISABLED")); - mNhNs.getParam("object_detection/mc_electronics", mObjDetElectronicsEnable); - NODELET_INFO_STREAM(" * Detect electronics\t\t-> " << (mObjDetElectronicsEnable ? "ENABLED" : "DISABLED")); - mNhNs.getParam("object_detection/mc_fruit_vegetable", mObjDetFruitsEnable); - NODELET_INFO_STREAM(" * Detect fruit and vegetables\t-> " << (mObjDetFruitsEnable ? "ENABLED" : "DISABLED")); - mNhNs.getParam("object_detection/mc_sport", mObjDetSportsEnable); - NODELET_INFO_STREAM(" * Detect sport-related objects\t-> " << (mObjDetSportsEnable ? "ENABLED" : "DISABLED")); - - mObjDetRunning = false; - mObjDetEnabled = true; - res.done = true; - - return res.done; -} - -/*! \brief Service callback to stop_object_detection service - */ -bool ZEDWrapperNodelet::on_stop_object_detection(zed_interfaces::stop_object_detection::Request& req, - zed_interfaces::stop_object_detection::Response& res) -{ - if (mObjDetEnabled) + else { - mObjDetMutex.lock(); + NODELET_INFO("Stopping Object Detection"); + // Stop + if (!mObjDetEnabled || !mObjDetRunning) + { + NODELET_WARN("Object Detection was not running"); + res.message = "Object Detection was not running"; + res.success = false; + return res.success; + } + stop_obj_detect(); - mObjDetMutex.unlock(); - res.done = true; + res.message = "Object Detection stopped"; + res.success = true; + return res.success; } - else - { - res.done = false; - } - - return res.done; } void ZEDWrapperNodelet::processDetectedObjects(ros::Time t) diff --git a/zed_wrapper/params/common.yaml b/zed_wrapper/params/common.yaml index 9ad141a9..f2c752bb 100644 --- a/zed_wrapper/params/common.yaml +++ b/zed_wrapper/params/common.yaml @@ -79,15 +79,16 @@ sensors: publish_imu_tf: true # publish `IMU -> _left_camera_frame` TF object_detection: - od_enabled: false # True to enable Object Detection [not available for ZED] - model: 'MULTI_CLASS_BOX_ACCURATE' # 'MULTI_CLASS_BOX_FAST', 'MULTI_CLASS_BOX_MEDIUM', 'MULTI_CLASS_BOX_ACCURATE', 'PERSON_HEAD_BOX_FAST', 'PERSON_HEAD_BOX_ACCURATE' - max_range: 15. # Maximum detection range - object_tracking_enabled: true # Enable/disable the tracking of the detected objects - mc_people: true # Enable/disable the detection of persons for 'MULTI_CLASS_BOX_X' models - mc_vehicle: true # Enable/disable the detection of vehicles for 'MULTI_CLASS_BOX_X' models - mc_bag: true # Enable/disable the detection of bags for 'MULTI_CLASS_BOX_X' models - mc_animal: true # Enable/disable the detection of animals for 'MULTI_CLASS_BOX_X' models - mc_electronics: true # Enable/disable the detection of electronic devices for 'MULTI_CLASS_BOX_X' models - mc_fruit_vegetable: true # Enable/disable the detection of fruits and vegetables for 'MULTI_CLASS_BOX_X' models - mc_sport: true # Enable/disable the detection of sport-related objects for 'MULTI_CLASS_BOX_X' models - + od_enabled: false # True to enable Object Detection [not available for ZED] + model: 'MULTI_CLASS_BOX_ACCURATE' # 'MULTI_CLASS_BOX_FAST', 'MULTI_CLASS_BOX_MEDIUM', 'MULTI_CLASS_BOX_ACCURATE', 'PERSON_HEAD_BOX_FAST', 'PERSON_HEAD_BOX_ACCURATE' + max_range: 15. # Maximum detection range + allow_reduced_precision_inference: true # Allow inference to run at a lower precision to improve runtime and memory usage + prediction_timeout: 0.5 # During this time [sec], the object will have OK state even if it is not detected. Set this parameter to 0 to disable SDK predictions + object_tracking_enabled: true # Enable/disable the tracking of the detected objects + mc_people: true # Enable/disable the detection of persons for 'MULTI_CLASS_BOX_X' models + mc_vehicle: true # Enable/disable the detection of vehicles for 'MULTI_CLASS_BOX_X' models + mc_bag: true # Enable/disable the detection of bags for 'MULTI_CLASS_BOX_X' models + mc_animal: true # Enable/disable the detection of animals for 'MULTI_CLASS_BOX_X' models + mc_electronics: true # Enable/disable the detection of electronic devices for 'MULTI_CLASS_BOX_X' models + mc_fruit_vegetable: true # Enable/disable the detection of fruits and vegetables for 'MULTI_CLASS_BOX_X' models + mc_sport: true # Enable/disable the detection of sport-related objects for 'MULTI_CLASS_BOX_X' models From c3f68db67cfc03509fa5b82d3990f0e2eeea3b2d Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Fri, 22 Sep 2023 15:00:50 +0200 Subject: [PATCH 184/199] Update Changelog --- CHANGELOG.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index d7fd4390..e38d5683 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -7,7 +7,7 @@ CHANGELOG - Add enable_object_detection service that takes a bool as parameter to enable/disable the OD module (same behavior as in ROS 2 Wrapper) - Add parameter 'object_detection/allow_reduced_precision_inference' - Add parameter 'object_detection/prediction_timeout' - +- The parameter 'object_detection/model' is no more an integer, but a string with the full name of the supported OD model 2023-09-15 ---------- From bfd23df6e7a6572fa1e24bf6b1f4aea5f7c9aba5 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Fri, 22 Sep 2023 17:45:55 +0200 Subject: [PATCH 185/199] Update zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp Co-authored-by: Matt Rousseau <32394882+mattrouss@users.noreply.github.com> --- zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 6f8387d6..445d6fab 100644 --- a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp @@ -5081,7 +5081,7 @@ bool ZEDWrapperNodelet::on_enable_object_detection(std_srvs::SetBool::Request& r mObjDetRunning = false; NODELET_ERROR_STREAM("Object detection not started. OD is not available for ZED camera model"); - res.message = "OObject detection not started. OD is not available for ZED camera model"; + res.message = "Object detection not started. OD is not available for ZED camera model"; res.success = false; return res.success; } From ce0d2c3da27fa8f5ebdc21ff8d04e341b3ba5484 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Fri, 22 Sep 2023 18:34:11 +0200 Subject: [PATCH 186/199] Fix enable_object_detection service behavior --- zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 6f8387d6..de8e1c9e 100644 --- a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp @@ -2078,7 +2078,7 @@ bool ZEDWrapperNodelet::start_obj_detect() } mObjDetRunning = true; - return false; + return true; } void ZEDWrapperNodelet::stop_obj_detect() From 3132dc66d4e3cb7a6a0a007a39cf103771887d1e Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Fri, 1 Dec 2023 12:10:16 +0100 Subject: [PATCH 187/199] Fix checkResolFps --- .../zed_nodelet/src/zed_wrapper_nodelet.cpp | 112 ++++++++---------- 1 file changed, 51 insertions(+), 61 deletions(-) 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 a291297d..84780d2f 100644 --- a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp @@ -1448,7 +1448,7 @@ void ZEDWrapperNodelet::checkResolFps() case sl::RESOLUTION::HD2K: if (mCamFrameRate != 15) { - NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD2K. Set to 15 FPS."); + NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD2K. Forced to 15 FPS."); mCamFrameRate = 15; } @@ -1457,26 +1457,26 @@ void ZEDWrapperNodelet::checkResolFps() case sl::RESOLUTION::HD1080: if (mZedUserCamModel == sl::MODEL::ZED_X || mZedUserCamModel == sl::MODEL::ZED_XM) { - if (mCamFrameRate == 60 || mCamFrameRate == 30) + if (mCamFrameRate == 60 || mCamFrameRate == 30 || mCamFrameRate == 15) { break; } - if (mCamFrameRate > 30 && mCamFrameRate < 60) + if (mCamFrameRate < 23) { - NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD1080. Set to 30 FPS."); + NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD1080. Forced to 15 FPS."); + mCamFrameRate = 15; + } + else if (mCamFrameRate >= 23 && mCamFrameRate < 45) + { + NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD1080. Forced to 30 FPS."); mCamFrameRate = 30; } - else if (mCamFrameRate > 60) + else if (mCamFrameRate >= 45) { - NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD1080. Set to 60 FPS."); + NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD1080. Forced to 60 FPS."); mCamFrameRate = 60; } - else - { - NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD1080. Set to 30 FPS."); - mCamFrameRate = 30; - } } else { @@ -1485,21 +1485,16 @@ void ZEDWrapperNodelet::checkResolFps() break; } - if (mCamFrameRate > 15 && mCamFrameRate < 30) + if (mCamFrameRate < 23) { - NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD1080. Set to 15 FPS."); + NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD1080. Forced to 15 FPS."); mCamFrameRate = 15; } - else if (mCamFrameRate > 30) + else if (mCamFrameRate >= 23) { - NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD1080. Set to 30 FPS."); + NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD1080. Forced to 30 FPS."); mCamFrameRate = 30; } - else - { - NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD1080. Set to 15 FPS."); - mCamFrameRate = 15; - } } break; @@ -1510,25 +1505,25 @@ void ZEDWrapperNodelet::checkResolFps() break; } - if (mCamFrameRate > 15 && mCamFrameRate < 30) + if (mCamFrameRate < 23) { - NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD720. Set to 15 FPS."); + NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD1080. Forced to 15 FPS."); mCamFrameRate = 15; - } - else if (mCamFrameRate > 30 && mCamFrameRate < 60) + } + else if (mCamFrameRate >= 23 && mCamFrameRate < 45) { - NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD720. Set to 30 FPS."); + NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD1080. Forced to 30 FPS."); mCamFrameRate = 30; } - else if (mCamFrameRate > 60) + else if (mCamFrameRate >= 45) { - NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD720. Set to 60 FPS."); + NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD1080. Forced to 60 FPS."); mCamFrameRate = 60; } else { - NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD720. Set to 15 FPS."); - mCamFrameRate = 15; + NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD1080. Forced to 30 FPS."); + mCamFrameRate = 30; } break; @@ -1539,31 +1534,26 @@ void ZEDWrapperNodelet::checkResolFps() break; } - if (mCamFrameRate > 15 && mCamFrameRate < 30) + if (mCamFrameRate < 23) { - NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution VGA. Set to 15 FPS."); + NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution VGA. Forced to 15 FPS."); mCamFrameRate = 15; } - else if (mCamFrameRate > 30 && mCamFrameRate < 60) + else if (mCamFrameRate >= 23 && mCamFrameRate < 45) { - NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution VGA. Set to 30 FPS."); + NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution VGA. Forced to 30 FPS."); mCamFrameRate = 30; } - else if (mCamFrameRate > 60 && mCamFrameRate < 100) + else if (mCamFrameRate >= 45 && mCamFrameRate < 80) { - NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution VGA. Set to 60 FPS."); + NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution VGA. Forced to 60 FPS."); mCamFrameRate = 60; } - else if (mCamFrameRate > 100) + else if (mCamFrameRate >= 80) { - NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution VGA. Set to 100 FPS."); + NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution VGA. Forced to 100 FPS."); mCamFrameRate = 100; } - else - { - NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution VGA. Set to 15 FPS."); - mCamFrameRate = 15; - } break; @@ -1573,45 +1563,45 @@ void ZEDWrapperNodelet::checkResolFps() break; } - if (mCamFrameRate > 30 && mCamFrameRate < 60) + if (mCamFrameRate < 45) { - NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD1200. Set to 30 FPS."); + NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD1200. Forced to 30 FPS."); mCamFrameRate = 30; } - else if (mCamFrameRate > 60) + else if (mCamFrameRate >= 45) { - NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD1200. Set to 60 FPS."); + NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD1200. Forced to 60 FPS."); mCamFrameRate = 60; } - else - { - NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD1200. Set to 30 FPS."); - mCamFrameRate = 30; - } break; case sl::RESOLUTION::SVGA: - if (mCamFrameRate == 120 || mCamFrameRate == 60) + if (mCamFrameRate == 120 || mCamFrameRate == 60 || mCamFrameRate == 30 || mCamFrameRate == 15) { break; } - if (mCamFrameRate > 60 && mCamFrameRate < 120) + if (mCamFrameRate < 23) { - NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution SVGA. Set to 60 FPS."); - mCamFrameRate = 60; - } - else if (mCamFrameRate > 120) + NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD1080. Forced to 15 FPS."); + mCamFrameRate = 15; + } + else if (mCamFrameRate >= 23 && mCamFrameRate < 45) { - NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution SVGA. Set to 120 FPS."); - mCamFrameRate = 120; + NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD1080. Forced to 30 FPS."); + mCamFrameRate = 30; } - else + else if (mCamFrameRate >= 45 && mCamFrameRate < 90) { - NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution SVGA. Set to 60 FPS."); + NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD1080. Forced to 60 FPS."); mCamFrameRate = 60; } + else if (mCamFrameRate >= 90) + { + NODELET_WARN_STREAM("Wrong FrameRate (" << mCamFrameRate << ") for the resolution HD1200. Forced to 120 FPS."); + mCamFrameRate = 120; + } break; From 7325920d041b0aa4191a93fbbcc5167c2202a6b1 Mon Sep 17 00:00:00 2001 From: Woz4tetra Date: Mon, 15 Jan 2024 18:55:48 -0500 Subject: [PATCH 188/199] fix pub typo --- zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 84780d2f..30b0752b 100644 --- a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp @@ -5207,7 +5207,7 @@ void ZEDWrapperNodelet::clickedPtCallback(geometry_msgs::PointStampedConstPtr ms { // ----> Check for result subscribers uint32_t markerSubNumber = mPubMarker.getNumSubscribers(); - uint32_t planeSubNumber = mPubMarker.getNumSubscribers(); + uint32_t planeSubNumber = mPubPlane.getNumSubscribers(); if ((markerSubNumber + planeSubNumber) == 0) { From ac8265040aa963ab16803aaaa4d22ec1422c119a Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Sat, 24 Feb 2024 12:26:30 +0100 Subject: [PATCH 189/199] Fix plane detection bug --- zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 84780d2f..8d844adc 100644 --- a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp @@ -5266,7 +5266,7 @@ void ZEDWrapperNodelet::clickedPtCallback(geometry_msgs::PointStampedConstPtr ms float c_x = zedParam.left_cam.cx; float c_y = zedParam.left_cam.cy; - float out_scale_factor = mMatResol.width / mCamWidth; + float out_scale_factor = static_cast(mMatResol.width) / mCamWidth; float u = ((camX / camZ) * f_x + c_x) / out_scale_factor; float v = ((camY / camZ) * f_y + c_y) / out_scale_factor; From 78391c6972496b95ad12d2404606b6fdd9644b8a Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Wed, 27 Mar 2024 11:27:02 +0100 Subject: [PATCH 190/199] Update CHANGELOG.rst --- CHANGELOG.rst | 25 +++---------------------- 1 file changed, 3 insertions(+), 22 deletions(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index e38d5683..b252b495 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -1,55 +1,36 @@ CHANGELOG ========= -2023-09-15 ----------- +v4.0.8 +------ + - Remove start_object_detection and stop_object_detection services - Add enable_object_detection service that takes a bool as parameter to enable/disable the OD module (same behavior as in ROS 2 Wrapper) - Add parameter 'object_detection/allow_reduced_precision_inference' - Add parameter 'object_detection/prediction_timeout' - The parameter 'object_detection/model' is no more an integer, but a string with the full name of the supported OD model - -2023-09-15 ----------- - Add pose and odometry status publishers - Improve odometry and pose publishing and TF broadcasting - -2023-09-13 ----------- - Add ROS '.clang-format' - Code refactoring - Add 'set_roi' and 'reset_roi' services - Add parameter 'pos_tracking/depth_min_range' - Add parameter 'pos_tracking/set_as_static' - -2023-09-12 ----------- - Change the parameter 'general/resolution' to 'general/grab_resolution' and replace numeric values with strings - Add 'general.svo_realtime' parameter - Change 'general/verbose' to 'general/sdk_verbose' - Add 'general/region_of_interest' parameter - -2023-09-10 ----------- - Change the parameter 'depth/quality' to 'depth/depth_mode' and replace numeric values with strings - Improve the behavior of the "NO DEPTH" mode - Remove the parameters 'depth_downsample_factor' and 'img_downsample_factor' - Add the parameter 'pub_resolution' and 'pub_downscale_factor' - -2023-09-08 ----------- - Add 'pos_tracking/set_gravity_as_origin' parameter. If 'true' align the positional tracking world to imu gravity measurement. Keep the yaw from the user initial pose. - Add 'pos_tracking/pos_tracking_mode' parameter. Matches the ZED SDK setting: 'QUALITY', 'STANDARD' - Fix the warning 'Elaboration takes longer [...]' - -2023-09-07 ----------- - 'pub_frame_rate' now controls the 'InitParameters::grab_compute_capping_fps' parameter of the ZED SDK instead of controlling the frequency of a parallel thread. It's not a Dynamic parameter anymore. - Change 'general/camera_flip' parameter to string: 'AUTO', 'ON', 'OFF' - Change 'general/verbose' from bool to integer - - v4.0.5 ------ - Support for ZED SDK v4.0 From 4818f9667a4e1224ce059529958f98a9f94c8842 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Fri, 5 Apr 2024 16:00:24 +0200 Subject: [PATCH 191/199] 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 192/199] 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: From 5e7734dc3cd1dde840f1434819c7542492772ebe Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Tue, 16 Jul 2024 08:35:08 +0000 Subject: [PATCH 193/199] Add files via upload --- images/Picto+STEREOLABS_Black.jpg | Bin 8013 -> 15698 bytes 1 file changed, 0 insertions(+), 0 deletions(-) diff --git a/images/Picto+STEREOLABS_Black.jpg b/images/Picto+STEREOLABS_Black.jpg index 744c9ffedc909b27275080abd46b0b2c38cf8a3e..060e243577740d8975c813f747e4c894d477518a 100644 GIT binary patch literal 15698 zcmeHubzD@>_xIgpfu&ix8!73fySq~mb%6zz?h=qLl_f+35hvN>sPPm_x$^OXZP;B@60)8&N*}E-aB(1r^ zfq5i&U=m>GymvtS7(Bq{&d>kKwR8-O*~ElIMTEpfKu!@cq^yXPtf(ZLsEDk%m@HBh zq=w}GOBawGAOKE)-%`)C10cWgnOMjl%mEfdLG5tBoDM9<`J*Wwc#iiEO#KI+(LtcU z<*S$;8Af#dlFe6p2K#^2jJii^&U#BgM{yKnMs3h>3{lNJ!|! z*qPYH{@3ZW6+plN4hU#02LMGta1fBwPJk8E6Bq31v(EmLAW$3_E*?IdfRG3zs3$-3 z6b=-IgNqA;fz;t(Jpe=CQm~7v;8B{O@Hu>_#G=v);GC*8ZPcbin_S}dzR?7PG_-W| z4BQuZc=`B|5|UEVGO}vw8k$<#I=W`&7M51lmuwsy(N4}T7*{|4fWV;OkkFXexcG#` zq~!FBJDGR0?q%l`78O5wTvA$AUi+-BzM=7XQ*(PqXIFPmZ(slL$moZUW1q$+W@hK+ zzbz~-Ew5~C|Jd2x+dueucxKm`oqwg@hW!V-5TIR97z_u7e`Xg18U!9V2pBHAC?181 z2|mh)l0z&CPNkYwP}4@hDQ>z+ZSOloNW+Dk;odqk?cB2eJHw*?pDg>^uz%S#0TAOr zz|O-#07^jpmhh}rqOkn6*MyrfNGlfmu8Tv*`>D6q&5ro-&31Pd<-=fGb_T}TWoLjv$Ln#&^!+z( z(kLyxxO*v@RBZHSgLwI*H)gJODcJUeu7A6I{Gb@o=Cu$5ch7iEIzLuv)yqm$DQ_24 z`9(NpgRS8Oxs&C+NJ390U$W3cJBj;Pf?%kEQCAk>eqi;&@?6^wR(af3=3pwwsL541 z>w{;r*v=QL6<8AO=D-I;9(4TWue*A=@3&BuZ4vdH0whNoxDRhbT59e=ZC%r?u`HTkeJzNrzLMYA1Awn<|)Fmr`u2AgyP@zTOgD5eZ;4ifq`iUwsVeFD6Jl zOZ=;quQt_9gJ!lmV{0T{dsyTq)Imlm4sbEKDnGDA+w%|+rX`UOjd=0|)kon0tPc=> zR{`=`BkJC1b+%BSFkZI3auDq}>c1HNgs!4M>GKr8AFfmhgcJB`*WXJa?LM*i@pbJK zc(Rg{bu$3IQ7`bO*!E)ifSfeG_dL{X5=lk@y0jhh1oQ1>_N;qF`200ET1o8+(Jz@B zy>n$Z#;_;v>mWO?sLW{_?;eKV?phy>7PuZfamlWkzqC&&HJ@Oq>cJ^6b1h8uAY+!X z;woO6gM+7dS{-?lw6ha_MoP>nP^JGtUTw>Hrt{<2;d=kq@kp4T0z znC#UYk@;hw?UjSstAm>INxK!REShC6Z%aOBXA2H<93YoeU@AlqMHBFiuzPt7_C3z!r}wvLJ7h}|tB}0)xLD(c)NY4+?i7eIlss#RP3M8h7WYv| z$z`>0LbbB-xF2D!DKr%5NT*UXd~Yy(mnRGHP$=xoQWUVSAgHk$#@k6vDPqPd-hot3 zS2}4Obj?{m4Y=SYZBCc2^Mir-);erDf_iM3l|o+lsasH}<-^h^wG)r`voMH8X*t`1 zdF(*)&mUK?ZGHzFD{2@GMGUpz5h;BzJs zPKAo4hoq}~v%-X!!)BqOAotqLlT}3T5vkjv7xO{?ui2BPk|jmv5wzrxj6W&fop;!~K^R(BK8lk(RKd$xFH z?jG}crEBO-3eI8dA+3A;=}HX14`JxR*X|tr*o*E+EW{|pso;T-DW7&0SNb>(UirZ( zV7|48t$X1nPz8LQIL`Bua7_7PsQ~9p+c>mL=uo_N2>TfM zqS8gSg(nA-bM6y;^~3Ub!iwV;-<)K8BYC$}ULw=Cnl>CM`wB{;JF*x0yy3(8V&lsM zgBQ2DWT^s-FAJ$GexGgH7;pH(UhLK=ho3vrLm%-=H64G!AZQ}pjbQVp*~!-&zIwIC z`XhDP2o>HQI+hq5g?HXxp9LM=ocMYiUUCwp%<#32dus0#AT3DdP`!M$hw1srl9ks& z;d7&e2=Og!LKwRjx+s_-Ty^k8uh1|b{7%LI+qJopnJYtEwC(G&rvU8hvWdqp9jI-1 z56;^w$^-5@M{Yf@>BCGDT0i<7*mha&)2zA)gm~86bxD#zqsFS(ur>g;Cv*`jA5`!- zJ7DwV@Do?NMW*04Cm2)?8#>yIwH`VJoQUols1n_mE1gHsEtsZoxe|4N@m=1l#-oq@9Mn?d%*!iC9qp zwN_CcxXnG=Y&Zo>H^-7ME=FIt|Mud|rou2F_-fS#&?Yuf@Zkk`32F8c5qmG|e(%w0_Hk~@yw7OMD)`8A zw%Ij`yMlPWiS6i)KNJ=^S$p+%IrmO)d)YoAUignIh)ryFj&roUN>F(DlXa?(Urc}U z(k*%`77a%#HkXG!xr@L^d6oZE8egSk#L7eOi@u)@=A>SDxH%X>dJ5#(WE~%{9$Cm) zZ+Vn-ktMKj*KbZqIR&#{F6xFv1bG0IxpACZ`J`4x2PF=uvM)Aaa+{RzYpccTcsnAd zA}7h2NR1vjg{f~1+^YYIfBDm@BH=^*S`4RCbMQO_eogrev2XAZ<}3bd?%<%d3BfNJ zl|9_W>)z;Js;XCmTN!PcqD2o!zfzuX_sms7=d?GwZoligMc4GYIk-PggOiRq{`x~* zU?T!1szl_`CPU;S!tzxjB%!Us+EY>dt9(8s^zs{)8?RmpZ#&p!wUSX|MuPnhF4N;Z zBGU8K{CcM+b5Y5%TcGbqpz6wj!>q(YBgUC~)G}Rv->1X`68_`{aWe3W@HJ+0`lRpp zu~S3sPxkA5bBI$x8#b%;+@xroDR)?_qe6&AEb)Xjo-fc4m0)0t_^2s7!Sz{-jN z*KFyhiu1%|38!8i>`teZh(@(dR9 z*1yz=2rE)UCBOaZkQiPDUqJUVD(zNjavTLp?X(@!0~zm8j(J8bDSiNlDd1l**+T1j z!bP+0Rf06407Ra=pJ~9_>Ub`F^k`U&l} zryWL{!tr&LPOBER!-56PO$O=IMp^WI?K||+?-g+U&5dEScQ-%V=#X@ZzM~{H)gUID zc3Ws+aG9{Hx;Y*H)!ZIw3VHvWf< zb_OUn?*6jQ0m3Y%(6k)he7GVxhQTq^^m9*%jo^Fyjk~yHr$D%a#EQR(GLvxUx`5%! z&7+LZ);0rTeB)fz6=KH(^we*T=!gA>+&#eu)VktuOQ08bP|9`5GtE}FBkjF4Rm`r~ zodWQZ1f`{`!!4VnN}He(N;6cOi^l-6>IesWBv8KTnN_m<(9M>J$X5B69-Q5JKVUWL z9q7HBBl}@w@2&tQ!?5m>V_H}rjFA4MglVkgz)WwXGK@EM@{>&mRn5s3XPm`V&fG{D zQ1(I?q!`nn$klRp#IUY0m6Oe-z+5hfR1Dt`pXI}|ncK9wbswI7YZ$p$kZzsr7(8tu zLS9#-ZqV404^1F0xRrwwT$zMK77u@E&ZH}vM46wAC zXaHto?v=tjNi#3&;}-;r*K$Nwe=_`>)EKI3Qe;=C%kjA1N`^oZS~a@cC-dSg%Sp30 z)Fm?p7bSC>)N>k_D@K~{m?+~?nJBZKj)N;9#6d3p{@${}!k&IYC`T`Qw2*_Bhj1{; zTUb;`L>Q1)3id`hxS{>o?9t8`PX&&R7cCrY7)J#TD+vP;18+673r0KC7i|`5Xzmc| z<{;z9p`^$rA1oW};q8I;N3jKaxO@7^1}ku!3zr4)S+Ot&+c}B9n*xV5xFDkD<%?!R z3L%9=1i^)wKrs$bhOeWOtf{)@9|_=@0>>X!gMxyDg2aWqe4T|wWn^T8MZ|=~!~{VK zLB9}Be^jubryu8E64cRt4!#&~e~gzW+nGd^y;p$00teXsZ^b;k4GjJk{9pRO!{bcb zb80_-jX;q3fAW~0d5AY!*c9#O72xZD)(Av<`g8up?&$D0uXlj2`?&#*4#H@6vL48jV#LH>?%~?@IwWn9Xb9KI=G-bozdWU0{iqg8Roxmv81@8 zgrkFly`YpR3UsW!h@&7%OxjUU%3f4L%s~P)K}6~=em^fK{~(kvTE!XcZ%|XPx6cv z>o0O}%$~u&Zvl#I|4#q*z`s54Zx8(21ON8Gzdi8(p9lUmi9maTZ#qHX6ybChTCJm| zcGcL_NK;2&0}Nz>LCAb}2M<3O7#8*L^!GK@Qe^{UYHavRV04xoj7PEp6etHjZxv%> z{l6l6|2g0Lcow7tfDyqnS^v5Ie=H$!1Sbn%M34=nQg-n6^#}1a5K9L6d!ONZAf^Ie z*c?Ip1jHA8K?Onl>8#xTH(oo#&gb|nObk%@nwzSDww=X?*_{7@?f-!tTzox18YD== z<>=uF%7+LjWL)1%QUm zf0fy10YGB{7|0&?M){)7+c}F@L!H2Y{dNfekX!-)%0U1iwEgWjujF*bI@1% z?*Je>2LLWOgR(FGH+G*z{r~pc|48%4f9HX3TpSn}VFlx>FkD>vKA|fD0 zkWo+|$Pfrh8YWswDh6r<0Ryzvm$wdwBcZJ<1Kq%kr;E|G(--o}Lhz?$FGd^t4?Q-KD1&nvLTNZ1-*7UO5UQzg zFhKCKDdU1DtoGcV4aSn=;ZFCF+&aW`h{tL4K9GRiGjJ8^V(Z7b*(!g7Hr}1t$rqG> zkK-);0%?*nffG?^9-6cl`ji+(lgHw#SMLdG zljcEo2DxZbk2;MfgnQwUYNe4O@SrSvuE6MTTW_s!)Epyw3s{|X;Hs=(7xIG} z7jVw^F5uuo&N~w9KpZ>(LQYL1fuNMcXA`Fo5fxKb0Xr2A-cKR8&|$eU4O(&kkKae- zSd;zNqH03?bJ-hrcwOtw^Eyr#Tas*X=HKi;zf)Mki)mO=sv*$y9;Y&}QB?JQC~5ir zGtD(wb%S@qH(oLE-cpVE_$FLRJV5OgqHueC<~vQyXIcLS!wPnfu(JLaLiWq9Kc7lF z;iPNeSSaXf_gxn;T5bD&7l+@!&*On#TFk#C+!?8<&U6~yo?`!86M{sDnA@26KgwEcx&T7p}?EM!-kX%oeL<_83uHy(h3`I=t`{{`l@W4G^&X+9P44=*K5O~$g2%+IHop3i%isW) z^s~HY6$s}5aoDK4_{C!rr}5FYGD3>U%`@^Gzt9Z9ji*ay+j&8^E3+}au`B{l3fPFE z-Q0D9HfIqo#X`Mz?TntG1uEbReV3}x_8)jh?<}SJNOl*$CD895<|_@S$+<9E^P#N6 z_-&`Nkni)A;6M)*-$n|aa0PYyl+E`ZV#2Q}*?;m#TI#-Iuv2#Xc>R;2SaYrQg<30~ zLT2lcBfnkfmsO2yv}yy(9$8LSkreCBMELP>T>3Nh=){!!_iZ>eW0af&7w#ODWR0?y zvuxSv>wHS^9>3aacckc^ zr@JBkZk>4|fkqm)W^ck!%q;hc(DlLBFJvEC-jr3>(Rr7R;8ae}TY6vi(@2Q0>3*bd za@nICqZwNh>9MIjbSM+@*~k*ZqBMv8$ecfJJ+b z+Rr?Mxo|=MGYTff!B6IW8MLwUBZtP}Vj$%2E1yrX(Kh=jV!(?!;oIq9@g0n*AX zM|MXhVkA8mQ>eyPF`XOUTSCFu-7&iW;oY4NQ@_7}C@;t1YC{noYYEYEd73};CVIw9 z-y)jMKdqeUI!-sSk{@_oR?-{Ky-WOHWuepvj(%#y*(=gNlQ-ggpo8~(B?w`{DR8z) z=zI?l{y(VB_6Sh`xRmV1*Syckp=ac)z8_VHtk+vuZSUkH)#IQn2Z0|OAiv7_-Y5u^ z01n56oj+4SesFITxHXELl8Qr2#RQdx%c*MW;}=!KhP3xBsBIgf=JHR^5D`~1Q=ZxU zbLYWRf--#(n>i#n%o(no(!Q~pAiYW5L!_6v0j|i$!Jhm1k$+8p4>Ql$4Q-ZeZmM9)DXBfvNFP^ zu^jZF)5=@kM3P=2aQDM*F?CIiAtX<-!kddUBoVo=VP|C&^jQ z*7PUsX?{3*)UO?WrG=DPiq?ho1EbN5jmAR8-e58rBc9pw7^8gyo9jD2kBM^z_mg|p zpkMh18>5USaU%j!effV(DfK@&F4@i*3GgomRk%So#5^0n=54juakHXWb2sQA;Yk0^ z@jORts&+d2s##9X_yv({T818r(7o}!?i;Qw=J7rE>40l{9tVTZYGXUK7vYT$u@*Yr zUr>)^c#GIfwsU!Hq*#JjlGYzJ7dZ9OsF1!|E3mKN2%E1VF#;)NjKNZ(w zx)mbcz-6|yzv)x+KnR5{Ev_BVQ`UCSKfSzeV%PLICAy++kQoO~-ox0|Vr zJWid7;Vg&tQq&bj5f7E}!?KB}la=;Gml^cRO2|3aO-i_uuiq=NjWu|JoQ}HM!5wb$ z{B6jN5&FvYgrBAZIG8lQm~qwF?ZqVFM#BdNTThL-DBC`^UaP!8)D*5pANw%VEy1x? zGEU*a?uBe*+$0yaAu0N&-PVv}x10QQLESr658ot5^F-nm9<)-%(L;hX$Av7;8`V~u zzwXmZkG{XP_SIC+&uPLl6Eq_c8-`D~t@s@z=go5QrpsI_ zDX-*?$WSdlFx8yQxkiQsR89e|0;7SQq5!0Uut@pxN>$mwEI(Ds=egiX8PEQg*b3d8 z<_}A$MeUUwCRilFULHZ+8U?WHzIcNYOOlD%7+KsQJ~4|2Y^#{|HYZhiJ%k9^hNV2q zIOhHQ2A^(O-iWAD=r}4L=9zfuWYpmWOO@_#&XjO%UB7fELSg}e;Rp}aveW4*Y;G}N zczJy5<~}3RF`iCF?}55@?#gwHm1y%a$IH&pN2_Vg7H;21elUnkb=I=4Nbtt#`P|Nn zm#1u#DLL?IY(;g$6Cm zxfD~{-Zf?`6P%61 zJ{o>1W)Noi=Xjk?5vz|iriIY&(Kg;59p2W0z%~RkJe6bG=FY5Jh!o38(+hvN4WA_G+VG?@B<&eC@|~|-DeSSLmy!eFto23X#ry{m z6N<(!48%k_U4x<~)shZ7<;+^@%*ut7){<4MlZWXh>(X7)%El+fbb|!7VBvJ5yoWa5 z8k04d-6KC{5QqA`Fc79B_F0UG>w$+<7Notn1SuSx>|&&zumGmVE+XVex>6Z9^nx!t z4=cOymm13^04TIOqs#!6w61Kt*-M0OnHR;=AQ%;~hz7k}e<_o^fz&|2ETLGM{&`(}6qh6^V?y!W+uCC6 zF8EEMLt%{f(tW0H6aHeq7^9u^rK*Emg2pgy;=xob=dR(HyI|%O-$#0QVT||ZME4np zo%}Y`^Y4gXJ>@{N)YX-6)D=jpd*6+jmH0U^; zSpAb}TEyD@stO*387Q?P0HI1Qf}Z#ABE196AcXp@EJXKfr`MD+xz`&5sO_(%?rG4@ z+`EX43uUCY=zLE%1!>~+v>DwCfmKN* z2L!3Y>ct(^+|;Zq*-2E~Z_1r64rTHQ;4O{w)Q7zbh{KADcWDqQJN6tf~B4RnVH5N(!gkzJ+<0`i#nZgWx|Bs%O5nhEt>L;7xj8Q5&r#Ui4}{ zzMpxw$J?gNKI^0BBdLhl2?~T)+oe+J%dhTWOl<2HcFs(8*O!s(%%6y@uI&zZD$mwL zH8fB?_I6CYE>hwY=!nvjm*rNI;Spf%D|IQIrBjz1&<%}E`qsDhrD|aOM{+F&UkcOn z2#9T<3aE7&+Z~B?Zw{MjD$$g>XKst2o+8T&BQ_CUbr(NfpG1FFf{h>4Vt z|1$OEWcop-eP{0yGm6jZ*BM~17$}aV6#-mv4RGsTMhKx}@vk$h3?LjHE&})$;C4}h z#*p5uncu|{ zDoyZy$D}oHDGzUykQYy%`JkkututHdkxJs-%QXxZ@pEd8$B%CZuJeYJ`9nP3#(c*T z@(fXv_vzSphjL#rVJvUmSD&Z(<$A-$Y$iHQ@xgMNn6SwkVPwvacSJD{JYaVR$o$>niz;nYjzDY`GP_b^JN@(REjp{8p6Rl zZZA~f?N|6M=`UpRP{72Ua_BiNDq^*(U&tpJ(+Bi4vA<$ofEbO2yT%Z8Gd+JK5@YXOmOB-aFwrNbj~(7im&54DR}Cv^F3H`|R*9rDCfA{O&H5pXwfsQI9BM}M z=AK32OZBCXntmsQo_WT%b}GKz`}UFd(}A{3_b1~AoOm>&d@40KS~l*e>-qA2#`C0= zb`;ml7)Z@)dK`Y<0Pnx&e~CC>(j*56xYBk0e4}-^QdIy76(lAz&J|~#9tH1vw0`^$0dh} zkn^c0+wZC8Kjx?qb_8G|sshr3f2R3+?nyrBzz*#@ZuBRvDdCQ^w@5EQ<#WDQ1RJ|R z4mJjbz`z+NDEoI~zn@T1ej$UEFUtRevN>ckjF$yt9NiIr z;pKEQtu&g9a8INxMTKE3od#d|Kta}hDY3lllQ5omp@Kqw`mP#`NY8MNa=$``F zpAYsgelGxIpI%^5hUX?LJ-LE_F0S0LzQFYur=5rAeP-Dk6@7>Hbh7HUEZ6EyI8Rq6 z1i_p%Hlv(fJ@C@xu&XrjdVtOb(el?|H4&ZT6ql78T~WFkCXbg?Nujxw+gCd1qxw{2 zki!EG85gmH8F94hJcO;WQ`#K6D*X7Q=w6$n-WBG@pAjz?R(127V5BiTAppG&t9NN$ zHD=e~m}hIV^-99f80DkZU8cMR!YkO207hNSN?N)?PQTRSM4Vw^aq_0&Di56I^4uRj zj5m{h=?q*;_`>bO=;FwtRpZ&tTgs^#&`m=Tw-8?!FJf^k{9C=HMam-GwW=N{;ir7Y ziErLQtDpUm)FH{Ok=`pUPpQ7TIMi5dUa*;wHaymHoVIsV<57Vxm5s1iJLVR(wJJbD z?oIaE`*Fu*xyJu&8hS_&mR^N=7*hQcr}$F6ZvZTaBSV9iCh1_`I^uEXtK5OW&p__?mLubFh>f0ci?Mp|gZm$OZK$5M4nW5T2Qa~Q8tjf-dVOad$EYazkcTz?zA&+xG`hsz0ol#eBITQ#9cBU$=Md5ujR_CgM-mcqJX2<1 zS{bfpH+~8*o@`${`Kc`ti2Z0xemJDD_pnI=8PYx4-?n0*xMcWkg? zX4j>ngI}+t{822_bK$acmr;3KQo*D_X^7^CFOze@D_U1|lo&AcS&^5*WM3;v3&Q)s zwJqOR`3ELSF4ueSI-A7@IO2Fd0N0RImpR-^XSRLtgXB+vMFHNn#dV9D-)BBoq|!@_ zBQs(}t|6H=R8kM{?{D^`xmwx14~v$*6a|#Je?|n=Kd+4T8CIskdBwR%EYSPW_BuCZ z)0Wm~ujVUQFleG&a@^h#)mj6j@rBn`XczyS-#sW{mU@!JmdW)CO;oykEKJ!w?SnBs ziHucmTk~@-T7-KfWvWPvkz44E8O=VfknC2*Rj~A@rC8yE58oqtyPD$F$}PNf*mEoO z3f)!Yf#vvTrOk_>t@mg=0u{A3gRHG+bZ_1ccu67f!3ut#c|fz1mjh zsgtW+#c~Bthzr(8BC6F9MI8^40jT);wGi)njQ-iLI9aglr2LG@-T|Epci3Sj?VJyc zRLut87d*vAHUqEq8H(|7(q^kVDDKE90~jOb2=z8dD?CUW8US_2uC?g9p%5 z!+So`zc<%WnZc*N?!$Y9iKOW1MDw$J8HJvN#Fq7&BC~5SO5Fr!PW|kW`Z}q$2A~6? zw$6f=sembeu3^eFaOrVqiVjBfVU&P)dSpb~91BEJXf#XmU5El~#llyFRpp0xG~mc( zG!h>tTJSj5MHVY}XJATKtvhRlg&0b0LCdGx!`r@j*{&~|yz&9sV&zo~yn7-pFstUX z8|Gtz;mkFRnsLUiCSCH0F=Ztr1e+8;lUOfx19wx`cL@fF$a=(k2lI17O%r;zzU{fU zaN6GwbxJjIAMWcn4SdR%J0oP#>NZ=!xmp1d=blFnr)XPdB{sO~EZM-LYkk2J4?~-!o za^BDjCgNd-zM)U6wDnwMBV~Kf&3)JVjE6{Bt})Ew2JeZSu-=WO-n~eOlJRkhC~g;b zGTpz{!WA`0QVCa+F#i13K1!G2fU`M@^%_-Me=0BIQa1UhwH%~~g^7Gb(gvsc^VhpP z(^_klGSV|P13>oiHdiUp#yk!kxgm@%vh_{?K2;l^n1~#to5E3Dm+Oe=q0&2Q3#L?Y z&6>+}OwPtytXHA-im@Htnk!A$h!SO{lVz#(+oJB`ergY&NFyc*U#2u j2lnVRi8{X3Z}GLU$B}0ACMB!e`hw3QcL#B{Psjfcl7xaY literal 8013 zcmd6McU)6lu;5KX=rx4if`lq1gixh-L{NIQgx-5EDn)un1X1ZAf>NZT(yJ5!r6bZr z!~%i>B9guMsqfpj`+K`@|JswB-Xb0SL=P z2#p|T2LRB~5dug70DuFm5I6t;lq)!WeNJ z4#&kWt004w7nQ(aPlQ0o$jB%uD4D6Kn6cdK+}QtiBXj_Cq@V>vVUSY*lnw%;gAjTE zE^wc4@B~i|@ppqjVMK6Z5>hg93Xq_I7Jx!vFeniW4krR_3<(F<0U|m$f?HgLnBD+S za>^TtjYuyf4AhPsqr;nRP2W=XP#UaY<>}{qhGDkL&6i z8k?G1TD!V?di(kZ2A@y7d^I^WJu^GEyt2CXcKzMP`^_&qyI;TU?H_zUJkbjR9`&F0 zk7EBxFFH^!C=n5ih~z{s2s9AvFghYQw>U9E#ef9wO@9g-L5fsOFRbk(4jX0Y)Y zCuc<|2xGZ|DQDbtJr_&H3v|_AmHJ_=l~^P3s>&19cIsYcfVtk{8MoG zYk#Xa>=66elCb6%%E}y9Z7plxvnD+zzV-fk%5sgyLTOI$`TbVd?nimZ*tiB@7Ikj& zjb75vCcE-NmK#0MnGKJ9!u1ymwy7~Llcjk@c8$Xa!p{wEyvm6;8T4){UJE%~ttmN5hFLoK z`FY8SiFx>n;_W_u%o-Nl0NUSi^+7%@OzDaZ?N>uT?Zvaxq`_E6y8deY91a<)_8 zHm!Gqz$8TBoV3_+W?0-`1WaXD;AUg^re=?ObH?;Tlbn`#q zSCq!ei~TRizmyjLJIa3}|5E-3p^1mHpQ4V3t2&Kl8u*plAN(|FiUm^sfvxPd85=BhYp2LEHP;`*`5pQ1(7P zo<1l6e-Bp=&p;1U0N%~t9wj9yDIqENTin06K;IHOanOG`7wB;R;6SGZ-B;|dlY^7L z*Tmifyc<3zm*7`!fkB}sU$7B@UpVPWl!lX&kPwqnl2cMrkW)}lQ6p%nsOhLFC}FALR3=EVsOw5c(W(1M}`75`;KprAE85~ZAq@tig{m;{`LX=?;Fs%N`E$m>* zgn?oBPhtT_VmJxuNgM_vH1a<(&L0B8@AyUwqXiSql1q^I7%)4$=u@T^ZICc$e2j@o{`EYNV`^H>p)Ik*8KW8&eTpGB{AA z$;5px(JxHEVB1JoJlaPVg83fXiS|dN(rp`xZ>+ecU+F8&bMckrEWK;4X@h&8ul?9B zS6-v5Q{uKgrQ_(uequY(^no=Z+c9q;gPdga3mff$HNhsLOdeL+F?fLdZR;sbTaZoH z?XJ*cw5FuNZ9TLWlB_JwFjCZDU@eAzP_1w#fS&l2x&||OjssTKKljQ1g* zz4B(Uixxd&C3-h?gsj$r%{lvS@X!EqDI})~+Ro>*YzD}aL=~pSs%ddt|6`{=BMoRq zAuvCK4IqZV;Q!!>;4oq^w~^4&AsHB@nCK;>5!}4k3!rgHL6bw^&>@AKkQsN5-1V!C z<4>Oi9}KJ8_Kz%cu_YPvsfqOB5J$6|VY8f^+YBLL<9u3PdcsL82^BQC#T~*Fxt{NS zwmD@gI%P6R($!mk>U(h4{2(-2D6OGYv;zaKG4{aWEU(2YS z$-Ke&@OLy=Us;*^@5U9@0 z!NZ9bfup6SEAhbsHJPQu;wF~0TRQ;()1QhF)P~z=)?IKH&<2i-c%2+cF)0F|UzMn1 z86y5AxA}#Ma(EYy_Y?1o8!D`UrmoqON9>qo!(NlU>rt(@9y=S)<9o|~UK3&WW{$i= zcE`R|1ZO&{J*ryeKU$fyAWAn~fR3lgOV#>z_e+2Fjn?=r-yeF@dP1+S4G`P9%p*K4 zW}K))OPs2_HBTj`lrFxy`CjB4%Pq4lz8MP)t(Z>>%Sm?{vkMfQqe-8#6+azB*Y{q z68@=Z5pa62q&1*Lp2A}#4ZXpVww8ehEum`T+d00BGV;rKJh6@Q$-G%qcOf#mxTI_4 z_l+V}fxUT}ru?VZkUc9oo`C zKC~SflEV2^K$qwN&e6B9$Ea`rOJ$)@H>_ql$@UxTm!zx329mFzcRW6TH%}+o>`fG1 z?i=n0q^4Rw+ega#tb6#cGCym`nA%~~%rZadMdoAX{B zzL@r9T^vGqTNc_GF_>z z`6~xwrpu#3pX**MVSL^356fJOjxXU4Oe+LRTgFE_FTM)UR?;06vRGJfOdq<7UI}NQ zPE_MJb>=Zj3}{?V_G}>l^gE9ji@ipbVjelVCKgQeR4Jd18DB3iwMr}+ufZpZ(P;30 zqWCQ78Z5j|fi75~tzvuOAI5F#eL7Bin62y5wuTmN-oe_|cQ{4TpP_q@weNOJCG}O8 z5wWcz9=uUqGgv>@z#Ls-$uinB~sr&dMl=Y-~4dSW5DIYma!I*jk3yE%#R0qADSGUf!8CYhnVl8x2WOnhUW4yib3G-(#HK)2{SlM;}S1 zd-vzD^;%y%Lx1lHYWos2*Lj9QMxXNf-rb#D!y*B7gX<=2aON@=yNk8M#IKZM{dHV@ znzf%c(G45l3OYKsVI?-j9{!W2%+Jk!)nSS~O3E-ySW9Tgzc}koe9F1Dk$UcPW7fwN zmlSQs-S@`AU$aWQ;6HBg^xXCcH+JRpsEv`Kl+SH!JdoyVGftFhXBU?V#^u+GHBTia z?5rApPU^7dckq3l*Vj?s=O!_j4Um+j^Rw{XylruQynlzmTiW!4T<=g!ShM>s>zB*N z=LvvhsaOVeRW~zseWV?ddRVc(Mw6Uj9lZ}Iab`Eh+MWB1VNY=AK74Db^^pLmets7w zr8{YR{C)1-freU%E%OkbugMX)u(}`qswx^=(?i!+tc}yP?m=ghsc$qN*?LLVDYt(w z-+cQup`k4+-ODO~BP%YLNi1YKi5!V4ahX6zK1rM>B9k> zt9p(lpXfyEmf0t$%8%OR6kXb>6IPAoxl>uc#}NQOlgZqQg4^UD*~FJzRy8i)Qx2TX z-xzF;8rzo^%hFu#P31PTv@wV*v`wA%;H++~Po4d`aFL_6;bXPbl7e4)vM`VBy!T|* z6GvC~)yCUa=T=ItXc=4H(L6g_S`%*SDJWg|EwqBB{?5;-jiy$EFJsm{7gF}fwMyuX zg!OW|`5x@yqpoQcyaC{o#lq=I?A{#t`kSFvB_nL!EBABmH@EpWo6oNpdnlP7|FYXc zfKg;%Zg80|y2{(ZeZ6>sRYmc9n}F5({k*MfckWkN44|GKsvI%BU4Nu}Yn5NIrtQeP zw0*)b{Z-Z)WX zpCKNpd(TQ{t2_D3oaWxgd9oCL<{z%_s;-UD^xeNGI=Xw?+j__GML{miOaP| z1R&YPeeGtS6lL9Mv0_Owzuxg=t!%#9pX_igcemW;%vi<4XOToxXWvIw%YES`099h| zsu7nDSB^u*`$4F z%_-{)s?vQJx$x}BW@3U5AyV!({85@o{z)roKlP#WyH=}02^+K`%kHbd{o#z>XQ)d` ziv$3yX~3*1NDG#ZC(jrsuZvD{CID9YC+QMT`zup!kE1Z1%U_iLRGCWLm8YRF|0gw* zWmFvoy@gr=Wce?aWObb>egZ!6=!Hk;>V~z!J6%)d>&Yu0kaWGvHsq(+LG=UyNXSQH z!CHp*>WgL7#=b!}<^oc*4&voXyV-WhZNBGh&%be!>m=siG5dJT;C)7wlrrS37pk?n z^xarbcy#gO9h!7A1wT$hTJxaer~x;CZ8D2C-a_+*M;t`P*$}hvM(4g6F{capiuQS4 z8;2Mqik<)n3Vk@#2&{R&T*-oD3w+Wfq~4niQy!D58Y&hJV|z+5Z24VITbPyjFw792 z>~_P}p6^8~Z&zK#E1X^{x1Fy>sF(Os{kTlj;irUJx`p0T?joy>A&sBQ5vMs#&@LIu zYbh?%{j9_>_QP)Gs+n z45pb5J+JiRcYEhXtXyvpLglTJ?Vd7~OJQ$);B`6o&f^{RGw8lCg)at&=z@v5;JSsy z(`#3sWBjk8f&{h|68I@<0Iv6MUl@xDPzcx7O2xN_DNpWd?OB_M5Jj$%*Dk&B~7QW&EZZtPq7FN6(KGe zduOraxvF+^M8`yq$eJq^9n91}x`e{VulT)Q25Iwrozr7mocdd~gQm{{93gD)p&gzq zZ5QQa2aBNS^{;m_F+Mp_-J3<>JRGD=M^qcmgpi>~cn1EbEb7o#VC1pu^wHkwuk z4M0$|5Hv!WD3T0?P)0?P5+QWZkt#&A5LB8rfKWkY=m1a^bcPBFp-lt@h%$80P?Zxr z0(`>?k^yKa$f$yXlAeeHG1rw(0^xu4{6YJFdj9xDz)K50hJ&wcPWnIZK>i7(v=%lQZFki0Nq8b)%#jY#oXWkXp(f-3oIfkZv%GSQExtU*5l zyrK(eD&xn`h#Zo0Y2pVYZ-w=o^|~jkW@Kp}8ll9=UM5;chPsY{U};FCpAvwfp3P~$ zC9bnvcNglrA$sug_J;(Zb9g!PY_2n(+l*dLlrgkvJf1EF-ETDACcdQ<89=1A_k(VH83>0Z-Z&c9_LqlQQV~mP3VK(J}B)(59dslTogGxv`xbd z?{hzb^jcw;83C}aJgZ@MeOpcsR~%=S9GyD4THYSoy>|UZ6K#TVCo%~mp>W|owA`kJ zLhi{_})l|ZRVZKD-vS(I)X8IPRPtO$Fb+LSptayD8w=Q86#iV5w7?$*(mmoTrl zs0%$`m+7)xyyS>@vKkhB7@XogvY9aY3paiwsL#0ejuo&rCQ4;%c|9;S;AH?pFfAD9%QqN5zzZX-L4S92TV@%pLMvPK5RqT{Sl2T)Ha`we zLOwZ2A#_gs%;mJC4I97faN3?n+DxiT6W=2}pZqi@0Q?I{Pux$7iOLdy@w4-@H|sJx zxm*Opg+$fw9`z~|9a=!RE#dAFV~DMc1Q~nyP`LjCi==OJ%Sw`PMLjwwv^dez#MzCB7ya^qgcULdo8-& zx0lD1fwZyETQl-TX%zB_>H1O)ni#VD-1~L1b(z&me8g*)oe=@*1<)idMA$Boqrk>H zdY=cQkVejkBJL9XFgojjU7bgs%8F?!=`b}l;t1u4MW*ZCO6R}I7y?L%<{T_U`uumh zH==h+NYn^`o1DHl$I^9eHTryRz1VnjO$w zXN_)SXqtJY|MtAb%wRkzpv=$7v_;7iZFfcB6f51uQe!iw`=`9rzbczLMea1H@k@rdjJFgaMlul;W%2Hy4;;kzRiJndi~14DD4;? zCYyS>(7tMinCH!J?UpScQgc`d17_N0M5YL?kcvkr!!2&}msYS>9jg3Ri}cFOlYkTz z@jyJY=j^gt@rBU`+XI0=Y7I!C0UIOT@U$J{IV;o2p8)0gPYk@u-t!S8#I9 z_XHg=)JBTZMV+B|nf}`~$}nl Date: Tue, 16 Jul 2024 08:35:44 +0000 Subject: [PATCH 194/199] Add files via upload From 3503a1eb9f29200625c486f4a28868112a8eedb1 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Tue, 16 Jul 2024 08:36:24 +0000 Subject: [PATCH 195/199] Add files via upload From 060cf3d0d9f11795f49ef32ce34cccfbf440ffcc Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Tue, 16 Jul 2024 10:37:19 +0200 Subject: [PATCH 196/199] Delete images/Picto+STEREOLABS_Black.jpg --- images/Picto+STEREOLABS_Black.jpg | Bin 15698 -> 0 bytes 1 file changed, 0 insertions(+), 0 deletions(-) delete mode 100644 images/Picto+STEREOLABS_Black.jpg diff --git a/images/Picto+STEREOLABS_Black.jpg b/images/Picto+STEREOLABS_Black.jpg deleted file mode 100644 index 060e243577740d8975c813f747e4c894d477518a..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 15698 zcmeHubzD@>_xIgpfu&ix8!73fySq~mb%6zz?h=qLl_f+35hvN>sPPm_x$^OXZP;B@60)8&N*}E-aB(1r^ zfq5i&U=m>GymvtS7(Bq{&d>kKwR8-O*~ElIMTEpfKu!@cq^yXPtf(ZLsEDk%m@HBh zq=w}GOBawGAOKE)-%`)C10cWgnOMjl%mEfdLG5tBoDM9<`J*Wwc#iiEO#KI+(LtcU z<*S$;8Af#dlFe6p2K#^2jJii^&U#BgM{yKnMs3h>3{lNJ!|! z*qPYH{@3ZW6+plN4hU#02LMGta1fBwPJk8E6Bq31v(EmLAW$3_E*?IdfRG3zs3$-3 z6b=-IgNqA;fz;t(Jpe=CQm~7v;8B{O@Hu>_#G=v);GC*8ZPcbin_S}dzR?7PG_-W| z4BQuZc=`B|5|UEVGO}vw8k$<#I=W`&7M51lmuwsy(N4}T7*{|4fWV;OkkFXexcG#` zq~!FBJDGR0?q%l`78O5wTvA$AUi+-BzM=7XQ*(PqXIFPmZ(slL$moZUW1q$+W@hK+ zzbz~-Ew5~C|Jd2x+dueucxKm`oqwg@hW!V-5TIR97z_u7e`Xg18U!9V2pBHAC?181 z2|mh)l0z&CPNkYwP}4@hDQ>z+ZSOloNW+Dk;odqk?cB2eJHw*?pDg>^uz%S#0TAOr zz|O-#07^jpmhh}rqOkn6*MyrfNGlfmu8Tv*`>D6q&5ro-&31Pd<-=fGb_T}TWoLjv$Ln#&^!+z( z(kLyxxO*v@RBZHSgLwI*H)gJODcJUeu7A6I{Gb@o=Cu$5ch7iEIzLuv)yqm$DQ_24 z`9(NpgRS8Oxs&C+NJ390U$W3cJBj;Pf?%kEQCAk>eqi;&@?6^wR(af3=3pwwsL541 z>w{;r*v=QL6<8AO=D-I;9(4TWue*A=@3&BuZ4vdH0whNoxDRhbT59e=ZC%r?u`HTkeJzNrzLMYA1Awn<|)Fmr`u2AgyP@zTOgD5eZ;4ifq`iUwsVeFD6Jl zOZ=;quQt_9gJ!lmV{0T{dsyTq)Imlm4sbEKDnGDA+w%|+rX`UOjd=0|)kon0tPc=> zR{`=`BkJC1b+%BSFkZI3auDq}>c1HNgs!4M>GKr8AFfmhgcJB`*WXJa?LM*i@pbJK zc(Rg{bu$3IQ7`bO*!E)ifSfeG_dL{X5=lk@y0jhh1oQ1>_N;qF`200ET1o8+(Jz@B zy>n$Z#;_;v>mWO?sLW{_?;eKV?phy>7PuZfamlWkzqC&&HJ@Oq>cJ^6b1h8uAY+!X z;woO6gM+7dS{-?lw6ha_MoP>nP^JGtUTw>Hrt{<2;d=kq@kp4T0z znC#UYk@;hw?UjSstAm>INxK!REShC6Z%aOBXA2H<93YoeU@AlqMHBFiuzPt7_C3z!r}wvLJ7h}|tB}0)xLD(c)NY4+?i7eIlss#RP3M8h7WYv| z$z`>0LbbB-xF2D!DKr%5NT*UXd~Yy(mnRGHP$=xoQWUVSAgHk$#@k6vDPqPd-hot3 zS2}4Obj?{m4Y=SYZBCc2^Mir-);erDf_iM3l|o+lsasH}<-^h^wG)r`voMH8X*t`1 zdF(*)&mUK?ZGHzFD{2@GMGUpz5h;BzJs zPKAo4hoq}~v%-X!!)BqOAotqLlT}3T5vkjv7xO{?ui2BPk|jmv5wzrxj6W&fop;!~K^R(BK8lk(RKd$xFH z?jG}crEBO-3eI8dA+3A;=}HX14`JxR*X|tr*o*E+EW{|pso;T-DW7&0SNb>(UirZ( zV7|48t$X1nPz8LQIL`Bua7_7PsQ~9p+c>mL=uo_N2>TfM zqS8gSg(nA-bM6y;^~3Ub!iwV;-<)K8BYC$}ULw=Cnl>CM`wB{;JF*x0yy3(8V&lsM zgBQ2DWT^s-FAJ$GexGgH7;pH(UhLK=ho3vrLm%-=H64G!AZQ}pjbQVp*~!-&zIwIC z`XhDP2o>HQI+hq5g?HXxp9LM=ocMYiUUCwp%<#32dus0#AT3DdP`!M$hw1srl9ks& z;d7&e2=Og!LKwRjx+s_-Ty^k8uh1|b{7%LI+qJopnJYtEwC(G&rvU8hvWdqp9jI-1 z56;^w$^-5@M{Yf@>BCGDT0i<7*mha&)2zA)gm~86bxD#zqsFS(ur>g;Cv*`jA5`!- zJ7DwV@Do?NMW*04Cm2)?8#>yIwH`VJoQUols1n_mE1gHsEtsZoxe|4N@m=1l#-oq@9Mn?d%*!iC9qp zwN_CcxXnG=Y&Zo>H^-7ME=FIt|Mud|rou2F_-fS#&?Yuf@Zkk`32F8c5qmG|e(%w0_Hk~@yw7OMD)`8A zw%Ij`yMlPWiS6i)KNJ=^S$p+%IrmO)d)YoAUignIh)ryFj&roUN>F(DlXa?(Urc}U z(k*%`77a%#HkXG!xr@L^d6oZE8egSk#L7eOi@u)@=A>SDxH%X>dJ5#(WE~%{9$Cm) zZ+Vn-ktMKj*KbZqIR&#{F6xFv1bG0IxpACZ`J`4x2PF=uvM)Aaa+{RzYpccTcsnAd zA}7h2NR1vjg{f~1+^YYIfBDm@BH=^*S`4RCbMQO_eogrev2XAZ<}3bd?%<%d3BfNJ zl|9_W>)z;Js;XCmTN!PcqD2o!zfzuX_sms7=d?GwZoligMc4GYIk-PggOiRq{`x~* zU?T!1szl_`CPU;S!tzxjB%!Us+EY>dt9(8s^zs{)8?RmpZ#&p!wUSX|MuPnhF4N;Z zBGU8K{CcM+b5Y5%TcGbqpz6wj!>q(YBgUC~)G}Rv->1X`68_`{aWe3W@HJ+0`lRpp zu~S3sPxkA5bBI$x8#b%;+@xroDR)?_qe6&AEb)Xjo-fc4m0)0t_^2s7!Sz{-jN z*KFyhiu1%|38!8i>`teZh(@(dR9 z*1yz=2rE)UCBOaZkQiPDUqJUVD(zNjavTLp?X(@!0~zm8j(J8bDSiNlDd1l**+T1j z!bP+0Rf06407Ra=pJ~9_>Ub`F^k`U&l} zryWL{!tr&LPOBER!-56PO$O=IMp^WI?K||+?-g+U&5dEScQ-%V=#X@ZzM~{H)gUID zc3Ws+aG9{Hx;Y*H)!ZIw3VHvWf< zb_OUn?*6jQ0m3Y%(6k)he7GVxhQTq^^m9*%jo^Fyjk~yHr$D%a#EQR(GLvxUx`5%! z&7+LZ);0rTeB)fz6=KH(^we*T=!gA>+&#eu)VktuOQ08bP|9`5GtE}FBkjF4Rm`r~ zodWQZ1f`{`!!4VnN}He(N;6cOi^l-6>IesWBv8KTnN_m<(9M>J$X5B69-Q5JKVUWL z9q7HBBl}@w@2&tQ!?5m>V_H}rjFA4MglVkgz)WwXGK@EM@{>&mRn5s3XPm`V&fG{D zQ1(I?q!`nn$klRp#IUY0m6Oe-z+5hfR1Dt`pXI}|ncK9wbswI7YZ$p$kZzsr7(8tu zLS9#-ZqV404^1F0xRrwwT$zMK77u@E&ZH}vM46wAC zXaHto?v=tjNi#3&;}-;r*K$Nwe=_`>)EKI3Qe;=C%kjA1N`^oZS~a@cC-dSg%Sp30 z)Fm?p7bSC>)N>k_D@K~{m?+~?nJBZKj)N;9#6d3p{@${}!k&IYC`T`Qw2*_Bhj1{; zTUb;`L>Q1)3id`hxS{>o?9t8`PX&&R7cCrY7)J#TD+vP;18+673r0KC7i|`5Xzmc| z<{;z9p`^$rA1oW};q8I;N3jKaxO@7^1}ku!3zr4)S+Ot&+c}B9n*xV5xFDkD<%?!R z3L%9=1i^)wKrs$bhOeWOtf{)@9|_=@0>>X!gMxyDg2aWqe4T|wWn^T8MZ|=~!~{VK zLB9}Be^jubryu8E64cRt4!#&~e~gzW+nGd^y;p$00teXsZ^b;k4GjJk{9pRO!{bcb zb80_-jX;q3fAW~0d5AY!*c9#O72xZD)(Av<`g8up?&$D0uXlj2`?&#*4#H@6vL48jV#LH>?%~?@IwWn9Xb9KI=G-bozdWU0{iqg8Roxmv81@8 zgrkFly`YpR3UsW!h@&7%OxjUU%3f4L%s~P)K}6~=em^fK{~(kvTE!XcZ%|XPx6cv z>o0O}%$~u&Zvl#I|4#q*z`s54Zx8(21ON8Gzdi8(p9lUmi9maTZ#qHX6ybChTCJm| zcGcL_NK;2&0}Nz>LCAb}2M<3O7#8*L^!GK@Qe^{UYHavRV04xoj7PEp6etHjZxv%> z{l6l6|2g0Lcow7tfDyqnS^v5Ie=H$!1Sbn%M34=nQg-n6^#}1a5K9L6d!ONZAf^Ie z*c?Ip1jHA8K?Onl>8#xTH(oo#&gb|nObk%@nwzSDww=X?*_{7@?f-!tTzox18YD== z<>=uF%7+LjWL)1%QUm zf0fy10YGB{7|0&?M){)7+c}F@L!H2Y{dNfekX!-)%0U1iwEgWjujF*bI@1% z?*Je>2LLWOgR(FGH+G*z{r~pc|48%4f9HX3TpSn}VFlx>FkD>vKA|fD0 zkWo+|$Pfrh8YWswDh6r<0Ryzvm$wdwBcZJ<1Kq%kr;E|G(--o}Lhz?$FGd^t4?Q-KD1&nvLTNZ1-*7UO5UQzg zFhKCKDdU1DtoGcV4aSn=;ZFCF+&aW`h{tL4K9GRiGjJ8^V(Z7b*(!g7Hr}1t$rqG> zkK-);0%?*nffG?^9-6cl`ji+(lgHw#SMLdG zljcEo2DxZbk2;MfgnQwUYNe4O@SrSvuE6MTTW_s!)Epyw3s{|X;Hs=(7xIG} z7jVw^F5uuo&N~w9KpZ>(LQYL1fuNMcXA`Fo5fxKb0Xr2A-cKR8&|$eU4O(&kkKae- zSd;zNqH03?bJ-hrcwOtw^Eyr#Tas*X=HKi;zf)Mki)mO=sv*$y9;Y&}QB?JQC~5ir zGtD(wb%S@qH(oLE-cpVE_$FLRJV5OgqHueC<~vQyXIcLS!wPnfu(JLaLiWq9Kc7lF z;iPNeSSaXf_gxn;T5bD&7l+@!&*On#TFk#C+!?8<&U6~yo?`!86M{sDnA@26KgwEcx&T7p}?EM!-kX%oeL<_83uHy(h3`I=t`{{`l@W4G^&X+9P44=*K5O~$g2%+IHop3i%isW) z^s~HY6$s}5aoDK4_{C!rr}5FYGD3>U%`@^Gzt9Z9ji*ay+j&8^E3+}au`B{l3fPFE z-Q0D9HfIqo#X`Mz?TntG1uEbReV3}x_8)jh?<}SJNOl*$CD895<|_@S$+<9E^P#N6 z_-&`Nkni)A;6M)*-$n|aa0PYyl+E`ZV#2Q}*?;m#TI#-Iuv2#Xc>R;2SaYrQg<30~ zLT2lcBfnkfmsO2yv}yy(9$8LSkreCBMELP>T>3Nh=){!!_iZ>eW0af&7w#ODWR0?y zvuxSv>wHS^9>3aacckc^ zr@JBkZk>4|fkqm)W^ck!%q;hc(DlLBFJvEC-jr3>(Rr7R;8ae}TY6vi(@2Q0>3*bd za@nICqZwNh>9MIjbSM+@*~k*ZqBMv8$ecfJJ+b z+Rr?Mxo|=MGYTff!B6IW8MLwUBZtP}Vj$%2E1yrX(Kh=jV!(?!;oIq9@g0n*AX zM|MXhVkA8mQ>eyPF`XOUTSCFu-7&iW;oY4NQ@_7}C@;t1YC{noYYEYEd73};CVIw9 z-y)jMKdqeUI!-sSk{@_oR?-{Ky-WOHWuepvj(%#y*(=gNlQ-ggpo8~(B?w`{DR8z) z=zI?l{y(VB_6Sh`xRmV1*Syckp=ac)z8_VHtk+vuZSUkH)#IQn2Z0|OAiv7_-Y5u^ z01n56oj+4SesFITxHXELl8Qr2#RQdx%c*MW;}=!KhP3xBsBIgf=JHR^5D`~1Q=ZxU zbLYWRf--#(n>i#n%o(no(!Q~pAiYW5L!_6v0j|i$!Jhm1k$+8p4>Ql$4Q-ZeZmM9)DXBfvNFP^ zu^jZF)5=@kM3P=2aQDM*F?CIiAtX<-!kddUBoVo=VP|C&^jQ z*7PUsX?{3*)UO?WrG=DPiq?ho1EbN5jmAR8-e58rBc9pw7^8gyo9jD2kBM^z_mg|p zpkMh18>5USaU%j!effV(DfK@&F4@i*3GgomRk%So#5^0n=54juakHXWb2sQA;Yk0^ z@jORts&+d2s##9X_yv({T818r(7o}!?i;Qw=J7rE>40l{9tVTZYGXUK7vYT$u@*Yr zUr>)^c#GIfwsU!Hq*#JjlGYzJ7dZ9OsF1!|E3mKN2%E1VF#;)NjKNZ(w zx)mbcz-6|yzv)x+KnR5{Ev_BVQ`UCSKfSzeV%PLICAy++kQoO~-ox0|Vr zJWid7;Vg&tQq&bj5f7E}!?KB}la=;Gml^cRO2|3aO-i_uuiq=NjWu|JoQ}HM!5wb$ z{B6jN5&FvYgrBAZIG8lQm~qwF?ZqVFM#BdNTThL-DBC`^UaP!8)D*5pANw%VEy1x? zGEU*a?uBe*+$0yaAu0N&-PVv}x10QQLESr658ot5^F-nm9<)-%(L;hX$Av7;8`V~u zzwXmZkG{XP_SIC+&uPLl6Eq_c8-`D~t@s@z=go5QrpsI_ zDX-*?$WSdlFx8yQxkiQsR89e|0;7SQq5!0Uut@pxN>$mwEI(Ds=egiX8PEQg*b3d8 z<_}A$MeUUwCRilFULHZ+8U?WHzIcNYOOlD%7+KsQJ~4|2Y^#{|HYZhiJ%k9^hNV2q zIOhHQ2A^(O-iWAD=r}4L=9zfuWYpmWOO@_#&XjO%UB7fELSg}e;Rp}aveW4*Y;G}N zczJy5<~}3RF`iCF?}55@?#gwHm1y%a$IH&pN2_Vg7H;21elUnkb=I=4Nbtt#`P|Nn zm#1u#DLL?IY(;g$6Cm zxfD~{-Zf?`6P%61 zJ{o>1W)Noi=Xjk?5vz|iriIY&(Kg;59p2W0z%~RkJe6bG=FY5Jh!o38(+hvN4WA_G+VG?@B<&eC@|~|-DeSSLmy!eFto23X#ry{m z6N<(!48%k_U4x<~)shZ7<;+^@%*ut7){<4MlZWXh>(X7)%El+fbb|!7VBvJ5yoWa5 z8k04d-6KC{5QqA`Fc79B_F0UG>w$+<7Notn1SuSx>|&&zumGmVE+XVex>6Z9^nx!t z4=cOymm13^04TIOqs#!6w61Kt*-M0OnHR;=AQ%;~hz7k}e<_o^fz&|2ETLGM{&`(}6qh6^V?y!W+uCC6 zF8EEMLt%{f(tW0H6aHeq7^9u^rK*Emg2pgy;=xob=dR(HyI|%O-$#0QVT||ZME4np zo%}Y`^Y4gXJ>@{N)YX-6)D=jpd*6+jmH0U^; zSpAb}TEyD@stO*387Q?P0HI1Qf}Z#ABE196AcXp@EJXKfr`MD+xz`&5sO_(%?rG4@ z+`EX43uUCY=zLE%1!>~+v>DwCfmKN* z2L!3Y>ct(^+|;Zq*-2E~Z_1r64rTHQ;4O{w)Q7zbh{KADcWDqQJN6tf~B4RnVH5N(!gkzJ+<0`i#nZgWx|Bs%O5nhEt>L;7xj8Q5&r#Ui4}{ zzMpxw$J?gNKI^0BBdLhl2?~T)+oe+J%dhTWOl<2HcFs(8*O!s(%%6y@uI&zZD$mwL zH8fB?_I6CYE>hwY=!nvjm*rNI;Spf%D|IQIrBjz1&<%}E`qsDhrD|aOM{+F&UkcOn z2#9T<3aE7&+Z~B?Zw{MjD$$g>XKst2o+8T&BQ_CUbr(NfpG1FFf{h>4Vt z|1$OEWcop-eP{0yGm6jZ*BM~17$}aV6#-mv4RGsTMhKx}@vk$h3?LjHE&})$;C4}h z#*p5uncu|{ zDoyZy$D}oHDGzUykQYy%`JkkututHdkxJs-%QXxZ@pEd8$B%CZuJeYJ`9nP3#(c*T z@(fXv_vzSphjL#rVJvUmSD&Z(<$A-$Y$iHQ@xgMNn6SwkVPwvacSJD{JYaVR$o$>niz;nYjzDY`GP_b^JN@(REjp{8p6Rl zZZA~f?N|6M=`UpRP{72Ua_BiNDq^*(U&tpJ(+Bi4vA<$ofEbO2yT%Z8Gd+JK5@YXOmOB-aFwrNbj~(7im&54DR}Cv^F3H`|R*9rDCfA{O&H5pXwfsQI9BM}M z=AK32OZBCXntmsQo_WT%b}GKz`}UFd(}A{3_b1~AoOm>&d@40KS~l*e>-qA2#`C0= zb`;ml7)Z@)dK`Y<0Pnx&e~CC>(j*56xYBk0e4}-^QdIy76(lAz&J|~#9tH1vw0`^$0dh} zkn^c0+wZC8Kjx?qb_8G|sshr3f2R3+?nyrBzz*#@ZuBRvDdCQ^w@5EQ<#WDQ1RJ|R z4mJjbz`z+NDEoI~zn@T1ej$UEFUtRevN>ckjF$yt9NiIr z;pKEQtu&g9a8INxMTKE3od#d|Kta}hDY3lllQ5omp@Kqw`mP#`NY8MNa=$``F zpAYsgelGxIpI%^5hUX?LJ-LE_F0S0LzQFYur=5rAeP-Dk6@7>Hbh7HUEZ6EyI8Rq6 z1i_p%Hlv(fJ@C@xu&XrjdVtOb(el?|H4&ZT6ql78T~WFkCXbg?Nujxw+gCd1qxw{2 zki!EG85gmH8F94hJcO;WQ`#K6D*X7Q=w6$n-WBG@pAjz?R(127V5BiTAppG&t9NN$ zHD=e~m}hIV^-99f80DkZU8cMR!YkO207hNSN?N)?PQTRSM4Vw^aq_0&Di56I^4uRj zj5m{h=?q*;_`>bO=;FwtRpZ&tTgs^#&`m=Tw-8?!FJf^k{9C=HMam-GwW=N{;ir7Y ziErLQtDpUm)FH{Ok=`pUPpQ7TIMi5dUa*;wHaymHoVIsV<57Vxm5s1iJLVR(wJJbD z?oIaE`*Fu*xyJu&8hS_&mR^N=7*hQcr}$F6ZvZTaBSV9iCh1_`I^uEXtK5OW&p__?mLubFh>f0ci?Mp|gZm$OZK$5M4nW5T2Qa~Q8tjf-dVOad$EYazkcTz?zA&+xG`hsz0ol#eBITQ#9cBU$=Md5ujR_CgM-mcqJX2<1 zS{bfpH+~8*o@`${`Kc`ti2Z0xemJDD_pnI=8PYx4-?n0*xMcWkg? zX4j>ngI}+t{822_bK$acmr;3KQo*D_X^7^CFOze@D_U1|lo&AcS&^5*WM3;v3&Q)s zwJqOR`3ELSF4ueSI-A7@IO2Fd0N0RImpR-^XSRLtgXB+vMFHNn#dV9D-)BBoq|!@_ zBQs(}t|6H=R8kM{?{D^`xmwx14~v$*6a|#Je?|n=Kd+4T8CIskdBwR%EYSPW_BuCZ z)0Wm~ujVUQFleG&a@^h#)mj6j@rBn`XczyS-#sW{mU@!JmdW)CO;oykEKJ!w?SnBs ziHucmTk~@-T7-KfWvWPvkz44E8O=VfknC2*Rj~A@rC8yE58oqtyPD$F$}PNf*mEoO z3f)!Yf#vvTrOk_>t@mg=0u{A3gRHG+bZ_1ccu67f!3ut#c|fz1mjh zsgtW+#c~Bthzr(8BC6F9MI8^40jT);wGi)njQ-iLI9aglr2LG@-T|Epci3Sj?VJyc zRLut87d*vAHUqEq8H(|7(q^kVDDKE90~jOb2=z8dD?CUW8US_2uC?g9p%5 z!+So`zc<%WnZc*N?!$Y9iKOW1MDw$J8HJvN#Fq7&BC~5SO5Fr!PW|kW`Z}q$2A~6? zw$6f=sembeu3^eFaOrVqiVjBfVU&P)dSpb~91BEJXf#XmU5El~#llyFRpp0xG~mc( zG!h>tTJSj5MHVY}XJATKtvhRlg&0b0LCdGx!`r@j*{&~|yz&9sV&zo~yn7-pFstUX z8|Gtz;mkFRnsLUiCSCH0F=Ztr1e+8;lUOfx19wx`cL@fF$a=(k2lI17O%r;zzU{fU zaN6GwbxJjIAMWcn4SdR%J0oP#>NZ=!xmp1d=blFnr)XPdB{sO~EZM-LYkk2J4?~-!o za^BDjCgNd-zM)U6wDnwMBV~Kf&3)JVjE6{Bt})Ew2JeZSu-=WO-n~eOlJRkhC~g;b zGTpz{!WA`0QVCa+F#i13K1!G2fU`M@^%_-Me=0BIQa1UhwH%~~g^7Gb(gvsc^VhpP z(^_klGSV|P13>oiHdiUp#yk!kxgm@%vh_{?K2;l^n1~#to5E3Dm+Oe=q0&2Q3#L?Y z&6>+}OwPtytXHA-im@Htnk!A$h!SO{lVz#(+oJB`ergY&NFyc*U#2u j2lnVRi8{X3Z}GLU$B}0ACMB!e`hw3QcL#B{Psjfcl7xaY From 049f72c71c0d2300ec04211adb729356e9c75030 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Tue, 16 Jul 2024 08:37:33 +0000 Subject: [PATCH 197/199] Add files via upload --- images/Picto+STEREOLABS_Black.jpg | Bin 0 -> 15698 bytes 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100644 images/Picto+STEREOLABS_Black.jpg diff --git a/images/Picto+STEREOLABS_Black.jpg b/images/Picto+STEREOLABS_Black.jpg new file mode 100644 index 0000000000000000000000000000000000000000..060e243577740d8975c813f747e4c894d477518a GIT binary patch literal 15698 zcmeHubzD@>_xIgpfu&ix8!73fySq~mb%6zz?h=qLl_f+35hvN>sPPm_x$^OXZP;B@60)8&N*}E-aB(1r^ zfq5i&U=m>GymvtS7(Bq{&d>kKwR8-O*~ElIMTEpfKu!@cq^yXPtf(ZLsEDk%m@HBh zq=w}GOBawGAOKE)-%`)C10cWgnOMjl%mEfdLG5tBoDM9<`J*Wwc#iiEO#KI+(LtcU z<*S$;8Af#dlFe6p2K#^2jJii^&U#BgM{yKnMs3h>3{lNJ!|! z*qPYH{@3ZW6+plN4hU#02LMGta1fBwPJk8E6Bq31v(EmLAW$3_E*?IdfRG3zs3$-3 z6b=-IgNqA;fz;t(Jpe=CQm~7v;8B{O@Hu>_#G=v);GC*8ZPcbin_S}dzR?7PG_-W| z4BQuZc=`B|5|UEVGO}vw8k$<#I=W`&7M51lmuwsy(N4}T7*{|4fWV;OkkFXexcG#` zq~!FBJDGR0?q%l`78O5wTvA$AUi+-BzM=7XQ*(PqXIFPmZ(slL$moZUW1q$+W@hK+ zzbz~-Ew5~C|Jd2x+dueucxKm`oqwg@hW!V-5TIR97z_u7e`Xg18U!9V2pBHAC?181 z2|mh)l0z&CPNkYwP}4@hDQ>z+ZSOloNW+Dk;odqk?cB2eJHw*?pDg>^uz%S#0TAOr zz|O-#07^jpmhh}rqOkn6*MyrfNGlfmu8Tv*`>D6q&5ro-&31Pd<-=fGb_T}TWoLjv$Ln#&^!+z( z(kLyxxO*v@RBZHSgLwI*H)gJODcJUeu7A6I{Gb@o=Cu$5ch7iEIzLuv)yqm$DQ_24 z`9(NpgRS8Oxs&C+NJ390U$W3cJBj;Pf?%kEQCAk>eqi;&@?6^wR(af3=3pwwsL541 z>w{;r*v=QL6<8AO=D-I;9(4TWue*A=@3&BuZ4vdH0whNoxDRhbT59e=ZC%r?u`HTkeJzNrzLMYA1Awn<|)Fmr`u2AgyP@zTOgD5eZ;4ifq`iUwsVeFD6Jl zOZ=;quQt_9gJ!lmV{0T{dsyTq)Imlm4sbEKDnGDA+w%|+rX`UOjd=0|)kon0tPc=> zR{`=`BkJC1b+%BSFkZI3auDq}>c1HNgs!4M>GKr8AFfmhgcJB`*WXJa?LM*i@pbJK zc(Rg{bu$3IQ7`bO*!E)ifSfeG_dL{X5=lk@y0jhh1oQ1>_N;qF`200ET1o8+(Jz@B zy>n$Z#;_;v>mWO?sLW{_?;eKV?phy>7PuZfamlWkzqC&&HJ@Oq>cJ^6b1h8uAY+!X z;woO6gM+7dS{-?lw6ha_MoP>nP^JGtUTw>Hrt{<2;d=kq@kp4T0z znC#UYk@;hw?UjSstAm>INxK!REShC6Z%aOBXA2H<93YoeU@AlqMHBFiuzPt7_C3z!r}wvLJ7h}|tB}0)xLD(c)NY4+?i7eIlss#RP3M8h7WYv| z$z`>0LbbB-xF2D!DKr%5NT*UXd~Yy(mnRGHP$=xoQWUVSAgHk$#@k6vDPqPd-hot3 zS2}4Obj?{m4Y=SYZBCc2^Mir-);erDf_iM3l|o+lsasH}<-^h^wG)r`voMH8X*t`1 zdF(*)&mUK?ZGHzFD{2@GMGUpz5h;BzJs zPKAo4hoq}~v%-X!!)BqOAotqLlT}3T5vkjv7xO{?ui2BPk|jmv5wzrxj6W&fop;!~K^R(BK8lk(RKd$xFH z?jG}crEBO-3eI8dA+3A;=}HX14`JxR*X|tr*o*E+EW{|pso;T-DW7&0SNb>(UirZ( zV7|48t$X1nPz8LQIL`Bua7_7PsQ~9p+c>mL=uo_N2>TfM zqS8gSg(nA-bM6y;^~3Ub!iwV;-<)K8BYC$}ULw=Cnl>CM`wB{;JF*x0yy3(8V&lsM zgBQ2DWT^s-FAJ$GexGgH7;pH(UhLK=ho3vrLm%-=H64G!AZQ}pjbQVp*~!-&zIwIC z`XhDP2o>HQI+hq5g?HXxp9LM=ocMYiUUCwp%<#32dus0#AT3DdP`!M$hw1srl9ks& z;d7&e2=Og!LKwRjx+s_-Ty^k8uh1|b{7%LI+qJopnJYtEwC(G&rvU8hvWdqp9jI-1 z56;^w$^-5@M{Yf@>BCGDT0i<7*mha&)2zA)gm~86bxD#zqsFS(ur>g;Cv*`jA5`!- zJ7DwV@Do?NMW*04Cm2)?8#>yIwH`VJoQUols1n_mE1gHsEtsZoxe|4N@m=1l#-oq@9Mn?d%*!iC9qp zwN_CcxXnG=Y&Zo>H^-7ME=FIt|Mud|rou2F_-fS#&?Yuf@Zkk`32F8c5qmG|e(%w0_Hk~@yw7OMD)`8A zw%Ij`yMlPWiS6i)KNJ=^S$p+%IrmO)d)YoAUignIh)ryFj&roUN>F(DlXa?(Urc}U z(k*%`77a%#HkXG!xr@L^d6oZE8egSk#L7eOi@u)@=A>SDxH%X>dJ5#(WE~%{9$Cm) zZ+Vn-ktMKj*KbZqIR&#{F6xFv1bG0IxpACZ`J`4x2PF=uvM)Aaa+{RzYpccTcsnAd zA}7h2NR1vjg{f~1+^YYIfBDm@BH=^*S`4RCbMQO_eogrev2XAZ<}3bd?%<%d3BfNJ zl|9_W>)z;Js;XCmTN!PcqD2o!zfzuX_sms7=d?GwZoligMc4GYIk-PggOiRq{`x~* zU?T!1szl_`CPU;S!tzxjB%!Us+EY>dt9(8s^zs{)8?RmpZ#&p!wUSX|MuPnhF4N;Z zBGU8K{CcM+b5Y5%TcGbqpz6wj!>q(YBgUC~)G}Rv->1X`68_`{aWe3W@HJ+0`lRpp zu~S3sPxkA5bBI$x8#b%;+@xroDR)?_qe6&AEb)Xjo-fc4m0)0t_^2s7!Sz{-jN z*KFyhiu1%|38!8i>`teZh(@(dR9 z*1yz=2rE)UCBOaZkQiPDUqJUVD(zNjavTLp?X(@!0~zm8j(J8bDSiNlDd1l**+T1j z!bP+0Rf06407Ra=pJ~9_>Ub`F^k`U&l} zryWL{!tr&LPOBER!-56PO$O=IMp^WI?K||+?-g+U&5dEScQ-%V=#X@ZzM~{H)gUID zc3Ws+aG9{Hx;Y*H)!ZIw3VHvWf< zb_OUn?*6jQ0m3Y%(6k)he7GVxhQTq^^m9*%jo^Fyjk~yHr$D%a#EQR(GLvxUx`5%! z&7+LZ);0rTeB)fz6=KH(^we*T=!gA>+&#eu)VktuOQ08bP|9`5GtE}FBkjF4Rm`r~ zodWQZ1f`{`!!4VnN}He(N;6cOi^l-6>IesWBv8KTnN_m<(9M>J$X5B69-Q5JKVUWL z9q7HBBl}@w@2&tQ!?5m>V_H}rjFA4MglVkgz)WwXGK@EM@{>&mRn5s3XPm`V&fG{D zQ1(I?q!`nn$klRp#IUY0m6Oe-z+5hfR1Dt`pXI}|ncK9wbswI7YZ$p$kZzsr7(8tu zLS9#-ZqV404^1F0xRrwwT$zMK77u@E&ZH}vM46wAC zXaHto?v=tjNi#3&;}-;r*K$Nwe=_`>)EKI3Qe;=C%kjA1N`^oZS~a@cC-dSg%Sp30 z)Fm?p7bSC>)N>k_D@K~{m?+~?nJBZKj)N;9#6d3p{@${}!k&IYC`T`Qw2*_Bhj1{; zTUb;`L>Q1)3id`hxS{>o?9t8`PX&&R7cCrY7)J#TD+vP;18+673r0KC7i|`5Xzmc| z<{;z9p`^$rA1oW};q8I;N3jKaxO@7^1}ku!3zr4)S+Ot&+c}B9n*xV5xFDkD<%?!R z3L%9=1i^)wKrs$bhOeWOtf{)@9|_=@0>>X!gMxyDg2aWqe4T|wWn^T8MZ|=~!~{VK zLB9}Be^jubryu8E64cRt4!#&~e~gzW+nGd^y;p$00teXsZ^b;k4GjJk{9pRO!{bcb zb80_-jX;q3fAW~0d5AY!*c9#O72xZD)(Av<`g8up?&$D0uXlj2`?&#*4#H@6vL48jV#LH>?%~?@IwWn9Xb9KI=G-bozdWU0{iqg8Roxmv81@8 zgrkFly`YpR3UsW!h@&7%OxjUU%3f4L%s~P)K}6~=em^fK{~(kvTE!XcZ%|XPx6cv z>o0O}%$~u&Zvl#I|4#q*z`s54Zx8(21ON8Gzdi8(p9lUmi9maTZ#qHX6ybChTCJm| zcGcL_NK;2&0}Nz>LCAb}2M<3O7#8*L^!GK@Qe^{UYHavRV04xoj7PEp6etHjZxv%> z{l6l6|2g0Lcow7tfDyqnS^v5Ie=H$!1Sbn%M34=nQg-n6^#}1a5K9L6d!ONZAf^Ie z*c?Ip1jHA8K?Onl>8#xTH(oo#&gb|nObk%@nwzSDww=X?*_{7@?f-!tTzox18YD== z<>=uF%7+LjWL)1%QUm zf0fy10YGB{7|0&?M){)7+c}F@L!H2Y{dNfekX!-)%0U1iwEgWjujF*bI@1% z?*Je>2LLWOgR(FGH+G*z{r~pc|48%4f9HX3TpSn}VFlx>FkD>vKA|fD0 zkWo+|$Pfrh8YWswDh6r<0Ryzvm$wdwBcZJ<1Kq%kr;E|G(--o}Lhz?$FGd^t4?Q-KD1&nvLTNZ1-*7UO5UQzg zFhKCKDdU1DtoGcV4aSn=;ZFCF+&aW`h{tL4K9GRiGjJ8^V(Z7b*(!g7Hr}1t$rqG> zkK-);0%?*nffG?^9-6cl`ji+(lgHw#SMLdG zljcEo2DxZbk2;MfgnQwUYNe4O@SrSvuE6MTTW_s!)Epyw3s{|X;Hs=(7xIG} z7jVw^F5uuo&N~w9KpZ>(LQYL1fuNMcXA`Fo5fxKb0Xr2A-cKR8&|$eU4O(&kkKae- zSd;zNqH03?bJ-hrcwOtw^Eyr#Tas*X=HKi;zf)Mki)mO=sv*$y9;Y&}QB?JQC~5ir zGtD(wb%S@qH(oLE-cpVE_$FLRJV5OgqHueC<~vQyXIcLS!wPnfu(JLaLiWq9Kc7lF z;iPNeSSaXf_gxn;T5bD&7l+@!&*On#TFk#C+!?8<&U6~yo?`!86M{sDnA@26KgwEcx&T7p}?EM!-kX%oeL<_83uHy(h3`I=t`{{`l@W4G^&X+9P44=*K5O~$g2%+IHop3i%isW) z^s~HY6$s}5aoDK4_{C!rr}5FYGD3>U%`@^Gzt9Z9ji*ay+j&8^E3+}au`B{l3fPFE z-Q0D9HfIqo#X`Mz?TntG1uEbReV3}x_8)jh?<}SJNOl*$CD895<|_@S$+<9E^P#N6 z_-&`Nkni)A;6M)*-$n|aa0PYyl+E`ZV#2Q}*?;m#TI#-Iuv2#Xc>R;2SaYrQg<30~ zLT2lcBfnkfmsO2yv}yy(9$8LSkreCBMELP>T>3Nh=){!!_iZ>eW0af&7w#ODWR0?y zvuxSv>wHS^9>3aacckc^ zr@JBkZk>4|fkqm)W^ck!%q;hc(DlLBFJvEC-jr3>(Rr7R;8ae}TY6vi(@2Q0>3*bd za@nICqZwNh>9MIjbSM+@*~k*ZqBMv8$ecfJJ+b z+Rr?Mxo|=MGYTff!B6IW8MLwUBZtP}Vj$%2E1yrX(Kh=jV!(?!;oIq9@g0n*AX zM|MXhVkA8mQ>eyPF`XOUTSCFu-7&iW;oY4NQ@_7}C@;t1YC{noYYEYEd73};CVIw9 z-y)jMKdqeUI!-sSk{@_oR?-{Ky-WOHWuepvj(%#y*(=gNlQ-ggpo8~(B?w`{DR8z) z=zI?l{y(VB_6Sh`xRmV1*Syckp=ac)z8_VHtk+vuZSUkH)#IQn2Z0|OAiv7_-Y5u^ z01n56oj+4SesFITxHXELl8Qr2#RQdx%c*MW;}=!KhP3xBsBIgf=JHR^5D`~1Q=ZxU zbLYWRf--#(n>i#n%o(no(!Q~pAiYW5L!_6v0j|i$!Jhm1k$+8p4>Ql$4Q-ZeZmM9)DXBfvNFP^ zu^jZF)5=@kM3P=2aQDM*F?CIiAtX<-!kddUBoVo=VP|C&^jQ z*7PUsX?{3*)UO?WrG=DPiq?ho1EbN5jmAR8-e58rBc9pw7^8gyo9jD2kBM^z_mg|p zpkMh18>5USaU%j!effV(DfK@&F4@i*3GgomRk%So#5^0n=54juakHXWb2sQA;Yk0^ z@jORts&+d2s##9X_yv({T818r(7o}!?i;Qw=J7rE>40l{9tVTZYGXUK7vYT$u@*Yr zUr>)^c#GIfwsU!Hq*#JjlGYzJ7dZ9OsF1!|E3mKN2%E1VF#;)NjKNZ(w zx)mbcz-6|yzv)x+KnR5{Ev_BVQ`UCSKfSzeV%PLICAy++kQoO~-ox0|Vr zJWid7;Vg&tQq&bj5f7E}!?KB}la=;Gml^cRO2|3aO-i_uuiq=NjWu|JoQ}HM!5wb$ z{B6jN5&FvYgrBAZIG8lQm~qwF?ZqVFM#BdNTThL-DBC`^UaP!8)D*5pANw%VEy1x? zGEU*a?uBe*+$0yaAu0N&-PVv}x10QQLESr658ot5^F-nm9<)-%(L;hX$Av7;8`V~u zzwXmZkG{XP_SIC+&uPLl6Eq_c8-`D~t@s@z=go5QrpsI_ zDX-*?$WSdlFx8yQxkiQsR89e|0;7SQq5!0Uut@pxN>$mwEI(Ds=egiX8PEQg*b3d8 z<_}A$MeUUwCRilFULHZ+8U?WHzIcNYOOlD%7+KsQJ~4|2Y^#{|HYZhiJ%k9^hNV2q zIOhHQ2A^(O-iWAD=r}4L=9zfuWYpmWOO@_#&XjO%UB7fELSg}e;Rp}aveW4*Y;G}N zczJy5<~}3RF`iCF?}55@?#gwHm1y%a$IH&pN2_Vg7H;21elUnkb=I=4Nbtt#`P|Nn zm#1u#DLL?IY(;g$6Cm zxfD~{-Zf?`6P%61 zJ{o>1W)Noi=Xjk?5vz|iriIY&(Kg;59p2W0z%~RkJe6bG=FY5Jh!o38(+hvN4WA_G+VG?@B<&eC@|~|-DeSSLmy!eFto23X#ry{m z6N<(!48%k_U4x<~)shZ7<;+^@%*ut7){<4MlZWXh>(X7)%El+fbb|!7VBvJ5yoWa5 z8k04d-6KC{5QqA`Fc79B_F0UG>w$+<7Notn1SuSx>|&&zumGmVE+XVex>6Z9^nx!t z4=cOymm13^04TIOqs#!6w61Kt*-M0OnHR;=AQ%;~hz7k}e<_o^fz&|2ETLGM{&`(}6qh6^V?y!W+uCC6 zF8EEMLt%{f(tW0H6aHeq7^9u^rK*Emg2pgy;=xob=dR(HyI|%O-$#0QVT||ZME4np zo%}Y`^Y4gXJ>@{N)YX-6)D=jpd*6+jmH0U^; zSpAb}TEyD@stO*387Q?P0HI1Qf}Z#ABE196AcXp@EJXKfr`MD+xz`&5sO_(%?rG4@ z+`EX43uUCY=zLE%1!>~+v>DwCfmKN* z2L!3Y>ct(^+|;Zq*-2E~Z_1r64rTHQ;4O{w)Q7zbh{KADcWDqQJN6tf~B4RnVH5N(!gkzJ+<0`i#nZgWx|Bs%O5nhEt>L;7xj8Q5&r#Ui4}{ zzMpxw$J?gNKI^0BBdLhl2?~T)+oe+J%dhTWOl<2HcFs(8*O!s(%%6y@uI&zZD$mwL zH8fB?_I6CYE>hwY=!nvjm*rNI;Spf%D|IQIrBjz1&<%}E`qsDhrD|aOM{+F&UkcOn z2#9T<3aE7&+Z~B?Zw{MjD$$g>XKst2o+8T&BQ_CUbr(NfpG1FFf{h>4Vt z|1$OEWcop-eP{0yGm6jZ*BM~17$}aV6#-mv4RGsTMhKx}@vk$h3?LjHE&})$;C4}h z#*p5uncu|{ zDoyZy$D}oHDGzUykQYy%`JkkututHdkxJs-%QXxZ@pEd8$B%CZuJeYJ`9nP3#(c*T z@(fXv_vzSphjL#rVJvUmSD&Z(<$A-$Y$iHQ@xgMNn6SwkVPwvacSJD{JYaVR$o$>niz;nYjzDY`GP_b^JN@(REjp{8p6Rl zZZA~f?N|6M=`UpRP{72Ua_BiNDq^*(U&tpJ(+Bi4vA<$ofEbO2yT%Z8Gd+JK5@YXOmOB-aFwrNbj~(7im&54DR}Cv^F3H`|R*9rDCfA{O&H5pXwfsQI9BM}M z=AK32OZBCXntmsQo_WT%b}GKz`}UFd(}A{3_b1~AoOm>&d@40KS~l*e>-qA2#`C0= zb`;ml7)Z@)dK`Y<0Pnx&e~CC>(j*56xYBk0e4}-^QdIy76(lAz&J|~#9tH1vw0`^$0dh} zkn^c0+wZC8Kjx?qb_8G|sshr3f2R3+?nyrBzz*#@ZuBRvDdCQ^w@5EQ<#WDQ1RJ|R z4mJjbz`z+NDEoI~zn@T1ej$UEFUtRevN>ckjF$yt9NiIr z;pKEQtu&g9a8INxMTKE3od#d|Kta}hDY3lllQ5omp@Kqw`mP#`NY8MNa=$``F zpAYsgelGxIpI%^5hUX?LJ-LE_F0S0LzQFYur=5rAeP-Dk6@7>Hbh7HUEZ6EyI8Rq6 z1i_p%Hlv(fJ@C@xu&XrjdVtOb(el?|H4&ZT6ql78T~WFkCXbg?Nujxw+gCd1qxw{2 zki!EG85gmH8F94hJcO;WQ`#K6D*X7Q=w6$n-WBG@pAjz?R(127V5BiTAppG&t9NN$ zHD=e~m}hIV^-99f80DkZU8cMR!YkO207hNSN?N)?PQTRSM4Vw^aq_0&Di56I^4uRj zj5m{h=?q*;_`>bO=;FwtRpZ&tTgs^#&`m=Tw-8?!FJf^k{9C=HMam-GwW=N{;ir7Y ziErLQtDpUm)FH{Ok=`pUPpQ7TIMi5dUa*;wHaymHoVIsV<57Vxm5s1iJLVR(wJJbD z?oIaE`*Fu*xyJu&8hS_&mR^N=7*hQcr}$F6ZvZTaBSV9iCh1_`I^uEXtK5OW&p__?mLubFh>f0ci?Mp|gZm$OZK$5M4nW5T2Qa~Q8tjf-dVOad$EYazkcTz?zA&+xG`hsz0ol#eBITQ#9cBU$=Md5ujR_CgM-mcqJX2<1 zS{bfpH+~8*o@`${`Kc`ti2Z0xemJDD_pnI=8PYx4-?n0*xMcWkg? zX4j>ngI}+t{822_bK$acmr;3KQo*D_X^7^CFOze@D_U1|lo&AcS&^5*WM3;v3&Q)s zwJqOR`3ELSF4ueSI-A7@IO2Fd0N0RImpR-^XSRLtgXB+vMFHNn#dV9D-)BBoq|!@_ zBQs(}t|6H=R8kM{?{D^`xmwx14~v$*6a|#Je?|n=Kd+4T8CIskdBwR%EYSPW_BuCZ z)0Wm~ujVUQFleG&a@^h#)mj6j@rBn`XczyS-#sW{mU@!JmdW)CO;oykEKJ!w?SnBs ziHucmTk~@-T7-KfWvWPvkz44E8O=VfknC2*Rj~A@rC8yE58oqtyPD$F$}PNf*mEoO z3f)!Yf#vvTrOk_>t@mg=0u{A3gRHG+bZ_1ccu67f!3ut#c|fz1mjh zsgtW+#c~Bthzr(8BC6F9MI8^40jT);wGi)njQ-iLI9aglr2LG@-T|Epci3Sj?VJyc zRLut87d*vAHUqEq8H(|7(q^kVDDKE90~jOb2=z8dD?CUW8US_2uC?g9p%5 z!+So`zc<%WnZc*N?!$Y9iKOW1MDw$J8HJvN#Fq7&BC~5SO5Fr!PW|kW`Z}q$2A~6? zw$6f=sembeu3^eFaOrVqiVjBfVU&P)dSpb~91BEJXf#XmU5El~#llyFRpp0xG~mc( zG!h>tTJSj5MHVY}XJATKtvhRlg&0b0LCdGx!`r@j*{&~|yz&9sV&zo~yn7-pFstUX z8|Gtz;mkFRnsLUiCSCH0F=Ztr1e+8;lUOfx19wx`cL@fF$a=(k2lI17O%r;zzU{fU zaN6GwbxJjIAMWcn4SdR%J0oP#>NZ=!xmp1d=blFnr)XPdB{sO~EZM-LYkk2J4?~-!o za^BDjCgNd-zM)U6wDnwMBV~Kf&3)JVjE6{Bt})Ew2JeZSu-=WO-n~eOlJRkhC~g;b zGTpz{!WA`0QVCa+F#i13K1!G2fU`M@^%_-Me=0BIQa1UhwH%~~g^7Gb(gvsc^VhpP z(^_klGSV|P13>oiHdiUp#yk!kxgm@%vh_{?K2;l^n1~#to5E3Dm+Oe=q0&2Q3#L?Y z&6>+}OwPtytXHA-im@Htnk!A$h!SO{lVz#(+oJB`ergY&NFyc*U#2u j2lnVRi8{X3Z}GLU$B}0ACMB!e`hw3QcL#B{Psjfcl7xaY literal 0 HcmV?d00001 From 5f62b7b294784d798761f2bc64b6e3824c9ccd34 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Tue, 16 Jul 2024 10:47:54 +0200 Subject: [PATCH 198/199] Update README.md --- README.md | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/README.md b/README.md index f0e43cf2..bd988680 100644 --- a/README.md +++ b/README.md @@ -1,12 +1,13 @@ -![](./images/Picto+STEREOLABS_Black.jpg) +

+ Stereolabs
+ ROS Noetic Ninjemis Integration +

-# Stereolabs ZED Camera - ROS Noetic Ninjemis Integration - -This package lets you use the ZED stereo camera with ROS. It outputs the camera left and right images, depth map, point cloud, pose information and supports the use of multiple ZED cameras. +This package lets you use the ZED stereo camera with ROS. It outputs the camera's left and right images, depth map, point cloud, and pose information and supports the use of multiple ZED cameras. [More information](https://www.stereolabs.com/documentation/guides/using-zed-with-ros/introduction.html) -**Note:** The `zed_interfaces` package has been removed from this repository and moved to its own [`zed-ros-interfaces` repository](https://github.com/stereolabs/zed-ros-interfaces) for allowing better integration of the ZED Wrapper on remote ground stations that do not require the full package to be installed. To update your repository please follow the [new update instructions](https://github.com/stereolabs/zed-ros-wrapper#update-the-repository). For more information please read issue [#750](https://github.com/stereolabs/zed-ros-wrapper/issues/750). +**Note:** The `zed_interfaces` package has been removed from this repository and moved to its own [`zed-ros-interfaces` repository](https://github.com/stereolabs/zed-ros-interfaces) to allow better integration of the ZED Wrapper on remote ground stations that do not require the full package to be installed. To update your repository please follow the [new update instructions](https://github.com/stereolabs/zed-ros-wrapper#update-the-repository). For more information please read issue [#750](https://github.com/stereolabs/zed-ros-wrapper/issues/750). ## Getting started From 3af19a269b0fcdbd43029f85568cfbd42504fde4 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Mon, 29 Jul 2024 14:17:44 +0200 Subject: [PATCH 199/199] Fix Dynamic parameters initialization bug --- CHANGELOG.rst | 4 ++++ .../src/zed_nodelet/src/zed_wrapper_nodelet.cpp | 10 ++++++---- 2 files changed, 10 insertions(+), 4 deletions(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index b3b4fa93..ff498829 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -1,6 +1,10 @@ CHANGELOG ========= +07-29-2024 +---------- +- Fix wrong dynamic parameters initialization + 05-04-2024 ---------- - Fix compatibility with ZED SDK v4.1 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 5492de5e..7150918c 100644 --- a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp @@ -1191,6 +1191,8 @@ void ZEDWrapperNodelet::readSvoParams() void ZEDWrapperNodelet::readDynParams() { + std::cerr << "Ci sei o no?" << std::endl << std::flush; + NODELET_INFO_STREAM("*** DYNAMIC PARAMETERS (Init. values) ***"); mNhNs.getParam("brightness", mCamBrightness); @@ -1255,6 +1257,10 @@ void ZEDWrapperNodelet::readParameters() readGeneralParams(); // <---- General + // ----> Dynamic + readDynParams(); + // <---- Dynamic + // ----> Video // NODELET_INFO_STREAM("*** VIDEO PARAMETERS ***"); // Note: there are no "static" video parameters @@ -1264,10 +1270,6 @@ void ZEDWrapperNodelet::readParameters() readDepthParams(); // <----- Depth - // ----> Dynamic - void readDynParams(); - // <---- Dynamic - // ----> Positional Tracking readPosTrkParams(); // <---- Positional Tracking