def send_request(self, req):
self.req = req
self.future = self.client.call_async(self.req)
if __name__ == "__main__":
if len(sys.argv) < 2:
print("usage: start_controller.py <controller_name> [optional_controller_type]")
exit(-1)
rclpy.init()
client = MinimalClientAsync()
state = ControllerState()
state.name = sys.argv[1]
if len(sys.argv) > 2:
state.type = sys.argv[2]
state.state = state.RUNNING
req = QueryControllerStates.Request()
req.updates.append(state)
client.send_request(req)
while rclpy.ok():
rclpy.spin_once(client)
if client.future.done():
response = client.future.result()
for state in response.state:
if state.name == sys.argv[1]:
if state.state == state.RUNNING:
def main():
rospy.init_node('create_pbd_action')
wait_for_time()
# Check to see the proper args were given
argv = rospy.myargv()
if len(argv) < 2:
print_usage()
return
save_file_name = argv[1]
# This is the list of poses that we will save
sequence = []
# The Arm and the gripper on the robot
gripper = fetch_api.Gripper()
arm = fetch_api.Arm()
gripper_open = True
# Init a a tfListener for reading the gripper pose
listener = tf.TransformListener()
rospy.sleep(rospy.Duration.from_sec(1))
# Init the reader for the tags
reader = ArTagReader()
sub = rospy.Subscriber('/ar_pose_marker', AlvarMarkers, reader.callback)
# Step 1: Relax the arm
if not IN_SIM:
controller_client = actionlib.SimpleActionClient(
'/query_controller_states', QueryControllerStatesAction)
# Sleep for a second to make sure the client starts up
rospy.sleep(1.0)
# The rest of this code is given in the lab
goal = QueryControllerStatesGoal()
state = ControllerState()
state.name = 'arm_controller/follow_joint_trajectory'
state.state = ControllerState.STOPPED
goal.updates.append(state)
controller_client.send_goal(goal)
controller_client.wait_for_result()
# Get the user interface going
print_user_options()
running = True
while running:
user_input = raw_input("")
if user_input == "save_pose":
# Get the current pose with respect to the base_link
current_gripper_pose = get_current_gripper_pose(listener)
# Ask the user which frame they would like to save the pose to
# Eg: base_link, tag 1, tag 2
print "Please input the frame you would like to save the pose in. The options are:"
print " base_link"
for marker in reader.markers:
print " %s" % marker.id
frame = raw_input("")
# Check the frame the pose should be saved in
pose = Pose()
frame_id = frame
if frame == "base_link":
pose = current_gripper_pose
frame_id = 'base_link'
else:
for marker in reader.markers:
if frame == str(marker.id):
# First, we need to turn the poses into transform matrixes
pose_in_base_matrix = pose_to_transform(
current_gripper_pose)
tag_in_base_matrix = pose_to_transform(
marker.pose.pose)
# Then take the inverse of the tag in the base fram
inv_matrix = np.linalg.inv(tag_in_base_matrix)
# Then take the dot product
pose_to_save = np.dot(inv_matrix, pose_in_base_matrix)
# The target pose is then transformed back into a pose object
pose = transform_to_pose(pose_to_save)
frame_id = str(marker.id)
# Create and append the PBD_Pose
pbd_pose = PBD_Pose(pose, gripper_open, frame)
sequence.append(pbd_pose)
if user_input == "open_gripper":
gripper.open()
gripper_open = True
if user_input == "close_gripper":
gripper.close()
gripper_open = False
if user_input == "save_program":
pickle.dump(sequence, open(save_file_name, "wb"))
if user_input == "help":
print_user_options()
if user_input == "quit":
running = False
def main():
rospy.init_node('create_save_program_demo')
wait_for_time()
print('Welcome to the gripper program!\n')
print_usage()
rospy.sleep(0.5)
control_client = actionlib.SimpleActionClient(
"/query_controller_states",
robot_controllers_msgs.msg.QueryControllerStatesAction)
# TODO: Wait for server
control_client.wait_for_server()
goal = QueryControllerStatesGoal()
state = ControllerState()
state.name = 'arm_controller/follow_joint_trajectory'
state.state = ControllerState.STOPPED
goal.updates.append(state)
control_client.send_goal(goal)
control_client.wait_for_result()
# program = gripper_program.GripperProgram()
# gripper = robot_api.Gripper()
# gripper_open = True
# 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)
#
# while True :
# print('\n> '),
# l = sys.stdin.readline().split()
# c = l[0]
# name = None
# if len(l) > 1:
# name = ' '.join(l[1:])
#
# if c == 'q':
# break
# elif c == 'help':
# print_usage()
# elif c == '1':
# program.create_program()
# gripper.open()
# gripper_open = True
# print("program created")
# elif c == '2':
# print("You have the following options as reference frame:")
# reader.print_marker_id()
# print('frame: '),
# l = sys.stdin.readline().split()
# c = l[0]
# if c not in reader.markers and c is not 'base_link':
# print("reference name not found")
# continue
# # l = sys.stdin.readline().split()
# now = rospy.Time.now()
# tf_listener.waitForTransform('base_link', 'wrist_roll_link', now, rospy.Duration(4.0))
# trans, rot = tf_listener.lookupTransform('base_link', 'wrist_roll_link', now)
#
# ps = PoseStamped()
# ps.header.frame_id = 'base_link'
# pose = Pose()
# pose.position.x = trans[0]
# pose.position.y = trans[1]
# pose.position.z = trans[2]
#
# pose.orientation.x = rot[0]
# pose.orientation.y = rot[1]
# pose.orientation.z = rot[2]
# pose.orientation.w = rot[3]
#
# if c in reader.markers:
# gripper_pos_tf = pose_to_transform(pose)
# dest_marker_tf = pose_to_transform(reader.markers[c].pose.pose)
# inverse = np.matrix(dest_marker_tf).I
# result_tf = np.dot(inverse, gripper_pos_tf)
# ps.header.frame_id = c
# ps.pose = transform_to_pose(result_tf)
# else:
# ps.pose = pose
#
# program.save_pose(ps, gripper_open)
# print("pose saved")
# elif c == '3':
# gripper.open()
# gripper_open = True
# print("gripper opened")
# elif c == '4':
# gripper.close(robot_api.Gripper.MAX_EFFORT)
# gripper_open = False
# print("gripper closed")
# elif c == '5':
# if name is None:
# print("save program requires a name")
# continue
# program.save_program(name)
# print("program saved")
# rate.sleep()
rospy.spin()
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()
ACTION_NAME = "/query_controller_states"
if __name__ == "__main__":
if len(sys.argv) < 2:
print("usage: start_controller.py <controller_name> [optional_controller_type]")
exit(-1)
rospy.init_node("start_robot_controllers")
rospy.loginfo("Connecting to %s..." % ACTION_NAME)
client = actionlib.SimpleActionClient(ACTION_NAME, QueryControllerStatesAction)
client.wait_for_server()
rospy.loginfo("Done.")
state = ControllerState()
state.name = sys.argv[1]
if len(sys.argv) > 2:
state.type = sys.argv[2]
state.state = state.RUNNING
goal = QueryControllerStatesGoal()
goal.updates.append(state)
rospy.loginfo("Requesting that %s be started..." % state.name)
client.send_goal(goal)
client.wait_for_result()
if client.get_state() == GoalStatus.SUCCEEDED:
rospy.loginfo("Done.")
elif client.get_state() == GoalStatus.ABORTED:
rospy.logerr(client.get_goal_status_text())
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():
rospy.init_node("execute_pbd_action")
wait_for_time()
argv = rospy.myargv()
if len(argv) < 2:
print_usage()
return
load_file_name = argv[1]
# TODO: Get this to work. I'm not sure why it isn't. Probably the same problem as the create file
controller_client = actionlib.SimpleActionClient(
'/query_controller_states', QueryControllerStatesAction)
# Sleep for a second to make sure the client starts up
rospy.sleep(1.0)
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()
# Load the sequence of poses
sequence = pickle.load(open(load_file_name, "rb"))
# The Arm and the gripper on the robot
gripper = Gripper()
arm = Arm()
gripper_open = True
# Init the reader for the tags
reader = ArTagReader()
sub = rospy.Subscriber('/ar_pose_marker', AlvarMarkers, reader.callback)
# Sleep to make sure the markers show up
rospy.sleep(1.0)
# For each pose in the list, move to that pose
# and open the gripper as necessary
for pbd_pose in sequence:
move_pose = PoseStamped()
move_pose.header.frame_id = 'base_link'
if pbd_pose.frame == 'base_link':
move_pose.pose = pbd_pose.pose
else:
for marker in reader.markers:
if pbd_pose.frame == str(marker.id):
# Transform the pose to be in the base_link frame
pose_in_tag_frame = pose_to_transform(pbd_pose.pose)
tag_in_base_frame = pose_to_transform(marker.pose.pose)
target_matrix = np.dot(tag_in_base_frame,
pose_in_tag_frame)
target_pose = transform_to_pose(target_matrix)
move_pose.pose = target_pose
err = arm.move_to_pose(
move_pose
) # TODO: maybe not fail? or just use better sequences of poses less likely to fail
if err is not None:
print "Error in move to pose: ", err
# Check the gripper to open/close
if pbd_pose.gripper_open != gripper_open:
if gripper_open == True:
gripper.close()
gripper_open = False
else:
gripper.open()
gripper_open = True