基于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))
基于ROS和python，通过TCP通信协议，完成键盘无线控制移动机器人运动的更多相关文章
- 基于Arduino和python的串口通信和上位机控制
引言 经常的时候我们要实现两个代码之间的通信,比如说两个不同不同人写的代码要对接,例如将python指令控制Arduino控件的开关,此处使用串口通信是非常方便的,下面笔者将结合自己踩过的坑来讲述下自 ...
- Python网络编程基础 ❷ 基于upd的socket服务 TCP黏包现象
TCP的长连接 基于upd的socket服务 TCP黏包现象
- (29)网络编程之TCP通信协议
TCP通信协议特点: 1.tcp协议是基于IO流进行数据的传输,是面向链接的. 2.tcp进行数据传输的时候,数据没有大小限制的. 3.面向链接,通过三次握手的机制,保证数据的完整性,是一个可靠的协议 ...
- 基于ROS的分布式机器人远程控制平台
基于ROS的分布式机器人远程控制平台 1 结构说明 HiBot架构主要使用C/S架构,其中HibotServer为服务器,Muqutte为消息服务器中间件,HiBotClient为运行在机器人上的 ...
- Modbus RTU通信协议详解以及与Modbus TCP通信协议之间的区别和联系
Modbus通信协议由Modicon公司(现已经为施耐德公司并购,成为其旗下的子品牌)于1979年发明的,是全球最早用于工业现场的总线规约.由于其免费公开发行,使用该协议的厂家无需缴纳任何费用,Mod ...
- ROS系统python代码测试之rostest
ROS系统中提供了测试框架,可以实现python/c++代码的单元测试,python和C++通过不同的方式实现, 之后的两篇文档分别详细介绍各自的实现步骤,以及测试结果和覆盖率的获取. ROS系统中p ...
- python之tcp自动重连
操作系统: CentOS 6.9_x64 python语言版本: 2.7.13 问题描述 现有一个tcp客户端程序,需定期从服务器取数据,但由于种种原因(网络不稳定等)需要自动重连. 测试服务器示例代 ...
- 基于SQL和PYTHON的数据库数据查询select语句
#xiaodeng#python3#基于SQL和PYTHON的数据库数据查询语句import pymysql #1.基本用法cur.execute("select * from biao&q ...
- DIY一个基于树莓派和Python的无人机视觉跟踪系统
DIY一个基于树莓派和Python的无人机视觉跟踪系统 无人机通过图传将航拍到的图像存储并实时传送回地面站差点儿已经是标配.假设想来点高级的--在无人机上直接处理拍摄的图像并实现自己主动控制要怎么实现 ...
随机推荐
- Laravel Homestead 离线安装
一.写在之前,网络不够快想要安装Homestead,也是一个浩大的工程,对于下载一个 1.22G左右的 laravel/homestead box 也是非常的麻烦.那么如何才能离线安装呢? 接着往下看 ...
- MySQL命令学习
上面两篇博客讲了MySQL的安装.登录,密码重置,为接下来的MySQL命令学习做好了准备,现在开启MySQL命令学习之旅吧. 首先打开CMD,输入命令:mysql -u root -p 登录MySQ ...
- ELK日志收集
目前日志的痛点 运维要经常登陆到服务器上拿日志给开发.测试 每次都是出问题后才去看日志,不能提前通过日志预判问题 如果是集群服务,日志将要从多台机器取 开发人员搞出来的日志不规范,没有标准.日志目录不 ...
- iOS - 提示用户升级版本并跳转到AppStore
一.问题:自己做提示用户升级? 由于苹果做了自动升级,所有只要在应用程序中出现从AppStore检查版本更新,或者出现任何有关升级的提醒都会被拒,但是如果必须添加升级提示的话,可以配合后台通过添加AP ...
- centos 7 rpm方式安装mysql
一.下载rpm 二.安装 1.用rz上传到centos上,目录为/home/upload 2.解压 tar -xvf mysql-5.7.25-1.el7.x86_64.rpm-bundle.tar ...
- 浅析 <路印协议--Loopring> 及整体分析 Relay 源码
作者:林冠宏 / 指尖下的幽灵 前序: 路印协议功能非常之多及强大,本文只做入门级别的分析. 理论部分请细看其白皮书,https://github.com/Loopring/whitepaper 实际 ...
- 搭建Keil C51开发环境
安装Keil 首先需要获取Keil的安装程序,然后双击开始安装.安装过程非常简单,基本都是一路next.具体步骤如下图: 注册 如果不注册,会有许多功能限制,例如限制生成的代码的大小.为了学习和和教学 ...
- axios 使用post方式传递参数，后端接受不到
参考 https://segmentfault.com/a/1190000012635783
- jQuery 中的 39 个技巧【申明：来源于网络】
jQuery 中的 39 个技巧[申明:来源于网络] 地址:http://blog.csdn.net/zhongqi2513/article/details/53704812?ref=myread
- I2C写时序图[转]
1. I2C写时序图: 注意:最后一个byte后,结束标志在第十个CLK上升沿之后: 2. I2C读时序图: 注意:restart信号格式:读操作结束前最后一组clk的最后一个上升沿,主机应发送NAC ...