ROS Workshop 2016: разбор задачи безопасного управления роботом

    Добрый день, уважаемые хабрачитатели! В прошлую пятницу в нашей лаборатории проходил практический мастеркласс по платформе ROS — ROS workshop. Воркшоп был организован для студентов факультета информационных технологий Технического университета Брно, желающих познакомиться с этой платформой. В отличие от предыдущих лет (воркшоп проводится уже 4 года), в этот раз ROS workshop был ориентирован на самостоятельную практическую работу. В статье я собираюсь рассказать о задаче, которая была поставлена перед участниками воркшопа. Кому интересно, прошу под кат.

    image

    Постановка задачи


    Перед участниками была поставлена задача реализовать безопасное управление роботом с остановкой перед препятствиями. Цель задачи — контроль скорости движения робота по направлению вперед. Робот получает данные от сенсора глубины (в нашем случае ASUS Xtion в симуляторе turtlebot_gazebo), находит ближайшее препятствие в направлении движения и определяет три зоны:

    • Safe — робот на безопасном расстоянии, движется без замедления
    • Warning — робот приближается к препятствию, выдает предупреждающий сигнал (например, звуковой сигнал) и замедляется
    • Danger — препятствие очень близко, робот останавливается

    Реализация


    Сразу отмечу, что на воркшопе для выполнения задачи использовался ROS Indigo на Ubuntu 14.04. Я так же использовал ROS Indigo для экспериментов.

    Итак начнем! Создадим пакет с зависимостями roscpp, pcl_ros, pcl_conversions, sensor_msgs и geometry_msgs:

    cd ~/catkin_ws/src
    catkin_create_pkg safety_control_cloud roscpp pcl_ros pcl_conversions sensor_msgs geometry_msgs
    cd ~/catkin_ws
    

    Добавим в зависимости библиотеки PCL в package.xml:

    <build_depend>libpcll-all-dev</build_depend>
    ...
    <run_depend>libpcl-all</run_depend>
    
    

    и в CMakeLists.txt:

    find_package(PCL REQUIRED)
    ...
    include_directories(${PCL_INCLUDE_DIRS})
    

    Добавим скрипт safety_control.cpp в папку src:

    safety_control.cpp
    #include "ros/ros.h"
    #include "pcl_conversions/pcl_conversions.h"
    #include <pcl/pcl_base.h>
    #include <sstream>
    #include <sensor_msgs/PointCloud2.h>
    #include <pcl/filters/passthrough.h>
    #include <pcl/common/common.h>
    #include <geometry_msgs/Twist.h>
    
    typedef pcl::PointXYZ PointType;
    typedef pcl::PointCloud<PointType> PointCloud;
    typedef PointCloud::Ptr PointCloudPtr;
    
    ros::Publisher pcd_pub_, cmd_vel_pub_;
    
    void pcd_cb(const sensor_msgs::PointCloud2ConstPtr& pcd) {
        ROS_INFO_STREAM_ONCE("Point cloud arrived");
        PointCloudPtr pcd_pcl = PointCloudPtr(new PointCloud), pcd_filtered = PointCloudPtr(new PointCloud);
        PointType pt_min, pt_max;
        pcl::fromROSMsg(*pcd, *pcd_pcl);
    
        pcl::PassThrough<PointType> pass;
        pass.setInputCloud(pcd_pcl);
        pass.setFilterFieldName("y");
        pass.setFilterLimits(-0.25,0.20);
        pass.filter(*pcd_filtered);
    
        pass.setInputCloud(pcd_filtered);
        pass.setFilterFieldName("x");
        pass.setFilterLimits(-0.3,0.3);
        pass.filter(*pcd_pcl);
    
        pcl::getMinMax3D(*pcd_pcl, pt_min, pt_max);
    
        geometry_msgs::Twist vel;
        if (pt_min.z > 1.0) {
            vel.linear.x = 0.2;
            ROS_INFO_STREAM("Safe zone");
        } else if (pt_min.z > 0.5) {
            vel.linear.x = 0.1;
            ROS_INFO_STREAM("Warning zone");
        } else {
            vel.linear.x = 0.0;
            ROS_INFO_STREAM("Danger zone");
        }
    
        cmd_vel_pub_.publish(vel);
    
        sensor_msgs::PointCloud2 pcd_out;
        pcl::toROSMsg(*pcd_pcl, pcd_out);
    
        pcd_pub_.publish(pcd_out);
    }
    
    int main(int argc, char **argv)
    {
      /**
       * The ros::init() function needs to see argc and argv so that it can perform
       * any ROS arguments and name remapping that were provided at the command line.
       * For programmatic remappings you can use a different version of init() which takes
       * remappings directly, but for most command-line programs, passing argc and argv is
       * the easiest way to do it.  The third argument to init() is the name of the node.
       *
       * You must call one of the versions of ros::init() before using any other
       * part of the ROS system.
       */
      ros::init(argc, argv, "safety_control_cloud");
    
      /**
       * NodeHandle is the main access point to communications with the ROS system.
       * The first NodeHandle constructed will fully initialize this node, and the last
       * NodeHandle destructed will close down the node.
       */
      ros::NodeHandle n;
    
      ros::Subscriber pcd_sub = n.subscribe("/camera/depth/points", 1, pcd_cb);
    
      pcd_pub_ = n.advertise<sensor_msgs::PointCloud2>("/output", 1);
      cmd_vel_pub_ = n.advertise<geometry_msgs::Twist>("/cmd_vel_mux/input/teleop", 1);
    
      ros::spin();
    
      return 0;
    }
    


    Добавим скрипт safety_control.cpp в CMakeLists.txt:

    add_executable(safety_control_node src/safety_control.cpp)
    target_link_libraries(safety_control_node ${catkin_LIBRARIES} ${PCL_LIBRARIES})
    

    В логике узла мы подписываемся на данные с топика /camera/depth/points, получаем облако точек, вычисляем координаты ближайшей точки к сенсору глубины в облаке точек и в зависимости от ситуации публикуем линейную скорость типа geometry_msgs/Twister в топик /cmd_vel_mux/input/teleop.

    Нам также необходимо сделать срез облака точек в нескольких осях в определенном диапазоне для более эффективной обработки. В следующих строках:

    
        pcl::PassThrough<PointType> pass;
        pass.setInputCloud(pcd_pcl);
        pass.setFilterFieldName("y");
        pass.setFilterLimits(-0.25,0.20);
        pass.filter(*pcd_filtered);
    

    обрезаем облако методом PassThrough на 25 см вниз и на 20 см вверх от начала системы координат сенсора глубины (по оси y).

    В строках:

    
    pass.setInputCloud(pcd_filtered);
    pass.setFilterFieldName("x");
    pass.setFilterLimits(-0.3,0.3);
    pass.filter(*pcd_pcl);
    

    Обрезаем облако на 0.3 м (30 см) влево и вправо от начала системы координат сенсора (оси z). Затем ищем ближайшую точку в облаке точек по оси z (ось из центра сенсора глубины в направлении обзора) — это и будет точка ближайшего объекта:

    
    pcl::getMinMax3D(*pcd_pcl, pt_min, pt_max);
    

    Скорость будет опубликована также на топик /mobile_base/commands/velocity. Скомпилируем пакет:

    cd ~/catkin_ws
    catkin_make
    source devel/setup.bash

    Тестирование в симуляторе Turtle Bot в Gazebo


    Вторая задача заключалась в испытании логики управления роботом с симуляторе TurtleBot в Gazebo. Для этого необходимо установить turtlebot_gazebo с помощью apt-get:

    sudo apt-get install ros-indigo-turtlebot*
    

    Здесь можно найти несколько полезных туториалов по использованию симулятора. Симулятор может быть хорошим решением в случае, когда хочется изучить пакеты навигации в ROS и нет под рукой реального робота. Запустим симулятор:

    roslaunch turtlebot_gazebo turtlebot_world.launch
    

    Откроется окно Gazebo как на картинке:

    image

    Мы можем приближать и удалять картинку колесом мышки. С помощью зажатой левой кнопки мыши и курсора мы можем перемещать картинку влево, вправо, вверх и вниз. С помощью зажатого колеса мыши и курсора можно изменять вертикальный угол обзора. Теперь повернем робота, чтобы он смотрел прямо на шкаф. В верхнем ряду инструментов над окном просмотра симуляции выберем третью иконку:

    image

    И кликнем на него. Мы увидим что-то подобное

    image

    Повернем робота, кликнув и потянув за синию дугу. У нас получится такая картинка:

    image

    Запустим rviz:

    rosrun rviz rviz
    

    Добавим дисплей RobotModel, как это уже было описано в статье. Добавим также дисплей PointCloud2 и выберем топик /camera/depth/points. В итоге мы получим такую картинку:

    image

    Для дисплея PointCloud2 выберем для поля Color Transformer значение RGB8. Мы получим облако точек в цвете:

    image

    Запустим наш узел safety_control_node:

    rosrun safety_control_cloud safety_control_node
    

    Вывод в терминале будет такой:

    [ INFO] [1479229421.537897080, 2653.960000000]: Point cloud arrived
    [ INFO] [1479229421.572338588, 2654.000000000]: Warning zone
    [ INFO] [1479229421.641967924, 2654.070000000]: Warning zone
    

    Выведем список топиков:

    rostopic list
    

    Среди топиков мы увидим:

    /cmd_vel_mux/input/teleop
    ...
    /mobile_base/commands/velocity
    

    Покажем сообщения в топик /mobile_base/commands/velocity:

    rostopic echo /mobile_base/commands/velocity
    

    Получим скорость робота:

    linear: 
      x: 0.1
      y: 0.0
      z: 0.0
    angular: 
      x: 0.0
      y: 0.0
      z: 0.0
    ---
    

    Робот будет двигаться по направлению к шкафу и наконец остановится рядом со шкафом в зоне Danger. В Gazebo мы увидим полную остановку робота:

    image

    В выводе для узла safety_control_node увидим сообщения:

    [ INFO] [1479229426.604300460, 2658.980000000]: Danger zone
    [ INFO] [1479229426.717093096, 2659.100000000]: Danger zone
    

    И в топик /mobile_base/commands/velocity теперь будет публиковаться сообщение с нулевой скоростью:

    linear: 
      x: 0.0
      y: 0.0
      z: 0.0
    angular: 
      x: 0.0
      y: 0.0
      z: 0.0
    ---
    

    Добавим дисплей типа PointCloud2 с топиком /output в rviz. Выберем для дисплея Color Transformer значение FlatColor и зеленый цвет в поле Color. Это будет наш срез облака точек из узла safety_control_node:

    image

    Переместим робота еще дальше, на безопасное расстояние от препятствия. Для этого нажмем вторую иконку вверху:

    image

    и переместим робота, перетащив его курсором:

    image

    В rviz мы увидим следующее:

    image

    Мы будем получать такие сообщения от нашего узла:

    [ INFO] [1479230429.902116395, 3658.000000000]: Safe zone
    [ INFO] [1479230429.992468971, 3658.090000000]: Safe zone
    

    Скорость робота будет такой:

    ---                                                                                                                                                                                 
    linear: 
      x: 0.2
      y: 0.0
      z: 0.0
    angular: 
      x: 0.0
      y: 0.0
      z: 0.0
    ---
    

    Далее повторится все описанное ранее: замедление в зоне предупреждения и остановка рядом со шкафом.

    Теперь наш робот TurtleBot способен останавливаться перед любым препятствием, которое способен обнаружить сенсор глубины (ASUS Xtion в случае ROS Indigo). Можно попробовать программу управления на реальном роботе, оснащенном сенсором типа Microsoft Kinect.

    На этом все. Мы написали простую программу для управления скоростью робота по направлению вперед с использованием данных сенсора глубины — облака точек — и протестировали его на симуляторе робота TurtleBot в Gazebo.

    Желаю удачи в экспериментах и до новых встреч!

    Похожие публикации

    Реклама
    AdBlock похитил этот баннер, но баннеры не зубы — отрастут

    Подробнее

    Комментарии 0

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

    Самое читаемое