def setUp(self):
self.tfl = tf.TransformListener()
rospy.sleep(2.0)
self.world_frame = "marker"
self.object_id = "21"
api = ArtApiHelper()
api.wait_for_db_api()
api.store_object_type(obj_type("fake_object_type", 0.046, 0.046, 0.154))
def __init__(self):
self.ar_code_sub = rospy.Subscriber("ar_pose_marker", AlvarMarkers,
self.ar_code_cb)
self.detected_objects_pub = rospy.Publisher(
"/art/object_detector/object", InstancesArray, queue_size=10)
self.visualize_pub = rospy.Publisher(
"art/object_detector/visualize_objects", Marker, queue_size=10)
# TODO make a timer clearing this cache from time to time
self.objects_cache = {}
self.art = ArtApiHelper()
self.art.wait_for_api()
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 __init__(self, target_frame="marker"):
self.target_frame = target_frame
self.tfl = tf.TransformListener()
self.lock = threading.Lock()
self.detection_enabled = True
self.use_forearm_cams = False
self.table_size = array_from_param("/art/conf/table/size", float, 2, wait=True)
self.ground_objects_on_table = rospy.get_param("~ground_objects_on_table", False)
self.yaw_only_on_table = rospy.get_param("~yaw_only_on_table", False)
self.ground_bb_axis = rospy.get_param("~ground_bb_axis", SolidPrimitive.BOX_Z)
if self.ground_objects_on_table:
rospy.loginfo("Objects on table will be grounded.")
self.api = ArtApiHelper()
self.api.wait_for_db_api()
self.meas_max_age = rospy.Duration(5.0)
self.prune_timer = rospy.Timer(rospy.Duration(1.0), self.prune_timer_cb)
self.objects = {}
self.br = tf.TransformBroadcaster()
self.sub = rospy.Subscriber(
"/art/object_detector/object", InstancesArray, self.cb, queue_size=1)
self.pub = rospy.Publisher(
"/art/object_detector/object_filtered", InstancesArray, queue_size=1, latch=True)
self.timer = rospy.Timer(rospy.Duration(0.1), self.timer_cb)
self.srv_set_flag = rospy.Service('/art/object_detector/flag/set', ObjectFlagSet, self.srv_set_flag_cb)
self.srv_clear_flag = rospy.Service('/art/object_detector/flag/clear', ObjectFlagClear, self.srv_clear_flag_cb)
self.srv_clear_all_flags = rospy.Service('/art/object_detector/flag/clear_all',
Empty, self.srv_clear_all_flags_cb)
self.forearm_cams = ("/l_forearm_cam_optical_frame", "/r_forearm_cam_optical_frame")
self.srv_enable_forearm = rospy.Service('/art/object_detector/forearm/enable',
Empty, self.srv_enable_forearm_cb)
self.srv_disable_forearm = rospy.Service('/art/object_detector/forearm/disable',
Empty, self.srv_disable_forearm_cb)
self.srv_enable_detection = rospy.Service('/art/object_detector/all/enable', Empty,
self.srv_enable_detection_cb)
self.srv_disable_detection = rospy.Service('/art/object_detector/all/disable', Empty,
self.srv_disable_detection_cb)
def __init__(self, setup, world_frame):
assert setup != ""
self.ready = False
self.setup = setup
self.world_frame = world_frame
self.tf_listener = TransformListener()
self.api = ArtApiHelper()
rospy.loginfo("Waiting for DB API")
self.api.wait_for_db_api()
self.ignored_prefixes = array_from_param("~ignored_prefixes")
rospy.loginfo("Will ignore following prefixes: " +
str(self.ignored_prefixes))
self.lock = RLock()
self.ps = PlanningSceneInterface()
self._paused = False
self.oh = ObjectHelper(self.object_cb)
self.artificial_objects = {}
self.collision_objects_pub = rospy.Publisher(self.NS + "artificial",
CollisionObjects,
latch=True,
queue_size=1)
self.pub_artificial()
self.timer = rospy.Timer(rospy.Duration(1.0), self.timer_cb)
self.paused_pub = rospy.Publisher(self.NS + "paused",
Bool,
latch=True,
queue_size=1)
self.paused = False
def main():
rospy.init_node('simple_trolley_init_script', anonymous=True)
art = ArtApiHelper()
art.wait_for_api()
art.store_object_type(obj_type("mala_desticka", 0.0125, 0.06, 0.06))
art.store_object_type(
obj_type("Modry_kontejner", 0.14, 0.195, 0.075, container=True))
# delete all created programs
ph = art.get_program_headers()
if ph:
for h in ph:
art.program_clear_ro(h.id)
art.delete_program(h.id)
prog = Program()
prog.header.id = 1
prog.header.name = "Pick & Place"
pb = ProgramBlock()
pb.id = 1
pb.name = "Pick & Place"
pb.on_success = 1
pb.on_failure = 0
prog.blocks.append(pb)
pb.items.append(polygon_item(1))
pb.items.append(place_item(2, ref_id=[1], on_success=3, on_failure=0))
pb.items.append(item(3, "GetReady", on_success=4, on_failure=0))
pb.items.append(polygon_item(4))
pb.items.append(place_item(5, ref_id=[4], on_success=6, on_failure=0))
pb.items.append(item(6, "GetReady", on_success=1, on_failure=0))
art.store_program(prog)
art.program_set_ro(prog.header.id)
def __init__(self):
self.ih = InstructionsHelper()
states = []
transitions = []
self.instruction_fsm = {}
for instruction in self.ih.known_instructions():
states += self.ih[instruction].brain.fsm.states
transitions += self.ih[instruction].brain.fsm.transitions
self.fsm = ArtBrainMachine(states, transitions)
for instruction in self.ih.known_instructions():
self.instruction_fsm[instruction] = self.ih[instruction].brain.fsm(
self)
for _, fsm in self.instruction_fsm.iteritems():
for state_function in fsm.state_functions:
setattr(self.fsm, state_function, getattr(fsm, state_function))
self.fsm.check_robot_in = self.check_robot_in
self.fsm.check_robot_out = self.check_robot_out
self.fsm.is_everything_calibrated = self.is_everything_calibrated
self.fsm.state_init_ros = self.state_init_ros
self.fsm.state_waiting_for_action = self.state_waiting_for_action
self.fsm.state_program_init = self.state_program_init
self.fsm.state_program_run = self.state_program_run
self.fsm.state_learning_init = self.state_learning_init
self.fsm.state_learning_run = self.state_learning_run
self.fsm.learning_load_block_id = self.learning_load_block_id
self.fsm.state_program_error = self.state_program_error
self.fsm.state_program_paused = self.state_program_paused
self.fsm.state_program_finished = self.state_program_finished
self.fsm.state_program_load_instruction = self.state_program_load_instruction
self.fsm.state_learning_step_done = self.state_learning_step_done
self.fsm.state_learning_step_error = self.state_learning_step_error
self.fsm.state_learning_done = self.state_learning_done
self.fsm.state_update_program_item = self.state_update_program_item
self.fsm.visualize_load_block_id = self.visualize_load_block_id
self.fsm.state_visualize_run = self.state_visualize_run
self.fsm.state_visualize_done = self.state_visualize_done
self.block_id = None
self.user_id = 0
self.objects = InstancesArray()
self.executing_program = False
self.program_paused = False
self.program_pause_request = False
self.learning = False
self.instruction = None # type: ProgramItem
self.holding_object_left = None
self.holding_object_right = None
self.stop_server = False
self.recalibrate = False
self.table_calibrated = False
self.table_calibrating = False
self.cells_calibrated = False
self.system_calibrated = False
self.initialized = False
self.projectors_calibrated = False
self.last_drill_arm_id = None
self.learning_block_id = None
self.learning_item_id = None
self.quit = False
self.user_activity = None
rospy.loginfo('Waiting for other nodes to come up...')
self.program_resume_after_restart = rospy.get_param(
"executing_program", False)
self.learning_resume_after_restart = rospy.get_param(
"learning_program", False)
self.rh = None
while not self.rh and not rospy.is_shutdown():
try:
self.rh = ArtRobotHelper()
except UnknownRobot:
ArtBrain.fatal("Unknown robot")
return
except RobotParametersNotOnParameterServer:
rospy.logerr("Robot parameters not on parameter server yet...")
rospy.sleep(1)
try:
p, m = rospy.get_param("robot_interface").rsplit('.', 1)
except (KeyError, ValueError) as e:
ArtBrain.fatal("Robot interface not set!")
return
try:
mod = importlib.import_module(p)
self.robot = getattr(mod, m)(self.rh)
except (ImportError, AttributeError, TypeError) as e:
ArtBrain.fatal("Failed to import robot interface: " + str(e))
return
if not isinstance(self.robot, ArtBrainRobotInterface):
ArtBrain.fatal("Invalid robot interface.")
return
rospy.loginfo("Robot initialized")
self.user_status_sub = rospy.Subscriber("/art/user/status", UserStatus,
self.user_status_cb)
self.user_activity_sub = rospy.Subscriber("/art/user/activity",
UserActivity,
self.user_activity_cb)
self.table_calibrated_sub = rospy.Subscriber(
"/art/interface/touchtable/calibrated", Bool,
self.table_calibrated_cb)
self.table_calibrating_sub = rospy.Subscriber(
"/art/interface/touchtable/calibrating", Bool,
self.table_calibrating_cb)
self.system_calibrated_sub = rospy.Subscriber(
"/art/system/calibrated", Bool, self.system_calibrated_cb)
self.projectors_calibrated_sub = rospy.Subscriber(
"/art/interface/projected_gui/projectors_calibrated", Bool,
self.projectors_calibrated_cb)
self.srv_program_start = rospy.Service('program/start',
ProgramIdTrigger,
self.program_start_cb)
self.srv_program_stop = rospy.Service('program/stop', Trigger,
self.program_stop_cb)
self.srv_program_pause = rospy.Service('program/pause', Trigger,
self.program_pause_cb)
self.srv_program_resume = rospy.Service('program/resume', Trigger,
self.program_resume_cb)
self.srv_learning_start = rospy.Service('learning/start',
ProgramIdTrigger,
self.learning_start_cb)
self.srv_learning_stop = rospy.Service('learning/stop', Trigger,
self.learning_stop_cb)
self.srv_learning_start = rospy.Service('/art/brain/visualize/start',
ProgramIdTrigger,
self.visualize_start_cb)
self.srv_learning_stop = rospy.Service('/art/brain/visualize/stop',
Trigger, self.visualize_stop_cb)
self.srv_program_error_response = rospy.Service(
'program/error_response', ProgramErrorResolve,
self.program_error_response_cb)
self.as_learning_request = actionlib.SimpleActionServer(
"learning_request",
LearningRequestAction,
execute_cb=self.learning_request_cb,
auto_start=True)
self.state_manager = InterfaceStateManager(
InterfaceState.BRAIN_ID,
cb=self.interface_state_manager_cb) # TODO callback?
self.state_manager.set_system_state(InterfaceState.STATE_INITIALIZING)
self.art = ArtApiHelper(brain=True)
self.ph = ProgramHelper()
self.objects_sub = rospy.Subscriber(
"/art/object_detector/object_filtered", InstancesArray,
self.objects_cb)
# TODO use this topic instead of system_state in InterfaceState (duplication) ??
# TODO move (pub/sub) to InterfaceStateManager?
self.state_publisher = rospy.Publisher("system_state",
SystemState,
queue_size=1)
self.tf_listener = TransformListener()
self.art.wait_for_api()
self.get_obj_type_srv_client = ArtBrainUtils.create_service_client(
'/art/db/object_type/get', getObjectType)
# self.select_arm_srv_client = ArtBrainUtils.create_service_client(
# '/art/fuzzy/select_arm', SelectArm)
self.clear_all_object_flags_srv_client = ArtBrainUtils.create_service_client(
'/art/object_detector/flag/clear_all', Empty)
self.clear_object_flag_srv_client = ArtBrainUtils.create_service_client(
'/art/object_detector/flag/clear', ObjectFlagClear)
self.set_object_flag_srv_client = ArtBrainUtils.create_service_client(
'/art/object_detector/flag/set', ObjectFlagSet)
# TODO 'hack' for experiment
self.forearm_enable_srv_client = ArtBrainUtils.create_service_client(
'/art/object_detector/forearm/enable', Empty)
self.forearm_disable_srv_client = ArtBrainUtils.create_service_client(
'/art/object_detector/forearm/disable', Empty)
self.forearm_disable_srv_client.call()
r = rospy.Rate(1)
if not self.system_calibrated or not self.projectors_calibrated:
self.robot.prepare_for_calibration()
while not self.system_calibrated:
if rospy.is_shutdown():
return
rospy.loginfo("Waiting for system calibration")
r.sleep()
self.calibrate_projectors_srv_client = ArtBrainUtils.create_service_client(
'/art/interface/projected_gui/calibrate_projectors', Trigger)
if not self.projectors_calibrated:
resp = self.calibrate_projectors_srv_client.call()
if not resp.success:
rospy.logerr("Failed to start projector calibration: " +
resp.message)
# TODO what to do?
rospy.loginfo("Waiting for projectors to calibrate...")
while not self.projectors_calibrated:
if rospy.is_shutdown():
return
rospy.sleep(1)
if not self.table_calibrated:
rospy.loginfo(
'Waiting for /art/interface/touchtable/calibrate service')
rospy.wait_for_service('/art/interface/touchtable/calibrate')
rospy.loginfo('Get /art/interface/touchtable/calibrate service')
self.calibrate_table_srv_client = ArtBrainUtils.create_service_client(
'/art/interface/touchtable/calibrate', Empty)
attempt = 1
rospy.sleep(1)
while not self.table_calibrated:
rospy.loginfo("Trying to calibrate table, attempt " +
str(attempt))
self.calibrate_table_srv_client.call()
rospy.sleep(1)
attempt += 1
while not self.table_calibrated and self.table_calibrating:
rospy.sleep(1)
r = rospy.Rate(1)
while self.robot.halted and not rospy.is_shutdown():
rospy.logwarn("Robot halted! Please unhalt!")
r.sleep()
self.try_robot_arms_get_ready()
rospy.loginfo("Brain init done")
self.initialized = True
self.fsm.init()
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 main(args):
global setup
rospy.init_node('setup_init_script', anonymous=True)
try:
setup = os.environ["ARTABLE_SETUP"]
except KeyError:
rospy.logerr("ARTABLE_SETUP has to be set!")
return
api = ArtApiHelper()
api.wait_for_db_api()
rospy.loginfo("Refreshing collision environment...")
api.clear_collision_primitives(setup)
api.store_object_type(obj_type("desticka", 0.08, 0.08, 0.0125))
api.store_object_type(
obj_type("Modry_kontejner", 0.11, 0.165, 0.075, container=True))
# delete all created programs
ph = api.get_program_headers()
if ph:
for h in ph:
api.program_clear_ro(h.id)
api.delete_program(h.id)
# TODO add parameters
prog = Program()
prog.header.id = 1
prog.header.name = "MSV DEMO"
pb = ProgramBlock()
pb.id = 1
pb.name = "Pick and place"
pb.on_success = 1
pb.on_failure = 0
prog.blocks.append(pb)
pi = ProgramItem()
pi.id = 1
pi.on_success = 2
pi.on_failure = 0
pi.type = "PickFromPolygon"
pi.object.append("")
pi.polygon.append(PolygonStamped())
pb.items.append(pi)
pi = ProgramItem()
pi.id = 2
pi.on_success = 1
pi.on_failure = 0
pi.type = "PlaceToContainer"
pi.object.append("")
pi.polygon.append(PolygonStamped())
pi.ref_id.append(1)
pb.items.append(pi)
api.store_program(prog)
rospy.loginfo("Done!")
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 main(args):
rospy.init_node('simple_trolley_init_script', anonymous=True)
art = ArtApiHelper()
# art.wait_for_api()
# delete all created programs
ph = art.get_program_headers()
if ph:
for h in ph:
art.program_clear_ro(h.id)
art.delete_program(h.id)
# -------------------------------------------------------------------------------------------
# Training program 1
# -------------------------------------------------------------------------------------------
prog = Program()
prog.header.id = 1
prog.header.name = "Trenink - oblast"
pb = ProgramBlock()
pb.id = 1
pb.name = "Zvedni ze stolu a poloz"
pb.on_success = 1
pb.on_failure = 0
prog.blocks.append(pb)
pb.items.append(polygon_item(1))
pb.items.append(place_item(2, ref_id=[1], on_success=3, on_failure=0))
pb.items.append(item(3, "GetReady", on_success=4, on_failure=0))
pb.items.append(wait_item(4, ref_id=[2], on_success=1, on_failure=0))
art.store_program(prog)
art.program_set_ro(prog.header.id)
# -------------------------------------------------------------------------------------------
# Training program 2
# -------------------------------------------------------------------------------------------
prog = Program()
prog.header.id = 2
prog.header.name = "Trenink - podavac"
pb = ProgramBlock()
pb.id = 1
pb.name = "Zvedni z podavace a poloz"
pb.on_success = 1
pb.on_failure = 0
prog.blocks.append(pb)
pb.items.append(feeder_item(1))
pb.items.append(place_item(2, ref_id=[1], on_success=3, on_failure=0))
pb.items.append(feeder_item(3, ref_id=[1]))
pb.items.append(place_item(4, ref_id=[3], on_success=5, on_failure=0))
pb.items.append(item(5, "GetReady", on_success=6, on_failure=0))
pb.items.append(wait_item(6, ref_id=[4], on_success=1, on_failure=0))
art.store_program(prog)
art.program_set_ro(prog.header.id)
# -------------------------------------------------------------------------------------------
# Training program 3
# -------------------------------------------------------------------------------------------
prog = Program()
prog.header.id = 3
prog.header.name = "Trenink - lepidlo"
pb = ProgramBlock()
pb.id = 1
pb.name = "Aplikace lepidla"
pb.on_success = 0
pb.on_failure = 0
prog.blocks.append(pb)
pb.items.append(drill_item(1, on_success=1, on_failure=2, obj_type=[""]))
pb.items.append(item(2, "GetReady", on_success=3, on_failure=0))
pb.items.append(drill_item(3, on_success=3, on_failure=4, obj_type=[""]))
pb.items.append(item(4, "GetReady", on_success=0, on_failure=0))
art.store_program(prog)
art.program_set_ro(prog.header.id)
# -------------------------------------------------------------------------------------------
# Training program 4
# -------------------------------------------------------------------------------------------
prog = Program()
prog.header.id = 4
prog.header.name = "Trenink - inspekce"
pb = ProgramBlock()
pb.id = 1
pb.name = "Vizualni inspekce"
pb.on_success = 0
pb.on_failure = 0
prog.blocks.append(pb)
pb.items.append(polygon_item(1))
pb.items.append(
visual_inspection_item(2, ref_id=[1], on_success=3, on_failure=5))
pb.items.append(
visual_inspection_item(3, ref_id=[1], on_success=4, on_failure=5))
pb.items.append(
place_item(4, ref_id=[1], on_success=1, on_failure=0, name="OK parts"))
pb.items.append(
place_item(5, ref_id=[1], on_success=1, on_failure=0,
name="NOK parts"))
art.store_program(prog)
art.program_set_ro(prog.header.id)
# -------------------------------------------------------------------------------------------
# Training program 5
# -------------------------------------------------------------------------------------------
prog = Program()
prog.header.id = 5
prog.header.name = "Trenink - krabice"
pb = ProgramBlock()
pb.id = 1
pb.name = "Polozeni do krabice"
pb.on_success = 0
pb.on_failure = 0
prog.blocks.append(pb)
pb.items.append(polygon_item(1))
p = ProgramItem()
p.id = 2
p.type = "PlaceToContainer"
p.polygon.append(PolygonStamped())
p.object.append("")
p.on_success = 1
p.on_failure = 0
p.ref_id.append(1)
pb.items.append(p)
art.store_program(prog)
art.program_set_ro(prog.header.id)
# -------------------------------------------------------------------------------------------
# Simplified trolley assembly: object types
# -------------------------------------------------------------------------------------------
art.store_object_type(obj_type("Spojka", 0.046, 0.046, 0.154))
art.store_object_type(
obj_type("Kratka_noha", 0.046, 0.046, 0.298, container=True))
art.store_object_type(obj_type("Dlouha_noha", 0.046, 0.046, 0.398))
art.store_object_type(
obj_type("Modry_kontejner", 0.1, 0.14, 0.08, container=True))
# -------------------------------------------------------------------------------------------
# Simplified trolley assembly: program
# -------------------------------------------------------------------------------------------
prog = Program()
prog.header.id = 20
prog.header.name = "Montaz stolicky"
# --- left side of the trolley ------------------------------------------------------
pb = ProgramBlock()
pb.id = 1
pb.name = "Bocnice 1"
pb.on_success = 2
pb.on_failure = 0
prog.blocks.append(pb)
pb.items.append(wait_item(2, on_failure=2))
# each side consists of four profiles (of two types)
pb.items.append(feeder_item(3, obj_type=""))
pb.items.append(place_item(4, ref_id=[3], on_failure=4))
pb.items.append(feeder_item(5, ref_id=[3]))
pb.items.append(place_item(6, ref_id=[5], on_failure=6))
pb.items.append(feeder_item(7, obj_type=""))
pb.items.append(place_item(8, ref_id=[7], on_failure=8))
pb.items.append(feeder_item(9, ref_id=[7]))
pb.items.append(place_item(10, ref_id=[9], on_failure=10))
# after p&p, let's drill holes
pb.items.append(drill_item(11, on_success=11, on_failure=13,
obj_type=[""]))
pb.items.append(item(13, "GetReady", on_success=0, on_failure=13))
# --- right side of the trolley ------------------------------------------------------
pb = deepcopy(pb)
pb.id = 2
pb.name = "Bocnice 2"
pb.on_success = 3
pb.on_failure = 0
prog.blocks.append(pb)
# --- connecting profiles ------------------------------------------------------------
pb = ProgramBlock()
pb.id = 3
pb.name = "Spojovaci dily"
pb.on_success = 1
pb.on_failure = 0
prog.blocks.append(pb)
pb.items.append(wait_item(1, on_success=10, on_failure=1))
pb.items.append(feeder_item(10, obj_type=""))
pb.items.append(place_item(11, ref_id=[10], on_failure=11))
pb.items.append(feeder_item(12, ref_id=[10]))
pb.items.append(place_item(13, ref_id=[12], on_failure=13))
pb.items.append(feeder_item(14, ref_id=[10]))
pb.items.append(place_item(15, ref_id=[14], on_failure=15))
pb.items.append(feeder_item(16, ref_id=[10]))
pb.items.append(place_item(17, ref_id=[16], on_failure=17))
pb.items.append(item(18, "GetReady", on_success=0, on_failure=18))
art.store_program(prog)
art.program_set_ro(prog.header.id)
def main(args):
rospy.init_node('simple_trolley_init_script', anonymous=True)
art = ArtApiHelper()
# art.wait_for_api()
# delete all created programs
ph = art.get_program_headers()
if ph:
for h in ph:
art.program_clear_ro(h.id)
art.delete_program(h.id)
# -------------------------------------------------------------------------------------------
# Training program 1
# -------------------------------------------------------------------------------------------
prog = Program()
prog.header.id = 1
prog.header.name = "Training - polygon"
pb = ProgramBlock()
pb.id = 1
pb.name = "Pick from table and place"
pb.on_success = 1
pb.on_failure = 0
prog.blocks.append(pb)
pb.items.append(polygon_item(1))
pb.items.append(place_item(2, ref_id=[1], on_success=3, on_failure=0))
pb.items.append(item(3, "GetReady", on_success=4, on_failure=0))
pb.items.append(wait_item(4, ref_id=[2], on_success=1, on_failure=0))
art.store_program(prog)
art.program_set_ro(prog.header.id)
# -------------------------------------------------------------------------------------------
# Training program 2
# -------------------------------------------------------------------------------------------
prog = Program()
prog.header.id = 2
prog.header.name = "Training - feeder"
pb = ProgramBlock()
pb.id = 1
pb.name = "Pick from feeder and place"
pb.on_success = 1
pb.on_failure = 0
prog.blocks.append(pb)
pb.items.append(feeder_item(1))
pb.items.append(place_item(2, ref_id=[1], on_success=3, on_failure=0))
pb.items.append(feeder_item(3, ref_id=[1]))
pb.items.append(place_item(4, ref_id=[3], on_success=5, on_failure=0))
pb.items.append(item(5, "GetReady", on_success=6, on_failure=0))
pb.items.append(wait_item(6, ref_id=[4], on_success=1, on_failure=0))
art.store_program(prog)
art.program_set_ro(prog.header.id)
# -------------------------------------------------------------------------------------------
# Training program 3
# -------------------------------------------------------------------------------------------
prog = Program()
prog.header.id = 3
prog.header.name = "Training - glue"
pb = ProgramBlock()
pb.id = 1
pb.name = "Glue application"
pb.on_success = 0
pb.on_failure = 0
prog.blocks.append(pb)
pb.items.append(drill_item(1, on_success=1, on_failure=2, obj_type=[""]))
pb.items.append(item(2, "GetReady", on_success=3, on_failure=0))
pb.items.append(drill_item(3, on_success=3, on_failure=4, obj_type=[""]))
pb.items.append(item(4, "GetReady", on_success=0, on_failure=0))
art.store_program(prog)
art.program_set_ro(prog.header.id)
# -------------------------------------------------------------------------------------------
# Simplified trolley assembly: object types
# -------------------------------------------------------------------------------------------
art.store_object_type(obj_type("Stretcher", 0.046, 0.046, 0.154))
art.store_object_type(obj_type("ShortLeg", 0.046, 0.046, 0.298))
art.store_object_type(obj_type("LongLeg", 0.046, 0.046, 0.398))
# -------------------------------------------------------------------------------------------
# Simplified trolley assembly: program
# -------------------------------------------------------------------------------------------
prog = Program()
prog.header.id = 4
prog.header.name = "Stool assembly"
# --- left side of the trolley ------------------------------------------------------
pb = ProgramBlock()
pb.id = 1
pb.name = "Side 1"
pb.on_success = 2
pb.on_failure = 0
prog.blocks.append(pb)
pb.items.append(wait_item(2, on_failure=2))
# each side consists of four profiles (of two types)
pb.items.append(feeder_item(3, obj_type=""))
pb.items.append(place_item(4, ref_id=[3], on_failure=4))
pb.items.append(feeder_item(5, ref_id=[3]))
pb.items.append(place_item(6, ref_id=[5], on_failure=6))
pb.items.append(feeder_item(7, obj_type=""))
pb.items.append(place_item(8, ref_id=[7], on_failure=8))
pb.items.append(feeder_item(9, ref_id=[7]))
pb.items.append(place_item(10, ref_id=[9], on_failure=10))
# after p&p, let's drill holes
pb.items.append(drill_item(11, on_success=11, on_failure=13,
obj_type=[""]))
pb.items.append(item(13, "GetReady", on_success=0, on_failure=13))
# --- right side of the trolley ------------------------------------------------------
pb = deepcopy(pb)
pb.id = 2
pb.name = "Side 2"
pb.on_success = 3
pb.on_failure = 0
prog.blocks.append(pb)
# --- connecting profiles ------------------------------------------------------------
pb = ProgramBlock()
pb.id = 3
pb.name = "Stretchers"
pb.on_success = 1
pb.on_failure = 0
prog.blocks.append(pb)
pb.items.append(wait_item(1, on_success=10, on_failure=1))
pb.items.append(feeder_item(10, obj_type=""))
pb.items.append(place_item(11, ref_id=[10], on_failure=11))
pb.items.append(feeder_item(12, ref_id=[10]))
pb.items.append(place_item(13, ref_id=[12], on_failure=13))
pb.items.append(feeder_item(14, ref_id=[10]))
pb.items.append(place_item(15, ref_id=[14], on_failure=15))
pb.items.append(feeder_item(16, ref_id=[10]))
pb.items.append(place_item(17, ref_id=[16], on_failure=17))
pb.items.append(item(18, "GetReady", on_success=0, on_failure=18))
art.store_program(prog)
art.program_set_ro(prog.header.id)
def main(args):
global setup
rospy.init_node('setup_init_script', anonymous=True)
try:
setup = os.environ["ARTABLE_SETUP"]
except KeyError:
rospy.logerr("ARTABLE_SETUP has to be set!")
return
api = ArtApiHelper()
api.wait_for_db_api()
rospy.loginfo("Refreshing collision environment...")
api.clear_collision_primitives(setup)
table_width = 1.5
table_depth = 0.7
api.add_collision_primitive(
makePrimitive("table", [table_width, table_depth, 0.78], z=-0.78 / 2))
feeder_depth = 0.35
feeder_thickness = 0.001
feeder_front_to_table = 0.15
# left feeder (1)
api.add_collision_primitive(
makePrimitive("lf-front", [feeder_depth, feeder_thickness, 0.175],
x=-feeder_depth / 2 - feeder_front_to_table,
y=table_depth - 0.495))
api.add_collision_primitive(
makePrimitive("lf-middle", [feeder_depth, feeder_thickness, 0.35],
x=-feeder_depth / 2 - feeder_front_to_table,
y=table_depth - 0.18))
api.add_collision_primitive(
makePrimitive("lf-rear", [feeder_depth, feeder_thickness, 0.35],
x=-feeder_depth / 2 - feeder_front_to_table,
y=table_depth))
# right feeder (2)
api.add_collision_primitive(
makePrimitive("rf-front", [feeder_depth, feeder_thickness, 0.175],
x=table_width + feeder_depth / 2 + feeder_front_to_table,
y=table_depth - 0.495))
api.add_collision_primitive(
makePrimitive("rf-middle", [feeder_depth, feeder_thickness, 0.35],
x=table_width + feeder_depth / 2 + feeder_front_to_table,
y=table_depth - 0.18))
api.add_collision_primitive(
makePrimitive("rf-rear", [feeder_depth, feeder_thickness, 0.35],
x=table_width + feeder_depth / 2 + feeder_front_to_table,
y=table_depth))
api.add_collision_primitive(
makePrimitive("kinect-n1", [0.3, 0.3, 0.3],
x=0,
y=0,
z=0,
frame_id="n1_kinect2_link"))
api.add_collision_primitive(
makePrimitive("kinect-n2", [0.3, 0.3, 0.3],
x=0,
y=0,
z=0,
frame_id="n2_kinect2_link"))
api.store_object_type(obj_type("Stretcher", 0.046, 0.046, 0.154))
api.store_object_type(obj_type("ShortLeg", 0.046, 0.046, 0.298))
api.store_object_type(obj_type("LongLeg", 0.046, 0.046, 0.398))
api.store_object_type(obj_type("Spojka", 0.046, 0.046, 0.154))
api.store_object_type(obj_type("Kratka_noha", 0.046, 0.046, 0.298))
api.store_object_type(obj_type("Dlouha_noha", 0.046, 0.046, 0.398))
api.store_object_type(
obj_type("Modry_kontejner", 0.11, 0.165, 0.075, container=True))
api.store_object_type(
obj_type("Bily_kontejner_velky", 0.245, 0.37, 0.15, container=True))
api.store_object_type(
obj_type("Bily_kontejner_maly", 0.18, 0.245, 0.15, container=True))
# delete all created programs
ph = api.get_program_headers()
if ph:
for h in ph:
api.program_clear_ro(h.id)
api.delete_program(h.id)
# TODO add parameters
prog = Program()
prog.header.id = 1
prog.header.name = "MSV DEMO"
pb = ProgramBlock()
pb.id = 1
pb.name = "Pick and place"
pb.on_success = 2
pb.on_failure = 0
prog.blocks.append(pb)
pi = ProgramItem()
pi.id = 1
pi.on_success = 2
pi.on_failure = 0
pi.type = "PickFromFeeder"
pi.object.append("")
pi.pose.append(PoseStamped())
pb.items.append(pi)
pi = ProgramItem()
pi.id = 2
pi.on_success = 3
pi.on_failure = 0
pi.type = "PlaceToPose"
pi.pose.append(PoseStamped())
pi.ref_id.append(1)
pb.items.append(pi)
pi = ProgramItem()
pi.id = 3
pi.on_success = 0
pi.on_failure = 0
pi.type = "GetReady"
pb.items.append(pi)
pb = ProgramBlock()
pb.id = 2
pb.name = "Inspect and sort"
pb.on_success = 1
pb.on_failure = 0
prog.blocks.append(pb)
pi = ProgramItem()
pi.id = 1
pi.on_success = 2
pi.on_failure = 0
pi.type = "PickFromPolygon"
pi.object.append("")
pi.polygon.append(PolygonStamped())
pb.items.append(pi)
pi = ProgramItem()
pi.id = 2
pi.on_success = 3
pi.on_failure = 4
pi.type = "VisualInspection"
pi.pose.append(PoseStamped())
pi.ref_id.append(1)
pb.items.append(pi)
pi = ProgramItem()
pi.id = 3
pi.on_success = 5
pi.on_failure = 0
pi.type = "PlaceToContainer"
pi.name = "OK"
pi.object.append("")
pi.polygon.append(PolygonStamped())
pi.ref_id.append(1)
pb.items.append(pi)
pi = ProgramItem()
pi.id = 4
pi.on_success = 5
pi.on_failure = 0
pi.type = "PlaceToContainer"
pi.name = "NOK"
pi.object.append("")
pi.polygon.append(PolygonStamped())
pi.ref_id.append(1)
pb.items.append(pi)
pi = ProgramItem()
pi.id = 5
pi.on_success = 0
pi.on_failure = 0
pi.type = "GetReady"
pb.items.append(pi)
api.store_program(prog)
rospy.loginfo("Done!")
def __init__(self):
self.art = ArtApiHelper()
