<commit_before># -*- coding: utf-8 -*-
"""
Defines views.
"""

import calendar

from flask import redirect, render_template, url_for
from flask.helpers import make_response
from jinja2.exceptions import TemplateNotFound

from presence_analyzer.main import app
from presence_analyzer import utils

import logging
log = logging.getLogger(__name__)  # pylint: disable-msg=C0103

@app.route('/')
def mainpage():
    """
    Redirects to front page.
    """
    return redirect(
        url_for('template_view', template_name='presence_weekday')
    )

@app.route('/api/v1/users', methods=['GET'])
@utils.jsonify
def users_view():
    """
    Users listing for dropdown.
    """
    data = utils.get_data()
    return [{'user_id': i, 'name': 'User {0}'.format(str(i))}
            for i in data.keys()]

@app.route('/api/v1/mean_time_weekday/<int:user_id>', methods=['GET'])
@utils.jsonify
def mean_time_weekday_view(user_id):
    """
    Returns mean presence time of given user grouped by weekday.
    """
    data = utils.get_data()
    if user_id not in data:
        log.debug('User %s not found!', user_id)
        return []

    weekdays = utils.group_by_weekday(data[user_id])
    result = [(calendar.day_abbr[weekday], utils.mean(intervals))
              for weekday, intervals in weekdays.items()]

    return result

@app.route('/api/v1/presence_weekday/<int:user_id>', methods=['GET'])
@utils.jsonify
def presence_weekday_view(user_id):
    """
    Returns total presence time of given user grouped by weekday.
    """
    data = utils.get_data()
    if user_id not in data:
        log.debug('User %s not found!', user_id)
        return []

    weekdays = utils.group_by_weekday(data[user_id])
    result = [(calendar.day_abbr[weekday], sum(intervals))
              for weekday, intervals in weekdays.items()]

    result.insert(0, ('Weekday', 'Presence (s)'))
    return result

@app.route('/api/v1/presence_start_end/<int:user_id>', methods=['GET'])
@utils.jsonify
def presence_start_end(user_id):
    """
    Returns mean presence time of given user
    """
    data = utils.get_data()
    if user_id not in data:
        log.debug('User %s not found!', user_id)
        return []

    weekdays = utils.group_by_weekday_in_secs(data[user_id])
    result = [(calendar.day_abbr[weekday],
              utils.mean(mean_per_day['start']),
              utils.mean(mean_per_day['end']))
              for weekday, mean_per_day in weekdays.items()]
    return result

@app.route('/<string:template_name>', methods=['GET'])
def template_view(template_name):
    """
    Renders a proper template based on template NAME given in request params.
    """
    try:
        return render_template("{}.html".format(template_name))
    except TemplateNotFound:
        return make_response("Strona o podanym adresie nie istnieje")
<commit_msg>changed comment text<commit_after># -*- coding: utf-8 -*-
"""
Defines views.
"""

import calendar

from flask import redirect, render_template, url_for
from flask.helpers import make_response
from jinja2.exceptions import TemplateNotFound

from presence_analyzer.main import app
from presence_analyzer import utils

import logging
log = logging.getLogger(__name__)  # pylint: disable-msg=C0103

@app.route('/')
def mainpage():
    """
    Redirects to front page.
    """
    return redirect(
        url_for('template_view', template_name='presence_weekday')
    )

@app.route('/api/v1/users', methods=['GET'])
@utils.jsonify
def users_view():
    """
    Users listing for dropdown.
    """
    data = utils.get_data()
    return [{'user_id': i, 'name': 'User {0}'.format(str(i))}
            for i in data.keys()]

@app.route('/api/v1/mean_time_weekday/<int:user_id>', methods=['GET'])
@utils.jsonify
def mean_time_weekday_view(user_id):
    """
    Returns mean presence time of given user grouped by weekday.
    """
    data = utils.get_data()
    if user_id not in data:
        log.debug('User %s not found!', user_id)
        return []

    weekdays = utils.group_by_weekday(data[user_id])
    result = [(calendar.day_abbr[weekday], utils.mean(intervals))
              for weekday, intervals in weekdays.items()]

    return result

@app.route('/api/v1/presence_weekday/<int:user_id>', methods=['GET'])
@utils.jsonify
def presence_weekday_view(user_id):
    """
    Returns total presence time of given user grouped by weekday.
    """
    data = utils.get_data()
    if user_id not in data:
        log.debug('User %s not found!', user_id)
        return []

    weekdays = utils.group_by_weekday(data[user_id])
    result = [(calendar.day_abbr[weekday], sum(intervals))
              for weekday, intervals in weekdays.items()]

    result.insert(0, ('Weekday', 'Presence (s)'))
    return result

@app.route('/api/v1/presence_start_end/<int:user_id>', methods=['GET'])
@utils.jsonify
def presence_start_end(user_id):
    """
    Returns mean presence time of given user
    """
    data = utils.get_data()
    if user_id not in data:
        log.debug('User %s not found!', user_id)
        return []

    weekdays = utils.group_by_weekday_in_secs(data[user_id])
    result = [(calendar.day_abbr[weekday],
              utils.mean(mean_per_day['start']),
              utils.mean(mean_per_day['end']))
              for weekday, mean_per_day in weekdays.items()]
    return result

@app.route('/<string:template_name>', methods=['GET'])
def template_view(template_name):
    """
    Renders a proper template based on template name given in request params.
    """
    try:
        return render_template("{}.html".format(template_name))
    except TemplateNotFound:
        return make_response("Strona o podanym adresie nie istnieje")
<|endoftext|>


<commit_before># -*- coding: utf-8 -*-
"""
Defines views.
"""

import calendar
from flask import redirect

from presence_analyzer.main import app
from presence_analyzer.utils import jsonify, get_data, mean, group_by_weekday, seconds_since_midnight, group_start_end_by_weekday

import logging
log = logging.getLogger(__name__)  # pylint: disable-msg=C0103

@app.route('/')
def mainpage():
    """
    Redirects to front page.
    """
    return redirect('/static/presence_weekday.html')

@app.route('/api/v1/users', methods=['GET'])
@jsonify
def users_view():
    """
    Users listing for dropdown.
    """
    data = get_data()
    return [{'user_id': i, 'name': 'User {0}'.format(str(i))}
            for i in data.keys()]

@app.route('/api/v1/mean_time_weekday/<int:user_id>', methods=['GET'])
@jsonify
def mean_time_weekday_view(user_id):
    """
    Returns mean presence time of given user grouped by weekday.
    """
    data = get_data()
    if user_id not in data:
        log.debug('User %s not found!', user_id)
        return []

    weekdays = group_by_weekday(data[user_id])
    result = [(calendar.day_abbr[weekday], mean(intervals))
              for weekday, intervals in weekdays.items()]

    return result

@app.route('/api/v1/presence_weekday/<int:user_id>', methods=['GET'])
@jsonify
def presence_weekday_view(user_id):
    """
    Returns total presence time of given user grouped by weekday.
    """
    data = get_data()
    if user_id not in data:
        log.debug('User %s not found!', user_id)
        return []

    weekdays = group_by_weekday(data[user_id])
    result = [(calendar.day_abbr[weekday], sum(intervals))
              for weekday, intervals in weekdays.items()]

    result.insert(0, ('Weekday', 'Presence (s)'))
    return result

@app.route('/api/v1/presence_start_end/<int:user_id>', methods=['GET'])
@jsonify
def presence_start_end_view(user_id):
    """
    Returns mean start and end time of work for user
    """
    data = get_data()
    if user_id not in data:
        log.debug('User %s not found!', user_id)
        return []

    weekdays = group_start_end_by_weekday(data[user_id])

    result = [
                [
                    calendar.day_abbr[day_number],
                    mean([seconds_since_midnight(start) for start in day['start']]),
                    mean([seconds_since_midnight(end) for end in day['end']])
                ]
             for day_number, day in weekdays.items()]

    return result
<commit_msg>pep8 fixed<commit_after># -*- coding: utf-8 -*-
"""
Defines views.
"""

import calendar
from flask import redirect

from presence_analyzer.main import app
from presence_analyzer.utils import jsonify, get_data, mean, group_by_weekday,
seconds_since_midnight,
group_start_end_by_weekday

import logging
log = logging.getLogger(__name__)  # pylint: disable-msg=C0103

@app.route('/')
def mainpage():
    """
    Redirects to front page.
    """
    return redirect('/static/presence_weekday.html')

@app.route('/api/v1/users', methods=['GET'])
@jsonify
def users_view():
    """
    Users listing for dropdown.
    """
    data = get_data()
    return [{'user_id': i, 'name': 'User {0}'.format(str(i))}
            for i in data.keys()]

@app.route('/api/v1/mean_time_weekday/<int:user_id>', methods=['GET'])
@jsonify
def mean_time_weekday_view(user_id):
    """
    Returns mean presence time of given user grouped by weekday.
    """
    data = get_data()
    if user_id not in data:
        log.debug('User %s not found!', user_id)
        return []

    weekdays = group_by_weekday(data[user_id])
    result = [(calendar.day_abbr[weekday], mean(intervals))
              for weekday, intervals in weekdays.items()]

    return result

@app.route('/api/v1/presence_weekday/<int:user_id>', methods=['GET'])
@jsonify
def presence_weekday_view(user_id):
    """
    Returns total presence time of given user grouped by weekday.
    """
    data = get_data()
    if user_id not in data:
        log.debug('User %s not found!', user_id)
        return []

    weekdays = group_by_weekday(data[user_id])
    result = [(calendar.day_abbr[weekday], sum(intervals))
              for weekday, intervals in weekdays.items()]

    result.insert(0, ('Weekday', 'Presence (s)'))
    return result

@app.route('/api/v1/presence_start_end/<int:user_id>', methods=['GET'])
@jsonify
def presence_start_end_view(user_id):
    """
    Returns mean start and end time of work for user
    """
    data = get_data()
    if user_id not in data:
        log.debug('User %s not found!', user_id)
        return []

    weekdays = group_start_end_by_weekday(data[user_id])

    result = [[
        calendar.day_abbr[day_number],
        mean([seconds_since_midnight(start) for start in day['start']]),
        mean([seconds_since_midnight(end) for end in day['end']])
    ] for day_number, day in weekdays.items()]

    return result
<|endoftext|>


<commit_before>#!/usr/bin/env python
# vim: set ts=4 sw=4 et:

import time

import cv2
import numpy
import rospy

import geometry_msgs.msg
import sensor_msgs.msg

from lander.drivers.camera import OpenCVCamera, SimulatedCamera
from lander.lib.kalman import KalmanFilter
from lander.msg import TrackStamped

# Primary colors (BGR)
BLUE  = (255, 0, 0)
GREEN = (0, 255, 0)
RED   = (0, 0, 255)

