(self)
| 113 | return self.msg_d |
| 114 | |
| 115 | def CreateInfoMessage(self): |
| 116 | |
| 117 | self.msg_info.header.frame_id = "camera_rgb_optical_frame" |
| 118 | self.msg_info.height = self.msg_rgb.height |
| 119 | self.msg_info.width = self.msg_rgb.width |
| 120 | self.msg_info.distortion_model = "plumb_bob" |
| 121 | |
| 122 | self.msg_info.D = [] |
| 123 | self.msg_info.D.append(CAMERA_K1) |
| 124 | self.msg_info.D.append(CAMERA_K2) |
| 125 | self.msg_info.D.append(CAMERA_P1) |
| 126 | self.msg_info.D.append(CAMERA_P2) |
| 127 | self.msg_info.D.append(CAMERA_P3) |
| 128 | |
| 129 | CAMERA_FX = self.img_width // 2 # 320 # focal length f = W / 2 / tan(FOV/2) = W/2 |
| 130 | CAMERA_FY = self.img_width // 2 # 320 |
| 131 | CAMERA_CX = self.img_width // 2 # 320 |
| 132 | CAMERA_CY = self.img_height // 2 # 240 |
| 133 | self.msg_info.K[0] = CAMERA_FX |
| 134 | self.msg_info.K[1] = 0 |
| 135 | self.msg_info.K[2] = CAMERA_CX |
| 136 | self.msg_info.K[3] = 0 |
| 137 | self.msg_info.K[4] = CAMERA_FY |
| 138 | self.msg_info.K[5] = CAMERA_CY |
| 139 | self.msg_info.K[6] = 0 |
| 140 | self.msg_info.K[7] = 0 |
| 141 | self.msg_info.K[8] = 1 |
| 142 | |
| 143 | self.msg_info.R[0] = 1 |
| 144 | self.msg_info.R[1] = 0 |
| 145 | self.msg_info.R[2] = 0 |
| 146 | self.msg_info.R[3] = 0 |
| 147 | self.msg_info.R[4] = 1 |
| 148 | self.msg_info.R[5] = 0 |
| 149 | self.msg_info.R[6] = 0 |
| 150 | self.msg_info.R[7] = 0 |
| 151 | self.msg_info.R[8] = 1 |
| 152 | |
| 153 | self.msg_info.P[0] = CAMERA_FX |
| 154 | self.msg_info.P[1] = 0 |
| 155 | self.msg_info.P[2] = CAMERA_CX |
| 156 | self.msg_info.P[3] = 0 |
| 157 | self.msg_info.P[4] = 0 |
| 158 | self.msg_info.P[5] = CAMERA_FY |
| 159 | self.msg_info.P[6] = CAMERA_CY |
| 160 | self.msg_info.P[7] = 0 |
| 161 | self.msg_info.P[8] = 0 |
| 162 | self.msg_info.P[9] = 0 |
| 163 | self.msg_info.P[10] = 1 |
| 164 | self.msg_info.P[11] = 0 |
| 165 | |
| 166 | self.msg_info.binning_x = self.msg_info.binning_y = 0 |
| 167 | self.msg_info.roi.x_offset = self.msg_info.roi.y_offset = self.msg_info.roi.height = self.msg_info.roi.width = 0 |
| 168 | self.msg_info.roi.do_rectify = False |
| 169 | self.msg_info.header.stamp = self.msg_rgb.header.stamp |
| 170 | return self.msg_info |
| 171 | |
| 172 | def parse_lidarData(self, data): |
no outgoing calls
no test coverage detected