void SensorHandler::receiveMsg(const ComsPacket & msg) { if ( msg.getModuleId() != MODULE_ID) { return; } if ( msg.getPacketId() != DATA_PACKET_ID ) { return; } uint32_t s = msg.getSeqNumber(); if ( s == lastSeqNum_ ) { return; } if ( (s+1) != lastSeqNum_ ) { ROS_INFO("We appear to have missed USB packets"); ROS_INFO("S: %d", s); ROS_INFO("LastSeqNum_: %d", lastSeqNum_); } lastSeqNum_ = s; const uint8_t *data = msg.getPayload(); processStatus(data); processDepth(data); processINS(data); processPower(data); }
void processRGBD() { USE_TIMES_START( start ); struct timeval process_start, process_end; USE_TIMES_START( process_start ); int stream_depth_state = 0; int stream_rgb_state = 0; int count = 0; stream_depth_state = true; while ((stream_depth_state) && (count < 1000)) { stream_depth_state = processDepth(); ++count; } if(stream_depth_state) { printf("\nstream state error depth = %d, rgb = %d\n", stream_depth_state, stream_rgb_state); return; } count = 0; stream_rgb_state = true; while((stream_rgb_state) && (count < 100)) { stream_rgb_state = processRGB(); ++count; } USE_TIMES_END_SHOW ( process_start, process_end, "get RGBD time" ); if(stream_rgb_state) { printf("\nstream state error depth = %d, rgb = %d\n", stream_depth_state, stream_rgb_state); return; } USE_TIMES_START( process_start ); cv::Mat depth_frame(depth_stream.height, depth_stream.width, CV_8UC1, depth_frame_buffer); //added cv::Mat cv_img_depth(depth_stream.height, depth_stream.width, CV_32FC1); /*img_depth.encoding = sensor_msgs::image_encodings::TYPE_32FC1; img_depth.is_bigendian = 0; img_depth.step = sizeof(float) * depth_stream.width; img_depth.data.resize(depth_stream.width * depth_stream.height * sizeof(float));*/ // #ifdef V4L2_PIX_FMT_INZI cv::Mat ir_frame(depth_stream.height, depth_stream.width, CV_8UC1, ir_frame_buffer); #endif cv::Mat rgb_frame; #if USE_BGR24 rgb_frame = cv::Mat(rgb_stream.height, rgb_stream.width, CV_8UC3, rgb_frame_buffer); memcpy(rgb_frame_buffer, rgb_stream.fillbuf, rgb_stream.buflen); #else cv::Mat rgb_frame_yuv(rgb_stream.height, rgb_stream.width, CV_8UC2, rgb_frame_buffer); memcpy(rgb_frame_buffer, rgb_stream.fillbuf, rgb_stream.buflen); struct timeval cvt_start, cvt_end; USE_TIMES_START( cvt_start ); cv::cvtColor(rgb_frame_yuv,rgb_frame,CV_YUV2BGR_YUYV); USE_TIMES_END_SHOW ( cvt_start, cvt_end, "CV_YUV2BGR_YUYV time" ); #endif USE_TIMES_END_SHOW ( process_start, process_end, "new cv::Mat object RGBD time" ); USE_TIMES_START( process_start ); if(!resize_point_cloud) { realsense_xyz_cloud.reset(new pcl::PointCloud<pcl::PointXYZ>()); realsense_xyz_cloud->width = depth_stream.width; realsense_xyz_cloud->height = depth_stream.height; realsense_xyz_cloud->is_dense = false; realsense_xyz_cloud->points.resize(depth_stream.width * depth_stream.height); if(isHaveD2RGBUVMap) { realsense_xyzrgb_cloud.reset(new pcl::PointCloud<PointType>()); realsense_xyzrgb_cloud->width = depth_stream.width; realsense_xyzrgb_cloud->height = depth_stream.height; realsense_xyzrgb_cloud->is_dense = false; realsense_xyzrgb_cloud->points.resize(depth_stream.width * depth_stream.height); } resize_point_cloud = true; } USE_TIMES_END_SHOW ( process_start, process_end, "new point cloud object time" ); USE_TIMES_START( process_start ); cv::MatIterator_<float> it; it = cv_img_depth.begin<float>(); //depth value //#pragma omp parallel for for(int i=0; i<depth_stream.width * depth_stream.height; ++i) { float depth = 0.0f; #ifdef V4L2_PIX_FMT_INZI unsigned short* depth_ptr = (unsigned short*)((unsigned char*)(depth_stream.fillbuf) + i*3); unsigned char* ir_ptr = (unsigned char*)(depth_stream.fillbuf) + i*3+2; unsigned char ir_raw = *ir_ptr; ir_frame_buffer[i] = ir_raw; unsigned short depth_raw = *depth_ptr; depth = (float)depth_raw / depth_unit; #else unsigned short depth_raw = *((unsigned short*)(depth_stream.fillbuf) + i); depth = (float)depth_raw / depth_unit; #endif depth_frame_buffer[i] = depth ? 255 * (sensor_depth_max - depth) / sensor_depth_max : 0; if (it != cv_img_depth.end<float>()) { if(depth>0.000001f) *it = depth * depth_scale; else *it = std::numeric_limits<float>::quiet_NaN(); ++it; } else std::cout << " ********** out of data matrix" << std::endl; float uvx = -1.0f; float uvy = -1.0f; if(depth>0.000001f) { float pixel_x = (i % depth_stream.width) - depth_cx; float pixel_y = (i / depth_stream.width) - depth_cy; float zz = depth * depth_scale; realsense_xyz_cloud->points[i].x = pixel_x * zz * depth_fxinv; realsense_xyz_cloud->points[i].y = pixel_y * zz * depth_fyinv; realsense_xyz_cloud->points[i].z = zz; if(isHaveD2RGBUVMap) { realsense_xyzrgb_cloud->points[i].x = realsense_xyz_cloud->points[i].x; realsense_xyzrgb_cloud->points[i].y = realsense_xyz_cloud->points[i].y; realsense_xyzrgb_cloud->points[i].z = realsense_xyz_cloud->points[i].z; if(!getUVWithDXY(depth, i, uvx, uvy)) { int cx = (int)(uvx * rgb_stream.width + 0.5f); int cy = (int)(uvy * rgb_stream.height + 0.5f); if (cx >= 0 && cx < rgb_stream.width && cy >= 0 && cy < rgb_stream.height) { unsigned char *rgb = rgb_frame.data + (cx+cy*rgb_stream.width)*3; unsigned char r = rgb[2]; unsigned char g = rgb[1]; unsigned char b = rgb[0]; if(debug_depth_unit && pixel_x > -center_offset_pixel && pixel_x < center_offset_pixel && pixel_y > -center_offset_pixel && pixel_y < center_offset_pixel ) { center_z += zz; center_z_count++; realsense_xyzrgb_cloud->points[i].rgba = (0 << 24) | (0 << 16) | (0 << 8) | 255; } else { realsense_xyzrgb_cloud->points[i].rgba = (0 << 24) | (r << 16) | (g << 8) | b; } } else { realsense_xyzrgb_cloud->points[i].x = std::numeric_limits<float>::quiet_NaN(); realsense_xyzrgb_cloud->points[i].y = std::numeric_limits<float>::quiet_NaN(); realsense_xyzrgb_cloud->points[i].z = std::numeric_limits<float>::quiet_NaN(); realsense_xyzrgb_cloud->points[i].rgba = 0; } } else { realsense_xyzrgb_cloud->points[i].x = std::numeric_limits<float>::quiet_NaN(); realsense_xyzrgb_cloud->points[i].y = std::numeric_limits<float>::quiet_NaN(); realsense_xyzrgb_cloud->points[i].z = std::numeric_limits<float>::quiet_NaN(); realsense_xyzrgb_cloud->points[i].rgba = 0; } } } else { realsense_xyz_cloud->points[i].x = std::numeric_limits<float>::quiet_NaN(); realsense_xyz_cloud->points[i].y = std::numeric_limits<float>::quiet_NaN(); realsense_xyz_cloud->points[i].z = std::numeric_limits<float>::quiet_NaN(); if(isHaveD2RGBUVMap) { realsense_xyzrgb_cloud->points[i].x = std::numeric_limits<float>::quiet_NaN(); realsense_xyzrgb_cloud->points[i].y = std::numeric_limits<float>::quiet_NaN(); realsense_xyzrgb_cloud->points[i].z = std::numeric_limits<float>::quiet_NaN(); realsense_xyzrgb_cloud->points[i].rgba = 0; } } } USE_TIMES_END_SHOW ( process_start, process_end, "fill point cloud data time" ); if(debug_depth_unit && center_z_count) { if(usbContext && usbHandle) { realsenseTemperature = getRealsenseTemperature(usbHandle); } center_z /= center_z_count; printf("average center z value = %f temp = %d depth_unit = %f\n", center_z, realsenseTemperature, depth_unit); center_z_count = 0; } USE_TIMES_END_SHOW ( start, end, "process result time" ); #if SHOW_RGBD_FRAME cv::imshow("depth frame view", depth_frame); #ifdef V4L2_PIX_FMT_INZI cv::imshow("ir frame view", ir_frame); #endif cv::imshow("RGB frame view", rgb_frame); #endif //pub msgs head_sequence_id++; head_time_stamp = ros::Time::now(); pubRealSenseRGBImageMsg(rgb_frame); #ifdef V4L2_PIX_FMT_INZI pubRealSenseInfraredImageMsg(ir_frame); #endif //pubRealSenseDepthImageMsg(depth_frame); pubRealSenseDepthImageMsg32F(cv_img_depth); pubRealSensePointsXYZCloudMsg(realsense_xyz_cloud); if(isHaveD2RGBUVMap) { pubRealSensePointsXYZRGBCloudMsg(realsense_xyzrgb_cloud); } }
//Stage 6 void HodginDidItApp::updateVeil() { processDepth(); updateFboPoint(); updateFboWater(); }