def __init__(self):
# This checks if the head camera is open. If it isn't, this will
# close the left hand camera.
try:
CameraController('head_camera')
except AttributeError:
left_cam = CameraController('left_hand_camera')
left_cam.close()
head_cam = CameraController('head_camera')
head_cam.resolution = (1280, 800)
head_cam.open()
# Creating the head variable to mess with later
self.head = Head()
self.head.set_pan(angle=0.)
# Field of view
self.CENTER_X = int(1280 / 2)
self.FOV = pi / 3
# Range away from center you want the arm to stop within
self.FACE_RANGE = 50
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.subscription = rospy.Subscriber('/cameras/head_camera/image',
Image, self.send)
rospy.on_shutdown(self.leave_subs_n_pubs)
def track():
def leave_subs_n_pubs():
subscription.unregister()
right_hand_pub.unregister()
display_pub.unregister()
# Initializing nodes
rospy.init_node('wave_for_faces', anonymous=True)
# Defining global publishers
global display_pub
display_pub = rospy.Publisher('/robot/xdisplay', Image, queue_size=0)
global right_hand_pub
right_hand_pub = rospy.Publisher('/ein/right/forth_commands',
String,
queue_size=0)
# Needs time to start up
sleep(3)
right_hand_pub.publish(String('goHome'))
# Needs time to go home
sleep(3)
# Setting and opening camera
global camera_name
camera_name = 'head_camera'
head_cam = CameraController(camera_name)
head_cam.resolution = (1280, 800)
head_cam.open()
rospy.on_shutdown(leave_subs_n_pubs)
# Setting subscription
subscription = rospy.Subscriber('/cameras/' + camera_name + '/image',
Image, send)
rospy.spin()
def __init__(self, usegripper = True):
rospy.init_node("Baxter")
threading.Thread(target=self.__jointtraj_server).start()
self.trajrgt = Trajectory("right")
self.trajlft = Trajectory("left")
if usegripper:
self.__right_hand = Gripper("right")
self.__left_hand = Gripper("left")
self.__right_hand.calibrate()
self.__left_hand.calibrate()
self.__right_limb = Limb("right")
self.__left_limb = Limb("left")
self.baxter = RobotEnable()
self.__enabled = self.baxter.state().enabled
self.enable_baxter()
self.img = None
for camname in ['head_camera', 'left_hand_camera', 'right_hand_camera']:
try:
cam = CameraController(camname)
# cam.resolution = (1280, 800)
cam.resolution = (320, 200)
except:
pass
print("baxter opened")
def set_camera(camera_name, res):
print "setting", camera_name
camera = CameraController(camera_name)
# camera.resolution = (1280,800)
camera.resolution = res
camera.exposure = 50
camera.gain = 30 #left_hand_camera.gain = CameraController.CONTROL_AUTO
camera.white_balance_red = CameraController.CONTROL_AUTO
camera.white_balance_green = CameraController.CONTROL_AUTO
camera.white_balance_blue = CameraController.CONTROL_AUTO
camera.open()
print "done", camera_name
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
([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)
mask = cv2.inRange(camera_image, lower1, upper1)
mask2 = cv2.inRange(camera_image, lower2, upper2)
mask3 = cv2.bitwise_or(mask, mask2)
blur = cv2.GaussianBlur(camera_image, (5, 5), 0)
output = cv2.bitwise_and(camera_image, camera_image, mask=mask3)
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
