def __init__(self, arm, starting_pose):
# Range away from center you want the arm to stop within
self.FACE_RANGE = 50
# Denominator for movement, when rotational.
self.MOVEMENT_DENOMINATOR = 24
self.face_count = 0
# Starts at the end point so that it will see a new face automatically
self.face_time_counter = 80
rospy.on_shutdown(self.leave_subs_n_pubs)
# Finding center point
desired_resolution = (640, 400)
# If the arm's camera is not available, the other arm's camera will be
# closed.
try:
CameraController(arm + '_hand_camera')
except AttributeError:
if arm == 'right':
left_cam = CameraController('left_hand_camera')
left_cam.close()
elif arm == 'left':
right_cam = CameraController('right_hand_camera')
right_cam.close()
cam = CameraController(arm + '_hand_camera')
cam.resolution = desired_resolution
cam.open()
# Getting the center of the camera
(x, y) = desired_resolution
self.CENTER_X = x / 2
self.CENTER_Y = y / 2
# Position of last face seen. Initialized as something impossible.
self.last_face_x = -maxint
self.last_face_y = -maxint
self.FACE_CASCADE = cv2.CascadeClassifier(
'src/baxter_face_tracking_demos/src' +
'/haarcascade_frontalface_default.xml')
self.EYE_CASCADE = cv2.CascadeClassifier(
'src/baxter_face_tracking_demos/src/haarcascade_eye.xml')
self.cam_sub = rospy.Subscriber(
'/cameras/' + arm + '_hand_camera/image', Image, self.follow)
self.display_pub = rospy.Publisher('/robot/xdisplay',
Image,
queue_size=0)
self.hand_pub = rospy.Publisher('/ein/' + arm + '/forth_commands',
String,
queue_size=0)
sleep(3)
# Assume starting position
self.hand_pub.publish(
String(starting_pose + ' createEEPose moveEeToPoseWord'))
# It takes time to get there
sleep(1)
# Setting good camera settings
exposure_and_gain = '65 40'
self.hand_pub.publish(
String(exposure_and_gain + ' 1024 1024 2048 fixCameraLighting'))
#!/usr/bin/env python
from baxter_interface import CameraController
if __name__ == '__main__':
camera = CameraController("left_hand_camera")
camera.open()
res = (1280, 800)
camera.resolution = res
camera.exposure = camera.CONTROL_AUTO
camera.fps = 1
camera.gain = camera.CONTROL_AUTO
def interact_with_customer(self):
"""
Main interaction logic.
Handles the main logic of detecting the entrance of new customer, and
determining if the next action is to get or give money.
This will run until the amount due variable (self.amount_due) becomes
zero. Therefore, before calling this function, ensure that you have
changed the self.amount_due variable to either a positive or negative
value.
"""
def pose_is_outdated(pose):
"""Will check whether the pose is recent or not."""
return (time.time() - pose.created) > 3
# Disable some Baxter's cameras and ensure that head camera is enabled.
try:
left_hand_camera = CameraController('left_hand_camera')
head_camera = CameraController('head_camera')
left_hand_camera.close()
head_camera.open()
except:
pass
# Make Baxter's screen eyes to shown normal
self.show_eyes_normal()
self.banknotes_given = []
# Since we have new iteration here, ensure that the position of the
# banknotes on the table is reset to normal.
self.banknotes_table_left.reset_availability_for_all_banknotes()
self.banknotes_table_right.reset_availability_for_all_banknotes()
# Do this while customer own money or baxter owns money
while self.amount_due != 0:
# If the amount due is negative, Baxter owns money
if self.amount_due < 0:
change_due_image = self.image_generator.generate_change_due(
change_due=abs(self.amount_due))
self.show_image_to_baxters_head_screen(image_path=None,
image=change_due_image)
self.give_money_to_customer()
continue
print self.banknotes_given
amount_due_image = self.image_generator.generate_amount_due(
amount_due=self.amount_due,
banknotes_given=self.banknotes_given)
self.show_image_to_baxters_head_screen(image_path=None,
image=amount_due_image)
# Get the hand pose of customer's two hands.
left_pose, right_pose = self.get_pose_from_space()
# If the pose detected is not too recent, ignore.
if pose_is_outdated(left_pose) and pose_is_outdated(right_pose):
continue
# NOTE that we use right hand for left pose and left hand for right
# pose. Baxter's left arm is closer to user's right hand and vice
# versa.
if self.pose_is_reachable(left_pose):
self.take_money_from_customer(left_pose,
self.planner.right_arm)
elif self.pose_is_reachable(right_pose):
self.take_money_from_customer(right_pose,
self.planner.left_arm)
else:
print("Wasn't able to move hand to goal position")
thank_you_message_image = self.image_generator.generate_change_due(
change_due=0)
self.show_image_to_baxters_head_screen(image_path=None,
image=thank_you_message_image)
rospy.sleep(3)
rospy.init_node('get_camera_image')
#right_camera = CameraController('right_hand_camera')
boundaries = [([17, 15, 100], [50, 56, 200]), ([86, 31, 4], [220, 88, 50]),
([25, 146, 190], [62, 174, 250]), ([103, 86,
65], [145, 133, 128])]
low1 = [10, 10, 70]
up1 = [70, 70, 205] #190
lower1 = np.array(low1, dtype="uint8")
upper1 = np.array(up1, dtype="uint8")
low2 = [10, 10, 125]
up2 = [120, 120, 205] #190
lower2 = np.array(low2, dtype="uint8")
upper2 = np.array(up2, dtype="uint8")
head_camera = CameraController('head_camera')
#right_camera.close()
print("closed right")
left_camera = CameraController('left_hand_camera')
head_camera.open()
left_camera.resolution = (640, 400)
left_camera.open()
print("camera open")
camera_image = None
cv2.namedWindow('image', cv2.WINDOW_NORMAL)
def get_img(msg):
global camera_image
camera_image = msg_to_cv(msg)
def main():
"""
Main Script
"""
# Setting up head camera
head_camera = CameraController("left_hand_camera")
head_camera.resolution = (1280, 800)
head_camera.open()
print("setting balance")
happy = False
while not happy:
e = head_camera.exposure
print("exposue value: " + str(e))
e_val = int(input("new exposure value"))
head_camera.exposure = e_val
satisfaction = str(raw_input("satified? y/n"))
happy = 'y' == satisfaction
planner = PathPlanner("right_arm")
listener = tf.TransformListener()
grip = Gripper('right')
grip.calibrate()
rospy.sleep(3)
grip.open()
# creating the table obstacle so that Baxter doesn't hit it
table_size = np.array([.5, 1, 10])
table_pose = PoseStamped()
table_pose.header.frame_id = "base"
thickness = 1
table_pose.pose.position.x = .9
table_pose.pose.position.y = 0.0
table_pose.pose.position.z = -.112 - thickness / 2
table_size = np.array([.5, 1, thickness])
planner.add_box_obstacle(table_size, "table", table_pose)
raw_input("gripper close")
grip.close()
# ###load cup positions found using cup_detection.py ran in virtual environment
# start_pos_xy = np.load(POURING_CUP_PATH)
# goal_pos_xy = np.load(RECEIVING_CUP_PATH)
# start_pos = np.append(start_pos_xy, OBJECT_HEIGHT - GRABBING_OFFSET)
# goal_pos = np.append(start_pos_xy, OBJECT_HEIGHT - GRABBING_OFFSET)
# #### END LOADING CUP POSITIONS
camera_subscriber = rospy.Subscriber("cameras/left_hand_camera/image", Image, get_img)
Kp = 0.1 * np.array([0.3, 2, 1, 1.5, 2, 2, 3]) # Stolen from 106B Students
Kd = 0.01 * np.array([2, 1, 2, 0.5, 0.5, 0.5, 0.5]) # Stolen from 106B Students
Ki = 0.01 * np.array([1, 1, 1, 1, 1, 1, 1]) # Untuned
Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9]) # Untuned
cam_pos= [0.188, -0.012, 0.750]
##
## Add the obstacle to the planning scene here
##
# Create a path constraint for the arm
orien_const = OrientationConstraint()
orien_const.link_name = "right_gripper";
orien_const.header.frame_id = "base";
orien_const.orientation.y = -1.0;
orien_const.absolute_x_axis_tolerance = 0.1;
orien_const.absolute_y_axis_tolerance = 0.1;
orien_const.absolute_z_axis_tolerance = 0.1;
orien_const.weight = 1.0;
horizontal = getQuaternion(np.array([0,1,0]), np.pi)
z_rot_pos = getQuaternion(np.array([0,0,1]), np.pi / 2)
orig = quatMult(z_rot_pos, horizontal)
orig = getQuaternion(np.array([0,1,0]), np.pi / 2)
#IN THE VIEW OF THE CAMERA
#CORNER1--------->CORNER2
# | |
# | |
# | |
#CORNER3 ------------|
width = 0.3
length = 0.6
CORNER1 = np.array([0.799, -0.524, -0.03])
CORNER2 = CORNER1 + np.array([-width, 0, 0])
CORNER3 = CORNER1 + np.array([0, length, 0])
#CREATE THE GRID
dir1 = CORNER2 - CORNER1
dir2 = CORNER3 - CORNER1
grid_vals = []
ret_vals = []
for i in range(0, 3):
for j in range(0, 4):
grid = CORNER1 + i * dir1 / 2 + j * dir2 / 3
grid_vals.append(grid)
ret_vals.append(np.array([grid[0], grid[1], OBJECT_HEIGHT]))
i = -1
while not rospy.is_shutdown():
for g in grid_vals:
i += 1
while not rospy.is_shutdown():
try:
goal_1 = PoseStamped()
goal_1.header.frame_id = "base"
#x, y, and z position
goal_1.pose.position.x = g[0]
goal_1.pose.position.y = g[1]
goal_1.pose.position.z = g[2]
y = - g[1] + cam_pos[1]
x = g[0] - cam_pos[0]
q = orig
#Orientation as a quaternion
goal_1.pose.orientation.x = q[1][0]
goal_1.pose.orientation.y = q[1][1]
goal_1.pose.orientation.z = q[1][2]
goal_1.pose.orientation.w = q[0]
plan = planner.plan_to_pose(goal_1, [])
if planner.execute_plan(plan):
# raise Exception("Execution failed")
rospy.sleep(1)
#grip.open()
raw_input("open")
rospy.sleep(1)
# plt.imshow(camera_image)
print("yay: " + str(i))
pos = listener.lookupTransform("/reference/right_gripper", "/reference/base", rospy.Time(0))[0]
print("move succesfully to " + str(pos))
fname = os.path.join(IMAGE_DIR, "calib_{}.jpg".format(i))
skimage.io.imsave(fname, camera_image)
print(e)
print("index: ", i)
else:
break
print(np.array(ret_vals))
# save the positions of the gripper so that the homography matrix can be calculated
np.save(POINTS_DIR, np.array(ret_vals))
print(np.load(POINTS_DIR))
break
def __init__(self, share_path, conf_path, commands):
self.share_path = share_path
self.conf_path = conf_path
self.windows = dict()
self._btn_context = dict()
self._functions = demo_functions
self._load_config()
self.img = Image.new('RGB', (1024, 600), 'white')
self.active_window = self.windows['demo_1']
self.xdisp = rospy.Publisher('/robot/xdisplay',
ImageMsg,
latch=True,
queue_size=1)
self._status = RobotEnable()
self._commands = commands
self._font = ImageFont.truetype('%s/HelveticaLight.ttf' % share_path,
30)
self._textHeight = self._font.getsize('W')[1]
self._active_example = False
self._navigators = {
'left': Navigator('left'),
'torso_left': Navigator('torso_left'),
'right': Navigator('right'),
'torso_right': Navigator('torso_right')
}
self._listeners_connected = False
self._connect_listeners()
self._estop_state = False
self._estop_sub = rospy.Subscriber('robot/state', AssemblyState,
self._estop_callback)
self._wheel_ok = True
self.cameras = dict()
camera_list = ['left_hand', 'right_hand', 'head']
for idx, cam in enumerate(camera_list):
try:
self.cameras[cam] = CameraController('%s_camera' % cam)
except AttributeError:
try:
# This camera might not be powered
# Turn off the power to the last camera
# this will turn power on to the current camera
CameraController('%s_camera' %
camera_list[idx - 1]).close()
# And try again to locate the camera service
self.cameras[cam] = CameraController('%s_camera' % cam)
except AttributeError:
# This camera is unavailable (might be broken)
# Disable camera button in the UI
self.windows['cam_submenu'].set_btn_selectable(
'cam_%s' % cam, False)
sel = self.windows['cam_submenu'].selected_btn()
bad_cam = self.windows['cam_submenu'].get_btn('cam_%s' %
cam)
if (sel == bad_cam
and not self.windows['cam_submenu'].scroll(1)):
self.windows['cam_submenu'].selected_btn_index = 0
self.cam_sub = None
self._l_grip = {'interface': Gripper('left'), 'type': 'custom'}
self._r_grip = {'interface': Gripper('right'), 'type': 'custom'}
rospy.Timer(rospy.Duration(.5), self._check_enable)
self.error_state = False
self._enable()
self.calib_stage = 0
self.draw()
mk_process('rosrun baxter_tools tuck_arms.py -u')
