| 402 | return img_numpy |
| 403 | |
| 404 | def _save_pose(self): |
| 405 | x = self.inv_transform_msg.transform.translation.x |
| 406 | y = self.inv_transform_msg.transform.translation.y |
| 407 | z = self.inv_transform_msg.transform.translation.z |
| 408 | qx = self.inv_transform_msg.transform.rotation.x |
| 409 | qy = self.inv_transform_msg.transform.rotation.y |
| 410 | qz = self.inv_transform_msg.transform.rotation.z |
| 411 | qw = self.inv_transform_msg.transform.rotation.w |
| 412 | self.current_pose_numpy = np.array([x,y,z,qx,qy,qz,qw]) |
| 413 | tf = open(self.path+'/pose/{}.txt'.format(self.cnt//self.save_interval),mode='w') |
| 414 | tf.write('{} {} {} {} {} {} {}'.format(x,y,z,qx,qy,qz,qw)) |
| 415 | tf.close() |
| 416 | |
| 417 | opt = parser.parse_args() |
| 418 | rospy.init_node('cloud_to_img', anonymous=True) |