def call_back(self, msg): obj_list = ObjectPoseList() obj_list.header.frame_id = msg.header.frame_id obj_list.size = 0 cluster_num = len(msg.list) #pcl_size = len(msg.poses) for i in range(cluster_num): self.image = np.zeros((int(self.height), int(self.width), 3), np.uint8) img = [] pcl_size = len(msg.list[i].poses) avg_x = avg_y = avg_z = 0 for j in range(pcl_size): # project to XY, YZ, XZ plane avg_x = avg_x + msg.list[i].poses[j].position.x avg_y = avg_y + msg.list[i].poses[j].position.y avg_z = avg_z + msg.list[i].poses[j].position.z img.append([msg.list[i].poses[j].position.x, msg.list[i].poses[j].position.y, msg.list[i].poses[j].position.z]) self.toIMG(pcl_size, img, 'img') model_type = self.classify() # *************************************************************** # Add to object list # *************************************************************** obj = ObjectPose() obj.position.x = float(avg_x/pcl_size) obj.position.y = float(avg_y/pcl_size) obj.position.z = float(avg_z/pcl_size) obj.type = model_type obj_list.list.append(obj) #cv2.imwrite( "Image.jpg", self.image) #self.index = self.index + 1 #print "Save image" obj_list.size = cluster_num self.pub_obj.publish(obj_list) self.drawRviz(obj_list)
def call_back(self, msg): tf_points = PCL_points() tf_points = msg obj_list = ObjectPoseList() obj_list.header.frame_id = tf_points.header.frame_id obj_list.size = 0 cluster_num = len(tf_points.list) #pcl_size = len(msg.poses) for i in range(cluster_num): self.image = np.zeros((int(self.height), int(self.width), 3), np.uint8) plane_xy = [] plane_yz = [] plane_xz = [] pcl_size = len(tf_points.list[i].poses) # ======= Coordinate transform for better project performance ====== position = [0, 0, 0] rad = math.atan2(tf_points.centroids[i].y, tf_points.centroids[i].x) quaternion = tf.transformations.quaternion_from_euler(0., 0., -rad) transformer = tf.TransformerROS() transpose_matrix = transformer.fromTranslationRotation(position, quaternion) for m in range(pcl_size): new_x = tf_points.list[i].poses[m].position.x new_y = tf_points.list[i].poses[m].position.y new_z = tf_points.list[i].poses[m].position.z orig_point = np.array([new_x, new_y, new_z, 1]) new_center = np.dot(transpose_matrix, orig_point) tf_points.list[i].poses[m].position.x = new_center[0] tf_points.list[i].poses[m].position.y = new_center[1] tf_points.list[i].poses[m].position.z = new_center[2] # ======= project to XY, YZ, XZ plane ======= for j in range(pcl_size): plane_xy.append([tf_points.list[i].poses[j].position.x, tf_points.list[i].poses[j].position.y]) plane_yz.append([tf_points.list[i].poses[j].position.y, tf_points.list[i].poses[j].position.z]) plane_xz.append([tf_points.list[i].poses[j].position.x, tf_points.list[i].poses[j].position.z]) self.toIMG(pcl_size, plane_xy, 'xy') self.toIMG(pcl_size, plane_yz, 'yz') self.toIMG(pcl_size, plane_xz, 'xz') model_type = self.classify() # *************************************************************** # Add to object list # *************************************************************** obj = ObjectPose() obj.position.x = msg.centroids[i].x obj.position.y = msg.centroids[i].y obj.position.z = msg.centroids[i].z obj.varianceX = msg.varianceX obj.varianceY = msg.varianceY obj.type = model_type obj_list.list.append(obj) #cv2.imwrite( "Image" + str(self.index) + ".jpg", self.image) #self.index = self.index + 1 #print "Save image" obj_list.size = cluster_num self.pub_obj.publish(obj_list) self.drawRviz(obj_list)