# -*- coding:utf8 -*-
import pickle
import rospy # 导入rospy模块
from geometry_msgs.msg import PointStamped # 导入PointStamped节点
import time
import socket
class Ros_Get_Data():
def __init__(self):
# 初始化ROS节点
print "请稍候..."
rospy.init_node("Get_clicked_point_file", anonymous=True) # 该句初始化了一个node,node的名 字为Get_clicked_point_file.
# anonymous=True, 表示后面定义相同的node名字时候,按照序号进行排列
self.ros_location = [] # 为了存放获取到的数据
rospy.Subscriber("/clicked_point", PointStamped, callback=self.data_list) # 订阅clicked_point话题,并回调data_list
self.ros_coordinates = [] # 定义一个空列表,为了接收想要的x,y,z的数据
self.i = 0 # 循环条件起始值
self.sock = socket.socket(socket.AF_UNIX, socket.SOCK_STREAM) # socket连接
self.sockpath = "/tmp/testss"
self.sock.connect(self.sockpath) # 连接到本地
def data_list(self,data):
self.ros_location.append(data) # 将话题中的坐标数据放到列表里
def connectAndSendData(self):
while self.i < 36: # 循环条件小于36 进入循环
time.sleep(1.5) #
print "login..."
if len(self.ros_location): # 如果列表不为空
self.ros_location_one = self.ros_location.pop() # 将列表里的数据弹出 并赋值给新对象
x = float('%.4f' % self.ros_location_one.point.x) # x
y = float('%.4f' % self.ros_location_one.point.y) # y
z = float('%.4f' % self.ros_location_one.point.z) # z
self.ros_coordinates.append(x) # 将x数据放到ros_coordinates
self.ros_coordinates.append(y) # # 将y数据放到ros_coordinates
self.ros_coordinates.append(z) # # 将z数据放到ros_coordinates
print (time.strftime('%H:%M:%S ', time.localtime(time.time()))) # 输出当前时间
self.sock.send(pickle.dumps({"type":"data","data":self.ros_coordinates})) # 发送数据
data = self.sock.recv(1024) # 数据接收最大为1024
print data # 反馈服务端发送的信息
self.i += 1
else: # 如果列表为空
try: # 尝试发送数据
self.sock.send(pickle.dumps({"type" : "wait"})) # 发送数据
data = self.sock.recv(1024) # 数据接收最大为1024
except:
print "连接已断开"
self.sock.close()
rospy.signal_shutdown("杀死节点")
self.sock = None
break
else:
self.sock.close()
rospy.signal_shutdown("杀死节点")
print "节点已关闭"
self.sock = None
关于获取多线雷达实际位置-Ros端
猜你喜欢
转载自blog.csdn.net/weixin_54062353/article/details/124292981
今日推荐
周排行