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 __del__(self):
self.baxter.disable()
for camname in ['head_camera', 'left_hand_camera', 'right_hand_camera']:
try:
cam = CameraController(camname)
cam.close()
except:
pass
def CameraInit():
global left_hand_camera
global right_hand_camera
left_hand_camera = CameraController('left_hand_camera')
left_hand_camera.close()
right_hand_camera = CameraController('right_hand_camera')
right_hand_camera.close()
CameraStart(left_hand_camera)
CameraClose(left_hand_camera)
CameraStart(right_hand_camera)
CameraClose(right_hand_camera)
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'))
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)
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
