def load_side_table(self):
"""
Loads the side table
:return:
:rtype:
"""
pdc_data_dir = os.path.join(spartan_utils.get_data_dir(), 'pdc')
side_table_ply_file = os.path.join(
pdc_data_dir, self._mug_rack_config['background_mesh'])
side_table_poly_data = ioUtils.readPolyData(side_table_ply_file)
vis.showPolyData(side_table_poly_data,
'Side Table',
parent=self._vis_container)
def load_object_model(self):
"""
Load the object model poly data
:return:
:rtype:
"""
model_filename = os.path.join(spartan_utils.get_data_dir(),
"pdc/templates/mugs/mug.stl")
print("model_filename", model_filename)
self._poly_data = ioUtils.readPolyData(model_filename)
self._object = vis.updatePolyData(self._poly_data,
"Object",
parent=self._vis_container)
vis.addChildFrame(self._object)
pos = [0.34301733, 0.00805546, 0.92509635]
quat = [0.17589242, 0.98340013, -0.04305877, 0.01148846]
t = transformUtils.transformFromPose(pos, quat)
self._object.getChildFrame().copyFrame(t)
def load_mug_rack(self):
"""
Loads a mug rack and side table poly data
:return:
:rtype:
"""
pdc_data_dir = os.path.join(spartan_utils.get_data_dir(), 'pdc')
rack_ply_file = os.path.join(pdc_data_dir,
self._mug_rack_config["mesh"])
rack_poly_data = ioUtils.readPolyData(rack_ply_file)
self._mug_rack = vis.showPolyData(rack_poly_data,
'Mug Rack',
parent=self._vis_container)
# rack_target_position
target_pose_name = "left_side_table"
target_pose = director_utils.transformFromPose(
self._mug_rack_config['poses'][target_pose_name])
vis.addChildFrame(self._mug_rack)
self._mug_rack.getChildFrame().copyFrame(target_pose)
def load_mug_platform(self):
"""
Load the mug platform
:return:
:rtype:
"""
pdc_data_dir = os.path.join(spartan_utils.get_data_dir(), 'pdc')
platform_ply_file = os.path.join(pdc_data_dir,
self._mug_platform_config['mesh'])
platform_poly_data = ioUtils.readPolyData(platform_ply_file)
self._mug_platform = vis.showPolyData(platform_poly_data,
'Platform',
parent=self._vis_container)
target_pose_name = "left_side_table"
target_pose = director_utils.transformFromPose(
self._mug_platform_config['poses'][target_pose_name])
vis.addChildFrame(self._mug_platform)
self._mug_platform.getChildFrame().copyFrame(target_pose)
def capture_scene(self, use_close_up_poses=False):
"""
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.get_data_dir(), 'pdc',
'logs_test')
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']
pose_list = self.config['scan']['left_table']
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
if use_close_up_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
app.viewOptions.setProperty('Orientation widget', True)
app.viewOptions.setProperty('View angle', 30)
app.sceneBrowserDock.setVisible(True)
app.propertiesDock.setVisible(False)
app.mainWindow.setWindowTitle('Depth Scanner')
app.mainWindow.show()
app.mainWindow.resize(920, 600)
app.mainWindow.move(0, 0)
view = app.view
globalsDict['app'] = app
globalsDict['view'] = view
# load background scene if it exists
background_ply_file = os.path.join(spartanUtils.get_data_dir(), 'pdc',
'logs_special', '2019-01-03-22-43-55',
'processed', 'fusion_mesh.ply')
def visualize_background():
if not os.path.exists(background_ply_file):
return
parent = om.getOrCreateContainer("scene")
poly_data = ioUtils.readPolyData(background_ply_file)
vis.updatePolyData(poly_data, 'table', parent=parent)
visualize_background()
poser_vis = PoserVisualizer.make_default()
object_manip = ObjectManipulation(poser_visualizer=poser_vis)
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)
#
ros_visualizer = DirectorROSVisualizer(tf_buffer=tfBuffer)
topic = "/camera_carmine_1/depth/points"
ros_visualizer.add_subscriber(topic, name="Carmine", visualize=True)
globalsDict['ros_visualizer'] = ros_visualizer
ros_visualizer.start()
# load background scene if it exists
background_ply_file = os.path.join(spartanUtils.get_data_dir(), 'pdc',
'logs_special', '2019-01-03-22-43-55',
'processed', 'fusion_mesh.ply')
robotSystem = globalsDict['robotSystem']
robotStateModel = robotSystem.robotStateModel
category_manip = CategoryManipulation(robotStateModel)
category_manip.load_side_table()
category_manip.load_mug_rack()
# category_manip.setup_horizontal_mug_grasp()
# category_manip.load_mug_platform()
# category_manip.load_shoe_rack()
graspSupervisor._category_manip = category_manip
def visualize_background():
if not os.path.exists(background_ply_file):
return
parent = om.getOrCreateContainer("scene")
poly_data = ioUtils.readPolyData(background_ply_file)
vis.updatePolyData(poly_data, 'table', parent=parent)
visualize_background()
poser_vis = PoserVisualizer.make_default()
object_manip = ObjectManipulation()
globalsDict['object_manip'] = object_manip
globalsDict['o'] = object_manip
globalsDict['c'] = category_manip
globalsDict['poser_vis'] = poser_vis
globalsDict['g'] = graspSupervisor