Только начиная проходить 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
Запускаем каждую программу в новом терминале:
- $ roscore # Ядро ROS
- $ rosrun turtlesim turtle_teleop_key # Терминал с которого будем управлять
- $ rosrun turtlesim turtlesim_node # Необязательно, но если захочется - черепашка будет на экране дублировать действия робота
- $ rfcomm connect 0 00:13:02:01:70:17 1 # Включаем bluetooth соединение - здесь 00:13:02:01:70:17 - MAC адрес bluetooth робота.
- $ rosrun katusha katusha_turtlesim.py # Наш модуль, хотя у меня работает и просто запуск из директории с исходником: ./katusha_turtlesim.py
Все, можно переключиться на окно с turtle_teleop_key и управлять Катюшей!
Чтобы посмотреть как работают сообщения в ROS и проверить, подписаны ли мы на правильный топик, можем воспользоваться rqt_graph:
$ rosrun rqt_graph rqt_graph