Node slowing down other node on raspberry PI

edit

Hi, I'm currently playing around with a GoPiGo, which is a small differential drive robot based on a Raspberry Pi. To control the robot I use the ROS node that developers of GoPiGo have made ( https://github.com/ros-gopigo/gopigo3... ).

I added my own node to read the data from to Ultrasonic sensors that I connected to the Raspberry GPIO pins. The node publishes the ultrasonic data to a laserscan message.

Now when I run my Ultrasonic node and send cmd_vel to the GoPiGo node the robot stutters extremely, It seems like the GoPiGo node gets slowed down or "sleeps" sometimes.

The CPU load is around 30 %, and memory isn't full. When I stop my ultrasonic node the robot keeps stuttering. I have to restart the GoPiGo node to stop the stuttering

It's not the time.sleep() functions, because I tried running a node that only runs time.sleep() but this didn't cause the stuttering. The same goes for rospy.sleep(). Could it be that the GPIO pins cause the stuttering? Then why is stuttering still there after I close the node?

The raspberry runs ubuntu 16.04 with kinetic (this to be exact: https://downloads.ubiquityrobotics.co... )

The code is below, please note that it's still WIP, so it's fairly rough. Also on github: https://github.com/kwint/ros-hcsr04-l...

#!/usr/bin/env python
import rospy
from sensor_msgs.msg import LaserScan

import time
import math
import RPi.GPIO as GPIO


class Measurement(object):
    '''Create a measurement using a HC-SR04 Ultrasonic Sensor connected to
    the GPIO pins of a Raspberry Pi.

    Metric values are used by default. For imperial values use
    unit='imperial'
    temperature=<Desired temperature in Fahrenheit>
    '''

    def __init__(self,
                 trig_pin,
                 echo_pin,
                 temperature=20,
                 round_to=1,
                 gpio_mode=GPIO.BCM
                 ):
        self.trig_pin = trig_pin
        self.echo_pin = echo_pin
        self.temperature = temperature
        self.round_to = round_to
        self.gpio_mode = gpio_mode

    def raw_distance(self, sample_size=11, sample_wait=0.1):
        """Return an error corrected unrounded distance, in cm, of an object
        adjusted for temperature in Celcius.  The distance calculated
        is the median value of a sample of `sample_size` readings.


        Speed of readings is a result of two variables.  The sample_size
        per reading and the sample_wait (interval between individual samples).

        Example: To use a sample size of 5 instead of 11 will increase the
        speed of your reading but could increase variance in readings;

        value = sensor.Measurement(trig_pin, echo_pin)
        r = value.raw_distance(sample_size=5)

        Adjusting the interval between individual samples can also
        increase the speed of the reading.  Increasing the speed will also
        increase CPU usage.  Setting it too low will cause errors.  A default
        of sample_wait=0.1 is a good balance between speed and minimizing
        CPU usage.  It is also a safe setting that should not cause errors.

        e.g.

        r = value.raw_distance(sample_wait=0.03)
        """
        speed_of_sound = 331.3 * math.sqrt(1 + (self.temperature / 273.15))
        sample = []
        # setup input/output pins
        GPIO.setwarnings(False)
        GPIO.setmode(self.gpio_mode)
        GPIO.setup(self.trig_pin, GPIO.OUT)
        GPIO.setup(self.echo_pin, GPIO.IN)

        for ...