Raspberry Pi & Arch Linux - Day 18 - Noncanonical mode input and child process in Python

In this post, we will talk about two thing: (1) noncanonical mode of input and (2) child process in Python. The knowledge of the child process is not necessary for our project whereas the first is crucial. Before we can build algorithm to instruct the robot to move automatically, we first need to know how to control the robot manually. In other words, we are going to build a remote control robot car before anything else.

The idea is that we enter the command through the keyboard and once the program receives the command it controls the speed and the direction of the robot accordingly. For example, when we type the up arrow key, the robot will increase its speed or when we type the left arrow key, the robot will turn left.

There is one problem. Most of the time, the default setting of the user input is based on line. The user will enter whatever he wants and then press "return" to submit the command. However in our case, we want to enter the command char by char. We do not want to type 10 times the up arrow key plus a "return" key to tell the program that we want to quickly increase the speed of the robot.

A quick search on Google gives us the following solution.

def getchar():
   #Returns a single character from standard input
   import tty, termios, sys
   fd = sys.stdin.fileno()
   old_settings = termios.tcgetattr(fd)
   try:
      tty.setraw(sys.stdin.fileno())
      ch = sys.stdin.read(1)
   finally:
      termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
   return ch

Note that the three packages used in this function are quite low-level.

This solution has one drawback. When we call the getchar() function, it will block and wait for user to enter the character. This is not convenient when the user does not want to any command. To deal with this problem, we need to have timeout feature. We can find many solutions on this issue.  Some of the solutions uses a separate process to handle the inputs. Here we have another solution.

def getchar(timeout=3):
    fd = sys.stdin.fileno()
    old_setting = termios.tcgetattr(fd)

    try:
        # switch to noncanonical mode
        tty.setraw(sys.stdin.fileno())
        new_setting = termios.tcgetattr(fd)

        # set the timeout
        cc = new_setting[6]
        cc[termios.VTIME] = timeout # timeout = 0.3s
        cc[termios.VMIN]  = 0       # start the timer immediately

        # update termios struct
        termios.tcsetattr(fd, termios.TCSADRAIN, new_setting)

        # read
        ch = sys.stdin.read(1)
    finally:
        termios.tcsetattr(fd, termios.TCSADRAIN, old_setting)
    return ch

As we mentioned, termios is a quite low-level package. What we need is actually called the noncanonical mode of input. It is controlled in the struct termios and it does provides parameters to control the timeout behavior.

The second thing we want to cover in this post is the child process. The following code shows how we can send the interrupt signal to all the processes.

if key_press == 'q':
    print("Exiting system")
    pid = os.getpid()
    parent = psutil.Process(pid)
    children = parent.children(recursive=True)
    for process in children:
        os.kill(process.pid, signal.SIGINT)

    os.kill(pid, signal.SIGINT)

In Python, we use os package to get the current process id and send signal to a given process. The signals are defined in the signal package. To send the signals to the child processes, we need to get the child process id of a given process. This functionality is provided in the psutil package. More specifically, we use the psutil.Process(pid) to get a list of child process object of the given process id.


--END--

Raspberry Pi & Arch Linux - Day 17 - Visualization in terminal and KeyboardInterrupt handler

Happy Halloween!

Last time, we saw that the robot car could run without any direction and speed control. To make the robot more intelligent, we should first make sure that it can interact with the environment. All the communication between the robot car and the environment go through the radar in the front of the car. It consists of a stepper motor as the base and a ultrasonic distance sensor which measure the distance to the obstacles.

As we add more components to the robot car, the number of parameters in the system increase dramatically. (probably it is a good idea to have an overview of all the parameters currently used in the robot car). Before we code the direction and speed control algorithm, it is necessary to configure all the parameters so that the robot car can function properly. The parameters of the most interests are

• the range of the radar
• the angle speed of the radar
• the frequency of the sampling (radar base and distance sensor)
• the buffer size of the data

One of the problems is how we can visualize the distance map in the terminal. There are two approaches:

• gnuplot
• bashplotlib in python

If we use gnuplot, we need to start a subprocess in python and sometimes it is very difficult to deal with the subprocess, especially when we want to load the outputs of the subprocess. So for our testing purpose, we choose the bashplotlib package. 

The function we use is plot_hist because the distance map is essentially a histogram. The x-axis is the degree, which indicates the position of the radar base (stepper motor); the y-axis is the distance to the obstacles. To plot the histogram, we need to convert the floating distance to int. 

I do not know if I missing something about the bashplotlib package. It seems to me that the plot_hist function only accepts the raw data, which means we can not pass a dictionary or pd.Series to it. So if you have a pd.Series 