class TrackerNode(object):
    """
    The Tracker node is responsible for vision-based tracking of the landing pad.
    """

    def __init__(self):
        """
        Initialize the camera and subscribe to mavros topics.
        """
        rospy.init_node("tracker")

        camera_matrix = numpy.matrix(rospy.get_param("~camera_matrix"))
        use_sim = rospy.get_param("~use_sim", False)

        if use_sim:
            self.camera = SimulatedCamera(camera_matrix)
            self.camera.set_target(
                    image_file=rospy.get_param("~target_image"),
                    position=rospy.get_param("~target_position"),
                    size_in_meters=rospy.get_param("~target_size"))
        else:
            self.camera = OpenCVCamera(camera_matrix)

        self.tracking = False
        self.last_frame_time = 0
        self.last_seen_time = 0
        self.initialize_track_filter()

        self.image_publisher = rospy.Publisher("tracker/image",
                sensor_msgs.msg.Image, queue_size=1)

        self.track_publisher = rospy.Publisher("tracker/track",
                TrackStamped, queue_size=1)

    def initialize_track_filter(self, x=0, vx=0, y=0, vy=0, z=0, vz=0, p=10, q=1, r=1):
        """
        Construct a Kalman filter for a discretized continuous-time 3D kinematic model.

        From: Estimation with Applications to Tracking and Navigation (Bar-Shalom)
        """
        # Process model (position and velocity terms)
        F = lambda dt: numpy.matrix([
            [  1, dt,  0,  0,  0,  0  ],  # x' = x + vx*dt
            [  0,  1,  0,  0,  0,  0  ],  # vx' = vx
            [  0,  0,  1, dt,  0,  0  ],  # y' = y + vy*dt
            [  0,  0,  0,  1,  0,  0  ],  # vy' = vy
            [  0,  0,  0,  0,  1, dt  ],  # z' = z + vz*dt
            [  0,  0,  0,  0,  0,  1  ],  # vz' = vz
        ])

        # Control model (TODO)
        B = 0

        # Measurement model
        H = numpy.matrix([
            [  1, 0, 0, 0, 0, 0  ], # x
            [  0, 0, 1, 0, 0, 0  ], # y
            [  0, 0, 0, 0, 1, 0  ], # z
        ])

        # Initial state vector
        x0 = numpy.matrix([ x, vx, y, vy, z, vz ]).T

        # Initial state covariance
        P0 = numpy.matrix(numpy.eye(6)) * p

        # Process error covariance (continuous white noise acceleration model)
        Q = lambda dt: numpy.matrix([
            [  dt**3/3.0,  dt**2/2.0,          0,          0,          0,          0  ],
            [  dt**2/2.0,         dt,          0,          0,          0,          0  ],
            [          0,          0,  dt**3/3.0,  dt**2/2.0,          0,          0  ],
            [          0,          0,  dt**2/2.0,         dt,          0,          0  ],
            [          0,          0,          0,          0,  dt**3/3.0,  dt**2/2.0  ],
            [          0,          0,          0,          0,  dt**2/2.0,         dt  ],
        ]) * q

        # Measurement error covariance
        R = numpy.matrix(numpy.eye(3)) * r

        self.track_filter = KalmanFilter(F, B, H, x0, P0, Q, R)

    def publish_track(self, position, velocity):
        """
        Publish a TrackStamped message containing relative position
        (and eventually velocity) of the tracked object.
        """
        msg = TrackStamped()
        msg.track.tracking.data = self.tracking

        if self.tracking:
            msg.track.position.x = position[0]
            msg.track.position.y = position[1]
            msg.track.position.z = position[2]
            msg.track.velocity.x = velocity[0]
            msg.track.velocity.y = velocity[1]
            msg.track.velocity.z = velocity[2]

        self.track_publisher.publish(msg)

    def publish_image(self, image):
        """
        Construct and publish an Image message for the given image.
        """
        # TODO: Publish monochrome image + metadata to minimize telemetry bandwidth
        # TODO: Use ROS image_transport?
        msg = sensor_msgs.msg.Image()
        msg.height   = image.shape[0]
        msg.width    = image.shape[1]
        msg.encoding = "mono8" if image.ndim == 2 else "bgr8"
        msg.step     = msg.width if image.ndim == 2 else msg.width * 3
        msg.data     = image.reshape(-1).tolist()
        self.image_publisher.publish(msg)

    def process_frame(self, frame):
        """
        Find and track the landing pad.
        """
        now = time.time()

        # Compute the time step (dt) between frames
        dt = now - self.last_frame_time if self.last_frame_time else 0
        self.last_frame_time = now

        # Extract target position from the frame
        raw_position = self.detect_target(frame)

        # Determine whether we see the target now or whether we've seen it recently
        see_currently = raw_position is not None
        seen_recently = now - self.last_seen_time < 2

        if see_currently: self.last_seen_time = now

        # Reset filter if this is the first time we've seen the target recently
        if see_currently and not seen_recently and not self.tracking:
            # TODO: Tune q and r
            x, y, z = raw_position
            self.initialize_track_filter(x=x, y=y, z=z)

        # Time update
        if self.tracking or (see_currently and seen_recently):
            self.track_filter.predict(dt=dt)

        # Measurement update
        if see_currently and (seen_recently or self.tracking):
            self.track_filter.update(raw_position)

        # Extract position and velocity from filter state vector
        position = self.track_filter.x[[0, 2, 4], 0].T.tolist()[0]
        velocity = self.track_filter.x[[1, 3, 5], 0].T.tolist()[0]

        # Determine whether we are certain about the target position
        certain = (self.track_filter.P < 2).all()

        if certain and not self.tracking:
            rospy.loginfo("Found target at (%6.4f, %6.4f, %6.4f)", *position)
        elif not certain and self.tracking:
            rospy.loginfo("Lost target")

        self.tracking = certain

        self.publish_track(position, velocity)

    def detect_target(self, frame):
        """
        Detect the landing pad target in the camera frame and compute its center
        in body-relative camera coordinates.

        Returns a (dx, dy, dz) tuple giving the body-relative location of the
        target center, in meters, or None if the target could not be found.
        """
        frame = cv2.GaussianBlur(frame, (5,5), 0)

        # NB: OpenCV's HoughCircles routine excludes concentric circles, so we
        #     can't do anything fancy with radii ratio to validate the signals.
        circles = cv2.HoughCircles(frame, cv2.cv.CV_HOUGH_GRADIENT, 1.5, 100)

        # Publish telemetry image
        self.publish_image(draw_circles(frame, circles))

        # Fail fast if we didn't detect any circles
        if circles is None or len(circles) == 0: return

        # Extract the pixel coordinates of the strongest circle center
        target_xy_pixels = circles[0,0,:2]

        # Back-project the pixel coordinates to body-relative camera coordinates
        target_xyz_camera = self.camera.back_project(*target_xy_pixels)

        return target_xyz_camera

    def wait_for_position(self):
        """
        Wait until we start receiving position messages.
        """
        while not rospy.is_shutdown() and self.camera.position is None:
            rospy.sleep(1)

    def run(self):
        """
        Capture frames from the camera and publish them.
        """
        self.wait_for_position()

        while not rospy.is_shutdown():
            frame = self.camera.get_frame()
            self.process_frame(frame)

        self.camera.release()

def draw_circles(image, circles):
    output = cv2.cvtColor(image, cv2.COLOR_GRAY2BGR)

    if circles is not None:
        circles = numpy.round(circles[0, :]).astype("int")

        for x, y, r in circles:
            cv2.circle(output, (x, y), r, GREEN, 4)
            cv2.rectangle(output, (x - 3, y - 3), (x + 3, y + 3), RED, -1)

    return output

if __name__ == "__main__":
    node = TrackerNode()
    node.run()

<commit_msg>tracker: don't publish telemetry images (for now)<commit_after>#!/usr/bin/env python
# vim: set ts=4 sw=4 et:

import time

import cv2
import numpy
import rospy

import geometry_msgs.msg
import sensor_msgs.msg

from lander.drivers.camera import OpenCVCamera, SimulatedCamera
from lander.lib.kalman import KalmanFilter
from lander.msg import TrackStamped

# Primary colors (BGR)
BLUE  = (255, 0, 0)
GREEN = (0, 255, 0)
RED   = (0, 0, 255)

class TrackerNode(object):
    """
    The Tracker node is responsible for vision-based tracking of the landing pad.
    """

    def __init__(self):
        """
        Initialize the camera and subscribe to mavros topics.
        """
        rospy.init_node("tracker")

        camera_matrix = numpy.matrix(rospy.get_param("~camera_matrix"))
        use_sim = rospy.get_param("~use_sim", False)

        if use_sim:
            self.camera = SimulatedCamera(camera_matrix)
            self.camera.set_target(
                    image_file=rospy.get_param("~target_image"),
                    position=rospy.get_param("~target_position"),
                    size_in_meters=rospy.get_param("~target_size"))
        else:
            self.camera = OpenCVCamera(camera_matrix)

        self.tracking = False
        self.last_frame_time = 0
        self.last_seen_time = 0
        self.initialize_track_filter()

        self.image_publisher = rospy.Publisher("tracker/image",
                sensor_msgs.msg.Image, queue_size=1)

        self.track_publisher = rospy.Publisher("tracker/track",
                TrackStamped, queue_size=1)

    def initialize_track_filter(self, x=0, vx=0, y=0, vy=0, z=0, vz=0, p=10, q=1, r=1):
        """
        Construct a Kalman filter for a discretized continuous-time 3D kinematic model.

        From: Estimation with Applications to Tracking and Navigation (Bar-Shalom)
        """
        # Process model (position and velocity terms)
        F = lambda dt: numpy.matrix([
            [  1, dt,  0,  0,  0,  0  ],  # x' = x + vx*dt
            [  0,  1,  0,  0,  0,  0  ],  # vx' = vx
            [  0,  0,  1, dt,  0,  0  ],  # y' = y + vy*dt
            [  0,  0,  0,  1,  0,  0  ],  # vy' = vy
            [  0,  0,  0,  0,  1, dt  ],  # z' = z + vz*dt
            [  0,  0,  0,  0,  0,  1  ],  # vz' = vz
        ])

        # Control model (TODO)
        B = 0

        # Measurement model
        H = numpy.matrix([
            [  1, 0, 0, 0, 0, 0  ], # x
            [  0, 0, 1, 0, 0, 0  ], # y
            [  0, 0, 0, 0, 1, 0  ], # z
        ])

        # Initial state vector
        x0 = numpy.matrix([ x, vx, y, vy, z, vz ]).T

        # Initial state covariance
        P0 = numpy.matrix(numpy.eye(6)) * p

        # Process error covariance (continuous white noise acceleration model)
        Q = lambda dt: numpy.matrix([
            [  dt**3/3.0,  dt**2/2.0,          0,          0,          0,          0  ],
            [  dt**2/2.0,         dt,          0,          0,          0,          0  ],
            [          0,          0,  dt**3/3.0,  dt**2/2.0,          0,          0  ],
            [          0,          0,  dt**2/2.0,         dt,          0,          0  ],
            [          0,          0,          0,          0,  dt**3/3.0,  dt**2/2.0  ],
            [          0,          0,          0,          0,  dt**2/2.0,         dt  ],
        ]) * q

        # Measurement error covariance
        R = numpy.matrix(numpy.eye(3)) * r

        self.track_filter = KalmanFilter(F, B, H, x0, P0, Q, R)

    def publish_track(self, position, velocity):
        """
        Publish a TrackStamped message containing relative position
        (and eventually velocity) of the tracked object.
        """
        msg = TrackStamped()
        msg.track.tracking.data = self.tracking

        if self.tracking:
            msg.track.position.x = position[0]
            msg.track.position.y = position[1]
            msg.track.position.z = position[2]
            msg.track.velocity.x = velocity[0]
            msg.track.velocity.y = velocity[1]
            msg.track.velocity.z = velocity[2]

        self.track_publisher.publish(msg)

    def publish_image(self, image):
        """
        Construct and publish an Image message for the given image.
        """
        # TODO: Publish monochrome image + metadata to minimize telemetry bandwidth
        # TODO: Use ROS image_transport?
        msg = sensor_msgs.msg.Image()
        msg.height   = image.shape[0]
        msg.width    = image.shape[1]
        msg.encoding = "mono8" if image.ndim == 2 else "bgr8"
        msg.step     = msg.width if image.ndim == 2 else msg.width * 3
        msg.data     = image.reshape(-1).tolist()
        self.image_publisher.publish(msg)

    def process_frame(self, frame):
        """
        Find and track the landing pad.
        """
        now = time.time()

        # Compute the time step (dt) between frames
        dt = now - self.last_frame_time if self.last_frame_time else 0
        self.last_frame_time = now

        # Extract target position from the frame
        raw_position = self.detect_target(frame)

        # Determine whether we see the target now or whether we've seen it recently
        see_currently = raw_position is not None
        seen_recently = now - self.last_seen_time < 2

        if see_currently: self.last_seen_time = now

        # Reset filter if this is the first time we've seen the target recently
        if see_currently and not seen_recently and not self.tracking:
            # TODO: Tune q and r
            x, y, z = raw_position
            self.initialize_track_filter(x=x, y=y, z=z)

        # Time update
        if self.tracking or (see_currently and seen_recently):
            self.track_filter.predict(dt=dt)

        # Measurement update
        if see_currently and (seen_recently or self.tracking):
            self.track_filter.update(raw_position)

        # Extract position and velocity from filter state vector
        position = self.track_filter.x[[0, 2, 4], 0].T.tolist()[0]
        velocity = self.track_filter.x[[1, 3, 5], 0].T.tolist()[0]

        # Determine whether we are certain about the target position
        certain = (self.track_filter.P < 2).all()

        if certain and not self.tracking:
            rospy.loginfo("Found target at (%6.4f, %6.4f, %6.4f)", *position)
        elif not certain and self.tracking:
            rospy.loginfo("Lost target")

        self.tracking = certain

        self.publish_track(position, velocity)

    def detect_target(self, frame):
        """
        Detect the landing pad target in the camera frame and compute its center
        in body-relative camera coordinates.

        Returns a (dx, dy, dz) tuple giving the body-relative location of the
        target center, in meters, or None if the target could not be found.
        """
        frame = cv2.GaussianBlur(frame, (5,5), 0)

        # NB: OpenCV's HoughCircles routine excludes concentric circles, so we
        #     can't do anything fancy with radii ratio to validate the signals.
        circles = cv2.HoughCircles(frame, cv2.cv.CV_HOUGH_GRADIENT, 1.5, 100)

        # Publish telemetry image
        # self.publish_image(draw_circles(frame, circles))

        # Fail fast if we didn't detect any circles
        if circles is None or len(circles) == 0: return

        # Extract the pixel coordinates of the strongest circle center
        target_xy_pixels = circles[0,0,:2]

        # Back-project the pixel coordinates to body-relative camera coordinates
        target_xyz_camera = self.camera.back_project(*target_xy_pixels)

        return target_xyz_camera

    def wait_for_position(self):
        """
        Wait until we start receiving position messages.
        """
        while not rospy.is_shutdown() and self.camera.position is None:
            rospy.sleep(1)

    def run(self):
        """
        Capture frames from the camera and publish them.
        """
        self.wait_for_position()

        while not rospy.is_shutdown():
            frame = self.camera.get_frame()
            self.process_frame(frame)

        self.camera.release()

