Ros ultrasonic sensor node convert from float to range for Rviz?

asked 2021-07-26 15:02:16 -0500

matthewfwork gravatar image

updated 2021-07-26 15:02:46 -0500

I am trying to build a custom bot with a few different sensors publishing to nodes and using Rviz to visualize said nodes. I need to convert the float to range msg is this possible? I am new to this any and all help is greatly appreciated.

import RPi.GPIO as GPIO
import rospy
import time
from std_msgs.msg import Float64
trigPin = 16
echoPin = 18
MAX_DISTANCE = 220          # define the maximum measuring distance, unit: cm
timeOut = MAX_DISTANCE*60   # calculate timeout according to the maximum measuring distance
def pulseIn(pin,level,timeOut): # obtain pulse time of a pin under timeOut
    t0 = time.time()
    while(GPIO.input(pin) != level):
        if((time.time() - t0) > timeOut*0.000001):
            return 0;
    t0 = time.time()
    while(GPIO.input(pin) == level):
        if((time.time() - t0) > timeOut*0.000001):
            return 0;
    pulseTime = (time.time() - t0)*1000000
    return pulseTime
def getSonar():     # get the measurement results of ultrasonic module,with unit: cm
    GPIO.output(trigPin,GPIO.HIGH)      # make trigPin output 10us HIGH level
    time.sleep(0.00001)     # 10us
    GPIO.output(trigPin,GPIO.LOW) # make trigPin output LOW level
    pingTime = pulseIn(echoPin,GPIO.HIGH,timeOut)   # read plus time of echoPin
    distance = pingTime * 340.0 / 2.0 / 10000.0     # calculate distance with sound speed 340m/s
    return distance
def setup():
    GPIO.setmode(GPIO.BOARD)      # use PHYSICAL GPIO Numbering
    GPIO.setup(trigPin, GPIO.OUT)   # set trigPin to OUTPUT mode
    GPIO.setup(echoPin, GPIO.IN)    # set echoPin to INPUT mode
def loop():
    while(True):
        distance = getSonar() # get distance
        time.sleep(1)
if __name__ == '__main__':     # Program entrance
    print ('Program is starting...')
    rospy.init_node('ultrasonic_trans')
    pub = rospy.Publisher("/ultra", Float64, queue_size=10)
    rate = rospy.Rate(1)
    setup()
    while not rospy.is_shutdown():
        distance  = getSonar()
        pub.publish(distance)
        rate.sleep()
    rospy.loginfo("Ultra Sonic Pub Closed")
    GPIO.cleanup()

Here is the error code

[WARN] [1627328280.988570]: Could not process inbound connection: topic types do not match: [sensor_msgs/Range] vs. [std_msgs/Float64]{'callerid': '/rviz_1627328246890519494', 'md5sum': 'c005c34273dc426c67a020a87bc24148', 'tcp_nodelay': '0', 'topic': '/ultra', 'type': 'sensor_msgs/Range'}
edit retag flag offensive close merge delete

Comments

You are supposed to publish a message of type sensor_msgs/Range. Your float values go into the "range" attribute, see the documentation I linked.

Humpelstilzchen gravatar image Humpelstilzchen  ( 2021-07-27 09:16:52 -0500 )edit