| 33 | # Cv=image vertical center = imageHight/2 |
| 34 | |
| 35 | class KinectPublisher: |
| 36 | def __init__(self,opt): |
| 37 | self.bridge_rgb = CvBridge() |
| 38 | self.msg_rgb = Image() |
| 39 | self.msg_seg = Image() |
| 40 | self.bridge_d = CvBridge() |
| 41 | self.msg_d = Image() |
| 42 | self.msg_info = CameraInfo() |
| 43 | self.msg_tf = TFMessage() |
| 44 | self.opt = opt |
| 45 | self.img_height = False |
| 46 | self.img_width = False |
| 47 | self.pitch_degree = self.opt.pitch_degree |
| 48 | # Prepare dataset folder to be saved |
| 49 | # self.dataset_path = os.path.join('segmentation',self.opt.dataset_path, self.opt.dataset_type) |
| 50 | # if not os.path.exists(self.dataset_path): |
| 51 | # os.makedirs(self.dataset_path) |
| 52 | # for data_type in ('rgb', 'label'): |
| 53 | # if not os.path.exists(os.path.join(self.dataset_path, data_type)): |
| 54 | # os.makedirs(os.path.join(self.dataset_path, data_type)) |
| 55 | |
| 56 | def getDepthImage(self,response_d): |
| 57 | img_depth = np.array(response_d.image_data_float, dtype=np.float32) |
| 58 | img_depth = img_depth.reshape(response_d.height, response_d.width) |
| 59 | if not self.img_height: |
| 60 | self.img_height = response_d.height |
| 61 | self.img_width = response_d.width |
| 62 | return img_depth |
| 63 | |
| 64 | def getRGBImage(self,response_rgb): |
| 65 | img1d = np.fromstring(response_rgb.image_data_uint8, dtype=np.uint8) |
| 66 | img_rgb = img1d.reshape(response_rgb.height, response_rgb.width, 3) |
| 67 | img_rgb = img_rgb[..., :3][..., ::-1] |
| 68 | return img_rgb |
| 69 | |
| 70 | def enhanceRGB(self,img_rgb): |
| 71 | lab = cv2.cvtColor(img_rgb, cv2.COLOR_BGR2LAB) |
| 72 | lab_planes = cv2.split(lab) |
| 73 | clahe = cv2.createCLAHE(clipLimit=2.5, tileGridSize=(10, 10)) |
| 74 | lab_planes[0] = clahe.apply(lab_planes[0]) |
| 75 | lab = cv2.merge(lab_planes) |
| 76 | img_rgb = cv2.cvtColor(lab, cv2.COLOR_LAB2BGR) |
| 77 | return img_rgb |
| 78 | |
| 79 | def GetCurrentTime(self): |
| 80 | self.ros_time = rospy.Time.now() |
| 81 | |
| 82 | def CreateRGBMessage(self,img_rgb): |
| 83 | self.msg_rgb.header.stamp = self.ros_time |
| 84 | self.msg_rgb.header.frame_id = "camera_rgb_optical_frame" |
| 85 | self.msg_rgb.encoding = "rgb8" |
| 86 | self.msg_rgb.height = self.img_height |
| 87 | self.msg_rgb.width = self.img_width |
| 88 | self.msg_rgb.data = self.bridge_rgb.cv2_to_imgmsg(img_rgb, "rgb8").data |
| 89 | self.msg_rgb.is_bigendian = 0 |
| 90 | self.msg_rgb.step = self.msg_rgb.width * 3 |
| 91 | return self.msg_rgb |
| 92 |
no outgoing calls
no test coverage detected