Задание
Робот потерялся и не может найти точку выхода. В эфир он посылает сообщения о собственныых координатах. Ему на помощь из точки выхода выдвинулся второй робот. Второй робот должен добраться до первого. Как только их координаты совпадут - второй робот начинает следовать к выходу - точке, откуда он начал своё движение. Первый робот начинает движение за вторым. Процесс заканчивается, когда оба робота достигают точки выхода.
(Усложнение) Потерявшийся робот мечется в пространстве, пока до него добирается второй робот.
Указание
В rviz можно писать различные типы сообщений, которые уже описаны за нас (не надо каждый раз строить велосипед из маркеров-точек). Полный список и описание поддерживаемых сообщений можно найти на вики.
Так, например, сферу можно создать так:
visualization_msgs::Marker marker; marker.type = visualization_msgs::Marker::SPHERE;
Если в тот же топик послать маркер с тем же id, но с другими координатами, то rviz сотрёт старый маркер и нарисует новый в новых координатах.
В целом структуру программы можно описать так: в tf попадают и координаты потерявшегося робота и робота-помощника. По координатам потерявшегося робота помощник определяет направление движения и движется вдоль этого направления. Как только помощник достигает своей цели, происходит два события: помощник начинает движение к выходу, а потерявшийся робот едет вслед за помощником. (Понятно, что помощник всё ещё должен посылать в эфир собственные координаты, чтобы потерявшийся робот знал, куда ехать)