ros 数据类型_convertio转换器

前言

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

原理

首先看一下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- # 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() 

注意事项哦嗯

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

问题1

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

#!/usr/bin/python 

问题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) 

其他好像没啥问题了