def srv_store_program_cb(self, req):
with self.lock:
resp = storeProgramResponse()
resp.success = False
name = "program:" + str(req.program.header.id)
try:
prog = self.db.query_named(name, Program._type)[0]
except rospy.ServiceException as e:
print "Service call failed: " + str(e)
return resp
if prog is not None and prog.header.readonly:
resp.error = "Readonly program."
return resp
ph = ProgramHelper()
if not ph.load(req.program):
resp.error = "Invalid program"
return resp
try:
ret = self.db.update_named(name, req.program, upsert=True)
except rospy.ServiceException as e:
print "Service call failed: " + str(e)
return resp
resp.success = ret.success
return resp
def show_program_list(self):
self.notif(translate("UICoreRos", "Please select a program"))
headers = self.art.get_program_headers()
d = {}
headers_to_show = []
for header in headers:
ph = ProgramHelper()
d[header.id] = None
if ph.load(self.art.load_program(header.id)):
headers_to_show.append(header)
d[header.id] = ph.program_learned()
self.program_list = ProgramListItem(self.scene, self.last_prog_pos[0],
self.last_prog_pos[1],
headers_to_show, d,
self.last_edited_prog_id,
self.program_selected_cb)
def main(args):
global state_manager
global ph
global art
global action_server
global left_gripper_pub
global right_gripper_pub
global gripper_timer
rospy.init_node('test_brain')
state_manager = InterfaceStateManager(InterfaceState.BRAIN_ID, callback)
ph = ProgramHelper()
state_manager.set_system_state(InterfaceState.STATE_IDLE)
rospy.Service('/art/brain/program/start', ProgramIdTrigger, start_program)
rospy.Service('/art/brain/learning/start', ProgramIdTrigger,
learning_srv_cb)
rospy.Service('/art/brain/learning/stop', Trigger, learning_srv_cb)
# rospy.Service('/art/brain/program/stop', stopProgram, stop_program)
art = ArtApiHelper()
action_server = actionlib.SimpleActionServer(
'/art/brain/learning_request',
LearningRequestAction,
execute_cb=learning_request_cb,
auto_start=False)
action_server.start()
left_gripper_pub = rospy.Publisher('/art/pr2/left_arm/gripper/pose',
PoseStamped,
queue_size=10)
right_gripper_pub = rospy.Publisher('/art/pr2/right_arm/gripper/pose',
PoseStamped,
queue_size=10)
gripper_timer = rospy.Timer(rospy.Duration(0.5), gripper_timer)
rospy.spin()
def cb_program_selection(self):
self.notif(translate("UICoreRos", "Please select a program"))
prog_id = None
if self.program_vis is not None:
pos = self.program_vis.get_pos()
prog_id = self.ph.get_program_id()
else:
pos = (0.2, self.height - 0.2)
self.remove_scene_items_by_type(ProgramItem)
self.program_vis = None
self.remove_scene_items_by_type(ProgramListItem)
headers = self.art.get_program_headers()
d = {}
headers_to_show = []
for header in headers:
ph = ProgramHelper()
d[header.id] = None
if ph.load(self.art.load_program(header.id)):
headers_to_show.append(header)
d[header.id] = ph.program_learned()
# rospy.loginfo(str(d))
self.program_list = ProgramListItem(self.scene, pos[0], pos[1],
headers_to_show, d, prog_id,
self.program_selected_cb)
def __init__(self):
origin = rospy.get_param("scene_origin")
size = rospy.get_param("scene_size")
rpm = rospy.get_param("rpm")
port = rospy.get_param("scene_server_port")
super(UICoreRos, self).__init__(origin[0], origin[1], size[0], size[1],
rpm, port)
QtCore.QObject.connect(self, QtCore.SIGNAL('objects'),
self.object_cb_evt)
QtCore.QObject.connect(self, QtCore.SIGNAL('user_status'),
self.user_status_cb_evt)
QtCore.QObject.connect(self, QtCore.SIGNAL('interface_state'),
self.interface_state_evt)
QtCore.QObject.connect(self,
QtCore.SIGNAL('touch_calibration_points_evt'),
self.touch_calibration_points_evt)
QtCore.QObject.connect(self, QtCore.SIGNAL('touch_detected_evt'),
self.touch_detected_evt)
QtCore.QObject.connect(self, QtCore.SIGNAL('notify_user_evt'),
self.notify_user_evt)
QtCore.QObject.connect(self,
QtCore.SIGNAL('learning_request_done_evt'),
self.learning_request_done_evt)
self.user_state = None
self.program_list = None
self.program_vis = None
self.template = False # TODO this should be stored in program_vis?
self.last_prog_pos = (0.2, self.height - 0.2)
self.last_edited_prog_id = None
self.ph = ProgramHelper()
cursors = rospy.get_param("~cursors", [])
for cur in cursors:
PoseStampedCursorItem(self.scene, cur)
TouchTableItem(
self.scene, '/art/interface/touchtable/touch',
list(self.get_scene_items_by_type(PoseStampedCursorItem)))
self.stop_btn = ButtonItem(self.scene, 0, 0, "STOP", None,
self.stop_btn_clicked, 2.0, QtCore.Qt.red)
self.stop_btn.setPos(
self.scene.width() - self.stop_btn.boundingRect().width() - 300,
self.scene.height() - self.stop_btn.boundingRect().height() - 60)
self.stop_btn.set_enabled(True)
self.projectors = []
projs = rospy.get_param("~projectors", [])
for proj in projs:
self.projectors.append(ProjectorHelper(proj))
rospy.loginfo("Waiting for /art/brain/learning_request")
self.learning_action_cl = actionlib.SimpleActionClient(
'/art/brain/learning_request', LearningRequestAction)
self.learning_action_cl.wait_for_server()
self.art = ArtApiHelper()
self.projectors_calibrated_pub = rospy.Publisher(
"~projectors_calibrated", Bool, queue_size=1, latch=True)
self.projectors_calibrated_pub.publish(False)
self.start_learning_srv = rospy.ServiceProxy(
'/art/brain/learning/start',
startProgram) # TODO wait for service? where?
self.stop_learning_srv = rospy.ServiceProxy(
'/art/brain/learning/stop',
Trigger) # TODO wait for service? where?
self.program_pause_srv = rospy.ServiceProxy('/art/brain/program/pause',
Trigger)
self.program_resume_srv = rospy.ServiceProxy(
'/art/brain/program/resume', Trigger)
self.program_stop_srv = rospy.ServiceProxy('/art/brain/program/stop',
Trigger)
self.emergency_stop_srv = rospy.ServiceProxy(
'/pr2_ethercat/halt_motors',
EmptyService) # TODO wait for service? where?
self.emergency_stop_reset_srv = rospy.ServiceProxy(
'/pr2_ethercat/reset_motors',
EmptyService) # TODO wait for service? where?
self.program_error_resolve_srv = rospy.ServiceProxy(
'/art/brain/program/error_response',
ProgramErrorResolve) # TODO wait for service? where?
self.program_error_dialog = None
self.grasp_dialog = None
self.emergency_stopped = False
rospy.loginfo("Waiting for ART services...")
self.art.wait_for_api()
rospy.loginfo("Ready! Starting state machine.")
# TODO move this to ArtApiHelper ??
self.obj_sub = rospy.Subscriber('/art/object_detector/object_filtered',
InstancesArray,
self.object_cb,
queue_size=1)
self.user_status_sub = rospy.Subscriber('/art/user/status',
UserStatus,
self.user_status_cb,
queue_size=1)
self.touch_points = None
self.touch_calib_srv = rospy.Service(
'/art/interface/projected_gui/touch_calibration',
TouchCalibrationPoints, self.touch_calibration_points_cb)
self.notify_user_srv = rospy.Service(
'/art/interface/projected_gui/notify_user', NotifyUser,
self.notify_user_srv_cb)
proj_calib = True
if len(self.projectors) > 0:
rospy.loginfo("Waiting for projector nodes...")
for proj in self.projectors:
proj.wait_until_available()
if not proj.is_calibrated():
proj_calib = False
if proj_calib:
rospy.loginfo('Projectors already calibrated.')
self.projectors_calibrated_pub.publish(True)
else:
rospy.loginfo(
'Projectors not calibrated yet - waiting for command...')
self.projector_calib_srv = rospy.Service(
'/art/interface/projected_gui/calibrate_projectors', Trigger,
self.calibrate_projectors_cb)
self.state_manager = InterfaceStateManager("PROJECTED UI",
cb=self.interface_state_cb)
def main(args):
if len(args) < 2:
print "This script needs program id as argument."
return
rospy.init_node('plot_program', anonymous=True)
art = ArtApiHelper()
ph = ProgramHelper()
prog = art.load_program(int(args[1]))
if prog is None:
print "Failed to load program"
return
if not ph.load(prog):
print "Faulty program"
return
A = pgv.AGraph(label="<<B>Program ID: " + str(prog.header.id) + "<br/>" +
escape(prog.header.name) + "</B>>",
directed=True,
strict=False)
A.graph_attr['outputorder'] = 'edgesfirst'
A.add_node("start", label="Program start")
A.add_node("b0i0", label="Program end")
A.add_edge("start", get_node_name(*ph.get_first_item_id()))
for block_id in ph.get_block_ids():
block_msg = ph.get_block_msg(block_id)
ids = []
for item_id in ph.get_items_ids(block_id):
nn = get_node_name(block_id, item_id)
item_msg = ph.get_item_msg(block_id, item_id)
osn = get_node_name(*ph.get_id_on_success(block_id, item_id))
ofn = get_node_name(*ph.get_id_on_failure(block_id, item_id))
A.add_edge(nn, osn, color="green", constraint=True)
A.add_edge(nn, ofn, color="red", constraint=True)
for ref in item_msg.ref_id:
A.add_edge(get_node_name(block_id, ref),
nn,
color="gray",
style="dashed",
key="ref_" + nn + "_" +
get_node_name(block_id, ref))
A.get_node(nn).attr['label'] = 'Item ID: ' + str(
item_id) + '\n' + get_type_string(item_msg)
A.get_node(nn).attr['shape'] = 'box'
A.get_node(nn).attr['style'] = 'rounded'
ids.append(nn)
sg = A.subgraph(name="cluster_" + str(block_id),
label="<<b>Group ID: " + str(block_id) + "<br/>" +
escape(block_msg.name) + "</b>>",
color="gray")
sg.add_nodes_from(ids)
A.layout(prog='dot')
A.draw('output.pdf')
def __init__(self):
origin = rospy.get_param("scene_origin")
size = rospy.get_param("scene_size")
rpm = rospy.get_param("rpm")
port = rospy.get_param("scene_server_port")
super(UICoreRos, self).__init__(origin[0], origin[1], size[0], size[1],
rpm, port)
QtCore.QObject.connect(self, QtCore.SIGNAL('objects'),
self.object_cb_evt)
QtCore.QObject.connect(self, QtCore.SIGNAL('user_status'),
self.user_status_cb_evt)
QtCore.QObject.connect(self, QtCore.SIGNAL('interface_state'),
self.interface_state_evt)
QtCore.QObject.connect(self,
QtCore.SIGNAL('touch_calibration_points_evt'),
self.touch_calibration_points_evt)
QtCore.QObject.connect(self, QtCore.SIGNAL('touch_detected_evt'),
self.touch_detected_evt)
QtCore.QObject.connect(self, QtCore.SIGNAL('notify_user_evt'),
self.notify_user_evt)
QtCore.QObject.connect(self,
QtCore.SIGNAL('learning_request_done_evt'),
self.learning_request_done_evt)
self.user_status = None
self.fsm = FSM()
# TODO do this automatically??
# map callbacks from FSM to this instance
self.fsm.cb_start_calibration = self.cb_start_calibration
self.fsm.cb_waiting_for_user = self.cb_waiting_for_user
self.fsm.cb_program_selection = self.cb_program_selection
self.fsm.cb_waiting_for_user_calibration = self.cb_waiting_for_user_calibration
self.fsm.cb_learning = self.cb_learning
self.fsm.cb_running = self.cb_running
self.fsm.is_template = self.is_template
self.program_vis = None
self.template = False # TODO this should be stored in program_vis?
self.state_manager = InterfaceStateManager("PROJECTED UI",
cb=self.interface_state_cb)
self.ph = ProgramHelper()
cursors = rospy.get_param("~cursors", [])
for cur in cursors:
PoseStampedCursorItem(self.scene, cur)
TouchTableItem(
self.scene, '/art/interface/touchtable/touch',
list(self.get_scene_items_by_type(PoseStampedCursorItem)))
self.stop_btn = ButtonItem(self.scene, 0, 0, "STOP", None,
self.stop_btn_clicked, 2.0, QtCore.Qt.red)
self.stop_btn.setPos(
self.scene.width() - self.stop_btn.boundingRect().width() - 300,
self.scene.height() - self.stop_btn.boundingRect().height() - 60)
self.stop_btn.set_enabled(True)
self.projectors = []
projs = rospy.get_param("~projectors", [])
for proj in projs:
self.add_projector(proj)
rospy.loginfo("Waiting for /art/brain/learning_request")
self.learning_action_cl = actionlib.SimpleActionClient(
'/art/brain/learning_request', LearningRequestAction)
self.learning_action_cl.wait_for_server()
self.art = ArtApiHelper()
self.projectors_calibrated_pub = rospy.Publisher(
"~projectors_calibrated", Bool, queue_size=1, latch=True)
self.projectors_calibrated_pub.publish(False)
self.start_learning_srv = rospy.ServiceProxy(
'/art/brain/learning/start',
Trigger) # TODO wait for service? where?
self.stop_learning_srv = rospy.ServiceProxy(
'/art/brain/learning/stop',
Trigger) # TODO wait for service? where?
self.emergency_stop_srv = rospy.ServiceProxy(
'/pr2_ethercat/halt_motors',
EmptyService) # TODO wait for service? where?
self.emergency_stop_reset_srv = rospy.ServiceProxy(
'/pr2_ethercat/reset_motors',
EmptyService) # TODO wait for service? where?
self.program_error_resolve_srv = rospy.ServiceProxy(
'/art/brain/program/error_response',
ProgramErrorResolve) # TODO wait for service? where?
self.program_error_dialog = None
self.grasp_dialog = None
self.emergency_stoped = False
def setUp(self):
# TODO add more blocks
self.prog = Program()
self.ph = ProgramHelper()
self.prog.header.id = 666
self.prog.header.name = "Basic pick&place"
pb = ProgramBlock()
pb.id = 1 # can't be zero
pb.name = "First block"
pb.on_success = 1
pb.on_failure = 0
self.prog.blocks.append(pb)
p = ProgramItem()
p.id = 1
p.on_success = 2
p.on_failure = 0
p.type = ProgramItem.GET_READY
pb.items.append(deepcopy(p))
p = ProgramItem()
p.id = 2
p.on_success = 3
p.on_failure = 0
p.type = ProgramItem.WAIT_FOR_USER
pb.items.append(deepcopy(p))
p = ProgramItem()
p.id = 3
p.on_success = 31
p.on_failure = 0
p.type = ProgramItem.PICK_FROM_FEEDER
p.object.append("profile")
pf = PoseStamped()
pf.header.frame_id = "marker"
pf.pose.position.x = 0.75
pf.pose.position.y = 0.5
p.pose.append(pf)
pb.items.append(deepcopy(p))
p = ProgramItem()
p.id = 31
p.on_success = 4
p.on_failure = 0
p.type = ProgramItem.VISUAL_INSPECTION
p.ref_id.append(3)
pf = PoseStamped()
pf.header.frame_id = "marker"
pf.pose.position.x = 0.75
pf.pose.position.y = 0.5
pf.pose.position.z = 0.1
p.pose.append(pf)
pb.items.append(deepcopy(p))
p = ProgramItem()
p.id = 4
p.on_success = 5
p.on_failure = 0
p.type = ProgramItem.PLACE_TO_POSE
p.ref_id.append(3)
p.ref_id.append(5)
pp = PoseStamped()
pp.header.frame_id = "marker"
pp.pose.position.x = 0.75
pp.pose.position.y = 0.5
p.pose.append(pp)
pb.items.append(deepcopy(p))
p = ProgramItem()
p.id = 5
p.on_success = 6
p.on_failure = 0
p.type = ProgramItem.PICK_FROM_FEEDER
p.object.append("profile")
pf = PoseStamped()
pf.header.frame_id = "marker"
pf.pose.position.x = 0.75
pf.pose.position.y = 0.5
p.pose.append(pf)
pb.items.append(deepcopy(p))
p = ProgramItem()
p.id = 6
p.on_success = 7
p.on_failure = 0
p.type = ProgramItem.GET_READY
pb.items.append(deepcopy(p))
p = ProgramItem()
p.id = 7
p.on_success = 8
p.on_failure = 0
p.type = ProgramItem.WAIT_UNTIL_USER_FINISHES
pb.items.append(deepcopy(p))
p = ProgramItem()
p.id = 8
p.on_success = 9
p.on_failure = 0
p.type = ProgramItem.PICK_FROM_POLYGON
p.object.append("profile")
pp = PolygonStamped()
pp.header.frame_id = "marker"
pp.polygon.points.append(Point32(0.4, 0.1, 0))
pp.polygon.points.append(Point32(1.0, 0.1, 0))
pp.polygon.points.append(Point32(1.0, 0.6, 0))
pp.polygon.points.append(Point32(0.4, 0.6, 0))
p.polygon.append(pp)
pb.items.append(deepcopy(p))
p = ProgramItem()
p.id = 9
p.on_success = 4
p.on_failure = 0
p.type = ProgramItem.PLACE_TO_POSE
p.ref_id.append(8)
pp = PoseStamped()
pp.header.frame_id = "marker"
pp.pose.position.x = 0.75
pp.pose.position.y = 0.5
p.pose.append(pp)
pb.items.append(deepcopy(p))