Esempio n. 1
0
    def test_add_or_update_ses__add_2_without_best_candidates__len_is_2(self):
        tracker = SituationElementTracker(self.mock, 1)

        tracker.add_or_update_ses([ROI(), ROI()])

        self.assertEqual(2, len(tracker.se_list))
        self.assertFalse(tracker.non_roi_gazes)
Esempio n. 2
0
    def test_update_elements__gazes_and_no_ses__two_non_roi_gaze(self):
        tracker = SituationElementTracker(self.mock, TEST_SA_PARAMETER)

        tracker.update_elements([create_gaze(0, 0), create_gaze(1, 1)])

        self.assertFalse(tracker.se_list)
        self.assertEqual(2, len(tracker.non_roi_gazes))
Esempio n. 3
0
    def test_update_best_candidate__one_candidate__candidate_updated(self):
        element = SituationElement(self.se_list[1].roi_msg, TEST_SA_PARAMETER)
        element.update(0)
        roi = create_roi(0.5, 0.5)

        SituationElementTracker.update_best_candidate([element], roi)

        self.assertEqual(element.roi_msg, roi)
Esempio n. 4
0
    def test_update_elements__no_gazes_inside__has_non_roi_gazes(self):
        tracker = SituationElementTracker(self.mock, TEST_SA_PARAMETER)
        tracker.add_or_update_ses([create_roi(0, 0, radius=1)])

        tracker.update_elements([create_gaze(1, 1)])

        self.assertEqual(1, len(tracker.se_list))
        self.assertEqual(1, len(tracker.non_roi_gazes))
Esempio n. 5
0
    def test_update_elements__2_elements_update_once__len_is_2(self):
        tracker = SituationElementTracker(self.mock, TEST_SA_PARAMETER)
        tracker.add_or_update_ses([ROI(), ROI()])

        tracker.update_elements([])

        self.assertEqual(2, len(tracker.se_list))
        self.assertFalse(tracker.non_roi_gazes)
Esempio n. 6
0
    def test_update_elements__2_elements_update_twice__empty(self):
        tracker = SituationElementTracker(self.mock, TEST_SA_PARAMETER)
        tracker.add_or_update_ses([ROI(), ROI()])

        tracker.update_elements([])
        tracker.update_elements([])

        self.assertFalse(tracker.se_list)
        self.assertFalse(tracker.non_roi_gazes)
Esempio n. 7
0
    def test_add_or_update_ses__add_2_with_best_candidates__len_is_2(self):
        tracker = SituationElementTracker(self.mock, 1)
        tracker.add_or_update_ses([ROI(), ROI()])
        self.mock.find_best_candidates.return_value = [
            SituationElement(ROI(), TEST_SA_PARAMETER)
        ]

        tracker.add_or_update_ses([ROI(), ROI()])

        self.assertEqual(2, len(tracker.se_list))
        self.assertFalse(tracker.non_roi_gazes)
