Search code examples
pythonrosgazebo-simu

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


I would like to improve the LDS-01 sensor of the turtlebot3 by subscribing the message.ranges, modify the messange.ranges by applying some algorithm and publish it to the new topic as shown below. But there are an error when I run the coding. The error is

  1. There are overflow encountered. the error is "RuntimeWarning: overflow encountered in exp a1 = 2/(1+exp(-2*n1))-1" and the value that being publish become red on the terminal after subscribing the "/scan_new" topic
  2. When I subscribing the new topic which is "/scan_new", the error is "WARNING: no messages received and simulated time is active. Is /clock being published?".

May I know why? and is there anyone can help with my coding? Thank you in advance!

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

Solution

  • The reason you're not seeing output is because the message field ranges has the wrong type when trying to publish it out. y1 is a numpy.matrix and when you call tolist() it doesn't flatten it out to a 1D array, it returns a an array that contains your list. This is where the red output text is coming from. To fix the problem do:

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