Подглава 11.3.1 Добавление глубины в трекер объектов

Программа следовантя за объектами похожа на скрипт object tracker, но на этот раз мы добавим информацию о глубине. Существует два способа получения значений глубины из камеры RGBD с помощью ROS: мы можем подписаться либо на карту глубины, опубликованную узлом openni или freenect, и использовать OpenCV для обработки этого изображения, либо мы можем подписаться на облако точек глубины и использовать PCL. Для узла Object follower мы будем использовать карту глубины и OpenCV. Для нашего скрипта "person follower", описанного в следующем разделе, мы покажем, как использовать облако точек и узлы PCL.

Драйвер камеры openni или freenect публикует карту глубины в разделе /camera/depth_registered/image_raw, используя тип сообщения sensor_msgs / Image. Каждый "пиксель" в изображении хранит значение глубины в этой точке в миллиметрах. Поэтому нам нужно будет разделить эти значения на 1000, чтобы получить результаты в метрах. Наша функция обратного вызова будет использовать cv_bridge для преобразования карты глубины в массив Numpy, который мы можем использовать для вычисления средней глубины по ROI.

Вместо того, чтобы перечислять весь скрипт, давайте сосредоточимся на том, что нового относительно узла отслеживания объектов из предыдущего раздела.

self.depth_subscriber = rospy.Subscriber("depth_image", Image, self.convert_depth_image, queue_size=1)

Здесь мы подписываемся на топик карты глубины и назначаем функцию обратного вызова convert_depth_image (). Мы используем общее имя топика "depth_image", чтобы можно было переназначить ее в файле запуска. Если вы посмотрите на launch-файл object_follower.launch, расположенный в каталоге rbx1_apps/launch, вы увидите, что мы делаем соответствующее переназначение следующим образом:

<remap from="camera_info" to="/camera/rgb/camera_info" />

<remap from="depth_image" to="/camera/depth_registered/image_rect" />

Наша функция обратного вызова convert_depth_image() выглядит следующим образом:

def convert_depth_image(self, ros_image):
    # Используем cv_bridge(), чтобы конвертировать изображение ROS в формат OpenCV
    try:
        # Преобразование карты глубины с помощью стандартного сквозного шифрования
        depth_image = self.cv_bridge.imgmsg_to_cv2(ros_image, "passthrough")
    except CvBridgeError, e:
        print e
        # Преобразование карты глубины в массив numpy
        self.depth_array = np.array(depth_image, dtype=np.float32)

Вспомним из скрипта отслеживания объектов, что мы используем функцию обратного вызова set_cmd_vel () для сопоставления сообщений в топике /roi с командами Twist для перемещения робота. В этом случае мы рассчитали смещение цели влево-вправо, чтобы можно было поворачивать робота, чтобы держать мишень в центре обзора камеры. Теперь мы добавим следующий код, чтобы получить среднюю глубину для ROI.

self.lock.acquire()
 
try:
   
    # Инициализируем несколько переменных глубины
    n_z = sum_z = mean_z = 0
 
    # Немного сузим ROI, чтобы избежать глубины фона вблизи границ
    scaled_width = int(self.roi.width * self.scale_roi)
    scaled_height = int(self.roi.height * self.scale_roi)
 
    # Получим минимальные/максимальные значения x и y из масштабируемого ROI
    min_x = int(self.roi.x_offset + self.roi.width * (1.0 - self.scale_roi) / 2.0)
    max_x = min_x + scaled_width
    min_y = int(self.roi.y_offset + self.roi.height * (1.0 - self.scale_roi) / 2.0)
    max_y = min_y + scaled_height
 
    # Получим среднее значение глубины по ROI
    for x in range(min_x, max_x):
        for y in range(min_y, max_y):
            try:
                # Получим значение глубины в метрах
                z = self.depth_array[y, x]
 
                # Проверим наличие значений NaN, возвращаемых драйвером камеры
                if isnan(z):
                    continue
 
            except:
                # It seems to work best if we convert exceptions to 0
                z = 0
 
            # Способ преобразования миллиметров в метры для драйвера freenect
            if z > 100:
                z /= 1000.0
 
            # Проверим наличие значений за пределами максимального диапазона
            if z > self.max_z:
                continue
 
            # Увеличим сумму и подсчитаем
            sum_z = sum_z + z
            n_z += 1
 
    # Остановим движение робота вперед/назад
    linear_x = 0
 
    # Если у нас есть значения глубины...
    if n_z:
        mean_z = float(sum_z) / n_z
 
        # Не позволяем среднему значению упасть ниже минимального надежного диапазона
        mean_z = max(self.min_z, mean_z)
 
        # Проверим среднее значение по отношению к минимальному диапазону
        if mean_z > self.min_z:
            # Check the max range and goal threshold
            if mean_z < self.max_z and (abs(mean_z - self.goal_z) > self.z_threshold):
                speed = (mean_z - self.goal_z) * self.z_scale
                linear_x = copysign(min(self.max_linear_speed, max(self.min_linear_speed, abs(speed))), speed)
 
    if linear_x == 0:
        # Плавно остановим робота
        self.move_cmd.linear.x *= self.slow_down_factor
    else:
        self.move_cmd.linear.x = linear_x
 
