python 基于标准输入输出的通讯协议_基于ROS和python，通过TCP通信协议，完成键盘无线控制移动机器人运动...

一、所需工具包

1.ROS键盘包：teleop_twist_keyboard

2.TCP通讯包：socket

$ cd ~/catkin_ws/src

$ git clone https://github.com/Forrest-Z/teleop_twist_keyboard.git

$ catkin_make

3.在ubuntu的ros中建立一个ros_car_py包:

$ cd ~/catkin_ws/src

$ catkin_create_pkg ros_car_py roscpp rospy std_msgs

4.新建 base 文件:

$ cd catkin_ws/src/base

$ mkdir src

$ vim src/base.py

代码如下(ROS作为TCP服务器):

#!/usr/bin/env python

# coding=utf-8

import rospy

from socket import *

import time

from threading import Thread

from std_msgs.msg import String

from geometry_msgs.msg import Twist

msg_list = []

def callback(cmd_input, Socket):

print("-----服务器已经启动成功，准备接收数据-----")

Socket.settimeout(5)

recvdata = Socket.recv(4096)

t = Twist()

t.angular.z = cmd_input.angular.z

t.linear.x = cmd_input.linear.x

left_speed = t.linear.x - t.angular.z * 0.5 * 0.2

right_speed = t.linear.x + t.angular.z * 0.5 * 0.2

left_speed *= 1000

right_speed *= 1000

left_speed = str(left_speed)

right_speed = str(right_speed)

# msg_list.append(left_speed)

# msg_list.append(right_speed)

print("left_speed=%s" % left_speed)

print("right_speed=%s" % right_speed)

if len(recvdata) != 0:

print("-----接收到数据-----")

print("recvdata:%s" % recvdata)

# Socket.send(b"hello beaglebone")

# Socket.send(b"左轮速度")

Socket.send(left_speed.encode("utf-8"))

# Socket.send(b"右轮速度")

Socket.send(right_speed.encode("utf-8"))

# Socket.send(msg_list)

else:

print('-----未接收到客户端数据，可能连接已经断开-----')

# Socket.send(b'client off')

# 数据中断时进行服务重启程序，先close 再accept等待重新连线

# 可以防止出现当client意外终止导致server的中断（Broken pipe错误）

print('-----正在重新建立连接-----')

Socket.close()

Socket, clientInfo = serverSocket.accept()

# serverSocket.close()

def main():

rospy.init_node("base")

serverSocket = socket(AF_INET, SOCK_STREAM)

serverSocket.setsockopt(SOL_SOCKET, SO_REUSEADDR, 1)

serverSocket.bind(('', 8899))

serverSocket.listen(5)

print("-----服务器正在启动-----")

Socket, clientInfo = serverSocket.accept()

sub = rospy.Subscriber("cmd_vel", Twist, callback, Socket)

rate = rospy.Rate(10)

rospy.spin()

if __name__ == "__main__":

main()

注意事项:

ROS中的python是python2,使用python3会出错，所以需要在开头加上#!/usr/bin/env python

编写好python程序后，编译成功但是无法运行，报错Couldn't find executable named XXX.py，无法执行

问题原因:

文件没有执行权限

解决办法:

给文件添加执行权限

命令：chmod +x base.py

修改CMakeList.txt:

cmake_minimum_required(VERSION 2.8.3)

project(ros_car_py)

find_package(catkin REQUIRED COMPONENTS

roscpp

rospy

std_msgs

message_generation

)

add_message_files(

FILES

MsgCar.msg

)

generate_messages(

DEPENDENCIES

std_msgs

)

catkin_package(

# INCLUDE_DIRS include

# LIBRARIES ros_car_py

CATKIN_DEPENDS message_runtime

#roscpp rospy std_msgs

# DEPENDS system_lib

)

include_directories(

# include

${catkin_INCLUDE_DIRS}

)

单独编译ros_car_pkg包:

$ catkin_make -DCATKIN_WHITELIST_PACKAGES='ros_car_py'

二、控制原理：

当我们按下键盘时，teleop_twist_keyboard 包会发布 /cmd_vel 发布速度主题

在 base 节点订阅这个话题，接收速度数据，转换成字符串(TCP只允许发送字符串)，然后发送至客户端

设置beaglebone作为客户端：

运行代码加载设备树并读串口数据用于控制PWM，进而控制小车运动

from socket import *

import time

SLOTS = "/sys/devices/bone_capemgr.9/slots"

p1_duty = "/sys/devices/ocp.3/pwm_test_P9_16.16/duty"

p2_duty = "/sys/devices/ocp.3/pwm_test_P8_13.15/duty"

p1_period = "/sys/devices/ocp.3/pwm_test_P9_16.16/period"

p2_period = "/sys/devices/ocp.3/pwm_test_P8_13.15/period"

p1_run = "/sys/devices/ocp.3/pwm_test_P9_16.16/run"

p2_run = "/sys/devices/ocp.3/pwm_test_P8_13.15/run"

p1_export = "/sys/class/gpio/export"

p2_export = "/sys/class/gpio/export"

p1_direction = "/sys/class/gpio/gpio44/direction"

