Занятие № 3: Базовые пакеты ROS: rviz и tf

Очень часто, особенно при работе с большим количеством информации, отображения данных в текстовом виде бывает малоинформативным. Так, например, если граф представляется в виде матрицы смежности и имеет размеры 100 на 100, то по текстовому виду этой матрицы довольно сложно представить себе вид этого графа. В таких случаях гораздо удобнее использовать графическое представление данных. Конечно, как любой проект С++, проекты ROS могут использовать графические библиотеки, например, openGL. И, конечно, можно вручную описывать способы представления информации на экране. Но для этих нужд в ROS предусмотрен пакет rviZ, который представляет ноду графического представления (наприсанную с использованием openGL). Запуск rviz происходит по команде

rosrun rviz rviz

Окно разделено на три участка. Слева отображаются компоненты, которые могут быть нарисованы. Их довольно много.

Rviz позволяет отображать точки, линии, сетки, объёмные фигуры, направления изменений и многое другое. По центру располагается поле для рисования. Именно здесь появляются изображаемые объекты. Справа располагается настройка Current View. Подразумевается, что rviz будет использоваться для изображения того, что видит робот. А робот имеет свою точку наблюдения. Таким образом можно передавать, например, координаты объектов, помеченные так, как их видит робот, и это координаты будут пересчитаны в абсолютные - те, какими они являются в общем мире на общей карте.

Rviz - обычная нода, и, как и следовало ожидать, она подписывается на топики. Однако, если, ничего не изменяя, выполнить команду

rosrun rqt_graph rqt_graph

то будет видно, что rviz подписан только на топики /tf и /tf_static. Для того, чтоб rviz читал данные из других топиков, куда будет передаваться информация, в левой части окна rviz необходимо добавить отслеживаемые объекты. Ниже будет продемонстрировано, как отобразить точку.

Итак, отображение точки - это сообщение типа visualization_msgs::Marker. Для того, чтоб rviz был подписан на топик с сообщениями этого типа необходимо в левой части окна представленного на рисунке 6.1, нажав на кнопку Add, выбрать поле Marker. Теперь rqt_graph покажет, что rviz подписан ещё на два топика: /visualisation_marker и /visualisation_marker_array (имя может быть изменено). Теперь в этот топик можно отправлять сообщения. Чем использовать для этих целей команду

rostopic pub …

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

1. #include <ros/ros.h> 2. #include <visualization_msgs/Marker.h> 3. 4. int main(int argc, char **argv) { 5. ros::init(argc,argv,"point_publisher"); 6. ros::NodeHandle nh; 7. ros::Publisher pub = 8. nh.advertise<visualization_msgs::Marker>("pt_topic",10,true); 9. visualization_msgs::Marker point; 10. point.header.frame_id = "/point_on_map"; 11. point.header.stamp = ros::Time::now(); 12. point.ns = "there_is_point"; 13. point.action = visualization_msgs::Marker::ADD; 14. point.pose.orientation.w = 1; 15. point.id = 0; 16. point.type = visualization_msgs::Marker::POINTS; 17. point.scale.x = 0.5; 18. point.scale.y = 0.5; 19. point.color.r = 1.0; 20. point.color.g = 0.0; 21. point.color.b = 0.0; 22. point.color.a = 1.0; 23. geometry_msgs::Point p; 24. p.x = 10; 25. p.y = 10; 26. p.z = 5; 27. point.points.push_back(p); 28. pub.publish(point); 29. sleep(1); 30. return 0; 31. }

Разберёмся по порядку, что значат каждые строки в этом коде. В строке 1 подключается общий интерфейс ros. В строке 2 подключается интерфейс использования сообщений типа visualization_msgs::Marker. Как выше было указано, для изображения точки на экран, её необходимо передать для rviz-а именно в формате такого сообщения. Далее в строках 5-8 создаётся топик с именем /pt_topic. Это имя может быть произвольным и его нужно будет указать в rviz. затем на строках 9-27 заполняется сообщение visualization_msgs::Marker. В общем виде visualization_msgs::Marker - это множество точек, которое по-разному интерпретируется с помощью поля класса type. В строке 16 это поле устанавливается в visualization_msgs::Marker::POINTS. Что означает воспринимать все точки, сохранённые в сообщении visualization_msgs::Marker, как просто разрозненный набор точек. Сами точки (которая в примере всего одна) задаётся в строках 23-26 и помещается в сообщение на строке 27. В это сообщение могут быть добавлены ещё точки, и не обязательно создавать новое сообщение, чтобы вывести их на экран.

