#!/usr/bin/python import roslib roslib.load_manifest('action_cmdr') import rospy import action_cmdr action_cmdr.load(["generic_actions", "youbot_actions"]) from geometry_msgs.msg import PoseStamped 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!")
roslib.load_manifest(PKG) import rospy import smach import smach_ros import actionlib import raw_base_placement.msg from os import getenv robot_platform = getenv("ROBOT") # yes, this is a hack, a better solution is known and has been discussed, # cf. the Lua-based Behavior Engine, but is pending implementation after RC5 if robot_platform == "cob3-3": robot_platform = "cob" import action_cmdr action_cmdr.load(["generic_actions", robot_platform + "_actions"]) class approach_pose(smach.State): def __init__(self, pose=""): smach.State.__init__(self, outcomes=['succeeded', 'failed'], input_keys=['base_pose_to_approach']) self.pose = pose def execute(self, userdata): if (self.pose == ""): self.pose2 = userdata.base_pose_to_approach else:
roslib.load_manifest('raw_generic_states') import rospy import smach import smach_ros import math from os import getenv robot_platform = getenv("ROBOT") # yes, this is a hack, a better solution is known and has been discussed, # cf. the Lua-based Behavior Engine, but is pending implementation after RC5 if robot_platform == "cob3-3": robot_platform = "cob" import action_cmdr action_cmdr.load(["generic_actions", robot_platform + "_actions"]) class grasp_random_object(smach.State): def __init__(self): smach.State.__init__(self, outcomes=['succeeded', 'failed'], input_keys=['object_list']) 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
#!/usr/bin/python import roslib roslib.load_manifest('action_cmdr') import rospy import action_cmdr action_cmdr.load(["generic_actions","youbot_actions"]) from geometry_msgs.msg import PoseStamped 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)