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 close_camera(camera, *_args, **_kwds):
cam = CameraController(camera)
cam.close()
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
def open_camera(camera, res, *_args, **_kwds):
cam = CameraController(camera)
cam.close()
cam.resolution = res
cam.open()