p2_direction = "/sys/class/gpio/gpio45/direction"

p1_polarity = "/sys/class/gpio/gpio44/value"

p2_polarity = "/sys/class/gpio/gpio45/value"

msg_lists = []

clientSocket = socket(AF_INET, SOCK_STREAM)

clientSocket.connect(("192.168.1.151", 8899))

try:

with open(SLOTS, "a") as f:

f.write("am33xx_pwm")

with open(SLOTS, "a") as f:

f.write("bone_pwm_P9_22")

with open(SLOTS, "a") as f:

f.write("bone_pwm_P9_16")

with open(p1_export, "a") as f:

f.write("44")

with open(p2_export, "a") as f:

f.write("45")

with open(p1_direction, "a") as f:

f.write("in")

with open(p2_direction, "a") as f:

f.write("in")

except:

pass

while True:

clientSocket.send(b"hello ROS")

msg_list1 = clientSocket.recv(1024)

msg_list2 = clientSocket.recv(1024)

if len(msg_list1) or len(msg_list2) > 0:

msg_lists.append(msg_list1)

msg_lists.append(msg_list2)

for msg in msg_lists:

print("recvData:%s" % msg)

if msg_lists[0] == '500.0' and msg_lists[1] == '500.0':

msg_lists = []

print("succes")

try:

with open(p1_period, "a") as f:

f.write("500000")

with open(p1_duty, "a") as f:

f.write("250000")

with open(p2_period, "a") as f:

f.write("500000")

with open(p2_duty, "a") as f:

f.write("250000")

with open(p1_run, "a") as f:

f.write("1")

with open(p2_run, "a") as f:

f.write("1")

with open(p1_polarity, "a") as f:

f.write("1")

with open(p2_polarity, "a") as f:

f.write("1")

except:

pass

elif msg_lists[0] == '400.0' and msg_lists[1] == '600.0':

msg_lists = []

try:

with open(p1_period, "a") as f:

f.write("400000")

with open(p1_duty, "a") as f:

f.write("200000")

with open(p2_period, "a") as f:

f.write("600000")

with open(p2_duty, "a") as f:

f.write("300000")

with open(p1_run, "a") as f:

f.write("1")

with open(p2_run, "a") as f:

f.write("1")

with open(p1_polarity, "a") as f:

f.write("1")

with open(p2_polarity, "a") as f:

f.write("1")

except:

pass

elif msg_lists[0] == '600.0' and msg_lists[1] == '400.0':

msg_lists = []

try:

with open(p1_period, "a") as f:

f.write("600000")

with open(p1_duty, "a") as f:

f.write("300000")

with open(p2_period, "a") as f:

f.write("400000")

with open(p2_duty, "a") as f:

f.write("200000")

with open(p1_run, "a") as f:

f.write("1")

with open(p2_run, "a") as f:

f.write("1")

with open(p1_polarity, "a") as f:

f.write("1")

with open(p2_polarity, "a") as f:

f.write("1")

except:

pass

elif msg_lists[0] == '-500.0' and msg_lists[1] == '-500.0':

msg_lists = []

try:

with open(p1_period, "a") as f:

f.write("500000")

with open(p1_duty, "a") as f:

f.write("250000")

with open(p2_period, "a") as f:

f.write("500000")

with open(p2_duty, "a") as f:

f.write("250000")

with open(p1_run, "a") as f:

f.write("1")

with open(p2_run, "a") as f:

f.write("1")

with open(p1_polarity, "a") as f:

f.write("0")

with open(p2_polarity, "a") as f:

f.write("0")

except:

pass

elif msg_lists[0] == '-600.0' and msg_lists[1] == '-400.0':

msg_lists = []

try:

with open(p1_period, "a") as f:

f.write("600000")

with open(p1_duty, "a") as f:

f.write("300000")

with open(p2_period, "a") as f:

f.write("400000")

with open(p2_duty, "a") as f:

f.write("200000")

with open(p1_run, "a") as f:

f.write("1")

with open(p2_run, "a") as f:

f.write("1")

with open(p1_polarity, "a") as f:

f.write("0")

with open(p2_polarity, "a") as f:

f.write("0")

except:

pass

elif msg_lists[0] == '-400.0' and msg_lists[1] == '-600.0':

msg_lists = []

try:

with open(p1_period, "a") as f:

f.write("400000")

with open(p1_duty, "a") as f:

f.write("200000")

with open(p2_period, "a") as f:

f.write("600000")

with open(p2_duty, "a") as f:

f.write("300000")

with open(p1_run, "a") as f:

f.write("1")

with open(p2_run, "a") as f:

f.write("1")

with open(p1_polarity, "a") as f:

f.write("0")

with open(p2_polarity, "a") as f:

f.write("0")

except:

pass

elif msg_lists[0] == '0.0' and msg_lists[1] == '0.0':

msg_lists = []

try:

with open(p1_run, "a") as f:

f.write("0")

with open(p2_run, "a") as f:

f.write("0")

except:

pass

else:

pass

else:

time.sleep(0.1)

clientSocket = socket(AF_INET, SOCK_STREAM)

clientSocket.connect(("192.168.1.138", 8899))