def execute(self, userdata): action_cmdr.move_gripper("open") action_cmdr.move_arm("zeroposition") for object in userdata.object_list: sss.move_arm("zeroposition") ##object.pose.pose.position.z = object.pose.pose.position.z + 0.02 #object.pose.pose.position.x = object.pose.pose.position.x + 0.01 #object.pose.pose.position.y = object.pose.pose.position.y - 0.005 #object.transform.transform.translation.z = object.transform.transform.translation.z + 0.02 #object.transform.transform.translation.x = object.transform.transform.translation.x + 0.01 #object.transform.transform.translation.y = object.transform.transform.translation.y - 0.005 #handle_arm = sss.move_arm([object.pose.pose.position.x, object.pose.pose.position.y, object.pose.pose.position.z, "/base_link"]) object_pose = PoseStamped() object_pose.pose.position.x = object.transform.transform.x object_pose.pose.position.y = object.transform.transform.y object_pose.pose.position.z = object.transform.transform.z object_pose.pose.orientation = object.transform.tranform.rotation object_pose.header.frame_id = object.transform.header.frame_id handle_arm = sss.move_arm(object_pose) if handle_arm.get_state() == 3: sss.move_gripper("close") rospy.sleep(3.0) sss.move_arm("zeroposition") return 'succeeded' else: rospy.logerr('could not find IK for current object') return 'failed'
rospy.sleep(2) action_cmdr.move_base(target="S1", blocking=True) action_cmdr.prepare_perception() det_objects = action_cmdr.execute_perception(wait_time=1, retries=10, blocking=True) if len(det_objects) > 0: print("Got: %s", det_objects) obj = det_objects[0] ret = action_cmdr.pick_up(target=obj, blocking=True) print("Got: %s", ret) action_cmdr.move_base(target="D1", blocking=True) action_cmdr.move_gripper(target="open", blocking=True) else: print("didn't find anything!") #action_cmdr.test("foo") #action_cmdr.move_gripper(target="cylopen", blocking=True) #action_cmdr.move_torso(target="front", blocking=True) #action_cmdr.move_tray(target="down", blocking=True) #action_cmdr.move_head(target="front", blocking=True) #action_cmdr.move_arm(target=["wavein","waveout","wavein"], blocking=True) #action_cmdr.move_arm_planned(target=["waveout","wavein","waveout"], blocking=True)
if __name__ == "__main__": rospy.init_node('test') # needed to add this so I could make trajectory messages, require timestamp rospy.sleep(2) action_cmdr.move_base(target="S1",blocking=True) action_cmdr.prepare_perception() det_objects = action_cmdr.execute_perception(wait_time=1,retries=10,blocking=True) if len(det_objects) > 0: print("Got: %s",det_objects); obj = det_objects[0] ret = action_cmdr.pick_up(target=obj,blocking=True) print("Got: %s",ret); action_cmdr.move_base(target="D1",blocking=True) action_cmdr.move_gripper(target="open",blocking=True) else: print("didn't find anything!") #action_cmdr.test("foo") #action_cmdr.move_gripper(target="cylopen", blocking=True) #action_cmdr.move_torso(target="front", blocking=True) #action_cmdr.move_tray(target="down", blocking=True) #action_cmdr.move_head(target="front", blocking=True)