Ask Your Question
0

What makes my node get jammed with old messages?

asked 2018-11-26 10:59:35 -0600

kump gravatar image

I have a Gazebo simulation controlled with ROS. My robot has one ultrasonic sensor using following Gazebo sensor and plugin:

        <sensor name="ray_sensor" type="ray">
            <ray>
                <scan>
                    <horizontal>
                        <samples>10</samples>
                        <resolution>1</resolution>
                        <min_angle>-${field_of_view/2}</min_angle>
                        <max_angle>${field_of_view/2}</max_angle>
                    </horizontal>
                    <vertical>
                        <samples>10</samples>
                        <resolution>1</resolution>
                        <min_angle>-${field_of_view/2}</min_angle> 
                        <max_angle>${field_of_view/2}</max_angle>
                    </vertical>
                </scan>
                <range>
                    <min>${min_range}</min>
                    <max>${max_range}</max>
                    <resolution>1.0</resolution>
                </range>

            </ray>
            <visualize>true</visualize>
            <plugin filename="libgazebo_ros_range.so" name="gazebo_ros_range">
                <gaussianNoise>0.005</gaussianNoise>
                <alwaysOn>true</alwaysOn>
                <updateRate>10</updateRate>
                <topicName>gazebo/ultrasonic_ray</topicName>
                <frameName>base_footprint</frameName>
                <fov>${field_of_view}</fov>
                <radiation>ultrasound</radiation>
            </plugin>
        </sensor>

Next I have a ROS node subscribing to the gazebo/ultrasonic_ray topic and publishing the distance to the '/ultrasonic' topic. The node code is shown below:

#!/usr/bin/python
import rospy
import os
import rospkg

from sensor_msgs.msg import Range
from std_msgs.msg import Float32

class UltrasonicSensor:

    def __init__(self):

        #maximal detection range in cm 
        self.maximal_ray = 250 
        self.range = self.maximal_ray

        # Create a publisher for our custom message.
        self.pub = rospy.Publisher("ultrasonic", Float32, queue_size=1)

        self.sub = rospy.Subscriber( "gazebo/ultrasonic_ray", Range, self.callback )

    def callback( self, msg ):
        if (msg.range == msg.max_range):
            self.range = self.maximal_ray
        else:
            self.range = msg.range*100

        print( self.range )

    def sonic_publisher(self):
        msg = Float32()

        msg.data = self.range

        self.pub.publish(msg)

if __name__ == '__main__':
    rospy.init_node('ultrasonic_sensor', anonymous=True)
    try:
        nh = UltrasonicSensor()

        rate = rospy.Rate(10)
        while not rospy.is_shutdown():
           nh.sonic_publisher()
           rate.sleep()

    except rospy.ROSInterruptException:
        pass

Then I have a ROS node that subscribes to /utrasonic topic and publishes to joint_right_wheel_controller/command and joint_left_wheel_controller/command, by which it drives the robot. Code shown below.

#!/usr/bin/env python

import rospy
import rospkg
from std_msgs.msg import Float32
from std_msgs.msg import Float64

import time

def callback(data):
    if ( data.data < 20 ):
        turn_around()

def turn_around():
    _robot_stop()
    _robot_backup()
    _robot_turn90right()
    _robot_forward()


def _robot_stop():
    publisher_dict['right'].publish(0.0)
    publisher_dict['left'].publish(0.0)

def _robot_backup():
    publisher_dict['right'].publish(-5.0)
    publisher_dict['left'].publish(-5.0)
    time.sleep(1)
    _robot_stop()

def _robot_turn90right():
    publisher_dict['right'].publish(-5.0)
    publisher_dict['left'].publish(5.0)
    time.sleep(1)
    _robot_stop()  

def _robot_forward():
    publisher_dict['right'].publish(5.0)
    publisher_dict['left'].publish(5.0)

def mynode():

    rospy.init_node('mynode')

    publisher_dict['right'] = rospy.Publisher('/joint_right_wheel_controller/command', Float64, queue_size=1)
    publisher_dict['left'] = rospy.Publisher('/joint_left_wheel_controller/command', Float64, queue_size=1)

    time.sleep(.5)

    rospy.Subscriber("/ultrasonic", Float32, callback)

    time.sleep(.5)

    _robot_forward()

    rospy.spin()


publisher_dict = { } 

if __name__ == '__main__':
    try:
        mynode()
    except rospy.ROSInterruptException:
        pass

Now if I run this simulation and this node, the robot starts driving forward, and as soon as it approaches a wall, starts to back up and turn, but after that it repeats the turning instead of driving forward. As if there were messages about close proximity even when the robot does not face any obstacle.

So I'm thinking, during the first encounter with the wall, the sensor has sent several messages with the close distance that is classified ... (more)

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2018-11-26 12:47:55 -0600

This is not a full answer but it may help.

Your node which subscribes to gazebo/ultrasonic_ray is publishing messages at a rate of 10 per second. However the node which subscribes to these messages on the /utrasonic topic will block for at least 2 seconds when avoiding an obstacle. This means there will be 20 messages in the queue when it has finished blocking.

You have set the queue size for the publishers, but not for the subscriber and that is the point at which these messages will be building up. Try changing the subscriber line to:

rospy.Subscriber("/ultrasonic", Float32, callback, queue_size=1)

This may solve your problem in this case, however writing callbacks that block for 2 seconds is not an ideal design approach.

edit flag offensive delete link more

Comments

1

It definitely helps. Thank you. Both points are very true. I didn't know you can put queue limit on subscriber. Blocking the program in callback function for this long is simply bad programming. It didn't occur to me. Thank you.

kump gravatar imagekump ( 2018-11-27 04:13:34 -0600 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2018-11-26 10:59:35 -0600

Seen: 132 times

Last updated: Nov 26 '18