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 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 __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 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 main():
camname = 'right_hand_camera'
####cam = CameraController('head_camera')
####cam.close()
reset_cameras()
cam = CameraController(camname)
print 'camera ', camname
print 'gain ', cam.gain
print 'res ', cam.resolution
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 main(camera_name):
camera = CameraController(camera_name)
print('Resolution:', camera.resolution)
print('Frames per second:', camera.fps)
print('Exposure:', camera.exposure)
print('Gain:', camera.gain)
print('White balance:')
print(' Red:', camera.white_balance_red)
print(' Green:', camera.white_balance_green)
print(' Blue:', camera.white_balance_blue)
print('Flip:', camera.flip)
print('Mirror:', camera.mirror)
print('Binning/half resolution:', camera.half_resolution)
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 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 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 close_camera(camera, *_args, **_kwds):
cam = CameraController(camera)
cam.close()
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()
def close_cam(camera):
cam = CameraController(camera) # Create camera object
cam.close() # close
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()
