如何订阅&q;/扫描&q;主题、修改消息并发布到新主题?

2022-06-06 00:00:00 python ros gazebo-simu

问题描述

我想通过订阅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在尝试发布时类型错误。y1numpy.matrix,当您调用tolist()时,它不会将其展平为一维数组,而是返回一个包含列表的an数组。这就是红色输出文本的来源。要解决此问题,请执行以下操作:

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

相关文章