def draw_circles(image, circles):
    output = cv2.cvtColor(image, cv2.COLOR_GRAY2BGR)

    if circles is not None:
        circles = numpy.round(circles[0, :]).astype("int")

        for x, y, r in circles:
            cv2.circle(output, (x, y), r, GREEN, 4)
            cv2.rectangle(output, (x - 3, y - 3), (x + 3, y + 3), RED, -1)

    return output

if __name__ == "__main__":
    node = TrackerNode()
    node.run()

<|endoftext|>


<commit_before>import os
import subprocess
import sys
from pyshark.tshark.tshark import get_tshark_path
from pyshark.tshark.tshark_xml import packet_from_xml_packet, psml_structure_from_xml

class TSharkCrashException(Exception):
    pass

class UnknownEncyptionStandardException(Exception):
    pass

class Capture(object):
    """
    Base class for packet captures.
    """
    SUPPORTED_ENCRYPTION_STANDARDS=('wep', 'wpa-pwd', 'wpa-psk')

    def __init__(self, display_filter=None, only_summaries=False,
                 decryption_key=None, encryption_type='wpa-pwd'):
        self._packets = []
        self.current_packet = 0
        self.display_filter = display_filter
        self.only_summaries = only_summaries
        self.tshark_process = None
        if encryption_type and encryption_type.lower() in self.SUPPORTED_ENCRYPTION_STANDARDS:
            self.encryption=(decryption_key, encryption_type.lower())
        else:
            encryption_standards = "', '".join(self.SUPPORTED_ENCRYPTION_STANDARDS[:-1]) + "', and '" + self.SUPPORTED_ENCRYPTION_STANDARDS[-1]
            raise UnknownEncyptionStandardException("please choose between '" + encryption_standards + "'.")

    def __getitem__(self, item):
        """
        Gets the packet in the given index.

        :param item: packet index
        :return: Packet object.
        """
        return self._packets[item]

    def __len__(self):
        return len(self._packets)

    def next(self):
        return self.next_packet()

    # Allows for child classes to call next() from super() without 2to3 "fixing"
    # the call
    def next_packet(self):
        if self.current_packet >= len(self._packets):
            raise StopIteration()
        cur_packet = self._packets[self.current_packet]
        self.current_packet += 1
        return cur_packet

    def clear(self):
        """
        Empties the capture of any saved packets.
        """
        self._packets = []
        self.current_packet = 0

    def reset(self):
        """
        Starts iterating packets from the first one.
        """
        self.current_packet = 0

    @staticmethod
    def _extract_tag_from_data(data, tag_name='packet'):
        """
        Gets data containing a (part of) tshark xml.
        If the given tag is found in it, returns the tag data and the remaining data.
        Otherwise returns None and the same data.

        :param data: string of a partial tshark xml.
        :return: a tuple of (tag, data). tag will be None if none is found.
        """
        opening_tag, closing_tag = b'<%s>' % tag_name, b'</%s>' % tag_name
        tag_end = data.find(closing_tag)
        if tag_end != -1:
            tag_end += len(closing_tag)
            tag_start = data.find(opening_tag)
            return data[tag_start:tag_end], data[tag_end:]
        return None, data

    def _packets_from_fd(self, fd, previous_data=b'', packet_count=None, wait_for_more_data=True, batch_size=4096):
        """
        Reads packets from a file-like object containing a TShark XML.
        Returns a generator.

        :param fd: A file-like object containing a TShark XML
        :param previous_data: Any data to put before the file.
        :param packet_count: A maximum amount of packets to stop after.
        :param wait_for_more_data: Whether to wait for more data or stop when
            none is available (i.e. when the fd is a standard file)
        """
        data = previous_data
        packets_captured = 0
        psml_struct = None

        if self.only_summaries:
            # If summaries are read, we need the psdml structure which appears on top of the file.
            while not psml_struct:
                data += fd.read(batch_size)
                psml_struct, data = self._extract_tag_from_data(data, 'structure')
                psml_struct = psml_structure_from_xml(psml_struct)

        while True:
            # Read data until we get a packet, and yield it.
            new_data = fd.read(batch_size)
            data += new_data
            packet, data = self._extract_tag_from_data(data)

            if packet:
                packets_captured += 1
                yield packet_from_xml_packet(packet, psml_structure=psml_struct)

            if packet is None and not wait_for_more_data and len(new_data) < batch_size:
                break

            if packet_count and packets_captured >= packet_count:
                break

    def _set_tshark_process(self, packet_count=None, encryption=None,
                            extra_params=[]):
        """
        Sets the internal tshark to a new tshark process with the
        previously-set paramaters.
        """
        if self.encryption:
            extra_params+=['-o', 'wlan.enable_decryption:TRUE', '-o',
                'uat:80211_keys:"'+self.encryption[1]+'","'+self.encryption[0]+'"']
        xml_type = 'psml' if self.only_summaries else 'pdml'
        parameters = [get_tshark_path(), '-T', xml_type] +\
                     self.get_parameters(packet_count=packet_count) +\
                     extra_params
        # Re-direct TShark's stderr to the null device
        self.tshark_stderr = open(os.devnull, "wb")
        # Start the TShark subprocess
        self.tshark_process = subprocess.Popen(parameters,
                                               stdout=subprocess.PIPE,
                                               stderr=self.tshark_stderr)
        retcode = self.tshark_process.poll()
        if retcode is not None and retcode != 0:
            raise TSharkCrashException('TShark seems to have crashed. Try '+\
                    'updating it. (command ran: "%s")' % ' '.join(parameters))

    def _cleanup_subprocess(self):
        try:
            self.tshark_process.terminate()
        except OSError:
            if 'win' not in sys.platform:
                raise
        self.tshark_process.stdout.close()
        self.tshark_stderr.close()
        self.tshark_process = None

    def get_parameters(self, packet_count=None):
        """
        Returns the special tshark parameters to be used according to the
        configuration of this class.
        """
        params = []
        if self.display_filter:
            params += ['-R', self.display_filter]
        if packet_count:
            params += ['-c', str(packet_count)]
        return params

    def __iter__(self):
        while True:
            yield self.next()

    def __repr__(self):
        return '<%s (%d packets)>' %(self.__class__.__name__, len(self._packets))<commit_msg>Fixed a dumb error where it wouldn't work without encyption. Oops...<commit_after>import os
import subprocess
import sys
from pyshark.tshark.tshark import get_tshark_path
from pyshark.tshark.tshark_xml import packet_from_xml_packet, psml_structure_from_xml

class TSharkCrashException(Exception):
    pass

class UnknownEncyptionStandardException(Exception):
    pass

class Capture(object):
    """
    Base class for packet captures.
    """
    SUPPORTED_ENCRYPTION_STANDARDS=('wep', 'wpa-pwd', 'wpa-psk')

    def __init__(self, display_filter=None, only_summaries=False,
                 decryption_key=None, encryption_type='wpa-pwd'):
        self._packets = []
        self.current_packet = 0
        self.display_filter = display_filter
        self.only_summaries = only_summaries
        self.tshark_process = None
        if not(decryption_key):
            self.encryption=None
        elif  encryption_type and (encryption_type.lower() in
                                   self.SUPPORTED_ENCRYPTION_STANDARDS):
            self.encryption=(decryption_key, encryption_type.lower())
        else:
            encryption_standards = "', '".join(
                    self.SUPPORTED_ENCRYPTION_STANDARDS[:-1]) + "', and '" + \
                    self.SUPPORTED_ENCRYPTION_STANDARDS[-1]
            raise UnknownEncyptionStandardException("please choose between '" +\
                                                    encryption_standards + "'.")

    def __getitem__(self, item):
        """
        Gets the packet in the given index.

        :param item: packet index
        :return: Packet object.
        """
        return self._packets[item]

    def __len__(self):
        return len(self._packets)

    def next(self):
        return self.next_packet()

    # Allows for child classes to call next() from super() without 2to3 "fixing"
    # the call
    def next_packet(self):
        if self.current_packet >= len(self._packets):
            raise StopIteration()
        cur_packet = self._packets[self.current_packet]
        self.current_packet += 1
        return cur_packet

    def clear(self):
        """
        Empties the capture of any saved packets.
        """
        self._packets = []
        self.current_packet = 0

    def reset(self):
        """
        Starts iterating packets from the first one.
        """
        self.current_packet = 0

    @staticmethod
    def _extract_tag_from_data(data, tag_name='packet'):
        """
        Gets data containing a (part of) tshark xml.
        If the given tag is found in it, returns the tag data and the
        remaining data.
        Otherwise returns None and the same data.

        :param data: string of a partial tshark xml.
        :return: a tuple of (tag, data). tag will be None if none is found.
        """
        opening_tag, closing_tag = b'<%s>' % tag_name, b'</%s>' % tag_name
        tag_end = data.find(closing_tag)
        if tag_end != -1:
            tag_end += len(closing_tag)
            tag_start = data.find(opening_tag)
            return data[tag_start:tag_end], data[tag_end:]
        return None, data

    def _packets_from_fd(self, fd, previous_data=b'', packet_count=None,
                         wait_for_more_data=True, batch_size=4096):
        """
        Reads packets from a file-like object containing a TShark XML.
        Returns a generator.

        :param fd: A file-like object containing a TShark XML
        :param previous_data: Any data to put before the file.
        :param packet_count: A maximum amount of packets to stop after.
        :param wait_for_more_data: Whether to wait for more data or stop when
            none is available (i.e. when the fd is a standard file)
        """
        data = previous_data
        packets_captured = 0
        psml_struct = None

        if self.only_summaries:
            # If summaries are read, we need the psdml structure which appears
            # on top of the file.
            while not psml_struct:
                data += fd.read(batch_size)
                psml_struct, data = self._extract_tag_from_data(data,
                                                                'structure')
                psml_struct = psml_structure_from_xml(psml_struct)

        while True:
            # Read data until we get a packet, and yield it.
            new_data = fd.read(batch_size)
            data += new_data
            packet, data = self._extract_tag_from_data(data)

            if packet:
                packets_captured += 1
                yield packet_from_xml_packet(packet, psml_structure=psml_struct)

            if packet is None and not wait_for_more_data and \
                                    len(new_data) < batch_size:
                break

            if packet_count and packets_captured >= packet_count:
                break

    def _set_tshark_process(self, packet_count=None, encryption=None,
                            extra_params=[]):
        """
        Sets the internal tshark to a new tshark process with the
        previously-set paramaters.
        """
        if self.encryption:
            extra_params+=['-o', 'wlan.enable_decryption:TRUE', '-o',
                           'uat:80211_keys:"'+self.encryption[1]+'","'+\
                           self.encryption[0]+'"']
        xml_type = 'psml' if self.only_summaries else 'pdml'
        parameters = [get_tshark_path(), '-T', xml_type] +\
                     self.get_parameters(packet_count=packet_count) +\
                     extra_params
        # Re-direct TShark's stderr to the null device
        self.tshark_stderr = open(os.devnull, "wb")
        # Start the TShark subprocess
        self.tshark_process = subprocess.Popen(parameters,
                                               stdout=subprocess.PIPE,
                                               stderr=self.tshark_stderr)
        retcode = self.tshark_process.poll()
        if retcode is not None and retcode != 0:
            raise TSharkCrashException('TShark seems to have crashed. Try '+\
                    'updating it. (command ran: "%s")' % ' '.join(parameters))

    def _cleanup_subprocess(self):
        try:
            self.tshark_process.terminate()
        except OSError:
            if 'win' not in sys.platform:
                raise
        self.tshark_process.stdout.close()
        self.tshark_stderr.close()
        self.tshark_process = None

    def get_parameters(self, packet_count=None):
        """
        Returns the special tshark parameters to be used according to the
        configuration of this class.
        """
        params = []
        if self.display_filter:
            params += ['-R', self.display_filter]
        if packet_count:
            params += ['-c', str(packet_count)]
        return params

    def __iter__(self):
        while True:
            yield self.next()

    def __repr__(self):
        return '<%s (%d packets)>' %(self.__class__.__name__, len(self._packets))
