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

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

Перед участниками была поставлена задача реализовать безопасное управление роботом с остановкой перед препятствиями. Цель задачи — контроль скорости движения робота по направлению вперед. Робот получает данные от сенсора глубины (в нашем случае 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 как на картинке:

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

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

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

Запустим rviz:

rosrun rviz rviz

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

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

Запустим наш узел 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 мы увидим полную остановку робота:

В выводе для узла 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:

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

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

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

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

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

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

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

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

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

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

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