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