Raspberry Pi & Linux Arch - Day 11 - Build the Ultrasonic Radar

In this post, we will show how we combine the stepper motor and the ultrasonic sensor together to make a simple ultrasonic radar.

To combine the motor and the sensor together, it is essential to establish communications between them. Our design is to connect all the devices to a central controller and it will handle the communications among components holistically.

As we mentioned earlier, the components work in a parallel way so we need to use the multiprocessing module in python. When we write the code to control a device, we need to implement two functionalities: (1) code that controls the physical device and (2) code that handle the communication between different components and processes.

We have already seen the code that controls the physical device. StepperMotor class and DistanceSensor are two examples. They are sufficient if we just want to make the device work; however if we want to establish communication between devices and dynamically change the parameters of the device, we need new code.

Here, we will create a new class called Component to represent each physical component in the robot. The Component class defines an interface and provides basic functions that allows the physic device to run and to communicate.

class Component(metaclass=ABCMeta):
    #TODO: add lock to component to make it safer
    @abstractmethod
    def send_msg(self,Q):
        """
        Send the message to the Q. Q should support put operation.
        Note that message is not incluced in the parameter list, it is the 
        responsibility of class to implement the process.
        """
        pass

    @abstractmethod
    def run(self):
        """
        Upon calling this function, the component will perform its routine work.
        """
        pass

    def parse_and_execute(self, msg_tuple):
        """
        This function will parse the arguments and call the member function accordingly.
        The msg_tuple should be (func_name, args, kwargs):
            func_name : the name fo the member function that will be called
            args      : the argument list. It is also a tuple.
            kwargs    : keyword argumetns. It is a dictionary

        """

        func_name, args, kwargs = msg_tuple
        func = getattr(self, func_name)
        func(*args, **kwargs)

    def update(self, attr, val):
        setattr(self, attr, val)

    def d_update(self, attr, val):
        orig_val = getattr(self,attr)
        setattr(self, attr, orig_val + val)

    @property
    def name(self):
        return self._name

    @property
    def parser(self):
        return self._parser

Note that all the communication is handled by multiprocessing.Queue. It is the bridge between a component and the central controller (we will talk about the controller later).

Let us write some codes to create sensor component:

class DistanceRadarSensorComponent(Component):
    PARSER = [(x,i) for i,x in enumerate(['timestamp','comp_name','distance', 'status'])]
    def __init__(self, name=None, pin_echo=None, pin_trig=None, unit='m', delay=0.00001):
        assert name is not None

        self._name = name
        self._sensor = DistanceSensor(pin_echo=pin_echo, pin_trig=pin_trig, unit=unit)
        self._measure_result = (None,DistanceSensor.INIT)
        self._delay = delay

        self._parser =  DistanceRadarSensorComponent.PARSER

        super(DistanceRadarSensorComponent,self).__init__()

    def run(self):
        self._measure_result = self._sensor.measure()
        time.sleep(self._delay)

    def send_msg(self,Q):
        res = self._measure_result
        msg = (time.time(), 'DistanceRadarSensor::{}'.format(self._name), res[0], res[1])
        Q.put(msg)

Nothing special here. As mentioned in the comments, we need to put the routine work in the run() method and for the distance sensor, the routine work is of courser to measure the distance. The send_msg method defines which information we want to send back to the controller.

As you can see, the Component class has nothing to do the multiprocessing and it cannot work continuously. Though we can call the component.run() is a while loop, we wipe out the possibility to communicate with the component not to mention to control the device dynamically.

To solve this problem, we will write a kind of decorator that wraps a component and make it possible to communicate with other components and run continuously.

class ContinuousComponentWrapper(mp.Process):
    """
    The ContinuousComponentWrapper is use to make a component running in the background.
    

    Args of __init__:
        component: an instance of component class.
        cmd_Q:     command queue. It is used to send command to the component when it is running in the background.
        output_Q:  output queue. The wrapper sends out message or informaiton through this queue.


    """
    def __init__(self, component=None, cmd_Q=None,output_Q=None):
        assert isinstance(component, Component)
        assert isinstance(cmd_Q, mp.queues.Queue)
        assert isinstance(output_Q, mp.queues.Queue)

        self._component = component
        self._output_Q = output_Q
        self._cmd_Q = cmd_Q
        super(ContinuousComponentWrapper,self).__init__()

    def run(self):
        """
        Running the component in the infinite loop. To change the status of the component, one can send command to the command queue.
        #TODO: add a stop-pill
        """
        while True:
            while not self._cmd_Q.empty():
                cmd = self._cmd_Q.get()
                self._component.parse_and_execute(cmd)

            self._component.run()
            self._component.send_msg(self._output_Q)



    @property
    def component(self):
        return self._component

A flow chart helps us understand the structure better:

In the run() method, we have a while loop that makes the component run continuously. At the beginning of each loop, we first check if the component receives new command. The command is sent through the cmd_Q (command queue). If we do have new commands, the component will parse the command and execute it. In general, the command is used to change the parameters of the underlying component and it is in this way we can control the physical device dynamically. After the component finishes the run, it will send message through the output_Q (output queue).  Most of the time, the output_Q is connect to the controller. As you can see here, it is really the cmd_Q and the output_Q that define the scope of the component. 

The next step is to write a DataHandler class that can deal with output messages from different components. We are now close to the moment to create the "brain" of the robot and it really seems exciting.

--END--