def main():
img_publisher = rospy.Publisher(
'raw_image_publisher', Image, queue_size=10
) # Create a publisher that gets passed to the callback so that we can push an image to some subscribers
rospy.init_node('image_pub_node', anonymous=True)
rate = rospy.Rate(10) # Set the publishing rate for the images
camera = intera_interface.Cameras()
camera.start_streaming('head_camera') # Pick the head camera by default
# Create callback function that gets called every time an image is received from the integrated camera
camera.set_callback(
camera_name='head_camera',
callback=pub_image_callback,
callback_args=img_publisher
) # NOTE The image data is pass already (otherwise why have the callback) so we dont pass anything in here
# This method ensures that we shutdown the node in a way that leaves no 'garbage' behind
def clean_shutdown():
rospy.loginfo("Shutting down image_pub_node...")
cv2.destroyAllWindows()
rospy.on_shutdown(clean_shutdown)
rospy.spin()
def __init__(self, camera_name, base_frame, table_height):
"""
Initialize the instance
:param camera_name: The camera name. One of (head_camera, right_hand_camera)
:param base_frame: The frame for the robot base
:param table_height: The table height with respect to base_frame
"""
self.camera_name = camera_name
self.base_frame = base_frame
self.table_height = table_height
self.image_queue = Queue.Queue()
self.pinhole_camera_model = PinholeCameraModel()
self.tf_listener = tf.TransformListener()
camera_info_topic = "/io/internal_camera/{}/camera_info".format(
camera_name)
camera_info = rospy.wait_for_message(camera_info_topic, CameraInfo)
self.pinhole_camera_model.fromCameraInfo(camera_info)
cameras = intera_interface.Cameras()
cameras.set_callback(camera_name,
self.__show_image_callback,
rectify_image=True)
def check():
rp = intera_interface.RobotParams()
valid_cameras = rp.get_camera_names()
camera = "right_hand_camera"
if not valid_cameras:
rp.log_message(("Cannot detect any camera_config"
" parameters on this robot. Exiting."), "ERROR")
return
cameras = intera_interface.Cameras()
if not cameras.verify_camera_exists(camera):
rospy.logerr(
"Could not detect the specified camera, exiting the example.")
return
rospy.loginfo("Opening camera '{0}'...".format(camera))
cameras.start_streaming(camera)
rectify_image = not None
use_canny_edge = None
cameras.set_callback(camera,
show_image_callback,
rectify_image=rectify_image,
callback_args=(True, camera))
rospy.loginfo("Camera_display node running. Ctrl-c to quit")
rospy.sleep(3.0)
cv2.destroyAllWindows()
return decision
def main():
camera_name = "right_hand_camera"
use_canny_edge = 1
rp = intera_interface.RobotParams()
valid_cameras = rp.get_camera_names()
if not valid_cameras:
rp.log_message(("Cannot detect any camera_config"
" parameters on this robot. Exiting."), "ERROR")
return
print("Initializing node... ")
rospy.init_node('camera_display', anonymous=True)
cameras = intera_interface.Cameras()
if not cameras.verify_camera_exists(camera_name):
rospy.logerr(
"Could not detect the specified camera, exiting the example.")
return
rospy.loginfo("Opening camera '{0}'...".format(camera_name))
cameras.start_streaming(camera_name)
cameras.set_cognex_strobe(False)
cameras.set_callback(camera_name,
show_image_callback,
rectify_image=False,
callback_args=None)
def clean_shutdown():
print("Shutting down camera_display node.")
cv2.destroyAllWindows()
rospy.on_shutdown(clean_shutdown)
rospy.loginfo("Camera_display node running. Ctrl-c to quit")
rospy.spin()
def cb_camera(self, data):
if self.VERBOSE: rospy.loginfo('Accessing camera')
rp = intera_interface.RobotParams()
valid_cameras = rp.get_camera_names()
camera = intera_interface.Cameras()
camera.start_streaming('right_hand_camera')
rectify_image = False
camera.set_callback('right_hand_camera', self.display_camera_callback)
def start(self):
rp = intera_interface.RobotParams()
valid_cameras = rp.get_camera_names()
camera = intera_interface.Cameras()
if not camera.verify_camera_exists('right_hand_camera'):
rospy.logerr('Invalid camera name')
camera.start_streaming('right_hand_camera')
rectify_image = False
camera.set_callback('right_hand_camera', self.display_camera_callback)
def camera_setup(exposure=10, gain=60):
_cameras = intera_interface.Cameras()
_name_camera = 'right_hand_camera'
if _cameras.verify_camera_exists(_name_camera):
_cameras.start_streaming(_name_camera)
else:
rospy.logerr(
"Could not detect the specified camera, exiting the example.")
return
_cameras.set_exposure(_name_camera, exposure)
_cameras.set_gain(_name_camera, gain)
def main():
"""Camera Display Example
"""
rp = intera_interface.RobotParams()
valid_cameras = rp.get_camera_names()
if not valid_cameras:
rp.log_message(("Cannot detect any camera_config"
" parameters on this robot. Exiting."), "ERROR")
return
arg_fmt = argparse.RawDescriptionHelpFormatter
parser = argparse.ArgumentParser(formatter_class=arg_fmt,
description=main.__doc__)
parser.add_argument('-c',
'--camera',
type=str,
default="head_camera",
choices=valid_cameras,
help='Setup Camera Name for Camera Display')
parser.add_argument(
'-r',
'--raw',
action='store_true',
help='Specify use of the raw image (unrectified) topic')
parser.add_argument('-e',
'--edge',
action='store_true',
help='Streaming the Canny edge detection image')
args = parser.parse_args()
print("Initializing node... ")
# rospy.init_node('camera_display', anonymous=True)
camera = intera_interface.Cameras()
if not camera.verify_camera_exists(args.camera):
rospy.logerr("Invalid camera name, exiting the example.")
return
camera.start_streaming(args.camera)
rectify_image = not args.raw
use_canny_edge = args.edge
camera.set_callback(args.camera,
show_image_callback,
rectify_image=rectify_image,
callback_args=(use_canny_edge, args.camera))
global pub
pub = rospy.Publisher('sawyer_cam', Image, queue_size=10)
def clean_shutdown():
print("Shutting down camera_display node.")
cv2.destroyAllWindows()
rospy.on_shutdown(clean_shutdown)
rospy.loginfo("Camera_display node running. Ctrl-c to quit")
rospy.spin()
def __init__(self, camera_name, mode, half_res):
print "Initializing ROS Node"
rospy.init_node('sawyer_cameras', anonymous = True)
# Lock for multithreading
self._lock = threading.RLock()
# for image pipe
self._imagestream = None
self._imagestream_endpoints = dict()
self._imagestream_endpoints_lock = threading.RLock()
# get access to camera controls from RSDK
self._camera = intera_interface.Cameras()
# self._camera = intera_interface.CameraController(camera_name)
self._camera_name = camera_name;
# automatically close camera at start
# self._camera.close()
self._camera_open = False
# set constant ImageHeader structure
self.setResolution(mode,half_res)
self._image_header = RR.RobotRaconteurNode.s.NewStructure(
"SawyerCamera_interface.ImageHeader" )
self._image_header.width = int(self._camera.resolution[0])
self._image_header.height = int(self._camera.resolution[1])
self._image_header.step = int(4)
self._camera_intrinsics = None
# set exposure, gain, white_balance to auto
self._camera.exposure = self._camera.CONTROL_AUTO
self._camera.gain = self._camera.CONTROL_AUTO
self._camera.white_balance_red = self._camera.CONTROL_AUTO
self._camera.white_balance_green = self._camera.CONTROL_AUTO
self._camera.white_balance_blue = self._camera.CONTROL_AUTO
# set SawyerImage struct
self._image = RR.RobotRaconteurNode.s.NewStructure("SawyerCamera_interface.SawyerImage")
self._image.width = self._image_header.width
self._image.height = self._image_header.height
self._image.step = self._image_header.step
# Initialize ARtag detection
self._aruco_dict = aruco.Dictionary_get(aruco.DICT_ARUCO_ORIGINAL)
self._arucoParams = aruco.DetectorParameters_create()
self._markerSize = 0.085
def main():
"""Camera Display Example
Cognex Hand Camera Ranges
- exposure: [0.01-100]
- gain: [0-255]
"""
camera_name = "right_hand_camera"
use_canny_edge = 1
rp = intera_interface.RobotParams()
valid_cameras = rp.get_camera_names()
if not valid_cameras:
rp.log_message(("Cannot detect any camera_config"
" parameters on this robot. Exiting."), "ERROR")
return
print("Initializing node... ")
rospy.init_node('camera_display', anonymous=True)
cameras = intera_interface.Cameras()
if not cameras.verify_camera_exists(camera_name):
rospy.logerr(
"Could not detect the specified camera, exiting the example.")
return
#print cameras.get_gain(camera_name)
#print cameras.get_exposure(camera_name)
#cv2.namedWindow('image')
# cv2.createTrackbar('threshold_int','image',0,255,nothing)
# cv2.createTrackbar('threshold','image',1,100,nothing)
#with camera and light exposure 0.2 and light on
cameras.set_gain(camera_name, 5)
cameras.set_exposure(camera_name, 20)
rospy.loginfo("Opening camera '{0}'...".format(camera_name))
cameras.start_streaming(camera_name)
cameras.set_cognex_strobe(False)
cameras.set_callback(camera_name,
show_image_callback,
rectify_image=False,
callback_args=None)
def clean_shutdown():
print("Shutting down camera_display node.")
cv2.destroyAllWindows()
rospy.on_shutdown(clean_shutdown)
rospy.loginfo("Camera_display node running. Ctrl-c to quit")
rospy.spin()
def start_camera():
# right_gripper = robot_gripper.Gripper('right')
# right_gripper.close()
# rp = intera_interface.RobotParams()
# valid_cameras = rp.get_camera_names()
# if not valid_cameras:
# rp.log_message(("Cannot detect any camera_config"
# " parameters on this robot. Exiting."), "ERROR")
# return
print("Initializing node... ")
# rospy.init_node('camera_display', anonymous=True)
cameras = intera_interface.Cameras()
# if not cameras.verify_camera_exists(cam):
# rospy.logerr("Could not detect the specified camera, exiting the example.")
# return
rospy.loginfo("Opening camera '{0}'...".format(cam))
cameras.start_streaming(cam)
def take_single_picture(self):
"""
Take one picture from the specified camera
:returns: The image
"""
with self.image_queue.mutex:
self.image_queue.queue.clear()
cameras = intera_interface.Cameras()
cameras.start_streaming(self.camera_name)
image_data = self.image_queue.get(block=True, timeout=None)
cameras.stop_streaming(self.camera_name)
return image_data
def __init__(self, camera_name): #, mode, half_res):
rp = intera_interface.RobotParams()
valid_cameras = rp.get_camera_names()
if not valid_cameras:
rp.log_message(("Cannot detect any camera_config"
" parameters on this robot. Exiting."), "ERROR")
return
print "Initializing ROS Node"
rospy.init_node('sawyer_cameras', anonymous=True)
# Lock for multithreading
self._lock = threading.RLock()
# for image pipe
self._imagestream = None
self._imagestream_endpoints = dict()
self._imagestream_endpoints_lock = threading.RLock()
# get access to camera controls from Intera SDK
self._camera = intera_interface.Cameras()
if not self._camera.verify_camera_exists(camera_name):
rospy.logerr("Invalid camera name, exiting the example.")
return
self._camera_name = camera_name
# automatically close camera at start
self._camera.stop_streaming(self._camera_name)
self._camera_open = False
self._ar_tracking = False
# set constant ImageHeader structure
self._image_header = RR.RobotRaconteurNode.s.NewStructure(
"SawyerCamera_interface.ImageHeader")
self._camera_intrinsics = None
# set SawyerImage struct
self._image = RR.RobotRaconteurNode.s.NewStructure(
"SawyerCamera_interface.SawyerImage")
# get access to light controls from Intera SDK
self._lights = intera_interface.Lights()
def __init__(self,
limb="right",
hover_distance=0.250,
tip_name="right_hand_camera",
camera_name="right_hand_camera"):
self._limb_name = limb # string
self._tip_name = tip_name # string
self._hover_distance = hover_distance # in meters
self._camera_name = camera_name
self._limb = intera_interface.Limb(limb)
self._gripper = intera_interface.Gripper()
self._rp = intera_interface.RobotParams()
self._light = intera_interface.Lights()
self._cameras = intera_interface.Cameras()
self._camera_orientation = Quaternion(x=0.0, y=1.0, z=0.0, w=0.0)
## Create Kitchen Items
self.kitchen_objects = {}
self.kitchen_objects['ketchup'] = KitchenObject(
'ketchup', 0, 0.450, -0.455, 0.245)
self.kitchen_objects['mayonaise'] = KitchenObject(
'mayonaise', 1, 0.450, -0.553, 0.245)
self.kitchen_objects['barbecue'] = KitchenObject(
'barbecue', 2, 0.362, -0.453, 0.245)
self.kitchen_objects['salad'] = KitchenObject('salad', 3, 0.362,
-0.553, 0.245)
self.check_positions = [
(0.450, -0.455),
(0.450, -0.553),
(0.362, -0.453),
(0.362, -0.553),
]
self.detected_object_data = {
'frame': None,
'obj_id': None,
'obj_pose': None
}
def take_pic(camera='head_camera',
raw=False,
edge=False,
gain=-1,
exposure=-1):
# rospy.init_node('camera_display', anonymous=True)
cameras = intera_interface.Cameras()
if not cameras.verify_camera_exists(camera):
rospy.logerr(
"Could not detect the specified camera, exiting the example.")
return
cameras.start_streaming(camera)
rectify_image = not raw
use_canny_edge = edge
# optionally set gain and exposure parameters
if gain is not None:
if cameras.set_gain(camera, gain):
rospy.loginfo("Gain set to: {0}".format(cameras.get_gain(camera)))
if exposure is not None:
if cameras.set_exposure(camera, exposure):
rospy.loginfo("Exposure set to: {0}".format(
cameras.get_exposure(camera)))
cameras.set_callback(camera,
save_image_callback,
rectify_image=rectify_image,
callback_args=(use_canny_edge, camera))
def clean_shutdown():
print("Shutting down camera_display node.")
cv2.destroyAllWindows()
rospy.on_shutdown(clean_shutdown)
rospy.loginfo("Camera_display node running. Ctrl-c to quit")
cameras.stop_streaming(camera)
print('done')
def main():
rp = intera_interface.RobotParams()
valid_cameras = rp.get_camera_names()
for c in valid_cameras:
print('Try: {}'.format(c))
if not valid_cameras:
rp.log_message(("Unable to detect cameras on robot. Shutting down"),
"ERROR")
return
arg_fmt = argparse.RawDescriptionHelpFormatter # Create a formatter/ parser for the user input
parser = argparse.ArgumentParser(formatter_class=arg_fmt,
description=main.__doc__)
# Add flag for user to say if edge detection should be run or not
parser.add_argument('-c',
'--camera',
type=str,
default='head_camera',
choices=valid_cameras,
help='The default camera name for the camera display')
parser.add_argument('-r',
'--raw',
action='store_true',
help='Should the raw image be used?')
parser.add_argument(
'-e',
'--edge',
action='store_true',
help=
'Should Canny edge detection be streamed? takes argument of type bool.'
)
args = parser.parse_args()
# Initialize a new node if the things above have been completed
print('Initializing new node...')
rospy.init_node('camera_display', anonymous=True) # Initializes a new node
camera = intera_interface.Cameras()
if not camera.verify_camera_exists(args.camera):
rospy.logerr("Invalid camera name. Shutting down.")
return
# web_cam = cv2.VideoCapture(0)
camera.start_streaming(args.camera) # Get input form the selected camera
rectify_image = not args.raw
use_canny_edge = args.edge
camera.set_callback(
args.camera,
show_image_callback,
rectify_image=rectify_image,
callback_args=(use_canny_edge,
args.camera)) # Send args to callback function
# Function that performs a clean shutdown
def clean_shutdown():
print("Shutting down camera_display node")
cv2.destroyAllWindows()
rospy.on_shutdown(
clean_shutdown
) # Call the clean_shutdown() function when rospy shuts off
rospy.loginfo("camera_display node is running. Use ctrl+c to quit")
rospy.spin(
) # Keep spinning, "hey python, you can't exit until this node has been shutdown"
def __init__(self):
print("Initialized camera ")
self.bridge = CvBridge()
self.camera = intera_interface.Cameras()
def __init__(self):
self.gamestate = np.zeros((3,3))
self.once = False
"""Camera Display
Hand Camera Ranges
- exposure: [0.01-100]
- gain: [0-255]
Head Camera Ranges:
- exposure: [0-100], -1 for auto-exposure
- gain: [0-79], -1 for auto-gain
"""
rp = intera_interface.RobotParams()
valid_cameras = rp.get_camera_names()
if not valid_cameras:
rp.log_message(("Cannot detect any camera_config"
" parameters on this robot. Exiting."), "ERROR")
return
arg_fmt = argparse.RawDescriptionHelpFormatter
parser = argparse.ArgumentParser(formatter_class=arg_fmt)
parser.add_argument(
'-c', '--camera', type=str, default="right_hand_camera",
choices=valid_cameras, help='Setup Camera Name for Camera Display')
parser.add_argument(
'-r', '--raw', action='store_true',
help='Specify use of the raw image (unrectified) topic')
parser.add_argument(
'-e', '--edge', action='store_true',
help='Streaming the Canny edge detection image')
parser.add_argument(
'-g', '--gain', type=int, default=15,
help='Set gain for camera (-1 = auto)')
parser.add_argument(
'-x', '--exposure', type=float, default=5,
help='Set exposure for camera (-1 = auto)')
args = parser.parse_args(rospy.myargv()[1:])
print("Initializing node... ")
#rospy.init_node('camera_display', anonymous=True)
cameras = intera_interface.Cameras()
if not cameras.verify_camera_exists(args.camera):
rospy.logerr("Could not detect the specified camera, exiting.")
return
rospy.loginfo("Opening camera '{0}'...".format(args.camera))
rectify_image = not args.raw
use_canny_edge = args.edge
# optionally set gain and exposure parameters
if args.gain is not None:
if cameras.set_gain(args.camera, args.gain):
rospy.loginfo("Gain set to: {0}".format(cameras.get_gain(args.camera)))
if args.exposure is not None:
if cameras.set_exposure(args.camera, args.exposure):
rospy.loginfo("Exposure set to: {0}".format(cameras.get_exposure(args.camera)))
self.cameras = cameras
self.argsCamera = args.camera
cameras.set_callback(args.camera, self.get_game_state,
rectify_image=rectify_image, callback_args=(use_canny_edge, args.camera, cameras))
cameras.stop_streaming(args.camera)
return
def main():
while not rospy.core.is_shutdown():
rospy.init_node('camera_display', anonymous=True)
cameras = intera_interface.Cameras()
cameras.start_streaming('right_hand_camera')
def __init__(self):
rospy.init_node("face_follower_sawyer")
# Set the shutdown function
rospy.on_shutdown(self.shutdown)
self.head_camera = intera_interface.Cameras()
self.face_cascade = cv2.CascadeClassifier('')
# The tate of updating the robot's motion
self.rate = rospy.get_param("~rate", 10)
r = rospy.Rate(self.rate)
# The maximum rotation speed (speed range 0-100)
self.max_speed = rospy.get_param("~max_speed", 1)
# The minimum rotation speed (speed range 0-100)
self.min_speed = rospy.get_param("~min_speed", 0.05)
# Use PD Algorithm to control the robot
self.dist_gain = rospy.get_param("~dist_gain", 2.0)
self.speed_Kp = rospy.get_param("~speed_Kp", 2.0)
self.speed_Kd = rospy.get_param("~speed_Kd", 0)
# Only do some action if the displacement exceeds the threshold
# This can reduce shaking (in percentage format)
self.x_threshold = rospy.get_param("~x_threshold", 0.01)
# Intialize the head movement command
self.head_ = head.Head()
self.pan_distance_ = 0
self.pan_speed_ = 0.1
self.pre_pan_speed_ = 0
rospy.loginfo("Turning the head to the exactly middle...")
self.head_.set_pan(0, 0.1, 10.0)
# Get the image width and height from the camera_info topic
self.image_width = 0
self.image_height = 0
# Set flag to indicate when the ROI stops updating
self.target_visible = False
# Wait for the camera_info topic to become available
rospy.loginfo("Waiting for camera_info topic...")
rospy.wait_for_message("camera_info", CameraInfo)
# Subscribe the camera_info topic to get the image width and height
rospy.Subscriber("camera_info", CameraInfo, self.get_camera_info)
self.head_camera.start_streaming("head_camera", self.get_camera_frame)
# Wait until we actually have the camera data
while self.image_width == 0 or self.image_height == 0:
rospy.sleep(1)
# Subscribe to the ROI topic and set the callback to update the robot's motion
#rospy.Subscriber('roi', RegionOfInterest, self.PD_cal)
# Wait until ROI is detected
#rospy.wait_for_message('roi', RegionOfInterest)
#rospy.loginfo("ROI messages detected. Starting follower...")
# Begin the following loop
while not rospy.is_shutdown():
# If the target is not visible, stop the robot
if not self.target_visible:
rospy.loginfo("No visible target!")
else:
# Reset the flag to False by default
self.target_visible = False
# Output data for debug
rospy.loginfo(
"Following the targrt! Moving head to location %f in speed %d",
self.pan_distance_, self.pan_speed_)
self.head_.set_pan(self.pan_distance_, self.pan_speed_, 0.1)
# Sleep for 1/self.rate seconds
r.sleep()
def main(isMain=False):
"""Camera Display Example
Cognex Hand Camera Ranges
- exposure: [0.01-100]
- gain: [0-255]
Head Camera Ranges:
- exposure: [0-100], -1 for auto-exposure
- gain: [0-79], -1 for auto-gain
"""
global cv_image
rp = intera_interface.RobotParams()
valid_cameras = rp.get_camera_names()
if not valid_cameras:
rp.log_message(("Cannot detect any camera_config"
" parameters on this robot. Exiting."), "ERROR")
return
arg_fmt = argparse.RawDescriptionHelpFormatter
parser = argparse.ArgumentParser(formatter_class=arg_fmt, description=main.__doc__)
parser.add_argument(
'-c', '--camera', type=str, default="right_hand_camera",
choices=valid_cameras, help='Setup Camera Name for Camera Display')
parser.add_argument(
'-r', '--raw', action='store_true',
help='Specify use of the raw image (unrectified) topic')
parser.add_argument(
'-e', '--edge', action='store_true',
help='Streaming the Canny edge detection image')
parser.add_argument(
'-g', '--gain', type=int,
help='Set gain for camera (-1 = auto)')
parser.add_argument(
'-x', '--exposure', type=float,
help='Set exposure for camera (-1 = auto)')
args = parser.parse_args(rospy.myargv()[1:])
if isMain==True:
print("Initializing node... ")
rospy.init_node('camera_display', anonymous=True)
cameras = intera_interface.Cameras()
if not cameras.verify_camera_exists(args.camera):
rospy.logerr("Could not detect the specified camera, exiting the example.")
return
rospy.loginfo("Opening camera '{0}'...".format(args.camera))
cameras.start_streaming(args.camera)
rectify_image = not args.raw
use_canny_edge = args.edge
bridge = CvBridge()
# try:
# cv_image = bridge.imgmsg_to_cv2(img_data, "bgr8")
# except CvBridgeError as err:
# rospy.logerr(err)
# return
# cv_win_name = 'name'
# cv2.namedWindow(cv_win_name, 0)
# # refresh the image on the screen
# cv2.imshow(cv_win_name, cv_image)
# cv2.waitkey(-1)
# cv2.destroyAllWindows()
# cv2.imwrite("hand_image.jpg", cv_image)
# rospy.sleep(99999)
# cv2.waitKey(99999999)
cameras.set_callback(args.camera, show_image_callback, rectify_image=rectify_image, callback_args=(use_canny_edge, args.camera))
# show_image_callback(args.camera, (use_canny_edge,"win_name"))
print("ARGS rectify TYPE", type(rectify_image))
# show_image_callback(rectify_image=rectify_image, callback_args=(use_canny_edge, args.camera))
# optionally set gain and exposure parameters
if args.gain is not None:
if cameras.set_gain(args.camera, args.gain):
rospy.loginfo("Gain set to: {0}".format(cameras.get_gain(args.camera)))
if args.exposure is not None:
if cameras.set_exposure(args.camera, args.exposure):
# rospy.loginfo("Exposure set to: {0}".format(cameras.get_exposure(args.camera)))
rospy.loginfo("Exposure set to: {0}".format(cameras.get_exposure(args.camera)))
rospy.on_shutdown(clean_shutdown)
rospy.loginfo("Camera_display node running. Ctrl-c to quit")
def main():
"""Camera Display Example
Cognex Hand Camera Ranges
- exposure: [0.01-100]
- gain: [0-255]
Head Camera Ranges:
- exposure: [0-100], -1 for auto-exposure
- gain: [0-79], -1 for auto-gain
"""
#Read the parameters called for the execution of the script
rp = intera_interface.RobotParams()
valid_cameras = rp.get_camera_names()
arg_fmt = argparse.RawDescriptionHelpFormatter
parser = argparse.ArgumentParser(formatter_class=arg_fmt,
description=main.__doc__)
parser.add_argument('-c',
'--camera',
type=str,
default="head_camera",
choices=valid_cameras,
help='Setup Camera Name for recording')
parser.add_argument(
'-r',
'--raw',
action='store_true',
help='Specify use of the raw image (unrectified) topic')
parser.add_argument('-e',
'--edge',
action='store_true',
help='Streaming the Canny edge detection image')
parser.add_argument('-g',
'--gain',
type=int,
help='Set gain for camera (-1 = auto)')
parser.add_argument('-x',
'--exposure',
type=float,
help='Set exposure for camera (-1 = auto)')
parser.add_argument('-f',
'--filename',
type=str,
default="Sawyer_recording.avi",
help='Setup filename for recording')
args = parser.parse_args(rospy.myargv()[1:])
#######################
#Create the video writer
width = 1280
height = 800
if args.camera == "right_hand_camera":
width = 752
height = 480
if args.filename == "Sawyer_recording.avi":
print(
"\n\nUsing the default filename ! You might erase the previous default recording this way. Do you wish to continue ?\n"
)
print("Ctrl-c to quit, ENTER to continue...\n\n")
raw_input()
out = cv2.VideoWriter(args.filename,
cv2.VideoWriter_fourcc('M', 'J', 'P', 'G'), 24,
(width, height))
if not out.isOpened():
print("Video writer not open !\n")
print("Shutdown...\n\n")
return
########################
#Setting the node and starting the communication with the camera
print("Initializing node... ")
rospy.init_node('recording_camera', anonymous=True)
cameras = intera_interface.Cameras()
if not cameras.verify_camera_exists(args.camera):
rospy.logerr(
"Could not detect the specified camera, exiting the example.")
return
rospy.loginfo("Opening camera '{0}'...".format(args.camera))
cameras.start_streaming(args.camera)
rectify_image = not args.raw
use_canny_edge = args.edge
cameras.set_callback(args.camera,
record_callback,
rectify_image=rectify_image,
callback_args=(use_canny_edge, out))
# optionally set gain and exposure parameters
if args.gain is not None:
if cameras.set_gain(args.camera, args.gain):
rospy.loginfo("Gain set to: {0}".format(
cameras.get_gain(args.camera)))
if args.exposure is not None:
if cameras.set_exposure(args.camera, args.exposure):
rospy.loginfo("Exposure set to: {0}".format(
cameras.get_exposure(args.camera)))
#Clean shutdown function
def clean_shutdown():
print("Shutting down recording node.")
out.release()
rospy.on_shutdown(clean_shutdown)
rospy.loginfo("Recording node running. Ctrl-c to quit")
rospy.spin()
# Define the position coordinates that will be used
target_1 = [0.481411121842, 0.520207404335, 0.0229609522373]
target_2 = [0.488816426572, 0.221152456584, 0.0219645763764]
pick_location = [0.660254242183, -0.141582855917, -0.0605712484198]
# Display information on the terminal with the time
rospy.loginfo("Robot Initialising")
# Get the robot in neutral position
neutral_position()
rospy.loginfo("Initialising Camera")
# Specify which robot camera will be used
camera = 'right_hand_camera'
cameras = intera_interface.Cameras()
rospy.loginfo("Starting up Camera")
use_canny_edge = True
rate.sleep()
# Read the command list to check word used
for n in range(len(command)):
if command[n][0] == "HUMAN":
# Start in neutral position
neutral_position()
start = False
# Start up the camera
cameras.start_streaming(camera)
def main():
"""Camera Display Example
Cognex Hand Camera Ranges
- exposure: [0.01-100]
- gain: [0-255]
Head Camera Ranges:
- exposure: [0-100], -1 for auto-exposure
- gain: [0-79], -1 for auto-gain
"""
rp = intera_interface.RobotParams()
valid_cameras = rp.get_camera_names()
if not valid_cameras:
rp.log_message(("Cannot detect any camera_config"
" parameters on this robot. Exiting."), "ERROR")
return
arg_fmt = argparse.RawDescriptionHelpFormatter
parser = argparse.ArgumentParser(formatter_class=arg_fmt,
description=main.__doc__)
parser.add_argument(
'-c', '--camera', type=str, default="head_camera",
choices=valid_cameras, help='Setup Camera Name for Camera Display')
parser.add_argument(
'-r', '--raw', action='store_true',
help='Specify use of the raw image (unrectified) topic')
parser.add_argument(
'-e', '--edge', action='store_true',
help='Streaming the Canny edge detection image')
parser.add_argument(
'-g', '--gain', type=int,
help='Set gain for camera (-1 = auto)')
parser.add_argument(
'-x', '--exposure', type=float,
help='Set exposure for camera (-1 = auto)')
args = parser.parse_args(rospy.myargv()[1:])
print("Initializing node... ")
rospy.init_node('camera_display', anonymous=True)
cameras = intera_interface.Cameras()
if not cameras.verify_camera_exists(args.camera):
rospy.logerr("Could not detect the specified camera, exiting the example.")
return
rospy.loginfo("Opening camera '{0}'...".format(args.camera))
cameras.start_streaming(args.camera)
rectify_image = not args.raw
use_canny_edge = args.edge
cameras.set_callback(args.camera, show_image_callback,
rectify_image=rectify_image, callback_args=(use_canny_edge, args.camera))
# optionally set gain and exposure parameters
if args.gain is not None:
if cameras.set_gain(args.camera, args.gain):
rospy.loginfo("Gain set to: {0}".format(cameras.get_gain(args.camera)))
if args.exposure is not None:
if cameras.set_exposure(args.camera, args.exposure):
rospy.loginfo("Exposure set to: {0}".format(cameras.get_exposure(args.camera)))
def clean_shutdown():
print("Shutting down camera_display node.")
cv2.destroyAllWindows()
rospy.on_shutdown(clean_shutdown)
rospy.loginfo("Camera_display node running. Ctrl-c to quit")
rospy.spin()
def main():
rospy.init_node('camera_display')
cameras = intera_interface.Cameras()
cameras.start_streaming("right_hand_camera")
