| 1846 | } |
| 1847 | |
| 1848 | void* displayFrame(void *i) { //single thread |
| 1849 | Frame frame; |
| 1850 | while(1) { |
| 1851 | if (global.quit_threads or !ros::ok()) |
| 1852 | break; |
| 1853 | ROS_INFO_STREAM_COND(DEBUG > 1, "DSPFRAME: Wait"); |
| 1854 | std::unique_lock<std::mutex> lk(global.output_queue_ordered_mutex); |
| 1855 | global.output_queue_ordered_cond.wait(lk, [](){return |
| 1856 | global.output_queue_ordered.size() > 0;}); |
| 1857 | ROS_INFO_STREAM_COND(DEBUG > 1, "DSPFRAME: Gooo"); |
| 1858 | frame = global.output_queue_ordered.pop(); |
| 1859 | double tic = get_wall_time(); |
| 1860 | cv::Mat wrap_frame(DISPLAY_RESOLUTION_HEIGHT, DISPLAY_RESOLUTION_WIDTH, |
| 1861 | CV_8UC3, frame.data_for_wrap); |
| 1862 | cv::Mat depth_frame; |
| 1863 | { |
| 1864 | ROS_INFO_STREAM_COND(DEBUG > 1, "DSPFRAME: QueueWait"); |
| 1865 | std::lock_guard<std::recursive_mutex> lock(_depth_queue_mutex); |
| 1866 | if (global.quit_threads or !ros::ok()) |
| 1867 | break; |
| 1868 | depth_frame = _depth_queue[frame.index]; |
| 1869 | _depth_queue.erase(frame.index); |
| 1870 | ROS_INFO_STREAM_COND(DEBUG > 1, "DSPFRAME: QueueRelease"); |
| 1871 | } |
| 1872 | // if(!_no_display) |
| 1873 | // { |
| 1874 | #if DEBUG > 0 |
| 1875 | depth_frame.convertTo(_colored_depth_image, CV_8UC1); |
| 1876 | //cv::equalizeHist(_colored_depth_image,_colored_depth_image); |
| 1877 | cv::cvtColor(_colored_depth_image, _colored_depth_image, CV_GRAY2RGB); |
| 1878 | #endif |
| 1879 | // } |
| 1880 | if (!_skeleton_topic_to_publish.empty() && !_rgb_topic.empty()) // publish |
| 1881 | { |
| 1882 | double scale = 1.0/frame.scale; |
| 1883 | const int num_parts = |
| 1884 | net_copies.at(0).up_model_descriptor->get_number_parts(); |
| 1885 | char fname[256]; |
| 1886 | |
| 1887 | // visualization_msgs::MarkerArray skeleton_marker_array; |
| 1888 | rtpose_wrapper::SkeletonArrayMsg skeleton_array; |
| 1889 | skeleton_array.header = |
| 1890 | _depth_frame_from_sensor.header; |
| 1891 | skeleton_array.header.stamp = |
| 1892 | ros::Time::now(); |
| 1893 | skeleton_array.rgb_header = _depth_frame_from_sensor.header; |
| 1894 | skeleton_array.intrinsic_matrix.reserve(9); |
| 1895 | for(int i = 0; i < 3; i++) |
| 1896 | for(int j = 0; j < 3; j++) |
| 1897 | skeleton_array.intrinsic_matrix.push_back(_camera_info.K[i * 3 + j]); |
| 1898 | |
| 1899 | { |
| 1900 | for(int person = 0; person < frame.numPeople and not |
| 1901 | (global.quit_threads or not ros::ok()); ++person) |
| 1902 | { |
| 1903 | rtpose_wrapper::SkeletonMsg skeleton; |
| 1904 | skeleton.joints.resize(num_parts); |
| 1905 | for(int joint = 0; joint < 15 and not |
nothing calls this directly
no test coverage detected