<|endoftext|>


<commit_before>from __future__ import division
from __future__ import print_function

import argparse
import operator
import platform
import sys
import traceback
from collections import defaultdict
from datetime import datetime

import pytest

from . import __version__
from .fixture import BenchmarkFixture
from .session import BenchmarkSession
from .session import PerformanceRegression
from .timers import default_timer
from .utils import NameWrapper
from .utils import format_dict
from .utils import get_commit_info
from .utils import get_current_time
from .utils import get_tag
from .utils import parse_columns
from .utils import parse_compare_fail
from .utils import parse_name_format
from .utils import parse_rounds
from .utils import parse_save
from .utils import parse_seconds
from .utils import parse_sort
from .utils import parse_timer
from .utils import parse_warmup

def pytest_report_header(config):
    bs = config._benchmarksession

    return ("benchmark: {version} (defaults:"
            " timer={timer}"
            " disable_gc={0[disable_gc]}"
            " min_rounds={0[min_rounds]}"
            " min_time={0[min_time]}"
            " max_time={0[max_time]}"
            " calibration_precision={0[calibration_precision]}"
            " warmup={0[warmup]}"
            " warmup_iterations={0[warmup_iterations]}"
            ")").format(
        bs.options,
        version=__version__,
        timer=bs.options.get("timer"),
    )

def add_display_options(addoption, prefix="benchmark-"):
    addoption(
        "--{0}sort".format(prefix),
        metavar="COL", type=parse_sort, default="min",
        help="Column to sort on. Can be one of: 'min', 'max', 'mean', 'stddev', "
             "'name', 'fullname'. Default: %(default)r"
    )
    addoption(
        "--{0}group-by".format(prefix),
        metavar="LABEL", default="group",
        help="How to group tests. Can be one of: 'group', 'name', 'fullname', 'func', 'fullfunc', "
             "'param' or 'param:NAME', where NAME is the name passed to @pytest.parametrize."
             " Default: %(default)r"
    )
    addoption(
        "--{0}columns".format(prefix),
        metavar="LABELS", type=parse_columns,
        default=["min", "max", "mean", "stddev", "median", "iqr", "outliers", "ops", "rounds", "iterations"],
        help="Comma-separated list of columns to show in the result table. Default: "
             "'min, max, mean, stddev, median, iqr, outliers, rounds, iterations'"
    )
    addoption(
        "--{0}name".format(prefix),
        metavar="FORMAT", type=parse_name_format,
        default="normal",
        help="How to format names in results. Can be one of 'short', 'normal', 'long'. Default: %(default)r"
    )

def add_histogram_options(addoption, prefix="benchmark-"):
    filename_prefix = "benchmark_%s" % get_current_time()
    addoption(
        "--{0}histogram".format(prefix),
        action="append", metavar="FILENAME-PREFIX", nargs="?", default=[], const=filename_prefix,
        help="Plot graphs of min/max/avg/stddev over time in FILENAME-PREFIX-test_name.svg. If FILENAME-PREFIX contains"
             " slashes ('/') then directories will be created. Default: %r" % filename_prefix
    )

def add_csv_options(addoption, prefix="benchmark-"):
    filename_prefix = "benchmark_%s" % get_current_time()
    addoption(
        "--{0}csv".format(prefix),
        action="append", metavar="FILENAME", nargs="?", default=[], const=filename_prefix,
        help="Save a csv report. If FILENAME contains"
             " slashes ('/') then directories will be created. Default: %r" % filename_prefix
    )

def add_global_options(addoption, prefix="benchmark-"):
    addoption(
        "--{0}storage".format(prefix), *[] if prefix else ['-s'],
        metavar="URI", default="file://./.benchmarks",
        help="Specify a path to store the runs as uri in form file://path or"
             " elasticsearch+http[s]://host1,host2/[index/doctype?project_name=Project] "
             "(when --benchmark-save or --benchmark-autosave are used). For backwards compatibility unexpected values "
             "are converted to file://<value>. Default: %(default)r."
    )
    addoption(
        "--{0}netrc".format(prefix),
        nargs="?", default='', const='~/.netrc',
        help="Load elasticsearch credentials from a netrc file. Default: %(default)r.",
    )
    addoption(
        "--{0}verbose".format(prefix), *[] if prefix else ['-v'],
        action="store_true", default=False,
        help="Dump diagnostic and progress information."
    )

def pytest_addoption(parser):
    group = parser.getgroup("benchmark")
    group.addoption(
        "--benchmark-min-time",
        metavar="SECONDS", type=parse_seconds, default="0.000005",
        help="Minimum time per round in seconds. Default: %(default)r"
    )
    group.addoption(
        "--benchmark-max-time",
        metavar="SECONDS", type=parse_seconds, default="1.0",
        help="Maximum run time per test - it will be repeated until this total time is reached. It may be "
             "exceeded if test function is very slow or --benchmark-min-rounds is large (it takes precedence). "
             "Default: %(default)r"
    )
    group.addoption(
        "--benchmark-min-rounds",
        metavar="NUM", type=parse_rounds, default=5,
        help="Minimum rounds, even if total time would exceed `--max-time`. Default: %(default)r"
    )
    group.addoption(
        "--benchmark-timer",
        metavar="FUNC", type=parse_timer, default=str(NameWrapper(default_timer)),
        help="Timer to use when measuring time. Default: %(default)r"
    )
    group.addoption(
        "--benchmark-calibration-precision",
        metavar="NUM", type=int, default=10,
        help="Precision to use when calibrating number of iterations. Precision of 10 will make the timer look 10 times"
             " more accurate, at a cost of less precise measure of deviations. Default: %(default)r"
    )
    group.addoption(
        "--benchmark-warmup",
        metavar="KIND", nargs="?", default=parse_warmup("auto"), type=parse_warmup,
        help="Activates warmup. Will run the test function up to number of times in the calibration phase. "
             "See `--benchmark-warmup-iterations`. Note: Even the warmup phase obeys --benchmark-max-time. "
             "Available KIND: 'auto', 'off', 'on'. Default: 'auto' (automatically activate on PyPy)."
    )
    group.addoption(
        "--benchmark-warmup-iterations",
        metavar="NUM", type=int, default=100000,
        help="Max number of iterations to run in the warmup phase. Default: %(default)r"
    )
    group.addoption(
        "--benchmark-disable-gc",
        action="store_true", default=False,
        help="Disable GC during benchmarks."
    )
    group.addoption(
        "--benchmark-skip",
        action="store_true", default=False,
        help="Skip running any tests that contain benchmarks."
    )
    group.addoption(
        "--benchmark-disable",
        action="store_true", default=False,
        help="Disable benchmarks. Benchmarked functions are only ran once and no stats are reported. Use this is you "
             "want to run the test but don't do any benchmarking."
    )
    group.addoption(
        "--benchmark-enable",
        action="store_true", default=False,
        help="Forcibly enable benchmarks. Use this option to override --benchmark-disable (in case you have it in "
             "pytest configuration)."
    )
    group.addoption(
        "--benchmark-only",
        action="store_true", default=False,
        help="Only run benchmarks."
    )
    group.addoption(
        "--benchmark-save",
        metavar="NAME", type=parse_save,
        help="Save the current run into 'STORAGE-PATH/counter_NAME.json'."
    )
    tag = get_tag()
    group.addoption(
        "--benchmark-autosave",
        action='store_const', const=tag,
        help="Autosave the current run into 'STORAGE-PATH/counter_%s.json" % tag,
    )
    group.addoption(
        "--benchmark-save-data",
        action="store_true",
        help="Use this to make --benchmark-save and --benchmark-autosave include all the timing data,"
             " not just the stats.",
    )
    group.addoption(
        "--benchmark-json",
        metavar="PATH", type=argparse.FileType('wb'),
        help="Dump a JSON report into PATH. "
             "Note that this will include the complete data (all the timings, not just the stats)."
    )
    group.addoption(
        "--benchmark-compare",
        metavar="NUM|_ID", nargs="?", default=[], const=True,
        help="Compare the current run against run NUM (or prefix of _id in elasticsearch) or the latest "
             "saved run if unspecified."
    )
    group.addoption(
        "--benchmark-compare-fail",
        metavar="EXPR", nargs="+", type=parse_compare_fail,
        help="Fail test if performance regresses according to given EXPR"
             " (eg: min:5%% or mean:0.001 for number of seconds). Can be used multiple times."
    )
    group.addoption(
        "--benchmark-cprofile",
        metavar="COLUMN", default=None,
        choices=['ncalls_recursion', 'ncalls', 'tottime', 'tottime_per', 'cumtime', 'cumtime_per', 'function_name'],
        help="If specified measure one run with cProfile and stores 10 top functions."
             " Argument is a column to sort by. Available columns: 'ncallls_recursion',"
             " 'ncalls', 'tottime', 'tottime_per', 'cumtime', 'cumtime_per', 'function_name'."
    )
    add_global_options(group.addoption)
    add_display_options(group.addoption)
    add_histogram_options(group.addoption)

def pytest_addhooks(pluginmanager):
    from . import hookspec

    method = getattr(pluginmanager, "add_hookspecs", None)
    if method is None:
        method = pluginmanager.addhooks
    method(hookspec)

def pytest_benchmark_compare_machine_info(config, benchmarksession, machine_info, compared_benchmark):
    machine_info = format_dict(machine_info)
    compared_machine_info = format_dict(compared_benchmark["machine_info"])

    if compared_machine_info != machine_info:
        benchmarksession.logger.warn(
            "BENCHMARK-C6",
            "Benchmark machine_info is different. Current: %s VS saved: %s." % (
                machine_info,
                compared_machine_info,
            ),
            fslocation=benchmarksession.storage.location
        )

