| 852 | } |
| 853 | |
| 854 | void* getFrames(void *i) { |
| 855 | if (_rgb_topic.empty()) |
| 856 | { |
| 857 | getFrameFromCam(i); |
| 858 | } |
| 859 | else |
| 860 | { |
| 861 | int global_counter = 1; |
| 862 | cv::Mat mat_from_frame, image_uchar; |
| 863 | double scale; |
| 864 | while(1) |
| 865 | { |
| 866 | |
| 867 | ROS_INFO_STREAM_COND(DEBUG > 1, "GETFRAME: Wait"); |
| 868 | std::unique_lock<std::mutex> lk(global.get_frames_mutex); |
| 869 | global.get_frames_cond.wait(lk, []{return |
| 870 | global.input_queue.size() < _max_size_input_queue;}); |
| 871 | ROS_INFO_STREAM_COND(DEBUG > 1, "GETFRAME: Gooo"); |
| 872 | if (global.quit_threads or !ros::ok()) |
| 873 | break; |
| 874 | { |
| 875 | ROS_INFO_STREAM_COND(DEBUG > 1, "GETFRAME: Wait CBK"); |
| 876 | std::lock_guard<std::mutex> lock(_callback_mutex); |
| 877 | ROS_INFO_STREAM_COND(DEBUG > 1, "GETFRAME: Locked CBK"); |
| 878 | if (global.quit_threads or !ros::ok()) |
| 879 | break; |
| 880 | cv_bridge::CvImagePtr cv_ptr; |
| 881 | try |
| 882 | { |
| 883 | cv_ptr = cv_bridge::toCvCopy(_rgb_frame_from_sensor, |
| 884 | sensor_msgs::image_encodings::BGR8); |
| 885 | } |
| 886 | catch (cv_bridge::Exception& e) |
| 887 | { |
| 888 | ROS_ERROR("cv_bridge exception: %s", e.what()); |
| 889 | break; |
| 890 | } |
| 891 | mat_from_frame = cv_ptr->image; |
| 892 | try |
| 893 | { |
| 894 | cv_ptr = cv_bridge::toCvCopy(_depth_frame_from_sensor, |
| 895 | sensor_msgs::image_encodings::TYPE_16UC1); |
| 896 | } |
| 897 | catch (cv_bridge::Exception& e) |
| 898 | { |
| 899 | ROS_ERROR("cv_bridge exception: %s", e.what()); |
| 900 | break; |
| 901 | } |
| 902 | _depth_image = cv_ptr->image; |
| 903 | _fx = _camera_info.K.at(0); |
| 904 | _fy = _camera_info.K.at(4); |
| 905 | _constant_x = 1.0f / _fx; |
| 906 | _constant_y = 1.0f / _fy; |
| 907 | _cx = _camera_info.K.at(2); |
| 908 | _cy = _camera_info.K.at(5); |
| 909 | if (mat_from_frame.cols/(double)mat_from_frame.rows> |
| 910 | DISPLAY_RESOLUTION_WIDTH/(double)DISPLAY_RESOLUTION_HEIGHT) { |
| 911 | scale = DISPLAY_RESOLUTION_WIDTH/(double)mat_from_frame.cols; |
nothing calls this directly
no test coverage detected