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 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
class Mimic(object):
IKSVC_LEFT_URI = 'ExternalTools/left/PositionKinematicsNode/IKService'
IKSVC_RIGHT_URI = 'ExternalTools/right/PositionKinematicsNode/IKService'
def __init__(self):
self._tf = tf.TransformListener()
self._rs = RobotEnable()
self._left_arm = Limb('left')
self._left_arm.set_joint_position_speed(0.5)
self._right_arm = Limb('right')
self._right_arm.set_joint_position_speed(0.5)
self._left_arm_neutral = None
self._right_arm_neutral = None
self._left_iksvc = rospy.ServiceProxy(Mimic.IKSVC_LEFT_URI,
SolvePositionIK)
self._right_iksvc = rospy.ServiceProxy(Mimic.IKSVC_RIGHT_URI,
SolvePositionIK)
self._left_camera = CameraController('left_hand_camera')
self._right_camera = CameraController('right_hand_camera')
self._head_camera = CameraController('head_camera')
self._left_camera.close()
self._right_camera.close()
self._head_camera.close()
self._head_camera.resolution = CameraController.MODES[0]
self._head_camera.open()
self._head_camera_sub = rospy.Subscriber(
'/cameras/head_camera/image', Image,
self._head_camera_sub_callback)
self._xdisplay_pub = rospy.Publisher('/robot/xdisplay',
Image,
latch=True)
self._out_of_range = False
self._ik_smooth = 4
self._ik_rate = 200
self._avg_window = 1000
self._r_trans_prev = []
self._l_trans_prev = []
self._r_ik_prev = []
self._l_ik_prev = []
self._joint_update_pub = rospy.Publisher(
'/robot/joint_state_publish_rate', UInt16)
self._joint_update_pub.publish(self._ik_rate)
self._mimic_timer = None
############################################################################
def start(self):
self._rs.enable()
self._reset()
self._mimic_timer = rospy.Timer(rospy.Duration(1.0 / self._ik_rate),
self._mimic_callback)
rate = rospy.Rate(self._avg_window)
while not rospy.is_shutdown():
try:
(r_trans, r_rot) = self._tf.lookupTransform(
'/skeleton/user_1/right_hand', '/skeleton/user_1/torso',
rospy.Time(0))
(l_trans, l_rot) = self._tf.lookupTransform(
'/skeleton/user_1/left_hand', '/skeleton/user_1/torso',
rospy.Time(0))
self._l_trans_prev.append(l_trans)
if (len(self._l_trans_prev) > self._avg_window):
self._l_trans_prev.pop(0)
self._r_trans_prev.append(r_trans)
if (len(self._r_trans_prev) > self._avg_window):
self._r_trans_prev.pop(0)
except:
self._out_of_range = True
rate.sleep()
continue
rate.sleep()
def _reset(self):
if self._mimic_timer:
self._mimic_timer.shutdown()
self._left_arm.move_to_neutral()
self._left_arm_neutral = self._left_arm.joint_angles()
self._right_arm.move_to_neutral()
self._right_arm_neutral = self._right_arm.joint_angles()
print(self._left_arm_neutral)
print(self._right_arm_neutral)
def shutdown(self):
self._reset()
############################################################################
def _mimic_callback(self, event):
try:
l_trans = (sum(map(lambda x: x[0], self._l_trans_prev)) /
self._avg_window,
sum(map(lambda x: x[1], self._l_trans_prev)) /
self._avg_window,
sum(map(lambda x: x[2], self._l_trans_prev)) /
self._avg_window)
r_trans = (sum(map(lambda x: x[0], self._r_trans_prev)) /
self._avg_window,
sum(map(lambda x: x[1], self._r_trans_prev)) /
self._avg_window,
sum(map(lambda x: x[2], self._r_trans_prev)) /
self._avg_window)
rh_x = clamp(0.65 + (l_trans[2] / 1.5), 0.5, 0.9)
rh_y = clamp(-l_trans[0], -0.8, 0)
rh_z = clamp(-l_trans[1], -0.1, 0.8)
lh_x = clamp(0.65 + (r_trans[2] / 1.5), 0.5, 0.9)
lh_y = clamp(-r_trans[0], 0, 0.8)
lh_z = clamp(-r_trans[1], -0.1, 0.8)
self.set_left_coords(lh_x, lh_y, lh_z)
self.set_right_coords(rh_x, rh_y, rh_z)
except:
return
def _head_camera_sub_callback(self, data):
orig_img = cvbr.imgmsg_to_cv2(data, 'rgb8')
img = cv2.cvtColor(orig_img, cv2.COLOR_RGB2GRAY)
img = cv2.equalizeHist(img)
img = cv2.GaussianBlur(img, (5, 5), 0)
img = cv2.Canny(img, 100, 200)
img = cv2.cvtColor(img, cv2.COLOR_GRAY2RGB)
img = cv2.addWeighted(img, 0.75, orig_img, 0.25, 0)
img = cv2.resize(img, (1024, 768))
if self._out_of_range:
cv2.putText(img, 'Out of range!', (50, 768 - 50),
cv2.FONT_HERSHEY_SIMPLEX, 2, (255, 0, 0), 4)
img = cv2.cvtColor(img, cv2.COLOR_RGB2BGR)
img = cvbr.cv2_to_imgmsg(img)
self._xdisplay_pub.publish(img)
############################################################################
def set_left_coords(self,
x,
y,
z,
er=math.pi * -1,
ep=math.pi * 0.5,
ey=math.pi * -1):
self._set_arm_coords(
self._left_iksvc, self._left_arm, x, y, z, er, ep, ey,
self._get_neutral_joint_state(self._left_arm_neutral),
self._l_ik_prev)
def set_right_coords(self,
x,
y,
z,
er=math.pi * -1,
ep=math.pi * 0.5,
ey=math.pi * -1):
self._set_arm_coords(
self._right_iksvc, self._right_arm, x, y, z, er, ep, ey,
self._get_neutral_joint_state(self._right_arm_neutral),
self._r_ik_prev)
############################################################################
def _set_arm_coords(self, iksvc, limb, x, y, z, er, ep, ey, njs, prev):
resp = self._get_ik(iksvc, x, y, z, er, ep, ey, njs)
positions = resp[0]
isValid = resp[1]
self._out_of_range = not isValid
prev.append(positions)
if (len(prev) > self._ik_smooth):
prev.pop(0)
smooth_positions = {}
for joint_name in positions:
smooth_positions[joint_name] = \
sum(map(lambda x: x[joint_name], prev)) / self._ik_smooth
limb.set_joint_positions(smooth_positions)
def _get_ik(self, iksvc, x, y, z, er, ep, ey, njs):
q = quaternion_from_euler(er, ep, ey)
pose = PoseStamped(
header=Header(stamp=rospy.Time.now(), frame_id='base'),
pose=Pose(position=Point(
x=x,
y=y,
z=z,
),
orientation=Quaternion(q[0], q[1], q[2], q[3])),
)
ikreq = SolvePositionIKRequest()
ikreq.pose_stamp.append(pose)
ikreq.seed_angles.append(njs)
iksvc.wait_for_service(1.0)
resp = iksvc(ikreq)
positions = dict(zip(resp.joints[0].name, resp.joints[0].position))
return (positions, resp.isValid[0])
def _get_neutral_joint_state(self, njs):
js = JointState()
js.header = Header(stamp=rospy.Time.now(), frame_id='base')
for j in njs:
js.name.append(j)
js.position.append(njs[j])
return js
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)
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)
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)
class ColorTest ():
def __init__(self):
self.cam ="/cameras/left_hand_camera/image"
self.image_pub=rospy.Publisher("image_topic_2",Image)
self.disp_pub=rospy.Publisher('/robot/xdisplay', Image, latch=True)#Pub to xdisplay, but have to us cv_brige to convert befor pub
self.image_sub=rospy.Subscriber(self.cam,Image,self.image_callback)
self.bridge=CvBridge()
self.left_camera=CameraController("left_hand_camera")
self.left_camera.resolution=(640,400)
self.reset_cameras()
self.left_camera.close()
self.left_camera.open()
########################################################################################
# def color_mask (self,ball_color):
# lower = (86,31,4)
# upper = (220,88,50)
# if ball_color == "blue":
# lower = (86,31,4)
# upper = (220,88,50)
#
# if ball_color == "green":
# lower = (29,86,6)
# upper = (64,255,255)
#
# if ball_color == "red":
# lower = (17,15,100)
# upper = (50,56,200)
# image_hsv = cv2.cvtColor(image_src,cv2.COLOR_BGR2HSV)##################
# image_mask = cv2.inRange(image_hsv,lower,upper)
########################################################################################
def reset_cameras (self):
reset_srv = rospy.ServiceProxy('cameras/reset', std_srvs.srv.Empty)
rospy.wait_for_service('cameras/reset', timeout=10)
reset_srv()
def disp_img(self,image):
msg=CvBridge().cv2_to_imgmsg(image, encoding="mono16")
self.disp_pub.publish(msg)
##################################################################################
def image_callback (self,data):
print "image_callbacking........./n"
#converting the image message to opencv message
try:
#image_cv = self.bridge.imgmsg_to_cv2 (data,"bgr8")
image_cv=np.squeeze(np.array(self.bridge.imgmsg_to_cv2(data, "passthrough")))
except CvBridgeError,e:
print e
if image_cv is not None :
print "get the image_cv...../n "
height, width = image_cv.shape[:2]
#print ("/n{0}",format(height))
#print "----------------------"
#print ("/n{0}",format(width))
#print "----------------------"
#converting the opencv msg to image msg then publish
try:
self.image_pub.publish(self.bridge.cv2_to_imgmsg(image_cv, "bgr8"))
except CvBridgeError as e:
print(e)
print "finished image_callback........"
class Mimic(object):
IKSVC_LEFT_URI = 'ExternalTools/left/PositionKinematicsNode/IKService'
IKSVC_RIGHT_URI = 'ExternalTools/right/PositionKinematicsNode/IKService'
def __init__(self):
self._tf = tf.TransformListener()
self._rs = RobotEnable()
self._left_arm = Limb('left')
self._left_arm.set_joint_position_speed(0.5)
self._right_arm = Limb('right')
self._right_arm.set_joint_position_speed(0.5)
self._left_arm_neutral = None
self._right_arm_neutral = None
self._left_iksvc = rospy.ServiceProxy(
Mimic.IKSVC_LEFT_URI,
SolvePositionIK)
self._right_iksvc = rospy.ServiceProxy(
Mimic.IKSVC_RIGHT_URI,
SolvePositionIK)
self._left_camera = CameraController('left_hand_camera')
self._right_camera = CameraController('right_hand_camera')
self._head_camera = CameraController('head_camera')
self._left_camera.close()
self._right_camera.close()
self._head_camera.close()
self._head_camera.resolution = CameraController.MODES[0]
self._head_camera.open()
self._head_camera_sub = rospy.Subscriber('/cameras/head_camera/image', Image,
self._head_camera_sub_callback)
self._xdisplay_pub = rospy.Publisher('/robot/xdisplay', Image, latch=True)
self._out_of_range = False
self._ik_smooth = 4
self._ik_rate = 200
self._avg_window = 1000
self._r_trans_prev = []
self._l_trans_prev = []
self._r_ik_prev = []
self._l_ik_prev = []
self._joint_update_pub = rospy.Publisher('/robot/joint_state_publish_rate', UInt16)
self._joint_update_pub.publish(self._ik_rate)
self._mimic_timer = None
############################################################################
def start(self):
self._rs.enable()
self._reset()
self._mimic_timer = rospy.Timer(rospy.Duration(1.0 / self._ik_rate), self._mimic_callback)
rate = rospy.Rate(self._avg_window)
while not rospy.is_shutdown():
try:
(r_trans,r_rot) = self._tf.lookupTransform(
'/skeleton/user_1/right_hand',
'/skeleton/user_1/torso',
rospy.Time(0))
(l_trans,l_rot) = self._tf.lookupTransform(
'/skeleton/user_1/left_hand',
'/skeleton/user_1/torso',
rospy.Time(0))
self._l_trans_prev.append(l_trans)
if (len(self._l_trans_prev) > self._avg_window):
self._l_trans_prev.pop(0)
self._r_trans_prev.append(r_trans)
if (len(self._r_trans_prev) > self._avg_window):
self._r_trans_prev.pop(0)
except:
self._out_of_range = True
rate.sleep()
continue
rate.sleep()
def _reset(self):
if self._mimic_timer:
self._mimic_timer.shutdown()
self._left_arm.move_to_neutral()
self._left_arm_neutral = self._left_arm.joint_angles()
self._right_arm.move_to_neutral()
self._right_arm_neutral = self._right_arm.joint_angles()
print(self._left_arm_neutral)
print(self._right_arm_neutral)
def shutdown(self):
self._reset()
############################################################################
def _mimic_callback(self, event):
try:
l_trans = (
sum(map(lambda x: x[0], self._l_trans_prev)) / self._avg_window,
sum(map(lambda x: x[1], self._l_trans_prev)) / self._avg_window,
sum(map(lambda x: x[2], self._l_trans_prev)) / self._avg_window)
r_trans = (
sum(map(lambda x: x[0], self._r_trans_prev)) / self._avg_window,
sum(map(lambda x: x[1], self._r_trans_prev)) / self._avg_window,
sum(map(lambda x: x[2], self._r_trans_prev)) / self._avg_window)
rh_x = clamp(0.65 + (l_trans[2] / 1.5), 0.5, 0.9)
rh_y = clamp(-l_trans[0], -0.8, 0)
rh_z = clamp(-l_trans[1], -0.1, 0.8)
lh_x = clamp(0.65 + (r_trans[2] / 1.5), 0.5, 0.9)
lh_y = clamp(-r_trans[0], 0, 0.8)
lh_z = clamp(-r_trans[1], -0.1, 0.8)
self.set_left_coords(lh_x, lh_y, lh_z)
self.set_right_coords(rh_x, rh_y, rh_z)
except:
return
def _head_camera_sub_callback(self, data):
orig_img = cvbr.imgmsg_to_cv2(data, 'rgb8')
img = cv2.cvtColor(orig_img, cv2.COLOR_RGB2GRAY)
img = cv2.equalizeHist(img)
img = cv2.GaussianBlur(img, (5, 5), 0)
img = cv2.Canny(img, 100, 200)
img = cv2.cvtColor(img, cv2.COLOR_GRAY2RGB)
img = cv2.addWeighted(img, 0.75, orig_img, 0.25, 0)
img = cv2.resize(img, (1024, 768))
if self._out_of_range:
cv2.putText(img, 'Out of range!', (50, 768-50), cv2.FONT_HERSHEY_SIMPLEX, 2, (255, 0, 0), 4)
img = cv2.cvtColor(img, cv2.COLOR_RGB2BGR)
img = cvbr.cv2_to_imgmsg(img)
self._xdisplay_pub.publish(img)
############################################################################
def set_left_coords(self,
x, y, z,
er=math.pi * -1, ep=math.pi * 0.5, ey=math.pi * -1):
self._set_arm_coords(self._left_iksvc, self._left_arm,
x, y, z,
er, ep, ey,
self._get_neutral_joint_state(self._left_arm_neutral),
self._l_ik_prev)
def set_right_coords(self,
x, y, z,
er=math.pi * -1, ep=math.pi * 0.5, ey=math.pi * -1):
self._set_arm_coords(self._right_iksvc, self._right_arm,
x, y, z,
er, ep, ey,
self._get_neutral_joint_state(self._right_arm_neutral),
self._r_ik_prev)
############################################################################
def _set_arm_coords(self, iksvc, limb, x, y, z, er, ep, ey, njs, prev):
resp = self._get_ik(iksvc, x, y, z, er, ep, ey, njs)
positions = resp[0]
isValid = resp[1]
self._out_of_range = not isValid
prev.append(positions)
if (len(prev) > self._ik_smooth):
prev.pop(0)
smooth_positions = {}
for joint_name in positions:
smooth_positions[joint_name] = \
sum(map(lambda x: x[joint_name], prev)) / self._ik_smooth
limb.set_joint_positions(smooth_positions)
def _get_ik(self, iksvc, x, y, z, er, ep, ey, njs):
q = quaternion_from_euler(er, ep, ey)
pose = PoseStamped(
header=Header(stamp=rospy.Time.now(), frame_id='base'),
pose=Pose(
position=Point(
x=x,
y=y,
z=z,
),
orientation=Quaternion(q[0], q[1], q[2], q[3])
),
)
ikreq = SolvePositionIKRequest()
ikreq.pose_stamp.append(pose)
ikreq.seed_angles.append(njs)
iksvc.wait_for_service(1.0)
resp = iksvc(ikreq)
positions = dict(zip(resp.joints[0].name, resp.joints[0].position))
return (positions, resp.isValid[0])
def _get_neutral_joint_state(self, njs):
js = JointState()
js.header = Header(stamp=rospy.Time.now(), frame_id='base')
for j in njs:
js.name.append(j)
js.position.append(njs[j])
return js
class BaxterCamera(object):
LEFT = -1
RIGHT = 1
def __init__(self,
node,
limb=+1,
camera_matrix=None,
camera_extrinsics=None):
self.node = node
self.lns = "right" if limb == BaxterCamera.RIGHT else "left"
self.image = None
self.camera_matrix = camera_matrix
self.camera_frame = None
self.camera_extrinsics = camera_extrinsics
# Camera Control
self.cameracontrol = CameraController(self.lns + '_hand_camera')
self.cameracontrol.resolution = (960, 600)
self.cameracontrol.open()
# Camera Topic
self.cvbridge = CvBridge()
node.createSubscriber('cameras/' + self.lns + '_hand_camera/image',
Image, self._image_callback)
# we create the subscriver only if we are not recieving any explicit camera matrix
if camera_matrix is None:
node.createSubscriber(
'cameras/' + self.lns + '_hand_camera/camera_info', CameraInfo,
self._camera_info_callback)
def getExtrinsics(self):
''' The extrinsics are wrt the hand'''
# we take them only once or we use those that we defined in the init
if self.camera_extrinsics is None:
wait = 50
tf = None
while tf is None and wait > 0:
wait -= 1
try:
tf = self.node.retrieveTransform(
frame_id=self.lns + '_hand_camera',
parent_frame_id=self.lns + '_hand',
time=-1)
except Exception as e:
tf = None
print(e)
self.camera_extrinsics = tf
return self.camera_extrinsics
def camera2gripper(self):
wait = 50
tf = None
while tf is None and wait > 0:
wait -= 1
try:
tf = self.node.retrieveTransform(
frame_id=self.lns + '_hand_camera',
parent_frame_id=self.lns + '_gripper',
time=-1)
except Exception as e:
tf = None
print(e)
self.camera_extrinsics = tf
return tf
def _camera_info_callback(self, msg):
self.camera_matrix = np.array(msg.K).reshape((3, 3))
def _image_callback(self, msg):
try:
self.image = self.cvbridge.imgmsg_to_cv2(msg, "bgr8")
except CvBridgeError as e:
print(e)
def getCameraFrame(self, wait=9999):
self.camera_frame = None
while self.camera_frame is None and wait > 0:
wait -= 1
try:
self.camera_frame = self.node.retrieveTransform(
frame_id=self.lns +
"_hand_camera", # TODO "_hand_camera" !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
parent_frame_id="base",
time=-1)
except Exception as e:
print(e)
return self.camera_frame
def getimage(self):
return self.image
def setting(self,
state="o",
resolution=(960, 600),
exposure=CameraController.CONTROL_AUTO,
gain=CameraController.CONTROL_AUTO):
'''
Camera Exposure: range is 0-100
Camera gain: is 0-79
'''
if state == "o":
self.cameracontrol.open()
elif state == "c":
self.cameracontrol.close()
self.cameracontrol.resolution = resolution
self.cameracontrol.exposure = exposure
self.cameracontrol.gain = gain
def reset(self, pose=None):
self.setting()
def px2world(self, u, v, plane_coefficients, frame_camera=None):
'''
@u,v: pixels coordinates in image plane (wrttop-left corner);
@plane_coefficients: coefficents of the plane where the object lies (wrt the Base reference frame);
@frame_camera: camera reference frame wrt the robot Base reference frame.
'''
if frame_camera is None:
tf_camera_base = self.getCameraFrame()
if tf_camera_base is None:
return None
else:
tf_camera_base = frame_camera
# camera frame wrt base frame
camera_rotation_matrix = transformations.KLDFrameToNumpyRotation(
tf_camera_base)
camera_position_vector = transformations.KLDFrameToNumpyPosition(
tf_camera_base)
# from image point [pixels] to cartasian ray [meters]
image_point = np.array([u, v, 1.0]).reshape(3, 1)
ray = np.matmul(np.linalg.inv(self.camera_matrix), image_point)
ray = ray / np.linalg.norm(ray)
ray = ray.reshape(3)
# intersection cartesian ray and target plane
camera_frame = np.concatenate(
(camera_rotation_matrix, camera_position_vector.reshape(3, 1)),
axis=1)
camera_frame_h = np.concatenate((camera_frame, [[0, 0, 0, 1]]))
# camera_extrinsics = np.array([0.0403169, 0.998361, -0.0406236, 0.04188, # TODO my calibration
# -0.999135, 0.0406977, 0.00859053, 0.013019,
# 0.0102297, 0.0402421, 0.999138, 0.017091,
# 0, 0, 0, 1]).reshape((4, 4))
# camera_frame_h = np.matmul(camera_frame_h, camera_extrinsics) # TODO my calibration
# Transform plane from base frame to camera frame
# https://math.stackexchange.com/questions/2502857/transform-plane-to-another-coordinate-system
plane_coefficients = np.matmul(
np.matrix.transpose(np.linalg.inv(camera_frame_h)),
plane_coefficients)
t = -(plane_coefficients[3]) / (plane_coefficients[0] * ray[0] +
plane_coefficients[1] * ray[1] +
plane_coefficients[2] * ray[2])
x = ray[0] * t
y = ray[1] * t
z = ray[2] * t
plane_point = np.array(
[x, y, z]
) # + np.array([0.105, 0.055, 0]) # BUG offset CAMERA extrinsics @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
# point with respect to the base frame
plane_point_h = np.concatenate((plane_point, [1]))
plane_point_h = np.matmul(camera_frame_h, plane_point_h)
plane_point = plane_point_h[:3].reshape(3)
return plane_point
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
