Комплексное робототехническое решение для сбора клубники. Часть 1

[Poison48]

Всегда думал, что в промышленности клубнику выращиваю на подвесных грядках.

[wowa144]

Не обязательно так, в нашей ферме клубника находится на стеллажах, а ягоды свешиваются на длинных стеблях вниз.

В качестве иллюстрации:

[Sencis]

нужно решить гораздо более фундаментальную задачу — научить мобильную платформу нормально перемещаться между заданными точками.

Я давно решаю эту фундаментальную задачу, и, честно говоря, конца и края ей не видно. Если вкратце, то карты в ROS (в частности, costmap) оставляют желать лучшего. Карта представляет собой огромный массив данных, который хорошо параллелится, но в ROS он обрабатывается на CPU. Из-за этого карта обновляется медленно и на небольшом участке.

При этом на GPU можно легко в реальном времени (например, на частоте 60 Гц) обновлять кусок карты 800х800х100 с разрешением 10 см. Это примерно 6 400 кв. м., то есть 40 метров впереди робота. Для дешевого 3д лидара этого вполне достаточно, так как он видит не далее чем 40 метров тёмные предметы такие как дорога. В ROS же скользящее окно составляет всего 5х5 (25 кв. м) или 10х10 (100 кв. м.) метров, чего для улицы явно мало, ведь машины и пешеходы преодолевают это расстояние слишком быстро. В общем, построение карты нужно переносить на GPU, так как использовать для этого CPU — архаизм.Сам алгоритм построения карты в ROS тоже примитивный, или 2д срез или проекцию карты сверху вниз строит в costmap в которой роботу доступны далеко не все зоны где он может проехать.

Существующие планировщики (SmacPlanner Hybrid A*, State Lattice) работают медленно, поскольку тоже задействуют CPU. Пути они ищут плохо, особенно для ходовой Аккермана: робот порой заезжает в тупик, а выехать обратно задом по тому же пути уже не может. Примитивы приходится закладывать большие, с запасом, чтобы контроллеры могли по ним нормально идти. Однако тот же State Lattice не позволяет бесконечно увеличивать их размер — слишком большие примитивы не сходятся в сетке. Приходится балансировать на грани, ведь чем больше примитив, тем плавнее маршрут и тем легче контроллеру вести платформу.

Что касается контроллеров, то радует появление более современного решения с поддержкой CUDA mppi т.к. контроллеры прогностические с моделью едят много ресурсов и особенно MPPI. Но без обработки карты на CUDA его смысл частично теряется, контроллер работает быстро и прогнозирует далеко а карта маленькая, не раскрывает его потенциал. Тем не менее разработчикам всё равно спасибо. Правда, модель оставили примитивную — кинематическую. Хотя в MPPI Generic есть примеры моделей, поддерживающих динамику, моделирование сил инерции, трения и заноса. Это необходимо для того, чтобы базовая кинематика работала точнее, а MPPI понимал, что колёса при повороте плугуют и мог разворачиваться на месте с учётом ограничений ходовой, и парковаться правильно.

Одометрия во всех стандартных пакетах идет без детектирования планарных сцен, так что этот узел снова придётся собирать самостоятельно и настраивать переключение на резервный источник. Динамические препятствия тоже нужно детектировать своими силами. Всё начинается с поиска движущихся точек: большинство систем одометрии внутри себя их рассчитывают и фильтруют, но наружу не выдают. Они отдают очищенные облака, а сами подвижные точки — нет. А значит, опять придётся изобретать велосипед.

Примерно так выглядит RoadMap для решения задачи «научить мобильную платформу нормально перемещаться между заданными точками». И это уровень еще даже не Яндекс ровера (там нейросети распознают прохожих, тротуары, преграды на пути не геометрические, дорожные знаки, разметку и т.д. ), а просто базовой езды по координатам на улице.

[wowa144]

Если вы хотите использовать код из предыдущей статьи для управления манипулятором, то потребуется ряд правок в текущем коде.

Да. Тут не надо переделывать архитектуру. Самый простой вариант — добавить управление манипулятором прямо в RMC2Controller, а потом вызвать его в нужном месте main().

Но у тебя есть один важный момент: первый код управляет манипулятором RMC1, а второй — мобильной платформой RMC2. Ниже я оставляю топики манипулятора именно /RMC1/..., как в твоём рабочем примере. Если манипулятор физически установлен на РМК-2, потом просто заменишь RMC1 на RMC2.

1. Добавление импортов

К существующим импортам добавить еще 3, отвечающие за траектории, action и захват:

from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
from control_msgs.action import GripperCommand
from rclpy.action import ActionClient

2. В init добавить манипулятор и захват

В конец init класса RMC2Controller:

        self.arm = self.create_publisher(
            JointTrajectory,
            "/RMC1/arm95/arm_joint_trajectory_controller/joint_trajectory",
            10
        )

        self.gripper = ActionClient(
            self,
            GripperCommand,
            "/RMC1/arm95/gripper_controller/gripper_cmd"
        )

3. Добавить две функции в RMC2Controller

Например, сразу после lift_down():

    def move_arm(self):
        msg = JointTrajectory()

        msg.joint_names = [
            "joint1",
            "joint2",
            "joint3",
            "joint4",
            "joint5",
            "joint6"
        ]

        point = JointTrajectoryPoint()

        point.positions = [
            0.1,
            0.3,
            -0.0,
            0.0,
            0.0,
            0.0
        ]

        point.time_from_start.sec = 3

        msg.points = [point]

        self.arm.publish(msg)

        time.sleep(3)


    def close_gripper(self):
        self.gripper.wait_for_server()

        goal = GripperCommand.Goal()
        goal.command.position = 0.0 #0.04 для открытого

        self.gripper.send_goal_async(goal)

        rclpy.spin_once(
            self,
            timeout_sec=1
        )

На этом основная часть правок завершена, отдельный код манипулятора теперь превратился фактически в две команды:

controller.move_arm()
controller.close_gripper()

4. Теперь вставляем их в сценарий

Допустим, логика должна быть такой:

СТАРТ  ↓
едем к хранению  ↓
поднимаем лифт  ↓
едем к комплектации  ↓
двигаем манипулятор  ↓
закрываем захват  ↓
опускаем лифт  ↓
возвращаемся домой

Тогда в main() меняется только этот кусок:

    try:
        controller.follow_route(route_storage)

        if not controller.lift_up():
            return

        controller.follow_route(route_picking)

        controller.move_arm()
        controller.close_gripper()

        if not controller.lift_down():
            return

        controller.follow_route(route_home)

То есть в коде фактически нужно сделать три изменения: добавить импорты, добавить publisher/action client и добавить две функции.

Есть только один нюанс в move_arm(): установлено time.sleep(3), чтобы следующий этап не начался раньше, чем закончится заданное трёхсекундное движение манипулятора. Это самый простой вариант для твоего текущего кода. Позже лучше заменить его на нормальное ожидание результата контроллера.

Также стоит напомнить, что возможен запуск методом ros2 run имя пакета имя ноды. Можно также запустить стандартным методом: python3 имя_файла.py из той директории, в которой лежит файл.