Issue: Basic Vacuum Cleaner. Delayed turning using yaw

Academy: Mobile Robotics
1

Greetings.

Working on the Basic Vacuum Cleaner, I’m using the attached code to turn the robot. I have an issue with it, where, only in some executions, the robot does not reduce the speed, even when the error is lower than the first threshold.
I have tried it in different computers, and it works correctly in one of them, so I don’t know if it is due a simulator problem.

import WebGUI
import HAL
import Frequency
import time
from math import pi, atan2, sin, cos, sqrt, isnan


def normalizar_angulo(angulo):
        return atan2(sin(angulo), cos(angulo))

def girar(direccion: str):

        objetivos = {
            "n": -pi / 2,
            "s": pi / 2,
            "e": pi,
            "o": 0
        }

        tiempo_inicial = HAL.getPose3d().timeStamp
        HAL.setV(-0.2)
        while abs(HAL.getPose3d().timeStamp - tiempo_inicial) < 0.3:
            Frequency.tick()
        HAL.setV(0)

        """Gira hacia un punto cardinal específico."""
        if direccion not in objetivos: return

        target = objetivos[direccion]
        error1 = 0.3
        error2 = 0.001

        while True:
            pos_actual = HAL.getPose3d().yaw

            diff = normalizar_angulo(target - pos_actual)

            abs_diff = abs(diff)

            if abs_diff <= error2:
                break

            if abs_diff > error1:
                HAL.setW(1.0 if diff > 0 else -1.0)
            else:
                HAL.setW(0.05 if diff > 0 else -0.05)

        HAL.setW(0)

while True:
    girar("n")
    girar("e")
    girar("s")
    girar("o")

Adding a print(abs_diff) inside the while loop makes it works in more situations in the computer that it normally fails.

:wrench: My computer specs:

  • Operating system: Windows 10 Home (64-bit)
  • Processor: Intel Core i5-10500H @ 2.50 GHz
  • RAM: 32 GB
  • GPU: NVIDIA 6 GB
  • Browser: Google Chrome

I would appreciate any help to fix the problem.
Thank you beforehand.

2

Hi Timm002,

This part of your code may caused some unwanted behavior. Instead of this I suggest using sleep.

And in this section you are not calling Frequency.tick() which can cause your application to behavior different between executions, which may explain your problem. Right now your code will try to execute as fast as possible, which will create inconsistencies.

Also note that you may need to send multiple times the command to stop the execution because with your current code you are overflowing the internal ROS buffer of speed commands.

        while True:
            pos_actual = HAL.getPose3d().yaw