From b9ebd2e4d12cf48722df89f2d1d91c1447e52cc5 Mon Sep 17 00:00:00 2001 From: ok Date: Thu, 28 Nov 2024 16:10:40 -0300 Subject: [PATCH 01/12] ok --- src/kinect_ros2_component.cpp | 27 ++++++++++++--------------- 1 file changed, 12 insertions(+), 15 deletions(-) diff --git a/src/kinect_ros2_component.cpp b/src/kinect_ros2_component.cpp index 5ce32de..ce925c0 100644 --- a/src/kinect_ros2_component.cpp +++ b/src/kinect_ros2_component.cpp @@ -137,32 +137,29 @@ void KinectRosComponent::rgb_cb(freenect_device * dev, void * rgb_ptr, uint32_t void KinectRosComponent::timer_callback() { freenect_process_events(fn_ctx_); - auto header = std_msgs::msg::Header(); - header.frame_id = "kinect_depth"; + auto rgb_header = std_msgs::msg::Header(); + auto depth_header = std_msgs::msg::Header(); + rgb_header.frame_id = "kinect_rgb"; + depth_header.frame_id = "kinect_depth"; auto stamp = now(); - header.stamp = stamp; - depth_info_.header.stamp = stamp; + rgb_header.stamp = stamp; + depth_header.stamp = stamp; + + rgb_info_.header = rgb_header; + depth_info_.header = depth_header; + if (_depth_flag) { - //convert 16bit to 8bit mono - // cv::Mat depth_8UC1(_depth_image, CV_16UC1); - // depth_8UC1.convertTo(depth_8UC1, CV_8UC1); - auto msg = cv_bridge::CvImage(header, "16UC1", _depth_image).toImageMsg(); + auto msg = cv_bridge::CvImage(depth_header, "16UC1", _depth_image).toImageMsg(); depth_pub_.publish(*msg, depth_info_); - - // cv::imshow("Depth", _depth_image); - // cv::waitKey(1); _depth_flag = false; } if (_rgb_flag) { - auto msg = cv_bridge::CvImage(std_msgs::msg::Header(), "rgb8", _rgb_image).toImageMsg(); + auto msg = cv_bridge::CvImage(rgb_header, "rgb8", _rgb_image).toImageMsg(); rgb_pub_.publish(*msg, rgb_info_); - - // cv::imshow("RGB", _rgb_image); - // cv::waitKey(1); _rgb_flag = false; } } From 19df4b3a22b404652a94f892bcb574aa3303f8dd Mon Sep 17 00:00:00 2001 From: "Rinaldo T.S Filho" Date: Mon, 2 Dec 2024 17:50:06 -0300 Subject: [PATCH 02/12] Update kinect_ros2_component.cpp From a5ecd53d42530f3121edb2b58fb8ae8284d45074 Mon Sep 17 00:00:00 2001 From: rinaldo Date: Wed, 11 Dec 2024 19:56:25 -0300 Subject: [PATCH 03/12] small update --- CMakeLists.txt | 0 README.md | 0 cfg/calibration_depth.yaml | 0 cfg/calibration_rgb.yaml | 0 include/kinect_ros2/kinect_ros2_component.hpp | 0 launch/pointcloud.launch.py | 0 launch/showimage.launch.py | 0 package.xml | 0 rviz/pointcloud.rviz | 0 src/kinect_ros2_component.cpp | 0 src/kinect_ros2_node.cpp | 0 11 files changed, 0 insertions(+), 0 deletions(-) mode change 100644 => 100755 CMakeLists.txt mode change 100644 => 100755 README.md mode change 100644 => 100755 cfg/calibration_depth.yaml mode change 100644 => 100755 cfg/calibration_rgb.yaml mode change 100644 => 100755 include/kinect_ros2/kinect_ros2_component.hpp mode change 100644 => 100755 launch/pointcloud.launch.py mode change 100644 => 100755 launch/showimage.launch.py mode change 100644 => 100755 package.xml mode change 100644 => 100755 rviz/pointcloud.rviz mode change 100644 => 100755 src/kinect_ros2_component.cpp mode change 100644 => 100755 src/kinect_ros2_node.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt old mode 100644 new mode 100755 diff --git a/README.md b/README.md old mode 100644 new mode 100755 diff --git a/cfg/calibration_depth.yaml b/cfg/calibration_depth.yaml old mode 100644 new mode 100755 diff --git a/cfg/calibration_rgb.yaml b/cfg/calibration_rgb.yaml old mode 100644 new mode 100755 diff --git a/include/kinect_ros2/kinect_ros2_component.hpp b/include/kinect_ros2/kinect_ros2_component.hpp old mode 100644 new mode 100755 diff --git a/launch/pointcloud.launch.py b/launch/pointcloud.launch.py old mode 100644 new mode 100755 diff --git a/launch/showimage.launch.py b/launch/showimage.launch.py old mode 100644 new mode 100755 diff --git a/package.xml b/package.xml old mode 100644 new mode 100755 diff --git a/rviz/pointcloud.rviz b/rviz/pointcloud.rviz old mode 100644 new mode 100755 diff --git a/src/kinect_ros2_component.cpp b/src/kinect_ros2_component.cpp old mode 100644 new mode 100755 diff --git a/src/kinect_ros2_node.cpp b/src/kinect_ros2_node.cpp old mode 100644 new mode 100755 From eeb658c2e18cee114d7a5d4b284875b09418c91a Mon Sep 17 00:00:00 2001 From: "Rinaldo T.S Filho" Date: Mon, 16 Dec 2024 14:10:02 -0300 Subject: [PATCH 04/12] Update kinect_ros2_component.hpp From dc50ebe410a541e3260ffc7479f57f634093ac8a Mon Sep 17 00:00:00 2001 From: "Rinaldo T.S Filho" Date: Mon, 16 Dec 2024 14:10:32 -0300 Subject: [PATCH 05/12] Update kinect_ros2_component.cpp From a84629e5c036d7918bb1673f52bf0a1538bdd902 Mon Sep 17 00:00:00 2001 From: "Rinaldo T.S Filho" Date: Mon, 16 Dec 2024 14:11:27 -0300 Subject: [PATCH 06/12] Update kinect_ros2_component.cpp From 993a87651f56f64b440921a7140e30d23e5f078d Mon Sep 17 00:00:00 2001 From: "Rinaldo T.S Filho" Date: Mon, 16 Dec 2024 14:12:10 -0300 Subject: [PATCH 07/12] Update kinect_ros2_component.cpp --- src/kinect_ros2_component.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/kinect_ros2_component.cpp b/src/kinect_ros2_component.cpp index ce925c0..4a39a35 100644 --- a/src/kinect_ros2_component.cpp +++ b/src/kinect_ros2_component.cpp @@ -167,3 +167,5 @@ void KinectRosComponent::timer_callback() #include "rclcpp_components/register_node_macro.hpp" RCLCPP_COMPONENTS_REGISTER_NODE(kinect_ros2::KinectRosComponent) + + From 462ff1b9ec3941422b4bf94a561b7c55849eb2d3 Mon Sep 17 00:00:00 2001 From: rinaldots Date: Mon, 9 Jun 2025 19:35:33 -0300 Subject: [PATCH 08/12] ok --- include/kinect_ros2/kinect_ros2_component.hpp | 5 +- src/kinect_ros2_component.cpp | 47 +++++++++++++------ 2 files changed, 37 insertions(+), 15 deletions(-) diff --git a/include/kinect_ros2/kinect_ros2_component.hpp b/include/kinect_ros2/kinect_ros2_component.hpp index 3a9f234..1852df7 100755 --- a/include/kinect_ros2/kinect_ros2_component.hpp +++ b/include/kinect_ros2/kinect_ros2_component.hpp @@ -34,7 +34,10 @@ class KinectRosComponent : public rclcpp::Node static void rgb_cb(freenect_device * dev, void * rgb_ptr, uint32_t timestamp); void timer_callback(); + + // processamento contínuo de eventos do Kinect + std::thread processing_thread_; }; } -#endif // KINECT_ROS2__KINECT_ROS2_COMPONENT_HPP_ +#endif // KINECT_ROS2__KINECT_ROS2_COMPONENT_HPP_ \ No newline at end of file diff --git a/src/kinect_ros2_component.cpp b/src/kinect_ros2_component.cpp index 4a39a35..b02fdf8 100755 --- a/src/kinect_ros2_component.cpp +++ b/src/kinect_ros2_component.cpp @@ -1,5 +1,7 @@ #include "kinect_ros2/kinect_ros2_component.hpp" #include +#include +#include using namespace std::chrono_literals; @@ -17,8 +19,9 @@ static bool _rgb_flag; KinectRosComponent::KinectRosComponent(const rclcpp::NodeOptions & options) : Node("kinect_ros2", options) { - timer_ = create_wall_timer(1ms, std::bind(&KinectRosComponent::timer_callback, this)); + timer_ = create_wall_timer(1ms, std::bind(&KinectRosComponent::timer_callback, this)); + std::string pkg_share = ament_index_cpp::get_package_share_directory("kinect_ros2"); //todo: use parameters @@ -35,8 +38,11 @@ KinectRosComponent::KinectRosComponent(const rclcpp::NodeOptions & options) depth_info_ = depth_info_manager_->getCameraInfo(); depth_info_.header.frame_id = "kinect_depth"; - depth_pub_ = image_transport::create_camera_publisher(this, "depth/image_raw"); - rgb_pub_ = image_transport::create_camera_publisher(this, "image_raw"); + // uso de QoS sensor_data (best effort) + depth_pub_ = image_transport::create_camera_publisher( + this, "depth/image_raw", rmw_qos_profile_sensor_data); + rgb_pub_ = image_transport::create_camera_publisher( + this, "image_raw", rmw_qos_profile_sensor_data); int ret = freenect_init(&fn_ctx_, NULL); if (ret < 0) { @@ -90,10 +96,23 @@ KinectRosComponent::KinectRosComponent(const rclcpp::NodeOptions & options) RCLCPP_ERROR(get_logger(), "FREENECT - ERROR START RGB"); rclcpp::shutdown(); } + + // thread não bloqueante para processar eventos + processing_thread_ = std::thread([this]() { + struct timeval tv{0, 10000}; // timeout de 10μs + while (rclcpp::ok()) { + freenect_process_events_timeout(fn_ctx_, &tv); + } + }); } KinectRosComponent::~KinectRosComponent() { + // garante que a thread seja finalizada + if (processing_thread_.joinable()) { + processing_thread_.join(); + } + RCLCPP_INFO(get_logger(), "stoping kinnect"); freenect_stop_depth(fn_dev_); freenect_stop_video(fn_dev_); @@ -108,6 +127,8 @@ to a new cv::Mat. This way, the callback only used to set a flag that indicates has arrived. The flag is unset when a msg is published */ void KinectRosComponent::depth_cb(freenect_device * dev, void * depth_ptr, uint32_t timestamp) { + (void)dev; + (void)timestamp; if (_depth_flag) { return; } @@ -122,6 +143,8 @@ void KinectRosComponent::depth_cb(freenect_device * dev, void * depth_ptr, uint3 void KinectRosComponent::rgb_cb(freenect_device * dev, void * rgb_ptr, uint32_t timestamp) { + (void)dev; + (void)timestamp; if (_rgb_flag) { return; } @@ -136,27 +159,23 @@ void KinectRosComponent::rgb_cb(freenect_device * dev, void * rgb_ptr, uint32_t void KinectRosComponent::timer_callback() { - freenect_process_events(fn_ctx_); - auto rgb_header = std_msgs::msg::Header(); - auto depth_header = std_msgs::msg::Header(); - rgb_header.frame_id = "kinect_rgb"; - depth_header.frame_id = "kinect_depth"; + // somente publica se novos frames chegaram + if (!_depth_flag && !_rgb_flag) { + return; + } auto stamp = now(); - rgb_header.stamp = stamp; - depth_header.stamp = stamp; - + std_msgs::msg::Header rgb_header, depth_header; + rgb_header.frame_id = "kinect_rgb"; rgb_header.stamp = stamp; + depth_header.frame_id = "kinect_depth"; depth_header.stamp = stamp; rgb_info_.header = rgb_header; depth_info_.header = depth_header; - if (_depth_flag) { - auto msg = cv_bridge::CvImage(depth_header, "16UC1", _depth_image).toImageMsg(); depth_pub_.publish(*msg, depth_info_); _depth_flag = false; } - if (_rgb_flag) { auto msg = cv_bridge::CvImage(rgb_header, "rgb8", _rgb_image).toImageMsg(); rgb_pub_.publish(*msg, rgb_info_); From 54a775fc2e84837bf88891369187c6c00c2b2be5 Mon Sep 17 00:00:00 2001 From: "Rinaldo T.S Filho" Date: Fri, 13 Jun 2025 14:29:53 -0300 Subject: [PATCH 09/12] Update kinect_ros2_component.hpp --- include/kinect_ros2/kinect_ros2_component.hpp | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/include/kinect_ros2/kinect_ros2_component.hpp b/include/kinect_ros2/kinect_ros2_component.hpp index 1852df7..3a9f234 100755 --- a/include/kinect_ros2/kinect_ros2_component.hpp +++ b/include/kinect_ros2/kinect_ros2_component.hpp @@ -34,10 +34,7 @@ class KinectRosComponent : public rclcpp::Node static void rgb_cb(freenect_device * dev, void * rgb_ptr, uint32_t timestamp); void timer_callback(); - - // processamento contínuo de eventos do Kinect - std::thread processing_thread_; }; } -#endif // KINECT_ROS2__KINECT_ROS2_COMPONENT_HPP_ \ No newline at end of file +#endif // KINECT_ROS2__KINECT_ROS2_COMPONENT_HPP_ From fc41fe94a099adfa30f72d76504f337d29c93d6a Mon Sep 17 00:00:00 2001 From: "Rinaldo T.S Filho" Date: Fri, 13 Jun 2025 14:30:11 -0300 Subject: [PATCH 10/12] Update kinect_ros2_component.cpp --- src/kinect_ros2_component.cpp | 52 +++++++++++++---------------------- 1 file changed, 19 insertions(+), 33 deletions(-) diff --git a/src/kinect_ros2_component.cpp b/src/kinect_ros2_component.cpp index b02fdf8..c03c892 100755 --- a/src/kinect_ros2_component.cpp +++ b/src/kinect_ros2_component.cpp @@ -1,7 +1,5 @@ #include "kinect_ros2/kinect_ros2_component.hpp" #include -#include -#include using namespace std::chrono_literals; @@ -19,9 +17,8 @@ static bool _rgb_flag; KinectRosComponent::KinectRosComponent(const rclcpp::NodeOptions & options) : Node("kinect_ros2", options) { + timer_ = create_wall_timer(33ms, std::bind(&KinectRosComponent::timer_callback, this)); - timer_ = create_wall_timer(1ms, std::bind(&KinectRosComponent::timer_callback, this)); - std::string pkg_share = ament_index_cpp::get_package_share_directory("kinect_ros2"); //todo: use parameters @@ -38,11 +35,8 @@ KinectRosComponent::KinectRosComponent(const rclcpp::NodeOptions & options) depth_info_ = depth_info_manager_->getCameraInfo(); depth_info_.header.frame_id = "kinect_depth"; - // uso de QoS sensor_data (best effort) - depth_pub_ = image_transport::create_camera_publisher( - this, "depth/image_raw", rmw_qos_profile_sensor_data); - rgb_pub_ = image_transport::create_camera_publisher( - this, "image_raw", rmw_qos_profile_sensor_data); + depth_pub_ = image_transport::create_camera_publisher(this, "depth/image_raw"); + rgb_pub_ = image_transport::create_camera_publisher(this, "image_raw"); int ret = freenect_init(&fn_ctx_, NULL); if (ret < 0) { @@ -96,23 +90,10 @@ KinectRosComponent::KinectRosComponent(const rclcpp::NodeOptions & options) RCLCPP_ERROR(get_logger(), "FREENECT - ERROR START RGB"); rclcpp::shutdown(); } - - // thread não bloqueante para processar eventos - processing_thread_ = std::thread([this]() { - struct timeval tv{0, 10000}; // timeout de 10μs - while (rclcpp::ok()) { - freenect_process_events_timeout(fn_ctx_, &tv); - } - }); } KinectRosComponent::~KinectRosComponent() { - // garante que a thread seja finalizada - if (processing_thread_.joinable()) { - processing_thread_.join(); - } - RCLCPP_INFO(get_logger(), "stoping kinnect"); freenect_stop_depth(fn_dev_); freenect_stop_video(fn_dev_); @@ -127,8 +108,6 @@ to a new cv::Mat. This way, the callback only used to set a flag that indicates has arrived. The flag is unset when a msg is published */ void KinectRosComponent::depth_cb(freenect_device * dev, void * depth_ptr, uint32_t timestamp) { - (void)dev; - (void)timestamp; if (_depth_flag) { return; } @@ -143,8 +122,6 @@ void KinectRosComponent::depth_cb(freenect_device * dev, void * depth_ptr, uint3 void KinectRosComponent::rgb_cb(freenect_device * dev, void * rgb_ptr, uint32_t timestamp) { - (void)dev; - (void)timestamp; if (_rgb_flag) { return; } @@ -159,26 +136,37 @@ void KinectRosComponent::rgb_cb(freenect_device * dev, void * rgb_ptr, uint32_t void KinectRosComponent::timer_callback() { - // somente publica se novos frames chegaram - if (!_depth_flag && !_rgb_flag) { - return; - } + freenect_process_events(fn_ctx_); + auto stamp = now(); std_msgs::msg::Header rgb_header, depth_header; rgb_header.frame_id = "kinect_rgb"; rgb_header.stamp = stamp; depth_header.frame_id = "kinect_depth"; depth_header.stamp = stamp; rgb_info_.header = rgb_header; depth_info_.header = depth_header; - + if (_depth_flag) { + //convert 16bit to 8bit mono + // cv::Mat depth_8UC1(_depth_image, CV_16UC1); + // depth_8UC1.convertTo(depth_8UC1, CV_8UC1); auto msg = cv_bridge::CvImage(depth_header, "16UC1", _depth_image).toImageMsg(); + depth_pub_.publish(*msg, depth_info_); + + // cv::imshow("Depth", _depth_image); + // cv::waitKey(1); _depth_flag = false; } + if (_rgb_flag) { + + auto msg = cv_bridge::CvImage(rgb_header, "rgb8", _rgb_image).toImageMsg(); rgb_pub_.publish(*msg, rgb_info_); + + // cv::imshow("RGB", _rgb_image); + // cv::waitKey(1); _rgb_flag = false; } } @@ -186,5 +174,3 @@ void KinectRosComponent::timer_callback() #include "rclcpp_components/register_node_macro.hpp" RCLCPP_COMPONENTS_REGISTER_NODE(kinect_ros2::KinectRosComponent) - - From 96c207911e19138afb815ba2834929c0b2b64feb Mon Sep 17 00:00:00 2001 From: "Rinaldo T.S Filho" Date: Fri, 13 Jun 2025 14:30:25 -0300 Subject: [PATCH 11/12] Update kinect_ros2_node.cpp From 4c0fdb762b6c970e6feb2fb8532885e7f118b945 Mon Sep 17 00:00:00 2001 From: rinaldots Date: Mon, 23 Jun 2025 15:05:36 -0300 Subject: [PATCH 12/12] ok --- src/kinect_ros2_component.cpp | 15 ++------------- src/kinect_ros2_node.cpp | 2 +- 2 files changed, 3 insertions(+), 14 deletions(-) diff --git a/src/kinect_ros2_component.cpp b/src/kinect_ros2_component.cpp index c03c892..f770342 100755 --- a/src/kinect_ros2_component.cpp +++ b/src/kinect_ros2_component.cpp @@ -17,7 +17,7 @@ static bool _rgb_flag; KinectRosComponent::KinectRosComponent(const rclcpp::NodeOptions & options) : Node("kinect_ros2", options) { - timer_ = create_wall_timer(33ms, std::bind(&KinectRosComponent::timer_callback, this)); + timer_ = create_wall_timer(1ms, std::bind(&KinectRosComponent::timer_callback, this)); std::string pkg_share = ament_index_cpp::get_package_share_directory("kinect_ros2"); @@ -134,6 +134,7 @@ void KinectRosComponent::rgb_cb(freenect_device * dev, void * rgb_ptr, uint32_t _rgb_flag = true; } + void KinectRosComponent::timer_callback() { freenect_process_events(fn_ctx_); @@ -147,26 +148,14 @@ void KinectRosComponent::timer_callback() depth_info_.header = depth_header; if (_depth_flag) { - //convert 16bit to 8bit mono - // cv::Mat depth_8UC1(_depth_image, CV_16UC1); - // depth_8UC1.convertTo(depth_8UC1, CV_8UC1); auto msg = cv_bridge::CvImage(depth_header, "16UC1", _depth_image).toImageMsg(); - depth_pub_.publish(*msg, depth_info_); - - // cv::imshow("Depth", _depth_image); - // cv::waitKey(1); _depth_flag = false; } if (_rgb_flag) { - - auto msg = cv_bridge::CvImage(rgb_header, "rgb8", _rgb_image).toImageMsg(); rgb_pub_.publish(*msg, rgb_info_); - - // cv::imshow("RGB", _rgb_image); - // cv::waitKey(1); _rgb_flag = false; } } diff --git a/src/kinect_ros2_node.cpp b/src/kinect_ros2_node.cpp index 95fcdf9..5d87b81 100755 --- a/src/kinect_ros2_node.cpp +++ b/src/kinect_ros2_node.cpp @@ -62,4 +62,4 @@ int main(int argc, char * argv[]) rclcpp::shutdown(); return 0; -} +} \ No newline at end of file