def unfold_cloth():
print "Unfolding cloth"
#Run cloth tracker
success = _cloth_tracker.run()
#(waist_l,waist_r) = get_waist_points()
#fold_cloth()
#Kill the cloth tracker cpp node so it will relaunch
RosUtils.call_service('cloth_tracker/kill',KillMe)
return success
def execute(self, userdata):
userdata.locations = {}
print "Set Folding Locations?"
tf = raw_input()
if len(tf) > 0 and tf[0] == "y":
return FAILURE
else:
RosUtils.call_service("locations/load_locations", LoadLocations, "folding")
return SUCCESS
def unfold_cloth():
print "Unfolding cloth"
#Run cloth tracker
success = _cloth_tracker.run()
#(waist_l,waist_r) = get_waist_points()
#fold_cloth()
#Kill the cloth tracker cpp node so it will relaunch
RosUtils.call_service('cloth_tracker/kill', KillMe)
return success
def execute(self, userdata):
userdata.locations = {}
print "Set Folding Locations?"
tf = raw_input()
if len(tf) > 0 and tf[0] == 'y':
return FAILURE
else:
RosUtils.call_service("locations/load_locations", LoadLocations,
"folding")
return SUCCESS
def execute(self, userdata):
RosUtils.call_service("rotate_base", RotateBase, self.turn)
point = PointStamped()
point.header.stamp = rospy.Time()
point.header.frame_id = "torso_lift_link"
r = 0.35
theta = self.look + pi / 2
point.point.x = r * math.sin(theta)
point.point.y = -1 * r * math.cos(theta)
point.point.z = 0
RosUtils.call_service("look_at_point", LookAtPoint, point)
return SUCCESS
def execute(self, userdata):
RosUtils.call_service("rotate_base", RotateBase, self.turn)
point = PointStamped()
point.header.stamp = rospy.Time()
point.header.frame_id = "torso_lift_link"
r = 0.35
theta = self.look + pi / 2
point.point.x = r * math.sin(theta)
point.point.y = -1 * r * math.cos(theta)
point.point.z = 0
RosUtils.call_service("look_at_point", LookAtPoint, point)
return SUCCESS
def take_out_cloth():
print "Taking out cloth"
RosUtils.call_service("move_torso",MoveTorso,height=0.01)
DryerNavigationUtils.goToPosition("enter_dryer")
GripUtils.go_to(x=-0.1,y=-0.33,z=-0.4,roll=0,pitch=0,yaw=0,grip=False,arm="l",frame="dryer")
DryerNavigationUtils.goToPosition("into_dryer")
GripUtils.go_to(x=0.15,y=-0.33,z=-0.65,roll=0,pitch=pi/2,yaw=0,grip=False,arm="l",frame="dryer")
GripUtils.close_gripper("l")
GripUtils.go_to(x=0.15,y=-0.33,z=-0.4,roll=0,pitch=0,yaw=0,grip=True,arm="l",frame="dryer")
DryerNavigationUtils.goToPosition("enter_dryer")
RosUtils.call_service("move_torso",MoveTorso,height=0.3)
return True
def adjust_fold(self, req):
#Go to viewing stance
StanceUtils.call_stance("open_both", 5.0)
StanceUtils.call_stance("viewing", 5.0)
last_model = pickle.load(open("/tmp/last_model.pickle"))
camera_model = image_geometry.PinholeCameraModel()
info = RosUtils.get_next_message("wide_stereo/left/camera_info",
CameraInfo)
cam_frame = info.header.frame_id
camera_model.fromCameraInfo(info)
now = rospy.Time.now()
req.start.header.stamp = now
req.end.header.stamp = now
self.listener.waitForTransform(cam_frame, req.start.header.frame_id,
now, rospy.Duration(20.0))
start_cam = self.listener.transformPoint(cam_frame, req.start)
end_cam = self.listener.transformPoint(cam_frame, req.end)
start = camera_model.project3dToPixel(
(start_cam.point.x, start_cam.point.y, start_cam.point.z))
end = camera_model.project3dToPixel(
(end_cam.point.x, end_cam.point.y, end_cam.point.z))
folded_model = Models.Point_Model_Folded(last_model, start, end)
folded_model.image = None
folded_model.initial_model.image = None
pickle.dump(folded_model, open("/tmp/last_model.pickle", 'w'))
adjust_folded_model = rospy.ServiceProxy(
"fold_finder_node/process_mono", ProcessMono)
resp = adjust_folded_model("wide_stereo/left")
new_start = resp.pts3d[0]
new_end = resp.pts3d[1]
return AdjustFoldResponse(start=new_start, end=new_end)
def adjust_fold(self,req):
#Go to viewing stance
StanceUtils.call_stance("open_both",5.0)
StanceUtils.call_stance("viewing",5.0)
last_model = pickle.load(open("/tmp/last_model.pickle"))
camera_model = image_geometry.PinholeCameraModel()
info = RosUtils.get_next_message("wide_stereo/left/camera_info",CameraInfo)
cam_frame = info.header.frame_id
camera_model.fromCameraInfo(info)
now = rospy.Time.now()
req.start.header.stamp = now
req.end.header.stamp=now
self.listener.waitForTransform(cam_frame,req.start.header.frame_id,now,rospy.Duration(20.0))
start_cam = self.listener.transformPoint(cam_frame,req.start)
end_cam = self.listener.transformPoint(cam_frame,req.end)
start = camera_model.project3dToPixel((start_cam.point.x,start_cam.point.y,start_cam.point.z))
end = camera_model.project3dToPixel((end_cam.point.x,end_cam.point.y,end_cam.point.z))
folded_model = Models.Point_Model_Folded(last_model,start,end)
folded_model.image = None
folded_model.initial_model.image = None
pickle.dump(folded_model,open("/tmp/last_model.pickle",'w'))
adjust_folded_model = rospy.ServiceProxy("fold_finder_node/process_mono",ProcessMono)
resp = adjust_folded_model("wide_stereo/left")
new_start = resp.pts3d[0]
new_end = resp.pts3d[1]
return AdjustFoldResponse(start=new_start,end=new_end)
def execute(self, userdata):
print "Please move Brett to", self.positionName, "location."
print "Is Brett ready?"
waitForInput = raw_input()
# Save location .5 behind Brett as pre-location
ps = PoseStamped()
ps.header.frame_id = "/base_footprint"
ps.pose.position.x = -0.5
prelocation = self.listener.transformPose("/map", ps)
RosUtils.call_service("locations/add_location", AddLocation, prelocation, "pre" + self.positionName)
# Save current location as actual location
RosUtils.call_service("locations/add_current_location", AddCurrentLocation, self.positionName, "/map")
userdata.locations["current"] = self.positionName
return SUCCESS
def execute(self, userdata):
detection = RosUtils.call_service("object_detection", TabletopDetection, False, False)
if len(detection.clusters) != 0:
return SUCCESS
else:
print "Didn't find any more towels. Continue loop anyway?"
tf = raw_input()
if len(tf) > 0 and tf[0] == "y":
return SUCCESS
else:
return FAILURE
def execute(self, userdata):
detection = RosUtils.call_service("object_detection",
TabletopDetection, False, False)
if len(detection.clusters) != 0:
return SUCCESS
else:
print "Didn't find any more towels. Continue loop anyway?"
tf = raw_input()
if len(tf) > 0 and tf[0] == 'y':
return SUCCESS
else:
return FAILURE
def execute(self, userdata):
print "Please move Brett to", self.positionName, "location."
print "Is Brett ready?"
waitForInput = raw_input()
#Save location .5 behind Brett as pre-location
ps = PoseStamped()
ps.header.frame_id = "/base_footprint"
ps.pose.position.x = -0.5
prelocation = self.listener.transformPose("/map", ps)
RosUtils.call_service("locations/add_location", AddLocation,
prelocation, "pre" + self.positionName)
#Save current location as actual location
RosUtils.call_service("locations/add_current_location",
AddCurrentLocation, self.positionName, "/map")
userdata.locations["current"] = self.positionName
return SUCCESS
def take_out_cloth():
print "Taking out cloth"
RosUtils.call_service("move_torso", MoveTorso, height=0.01)
DryerNavigationUtils.goToPosition("enter_dryer")
GripUtils.go_to(x=-0.1,
y=-0.33,
z=-0.4,
roll=0,
pitch=0,
yaw=0,
grip=False,
arm="l",
frame="dryer")
DryerNavigationUtils.goToPosition("into_dryer")
GripUtils.go_to(x=0.15,
y=-0.33,
z=-0.65,
roll=0,
pitch=pi / 2,
yaw=0,
grip=False,
arm="l",
frame="dryer")
GripUtils.close_gripper("l")
GripUtils.go_to(x=0.15,
y=-0.33,
z=-0.4,
roll=0,
pitch=0,
yaw=0,
grip=True,
arm="l",
frame="dryer")
DryerNavigationUtils.goToPosition("enter_dryer")
RosUtils.call_service("move_torso", MoveTorso, height=0.3)
return True
def execute(self, userdata):
# Go to pre-current location (only if not None)
try:
if userdata.locations["current"]:
RosUtils.call_service("locations/pre%s" % userdata.locations["current"], ExecuteLocation, 0)
except KeyError:
pass
# Go to pre-location location
RosUtils.call_service("locations/pre%s" % self.positionName, ExecuteLocation, 0)
# Go to location
RosUtils.call_service("locations/%s" % self.positionName, ExecuteLocation, 0)
userdata.locations["current"] = self.positionName
return SUCCESS
def execute(self, userdata):
resp = RosUtils.call_service("towel_fitter_node/process_mono", ProcessMono, "wide_stereo/left")
userdata.bl = resp.pts3d[0]
userdata.tl = resp.pts3d[1]
userdata.tr = resp.pts3d[2]
userdata.br = resp.pts3d[3]
score = resp.params[resp.param_names.index("score")]
to_flip = False
if abs(score) < 0.00003 or self.flip_count >= MAX_FLIPS:
to_flip = False
print "Decided not to flip"
else:
self.flip_count += 1
to_flip = True
print "Decided to flip"
print "Flip?"
flip = raw_input()
if flip[0] == "t" or flip[0] == "T" or flip[0] == "y" or flip[0] == "Y":
to_flip = True
else:
to_flip = False
return FAILURE if to_flip else SUCCESS
def execute(self, userdata):
resp = RosUtils.call_service("towel_fitter_node/process_mono",
ProcessMono, "wide_stereo/left")
userdata.bl = resp.pts3d[0]
userdata.tl = resp.pts3d[1]
userdata.tr = resp.pts3d[2]
userdata.br = resp.pts3d[3]
score = resp.params[resp.param_names.index("score")]
to_flip = False
if abs(score) < 0.00003 or self.flip_count >= MAX_FLIPS:
to_flip = False
print "Decided not to flip"
else:
self.flip_count += 1
to_flip = True
print "Decided to flip"
print "Flip?"
flip = raw_input()
if (flip[0] == "t" or flip[0] == "T" or flip[0] == "y"
or flip[0] == "Y"):
to_flip = True
else:
to_flip = False
return FAILURE if to_flip else SUCCESS
def execute(self, userdata):
#Go to pre-current location (only if not None)
try:
if userdata.locations["current"]:
RosUtils.call_service(
"locations/pre%s" % userdata.locations["current"],
ExecuteLocation, 0)
except KeyError:
pass
#Go to pre-location location
RosUtils.call_service("locations/pre%s" % self.positionName,
ExecuteLocation, 0)
#Go to location
RosUtils.call_service("locations/%s" % self.positionName,
ExecuteLocation, 0)
userdata.locations["current"] = self.positionName
return SUCCESS
def execute(self, userdata):
RosUtils.call_service("locations/save_locations", SaveLocations, "folding")
return SUCCESS
def execute(self, userdata):
RosUtils.call_service("locations/save_locations", SaveLocations,
"folding")
return SUCCESS