制作一个ROS节点，对“msg.ranges”进行空间中值过滤

Question

我希望编写一个窗口宽度为3的中值滤波器，即三个[1,2,3,4,5,6]输出为[1,3,4,6]。因此，我尝试编写以下节点。但是，作为新手，我仍然不知道如何将python代码转换成ROS节点来工作。那么，你能帮我写这个节点吗？

#!/usr/bin/env python
"""
"""

import rospy
from sensor_msgs.msg import LaserScan

def las_callback(msg):

#function definition pass two angument
 def medfiter(X,win_len):


#this function return shape and type of x
  Y=np.zeros_like(X)


#initualize value of padwidth
  padwidth=int((win_len-1)/2)

#pading array X
  X=np.pad(X,((0,0),(padwidth,padwidth)))

#loop in i read from padwidth value to the X.shape[1]-padwidth
for i in range(padwidth,X.shape[1]-padwidth):

#store median in y variable of x variable by passing columns and rows of data in median function

  Y[:,i-padwidth]=np.median(X[:,i-padwidth:i+padwidth+1],axis=1)

#return variable y
return Y
 
     
rospy.init_node('laser_readings')
sub = rospy.Subscriber('/base_scan', LaserScan, las_callback)
rospy.spin()

Answer 1

您应该首先检查作为数据格式的消息类型。就你而言，你有激光扫描。在这里可以看到LaserScan的数据结构

所以你们必须对应用中值滤波的范围（浮动列表）感兴趣。因此，请按如下方式获取数据：

data = msg.ranges 

现在您有了一个列表（数据），可以进行操作了