如何订阅&q；/扫描&q；主题、修改消息并发布到新主题？ [英] How to subscribe "/scan" topic, modify the messages and publish to the new topic?

问题描述

我想通过订阅Message.range来改进Turtlebot3的LDS-01传感器，通过应用一些算法来修改Messange.range，并将其发布到新的主题，如下所示。但当我运行代码时出现错误。错误为

1. 遇到溢出。错误为"；RounmeWarning：Exp中遇到溢出 A1=2/(1+exp(-2*n1))-1"；订阅"；/can_new"；主题后，正在发布的值在终端上变为红色
2. 当我订阅新主题"；/can_new"；时，错误为"；Warning：未收到消息，模拟时间处于活动状态。/时钟是否正在发布？"；。

我可以知道为什么吗？有没有人能帮我写代码？提前感谢您！

#! /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()

推荐答案

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

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

这篇关于如何订阅&q；/扫描&q；主题、修改消息并发布到新主题？的文章就介绍到这了，希望我们推荐的答案对大家有所帮助，也希望大家多多支持IT屋！