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():
# extract_images_from_rosbag()
# tsdf_fusion.format_data_for_tsdf(image_folder)
tsdf_fusion.run_tsdf_fusion_cuda(image_folder)
tsdf_bin_file = os.path.join(data_folder, 'tsdf.bin')
tsdf_mesh_file = os.path.join(data_folder, 'fusion_mesh.ply')
tsdf_fusion.convert_tsdf_to_ply(tsdf_bin_file, tsdf_mesh_file)
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"
# 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')
# downsample data (this should be specifiable by an arg)
print "downsampling image folder"
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()