Добрый день, уважаемые хабрачитатели! В прошлую пятницу в нашей лаборатории проходил практический мастеркласс по платформе 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:
#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
Скорость робота будет такой:
---
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.
Желаю удачи в экспериментах и до новых встреч!
Автор: vovaekb90