На строке 10 задаётся frame_id - имя кадра. Это такое имя кадра, которое воспроизводится на ноде rviz. Как было сказано ранее, rviz предоставляет два различных вида: вид общей карты и вид робота. Вот frame_id - это имя, присваемое одному из видов. При запуске rvis необходимо будет связать вид карты с этим именем.

На 11 строке создаётся метка сообщения, обычно эта метка выполняется в виде времени создания этого сообщения.

В 12 строке определяется namespace. Namespace и id (определяемое в 15 строке) однозначно определяют сообщение. Если из двух сообщений поступает информация об объекте с одним и тем же id одного и того же namespace-а, то применяется состояние объекта из последнего сообщения (причём не важно, получено первое сообщение 10 минут или секунду назад, и была ли уже отображена информация на экране или нет).

В 13 строке определяется, что будет сделано с фигурой с указанным namespace-ом и id. Выбран параметр ADD, однако, в случае, если такой объект уже будет существовать, параметр будет выполнять функцию UPDATE.

На 14 строке устанавливается ориентация общего набора точек, передаваемого генерируемым сообщением. Но, так как, всего здесь создаётся только одна точка - изменение этого параметра никак не отобразиться. По параметру point.pose.position с помощью полей х, y, z можно установить координаты (0;0;0) для общего набора, а также с помощью point.pose.orientation повернуть в пространстве.

На строках 17-18 устанавливается толщина точки в местных координатах.

На строках 19-22 устанавливается цвет точек, где а - альфа - параметр прозрачности (1,0 - совсем не прозрачный)

На строке 27 массив точек заполняется единственной точкой.

На строке 28 сгенерированное сообщение посылается в топик.

На строке 29 происходит небольшое ожидание для того, чтоб сообщение не разрушилось деструктором до его помещения в очередь.

В 16 строке можно указать также типы visualization_msgs::Marker::LINE_LIST или visualization_msgs::Marker::LINE_STRIP. LINE_LIST попарно соединяет точки, образуя массив отрезков, в то время как LINE_STRIP просто соединяет точки друг с другом последовательно.

Теперь, как было указано выше, требуется, запустив ноду rviz, указать ему, какой топик слушать и какой frame изменять. Имя frame можно указать в левой части в поле fixed_frame. Название топика же следует указывать в графе Marker. Если такой графы нет, следует добавить с помощью кнопки Add. Имя топика указывается в графе Marker в поле “Marker Topic”. В описанном случае в поле Marker Topic следует указать pt_topic (из строки 8), а в поле “Fixed Frame” указать point_on_map (из строки 10).

Конечно, за одно сообщение можно посылать сразу несколько точек, и не обязательно делать много сообщений такого вида. Однако в этом случае, например, нельзя будет установить цвет каждой точки по отдельности. Для того, чтоб добавить точки для вывода нужно создать больше точек в строках 23-27 и поместить их в вектор .points. А также можно создавать и передавать множество сообщений, а не только одно. Для этого строки 9-27 следует поместить в цикл. Например, учачток кода, который выводит на экран совершающую колебания синусоиду, представлен на листинге ниже.

ros::Rate r(30); double offset = 0; while(ros::ok()){ visualization_msgs::Marker msg; msg.header.frame_id = "/point_on_map"; msg.header.stamp = ros::Time::now(); msg.ns = "there_is_point"; msg.action = visualization_msgs::Marker::ADD; msg.pose.position.x = 5; msg.pose.position.z = -100; msg.pose.orientation.x = 100; msg.pose.orientation.z = 100; msg.id = 0; msg.type = visualization_msgs::Marker::POINTS; msg.scale.x = 0.5; msg.scale.y = 0.5; msg.color.r = 1.0; msg.color.g = 0.0; msg.color.b = 0.0; msg.color.a = 1.0; for (int x = -20; x <= 20; x++){ geometry_msgs::Point p; p.x = x; p.y = 2*sin(x+offset); p.z = 2*cos(x+offset); msg.points.push_back(p); } pub.publish(msg); offset+=0.4; r.sleep(); }