Esempio n. 8
0
    def __init__(self):
        rospy.loginfo("Starting CARLA Awareness Model...")

        gaze_topic = rospy.get_param("~gaze_topic", "/gaze_publisher/gaze")
        max_object_distance = rospy.get_param("~objects/max_distance_m", 100)
        roi_required_object_distance = rospy.get_param(
            "~objects/roi_required_distance_m", 30)

        self.__screen_parameter = ScreenParameter(
            rospy.get_param("~screen/padding/top_px", 0),
            rospy.get_param("~screen/padding/left_px", 0),
            rospy.get_param("~screen/padding/right_px", 0),
            rospy.get_param("~screen/padding/bottom_px", 0),
            rospy.get_param("~screen/window_pos/x_px", 1920),
            rospy.get_param("~screen/window_pos/y_px", 0),
            rospy.get_param("~screen/monitor_resolution/x_px", 1920),
            rospy.get_param("~screen/monitor_resolution/y_px", 1080),
            rospy.get_param("~screen/camera_resolution/x_px", 640),
            rospy.get_param("~screen/camera_resolution/y_px", 480),
        )

        self.__offset_topbar_px = rospy.get_param("~offset_topbar_px", 30)

        camera_fov_deg = rospy.get_param("~camera_fov_deg", 90)
        min_2d_boundingbox_x = rospy.get_param("~min_2d_boundingbox/x_px", 90)
        min_2d_boundingbox_y = rospy.get_param("~min_2d_boundingbox/y_px", 90)
        time_last_looked_detected_s = rospy.get_param(
            "~awareness_detection/time_last_looked_detected_s", 5)
        num_gaze_comprehended = rospy.get_param(
            "~awareness_detection/num_gaze_comprehended_s", 35)
        dbscan_max_distance_px = rospy.get_param(
            "~awareness_detection/dbscan_max_distance_px", 15)
        dbscan_min_samples = rospy.get_param(
            "~awareness_detection/dbscan_min_samples", 3)
        punishment_factor = rospy.get_param(
            "~awareness_detection/punishment_factor", 0.25)

        rospy.loginfo(
            "Waiting for vehicle_info and camera_info topics from carla ros bridge"
        )
        self.__camera_tf_name = (rospy.get_param(
            "~camera_tf_name", "/carla/ego_vehicle/camera/rgb/front"), )
        ego_vehicle_info = rospy.wait_for_message(
            rospy.get_param("~vehicle_info_topic",
                            "/carla/ego_vehicle/vehicle_info"),
            CarlaEgoVehicleInfo,
        )
        camera_info = rospy.wait_for_message(
            f"{self.__camera_tf_name}/camera_info",
            CameraInfo,
        )
        rospy.loginfo("Carla ros bridge is online.")

        # Subscribers
        self.__sub_gaze = rospy.Subscriber(gaze_topic, GazeArray,
                                           self.gaze_callback)
        self.__sub_objects = rospy.Subscriber("/carla/objects", ObjectArray,
                                              self.objects_callback)

        # Create camera model from camera info
        cam_model = image_geometry.PinholeCameraModel()
        cam_model.fromCameraInfo(camera_info)

        self.__listener = tf.TransformListener()
        self.__bounding_box_converter = BoundingBoxConverter(
            self.__listener,
            cam_model,
            camera_fov_deg,
            min_2d_boundingbox_x,
            min_2d_boundingbox_y,
            roi_required_object_distance,
        )

        # Data variables
        self.__camera_adjustment = CameraAdjustment(self.__screen_parameter)

        self.__gaze_buffer = GazeBuffer()
        punishment_model = PunishmentModel(dbscan_max_distance_px,
                                           dbscan_min_samples,
                                           punishment_factor)
        self.__situation_awareness = SituationAwareness(punishment_model)
        self.__roi_tracker = SituationElementTracker(
            IdTrackingStrategy(),
            SituationAwarenessParameter(time_last_looked_detected_s,
                                        num_gaze_comprehended, 1),
        )
        self.__object_filter = ObjectFilter(
            max_object_distance,
            FieldOfView(camera_fov_deg, cam_model),
            ego_vehicle_info.id,
            {
                Object.CLASSIFICATION_PEDESTRIAN: "/walker/",
                Object.CLASSIFICATION_BIKE: "/vehicle/",
                Object.CLASSIFICATION_CAR: "/vehicle/",
                Object.CLASSIFICATION_TRUCK: "/vehicle/",
                Object.CLASSIFICATION_MOTORCYCLE: "/vehicle/",
                Object.CLASSIFICATION_OTHER_VEHICLE: "/vehicle/",
            },
        )
        # Publishers
        self.__roi_pub = rospy.Publisher("~roi", ROIArray, queue_size=10)
        self.__sa_pub = rospy.Publisher("~sa", SA, queue_size=10)

        # Visualization
        if rospy.get_param("~visualize", True):
            self.__image_visualization = ImageVisualization(
                None,
                False,
                True,
            )
            rospy.loginfo("Visualization enabled.")
            self.__image_pub = rospy.Publisher("~image", Image, queue_size=10)
            self.__sub_camera = rospy.Subscriber(
                f"{self.__camera_tf_name}/image_color",
                Image,
                self.image_callback,
            )

        rospy.loginfo("CARLA Awareness Model started.")
