diff --git a/CMakeLists.txt b/CMakeLists.txt index 240760f..5fe64f6 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -4,41 +4,21 @@ project(sick_visionary_ros VERSION 1.1.2 LANGUAGES CXX) -find_package(catkin REQUIRED COMPONENTS - cv_bridge - diagnostic_updater - pcl_conversions - pcl_ros - image_transport - roscpp - roslaunch -) - -find_package(Boost REQUIRED COMPONENTS system) +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +# find package for header message type +find_package(std_msgs REQUIRED) +find_package(builtin_interfaces REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(image_transport REQUIRED) +find_package(cv_bridge REQUIRED) +find_package(diagnostic_updater REQUIRED) find_package(OpenCV REQUIRED) -find_package(PCL 1.3 REQUIRED) - -catkin_package( - INCLUDE_DIRS include - CATKIN_DEPENDS pcl_conversions pcl_ros roscpp -) - -### COMPILER FLAGS ### -if(NOT CMAKE_BUILD_TYPE) - set(CMAKE_BUILD_TYPE Release) -endif() - -set(CMAKE_CXX_STANDARD 11) -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_CXX_EXTENSIONS OFF) -set(CMAKE_CXX_FLAGS "-Wall -Wextra") -set(CMAKE_CXX_FLAGS_DEBUG "-g") -set(CMAKE_CXX_FLAGS_RELEASE "-O3") +find_package(Threads REQUIRED) +find_package(Boost 1.41 REQUIRED COMPONENTS thread system) ### BUILD ### include_directories( - ${catkin_INCLUDE_DIRS} - ${PCL_INCLUDE_DIRS} include sick_visionary_cpp_shared ) @@ -46,36 +26,40 @@ include_directories( add_subdirectory(sick_visionary_cpp_shared) add_executable(sick_visionary_t_mini_node src/visionary_t_mini.cpp) -add_executable(sick_visionary_s_node src/visionary_s.cpp) +## ignore visionary_s for ROS2 port +ament_target_dependencies(sick_visionary_t_mini_node + rclcpp + sensor_msgs + std_msgs + builtin_interfaces + image_transport + cv_bridge + diagnostic_updater + OpenCV) +# link against the local shared library built in sick_visionary_cpp_shared target_link_libraries(sick_visionary_t_mini_node sick_visionary_cpp_shared - ${catkin_LIBRARIES} - ${OpenCV_LIBS} - ${PCL_LIBRARY} -) - -target_link_libraries(sick_visionary_s_node - sick_visionary_cpp_shared - ${catkin_LIBRARIES} - ${OpenCV_LIBS} - ${PCL_LIBRARY} + Threads::Threads + Boost::thread + Boost::system ) -add_definitions(${PCL_DEFINITIONS}) - -### TEST ### -if(CATKIN_ENABLE_TESTING) - roslaunch_add_file_check(launch) -endif() +## skip visionary_s ### INSTALL ### -install(TARGETS sick_visionary_t_mini_node sick_visionary_s_node - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) +# install executable +install(TARGETS sick_visionary_t_mini_node + RUNTIME DESTINATION lib/${PROJECT_NAME}) install(DIRECTORY launch - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + DESTINATION share/${PROJECT_NAME}/launch ) + +install(FILES launch/sick_visionary-t_mini.launch + DESTINATION share/${PROJECT_NAME}/launch + RENAME sick_visionary-t_mini.launch.xml +) + +# ament package directive +ament_package() diff --git a/launch/sick_visionary-t_mini.launch b/launch/sick_visionary-t_mini.launch index cc8ffa2..3325715 100644 --- a/launch/sick_visionary-t_mini.launch +++ b/launch/sick_visionary-t_mini.launch @@ -1,9 +1,8 @@ - - - + + diff --git a/package.xml b/package.xml index 67e2dea..864d58d 100644 --- a/package.xml +++ b/package.xml @@ -1,32 +1,21 @@ - + sick_visionary_ros 1.1.2 - Open source drivers for the SICK Visionary-S 3D camera and Visionary-T Mini 3D-ToF camera. + Open source drivers for the SICK Visionary-T Mini 3D-ToF camera. Unlicense SICK AG TechSupport 3D Snapshot SICK AG TechSupport 3D Snapshot - catkin - libpcl-all-dev - pcl_conversions - pcl_ros + ament_cmake + rclcpp + sensor_msgs + std_msgs + builtin_interfaces + image_transport cv_bridge diagnostic_updater - image_transport - roscpp - roslaunch - sensor_msgs + libopencv-dev - - - SICK - Visionary-S - Visionary-T Mini - 3D snapshot - 3D vision - stereo - time-of-flight - - + ament_cmake diff --git a/src/visionary_t_mini.cpp b/src/visionary_t_mini.cpp index 32a473e..246d5bf 100644 --- a/src/visionary_t_mini.cpp +++ b/src/visionary_t_mini.cpp @@ -5,17 +5,17 @@ #include #include -#include -#include -#include -#include -#include -#include -#include +#include +// ROS2 includes +#include +#include +#include +#include +#include #include -#include -#include +#include +#include #include "VisionaryControl.h" #include "VisionaryDataStream.h" @@ -23,13 +23,16 @@ using namespace visionary; +// ROS2 global node and publishers +static std::shared_ptr gNode; +static image_transport::Publisher gPubDepth, gPubIntensity, gPubStatemap; +static std::shared_ptr> gPubCameraInfo; +static std::shared_ptr> gPubPoints; + std::shared_ptr gControl; std::shared_ptr gDataHandler; -image_transport::Publisher gPubDepth, gPubIntensity, gPubStatemap; -ros::Publisher gPubCameraInfo, gPubPoints; - std::shared_ptr updater; std::shared_ptr gPubDepth_freq, gPubIntensity_freq, gPubStatemap_freq; std::shared_ptr gPubCameraInfo_freq, gPubPoints_freq; @@ -43,20 +46,21 @@ bool gReceive = true; int gNumSubs = 0; -void diag_timer_cb(const ros::TimerEvent&) -{ - updater->update(); -} +// no longer used +// void diag_timer_cb(const ros::TimerEvent&) +// { +// updater->update(); +// } void driver_diagnostics(diagnostic_updater::DiagnosticStatusWrapper& stat) { - stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "driver running"); + stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "driver running"); stat.add("frame_id", gFrameId); stat.add("device_ident", gDeviceIdent); - stat.add("NumSubscribers_CameraInfo", gPubCameraInfo.getNumSubscribers()); + stat.add("NumSubscribers_CameraInfo", gPubCameraInfo->get_subscription_count()); stat.add("gEnablePoints", gEnablePoints); if (gEnablePoints) - stat.add("NumSubscribers_Points", gPubPoints.getNumSubscribers()); + stat.add("NumSubscribers_Points", gPubPoints->get_subscription_count()); stat.add("gEnableDepth", gEnableDepth); if (gEnableDepth) stat.add("NumSubscribers_Depth", gPubDepth.getNumSubscribers()); @@ -68,86 +72,86 @@ void driver_diagnostics(diagnostic_updater::DiagnosticStatusWrapper& stat) stat.add("NumSubscribers_Statemap", gPubStatemap.getNumSubscribers()); } -void publishCameraInfo(std_msgs::Header header, VisionaryTMiniData& dataHandler) +void publishCameraInfo(std_msgs::msg::Header header, VisionaryTMiniData& dataHandler) { - sensor_msgs::CameraInfo ci; + sensor_msgs::msg::CameraInfo ci; ci.header = header; ci.height = dataHandler.getHeight(); ci.width = dataHandler.getWidth(); - ci.D.clear(); - ci.D.resize(5, 0); - ci.D[0] = dataHandler.getCameraParameters().k1; - ci.D[1] = dataHandler.getCameraParameters().k2; - ci.D[2] = dataHandler.getCameraParameters().p1; - ci.D[3] = dataHandler.getCameraParameters().p2; - ci.D[4] = dataHandler.getCameraParameters().k3; + ci.d.clear(); + ci.d.resize(5, 0); + ci.d[0] = dataHandler.getCameraParameters().k1; + ci.d[1] = dataHandler.getCameraParameters().k2; + ci.d[2] = dataHandler.getCameraParameters().p1; + ci.d[3] = dataHandler.getCameraParameters().p2; + ci.d[4] = dataHandler.getCameraParameters().k3; for (int i = 0; i < 9; i++) { - ci.K[i] = 0; + ci.k[i] = 0; } - ci.K[0] = dataHandler.getCameraParameters().fx; - ci.K[4] = dataHandler.getCameraParameters().fy; - ci.K[2] = dataHandler.getCameraParameters().cx; - ci.K[5] = dataHandler.getCameraParameters().cy; - ci.K[8] = 1; + ci.k[0] = dataHandler.getCameraParameters().fx; + ci.k[4] = dataHandler.getCameraParameters().fy; + ci.k[2] = dataHandler.getCameraParameters().cx; + ci.k[5] = dataHandler.getCameraParameters().cy; + ci.k[8] = 1; for (int i = 0; i < 12; i++) - ci.P[i] = 0; // data.getCameraParameters().cam2worldMatrix[i]; + ci.p[i] = 0; // data.getCameraParameters().cam2worldMatrix[i]; // TODO:.... - ci.P[0] = dataHandler.getCameraParameters().fx; - ci.P[5] = dataHandler.getCameraParameters().fy; - ci.P[10] = 1; - ci.P[2] = dataHandler.getCameraParameters().cx; - ci.P[6] = dataHandler.getCameraParameters().cy; + ci.p[0] = dataHandler.getCameraParameters().fx; + ci.p[5] = dataHandler.getCameraParameters().fy; + ci.p[10] = 1; + ci.p[2] = dataHandler.getCameraParameters().cx; + ci.p[6] = dataHandler.getCameraParameters().cy; - gPubCameraInfo.publish(ci); + gPubCameraInfo->publish(ci); } -void publishDepth(std_msgs::Header header, VisionaryTMiniData& dataHandler) +void publishDepth(std_msgs::msg::Header header, VisionaryTMiniData& dataHandler) { std::vector vec = dataHandler.getDistanceMap(); cv::Mat m = cv::Mat(dataHandler.getHeight(), dataHandler.getWidth(), CV_16UC1); memcpy(m.data, vec.data(), vec.size() * sizeof(uint16_t)); - sensor_msgs::ImagePtr msg = - cv_bridge::CvImage(std_msgs::Header(), sensor_msgs::image_encodings::TYPE_16UC1, m).toImageMsg(); + sensor_msgs::msg::Image::SharedPtr msg = + cv_bridge::CvImage(std_msgs::msg::Header(), sensor_msgs::image_encodings::TYPE_16UC1, m).toImageMsg(); msg->header = header; gPubDepth.publish(msg); } -void publishIntensity(std_msgs::Header header, VisionaryTMiniData& dataHandler) +void publishIntensity(std_msgs::msg::Header header, VisionaryTMiniData& dataHandler) { std::vector vec = dataHandler.getIntensityMap(); cv::Mat m = cv::Mat(dataHandler.getHeight(), dataHandler.getWidth(), CV_16UC1); memcpy(m.data, vec.data(), vec.size() * sizeof(uint16_t)); - sensor_msgs::ImagePtr msg = - cv_bridge::CvImage(std_msgs::Header(), sensor_msgs::image_encodings::TYPE_16UC1, m).toImageMsg(); + sensor_msgs::msg::Image::SharedPtr msg = + cv_bridge::CvImage(std_msgs::msg::Header(), sensor_msgs::image_encodings::TYPE_16UC1, m).toImageMsg(); msg->header = header; gPubIntensity.publish(msg); } -void publishStateMap(std_msgs::Header header, VisionaryTMiniData& dataHandler) +void publishStateMap(std_msgs::msg::Header header, VisionaryTMiniData& dataHandler) { std::vector vec = dataHandler.getStateMap(); cv::Mat m = cv::Mat(dataHandler.getHeight(), dataHandler.getWidth(), CV_16UC1); memcpy(m.data, vec.data(), vec.size() * sizeof(uint16_t)); - sensor_msgs::ImagePtr msg = - cv_bridge::CvImage(std_msgs::Header(), sensor_msgs::image_encodings::TYPE_16UC1, m).toImageMsg(); + sensor_msgs::msg::Image::SharedPtr msg = + cv_bridge::CvImage(std_msgs::msg::Header(), sensor_msgs::image_encodings::TYPE_16UC1, m).toImageMsg(); msg->header = header; gPubStatemap.publish(msg); } -void publishPointCloud(std_msgs::Header header, VisionaryTMiniData& dataHandler) +void publishPointCloud(std_msgs::msg::Header header, VisionaryTMiniData& dataHandler) { - typedef sensor_msgs::PointCloud2 PointCloud; + typedef sensor_msgs::msg::PointCloud2 PointCloud; // Allocate new point cloud message - PointCloud::Ptr cloudMsg(new PointCloud); + PointCloud::SharedPtr cloudMsg(new PointCloud); cloudMsg->header = header; cloudMsg->height = dataHandler.getHeight(); cloudMsg->width = dataHandler.getWidth(); @@ -163,12 +167,12 @@ void publishPointCloud(std_msgs::Header header, VisionaryTMiniData& dataHandler) for (size_t d = 0; d < 3; ++d, offset += sizeof(float)) { cloudMsg->fields[d].offset = offset; - cloudMsg->fields[d].datatype = int(sensor_msgs::PointField::FLOAT32); + cloudMsg->fields[d].datatype = int(sensor_msgs::msg::PointField::FLOAT32); cloudMsg->fields[d].count = 1; } cloudMsg->fields[3].offset = offset; - cloudMsg->fields[3].datatype = int(sensor_msgs::PointField::UINT16); + cloudMsg->fields[3].datatype = int(sensor_msgs::msg::PointField::UINT16); cloudMsg->fields[3].count = 1; offset += sizeof(uint16_t); @@ -191,66 +195,38 @@ void publishPointCloud(std_msgs::Header header, VisionaryTMiniData& dataHandler) memcpy(&cloudMsg->data[index * cloudMsg->point_step + cloudMsg->fields[0].offset], &*itPC, sizeof(PointXYZ)); memcpy(&cloudMsg->data[index * cloudMsg->point_step + cloudMsg->fields[3].offset], &*itIntens, sizeof(uint16_t)); } - gPubPoints.publish(cloudMsg); + gPubPoints->publish(*cloudMsg); } void publish_frame(VisionaryTMiniData& dataHandler) { bool publishedAnything = false; - std_msgs::Header header; - header.stamp = ros::Time::now(); + std_msgs::msg::Header header; + header.stamp = gNode->now(); header.frame_id = gFrameId; - if (gPubCameraInfo.getNumSubscribers() > 0) - { - publishedAnything = true; - publishCameraInfo(header, dataHandler); - // gPubCameraInfo_freq->tick(header.stamp); - } - if (gEnableDepth && gPubDepth.getNumSubscribers() > 0) - { - publishedAnything = true; + // publish all enabled streams unconditionally + publishCameraInfo(header, dataHandler); + if (gEnableDepth) publishDepth(header, dataHandler); - // gPubDepth_freq->tick(header.stamp); - } - if (gEnableIntensity && gPubIntensity.getNumSubscribers() > 0) - { - publishedAnything = true; + if (gEnableIntensity) publishIntensity(header, dataHandler); - // gPubIntensity_freq->tick(header.stamp); - } - if (gEnableStatemap && gPubStatemap.getNumSubscribers() > 0) - { - publishedAnything = true; + if (gEnableStatemap) publishStateMap(header, dataHandler); - // gPubStatemap_freq->tick(header.stamp); - } - if (gEnablePoints && gPubPoints.getNumSubscribers() > 0) - { - publishedAnything = true; + if (gEnablePoints) publishPointCloud(header, dataHandler); - // gPubPoints_freq->tick(header.stamp); - } - if (publishedAnything) - { - gPubCameraInfo_freq->tick(header.stamp); - if (gEnableDepth) - gPubDepth_freq->tick(header.stamp); - if (gEnableIntensity) - gPubIntensity_freq->tick(header.stamp); - if (gEnableStatemap) - gPubStatemap_freq->tick(header.stamp); - if (gEnablePoints) - gPubPoints_freq->tick(header.stamp); - } - else - { - ROS_DEBUG("Nothing published"); - if (gControl) - gControl->stopAcquisition(); - } + // tick diagnostics + gPubCameraInfo_freq->tick(header.stamp); + if (gEnableDepth) + gPubDepth_freq->tick(header.stamp); + if (gEnableIntensity) + gPubIntensity_freq->tick(header.stamp); + if (gEnableStatemap) + gPubStatemap_freq->tick(header.stamp); + if (gEnablePoints) + gPubPoints_freq->tick(header.stamp); } void thr_publish_frame() @@ -276,14 +252,14 @@ void thr_receive_frame(std::shared_ptr pDataStream, boost::thread thr(&thr_publish_frame); } else - ROS_INFO("skipping frame with number %d", pDataHandler->getFrameNum()); + RCLCPP_INFO(gNode->get_logger(), "skipping frame with number %d", pDataHandler->getFrameNum()); } } void _on_new_subscriber() { gNumSubs++; - ROS_DEBUG_STREAM("Got new subscriber, total amount of subscribers: " << gNumSubs); + RCLCPP_DEBUG(gNode->get_logger(), "Got new subscriber, total amount of subscribers: %d", gNumSubs); if (gControl) gControl->startAcquisition(); } @@ -291,103 +267,87 @@ void _on_new_subscriber() void _on_subscriber_disconnected() { gNumSubs--; - ROS_DEBUG_STREAM("Subscriber disconnected, total amount of subscribers: " << gNumSubs); -} - -void on_new_subscriber_ros(const ros::SingleSubscriberPublisher&) -{ - _on_new_subscriber(); -} - -void on_new_subscriber_it(const image_transport::SingleSubscriberPublisher&) -{ - _on_new_subscriber(); -} - -void on_subscriber_disconnected_ros(const ros::SingleSubscriberPublisher&) -{ - _on_subscriber_disconnected(); -} - -void on_subscriber_disconnected_it(const image_transport::SingleSubscriberPublisher&) -{ - _on_subscriber_disconnected(); + RCLCPP_DEBUG(gNode->get_logger(), "Subscriber disconnected, total amount of subscribers: %d", gNumSubs); } int main(int argc, char** argv) { - ros::init(argc, argv, "sick_visionary_t_mini"); - ros::NodeHandle nh("~"); + rclcpp::init(argc, argv); + gNode = std::make_shared("sick_visionary_t_mini"); // default parameters std::string remoteDeviceIp = "192.168.1.10"; gFrameId = "camera"; - ros::param::get("~remote_device_ip", remoteDeviceIp); - ros::param::get("~frame_id", gFrameId); - ros::param::get("~enable_depth", gEnableDepth); - ros::param::get("~enable_intensity", gEnableIntensity); - ros::param::get("~enable_statemap", gEnableStatemap); - ros::param::get("~enable_points", gEnablePoints); + // declare and get parameters + gNode->declare_parameter("remote_device_ip", "192.168.1.10"); + gNode->declare_parameter("frame_id", "camera"); + gNode->declare_parameter("enable_depth", true); + gNode->declare_parameter("enable_intensity", true); + gNode->declare_parameter("enable_statemap", true); + gNode->declare_parameter("enable_points", true); + gNode->declare_parameter("desired_frequency", 15.0); + gNode->get_parameter("remote_device_ip", remoteDeviceIp); + gNode->get_parameter("frame_id", gFrameId); + gNode->get_parameter("enable_depth", gEnableDepth); + gNode->get_parameter("enable_intensity", gEnableIntensity); + gNode->get_parameter("enable_statemap", gEnableStatemap); + gNode->get_parameter("enable_points", gEnablePoints); std::shared_ptr pDataHandler = std::make_shared(); std::shared_ptr pDataStream = std::make_shared(pDataHandler); gControl = std::make_shared(); - ROS_INFO("Connecting to device at %s", remoteDeviceIp.c_str()); + RCLCPP_INFO(gNode->get_logger(), "Connecting to device at %s", remoteDeviceIp.c_str()); if (!gControl->open(VisionaryControl::ProtocolType::COLA_2, remoteDeviceIp.c_str(), 5000 /*ms*/)) { - ROS_ERROR("Connection with devices control channel failed"); + RCLCPP_ERROR(gNode->get_logger(), "Connection with devices control channel failed"); + return -1; + } + // Configure the device to send blob data on the data channel + if (!gControl->getDataStreamConfig()) + { + RCLCPP_ERROR(gNode->get_logger(), "Failed to configure data stream on device"); return -1; } // To be sure the acquisition is currently stopped. gControl->stopAcquisition(); - if (!pDataStream->open(remoteDeviceIp.c_str(), 2114u)) + // open data channel on device's blob port + uint16_t dataPort = gControl->GetBlobPort(); + if (!pDataStream->open(remoteDeviceIp.c_str(), dataPort)) { - ROS_ERROR("Connection with devices data channel failed"); + RCLCPP_ERROR(gNode->get_logger(), "Failed to open data channel on port %u", dataPort); return -1; } // TODO: add get device name and device version and print to ros info. - ROS_INFO("Connected with Visionary-T Mini"); - - // make me public (after init.) - image_transport::ImageTransport it(nh); - gPubCameraInfo = nh.advertise("camera_info", - 1, - (ros::SubscriberStatusCallback)on_new_subscriber_ros, - (ros::SubscriberStatusCallback)on_subscriber_disconnected_ros); + RCLCPP_INFO(gNode->get_logger(), "Connected with Visionary-T Mini"); + + // setup publishers + image_transport::ImageTransport it(gNode); + gPubCameraInfo = gNode->create_publisher("camera_info", 10); if (gEnableDepth) - gPubDepth = it.advertise("depth", - 1, - (image_transport::SubscriberStatusCallback)on_new_subscriber_it, - (image_transport::SubscriberStatusCallback)on_subscriber_disconnected_it); + gPubDepth = it.advertise("depth", 10); if (gEnablePoints) - gPubPoints = nh.advertise("points", - 2, - (ros::SubscriberStatusCallback)on_new_subscriber_ros, - (ros::SubscriberStatusCallback)on_subscriber_disconnected_ros); + gPubPoints = gNode->create_publisher("points", 10); if (gEnableIntensity) - gPubIntensity = it.advertise("intensity", - 1, - (image_transport::SubscriberStatusCallback)on_new_subscriber_it, - (image_transport::SubscriberStatusCallback)on_subscriber_disconnected_it); + gPubIntensity = it.advertise("intensity", 10); if (gEnableStatemap) - gPubStatemap = it.advertise("statemap", - 1, - (image_transport::SubscriberStatusCallback)on_new_subscriber_it, - (image_transport::SubscriberStatusCallback)on_subscriber_disconnected_it); + gPubStatemap = it.advertise("statemap", 10); + + // for T-Mini, perform single-step acquisitions via timer; do not call continuous startAcquisition + RCLCPP_INFO(gNode->get_logger(), "Using step-based acquisition for Visionary-T Mini"); gDeviceIdent = gControl->getDeviceIdent(); // diagnostics - updater.reset(new diagnostic_updater::Updater()); - updater->setHardwareID(nh.getNamespace()); + updater.reset(new diagnostic_updater::Updater(gNode)); + updater->setHardwareID(gNode->get_name()); updater->add("driver", driver_diagnostics); double desiredFreq; // device max freq is 30FPS - ros::param::get("~desired_frequency", desiredFreq); + gNode->get_parameter("desired_frequency", desiredFreq); double min_freq = desiredFreq * 0.9; double max_freq = desiredFreq * 1.1; @@ -421,13 +381,22 @@ int main(int argc, char** argv) diagnostic_updater::FrequencyStatusParam(&min_freq, &max_freq), diagnostic_updater::TimeStampStatusParam())); - ros::Timer timer = nh.createTimer(ros::Duration(1.0), diag_timer_cb); + // diagnostic updater with ROS2 timer + auto diag_timer = gNode->create_wall_timer(std::chrono::seconds(1), [=]() { updater->force_update(); }); + // Periodically trigger next frame on T-Mini (step acquisition) since continuous mode may not auto-stream + auto step_timer = gNode->create_wall_timer(std::chrono::milliseconds(static_cast(1000.0 / desiredFreq)), [=]() { + if (gControl) + { + gControl->stepAcquisition(); + } + }); // start receiver thread for camera images - boost::thread rec_thr(boost::bind(&thr_receive_frame, pDataStream, pDataHandler)); + std::thread rec_thr(boost::bind(&thr_receive_frame, pDataStream, pDataHandler)); - // wait til end of exec. - ros::spin(); + // spin ROS2 node + rclcpp::spin(gNode); + rclcpp::shutdown(); gReceive = false; rec_thr.join(); @@ -436,15 +405,5 @@ int main(int argc, char** argv) gControl->close(); pDataStream->close(); - if (gEnableDepth) - gPubDepth.shutdown(); - if (gEnablePoints) - gPubPoints.shutdown(); - if (gEnableIntensity) - gPubIntensity.shutdown(); - if (gEnableStatemap) - gPubStatemap.shutdown(); - gPubCameraInfo.shutdown(); - return 0; }