iRobot Сreate #4 – управление движением (акселерометр, голосовое) с Android через ROS

ПРОДОЛЖЕНИЕ и ОКОНЧАНИЕ Создание проекта в ROS, написание subscriber для получения сообщений из узла rosserial и запуска сервисов irobot_create_2_1 — начало здесь далее здесь и еще здесь

Создаем новый проект vp_arduino1
cd ~/ros_pkgs
roscreate-pkg vp_arduino1 std_msgs rospy

Cоздадим файл сообщений Arduino1.msg. Для этого создадим в директории нашего пакета папку msg и в ней файл Arduino1.msg. Файл наполним следующим содержанием:
int16 var1
int16 var2
int16 var3
int16 var4
int16 var5

Установка системы зависимостей и сборка пакета
rosdep install vp_arduino1
rosmake vp_arduino1

Пакет собран.
Теперь пишем код для subscriber на python
Получая сообщения анализируем первый параметр и запускаем сервисы пакета irobot_create
Создаем файл node_arduino_1_1.py:
roscd vp_aruino1
mkdir nodes
cd nodes
touch  node_arduino_1_1.py

Содержимое файла
#!/usr/bin/env python
import roslib; roslib.load_manifest('vp_arduino1')
import rospy
import irobot_create_2_1

from vp_arduino1.msg import Arduino1
from irobot_create_2_1.srv import *

def callback(data):
    rospy.loginfo(rospy.get_name()+"Params %s %s %s %s %s"%(data.var1,data.var2,data.var3,data.var4,data.var5))
    if data.var1==139:
      rospy.wait_for_service('leds')
      try:
        srvLeds = rospy.ServiceProxy('leds', Leds)
        resp1 = srvLeds(data.var2,data.var3,data.var4,data.var5)
        print resp1.success
      except rospy.ServiceException, e:
        print "Service Led failed: %s"%e
    elif data.var1==136:
      rospy.wait_for_service('demo')
      try:
        srvDemo = rospy.ServiceProxy('demo', Demo)
        resp1 = srvDemo(data.var2)
        print resp1.success
      except rospy.ServiceException, e:
        print "Service Demo failed: %s"%e
    elif data.var1==128:
      rospy.wait_for_service('break')
      try:
        srvBreak = rospy.ServiceProxy('break', Break)
        resp1 = srvBreak(data.var2)
        print resp1.success
      except rospy.ServiceException, e:
        print "Service Break-start failed: %s"%e
    elif data.var1==145:
      rospy.wait_for_service('tank')
      try:
        srvTank = rospy.ServiceProxy('tank', Tank)
        resp1 = srvTank(1,data.var3,data.var4)
        print "%s %s %s"%(resp1.success,data.var3,data.var4)
      except rospy.ServiceException, e:
        print "Service Tank failed: %s"%e
    else:
        print "not Services"
def listener():
    rospy.init_node('listener', anonymous=True)
    rospy.Subscriber("Theme_arduino1", Arduino1, callback)
    rospy.spin()

if __name__ == '__main__':
    listener()


И теперь запускаем робота
Первый терминал
roscore

Второй терминал
rosrun rosserial_python serial_node.py /dev/ttyUSB0


Получаем список тем (топиков)
rostopic list

Список
/Theme_arduino1
/rosout
/rosout_agg


Выдача сообщений в топик Theme_arduino1
rostopic echo Theme_arduino1

при посылке из Android-устройства видим

var1: 139
var2: 8
var3: 153
var4: 170
var5: 180
---
var1: 145
var2: 0
var3: 100
var4: 0
var5: 0
---
var1: 145
var2: 0
var3: 50
var4: 255
var5: 156


Третий терминал — запуск драйвера irobot_create_2_1
rosparam set /brown/irobot_create_2_1/port /dev/ttyACM0
rosrun irobot_create_2_1 driver.py


Четвертый терминал — запуск subscriber
rosrun vp_arduino1 node_arduino_1_1.py

И получение сообщений при посылке из Android-устройства
Результат наблюдаем в терминале
[INFO] [WallTime: 1348674186.519380] /listener_7319_1348674183862Params 139 8 153 170 156
[INFO] [WallTime: 1348674194.157557] /listener_7319_1348674183862Params 145 0 0 0 0
[INFO] [WallTime: 1348674197.132703] /listener_7319_1348674183862Params 145 255 56 0 0
[INFO] [WallTime: 1348674200.168712] /listener_7319_1348674183862Params 145 254 162 0 0
[INFO] [WallTime: 1348674203.193468] /listener_7319_1348674183862Params 145 255 6 255 56
[INFO] [WallTime: 1348674206.212403] /listener_7319_1348674183862Params 145 254 212 254 212
[INFO] [WallTime: 1348674209.231324] /listener_7319_1348674183862Params 145 255 106 0 100
[INFO] [WallTime: 1348674212.261905] /listener_7319_1348674183862Params 145 0 200 0 0


И результат — видео

Комментарии (0)

RSS свернуть / развернуть

Только зарегистрированные и авторизованные пользователи могут оставлять комментарии.