Esempio n. 9
0
class ObjectAwarenessModel:
    """Detects which objects were recognized by the operator"""
    def __init__(self):
        rospy.loginfo("Starting CARLA Awareness Model...")

        gaze_topic = rospy.get_param("~gaze_topic", "/gaze_publisher/gaze")
        max_object_distance = rospy.get_param("~objects/max_distance_m", 100)
        roi_required_object_distance = rospy.get_param(
            "~objects/roi_required_distance_m", 30)

        self.__screen_parameter = ScreenParameter(
            rospy.get_param("~screen/padding/top_px", 0),
            rospy.get_param("~screen/padding/left_px", 0),
            rospy.get_param("~screen/padding/right_px", 0),
            rospy.get_param("~screen/padding/bottom_px", 0),
            rospy.get_param("~screen/window_pos/x_px", 1920),
            rospy.get_param("~screen/window_pos/y_px", 0),
            rospy.get_param("~screen/monitor_resolution/x_px", 1920),
            rospy.get_param("~screen/monitor_resolution/y_px", 1080),
            rospy.get_param("~screen/camera_resolution/x_px", 640),
            rospy.get_param("~screen/camera_resolution/y_px", 480),
        )

        self.__offset_topbar_px = rospy.get_param("~offset_topbar_px", 30)

        camera_fov_deg = rospy.get_param("~camera_fov_deg", 90)
        min_2d_boundingbox_x = rospy.get_param("~min_2d_boundingbox/x_px", 90)
        min_2d_boundingbox_y = rospy.get_param("~min_2d_boundingbox/y_px", 90)
        time_last_looked_detected_s = rospy.get_param(
            "~awareness_detection/time_last_looked_detected_s", 5)
        num_gaze_comprehended = rospy.get_param(
            "~awareness_detection/num_gaze_comprehended_s", 35)
        dbscan_max_distance_px = rospy.get_param(
            "~awareness_detection/dbscan_max_distance_px", 15)
        dbscan_min_samples = rospy.get_param(
            "~awareness_detection/dbscan_min_samples", 3)
        punishment_factor = rospy.get_param(
            "~awareness_detection/punishment_factor", 0.25)

        rospy.loginfo(
            "Waiting for vehicle_info and camera_info topics from carla ros bridge"
        )
        self.__camera_tf_name = (rospy.get_param(
            "~camera_tf_name", "/carla/ego_vehicle/camera/rgb/front"), )
        ego_vehicle_info = rospy.wait_for_message(
            rospy.get_param("~vehicle_info_topic",
                            "/carla/ego_vehicle/vehicle_info"),
            CarlaEgoVehicleInfo,
        )
        camera_info = rospy.wait_for_message(
            f"{self.__camera_tf_name}/camera_info",
            CameraInfo,
        )
        rospy.loginfo("Carla ros bridge is online.")

        # Subscribers
        self.__sub_gaze = rospy.Subscriber(gaze_topic, GazeArray,
                                           self.gaze_callback)
        self.__sub_objects = rospy.Subscriber("/carla/objects", ObjectArray,
                                              self.objects_callback)

        # Create camera model from camera info
        cam_model = image_geometry.PinholeCameraModel()
        cam_model.fromCameraInfo(camera_info)

        self.__listener = tf.TransformListener()
        self.__bounding_box_converter = BoundingBoxConverter(
            self.__listener,
            cam_model,
            camera_fov_deg,
            min_2d_boundingbox_x,
            min_2d_boundingbox_y,
            roi_required_object_distance,
        )

        # Data variables
        self.__camera_adjustment = CameraAdjustment(self.__screen_parameter)

        self.__gaze_buffer = GazeBuffer()
        punishment_model = PunishmentModel(dbscan_max_distance_px,
                                           dbscan_min_samples,
                                           punishment_factor)
        self.__situation_awareness = SituationAwareness(punishment_model)
        self.__roi_tracker = SituationElementTracker(
            IdTrackingStrategy(),
            SituationAwarenessParameter(time_last_looked_detected_s,
                                        num_gaze_comprehended, 1),
        )
        self.__object_filter = ObjectFilter(
            max_object_distance,
            FieldOfView(camera_fov_deg, cam_model),
            ego_vehicle_info.id,
            {
                Object.CLASSIFICATION_PEDESTRIAN: "/walker/",
                Object.CLASSIFICATION_BIKE: "/vehicle/",
                Object.CLASSIFICATION_CAR: "/vehicle/",
                Object.CLASSIFICATION_TRUCK: "/vehicle/",
                Object.CLASSIFICATION_MOTORCYCLE: "/vehicle/",
                Object.CLASSIFICATION_OTHER_VEHICLE: "/vehicle/",
            },
        )
        # Publishers
        self.__roi_pub = rospy.Publisher("~roi", ROIArray, queue_size=10)
        self.__sa_pub = rospy.Publisher("~sa", SA, queue_size=10)

        # Visualization
        if rospy.get_param("~visualize", True):
            self.__image_visualization = ImageVisualization(
                None,
                False,
                True,
            )
            rospy.loginfo("Visualization enabled.")
            self.__image_pub = rospy.Publisher("~image", Image, queue_size=10)
            self.__sub_camera = rospy.Subscriber(
                f"{self.__camera_tf_name}/image_color",
                Image,
                self.image_callback,
            )

        rospy.loginfo("CARLA Awareness Model started.")

    def gaze_callback(self, gaze_data):
        # Subtract top bar because simulator is not fullscreen
        for gaze in gaze_data.gazes:
            self.__camera_adjustment.adjust_gaze_data_to_camera_resolution(
                gaze)
        self.__gaze_buffer.push_array(gaze_data.gazes)

    def transform_objects(self, object_msgs):
        """Transform objects to camera coordinate system"""
        transformed_objects = []
        for object_msg in object_msgs:
            object_topic = self.__object_filter.get_topic_for_object(
                object_msg)
            if self.__listener.canTransform(self.__camera_tf_name,
                                            object_topic, rospy.Time(0)):
                try:
                    (
                        [
                            object_msg.pose.position.x,
                            object_msg.pose.position.y,
                            object_msg.pose.position.z,
                        ],
                        [
                            object_msg.pose.orientation.x,
                            object_msg.pose.orientation.y,
                            object_msg.pose.orientation.z,
                            object_msg.pose.orientation.w,
                        ],
                    ) = self.__listener.lookupTransform(
                        self.__camera_tf_name, object_topic, rospy.Time(0))
                    transformed_objects.append(object_msg)
                except ():
                    pass
        return transformed_objects

    def objects_callback(self, object_array):
        """Callback for carla objects, position of objects in image plane is calculated here"""
        filtered_objects = self.__object_filter.type_filter(
            object_array.objects)
        filtered_objects = self.transform_objects(filtered_objects)
        filtered_objects = self.__object_filter.geometry_filter(
            filtered_objects)
        rois = [
            self.__bounding_box_converter.convert(
                obj,
                self.__object_filter.get_topic_for_object(obj)).to_roi_msg()
            for obj in filtered_objects
        ]

        self.__roi_tracker.add_or_update_ses(rois)
        self.__roi_tracker.update_elements(self.__gaze_buffer.data)
        self.__situation_awareness.calculate_sa(
            self.__roi_tracker.se_list, self.__roi_tracker.non_roi_gazes)

        self.__gaze_buffer.clear()
        self.__sa_pub.publish(self.__situation_awareness.sa_msg)

        roi_array = ROIArray()
        roi_array.rois = [
            element.roi_msg for element in self.__roi_tracker.se_list
        ]
        self.__roi_pub.publish(roi_array)

    def image_callback(self, image):
        """Callback for camera image from carla, objects and their recognition is visualized here"""
        try:
            img = self.__image_visualization.get_image_from_msg(image)
        except CvBridgeError as error:
            rospy.logerr(error)
            return

        self.__image_visualization.display_debug_visualization(
            img, self.__gaze_buffer.current_gaze, self.__roi_tracker.se_list)

        self.__image_pub.publish(self.__image_visualization.image_to_msg(img))
