def extract_and_fuse_single_scene(log_full_path, downsample=False): print "extracting and fusing scene:", log_full_path log = os.path.split(log_full_path)[-1] mem() if not already_extracted_rosbag(log_full_path): print "extracting", log_full_path bag_filepath = os.path.join(log_full_path, 'raw', 'fusion_' + log + '.bag') # run this memory intensive step in a process, to allow # it to release memory back to the operating system # when finished proc = mp.Process(target=extract_data_from_rosbag, args=(bag_filepath, )) proc.start() proc.join() print "finished extracting", log_full_path # counter_new_extracted += 1 else: print "already extracted", log_full_path processed_dir = os.path.join(log_full_path, 'processed') images_dir = os.path.join(processed_dir, 'images') # we need to free some memory gc.collect() mem() if not already_ran_tsdf_fusion(log_full_path): print "preparing for tsdf_fusion for", log_full_path tsdf_fusion.format_data_for_tsdf(images_dir) gc.collect() print "running tsdf fusion" tsdf_fusion.run_tsdf_fusion_cuda(images_dir) gc.collect() print "converting tsdf to ply" tsdf_bin_filename = os.path.join(processed_dir, 'tsdf.bin') tsdf_mesh_filename = os.path.join(processed_dir, 'fusion_mesh.ply') tsdf_fusion.convert_tsdf_to_ply(tsdf_bin_filename, tsdf_mesh_filename) # counter_new_fused += 1 else: print "already ran tsdf_fusion for", log_full_path if not already_downsampled(log_full_path) and downsample: fs = FusionServer() print "downsampling image folder" linear_distance_threshold = 0.03 angle_distance_threshold = 10 # in degrees fs.downsample_by_pose_difference_threshold(images_dir, linear_distance_threshold, angle_distance_threshold) # counter_new_downsampled += 1 else: print "already downsampled for", log_full_path
def main(): print "preparing for tsdf_fusion for" tsdf_fusion.format_data_for_tsdf(images_dir) print "running tsdf fusion" tsdf_fusion.run_tsdf_fusion_cuda(images_dir) print "converting tsdf to ply" tsdf_bin_filename = os.path.join(processed_dir, 'tsdf.bin') tsdf_mesh_filename = os.path.join(processed_dir, 'fusion_mesh.ply') tsdf_fusion.convert_tsdf_to_ply(tsdf_bin_filename, tsdf_mesh_filename)
def extract_and_fuse_single_scene(log_full_path, downsample=False): print "extracting and fusing scene:", log_full_path if not already_extracted_rosbag(log_full_path): print "extracting", log_full_path bag_filepath = os.path.join(log_full_path, 'raw', 'fusion_' + log + '.bag') processed_dir, images_dir = fs.extract_data_from_rosbag(bag_filepath) # counter_new_extracted += 1 else: print "already extracted", log_full_path processed_dir = os.path.join(log_full_path, 'processed') images_dir = os.path.join(processed_dir, 'images') if not already_ran_tsdf_fusion(log_full_path): print "preparing for tsdf_fusion for", log_full_path tsdf_fusion.format_data_for_tsdf(images_dir) print "running tsdf fusion" tsdf_fusion.run_tsdf_fusion_cuda(images_dir) print "converting tsdf to ply" tsdf_bin_filename = os.path.join(processed_dir, 'tsdf.bin') tsdf_mesh_filename = os.path.join(processed_dir, 'fusion_mesh.ply') tsdf_fusion.convert_tsdf_to_ply(tsdf_bin_filename, tsdf_mesh_filename) # counter_new_fused += 1 else: print "already ran tsdf_fusion for", log_full_path if not already_downsampled(log_full_path) and downsample: print "downsampling image folder" linear_distance_threshold = 0.03 angle_distance_threshold = 10 # in degrees fs.downsample_by_pose_difference_threshold(images_dir, linear_distance_threshold, angle_distance_threshold) # counter_new_downsampled += 1 else: print "already downsampled for", log_full_path
def extract_and_fuse_single_scene( log_full_path, downsample=True, linear_distance_threshold=LINEAR_DISTANCE_THRESHOLD, angle_distance_threshold=ANGLE_DISTANCE_THRESHOLD): """ Extracts all the images from the rosbag. :param log_full_path: :param downsample: whether or not to downsample :param linear_distance_threshold: threshold used in downsampling :param angle_distance_threshold: threshold used in downsampling :return: """ global fs if fs is None: fs = FusionServer() print "extracting and fusing scene:", log_full_path log = os.path.split(log_full_path)[-1] mem() processed_dir = os.path.join(log_full_path, 'processed') images_dir = os.path.join(processed_dir, 'images') raw_dir = os.path.join(log_full_path, 'raw') raw_close_up_dir = os.path.join(log_full_path, 'raw_close_up') bag_filepath = os.path.join(log_full_path, 'raw', 'fusion_' + log + '.bag') bag_close_up_filepath = os.path.join(raw_close_up_dir, 'fusion_' + log + '.bag') pose_data_filename = os.path.join(images_dir, 'pose_data.yaml') metadata_filename = os.path.join(images_dir, 'metadata.yaml') if not already_extracted_rosbag(log_full_path): print "extracting", bag_filepath # run this memory intensive step in a process, to allow # it to release memory back to the operating system # when finished proc = mp.Process(target=extract_data_from_rosbag, args=(bag_filepath, images_dir)) proc.start() proc.join() print "finished extracting", bag_filepath # counter_new_extracted += 1 else: print "already extracted", bag_filepath # we need to free some memory gc.collect() mem() if not already_ran_tsdf_fusion(log_full_path): print "preparing for tsdf_fusion for", log_full_path tsdf_fusion.format_data_for_tsdf(images_dir) gc.collect() print "running tsdf fusion" fusion_params = FUSION_CONFIG[FUSION_LOCATION] tsdf_fusion.run_tsdf_fusion_cuda( images_dir, bbox_min=fusion_params['bbox_min'], bbox_max=fusion_params['bbox_max'], voxel_size=fusion_params['voxel_size']) gc.collect() print "converting tsdf to ply" tsdf_bin_filename = os.path.join(processed_dir, 'tsdf.bin') tsdf_mesh_filename = os.path.join(processed_dir, 'fusion_mesh.ply') tsdf_fusion.convert_tsdf_to_ply(tsdf_bin_filename, tsdf_mesh_filename) # counter_new_fused += 1 else: print "already ran tsdf_fusion for", log_full_path if not already_downsampled(log_full_path) and downsample: print "downsampling image folder" # fs = FusionServer() fs.downsample_by_pose_difference_threshold(images_dir, linear_distance_threshold, angle_distance_threshold) # counter_new_downsampled += 1 # create metadata.yaml file if it doesn't yet exist if not os.path.exists(metadata_filename): pose_data = spartanUtils.getDictFromYamlFilename( pose_data_filename) original_image_indices = pose_data.keys() metadata = dict() metadata['original_image_indices'] = original_image_indices metadata['close_up_image_indices'] = [] spartanUtils.saveToYaml(metadata, metadata_filename) else: print "already downsampled for", log_full_path # extract the up close up log if it exists if os.path.exists( bag_close_up_filepath ) and not already_extracted_close_up_log(metadata_filename): print "\n\n-----------extracting close up images----------\n\n" print "extracting", bag_close_up_filepath close_up_temp_dir = os.path.join(processed_dir, 'images_close_up') # run this memory intensive step in a process, to allow # it to release memory back to the operating system # when finished proc = mp.Process(target=extract_data_from_rosbag, args=(bag_close_up_filepath, close_up_temp_dir)) proc.start() proc.join() print "downsampling image folder %s" % (close_up_temp_dir) if downsample: fs.downsample_by_pose_difference_threshold( close_up_temp_dir, linear_distance_threshold, angle_distance_threshold) pose_data_close_up_filename = os.path.join(close_up_temp_dir, 'pose_data.yaml') pose_data_close_up = spartanUtils.getDictFromYamlFilename( pose_data_close_up_filename) pose_data = spartanUtils.getDictFromYamlFilename(pose_data_filename) original_image_indices = pose_data.keys() largest_original_image_idx = max(original_image_indices) close_up_image_indices = [] print "largest_original_image_idx", largest_original_image_idx for img_idx, data in pose_data_close_up.iteritems(): # just to give some buffer and avoid edge cases in some checks img_idx_new = img_idx + largest_original_image_idx + 4 rgb_filename = "%06i_%s.png" % (img_idx_new, "rgb") depth_filename = "%06i_%s.png" % (img_idx_new, "depth") rgb_filename_prev = data['rgb_image_filename'] depth_filename_prev = data['depth_image_filename'] data['rgb_image_filename'] = rgb_filename data['depth_image_filename'] = depth_filename pose_data[img_idx_new] = data close_up_image_indices.append(img_idx_new) # move images around shutil.move(os.path.join(close_up_temp_dir, rgb_filename_prev), os.path.join(images_dir, rgb_filename)) shutil.move(os.path.join(close_up_temp_dir, depth_filename_prev), os.path.join(images_dir, depth_filename)) shutil.rmtree(close_up_temp_dir) original_image_indices.sort() close_up_image_indices.sort() spartanUtils.saveToYaml(pose_data, pose_data_filename) metadata = dict() metadata['normal_image_indices'] = original_image_indices metadata['close_up_image_indices'] = close_up_image_indices spartanUtils.saveToYaml(metadata, metadata_filename) else: if os.path.exists(bag_close_up_filepath): print "already extracted close up log, skipping" else: print "raw_close_up doesn't exist, skipping"
except rospy.ServiceException, e: print "Service call failed: %s" % e # publish the pointcloud to RVIZ elastic_fusion_output = resp3.fusion_output self.cache['fusion_output'] = elastic_fusion_output self.publish_pointcloud_to_rviz( elastic_fusion_output.point_cloud, self.cache['point_cloud_to_world_stamped']) response = CaptureSceneAndFuseResponse(elastic_fusion_output) elif self.config['fusion_type'] == FusionType.TSDF_FUSION: print "formatting data for tsdf fusion" tsdf_fusion.format_data_for_tsdf(images_dir) print "running tsdf fusion" tsdf_fusion.run_tsdf_fusion_cuda(images_dir) print "converting tsdf to ply" tsdf_bin_filename = os.path.join(processed_dir, 'tsdf.bin') tsdf_mesh_filename = os.path.join(processed_dir, 'fusion_mesh.ply') tsdf_fusion.convert_tsdf_to_ply(tsdf_bin_filename, tsdf_mesh_filename) # the response is not meaningful right now response = CaptureSceneAndFuseResponse() else: raise ValueError('unknown fusion type')
class FusionServer(object): def __init__(self, camera_serial_number="carmine_1"): self.camera_serial_number = camera_serial_number self.bagging = False self.rosbag_proc = None self.tfBuffer = None storedPosesFile = os.path.join(spartanUtils.getSpartanSourceDir(), 'src', 'catkin_projects', 'station_config', 'RLG_iiwa_1', 'stored_poses.yaml') self.storedPoses = spartanUtils.getDictFromYamlFilename( storedPosesFile) self.robotService = rosUtils.RobotService( self.storedPoses['header']['joint_names']) self.setupConfig() self.setupSubscribers() self.setupPublishers() self.setupTF() self.setupCache() def setupConfig(self): self.config = dict() self.config['scan'] = dict() #self.config['scan']['pose_list'] = ['scan_back', 'scan_left', 'scan_top', 'scan_right', 'scan_back'] # with the Grasping group #self.config['scan']['pose_group'] = 'Grasping' #self.config['scan']['pose_list'] = ['scan_left_close', 'scan_left', 'scan_left_center', 'scan_above_table_far', 'scan_right_center', 'scan_right', 'scan_right_close'] # regular far out scanning poses pose_list = [] pose_list.append(["Elastic Fusion", 'home']) pose_list.append(["Elastic Fusion", 'home_closer']) pose_list.append(["Elastic Fusion", 'center_right']) pose_list.append(["Elastic Fusion", 'right']) pose_list.append(["Elastic Fusion", 'right_low']) pose_list.append(["Elastic Fusion", 'right_low_closer']) pose_list.append(["Elastic Fusion", 'center_right']) pose_list.append(["Elastic Fusion", 'home_closer']) pose_list.append(["Elastic Fusion", 'center_left_closer']) pose_list.append(["Elastic Fusion", 'center_left_low_closer']) pose_list.append(["Elastic Fusion", 'left_low']) pose_list.append(["Elastic Fusion", 'left_mid']) pose_list.append(["Elastic Fusion", 'center_left_low']) pose_list.append(["Elastic Fusion", 'center_left_low_closer']) pose_list.append(["Elastic Fusion", 'center_left_closer']) pose_list.append(["Elastic Fusion", 'home_closer']) pose_list.append(["Elastic Fusion", 'top_down']) pose_list.append(["Elastic Fusion", 'top_down_right']) pose_list.append(["Elastic Fusion", 'top_down_left']) self.config['scan']['pose_list'] = pose_list # quick scan for testing purposes pose_list = [] pose_list.append(["Elastic Fusion", 'home']) pose_list.append(["Elastic Fusion", 'home_closer']) pose_list.append(["Elastic Fusion", 'top_down']) pose_list.append(["Elastic Fusion", 'top_down_right']) pose_list.append(["Elastic Fusion", 'top_down_left']) pose_list.append(["Elastic Fusion", 'home']) self.config['scan']['pose_list_quick'] = pose_list # close up scanning pose_list = [] pose_list.append(["close_up_scan", "center"]) pose_list.append(["close_up_scan", "center_2"]) pose_list.append(["close_up_scan", "center_3"]) pose_list.append(["close_up_scan", "center_back_1"]) pose_list.append(["close_up_scan", "center_back_2"]) pose_list.append(["close_up_scan", "center_back_3"]) pose_list.append(["close_up_scan", "center"]) pose_list.append(["close_up_scan", "right_1"]) pose_list.append(["close_up_scan", "right_2"]) pose_list.append(["close_up_scan", "right_3"]) pose_list.append(["close_up_scan", "right_4"]) pose_list.append(["close_up_scan", "far_right_1"]) # pose_list.append(["close_up_scan", "far_right_2"]) # pose_list.append(["close_up_scan", "far_right_1"]) pose_list.append(["close_up_scan", "right_1"]) pose_list.append(["close_up_scan", "center"]) pose_list.append(["close_up_scan", "left_1"]) pose_list.append(["close_up_scan", "left_2"]) pose_list.append(["close_up_scan", "left_3"]) pose_list.append(["close_up_scan", "far_left_1"]) pose_list.append(["close_up_scan", "far_left_2"]) pose_list.append(["close_up_scan", "extreme_left_1"]) pose_list.append(["close_up_scan", "extreme_left_2"]) pose_list.append(["close_up_scan", "extreme_left_3"]) pose_list.append(["close_up_scan", "left_1"]) pose_list.append(["close_up_scan", "center"]) self.config['scan']['close_up'] = pose_list self.config['speed'] = dict() self.config['speed']['scan'] = 25 self.config['speed']['fast'] = 30 self.config['speed']['wrist_rotation'] = 45 self.config['spin_rate'] = 1 self.config['home_pose_name'] = 'home' self.config['sleep_time_before_bagging'] = 3.0 self.config['world_frame'] = 'base' self.config[ 'camera_frame'] = "camera_" + self.camera_serial_number + "_rgb_optical_frame" self.config['sleep_time_at_each_pose'] = 0.01 self.config["reconstruction_frame_id"] = "fusion_reconstruction" self.config['fusion_type'] = FusionType.TSDF_FUSION self.topics_to_bag = [ "/tf", "/tf_static", "/camera_" + self.camera_serial_number + "/depth_registered/sw_registered/image_rect", "/camera_" + self.camera_serial_number + "/depth_registered/sw_registered/camera_info", "/camera_" + self.camera_serial_number + "/rgb/camera_info", "/camera_" + self.camera_serial_number + "/rgb/image_rect_color" ] self.topics_dict = dict() self.topics_dict[ 'rgb'] = "/camera_" + self.camera_serial_number + "/rgb/image_rect_color" self.topics_dict[ 'depth'] = "/camera_" + self.camera_serial_number + "/depth_registered/sw_registered/image_rect" self.topics_dict[ 'camera_info'] = "/camera_" + self.camera_serial_number + "/rgb/camera_info" def setupTF(self): tfWrapper = TFWrapper() tfBuffer = tfWrapper.getBuffer() if self.tfBuffer is None: self.tfBuffer = tfBuffer self.tf_broadcaster = tf2_ros.TransformBroadcaster() def setupSubscribers(self): self.subscribers = dict() self.subscribers['rgb'] = rosUtils.SimpleSubscriber( self.topics_dict['rgb'], sensor_msgs.msg.Image) self.subscribers['depth'] = rosUtils.SimpleSubscriber( self.topics_dict['depth'], sensor_msgs.msg.Image) def setupPublishers(self): topic_name = "fusion_reconstruction" self.pointcloud_publisher = rospy.Publisher( topic_name, sensor_msgs.msg.PointCloud2, queue_size=1) def startImageSubscribers(self): for key, sub in self.subscribers.iteritems(): sub.start() def stopImageSubscribers(self): for key, sub in self.subscribers.iteritems(): sub.stop() def setupCache(self): self.cache = dict() def flushCache(self): self.setupCache() """ Get transform from rgb_optical_frame to world """ def getRGBOpticalFrameToWorldTransform(self): rgbOpticalFrameToWorld = self.tfBuffer.lookup_transform( self.config['world_frame'], rosUtils.getRGBOpticalFrameName(self.camera_serial_number), rospy.Time(0)) return rgbOpticalFrameToWorld def start_bagging(self, bag_folder='logs_proto', full_path_to_bag_file=None): """ :param bag_folder: :type bag_folder: :param full_path_to_bag_file: (optional) full path to where we save bag :type full_path_to_bag_file: :return: :rtype: """ self.flushCache() if full_path_to_bag_file is None: base_path = os.path.join(spartanUtils.getSpartanSourceDir(), 'data_volume', 'pdc', bag_folder) log_id_name = spartanUtils.get_current_YYYY_MM_DD_hh_mm_ss() log_subdir = "raw" bagfile_directory = os.path.join(base_path, log_id_name, log_subdir) bagfile_name = "fusion_" + log_id_name full_path_to_bag_file = os.path.join(bagfile_directory, bagfile_name) # make bagfile directory with name os.system("mkdir -p " + bagfile_directory) # create parent folder if it doesn't exist parent_folder = os.path.dirname(full_path_to_bag_file) if not os.path.exists(parent_folder): os.makedirs(parent_folder) topics_to_bag = [ "/tf", "/tf_static", "/camera_" + self.camera_serial_number + "/depth_registered/sw_registered/image_rect", "/camera_" + self.camera_serial_number + "/depth_registered/sw_registered/camera_info", "/camera_" + self.camera_serial_number + "/rgb/camera_info", "/camera_" + self.camera_serial_number + "/rgb/image_rect_color" ] # add simple subscribers to fix xtion driver issues self.startImageSubscribers() # sleep to allow for xtion driver compression issues to be resolved rospy.sleep(self.config['sleep_time_before_bagging']) # get camera to world transform self.cache[ 'point_cloud_to_world_stamped'] = self.getRGBOpticalFrameToWorldTransform( ) transform_stamped = self.cache['point_cloud_to_world_stamped'] # build up command string rosbag_cmd = "rosbag record __name:=" + ROS_BAGGING_NODE_NAME rosbag_cmd += " -O " + full_path_to_bag_file for i in topics_to_bag: rosbag_cmd += " " + i # add some visibility print rosbag_cmd # start bagging self.bagging = True rosbag_proc = subprocess.Popen(rosbag_cmd, stdin=subprocess.PIPE, shell=True, cwd=parent_folder) rospy.loginfo("started image subscribers, sleeping for %d seconds", self.config['sleep_time_before_bagging']) return os.path.join(full_path_to_bag_file), rosbag_proc def _stop_bagging(self): """ Stops ROS bagging :return: :rtype: """ cmd = "rosnode kill /" + ROS_BAGGING_NODE_NAME print "cmd", cmd os.system(cmd) self.bagging = False self.stopImageSubscribers() def handle_start_bagging_fusion_data(self, req): ## check if bagging already if self.bagging: return StartBaggingFusionDataResponse("ERROR: Already bagging!") ## start bagging filepath, self.rosbag_proc = self.start_bagging() self.bagging = True ## return the full path string to the data print "Returning filepath" return StartBaggingFusionDataResponse(filepath) def handle_stop_bagging_fusion_data(self, req): ## check if bagging already if not self.bagging: return StopBaggingFusionDataResponse( "ERROR: Not currently bagging! Nothing to stop...") self._stop_bagging() return StopBaggingFusionDataResponse("success") def handle_perform_elastic_fusion(self, req): ## call executable for filename fusion_folder = os.path.join(os.path.dirname(req.bag_filepath), "images") cmd = ". /opt/ros/kinetic/setup.sh && $SPARTAN_SOURCE_DIR/src/ElasticFusion/GUI/build/ElasticFusion -f -q -t 900 -d 2 -l " + fusion_folder rospy.loginfo("elastic fusion cmd = %s", cmd) os.system(cmd) ply_filename = fusion_folder + ".ply" # of type sensor_msgs.msg.PointCloud2 point_cloud = self.convert_ply_to_pointcloud2( PlyData.read(ply_filename)) res = PerformElasticFusionResponse() res.fusion_output.pointcloud_filepath = ply_filename res.fusion_output.point_cloud = point_cloud res.fusion_output.point_cloud_to_world_stamped = self.cache[ 'point_cloud_to_world_stamped'] return res def convert_ply_to_pointcloud2(self, plydata): cloud_arr = np.zeros((len(plydata.elements[0].data), 3)) for i in xrange(len(plydata.elements[0].data)): cloud_arr[i] = list(plydata.elements[0].data[i])[:3] return array_to_xyz_pointcloud2f(cloud_arr) @staticmethod def get_numpy_position_from_pose(pose): x = pose["camera_to_world"]["translation"]["x"] y = pose["camera_to_world"]["translation"]["y"] z = pose["camera_to_world"]["translation"]["z"] return np.asarray([x, y, z]) @staticmethod def get_quaternion_from_pose(pose): quat = spartanUtils.getQuaternionFromDict(pose["camera_to_world"]) x = quat["x"] y = quat["y"] z = quat["z"] w = quat["w"] return np.asarray([w, x, y, z]) def get_joint_positions_for_pose(self, pose_data): """ Looks up the joint positions for a given pose :param pose_data: list of strings [group_name, pose_name]. e.g. ["General", "q_nom"] :return: list[double] of joint angles """ return copy.copy(self.storedPoses[pose_data[0]][pose_data[1]]) def _move_robot_through_pose_list(self, pose_list, randomize_wrist=False, hit_original_poses=True): """ Moves robot through the given list of poses :param pose_list: list of list of strings of form [pose_group, pose_name] :type pose_list: :param with_randomize_wrist: boolean flag on whether to randomizde wrist or not :type with_randomize_write: :return: :rtype: """ joint_limit_safety_factor = 0.9 wrist_joint_limit_degrees = 175.0 safe_wrist_joint_limit_radians = (wrist_joint_limit_degrees * np.pi / 180.0) * joint_limit_safety_factor for pose_data in pose_list: print "moving to", pose_data[1] joint_positions = self.get_joint_positions_for_pose(pose_data) if randomize_wrist: print "before randomize wrist", joint_positions joint_positions_random_wrist = copy.copy(joint_positions) joint_positions_random_wrist[-1] = np.random.uniform( -safe_wrist_joint_limit_radians, safe_wrist_joint_limit_radians) print "after randomize wrist", joint_positions self.robotService.moveToJointPosition( joint_positions_random_wrist, maxJointDegreesPerSecond=self.config['speed']['scan']) if hit_original_poses: self.robotService.moveToJointPosition( joint_positions, maxJointDegreesPerSecond=self.config['speed'] ['wrist_rotation']) else: self.robotService.moveToJointPosition( joint_positions, maxJointDegreesPerSecond=self.config['speed']['scan']) rospy.sleep(self.config['sleep_time_at_each_pose']) def capture_scene(self): """ This "moves around and captures all of the data needed for fusion". I.e., it: 1. Moves the robot "home" 2. Starts bagging all data needed for fusion 3. Moves the robot around 4. Stops bagging data 5. Moves the robot back home This is not a service handler itself, but intended to be modularly called by service handlers. :return: bag_filepath, the full path to where (one) of the rosbag (fusion-*.bag) was saved :rtype: string """ # first move home home_pose_joint_positions = self.storedPoses["Elastic Fusion"][ self.config['home_pose_name']] print home_pose_joint_positions self.robotService.moveToJointPosition( home_pose_joint_positions, maxJointDegreesPerSecond=self.config['speed']['fast']) print "moved to home" # Start bagging for far out data collection # base_path = os.path.join(spartanUtils.getSpartanSourceDir(), 'data_volume', 'pdc', 'logs_proto') base_path = os.path.join(spartanUtils.getSpartanSourceDir(), 'data_volume', 'pdc', 'logs_shoes') log_id_name = spartanUtils.get_current_YYYY_MM_DD_hh_mm_ss() log_subdir = "raw" bagfile_directory = os.path.join(base_path, log_id_name, log_subdir) bagfile_name = "fusion_" + log_id_name + ".bag" full_path_to_bagfile = os.path.join(bagfile_directory, bagfile_name) print "moving robot through regular scan poses" self.start_bagging(full_path_to_bag_file=full_path_to_bagfile) pose_list = self.config['scan']['pose_list'] joint_positions = self.get_joint_positions_for_pose(pose_list[0]) self.robotService.moveToJointPosition( joint_positions, maxJointDegreesPerSecond=self.config['speed']['scan']) # rospy.sleep(3.0) self._move_robot_through_pose_list(pose_list, randomize_wrist=True, hit_original_poses=True) self._stop_bagging() # # move robot through close up scan poses log_subdir = "raw_close_up" bagfile_directory = os.path.join(base_path, log_id_name, log_subdir) bagfile_name = "fusion_" + log_id_name + ".bag" full_path_to_bagfile = os.path.join(bagfile_directory, bagfile_name) # print "moving robot through close up scan poses" pose_list = self.config['scan']['close_up'] # move to first pose before we start bagging joint_positions = self.get_joint_positions_for_pose(pose_list[0]) self.robotService.moveToJointPosition( joint_positions, maxJointDegreesPerSecond=self.config['speed']['scan']) # now start bagging and move the robot through the poses self.start_bagging(full_path_to_bag_file=full_path_to_bagfile) self._move_robot_through_pose_list(pose_list, randomize_wrist=True, hit_original_poses=True) # rospy.sleep(3.0) self._stop_bagging() rospy.sleep(1.0) # move back home self.robotService.moveToJointPosition( home_pose_joint_positions, maxJointDegreesPerSecond=self.config['speed']['fast']) return full_path_to_bagfile def extract_data_from_rosbag(self, bag_filepath, images_dir=None, rgb_only=False): """ This wraps the ImageCapture calls to load and process the raw rosbags, to prepare for fusion. :param: bag_filepath, the full path to where the rosbag (fusion-*.bag) was saved :ptype: string :return: data dir, images_dir the full path to the directory where all the extracted data is saved and its images subdirectory :rtype: two strings, separated by commas """ # extract RGB and Depth images from Rosbag rgb_topic = self.topics_dict['rgb'] depth_topic = self.topics_dict['depth'] camera_info_topic = self.topics_dict['camera_info'] if images_dir is None: log_dir = os.path.dirname(os.path.dirname(bag_filepath)) processed_dir = os.path.join(log_dir, 'processed') images_dir = os.path.join(processed_dir, 'images') print "Using images_dir %s" % (images_dir) image_capture = ImageCapture(rgb_topic, depth_topic, camera_info_topic, self.config['camera_frame'], self.config['world_frame'], rgb_encoding='bgr8') image_capture.load_ros_bag(bag_filepath) image_capture.process_ros_bag(image_capture.ros_bag, images_dir, rgb_only=rgb_only) rospy.loginfo("Finished writing images to disk") return images_dir def handle_capture_scene(self, req): print "handling capture_scene" # Capture scene bag_filepath = self.capture_scene() response = CaptureSceneResponse() response.bag_filepath = bag_filepath rospy.loginfo("handle_capture_scene finished!") return response def handle_capture_scene_and_fuse(self, req): print "handling capture_scene_and_fuse" # Capture scene bag_filepath = self.capture_scene() # Extract images from bag processed_dir, images_dir = self.extract_data_from_rosbag(bag_filepath) if self.config['fusion_type'] == FusionType.ELASTIC_FUSION: # Perform fusion try: perform_elastic_fusion = rospy.ServiceProxy( 'perform_elastic_fusion', PerformElasticFusion) resp3 = perform_elastic_fusion(bag_filepath) except rospy.ServiceException, e: print "Service call failed: %s" % e # publish the pointcloud to RVIZ elastic_fusion_output = resp3.fusion_output self.cache['fusion_output'] = elastic_fusion_output self.publish_pointcloud_to_rviz( elastic_fusion_output.point_cloud, self.cache['point_cloud_to_world_stamped']) response = CaptureSceneAndFuseResponse(elastic_fusion_output) elif self.config['fusion_type'] == FusionType.TSDF_FUSION: print "formatting data for tsdf fusion" tsdf_fusion.format_data_for_tsdf(images_dir) print "running tsdf fusion" tsdf_fusion.run_tsdf_fusion_cuda(images_dir) print "converting tsdf to ply" tsdf_bin_filename = os.path.join(processed_dir, 'tsdf.bin') tsdf_mesh_filename = os.path.join(processed_dir, 'fusion_mesh.ply') tsdf_fusion.convert_tsdf_to_ply(tsdf_bin_filename, tsdf_mesh_filename) # the response is not meaningful right now response = CaptureSceneAndFuseResponse()