def __init__(self): #Create an instance variable to store the last image received self.lastImage = None #Initialize the node rospy.init_node('right_hand_camera_srv') left_cam = CameraController("left_hand_camera") left_cam.close() right_cam = CameraController("right_hand_camera") right_cam.resolution = (1280, 800) right_cam.open() # cameraController = baxter_interface.CameraController("right_hand_camera") # # cameraController.close() # cameraController.resolution= (1280,800) # cameraController.open() #Subscribe to the image topic rospy.Subscriber("/cameras/right_hand_camera/image", Image, self.imgReceived) #Create the service rospy.Service('last_image', ImageSrv, self.getLastImage) print "created service"
def close_camera(self, camera): if camera == "left": cam = CameraController("left_hand_camera") elif camera == "right": cam = CameraController("right_hand_camera") elif camera == "head": cam = CameraController("head_camera") else: sys.exit("ERROR - close_camera - Invalid camera") cam.close()
def close_camera(camera, *_args, **_kwds): cam = CameraController(camera) cam.close()
def open_camera(camera, res, *_args, **_kwds): cam = CameraController(camera) cam.close() cam.resolution = res cam.open()
def main(): rospy.init_node('multiple_camera_calibration') print "===== Initialized multiple camera calibration" # print "Starting camera calibration" if len(sys.argv)>8: c1_output_frame = sys.argv[1] c1_image_topic = sys.argv[2] c1_info_topic = sys.argv[3] c2_output_frame = sys.argv[4] c2_image_topic = sys.argv[5] c2_info_topic = sys.argv[6] marker_frame = sys.argv[7] #such as "ar_marker_0" marker_size = sys.argv[8] else: print "=====obj_dregistered node requires 8 arguments: \ncamera1: output_frame, image_topic, info_topic \ncamera2: output_frame, image_topic, info_topic \nmarker_frame, marker_size" c1 = Calibrator(c1_output_frame,c1_image_topic,c1_info_topic,marker_frame) subprocess.Popen(["roslaunch", "liam_neeson","multiple_camera_calibration_aux.launch","c1:=true","c1_image_topic:="+c1_image_topic,"c1_info_topic:="+c1_info_topic,"c1_output_frame:="+c1_output_frame, "marker_frame:="+marker_frame, "marker_size:="+marker_size]) ##should pass in other arguments as well #be sure openni is launched # os.system("rosrun dynamic_reconfigure dynparam set /camera/driver image_mode 4") rospy.sleep(1) raw_input("===== Ready to search for AR code from first camera. Press Enter to continue.") while (not c1.set_transform() and not rospy.is_shutdown()): raw_input("===== AR code transform not found. Press Enter to try again.") raw_input("\n===== Press Enter to continue.\n") os.system('rosnode kill c1_ar_track_alvar') # os.system("rosrun dynamic_reconfigure dynparam set /camera/driver image_mode 2") #set back to default try: cam_control_right = CameraController('right_hand_camera') cam_control_right.close() except: pass cam_control_left = CameraController('left_hand_camera') res = (1280, 800) #apparently this resolution is necessary to properly derive AR transform cam_control_left.resolution = res print "Opening left-hand camera." cam_control_left.open() c2 = Calibrator(c2_output_frame,c2_image_topic,c2_info_topic,marker_frame) subprocess.Popen(["roslaunch", "liam_neeson","multiple_camera_calibration_aux.launch","c2:=true","c2_image_topic:="+c2_image_topic,"c2_info_topic:="+c2_info_topic,"c2_output_frame:="+c2_output_frame, "marker_frame:="+marker_frame, "marker_size:="+marker_size]) ##should pass in other arguments as well rospy.sleep(1) raw_input("===== Ready to search for AR code from second camera. Press Enter to continue.") while (not c2.set_transform() and not rospy.is_shutdown()): raw_input("===== AR code transform not found. Press Enter to try again.") # os.system('rosnode kill c2_ar_track_alvar') rospy.sleep(1) raw_input("===== Both AR code transforms found. Press Enter to continue.") # tf2_ros.StaticTransformBroadcaster() # ASSUMING c2 is Baxter's left hand camera frame print "Waiting for transform from Baxter camera to base frame" left_in_base = get_trans("base","left_hand_camera") while (left_in_base == [] and not rospy.is_shutdown()): print "Transform not found..." left_in_base = get_trans("base","left_hand_camera") print "Transform found!" left_in_base = tf.transformations.inverse_matrix(left_in_base) print "Base to Left: ", tf.transformations.decompose_matrix(left_in_base)[3] ar_in_base = np.dot(c2.transform,left_in_base) print "Base to AR: ", tf.transformations.decompose_matrix(ar_in_base)[3] #since the frame doesn't seem right, we need to roll -90deg, yaw -90deg frame_adjustment=tf.transformations.compose_matrix(angles=[-1.57,0,-1.57], translate=[0,0,0]) c1.transform = np.dot(c1.transform,frame_adjustment) cam_in_ar = tf.transformations.inverse_matrix(c1.transform) print "AR to Camera: ", tf.transformations.decompose_matrix(cam_in_ar)[3] cam_in_base = np.dot(cam_in_ar, ar_in_base) print "Base to Camera: ",tf.transformations.decompose_matrix(cam_in_base)[3] cam_in_base_decomp = tf.transformations.decompose_matrix(cam_in_base) euler = cam_in_base_decomp[2] pos = cam_in_base_decomp[3] print "Euler: ", euler, " Position: ", pos quat = tf.transformations.quaternion_from_euler(euler[0],euler[1],euler[2]) #Let's publish the static transform! broadcaster = tf.TransformBroadcaster() # static_transformStamped = TransformStamped() # # static_transformStamped.header.stamp = rospy.Time.now() # static_transformStamped.header.frame_id = "base" # static_transformStamped.child_frame_id = "camera_link1" ###########SUBJECT TO CHANGE # static_transformStamped.transform.translation.x = pos[0] # static_transformStamped.transform.translation.y = pos[1] # static_transformStamped.transform.translation.z = pos[2] # static_transformStamped.transform.rotation.x = quat[0] # static_transformStamped.transform.rotation.y = quat[1] # static_transformStamped.transform.rotation.z = quat[2] # static_transformStamped.transform.rotation.w = quat[3] rate = rospy.Rate(20) while not rospy.is_shutdown(): broadcaster.sendTransform(pos,quat,rospy.Time.now(),"base","camera_link1") # static_transformStamped.header.stamp = rospy.Time.now() # broadcaster.sendTransform(static_transformStamped) rate.sleep()
def close_cam(camera): cam = CameraController(camera) # Create camera object cam.close() # close