diff --git a/CMakeLists.txt b/CMakeLists.txt index a653bca8..728e47f0 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -81,6 +81,7 @@ add_definitions(-std=c++11)# -m64) #-Wall) add_executable( zed_wrapper_node src/zed_wrapper_node.cpp + src/point_cloud.cpp ) target_link_libraries( @@ -90,6 +91,7 @@ target_link_libraries( ${CUDA_LIBRARIES} ${CUDA_nppi_LIBRARY} ${CUDA_npps_LIBRARY} ${OpenCV_LIBS} ${PCL_LIBRARIES} + -lcuda ) add_dependencies(zed_wrapper_node ${PROJECT_NAME}_gencfg) diff --git a/cfg/Zed.cfg b/cfg/Zed.cfg index cad5bc21..86d31830 100755 --- a/cfg/Zed.cfg +++ b/cfg/Zed.cfg @@ -5,6 +5,6 @@ from dynamic_reconfigure.parameter_generator_catkin import * gen = ParameterGenerator() -gen.add("confidence", int_t, 0, "Confidence threshold, the lower the better", 80, 1, 100) +gen.add("confidence", int_t, 0, "Confidence threshold, the lower the better", 100, 1, 100) exit(gen.generate(PACKAGE, "zed_ros_wrapper", "Zed")) diff --git a/launch/zed.launch b/launch/zed.launch index 24ced3cf..2285e8b9 100644 --- a/launch/zed.launch +++ b/launch/zed.launch @@ -7,12 +7,31 @@ + - + + + + + - + + - + diff --git a/src/point_cloud.cpp b/src/point_cloud.cpp new file mode 100644 index 00000000..a5b50951 --- /dev/null +++ b/src/point_cloud.cpp @@ -0,0 +1,209 @@ +#include "point_cloud.h" + +// ZW_CLOUD_ALGO=1: +// - Use a background thread to publish the PointCloud2 message +// - Call retrieveMeasure to get the point cloud in CPU memory +// - Convert the sl::zed::Mat to pcl::PointCloud +// - Convert from pcl::PointCloud to the ROS message +// ZW_CLOUD_ALGO=2: +// - No background thread +// - Call retrieveMeasure_gpu to get the point cloud in a CUDA buffer +// - Copy the buffer data straight into the ROS message buffer +#define ZW_CLOUD_ALGO 2 + +#if ZW_CLOUD_ALGO == 1 + +#include +#include +#include +#include +#include + +//PCL includes +#include +#include +#include +#include + +//ZED Includes +#include + +using namespace std; + +namespace zw_cloud { + +std::mutex mut; +std::condition_variable cv; +std::unique_ptr th; +bool thread_running = true; +bool data_available = false; + +pcl::PointCloud point_cloud; +string frame_id = ""; +ros::Time frame_time; + +void publish(ros::Publisher &pub_cloud); + +void start(ros::Publisher &pub_cloud) { + if (th) return; + + thread_running = true; + data_available = false; + th.reset(new std::thread(&publish, std::ref(pub_cloud))); +} + +void quit() { + if (!th) return; + + { + lock_guard lock(mut); + thread_running = false; + cv.notify_all(); + } + th->join(); + th.reset(); +} + +bool store(sl::zed::Camera& zed, const std::string& cloud_frame_id, ros::Time cloud_time) { + // Check if the point cloud thread is busy + unique_lock lock(mut, try_to_lock); + if (!lock.owns_lock()) + return false; + // Check if the point cloud thread already has a queued message + if (data_available) + return false; + + // Retrieve raw pointCloud data + sl::zed::Mat measure = zed.retrieveMeasure(sl::zed::MEASURE::XYZBGRA); + int size = measure.width * measure.height; + + point_cloud.is_dense = false; + point_cloud.width = measure.width; + point_cloud.height = measure.height; + point_cloud.points.resize(size); + + int data_width = measure.getWidthByte(); + const sl::uchar* data_ptr = measure.data; + pcl::PointXYZRGB* dest_ptr = point_cloud.points.data(); + + int transp_count = 0; + for (int row = 0; row < measure.height; ++row, data_ptr += data_width) { + const float* row_data = reinterpret_cast(data_ptr); + for (int col = 0; col < measure.width; ++col, ++dest_ptr) { + dest_ptr->y = -row_data[4*col+0]; + dest_ptr->z = row_data[4*col+1]; + dest_ptr->x = -row_data[4*col+2]; + dest_ptr->rgb = row_data[4*col+3]; + uint8_t alpha = reinterpret_cast(&dest_ptr->rgb)[3]; + if (alpha != 255) + ++transp_count; + } + } + + ROS_INFO_STREAM("Points: " << size << ", transparent: " << transp_count); + + frame_id = cloud_frame_id; + frame_time = cloud_time; + + data_available = true; + cv.notify_all(); +} + +/* \brief Publish a pointCloud with a ros Publisher + * \param pub_cloud : the publisher object to use + * \param cloud_frame_id : the id of the reference frame of the point cloud + * \param t : the ros::Time to stamp the point cloud + */ +void publish(ros::Publisher &pub_cloud) { + unique_lock lock(mut); + for (;;) { + while (thread_running && !data_available) + cv.wait(lock); + // check if the thread has to continue + if (!thread_running) + break; + + sensor_msgs::PointCloud2 output; + pcl::toROSMsg(point_cloud, output); // Convert the point cloud to a ROS message + output.header.frame_id = frame_id; // Set the header values of the ROS message + output.header.stamp = frame_time; + ROS_INFO_STREAM("Publishing message: " << + output.width << "x" << output.height << "=" << output.data.size() << " bytes" << + " point_step=" << output.point_step << " row_step=" << output.row_step); + + pub_cloud.publish(output); + data_available = false; + } +} + +} +#endif + +#if ZW_CLOUD_ALGO == 2 + +#include + +// ROS includes +#include +#include +#include +#include + +// ZED includes +#include + +// CUDA includes +#include +#include + +using namespace std; + +namespace zw_cloud { + +ros::Publisher* pub_cloud = nullptr; + +void start(ros::Publisher &publisher) { + pub_cloud = &publisher; +} + +void quit() { + pub_cloud = nullptr; +} + +bool store(sl::zed::Camera& zed, const std::string& cloud_frame_id, ros::Time cloud_time) { + // Get the point cloud data in a GPU buffer + sl::zed::Mat measure = zed.retrieveMeasure_gpu(sl::zed::MEASURE::XYZBGRA); + + // Build the message + sensor_msgs::PointCloud2 msg; + msg.header.stamp = cloud_time; + msg.header.frame_id = cloud_frame_id; + + msg.height = measure.height; + msg.width = measure.width; + msg.is_bigendian = false; + msg.is_dense = false; + msg.point_step = measure.channels * measure.getDataSize(); + msg.row_step = msg.point_step * msg.width; + + sensor_msgs::PointCloud2Modifier modifier(msg); + modifier.setPointCloud2Fields(4, + "y", 1, sensor_msgs::PointField::FLOAT32, + "z", 1, sensor_msgs::PointField::FLOAT32, + "x", 1, sensor_msgs::PointField::FLOAT32, + "rgba", 1, sensor_msgs::PointField::FLOAT32); + + cuCtxSetCurrent(zed.getCUDAContext()); + cudaError_t err = cudaMemcpy2D( + msg.data.data(), msg.row_step, measure.data, measure.getWidthByte(), + msg.row_step, msg.height, cudaMemcpyDeviceToHost); + + if (err != cudaSuccess) { + ROS_WARN_STREAM("Error: cudaMemcpy2D failed with error " << err << ": " << cudaGetErrorString(err)); + } else { + pub_cloud->publish(msg); + } +} + +} +#endif diff --git a/src/point_cloud.h b/src/point_cloud.h new file mode 100644 index 00000000..6848dd98 --- /dev/null +++ b/src/point_cloud.h @@ -0,0 +1,21 @@ +#pragma once + +#include + +namespace ros { + class Publisher; + class Time; +} +namespace sl { + namespace zed { + class Camera; + } +} + +namespace zw_cloud { + + void start(ros::Publisher &pub_cloud); + void quit(); + bool store(sl::zed::Camera& zed, const std::string& cloud_frame_id, ros::Time cloud_time); + +} diff --git a/src/zed_wrapper_node.cpp b/src/zed_wrapper_node.cpp index 8f3a06ab..0b0c5de4 100644 --- a/src/zed_wrapper_node.cpp +++ b/src/zed_wrapper_node.cpp @@ -44,6 +44,7 @@ #include #include #include +#include #include #include #include @@ -63,12 +64,6 @@ //Boost includes #include -//PCL includes -#include -#include -#include -#include - //ZED Includes #include @@ -81,11 +76,7 @@ bool computeDepth; int openniDepthMode = 0; // 16 bit UC data in mm else 32F in m, for more info http://www.ros.org/reps/rep-0118.html // Point cloud thread variables -float* cloud; -bool pointCloudThreadRunning = true; -bool point_cloud_data_processing = false; -string point_cloud_frame_id = ""; -ros::Time point_cloud_time; +#include "point_cloud.h" /* \brief Test if a file exist * \param name : the path to the file @@ -210,49 +201,6 @@ void publishDepth(cv::Mat depth, image_transport::Publisher &pub_depth, string d , t )); } - -/* \brief Publish a pointCloud with a ros Publisher - * \param p_could : the float pointer to point cloud datas - * \param width : the width of the point cloud - * \param height : the height of the point cloud - * \param pub_cloud : the publisher object to use - * \param cloud_frame_id : the id of the reference frame of the point cloud - * \param t : the ros::Time to stamp the point cloud - */ -void publishPointCloud(int width, int height, ros::Publisher &pub_cloud) { - while (pointCloudThreadRunning) { // check if the thread has to continue - if (!point_cloud_data_processing) { // check if datas are available - std::this_thread::sleep_for(std::chrono::milliseconds(1)); // No data, we just wait - continue; - } - pcl::PointCloud point_cloud; - point_cloud.width = width; - point_cloud.height = height; - int size = width*height; - point_cloud.points.resize(size); - int index4 = 0; - float color; - - for (int i = 0; i < size; i++) { - if (cloud[index4 + 2] > 0) { // Check if it's an unvalid point, the depth is more than 0 - index4 += 4; - continue; - } - point_cloud.points[i].y = -cloud[index4++]; - point_cloud.points[i].z = cloud[index4++]; - point_cloud.points[i].x = -cloud[index4++]; - point_cloud.points[i].rgb = cloud[index4++]; - } - - sensor_msgs::PointCloud2 output; - pcl::toROSMsg(point_cloud, output); // Convert the point cloud to a ROS message - output.header.frame_id = point_cloud_frame_id; // Set the header values of the ROS message - output.header.stamp = point_cloud_time; - pub_cloud.publish(output); - point_cloud_data_processing = false; - } -} - /* \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 @@ -334,6 +282,7 @@ void fillCamInfo(Camera* zed, sensor_msgs::CameraInfoPtr left_cam_info_msg, sens } void callback(zed_ros_wrapper::ZedConfig &config, uint32_t level) { + config.confidence = 100; ROS_INFO("Reconfigure confidence : %d", config.confidence); confidence = config.confidence; } @@ -454,7 +403,7 @@ int main(int argc, char **argv) { f = boost::bind(&callback, _1, _2); server.setCallback(f); - confidence = 80; + confidence = 100; // Get the parameters of the ZED images int width = zed->getImageSize().width; @@ -508,9 +457,9 @@ int main(int argc, char **argv) { ros::Rate loop_rate(rate); ros::Time old_t = ros::Time::now(); bool old_image = false; - std::unique_ptr pointCloudThread = nullptr; - pointCloudThread.reset(new std::thread(&publishPointCloud, width, height, std::ref(pub_cloud))); bool tracking_activated = false; + + zw_cloud::start(pub_cloud); try { // Main loop @@ -542,7 +491,7 @@ int main(int argc, char **argv) { if (computeDepth) { int actual_confidence = zed->getConfidenceThreshold(); if (actual_confidence != confidence) - zed->setConfidenceThreshold(confidence); + zed->setConfidenceThreshold(100 /*confidence*/); old_image = zed->grab(static_cast (sensing_mode), true, true, cloud_SubNumber > 0); // Ask to compute the depth } else old_image = zed->grab(static_cast (sensing_mode), false, false); // Ask to not compute the depth @@ -613,13 +562,8 @@ int main(int argc, char **argv) { } // Publish the point cloud if someone has subscribed to - if (cloud_SubNumber > 0 && point_cloud_data_processing == false) { - // Run the point cloud convertion asynchronously to avoid slowing down all the program - // Retrieve raw pointCloud data - cloud = (float*) zed->retrieveMeasure(sl::zed::MEASURE::XYZBGRA).data; - point_cloud_frame_id = cloud_frame_id; - point_cloud_time = t; - point_cloud_data_processing = true; + if (cloud_SubNumber > 0) { + zw_cloud::store(*zed, cloud_frame_id, t); } // Publish the odometry if someone has subscribed to @@ -640,19 +584,12 @@ int main(int argc, char **argv) { } } } catch (...) { - if (pointCloudThread && pointCloudThreadRunning) { - pointCloudThreadRunning = false; - pointCloudThread->join(); - } + zw_cloud::quit(); ROS_ERROR("Unknown error."); return 1; } - - if (pointCloudThread && pointCloudThreadRunning) { - pointCloudThreadRunning = false; - pointCloudThread->join(); - } + + zw_cloud::quit(); ROS_INFO("Quitting zed_depth_stereo_wrapper_node ...\n"); - return 0; }