if hasattr(pytest, 'hookimpl'):
    _hookwrapper = pytest.hookimpl(hookwrapper=True)
else:
    _hookwrapper = pytest.mark.hookwrapper

@_hookwrapper
def pytest_runtest_call(item):
    bs = item.config._benchmarksession
    fixure = hasattr(item, "funcargs") and item.funcargs.get("benchmark")
    if isinstance(fixure, BenchmarkFixture):
        if bs.skip:
            pytest.skip("Skipping benchmark (--benchmark-skip active).")
        else:
            yield
    else:
        if bs.only:
            pytest.skip("Skipping non-benchmark (--benchmark-only active).")
        else:
            yield

def pytest_benchmark_group_stats(config, benchmarks, group_by):
    groups = defaultdict(list)
    for bench in benchmarks:
        key = ()
        for grouping in group_by.split(','):
            if grouping == "group":
                key += bench["group"],
            elif grouping == "name":
                key += bench["name"],
            elif grouping == "func":
                key += bench["name"].split("[")[0],
            elif grouping == "fullname":
                key += bench["fullname"],
            elif grouping == "fullfunc":
                key += bench["fullname"].split("[")[0],
            elif grouping == "param":
                key += bench["param"],
            elif grouping.startswith("param:"):
                param_name = grouping[len("param:"):]
                key += '%s=%s' % (param_name, bench["params"][param_name]),
            else:
                raise NotImplementedError("Unsupported grouping %r." % group_by)
        groups[' '.join(str(p) for p in key if p) or None].append(bench)

    for grouped_benchmarks in groups.values():
        grouped_benchmarks.sort(key=operator.itemgetter("fullname" if "full" in group_by else "name"))
    return sorted(groups.items(), key=lambda pair: pair[0] or "")

@_hookwrapper
def pytest_sessionfinish(session, exitstatus):
    session.config._benchmarksession.finish()
    yield

def pytest_terminal_summary(terminalreporter):
    try:
        terminalreporter.config._benchmarksession.display(terminalreporter)
    except PerformanceRegression:
        raise
    except Exception:
        terminalreporter.config._benchmarksession.logger.error("\n%s" % traceback.format_exc())
        raise

def get_cpu_info():
    import cpuinfo
    all_info = cpuinfo.get_cpu_info()
    all_info = all_info or {}
    info = {}
    for key in ('vendor_id', 'hardware', 'brand'):
        info[key] = all_info.get(key, 'unknown')
    return info

def pytest_benchmark_generate_machine_info():
    python_implementation = platform.python_implementation()
    python_implementation_version = platform.python_version()
    if python_implementation == 'PyPy':
        python_implementation_version = '%d.%d.%d' % sys.pypy_version_info[:3]
        if sys.pypy_version_info.releaselevel != 'final':
            python_implementation_version += '-%s%d' % sys.pypy_version_info[3:]
    return {
        "node": platform.node(),
        "processor": platform.processor(),
        "machine": platform.machine(),
        "python_compiler": platform.python_compiler(),
        "python_implementation": python_implementation,
        "python_implementation_version": python_implementation_version,
        "python_version": platform.python_version(),
        "python_build": platform.python_build(),
        "release": platform.release(),
        "system": platform.system(),
        "cpu": get_cpu_info(),
    }

def pytest_benchmark_generate_commit_info(config):
    return get_commit_info(config.getoption("benchmark_project_name", None))

def pytest_benchmark_generate_json(config, benchmarks, include_data, machine_info, commit_info):
    benchmarks_json = []
    output_json = {
        "machine_info": machine_info,
        "commit_info": commit_info,
        "benchmarks": benchmarks_json,
        "datetime": datetime.utcnow().isoformat(),
        "version": __version__,
    }
    for bench in benchmarks:
        if not bench.has_error:
            benchmarks_json.append(bench.as_dict(include_data=include_data))
    return output_json

@pytest.fixture(scope="function")
def benchmark(request):
    bs = request.config._benchmarksession

    if bs.skip:
        pytest.skip("Benchmarks are skipped (--benchmark-skip was used).")
    else:
        node = request.node
        marker = node.get_marker("benchmark")
        options = marker.kwargs if marker else {}
        if "timer" in options:
            options["timer"] = NameWrapper(options["timer"])
        fixture = BenchmarkFixture(
            node,
            add_stats=bs.benchmarks.append,
            logger=bs.logger,
            warner=request.node.warn,
            disabled=bs.disabled,
            **dict(bs.options, **options)
        )
        request.addfinalizer(fixture._cleanup)
        return fixture

@pytest.fixture(scope="function")
def benchmark_weave(benchmark):
    return benchmark.weave

def pytest_runtest_setup(item):
    marker = item.get_marker("benchmark")
    if marker:
        if marker.args:
            raise ValueError("benchmark mark can't have positional arguments.")
        for name in marker.kwargs:
            if name not in (
                    "max_time", "min_rounds", "min_time", "timer", "group", "disable_gc", "warmup",
                    "warmup_iterations", "calibration_precision"):
                raise ValueError("benchmark mark can't have %r keyword argument." % name)

@pytest.mark.trylast  # force the other plugins to initialise, fixes issue with capture not being properly initialised
def pytest_configure(config):
    config.addinivalue_line("markers", "benchmark: mark a test with custom benchmark settings.")
    bs = config._benchmarksession = BenchmarkSession(config)
    bs.handle_loading()
    config.pluginmanager.register(bs, "pytest-benchmark")
<commit_msg>Handle --benchmark-skip/-only during test collection.<commit_after>from __future__ import division
from __future__ import print_function

import argparse
import operator
import platform
import sys
import traceback
from collections import defaultdict
from datetime import datetime

import pytest

from . import __version__
from .fixture import BenchmarkFixture
from .session import BenchmarkSession
from .session import PerformanceRegression
from .timers import default_timer
from .utils import NameWrapper
from .utils import format_dict
from .utils import get_commit_info
from .utils import get_current_time
from .utils import get_tag
from .utils import parse_columns
from .utils import parse_compare_fail
from .utils import parse_name_format
from .utils import parse_rounds
from .utils import parse_save
from .utils import parse_seconds
from .utils import parse_sort
from .utils import parse_timer
from .utils import parse_warmup

def pytest_report_header(config):
    bs = config._benchmarksession

    return ("benchmark: {version} (defaults:"
            " timer={timer}"
            " disable_gc={0[disable_gc]}"
            " min_rounds={0[min_rounds]}"
            " min_time={0[min_time]}"
            " max_time={0[max_time]}"
            " calibration_precision={0[calibration_precision]}"
            " warmup={0[warmup]}"
            " warmup_iterations={0[warmup_iterations]}"
            ")").format(
        bs.options,
        version=__version__,
        timer=bs.options.get("timer"),
    )

def add_display_options(addoption, prefix="benchmark-"):
    addoption(
        "--{0}sort".format(prefix),
        metavar="COL", type=parse_sort, default="min",
        help="Column to sort on. Can be one of: 'min', 'max', 'mean', 'stddev', "
             "'name', 'fullname'. Default: %(default)r"
    )
    addoption(
        "--{0}group-by".format(prefix),
        metavar="LABEL", default="group",
        help="How to group tests. Can be one of: 'group', 'name', 'fullname', 'func', 'fullfunc', "
             "'param' or 'param:NAME', where NAME is the name passed to @pytest.parametrize."
             " Default: %(default)r"
    )
    addoption(
        "--{0}columns".format(prefix),
        metavar="LABELS", type=parse_columns,
        default=["min", "max", "mean", "stddev", "median", "iqr", "outliers", "ops", "rounds", "iterations"],
        help="Comma-separated list of columns to show in the result table. Default: "
             "'min, max, mean, stddev, median, iqr, outliers, rounds, iterations'"
    )
    addoption(
        "--{0}name".format(prefix),
        metavar="FORMAT", type=parse_name_format,
        default="normal",
        help="How to format names in results. Can be one of 'short', 'normal', 'long'. Default: %(default)r"
    )

def add_histogram_options(addoption, prefix="benchmark-"):
    filename_prefix = "benchmark_%s" % get_current_time()
    addoption(
        "--{0}histogram".format(prefix),
        action="append", metavar="FILENAME-PREFIX", nargs="?", default=[], const=filename_prefix,
        help="Plot graphs of min/max/avg/stddev over time in FILENAME-PREFIX-test_name.svg. If FILENAME-PREFIX contains"
             " slashes ('/') then directories will be created. Default: %r" % filename_prefix
    )

def add_csv_options(addoption, prefix="benchmark-"):
    filename_prefix = "benchmark_%s" % get_current_time()
    addoption(
        "--{0}csv".format(prefix),
        action="append", metavar="FILENAME", nargs="?", default=[], const=filename_prefix,
        help="Save a csv report. If FILENAME contains"
             " slashes ('/') then directories will be created. Default: %r" % filename_prefix
    )

def add_global_options(addoption, prefix="benchmark-"):
    addoption(
        "--{0}storage".format(prefix), *[] if prefix else ['-s'],
        metavar="URI", default="file://./.benchmarks",
        help="Specify a path to store the runs as uri in form file://path or"
             " elasticsearch+http[s]://host1,host2/[index/doctype?project_name=Project] "
             "(when --benchmark-save or --benchmark-autosave are used). For backwards compatibility unexpected values "
             "are converted to file://<value>. Default: %(default)r."
    )
    addoption(
        "--{0}netrc".format(prefix),
        nargs="?", default='', const='~/.netrc',
        help="Load elasticsearch credentials from a netrc file. Default: %(default)r.",
    )
    addoption(
        "--{0}verbose".format(prefix), *[] if prefix else ['-v'],
        action="store_true", default=False,
        help="Dump diagnostic and progress information."
    )