Esempio n. 10
0
    def __init__(self):
        rospy.loginfo("Starting Awareness model...")

        gaze_topic = rospy.get_param("~gaze_topic", "/gaze_publisher/gaze")
        roi_topic = rospy.get_param(
            "~roi_topic", "/carla/ego_vehicle/camera/rgb/front/rois")

        self.__screen_parameter = ScreenParameter(
            rospy.get_param("~screen/padding/top_px", 0),
            rospy.get_param("~screen/padding/left_px", 0),
            rospy.get_param("~screen/padding/right_px", 0),
            rospy.get_param("~screen/padding/bottom_px", 0),
            rospy.get_param("~screen/window_pos/x_px", 1920),
            rospy.get_param("~screen/window_pos/y_px", 0),
            rospy.get_param("~screen/monitor_resolution/x_px", 1920),
            rospy.get_param("~screen/monitor_resolution/y_px", 1080),
            rospy.get_param("~screen/camera_resolution/x_px", 640),
            rospy.get_param("~screen/camera_resolution/y_px", 480),
        )
        time_last_looked_detected_s = rospy.get_param(
            "~awareness_detection/time_last_looked_detected_s", 5)
        tracker_roi_max_distance_px = rospy.get_param(
            "~awareness_detection/tracker_roi_max_distance_px", 50)
        num_gaze_comprehended = rospy.get_param(
            "~awareness_detection/num_gaze_comprehended_s", 35)
        dbscan_max_distance_px = rospy.get_param(
            "~awareness_detection/dbscan_max_distance_px", 15)
        dbscan_min_samples = rospy.get_param(
            "~awareness_detection/dbscan_min_samples", 3)
        punishment_factor = rospy.get_param(
            "~awareness_detection/punishment_factor", 0.25)

        # Data variables
        self.__camera_adjustment = CameraAdjustment(self.__screen_parameter)
        self.__gaze_buffer = GazeBuffer()
        punishment_model = PunishmentModel(dbscan_max_distance_px,
                                           dbscan_min_samples,
                                           punishment_factor)
        self.__situation_awareness = SituationAwareness(punishment_model)
        self.__roi_tracker = SituationElementTracker(
            DistanceTrackingStrategy(tracker_roi_max_distance_px),
            SituationAwarenessParameter(time_last_looked_detected_s,
                                        num_gaze_comprehended, 1),
        )

        # Subscribers
        self.__sub_gaze = rospy.Subscriber(gaze_topic, GazeArray,
                                           self.gaze_callback)
        self.__sub_rois = rospy.Subscriber(roi_topic, ROIArray,
                                           self.roi_callback)

        # Publishers
        self.__pub_sa = rospy.Publisher("~sa", SA, queue_size=10)

        # Visualization
        if rospy.get_param("~visualize", True):
            compress = rospy.get_param("~compress", False)
            self.__image_visualization = ImageVisualization(
                self.__screen_parameter,
                compress,
                rospy.get_param("~visualize_debug", False),
            )
            rospy.loginfo("Visualization enabled.")
            self.__image_pub = rospy.Publisher("~image", Image, queue_size=10)
            topic = rospy.get_param(
                "~image_topic",
                "/carla/ego_vehicle/camera/rgb/front/image_color")
            msg_type = CompressedImage if compress else Image
            if compress:
                topic += "/compressed"
            self.__sub_camera = rospy.Subscriber(topic, msg_type,
                                                 self.image_callback)

        rospy.loginfo("Awareness model started.")
