前言

最近在鼓捣这玩意儿，自己不是很懂，没有别的博客可以参考，然后打算自己写一个脚本

原理

首先看一下LaserScan、MultiEchoLaserScan数据之间的差别
这个可以在其他博客上参考到
或者通过rosmsg命令查看主要差别

总结，起始差别的不多，主要就是ranges和intensities在LaserScan只是float32的数组，但是在MultiEchoLaserScan中变得msg对象的数组了，这个msg对象叫LaserEcho，然后他里面只有一个参数叫echoes

echoes本来是用来存放float32的数组，也就是存放LaserScan中ranges和intensities的元素，那么程序主要要实现的就是

• 将其他变化不大的数据直接复制
• 将LaserScan中的ranges的每个数据存到LaserEcho数据格式中的echoes中，一个ranges数据对应一个LaserEcho的msg
• 同样的，将LaserScan中的intensities也进行转换

其实这种方法太机械了，echoes中可以包含更多有用的信息

Show you the Code

#!/usr/bin/python3
# -*- coding: utf-8 -*-


from cmath import inf
import rospy
from tf.msg import tfMessage
import time
from sensor_msgs.msg import LaserScan
from sensor_msgs.msg import MultiEchoLaserScan
from sensor_msgs.msg import LaserEcho 


class A():
  def __init__(self) -> None:
    self.pub = rospy.Publisher('/horizontal_laser_2d', MultiEchoLaserScan, queue_size=1)
    self.val = MultiEchoLaserScan()

    self.if_print = True
    self.data_LaserEcho = LaserEcho()

  def callback(self, data):
    t_begin = time.time()
    self.val.header = data.header
    self.val.header.frame_id = "horizontal_laser_link"
    # print(rospy.Time.now().to_sec())
    # print(rospy.Time.now().secs)
    # print(rospy.Time.now().nsecs)
    # t = rospy.Time.from_sec(rospy.Time.now())
    
    # self.val.header.stamp.secs = rospy.Time.now().secs
    # self.val.header.stamp.nsecs = rospy.Time.now().nsecs.
    # self.val.header.stamp.secs = self.val.header.stamp.secs-252777018
    # print(self.val.header.stamp.secs, '000000000000')
    self.val.angle_min = data.angle_min
    self.val.angle_max = data.angle_max
    self.val.angle_increment = data.angle_increment
    self.val.time_increment = data.time_increment
    self.val.scan_time = data.scan_time
    self.val.range_min = data.range_min
    self.val.range_max = data.range_max
    self.val.ranges = []
    self.val.intensities = []

    # 接下来处理ranges和intensities
    for d in data.ranges:
      if d == inf:
        d = data.range_max
      dt = LaserEcho()
      dt.echoes.append(d)
      self.val.ranges.append(dt)
    for d in data.intensities:
      if d == inf:
        d = data.range_max
      dt = LaserEcho()
      dt.echoes.append(d)
      self.val.intensities.append(dt)

    if self.if_print:
      # print(self.val)
      self.if_print = False
    
    # 发布
    self.pub.publish(self.val)
    print(time.time()-t_begin)

def main():
  print('begin')
  rospy.init_node('change_bag_laser')
  a = A()
  rospy.Subscriber('/scan', LaserScan, a.callback)
  rospy.spin()

if __name__ == '__main__':
  main()
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75

注意事项哦嗯

这个程序用于ubuntu20的ROS中，我在ubuntu18用的过程中出现过问题

问题1

指定的版本不对，ubuntu20以下的默认python版本不是3，所以会出问题
需要将第一句话改为

#!/usr/bin/python
1

问题2

ubuntu20以下的cmath包没有inf这个对象，但是我查看了inf的意思，就是无穷大的意思，在程序中引用也就是为了剔除无效值

解决办法的话，就是不用他，中间的if判断直接改为如下

    # 接下来处理ranges和intensities
    for d in data.ranges:
      if d >= data.range_max:
        d = data.range_max
      dt = LaserEcho()
      dt.echoes.append(d)
      self.val.ranges.append(dt)
    for d in data.intensities:
      if d >= data.range_max:
        d = data.range_max
      dt = LaserEcho()
      dt.echoes.append(d)
      self.val.intensities.append(dt)
1
2
3
4
5
6
7
8
9
10
11
12
13

其他好像没啥问题了