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

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

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

libpcll-all-dev ... libpcl-all  

и в CMakeLists.txt:

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

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

#include "ros/ros.h" #include "pcl_conversions/pcl_conversions.h" #include  #include  #include  #include  #include  #include   typedef pcl::PointXYZ PointType; typedef pcl::PointCloud 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 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("/output", 1);   cmd_vel_pub_ = n.advertise("/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 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 

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

---                                                                                                                                                                                  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.

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