From 7cb421895ca61541c7305613ccf98dcbaabb0f27 Mon Sep 17 00:00:00 2001 From: jbrhm Date: Sat, 14 Sep 2024 20:33:19 -0400 Subject: [PATCH] CLANG FORMAT IS CRACCCCCCKKKKKKED bro :)))))))))))) --- perception/zed_wrapper/pch.hpp | 18 +- perception/zed_wrapper/zed_wrapper.bridge.cpp | 208 +++---- perception/zed_wrapper/zed_wrapper.bridge.cu | 2 +- perception/zed_wrapper/zed_wrapper.cpp | 508 +++++++++--------- perception/zed_wrapper/zed_wrapper.hpp | 130 ++--- 5 files changed, 431 insertions(+), 435 deletions(-) diff --git a/perception/zed_wrapper/pch.hpp b/perception/zed_wrapper/pch.hpp index e4f30010..6f072f76 100644 --- a/perception/zed_wrapper/pch.hpp +++ b/perception/zed_wrapper/pch.hpp @@ -4,16 +4,14 @@ #include // Messages -#include -#include +#include #include -#include #include -#include +#include +#include +#include #include #include -#include "sensor_msgs/msg/image.hpp" -#include "sensor_msgs/image_encodings.hpp" // ZED #include @@ -22,12 +20,12 @@ #include // STD -#include -#include #include +#include +#include // Utils -#include "parameter.hpp" #include "lie.hpp" -#include "point.hpp" #include "loop_profiler.hpp" +#include "parameter.hpp" +#include "point.hpp" diff --git a/perception/zed_wrapper/zed_wrapper.bridge.cpp b/perception/zed_wrapper/zed_wrapper.bridge.cpp index 629981cb..9f7e2348 100644 --- a/perception/zed_wrapper/zed_wrapper.bridge.cpp +++ b/perception/zed_wrapper/zed_wrapper.bridge.cpp @@ -2,115 +2,115 @@ namespace mrover { - constexpr static float DEG_TO_RAD = std::numbers::pi_v / 180.0f; - constexpr static std::uint64_t NS_PER_S = 1000000000; + constexpr static float DEG_TO_RAD = std::numbers::pi_v / 180.0f; + constexpr static std::uint64_t NS_PER_S = 1000000000; - auto slTime2Ros(sl::Timestamp t) -> rclcpp::Time { - return {static_cast(t.getNanoseconds() / NS_PER_S), - static_cast(t.getNanoseconds() % NS_PER_S)}; - } + auto slTime2Ros(sl::Timestamp t) -> rclcpp::Time { + return {static_cast(t.getNanoseconds() / NS_PER_S), + static_cast(t.getNanoseconds() % NS_PER_S)}; + } - auto fillImuMessage(rclcpp::Node* node, sl::SensorsData::IMUData& imuData, sensor_msgs::msg::Imu& msg) -> void { - msg.header.stamp = node->now(); - msg.orientation.x = imuData.pose.getOrientation().x; - msg.orientation.y = imuData.pose.getOrientation().y; - msg.orientation.z = imuData.pose.getOrientation().z; - msg.orientation.w = imuData.pose.getOrientation().w; - for (int i = 0; i < 3; ++i) - for (int j = 0; j < 3; ++j) - msg.orientation_covariance[i * 3 + j] = imuData.pose_covariance(i, j) * DEG_TO_RAD * DEG_TO_RAD; - msg.angular_velocity.x = imuData.angular_velocity.x * DEG_TO_RAD; - msg.angular_velocity.y = imuData.angular_velocity.y * DEG_TO_RAD; - msg.angular_velocity.z = imuData.angular_velocity.z * DEG_TO_RAD; - for (int i = 0; i < 3; ++i) - for (int j = 0; j < 3; ++j) - msg.angular_velocity_covariance[i * 3 + j] = imuData.angular_velocity_covariance(i, j) * DEG_TO_RAD * DEG_TO_RAD; - msg.linear_acceleration.x = imuData.linear_acceleration.x; - msg.linear_acceleration.y = imuData.linear_acceleration.y; - msg.linear_acceleration.z = imuData.linear_acceleration.z; - for (int i = 0; i < 3; ++i) - for (int j = 0; j < 3; ++j) - msg.linear_acceleration_covariance[i * 3 + j] = imuData.linear_acceleration_covariance(i, j); - } + auto fillImuMessage(rclcpp::Node* node, sl::SensorsData::IMUData& imuData, sensor_msgs::msg::Imu& msg) -> void { + msg.header.stamp = node->now(); + msg.orientation.x = imuData.pose.getOrientation().x; + msg.orientation.y = imuData.pose.getOrientation().y; + msg.orientation.z = imuData.pose.getOrientation().z; + msg.orientation.w = imuData.pose.getOrientation().w; + for (int i = 0; i < 3; ++i) + for (int j = 0; j < 3; ++j) + msg.orientation_covariance[i * 3 + j] = imuData.pose_covariance(i, j) * DEG_TO_RAD * DEG_TO_RAD; + msg.angular_velocity.x = imuData.angular_velocity.x * DEG_TO_RAD; + msg.angular_velocity.y = imuData.angular_velocity.y * DEG_TO_RAD; + msg.angular_velocity.z = imuData.angular_velocity.z * DEG_TO_RAD; + for (int i = 0; i < 3; ++i) + for (int j = 0; j < 3; ++j) + msg.angular_velocity_covariance[i * 3 + j] = imuData.angular_velocity_covariance(i, j) * DEG_TO_RAD * DEG_TO_RAD; + msg.linear_acceleration.x = imuData.linear_acceleration.x; + msg.linear_acceleration.y = imuData.linear_acceleration.y; + msg.linear_acceleration.z = imuData.linear_acceleration.z; + for (int i = 0; i < 3; ++i) + for (int j = 0; j < 3; ++j) + msg.linear_acceleration_covariance[i * 3 + j] = imuData.linear_acceleration_covariance(i, j); + } - auto fillMagMessage(sl::SensorsData::MagnetometerData const& magData, sensor_msgs::msg::MagneticField& msg) -> void { - msg.magnetic_field.x = magData.magnetic_field_calibrated.x; - msg.magnetic_field.y = magData.magnetic_field_calibrated.y; - msg.magnetic_field.z = magData.magnetic_field_calibrated.z; - - msg.magnetic_field_covariance.fill(0.0f); - msg.magnetic_field_covariance[0] = 0.039e-6; - msg.magnetic_field_covariance[4] = 0.037e-6; - msg.magnetic_field_covariance[8] = 0.047e-6; - } + auto fillMagMessage(sl::SensorsData::MagnetometerData const& magData, sensor_msgs::msg::MagneticField& msg) -> void { + msg.magnetic_field.x = magData.magnetic_field_calibrated.x; + msg.magnetic_field.y = magData.magnetic_field_calibrated.y; + msg.magnetic_field.z = magData.magnetic_field_calibrated.z; - auto fillImageMessage(sl::Mat const& bgra, sensor_msgs::msg::Image::UniquePtr const& msg) -> void { - assert(bgra.getChannels() == 4); - assert(msg); - - msg->height = bgra.getHeight(); - msg->width = bgra.getWidth(); - msg->encoding = sensor_msgs::image_encodings::BGRA8; - msg->step = bgra.getStepBytes(); - msg->is_bigendian = __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__; - auto* bgrGpuPtr = bgra.getPtr(sl::MEM::GPU); - size_t size = msg->step * msg->height; - msg->data.resize(size); - checkCudaError(cudaMemcpy(msg->data.data(), bgrGpuPtr, size, cudaMemcpyDeviceToHost)); - } + msg.magnetic_field_covariance.fill(0.0f); + msg.magnetic_field_covariance[0] = 0.039e-6; + msg.magnetic_field_covariance[4] = 0.037e-6; + msg.magnetic_field_covariance[8] = 0.047e-6; + } - auto fillCameraInfoMessages(sl::CalibrationParameters& calibration, sl::Resolution const& resolution, + auto fillImageMessage(sl::Mat const& bgra, sensor_msgs::msg::Image::UniquePtr const& msg) -> void { + assert(bgra.getChannels() == 4); + assert(msg); + + msg->height = bgra.getHeight(); + msg->width = bgra.getWidth(); + msg->encoding = sensor_msgs::image_encodings::BGRA8; + msg->step = bgra.getStepBytes(); + msg->is_bigendian = __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__; + auto* bgrGpuPtr = bgra.getPtr(sl::MEM::GPU); + size_t size = msg->step * msg->height; + msg->data.resize(size); + checkCudaError(cudaMemcpy(msg->data.data(), bgrGpuPtr, size, cudaMemcpyDeviceToHost)); + } + + auto fillCameraInfoMessages(sl::CalibrationParameters& calibration, sl::Resolution const& resolution, sensor_msgs::msg::CameraInfo& leftInfoMsg, sensor_msgs::msg::CameraInfo& rightInfoMsg) -> void { - leftInfoMsg.width = resolution.width; - leftInfoMsg.height = resolution.height; - rightInfoMsg.width = resolution.width; - rightInfoMsg.height = resolution.height; - leftInfoMsg.distortion_model = sensor_msgs::distortion_models::PLUMB_BOB; - rightInfoMsg.distortion_model = sensor_msgs::distortion_models::PLUMB_BOB; - leftInfoMsg.d.resize(5); - rightInfoMsg.d.resize(5); - leftInfoMsg.d[0] = calibration.left_cam.disto[0]; - leftInfoMsg.d[1] = calibration.left_cam.disto[1]; - leftInfoMsg.d[2] = calibration.left_cam.disto[4]; - leftInfoMsg.d[3] = calibration.left_cam.disto[2]; - leftInfoMsg.d[4] = calibration.left_cam.disto[3]; - rightInfoMsg.d[0] = calibration.right_cam.disto[0]; - rightInfoMsg.d[1] = calibration.right_cam.disto[1]; - rightInfoMsg.d[2] = calibration.right_cam.disto[4]; - rightInfoMsg.d[3] = calibration.right_cam.disto[2]; - rightInfoMsg.d[4] = calibration.right_cam.disto[3]; - leftInfoMsg.k.fill(0.0); - rightInfoMsg.k.fill(0.0); - leftInfoMsg.k[0] = calibration.left_cam.fx; - leftInfoMsg.k[2] = calibration.left_cam.cx; - leftInfoMsg.k[4] = calibration.left_cam.fy; - leftInfoMsg.k[5] = calibration.left_cam.cy; - leftInfoMsg.k[8] = 1.0; - rightInfoMsg.k[0] = calibration.right_cam.fx; - rightInfoMsg.k[2] = calibration.right_cam.cx; - rightInfoMsg.k[4] = calibration.right_cam.fy; - rightInfoMsg.k[5] = calibration.right_cam.cy; - rightInfoMsg.k[8] = 1; - leftInfoMsg.r.fill(0.0); - rightInfoMsg.r.fill(0.0); - for (size_t i = 0; i < 3; ++i) { - leftInfoMsg.r[i * 3 + i] = 1.0; - rightInfoMsg.r[i * 3 + i] = 1.0; - } - leftInfoMsg.p.fill(0.0); - rightInfoMsg.p.fill(0.0); - leftInfoMsg.p[0] = calibration.left_cam.fx; - leftInfoMsg.p[2] = calibration.left_cam.cx; - leftInfoMsg.p[5] = calibration.left_cam.fy; - leftInfoMsg.p[6] = calibration.left_cam.cy; - leftInfoMsg.p[10] = 1.0; - rightInfoMsg.p[3] = -1.0 * calibration.left_cam.fx * calibration.getCameraBaseline(); - rightInfoMsg.p[0] = calibration.right_cam.fx; - rightInfoMsg.p[2] = calibration.right_cam.cx; - rightInfoMsg.p[5] = calibration.right_cam.fy; - rightInfoMsg.p[6] = calibration.right_cam.cy; - rightInfoMsg.p[10] = 1.0; - } -}; + leftInfoMsg.width = resolution.width; + leftInfoMsg.height = resolution.height; + rightInfoMsg.width = resolution.width; + rightInfoMsg.height = resolution.height; + leftInfoMsg.distortion_model = sensor_msgs::distortion_models::PLUMB_BOB; + rightInfoMsg.distortion_model = sensor_msgs::distortion_models::PLUMB_BOB; + leftInfoMsg.d.resize(5); + rightInfoMsg.d.resize(5); + leftInfoMsg.d[0] = calibration.left_cam.disto[0]; + leftInfoMsg.d[1] = calibration.left_cam.disto[1]; + leftInfoMsg.d[2] = calibration.left_cam.disto[4]; + leftInfoMsg.d[3] = calibration.left_cam.disto[2]; + leftInfoMsg.d[4] = calibration.left_cam.disto[3]; + rightInfoMsg.d[0] = calibration.right_cam.disto[0]; + rightInfoMsg.d[1] = calibration.right_cam.disto[1]; + rightInfoMsg.d[2] = calibration.right_cam.disto[4]; + rightInfoMsg.d[3] = calibration.right_cam.disto[2]; + rightInfoMsg.d[4] = calibration.right_cam.disto[3]; + leftInfoMsg.k.fill(0.0); + rightInfoMsg.k.fill(0.0); + leftInfoMsg.k[0] = calibration.left_cam.fx; + leftInfoMsg.k[2] = calibration.left_cam.cx; + leftInfoMsg.k[4] = calibration.left_cam.fy; + leftInfoMsg.k[5] = calibration.left_cam.cy; + leftInfoMsg.k[8] = 1.0; + rightInfoMsg.k[0] = calibration.right_cam.fx; + rightInfoMsg.k[2] = calibration.right_cam.cx; + rightInfoMsg.k[4] = calibration.right_cam.fy; + rightInfoMsg.k[5] = calibration.right_cam.cy; + rightInfoMsg.k[8] = 1; + leftInfoMsg.r.fill(0.0); + rightInfoMsg.r.fill(0.0); + for (size_t i = 0; i < 3; ++i) { + leftInfoMsg.r[i * 3 + i] = 1.0; + rightInfoMsg.r[i * 3 + i] = 1.0; + } + leftInfoMsg.p.fill(0.0); + rightInfoMsg.p.fill(0.0); + leftInfoMsg.p[0] = calibration.left_cam.fx; + leftInfoMsg.p[2] = calibration.left_cam.cx; + leftInfoMsg.p[5] = calibration.left_cam.fy; + leftInfoMsg.p[6] = calibration.left_cam.cy; + leftInfoMsg.p[10] = 1.0; + rightInfoMsg.p[3] = -1.0 * calibration.left_cam.fx * calibration.getCameraBaseline(); + rightInfoMsg.p[0] = calibration.right_cam.fx; + rightInfoMsg.p[2] = calibration.right_cam.cx; + rightInfoMsg.p[5] = calibration.right_cam.fy; + rightInfoMsg.p[6] = calibration.right_cam.cy; + rightInfoMsg.p[10] = 1.0; + } +}; // namespace mrover diff --git a/perception/zed_wrapper/zed_wrapper.bridge.cu b/perception/zed_wrapper/zed_wrapper.bridge.cu index 492c8dfb..aefb8a7c 100644 --- a/perception/zed_wrapper/zed_wrapper.bridge.cu +++ b/perception/zed_wrapper/zed_wrapper.bridge.cu @@ -11,7 +11,7 @@ namespace mrover { - using PointCloudGpu = thrust::device_vector; + using PointCloudGpu = thrust::device_vector; // Optimal for the Jetson Xavier NX - this is max threads per block and each block has a max of 2048 threads constexpr uint BLOCK_SIZE = 1024; diff --git a/perception/zed_wrapper/zed_wrapper.cpp b/perception/zed_wrapper/zed_wrapper.cpp index a2104cd9..ebaae9d5 100644 --- a/perception/zed_wrapper/zed_wrapper.cpp +++ b/perception/zed_wrapper/zed_wrapper.cpp @@ -1,7 +1,7 @@ #include "zed_wrapper.hpp" namespace mrover { - template + template [[nodiscard]] auto stringToZedEnum(std::string_view string) -> TEnum { using int_t = std::underlying_type_t; for (int_t i = 0; i < static_cast(TEnum::LAST); ++i) { @@ -13,292 +13,291 @@ namespace mrover { } - ZedWrapper::ZedWrapper() : Node(NODE_NAME, rclcpp::NodeOptions().use_intra_process_comms(true)), mLoopProfilerGrab{get_logger()}, mLoopProfilerUpdate{get_logger()} { - try{ - RCLCPP_INFO(this->get_logger(), "Created Zed Wrapper Node, %s", NODE_NAME); - - // Publishers - mRightImgPub = create_publisher("right/image", 1); - mLeftImgPub = create_publisher("left/image", 1); - mImuPub = create_publisher("zed_imu/data_raw", 1); - mMagPub = create_publisher("zed_imu/mag", 1); - mPcPub = create_publisher("camera/left/points", 1); - mRightCamInfoPub = create_publisher("camera/right/camera_info", 1); - mLeftCamInfoPub = create_publisher("camera/left/camera_info", 1); - - // Declare and set Params - auto paramSub = std::make_shared(this); - - int imageWidth{}; - int imageHeight{}; - - std::string svoFile{}; - - std::string grabResolutionString, depthModeString; - - std::vector params{ - {"depth_confidence", mDepthConfidence}, - {"serial_number", mSerialNumber}, - {"grab_target_fps", mGrabTargetFps}, - {"texture_confidence", mTextureConfidence}, - {"image_width", imageWidth}, - {"image_height", imageHeight}, - {"svo_file", svoFile}, - {"use_depth_stabilization", mUseDepthStabilization}, - {"grab_resolution", grabResolutionString}, - {"depth_mode", depthModeString}, - {"depth_maximum_distance", mDepthMaximumDistance}, - {"use_builtin_visual_odom", mUseBuiltinPosTracking}, - {"use_pose_smoothing", mUsePoseSmoothing}, - {"use_area_memory", mUseAreaMemory} - }; - - ParameterWrapper::declareParameters(this, paramSub, params); - - mSvoPath = svoFile.c_str(); - - if (imageWidth < 0 || imageHeight < 0) { - throw std::invalid_argument("Invalid image dimensions"); - } - if (mGrabTargetFps < 0) { - throw std::invalid_argument("Invalid grab target framerate"); - } - - mImageResolution = sl::Resolution(imageWidth, imageHeight); - mPointResolution = sl::Resolution(imageWidth, imageHeight); - mNormalsResolution = sl::Resolution(imageWidth, imageHeight); - - RCLCPP_INFO_STREAM(get_logger(), std::format("Resolution: {} image: {}x{} points: {}x{}", - grabResolutionString, mImageResolution.width, mImageResolution.height, mPointResolution.width, mPointResolution.height).c_str()); - RCLCPP_INFO_STREAM(get_logger(), std::format("Use builtin visual odometry: {}", mUseBuiltinPosTracking ? "true" : "false")); - - sl::InitParameters initParameters; - - if (mSvoPath) { - initParameters.input.setFromSVOFile(mSvoPath); - } else { - if (mSerialNumber == -1) { - initParameters.input.setFromCameraID(-1, sl::BUS_TYPE::USB); - } else { - initParameters.input.setFromSerialNumber(mSerialNumber, sl::BUS_TYPE::USB); - } - } - - RCLCPP_INFO_STREAM(get_logger(), grabResolutionString); - - initParameters.depth_stabilization = mUseDepthStabilization; - initParameters.camera_resolution = stringToZedEnum(grabResolutionString); - initParameters.depth_mode = stringToZedEnum(depthModeString); - initParameters.coordinate_units = sl::UNIT::METER; - initParameters.sdk_verbose = true; // Log useful information - initParameters.camera_fps = mGrabTargetFps; - initParameters.coordinate_system = sl::COORDINATE_SYSTEM::RIGHT_HANDED_Z_UP_X_FWD; // Match ROS - initParameters.depth_maximum_distance = static_cast(mDepthMaximumDistance); - RCLCPP_INFO_STREAM(get_logger(), depthModeString); - - mDepthEnabled = initParameters.depth_mode != sl::DEPTH_MODE::NONE; - - if (mZed.open(initParameters) != sl::ERROR_CODE::SUCCESS) { - throw std::runtime_error("ZED failed to open"); - } - - mZedInfo = mZed.getCameraInformation(); - - if (mUseBuiltinPosTracking) { - sl::PositionalTrackingParameters positionalTrackingParameters; - positionalTrackingParameters.enable_pose_smoothing = mUsePoseSmoothing; - positionalTrackingParameters.enable_area_memory = mUseAreaMemory; - mZed.enablePositionalTracking(positionalTrackingParameters); - } - - cudaDeviceProp prop{}; - cudaGetDeviceProperties(&prop, 0); - RCLCPP_INFO_STREAM(get_logger(), std::format("MP count: {}, Max threads/MP: {}, Max blocks/MP: {}, max threads/block: {}", - prop.multiProcessorCount, prop.maxThreadsPerMultiProcessor, prop.maxBlocksPerMultiProcessor, prop.maxThreadsPerBlock)); - - mGrabThread = std::thread(&ZedWrapper::grabThread, this); + ZedWrapper::ZedWrapper() : Node(NODE_NAME, rclcpp::NodeOptions().use_intra_process_comms(true)), mLoopProfilerGrab{get_logger()}, mLoopProfilerUpdate{get_logger()} { + try { + RCLCPP_INFO(this->get_logger(), "Created Zed Wrapper Node, %s", NODE_NAME); + + // Publishers + mRightImgPub = create_publisher("right/image", 1); + mLeftImgPub = create_publisher("left/image", 1); + mImuPub = create_publisher("zed_imu/data_raw", 1); + mMagPub = create_publisher("zed_imu/mag", 1); + mPcPub = create_publisher("camera/left/points", 1); + mRightCamInfoPub = create_publisher("camera/right/camera_info", 1); + mLeftCamInfoPub = create_publisher("camera/left/camera_info", 1); + + // Declare and set Params + auto paramSub = std::make_shared(this); + + int imageWidth{}; + int imageHeight{}; + + std::string svoFile{}; + + std::string grabResolutionString, depthModeString; + + std::vector params{ + {"depth_confidence", mDepthConfidence}, + {"serial_number", mSerialNumber}, + {"grab_target_fps", mGrabTargetFps}, + {"texture_confidence", mTextureConfidence}, + {"image_width", imageWidth}, + {"image_height", imageHeight}, + {"svo_file", svoFile}, + {"use_depth_stabilization", mUseDepthStabilization}, + {"grab_resolution", grabResolutionString}, + {"depth_mode", depthModeString}, + {"depth_maximum_distance", mDepthMaximumDistance}, + {"use_builtin_visual_odom", mUseBuiltinPosTracking}, + {"use_pose_smoothing", mUsePoseSmoothing}, + {"use_area_memory", mUseAreaMemory}}; + + ParameterWrapper::declareParameters(this, paramSub, params); + + mSvoPath = svoFile.c_str(); + + if (imageWidth < 0 || imageHeight < 0) { + throw std::invalid_argument("Invalid image dimensions"); + } + if (mGrabTargetFps < 0) { + throw std::invalid_argument("Invalid grab target framerate"); + } + + mImageResolution = sl::Resolution(imageWidth, imageHeight); + mPointResolution = sl::Resolution(imageWidth, imageHeight); + mNormalsResolution = sl::Resolution(imageWidth, imageHeight); + + RCLCPP_INFO_STREAM(get_logger(), std::format("Resolution: {} image: {}x{} points: {}x{}", + grabResolutionString, mImageResolution.width, mImageResolution.height, mPointResolution.width, mPointResolution.height) + .c_str()); + RCLCPP_INFO_STREAM(get_logger(), std::format("Use builtin visual odometry: {}", mUseBuiltinPosTracking ? "true" : "false")); + + sl::InitParameters initParameters; + + if (mSvoPath) { + initParameters.input.setFromSVOFile(mSvoPath); + } else { + if (mSerialNumber == -1) { + initParameters.input.setFromCameraID(-1, sl::BUS_TYPE::USB); + } else { + initParameters.input.setFromSerialNumber(mSerialNumber, sl::BUS_TYPE::USB); + } + } + + RCLCPP_INFO_STREAM(get_logger(), grabResolutionString); + + initParameters.depth_stabilization = mUseDepthStabilization; + initParameters.camera_resolution = stringToZedEnum(grabResolutionString); + initParameters.depth_mode = stringToZedEnum(depthModeString); + initParameters.coordinate_units = sl::UNIT::METER; + initParameters.sdk_verbose = true; // Log useful information + initParameters.camera_fps = mGrabTargetFps; + initParameters.coordinate_system = sl::COORDINATE_SYSTEM::RIGHT_HANDED_Z_UP_X_FWD; // Match ROS + initParameters.depth_maximum_distance = static_cast(mDepthMaximumDistance); + RCLCPP_INFO_STREAM(get_logger(), depthModeString); + + mDepthEnabled = initParameters.depth_mode != sl::DEPTH_MODE::NONE; + + if (mZed.open(initParameters) != sl::ERROR_CODE::SUCCESS) { + throw std::runtime_error("ZED failed to open"); + } + + mZedInfo = mZed.getCameraInformation(); + + if (mUseBuiltinPosTracking) { + sl::PositionalTrackingParameters positionalTrackingParameters; + positionalTrackingParameters.enable_pose_smoothing = mUsePoseSmoothing; + positionalTrackingParameters.enable_area_memory = mUseAreaMemory; + mZed.enablePositionalTracking(positionalTrackingParameters); + } + + cudaDeviceProp prop{}; + cudaGetDeviceProperties(&prop, 0); + RCLCPP_INFO_STREAM(get_logger(), std::format("MP count: {}, Max threads/MP: {}, Max blocks/MP: {}, max threads/block: {}", + prop.multiProcessorCount, prop.maxThreadsPerMultiProcessor, prop.maxBlocksPerMultiProcessor, prop.maxThreadsPerBlock)); + + mGrabThread = std::thread(&ZedWrapper::grabThread, this); mPointCloudThread = std::thread(&ZedWrapper::pointCloudUpdateThread, this); - }catch (std::exception const& e) { + } catch (std::exception const& e) { RCLCPP_FATAL_STREAM(get_logger(), std::format("Exception while starting: {}", e.what())); rclcpp::shutdown(); } - } - - - auto ZedWrapper::grabThread() -> void{ - try{ - RCLCPP_INFO(this->get_logger(), "Starting grab thread"); - while(rclcpp::ok()){ - //mLoopProfilerGrab.beginLoop(); - - sl::RuntimeParameters runtimeParameters; - runtimeParameters.confidence_threshold = mDepthConfidence; - runtimeParameters.texture_confidence_threshold = mTextureConfidence; - - - if (sl::ERROR_CODE error = mZed.grab(runtimeParameters); error != sl::ERROR_CODE::SUCCESS) - throw std::runtime_error(std::format("ZED failed to grab {}", sl::toString(error).c_str())); - - mLoopProfilerGrab.measureEvent("zed_grab"); - - // Retrieval has to happen on the same thread as grab so that the image and point cloud are synced - if (mZed.retrieveImage(mGrabMeasures.rightImage, sl::VIEW::RIGHT, sl::MEM::GPU, mImageResolution) != sl::ERROR_CODE::SUCCESS) - throw std::runtime_error("ZED failed to retrieve right image"); - - mLoopProfilerGrab.measureEvent("zed_retrieve_right_image"); - - // Only left set is used for processing - if (mDepthEnabled) { - if (mZed.retrieveImage(mGrabMeasures.leftImage, sl::VIEW::LEFT, sl::MEM::GPU, mImageResolution) != sl::ERROR_CODE::SUCCESS) - throw std::runtime_error("ZED failed to retrieve left image"); - if (mZed.retrieveMeasure(mGrabMeasures.leftPoints, sl::MEASURE::XYZ, sl::MEM::GPU, mPointResolution) != sl::ERROR_CODE::SUCCESS) - throw std::runtime_error("ZED failed to retrieve point cloud"); - } - - mLoopProfilerGrab.measureEvent("zed_retrieve_left_image"); - - // if (mZed.retrieveMeasure(mGrabMeasures.leftNormals, sl::MEASURE::NORMALS, sl::MEM::GPU, mNormalsResolution) != sl::ERROR_CODE::SUCCESS) - // throw std::runtime_error("ZED failed to retrieve point cloud normals"); - - assert(mGrabMeasures.leftImage.timestamp == mGrabMeasures.leftPoints.timestamp); - - mGrabMeasures.time = mSvoPath.c_str() ? now() : slTime2Ros(mZed.getTimestamp(sl::TIME_REFERENCE::IMAGE)); - - // If the processing thread is busy skip - // We want this thread to run as fast as possible for grab and positional tracking - if (mSwapMutex.try_lock()) { - std::swap(mGrabMeasures, mPcMeasures); - mIsSwapReady = true; - mSwapMutex.unlock(); - mSwapCv.notify_one(); - } - - // Positional tracking module publishing - if (mUseBuiltinPosTracking) { - sl::Pose pose; - sl::POSITIONAL_TRACKING_STATE status = mZed.getPosition(pose); - if (status == sl::POSITIONAL_TRACKING_STATE::OK) { - sl::Translation const& translation = pose.getTranslation(); - sl::Orientation const& orientation = pose.getOrientation(); - try { - SE3d leftCameraInOdom{{translation.x, translation.y, translation.z}, - Eigen::Quaterniond{orientation.w, orientation.x, orientation.y, orientation.z}.normalized()}; - SE3d baseLinkToLeftCamera = SE3Conversions::fromTfTree(*mTfBuffer, "base_link", "zed_left_camera_frame"); - SE3d baseLinkInOdom = leftCameraInOdom * baseLinkToLeftCamera; - SE3Conversions::pushToTfTree(*mTfBroadcaster, "base_link", "odom", baseLinkInOdom, now()); - } catch (tf2::TransformException& e) { - RCLCPP_INFO_STREAM(get_logger(), "Failed to get transform: " << e.what()); - } - } else { - RCLCPP_INFO_STREAM(get_logger(), "Positional tracking failed: " << status); - } - } - - mLoopProfilerGrab.measureEvent("pose_tracking"); - - if (mZedInfo.camera_model != sl::MODEL::ZED) { - sl::SensorsData sensorData; - mZed.getSensorsData(sensorData, sl::TIME_REFERENCE::CURRENT); - - sensor_msgs::msg::Imu imuMsg; - fillImuMessage(this, sensorData.imu, imuMsg); - imuMsg.header.frame_id = "zed_mag_frame"; - imuMsg.header.stamp = now(); - mImuPub->publish(imuMsg); - - if (mZedInfo.camera_model != sl::MODEL::ZED_M) { - sensor_msgs::msg::MagneticField magMsg; - fillMagMessage(sensorData.magnetometer, magMsg); - magMsg.header.frame_id = "zed_mag_frame"; - magMsg.header.stamp = now(); - mMagPub->publish(magMsg); - } - } - - mLoopProfilerGrab.measureEvent("publish_imu_and_mag"); - } - - mZed.close(); + } + + + auto ZedWrapper::grabThread() -> void { + try { + RCLCPP_INFO(this->get_logger(), "Starting grab thread"); + while (rclcpp::ok()) { + //mLoopProfilerGrab.beginLoop(); + + sl::RuntimeParameters runtimeParameters; + runtimeParameters.confidence_threshold = mDepthConfidence; + runtimeParameters.texture_confidence_threshold = mTextureConfidence; + + + if (sl::ERROR_CODE error = mZed.grab(runtimeParameters); error != sl::ERROR_CODE::SUCCESS) + throw std::runtime_error(std::format("ZED failed to grab {}", sl::toString(error).c_str())); + + mLoopProfilerGrab.measureEvent("zed_grab"); + + // Retrieval has to happen on the same thread as grab so that the image and point cloud are synced + if (mZed.retrieveImage(mGrabMeasures.rightImage, sl::VIEW::RIGHT, sl::MEM::GPU, mImageResolution) != sl::ERROR_CODE::SUCCESS) + throw std::runtime_error("ZED failed to retrieve right image"); + + mLoopProfilerGrab.measureEvent("zed_retrieve_right_image"); + + // Only left set is used for processing + if (mDepthEnabled) { + if (mZed.retrieveImage(mGrabMeasures.leftImage, sl::VIEW::LEFT, sl::MEM::GPU, mImageResolution) != sl::ERROR_CODE::SUCCESS) + throw std::runtime_error("ZED failed to retrieve left image"); + if (mZed.retrieveMeasure(mGrabMeasures.leftPoints, sl::MEASURE::XYZ, sl::MEM::GPU, mPointResolution) != sl::ERROR_CODE::SUCCESS) + throw std::runtime_error("ZED failed to retrieve point cloud"); + } + + mLoopProfilerGrab.measureEvent("zed_retrieve_left_image"); + + // if (mZed.retrieveMeasure(mGrabMeasures.leftNormals, sl::MEASURE::NORMALS, sl::MEM::GPU, mNormalsResolution) != sl::ERROR_CODE::SUCCESS) + // throw std::runtime_error("ZED failed to retrieve point cloud normals"); + + assert(mGrabMeasures.leftImage.timestamp == mGrabMeasures.leftPoints.timestamp); + + mGrabMeasures.time = mSvoPath.c_str() ? now() : slTime2Ros(mZed.getTimestamp(sl::TIME_REFERENCE::IMAGE)); + + // If the processing thread is busy skip + // We want this thread to run as fast as possible for grab and positional tracking + if (mSwapMutex.try_lock()) { + std::swap(mGrabMeasures, mPcMeasures); + mIsSwapReady = true; + mSwapMutex.unlock(); + mSwapCv.notify_one(); + } + + // Positional tracking module publishing + if (mUseBuiltinPosTracking) { + sl::Pose pose; + sl::POSITIONAL_TRACKING_STATE status = mZed.getPosition(pose); + if (status == sl::POSITIONAL_TRACKING_STATE::OK) { + sl::Translation const& translation = pose.getTranslation(); + sl::Orientation const& orientation = pose.getOrientation(); + try { + SE3d leftCameraInOdom{{translation.x, translation.y, translation.z}, + Eigen::Quaterniond{orientation.w, orientation.x, orientation.y, orientation.z}.normalized()}; + SE3d baseLinkToLeftCamera = SE3Conversions::fromTfTree(*mTfBuffer, "base_link", "zed_left_camera_frame"); + SE3d baseLinkInOdom = leftCameraInOdom * baseLinkToLeftCamera; + SE3Conversions::pushToTfTree(*mTfBroadcaster, "base_link", "odom", baseLinkInOdom, now()); + } catch (tf2::TransformException& e) { + RCLCPP_INFO_STREAM(get_logger(), "Failed to get transform: " << e.what()); + } + } else { + RCLCPP_INFO_STREAM(get_logger(), "Positional tracking failed: " << status); + } + } + + mLoopProfilerGrab.measureEvent("pose_tracking"); + + if (mZedInfo.camera_model != sl::MODEL::ZED) { + sl::SensorsData sensorData; + mZed.getSensorsData(sensorData, sl::TIME_REFERENCE::CURRENT); + + sensor_msgs::msg::Imu imuMsg; + fillImuMessage(this, sensorData.imu, imuMsg); + imuMsg.header.frame_id = "zed_mag_frame"; + imuMsg.header.stamp = now(); + mImuPub->publish(imuMsg); + + if (mZedInfo.camera_model != sl::MODEL::ZED_M) { + sensor_msgs::msg::MagneticField magMsg; + fillMagMessage(sensorData.magnetometer, magMsg); + magMsg.header.frame_id = "zed_mag_frame"; + magMsg.header.stamp = now(); + mMagPub->publish(magMsg); + } + } + + mLoopProfilerGrab.measureEvent("publish_imu_and_mag"); + } + + mZed.close(); RCLCPP_INFO(get_logger(), "Grab thread finished"); - } catch (std::exception const& e) { - RCLCPP_FATAL_STREAM(get_logger(), std::format("Exception while running grab thread: {}", e.what())); + } catch (std::exception const& e) { + RCLCPP_FATAL_STREAM(get_logger(), std::format("Exception while running grab thread: {}", e.what())); mZed.close(); rclcpp::shutdown(); std::exit(EXIT_FAILURE); } - } + } - auto ZedWrapper::pointCloudUpdateThread() -> void{ - try { + auto ZedWrapper::pointCloudUpdateThread() -> void { + try { RCLCPP_INFO(get_logger(), "Starting point cloud thread"); while (rclcpp::ok()) { - //mLoopProfilerUpdate.beginLoop(); + //mLoopProfilerUpdate.beginLoop(); - // TODO(quintin): May be bad to allocate every update, best case optimized by tcache + // TODO(quintin): May be bad to allocate every update, best case optimized by tcache // Needed because publish directly shares the pointer to other nodelets running in this process - auto pointCloudMsg = std::make_unique(); + auto pointCloudMsg = std::make_unique(); - // Swap critical section + // Swap critical section { std::unique_lock lock{mSwapMutex}; // Waiting on the condition variable will drop the lock and reacquire it when the condition is met mSwapCv.wait(lock, [this] { return mIsSwapReady; }); mIsSwapReady = false; - mLoopProfilerUpdate.measureEvent("wait_and_alloc"); + mLoopProfilerUpdate.measureEvent("wait_and_alloc"); if (mDepthEnabled) { fillPointCloudMessageFromGpu(mPcMeasures.leftPoints, mPcMeasures.leftImage, mPcMeasures.leftNormals, mPointCloudGpu, pointCloudMsg); pointCloudMsg->header.stamp = mPcMeasures.time; pointCloudMsg->header.frame_id = "zed_left_camera_frame"; - mLoopProfilerUpdate.measureEvent("fill_pc"); + mLoopProfilerUpdate.measureEvent("fill_pc"); } - auto leftImgMsg = std::make_unique(); - fillImageMessage(mPcMeasures.leftImage, leftImgMsg); - leftImgMsg->header.frame_id = "zed_left_camera_optical_frame"; - leftImgMsg->header.stamp = mPcMeasures.time; - mLeftImgPub->publish(std::move(leftImgMsg)); - mLoopProfilerUpdate.measureEvent("pub_left"); - - auto rightImgMsg = std::make_unique(); - fillImageMessage(mPcMeasures.rightImage, rightImgMsg); - rightImgMsg->header.frame_id = "zed_right_camera_optical_frame"; - rightImgMsg->header.stamp = mPcMeasures.time; - mRightImgPub->publish(std::move(rightImgMsg)); - mLoopProfilerUpdate.measureEvent("pub_right"); - + auto leftImgMsg = std::make_unique(); + fillImageMessage(mPcMeasures.leftImage, leftImgMsg); + leftImgMsg->header.frame_id = "zed_left_camera_optical_frame"; + leftImgMsg->header.stamp = mPcMeasures.time; + mLeftImgPub->publish(std::move(leftImgMsg)); + mLoopProfilerUpdate.measureEvent("pub_left"); + + auto rightImgMsg = std::make_unique(); + fillImageMessage(mPcMeasures.rightImage, rightImgMsg); + rightImgMsg->header.frame_id = "zed_right_camera_optical_frame"; + rightImgMsg->header.stamp = mPcMeasures.time; + mRightImgPub->publish(std::move(rightImgMsg)); + mLoopProfilerUpdate.measureEvent("pub_right"); } if (mDepthEnabled) { mPcPub->publish(std::move(pointCloudMsg)); } - mLoopProfilerUpdate.measureEvent("pub_pc"); - - sl::CalibrationParameters calibration = mZedInfo.camera_configuration.calibration_parameters; - auto leftCamInfoMsg = sensor_msgs::msg::CameraInfo(); - auto rightCamInfoMsg = sensor_msgs::msg::CameraInfo(); - fillCameraInfoMessages(calibration, mImageResolution, leftCamInfoMsg, rightCamInfoMsg); - leftCamInfoMsg.header.frame_id = "zed_left_camera_optical_frame"; - leftCamInfoMsg.header.stamp = mPcMeasures.time; - rightCamInfoMsg.header.frame_id = "zed_right_camera_optical_frame"; - rightCamInfoMsg.header.stamp = mPcMeasures.time; - mLeftCamInfoPub->publish(leftCamInfoMsg); - mRightCamInfoPub->publish(rightCamInfoMsg); - mLoopProfilerUpdate.measureEvent("pub_camera_info"); - } - - RCLCPP_INFO(get_logger(), "Tag thread finished"); - } catch (std::exception const& e) { + mLoopProfilerUpdate.measureEvent("pub_pc"); + + sl::CalibrationParameters calibration = mZedInfo.camera_configuration.calibration_parameters; + auto leftCamInfoMsg = sensor_msgs::msg::CameraInfo(); + auto rightCamInfoMsg = sensor_msgs::msg::CameraInfo(); + fillCameraInfoMessages(calibration, mImageResolution, leftCamInfoMsg, rightCamInfoMsg); + leftCamInfoMsg.header.frame_id = "zed_left_camera_optical_frame"; + leftCamInfoMsg.header.stamp = mPcMeasures.time; + rightCamInfoMsg.header.frame_id = "zed_right_camera_optical_frame"; + rightCamInfoMsg.header.stamp = mPcMeasures.time; + mLeftCamInfoPub->publish(leftCamInfoMsg); + mRightCamInfoPub->publish(rightCamInfoMsg); + mLoopProfilerUpdate.measureEvent("pub_camera_info"); + } + + RCLCPP_INFO(get_logger(), "Tag thread finished"); + } catch (std::exception const& e) { RCLCPP_FATAL_STREAM(get_logger(), std::format("Exception while running point cloud thread: {}", e.what())); rclcpp::shutdown(); std::exit(EXIT_FAILURE); } - } + } - ZedWrapper::~ZedWrapper() { + ZedWrapper::~ZedWrapper() { RCLCPP_INFO(get_logger(), "ZED node shutting down"); mPointCloudThread.join(); mGrabThread.join(); @@ -316,12 +315,11 @@ namespace mrover { std::swap(time, other.time); return *this; } -}; - -auto main(int argc, char * argv[]) -> int -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; +}; // namespace mrover + +auto main(int argc, char* argv[]) -> int { + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; } diff --git a/perception/zed_wrapper/zed_wrapper.hpp b/perception/zed_wrapper/zed_wrapper.hpp index 621079d2..f9b1f6d0 100644 --- a/perception/zed_wrapper/zed_wrapper.hpp +++ b/perception/zed_wrapper/zed_wrapper.hpp @@ -4,100 +4,100 @@ namespace mrover { - using PointCloudGpu = thrust::device_vector; + using PointCloudGpu = thrust::device_vector; - class ZedWrapper : public rclcpp::Node{ - private: - static constexpr char const* NODE_NAME = "zed_wrapper"; + class ZedWrapper : public rclcpp::Node { + private: + static constexpr char const* NODE_NAME = "zed_wrapper"; - std::unique_ptr mTfBuffer = std::make_unique(get_clock()); - std::shared_ptr mTfBroadcaster = std::make_shared(this); - std::shared_ptr mTfListener = std::make_shared(*mTfBuffer); + std::unique_ptr mTfBuffer = std::make_unique(get_clock()); + std::shared_ptr mTfBroadcaster = std::make_shared(this); + std::shared_ptr mTfListener = std::make_shared(*mTfBuffer); - struct Measures { - rclcpp::Time time; - sl::Mat leftImage; - sl::Mat rightImage; - sl::Mat leftPoints; - sl::Mat leftNormals; + struct Measures { + rclcpp::Time time; + sl::Mat leftImage; + sl::Mat rightImage; + sl::Mat leftPoints; + sl::Mat leftNormals; - Measures() = default; + Measures() = default; - Measures(Measures&) = delete; - auto operator=(Measures&) -> Measures& = delete; + Measures(Measures&) = delete; + auto operator=(Measures&) -> Measures& = delete; - Measures(Measures&&) noexcept; - auto operator=(Measures&&) noexcept -> Measures&; - }; + Measures(Measures&&) noexcept; + auto operator=(Measures&&) noexcept -> Measures&; + }; - LoopProfiler mLoopProfilerGrab; - LoopProfiler mLoopProfilerUpdate; + LoopProfiler mLoopProfilerGrab; + LoopProfiler mLoopProfilerUpdate; - // Params - int mSerialNumber{}; - int mGrabTargetFps{}; - int mDepthConfidence{}; - int mTextureConfidence{}; + // Params + int mSerialNumber{}; + int mGrabTargetFps{}; + int mDepthConfidence{}; + int mTextureConfidence{}; - bool mUseDepthStabilization{}; - bool mDepthEnabled{}; - bool mUseBuiltinPosTracking{}; - bool mUsePoseSmoothing{}; - bool mUseAreaMemory{}; + bool mUseDepthStabilization{}; + bool mDepthEnabled{}; + bool mUseBuiltinPosTracking{}; + bool mUsePoseSmoothing{}; + bool mUseAreaMemory{}; - double mDepthMaximumDistance{}; + double mDepthMaximumDistance{}; - // CUDA - PointCloudGpu mPointCloudGpu; + // CUDA + PointCloudGpu mPointCloudGpu; - // ZED - sl::Camera mZed; - sl::CameraInformation mZedInfo; + // ZED + sl::Camera mZed; + sl::CameraInformation mZedInfo; - Measures mGrabMeasures, mPcMeasures; + Measures mGrabMeasures, mPcMeasures; - sl::Resolution mImageResolution, mPointResolution, mNormalsResolution; + sl::Resolution mImageResolution, mPointResolution, mNormalsResolution; - sl::String mSvoPath; + sl::String mSvoPath; - // Publishers - rclcpp::Publisher::SharedPtr mRightImgPub; - rclcpp::Publisher::SharedPtr mLeftImgPub; - rclcpp::Publisher::SharedPtr mImuPub; - rclcpp::Publisher::SharedPtr mMagPub; - rclcpp::Publisher::SharedPtr mPcPub; - rclcpp::Publisher::SharedPtr mLeftCamInfoPub; - rclcpp::Publisher::SharedPtr mRightCamInfoPub; + // Publishers + rclcpp::Publisher::SharedPtr mRightImgPub; + rclcpp::Publisher::SharedPtr mLeftImgPub; + rclcpp::Publisher::SharedPtr mImuPub; + rclcpp::Publisher::SharedPtr mMagPub; + rclcpp::Publisher::SharedPtr mPcPub; + rclcpp::Publisher::SharedPtr mLeftCamInfoPub; + rclcpp::Publisher::SharedPtr mRightCamInfoPub; - // Thread - - std::thread mPointCloudThread, mGrabThread; - std::mutex mSwapMutex; - std::condition_variable mSwapCv; - bool mIsSwapReady = false; + // Thread - auto grabThread() -> void; + std::thread mPointCloudThread, mGrabThread; + std::mutex mSwapMutex; + std::condition_variable mSwapCv; + bool mIsSwapReady = false; - auto pointCloudUpdateThread() -> void; + auto grabThread() -> void; - public: - ZedWrapper(); + auto pointCloudUpdateThread() -> void; - ~ZedWrapper() override; - }; + public: + ZedWrapper(); - auto slTime2Ros(sl::Timestamp t) -> rclcpp::Time; + ~ZedWrapper() override; + }; - auto fillImuMessage(rclcpp::Node* node, sl::SensorsData::IMUData& imuData, sensor_msgs::msg::Imu& msg) -> void; + auto slTime2Ros(sl::Timestamp t) -> rclcpp::Time; + + auto fillImuMessage(rclcpp::Node* node, sl::SensorsData::IMUData& imuData, sensor_msgs::msg::Imu& msg) -> void; auto fillMagMessage(sl::SensorsData::MagnetometerData const& magData, sensor_msgs::msg::MagneticField& msg) -> void; - auto checkCudaError(cudaError_t error) -> void; + auto checkCudaError(cudaError_t error) -> void; - void fillPointCloudMessageFromGpu(sl::Mat& xyzGpu, sl::Mat& bgraGpu, sl::Mat& normalsGpu, PointCloudGpu& pcGpu, sensor_msgs::msg::PointCloud2::UniquePtr const& msg); + void fillPointCloudMessageFromGpu(sl::Mat& xyzGpu, sl::Mat& bgraGpu, sl::Mat& normalsGpu, PointCloudGpu& pcGpu, sensor_msgs::msg::PointCloud2::UniquePtr const& msg); auto fillCameraInfoMessages(sl::CalibrationParameters& calibration, sl::Resolution const& resolution, sensor_msgs::msg::CameraInfo& leftInfoMsg, sensor_msgs::msg::CameraInfo& rightInfoMsg) -> void; auto fillImageMessage(sl::Mat const& bgra, sensor_msgs::msg::Image::UniquePtr const& msg) -> void; -}; +}; // namespace mrover