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 index 5ce32de..f770342 --- a/src/kinect_ros2_component.cpp +++ b/src/kinect_ros2_component.cpp @@ -134,35 +134,28 @@ void KinectRosComponent::rgb_cb(freenect_device * dev, void * rgb_ptr, uint32_t _rgb_flag = true; } + void KinectRosComponent::timer_callback() { freenect_process_events(fn_ctx_); - auto header = std_msgs::msg::Header(); - header.frame_id = "kinect_depth"; + auto stamp = now(); - header.stamp = stamp; - depth_info_.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) { - //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; } } diff --git a/src/kinect_ros2_node.cpp b/src/kinect_ros2_node.cpp old mode 100644 new mode 100755 index 95fcdf9..5d87b81 --- 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