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)
class ArtBrain(object):
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()
@staticmethod
def fatal(msg):
rospy.logfatal(msg)
rospy.signal_shutdown(msg)
# ***************************************************************************************
# STATES
# ***************************************************************************************
def state_init_ros(self, event):
rospy.logdebug('Current state: state_init_ros')
self.fsm.init_done()
def state_waiting_for_action(self, event):
rospy.logdebug('Current state: state_waiting_for_action')
if self.program_resume_after_restart or self.learning_resume_after_restart:
program_id = rospy.get_param("program_id", None)
self.block_id = rospy.get_param("block_id", None)
item_id = rospy.get_param("item_id", None)
if self.block_id is None or item_id is None or program_id is None:
rospy.logwarn("Could not resume program!")
self.learning_resume_after_restart = False
self.program_resume_after_restart = False
return
program = self.art.load_program(program_id)
if not self.ph.load(program):
rospy.logwarn("Could not resume program!")
rospy.delete_param('program_id')
rospy.delete_param('block_id')
rospy.delete_param('item_id')
self.state_manager.set_system_state(InterfaceState.STATE_IDLE)
return
if self.program_resume_after_restart:
rospy.logdebug('Starting program')
rospy.Timer(rospy.Duration(1),
self.program_start_timer_cb,
oneshot=True)
rospy.logdebug("program started")
elif self.learning_resume_after_restart:
self.learning_resume_after_restart = False
rospy.logdebug('Starting learning')
self.state_manager.update_program_item(program_id,
self.block_id,
self.ph.get_item_msg(
self.block_id,
item_id),
auto_send=True)
self.state_manager.set_system_state(
InterfaceState.STATE_LEARNING)
self.fsm.learning_start()
else:
self.state_manager.set_system_state(InterfaceState.STATE_IDLE)
def state_shutdown(self, event):
rospy.logdebug('Current state: state_shutdown')
sys.exit()
# ***************************************************************************************
# STATES PROGRAM
# ***************************************************************************************
def state_program_init(self, event):
rospy.logdebug('Current state: state_program_init')
rospy.logdebug('New program ready!')
rospy.set_param("executing_program", True)
if self.ph is None:
self.fsm.error(severity=ArtBrainErrorSeverities.SEVERE,
error=ArtBrainErrors.ERROR_NO_PROGRAM_HELPER)
return
item_id = None
if self.program_resume_after_restart:
item_id = rospy.get_param("item_id", None)
else:
self.clear_all_object_flags_srv_client.call(EmptyRequest())
(self.block_id, item_id) = self.ph.get_first_item_id()
self.instruction = self.ph.get_item_msg(self.block_id, item_id)
rospy.set_param("program_id", self.ph.get_program_id())
rospy.set_param("block_id", self.block_id)
rospy.set_param("item_id", item_id)
self.executing_program = True
self.program_paused = False
self.program_pause_request = False
if self.program_resume_after_restart:
self.robot.init_arms(reset_holding_object=False)
else:
self.robot.init_arms(reset_holding_object=True)
self.state_manager.set_system_state(
InterfaceState.STATE_PROGRAM_RUNNING, auto_send=False)
self.fsm.program_init_done()
def state_program_run(self, event):
rospy.logdebug('Current state: state_program_run')
if not self.executing_program:
self.fsm.finished()
return
if self.instruction is None:
self.fsm.error(severity=ArtBrainErrorSeverities.SEVERE,
error=ArtBrainErrors.ERROR_NO_INSTRUCTION)
return
if not self.check_robot():
return
rospy.logdebug('Program id: ' + str(self.ph.get_program_id()) +
', item id: ' + str(self.instruction.id) +
', item type: ' + str(self.instruction.type))
for flag in self.instruction.flags:
if flag.key == "CLEAR_OBJECT_FLAGS" and flag.value == "true":
self.clear_all_object_flags_srv_client.call(EmptyRequest())
'''while self.program_resume_after_restart and self.instruction.type in [ProgramItem.PLACE_TO_POSE,
ProgramItem.PLACE_TO_GRID]:
(self.block_id, item_id) = self.ph.get_id_on_success(
self.block_id, self.instruction.id)
if self.block_id == 0:
self.fsm.finished()
return
self.instruction = self.ph.get_item_msg(self.block_id, item_id)
'''
self.program_resume_after_restart = False
rospy.set_param("program_id", self.ph.get_program_id())
rospy.set_param("block_id", self.block_id)
rospy.set_param("item_id", self.instruction.id)
try:
self.instruction_fsm[self.instruction.type].run()
except InstructionsHelperException:
self.fsm.error()
return
def state_program_load_instruction(self, event):
rospy.logdebug('Current state: state_program_load_instruction')
success = event.kwargs.get('success', True)
self.state_manager.set_error(0, 0, auto_send=False)
if not self.executing_program:
self.fsm.finished()
return
if success:
self.block_id, item_id = self.ph.get_id_on_success(
self.block_id, self.instruction.id)
else:
self.block_id, item_id = self.ph.get_id_on_failure(
self.block_id, self.instruction.id)
if self.block_id == 0:
self.fsm.finished()
return
self.instruction = self.ph.get_item_msg(self.block_id, item_id)
if self.instruction is None:
self.executing_program = False
self.fsm.error()
return
if self.program_pause_request:
self.fsm.pause()
return
self.fsm.done()
def state_update_program_item(self, event):
rospy.logerr("state_update_program_item")
self.state_manager.update_program_item(self.ph.get_program_id(),
self.block_id,
self.instruction,
auto_send=False)
def state_program_paused(self, event):
rospy.logdebug('Current state: state_program_paused')
self.state_manager.set_system_state(
InterfaceState.STATE_PROGRAM_STOPPED)
self.state_manager.send()
self.program_paused = True
self.program_pause_request = False
def state_program_finished(self, event):
rospy.logdebug('Current state: state_program_finished')
self.state_manager.set_system_state(
InterfaceState.STATE_PROGRAM_FINISHED)
self.robot.arms_reinit()
self.state_manager.send()
self.executing_program = False
self.robot.arms_reinit()
rospy.set_param("executing_program", False)
rospy.delete_param("program_id")
rospy.delete_param("block_id")
rospy.delete_param("item_id")
self.program_resume_after_restart = False
self.fsm.done()
def state_program_error(self, event):
rospy.logdebug('Current state: state_program_error')
severity = event.kwargs.get('severity', ArtBrainErrorSeverities.SEVERE)
error = event.kwargs.get('error', None)
rospy.logerr("Error: " + str(error))
rospy.logerr("Severity of error: " + str(repr(severity)))
if severity is None or error is None:
severity = ArtBrainErrorSeverities.SEVERE
error = ArtBrainErrors.ERROR_UNKNOWN
self.state_manager.set_error(severity, error)
if severity == ArtBrainErrorSeverities.SEVERE:
# handle
self.fsm.program_error_shutdown()
return
elif severity == ArtBrainErrorSeverities.ERROR:
self.fsm.program_error_fatal()
return
elif severity == ArtBrainErrorSeverities.WARNING:
if error == ArtBrainErrors.ERROR_OBJECT_MISSING or \
error == ArtBrainErrors.ERROR_OBJECT_MISSING_IN_POLYGON:
rospy.logwarn("Object is missing")
elif error == ArtBrainErrors.ERROR_PICK_FAILED:
rospy.logwarn("Pick failed")
self.try_robot_arms_get_ready()
self.robot.init_arms()
rospy.logwarn("Waiting for user response")
return
elif severity == ArtBrainErrorSeverities.INFO:
(self.block_id,
item_id) = self.ph.get_id_on_failure(self.block_id,
self.instruction.id)
self.fsm.done(success=False)
else:
pass
self.executing_program = False
self.program_error_handled()
# ***************************************************************************************
# STATES TEACHING
# ***************************************************************************************
def state_learning_init(self, event):
rospy.logdebug('Current state: Teaching init')
self.learning = True
rospy.set_param("learning_program", True)
self.fsm.init_done()
def learning_load_block_id(self, event):
self.block_id = self.state_manager.state.block_id
def state_learning_run(self, event):
rospy.logdebug('Current state: state_learning_run')
def state_learning_step_error(self, event):
rospy.logdebug('Current state: state_learning_step_error')
severity = event.kwargs.get('severity', ArtBrainErrorSeverities.SEVERE)
error = event.kwargs.get('error', ArtBrainErrors.ERROR_UNKNOWN)
rospy.logdebug("Severity of error: " + str(severity))
rospy.logdebug("Severity of error: " + str(error))
self.state_manager.set_error(severity, error)
self.state_manager.set_error(0, 0)
if error is None:
pass
# TODO: kill brain
if severity == ArtBrainErrorSeverities.SEVERE:
pass
# TODO: stop learning
elif severity == ArtBrainErrorSeverities.ERROR:
pass
elif severity == ArtBrainErrorSeverities.WARNING:
if error == ArtBrainErrors.ERROR_OBJECT_MISSING or \
error == ArtBrainErrors.ERROR_OBJECT_MISSING_IN_POLYGON:
rospy.logwarn("Object is missing")
elif error == ArtBrainErrors.ERROR_PICK_FAILED:
rospy.logwarn("Pick failed")
self.try_robot_arms_get_ready()
self.robot.init_arms()
elif severity == ArtBrainErrorSeverities.INFO:
pass
self.fsm.error_handled()
def state_learning_step_done(self, event):
rospy.logdebug('Current state: state_learning_step_done')
self.fsm.done()
pass
def state_learning_done(self, event):
rospy.logdebug('Current state: state_learning_done')
self.fsm.done()
pass
# ***************************************************************************************
# STATES VISUALIZING
# ***************************************************************************************
'''def state_visualize_init(self, event):
rospy.logdebug('Current state: Visualize init')
self.visualizing = True
rospy.set_param("visualize_program", True)
#self.fsm.init_done()'''
def visualize_load_block_id(self, event):
self.block_id = self.state_manager.state.block_id
def state_visualize_run(self, event):
rospy.logdebug('Current state: state_visualize_run')
def state_visualize_done(self, event):
rospy.logdebug('Current state: state_visualize_done')
self.fsm.done()
pass
# ***************************************************************************************
# MANIPULATION
# ***************************************************************************************
def try_robot_arms_get_ready(self, arm_ids=[], max_attempts=3):
assert isinstance(arm_ids, list)
if self.robot.halted:
return False
attempt = 0
while True:
if attempt >= max_attempts:
rospy.logerr("Failed to get ready")
return False
severity, error, arm_id = self.robot.arms_get_ready(arm_ids)
if error is not None:
attempt += 1
rospy.logwarn("Error while getting ready: " + str(arm_id) +
" , attempt: " + str(attempt))
continue
else:
rospy.loginfo("Robot ready")
return True
# ***************************************************************************************
# OTHERS
# ***************************************************************************************
def check_robot_in(self, event):
self.check_robot()
def check_robot_out(self, event):
halted = event.kwargs.get('halted', False)
# print halted
if halted:
return
self.check_robot()
def program_start_timer_cb(self, event):
self.fsm.program_start()
def program_resume_timer_cb(self, event):
self.state_manager.set_system_state(
InterfaceState.STATE_PROGRAM_RUNNING)
self.fsm.resume()
def program_try_again_timer_cb(self, event):
self.fsm.try_again()
def program_skip_instruction_timer_cb(self, event):
self.fsm.skip(success=True)
def program_fail_instruction_timer_cb(self, event):
self.fsm.fail(success=False)
def update_state_manager(self):
self.state_manager.update_program_item(self.ph.get_program_id(),
self.block_id, self.instruction)
def is_table_calibrated(self):
return self.table_calibrated
def is_everything_calibrated(self, event=None):
return self.table_calibrated and self.system_calibrated
def check_robot(self):
if self.robot.is_halted():
self.fsm.error(severity=ArtBrainErrorSeverities.WARNING,
error=ArtBrainErrors.ERROR_ROBOT_HALTED)
return False
else:
return True
def check_place_pose(self, place_pose, obj):
w1 = self.get_object_max_width(obj)
if w1 is None:
return False
for o in self.objects.instances:
if o.object_id == obj.object_id:
continue
w2 = self.get_object_max_width(o)
if w2 is None:
# TODO: how to deal with this
return False
d = ArtBrainUtils.distance_2d(place_pose.pose, o.pose)
if d < (w1 + w2):
rospy.logerr('Another object too close to desired place pose')
return False
return True
def check_gripper(self, gripper):
if gripper is None:
rospy.logerr("No gripper!")
self.fsm.error(severity=ArtBrainErrorSeverities.SEVERE,
error=ArtBrainErrors.ERROR_NO_GRIPPER_AVAILABLE)
return False
if gripper.pp_client is None:
rospy.logerr("No pick place client!")
self.fsm.error(
severity=ArtBrainErrorSeverities.SEVERE,
error=ArtBrainErrors.ERROR_GRIPPER_PP_CLIENT_MISSING)
return False
if not gripper.pp_client.wait_for_server(rospy.Duration(2)):
rospy.logerr("Pick place server is not running!")
self.fsm.error(
severity=ArtBrainErrorSeverities.WARNING,
error=ArtBrainErrors.ERROR_PICK_PLACE_SERVER_NOT_READY)
return False
return True
def check_gripper_for_place(self, gripper):
if not self.check_gripper(gripper):
return False
if gripper.holding_object is None:
rospy.logwarn("Place: gripper " + gripper.name +
" is not holding object")
self.fsm.error(severity=ArtBrainErrorSeverities.WARNING,
error=ArtBrainErrors.ERROR_NO_OBJECT_IN_GRIPPER)
return False
return True
def check_gripper_for_pick(self, gripper):
if not self.check_gripper(gripper):
return False
if gripper.holding_object is not None:
rospy.logwarn("Pick: gripper " + gripper.name +
" already holding an object (" +
gripper.holding_object.object_id + ")")
self.fsm.error(severity=ArtBrainErrorSeverities.WARNING,
error=ArtBrainErrors.ERROR_OBJECT_IN_GRIPPER)
return False
return True
# ***************************************************************************************
# ROS COMMUNICATION
# ***************************************************************************************
def program_pause_cb(self, req):
resp = TriggerResponse()
resp.success = True
if not self.executing_program:
resp.success = False
resp.message = "Program si not running!"
else:
self.program_pause_request = True
return resp
def program_resume_cb(self, req):
resp = TriggerResponse()
resp.success = True
if not self.executing_program:
resp.success = False
resp.message = "Program si not running!"
elif not self.program_paused:
resp.success = False
resp.message = "Program is not paused"
else:
self.program_paused = False
rospy.Timer(rospy.Duration(1),
self.program_resume_timer_cb,
oneshot=True)
return resp
def program_start_cb(self, req):
resp = ProgramIdTriggerResponse()
if not self.is_everything_calibrated():
resp.success = False
resp.error = 'Something is not calibrated'
rospy.logwarn('Something is not calibrated')
return resp
if self.executing_program:
resp.success = False
resp.error = 'Program already running'
return resp
if not self.fsm.is_waiting_for_action():
resp.success = False
resp.error = 'Not ready for program start!'
return resp
rospy.logdebug('Loading program ' + str(req.program_id) +
' from db...')
program = self.art.load_program(req.program_id)
if not self.ph.load(program):
resp.success = False
resp.error = 'Cannot get program'
return resp
rospy.logdebug('Starting program')
rospy.Timer(rospy.Duration(1),
self.program_start_timer_cb,
oneshot=True)
rospy.logdebug("program started")
resp.success = True
return resp
def program_stop_cb(self, req):
resp = TriggerResponse()
resp.success = True
if self.executing_program:
rospy.logdebug('Stopping program')
self.executing_program = False
if self.program_paused:
self.program_paused = False
rospy.Timer(rospy.Duration(0, 100),
self.program_resume_timer_cb,
oneshot=True)
else:
resp.success = False
resp.message = "Program is not running"
return resp
def program_error_response_cb(self, req):
resp = ProgramErrorResolveResponse()
if not self.fsm.is_program_error():
resp.success = False
return resp
rospy.logdebug('We\'ve got response to our program error')
if req.user_response_type == ProgramErrorResolveRequest.TRY_AGAIN:
self.state_manager.set_error(0, 0)
rospy.Timer(rospy.Duration(1),
self.program_try_again_timer_cb,
oneshot=True)
elif req.user_response_type == ProgramErrorResolveRequest.SKIP_INSTRUCTION:
rospy.Timer(rospy.Duration(1),
self.program_skip_instruction_timer_cb,
oneshot=True)
elif req.user_response_type == ProgramErrorResolveRequest.FAIL_INSTRUCTION:
rospy.Timer(rospy.Duration(1),
self.program_fail_instruction_timer_cb,
oneshot=True)
elif req.user_response_type == ProgramErrorResolveRequest.END_PROGRAM:
self.fsm.program_error_fatal()
resp.success = True
return resp
def learning_start_cb(self, req):
resp = ProgramIdTriggerResponse()
resp.success = False
if not self.is_everything_calibrated():
resp.error = 'Something is not calibrated'
rospy.logwarn('Something is not calibrated')
return resp
if not self.fsm.is_waiting_for_action():
resp.error = 'Not ready for learning start!'
rospy.logwarn('Not ready for learning start!')
return resp
program = self.art.load_program(req.program_id)
if not self.ph.load(program):
resp.success = False
resp.error = 'Cannot get program.'
return resp
rospy.logdebug('Starting learning')
(self.block_id, item_id) = self.ph.get_first_item_id()
self.state_manager.update_program_item(req.program_id,
self.block_id,
self.ph.get_item_msg(
self.block_id, item_id),
auto_send=False)
self.state_manager.set_system_state(InterfaceState.STATE_LEARNING)
resp.success = True
self.fsm.learning_start()
return resp
def visualize_start_cb(self, req):
resp = ProgramIdTriggerResponse()
resp.success = False
if not self.is_everything_calibrated():
resp.error = 'Something is not calibrated'
rospy.logwarn('Something is not calibrated')
return resp
if not self.fsm.is_waiting_for_action():
resp.error = 'Not ready for visualize start!'
rospy.logwarn('Not ready for visualize start!')
return resp
program = self.art.load_program(req.program_id)
if not self.ph.load(program):
resp.success = False
resp.error = 'Cannot get program.'
return resp
rospy.logdebug('Starting visualize')
(self.block_id, item_id) = self.ph.get_first_item_id()
self.state_manager.update_program_item(req.program_id,
self.block_id,
self.ph.get_item_msg(
self.block_id, item_id),
auto_send=False)
self.state_manager.set_system_state(InterfaceState.STATE_VISUALIZE)
resp.success = True
self.fsm.visualize_start()
return resp
def learning_stop_cb(self, req):
resp = TriggerResponse()
if not self.fsm.is_learning_run:
resp.success = False
rospy.logdebug('Stopping learning')
self.learning = False
rospy.set_param("learning_program", False)
resp.success = True
self.fsm.learning_done()
return resp
def visualize_stop_cb(self, req):
resp = TriggerResponse()
if not self.fsm.is_visualize_run:
resp.success = False
rospy.logdebug('Stopping visualize')
resp.success = True
self.fsm.visualize_done()
return resp
def interface_state_manager_cb(
self,
state, # type: InterfaceState
msg, # type: InterfaceState
flags):
if msg.interface_id != InterfaceState.BRAIN_ID:
if msg.system_state == InterfaceState.STATE_LEARNING:
self.ph.set_item_msg(msg.block_id, msg.program_current_item)
rospy.set_param("program_id", self.ph.get_program_id())
rospy.set_param("block_id", self.block_id)
rospy.set_param("item_id", msg.program_current_item.id)
self.art.store_program(self.ph.get_program())
pass
def projectors_calibrated_cb(self, msg):
self.projectors_calibrated = msg.data
def user_status_cb(self, req):
self.user_id = req.user_id
def user_activity_cb(self, req):
self.user_activity = req.activity
def objects_cb(self, req):
self.objects = req
def table_calibrated_cb(self, req):
self.table_calibrated = req.data
def table_calibrating_cb(self, req):
self.table_calibrating = req.data
def system_calibrated_cb(self, req):
self.system_calibrated = req.data
def get_object_max_width(self, obj):
if obj is None:
rospy.logerr('No object is specified')
return None
try:
obj_type = self.get_obj_type_srv_client.call(obj.object_type)
if not obj_type.success:
rospy.logerr('No object with id ' + str(obj.object_id) +
' found')
return None
if obj_type.object_type.bbox.type != SolidPrimitive.BOX:
rospy.logerr(
'Sorry, only BOX type objects are supported at the moment')
x = obj_type.object_type.bbox.dimensions[SolidPrimitive.BOX_X]
y = obj_type.object_type.bbox.dimensions[SolidPrimitive.BOX_Y]
return np.hypot(x / 2, y / 2)
except rospy.ServiceException as e:
rospy.logerr('Service call failed: ' + str(e))
return None
def learning_request_cb(self, goal):
result = LearningRequestResult()
result.success = True
if not self.fsm.is_learning_run:
rospy.logerr(
"Not in learning mode but got learning request goal: " +
str(goal.request))
result.success = False
result.message = "Not in learning mode!"
self.as_learning_request.set_aborted(result)
return
rospy.logdebug("Learning_request goal: " + str(goal.request))
instruction = self.state_manager.state.program_current_item # type: ProgramItem
self.instruction = instruction
self.state_manager.state.edit_enabled = False
self.state_manager.send()
if goal.request == LearningRequestGoal.GET_READY:
self.state_manager.state.edit_enabled = True
self.instruction_fsm[
instruction.type].learning() # TODO check and handle errors
self.state_manager.state.edit_enabled = True
self.state_manager.send()
self.as_learning_request.set_succeeded(result)
elif goal.request == LearningRequestGoal.EXECUTE_ITEM:
self.ph.set_item_msg(self.state_manager.state.block_id,
instruction)
self.instruction_fsm[instruction.type].learning_run(
) # TODO check and handle errors
self.as_learning_request.set_succeeded(result)
elif goal.request == LearningRequestGoal.DONE:
self.fsm.done() # TODO check and handle errors?
self.as_learning_request.set_succeeded(result)
else:
result.success = False
result.message = "Unknown request"
self.as_learning_request.set_aborted(result)
class UICoreRos(UICore):
"""The class builds on top of UICore and adds ROS-related stuff and application logic.
Attributes:
user_status (UserStatus): current user tracking status
fsm (FSM): state machine maintaining current state of the interface and proper transitions between states
state_manager (interface_state_manager): synchronization of interfaces within the ARTable system
scene_pub (rospy.Publisher): publisher for scene images
last_scene_update (rospy.Time): time when the last scene image was published
scene_img_deq (Queue.Queue): thread-safe queue for scene images (which are published in separate thread)
projectors (list): array of ProjectorHelper instances
art (ArtApiHelper): easy access to ARTable services
"""
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 touch_calibration_points_evt(self, pts):
for it in self.scene.items():
if isinstance(it, LabelItem):
continue
it.setVisible(False) # TODO remember settings (how?)
self.notif(translate(
"UICoreRos",
"Touch table calibration started. Please press the white point."),
temp=False)
self.touch_points = TouchPointsItem(self.scene, pts)
def save_gripper_pose_cb(self, idx):
topics = [
'/art/pr2/right_arm/gripper/pose', '/art/pr2/left_arm/gripper/pose'
]
# wait for message, set pose
try:
ps = rospy.wait_for_message(topics[idx], PoseStamped, timeout=2)
except (rospy.ROSException) as e:
rospy.logerr(str(e))
self.notif(translate("UICoreRos", "Failed to store gripper pose."),
temp=False)
return
self.notif(translate("UICoreRos", "Gripper pose stored."), temp=False)
self.program_vis.set_pose(ps)
def touch_calibration_points_cb(self, req):
resp = TouchCalibrationPointsResponse()
pts = []
for pt in req.points:
pts.append((pt.point.x, pt.point.y))
self.emit(QtCore.SIGNAL('touch_calibration_points_evt'), pts)
self.touched_sub = rospy.Subscriber(
'/art/interface/touchtable/touch_detected',
Empty,
self.touch_detected_cb,
queue_size=10)
resp.success = True
return resp
def touch_detected_evt(self, msg):
if self.touch_points is None:
return
if not self.touch_points.next():
self.notif(translate("UICoreRos", "Touch saved."), temp=True)
for it in self.scene.items():
if isinstance(it, LabelItem):
continue
# TODO fix this - in makes visible even items that are invisible by purpose
it.setVisible(True)
self.notif(translate("UICoreRos",
"Touch table calibration finished."),
temp=False)
self.scene.removeItem(self.touch_points)
self.touch_points = None
self.touched_sub.unregister()
else:
self.notif(translate("UICoreRos", "Touch saved."), temp=True)
self.notif(translate("UICoreRos", "Please press the next point."),
temp=False)
def touch_detected_cb(self, msg):
self.emit(QtCore.SIGNAL('touch_detected_evt'), msg)
def calibrate_projectors_cb(self, req):
resp = TriggerResponse()
resp.success = True
# call to start_projector_calibration is blocking
self.proj_calib_timer = rospy.Timer(rospy.Duration(0.001),
self.start_projector_calibration,
oneshot=True)
return resp
def notify_user_srv_cb(self, req):
self.emit(QtCore.SIGNAL('notify_user_evt'), req)
return NotifyUserResponse()
def notify_user_evt(self, req):
# TODO message should be displayed until user closes it
if req.duration == rospy.Duration(0):
self.notif(req.message, message_type=req.type)
else:
self.notif(req.message,
min_duration=req.duration.to_sec(),
temp=True,
message_type=req.type)
def stop_btn_clicked(self, btn):
try:
if self.emergency_stopped:
self.emergency_stop_reset_srv.call()
self.emergency_stopped = False
self.stop_btn.set_caption("STOP")
self.stop_btn.set_background_color(QtCore.Qt.red)
self.notif(translate("UICoreRos", "Resetting motors"),
temp=True)
else:
self.emergency_stop_srv.call()
self.emergency_stopped = True
self.stop_btn.set_caption("RUN")
self.stop_btn.set_background_color(QtCore.Qt.green)
self.notif(translate("UICoreRos", "Emergency stop pressed"),
temp=True)
except rospy.service.ServiceException:
self.notif(translate("UICoreRos", "Failed to stop/run robot."),
temp=True)
def program_error_dialog_cb(self, idx):
req = ProgramErrorResolveRequest()
req.user_response_type = idx + 1
resp = None
try:
resp = self.program_error_resolve_srv(req)
except rospy.ServiceException as e:
rospy.logerr("Service call failed: " + str(e))
if resp is None or not resp.success:
self.notif(translate("UICoreRos",
"System failure: failed to resolve error."),
temp=True)
self.scene.removeItem(self.program_error_dialog)
self.program_error_dialog = None
def interface_state_evt(self, old_state, state, flags):
system_state_changed = old_state.system_state != state.system_state
if system_state_changed:
rospy.logdebug("New system state: " + str(state.system_state) +
", was: " + str(old_state.system_state))
self.clear_all(True)
if state.error_severity == InterfaceState.NONE and self.program_error_dialog is not None:
# hide dialog
self.scene.removeItem(self.program_error_dialog)
self.program_error_dialog = None
# display info/warning/error if there is any - only once (on change)
if state.error_severity != old_state.error_severity:
if state.error_severity == InterfaceState.INFO:
self.notif(translate("UICoreRos", "Error occurred: ") +
error_strings.get_error_string(state.error_code),
temp=True)
elif state.error_severity == InterfaceState.WARNING:
# TODO translate error number to error message
self.program_error_dialog = DialogItem(
self.scene, self.width / 2, 0.1,
translate("UICoreRos", "Handle error: ") +
error_strings.get_error_string(state.error_code), [
translate("UICoreRos", "Try again"),
translate("UICoreRos", "Skip instruction"),
translate("UICoreRos", "Fail instruction"),
translate("UICoreRos", "End program")
], self.program_error_dialog_cb)
# TODO what to do with SEVERE?
if state.system_state == InterfaceState.STATE_PROGRAM_FINISHED:
if system_state_changed:
self.notif(translate("UICoreRos", "The program is done."))
elif state.system_state == InterfaceState.STATE_IDLE:
if system_state_changed:
self.show_program_list()
elif state.system_state == InterfaceState.STATE_LEARNING:
self.state_learning(old_state, state, flags, system_state_changed)
elif state.system_state in [
InterfaceState.STATE_PROGRAM_RUNNING,
InterfaceState.STATE_PROGRAM_STOPPED
]:
self.state_running(old_state, state, flags, system_state_changed)
def interface_state_cb(self, old_state, state, flags):
# print state
self.emit(QtCore.SIGNAL('interface_state'), old_state, state, flags)
def state_running(self, old_state, state, flags, system_state_changed):
if system_state_changed:
if not self.ph.load(self.art.load_program(state.program_id)):
self.notif(
translate("UICoreRos",
"Failed to load program from database."))
# TODO what to do?
return
stopped = state.system_state == InterfaceState.STATE_PROGRAM_STOPPED
self.show_program_vis(readonly=True, stopped=stopped)
if stopped:
self.notif(translate("UICoreRos", "Program paused."),
temp=True)
if not stopped and old_state.system_state == InterfaceState.STATE_PROGRAM_STOPPED:
self.notif(translate("UICoreRos", "Program resumed."),
temp=True)
# ignore not valid states
if state.block_id == 0 or state.program_current_item.id == 0:
rospy.logerr("Invalid state!")
return
# TODO if the item id is same - do rather update then clear + add everything?
self.clear_all()
self.program_vis.set_active(state.block_id,
state.program_current_item.id)
it = state.program_current_item
if it.type == ProgIt.GET_READY:
self.notif(translate("UICoreRos", "Robot is getting ready"))
elif it.type == ProgIt.WAIT_FOR_USER:
self.notif(translate("UICoreRos", "Waiting for user"))
elif it.type == ProgIt.WAIT_UNTIL_USER_FINISHES:
self.notif(translate("UICoreRos", "Waiting for user to finish"))
elif it.type == ProgIt.PICK_FROM_POLYGON:
obj_id = None
try:
obj_id = flags["SELECTED_OBJECT_ID"]
except KeyError:
rospy.logerr(
"PICK_FROM_POLYGON: SELECTED_OBJECT_ID flag not set")
if obj_id is not None:
self.select_object(obj_id)
obj = self.get_object(obj_id) # TODO notif - object type
if obj is not None:
self.notif(
translate("UICoreRos", "Going to pick object ID ") +
obj_id + translate("UICoreRos", " of type ") +
obj.object_type.name +
translate("UICoreRos", " from polygon."))
self.add_polygon(
translate("UICoreRos", "PICK POLYGON"),
poly_points=conversions.get_pick_polygon_points(it),
fixed=True)
elif it.type == ProgIt.PICK_FROM_FEEDER:
# TODO PICK_FROM_FEEDER
pass
elif it.type == ProgIt.PICK_OBJECT_ID:
self.notif(
translate("UICoreRos", "Picking object with ID=") +
it.object[0])
self.select_object(it.object[0])
elif it.type == ProgIt.PLACE_TO_POSE:
try:
obj_id = flags["SELECTED_OBJECT_ID"]
except KeyError:
rospy.logerr("PLACE_TO_POSE: SELECTED_OBJECT_ID flag not set")
return
obj = self.get_object(obj_id)
if obj is not None:
self.add_place(translate("UICoreRos", "OBJECT PLACE POSE"),
it.pose[0],
obj.object_type,
obj_id,
fixed=True)
elif it.type == ProgIt.PLACE_TO_GRID:
ref_msg = self.program_vis.get_ref_item(
it.ref_id) # obtaining reference instruction
if ref_msg.type == ProgIt.PICK_OBJECT_ID:
obj = self.get_object(ref_msg.object[0])
object_type = obj.object_type
self.program_vis.get_current_item().object[0] = obj.object_id
else:
object_type = self.art.get_object_type(ref_msg.object[0])
self.program_vis.get_current_item(
).object[0] = ref_msg.object[0]
self.notif(
translate("UICoreRos", "Going to place object into grid"))
self.add_square(
translate("UICoreRos", "PLACE SQUARE GRID"),
self.width / 2,
self.height / 2,
0.1,
0.075,
object_type,
it.pose,
grid_points=conversions.get_pick_polygon_points(it),
square_changed=self.square_changed,
fixed=True)
def show_program_vis(self, readonly=False, stopped=False):
rospy.logdebug("Showing ProgramItem with readonly=" + str(readonly) +
", stopped=" + str(stopped))
self.program_vis = ProgramItem(
self.scene,
self.last_prog_pos[0],
self.last_prog_pos[1],
self.ph,
done_cb=self.learning_done_cb,
item_switched_cb=self.active_item_switched,
learning_request_cb=self.learning_request_cb,
stopped=stopped,
pause_cb=self.pause_cb,
cancel_cb=self.cancel_cb)
self.program_vis.set_readonly(readonly)
def pause_cb(self):
if self.state_manager.state.system_state == InterfaceState.STATE_PROGRAM_STOPPED:
# TODO call trigger service method
try:
resp = self.program_resume_srv()
except rospy.ServiceException:
pass
if resp is not None and resp.success:
return True
else:
self.notif(translate("UICoreRos", "Failed to resume program."),
temp=True)
return False
elif self.state_manager.state.system_state == InterfaceState.STATE_PROGRAM_RUNNING:
try:
resp = self.program_pause_srv()
except rospy.ServiceException:
pass
if resp is not None and resp.success:
self.notif(translate("UICoreRos", "Program paused."),
temp=True)
return True
else:
self.notif(translate("UICoreRos", "Failed to pause program."),
temp=True)
return True
else:
rospy.logdebug(
"Attempt to pause/resume program in strange state: " +
str(self.state_manager.state.system_state))
return False
def cancel_cb(self):
if self.state_manager.state.system_state in [
InterfaceState.STATE_PROGRAM_RUNNING,
InterfaceState.STATE_PROGRAM_STOPPED
]:
try:
resp = self.program_stop_srv()
except rospy.ServiceException:
pass
if resp is not None and resp.success:
self.notif(translate("UICoreRos", "Program stopped."),
temp=True)
return True
else:
self.notif(translate("UICoreRos", "Failed to stop program."),
temp=True)
return True
else:
rospy.logdebug("Attempt to stop program in strange state: " +
str(self.state_manager.state.system_state))
return False
def clear_all(self, include_dialogs=False):
rospy.logdebug("Clear all")
super(UICoreRos, self).clear_all()
if include_dialogs:
for it in [self.program_list, self.program_vis]:
if it is None:
continue
try:
self.last_prog_pos = it.get_pos()
except AttributeError:
pass
break
for it in [
self.program_error_dialog, self.grasp_dialog,
self.program_vis, self.program_list
]:
if it is None:
continue
self.remove_scene_items_by_type(type(it))
it = None
def state_learning(self, old_state, state, flags, system_state_changed):
if system_state_changed:
self.last_edited_prog_id = state.program_id
if not self.ph.load(self.art.load_program(state.program_id)):
self.notif(
translate("UICoreRos",
"Failed to load program from database."))
# TODO what to do?
return
if state.block_id != 0 and state.program_current_item.id != 0:
# there may be unsaved changes - let's use ProgramItem from brain
self.ph.set_item_msg(state.block_id,
state.program_current_item)
self.show_program_vis()
if state.block_id == 0 or state.program_current_item.id == 0:
rospy.logerr("Invalid state!")
return
if old_state.block_id != state.block_id or old_state.program_current_item.id != state.program_current_item.id:
self.clear_all()
# TODO overit funkcnost - pokud ma state novejsi timestamp nez nas - ulozit ProgramItem
if old_state.timestamp == rospy.Time(
0
) or old_state.timestamp - state.timestamp > rospy.Duration(0):
rospy.logdebug('Got state with newer timestamp!')
item = self.ph.get_item_msg(state.block_id,
state.program_current_item.id)
item = state.program_current_item
self.clear_all()
self.learning_vis(state.block_id, state.program_current_item.id,
not state.edit_enabled)
def learning_vis(self, block_id, item_id, read_only):
if not self.ph.item_requires_learning(block_id, item_id):
self.notif(translate("UICoreRos", "Item has no parameters."))
return
self.program_vis.editing_item = not read_only
# TODO Edit/Done button not visible when there is work in progress!
if block_id != self.program_vis.block_id or item_id != self.program_vis.item_id:
self.program_vis.set_active(block_id, item_id)
msg = self.ph.get_item_msg(block_id, item_id)
if self.ph.item_learned(block_id, item_id):
self.notif(
translate("UICoreRos", "This program item seems to be done"))
else:
if msg.type in [
ProgIt.PICK_FROM_POLYGON, ProgIt.PICK_FROM_FEEDER,
ProgIt.PICK_OBJECT_ID, ProgIt.PLACE_TO_POSE
]:
self.notif(
translate("UICoreRos",
"Program current manipulation task"))
if msg.type == ProgIt.PICK_FROM_POLYGON:
if not self.ph.is_object_set(block_id, item_id):
self.notif(translate("UICoreRos",
"Select object type to be picked up"),
temp=True)
else:
self.select_object_type(msg.object[0])
if self.ph.is_polygon_set(block_id, item_id):
self.add_polygon(
translate("UICoreRos", "PICK POLYGON"),
poly_points=conversions.get_pick_polygon_points(msg),
polygon_changed=self.polygon_changed,
fixed=read_only)
elif msg.type == ProgIt.PICK_FROM_FEEDER:
if self.state_manager.state.edit_enabled and self.grasp_dialog is None:
self.grasp_dialog = DialogItem(self.scene, self.width / 2, 0.1,
"Save gripper pose",
["Right arm", "Left arm"],
self.save_gripper_pose_cb)
if self.ph.is_object_set(block_id, item_id):
self.select_object_type(msg.object[0])
else:
self.notif(translate("UICoreRos",
"Select object type to be picked up"),
temp=True)
# TODO show pick pose somehow (arrow??)
elif msg.type == ProgIt.PICK_OBJECT_ID:
if self.ph.is_object_set(block_id, item_id):
self.select_object(msg.object[0])
else:
self.notif(translate("UICoreRos",
"Select object to be picked up"),
temp=True)
elif msg.type == ProgIt.PLACE_TO_POSE:
if not self.ph.is_object_set(block_id, msg.ref_id[0]):
self.notif(
translate("UICoreRos",
"Select object to be picked up in ID=") +
str(msg.ref_id[0]))
else:
# TODO what to do with more than 1 reference?
ref_msg = self.ph.get_item_msg(block_id, msg.ref_id[0])
if ref_msg.type == ProgIt.PICK_OBJECT_ID:
obj = self.get_object(ref_msg.object[0])
object_type = obj.object_type
object_id = obj.object_id
self.select_object(ref_msg.object[0])
else:
object_type = self.art.get_object_type(ref_msg.object[0])
object_id = None
self.select_object_type(ref_msg.object[0])
if self.ph.is_object_set(block_id, msg.ref_id[0]):
if self.ph.is_pose_set(block_id, item_id):
if object_type is not None:
self.add_place(translate("UICoreRos",
"OBJECT PLACE POSE"),
msg.pose[0],
object_type,
object_id,
place_cb=self.place_pose_changed,
fixed=read_only)
else:
self.notif(
translate("UICoreRos",
"Set where to place picked object"))
self.add_place(translate("UICoreRos",
"OBJECT PLACE POSE"),
self.get_def_pose(),
object_type,
object_id,
place_cb=self.place_pose_changed,
fixed=read_only)
elif msg.type == ProgIt.PLACE_TO_GRID:
ref_msg = self.program_vis.get_ref_item(
msg.ref_id) # obtaining reference instruction
if ref_msg.type == ProgIt.PICK_OBJECT_ID:
obj = self.get_object(ref_msg.object[0])
object_type = obj.object_type
self.program_vis.get_current_item().object[0] = obj.object_id
else:
object_type = self.art.get_object_type(ref_msg.object[0])
self.program_vis.get_current_item(
).object[0] = ref_msg.object[0]
self.notif(translate("UICoreRos", "Place grid"))
self.add_square(
translate("UICoreRos", "PLACE SQUARE GRID"),
self.width / 2,
self.height / 2,
0.1,
0.075,
object_type,
msg.pose,
grid_points=conversions.get_pick_polygon_points(msg),
square_changed=self.square_changed,
fixed=read_only)
def active_item_switched(self, block_id, item_id, read_only=True):
rospy.logdebug("Program ID:" + str(self.ph.get_program_id()) +
", active item ID: " + str((block_id, item_id)))
self.clear_all()
if item_id is None:
# TODO hlaska
return
self.learning_vis(block_id, item_id, read_only)
self.state_manager.update_program_item(
self.ph.get_program_id(), block_id,
self.ph.get_item_msg(block_id, item_id))
def get_def_pose(self):
ps = PoseStamped()
ps.pose.position.x = self.width / 2
ps.pose.position.y = self.height / 2
ps.pose.orientation.w = 1.0
return ps
def place_pose_changed(self, place):
if self.program_vis.editing_item:
self.program_vis.set_place_pose(place)
self.state_manager.update_program_item(
self.ph.get_program_id(), self.program_vis.block_id,
self.program_vis.get_current_item())
def calib_done_cb(self, proj):
if proj.is_calibrated():
self.calib_proj_cnt += 1
while self.calib_proj_cnt < len(self.projectors):
if self.projectors[self.calib_proj_cnt].is_calibrated():
self.calib_proj_cnt += 1
continue
self.projectors[self.calib_proj_cnt].calibrate(
self.calib_done_cb)
return
rospy.loginfo('Projectors calibrated.')
self.projectors_calibrated_pub.publish(True)
else:
# calibration failed - let's try again
rospy.logerr('Calibration failed for projector: ' + proj.proj_id)
proj.calibrate(self.calib_done_cb)
def start_projector_calibration(self, evt):
if len(self.projectors) == 0:
rospy.loginfo('No projectors to calibrate.')
self.projectors_calibrated_pub.publish(True)
else:
self.projectors_calibrated_pub.publish(False)
rospy.loginfo('Starting calibration of ' +
str(len(self.projectors)) + ' projector(s)')
self.calib_proj_cnt = 0
for proj in self.projectors:
if proj.is_calibrated():
self.calib_proj_cnt += 1
continue
else:
if not proj.calibrate(self.calib_done_cb):
# TODO what to do?
rospy.logerr("Failed to start projector calibration")
return
rospy.loginfo('Projectors calibrated.')
self.projectors_calibrated_pub.publish(True)
def is_template(self):
return self.template
def learning_done_cb(self):
prog = self.ph.get_program()
# if it is template - save it with new id
if self.is_template():
self.template = False
headers = self.art.get_program_headers()
ids = []
for h in headers:
ids.append(h.id)
# is there a better way how to find not used ID for program?
for i in range(0, 2**16 - 1):
if i not in ids:
prog.header.id = i
break
else:
rospy.logerr("Failed to find available program ID")
if not self.art.store_program(prog):
self.notif(translate("UICoreRos", "Failed to store program"),
temp=True)
# TODO what to do?
self.notif(translate("UICoreRos", "Program stored with ID=") +
str(prog.header.id),
temp=True)
self.last_edited_prog_id = prog.header.id
resp = None
try:
resp = self.stop_learning_srv()
except rospy.ServiceException as e:
print "Service call failed: %s" % e
if resp is None or not resp.success:
rospy.logwarn("Failed to stop learning mode.")
return
def program_selected_cb(self, prog_id, run=False, template=False):
self.template = template
if run:
(started, error) = self.art.start_program(prog_id)
if not started:
self.notif(translate("UICoreRos", "Failed to start program."))
rospy.logerr("Brain refused to start program: " + error)
return
self.notif(translate("UICoreRos",
"Starting program ID=" + str(prog_id)),
temp=True)
self.program_list.set_enabled(False)
else:
if not self.ph.load(self.art.load_program(prog_id), template):
self.notif(
translate("UICoreRos",
"Failed to load program from database."))
# TODO what to do?
return
req = startProgramRequest()
req.program_id = prog_id
resp = None
try:
resp = self.start_learning_srv(req)
except rospy.ServiceException as e:
print "Service call failed: %s" % e
if resp is None or not resp.success:
self.notif(translate("UICoreRos",
"Failed to start edit mode."))
# else:
# self.clear_all()
# self.show_program_vis()
def learning_request_cb(self, req):
if req == LearningRequestGoal.GET_READY:
self.notif(
translate("UICoreRos", "Robot is getting ready for learning"))
elif req == LearningRequestGoal.DONE:
self.notif(
translate("UICoreRos", "Robot is getting into default state"))
if self.grasp_dialog is not None:
self.scene.removeItem(self.grasp_dialog)
self.grasp_dialog = None
elif req == LearningRequestGoal.EXECUTE_ITEM:
self.notif(
translate("UICoreRos",
"Robot is executing current program instruction"))
g = LearningRequestGoal()
g.request = req
self.learning_action_cl.send_goal(
g,
done_cb=self.learning_request_done_cb,
feedback_cb=self.learning_request_feedback_cb)
def learning_request_feedback_cb(self, fb):
rospy.logdebug('learning request progress: ' + str(fb.progress))
def learning_request_done_evt(self, status, result):
self.program_vis.learning_request_result(result.success)
def learning_request_done_cb(self, status, result):
self.emit(QtCore.SIGNAL('learning_request_done_evt'), status, result)
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 object_cb(self, msg):
self.emit(QtCore.SIGNAL('objects'), msg)
def object_cb_evt(self, msg):
for obj_id in msg.lost_objects:
self.remove_object(obj_id)
self.notif(translate("UICoreRos", "Object") + " ID=" +
str(obj_id) + " " +
translate("UICoreRos", "disappeared"),
temp=True)
for inst in msg.instances:
obj = self.get_object(inst.object_id)
if obj:
obj.set_pos(inst.pose.position.x, inst.pose.position.y,
inst.pose.position.z)
obj.set_orientation(conversions.q2a(inst.pose.orientation))
else:
obj_type = self.art.get_object_type(inst.object_type)
self.add_object(inst.object_id, obj_type, inst.pose.position.x,
inst.pose.position.y, inst.pose.position.z,
conversions.q2a(inst.pose.orientation),
self.object_selected)
self.notif(translate("UICoreRos", "New object") + " ID=" +
str(inst.object_id),
temp=True)
def polygon_changed(self, pts):
if self.program_vis.editing_item:
self.program_vis.set_polygon(pts)
self.state_manager.update_program_item(
self.ph.get_program_id(), self.program_vis.block_id,
self.program_vis.get_current_item())
'''
Method which saves grid points and place poses of all objects in grid.
'''
def square_changed(self, pts, poses=None):
self.program_vis.set_place_grid(
pts) # saving grid points into the ProgramItem message
self.program_vis.set_place_poses(
poses) # saving place poses into the ProgramItem message
self.state_manager.update_program_item(
self.ph.get_program_id(), self.program_vis.block_id,
self.program_vis.get_current_item())
def object_selected(self, id, selected):
if self.program_vis is None or not self.program_vis.editing_item:
rospy.logdebug("not in edit mode")
return False
rospy.logdebug("attempt to select object id: " + id)
obj = self.get_object(id)
msg = self.program_vis.get_current_item()
if msg is None:
return False
if msg.type in [ProgIt.PICK_FROM_FEEDER, ProgIt.PICK_FROM_POLYGON]:
# this type of object is already set
if len(msg.object) > 0 and obj.object_type.name == msg.object[0]:
rospy.logdebug("object type " + obj.object_type.name +
" already selected")
return
else:
# TODO remove previously inserted polygon, do not insert new
# place
rospy.logdebug("selecting new object type: " +
obj.object_type.name)
pass
if msg.type == ProgIt.PICK_FROM_FEEDER:
self.program_vis.set_object(obj.object_type.name)
self.select_object_type(obj.object_type.name)
elif msg.type == ProgIt.PICK_OBJECT_ID:
self.program_vis.set_object(obj.object_id)
self.select_object(obj.object_id)
elif msg.type == ProgIt.PICK_FROM_POLYGON:
if obj.object_type.name not in self.selected_object_types:
self.remove_scene_items_by_type(PolygonItem)
poly_points = []
self.program_vis.set_object(obj.object_type.name)
self.select_object_type(obj.object_type.name)
for ob in self.get_scene_items_by_type(ObjectItem):
if ob.object_type.name != obj.object_type.name:
continue
poly_points.append(ob.get_pos())
self.add_polygon(translate("UICoreRos", "PICK POLYGON"),
poly_points,
polygon_changed=self.polygon_changed)
self.notif(translate("UICoreRos",
"Check and adjust pick polygon"),
temp=True)
self.state_manager.update_program_item(
self.ph.get_program_id(), self.program_vis.block_id,
self.program_vis.get_current_item())
return True
def user_status_cb(self, msg):
self.emit(QtCore.SIGNAL('user_status'), msg)
def user_status_cb_evt(self, msg):
if msg.user_state != self.user_state:
if msg.user_state == UserStatus.USER_NOT_CALIBRATED:
self.notif(
translate("UICoreRos", "Please do a calibration pose"))
elif msg.user_state == UserStatus.USER_CALIBRATED:
self.notif(translate("UICoreRos", "Successfully calibrated"))
elif msg.user_state == UserStatus.NO_USER:
self.notif(translate("UICoreRos", "Waiting for user..."))
self.user_state = msg
class ArCodeDetector:
objects_table = None
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 ar_code_cb(self, data):
rospy.logdebug("New arcodes arrived:")
instances = InstancesArray()
id = 0
for arcode in data.markers:
aid = int(arcode.id)
# list of allowed object ids
# TODO load from param
if aid not in [4, 5, 3, 21, 26, 31]:
continue
if aid not in self.objects_cache:
# TODO AR code ID / object type assignment should be done somewhere...
# type "profile_20_60" is just for example (used in art_db/test_db.py)
if aid == 21:
object_type = self.art.get_object_type(
"profile_20_60_longer")
else:
object_type = self.art.get_object_type("profile_20_60")
if object_type is None:
# error or unknown object - let's ignore it
self.objects_cache[aid] = None
continue
self.objects_cache[aid] = {
'type': object_type.name,
'bb': object_type.bbox
}
# skip unknown objects
if self.objects_cache[aid] is None:
continue
obj_in = ObjInstance()
obj_in.object_id = str(aid)
obj_in.object_type = self.objects_cache[aid]['type']
obj_in.pose = arcode.pose.pose
self.show_rviz_bb(arcode.header.frame_id, obj_in, arcode.id,
self.objects_cache[aid]['bb'])
instances.header.stamp = arcode.header.stamp
instances.header.frame_id = arcode.header.frame_id
instances.instances.append(obj_in)
++id
if len(data.markers) == 0:
rospy.logdebug("Empty")
else:
# print instances
self.detected_objects_pub.publish(instances)
def show_rviz_bb(self, frame_id, obj_in, id, bb):
'''
:type obj: ObjInstance
:return:
'''
obj = copy.deepcopy(obj_in)
marker = Marker()
marker.type = marker.LINE_LIST
marker.id = int(id)
marker.action = marker.ADD
marker.scale.x = 0.001
marker.scale.y = 0.01
marker.scale.z = 0.01
marker.color.r = 0
marker.color.g = 1
marker.color.b = 0
marker.color.a = 1
marker.lifetime = rospy.Duration(5)
marker.pose = obj.pose
bbox_x = float(bb.dimensions[0] / 2)
bbox_y = float(bb.dimensions[1] / 2)
bbox_z = float(bb.dimensions[2])
marker.points = [
Point(-bbox_x, -bbox_y, -bbox_z / 2),
Point(+bbox_x, -bbox_y, -bbox_z / 2),
Point(+bbox_x, -bbox_y, -bbox_z / 2),
Point(+bbox_x, +bbox_y, -bbox_z / 2),
Point(+bbox_x, +bbox_y, -bbox_z / 2),
Point(-bbox_x, +bbox_y, -bbox_z / 2),
Point(-bbox_x, +bbox_y, -bbox_z / 2),
Point(-bbox_x, -bbox_y, -bbox_z / 2),
Point(-bbox_x, -bbox_y, -bbox_z / 2),
Point(-bbox_x, -bbox_y, +bbox_z / 2),
Point(+bbox_x, -bbox_y, -bbox_z / 2),
Point(+bbox_x, -bbox_y, +bbox_z / 2),
Point(+bbox_x, +bbox_y, -bbox_z / 2),
Point(+bbox_x, +bbox_y, +bbox_z / 2),
Point(-bbox_x, +bbox_y, -bbox_z / 2),
Point(-bbox_x, +bbox_y, +bbox_z / 2),
Point(-bbox_x, -bbox_y, +bbox_z / 2),
Point(+bbox_x, -bbox_y, +bbox_z / 2),
Point(+bbox_x, -bbox_y, +bbox_z / 2),
Point(+bbox_x, +bbox_y, +bbox_z / 2),
Point(+bbox_x, +bbox_y, +bbox_z / 2),
Point(-bbox_x, +bbox_y, +bbox_z / 2),
Point(-bbox_x, +bbox_y, +bbox_z / 2),
Point(-bbox_x, -bbox_y, +bbox_z / 2),
]
marker.header.frame_id = frame_id
self.visualize_pub.publish(marker)
marker.pose.position.z += 0.02 + bbox_z / 2
marker.id = int(id + 100)
marker.type = marker.TEXT_VIEW_FACING
marker.text = obj.object_id
marker.scale.z = 0.02
self.visualize_pub.publish(marker)
class UICoreRos(UICore):
"""The class builds on top of UICore and adds ROS-related stuff and application logic.
Attributes:
user_status (UserStatus): current user tracking status
fsm (FSM): state machine maintaining current state of the interface and proper transitions between states
state_manager (interface_state_manager): synchronization of interfaces within the ARTable system
scene_pub (rospy.Publisher): publisher for scene images
last_scene_update (rospy.Time): time when the last scene image was published
scene_img_deq (Queue.Queue): thread-safe queue for scene images (which are published in separate thread)
projectors (list): array of ProjectorHelper instances
art (ArtApiHelper): easy access to ARTable services
"""
def __init__(self):
origin = rospy.get_param("~scene_origin", [0, 0])
size = rospy.get_param("~scene_size", [1.2, 0.75])
rpm = rospy.get_param("~rpm", 1280)
super(UICoreRos, self).__init__(origin[0], origin[1], size[0], size[1], rpm)
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('send_scene'), self.send_to_clients_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)
self.user_status = None
self.program_vis.active_item_switched = self.active_item_switched
self.program_vis.program_state_changed = self.program_state_changed
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.state_manager = InterfaceStateManager("PROJECTED UI", cb=self.interface_state_cb)
cursors = rospy.get_param("~cursors", [])
for cur in cursors:
self.scene_items.append(PoseStampedCursorItem(self.scene, self.rpm, cur))
self.scene_items.append(TouchTableItem(self.scene, self.rpm, '/art/interface/touchtable/touch', self.get_scene_items_by_type(PoseStampedCursorItem)))
self.scene_items.append(ButtonItem(self.scene, self.rpm, 0, 0, "STOP", None, self.stop_btn_clicked, 2.0, QtCore.Qt.red))
self.scene_items[-1].setPos(self.scene.width() - self.scene_items[-1].w, self.scene.height() - self.scene_items[-1].h - 60)
self.scene_items[-1].set_enabled(True)
self.port = rospy.get_param("~scene_server_port", 1234)
self.tcpServer = QtNetwork.QTcpServer(self)
if not self.tcpServer.listen(port=self.port):
rospy.logerr('Failed to start scene TCP server on port ' + str(self.port))
self.tcpServer.newConnection.connect(self.newConnection)
self.connections = []
self.last_scene_update = None
self.scene.changed.connect(self.scene_changed)
self.projectors = []
projs = rospy.get_param("~projectors", [])
for proj in projs:
self.add_projector(proj)
self.art = ArtApiHelper()
def touch_calibration_points_evt(self, pts):
# TODO trigger state change?
for it in self.scene_items:
if isinstance(it, LabelItem):
continue
it.set_enabled(False, True)
self.notif(translate("UICoreRos", "Touch table calibration started. Please press the white point."), temp=False)
self.touch_points = TouchPointsItem(self.scene, self.rpm, pts)
def touch_calibration_points_cb(self, req):
resp = TouchCalibrationPointsResponse()
if self.fsm.state not in ['program_selection', 'learning', 'running']:
resp.success = False
rospy.logerr('Cannot start touchtable calibration without a user!')
return resp
pts = []
for pt in req.points:
pts.append((pt.point.x, pt.point.y))
self.emit(QtCore.SIGNAL('touch_calibration_points_evt'), pts)
self.touched_sub = rospy.Subscriber('/art/interface/touchtable/touch_detected', Empty, self.touch_detected_cb, queue_size=10)
resp.success = True
return resp
def touch_detected_evt(self, msg):
if self.touch_points is None:
return
if not self.touch_points.next():
self.notif(translate("UICoreRos", "Touch saved."), temp=True)
for it in self.scene_items:
if isinstance(it, LabelItem):
continue
it.set_enabled(True, True)
self.notif(translate("UICoreRos", "Touch table calibration finished."), temp=False)
self.scene.removeItem(self.touch_points)
self.touch_points = None
self.touched_sub.unregister()
else:
self.notif(translate("UICoreRos", "Touch saved."), temp=True)
self.notif(translate("UICoreRos", "Please press the next point."), temp=False)
def touch_detected_cb(self, msg):
self.emit(QtCore.SIGNAL('touch_detected_evt'), msg)
def newConnection(self):
rospy.loginfo('Some projector node just connected.')
self.connections.append(self.tcpServer.nextPendingConnection())
self.connections[-1].setSocketOption(QtNetwork.QAbstractSocket.LowDelayOption, 1)
self.emit(QtCore.SIGNAL('send_scene'), self.connections[-1])
# TODO deal with disconnected clients!
# self.connections[-1].disconnected.connect(clientConnection.deleteLater)
def send_to_clients_evt(self, client=None):
# if all connections are sending scene image, there is no need to render the new one
if client is None:
for con in self.connections:
if con.bytesToWrite() == 0:
break
else:
return
# TODO try to use Format_RGB16 - BMP is anyway converted to 32bits (send raw data instead)
pix = QtGui.QImage(self.scene.width(), self.scene.height(), QtGui.QImage.Format_ARGB32_Premultiplied)
painter = QtGui.QPainter(pix)
self.scene.render(painter)
painter.end()
block = QtCore.QByteArray()
out = QtCore.QDataStream(block, QtCore.QIODevice.WriteOnly)
out.setVersion(QtCore.QDataStream.Qt_4_0)
out.writeUInt32(0)
img = QtCore.QByteArray()
buffer = QtCore.QBuffer(img)
buffer.open(QtCore.QIODevice.WriteOnly)
pix.save(buffer, "BMP")
out << QtCore.qCompress(img, 1) # this seem to be much faster than using PNG compression
out.device().seek(0)
out.writeUInt32(block.size() - 4)
# print block.size()
if client is None:
for con in self.connections:
if con.bytesToWrite() > 0:
return
con.write(block)
else:
client.write(block)
def start(self):
rospy.loginfo("Waiting for ART services...")
self.art.wait_for_api()
if len(self.projectors) > 0:
rospy.loginfo("Waiting for projector nodes...")
for proj in self.projectors:
proj.wait_until_available()
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.fsm.tr_start()
def add_projector(self, proj_id):
self.projectors.append(ProjectorHelper(proj_id))
def scene_changed(self, rects):
if len(rects) == 0:
return
# TODO Publish only changes? How to accumulate them (to be able to send it only at certain fps)?
now = rospy.Time.now()
if self.last_scene_update is None:
self.last_scene_update = now
else:
if now - self.last_scene_update < rospy.Duration(1.0 / 20):
return
# print 1.0/(now - self.last_scene_update).to_sec()
self.last_scene_update = now
self.emit(QtCore.SIGNAL('send_scene'))
def stop_btn_clicked(self):
# TODO
self.notif(translate("UICoreRos", "Emergency stop pressed"), temp=True)
def interface_state_evt(self, our_state, state, flags):
if state.system_state == InterfaceState.STATE_LEARNING:
# TODO !!
pass
elif state.system_state == InterfaceState.STATE_PROGRAM_RUNNING:
if self.program_vis.prog is None or self.program_vis.prog.id != state.program_id:
program = self.art.load_program(state.program_id)
if program is not None:
self.program_vis.set_prog(program, False)
else:
pass # TODO error
if self.fsm.state != 'running':
self.clear_all()
self.program_vis.set_running()
self.fsm.tr_running()
# TODO updatovat program_vis itemem ze zpravy - pokud se lisi (timestamp) ??
self.program_vis.set_active(inst_id=state.program_current_item.id)
it = state.program_current_item
if our_state.program_current_item.id != state.program_current_item:
self.clear_all()
if it.type == ProgIt.GET_READY:
self.notif(translate("UICoreRos", "Robot is getting ready"))
elif it.type == ProgIt.WAIT:
if it.spec == ProgIt.WAIT_FOR_USER:
self.notif(translate("UICoreRos", "Waiting for user"))
elif it.spec == ProgIt.WAIT_UNTIL_USER_FINISHES:
self.notif(translate("UICoreRos", "Waiting for user to finish"))
# TODO MANIP_PICK, MANIP_PLACE
elif it.type == ProgIt.MANIP_PICK_PLACE:
obj_id = it.object
if it.spec == ProgIt.MANIP_ID:
self.select_object(it.object)
elif it.spec == ProgIt.MANIP_TYPE:
try:
obj_id = flags["SELECTED_OBJECT_ID"]
except KeyError:
rospy.logerr("MANIP_PICK_PLACE/MANIP_TYPE: SELECTED_OBJECT_ID flag not set")
pass
# TODO how to highlight selected object (by brain) ?
self.select_object_type(it.object)
self.add_polygon(translate("UICoreRos", "PICK POLYGON"), poly_points=self.program_vis.active_item.get_pick_polygon_points()) # TODO fixed
self.notif(translate("UICoreRos", "Going to manipulate with object ID=") + obj_id)
self.add_place(translate("UICoreRos", "PLACE POSE"), it.place_pose.pose.position.x, it.place_pose.pose.position.y, fixed=True)
def interface_state_cb(self, our_state, state, flags):
self.emit(QtCore.SIGNAL('interface_state'), our_state, state, flags)
# callback from ProgramItem (button press)
def program_state_changed(self, state):
if state == 'RUNNING':
prog = self.program_vis.get_prog()
prog.id = 1
if not self.art.store_program(prog):
# TODO what to do?
self.notif(translate("UICoreRos", "Failed to store program"), temp=True)
else:
self.notif(translate("UICoreRos", "Program stored. Starting..."), temp=True)
# clear all and wait for state update from brain
self.clear_all()
self.fsm.tr_program_learned()
self.art.start_program(prog.id)
# TODO pause / stop -> fsm
# elif state == ''
# callback from ProgramItem
def active_item_switched(self):
rospy.logdebug("Program ID:" + str(self.program_vis.prog.id) + ", active item ID: " + str(self.program_vis.active_item.item.id))
self.clear_all()
# TODO block_id
self.state_manager.update_program_item(self.program_vis.prog.id, self.program_vis.prog.blocks[0].id, self.program_vis.active_item.item)
if self.program_vis.active_item.item.type in [ProgIt.MANIP_PICK, ProgIt.MANIP_PLACE, ProgIt.MANIP_PICK_PLACE]:
if self.program_vis.active_item.item_learned():
self.notif(translate("UICoreRos", "This program item seems to be done"))
else:
# TODO vypsat jaky je to task?
self.notif(translate("UICoreRos", "Program current manipulation task"))
# TODO loop across program item ids - not indices!!
idx = self.program_vis.items.index(self.program_vis.active_item)
if idx > 0:
for i in range(idx - 1, -1, -1):
it = self.program_vis.items[i]
if it.item.type in [ProgIt.MANIP_PLACE, ProgIt.MANIP_PICK_PLACE] and it.is_place_pose_set():
# TODO add "artif." object instead of place??
self.add_place(translate("UICoreRos", "OBJECT FROM STEP") + " " + str(it.item.id), it.item.place_pose.pose.position.x, it.item.place_pose.pose.position.y, fixed=True)
break
if self.program_vis.active_item.item.spec == ProgIt.MANIP_TYPE:
if not self.program_vis.active_item.is_object_set():
self.notif(translate("UICoreRos", "Select object type to be picked up"), temp=True)
self.select_object_type(self.program_vis.active_item.item.object)
# if program item already contains polygon - let's display it
if self.program_vis.active_item.is_pick_polygon_set():
self.add_polygon(translate("UICoreRos", "PICK POLYGON"), poly_points=self.program_vis.active_item.get_pick_polygon_points(), polygon_changed=self.polygon_changed, fixed=True)
else:
self.notif(translate("UICoreRos", "Select object to be picked up"), temp=True)
self.select_object(self.program_vis.active_item.item.object)
if self.program_vis.active_item.is_object_set():
# TODO kdy misto place pose pouzi place polygon? umoznit zmenit pose na polygon a opacne?
if self.program_vis.active_item.is_place_pose_set():
(x, y) = self.program_vis.active_item.get_place_pose()
self.add_place(translate("UICoreRos", "PLACE POSE"), x, y, self.place_pose_changed)
else:
self.notif(translate("UICoreRos", "Set where to place picked object"))
self.add_place(translate("UICoreRos", "PLACE POSE"), self.width / 2, self.height / 2, self.place_pose_changed)
def place_pose_changed(self, pos):
self.program_vis.set_place_pose(pos[0], pos[1])
# TODO block_id
self.state_manager.update_program_item(self.program_vis.prog.id, self.program_vis.prog.blocks[0].id, self.program_vis.active_item.item)
def is_template(self):
return True
def cb_running(self):
pass
def cb_learning(self):
# TODO zobrazit instrukce k tasku
pass
def calib_done_cb(self, proj):
if proj.is_calibrated():
self.calib_proj_cnt += 1
if self.calib_proj_cnt < len(self.projectors):
self.projectors[self.calib_proj_cnt].calibrate(self.calib_done_cb)
else:
rospy.loginfo('Projectors calibrated.')
self.fsm.tr_calibrated()
else:
# calibration failed - let's try again
rospy.logerr('Calibration failed for projector: ' + proj.proj_id)
proj.calibrate(self.calib_done_cb)
def cb_start_calibration(self):
if len(self.projectors) == 0:
rospy.loginfo('No projectors to calibrate.')
self.fsm.tr_calibrated()
else:
rospy.loginfo('Starting calibration of ' + str(len(self.projectors)) + ' projector(s)')
self.calib_proj_cnt = 0
if not self.projectors[0].calibrate(self.calib_done_cb):
# TODO what to do?
rospy.logerr("Failed to start projector calibration")
def cb_waiting_for_user(self):
self.notif(translate("UICoreRos", "Waiting for user..."))
def cb_waiting_for_user_calibration(self):
self.notif(translate("UICoreRos", "Please do a calibration pose"))
def cb_program_selection(self):
self.notif(translate("UICoreRos", "Please select a program"))
# TODO display list of programs -> let user select -> then load it
program = self.art.load_program(0)
if program is not None:
self.program_vis.set_prog(program, self.is_template())
self.active_item_switched()
self.fsm.tr_program_selected()
else:
self.notif(translate("UICoreRos", "Loading of requested program failed"), temp=True)
def object_cb(self, msg):
self.emit(QtCore.SIGNAL('objects'), msg)
def object_cb_evt(self, msg):
for obj_id in msg.lost_objects:
self.remove_object(obj_id)
self.notif(translate("UICoreRos", "Object") + " ID=" + str(obj_id) + " " + translate("UICoreRos", "disappeared"), temp=True)
for inst in msg.instances:
obj = self.get_object(inst.object_id)
if obj:
obj.set_pos(inst.pose.position.x, inst.pose.position.y)
else:
# TODO get and display bounding box
# obj_type = self.art.get_object_type(inst.object_type)
self.add_object(inst.object_id, inst.object_type, inst.pose.position.x, inst.pose.position.y, self.object_selected)
self.notif(translate("UICoreRos", "New object") + " ID=" + str(inst.object_id), temp=True)
def polygon_changed(self, pts):
self.program_vis.set_polygon(pts)
# TODO block_id
self.state_manager.update_program_item(self.program_vis.prog.id, self.program_vis.prog.blocks[0].id, self.program_vis.active_item.item)
def object_selected(self, id, selected):
if self.fsm.state != 'learning':
return False
# TODO handle un-selected
print "selected object: " + str(id)
obj = self.get_object(id)
# TODO test typu operace
if self.program_vis.active_item.item.spec == ProgIt.MANIP_TYPE:
# this type of object is already set
if obj.object_type == self.program_vis.active_item.item.object:
return
else:
# TODO remove previously inserted polygon, do not insert new place
pass
poly_points = []
self.program_vis.set_object(obj.object_type)
self.select_object_type(obj.object_type)
for obj in self.get_scene_items_by_type(ObjectItem):
poly_points.append(obj.get_pos())
self.add_polygon(translate("UICoreRos", "PICK POLYGON"), poly_points, polygon_changed=self.polygon_changed)
self.notif(translate("UICoreRos", "Check and adjust pick polygon"), temp=True)
self.notif(translate("UICoreRos", "Set where to place picked object"), temp=True)
self.add_place(translate("UICoreRos", "PLACE POSE"), self.width / 2, self.height / 2, self.place_pose_changed)
elif self.program_vis.active_item.item.spec == ProgIt.MANIP_ID:
# TODO
pass
# TODO block_id
self.state_manager.update_program_item(self.program_vis.prog.id, self.program_vis.prog.blocks[0].id, self.program_vis.active_item.item)
return True
def user_status_cb(self, msg):
self.emit(QtCore.SIGNAL('user_status'), msg)
def user_status_cb_evt(self, msg):
self.user_status = msg
try:
if self.user_status.user_state == UserStatus.USER_NOT_CALIBRATED:
self.fsm.tr_user_present()
elif self.user_status.user_state == UserStatus.USER_CALIBRATED:
self.fsm.tr_user_calibrated()
except MachineError:
pass
class ArtBrain:
UNKNOWN = -2 # should not happen!
NOP = -1 # no operation
GET_READY = 0 # retract arms etc.
MANIP_PICK = 1
MANIP_PLACE = 2 # TODO proc nepouzit ProgramItem.MANIP_PLACE?
MANIP_PICK_PLACE = 3
WAIT = 4
INST_OK = 100
INST_BAD_DATA = 101
INST_FAILED = 102
SYSTEM_UNKNOWN = 0 # TODO proc nepouzit SystemState.SYSTEM_UNKNOWN?
SYSTEM_START = 1
SYSTEM_CALIBRATING = 2
SYSTEM_STARTING_PROGRAM_SERVER = 3
SYSTEM_READY_FOR_PROGRAM_REQUESTS = 4
SYSTEM_STOPPING_PROGRAM_SERVER = 5
def __init__(self):
self.show_marker_service = rospy.get_param('show_marker_service', '/art/interface/projected_gui/show_marker')
self.hide_marker_service = rospy.get_param('hide_marker_service', '/art/interface/projected_gui/hide_marker')
self.table_localize_action = rospy.get_param('table_localize_action', '/umf_localizer_node_table/localize')
self.pr2_localize_action = rospy.get_param('pr2_localize_action', '/umf_localizer_node/localize')
self.calibrate_pr2 = rospy.get_param('calibrate_pr2', False)
self.calibrate_table = rospy.get_param('calibrate_table', False)
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.srv_program_start = rospy.Service('/art/brain/program/start', startProgram, self.program_start_cb)
self.srv_program_stop = rospy.Service('/art/brain/program/stop', Empty, self.program_stop_cb)
# self.srv_program_pause = rospy.Service(/art/brain/program/pause', Empty, self.program_pause_cb)
# self.srv_program_resume = rospy.Service(/art/brain/program/resume', Empty, self.program_resume_cb)
self.state_manager = InterfaceStateManager(InterfaceState.BRAIN_ID) # TODO callback?
self.art = ArtApiHelper(brain=True)
self.ph = ProgramHelper()
self.user_activity = None
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("/art/brain/system_state", SystemState, queue_size=1)
self.pp_client = actionlib.SimpleActionClient('/art/pr2/left_arm/pp', pickplaceAction)
self.state = self.SYSTEM_START
self.user_id = 0
self.objects = InstancesArray()
self.executing_program = False
self.instruction = None
self.holding_object = None
self.stop_server = False
self.recalibrate = False
self.quit = False
self.art.wait_for_api()
def program_start_cb(self, req):
resp = startProgramResponse()
if self.executing_program:
resp.success = False
resp.error = 'Program already running'
return resp
rospy.loginfo('Loading program ' + str(req.program_id) + ' from db...')
program = self.art.load_program(req.program_id)
if not self.ph.load(program):
resp.success = False
resp.error = 'Cannot get program'
return resp
rospy.loginfo('Starting program')
self.executing_program = True
resp.success = True
return resp
def program_stop_cb(self, req):
rospy.loginfo('Stopping program ' + str(req.program_id) + '...')
self.executing_program = False
return EmptyResponse()
def instruction_switcher(self):
instructions = {
self.NOP: self.nop,
self.GET_READY: self.get_ready,
self.MANIP_PICK: self.manip_pick,
self.MANIP_PLACE: self.manip_place,
self.MANIP_PICK_PLACE: self.manip_pick_place,
self.WAIT: self.wait,
}
return instructions.get(self.instruction, self.unknown_instruction)
@staticmethod
def get_item_by_id(program, item_id): # TODO item should be specified using both block id and item id?
print "get_item_by_id: " + str(item_id)
for bl in program.blocks:
for it in bl.items:
if it.id == item_id:
return it
return None
def get_ready(self, instruction):
# TODO: call some service to set PR2 to ready position
self.state_manager.update_program_item(self.ph.get_program_id(), self.block_id, instruction)
return self.INST_OK
def get_pick_obj_id(self, instruction):
if instruction.spec == instruction.MANIP_ID:
obj_id = instruction.object
elif instruction.spec == instruction.MANIP_TYPE:
pick_polygon = []
pol = None
for point in instruction.pick_polygon.polygon.points: # TODO check frame_id and transform to table frame?
pick_polygon.append([point.x, point.y])
pick_polygon.append([0, 0])
if len(pick_polygon) > 0:
pol = mplPath.Path(np.array(pick_polygon), closed=True)
# shuffle the array to not get the same object each time
# random.shuffle(self.objects.instances)
print self.objects.instances
for obj in self.objects.instances:
if pol is None:
print "no polygon"
# if no pick polygon is specified - let's take the first object of that type
if obj.object_type == instruction.object:
obj_id = obj.object_id
break
else:
print "polygon"
# test if some object is in polygon and take the first one
if pol.contains_point([obj.pose.position.x, obj.pose.position.y]):
obj_id = obj.object_id
rospy.loginfo('Selected object: ' + obj_id)
break
else:
if pol is not None:
rospy.loginfo('No object in the specified polygon')
print pol
return self.INST_BAD_DATA
else:
print "strange instruction.spec: " + str(instruction.spec)
return self.INST_BAD_DATA
return obj_id
def get_place_pose(self, instruction):
# if self.holding_object is None:
# return None
if instruction.spec == instruction.MANIP_ID:
pose = instruction.place_pose
elif instruction.spec == instruction.MANIP_TYPE:
# pose = None
pose = instruction.place_pose
# TODO: how to get free position inside polygon? some perception node?
else:
return None
return pose
def manip_pick(self, instruction):
"""
:type instruction: ProgramItem
:return:
"""
obj_id = self.get_pick_obj_id(instruction)
self.state_manager.update_program_item(self.ph.get_program_id(), self.block_id, instruction, {"SELECTED_OBJECT_ID": obj_id})
if obj_id is None:
return self.INST_BAD_DATA
if self.pick_object(obj_id):
self.holding_object = obj_id
return self.INST_OK
else:
return self.INST_FAILED
def manip_place(self, instruction):
"""
:type instruction: ProgramItem
:return:
"""
pose = self.get_place_pose(instruction)
self.state_manager.update_program_item(self.ph.get_program_id(), self.block_id, instruction) # TODO place pose
if pose is None:
return self.INST_BAD_DATA
else:
if self.place_object(self.holding_object, pose):
self.holding_object = None
return self.INST_OK
else:
return self.INST_FAILED
def manip_pick_place(self, instruction):
print "manip_pick_place"
print self.objects
obj_id = self.get_pick_obj_id(instruction)
pose = self.get_place_pose(instruction)
self.state_manager.update_program_item(self.ph.get_program_id(), self.block_id, instruction, {"SELECTED_OBJECT_ID": obj_id})
# TODO also update p.i. with selected place pose when not given (place polygon)
if obj_id is None or pose is None:
print 'could not get obj_id or pose'
return self.INST_BAD_DATA
if self.pick_object(obj_id): # TODO call pick&place and not pick and then place
self.holding_object = obj_id
if self.place_object(obj_id, pose):
self.holding_object = None
return self.INST_OK
return self.INST_FAILED
def wait(self, instruction):
"""
:type instruction: ProgramItem
:return:
"""
print "waiting"
# return self.INST_OK
self.state_manager.update_program_item(self.ph.get_program_id(), self.block_id, instruction)
rate = rospy.Rate(10)
if instruction.spec == instruction.WAIT_FOR_USER:
while self.user_activity != UserActivity.READY:
rate.sleep()
elif instruction.spec == instruction.WAIT_UNTIL_USER_FINISHES:
while self.user_activity != UserActivity.WORKING:
rate.sleep()
else:
return self.INST_BAD_DATA
return self.INST_OK
def unknown_instruction(self, instruction):
print "F*ck it all, i don't know this instruction!"
return self.INST_FAILED
def nop(self, instruction=None):
return self.INST_OK
def user_activity_cb(self, data):
self.user_activity = data.activity
def user_status_cb(self, data):
"""
:type data: UserStatus
:return:
"""
self.user_id = data.user_id
pass
def objects_cb(self, objects_data):
"""
:type objects_data: InstancesArray
:return:
"""
self.objects = objects_data
def check_user_active(self):
return self.user_id != 0
def calibrate(self, action_name, server="unknown", timeout=5):
client = actionlib.SimpleActionClient(action_name, LocalizeAgainstUMFAction)
rospy.logdebug("Waiting for server (" + server + ")")
client.wait_for_server()
rospy.logdebug("Server ready (" + server + ")")
goal = LocalizeAgainstUMFGoal()
goal.timeout = rospy.Duration(timeout)
rospy.logdebug("Sending goal to server (" + server + ")")
client.send_goal(goal)
rospy.logdebug("Waiting for results (" + server + ")")
client.wait_for_result()
return not client.get_result().result
pass
def calibrate_all(self, table_calibration=True, pr2_calibration=True):
rospy.loginfo("Starting calibrating process")
rospy.logdebug("Waiting for service " + self.show_marker_service)
rospy.wait_for_service(self.show_marker_service)
try:
show_marker = rospy.ServiceProxy(self.show_marker_service, Empty)
show_marker()
except rospy.ServiceException, e:
rospy.logerr("Service call failed: " + str(e))
return False
while table_calibration:
if self.calibrate(self.table_localize_action, "table", 5):
table_calibration = False
rospy.loginfo("Table successfully calibrated")
else:
rospy.logwarn("Table calibration failed! Trying every 5 sec")
time.sleep(5)
while pr2_calibration:
if self.calibrate(self.pr2_localize_action, "pr2", 5):
pr2_calibration = False
rospy.loginfo("PR2 successfully calibrated")
else:
rospy.logwarn("PR2 calibration failed! Trying every 5 sec")
time.sleep(5)
rospy.loginfo("Calibration done, hiding umf marker")
rospy.logdebug("Waiting for service " + self.hide_marker_service)
rospy.wait_for_service(self.hide_marker_service)
try:
hide_marker = rospy.ServiceProxy(self.hide_marker_service, Empty)
hide_marker()
except rospy.ServiceException, e:
rospy.logerr("Service call failed: " + str(e))
return False