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