def pytest_addoption(parser):
    group = parser.getgroup("benchmark")
    group.addoption(
        "--benchmark-min-time",
        metavar="SECONDS", type=parse_seconds, default="0.000005",
        help="Minimum time per round in seconds. Default: %(default)r"
    )
    group.addoption(
        "--benchmark-max-time",
        metavar="SECONDS", type=parse_seconds, default="1.0",
        help="Maximum run time per test - it will be repeated until this total time is reached. It may be "
             "exceeded if test function is very slow or --benchmark-min-rounds is large (it takes precedence). "
             "Default: %(default)r"
    )
    group.addoption(
        "--benchmark-min-rounds",
        metavar="NUM", type=parse_rounds, default=5,
        help="Minimum rounds, even if total time would exceed `--max-time`. Default: %(default)r"
    )
    group.addoption(
        "--benchmark-timer",
        metavar="FUNC", type=parse_timer, default=str(NameWrapper(default_timer)),
        help="Timer to use when measuring time. Default: %(default)r"
    )
    group.addoption(
        "--benchmark-calibration-precision",
        metavar="NUM", type=int, default=10,
        help="Precision to use when calibrating number of iterations. Precision of 10 will make the timer look 10 times"
             " more accurate, at a cost of less precise measure of deviations. Default: %(default)r"
    )
    group.addoption(
        "--benchmark-warmup",
        metavar="KIND", nargs="?", default=parse_warmup("auto"), type=parse_warmup,
        help="Activates warmup. Will run the test function up to number of times in the calibration phase. "
             "See `--benchmark-warmup-iterations`. Note: Even the warmup phase obeys --benchmark-max-time. "
             "Available KIND: 'auto', 'off', 'on'. Default: 'auto' (automatically activate on PyPy)."
    )
    group.addoption(
        "--benchmark-warmup-iterations",
        metavar="NUM", type=int, default=100000,
        help="Max number of iterations to run in the warmup phase. Default: %(default)r"
    )
    group.addoption(
        "--benchmark-disable-gc",
        action="store_true", default=False,
        help="Disable GC during benchmarks."
    )
    group.addoption(
        "--benchmark-skip",
        action="store_true", default=False,
        help="Skip running any tests that contain benchmarks."
    )
    group.addoption(
        "--benchmark-disable",
        action="store_true", default=False,
        help="Disable benchmarks. Benchmarked functions are only ran once and no stats are reported. Use this is you "
             "want to run the test but don't do any benchmarking."
    )
    group.addoption(
        "--benchmark-enable",
        action="store_true", default=False,
        help="Forcibly enable benchmarks. Use this option to override --benchmark-disable (in case you have it in "
             "pytest configuration)."
    )
    group.addoption(
        "--benchmark-only",
        action="store_true", default=False,
        help="Only run benchmarks."
    )
    group.addoption(
        "--benchmark-save",
        metavar="NAME", type=parse_save,
        help="Save the current run into 'STORAGE-PATH/counter_NAME.json'."
    )
    tag = get_tag()
    group.addoption(
        "--benchmark-autosave",
        action='store_const', const=tag,
        help="Autosave the current run into 'STORAGE-PATH/counter_%s.json" % tag,
    )
    group.addoption(
        "--benchmark-save-data",
        action="store_true",
        help="Use this to make --benchmark-save and --benchmark-autosave include all the timing data,"
             " not just the stats.",
    )
    group.addoption(
        "--benchmark-json",
        metavar="PATH", type=argparse.FileType('wb'),
        help="Dump a JSON report into PATH. "
             "Note that this will include the complete data (all the timings, not just the stats)."
    )
    group.addoption(
        "--benchmark-compare",
        metavar="NUM|_ID", nargs="?", default=[], const=True,
        help="Compare the current run against run NUM (or prefix of _id in elasticsearch) or the latest "
             "saved run if unspecified."
    )
    group.addoption(
        "--benchmark-compare-fail",
        metavar="EXPR", nargs="+", type=parse_compare_fail,
        help="Fail test if performance regresses according to given EXPR"
             " (eg: min:5%% or mean:0.001 for number of seconds). Can be used multiple times."
    )
    group.addoption(
        "--benchmark-cprofile",
        metavar="COLUMN", default=None,
        choices=['ncalls_recursion', 'ncalls', 'tottime', 'tottime_per', 'cumtime', 'cumtime_per', 'function_name'],
        help="If specified measure one run with cProfile and stores 10 top functions."
             " Argument is a column to sort by. Available columns: 'ncallls_recursion',"
             " 'ncalls', 'tottime', 'tottime_per', 'cumtime', 'cumtime_per', 'function_name'."
    )
    add_global_options(group.addoption)
    add_display_options(group.addoption)
    add_histogram_options(group.addoption)

def pytest_addhooks(pluginmanager):
    from . import hookspec

    method = getattr(pluginmanager, "add_hookspecs", None)
    if method is None:
        method = pluginmanager.addhooks
    method(hookspec)

def pytest_benchmark_compare_machine_info(config, benchmarksession, machine_info, compared_benchmark):
    machine_info = format_dict(machine_info)
    compared_machine_info = format_dict(compared_benchmark["machine_info"])

    if compared_machine_info != machine_info:
        benchmarksession.logger.warn(
            "BENCHMARK-C6",
            "Benchmark machine_info is different. Current: %s VS saved: %s." % (
                machine_info,
                compared_machine_info,
            ),
            fslocation=benchmarksession.storage.location
        )

if hasattr(pytest, 'hookimpl'):
    _hookwrapper = pytest.hookimpl(hookwrapper=True)
else:
    _hookwrapper = pytest.mark.hookwrapper

def pytest_collection_modifyitems(items, config):
    selected_items = []
    deselected_items = []

    only = config.getvalue("benchmark_only", False)
    skip = config.getvalue("benchmark_skip", False)

    for item in items:
        uses_benchmark = hasattr(item, "funcargnames") and "benchmark" in item.funcargnames
        if uses_benchmark:
            if skip:
                deselected_items.append(item)
            else:
                selected_items.append(item)
        else:
            if only:
                deselected_items.append(item)
            else:
                selected_items.append(item)

        config.hook.pytest_deselected(items=deselected_items)
        items[:] = selected_items

def pytest_benchmark_group_stats(config, benchmarks, group_by):
    groups = defaultdict(list)
    for bench in benchmarks:
        key = ()
        for grouping in group_by.split(','):
            if grouping == "group":
                key += bench["group"],
            elif grouping == "name":
                key += bench["name"],
            elif grouping == "func":
                key += bench["name"].split("[")[0],
            elif grouping == "fullname":
                key += bench["fullname"],
            elif grouping == "fullfunc":
                key += bench["fullname"].split("[")[0],
            elif grouping == "param":
                key += bench["param"],
            elif grouping.startswith("param:"):
                param_name = grouping[len("param:"):]
                key += '%s=%s' % (param_name, bench["params"][param_name]),
            else:
                raise NotImplementedError("Unsupported grouping %r." % group_by)
        groups[' '.join(str(p) for p in key if p) or None].append(bench)

    for grouped_benchmarks in groups.values():
        grouped_benchmarks.sort(key=operator.itemgetter("fullname" if "full" in group_by else "name"))
    return sorted(groups.items(), key=lambda pair: pair[0] or "")

@_hookwrapper
def pytest_sessionfinish(session, exitstatus):
    session.config._benchmarksession.finish()
    yield

def pytest_terminal_summary(terminalreporter):
    try:
        terminalreporter.config._benchmarksession.display(terminalreporter)
    except PerformanceRegression:
        raise
    except Exception:
        terminalreporter.config._benchmarksession.logger.error("\n%s" % traceback.format_exc())
        raise

def get_cpu_info():
    import cpuinfo
    all_info = cpuinfo.get_cpu_info()
    all_info = all_info or {}
    info = {}
    for key in ('vendor_id', 'hardware', 'brand'):
        info[key] = all_info.get(key, 'unknown')
    return info

def pytest_benchmark_generate_machine_info():
    python_implementation = platform.python_implementation()
    python_implementation_version = platform.python_version()
    if python_implementation == 'PyPy':
        python_implementation_version = '%d.%d.%d' % sys.pypy_version_info[:3]
        if sys.pypy_version_info.releaselevel != 'final':
            python_implementation_version += '-%s%d' % sys.pypy_version_info[3:]
    return {
        "node": platform.node(),
        "processor": platform.processor(),
        "machine": platform.machine(),
        "python_compiler": platform.python_compiler(),
        "python_implementation": python_implementation,
        "python_implementation_version": python_implementation_version,
        "python_version": platform.python_version(),
        "python_build": platform.python_build(),
        "release": platform.release(),
        "system": platform.system(),
        "cpu": get_cpu_info(),
    }

def pytest_benchmark_generate_commit_info(config):
    return get_commit_info(config.getoption("benchmark_project_name", None))

def pytest_benchmark_generate_json(config, benchmarks, include_data, machine_info, commit_info):
    benchmarks_json = []
    output_json = {
        "machine_info": machine_info,
        "commit_info": commit_info,
        "benchmarks": benchmarks_json,
        "datetime": datetime.utcnow().isoformat(),
        "version": __version__,
    }
    for bench in benchmarks:
        if not bench.has_error:
            benchmarks_json.append(bench.as_dict(include_data=include_data))
    return output_json

@pytest.fixture(scope="function")
def benchmark(request):
    bs = request.config._benchmarksession

    if bs.skip:
        pytest.skip("Benchmarks are skipped (--benchmark-skip was used).")
    else:
        node = request.node
        marker = node.get_marker("benchmark")
        options = marker.kwargs if marker else {}
        if "timer" in options:
            options["timer"] = NameWrapper(options["timer"])
        fixture = BenchmarkFixture(
            node,
            add_stats=bs.benchmarks.append,
            logger=bs.logger,
            warner=request.node.warn,
            disabled=bs.disabled,
            **dict(bs.options, **options)
        )
        request.addfinalizer(fixture._cleanup)
        return fixture

@pytest.fixture(scope="function")
def benchmark_weave(benchmark):
    return benchmark.weave

def pytest_runtest_setup(item):
    marker = item.get_marker("benchmark")
    if marker:
        if marker.args:
            raise ValueError("benchmark mark can't have positional arguments.")
        for name in marker.kwargs:
            if name not in (
                    "max_time", "min_rounds", "min_time", "timer", "group", "disable_gc", "warmup",
                    "warmup_iterations", "calibration_precision"):
                raise ValueError("benchmark mark can't have %r keyword argument." % name)

@pytest.mark.trylast  # force the other plugins to initialise, fixes issue with capture not being properly initialised
def pytest_configure(config):
    config.addinivalue_line("markers", "benchmark: mark a test with custom benchmark settings.")
    bs = config._benchmarksession = BenchmarkSession(config)
    bs.handle_loading()
    config.pluginmanager.register(bs, "pytest-benchmark")
<|endoftext|>


<commit_before>import ddapp.applogic as app

from ddapp import lcmUtils
from ddapp import transformUtils
from ddapp import visualization as vis
from ddapp.shallowCopy import shallowCopy
from ddapp.timercallback import TimerCallback
from ddapp import vtkNumpy
import ddapp.vtkAll as vtk

import PythonQt
from PythonQt import QtCore, QtGui
import bot_core as lcmbotcore
import numpy as np
from ddapp.simpletimer import SimpleTimer
from ddapp import ioUtils
import sys
import drc as lcmdrc

def clipRange(dataObj, arrayName, thresholdRange):
    if not dataObj.GetPointData().GetArray(arrayName):
        raise Exception('clipRange: could not locate array: %s' % arrayName)

    dataObj.GetPointData().SetScalars(dataObj.GetPointData().GetArray(arrayName))

    f = vtk.vtkClipPolyData()
    f.SetInput(dataObj)
    f.SetValue(thresholdRange[0])
    f.SetInputArrayToProcess(0, 0, 0, vtk.vtkDataObject.FIELD_ASSOCIATION_POINTS, arrayName)

    f2 = vtk.vtkClipPolyData()
    f2.AddInputConnection(f.GetOutputPort())
    f2.SetValue(thresholdRange[1])
    f2.InsideOutOn()
    f2.SetInputArrayToProcess(0, 0, 0, vtk.vtkDataObject.FIELD_ASSOCIATION_POINTS, arrayName)

    f2.Update()
    return shallowCopy(f2.GetOutput())

def makeSphere(radius, resolution):

    s = vtk.vtkSphereSource()
    s.SetThetaResolution(resolution)
    s.SetPhiResolution(resolution)
    s.SetRadius(radius)
    s.SetEndPhi(85)
    s.Update()
    return shallowCopy(s.GetOutput())

def colorizePoints(polyData, cameraName='CAMERA_LEFT'):
    cameraView.queue.colorizePoints(cameraName, polyData)

def sendFOVRequest(channel, imagePoints):

    channelToImageType = {
        'CAMERA_LEFT' : lcmdrc.data_request_t.CAMERA_IMAGE_HEAD_LEFT,
        'CAMERACHEST_LEFT' : lcmdrc.data_request_t.CAMERA_IMAGE_LCHEST,
        'CAMERACHEST_RIGHT' : lcmdrc.data_request_t.CAMERA_IMAGE_RCHEST,
                         }

    dataRequest = lcmdrc.data_request_t()
    dataRequest.type = channelToImageType[channel]

    message = lcmdrc.subimage_request_t()
    message.data_request = dataRequest

    imagePoints = np.array([[pt[0], pt[1]] for pt in imagePoints])
    minX, maxX = imagePoints[:,0].min(), imagePoints[:,0].max()
    minY, maxY = imagePoints[:,1].min(), imagePoints[:,1].max()

    message.x = minX
    message.y = minY
    message.w = maxX - minX
    message.h = maxY - minY

    print message.x, message.y, message.w, message.h

    requestChannel = 'SUBIMAGE_REQUEST'
    lcmUtils.publish(requestChannel, message)

def testColorize():

    radius = 10
    resolution = 400
    s = makeSphere(radius, resolution)
    cameraView.queue.colorizePoints(s)
    showPolyData(p, 'sphere', colorByName='rgb')