finally:
    # Снимем блокировку
    self.lock.release()

Обратные вызовы ROS работают в своем собственном потоке, поэтому сначала мы устанавливаем блокировку для защиты любых переменных, которые также могут быть обновлены в основном теле нашего скрипта. Это включает в себя переменную self.move_cmd для управления движением робота и флаг self.target_visible , который указывает, потеряли ли мы цель или нет. Если бы мы не использовали блокировку, то могли бы возникнуть непредсказуемые результаты, когда и обратный вызов, и тело скрипта пытаются использовать эти переменные одновременно.

Сначала мы получаем диапазон значений x и y, который охватывает область, немного меньшую, чем ROI. Мы сужаем ROI за счет self.scale_roi (по умолчанию: 0.9), чтобы мы не получали значения расстояния от фона вблизи краев отслеживаемого объекта. Более изощренным подходом было бы отбрасывание значений глубины, превышающих среднюю глубину на определенный порог.

Затем мы делаем цикл по координатам x-y суженого ROI и извлекаем значение глубины в каждой точке из массива глубин, который мы получили из карты глубины обратного вызова. Обратите внимание, как меняется порядок x и y при индексации массива глубины. Мы также проверяем наличие исключений и устанавливаем нулевую глубину для этих точек. Мы также могли бы просто отбросить эти точки, но оказывается, что, установив их на ноль, мы на самом деле получаем лучшее следование поведению, когда человек находится рядом с роботом.

Наконец, мы вычисляем среднюю глубину и устанавливаем компонент линейной скорости робота, чтобы он либо двигался к цели, либо удалялся от нее в зависимости от того, находимся ли мы слишком далеко или слишком близко по сравнению с нашим целевым расстоянием, установленным в параметре self. goal_z. Если у нас нет никаких допустимых значений глубины, мы плавно останавливаем робота, снижая линейную скорость на 10%. (Не путайте использование "z" для получения информации о глубине от камеры и "x" для линейной скорости робота. Это просто следствие двух различных соглашений о координатах, используемых для камер и движения робота.)

Когда мы закончим с обратным вызовом, мы снимем блокировку.

В то же время наш основной цикл работает следующим образом:

 while not rospy.is_shutdown():
    # Если цель не видна, плавно остановим робота 
    self.lock.acquire()
 
    try:
        if not self.target_visible:
            self.move_cmd.linear.x *= self.slow_down_factor
            self.move_cmd.angular.z *= self.slow_down_factor
        else:
            # Reset the flag to False by default
            self.target_visible = False
    finally:
        self.lock.release()
 
    # Отправим команду Twist роботу
    self.cmd_vel_pub.publish(self.move_cmd)
 
    # Сон на 1/self.rate секунды
    r.sleep()

Единственное отличие по сравнению с циклом, который используется в скрипте object_tracker.py заключается в том, что теперь мы получаем блокировку в начале и освобождаем ее в конце каждой итерации цикла обновления. Это делается для того, чтобы защитить переменные self.move_cmd и self.target_visible, которые также могут быть изменены нашим обратным вызовом set_cmd_vel ().