In[4]: ts

Out[4]: 

-1    3

 0    2

 1    5

dtype: int64

You need to manually convert it to [-1,-1,-1,0,0,1,1,1,1,1].

The figures below present the plot in terminal. It is not very fancy but it is much better than looking at an array of 100 numbers. :)

To make the testing more convenient, we want the radar to go back to its zero or initial position when the process is interrupt by the ctr-c. When we enter the ctr-c, the python interpreter will raise a KeyboardInterrupt error in the "main" process and all the subprocess as well. It means that we need to handle the keyboard interrupt error in both the main script and the scripts that use multiprocessing.Process. Fortunately, all components that are running concurrently are wrapped in the ContinuousComponentWrapper. So we just need to add a exception handler in the run method of the ContinuousComponentWrapper

    # ContinuousComponentWrapper

    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
        """
        try:

            while True:
                while not self._cmd_Q.empty():
                    cmd = self._cmd_Q.get()
                    if cmd == CMD_EXIT or cmd == (CMD_EXIT,):
                        return 
                    self._component.parse_and_execute(cmd)


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

        except KeyboardInterrupt:
            if hasattr(self._component, 'KeyboardInterruptHandler'):
                self._component.KeyboardInterruptHandler()
            print('{} property exit after KeyboardInterrupt.'.format(self._component.name))

and create a KeyboardInterruptHandler method for the DistanceRadarBaseComponent.

    # DistanceRadarBaseComponent

    def KeyboardInterruptHandler(self):
        self._stepper_motor.back_to_zero_pos(delay=self._delay)

--END---

Raspberry Pi & Arch Linux - Day 16 - First Prototype

Here comes our first prototype!

To make the project portable, we need to first find a power supply for the electronic components. The one I used is RAVPower 22000mAh 5.8A Output 3-Port Portable Charger Power Bank External Battery Pack (2.4A Input, Triple iSmart 2.0 USB Ports, High-density Li-polymer Battery) For Smartphones and more- Black

This power bank is kind of heavy and it is very stable when we put it on the chassis.

After putting everything together, with the help of lots of tapes, the robot car looks like this:

It can move quite fast. I had concerns that the loading may be too heavy to carry but motor's performance beats my expectation and shows a lot of more potential. I think I can still add a few devices to the robot.

Here is the first drive

It is obviously a milestone of the project when all the components are on the chassis. However, some technical issues come to the surface immediately. One of the main issues is the position of the distance sensor. It turns out that there is no easy way to install the sensor to the center of the chassis. In this prototype, it is installed at the right corner of the robot.

Another problem is caused by the size of the power bank. This power bank is quite large and it cannot fully fit into the space. Even worse, it actually becomes an obstacle of the distance sensor and it limits the range of scanning area. As shown in the figure above, we have blind spot on the left side of the robot car. Therefore, the left side and the right side are not longer symmetric. This factor should be taken into account in the analytical algorithm development.

--END--

Raspberry Pi & Arch Linux - Day 15 - Obstacle Avoidance Method (1)

Let us talk about the obstacle avoidance algorithm today.

We start from some simple ideas. Here is a paper illustrating the idea of vector field histogram:

http://www-personal.umich.edu/~johannb/Papers/paper16.pdf

The main idea is to use a two-dimentional Cartesian histogram grids to represent the environment. Each cell of grid has a certainty value which represents the confidence level in the existence of an obstacle. We can think of this grid as a probabilistic view of the obstacle location.

One of the steps in the Vector Field Histogram (VFH) approach is that we need to reduce the two dimentional Cartesian histogram grids to one-dimentional polar histogram. Honestly I do not quite understand why we need to convert from two-dimentional Cartesian system to one-dimentional polar system instead of having the polar system from the beginning. In our case, we have a rotational distance radar; hence it is very natural to work with polar system in the first place.

Here is a draft version of code that creates the polar histogram. There is nothing specially here. What we need to do is to align the data from radar motor and the data from distance sensor based on the timestamp. The data from the radar motor provides the angle of the distance sensor and the data from the sensor is just the distance. Once the timestamp is aligned, we will know the distance measured when the radar is positioned at a particular angle. And this is essentially the polar histogram.

def create_distance_map(df_radar_base, df_distance_sensor):
    """
    Create the distance map. The map represents the environment in front of the robot. It is the association between position of
    radar (in degree) and the distance detected in that position.

    Args:
        df_radar_base:      data sent from distance radar component.
        df_distance_sensor: data sent from the distance radar sensor component.

    Return:
        A pd.Series. The index is the position of the radar base and the value is the distance deteced. 

    Note:
        The map is discrete. The value of index in the retured Series is integer.
    """



    df_sensor = df_distance_sensor
    df_base = df_radar_base

    df_sensor['timestamp'] = format_timestamp(df_sensor['timestamp'])
    df_base['timestamp']   = format_timestamp(df_base['timestamp'])

    df = pd.merge(df_sensor, df_base, on='timestamp', how='outer', suffixes=('_sensor','_base')).sort_values('timestamp')
    
    # smooth the distance.
    # window = int(df.shape[0] / df['pos'].notnull().sum())
    # df['avg_distance'] = df['distance'].rolling(window=window, min_periods=1).mean()
    # df['distance'] = df['distance'].combine_first(df['avg_distance'])
    # df = df.drop('avg_distance', axis=1)

    # clean the data
    df['distance'] = df['distance'].fillna(method='ffill')
    df['status']   = df['distance'].fillna(method='ffill')

    df_work = df[(df['pos'].notnull()) & (df['distance'].notnull())]

    # select useful information
    df_work = df_work[['timestamp','status','distance','pos','degree']]


    # create the distance map
    df_work['pos_bin'] = df_work['pos'].astype(int)

    ts_distance_map = df_work.gropuby(by='pos_bin')['distance'].mean().sort_index()

    return ts_distance_map

To align the timestamp, we merge the two data frame on timestamp and then fill the NaN. In pandas-0.19, there is a new function called merge_asof and it can merge two data frames without an exact match on keys.

In the context of the VFH method, obstacles exert repulsive forces and the magnitude of the such forces depends on the distance between obstacles and the robot. All these repulsive forces form a kind of filed and this is reason why we called this method Vector Field Histogram.

VFH is a simple idea but it is that easy to implement. The algorithm actually has three steps:

1. collect and transform data
2. generate signal/command
3. physical steer controllers

We will even experience difficulties for the first item for our ultrasonic sensor is a cheap one and it is not very stable. Also the measurement itself may not be accurate enough. For the second and third item, we have more serious problems. We can control the speed and the direction of the robot but we do not have any idea of the absolute speed and we loosely control the angle of the turn. 

I think the next big question is how we can continuously update the polar histogram without fine control on the speed and angle of the robot.

--END--

Raspberry Pi & Arch Linux - Day 14 - A simple Engine class the provides high level control over two wheels

Something else distracted me last week so there was no significant progress.

It is the moment to consolidate a little bit and put all the pieces together.

To control the speed and the direction of the robot, we need to have a Engine class which defines some high level commands such as turn_left, turn_right and stop. In theory, we can decrease the speed of the left wheel and increase the speed of the right wheel to achieve the turn-left effect. We will not know if this works until we do a real test. Also for the present, I can imagine that we may need different types of turn-left operation: it can turn slowly or quickly; it can be a slight turn or a sharp turn. For now, we have scale and period parameters as placeholder.

class Engine:
    """
    Engine class controls the movement of the robot. It consists of two wheels.
    """

    def __init__(self, startup_scale=0.1, stable_scale=0.6, cmd_Q=None, output_Q=None,left_wheel_comp=None, right_wheel_comp=None):
        """
        Args:
            startup_scale:    float. It controls the start up sclae of the two wheel. 
            stable_scale:     float. It controls the speed of the wheel in the stable status.
            left_wheel_comp:  Instance of WheelComponent.
            right_wheel_comp: Instance of WheelComponent.
            cmd_Q: Do we need this?
            output_Q: Do we need this?

        Note:
            The function will construct the ContinuousComponentWrapper internally. The user of the class will not be able to control the two wheels
            directly. All the operations are performed through the Engine class.
        """
#        assert isinstance(cmd_Q, mp.queues.Queue)
#        assert isinstance(output_Q, mp.queues.Queue)

        assert isinstance(left_wheel_comp, WheelComponent)
        assert isinstance(right_wheel_comp, WheelComponent)

        self._cmd_Q = cmd_Q
        self._output_Q = output_Q
        self._left_wheel_comp = left_wheel_comp
        self._right_wheel_comp = right_wheel_comp

#        self._reference_pulse_left = left_wheel_comp.reference_pulse
#        self._reference_pulse_right = right_wheel_comp.reference_pulse
#        self._pulse_left = left_wheel_comp.pulse
#        self._pulse_right = right_wheel_comp.pulse

        self._cmd_Q_left = mp.Queue()
        self._cmd_Q_right = mp.Queue()
        self._output_Q_left = mp.Queue()
        self._output_Q_right = mp.Queue()

        self._left_wheel = ContinuousComponentWrapper(component=self._left_wheel_comp, cmd_Q=self._cmd_Q_left,output_Q=self._output_Q_left)
        self._right_wheel = ContinuousComponentWrapper(component=self._right_wheel_comp, cmd_Q=self._cmd_Q_right,output_Q=self._output_Q_right)


    def _change_speed(self, left_scale=0., right_scale=0.):
        """
        change the speed of the two wheels. A utility function.
        """
        self._cmd_Q_left.put(('increase_speed',(left_scale,), {}))
        self._cmd_Q_right.put(('increase_speed', (right_scale,), {}))


    def stop(self):
        """
        Breaks the robot. Set the wheels to be still.
        """
        self._cmd_Q_left.put(('stop', (), {}))
        self._cmd_Q_right.put(('stop', (), {}))

    def turn_left(self, scale=0.5, period=1.):
        """
        Turn left. The if scale = 0, the turn is slow; if the scale = 1., the turn is sharp.
        """
        scale = min(scale, 1)
        scale = max(scale, 0)
        left_scale = -(1 - scale)
        right_scale = scale
        self._change_speed(left_scale, right_scale)
        time.sleep(period)
        self.stop()
        self.increase_speed(self._startup_scale)


    def turn_right(self, scale=0.5, period=1.):
        """
        Turn right. The if scale = 0, the turn is slow; if the scale = 1., the turn is sharp.
        """
        scale = min(scale, 1)
        scale = max(scale, 0)

        left_scale = scale
        right_scale = -(1 - scale)

        self._change_speed(left_scale, right_scale)
        self.stop()
        self.increase_speed(self._startup_scale)
                



    def start(self):
        """
        Start the engine. This function will start the two wheels. 

        Note:
            The wheels are wrapped in the ContinuousComponentWrapper and they are multiprocessing.Process.
        """

        self._left_wheel.start()
        self._right_wheel.start()

--END--

Raspberry Pi & Arch Linux - Day 13 - RawDataHandler

In this post, we will present the draft version of RawDataHandler.

Based on our design, all the physical devices is represented by a Component class and it is further wrapper in a ContinuousComponentWrapper class. The communication is always handled by a multiprocessing queue and the format is defined in each concrete component class. So it is very easy to come up with a RawDataHandler class that can handle all the data and messages from different component.

class RawDataHandler:
    def __init__(self, name=None, parser=None, record_size=100):
        assert name is not None and parser is not None
        self._name = name
        self._records     = []
        self._record_size = record_size
        self._parser = parser
        self._columns = [x[0] for x in self._parser]

    def update(self, msg):
        if len(self._records) == self._record_size:
            self._records.pop(0)

        # parse the msg
        record = []
        for attr, idx in self._parser:
            record.append(msg[idx])
        self._records.append(record)
    

    @property
    def data(self):
        df = pd.DataFrame(self._records, columns=self._columns)
        df['param_name'] = self._name
        return df

The most important parameter is the parser. It tells the handler how to extract information of different field from the message.

Now we have all the pieces ready and we can perform the first test of our small ultrasonic radar.

Here is the setup of the environment: we put one piece of paper in front of the radar and it forms a V-shape.

Here is the data generated by the distance sensor:

We do see a V-shape here (well...); however for some reasons, there is a gap at degree = -10. There might be two potential explanation for this phenomenon. The first one is the reflection, due the shape of the paper it is possible that the ultrasonic wave may propagate following the red line in the image. In this case, the path consists of three segments instead of two. Another possible reason is the fact that when the sensor sends and receives the signal, it is also spinning. In other words, between the time it sends the signal and it receives the signal, it will make a slight move and point to a different direction. Though this movement is relatively small because the speedo of motor is largely smaller than the speed of sound, it case it points to the corner of the V shape, the distance measured can be quite sensitive to the angle.

As we can see in the fig-1, on the left side the points lie on the a straight line as expected while on the right part we see some irregularity. I think this irregularity can be partly explained by the curvature of the paper.

Video time ^_^

--END--

Raspberry Pi & Arch Linux - Day 12 - Build Wheel Component

In this post, we will present the code of Wheel Component.

One of the interesting problems when we try to control a wheel is the fact that the left wheel  and right wheel are mirror symmetric. It means that we cannot use the same code to control the two wheels at the same time.

For example, let us assume that we have a Motor class and it has two methods: (1)spin_clockwisely and (2)spin_anti_clockwisely. If the Motor is used in the right wheel then spin_clockwisely function will make the robot move forward while the spin_anti_clockwisely function will make the robot move backward. It is easy to see if the Motor is used in the left wheel then the effect is opposite, meaning that the spin_clockwisely function will make the robot move backward and the spin_anti_clockwisely function will make the robot forward.

In our design, we do not want the Motor class to handle the mirror symmetry problem because the motor does not know if it is used in a wheel and our Component class is specifically designed to represent a physical device. Therefore, we will create a WheelComponent to control the motor used in a wheel.

class WheelComponent(Component):
    """
    Represent a single wheel.
    """

    def __init__(self, name=None, mirror=False, pin_signal=None, repeat=10, pulse=None, width=None):
        """
        Args:
            name:           the name of the component.
            mirror:         Bool. The left wheel and right wheel is a mirro image of each other. Therefore, with the same configuration and the 
                            same operaton the effect is opposite. For example, let assume we are in a scenario where when we increase the pulse, 
                            the motor spins faster clockwisely. If this motor is used for right wheel, when the pulse is increased, the robot will
                            be speed up; while if it is used for right wheel, the robot will be slowed down. This is a mirror effect. The mirror 
                            parameter is used to handle the mirror effet so we can have a unified interface to control both the left and right wheel.
            pin_signal:     the pin number for sending the pulse to the motor.
            pulse:          the pulse that is send to the motor. If the pulse is None,it will be set to the reference pulse of the underlying motor 
                            class, which make the motor still. The default value of pulse is None.
            repeat:         the number pulse sent to the motor in a cycle.
            width:          In the communication protocol, the signal consists of two parts: (1)pulse and (2)silence. The width specifies the length 
                            of the slient period. If the width is None, it will be set to the width value of the underlaying motor class.

        """

        assert name is not None
        assert pin_signal is not None
        self._name = name
        self._pin_signal = pin_signal
        self._motor = WheelMotor(pin_signal=self._pin_signal)
        self._reference_pulse = self._motor.reference_pulse
        self._max_deviation = self._motor.max_pulse_deviation
        self._max_pulse = self._reference_pulse + self._max_deviation
        self._min_pulse = self._reference_pulse - self._max_deviation

        self._pulse = pulse if pulse is not None else self._reference_pulse
        self._width = width if width is not None else self._motor.width
        self._repeat = repeat
        self._mirror = mirror


    def run(self):
        self._motor.generate_pulse(repeat=self._repeat,pulse=self._pulse, width=0.020)

    def send_msg(self,Q):
        pass

    @property
    def pulse(self):
        return self._pulse
    @pulse.setter
    def pulse(self, val):
        self._pulse = min(val, self._width)


    @property
    def repeat(self):
        return self._repeat
    @repeat.setter
    def repeat(self,val):
        self._repeat = min(val, 50)

    def increase_speed(self, scale):
        scale = min(scale, 1.)
        scale = max(scale, -1.)

        increment = scale *  self._max_deviation

        if self._mirror:
            increment = -1 * increment

        new_pulse = self.pulse + increment
        new_pulse = min(self._max_pulse, new_pulse)
        new_pulse = max(self._min_pulse, new_pulse)

        self._pulse = new_pulse

    def stop(self):
        self._pulse = self._reference_pulse

Note that in the __init__ function ,we have a parameter called mirror and it is used in the increase_speed method. This parameter indicates if we want to use the mirror effect of an operation. In case of increasing the speed of the rotation, it flips the sign of the incremental amount of pulse. In this way, we have a unified interface to control both the left and right wheel.

To use the component, we need to wrap it in a ContinuousComponentWrapper as we did before. Here is a sample code that shows how we can increase the speed of the wheel.

pin_signal_left = 13
pin_signal_right = 15

left_wheel_component  = WheelComponent(name='left_wheel', mirror=False, pin_signal=pin_signal_left, repeat=20, pulse=None, width=None)
right_wheel_component = WheelComponent(name='right_wheel', mirror=True, pin_signal=pin_signal_right, repeat=20, pulse=None, width=None)

cmd_Q_left_wheel     = mp.Queue()
output_Q_left_wheel  = mp.Queue()
cmd_Q_right_wheel    = mp.Queue()
output_Q_right_wheel = mp.Queue()


left_wheel  = ContinuousComponentWrapper(component=left_wheel_component,cmd_Q=cmd_Q_left_wheel,output_Q=output_Q_left_wheel)
right_wheel = ContinuousComponentWrapper(component=right_wheel_component,cmd_Q=cmd_Q_right_wheel,output_Q=output_Q_right_wheel)


left_wheel.start()
right_wheel.start()

scale = 0.
while True:
    print('scale: {}'.format(scale))
    time.sleep(3)
    cmd_Q_left_wheel.put(('increase_speed',(0.1,), {}))
    cmd_Q_right_wheel.put(('increase_speed', (0.1,), {}))
    scale += 0.1

--END--