(self, rgb_msg, seg_msg, depth_msg)
| 142 | # o3d.io.write_point_cloud(os.path.join(self.path, 'cloud_map.ply'), cloud_open3d) |
| 143 | |
| 144 | def callback(self, rgb_msg, seg_msg, depth_msg): |
| 145 | # increase counter variable |
| 146 | self.cnt += 1 |
| 147 | if self.cnt % self.save_interval != 0: |
| 148 | # measure fps |
| 149 | currTime = time.time() |
| 150 | self.elapsedTime = currTime - self.prevTime |
| 151 | self.prevTime = currTime |
| 152 | self.fps = 1/self.elapsedTime |
| 153 | print("[SKIP FRAME] sec:{0}, fps:{1}".format(self.elapsedTime,self.fps)) |
| 154 | else: |
| 155 | try: |
| 156 | self.transform_msg = self.tf_listener._buffer.lookup_transform('camera_rgb_optical_frame','map', rgb_msg.header.stamp, |
| 157 | timeout=rospy.Duration(0.5)) |
| 158 | self.inv_transform_msg = self.tf_listener._buffer.lookup_transform('map','camera_rgb_optical_frame', rgb_msg.header.stamp, |
| 159 | timeout=rospy.Duration(0.5)) |
| 160 | |
| 161 | except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): |
| 162 | print('maximum wait time exceeded!! use the last transform_msg instead.') |
| 163 | pass |
| 164 | if self.opt.save_cloud: |
| 165 | merged_cloud_array = self._transform_cloud(self.transform_msg) # (num_points,6) |
| 166 | projected, keep, cloud_in_fov = self._filter_out_of_view_points(merged_cloud_array) # (num_filtered_points, 6) |
| 167 | |
| 168 | # split semantic seg. and change seg. |
| 169 | seg_array = self._img_msg2arr(seg_msg) |
| 170 | seg_classmap = self.seg_helper.colormap2classmap(seg_array) |
| 171 | seg_classmap, change_classmap = self.seg_helper.split_SemAndChange(seg_classmap) |
| 172 | |
| 173 | missing_mask = change_classmap == 2 |
| 174 | |
| 175 | seg_classmap[missing_mask] = torch.zeros_like(seg_classmap[missing_mask]) |
| 176 | seg_colormap = self.seg_helper.classmap2colormap(seg_classmap) |
| 177 | seg_colormap[missing_mask.squeeze()] = torch.zeros_like(seg_colormap[missing_mask.squeeze()]) |
| 178 | change_colormap = self.seg_helper.classmap2colormap(change_classmap) |
| 179 | seg_semantic_msg = self._img_arr2msg(seg_colormap.cpu().numpy()) |
| 180 | seg_change_msg = self._img_arr2msg(change_colormap.cpu().numpy()) |
| 181 | # img_proj = self._project_points_to_img(projected).cpu().numpy() # depreciated |
| 182 | |
| 183 | #projected = torch.cat([projected[:,:3]*scale.squeeze(),projected[:,3:]],dim=1) |
| 184 | if self.opt.save_cloud: |
| 185 | cloud_in_fov = cloud_in_fov.t().cpu() |
| 186 | cloud_in_FoV_msg = self._cloud_arr2pc_msg(cloud_in_fov.numpy(),frame_id="camera_rgb_optical_frame") |
| 187 | # publish |
| 188 | if self.opt.publish_2d: |
| 189 | # img_msg = self._img_arr2msg(img_proj) |
| 190 | # self.pub_imgfied_cloud.publish(img_msg) |
| 191 | self.pub_current_rgb.publish(rgb_msg) |
| 192 | self.pub_current_depth.publish(depth_msg) |
| 193 | self.pub_current_sem_seg.publish(seg_semantic_msg) |
| 194 | self.pub_current_change_seg.publish(seg_change_msg) |
| 195 | self.pub_cloud_in_fov.publish(cloud_in_FoV_msg) |
| 196 | |
| 197 | ''' |
| 198 | merged_cloud_xyz, merged_cloud_rgb = merged_cloud_array[:,:3],merged_cloud_array[:,3:] # (num_points,3) |
| 199 | projected_x,projected_y = projected[:,0],projected[:,1] |
| 200 | change_masks = [] |
| 201 | for change_class,color in zip(range(1,5),colormap): |
nothing calls this directly
no test coverage detected