Управление роботом симулятором turtlesim

Только начиная проходить Tutorial по ROS, управляя черепахой из модуля turtlesim, пришла идея подписаться на топик turtle1/command_velocity, который транслирует команды черепахе и параллельно с черепахой управлять роботом. Дочитав весь туториал и изучив основы Python взялся за дело. На время обучения ROS и его возможностям, решил roscore запускать пока на компьютере и работать с Arduino через bluetooth напрямую, без участия Android-телефона.

В директории src в рабочем месте ~/catkin_ws/ (созданном ещё во время прохождения туториала), создаем проект для нашего робота Катюши:

$ cd ~/catkin_ws/src
$ catkin_create_pkg katusha std_msgs rospy roscpp
$ cd katusha
# Будем писать на Phyton, поэтому сделаем для него отдельную директорию:
$ mkdir source

Задача модуля на Phyton - связать ROS и наш Arduino скетч. Я попробовал использовать решения ROS для отправки сообщений между ROS и Arduino, но существующее решение оказалось очень не стабильно, а поддержка хромает т.к. разработчиков этой части мало. Поэтому будем использовать существующую и уже хорошо зарекомендовавшую себя систему команд нашего робота и просто напишем ROS прослойку, которая будет читать топики ROS и отправлять команды в Arduino. Сам модуль, я назвал katusha_turtlesim.py:

#!/usr/bin/env python
#coding=utf8
from turtlesim.msg import Velocity
from time import sleep
import serial
import rospy

def dec2hex(n):
    """return the hexademical string in XXXX format"""
    return ("%0.4X" % (n>=0 and n or n+256**2))[-4:]
# the mystery here n>=0 actually goes to n+256*2 and returns 10000 in hex. but it's cuted to 0000 by [-4:0]

def move(speed=255):
    """move foreward, or backward if speed<0"""
    ser.write('G'+dec2hex(speed))

def rotate(speed=255):
    """rotate clockwise if speed<0, or anti-clockwise if speed>0"""
    ser.write('R'+dec2hex(speed)+'L'+dec2hex(-speed))

def turtlesim_cmd(msg):
    """callback for 'turtle1/command_velocity' messages from turtlesim"""
    # Logs
    #rospy.loginfo(rospy.get_name() + ": I heard %s and %s" % (msg.linear, msg.angular))
    # Determine the direction to move
    if msg.linear+msg.angular>0:
        speed=255
    else:
        speed=-255
    # Go straight or rotate
    if msg.linear!=0:
        move(speed)
        sleep(abs(msg.linear/100))
    else:
        rotate(speed)
        sleep(abs(msg.angular/100))
    # Stop
    move(0)

def listener():
    rospy.init_node('katusha', anonymous=True)
    # Listen for messages from turtlesim
    rospy.Subscriber('turtle1/command_velocity', Velocity, turtlesim_cmd)

    while not rospy.is_shutdown():
        # read and print data from serial port (bluetooth)
        data = ser.read(9999)
        if len(data)>0:
            print data

if __name__ == '__main__':
    # connect to bluetooth. Make sure you have  connected robot with 
    # rfcomm connect 0 00:13:02:01:70:17 1 
    ser = serial.Serial('/dev/rfcomm0', 9600, timeout=0)
    listener()
    # stop before exit
    move(0)

Обязательно делаем модуль исполняемым

$ chmod +x katusha_turtlesim.py

Запускаем каждую программу в новом терминале:

  1. $ roscore   # Ядро ROS
  2. $ rosrun turtlesim turtle_teleop_key  # Терминал с которого будем управлять
  3. $ rosrun turtlesim turtlesim_node   # Необязательно, но если захочется - черепашка будет на экране дублировать действия робота
  4. $ rfcomm connect 0 00:13:02:01:70:17 1   # Включаем bluetooth соединение - здесь 00:13:02:01:70:17 - MAC адрес bluetooth робота.
  5. $ rosrun katusha katusha_turtlesim.py  # Наш модуль, хотя у меня работает и просто запуск из директории с исходником: ./katusha_turtlesim.py

Все, можно переключиться на окно с turtle_teleop_key и управлять Катюшей! :)

Чтобы посмотреть как работают сообщения в ROS и проверить, подписаны ли мы на правильный топик, можем воспользоваться rqt_graph:

$ rosrun rqt_graph rqt_graph

Топики ROS для нашего робота

Комментирование и размещение ссылок запрещено.

Комментарии закрыты.