def rayDebug(position, ray):
    d = vis.DebugData()
    d.addLine(position, position+ray*5.0)
    drcView = app.getViewManager().findView('DRC View')
    vis.updatePolyData(d.getPolyData(), 'camera ray', view=drcView)

class CameraView(object):

    def __init__(self):

        self.initImageQueue()

        self.initView()
        self.initEventFilter()
        self.rayCallback = rayDebug

    def initImageQueue(self):

        imageNames = [
            'CAMERA_LEFT',
            'CAMERACHEST_LEFT',
            'CAMERACHEST_RIGHT',
            ]

        self.images = {}
        self.transforms = {}
        self.imageUtimes = {}
        self.textures = {}

        for name in imageNames:
            self.addImage(name)

        self.queue = PythonQt.dd.ddBotImageQueue(lcmUtils.getGlobalLCMThread())
        self.queue.init(lcmUtils.getGlobalLCMThread())

    def addImage(self, name):
        image = vtk.vtkImageData()
        tex = vtk.vtkTexture()
        tex.SetInput(image)
        tex.EdgeClampOn()
        tex.RepeatOff()

        self.imageUtimes[name] = 0
        self.images[name] = image
        self.textures[name] = tex
        self.transforms[name] = vtk.vtkTransform()

    def onViewDoubleClicked(self, displayPoint):

        obj, pickedPoint = vis.findPickedObject(displayPoint, view=self.view)

        if pickedPoint is None or not obj:
            return

        imageName = obj.getProperty('Name')
        imageUtime = self.imageUtimes[imageName]

        cameraToLocal = vtk.vtkTransform()
        self.queue.getTransform(imageName, 'local', imageUtime, cameraToLocal)

        utorsoToLocal = vtk.vtkTransform()
        self.queue.getTransform('utorso', 'local', imageUtime, utorsoToLocal)

        p = range(3)
        utorsoToLocal.TransformPoint(pickedPoint, p)

        ray = np.array(p) - np.array(cameraToLocal.GetPosition())
        ray /= np.linalg.norm(ray)

        if self.rayCallback:
            self.rayCallback(np.array(cameraToLocal.GetPosition()), ray)

    def filterEvent(self, obj, event):
        if event.type() == QtCore.QEvent.MouseButtonDblClick:
            self.eventFilter.setEventHandlerResult(True)
            self.onViewDoubleClicked(vis.mapMousePosition(obj, event))

    def initEventFilter(self):
        self.eventFilter = PythonQt.dd.ddPythonEventFilter()
        qvtkwidget = self.view.vtkWidget()
        qvtkwidget.installEventFilter(self.eventFilter)
        self.eventFilter.addFilteredEventType(QtCore.QEvent.MouseButtonDblClick)
        self.eventFilter.connect('handleEvent(QObject*, QEvent*)', self.filterEvent)

    def initView(self):
        self.view = app.getViewManager().createView('Camera View', 'VTK View')

        self.view.camera().SetViewAngle(90)
        self.view.camera().SetPosition(-7.5, 0.0, 5.0)
        self.view.camera().SetFocalPoint(0.0, 0.0, 0.0)
        self.view.camera().SetViewUp(0.0, 0.0, 1.0)

        self.sphereObjects = {}

        app.toggleCameraTerrainMode(self.view)

        self.timerCallback = TimerCallback()
        self.timerCallback.targetFps = 60
        self.timerCallback.callback = self.updateView
        self.timerCallback.start()

    def getSphereGeometry(self, imageName):

        sphereObj = self.sphereObjects.get(imageName)
        if sphereObj:
            return sphereObj

        if not self.images[imageName].GetDimensions()[0]:
            return None

        sphereResolution = 50
        sphereRadii = {
                'CAMERA_LEFT' : 9.85,
                'CAMERACHEST_LEFT' : 9.95,
                'CAMERACHEST_RIGHT' : 10
                }

        geometry = makeSphere(sphereRadii[imageName], sphereResolution)
        self.queue.computeTextureCoords(imageName, geometry)

        tcoordsArrayName = 'tcoords_%s' % imageName
        vtkNumpy.addNumpyToVtk(geometry, vtkNumpy.getNumpyFromVtk(geometry, tcoordsArrayName)[:,0].copy(), 'tcoords_U')
        vtkNumpy.addNumpyToVtk(geometry, vtkNumpy.getNumpyFromVtk(geometry, tcoordsArrayName)[:,1].copy(), 'tcoords_V')
        geometry = clipRange(geometry, 'tcoords_U', [0.0, 1.0])
        geometry = clipRange(geometry, 'tcoords_V', [0.0, 1.0])
        geometry.GetPointData().SetTCoords(geometry.GetPointData().GetArray(tcoordsArrayName))

        sphereObj = vis.showPolyData(geometry, imageName, view=self.view, parent='cameras')
        sphereObj.actor.SetTexture(self.textures[imageName])
        sphereObj.actor.GetProperty().LightingOff()

        self.sphereObjects[imageName] = sphereObj
        return sphereObj

    def updateSphereGeometry(self):

        for imageName, transform in self.transforms.iteritems():
            sphereObj = self.getSphereGeometry(imageName)
            if not sphereObj:
                continue

            self.queue.getBodyToCameraTransform(imageName, transform)
            sphereObj.actor.SetUserTransform(transform.GetLinearInverse())

        return True

    def writeImage(self, imageName, outFile):
        writer = vtk.vtkPNGWriter()
        writer.SetInput(self.images[imageName])
        writer.SetFileName(outFile)
        writer.Write()

    def updateImages(self):
        updated = False
        for imageName, image in self.images.iteritems():
            imageUtime = self.queue.getCurrentImageTime(imageName)
            if imageUtime != self.imageUtimes.get(imageName, 0):
                self.imageUtimes[imageName] = self.queue.getImage(imageName, image)
                updated = True
        return updated

    def updateView(self):

        if not self.updateImages():
            return

        self.updateSphereGeometry()

        if not self.view.isVisible():
            return
        self.view.render()

class CameraImageView(object):

    def __init__(self, imageManager, imageName, viewName=None, view=None):

        self.imageManager = imageManager
        self.viewName = viewName or imageName
        self.imageName = imageName
        self.imageInitialized = False
        self.updateUtime = 0
        self.initView(view)
        self.initEventFilter()

    def onViewDoubleClicked(self, displayPoint):

        obj, pickedPoint = vis.findPickedObject(displayPoint, view=self.view)

        if pickedPoint is None or not obj:
            return

        imageName = obj.getProperty('Name')
        imageUtime = self.imageUtimes[imageName]

        cameraToLocal = vtk.vtkTransform()
        self.queue.getTransform(imageName, 'local', imageUtime, cameraToLocal)

        utorsoToLocal = vtk.vtkTransform()
        self.queue.getTransform('utorso', 'local', imageUtime, utorsoToLocal)

        drcView = app.getViewManager().findView('DRC View')

        p = range(3)
        utorsoToLocal.TransformPoint(pickedPoint, p)
        d = vis.DebugData()
        d.addLine(cameraToLocal.GetPosition(), p)
        vis.updatePolyData(d.getPolyData(), 'camera ray', view=drcView)

    def filterEvent(self, obj, event):
        if event.type() == QtCore.QEvent.MouseButtonDblClick:
            self.eventFilter.setEventHandlerResult(True)
            self.onViewDoubleClicked(vis.mapMousePosition(obj, event))

    def onRubberBandPick(self, obj, event):
        displayPoints = self.interactorStyle.GetStartPosition(), self.interactorStyle.GetEndPosition()
        imagePoints = [vis.pickImage(point, view=self.view)[1] for point in displayPoints]
        sendFOVRequest(self.imageName, imagePoints)

    def initView(self, view):
        self.view = view or app.getViewManager().createView(self.viewName, 'VTK View')
        self.view.installImageInteractor()
        self.interactorStyle = self.view.renderWindow().GetInteractor().GetInteractorStyle()
        self.interactorStyle.AddObserver('SelectionChangedEvent', self.onRubberBandPick)

        self.imageActor = vtk.vtkImageActor()
        self.imageActor.SetInput(self.imageManager.images[self.imageName])
        self.view.renderer().AddActor(self.imageActor)

        self.timerCallback = TimerCallback()
        self.timerCallback.targetFps = 60
        self.timerCallback.callback = self.updateView
        self.timerCallback.start()

    def initEventFilter(self):
        self.eventFilter = PythonQt.dd.ddPythonEventFilter()
        qvtkwidget = self.view.vtkWidget()
        qvtkwidget.installEventFilter(self.eventFilter)
        self.eventFilter.addFilteredEventType(QtCore.QEvent.MouseButtonDblClick)
        self.eventFilter.connect('handleEvent(QObject*, QEvent*)', self.filterEvent)

    def resetCamera(self):
        camera = self.view.camera()
        camera.ParallelProjectionOn()
        camera.SetFocalPoint(0,0,0)
        camera.SetPosition(0,0,-1)
        camera.SetViewUp(0,-1, 0)
        self.view.resetCamera()

    def updateView(self):

        if not self.view.isVisible():
            return

        currentUtime = self.imageManager.imageUtimes[self.imageName]
        if currentUtime != self.updateUtime:
            self.view.render()
            self.updateUtime = currentUtime

            if not self.imageInitialized and self.imageActor.GetInput().GetDimensions()[0]:
                self.resetCamera()
                self.imageInitialized = True

views = {}

def addCameraView(channel, name):

    cameraView.queue.addCameraStream(channel)
    cameraView.addImage(channel)
    view = CameraImageView(cameraView, channel, name)
    global views
    views[channel] = view
    return view

def init():
    global cameraView
    cameraView = CameraView()

    global headView, chestLeft, chestRight
    headView = CameraImageView(cameraView, 'CAMERA_LEFT', 'Head camera')
    chestLeft = CameraImageView(cameraView, 'CAMERACHEST_LEFT', 'Chest left')
    chestRight = CameraImageView(cameraView, 'CAMERACHEST_RIGHT', 'Chest right')

    #addCameraView('AFFORDANCE_OVERLAY', 'Affordance overlay')

    useAnaglyph = False
    if useAnaglyph:
        addCameraView('CAMERA_ANAGLYPH', 'Head Anaglyph')

<commit_msg>add flag to enable/disable the double click event  filter<commit_after>import ddapp.applogic as app

from ddapp import lcmUtils
from ddapp import transformUtils
from ddapp import visualization as vis
from ddapp.shallowCopy import shallowCopy
from ddapp.timercallback import TimerCallback
from ddapp import vtkNumpy
import ddapp.vtkAll as vtk

import PythonQt
from PythonQt import QtCore, QtGui
import bot_core as lcmbotcore
import numpy as np
from ddapp.simpletimer import SimpleTimer
from ddapp import ioUtils
import sys
import drc as lcmdrc

def clipRange(dataObj, arrayName, thresholdRange):
    if not dataObj.GetPointData().GetArray(arrayName):
        raise Exception('clipRange: could not locate array: %s' % arrayName)

    dataObj.GetPointData().SetScalars(dataObj.GetPointData().GetArray(arrayName))

    f = vtk.vtkClipPolyData()
    f.SetInput(dataObj)
    f.SetValue(thresholdRange[0])
    f.SetInputArrayToProcess(0, 0, 0, vtk.vtkDataObject.FIELD_ASSOCIATION_POINTS, arrayName)

    f2 = vtk.vtkClipPolyData()
    f2.AddInputConnection(f.GetOutputPort())
    f2.SetValue(thresholdRange[1])
    f2.InsideOutOn()
    f2.SetInputArrayToProcess(0, 0, 0, vtk.vtkDataObject.FIELD_ASSOCIATION_POINTS, arrayName)

    f2.Update()
    return shallowCopy(f2.GetOutput())

