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',