def setupRLGDirector(globalsDict=None): tfWrapper = TFWrapper() tfBuffer = tfWrapper.getBuffer() graspSupervisor = spartan.manipulation.grasp_supervisor.GraspSupervisor.makeDefault( tfBuffer=tfBuffer) graspSupervisor.robotSystem = globalsDict[ 'robotSystem'] # for visualization globalsDict['graspSupervisor'] = graspSupervisor backgroundSubtraction = spartan.manipulation.background_subtraction.BackgroundSubtractionDataCapture.makeDefault( tfBuffer=tfBuffer) globalsDict['backgroundSubtraction'] = backgroundSubtraction spartanSourceDir = spartanUtils.getSpartanSourceDir() handEyeCalibrationConfigFilename = os.path.join( spartanSourceDir, "src/catkin_projects/station_config/RLG_iiwa_1/hand_eye_calibration/carmine_1.yaml" ) cal = spartan.calibration.handeyecalibration.HandEyeCalibration( globalsDict['robotSystem'], configFilename=handEyeCalibrationConfigFilename) cal.loadConfigFromFile() globalsDict['cal'] = cal # set rate limit on RemoteTreeViewer # fix for https://github.com/RobotLocomotion/spartan/issues/244 globalsDict['treeViewer'].subscriber.setSpeedLimit(5)
def makeDefault(**kwargs): graspingParamsFile = os.path.join(spartanUtils.getSpartanSourceDir(), 'src', 'catkin_projects', 'station_config', 'RLG_iiwa_1', 'manipulation', 'params.yaml') return GraspSupervisor(graspingParamsFile=graspingParamsFile, **kwargs)
def makeDefault(): storedPosesFile = os.path.join(spartanUtils.getSpartanSourceDir(), 'src', 'catkin_projects', 'station_config', 'RLG_iiwa_1', 'stored_poses.yaml') return GraspSupervisor(storedPosesFile=storedPosesFile)
def setupRLGDirector(globalsDict=None): tfWrapper = TFWrapper() tfBuffer = tfWrapper.getBuffer() graspSupervisor = spartan.manipulation.grasp_supervisor.GraspSupervisor.makeDefault( tfBuffer=tfBuffer) graspSupervisor.robotSystem = globalsDict[ 'robotSystem'] # for visualization globalsDict['graspSupervisor'] = graspSupervisor backgroundSubtraction = spartan.manipulation.background_subtraction.BackgroundSubtractionDataCapture.makeDefault( tfBuffer=tfBuffer) globalsDict['backgroundSubtraction'] = backgroundSubtraction spartanSourceDir = spartanUtils.getSpartanSourceDir() handEyeCalibrationConfigFilename = os.path.join( spartanSourceDir, "src/catkin_projects/station_config/RLG_iiwa_1/hand_eye_calibration/carmine_1.yaml" ) cal = spartan.calibration.handeyecalibration.HandEyeCalibration( globalsDict['robotSystem'], configFilename=handEyeCalibrationConfigFilename) cal.loadConfigFromFile() globalsDict['cal'] = cal
def saveSingleImage(self, topic, filename, encoding): rosImageLoggerExecutable = os.path.join( spartanUtils.getSpartanSourceDir(), 'modules', "spartan", 'calibration', 'ros_image_logger.py') cmd = "%s -t %s -f %s -e %s" % (rosImageLoggerExecutable, topic, filename, encoding) os.system(cmd)
def saveCalibrationData(self, filename=None): if filename is None: filename = os.path.join(spartanUtils.getSpartanSourceDir(), 'sandbox', 'hand_eye_calibration_robot_data.yaml') spartanUtils.saveToYaml(self.calibrationData, filename)
def __init__(self): self.bagging = False self.rosbag_proc = 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()
def displayChessboardDetection(self, filename, duration): chessboardDetetctionVisualizerExecutable = os.path.join( spartanUtils.getSpartanSourceDir(), 'modules', "spartan", 'calibration', 'chessboard_detection_visualizer.py') cmd = "timeout %s %s -i %s" % ( str(duration), chessboardDetetctionVisualizerExecutable, filename) os.system(cmd)
def run_tsdf_fusion_cuda(image_folder, output_dir=None, voxel_grid_origin_x=0.4, voxel_grid_origin_y=-0.3, voxel_grid_origin_z=-0.2, voxel_size=0.0025, voxel_grid_dim_x=240, voxel_grid_dim_y=320, voxel_grid_dim_z=280, fast_tsdf_settings=False): """ Simple wrapper to call the tsdf-fusion executable with the desired args """ if output_dir is None: output_dir = os.path.dirname(image_folder) print "output_dir: ", output_dir camera_intrinsics_file = os.path.join(image_folder, 'camera-intrinsics.txt') spartan_source_dir = spartan_utils.getSpartanSourceDir() tsdf_fusion_dir = os.path.join(spartan_source_dir, "src", "tsdf-fusion") tsdf_executable = os.path.join(tsdf_fusion_dir, 'demo') cmd = "cd %s && %s %s %s" % (tsdf_fusion_dir, tsdf_executable, image_folder, camera_intrinsics_file) if fast_tsdf_settings: voxel_size = 0.005 voxel_grid_dim_x = 200 voxel_grid_dim_y = 200 voxel_grid_dim_z = 150 cmd += " " + str(voxel_size) cmd += " " + str(voxel_grid_dim_x) cmd += " " + str(voxel_grid_dim_y) cmd += " " + str(voxel_grid_dim_z) cmd += " " + str(voxel_grid_origin_x) cmd += " " + str(voxel_grid_origin_y) cmd += " " + str(voxel_grid_origin_z) print "cmd:\n", cmd start_time = time.time() process = subprocess.Popen(cmd, shell=True) print "started subprocess, waiting for it to finish" process.wait() elapsed = time.time() - start_time tsdf_bin = os.path.join(image_folder, 'tsdf.bin') tsdf_ply = os.path.join(image_folder, 'tsdf.ply') tsdf_bin_new = os.path.join(output_dir, 'tsdf.bin') tsdf_ply_new = os.path.join(output_dir, 'fusion_pointcloud.ply') shutil.move(tsdf_bin, tsdf_bin_new) shutil.move(tsdf_ply, tsdf_ply_new) print "tsdf-fusion took %d seconds" % (elapsed)
def saveSingleImage(topic, filename, encoding=None): rosImageLoggerExecutable = os.path.join(spartanUtils.getSpartanSourceDir(), 'modules',"spartan", 'calibration','ros_image_logger.py') cmd = "%s -t %s -f %s" % (rosImageLoggerExecutable, topic, filename) if encoding is not None: cmd += " -e " + encoding os.system(cmd)
def start_bagging(self, bag_folder='logs_special'): self.flushCache() 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) # make bagfile directory with name os.system("mkdir -p " + bagfile_directory) # redundantly tag on the string name, so we have extra defense on tracking its ID bagfile_name = "fusion_" + log_id_name 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" rosbag_cmd += " -O " + bagfile_name for i in topics_to_bag: rosbag_cmd += " " + i # add some visibility print rosbag_cmd # start bagging rosbag_proc = subprocess.Popen(rosbag_cmd, stdin=subprocess.PIPE, shell=True, cwd=bagfile_directory) rospy.loginfo("started image subscribers, sleeping for %d seconds", self.config['sleep_time_before_bagging']) return os.path.join(bagfile_directory, bagfile_name + ".bag"), rosbag_proc
def getDefaultPointCloudListMsg(): spartanSourceDir = spartanUtils.getSpartanSourceDir() # filename = "grasp_sensor_data.bag" filename = "sr300_box.bag" rosBagFilename = os.path.join(spartanSourceDir, 'data','rosbag','iiwa', filename) return GraspSupervisor.getPointCloudListMsg(rosBagFilename)
def __init__(self, robotStateModel, mug_rack_config=None, mug_platform_config=None, shoe_rack_config=None): self._robotStateModel = robotStateModel self.clear_visualation() self.setup_config() self._gripper_link_frame_name = "wsg_50_base_link" self._object = None robotStateModel.connectModelChanged(self.on_robot_model_changed) if mug_rack_config is None: mug_rack_config_file = os.path.join( spartan_utils.getSpartanSourceDir(), 'src/catkin_projects/station_config/RLG_iiwa_1/manipulation/mug_rack.yaml' ) self._mug_rack_config = spartan_utils.getDictFromYamlFilename( mug_rack_config_file) else: self._mug_rack_config = mug_rack_config if mug_platform_config is None: mug_platform_config_file = os.path.join( spartan_utils.getSpartanSourceDir(), 'src/catkin_projects/station_config/RLG_iiwa_1/manipulation/mug_platform.yaml' ) self._mug_platform_config = spartan_utils.getDictFromYamlFilename( mug_platform_config_file) else: self._mug_platform_config = mug_platform_config if shoe_rack_config is None: shoe_rack_config_file = os.path.join( spartan_utils.getSpartanSourceDir(), 'src/catkin_projects/station_config/RLG_iiwa_1/manipulation/shoe_rack.yaml' ) self._shoe_rack_config = spartan_utils.getDictFromYamlFilename( shoe_rack_config_file) else: self._shoe_rack_config = shoe_rack_config
def loadReconstructedPointCloud(self, filename=None): if filename is None: sourceDir = spartanUtils.getSpartanSourceDir() filename = os.path.join(sourceDir, 'logs', 'test', 'reconstructed_pointcloud.vtp') polyData = ioUtils.readPolyData(filename) self.pointcloud = vis.showPolyData(polyData, 'reconstructed pointcloud', colorByName='RGB') vis.addChildFrame(self.pointcloud)
def start_bagging(self): self.flushCache() bagfile_directory = os.path.join(spartanUtils.getSpartanSourceDir(), 'sandbox', 'fusion') # make sure bagfile_directory exists os.system("mkdir -p " + bagfile_directory) 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() rospy.loginfo("started image subscribers, sleeping for %d seconds", self.config['sleep_time_before_bagging']) # sleep for two seconds 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" bagfile_name = "fusion" + str(time.time()) rosbag_cmd += " -O " + bagfile_name for i in topics_to_bag: rosbag_cmd += " " + i # add some visibility print rosbag_cmd # start bagging rosbag_proc = subprocess.Popen(rosbag_cmd, stdin=subprocess.PIPE, shell=True, cwd=bagfile_directory) rospy.loginfo("sleeping while waiting for bagging to start") rospy.sleep(2.0) return os.path.join(bagfile_directory, bagfile_name + ".bag"), rosbag_proc
def __init__(self, folderName=None, logName="cup_on_table"): if folderName is None: sourceDir = spartanUtils.getSpartanSourceDir() folderName = os.path.join(sourceDir, 'logs', 'test') self.folderName = folderName self.logName = logName self.filenames = getFilenames(self.folderName, self.logName) self.loadReconstructedPointCloud(self.filenames['reconstruction']) self.cameraToWorldTransform = None self.setupSubscribers()
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 saveSensorDataToBagFile(self, pointCloudListMsg=None, filename=None, overwrite=True): if pointCloudListMsg is None: pointCloudListMsg = self.pointCloudListMsg if filename is None: filename = os.path.join(spartanUtils.getSpartanSourceDir(), 'sandbox', 'grasp_sensor_data.bag') if overwrite and os.path.isfile(filename): os.remove(filename) bag = rosbag.Bag(filename, 'w') bag.write('data', pointCloudListMsg) bag.close()
def makeDefault(**kwargs): storedPosesFile = os.path.join(spartanUtils.getSpartanSourceDir(), 'src', 'catkin_projects', 'station_config', 'RLG_iiwa_1', 'background_subtraction', 'stored_poses.yaml') cameraInfoFilename = os.path.join( spartanUtils.getSpartanSourceDir(), 'src/catkin_projects/camera_config/data/1112170110/master/camera_ros_data.yaml' ) poseData = spartanUtils.getDictFromYamlFilename(storedPosesFile) cameraInfoDict = spartanUtils.getDictFromYamlFilename( cameraInfoFilename) return BackgroundSubtractionDataCapture( poseData['header']['joint_names'], poseData, cameraInfoDict, poseFilename=storedPosesFile, cameraInfoFilename=cameraInfoFilename, **kwargs)
def setupDataCapture(self, objectName): unique_name = time.strftime("%Y%m%d-%H%M%S") self.folderName = os.path.join(spartanUtils.getSpartanSourceDir(), 'sandbox', 'background_subtraction_data', unique_name) os.system("mkdir -p " + self.folderName) os.chdir(self.folderName) self.startImageSubscribers() self.data = dict() self.data['header'] = dict() self.data['header']['object_name'] = objectName self.data['images'] = dict() for poseName in self.poseData['pose_scan_order']: self.data['images'][poseName] = dict()
import multiprocessing as mp import resource import shutil import argparse from fusion_server.fusion import FusionServer import fusion_server.tsdf_fusion as tsdf_fusion import spartan.utils.utils as spartanUtils fs = None LINEAR_DISTANCE_THRESHOLD = 0.03 # 3cm ANGLE_DISTANCE_THRESHOLD = 10 # 10 degrees FUSION_CONFIG_FILE = os.path.join( spartanUtils.getSpartanSourceDir(), "src/catkin_projects/station_config/RLG_iiwa_1/fusion/fusion_params.yaml") FUSION_CONFIG = spartanUtils.getDictFromYamlFilename(FUSION_CONFIG_FILE) FUSION_LOCATION = "front" def mem(): print( 'Memory usage : % 2.2f MB' % round(resource.getrusage(resource.RUSAGE_SELF).ru_maxrss / 1024.0, 1)) def already_extracted_rosbag(log_full_path): images_dir = os.path.join(log_full_path, 'processed', 'images') file_to_check = os.path.join(images_dir, 'camera_info.yaml') return os.path.exists(file_to_check)
def runROSCalibration(self, headerData): headerData['target'][ 'location_estimate_in_robot_base_frame'] = self.calibrationPosesConfig[ 'target_location'] calibrationRunData = dict() calibrationRunData['header'] = headerData #setup our passive subscribers to the IR or RGB data topicName = None self.passiveSubscriber = None if self.captureRGB: topicName = self.config['rgb_raw_topic'] if self.captureIR: topicName = self.config['ir_raw_topic'] if topicName: self.passiveSubscriber = spartanROSUtils.SimpleSubscriber( topicName, sensor_msgs.msg.Image) self.passiveSubscriber.start() unique_name = time.strftime( "%Y%m%d-%H%M%S") + "_" + self.calibrationType self.calibrationFolderName = os.path.join( spartanUtils.getSpartanSourceDir(), 'calibration_data', unique_name) os.system("mkdir -p " + self.calibrationFolderName) os.chdir(self.calibrationFolderName) poseDict = self.computeCalibrationPoses() self.drawResult(poseDict) rospy.loginfo("finished making calibration poses") rospy.loginfo("starting calibration run") calibrationData = [] # topic = self.config['rgb_raw_topic'] # msgType = sensor_msgs.msg.Image # self.subscribers = dict() # self.subscribers[topic] = spartanROSUtils.SimpleSubscriber(topic, msgType) # self.subscribers[topic].start() for pose in poseDict['feasiblePoses']: # move robot to that joint position rospy.loginfo("\n moving to pose") self.robotService.moveToJointPosition(pose['joint_angles']) rospy.loginfo("capturing images and robot data") data = self.captureCurrentRobotAndImageData( captureRGB=self.captureRGB, captureIR=self.captureIR) calibrationData.append(data) rospy.loginfo("finished calibration routine, saving data to file") calibrationRunData['data_list'] = calibrationData spartanUtils.saveToYaml( calibrationRunData, os.path.join(self.calibrationFolderName, 'robot_data.yaml')) if self.passiveSubscriber: self.passiveSubscriber.stop() return calibrationRunData
import json from pprint import pprint import os import spartan.utils.utils as spartanUtils stored_poses_json_source = os.path.join(spartanUtils.getSpartanSourceDir(), 'models', 'iiwa', 'director', 'stored_poses.json') data_in = json.load(open(stored_poses_json_source)) #pprint(data_in) stored_poses_yaml_destination = os.path.join( spartanUtils.getSpartanSourceDir(), 'src', 'catkin_projects', 'station_config', 'RLG_iiwa_1', 'stored_poses.yaml') data_out = dict() joint_names = [ 'iiwa_joint_1', 'iiwa_joint_2', 'iiwa_joint_3', 'iiwa_joint_4', 'iiwa_joint_5', 'iiwa_joint_6', 'iiwa_joint_7' ] data_out['header'] = dict() data_out['header']['robot'] = 'kuka_iiwa' data_out['header']['num_joints'] = 7 data_out['header']['joint_names'] = joint_names for group in data_in: data_out[str(group)] = dict() print "Group is", group for pose in data_in[group]: pose_name = pose['name']
corl.setup.setupDataCollection(globals()) app.restoreDefaultWindowState() app.initWindowSettings() applogic.resetCamera(viewDirection=[-1, 1, -0.5], view=view) useKukaRLGDev = True if useKukaRLGDev: # broadcast the pose of the wrist mounted Xtion import spartan.utils.utils as spartanUtils camera_serial_number = 1112170110 calibration_folder = 'master' cameraConfigFilename = os.path.join(spartanUtils.getSpartanSourceDir(), 'src', 'catkin_projects', 'camera_config', 'data', str(camera_serial_number), calibration_folder, 'camera_info.yaml') channelName = "OPENNI_FRAME_LEFT" import spartan.perception.cameratransform cameraTransform = spartan.perception.cameratransform.CameraTransform.fromConfigFilename( robotSystem, cameraConfigFilename, channelName) # import spartan.perception.dev as devUtils # efusion = devUtils.ElasticFusionReconstruction() import spartan.calibration.handeyecalibration
camera_info_dict['camera_name'] = camera_name spartanUtils.saveToYaml(camera_info_dict, path_to_camera_info_yaml_file) if __name__ == '__main__': parser = argparse.ArgumentParser() parser.add_argument('--camera_name', default='xtion_pro') parser.add_argument('--rgb', default=False, action='store_true') parser.add_argument('--ir', default=False, action='store_true') args = parser.parse_args() camera_info_filename = "" if args.ir: camera_info_filename = 'depth_camera_info.yaml' elif args.rgb: camera_info_filename = 'rgb_camera_info.yaml' else: print "Need to pass either --rgb or --ir" exit(0) path_to_camera_info_yaml_file = os.path.join( spartanUtils.getSpartanSourceDir(), 'src/catkin_projects/camera_config/data', args.camera_name, 'master', camera_info_filename) path_to_tar_file = '/tmp/calibrationdata.tar.gz' extract_intrinsics(path_to_tar_file, path_to_camera_info_yaml_file, args.camera_name)
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 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
if __name__ == "__main__": parser = argparse.ArgumentParser() parser.add_argument("--logs_dir", type=str, required=False) args = parser.parse_args() start = time.time() if args.logs_dir: logs_proto_path = args.logs_dir else: # logs_proto_path = os.path.join(spartanUtils.getSpartanSourceDir(), 'data_volume', 'pdc', 'logs_special', 'static_scenes') logs_proto_path = os.path.join(spartanUtils.getSpartanSourceDir(), 'data_volume', 'pdc', 'logs_proto') logs_proto_list = sorted(os.listdir(logs_proto_path)) counter_new_extracted = 0 counter_new_fused = 0 counter_new_downsampled = 0 for log in logs_proto_list: log_full_path = os.path.join(logs_proto_path, log) extract_and_fuse_single_scene(log_full_path, downsample=True) print "finished extracting and fusing all logs in logs_proto" print "SUMMARY:" print "number new logs extracted: ", counter_new_extracted
useROS = True if useROS: import rospy rospy.init_node('director', anonymous=True) useKukaRLGDev = True if useKukaRLGDev: # broadcast the pose of the wrist mounted Xtion import spartan.utils.utils as spartanUtils camera_serial_number = 1112170110 calibration_folder = 'master' cameraConfigFilename = os.path.join(spartanUtils.getSpartanSourceDir(), 'src', 'catkin_projects', 'camera_config', 'data', str(camera_serial_number), calibration_folder, 'camera_info.yaml') channelName = "OPENNI_FRAME_LEFT" import spartan.perception.cameratransform cameraTransform = spartan.perception.cameratransform.CameraTransform.fromConfigFilename(robotSystem, cameraConfigFilename, channelName) # import spartan.perception.dev as devUtils # efusion = devUtils.ElasticFusionReconstruction() import spartan.calibration.handeyecalibration cal = spartan.calibration.handeyecalibration.HandEyeCalibration(robotSystem) # import spartan.director.iiwamanipdev # spartan.director.iiwamanipdev.setupRLGDirector(globals())
from cv_bridge import CvBridge from geometry_msgs.msg import PoseStamped from sensor_msgs.msg import Image, CameraInfo from std_msgs.msg import Float32MultiArray import spartan.utils.utils as spartan_utils import spartan_grasp_msgs.msg from director.thirdparty import transformations bridge = CvBridge() DEBUG = True # Load the Network. spartan_source_dir = spartan_utils.getSpartanSourceDir() model_rel_to_spartan_source = 'data/networks/ggcnn_rss/epoch_29_model.hdf5' MODEL_FILE = os.path.join(spartan_source_dir, model_rel_to_spartan_source) # MODEL_FILE = 'PATH/TO/model.hdf5' model = load_model(MODEL_FILE) rospy.init_node('ggcnn_detection') # Output publishers. grasp_pub = rospy.Publisher('ggcnn/img/grasp', Image, queue_size=1) grasp_line_pub = rospy.Publisher('ggcnn/img/grasp_line', Image, queue_size=1) grasp_plain_pub = rospy.Publisher('ggcnn/img/grasp_plain', Image, queue_size=1) depth_pub = rospy.Publisher('ggcnn/img/depth', Image, queue_size=1) ang_pub = rospy.Publisher('ggcnn/img/ang', Image, queue_size=1) cmd_pub = rospy.Publisher('ggcnn/out/command', Float32MultiArray, queue_size=1) output_pub = rospy.Publisher('ggcnn/output',