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)
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))
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)
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))
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)
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)
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)
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.")
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))
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.")
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))
def test_update_best_candidate__no_candidates__nothing_changed(self):
SituationElementTracker.update_best_candidate([], ROI())
self.assertTrue(True)
def test_constructor__empty_list(self):
tracker = SituationElementTracker(None, 1)
self.assertFalse(tracker.se_list)
self.assertFalse(tracker.non_roi_gazes)