def __init__(self):
# search for the person only after the cube is picked up
self._cube_picked = False
self._is_planning = False
# Fetch controls
self._base = robot_api.Base()
self._arm = robot_api.Arm()
self._torso = robot_api.Torso()
self._head = robot_api.Head()
self._fetch_gripper = robot_api.Gripper()
self._move_base_client = actionlib.SimpleActionClient(
'move_base', MoveBaseAction)
self._move_base_client.wait_for_server()
# transformation
self._tf_listener = tf.TransformListener()
self._viz_pub = rospy.Publisher('debug_marker', Marker, queue_size=5)
self._cube_centroid_sub = rospy.Subscriber('/cube_centroid',
PoseStamped,
callback=self.pickup_cube)
self._target_pose_sub = rospy.Subscriber('/target_pose',
TargetPose,
callback=self.goto_target)
rospy.sleep(0.5)
# raise torso
self._torso.set_height(0)
# look down
self._head.pan_tilt(0, 0.8)
def __init__(self, save_file_path="food_items.pkl", nav_file_path="annotator_positions.pkl"):
self._food_items_pub = rospy.Publisher(FOOD_ITEMS_TOPIC,
FoodItems, queue_size=10, latch=True)
rospy.loginfo("Given save file path: " + save_file_path)
if os.path.isfile(save_file_path):
rospy.loginfo("File already exists, loading saved positions.")
with open(save_file_path, "rb") as save_file:
try:
self._food_items = pickle.load(save_file)
except EOFError:
# this can be caused if the file is empty.
self._food_items = {}
rospy.loginfo("File loaded...")
else:
rospy.loginfo("File doesn't exist.")
self._food_items = {}
self.__print_food_items__()
self._save_file_path = save_file_path
self.__pub_food_items__()
rospy.loginfo("initializing arm...")
self.arm = robot_api.Arm()
rospy.loginfo("initializing gripper...")
self.gripper = robot_api.Gripper()
rospy.loginfo("initializing head...")
self.head = robot_api.Head()
rospy.loginfo("initializing torso...")
self.torso = robot_api.Torso()
rospy.loginfo("initializing planning scene...")
self.planning_scene = PlanningSceneInterface('base_link')
self.curr_obstacle = None
rospy.loginfo("Starting map annotator...")
# We should expect the nav file given to contain the annotated positions:
# MICROWAVE_LOCATION_NAME - starting location in front of the microwave.
# DROPOFF_LOCATION_NAME - ending dropoff location.
# TODO: Remember to uncomment this section when we get the map working.
# self._map_annotator = Annotator(save_file_path=nav_file_path)
# if not self._map_annotator.exists(self.MICROWAVE_LOCATION_NAME):
# rospy.logwarn("Annotator is missing location '%s'" %
# (self.MICROWAVE_LOCATION_NAME))
# if not self._map_annotator.exists(self.DROPOFF_LOCATION_NAME):
# rospy.logwarn("Annotator is missing location '%s'" %
# (self.DROPOFF_LOCATION_NAME))
rospy.loginfo("Initialization finished...")
def main():
rospy.init_node('gripper_demo')
wait_for_time()
argv = rospy.myargv()
if len(argv) < 2:
print_usage()
return
command = argv[1]
gripper = robot_api.Gripper()
effort = gripper.MAX_EFFORT
if command == 'close' and len(argv) > 2:
effort = float(argv[2])
if command == 'open':
gripper.open()
elif command == 'close':
gripper.close(effort)
else:
print_usage()
def main():
rospy.init_node('gripper_teleop_node')
# raise torso
torso = robot_api.Torso()
torso.set_height(0.4)
# title head to look at the ground
head = robot_api.Head()
head.pan_tilt(0, math.pi / 8)
arm = robot_api.Arm()
gripper = robot_api.Gripper()
# im_server = InteractiveMarkerServer('gripper_im_server')
# teleop = GripperTeleop(arm, gripper, im_server)
# teleop.start()
auto_pick_im_server = InteractiveMarkerServer('auto_pick_im_server')
auto_pick = AutoPickTeleop(arm, gripper, auto_pick_im_server)
auto_pick.start()
rospy.spin()
def main():
rospy.init_node('arm_motion_planner_demo')
wait_for_time()
# argv = rospy.myargv()
# if len(argv) < 2:
# print_usage()
# return
# name = argv[1]
# rospy.sleep(0.5)
torso = robot_api.Torso()
torso.set_height(0)
arm = robot_api.Arm()
gripper = robot_api.Gripper()
reader = MarkerReader()
marker_sub = rospy.Subscriber('object_markers',
Marker,
callback=reader.callback)
rospy.sleep(2)
arm_planner = ArmMotionPlanner(arm, gripper)
rate = rospy.Rate(10)
while not rospy.is_shutdown():
# print(reader.obj_marker)
obj_list = copy.deepcopy(reader.obj_markers)
# obj_list = set(copy.deepcopy(obs_list))
for obj_marker_id in obj_list:
# for obs_marker in obs_list:
# print(obs_marker)
# print()
# print(obj_marker)
# obs_list.remove(obj_marker)
arm_planner.pick_up(obj_list[obj_marker_id])
# obs_list.add(obj_marker)
# arm_planner.pick_up(reader.obj_marker)
rate.sleep()
def main():
print("Initializing...")
rospy.init_node('project_master_node')
wait_for_time()
found_object = False
picked_object = False
face_detected = False
print("Initializing base...")
base = robot_api.Base()
print("Initializing head...")
head = robot_api.Head()
print("Initializing gripper...")
gripper = robot_api.Gripper()
gripper.open()
print("Initializing torso...")
torso = robot_api.Torso()
torso.set_height(0)
print("Initializing arm motion planner...")
arm = robot_api.Arm()
arm.move_to_initial_pose()
arm_planner = ArmMotionPlanner(arm, gripper)
print("Initializing object detector...")
object_reader = ObjectMarkerReader()
object_marker_sub = rospy.Subscriber('object_markers',
Marker,
callback=object_reader.callback)
print("Initializing face detector...")
face_detector = FaceDetector()
face_marker_sub = rospy.Subscriber('face_marker',
Marker,
callback=face_detector.callback)
print("Initializing navigation...")
navigator = Navigator()
print("Initialized")
while not rospy.is_shutdown():
# Part 1
# Look for objects
# If not found, turn around and repeat
head.pan_tilt(0.0, 0.85)
rospy.sleep(
1
) # wait extra time after tilting for the first point cloud to arrive
# #
head.pan_tilt(0.0, 0.9)
TURN_STEPS = 10
angular_distance = 2 * math.pi / TURN_STEPS
object_marker = None
while True:
print("Looking for objects...")
rospy.sleep(8)
# time.sleep(2)
if object_reader.object_detected():
print("Found object")
object_marker = object_reader.get_object()
break
print("Could not find object. turning around...")
base.turn(angular_distance)
print("Retrying...")
# Part 2
# Attempt to pickup object
# If success, go to next step
# If failure, restart from Part 1
if object_marker is None:
print("ERROR: Detected Object Gone...")
return
arm.move_to_hold_pose() # synchronized
picked_object = arm_planner.pick_up(object_marker)
if not picked_object:
print("Could not pick the object. Restart from looking objects...")
continue
print("Picked object")
arm.move_to_hold_pose()
# Part 3
# Look for face
# If found, move to face location
# If not found, turn around and repeat
head.pan_tilt(0.0, 0.0)
face_detected = False
face_location = None
max_dist = 2.5
while True:
print("Looking for people...")
rospy.sleep(5)
if face_detector.face_detected():
print("Found person")
face_marker = face_detector.get_face()
face_location = transform_marker(navigator, face_marker)
if face_location.pose.position.x < max_dist:
break
print("Person out of range...")
else:
print("Could not find face...")
print("Turning around...")
base.turn(angular_distance)
print("Retrying...")
# Part 4
# Go to the face location
# Wait for 5 seconds and open the gripper
# Go to Part 1
while True:
print("Moving to person...")
reached = go_forward(navigator, base, face_location)
# reached = go_to(navigator, face_location)
if not reached:
print("Could not reach the face. Retrying...")
# face_location.pose.position.x -= 0.05
# print("updated face_location_baselink x:", face_location.pose.position.x)
continue
print("Delivering object...")
time.sleep(5)
gripper.open()
print("Object delivered")
# arm.move_to_initial_pose()
print("Demo lite round 1 complete...")
return
def main():
rospy.init_node("arm_obstacle_demo")
wait_for_time()
argv = rospy.myargv()
planning_scene = PlanningSceneInterface("base_link")
# Create table obstacle
planning_scene.removeCollisionObject('table')
table_size_x = 0.5
table_size_y = 1
table_size_z = 0.03
table_x = 0.8
table_y = 0
table_z = 0.6
planning_scene.addBox('table', table_size_x, table_size_y, table_size_z,
table_x, table_y, table_z)
# Create divider obstacle
planning_scene.removeCollisionObject('divider')
size_x = 0.3
size_y = 0.01
size_z = 0.2
x = table_x - (table_size_x / 2) + (size_x / 2)
y = 0
z = table_z + (table_size_z / 2) + (size_z / 2)
planning_scene.addBox('divider', size_x, size_y, size_z, x, y, z)
pose1 = PoseStamped()
pose1.header.frame_id = 'base_link'
pose1.pose.position.x = 0.5
pose1.pose.position.y = -0.3
pose1.pose.position.z = 0.75
pose1.pose.orientation.w = 1
pose2 = PoseStamped()
pose2.header.frame_id = 'base_link'
pose2.pose.position.x = 0.5
pose2.pose.position.y = 0.3
pose2.pose.position.z = 0.75
pose2.pose.orientation.w = 1
oc = OrientationConstraint()
oc.header.frame_id = 'base_link'
oc.link_name = 'wrist_roll_link'
oc.orientation.w = 1
oc.absolute_x_axis_tolerance = 0.1
oc.absolute_y_axis_tolerance = 0.1
oc.absolute_z_axis_tolerance = 3.14
oc.weight = 1.0
gripper = robot_api.Gripper()
arm = robot_api.Arm()
def shutdown():
arm.cancel_all_goals()
rospy.on_shutdown(shutdown)
kwargs = {
'allowed_planning_time': 15,
'execution_timeout': 10,
'num_planning_attempts': 5,
'replan': True,
'orientation_constraint': oc
}
planning_scene.removeAttachedObject('tray')
gripper.open()
error = arm.move_to_pose(pose1, **kwargs)
if error is not None:
rospy.logerr('Pose 1 failed: {}'.format(error))
else:
rospy.loginfo('Pose 1 succeeded')
rospy.sleep(2)
frame_attached_to = 'gripper_link'
frames_okay_to_collide_with = [
'gripper_link', 'l_gripper_finger_link', 'r_gripper_finger_link'
]
planning_scene.attachBox('tray', 0.3, 0.07, 0.01, 0.05, 0, 0,
frame_attached_to, frames_okay_to_collide_with)
planning_scene.setColor('tray', 1, 0, 1)
planning_scene.sendColors()
gripper.close()
rospy.sleep(2)
error = arm.move_to_pose(pose2, **kwargs)
if error is not None:
rospy.logerr('Pose 2 failed: {}'.format(error))
else:
rospy.loginfo('Pose 2 succeeded')
gripper.open()
planning_scene.removeAttachedObject('tray')
planning_scene.removeCollisionObject('table')
planning_scene.removeCollisionObject('divider')
return
def main():
rospy.init_node('load_exec_program_demo')
wait_for_time()
argv = rospy.myargv()
if len(argv) < 2:
print_usage()
return
name = argv[1]
rospy.sleep(0.5)
goal = QueryControllerStatesGoal()
state = ControllerState()
state.name = 'arm_controller/follow_joint_trajectory'
state.state = ControllerState.RUNNING
goal.updates.append(state)
control_client = actionlib.SimpleActionClient(
"/query_controller_states",
robot_controllers_msgs.msg.QueryControllerStatesAction)
# TODO: Wait for server
control_client.wait_for_server()
control_client.send_goal(goal)
control_client.wait_for_result()
torso = robot_api.Torso()
torso.set_height(robot_api.Torso.MAX_HEIGHT)
arm = robot_api.Arm()
program = gripper_program.GripperProgram()
gripper = robot_api.Gripper()
tf_listener = tf.TransformListener()
reader = ArTagReader()
marker_sub = rospy.Subscriber(
'ar_pose_marker', AlvarMarkers, callback=reader.callback
) # Subscribe to AR tag poses, use reader.callback
# rate = rospy.Rate(10)
# rate.sleep()
rospy.sleep(2)
actions = program.load_program(name)
if actions is None:
exit(1)
for action in actions:
if action.gripper_open:
gripper.open()
else:
gripper.close(robot_api.Gripper.MAX_EFFORT)
pose_stamped = action.pose_stamped
id = pose_stamped.header.frame_id
# print(reader.markers)
if id not in reader.markers and id != 'base_link':
print("Frame " + id + " does not exits.")
exit(1)
if id in reader.markers:
marker_tf = pose_to_transform(reader.markers[id].pose.pose)
# print(reader.markers[id].pose)
goal_tf = pose_to_transform(pose_stamped.pose)
result_tf = np.dot(marker_tf, goal_tf)
pose_stamped.header.frame_id = 'base_link'
pose_stamped.pose = transform_to_pose(result_tf)
# pose_stamped.pose = reader.markers[id].pose.pose
# print(pose_stamped)
error = arm.move_to_pose(pose_stamped)
if error is not None:
rospy.logerr(error)
rospy.spin()
def __init__(self):
self._torso = robot_api.Torso()
self._head = robot_api.Head()
self._grip = robot_api.Gripper()
self._arm = robot_api.Arm()
def main():
rospy.init_node("annotator_node")
print("starting node")
wait_for_time()
print("finished node")
print("starting node 2")
listener = tf.TransformListener()
rospy.sleep(1)
print("finished node 2")
reader = ArTagReader()
sub = rospy.Subscriber("/ar_pose_marker", AlvarMarkers, callback=reader.callback)
print('finished subscribing to ARTag reader')
controller_client = actionlib.SimpleActionClient('query_controller_states', QueryControllerStatesAction)
print('passed action lib')
arm = robot_api.Arm()
print('got arm')
gripper = robot_api.Gripper()
print('got gripper')
torso = robot_api.Torso()
print('got torso')
head = robot_api.Head()
print('got head')
server = roboeats.RoboEatsServer()
print_intro()
program = Program(arm, gripper, head, torso)
print("Program created.")
running = True
food_id = None
while running:
user_input = raw_input(">>>")
if not user_input:
# string is empty, ignore
continue
args = user_input.split(" ")
cmd = args[0]
num_args = len(args) - 1
if cmd == "create":
program.reset()
print("Program created.")
elif cmd == "save-pose" or cmd == "sp":
if num_args >= 1:
try:
if num_args == 1:
alias = None
frame = args[1]
elif num_args == 2:
alias = args[1]
frame = args[2]
if frame == "base_link":
(pos, rot) = listener.lookupTransform(frame, "wrist_roll_link", rospy.Time(0))
ps = PoseStamped()
ps.header.frame_id = frame
ps.pose.position.x = pos[0]
ps.pose.position.y = pos[1]
ps.pose.position.z = pos[2]
ps.pose.orientation.x = rot[0]
ps.pose.orientation.y = rot[1]
ps.pose.orientation.z = rot[2]
ps.pose.orientation.w = rot[3]
else:
# while True:
transform = listener.lookupTransform(frame, "base_link", rospy.Time(0))
rot = transform[1]
x, y, z, w = rot
print("stage 1: " + pos_rot_str(transform[0], transform[1]))
tag_T_base = create_transform_matrix(transform)
user_input = raw_input("saved base relative to the frame, move the arm and press enter when done")
transform = listener.lookupTransform("base_link", "wrist_roll_link", rospy.Time(0))
print("stage 2: " + pos_rot_str(transform[0], transform[1]))
base_T_gripper = create_transform_matrix(transform)
ans = np.dot(tag_T_base, base_T_gripper)
ans2 = tft.quaternion_from_matrix(ans)
ps = PoseStamped()
ps.pose.position.x = ans[0, 3]
ps.pose.position.y = ans[1, 3]
ps.pose.position.z = ans[2, 3]
ps.pose.orientation.x = ans2[0]
ps.pose.orientation.y = ans2[1]
ps.pose.orientation.z = ans2[2]
ps.pose.orientation.w = ans2[3]
ps.header.frame_id = frame
print("Saved pose: " + ps_str(ps))
program.add_pose_command(ps, alias)
print("done")
except Exception as e:
print("Exception:", e)
else:
print("No frame given.")
elif cmd == "save-open-gripper" or cmd == "sog":
program.add_open_gripper_command()
gripper.open()
elif cmd == "save-close-gripper" or cmd == "scg":
program.add_close_gripper_command()
gripper.close()
elif cmd == "alias":
if num_args == 2:
i = int(args[1])
alias = args[2]
program.set_alias(i, alias)
else:
print("alias requires <i> <alias>")
elif cmd == "delete" or cmd == "d":
program.delete_last_command()
elif cmd == "replace-frame" or cmd == "rf":
if num_args == 2:
alias = args[1]
new_frame = args[2]
program.replace_frame(alias, new_frame)
else:
print("Expected 2 arguments, got " + str(num_args))
elif cmd == "run-program" or cmd == "rp":
program.run(None)
relax_arm(controller_client)
elif cmd == "savef":
if len(args) == 2:
try:
fp = args[1]
if program is None:
print("There is no active program currently.")
else:
program.save_program(fp)
except Exception as e:
print("Failed to save!\n", e)
else:
print("No save path given.")
elif cmd == "loadf":
if len(args) == 2:
fp = args[1]
if os.path.isfile(fp):
print("File " + fp + " exists. Loading...")
with open(fp, "rb") as load_file:
program.commands = pickle.load(load_file)
print("Program loaded...")
program.print_program()
else:
print("No frame given.")
elif cmd == "print-program" or cmd == "ls" or cmd == "list":
program.print_program()
elif cmd == "relax":
relax_arm(controller_client)
elif cmd == "unrelax":
goal = QueryControllerStatesGoal()
state = ControllerState()
state.name = 'arm_controller/follow_joint_trajectory'
state.state = ControllerState.RUNNING
goal.updates.append(state)
controller_client.send_goal(goal)
controller_client.wait_for_result()
print("Arm is now unrelaxed.")
elif cmd == "tags":
for frame in reader.get_available_tag_frames():
print("\t" + frame)
elif cmd == "help":
print_commands()
elif cmd == "stop":
running = False
elif cmd == "torso":
if num_args == 1:
height = float(args[1])
program.add_set_height_command(height)
torso.set_height(height)
else:
print("missing <height>")
elif cmd == "head":
if num_args == 2:
pan = float(args[1])
tilt = float(args[2])
program.add_set_pan_tilt_command(pan, tilt)
head.pan_tilt(pan, tilt)
else:
print("missing <pan> <tilt>")
elif cmd == "init":
server.init_robot()
elif cmd == "attachl":
server.attach_lunchbox()
elif cmd == "detachl":
server.remove_lunchbox()
elif cmd == "obstacles1":
server.start_obstacles_1()
elif cmd == "obstacles2":
server.start_obstacles_2()
elif cmd == "clear-obstacles":
server.clear_obstacles()
elif cmd == "segment1a":
server.start_segment1a(food_id)
elif cmd == "segment1b":
server.start_segment1b(food_id)
elif cmd == "segment2":
server.start_segment2(food_id)
elif cmd == "segment3":
server.start_segment3(food_id)
elif cmd == "segment4":
server.start_segment4(food_id)
elif cmd == "all-segments":
rqst = StartSequenceRequest()
rqst.id = food_id
server.handle_start_sequence(rqst)
elif cmd == "set-food-id":
if num_args == 1:
food_id = int(args[1])
else:
print("Requires <food_id>")
elif cmd == "addf":
if num_args == 3:
rqst = CreateFoodItemRequest()
rqst.name = args[1]
rqst.description = args[2]
rqst.id = int(args[3])
server.handle_create_food_item(rqst)
else:
print("Requires <name> <description> <id>")
elif cmd == "list-foods":
server.__print_food_items__()
else:
print("NO SUCH COMMAND: " + cmd)
print("")
def main():
print("Initializing...")
rospy.init_node('project_master_node')
wait_for_time()
found_object = False
picked_object = False
face_detected = False
print("Initializing base...")
base = robot_api.Base()
print("Initializing head...")
head = robot_api.Head()
print("Initializing gripper...")
gripper = robot_api.Gripper()
print("Initializing torso...")
torso = robot_api.Torso()
torso.set_height(0.1)
print("Initializing arm motion planner...")
arm = robot_api.Arm()
arm.move_to_initial_pose()
arm_planner = ArmMotionPlanner(arm, gripper)
print("Initializing object detector...")
object_reader = ObjectMarkerReader()
object_marker_sub = rospy.Subscriber('object_markers', Marker, callback=object_reader.callback)
print("Initializing face detector...")
face_detector = FaceDetector()
face_marker_sub = rospy.Subscriber('face_locations', PoseStamped, callback=face_detector.callback)
print("Initializing navigation...")
navigator = Navigator()
rospy.sleep(1)
rate = rospy.Rate(0.5)
print("Initialized")
while not rospy.is_shutdown():
# Part 1
# Look for objects
# If found, move to object location
# If not found, roam around and repeat
while not found_object:
print("Looking for objects...")
head.pan_tilt(0.0, 1.0)
time.sleep(10)
rate.sleep()
if object_reader.object_detected():
found_object = True
print("Found object")
else:
print("Could not find object. Moving around...")
move_random(navigator, base)
time.sleep(2)
rate.sleep()
print("Retrying...")
time.sleep(3)
rate.sleep()
# Part 2
# Pickup object
# Goto the object location and attempt to pick it up
# If success, go to next step
# If failure, go to Part 1
while found_object and not picked_object:
object = object_reader.get_object()
if object is None:
print("Detected Object Gone...")
found_object = False
arm.move_to_initial_pose()
continue
target_pose = transform_marker(navigator, object)
reached = True
if target_pose == None:
reached = True
else:
reached = navigator.goto(target_pose, MOVE_TIMEOUT)
if not reached:
print("Could not reach the object. Retrying...")
continue
else:
head.pan_tilt(0.0, 1.0)
time.sleep(10)
rate.sleep()
object_to_pick = object_reader.get_object(object.id)
if object_to_pick is None:
print("Object Gone...")
found_object = False
arm.move_to_initial_pose()
continue
print(object_to_pick)
arm.move_to_hold_pose()
picked_object = arm_planner.pick_up(object_to_pick)
if not picked_object:
print("Could not pick the object. Retrying...")
found_object = False
arm.move_to_initial_pose()
else:
print("Picked object")
arm.move_to_hold_pose()
# Part 3
# Look for face
# If found, move to face location
# If not found, roam around and repeat
while found_object and picked_object and not face_detected:
print("Looking for people...")
head.pan_tilt(0.0, 0.0)
time.sleep(3)
if face_detector.face_detected():
face_detected = True
print("Found person")
else:
print("Could not find face. Moving around...")
move_random(navigator, base)
time.sleep(2)
rate.sleep()
print("Retrying...")
# Part 4
# Go to the face location
# Wait for 5 seconds and open the gripper
# Go to Part 1
if found_object and picked_object and face_detected:
print("Moving to person...")
face_location = transform_marker(navigator, face_detector.get_face())
face_detected = navigator.goto(face_location, MOVE_TIMEOUT)
reached = navigator.goto(face_location, MOVE_TIMEOUT)
if not reached:
print("Count not reach the face. Retrying...")
continue
else:
print("Delivering object...")
time.sleep(3)
gripper.open()
found_object = False
picked_object = False
face_detected = False
print("Object delivered")
arm.move_to_initial_pose()
rate.sleep()