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 open_cam(camera, res): # Check if valid resolution if not any((res[0] == r[0] and res[1] == r[1]) for r in CameraController.MODES): rospy.logerr("Invalid resolution provided.") # Open camera cam = CameraController(camera) # Create camera object cam.resolution = res # Set resolution cam.open() # open
def main(): ####cam = CameraController('head_camera') ####cam.close() reset_cameras() cam = CameraController('right_hand_camera') res = (1280, 800) cam.resolution = res cam.open() #print 'gain was ',cam.gain cam.gain = 0
def start_cameras(self, camera1='left_hand_camera', camera2='right_hand_camera', resolution=(1280, 800)): """ Start Baxter cameras. Baxter accepts 2 and only 2 opened cameras due to USB bus restrictions. https://github.com/RethinkRobotics/sdk-docs/wiki/Using-the-Cameras :param camera1: camera name 1 :param camera2: camera name 2 :param resolution: tuple among 1280x800, 960x600, 640x400, 480x300, 384x240, 320x200 """ for camera in [camera1, camera2]: short_name = camera.split('_')[0] if self.cameras_enabled[short_name]: rospy.loginfo("Opening camera {} in {}x{} pixels...".format(camera, resolution[0], resolution[1])) controller = CameraController(camera) controller.resolution = resolution controller.open() rospy.loginfo("Cameras opened!")
def open_camera(self, camera, x_res, y_res): 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 - open_camera - Invalid camera") # set camera parameters cam.resolution = (int(x_res), int(y_res)) cam.exposure = -1 # range, 0-100 auto = -1 cam.gain = -1 # range, 0-79 auto = -1 cam.white_balance_blue = -1 # range 0-4095, auto = -1 cam.white_balance_green = -1 # range 0-4095, auto = -1 cam.white_balance_red = -1 # range 0-4095, auto = -1 cam.open()
def set_cameras(res): ls = rospy.ServiceProxy('cameras/list', ListCameras) rospy.wait_for_service('cameras/list', timeout=10) cameraList = ls() if cameraList.cameras.count('right_hand_camera') == 0: c = CameraController(cameraList.cameras[0]) c.close else: cRight = CameraController('right_hand_camera') cRight.open() cRight.resolution = res cRight.gain = 20 cRight.exposure = 30 print "Right Camera Open: RES = " + str(cRight.resolution) return None
def start_cameras(self, camera1='left_hand_camera', camera2='right_hand_camera', resolution=(1280, 800)): """ Start Baxter cameras. Baxter accepts 2 and only 2 opened cameras due to USB bus restrictions. https://github.com/RethinkRobotics/sdk-docs/wiki/Using-the-Cameras :param camera1: camera name 1 :param camera2: camera name 2 :param resolution: tuple among 1280x800, 960x600, 640x400, 480x300, 384x240, 320x200 """ for camera in [camera1, camera2]: short_name = camera.split('_')[0] if self.cameras_enabled[short_name]: rospy.loginfo("Opening camera {} in {}x{} pixels...".format( camera, resolution[0], resolution[1])) controller = CameraController(camera) controller.resolution = resolution controller.open() rospy.loginfo("Cameras opened!")
def open_camera(camera, res, *_args, **_kwds): cam = CameraController(camera) cam.close() cam.resolution = res cam.open()
def open_camera(camera, res, *_args, **_kwds): cam = CameraController(camera) 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 open_cam(camera, res): if not any((res[0] == r[0] and res[1] == r[1]) for r in CameraController.MODES): rospy.logerr("Invalid resolution provided.") cam = CameraController(camera) cam.resolution = res cam.open()