如何订阅“/scan”主题，修改消息并发布到新主题？

Question

我想通过订阅 message.ranges 改进 turtlebot3 的 LDS-01 传感器，通过应用一些算法修改 messange.ranges 并将其发布到新主题，如下所示。但是我运行编码时出现错误。错误是

遇到溢出。错误是“RuntimeWarning：exp 遇到溢出 a1 = 2/(1+exp(-2*n1))-1" 订阅“/scan_new”主题
当我订阅新主题“/scan_new”时，错误是“警告：没有收到消息并且模拟时间处于活动状态。/clock 正在发布吗？”。

请问为什么？有没有人可以帮助我编码？提前致谢！

#! /usr/bin/env python

import rospy
from sensor_msgs.msg import LaserScan   
from numpy import *
import numpy as np

def callback(msg):
    x1 = msg.ranges
    x2 = np.array(x1,dtype=float)
    #input 1
    x1_xoffset = 0.1656566113
    x1_gain = 0.599818245108751
    x1_ymin = -1

    #layer 1
    b1 = matrix ('-326.80027082597098342;-98.990085738415288574;83.870582554028558775;-72.368630055558369918;-0.7092849798153298968;57.269078212651997717;44.310310623213219117;-31.707975756385888388;-17.911078072875334044;-4.6394718363325218036')
    IW1_1 = matrix ('337.00974203853263589;112.59115212423027685;-110.55255345932357613;113.22379022448127728;1.3919097878355670694;-110.07199451465535844;-110.63343956448693461;112.90214029452344846;111.7555578995358303;114.3437445151164269')

    #layer 2
    b2 = matrix ('-2.978824073708045072e-05')
    LW2_1 = matrix ('0.030307489543199929438 0.060616873246115143825 -0.060618376238034066272 0.060619146713019073092 -0.00016486744836432447401 -0.060619845018730772468 -0.060619496805093056602 0.060618473675156582525 0.060617195348181586445 0.060615509952774870861')

    # Output 1
    y1_ymin = -1
    y1_gain = 0.606060606060606
    y1_xoffset = 0.2

    #input 1
    xp1 = ((x2 - x1_xoffset)*(x1_gain))+x1_ymin

    #layer 1
    n1 = IW1_1*xp1+b1
    a1 = 2/(1+exp(-2*n1))-1

    #layer 2
    a2 = LW2_1*a1+b2

    # Output 1
    y1 = ((a2-y1_ymin)/(y1_gain))+y1_xoffset
    y2 = y1.tolist()
    scan.ranges = y2
    pub.publish(scan)

rospy.init_node("ann_publisher")
sub = rospy.Subscriber('/scan', LaserScan, callback)
pub = rospy.Publisher('/scan_new', LaserScan, queue_size=1)
rate = rospy.Rate(2)
scan = LaserScan()
rospy.spin()

Answer 1

您看不到输出的原因是消息字段 ranges 在尝试发布时类型错误。 y1 是一个 numpy.matrix，当您调用 tolist() 时，它不会将其展平为一维数组，它 returns 是一个包含你的清单。这是红色输出文本的来源。要解决此问题，请执行以下操作：

y1 = ((a2-y1_ymin)/(y1_gain))+y1_xoffset
y2 = y1.tolist()[0]

如何订阅“/scan”主题，修改消息并发布到新主题？

How to subscribe "/scan" topic, modify the messages and publish to the new topic?

python

ros

gazebo-simu