Задание целей

Ранее мы задавали цели используя инструмент rviz. В этой главе мы рассмотрим каким образом задавать цели робота через скрипты используя механизмы ROS . Это позволит нам написать собственные программы для автономного управления роботом.

Командные топики move_base

Для управления целями навигации существует несколько топиков

• move_base/goal (move_base_msgs/MoveBaseActionGoal)
• move_base/cancel (actionlib_msgs/GoalID)
• move_base/feedback (move_base_msgs/MoveBaseActionFeedback)
• move_base/status (actionlib_msgs/GoalStatusArray)
• move_base/result (move_base_msgs/MoveBaseActionResult)

Топик move_base/goal обрабатывает сообщения, в которых мы сообщаем роботу о координатах цели, в которую робот должен переместиться.

Например, если мы через rviz зададим точку для перемещения робота и сделаем rostopic echo /move_base/goal, то увидим в нем сообщение с координатами цели.

rostopic echo /move_base/goal

header:
  seq: 0
  stamp:
    secs: 1559832317
    nsecs: 767230168
  frame_id: ''
goal_id:
  stamp:
    secs: 0
    nsecs:         0
  id: ''
goal:
  target_pose:
    header:
      seq: 0
      stamp:
        secs: 1559832317
        nsecs: 766657039
      frame_id: "map"
    pose:
      position:
        x: 0.390243262053
        y: 0.0208128485829
        z: 0.0
      orientation:
        x: 0.0
        y: 0.0
        z: -6.04692047546e-08
        w: 1.0
---

Если мы опубликуем топик, указав нулевые значения position то робот вернется в положение из которого начинал свое движение.

rostopic pub /move_base/goal move_base_msgs/MoveBaseActionGoal "header:
  seq: 0
  stamp:
    secs: 0
    nsecs: 0
  frame_id: ''
goal_id:
  stamp:
    secs: 0
    nsecs: 0
  id: ''
goal:
  target_pose:
    header:
      seq: 0
      stamp:
        secs: 0
        nsecs: 0
      frame_id: 'map'
    pose:
      position:
        x: 0.0
        y: 0.0
        z: 0.0
      orientation:
        x: 0.0
        y: 0.0
        z: 0.0
        w: 1.0"

При заполнении пустой структуры сообщения, необходимо указать frame_id: 'map' для указания фрейма. И правильно заполнить нулевой кватернион начального положения orientation.w: 1 (Ориентация указывается в кватернионах)

В процессе движения робота, нода move_base информирует в топике /move_base/feedback о текущем положении робота (процессе перемещения робота).

rostopic echo /move_base/feedback

header:
  seq: 277
  stamp:
    secs: 1559833600
    nsecs: 287525540
  frame_id: ''
status:
  goal_id:
    stamp:
      secs: 1559833590
      nsecs: 887314124
    id: "/move_base-6-1559833590.887314124"
  status: 1
  text: "This goal has been accepted by the simple action server"
feedback:
  base_position:
    header:
      seq: 0
      stamp:
        secs: 1559833600
        nsecs: 254664897
      frame_id: "map"
    pose:
      position:
        x: 0.458761239869
        y: 0.101565367688
        z: 0.0
      orientation:
        x: 0.0
        y: 0.0
        z: 0.10795690539
        w: 0.994155574635
---

В топике /move_base/status выводится статус работы навигации. Например при движении робота, мы увидим

header:
  seq: 9106
  stamp:
    secs: 1559834114
    nsecs: 257677935
  frame_id: ''
status_list:
  -
    goal_id:
      stamp:
        secs: 1559834112
        nsecs:  75201222
      id: "/move_base-7-1559834112.75201222"
    status: 1
    text: "This goal has been accepted by the simple action server"

А при достижении цели мы увидим сообщение text: "Goal reached."

header:
  seq: 9131
  stamp:
    secs: 1559834119
    nsecs:  57347347
  frame_id: ''
status_list:
  -
    goal_id:
      stamp:
        secs: 1559834112
        nsecs:  75201222
      id: "/move_base-7-1559834112.75201222"
    status: 3
    text: "Goal reached."

Для остановки робота при выполнении задачи, можно опубликовать в топик /move_base/cancel пустое сообщение, или указать в сообщении конкретный id задачи.

Работа с этими всеми топиками возможна как через консоль при помощи утилиты rostopic, так и обычным способом через библиотеку python Rospy созданием Подписчиков и Издателей.

Помимо публикации данных в топик, возможно использовать (Action). Тогда необходимо запустить действие move_base/goal (move_base_msgs/MoveBaseActionGoal) Использование модели Action позволит получать ряд дополнительных возможностей, например отмену задания, получение статуса выполнения задания и т.п. Пример такого скрипта, который отправляет робота на координаты (x,y) с сохранением первоначальной ориентации:

import rospy
import actionlib
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
rospy.init_node("my_node")

client = actionlib.SimpleActionClient('move_base',MoveBaseAction)
client.wait_for_server()

goal = MoveBaseGoal()
goal.target_pose.header.frame_id = "map"
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.pose.position.x = x # Координаты в системе отсчета карты
goal.target_pose.pose.position.y = y # (0,0) - точка включения навигации
goal.target_pose.pose.orientation.w = 1.0

client.send_goal(goal)
client.wait_for_result()