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