def test_monocular(self):
ci = sensor_msgs.msg.CameraInfo()
ci.width = 640
ci.height = 480
print ci
cam = PinholeCameraModel()
cam.fromCameraInfo(ci)
#print cam.rectifyPoint((0, 0))
print cam.project3dToPixel((0,0,0))
class ConverterToUV():
def __init__(self, info_topic):
self.camera_model = PinholeCameraModel()
rospy.Subscriber(info_topic, CameraInfo, self.update_model)
def update_model(self, info):
self.camera_model.fromCameraInfo(info)
def handle_img_msg(self, img_msg):
now = rospy.get_rostime()
img = None
bridge = cv_bridge.CvBridge()
try:
img = bridge.imgmsg_to_cv2(img_msg, 'bgr8')
except cv_bridge.CvBridgeError as e:
rospy.logerr( 'image message to cv conversion failed :' )
rospy.logerr( e )
print( e )
return
self.frame_index = self.timestamp_map[img_msg.header.stamp.to_nsec()]
f = self.frame_map[self.frame_index][0]
obs_centroid = np.array(f.trans) + np.array(self.offset)
R = tf.transformations.quaternion_matrix(self.rotation_offset)
rotated_centroid = R.dot(list(obs_centroid)+[1])
obs_centroid = rotated_centroid[:3]
#self.orient = list(f.rotq)
self.marker.header.stamp = now
self.marker.pose.position = Point(*list(obs_centroid))
self.marker.pose.orientation = Quaternion(*self.orient)
self.obs_marker.pose.position = Point(*list(obs_centroid))
self.obs_marker.pose.orientation = Quaternion(*self.orient)
self.add_bbox_lidar()
md = self.metadata
dims = np.array([md['l'], md['w'], md['h']])
outputName = '/image_bbox'
if obs_centroid is None:
rospy.loginfo("Couldn't find obstacle centroid")
imgOutput.publish(bridge.cv2_to_imgmsg(img, 'bgr8'))
return
# print centroid info
rospy.loginfo(str(obs_centroid))
# case when obstacle is not in camera frame
if obs_centroid[0]<2.5 :
self.imgOutput.publish(bridge.cv2_to_imgmsg(img, 'bgr8'))
return
# get bbox
R = tf.transformations.quaternion_matrix(self.orient)
corners = [0.5*np.array([i,j,k])*dims for i in [-1,1]
for j in [-1,1] for k in [-1,1]]
corners = [obs_centroid + R.dot(list(c)+[1])[:3] for c in corners]
projected_pts = []
cameraModel = PinholeCameraModel()
cam_info = load_cam_info(self.calib_file)
cameraModel.fromCameraInfo(cam_info)
projected_pts = [cameraModel.project3dToPixel(list(pt)+[1]) for pt in corners]
projected_pts = np.array(projected_pts)
center = np.mean(projected_pts, axis=0)
out_img = drawBbox(img, projected_pts)
self.imgOutput.publish(bridge.cv2_to_imgmsg(out_img, 'bgr8'))
def __init__(self):
cv2.namedWindow('AR_TRACKER')
self.bridge = cv_bridge.CvBridge()
self.cam_info_sub = rospy.Subscriber('camera/rgb/camera_info',
CameraInfo, self.info_cb)
# self.pose_sub = rospy.Subscriber('pose', Pose, self.pose_cb)
# self.img_sub = rospy.Subscriber('camera/rgb/image_raw/compressed', CompressedImage, self.img_cb, queue_size = 1, buff_size=2**24)
self.img_sub = rospy.Subscriber('usb_cam/image_raw/compressed',
CompressedImage,
self.img_cb,
queue_size=1,
buff_size=2**24)
self.ar_sub = rospy.Subscriber('ar_pose_marker', AlvarMarkers,
self.ar_cb)
self.pub = rospy.Publisher('ar_location', String, queue_size=5)
# self.control_pub = rospy.Publisher('ar_control', Bool, queue_size=5)
# self.stop_linear = False
# self.straight_ahead = False
# self.init = [False, False]
# self.initial_theta = None
# self.theta = None
# self.pos = False
self.count = 0
self.axis = np.float32([[0.1, 0, 0], [0, 0.1, 0], [0, 0, -0.1],
[0, 0, 0]]).reshape(-1, 3)
self.objp = np.zeros((6 * 8, 3), np.float32)
self.objp[:, :2] = np.mgrid[0:8, 0:6].T.reshape(-1, 2)
# self.criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
self.K = None
self.D = None
self.markers = []
self.cam_info = None
self.pinhole = PinholeCameraModel()
def cb_camera_info(self, msg):
# unsubscribe from camera_info
self.loginfo(
"Camera info message received. Unsubscribing from camera_info topic."
)
# noinspection PyBroadException
try:
self.sub_camera_info.shutdown()
except BaseException:
pass
# ---
H, W = msg.height, msg.width
# create new camera info
self.camera_model = PinholeCameraModel()
self.camera_model.fromCameraInfo(msg)
# find optimal rectified pinhole camera
with self.profiler("/cb/camera_info/get_optimal_new_camera_matrix"):
rect_camera_K, _ = cv2.getOptimalNewCameraMatrix(
self.camera_model.K, self.camera_model.D, (W, H),
self.alpha.value)
# create rectification map
with self.profiler("/cb/camera_info/init_undistort_rectify_map"):
self.mapx, self.mapy = cv2.initUndistortRectifyMap(
self.camera_model.K, self.camera_model.D, None, rect_camera_K,
(W, H), cv2.CV_32FC1)
# pack rectified camera info into a CameraInfo message
self.rect_camera_info = CameraInfo(
width=W,
height=H,
K=rect_camera_K.flatten().tolist(),
R=np.eye(3).flatten().tolist(),
P=np.zeros((3, 4)).flatten().tolist(),
)
def __init__(self):
"""
Constructor.
"""
# Publishing rate
self.rate = rospy.Rate(10)
# Set camera info boolean flag
self.cameraInfoInitialised = False
# PinholeCameraModel object
self.pcm = PinholeCameraModel()
# Tf listener
self.tf_listener = tf.TransformListener()
# Constant path
self.path = str(Path(os.path.dirname(os.path.abspath(__file__))).parents[0])
# Poses, distance and marker publishers
self.poses_pub = rospy.Publisher('poses', Poses, queue_size=5)
self.distance_pub = rospy.Publisher('distances', Distances, queue_size=5)
self.markers_pub = rospy.Publisher('visualization_marker_array', MarkerArray, queue_size=5)
# Camera info subscriber
self.info_sub = rospy.Subscriber("/xtion/rgb/camera_info", CameraInfo, self.setCameraInfo)
def __init__(self):
# defaults overwritten by param
self.robot_name = "shamrock"
self.rectified_input = False
# read calibrations
self.extrinsics_filename = (get_ros_package_path('duckietown') +
"/config/baseline/calibration/camera_extrinsic/" +
self.robot_name + ".yaml")
if not os.path.isfile(extrinsics_filename):
logger.warn("no robot specific extrinsic calibration, trying default")
alternate_extrinsics_filename = (get_ros_package_path('duckietown') +
"/config/baseline/calibration/camera_extrinsic/default.yaml")
if not os.path.isfile(alternate_extrinsics_filename):
logger.warn("can't find default either, something's wrong")
else:
self.H = self.load_homography(alternate_extrinsics_filename)
else:
self.H = self.load_homography(self.extrinsics_filename)
self.H_inv = np.linalg.inv(self.H)
intrinsics_filename = (get_ros_package_path('duckietown') + "/config/baseline/calibration/camera_intrinsics/" + self.robot_name + ".yaml")
if not os.path.isfile(intrinsics_filename):
logger.warn("no robot specific calibration, trying default")
intrinsics_filename = (get_ros_package_path('duckietown') +
"/config/baseline/calibration/camera_extrinsic/default.yaml")
if not os.path.isfile(extrinsics_filename):
logger.error("can't find default either, something's wrong")
self.ci_ = self.load_camera_info(intrinsics_filename)
self.pcm_ = PinholeCameraModel()
self.pcm_.fromCameraInfo(self.ci)
def __init__(self):
self.enabled = False
# Maps ID to running class probabilities
self.object_map = {}
# Maps ID to mean volume, used to discriminate buoys / black totem
self.volume_means = {}
self.area_means = {}
self.tf_buffer = tf2_ros.Buffer()
self.tf_listener = tf2_ros.TransformListener(self.tf_buffer)
self.get_params()
self.last_panel_points_msg = None
self.database_client = rospy.ServiceProxy('/database/requests',
ObjectDBQuery)
self.sub = Image_Subscriber(self.image_topic, self.img_cb)
self.camera_info = self.sub.wait_for_camera_info()
self.camera_model = PinholeCameraModel()
self.camera_model.fromCameraInfo(self.camera_info)
if self.debug:
self.image_mux = ImageMux(size=(self.camera_info.height,
self.camera_info.width),
shape=(1, 2),
labels=['Result', 'Mask'])
self.debug_pub = Image_Publisher('~debug_image')
self.last_objects = None
self.last_update_time = rospy.Time.now()
self.objects_sub = rospy.Subscriber('/pcodar/objects',
PerceptionObjectArray,
self.process_objects,
queue_size=2)
self.enabled_srv = rospy.Service('~set_enabled', SetBool,
self.set_enable_srv)
if self.is_training:
self.enabled = True
def get_camera_model(bag, camera_name):
camera_infos = [msg.message for msg in bag.read_messages(camera_name)]
camera_info = camera_infos[0]
from image_geometry import PinholeCameraModel
camera_model = PinholeCameraModel()
camera_model.fromCameraInfo(camera_info)
return camera_model
def centroid_to_pose(self, rect, moment):
"""
Publish the region of interest as a geometry_msgs/Pose message from
the ROI or the center of mass coordinates
@param rect - sensor_msgs/RegionOfInterest
@param moment - object's center of mass
"""
if not moment:
moment = (int(rect[0] + rect[2] / 2), int(rect[1] + rect[3] / 2))
u, v = int(moment[0]), int(moment[1])
ps = Pose()
ps.orientation.x = 0.0
ps.orientation.y = 0.0
ps.orientation.z = 0.0
ps.orientation.w = 1.0
cam = PinholeCameraModel()
cam.fromCameraInfo(self.cam_info)
(x, y, z) = cam.projectPixelTo3dRay((u, v))
ps.position.x = x
ps.position.y = y
ps.position.z = z
return ps
def __init__(self):
self.node_name = rospy.get_name()
self.bridge = CvBridge()
self.config = self.setupParam("~config", "baseline")
self.cali_file_name = self.setupParam("~cali_file_name", "default")
rospack = rospkg.RosPack()
self.cali_file = rospack.get_path('duckietown') + \
"/config/" + self.config + \
"/vehicle_detection/vehicle_filter_node/" + \
self.cali_file_name + ".yaml"
if not os.path.isfile(self.cali_file):
rospy.logwarn("[%s] Can't find calibration file: %s.\n" %
(self.node_name, self.cali_file))
self.loadConfig(self.cali_file)
self.sub_corners = rospy.Subscriber("~corners",
VehicleCorners,
self.cbCorners,
queue_size=1)
self.pub_pose = rospy.Publisher("~pose", VehiclePose, queue_size=1)
self.sub_info = rospy.Subscriber("~camera_info",
CameraInfo,
self.cbCameraInfo,
queue_size=1)
self.pcm = PinholeCameraModel()
rospy.loginfo("[%s] Initialization completed" % (self.node_name))
self.lock = mutex()
def __init__(self, img_proc=None):
super(LandmarkMethodROS,
self).__init__(device_id_facedetection=rospy.get_param(
"~device_id_facedetection", default="cuda:0"))
self.subject_tracker = FaceEncodingTracker() if rospy.get_param(
"~use_face_encoding_tracker",
default=True) else SequentialTracker()
self.bridge = CvBridge()
self.__subject_bridge = SubjectListBridge()
self.ros_tf_frame = rospy.get_param("~ros_tf_frame",
"/kinect2_nonrotated_link")
self.tf_broadcaster = TransformBroadcaster()
self.tf_prefix = rospy.get_param("~tf_prefix", default="gaze")
self.visualise_headpose = rospy.get_param("~visualise_headpose",
default=True)
self.pose_stabilizers = {} # Introduce scalar stabilizers for pose.
try:
if img_proc is None:
tqdm.write("Wait for camera message")
cam_info = rospy.wait_for_message("/camera_info",
CameraInfo,
timeout=None)
self.img_proc = PinholeCameraModel()
# noinspection PyTypeChecker
self.img_proc.fromCameraInfo(cam_info)
else:
self.img_proc = img_proc
if np.array_equal(
self.img_proc.intrinsicMatrix().A,
np.array([[0., 0., 0.], [0., 0., 0.], [0., 0., 0.]])):
raise Exception(
'Camera matrix is zero-matrix. Did you calibrate'
'the camera and linked to the yaml file in the launch file?'
)
tqdm.write("Camera message received")
except rospy.ROSException:
raise Exception("Could not get camera info")
# multiple person images publication
self.subject_pub = rospy.Publisher("/subjects/images",
MSG_SubjectImagesList,
queue_size=3)
# multiple person faces publication for visualisation
self.subject_faces_pub = rospy.Publisher("/subjects/faces",
Image,
queue_size=3)
Server(ModelSizeConfig, self._dyn_reconfig_callback)
# have the subscriber the last thing that's run to avoid conflicts
self.color_sub = rospy.Subscriber("/image",
Image,
self.process_image,
buff_size=2**24,
queue_size=3)
def __init__(self,resize_width,resize_height):
self.resize_width = resize_width
self.resize_height = resize_height
self.bridge = CvBridge()
camera_info = rospy.wait_for_message('/usb_cam/camera_info', CameraInfo, timeout=5)
# except(rospy.ROSException), e:
# print ("Camera info topic not available")
# print (e)
# exit()
waf = float(resize_width) / camera_info.width
haf = float(resize_height) / camera_info.height
camera_info.height = resize_height
camera_info.width = resize_width
# adjust the camera matrix
K = camera_info.K
camera_info.K = (K[0]*waf, 0., K[2]*waf,
0., K[4]*haf, K[5]*haf,
0., 0., 1.)
# adjust the projection matrix
P = camera_info.P
camera_info.P = (P[0]*waf, 0., P[2]*waf, 0.,
0., P[5]*haf, P[6]*haf, 0.,
0., 0., 1., 0.)
self.camera_model = PinholeCameraModel()
self.camera_model.fromCameraInfo(camera_info)
print (camera_info)
class Helpers:
def __init__(self, camera_info):
self.ci = camera_info
self.pcm = PinholeCameraModel()
self.pcm.fromCameraInfo(self.ci)
self._rectify_inited = False
self._distort_inited = False
def process_image(self, cv_image_raw, interpolation=cv2.INTER_NEAREST):
''' Undistort an image.
To be more precise, pass interpolation= cv2.INTER_CUBIC
'''
if not self._rectify_inited:
self._init_rectify_maps()
cv_image_rectified = np.empty_like(cv_image_raw)
res = cv2.remap(cv_image_raw, self.mapx, self.mapy, interpolation,
cv_image_rectified)
return res
def _init_rectify_maps(self):
W = self.pcm.width
H = self.pcm.height
mapx = np.ndarray(shape=(H, W, 1), dtype='float32')
mapy = np.ndarray(shape=(H, W, 1), dtype='float32')
mapx, mapy = cv2.initUndistortRectifyMap(self.pcm.K, self.pcm.D,
self.pcm.R, self.pcm.P,
(W, H), cv2.CV_32FC1, mapx,
mapy)
self.mapx = mapx
self.mapy = mapy
self._rectify_inited = True
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 __init__(self, cameraInfoFile):
""" Overlay lidar points onto images by setting up the camera model, camera intrinsics, and lidar-camera extrinsics.
===============
cameraInfoFile : an opened yaml file that stores camera intrinsics and other params
used to initialize the cameraInfo which is used to help cameraModel reproject lidar points to image space.
"""
self.cameraParams = yaml.load(cameraInfoFile)
self.cameraInfo = CameraInfo()
self._fillCameraInfo()
self.cameraModel = PinholeCameraModel()
self.cameraModel.fromCameraInfo(self.cameraInfo)
print('Distortion model:', self.cameraInfo.distortion_model)
self.bridge = cv_bridge.CvBridge()
# Get transformation/extrinsics between lidar (velodyne frame) and camera (world frame)
self.tf = tf.TransformListener()
# Get the topics' names to subscribe or publish
self.inImageName = rospy.resolve_name('image')
self.outImageName = rospy.resolve_name('lidar_image')
self.lidarName = rospy.remap_name('lidar')
# Create subscribers and publishers
self.subImage = rospy.Subscriber(self.inImageName,
Image,
callback=self.imageCallback,
queue_size=1)
self.lidar = rospy.Subscriber(self.lidarName,
PointCloud2,
callback=self.lidarCallback,
queue_size=1)
self.pubImage = rospy.Publisher(self.outImageName, Image, queue_size=1)
self.lidarPoints = None
def _enable(self):
if self._camera_info is None:
self._camera_info = self._image_sub.wait_for_camera_info()
self.camera_model = PinholeCameraModel()
self.camera_model.fromCameraInfo(self._camera_info)
self._enabled = True
rospy.loginfo("Enabled.")
def __init__(self):
# Pull constants from config file
self.lower = rospy.get_param('~lower_color_threshold', [0, 0, 80])
self.upper = rospy.get_param(
'~higher_color_threshold', [200, 200, 250])
self.min_contour_area = rospy.get_param('~min_contour_area', .001)
self.max_contour_area = rospy.get_param('~max_contour_area', 400)
self.min_trans = rospy.get_param('~min_trans', .05)
self.max_velocity = rospy.get_param('~max_velocity', 1)
self.timeout = rospy.Duration(
rospy.get_param('~timeout_seconds'), 250000)
self.min_observations = rospy.get_param('~min_observations', 8)
self.camera = rospy.get_param('~camera_topic',
'/camera/front/left/image_rect_color')
# Instantiate remaining variables and objects
self._observations = deque()
self._pose_pairs = deque()
self._times = deque()
self.last_image_time = None
self.last_image = None
self.tf_listener = tf.TransformListener()
self.status = ''
self.est = None
self.visual_id = 0
self.enabled = False
self.bridge = CvBridge()
# Image Subscriber and Camera Information
self.image_sub = Image_Subscriber(self.camera, self.image_cb)
self.camera_info = self.image_sub.wait_for_camera_info()
'''
These variables store the camera information required to perform
the transformations on the coordinates to move from the subs
perspective to our global map perspective. This information is
also necessary to perform the least squares intersection which
will find the 3D coordinates of the torpedo board based on 2D
observations from the Camera. More info on this can be found in
sub8_vision_tools.
'''
self.camera_info = self.image_sub.wait_for_camera_info()
self.camera_model = PinholeCameraModel()
self.camera_model.fromCameraInfo(self.camera_info)
self.frame_id = self.camera_model.tfFrame()
# Ros Services so mission can be toggled and info requested
rospy.Service('~enable', SetBool, self.toggle_search)
self.multi_obs = MultiObservation(self.camera_model)
rospy.Service('~pose', VisionRequest, self.request_board3d)
self.image_pub = Image_Publisher("torp_vision/debug")
self.point_pub = rospy.Publisher(
"torp_vision/points", Point, queue_size=1)
self.mask_image_pub = rospy.Publisher(
'torp_vison/mask', Image, queue_size=1)
# Debug
self.debug = rospy.get_param('~debug', True)
def __init__(self):
rospy.init_node('offboard_test', anonymous=True)
self.loc = []
rospy.Subscriber('/mavros/local_position/pose',
PoseStamped,
callback=self.mocap_cb)
rospy.Subscriber('/mavros/state', State, callback=self.state_cb)
rospy.Subscriber('/dreams/state',
String,
callback=self.update_state_cb)
rospy.Subscriber('/darknet_ros/bounding_boxes',
BoundingBoxes,
callback=self.yolo)
rospy.Subscriber('/darknet_ros/detection_image',
Image,
callback=self.detected_image)
rospy.Subscriber('/camera/depth/image_raw',
Image,
callback=self.depth_image)
#self.camera.fromCameraInfo(self.rgb_info())
# Zhiang: use this instead
camera_info = rospy.wait_for_message(
"/camera/depth/camera_info",
CameraInfo) # Zhiang: make sure this is the correct camera
#camera_info = rospy.wait_for_message("/camera/rgb/camera_info", CameraInfo)
self.pinhole_camera_model = PinholeCameraModel()
self.pinhole_camera_model.fromCameraInfo(camera_info)
#self.listener = tf.TransformListener() # linked to tf_static
self.planner()
def __init__(self):
self.node_name = rospy.get_name()
self.bridge = CvBridge()
self.active = True
self.currentImg = None
self.publish_duration = rospy.Duration.from_sec(1.0 / 2.0)
self.last_stamp = rospy.Time.now()
#Subscribers and publishers
self.imagesub = rospy.Subscriber("~image",
CompressedImage,
self.find_marker,
buff_size=921600,
queue_size=1)
self.movepub = rospy.Publisher("~car_cmd",
Twist2DStamped,
queue_size=10)
self.imagepub = rospy.Publisher("~image", Image, queue_size=1)
self.infosub = rospy.Subscriber("~camera_info",
CameraInfo,
self.get_camera_info,
queue_size=1)
self.camerainfo = PinholeCameraModel()
rospy.loginfo("[%s] Initialization completed" % (self.node_name))
def from_camera_info(CameraConfig, message):
''' Load camera intrinsics from ROS camera info topic, initialize
camera extrinsics with None value. '''
model = PinholeCameraModel()
model.fromCameraInfo(message)
return CameraConfig(model.K, model.D, None,
(model.width, model.height), model.P, model.R)
class CamPixelToPointServer:
def __init__(self):
self.camera_model = PinholeCameraModel()
self.bridge = CvBridge()
self.model_set = False
self.tf_listener = TransformListener()
def pixel_to_point(self,
cam_pos,
out_frame='map'): # type: (np.ndarray) -> PointStamped
if not self.model_set:
self.camera_model.fromCameraInfo(
rospy.wait_for_message('/camera/rgb/camera_info', CameraInfo))
self.model_set = True
x, y = int(cam_pos[0]), int(cam_pos[1])
d = self.read_depth_simple(x, y)
pos = np.array(self.camera_model.projectPixelTo3dRay((x, y))) * d
point = PointStamped()
point.header.frame_id = self.camera_model.tfFrame()
point.point.x, point.point.y, point.point.z = pos[0], pos[1], pos[2]
point = convert_frame(self.tf_listener, point, out_frame)
return point
def read_depth_simple(self, x, y): # (int, int) -> float
image = rospy.wait_for_message('/camera/depth_registered/image_raw',
Image)
image = self.bridge.imgmsg_to_cv2(image)
return image[y, x]
def __init__(self):
self.node_name = rospy.get_name()
self.bridge = CvBridge()
self.config = self.setupParam("~config", "baseline")
self.cali_file_name = self.setupParam("~cali_file_name", "default")
rospack = rospkg.RosPack()
self.cali_file = "/code/catkin_ws/src/dt-core/packages/vehicle_detection/config/vehicle_filter_node/" + \
self.cali_file_name + ".yaml"
if not os.path.isfile(self.cali_file):
rospy.logwarn("[%s] Can't find calibration file: %s.\n" %
(self.node_name, self.cali_file))
self.loadConfig(self.cali_file)
self.sub_corners = rospy.Subscriber("~corners",
VehicleCorners,
self.processCorners,
queue_size=1)
self.pub_pose = rospy.Publisher("~pose", VehiclePose, queue_size=1)
self.sub_info = rospy.Subscriber("~camera_info",
CameraInfo,
self.processCameraInfo,
queue_size=1)
self.pub_time_elapsed = rospy.Publisher("~pose_calculation_time",
Float32,
queue_size=1)
self.pcm = PinholeCameraModel()
rospy.loginfo("[%s] Initialization completed" % (self.node_name))
self.lock = mutex()
def __init__(self, camera_info, maxdist, transform=None):
"""
Parameters
----------
camera_info: sensor_msgs/CameraInfo
Meta information of the camera. For more details check the
`sensor_msgs/CameraInfo
<http://docs.ros.org/api/sensor_msgs/html/msg/CameraInfo.html>`_
documentation.
maxdist: float
Maximum distance the FOV covers in the Z direction of the camera frame.
transform: array_like
Homogenous transformation of the camera reference frame. If `None` then
the identity matrix is used.
"""
if transform is None:
transform = np.eye(4)
self.maxdist = maxdist
self.transform = np.array(transform)
self.cam_model = PinholeCameraModel()
self.cam_model.fromCameraInfo(camera_info)
# Compute FOV corners
delta_x = self.cam_model.getDeltaX(self.cam_model.width/2, self.maxdist)
delta_y = self.cam_model.getDeltaY(self.cam_model.height/2, self.maxdist)
self.corners = np.zeros((5,3))
self.corners[0,:] = transform[:3,3]
idx = 1
for k in itertools.product([-1,1],[-1,1]):
point = np.array([0, 0, self.maxdist, 1])
point[:2] = np.array([delta_x, delta_y]) * np.array(k)
self.corners[idx,:] = np.dot(transform, point)[:3]
idx += 1
def process_msgs(msgs, cinfo, shift):
ret = len(msgs) * [None]
cam_model = PinholeCameraModel()
if cinfo is not None:
cam_model.fromCameraInfo(cinfo)
offset = None
for it in range(0, len(msgs)):
cur_msg = msgs[it]
out_msg = msg(cur_msg.header.stamp.to_sec(), list())
if cinfo is not None:
# rospy.loginfo("using cinfo")
for det in cur_msg.points:
xyz = np.array([det.x, det.y, det.z])
if offset is None:
offset = xyz
xyz = xyz - offset + shift
# xyz = np.dot(xyz, R)
out_msg.positions.append(xyz)
else:
# rospy.loginfo("not using cinfo")
x = cur_msg.pose.pose.position.x
y = cur_msg.pose.pose.position.y
z = cur_msg.pose.pose.position.z
out_msg.positions.append(np.array([x, y, z]))
ret[it] = out_msg
return ret
def __init__(self, publish_tf):
#To take our Ros Image into a cv message and subsequently a numpy array.
self.bridge = CvBridge()
# To make the pixel to vector projection
self.cam_model = PinholeCameraModel()
#We need CameraInfo in order to use PinholeCameraModel below.
rospy.Subscriber("camera_topic", CameraInfo, self.camera_callback)
self.hasCameraInfo = False
while not self.hasCameraInfo:
print "waiting on camera info."
rospy.sleep(0.5)
#We are using a depth image to get depth information of what we're tracking.
rospy.Subscriber("depth_image", Image, self.depth_callback)
#This package is just an extension of cmvision to provide tf tracking of the blobs provided by cmvision.
rospy.Subscriber('blobs', Blobs, self.blob_callback)
#Subscribe to image for debugging.
# rospy.Subscriber('thing', Image, self.image_callback)
self.listener = tf.TransformListener()
self.broadcaster = tf.TransformBroadcaster()
#Republish each blob as part of a blob.
self.blob_pub = rospy.Publisher('/blobs_3d', Blobs3d)
self.publish_tf = publish_tf
def __init__(self):
self._imageInputName = rospy.resolve_name('image')
print(self._imageInputName)
self._velodyneOutputName = rospy.resolve_name('velodyne_rgb_points')
print(self._velodyneOutputName)
self._cameraName = rospy.resolve_name('camera')
print(self._cameraName)
self._velodyneName = rospy.resolve_name('velodyne')
print(self._velodyneName)
self._imageInput = rospy.Subscriber(self._imageInputName,
Image,
callback=self.imageCallback,
queue_size=1)
self._camera = rospy.Subscriber(self._cameraName,
CameraInfo,
callback=self.cameraCallback,
queue_size=1)
self._velodyne = rospy.Subscriber(self._velodyneName,
PointCloud2,
callback=self.velodyneCallback,
queue_size=1)
self._velodyneOutput = rospy.Publisher(self._velodyneOutputName,
PointCloud2,
queue_size=1)
self._bridge = cv_bridge.CvBridge()
self._cameraModel = PinholeCameraModel()
self._tf = tf.TransformListener()
def __init__(self, camera_info, homography, debug=False):
self.pcm = PinholeCameraModel()
self.pcm.fromCameraInfo(camera_info)
self.H = homography # maps points on ground plane to image plane
self.debug = debug
self.mapx, self.mapy = self._init_rectification()
def broadcast_poses():
robot = robot_interface.Robot_Interface()
cam = camera.RGBD()
not_read = True
while not_read:
try:
cam_info = cam.read_info_data()
if (not cam_info == None):
not_read = False
except:
rospy.logerr('info not recieved')
pcm = PCM()
pcm.fromCameraInfo(cam_info)
point = (208 + (323 - 208) / 2, 247 + (424 - 247) / 2)
print(point)
#(208.076538,247.264099) (323.411957,242.667325) (204.806457,424.053619) (324.232857,434.011618)
dep_data = robot.get_img_data()[1]
print(dep_data)
dep = robot.get_depth(point, dep_data)
print(dep)
br = tf.TransformBroadcaster()
td_points = pcm.projectPixelTo3dRay(point)
# pose = np.array([td_points[0], td_points[1], 0.001 * dep])
norm_pose = np.array(td_points)
norm_pose = norm_pose / norm_pose[2]
norm_pose = norm_pose * (dep)
a = tf.transformations.quaternion_from_euler(ai=-2.355, aj=-3.14, ak=0.0)
b = tf.transformations.quaternion_from_euler(ai=0.0, aj=0.0, ak=1.57)
base_rot = tf.transformations.quaternion_multiply(a, b)
br.sendTransform(norm_pose, base_rot, rospy.Time.now(), 'tree',
'head_camera_rgb_optical_frame')
class RGBD_Project(object):
def __init__(self):
self.cam = RGBD()
not_read = True
while not_read:
try:
cam_info = self.cam.read_info_data()
if (not cam_info == None):
not_read = False
except:
rospy.logerr('info not recieved')
self.pcm = PCM()
self.pcm.fromCameraInfo(cam_info)
def deproject_pose(self, pose):
""""
pose = (u_x,u_y,z)
u_x,u_y correspond to pixel value in image
x corresponds to depth
"""
td_points = self.pcm.projectPixelTo3dRay((pose[0], pose[1]))
norm_pose = np.array(td_points)
norm_pose = norm_pose / norm_pose[2]
norm_pose = norm_pose * (cfg.MM_TO_M * pose[2])
return norm_pose
def _cinfo_cb(self, msg):
# create mapx and mapy
H, W = msg.height, msg.width
# create new camera info
self.camera_model = PinholeCameraModel()
self.camera_model.fromCameraInfo(msg)
# find optimal rectified pinhole camera
with self.profiler('/cb/camera_info/get_optimal_new_camera_matrix'):
rect_K, _ = cv2.getOptimalNewCameraMatrix(
self.camera_model.K,
self.camera_model.D,
(W, H),
self.rectify_alpha
)
# store new camera parameters
self._camera_parameters = (rect_K[0, 0], rect_K[1, 1], rect_K[0, 2], rect_K[1, 2])
# create rectification map
with self.profiler('/cb/camera_info/init_undistort_rectify_map'):
self._mapx, self._mapy = cv2.initUndistortRectifyMap(
self.camera_model.K,
self.camera_model.D,
None,
rect_K,
(W, H),
cv2.CV_32FC1
)
# once we got the camera info, we can stop the subscriber
self.loginfo('Camera info message received. Unsubscribing from camera_info topic.')
# noinspection PyBroadException
try:
self._cinfo_sub.shutdown()
except BaseException:
pass
def __init__(self):
self.node_name = rospy.get_name()
self.bridge = CvBridge()
self.distance_between_centers = self.setupParam(
'distance_between_centers', 0.0125)
self.max_reproj_pixelerror_pose_estimation = self.setupParam(
'max_reproj_pixelerror_pose_estimation', 5)
self.sub_corners = rospy.Subscriber("~corners",
VehicleCorners,
self.processCorners,
queue_size=1)
self.pub_pose = rospy.Publisher("~pose", VehiclePose, queue_size=1)
self.sub_info = rospy.Subscriber("~camera_info",
CameraInfo,
self.processCameraInfo,
queue_size=1)
self.pub_time_elapsed = rospy.Publisher("~pose_calculation_time",
Float32,
queue_size=1)
self.pcm = PinholeCameraModel()
rospy.loginfo("[%s] Initialization completed" % (self.node_name))
self.lock = mutex()
def __init__(self):
"""
Initialize Color Tracking ROS interface.
"""
rospy.init_node('color_tracker')
self.tracker = ColorTracker()
self.cv_image = self.processed_image = None # the latest image from the camera
self.boxes = []
self.bridge = CvBridge() # used to convert ROS messages to OpenCV
self.cameraModel = PinholeCameraModel()
self.tf = TransformListener(cache_time=rospy.Duration.from_sec(20))
# parameters ...
self._gui = bool(rospy.get_param('~gui', default=False)) # set to _gui:=true for debug display
self._rate = float(rospy.get_param('~rate', default=50.0)) # processing rate
self._par = float(rospy.get_param('~plate_area', default=0.0338709))
# publishers ...
self.debug_pub = rospy.Publisher("tracker/debug", PoseWithCovarianceStamped, queue_size=10)
self.roomba_pub = rospy.Publisher("visible_roombas", RoombaList, queue_size = 10)
rospy.loginfo("Initializing Color Tracker")
if self._gui:
cv2.namedWindow('preview_window')
cv2.namedWindow('binary')
# start listening ...
rospy.Subscriber("/ardrone/bottom/image_raw", Image, self.process_image)
rospy.Subscriber("/ardrone/bottom/camera_info", CameraInfo, self.on_camera_info)
class DrawRect(): def __init__(self, topic_image, topic_blob, topic_info, topic_out): self.sync1_callback = False #synchronize the image self.sync2_callback = False #synchronize the camera info self.bridge = CvBridge() rospy.Subscriber(topic_image, Image, self.image_callback) rospy.Subscriber(topic_blob, Blobs, self.blob_callback) rospy.Subscriber(topic_info, CameraInfo, self.info_callback) self.pub = rospy.Publisher(topic_out, PointStamped) def image_callback(self, image): self.sync1_callback = True self.image = self.bridge.imgmsg_to_cv(image) self.image = np.asarray(self.image) def info_callback (self, info): #get the camera information self.sync2_callback = True self.info = info def blob_callback(self, blob): if self.sync1_callback and self.sync2_callback and (blob.blob_count != 0): bleb = self.FindBiggestBlob (blob) z = self.image[bleb.y][bleb.x] # depth in mm (INT!) z /= 1000.0 # now in meters (FLOAT!) self.UVtoXYZ = PinholeCameraModel() self.UVtoXYZ.fromCameraInfo(self.info) vec = self.UVtoXYZ.projectPixelTo3dRay ((bleb.x, bleb.y)) vec = [x * (z/vec[2]) for x in vec] self.P = PointStamped() self.P.header.frame_id = self.info.header.frame_id self.P.point.x = vec[0] #the np make the downward direction positive direction, #so we need to multiply to -1 to make upward direction #positive direction self.P.point.y = vec[1] self.P.point.z = vec[2] print self.P self.pub.publish(self.P) def FindBiggestBlob(self, blob): #Assume the hat is the only green blob in front of the camera x = 0 bleb = None while x < blob.blob_count: if (x == 0): bleb = blob.blobs[x] else: if (blob.blobs[x].area > bleb.area): bleb = blob.blobs[x] x = x + 1 return bleb
def get_rays(self):
model = PinholeCameraModel()
model.fromCameraInfo(self.depth_info)
self.array_rays = numpy.zeros((self.image_depth.height, self.image_depth.width, 3))
for u in range(self.image_depth.height):
for v in range(self.image_depth.width):
ray = model.projectPixelTo3dRay((u, v))
ray_z = [el / ray[2] for el in ray] # normalize the ray so its Z-component equals 1.0
self.array_rays[u, v, :] = ray_z
def create_cam_model(info_msg):
from image_geometry import PinholeCameraModel
import numpy as np
cam_model = PinholeCameraModel()
# <have an info message >
cam_model.fromCameraInfo(info_msg)
print "cam_model = ", np.array(cam_model.intrinsicMatrix())
print "cam tf = ", cam_model.tfFrame()
return cam_model
class VisionNode(object):
'''
Base class to be used unify the interfacing for MIL's computer vision scripts.
Handles the bootstrap of image subscription, enable/disable, etc.
Provides a callback for new images which is expected to return
'''
__metaclass__ = abc.ABCMeta
def __init__(self):
self._objects_pub = rospy.Publisher("~identified_objects", ObjectsInImage, queue_size=3)
self._camera_info = None
self.camera_model = None
self._enabled = False
self._image_sub = Image_Subscriber("image", callback=self._img_cb)
if rospy.get_param("~autostart", default=False):
self._enable()
else:
self._disable()
self._enable_srv = rospy.Service("~enable", SetBool, self._enable_cb)
def _enable_cb(self, req):
if req.data and not self._enabled:
self._enable()
elif not req.data and self._enabled:
self._disable()
return {'success': True}
def _enable(self):
if self._camera_info is None:
self._camera_info = self._image_sub.wait_for_camera_info()
self.camera_model = PinholeCameraModel()
self.camera_model.fromCameraInfo(self._camera_info)
self._enabled = True
rospy.loginfo("Enabled.")
def _disable(self):
self._enabled = False
rospy.loginfo("Disabled.")
def _img_cb(self, img):
if not self._enabled:
return
msg = ObjectsInImage()
msg.header = self._image_sub.last_image_header
msg.objects = self.find_objects(img)
if not isinstance(msg.objects, list) or (len(msg.objects) and not isinstance(msg.objects[0], ObjectInImage)):
rospy.logwarn("find_objects did not return a list of mil_msgs/ObjectInImage message. Ignoring.")
self._objects_pub.publish(msg)
@abc.abstractmethod
def find_objects(self, img):
pass
def handle_img_msg(self, img_msg):
img = None
bridge = cv_bridge.CvBridge()
try:
img = bridge.imgmsg_to_cv2(img_msg, 'bgr8')
except cv_bridge.CvBridgeError as e:
rospy.logerr( 'image message to cv conversion failed :' )
rospy.logerr( e )
print( e )
return
#tx, ty, tz, yaw, pitch, roll = [0.00749025, -0.40459941, -0.51372948,
# -1.66780896, -1.59875352, -3.05415572]
#translation = [tx, ty, tz, 1]
#rotationMatrix = tf.transformations.euler_matrix(roll, pitch, yaw)
#rotationMatrix[:, 3] = translation
md = self.metadata
dims = np.array([md['l'], md['w'], md['h']])
outputName = '/image_bbox'
imgOutput = rospy.Publisher(outputName, Image, queue_size=1)
obs_centroid = self.obs_centroid
if self.obs_centroid is None:
rospy.loginfo("Couldn't find obstacle centroid")
imgOutput.publish(bridge.cv2_to_imgmsg(img, 'bgr8'))
return
# print centroid info
rospy.loginfo(str(obs_centroid))
# case when obstacle is not in camera frame
if obs_centroid[0]<2.5 :
imgOutput.publish(bridge.cv2_to_imgmsg(img, 'bgr8'))
return
# get bbox
R = tf.transformations.quaternion_matrix(self.orient)
corners = [0.5*np.array([i,j,k])*dims for i in [-1,1]
for j in [-1,1] for k in [-1,1]]
corners = [obs_centroid + R.dot(list(c)+[1])[:3] for c in corners]
projected_pts = []
cameraModel = PinholeCameraModel()
cam_info = load_cam_info(self.calib_file)
cameraModel.fromCameraInfo(cam_info)
projected_pts = [cameraModel.project3dToPixel(list(pt)+[1]) for pt in corners]
#for pt in corners:
# rotated_pt = rotationMatrix.dot(list(pt)+[1])
# projected_pts.append(cameraModel.project3dToPixel(rotated_pt))
projected_pts = np.array(projected_pts)
center = np.mean(projected_pts, axis=0)
out_img = drawBbox(img, projected_pts)
imgOutput.publish(bridge.cv2_to_imgmsg(out_img, 'bgr8'))
class RayTransformer():
def __init__(self, topic):
self.model = PinholeCameraModel()
rospy.Subscriber(topic, CameraInfo, self.callback_info)
def callback_info(self, info):
self.model.fromCameraInfo(info)
def projectPointCloudToPixels(self, cloud):
pixel_cloud = []
for point in cloud.points:
pixel = self.model.project3dToPixel((point.x, point.y, point.z))
pixel_cloud.append(pixel)
return pixel_cloud
class PointFromPixel():
""" Given a pixel location, find its 3D location in the world """
def __init__(self, topic_camera_info, topic_depth):
self.need_camera_info = True
self.need_depth_image = True
self.model = PinholeCameraModel()
rospy.Subscriber(topic_camera_info, CameraInfo, self.callback_camera_info)
rospy.Subscriber(topic_depth, Image, self.callback_depth_image)
def callback_camera_info(self, info):
""" Define Pinhole Camera Model parameters using camera info msg """
if self.need_camera_info:
rospy.loginfo('Got camera info!')
self.model.fromCameraInfo(info) # define model params
self.frame = info.header.frame_id
self.need_camera_info = False
def callback_depth_image(self, depth_image):
""" Get depth at chosen pixel using depth image """
if self.need_depth_image:
rospy.loginfo('Got depth image!')
self.depth = img.fromstring("F", (depth_image.width, depth_image.height), depth_image.data)
self.need_depth_image = False
def calculate_3d_point(self, pixel):
""" Project ray through chosen pixel, then use pixel depth to get 3d point """
lookup = self.depth.load()
depth = lookup[pixel[0], pixel[1]] # lookup pixel in depth image
ray = self.model.projectPixelTo3dRay(tuple(pixel)) # get 3d ray of unit length through desired pixel
ray_z = [el / ray[2] for el in ray] # normalize the ray so its Z-component equals 1.0
pt = [el * depth for el in ray_z] # multiply the ray by the depth; its Z-component should now equal the depth value
point = PointStamped()
point.header.frame_id = self.frame
point.point.x = pt[0]
point.point.y = pt[1]
point.point.z = pt[2]
return point
def ray_plane_intersection(self, pixel, plane):
""" Given plane parameters [a, b, c, d] as in ax+by+cz+d=0,
finds intersection of 3D ray with the plane. """
ray = self.model.projectPixelTo3dRay(tuple(pixel)) # get 3d ray of unit length through desired pixel
scale = -plane[3] / (plane[0]*ray[0] + plane[1]*ray[1] + plane[2]*ray[2])
point = PointStamped()
point.header.frame_id = self.frame
point.point.x = ray[0] * scale
point.point.y = ray[1] * scale
point.point.z = ray[2] * scale
return point
def __init__(self):
# Get information for this particular camera
camera_info = get_single('head_camera/depth_registered/camera_info', CameraInfo)
print camera_info
# Set information for the camera
self.cam_model = PinholeCameraModel()
self.cam_model.fromCameraInfo(camera_info)
# Subscribe to the camera's image topic and depth image topic
self.rgb_image_sub = rospy.Subscriber("head_camera/rgb/image_raw",
Image, self.on_rgb)
self.depth_image_sub = rospy.Subscriber("head_camera/depth_registered/image_raw",
Image, self.on_depth)
# Publish where the button is in the base link frame
self.point_pub = rospy.Publisher('button_point', PointStamped, queue_size=10)
# Set up connection to CvBridge
self.bridge = CvBridge()
# Open up viewing windows
cv2.namedWindow('depth')
cv2.namedWindow('rgb')
# Set up the class variables
self.rgb_image = None
self.rgb_image_time = None
self.depth_image = None
self.center = None
self.return_point = PointStamped()
self.tf_listener = TransformListener()
def __init__(self):
self.image_topic = "wall_camera/image_raw"
self.image_info_topic = "wall_camera/camera_info"
self.point_topic = "/clicked_point"
self.frame_id = "/map"
self.occupancy
self.pixel_x = 350
self.pixel_y = 215
# What we do during shutdown
rospy.on_shutdown(self.cleanup)
# Create the OpenCV display window for the RGB image
self.cv_window_name = self.image_topic
cv.NamedWindow(self.cv_window_name, cv.CV_WINDOW_NORMAL)
cv.MoveWindow(self.cv_window_name, 25, 75)
# Create the cv_bridge object
self.bridge = CvBridge()
self.image_sub = rospy.Subscriber(self.image_topic, Image, self.image_cb, queue_size = 1)
self.image_info_sub = rospy.Subscriber(self.image_info_topic, CameraInfo, self.image_info_cb, queue_size = 1)
self.point_sub = rospy.Subscriber(self.point_topic, PointStamped, self.point_cb, queue_size = 4)
self.points = []
self.ready_for_image = False
self.has_pic = False
self.camera_info_msg = None
self.tf_listener = tf.TransformListener()
# self.display_picture()
self.camera_model = PinholeCameraModel()
def __init__(self, stillness_radius, mode_topic, torso_topic, info_topic, image_topic):
self.stillness_radius = stillness_radius
# Subscribe to mode info and torso location
self.need_mode = True
rospy.Subscriber(mode_topic, Bool, self.mode_callback)
self.need_torso = True
rospy.Subscriber(torso_topic, PointStamped, self.torso_callback, queue_size=1)
# Camera model stuff
self.model = PinholeCameraModel()
self.need_camera_info = True
rospy.Subscriber(info_topic, CameraInfo, self.info_callback, queue_size=1)
# Ready the face detector
self.classifier = cv2.CascadeClassifier(os.environ['ROS_ROOT'] + '/../OpenCV/haarcascades/haarcascade_frontalface_alt.xml')
# Wait for all those messages to begin arriving
while self.need_mode or self.need_torso or self.need_camera_info:
rospy.sleep(0.01)
# Subscribe to images
self.have_image = False
self.bridge = CvBridge()
rospy.Subscriber(image_topic, Image, self.handle_image, queue_size=1)
def __init__(self, publish_tf):
# To take our Ros Image into a cv message and subsequently a numpy array.
self.bridge = CvBridge()
# To make the pixel to vector projection
self.cam_model = PinholeCameraModel()
# We need CameraInfo in order to use PinholeCameraModel below.
rospy.Subscriber('camera_topic', CameraInfo, self.camera_callback)
self.hasCameraInfo = False
while not self.hasCameraInfo:
print 'waiting on camera info.'
rospy.sleep(0.5)
# We are using a depth image to get depth information of what we're tracking.
rospy.Subscriber('depth_image', Image, self.depth_callback)
# This package is just an extension of cmvision to provide tf tracking of the blobs provided by cmvision.
rospy.Subscriber('blobs', Blobs, self.blob_callback)
# Subscribe to image for debugging.
# rospy.Subscriber('thing', Image, self.image_callback)
self.listener = tf.TransformListener()
self.broadcaster = tf.TransformBroadcaster()
# Republish each blob as part of a blob.
self.blob_pub = rospy.Publisher('/blobs_3d', Blobs3d, queue_size=5)
self.publish_tf = publish_tf
def __init__(self, image_topic, camera_topic, robot_depth, robot_frame, pattern):
# A subscriber to the image topic
rospy.Subscriber(image_topic, Image, self.image_callback)
# To get which frame is the camera frame
rospy.Subscriber(camera_topic, CameraInfo, self.camera_callback)
# To make the pixel to vector projection
self.camModel = PinholeCameraModel()
# How far is the robot plane from the camera?
self.robot_depth = robot_depth
# What is the name of the frame we should publish?
self.robot_frame = robot_frame
# Pattern to match
self.image_orig = cv2.imread(pattern, 1)
image_bin = cv2.cvtColor(self.image_orig, cv2.COLOR_BGR2GRAY)
ret, image_bin = cv2.threshold(image_bin, 127,255,cv2.THRESH_BINARY_INV)
contours, hierarchy = cv2.findContours(image_bin, cv2.RETR_LIST, cv2.CHAIN_APPROX_SIMPLE)
sorted_contours = sorted(contours, key = cv2.contourArea, reverse = True)
# Our pattern is the second contour because for some reason its finding the edges of our image as a contour
self.pattern = sorted_contours[1]
self.pattern_area = 200
# To convert images from ROS to CV formats
self.bridge = CvBridge()
# Publish frames to tf
self.broadcaster = tf.TransformBroadcaster()
# Publisher for edited video
self.video = rospy.Publisher('ceilingMarker', Image)
def run(sub_singleton):
global marker_found
nh = sub_singleton._node_handle
#next_pose = yield nh.get_service_client('/next_search_pose', SearchPose)
cam = PinholeCameraModel()
# Go to max height to see as much as we can.
yield sub_singleton.move.depth(SEARCH_DEPTH).zero_roll_and_pitch().go()
yield nh.sleep(1.0)
# ---------------------------------------
# MOST OF THIS IS FOR THE SEARCH PATTERN.
# ---------------------------------------
# This is just to get camera info
resp = yield sub_singleton.channel_marker.get_2d()
cam.fromCameraInfo(resp.camera_info)
# intial_pose = sub_singleton.last_pose()
# intial_height = yield sub_singleton.get_dvl_range()
# radius = calculate_visual_radius(cam, intial_height)
# s = SearchPoseRequest()
# s.intial_position = (yield intial_pose).pose.pose
# s.search_radius = radius
# s.reset_search = True
# yield next_pose(s)
# s.reset_search = False
marker_found = False
goer = go_to_marker(nh, sub_singleton, cam)
# while not marker_found:
# # Move in search pattern until we find the marker
# resp = yield next_pose(s)
# print resp
# if resp.area > .8:
# continue
# print pose_to_numpy(resp.target_pose)[0]
# yield sub_singleton.move.set_position(pose_to_numpy(resp.target_pose)[0]).zero_roll_and_pitch().go()
# print "Searcher arrived"
yield goer
def run(sub_singleton):
global marker_found
print "Searching"
nh = sub_singleton._node_handle
next_pose = yield nh.get_service_client('/next_search_pose', SearchPose)
cam = PinholeCameraModel()
# Go to max height to see as much as we can.
yield sub_singleton.move.depth(0.6).zero_roll_and_pitch().go()
yield nh.sleep(1.0)
# This is just to get camera info
resp = yield sub_singleton.channel_marker.get_2d()
cam.fromCameraInfo(resp.camera_info)
intial_pose = sub_singleton.last_pose()
intial_height = yield sub_singleton.get_dvl_range()
raidus = calculate_visual_radius(cam, intial_height)
s = SearchPoseRequest()
s.intial_position = (yield intial_pose).pose.pose
s.search_radius = raidus
s.reset_search = True
yield next_pose(s)
s.reset_search = False
marker_found = False
goer = go_to_marker(nh, sub_singleton, cam)
while not marker_found:
# Move in search pattern until we find the marker
resp = yield next_pose(s)
print resp
if resp.area > .8:
continue
print pose_to_numpy(resp.target_pose)[0]
yield sub_singleton.move.set_position(pose_to_numpy(resp.target_pose)[0]).zero_roll_and_pitch().go()
print "Searcher arrived"
yield goer
def random_point_inside_fov(camera_info, maxdist, Tcamera=np.eye(4)): """ Generates a random XYZ point inside the camera field of view @type camera_info: sensor_msgs.CameraInfo @param camera_info: Message with the meta information for a camera @type maxdist: float @param maxdist: distance from the camera ref frame in the z direction @type Tcamera: np.array @param Tcamera: homogeneous transformation for the camera ref frame @rtype: array @return: The random XYZ point """ cam_model = PinholeCameraModel() cam_model.fromCameraInfo(camera_info) z = maxdist*np.random.random() delta_x = cam_model.getDeltaX(cam_model.width/2, z) delta_y = cam_model.getDeltaY(cam_model.height/2, z) point = np.array([0, 0, z, 1]) point[:2] = np.array([delta_x, delta_y]) * (2*np.random.random_sample(2) - 1.) return np.dot(Tcamera, point)[:3]
def get_caminfo(self):
cam_info = None
try:
cam_info = rospy.wait_for_message('camera_info', CameraInfo, 1.0)
except rospy.ROSException:
rospy.logerr("Could not get camera_info")
if cam_info is not None:
self.model = PinholeCameraModel()
self.model.fromCameraInfo(cam_info)
def __init__(self, image_sub, grid_res, grid_width, grid_height, grid_starting_pose):
super(self.__class__, self).__init__(res=grid_res, width=grid_width, height=grid_height, starting_pose=grid_starting_pose)
self.tf_listener = tf.TransformListener()
self.cam = PinholeCameraModel()
self.camera_info = image_sub.wait_for_camera_info()
if self.camera_info is None:
# Maybe raise an alarm here.
rospy.logerr("I don't know what to do without my camera info.")
self.cam.fromCameraInfo(self.camera_info)
def camera_fov_corners(camera_info, zdist, Tcamera=np.eye(4)):
"""
Generates the 5 corners of the camera field of view
@type camera_info: sensor_msgs.CameraInfo
@param camera_info: Message with the meta information for a camera
@type zdist: float
@param zdist: distance from the camera ref frame in the z direction
@type Tcamera: np.array
@param Tcamera: homogeneous transformation for the camera ref frame
@rtype: list
@return: The 5 corners of the camera field of view
"""
cam_model = PinholeCameraModel()
cam_model.fromCameraInfo(camera_info)
delta_x = cam_model.getDeltaX(cam_model.width/2, zdist)
delta_y = cam_model.getDeltaY(cam_model.height/2, zdist)
corners = [Tcamera[:3,3]]
for k in itertools.product([-1,1],[-1,1]):
point = np.array([0, 0, zdist, 1])
point[:2] = np.array([delta_x, delta_y]) * np.array(k)
corners.append( np.dot(Tcamera, point)[:3] )
return np.array(corners)
def __init__(self):
self.debug_gui = False
self.enabled = False
self.cam = None
# Constants from launch config file
self.debug_ros = rospy.get_param("~debug_ros", True)
self.canny_low = rospy.get_param("~canny_low", 100)
self.canny_ratio = rospy.get_param("~canny_ratio", 3.0)
self.thresh_hue_high = rospy.get_param("~thresh_hue_high", 60)
self.thresh_saturation_low = rospy.get_param("~thresh_satuation_low", 100)
self.min_contour_area = rospy.get_param("~min_contour_area", 100)
self.epsilon_range = rospy.get_param("~epsilon_range", (0.01, 0.1))
self.epsilon_step = rospy.get_param("~epsilon_step", 0.01)
self.shape_match_thresh = rospy.get_param("~shape_match_thresh", 0.4)
self.min_found_count = rospy.get_param("~min_found_count", 10)
self.timeout_seconds = rospy.get_param("~timeout_seconds", 2.0)
# Default to scale model of path marker. Please use set_geometry service
# to set to correct model of object.
length = rospy.get_param("~length", 1.2192)
width = rospy.get_param("~width", 0.1524)
self.rect_model = RectFinder(length, width)
self.do_3D = rospy.get_param("~do_3D", True)
camera = rospy.get_param("~image_topic", "/camera/down/left/image_rect_color")
self.tf_listener = tf.TransformListener()
# Create kalman filter to track 3d position and direction vector for marker in /map frame
self.state_size = 5 # X, Y, Z, DY, DX
self.filter = cv2.KalmanFilter(self.state_size, self.state_size)
self.filter.transitionMatrix = 1. * np.eye(self.state_size, dtype=np.float32)
self.filter.measurementMatrix = 1. * np.eye(self.state_size, dtype=np.float32)
self.filter.processNoiseCov = 1e-5 * np.eye(self.state_size, dtype=np.float32)
self.filter.measurementNoiseCov = 1e-4 * np.eye(self.state_size, dtype=np.float32)
self.filter.errorCovPost = 1. * np.eye(self.state_size, dtype=np.float32)
self.reset()
self.service_set_geometry = rospy.Service('~set_geometry', SetGeometry, self._set_geometry_cb)
if self.debug_ros:
self.debug_pub = Image_Publisher("~debug_image")
self.markerPub = rospy.Publisher('~marker', Marker, queue_size=10)
self.service2D = rospy.Service('~2D', VisionRequest2D, self._vision_cb_2D)
if self.do_3D:
self.service3D = rospy.Service('~pose', VisionRequest, self._vision_cb_3D)
self.toggle = rospy.Service('~enable', SetBool, self._enable_cb)
self.image_sub = Image_Subscriber(camera, self._img_cb)
self.camera_info = self.image_sub.wait_for_camera_info()
assert self.camera_info is not None
self.cam = PinholeCameraModel()
self.cam.fromCameraInfo(self.camera_info)
def store_image_info_cb(msg, topic_name):
''' Stores the topic name, pinhole camera model object, and
matrices from the camera to the map in pickle fiels.
'''
import pickle
import os
import rospkg
print "Storing info"
pinhole_camera_model = PinholeCameraModel()
pinhole_camera_model.fromCameraInfo(msg)
matrices = PixelConversion.get_matrices(msg.header.frame_id, "/map")
filename = topic_name.replace("/", "_") + ".pickle"
rospack = rospkg.RosPack()
pkg_path = rospack.get_path('contamination_monitor')
directory = os.path.join(pkg_path, "eval", "camera_map_tfs")
if not os.path.exists(directory):
os.makedirs(directory)
filepath = os.path.join(directory, filename)
with open(filepath, 'wb+') as f:
pickle.dump(topic_name, f)
pickle.dump(pinhole_camera_model, f)
pickle.dump(matrices, f)
print "pickle dumped to ", filepath
def __init__(self):
self.file = os.environ['HOME'] + '/ar_tag_corners_' + str(rospy.Time.now().to_nsec()) + '.pickle'
self.lis = tf.TransformListener()
self.bridge = CvBridge()
self.model = PinholeCameraModel()
self.need_info = True
rospy.Subscriber('/camera/rgb/camera_info', CameraInfo, self.info_callback)
self.need_bounds = True
rospy.Subscriber('/object_bounds', PolygonStamped, self.bounds_callback)
while self.need_info and self.need_bounds:
rospy.sleep(0.01) # block until have CameraInfo and object bounds
self.corners = []
rospy.Subscriber('/camera/rgb/image_color', Image, self.image_callback, queue_size=1)
def __init__(self):
self.tf_listener = tf.TransformListener()
self.enabled = False
self.last_image = None
self.last_image_time = None
self.camera_model = None
self.circle_finder = CircleFinder(1.0) # Model radius doesn't matter beacause it's not being used for 3D pose
# Various constants for tuning, debugging. See buoy_finder.yaml for more info
self.min_observations = rospy.get_param('~min_observations')
self.debug_ros = rospy.get_param('~debug/ros', True)
self.debug_cv = rospy.get_param('~debug/cv', False)
self.min_contour_area = rospy.get_param('~min_contour_area')
self.max_circle_error = rospy.get_param('~max_circle_error')
self.max_velocity = rospy.get_param('~max_velocity')
self.roi_y = rospy.get_param('~roi_y')
self.roi_height = rospy.get_param('~roi_height')
camera = rospy.get_param('~camera_topic', '/camera/front/right/image_rect_color')
self.buoys = {}
for color in ['red', 'yellow', 'green']:
self.buoys[color] = Buoy(color, debug_cv=self.debug_cv)
if self.debug_cv:
cv2.waitKey(1)
self.debug_images = {}
# Keep latest odom message for sanity check
self.last_odom = None
self.odom_sub = rospy.Subscriber('/odom', Odometry, self.odom_cb, queue_size=3)
self.image_sub = Image_Subscriber(camera, self.image_cb)
if self.debug_ros:
self.rviz = rviz.RvizVisualizer(topic='~markers')
self.mask_pub = Image_Publisher('~mask_image')
rospy.Timer(rospy.Duration(1), self.print_status)
self.camera_info = self.image_sub.wait_for_camera_info()
self.camera_model = PinholeCameraModel()
self.camera_model.fromCameraInfo(self.camera_info)
self.frame_id = self.camera_model.tfFrame()
self.multi_obs = MultiObservation(self.camera_model)
rospy.Service('~enable', SetBool, self.toggle_search)
rospy.Service('~2D', VisionRequest2D, self.request_buoy)
rospy.Service('~pose', VisionRequest, self.request_buoy3d)
rospy.loginfo("BUOY FINDER: initialized successfully")
def __init__(self, depthThreshold, image_topic):
self.listener = tf.TransformListener()
self.image_sub = rospy.Subscriber(image_topic, Image, self.image_callback)
self.info_sub = rospy.Subscriber("/camera/rgb/camera_info", CameraInfo, self.camera_callback)
self.depth_sub = rospy.Subscriber("/camera/depth/image_raw", Image, self.depth_callback)
self.depth_camera_info = rospy.Subscriber("/camera/depth/camera_info", CameraInfo, self.depthcamera_callback)
self.marker_pub = rospy.Publisher('pose_markers', PoseMarkers)
self.camModel = PinholeCameraModel()
#bridge to convert the image to cvMAT.
self.bridge = CvBridge()
#Do we want to try for occlusion? This isn't working properly so it's disabled via the script and not launch file for the time being.
self.doOcclusion = False
#the maximum distance between a point's depth in the camera and a marker's depth we wish to allow.
self.threshold = depthThreshold
def __init__(self, my_tf, debug, nh):
"""Initialize ScanTheCodePerception class."""
self.image_cache = deque()
self.bridge = CvBridge()
self.nh = nh
self.last_cam_info = None
self.debug = debug
self.pers_points = []
self.model_tracker = ScanTheCodeModelTracker()
self.camera_model = PinholeCameraModel()
self.my_tf = my_tf
self.rect_finder = RectangleFinderClustering()
self.color_finder = ColorFinder()
self.count = 0
self.depths = []
def __init__(self):
info_topic = camera_root + "/camera_info"
image_topic = camera_root + "/image_raw" # Change this to rect on boat
self.tf_listener = tf.TransformListener()
# Map id => [{color, error}, ...] for determining when a color is good
self.colored = {}
db_request = rospy.ServiceProxy("/database/requests", ObjectDBQuery)
self.make_request = lambda **kwargs: db_request(ObjectDBQueryRequest(**kwargs))
rospy.Service("/vision/buoy_colorizer", VisionRequest, self.got_request)
self.image_history = ImageHistory(image_topic)
# Wait for camera info, and exit if not found
try:
fprint("Waiting for camera info on: '{}'".format(info_topic))
camera_info_msg = rospy.wait_for_message(info_topic, CameraInfo, timeout=3)
except rospy.exceptions.ROSException:
fprint("Camera info not found! Terminating.", msg_color="red")
rospy.signal_shutdown("Camera not found!")
return
fprint("Camera info found!", msg_color="green")
self.camera_model = PinholeCameraModel()
self.camera_model.fromCameraInfo(camera_info_msg)
self.debug = DebugImage("/colorama/debug", prd=.5)
# These are color mappings from Hue values [0, 360]
self.color_map = {'red': np.radians(0), 'yellow': np.radians(60),
'green': np.radians(120), 'blue': np.radians(240)}
# Some tunable parameters
self.update_time = .5 # s
self.saturation_reject = 50
self.value_reject = 50
self.hue_error_reject = .4 # rads
# To decide when to accept color observations
self.error_tolerance = .4 # rads
self.spottings_req = 4
self.similarity_reject = .1 # If two colors are within this amount of error, reject
rospy.Timer(ros_t(self.update_time), self.do_update)
