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