def __init__(self, robot_helper):
self.gripper_usage = self.BOTH_ARM
super(ArtPr2Interface, self).__init__(robot_helper)
self.motors_halted_sub = rospy.Subscriber(
"/pr2_ethercat/motors_halted", Bool, self.motors_halted_cb)
self.halt_motors_srv = rospy.ServiceProxy(
'/pr2_ethercat/halt_motors',
Empty) # TODO wait for service? where?
self.reset_motors_srv = rospy.ServiceProxy(
'/pr2_ethercat/reset_motors',
Empty) # TODO wait for service? where?
self.look_at_pub = rospy.Publisher(robot_helper.robot_ns + "/look_at",
PointStamped,
queue_size=10)
self.gripper_pubs = []
for pref in ('/l_', '/r_'):
self.gripper_pubs.append(
rospy.Publisher(pref + 'gripper_controller/command',
Pr2GripperCommand,
queue_size=10))
for arm in self._arms: # type: ArtGripper
if arm.arm_id == self.LEFT_ARM:
arm.arm_up = ArtBrainUtils.create_service_client(
robot_helper.robot_ns + "/" + self.LEFT_ARM + "/arm_up",
Trigger)
elif arm.arm_id == self.RIGHT_ARM:
arm.arm_up = ArtBrainUtils.create_service_client(
robot_helper.robot_ns + "/" + self.RIGHT_ARM + "/arm_up",
Trigger)
def select_arm_for_drill(self, obj_to_drill, objects_frame_id, tf_listener):
assert isinstance(obj_to_drill, ObjInstance)
free_arm = self.select_free_arm()
if free_arm in [None, self.LEFT_ARM, self.RIGHT_ARM]:
return free_arm
objects_frame_id = ArtBrainUtils.normalize_frame_id(objects_frame_id)
# frameExists("marker") always returns False -> but it is probably not necessary to test it - it should exist
# if tf_listener.frameExists(
# "base_link") and tf_listener.frameExists(ArtBrainUtils.normalize_frame_id(objects_frame_id)):
obj_pose = PoseStamped()
obj_pose.pose = obj_to_drill.pose
obj_pose.header.frame_id = objects_frame_id
# exact time does not matter in this case
obj_pose.header.stamp = rospy.Time(0)
tf_listener.waitForTransform(
'base_link',
obj_pose.header.frame_id,
obj_pose.header.stamp,
rospy.Duration(1))
obj_pose = tf_listener.transformPose(
'base_link', obj_pose)
if obj_pose.pose.position.y < 0:
return self.RIGHT_ARM
else:
return self.LEFT_ARM
def select_arm_for_pick(self, obj, objects_frame_id, tf_listener):
assert isinstance(obj, ObjInstance)
free_arm = self.select_free_arm()
if free_arm in [None, self.LEFT_ARM, self.RIGHT_ARM]:
return free_arm
objects_frame_id = ArtBrainUtils.normalize_frame_id(objects_frame_id)
obj_pose = PoseStamped()
obj_pose.pose = obj.pose
obj_pose.header.frame_id = objects_frame_id
# exact time does not matter in this case
obj_pose.header.stamp = rospy.Time(0)
tf_listener.waitForTransform(
'base_link',
obj_pose.header.frame_id,
obj_pose.header.stamp,
rospy.Duration(1))
obj_pose = tf_listener.transformPose(
'base_link', obj_pose)
if obj_pose.pose.position.y < 0:
return self.RIGHT_ARM
else:
return self.LEFT_ARM
def __init__(self, *args, **kwargs):
super(VisualInspectionFSM, self).__init__(*args, **kwargs)
self.visual_inspection_srv = ArtBrainUtils.create_service_client('/art/visual_inspection/start', Trigger)
self.visual_inspection_result_sub = rospy.Subscriber(
'/art/visual_inspection/result', Bool, self.visual_inspection_result_cb, queue_size=1)
self.visual_inspection_result = None
def __init__(self, robot_helper):
self.gripper_usage = self.BOTH_ARM
super(ArtPr2Interface, self).__init__(robot_helper)
self.motors_halted_sub = rospy.Subscriber(
"/pr2_ethercat/motors_halted", Bool, self.motors_halted_cb)
self.halt_motors_srv = rospy.ServiceProxy(
'/pr2_ethercat/halt_motors', Empty) # TODO wait for service? where?
self.reset_motors_srv = rospy.ServiceProxy(
'/pr2_ethercat/reset_motors', Empty) # TODO wait for service? where?
self.gripper_pubs = []
for pref in ('/l_', '/r_'):
self.gripper_pubs.append(rospy.Publisher(pref + 'gripper_controller/command', Pr2GripperCommand, queue_size=10))
for arm in self._arms: # type: ArtGripper
if arm.arm_id == self.LEFT_ARM:
arm.arm_up = ArtBrainUtils.create_service_client(robot_helper.robot_ns + "/" + self.LEFT_ARM + "/arm_up", Trigger)
elif arm.arm_id == self.RIGHT_ARM:
arm.arm_up = ArtBrainUtils.create_service_client(robot_helper.robot_ns + "/" + self.RIGHT_ARM + "/arm_up", Trigger)
def select_arm_for_pick_from_feeder(self, pick_pose, tf_listener):
assert isinstance(pick_pose, PoseStamped)
pick_pose.header.frame_id = ArtBrainUtils.normalize_frame_id(
pick_pose.header.frame_id)
free_arm = self.select_free_arm()
if free_arm in [None, self.LEFT_ARM, self.RIGHT_ARM]:
return free_arm
pick_pose.header.stamp = rospy.Time(0)
tf_listener.waitForTransform('base_link', pick_pose.header.frame_id,
pick_pose.header.stamp, rospy.Duration(1))
obj_pose = tf_listener.transformPose('base_link', pick_pose)
if obj_pose.pose.position.y < 0:
return self.RIGHT_ARM
else:
return self.LEFT_ARM
def select_arm_for_pick_from_feeder(self, pick_pose, tf_listener):
assert isinstance(pick_pose, PoseStamped)
pick_pose.header.frame_id = ArtBrainUtils.normalize_frame_id(pick_pose.header.frame_id)
free_arm = self.select_free_arm()
if free_arm in [None, self.LEFT_ARM, self.RIGHT_ARM]:
return free_arm
pick_pose.header.stamp = rospy.Time(0)
tf_listener.waitForTransform(
'base_link',
pick_pose.header.frame_id,
pick_pose.header.stamp,
rospy.Duration(1))
obj_pose = tf_listener.transformPose(
'base_link', pick_pose)
if obj_pose.pose.position.y < 0:
return self.RIGHT_ARM
else:
return self.LEFT_ARM
def pick_object_from_polygon(self, instruction, update_state_manager=True):
obj_type = self.brain.ph.get_object(self.brain.block_id,
instruction.id)[0][0]
polygon = self.brain.ph.get_polygon(self.brain.block_id,
instruction.id)[0][0]
obj = ArtBrainUtils.get_pick_obj_from_polygon(obj_type, polygon,
self.brain.objects)
if obj is None or obj.object_id is None or obj.object_id == "":
self.fsm.error(
severity=ArtBrainErrorSeverities.WARNING,
error=ArtBrainErrors.ERROR_OBJECT_MISSING_IN_POLYGON)
if update_state_manager:
self.brain.state_manager.update_program_item(
self.brain.ph.get_program_id(), self.brain.block_id,
instruction)
return
if update_state_manager:
self.brain.state_manager.update_program_item(
self.brain.ph.get_program_id(), self.brain.block_id,
instruction, {"SELECTED_OBJECT_ID": obj.object_id})
arm_id = self.brain.robot.select_arm_for_pick(
obj, self.brain.objects.header.frame_id, self.brain.tf_listener)
severity, error, arm_id = self.brain.robot.pick_object(
obj, instruction.id, arm_id)
if error is not None:
if error is not ArtBrainErrors.ERROR_ROBOT_HALTED:
self.brain.try_robot_arms_get_ready([arm_id])
else:
self.fsm.error(severity=severity, error=error, halted=True)
return
self.fsm.error(severity=severity, error=error)
else:
self.fsm.done(success=True)
def place_object_to_grid(self,
instruction,
update_state_manager=True,
get_ready_after_place=True):
rospy.logerr(
"DO NOT USE, DEPRECATED! (place_object_to_grid in node.py)")
pose = ArtBrainUtils.get_place_pose(instruction)
if pose is None or len(pose) < 1:
self.fsm.error(severity=ArtBrainErrorSeverities.ERROR,
error=ArtBrainErrors.ERROR_NOT_ENOUGH_PLACE_POSES)
if update_state_manager:
self.brain.state_manager.update_program_item(
self.brain.ph.get_program_id(), self.brain.block_id,
instruction)
return
else:
if len(instruction.ref_id) < 1:
self.fsm.error(severity=ArtBrainErrorSeverities.ERROR,
error=ArtBrainErrors.
ERROR_NO_PICK_INSTRUCTION_ID_FOR_PLACE)
if update_state_manager:
self.brain.state_manager.update_program_item(
self.brain.ph.get_program_id(), self.brain.block_id,
instruction)
return
rospy.logdebug(self.brain.instruction)
gripper = self.brain.get_gripper_by_pick_instruction_id(
instruction.ref_id)
if not self.brain.check_gripper_for_place(gripper):
return
if gripper.holding_object is None:
rospy.logerr("Robot is not holding selected object")
self.fsm.error(severity=ArtBrainErrorSeverities.WARNING,
error=ArtBrainErrors.
ERROR_GRIPPER_NOT_HOLDING_SELECTED_OBJECT)
if update_state_manager:
self.brain.state_manager.update_program_item(
self.brain.ph.get_program_id(), self.brain.block_id,
instruction)
return
if update_state_manager:
self.brain.state_manager.update_program_item(
self.brain.ph.get_program_id(), self.brain.block_id,
instruction,
{"SELECTED_OBJECT_ID": gripper.holding_object.object_id})
if self.brain.place_object(gripper.holding_object,
pose[0],
gripper,
pick_only_y_axis=False):
instruction.pose.pop(0)
gripper.holding_object = None
if get_ready_after_place:
gripper.get_ready()
if len(instruction.pose) > 0:
self.fsm.done(success=True)
else:
self.fsm.error(
severity=ArtBrainErrorSeverities.ERROR,
error=ArtBrainErrors.ERROR_NOT_ENOUGH_PLACE_POSES)
else:
gripper.get_ready()
self.fsm.error(severity=ArtBrainErrorSeverities.WARNING,
error=ArtBrainErrors.ERROR_PLACE_FAILED)
def pick_object_from_feeder(self, instruction):
self.brain.state_manager.update_program_item(
self.brain.ph.get_program_id(), self.brain.block_id, instruction)
if not self.brain.ph.is_object_set(self.brain.block_id,
instruction.id):
self.fsm.error(severity=ArtBrainErrorSeverities.ERROR,
error=ArtBrainErrors.ERROR_OBJECT_NOT_DEFINED)
return
obj_type = self.brain.ph.get_object(self.brain.block_id,
instruction.id)[0][0]
obj = ArtBrainUtils.get_pick_obj_from_feeder(obj_type)
if not self.brain.ph.is_pose_set(self.brain.block_id, instruction.id):
self.fsm.error(severity=ArtBrainErrorSeverities.ERROR,
error=ArtBrainErrors.ERROR_PICK_POSE_NOT_SELECTED)
return
pick_pose, _ = self.brain.ph.get_pose(self.brain.block_id,
instruction.id)
if pick_pose is None:
self.fsm.error(severity=ArtBrainErrorSeverities.ERROR,
error=ArtBrainErrors.ERROR_PICK_POSE_NOT_SELECTED)
else:
pick_pose = pick_pose[0]
arm_id = self.brain.robot.select_arm_for_pick_from_feeder(
pick_pose, self.brain.tf_listener)
severity, error, arm_id = self.brain.robot.move_arm_to_pose(
pick_pose, arm_id, picking=True)
if error is not None:
if error is not ArtBrainErrors.ERROR_ROBOT_HALTED:
self.brain.try_robot_arms_get_ready([arm_id])
else:
self.fsm.error(severity=severity, error=error, halted=True)
return
self.fsm.error(severity=severity, error=error)
return
start_time = rospy.Time.now()
object_found_time = None
pick_object = None
pick_object_dist = None
rospy.loginfo("Looking for: " + str(obj.object_type))
ignored_objects = []
while True:
now = rospy.Time.now()
if start_time + rospy.Duration(5.0) < now:
rospy.logwarn("Can't find object in feeder in given time.")
break
if object_found_time and object_found_time + \
rospy.Duration(1.0) < now:
break
for inst in self.brain.objects.instances: # type: ObjInstance
if inst.object_id in ignored_objects:
continue
if inst.object_type != obj.object_type:
continue
# TODO read table size from some param
# TODO on_table -> use method from some helper class (shared
# with gui...), add it to message?
on_table = inst.pose.position.z < 0.1 and 0 < inst.pose.position.x < 1.5
if on_table:
rospy.logdebug("Ignoring 'on_table' object: " +
inst.object_id)
ignored_objects.append(inst.object_id)
continue
ps = PoseStamped()
ps.header.frame_id = self.brain.objects.header.frame_id
ps.header.stamp = rospy.Time(0)
ps.pose = inst.pose
# TODO compute transform once and then only apply it
ps = self.brain.tf_listener.transformPose(
self.brain.robot.get_arm_by_id(arm_id).gripper_link, ps)
# distance in x does not matter - we want the object closest to
# the x-axis of gripper
dist = math.sqrt(ps.pose.position.y**2 + ps.pose.position.z**2)
rospy.logdebug("Distance to object ID " + inst.object_id +
" is: " + str(dist) + ", dist to gripper: " +
str(ps.pose.position.x))
if dist > 0.1:
rospy.logdebug("Object is too far in y/z.")
continue
# dist in x has to be bigger than length of the gripper
if 0.05 < ps.pose.position.x < 0.2:
if pick_object_dist is None or dist < pick_object_dist:
object_found_time = now
pick_object = inst
pick_object_dist = dist
rospy.logdebug("Storing object: " + inst.object_id)
else:
rospy.logdebug("Object to far in x.")
if not pick_object:
self.brain.try_robot_arms_get_ready([arm_id])
self.fsm.error(severity=ArtBrainErrorSeverities.WARNING,
error=ArtBrainErrors.ERROR_OBJECT_MISSING)
return
severity, error, arm_id = self.brain.robot.pick_object(
pick_object, instruction.id, arm_id, from_feeder=True)
if error is not None:
if error is not ArtBrainErrors.ERROR_ROBOT_HALTED:
self.brain.try_robot_arms_get_ready([arm_id])
else:
self.fsm.error(severity=severity, error=error, halted=True)
return
self.fsm.error(severity=severity, error=error)
else:
self.fsm.done(success=True)
def drill_points(self, instruction, set_drilled_flag=True):
# TODO drill_enabled() je metoda ArtRobotArmHelper - jenze tady jeste nevim ktere rameno se bude pouzivat
# TODO ERROR_NOT_IMPLEMENTED -> myslim ze by bylo lepsi zkontrolovat program pri pozadavku na spusteni -
# jestli neobsahuje robotem nepodporovane instrukce a pak uz se tim nezabyvat - ke spusteni programu s
# instrukci co robot nepodporuje by vubec nemelo dojit
# if not self.rh.drill_enabled():
# self.fsm.error(severity=ArtBrainErrorSeverities.ERROR,
# error=ArtBrainErrors.ERROR_NOT_IMPLEMENTED)
# return
if not self.brain.check_robot():
return
objects, _ = self.brain.ph.get_object(self.brain.block_id,
instruction.id)
# TODO tohle nemuze nastat - kontroluje program helper
if len(objects) < 1:
self.fsm.error(severity=ArtBrainErrorSeverities.ERROR,
error=ArtBrainErrors.ERROR_OBJECT_NOT_DEFINED)
return
obj_type = self.brain.ph.get_object(self.brain.block_id,
instruction.id)[0][0]
obj_to_drill = None
objects_in_polygon = ArtBrainUtils.get_objects_in_polygon(
obj_type,
self.brain.ph.get_polygon(self.brain.block_id,
instruction.id)[0][0],
self.brain.objects)
if not objects_in_polygon:
self.fsm.error(
severity=ArtBrainErrorSeverities.WARNING,
error=ArtBrainErrors.ERROR_OBJECT_MISSING_IN_POLYGON)
return
for obj in objects_in_polygon:
drilled = False
for flag in obj.flags:
if flag.key == "drilled" and flag.value == "true":
rospy.logdebug("Object " + obj.object_id +
" already drilled.")
drilled = True
break
if drilled:
continue
obj_to_drill = obj
break
if obj_to_drill is None:
rospy.loginfo("All objects in polygon seems to be drilled.")
# self.try_robot_arms_get_ready([arm_id])
self.fsm.done(success=False)
return
arm_id = self.brain.robot.select_arm_for_drill(obj_to_drill,
self.brain.tf_listener)
if arm_id != self.brain.last_drill_arm_id:
if self.brain.last_drill_arm_id is not None:
self.brain.robot.arms_get_ready([self.brain.last_drill_arm_id])
self.brain.last_drill_arm_id = copy.deepcopy(arm_id)
rospy.loginfo("Drilling object: " + obj_to_drill.object_id)
poses = self.brain.ph.get_pose(self.brain.block_id, instruction.id)[0]
for hole_number, pose in enumerate(poses):
rospy.loginfo("Hole number: " + str(hole_number + 1) +
" (out of: " + str(len(poses)) + ")")
if self.brain.program_pause_request or self.brain.program_paused:
self.brain.program_pause_request = False
self.brain.program_paused = True
r = rospy.Rate(2)
while self.brain.program_paused:
r.sleep()
self.brain.state_manager.update_program_item(
self.brain.ph.get_program_id(), self.brain.block_id,
instruction, {
"SELECTED_OBJECT_ID": obj_to_drill.object_id,
"DRILLED_HOLE_NUMBER": str(hole_number + 1)
})
severity, error, arm_id = self.brain.robot.drill_point(
arm_id, [pose], obj_to_drill, "TODO", drill_duration=0)
if error:
rospy.logwarn("Drilling failed...")
self.fsm.error(severity=severity, error=error)
return
if set_drilled_flag:
req = ObjectFlagSetRequest()
req.object_id = obj_to_drill.object_id
req.flag.key = "drilled"
req.flag.value = "true"
ret = self.brain.set_object_flag_srv_client.call(req)
if not ret.success:
rospy.logerr("Failed to set flag!")
st = copy.deepcopy(self.brain.objects.header.stamp)
# need to wait (for new message from tracker) until flag is really set
# otherwise object might be drilled again...
# TODO check object flags insted of stamp? or remember that object
# was drilled e.g. in self.drilled_objects ?
while self.brain.objects.header.stamp == st:
rospy.sleep(0.1)
rospy.loginfo("Object drilled: " + obj_to_drill.object_id)
self.fsm.done(success=True)
def place_object_to_container(self,
instruction,
update_state_manager=True,
get_ready_after_place=False):
container_type = self.brain.ph.get_object(self.brain.block_id,
instruction.id)[0][0]
arm_id = self.brain.robot.select_arm_for_place("", instruction.ref_id)
if arm_id is None:
if update_state_manager:
self.brain.state_manager.update_program_item(
self.brain.ph.get_program_id(), self.brain.block_id,
instruction)
self.fsm.error(
severity=ArtBrainErrorSeverities.WARNING,
error=InterfaceState.ERROR_GRIPPER_NOT_HOLDING_SELECTED_OBJECT)
return
polygon = self.brain.ph.get_polygon(self.brain.block_id,
self.brain.instruction.id)[0][0]
container = ArtBrainUtils.get_pick_obj_from_polygon(
container_type, polygon, self.brain.objects)
if container is None:
self.fsm.error(
severity=ArtBrainErrorSeverities.WARNING,
error=ArtBrainErrors.ERROR_OBJECT_MISSING_IN_POLYGON)
return
if update_state_manager:
self.brain.state_manager.update_program_item(
self.brain.ph.get_program_id(), self.brain.block_id,
self.brain.instruction, {
"SELECTED_OBJECT_ID":
self.brain.robot.get_arm_holding_object(arm_id).object_id,
"SELECTED_CONTAINER_ID":
container.object_id
})
container_bb = self.brain.art.get_object_type(
container_type).bbox.dimensions
place_pose = PoseStamped()
place_pose.header.frame_id = "marker"
place_pose.pose = container.pose
place_pose.pose.position.z += container_bb[
2] / 2.0 + 0.065 # TODO plus half of bb height of the picked object
severity, error, _ = self.brain.robot.place_object_to_pose(
place_pose, arm_id)
if error is not None:
if error is not ArtBrainErrors.ERROR_ROBOT_HALTED:
self.brain.try_robot_arms_get_ready([arm_id])
else:
self.fsm.error(severity=severity, error=error, halted=True)
return
self.fsm.error(severity=severity, error=error)
return
else:
self.brain.robot.get_arm_by_id(
arm_id).last_pick_instruction_id = None
if get_ready_after_place:
self.brain.try_robot_arms_get_ready([arm_id])
self.fsm.done(success=True)
return