def makeSphere(radius, resolution):

    s = vtk.vtkSphereSource()
    s.SetThetaResolution(resolution)
    s.SetPhiResolution(resolution)
    s.SetRadius(radius)
    s.SetEndPhi(85)
    s.Update()
    return shallowCopy(s.GetOutput())

def colorizePoints(polyData, cameraName='CAMERA_LEFT'):
    cameraView.queue.colorizePoints(cameraName, polyData)

def sendFOVRequest(channel, imagePoints):

    channelToImageType = {
        'CAMERA_LEFT' : lcmdrc.data_request_t.CAMERA_IMAGE_HEAD_LEFT,
        'CAMERACHEST_LEFT' : lcmdrc.data_request_t.CAMERA_IMAGE_LCHEST,
        'CAMERACHEST_RIGHT' : lcmdrc.data_request_t.CAMERA_IMAGE_RCHEST,
                         }

    dataRequest = lcmdrc.data_request_t()
    dataRequest.type = channelToImageType[channel]

    message = lcmdrc.subimage_request_t()
    message.data_request = dataRequest

    imagePoints = np.array([[pt[0], pt[1]] for pt in imagePoints])
    minX, maxX = imagePoints[:,0].min(), imagePoints[:,0].max()
    minY, maxY = imagePoints[:,1].min(), imagePoints[:,1].max()

    message.x = minX
    message.y = minY
    message.w = maxX - minX
    message.h = maxY - minY

    print message.x, message.y, message.w, message.h

    requestChannel = 'SUBIMAGE_REQUEST'
    lcmUtils.publish(requestChannel, message)

def testColorize():

    radius = 10
    resolution = 400
    s = makeSphere(radius, resolution)
    cameraView.queue.colorizePoints(s)
    showPolyData(p, 'sphere', colorByName='rgb')

def rayDebug(position, ray):
    d = vis.DebugData()
    d.addLine(position, position+ray*5.0)
    drcView = app.getViewManager().findView('DRC View')
    vis.updatePolyData(d.getPolyData(), 'camera ray', view=drcView)

class CameraView(object):

    def __init__(self):

        self.initImageQueue()

        self.initView()
        self.initEventFilter()
        self.rayCallback = rayDebug

    def initImageQueue(self):

        imageNames = [
            'CAMERA_LEFT',
            'CAMERACHEST_LEFT',
            'CAMERACHEST_RIGHT',
            ]

        self.images = {}
        self.transforms = {}
        self.imageUtimes = {}
        self.textures = {}

        for name in imageNames:
            self.addImage(name)

        self.queue = PythonQt.dd.ddBotImageQueue(lcmUtils.getGlobalLCMThread())
        self.queue.init(lcmUtils.getGlobalLCMThread())

    def addImage(self, name):
        image = vtk.vtkImageData()
        tex = vtk.vtkTexture()
        tex.SetInput(image)
        tex.EdgeClampOn()
        tex.RepeatOff()

        self.imageUtimes[name] = 0
        self.images[name] = image
        self.textures[name] = tex
        self.transforms[name] = vtk.vtkTransform()

    def onViewDoubleClicked(self, displayPoint):

        obj, pickedPoint = vis.findPickedObject(displayPoint, view=self.view)

        if pickedPoint is None or not obj:
            return

        imageName = obj.getProperty('Name')
        imageUtime = self.imageUtimes[imageName]

        cameraToLocal = vtk.vtkTransform()
        self.queue.getTransform(imageName, 'local', imageUtime, cameraToLocal)

        utorsoToLocal = vtk.vtkTransform()
        self.queue.getTransform('utorso', 'local', imageUtime, utorsoToLocal)

        p = range(3)
        utorsoToLocal.TransformPoint(pickedPoint, p)

        ray = np.array(p) - np.array(cameraToLocal.GetPosition())
        ray /= np.linalg.norm(ray)

        if self.rayCallback:
            self.rayCallback(np.array(cameraToLocal.GetPosition()), ray)

    def filterEvent(self, obj, event):
        if event.type() == QtCore.QEvent.MouseButtonDblClick:
            self.eventFilter.setEventHandlerResult(True)
            self.onViewDoubleClicked(vis.mapMousePosition(obj, event))

    def initEventFilter(self):
        self.eventFilter = PythonQt.dd.ddPythonEventFilter()
        qvtkwidget = self.view.vtkWidget()
        qvtkwidget.installEventFilter(self.eventFilter)
        self.eventFilter.addFilteredEventType(QtCore.QEvent.MouseButtonDblClick)
        self.eventFilter.connect('handleEvent(QObject*, QEvent*)', self.filterEvent)

    def initView(self):
        self.view = app.getViewManager().createView('Camera View', 'VTK View')

        self.view.camera().SetViewAngle(90)
        self.view.camera().SetPosition(-7.5, 0.0, 5.0)
        self.view.camera().SetFocalPoint(0.0, 0.0, 0.0)
        self.view.camera().SetViewUp(0.0, 0.0, 1.0)

        self.sphereObjects = {}

        app.toggleCameraTerrainMode(self.view)

        self.timerCallback = TimerCallback()
        self.timerCallback.targetFps = 60
        self.timerCallback.callback = self.updateView
        self.timerCallback.start()

    def getSphereGeometry(self, imageName):

        sphereObj = self.sphereObjects.get(imageName)
        if sphereObj:
            return sphereObj

        if not self.images[imageName].GetDimensions()[0]:
            return None

        sphereResolution = 50
        sphereRadii = {
                'CAMERA_LEFT' : 9.85,
                'CAMERACHEST_LEFT' : 9.95,
                'CAMERACHEST_RIGHT' : 10
                }

        geometry = makeSphere(sphereRadii[imageName], sphereResolution)
        self.queue.computeTextureCoords(imageName, geometry)

        tcoordsArrayName = 'tcoords_%s' % imageName
        vtkNumpy.addNumpyToVtk(geometry, vtkNumpy.getNumpyFromVtk(geometry, tcoordsArrayName)[:,0].copy(), 'tcoords_U')
        vtkNumpy.addNumpyToVtk(geometry, vtkNumpy.getNumpyFromVtk(geometry, tcoordsArrayName)[:,1].copy(), 'tcoords_V')
        geometry = clipRange(geometry, 'tcoords_U', [0.0, 1.0])
        geometry = clipRange(geometry, 'tcoords_V', [0.0, 1.0])
        geometry.GetPointData().SetTCoords(geometry.GetPointData().GetArray(tcoordsArrayName))

        sphereObj = vis.showPolyData(geometry, imageName, view=self.view, parent='cameras')
        sphereObj.actor.SetTexture(self.textures[imageName])
        sphereObj.actor.GetProperty().LightingOff()

        self.sphereObjects[imageName] = sphereObj
        return sphereObj

    def updateSphereGeometry(self):

        for imageName, transform in self.transforms.iteritems():
            sphereObj = self.getSphereGeometry(imageName)
            if not sphereObj:
                continue

            self.queue.getBodyToCameraTransform(imageName, transform)
            sphereObj.actor.SetUserTransform(transform.GetLinearInverse())

        return True

    def writeImage(self, imageName, outFile):
        writer = vtk.vtkPNGWriter()
        writer.SetInput(self.images[imageName])
        writer.SetFileName(outFile)
        writer.Write()

    def updateImages(self):
        updated = False
        for imageName, image in self.images.iteritems():
            imageUtime = self.queue.getCurrentImageTime(imageName)
            if imageUtime != self.imageUtimes.get(imageName, 0):
                self.imageUtimes[imageName] = self.queue.getImage(imageName, image)
                updated = True
        return updated

    def updateView(self):

        if not self.updateImages():
            return

        self.updateSphereGeometry()

        if not self.view.isVisible():
            return
        self.view.render()

class CameraImageView(object):

    def __init__(self, imageManager, imageName, viewName=None, view=None):

        self.imageManager = imageManager
        self.viewName = viewName or imageName
        self.imageName = imageName
        self.imageInitialized = False
        self.updateUtime = 0
        self.initView(view)
        self.initEventFilter()

    def onViewDoubleClicked(self, displayPoint):

        obj, pickedPoint = vis.findPickedObject(displayPoint, view=self.view)

        if pickedPoint is None or not obj:
            return

        imageName = obj.getProperty('Name')
        imageUtime = self.imageUtimes[imageName]

        cameraToLocal = vtk.vtkTransform()
        self.queue.getTransform(imageName, 'local', imageUtime, cameraToLocal)

        utorsoToLocal = vtk.vtkTransform()
        self.queue.getTransform('utorso', 'local', imageUtime, utorsoToLocal)

        drcView = app.getViewManager().findView('DRC View')

        p = range(3)
        utorsoToLocal.TransformPoint(pickedPoint, p)
        d = vis.DebugData()
        d.addLine(cameraToLocal.GetPosition(), p)
        vis.updatePolyData(d.getPolyData(), 'camera ray', view=drcView)

    def filterEvent(self, obj, event):
        if self.eventFilterEnabled and event.type() == QtCore.QEvent.MouseButtonDblClick:
            self.eventFilter.setEventHandlerResult(True)
            self.onViewDoubleClicked(vis.mapMousePosition(obj, event))

    def onRubberBandPick(self, obj, event):
        displayPoints = self.interactorStyle.GetStartPosition(), self.interactorStyle.GetEndPosition()
        imagePoints = [vis.pickImage(point, view=self.view)[1] for point in displayPoints]
        sendFOVRequest(self.imageName, imagePoints)

    def initView(self, view):
        self.view = view or app.getViewManager().createView(self.viewName, 'VTK View')
        self.view.installImageInteractor()
        self.interactorStyle = self.view.renderWindow().GetInteractor().GetInteractorStyle()
        self.interactorStyle.AddObserver('SelectionChangedEvent', self.onRubberBandPick)

        self.imageActor = vtk.vtkImageActor()
        self.imageActor.SetInput(self.imageManager.images[self.imageName])
        self.view.renderer().AddActor(self.imageActor)

        self.timerCallback = TimerCallback()
        self.timerCallback.targetFps = 60
        self.timerCallback.callback = self.updateView
        self.timerCallback.start()

    def initEventFilter(self):
        self.eventFilter = PythonQt.dd.ddPythonEventFilter()
        qvtkwidget = self.view.vtkWidget()
        qvtkwidget.installEventFilter(self.eventFilter)
        self.eventFilter.addFilteredEventType(QtCore.QEvent.MouseButtonDblClick)
        self.eventFilter.connect('handleEvent(QObject*, QEvent*)', self.filterEvent)
        self.eventFilterEnabled = True

    def resetCamera(self):
        camera = self.view.camera()
        camera.ParallelProjectionOn()
        camera.SetFocalPoint(0,0,0)
        camera.SetPosition(0,0,-1)
        camera.SetViewUp(0,-1, 0)
        self.view.resetCamera()

    def updateView(self):

        if not self.view.isVisible():
            return

        currentUtime = self.imageManager.imageUtimes[self.imageName]
        if currentUtime != self.updateUtime:
            self.view.render()
            self.updateUtime = currentUtime

            if not self.imageInitialized and self.imageActor.GetInput().GetDimensions()[0]:
                self.resetCamera()
                self.imageInitialized = True

views = {}

def addCameraView(channel, name):

    cameraView.queue.addCameraStream(channel)
    cameraView.addImage(channel)
    view = CameraImageView(cameraView, channel, name)
    global views
    views[channel] = view
    return view

def init():
    global cameraView
    cameraView = CameraView()

    global headView, chestLeft, chestRight
    headView = CameraImageView(cameraView, 'CAMERA_LEFT', 'Head camera')
    chestLeft = CameraImageView(cameraView, 'CAMERACHEST_LEFT', 'Chest left')
    chestRight = CameraImageView(cameraView, 'CAMERACHEST_RIGHT', 'Chest right')

    #addCameraView('AFFORDANCE_OVERLAY', 'Affordance overlay')

    useAnaglyph = False
    if useAnaglyph:
        addCameraView('CAMERA_ANAGLYPH', 'Head Anaglyph')

<|endoftext|>