Laser subscribe thread block "move" function

[Md Rakin Sarder]

I am testing manual controlling of Pepper using pynput and qibullet.
After I subscribe to the "Laser", it blocks other threads from pynput from executing.
I even tried a multithreading approach (you can ignore that approach because regardless of it the robot doesn't move)
Here is my code so far:

import cv2
import time
from qibullet import SimulationManager
from qibullet import PepperVirtual
from qibullet import Camera
from pynput import keyboard
import pybullet
import pybullet_data
import threading


class PepperSim:
    def __init__(self):
        self.keyEvents=[]
        self.simulation_manager = SimulationManager()
        self.client = self.simulation_manager.launchSimulation(gui=True)
        self.pepper = self.simulation_manager.spawnPepper(self.client, spawn_ground_plane=True)
        
        
        pybullet.setAdditionalSearchPath(pybullet_data.getDataPath())
        pybullet.loadURDF(
            "block.urdf",
            basePosition=[6, 0, 0.5],
            globalScaling=10.0,
            physicsClientId=self.client)
        
        
        self.startLaser()
        self.pepper.showLaser(True)
        laser_thread=threading.Thread(target=self.pepper.subscribeLaser())
        laser_thread.start()       
        

        self.pepper.goToPosture("Crouch", 0.6)
        time.sleep(1)
        self.pepper.goToPosture("Stand", 0.6)
        time.sleep(1)
        
        with keyboard.Listener(
                on_press=self.on_press,
                on_release=self.on_release) as self.listener:
            self.listener.join()

    

    def detectObject(self):
        laser_list = self.pepper.getRightLaserValue()
        laser_list.extend(self.pepper.getFrontLaserValue())
        laser_list.extend(self.pepper.getLeftLaserValue())
        
        if all(laser == 3.0 for laser in laser_list):
            print('0') 
            print(laser_list)
            # self.pepper.unsubscribeLaser()         
            return False
        else:
            print(laser_list)
            print('1') 
            # self.pepper.unsubscribeLaser()   
            return True    
        
    def on_press(self,key):
        if(key==keyboard.Key.up):
            if not(self.detectObject()):
                self.pepper.move(1,0,0)
        elif(key==keyboard.Key.left):
            if not(self.detectObject()):
                self.pepper.move(0,1,0)
        elif(key==keyboard.Key.right):
            if not(self.detectObject()):
                self.pepper.move(0,-1,0)
        elif(key==keyboard.Key.shift_r):
            if not(self.detectObject()):
                self.pepper.move(0,0,-1.0)
        elif(key==keyboard.Key.shift_l):
            if not(self.detectObject()):
                self.pepper.move(0,0,1.0)
        
    def on_release(self,key):
        self.pepper.stopMove()
        if (key == keyboard.Key.esc):
            # Stop listener
            pybullet.disconnect()
            self.simulation_manager.stopSimulation(self.client)
            

def main():
    PepperSim()

if __name__ == "__main__":
    main()

[Maxime Busy]

I tried your code, the robot is moving on my end... (after commenting self.startLaser()). The following implementation allows you to correctly clean up the simulation when you press escape:

import cv2
import time
from qibullet import SimulationManager
from qibullet import PepperVirtual
from qibullet import Camera
from pynput import keyboard
import pybullet
import pybullet_data
import threading


class PepperSim:
    def __init__(self):
        self.keyEvents = []
        self.simulation_manager = SimulationManager()
        self.client = self.simulation_manager.launchSimulation(gui=True)
        self.pepper = self.simulation_manager.spawnPepper(
            self.client,
            spawn_ground_plane=True)

        pybullet.setAdditionalSearchPath(pybullet_data.getDataPath())
        pybullet.loadURDF(
            "block.urdf",
            basePosition=[6, 0, 0.5],
            globalScaling=10.0,
            physicsClientId=self.client)

        self.pepper.goToPosture("Stand", 0.6)
        time.sleep(1)

        self.pepper.subscribeLaser()
        self.pepper.showLaser(True)

        with keyboard.Listener(
                on_press=self.on_press,
                on_release=self.on_release) as self.listener:
            self.listener.join()

        self.pepper.unsubscribeLaser()
        self.simulation_manager.stopSimulation(self.client)

    def detectObject(self):
        laser_list = self.pepper.getRightLaserValue()
        laser_list.extend(self.pepper.getFrontLaserValue())
        laser_list.extend(self.pepper.getLeftLaserValue())

        return not all(laser == 3.0 for laser in laser_list)

    def on_press(self, key):
        if key == keyboard.Key.up:
            if not self.detectObject():
                self.pepper.move(1, 0, 0)
        elif key == keyboard.Key.left:
            if not self.detectObject():
                self.pepper.move(0, 1, 0)
        elif key == keyboard.Key.right:
            if not self.detectObject():
                self.pepper.move(0, -1, 0)
        elif key == keyboard.Key.shift_r:
            if not self.detectObject():
                self.pepper.move(0, 0, -1.0)
        elif key == keyboard.Key.shift_l:
            if not self.detectObject():
                self.pepper.move(0, 0, 1.0)

    def on_release(self, key):
        self.pepper.stopMove()

        # Stop if escape is pressed
        if (key == keyboard.Key.esc):
            self.listener.stop()


def main():
    PepperSim()


if __name__ == "__main__":
    main()

But again, you should be able to see the robot moving when pressing keys

[Md Rakin Sarder]

Thank you for your update.
Initially, your implementation did not work.
However, I was curious regarding the fact that it worked in your case. I was running this code in my Windows OS. Later, I switched to my Ubuntu VM and it worked perfectly with navigation.

I assume there can be a conflict of the pynput library with qibullet in Windows environment.

[Maxime Busy]

I used Windows to run my implementation, everything runs smoothly... Are you using the latest version of qiBullet (1.4.3) ? Are you using Python 2 or 3 ? I assume that you are using Windows 10 ?

[Md Rakin Sarder]

Hi,

These are the following configurations (for my Windows environment):

• Windows 10
• Python 3.9.1
• qibullet==1.4.3
• pybullet==3.0.8

[Maxime Busy]

Hi, thanks for the details. I have the same setup (except for Python, 3.7.7) and can't reproduce this blocking behaviour... If I have some time I will eventually try to test that with Python 3.9, but it shouldn't make much of a difference.

In any case, that issue is really specific to the usage of pynput, I will close it for now. I will eventually re-open it if I can reproduce what you experienced with Python 3.9 and if the controller thread of Pepper is responsible for that