ROS多个master消息互通
需求
有时候我们需要有几个不同的master, 他们之间要交换topic的内容,这时候就不能使用ros自带的设置同一个master的方法.
我们的处理方法是,构造一个client和一个server,他们运行在不同的master下面, client在master1下订阅topic1,然后通过tcp协议(自己定义一个消息协议格式)发到master2下面的server,进行消息解析,再发布出master2下面的topic1,这样我们不改变ros自带的topic框架,就实现topic1的消息从master1到master2的传播.
下面我们实现的是将一个amcl_pose
的topic,消息类型是PoseWithCovarianceStamped
从master1传到master2,其他的topic代码类似
client 的代码
#! /usr/bin/env python
# -*- coding=utf-8 -*-
import socket
import struct
import rospy
import time
from geometry_msgs.msg import PoseWithCovarianceStamped,PoseStamped
#message proto
# id | length | data
def send_msg(sock, msg ,id):
# Prefix each message with a 4-byte id and length (network byte order)
msg = struct.pack('>I',int(id)) + struct.pack('>I', len(msg)) + msg
sock.sendall(msg)
def odomCallback(msg):
global odom_socket
data = ""
id = msg.header.seq
print "send id: ",id
x = msg.pose.pose.position.x
y = msg.pose.pose.position.y
#orientation
orien_z = msg.pose.pose.orientation.z
orien_w = msg.pose.pose.orientation.w
data += str(x) + "," + str(y)+ "," + str(orien_z)+ "," + str(orien_w)
send_msg(odom_socket,data,id)
odom_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
odom_socket.connect(('127.0.0.1',8000))
rospy.init_node('server_node')
rospy.Subscriber('/amcl_pose',PoseWithCovarianceStamped,odomCallback)
rospy.spin()
server 的代码
#! /usr/bin/env python
# -*- coding=utf-8 -*-
import socket
import time,os,fcntl
import struct
import rospy
from geometry_msgs.msg import PoseWithCovarianceStamped,PoseStamped
#message proto
# id | length | data
def recv_msg(sock):
try:
# Read message length and unpack it into an integer
raw_id = recvall(sock, 4)
if not raw_id:
return None
id = struct.unpack('>I', raw_id)[0]
print "receive id: ",id
raw_msglen = recvall(sock, 4)
if not raw_msglen:
return None
msglen = struct.unpack('>I', raw_msglen)[0]
# Read the message data
return recvall(sock, msglen)
except Exception ,e:
return None
def recvall(sock, n):
# Helper function to recv n bytes or return None if EOF is hit
data = ''
while len(data) < n:
packet = sock.recv(n - len(data))
if not packet:
return None
data += packet
return data
##把接受的数据重新打包成ros topic发出去
def msg_construct(data):
list = data.split(',')
pose = PoseWithCovarianceStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = "/map"
pose.pose.pose.position.x = float(list[0])
pose.pose.pose.position.y = float(list[1])
pose.pose.pose.position.z = 0
pose.pose.pose.orientation.x = 0
pose.pose.pose.orientation.y = 0
pose.pose.pose.orientation.z = float(list[2])
pose.pose.pose.orientation.w = float(list[3])
pose.pose.covariance = [0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.06853891945200942]
return pose
#初始化socket,监听8000端口
odom_socket = socket.socket(socket.AF_INET,socket.SOCK_STREAM)
odom_socket.bind(('',8000))
odom_socket.listen(10)
(client,address) = odom_socket.accept()
rospy.init_node("client_node")
odom_pub = rospy.Publisher("/amcl_pose",PoseWithCovarianceStamped,queue_size=30)
r = rospy.Rate(1000)
#设置noblock,否则会阻塞在接听,下面while不会一直循环,只有在有数据才进行下一次循环
fcntl.fcntl(client, fcntl.F_SETFL, os.O_NONBLOCK)
while not rospy.is_shutdown():
data = recv_msg(client)
if data:
odom_pub.publish(msg_construct(data))
r.sleep()
结论
上面的代码在局域网内测试过,发送图像,激光的数据都可以保证不丢数据。
ROS多个master消息互通的更多相关文章
- ROS中发布激光扫描消息
激光雷达工作时会先在当前位置发出激光并接收反射光束,解析得到距离信息,而后激光发射器会转过一个角度分辨率对应的角度再次重复这个过程.限于物理及机械方面的限制,激光雷达通常会有一部分“盲区”.使用激光雷 ...
- 使用ROS节点——Node & Master——roscore、rosrun、rosnode
1.Node 在ROS的世界里, 最小的进程单元就是节点( node) . 一个软件包里可以有多个可执行文件, 可执行文件在运行之后就成了一个进程(process), 这个进程在ROS中就叫做节点. ...
- Matlab 读取 ROS bag 文件指定消息数据
近期在接触Ros的时候遇到了一些问题,如何将rosbag中的信息提取出来进行进一步处理呢? 如三维点位置信息,视频信息(如果有的话)等等. 我采用的是MATLAB 读取bag信息 filepath=f ...
- ROS Twist和Odometry消息类型使用(Python)
消息类型: 1. Twist - 线速度角速度 通常被用于发送到/cmd_vel话题,被base controller节点监听,控制机器人运动 geometry_msgs/Twist geometry ...
- ROS学习(十二)—— 编写简单的消息发布器和订阅器(C++)
一.创建发布器节点 1 节点功能: 不断的在ROS网络中广播消息 2 创建节点 (1)打开工作空间目录 cd ~/catkin_ws/src/beginner_tutorials 创建一个发布器节点( ...
- ROS 消息发布器和订阅器Publisher, Subscriber
博客参考:https://www.2cto.com/kf/201705/639776.html 1.编写发布器节点节点(Node) 是指 ROS 网络中可执行文件.接下来,将会创建一个发布器节点(“t ...
- SLAM+语音机器人DIY系列:(二)ROS入门——5.编写简单的消息发布器和订阅器
摘要 ROS机器人操作系统在机器人应用领域很流行,依托代码开源和模块间协作等特性,给机器人开发者带来了很大的方便.我们的机器人“miiboo”中的大部分程序也采用ROS进行开发,所以本文就重点对ROS ...
- Ros学习——创建ROS消息和ROS服务
1.rosed rosed 是 rosbash 的一部分.利用它可以直接通过package名来获取到待编辑的文件而无需指定该文件的存储路径了. rosed默认的编辑器是vim.修改其他的,打开~/.b ...
- ROS Learning-027 (提高篇-005 A Mobile Base-03) 控制移动平台 --- Twist 消息
ROS 提高篇 之 A Mobile Base-03 - 控制移动平台 - Twist 消息 我使用的虚拟机软件:VMware Workstation 11 使用的Ubuntu系统:Ubuntu 14 ...
随机推荐
- Spring定时任务配置
可参照http://gong1208.iteye.com/blog/1773177 的详细配置 <!-- 系统定时任务工厂类 --> <bean id="systemDes ...
- Intel HD4000的驱动-黑苹果之路
10.9.1装好后显卡HD4000能自动识别,但是识别的内存始终只有64M,使用了删.改AppleIntel*.kext驱动.修改clover的graphic的ig-platform-id.修改DSD ...
- Open XML SDK 在线编程黑客松
2015年2月10日-3月20日,开源社 成员 微软开放技术,GitCafe,极客学院联合举办" Open XML SDK 在线编程黑客松 ",为专注于开发提高生产力的应用及服务的 ...
- Android API 中文 ListView
正文 一.结构 public class RatingBar extends AbsSeekBar java.lang.Object android.view.View android.view.Vi ...
- 翻译「C++ Rvalue References Explained」C++右值引用详解 Part5:右值引用就是右值吗?
本文为第五部分,目录请参阅概述部分:http://www.cnblogs.com/harrywong/p/cpp-rvalue-references-explained-introduction.ht ...
- 解决setInterval计时器不准的问题
在js中如果打算使用setInterval进行倒数,计时等功能,往往是不准确的,因为setInterval的回调函数并不是到时后立即执行,而是等系统计算资源空闲下来后才会执行.而下一次触发时间则是在s ...
- Swift学习(三)类型推导&基本运算&分支&循环
一.Swift中类型推导&基本运算 Swift中类型推导 如果一个标识符在定义时有直接赋值,那么可以根据后面赋值的类型,来推导出前面标识符的类型,这样前面标识符的(:类型)可以省略 查看标识符 ...
- c++描述将一个2进制数转化成10进制数(用到初始化栈,进栈,入栈)
/* c++描述将2进制数转化成10进制数 问题,1.初始化栈后,用new,不知道delete是否要再写一个函数释放内存, 还是在哪里可以加上delete 2.如果栈满了,我要分配多点空间,我想的办法 ...
- 洛谷P1467 循环数 Runaround Numbers
P1467 循环数 Runaround Numbers 89通过 233提交 题目提供者该用户不存在 标签USACO 难度普及/提高- 提交 讨论 题解 最新讨论 暂时没有讨论 题目描述 循环数是 ...
- kickstart bonding安装
bonding用的是最简单的负载均衡模式,交换机不需要做配置. https://access.redhat.com/documentation/en-US/Red_Hat_Enterprise_Lin ...