def __init__(self):
self._drink_orders = Queue()
self._is_working = False
# Initialize Navigation Server.
self._nav_server = NavigationServer()
self._nav_server.loadMarkers()
# Initialize Arm and Gripper
self._gripper = fetch_api.Gripper()
self._arm = fetch_api.Arm()
rospy.logdebug('Gripper and Arm instantiated.');
self._arm_server = ArmServer()
# Startup Fetch ARM
self._controller_client = actionlib.SimpleActionClient('/query_controller_states', QueryControllerStatesAction)
rospy.sleep(0.5)
goal = QueryControllerStatesGoal()
state = ControllerState()
state.name = 'arm_controller/follow_joint_trajectory'
state.state = ControllerState.RUNNING
goal.updates.append(state)
self._controller_client.send_goal(goal)
# Wait for Fetch ARM
rospy.loginfo('Waiting for arm to start.')
self._controller_client.wait_for_result()
rospy.loginfo('Arm has started..')
# Instantiate subscription model with front-end
self._drink_order_subscriber = rospy.Subscriber('/drink_order', DrinkOrder, self.drink_order_handler)
self._drink_status_publisher = rospy.Publisher('/drink_status', DrinkStatus, queue_size=1)
def start_arm(self):
goal = QueryControllerStatesGoal()
state = ControllerState()
state.name = 'arm_controller/follow_joint_trajectory'
state.state = ControllerState.RUNNING
goal.updates.append(state)
self._controller_client.send_goal(goal)
self._controller_client.wait_for_result()
def create_program(self):
goal = QueryControllerStatesGoal()
state = ControllerState()
state.name = 'arm_controller/follow_joint_trajectory'
state.state = ControllerState.STOPPED
goal.updates.append(state)
self._controller_client.send_goal(goal)
self._controller_client.wait_for_result()
def servo_off(self):
# Reset controllers
goal_servo_off = QueryControllerStatesGoal()
for controller in self.gravity_compsensation:
state = ControllerState()
state.name = controller
state.state = state.RUNNING
goal_servo_off.updates.append(state)
for controller in self.follow_joint_trajectory:
state = ControllerState()
state.name = controller
state.state = state.STOPPED
goal_servo_off.updates.append(state)
self.controller_client.send_goal(goal_servo_off)
def relax_arm(controller_client):
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()
print("Arm is now relaxed.")
def _switch(self, command):
state = ControllerState()
state.name = 'arm_controller/follow_joint_trajectory'
state.state = command
goal = QueryControllerStatesGoal()
goal.updates.append(state)
self._client.send_goal(goal)
self._client.wait_for_result()
result = self._client.get_result()
def relax_arm(self):
'''Turns on gravity compensation controller and turns
off other controllers
'''
goal = QueryControllerStatesGoal()
for controller in self._gravity_comp_controllers:
state = ControllerState()
state.name = controller
state.state = state.RUNNING
goal.updates.append(state)
for controller in self._non_gravity_comp_controllers:
state = ControllerState()
state.name = controller
state.state = state.STOPPED
goal.updates.append(state)
self._controller_client.send_goal(goal)
def main():
global nav_server
global WORKING
global arm_server
global orders
global sound_handler
sound_handler = SoundClient()
orders = []
WORKING = None
rospy.init_node('barbot_controller_node')
wait_for_time()
nav_server = NavigationServer()
nav_server.loadMarkers()
gripper = fetch_api.Gripper()
arm = fetch_api.Arm()
arm_server = ArmServer()
print 'Arm server instantiated.'
# MOVE TO MICHAEL_NODE
controller_client = actionlib.SimpleActionClient(
'/query_controller_states', QueryControllerStatesAction)
rospy.sleep(0.5)
goal = QueryControllerStatesGoal()
state = ControllerState()
state.name = 'arm_controller/follow_joint_trajectory'
state.state = ControllerState.RUNNING
goal.updates.append(state)
controller_client.send_goal(goal)
print 'Waiting for arm to start.'
controller_client.wait_for_result()
arm_server.lookup()
arm_server.set_init_poses()
arm_server.set_prepose()
# nav_server.goToMarker(BAR_TABLE)
print 'going home'
nav_server.goToMarker(HOME)
print 'please start to order now'
sound_handler.say('You may now order.')
# # handle user actions
drink_order_sub = rospy.Subscriber('/drink_order', DrinkOrder,
handle_user_actions)
signal.signal(signal.SIGINT, signal_handler)
while True:
query_orders()
rospy.sleep(0.1)
def reset(self):
# Reset controllers
goal = QueryControllerStatesGoal()
for controller in self.start:
state = ControllerState()
state.name = controller
state.state = state.RUNNING
goal.updates.append(state)
for controller in self.stop:
state = ControllerState()
state.name = controller
state.state = state.STOPPED
goal.updates.append(state)
self.controller_client.send_goal(goal)
# Disable gripper torque
goal = GripperCommandGoal()
goal.command.max_effort = -1.0
self.gripper_client.send_goal(goal)
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():
pose_actions = None
# Get the actions from the pickel file.
argv = rospy.myargv()
if len(argv) < 2:
usage()
return
fileName = argv[1]
try:
pose_actions = pickle.load(open(fileName, "rb"))
print '{} loaded.'.format(fileName)
except:
print '{} could not be loaded.'.format(fileName)
usage()
return
rospy.init_node("load_file_actions")
wait_for_time()
print 'Time has begun.'
# Start the arm.
reader = ArTagReader()
sub = rospy.Subscriber('/ar_pose_marker', AlvarMarkers, reader.callback)
controller_client = actionlib.SimpleActionClient('/query_controller_states', QueryControllerStatesAction)
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)
print 'Waiting for arm to start.'
controller_client.wait_for_result()
print 'Arm has been started.'
gripper = fetch_api.Gripper()
arm = fetch_api.Arm()
print 'Arm and gripper instantiated.'
rospy.sleep(1.0)
count = 0
# Run through each of the actions
for pose_action in pose_actions:
print 'Performing action.'
if pose_action.actionType == PoseExecutable.OPEN:
print 'Opening the gripper'
gripper.open()
elif pose_action.actionType == PoseExecutable.CLOSE:
print 'Closing the gripper'
gripper.close()
elif pose_action.actionType == PoseExecutable.MOVETO:
count += 1
print 'Moving to location.'
pose_stamped = PoseStamped()
pose_stamped.header.frame_id = "base_link"
if pose_action.relativeFrame == 'base_link':
pose_stamped.pose = pose_action.pose
else:
for marker in reader.markers:
if pose_action.relativeFrame == marker.id:
wrist2 = makeMatrix(pose_action.pose)
tag = makeMatrix(pose_action.arPose)
tag2 = tf.transformations.inverse_matrix(tag)
result = np.dot(tag2, wrist2)
result2 = np.dot(makeMatrix(marker.pose.pose), result)
pose_stamped = PoseStamped()
pose_stamped.header.frame_id = "base_link"
pose_stamped.pose = transform_to_pose(result2)
error = arm.move_to_pose(pose_stamped, allowed_planning_time=40, num_planning_attempts=20)
if fileName == PICKLE_FILE_DISPENSE and count == 2:
print 'Dispensing!'
rospy.sleep(DISPENSE_TIME)
if error is not None:
print 'Error moving to {}.'.format(pose_action.pose)
else:
print 'invalid command {}'.format(pose_action.action)
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
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("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("")
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:
print(" RUNNING %s [%s]" % (state.name, state.type))
elif state.state == state.STOPPED:
print(" STOPPED %s [%s]" % (state.name, state.type))
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.STOPPED
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:
print(" RUNNING %s [%s]" % (state.name, state.type))
elif state.state == state.STOPPED:
print(" STOPPED %s [%s]" % (state.name, state.type))
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()
if __name__ == "__main__":
if len(sys.argv) < 2:
print("usage: stop_controller.py <controller_name> [optional_controller_type]")
exit(-1)
rospy.init_node("stop_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.STOPPED
goal = QueryControllerStatesGoal()
goal.updates.append(state)
rospy.loginfo("Requesting that %s be stopped..." % 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())