Esempio n. 11
0
class AwarenessModel:
    """Detects which ROIs were recognized by the operator using a free awareness model"""
    def __init__(self):
        rospy.loginfo("Starting Awareness model...")

        gaze_topic = rospy.get_param("~gaze_topic", "/gaze_publisher/gaze")
        roi_topic = rospy.get_param(
            "~roi_topic", "/carla/ego_vehicle/camera/rgb/front/rois")

        self.__screen_parameter = ScreenParameter(
            rospy.get_param("~screen/padding/top_px", 0),
            rospy.get_param("~screen/padding/left_px", 0),
            rospy.get_param("~screen/padding/right_px", 0),
            rospy.get_param("~screen/padding/bottom_px", 0),
            rospy.get_param("~screen/window_pos/x_px", 1920),
            rospy.get_param("~screen/window_pos/y_px", 0),
            rospy.get_param("~screen/monitor_resolution/x_px", 1920),
            rospy.get_param("~screen/monitor_resolution/y_px", 1080),
            rospy.get_param("~screen/camera_resolution/x_px", 640),
            rospy.get_param("~screen/camera_resolution/y_px", 480),
        )
        time_last_looked_detected_s = rospy.get_param(
            "~awareness_detection/time_last_looked_detected_s", 5)
        tracker_roi_max_distance_px = rospy.get_param(
            "~awareness_detection/tracker_roi_max_distance_px", 50)
        num_gaze_comprehended = rospy.get_param(
            "~awareness_detection/num_gaze_comprehended_s", 35)
        dbscan_max_distance_px = rospy.get_param(
            "~awareness_detection/dbscan_max_distance_px", 15)
        dbscan_min_samples = rospy.get_param(
            "~awareness_detection/dbscan_min_samples", 3)
        punishment_factor = rospy.get_param(
            "~awareness_detection/punishment_factor", 0.25)

        # Data variables
        self.__camera_adjustment = CameraAdjustment(self.__screen_parameter)
        self.__gaze_buffer = GazeBuffer()
        punishment_model = PunishmentModel(dbscan_max_distance_px,
                                           dbscan_min_samples,
                                           punishment_factor)
        self.__situation_awareness = SituationAwareness(punishment_model)
        self.__roi_tracker = SituationElementTracker(
            DistanceTrackingStrategy(tracker_roi_max_distance_px),
            SituationAwarenessParameter(time_last_looked_detected_s,
                                        num_gaze_comprehended, 1),
        )

        # Subscribers
        self.__sub_gaze = rospy.Subscriber(gaze_topic, GazeArray,
                                           self.gaze_callback)
        self.__sub_rois = rospy.Subscriber(roi_topic, ROIArray,
                                           self.roi_callback)

        # Publishers
        self.__pub_sa = rospy.Publisher("~sa", SA, queue_size=10)

        # Visualization
        if rospy.get_param("~visualize", True):
            compress = rospy.get_param("~compress", False)
            self.__image_visualization = ImageVisualization(
                self.__screen_parameter,
                compress,
                rospy.get_param("~visualize_debug", False),
            )
            rospy.loginfo("Visualization enabled.")
            self.__image_pub = rospy.Publisher("~image", Image, queue_size=10)
            topic = rospy.get_param(
                "~image_topic",
                "/carla/ego_vehicle/camera/rgb/front/image_color")
            msg_type = CompressedImage if compress else Image
            if compress:
                topic += "/compressed"
            self.__sub_camera = rospy.Subscriber(topic, msg_type,
                                                 self.image_callback)

        rospy.loginfo("Awareness model started.")

    def gaze_callback(self, gaze_data):
        """Callback for gaze data points"""
        for gaze in gaze_data.gazes:
            self.__camera_adjustment.adjust_gaze_data_to_camera_resolution(
                gaze)
        self.__gaze_buffer.push_array(gaze_data.gazes)

    def roi_callback(self, roi_array):
        """Callback for roi array messages"""
        self.__roi_tracker.add_or_update_ses(roi_array.rois)
        self.__roi_tracker.update_elements(self.__gaze_buffer.data)
        self.__situation_awareness.calculate_sa(
            self.__roi_tracker.se_list, self.__roi_tracker.non_roi_gazes)

        self.__gaze_buffer.clear()
        self.__pub_sa.publish(self.__situation_awareness.sa_msg)

    def image_callback(self, image):
        """Callback for camera image from carla, objects and their recognition is visualized here"""
        try:
            img = self.__image_visualization.get_image_from_msg(image)
        except CvBridgeError as error:
            rospy.logerr(error)
            return

        self.__image_visualization.display_debug_visualization(
            img, self.__gaze_buffer.current_gaze, self.__roi_tracker.se_list)
        self.__image_visualization.display_image(img)

        cv2.waitKey(3)
        self.__image_pub.publish(self.__image_visualization.image_to_msg(img))
Esempio n. 12
0
    def test_update_best_candidate__no_candidates__nothing_changed(self):
        SituationElementTracker.update_best_candidate([], ROI())

        self.assertTrue(True)
Esempio n. 13
0
    def test_constructor__empty_list(self):
        tracker = SituationElementTracker(None, 1)

        self.assertFalse(tracker.se_list)
        self.assertFalse(tracker.non_roi_gazes)