Make USB device visible with different vendor and product ID - usb

I'm looking for a way to make a USB device show up as if it has different vendor and product IDs. I'm trying to make a proprietary piece of software to work with a USB device that should be supported but gets rejected solely because of its ID.
The software is for Windows, but I can run it in a VM in Linux. So I'll be fine with either approach, whatever works:
Changing USB ID in Linux
Changing USB ID in Windows
Making Qemu (or perhaps some other equivalent) change USB ID in passthrough

There may be a simpler way to do this, but I was faced with a similar problem and was able to create a process in which I could change the device descriptor information for development purposes. The process is summarized in this diagram:
First configure a static IP address for you Raspberry Pi and configure your PC ethernet TCP/IPv4 settings so you are able to communicate with you Raspberry Pi over the LAN connection.
Download the Virtual Here Raspberry Pi server and the client software for your PC from the Virtual Here website. The trial version will work for this use case.
Move the Virtual Here server software to you Raspberry Pi. In order to run the USB server you need to change the privileges of the file with $ sudo chmod +x vhusbdarm then run with $ sudo ./vhusbdarm.
Run the client software on your local machine. You will see that the client detects the USB device on the USB device server at <Your Raspberry Pi IP address>:7575. Connecting to the device at this point will give no advantage and mimic a direct connection.
Run the python file bellow, which was modified from a solution I found here, but utilizes Scapy sniff to capture the incoming packets before forwarding the raw data. The original script in the linked solution should work fine as well. In the script you can see that I used port 12345.
#!/usr/bin/env python
from scapy.all import *
import socket
import threading
import select
from queue import Queue
main_queue = Queue()
terminateAll = False
class ClientThread(threading.Thread):
def __init__(self, clientSocket, targetHost, targetPort):
threading.Thread.__init__(self)
self.__clientSocket = clientSocket
self.__targetHost = targetHost
self.__targetPort = targetPort
def run(self):
print("Client Thread started")
self.__clientSocket.setblocking(0)
targetHostSocket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
targetHostSocket.connect((self.__targetHost, self.__targetPort))
targetHostSocket.setblocking(0)
clientData = b""
targetHostData = b""
terminate = False
while not terminate and not terminateAll:
inputs = [self.__clientSocket, targetHostSocket]
outputs = []
if len(clientData) > 0:
outputs.append(self.__clientSocket)
if len(targetHostData) > 0:
outputs.append(targetHostSocket)
try:
inputsReady, outputsReady, errorsReady = select.select(inputs, outputs, [], 1.0)
except Exception as e:
print(e)
break
for inp in inputsReady:
if inp == self.__clientSocket:
try:
data = self.__clientSocket.recv(4096)
#print(data)
#data = b""
#while not main_queue.empty():
# data += main_queue.get()
except Exception as e:
print(e)
if data != None:
if len(data) > 0:
targetHostData += data
#else:
# terminate = True
elif inp == targetHostSocket:
try:
data = b""
while not main_queue.empty():
data += main_queue.get()
except Exception as e:
print(e)
if data != None:
if len(data) > 0:
clientData += data
for out in outputsReady:
if out == self.__clientSocket and len(clientData) > 0:
#pck = Ether(clientData)
#pck.show()
bytesWritten = self.__clientSocket.send(clientData)
if bytesWritten > 0:
clientData = clientData[bytesWritten:]
elif out == targetHostSocket and len(targetHostData) > 0:
#pck = Ether(targetHostData)
#pck.show()
bytesWritten = targetHostSocket.send(targetHostData)
if bytesWritten > 0:
targetHostData = targetHostData[bytesWritten:]
self.__clientSocket.close()
targetHostSocket.close()
print ("ClientThread terminating")
def handle_sniff(pck):
if IP in pck:
if pck[IP].src == "192.168.1.48":
if Raw in pck:
payload = pck[Raw].load
if b'\x12\x01\x00\x01\x00\x00\x00\x08$\x07\x04\x00\x88#\x01\x02\x00\x01' in payload:
payload = payload.replace(b'\x00\x08$\x07\x04\x00\x88#\x01\x02\x00', b'\x00\x08$\x07\x04\x00\x88\x15\x01\x02\x00')
print(payload)
main_queue.put(payload)
if __name__ == '__main__':
localHost = "localhost"
localPort = 12345
targetHost = "192.168.1.12"
targetPort = 7575
serverSocket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
serverSocket.bind((localHost, localPort))
serverSocket.listen(5)
print("Waiting for client...")
while True:
try:
clientSocket, address = serverSocket.accept()
except KeyboardInterrupt:
print("\nTerminating...")
terminateAll = True
break
print("starting client")
ClientThread(clientSocket, targetHost, targetPort).start()
sniff(iface="Ethernet", prn=lambda pck: handle_sniff(pck))
serverSocket.close()
Once the script is running, configure the Virtual Here client to listen for USB servers at localhost:12345. The handle_sniff function is where the USB device descriptor information is being changed. Once connected you should be able to double click on the USB device in the dropdown tree. You will see the USB data begin to be printed in the your python console.
In the above example I changed the device bcdDevice bytes of the USB Descriptor.
Another helpful script I used to identify the packet that contained the information I was targeting is below. I modified a script I found in this solution. It is modified to print the raw data along with the unpacked device descriptor information, which can then be searched for in the TCP raw data to identify which bytes to replace.
#!/usr/bin/env python
from __future__ import print_function
import argparse
import string
import struct
import sys
import win32api
import win32file
import pywintypes
BUFF=b""
def CTL_CODE(DeviceType, Function, Method, Access):
return (DeviceType << 16) | (Access << 14) | (Function << 2) | Method
def USB_CTL(id):
# CTL_CODE(FILE_DEVICE_USB, (id), METHOD_BUFFERED, FILE_ANY_ACCESS)
return CTL_CODE(0x22, id, 0, 0)
IOCTL_USB_GET_ROOT_HUB_NAME = USB_CTL(258) # HCD_GET_ROOT_HUB_NAME
IOCTL_USB_GET_NODE_INFORMATION = USB_CTL(258) # USB_GET_NODE_INFORMATION
IOCTL_USB_GET_NODE_CONNECTION_INFORMATION = USB_CTL(259) # USB_GET_NODE_CONNECTION_INFORMATION
IOCTL_USB_GET_NODE_CONNECTION_DRIVERKEY_NAME = USB_CTL(264) # USB_GET_NODE_CONNECTION_DRIVERKEY_NAME
IOCTL_USB_GET_NODE_CONNECTION_NAME = USB_CTL(261) # USB_GET_NODE_CONNECTION_NAME
IOCTL_USB_GET_DESCRIPTOR_FROM_NODE_CONNECTION = USB_CTL(260) # USB_GET_DESCRIPTOR_FROM_NODE_CONNECTION
USB_CONFIGURATION_DESCRIPTOR_TYPE = 2
USB_STRING_DESCRIPTOR_TYPE = 3
USB_INTERFACE_DESCRIPTOR_TYPE = 4
MAXIMUM_USB_STRING_LENGTH = 255
def open_dev(name):
try:
handle = win32file.CreateFile(name,
win32file.GENERIC_WRITE,
win32file.FILE_SHARE_WRITE,
None,
win32file.OPEN_EXISTING,
0,
None)
except pywintypes.error as e:
return None
return handle
def get_root_hub_name(handle):
buf = win32file.DeviceIoControl(handle,
IOCTL_USB_GET_ROOT_HUB_NAME,
None,
6,
None)
act_len, _ = struct.unpack('LH', buf)
buf = win32file.DeviceIoControl(handle,
IOCTL_USB_GET_ROOT_HUB_NAME,
None,
act_len,
None)
return buf[4:].decode('utf-16le')
def get_driverkey_name(handle, index):
key_name = bytes(chr(index) + '\0'*9, 'utf-8')
try:
buf = win32file.DeviceIoControl(handle,
IOCTL_USB_GET_NODE_CONNECTION_DRIVERKEY_NAME,
key_name,
10,
None)
except pywintypes.error as e:
print(e.strerror, index)
sys.exit(1)
_, act_len, _ = struct.unpack('LLH', buf)
buf = win32file.DeviceIoControl(handle,
IOCTL_USB_GET_NODE_CONNECTION_DRIVERKEY_NAME,
key_name,
act_len,
None)
return buf[8:].decode('utf-16le')
def get_ext_hub_name(handle, index):
hub_name = chr(index) + '\0'*9
buf = win32file.DeviceIoControl(handle,
IOCTL_USB_GET_NODE_CONNECTION_NAME,
hub_name,
10,
None)
_, act_len, _ = struct.unpack('LLH', buf)
buf = win32file.DeviceIoControl(handle,
IOCTL_USB_GET_NODE_CONNECTION_NAME,
hub_name,
act_len,
None)
return buf[8:].decode('utf-16le')
def get_str_desc(handle, conn_idx, str_idx):
req = struct.pack('LBBHHH',
conn_idx,
0,
0,
(USB_STRING_DESCRIPTOR_TYPE<<8) | str_idx,
win32api.GetSystemDefaultLangID(),
MAXIMUM_USB_STRING_LENGTH)
try:
buf = win32file.DeviceIoControl(handle,
IOCTL_USB_GET_DESCRIPTOR_FROM_NODE_CONNECTION,
req,
12+MAXIMUM_USB_STRING_LENGTH,
None)
except pywintypes.error as e:
return 'ERROR: no String Descriptor for index {}'.format(str_idx)
if len(buf) > 16:
return buf[14:].decode('utf-16le')
return ''
def exam_hub(name, verbose, level):
handle = open_dev(r'\\.\{}'.format(name))
if not handle:
print('Failed to open device {}'.format(name))
return
buf = win32file.DeviceIoControl(handle,
IOCTL_USB_GET_NODE_INFORMATION,
None,
76,
None)
print_hub_ports(handle, ord(buf[6]), verbose, level)
handle.close()
def print_str_or_hex(to_be_print):
if all(c in string.printable for c in to_be_print):
print('"{}"'.format(to_be_print))
return
print('Hex: ', end='')
for x in to_be_print:
print('{:02x} '.format(ord(x)), end='')
print('')
def print_hub_ports(handle, num_ports, verbose, level):
print(handle, num_ports, verbose, level)
for idx in range(1, num_ports+1):
info = bytes(chr(idx) + '\0'*34, 'utf-8')
try:
buf = win32file.DeviceIoControl(handle,
IOCTL_USB_GET_NODE_CONNECTION_INFORMATION,
info,
34 + 11*30,
None)
except pywintypes.error as e:
print(e)
print(e.winerror, e.funcname, e.strerror)
return
print(buf)
_, vid, pid, vers, manu, prod, seri, _, ishub, _, stat = struct.unpack('=12sHHHBBB3s?6sL', buf[:35])
if ishub:
if verbose:
print('{} [Port{}] {}'.format(' '*level, idx, 'USB Hub'))
exam_hub(get_ext_hub_name(handle, idx), verbose, level)
elif stat == 0 and verbose:
print('{} [Port{}] {}'.format(' '*level, idx, 'NoDeviceConnected'))
elif stat == 1:
if verbose or (manu != 0 or prod != 0 or seri != 0):
print('{} [Port{}] {}'.format(' '*level, idx, get_driverkey_name(handle, idx)))
print('{} Vendor ID: 0x{:04X}'.format(' '*level, vid))
print('{} Product ID: 0x{:04X}'.format(' '*level, pid))
print('{} Device BCD: 0x{:04X}'.format(' '*level, vers))
print(vers)
if manu != 0:
print('{} Manufacturer (0x{:x}) -> '.format(' '*level, manu), end='')
print_str_or_hex(get_str_desc(handle, idx, manu))
if prod != 0:
print('{} Product (0x{:x}) -> '.format(' '*level, prod), end='')
print_str_or_hex(get_str_desc(handle, idx, prod))
if seri != 0:
print('{} Serial No (0x{:x}) -> '.format(' '*level, seri), end='')
print_str_or_hex(get_str_desc(handle, idx, seri))
def main():
parser = argparse.ArgumentParser()
parser.add_argument('-v', '--verbose', action='store_true',
help="Increase output verbosity.")
args = parser.parse_args()
for i in range(10):
name = r"\\.\HCD{}".format(i)
handle = open_dev(name)
if not handle:
continue
root = get_root_hub_name(handle)
print('{}RootHub: {}'.format('\n' if i != 0 else '', root))
dev_name = r'\\.\{}'.format(root)
dev_handle = open_dev(dev_name)
if not dev_handle:
print('Failed to open device {}'.format(dev_name))
continue
buf = win32file.DeviceIoControl(dev_handle,
IOCTL_USB_GET_NODE_INFORMATION,
None,
76,
None)
global BUFF
BUFF=buf
print_hub_ports(dev_handle, buf[6], args.verbose, 0)
dev_handle.close()
handle.close()
if __name__ == '__main__':
main()
P.S. This is also really helpful for filtering and modifying any of the USB data being transferred not just the device descriptor.

Related

MicroPython Thread not exiting (Pi Pico)

I'm attempting to write a fairly simple class to handle some relays for a robot costume. The user should be able to push some buttons to activate different sets of lights/EL wire. A basic thread seemed the best way to handle this, but maybe I'm missing something about the micropython implementation...
Here's the relevant two functions:
def run(self):
"""Start the default state"""
self.current_state.start_new_thread(self.activate_emotion, (self.state,))
def change_state(self):
"""Kill the old state (if applicable) and start a new one"""
print('Exiting thread...')
self.current_state.exit()
print('Starting new thread...')
self.current_state.start_new_thread(self.activate_emotion, (self.state,))
print('Done.')
When my script starts, run() throws "OSError: core1 in use", but the "default" state begins to run. It does this even after a fresh startup. When my button press is detected and activates change_state(), I get the output: "Exiting thread...", and it just hangs there indefinitely. What am I missing here? Any help would be greatly appreciated.
Here's the entirety of my script:
from machine import Pin
import utime
import _thread
import random
class LowRelay(Pin):
def turn_on(self):
self.low()
def turn_off(self):
self.high()
def test(self):
self.turn_on()
utime.sleep(0.5)
self.turn_off()
class Ariel():
def __init__(self):
self.happy_button = Pin(10, Pin.IN, Pin.PULL_DOWN)
self.sad_button = Pin(11, Pin.IN, Pin.PULL_DOWN)
self.scared_button = Pin(12, Pin.IN, Pin.PULL_DOWN)
self.thinking_button = Pin(13, Pin.IN, Pin.PULL_DOWN)
self.happy_relay = LowRelay(2, Pin.OUT)
self.sad_relay = LowRelay(3, Pin.OUT)
self.scared_relay = LowRelay(4, Pin.OUT)
self.thinking_relay = LowRelay(5, Pin.OUT)
self.group1_relay = LowRelay(6, Pin.OUT)
self.group2_relay = LowRelay(7, Pin.OUT)
self.group3_relay = LowRelay(8, Pin.OUT)
self.group4_relay = LowRelay(9, Pin.OUT)
self.led_groups = ['group1', 'group2',
'group3', 'group4']
self.off_led_groups = ['group1', 'group2',
'group3', 'group4']
self.on_led_groups = []
self.state = 'default'
self.current_state = _thread
def full_test(self):
""" Self test """
print('Testing....')
self.group1_relay.test()
self.group2_relay.test()
self.group3_relay.test()
self.group4_relay.test()
self.happy_relay.test()
self.sad_relay.test()
self.scared_relay.test()
self.thinking_relay.test()
print('Test complete.')
utime.sleep(1)
def test(self):
"""Quick Test"""
self.all_relays_off()
utime.sleep(1)
self.all_relays_on()
utime.sleep(1)
self.all_relays_off()
def run(self):
"""Start the default state"""
self.current_state.start_new_thread(self.activate_emotion, (self.state,))
def change_state(self):
"""Kill the old state (if applicable) and start a new one"""
print('Exiting thread...')
self.current_state.exit()
print('Starting new thread...')
self.current_state.start_new_thread(self.activate_emotion, (self.state,))
print('Done.')
def watch_and_wait(self, seconds):
"""Sleep while an effect processes, watching for a state change."""
start_state = self.state
ticks = int(float(seconds)/0.25)
for x in range(ticks):
self.check_buttons()
if self.state != start_state:
print('Changing state...')
self.change_state()
continue
utime.sleep(0.25)
def check_buttons(self):
"""Set states based on button presses"""
if self.happy_button.value():
self.state = 'happy'
if self.sad_button.value():
self.state = 'sad'
if self.scared_button.value():
self.state = 'scared'
if self.thinking_button.value():
self.state = 'thinking'
def swap_groups(self):
"""
Pick one of the two LED groups that is not on, and one that is.
Swap them. If none are on, turn on two.
"""
off_lg = self.off_led_groups
on_lg = self.on_led_groups
on_candidate = off_lg.pop(random.randrange(len(off_lg)))
off_candidate = on_lg.pop(random.randrange(len(on_lg))) if on_lg else None
on_lg.append(on_candidate)
if off_candidate:
off_lg.append(off_candidate)
if len(on_lg) < 2:
on_lg.append(off_lg.pop(random.randrange(len(off_lg))))
for x in self.led_groups:
r = getattr(self, '%s_relay' % x)
if x in on_lg:
r.turn_on()
else:
r.turn_off()
def all_relays_on(self):
"""Turn off all relays"""
self.happy_relay.turn_on()
self.sad_relay.turn_on()
self.scared_relay.turn_on()
self.thinking_relay.turn_on()
self.group1_relay.turn_on()
self.group2_relay.turn_on()
self.group3_relay.turn_on()
self.group4_relay.turn_on()
def all_relays_off(self):
"""Turn off all relays"""
self.happy_relay.turn_off()
self.sad_relay.turn_off()
self.scared_relay.turn_off()
self.thinking_relay.turn_off()
self.group1_relay.turn_off()
self.group2_relay.turn_off()
self.group3_relay.turn_off()
self.group4_relay.turn_off()
####################
# define states
# default - turn off EL wire, swap LED groups every second
# emotions - turn off other LEDs, and turn on relevant EL wire
####################
def activate_emotion(self, emotion):
"""Activates the selected emotional state"""
print('Entering %s state...' % emotion)
self.all_relays_off()
if self.state == 'default':
while self.state == 'default':
self.swap_groups()
self.watch_and_wait(1)
else:
r = getattr(self, '%s_relay' % emotion)
r.turn_on()
self.watch_and_wait(5)
self.state = 'default'
print('Changing state to default.')
self.change_state()
ariel = Ariel()
ariel.test()
while True:
ariel.run()
pass

Pybullet on colab, cannot connect X server

I am using rl-baselines-zoo 3 to run ddpg with my custom env on colab. After I used show video function in that zoo repo, it said it cannot connect to the server. It works fine on other built-in envs, so I guess it's my env problem. please, need some help...
I set every thing from zoo's tutorials
Traceback:
pybullet build time: Jul 12 2021 20:46:20
/usr/local/lib/python3.7/dist-packages/gym/logger.py:30: UserWarning:
WARN: Box bound precision lowered by casting to float32
startThreads creating 1 threads.
starting thread 0
started thread 0
argc=2
argv[0] = --unused
argv[1] = --start_demo_name=Physics Server
ExampleBrowserThreadFunc started
X11 functions dynamically loaded using dlopen/dlsym OK!
X11 functions dynamically loaded using dlopen/dlsym OK!
Creating context
Created GL 3.3 context
Direct GLX rendering context obtained
Making context current
GL_VENDOR=VMware, Inc.
GL_RENDERER=llvmpipe (LLVM 10.0.0, 256 bits)
GL_VERSION=3.3 (Core Profile) Mesa 20.0.8
GL_SHADING_LANGUAGE_VERSION=3.30
pthread_getconcurrency()=0
Version = 3.3 (Core Profile) Mesa 20.0.8
Vendor = VMware, Inc.
Renderer = llvmpipe (LLVM 10.0.0, 256 bits)
b3Printf: Selected demo: Physics Server
startThreads creating 1 threads.
starting thread 0
started thread 0
MotionThreadFunc thread started
ven = VMware, Inc.
ven = VMware, Inc.
Wrapping the env in a VecTransposeImage.
tcmalloc: large alloc 3276800000 bytes == 0x556b03bda000 # 0x7f7cad04a001 0x7f7caa3f554f 0x7f7caa445b58 0x7f7caa449b17 0x7f7caa4e8203 0x556a81194d54 0x556a81194a50 0x556a81209105 0x556a812037ad 0x556a81196c9f 0x556a811d7d79 0x556a811d4cc4 0x556a81196ea1 0x556a81205bb5 0x556a8119630a 0x556a812087f0 0x556a812037ad 0x556a811963ea 0x556a8120460e 0x556a812034ae 0x556a811963ea 0x556a8120532a 0x556a812034ae 0x556a812031b3 0x556a81201660 0x556a81194b59 0x556a81194a50 0x556a81208453 0x556a812034ae 0x556a811963ea 0x556a812043b5
tcmalloc: large alloc 3276800000 bytes == 0x556bc78da000 # 0x7f7cad04a001 0x7f7caa3f554f 0x7f7caa445b58 0x7f7caa449b17 0x7f7caa4e8203 0x556a81194d54 0x556a81194a50 0x556a81209105 0x556a812037ad 0x556a81196c9f 0x556a811d7d79 0x556a811d4cc4 0x556a81196ea1 0x556a81205bb5 0x556a8119630a 0x556a812087f0 0x556a812037ad 0x556a811963ea 0x556a8120460e 0x556a812034ae 0x556a811963ea 0x556a8120532a 0x556a812034ae 0x556a812031b3 0x556a81201660 0x556a81194b59 0x556a81194a50 0x556a81208453 0x556a812034ae 0x556a811963ea 0x556a812043b5
/content/gdrive/My Drive/hsr/rl-baselines3-zoo/logs/ddpg/FoodHuntingHSR-v0_3/videos/final-model-ddpg-FoodHuntingHSR-v0-step-0-to-step-200.mp4
/usr/local/lib/python3.7/dist-packages/gym/logger.py:30: UserWarning:
WARN: Tried to pass invalid video frame, marking as broken: Your frame has data type float32, but we require uint8 (i.e. RGB values from 0-255).
Saving video to /content/gdrive/My Drive/hsr/rl-baselines3-zoo/logs/ddpg/FoodHuntingHSR-v0_3/videos/final-model-ddpg-FoodHuntingHSR-v0-step-0-to-step-200.mp4
numActiveThreads = 0
stopping threads
destroy semaphore
semaphore destroyed
Thread with taskId 0 exiting
Thread TERMINATED
destroy main semaphore
main semaphore destroyed
finished
numActiveThreads = 0
btShutDownExampleBrowser stopping threads
Thread with taskId 0 exiting
Thread TERMINATED
destroy semaphore
semaphore destroyed
destroy main semaphore
main semaphore destroyed
Exception ignored in: <function VecVideoRecorder.__del__ at 0x7f7c2b5cc200>
Traceback (most recent call last):
File "/content/gdrive/My Drive/hsr/stable-baselines3/stable_baselines3/common/vec_env/vec_video_recorder.py", line 114, in __del__
File "/content/gdrive/My Drive/hsr/stable-baselines3/stable_baselines3/common/vec_env/vec_video_recorder.py", line 110, in close
File "/content/gdrive/My Drive/hsr/stable-baselines3/stable_baselines3/common/vec_env/base_vec_env.py", line 278, in close
File "/content/gdrive/My Drive/hsr/stable-baselines3/stable_baselines3/common/vec_env/dummy_vec_env.py", line 67, in close
File "/content/gdrive/My Drive/hsr/stable-baselines3/stable_baselines3/common/monitor.py", line 113, in close
File "/usr/local/lib/python3.7/dist-packages/gym/core.py", line 243, in close
File "/usr/local/lib/python3.7/dist-packages/gym/core.py", line 243, in close
File "/content/gdrive/My Drive/hsr/PyLIS/gym-foodhunting/gym_foodhunting/foodhunting/gym_foodhunting.py", line 538, in close
pybullet.error: Not connected to physics server
class FoodHuntingEnv(gym.Env):
metadata = {'render.modes': ['human','rgb_array']}
GRAVITY = -10.0
BULLET_STEPS = 120 # p.setTimeStep(1.0 / 240.0), so 1 gym step == 0.5 sec.
def __init__(self, render=False, robot_model=R2D2, max_steps=500, num_foods=3, num_fakes=0, object_size=1.0, object_radius_scale=1.0, object_radius_offset=1.0, object_angle_scale=1.0):
"""Initialize environment.
"""
### gym variables
self.observation_space = robot_model.getObservationSpace() # classmethod
self.action_space = robot_model.getActionSpace() # classmethod
self.reward_range = (-1.0, 1.0)
self.seed()
### pybullet settings
self.ifrender = render
self.physicsClient = p.connect(p.GUI if self.ifrender else p.DIRECT)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
### env variables
self.robot_model = robot_model
self.max_steps = max_steps
self.num_foods = num_foods
self.num_fakes = num_fakes
self.object_size = object_size
self.object_radius_scale = object_radius_scale
self.object_radius_offset = object_radius_offset
self.object_angle_scale = object_angle_scale
self.plane_id = None
self.robot = None
self.object_ids = []
### episode variables
self.steps = 0
self.episode_rewards = 0.0
def close(self):
"""Close environment.
"""
p.disconnect(self.physicsClient)
def reset(self):
"""Reset environment.
"""
self.steps = 0
self.episode_rewards = 0
p.resetSimulation()
# p.setTimeStep(1.0 / 240.0)
p.setGravity(0, 0, self.GRAVITY)
self.plane_id = p.loadURDF('plane.urdf')
self.robot = self.robot_model()
self.object_ids = []
for i, (pos, orn) in enumerate(self._generateObjectPositions(num=(self.num_foods+self.num_fakes), radius_scale=self.object_radius_scale, radius_offset=self.object_radius_offset, angle_scale=self.object_angle_scale)):
if i < self.num_foods:
urdfPath = 'food_sphere.urdf'
else:
urdfPath = 'food_cube.urdf'
object_id = p.loadURDF(urdfPath, pos, orn, globalScaling=self.object_size)
self.object_ids.append(object_id)
for i in range(self.BULLET_STEPS):
p.stepSimulation()
obs = self._getObservation()
#print('reset laile')
#self.robot.printAllJointInfo()
return obs
def step(self, action):
"""Apply action to environment, then return observation and reward.
"""
self.steps += 1
self.robot.setAction(action)
reward = -1.0 * float(self.num_foods) / float(self.max_steps) # so agent needs to eat foods quickly
for i in range(self.BULLET_STEPS):
p.stepSimulation()
reward += self._getReward()
self.episode_rewards += reward
obs = self._getObservation()
done = self._isDone()
pos, orn = self.robot.getPositionAndOrientation()
info = { 'steps': self.steps, 'pos': pos, 'orn': orn }
if done:
#print('Done laile')
info['episode'] = { 'r': self.episode_rewards, 'l': self.steps }
# print(self.episode_rewards, self.steps)
#print(self.robot.getBaseRollPosition(), self.robot.getTorsoLiftPosition(), self.robot.getHeadPosition(), self.robot.getArmPosition(), self.robot.getWristPosition(), self.robot.getGripperPosition()) # for HSR debug
#print(self.robot.getHeadPosition(), self.robot.getGripperPosition()) # for R2D2 debug
return obs, reward, done, info
def render(self, mode='human', close=False):
"""This is a dummy function. This environment cannot control rendering timing.
"""
if mode != 'rgb_array':
return np.array([])
return self._getObservation()
def seed(self, seed=None):
"""Set random seed.
"""
self.np_random, seed = seeding.np_random(seed)
return [seed]
def _getReward(self):
"""Detect contact points and return reward.
"""
reward = 0
contacted_object_ids = [ object_id for object_id in self.object_ids if self.robot.isContact(object_id) ]
for object_id in contacted_object_ids:
reward += 1 if self._isFood(object_id) else -1
p.removeBody(object_id)
self.object_ids.remove(object_id)
return reward
def _getObservation(self):
"""Get observation.
"""
obs = self.robot.getObservation()
return obs
def _isFood(self, object_id):
"""Check if object_id is a food.
"""
baseLink, urdfPath = p.getBodyInfo(object_id)
return urdfPath == b'food_sphere.urdf' # otherwise, fake
def _isDone(self):
"""Check if episode is done.
"""
#print(self.object_ids,'self')
available_object_ids = [ object_id for object_id in self.object_ids if self._isFood(object_id) ]
#print(available_object_ids)
return self.steps >= self.max_steps or len(available_object_ids) <= 0
def _generateObjectPositions(self, num=1, retry=100, radius_scale=1.0, radius_offset=1.0, angle_scale=1.0, angle_offset=0.5*np.pi, z=1.5, near_distance=0.5):
"""Generate food positions randomly.
"""
def genPos():
r = radius_scale * self.np_random.rand() + radius_offset
a = -np.pi * angle_scale + angle_offset
b = np.pi * angle_scale + angle_offset
ang = (b - a) * self.np_random.rand() + a
return np.array([r * np.sin(ang), r * np.cos(ang), z])
def isNear(pos, poss):
for p, o in poss:
if np.linalg.norm(p - pos) < near_distance:
return True
return False
def genPosRetry(poss):
for i in range(retry):
pos = genPos()
if not isNear(pos, poss):
return pos
return genPos()
poss = []
for i in range(num):
pos = genPosRetry(poss)
orn = p.getQuaternionFromEuler([0.0, 0.0, 2.0*np.pi*self.np_random.rand()])
poss.append((pos, orn))
return poss

Trying to take pictures with Coral camera with Coral edgeTPU dev board but it is really slow

To start with, I am not a developer, but a mere automation engineer that have worked a bit with coding in Java, python, C#, C++ and C.
I am trying to make a prototype that take pictures and stores them using a digital pin on the board. Atm I can take pictures using a switch, but it is really slow(around 3 seconds pr image).
My complete system is going to be like this:
A product passes by on a conveyor and a photo cell triggers the board to take an image and store it. If an operator removes a product(because of bad quality) the image is stored in a different folder.
I started with the snapshot function shipped with Mendel and have tried to get rid off the overhead, but the Gstream and pipeline-stuff confuses me a lot.
If someone could help me with how to understand the supplied code, or how to write a minimalistic solution to take an image i would be grateful :)
I have tried to understand and use project-teachable and examples-camera from Google coral https://github.com/google-coral, but with no luck. I have had the best luck with the snapshot tool that uses snapshot.py that are referenced here https://coral.withgoogle.com/docs/camera/datasheet/#snapshot-tool
from periphery import GPIO
import time
import argparse
import contextlib
import fcntl
import os
import select
import sys
import termios
import threading
import gi
gi.require_version('Gst', '1.0')
gi.require_version('GstBase', '1.0')
from functools import partial
from gi.repository import GLib, GObject, Gst, GstBase
from PIL import Image
GObject.threads_init()
Gst.init(None)
WIDTH = 2592
HEIGHT = 1944
FILENAME_PREFIX = 'img'
FILENAME_SUFFIX = '.png'
AF_SYSFS_NODE = '/sys/module/ov5645_camera_mipi_v2/parameters/ov5645_af'
CAMERA_INIT_QUERY_SYSFS_NODE = '/sys/module/ov5645_camera_mipi_v2/parameters/ov5645_initialized'
HDMI_SYSFS_NODE = '/sys/class/drm/card0/card0-HDMI-A-1/status'
# No of initial frames to throw away before camera has stabilized
SCRAP_FRAMES = 1
SRC_WIDTH = 2592
SRC_HEIGHT = 1944
SRC_RATE = '15/1'
SRC_ELEMENT = 'v4l2src'
SINK_WIDTH = 2592
SINK_HEIGHT = 1944
SINK_ELEMENT = ('appsink name=appsink sync=false emit-signals=true '
'max-buffers=1 drop=true')
SCREEN_SINK = 'glimagesink sync=false'
FAKE_SINK = 'fakesink sync=false'
SRC_CAPS = 'video/x-raw,format=YUY2,width={width},height={height},framerate={rate}'
SINK_CAPS = 'video/x-raw,format=RGB,width={width},height={height}'
LEAKY_Q = 'queue max-size-buffers=1 leaky=downstream'
PIPELINE = '''
{src_element} ! {src_caps} ! {leaky_q} ! tee name=t
t. ! {leaky_q} ! {screen_sink}
t. ! {leaky_q} ! videoconvert ! {sink_caps} ! {sink_element}
'''
def on_bus_message(bus, message, loop):
t = message.type
if t == Gst.MessageType.EOS:
loop.quit()
elif t == Gst.MessageType.WARNING:
err, debug = message.parse_warning()
sys.stderr.write('Warning: %s: %s\n' % (err, debug))
elif t == Gst.MessageType.ERROR:
err, debug = message.parse_error()
sys.stderr.write('Error: %s: %s\n' % (err, debug))
loop.quit()
return True
def on_new_sample(sink, snapinfo):
if not snapinfo.save_frame():
# Throw away the frame
return Gst.FlowReturn.OK
sample = sink.emit('pull-sample')
buf = sample.get_buffer()
result, mapinfo = buf.map(Gst.MapFlags.READ)
if result:
imgfile = snapinfo.get_filename()
caps = sample.get_caps()
width = WIDTH
height = HEIGHT
img = Image.frombytes('RGB', (width, height), mapinfo.data, 'raw')
img.save(imgfile)
img.close()
buf.unmap(mapinfo)
return Gst.FlowReturn.OK
def run_pipeline(snapinfo):
src_caps = SRC_CAPS.format(width=SRC_WIDTH, height=SRC_HEIGHT, rate=SRC_RATE)
sink_caps = SINK_CAPS.format(width=SINK_WIDTH, height=SINK_HEIGHT)
screen_sink = FAKE_SINK
pipeline = PIPELINE.format(
leaky_q=LEAKY_Q,
src_element=SRC_ELEMENT,
src_caps=src_caps,
sink_caps=sink_caps,
sink_element=SINK_ELEMENT,
screen_sink=screen_sink)
pipeline = Gst.parse_launch(pipeline)
appsink = pipeline.get_by_name('appsink')
appsink.connect('new-sample', partial(on_new_sample, snapinfo=snapinfo))
loop = GObject.MainLoop()
# Set up a pipeline bus watch to catch errors.
bus = pipeline.get_bus()
bus.add_signal_watch()
bus.connect('message', on_bus_message, loop)
# Connect the loop to the snaphelper
snapinfo.connect_loop(loop)
# Run pipeline.
pipeline.set_state(Gst.State.PLAYING)
try:
loop.run()
except:
pass
# Clean up.
pipeline.set_state(Gst.State.NULL)
while GLib.MainContext.default().iteration(False):
pass
class SnapHelper:
def __init__(self, sysfs, prefix='img', oneshot=True, suffix='jpg'):
self.prefix = prefix
self.oneshot = oneshot
self.suffix = suffix
self.snap_it = oneshot
self.num = 0
self.scrapframes = SCRAP_FRAMES
self.sysfs = sysfs
def get_filename(self):
while True:
filename = self.prefix + str(self.num).zfill(4) + '.' + self.suffix
self.num = self.num + 1
if not os.path.exists(filename):
break
return filename
#def check_af(self):
#try:
# self.sysfs.seek(0)
# v = self.sysfs.read()
# if int(v) != 0x10:
# print('NO Focus')
#except:
# pass
# def refocus(self):
# try:#
# self.sysfs.write('1')
# self.sysfs.flush()
# except:
# pass
def save_frame(self):
# We always want to throw away the initial frames to let the
# camera stabilize. This seemed empirically to be the right number
# when running on desktop.
if self.scrapframes > 0:
self.scrapframes = self.scrapframes - 1
return False
if self.snap_it:
self.snap_it = False
retval = True
else:
retval = False
if self.oneshot:
self.loop.quit()
return retval
def connect_loop(self, loop):
self.loop = loop
def take_picture(snap):
start_time = int(round(time.time()))
run_pipeline(snap)
print(time.time()- start_time)
def main():
button = GPIO(138, "in")
last_state = False
with open(AF_SYSFS_NODE, 'w+') as sysfs:
snap = SnapHelper(sysfs, 'test', 'oneshot', 'jpg')
sysfs.write('2')
while 1:
button_state = button.read()
if(button_state==True and last_state == False):
snap = SnapHelper(sysfs, 'test', 'oneshot', 'jpg')
take_picture(snap)
last_state = button_state
if __name__== "__main__":
main()
sys.exit()
Output is what i expect, but it is slow.
I switched to a USB-webcam and used the pygame library instead.

Twisted will not send data back only if I use async DB ops

After struggling with inlineCallbacks and yield of twisted/txredisapi, I can save my data into redis. Thanks to author of txredisapi. Now I met a new issue, socket server will not send back to client before/after saving into DB.
Twisted offers simple socket server as following:
from twisted.internet import protocol, reactor
class Echo(protocol.Protocol):
def dataReceived(self, data):
self.transport.write(data) ### write back
class EchoFactory(protocol.Factory):
def buildProtocol(self, addr):
return Echo()
reactor.listenTCP(8000, EchoFactory)
recctor.run()
My code is similiar, only with additional DB ops.
#!/usr/bin/env python
import time
import binascii
import txredisapi
from twisted.internet import defer
from twisted.internet import protocol, reactor
from twisted.internet.protocol import Factory
from twisted.enterprise import adbapi
from twisted.python import log
from dmpack import Dmpack
from dmdb import Dmdb
from dmconfig import DmConf
dm = Dmpack()
conf = DmConf().loadConf()
rcs = txredisapi.lazyConnection(password=conf['RedisPassword'])
dbpool = adbapi.ConnectionPool("MySQLdb",db=conf['DbName'],user=conf['DbAccount'],\
passwd=conf['DbPassword'],host=conf['DbHost'],\
use_unicode=True,charset=conf['DbCharset'])
def getDataParsed(data):
realtime = None
period = None
self.snrCode = dm.snrToAscii(data[2:7])
realtime = data[7:167] # save it into redis
period = data[167:-2] # save it into SQL
return (snrCode, realtime, period)
class PlainTCP(protocol.Protocol):
def __init__(self, factory):
self.factory = factory
self.factory.numConnections = 0
self.snrCode = None
self.rData = None
self.pData = None
self.err = None
def connectionMade(self):
self.factory.numConnections += 1
print "Nr. of connections: %d\n" %(self.factory.numConnections)
self.transport.write("Hello remote\r\n") # it only prints very 5 connections.
def connectionLost(self, reason):
self.factory.numConnections -= 1
print "Nr. of connections: %d\n" %(self.factory.numConnections)
#defer.inlineCallbacks
def dataReceived(self, data):
global dbpool, rcs
(self.snrCode,rDat,pDat) = getDataParsed(data)
if self.snrCode == None or rDat == None or pDat == None:
err = "Bad format"
else:
err = "OK"
print "err:%s"%(err) # debug print to show flow control
self.err = err
self.transport.write(self.snrCode)
self.transport.write(self.err)
self.transport.write(rDat)
self.transport.write(pDat)
self.transport.loseConnection()
if self.snrCode != None and rDat != None and pDat != None:
res = yield self.saveRealTimeData(rcs, rDat)
res = yield self.savePeriodData(dbpool, pDat, conf)
print "err2:%s"%(err) # debug print to show flow control
#defer.inlineCallbacks
def saveRealTimeData(self, rc, dat):
key = "somekey"
val = "somedata"
yield rc.set(key,val)
yield rc.expire(key,30)
#defer.inlineCallbacks
def savePeriodData(self,rc,dat,conf):
query = "some SQL statement"
yield rc.runQuery(query)
class PlainTCPFactory(protocol.Factory):
def buildProtocol(self, addr):
return PlainTCP(self)
def main():
dmdb = Dmdb()
if not dmdb.detectDb():
print "Please run MySQL RDBS first."
sys.exit()
log.startLogging(sys.stdout)
reactor.listenTCP(8080, PlainTCPFactory())
reactor.run()
if __name__ == "__main__":
main()
And clip of my client, which is a simple client:
def connectSend(host="127.0.0.1",port=8080):
global packet
s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
try:
s.connect((host, port))
s.sendall(''.join(packet))
data = s.recv(1024)
s.close()
print 'Received', repr(data)
except socket.error, err:
print "Remote socket is not available: %s"%str(err)
sys.exit(1)
The current status is:
If disable #defer.inlineCallbacks and yield opertions of dataReceived(), both self.transport.write() inside of connectionMode() and dataReceived() can output data to clients.
If we enabled #defer.inlineCallbacks and two yield DB ops of SQL/Redis, then self.transport.write() inside of connectionMode() prints every 5 connections, and dataReceived() will not output any data to clients.
the debug print statements will print on log regardless of #defer.inlineCallbacks anyway.
I was told that dataReceived() should not be #defer.inlineCallbacks. But it doesn't change anything if I removed that decoration.
I am thinking if gevent can help me out of this unpredicted behavior. I am twisted into an endless tornado, cyclone .....
Anyone who has similiar experience, please help me. Thanks.
By changing function as following, the code works.
#COMMENT OUT decorator of #defer.inlineCallbacks
def dataReceived(self, data):
global dbpool, rcs
(self.snrCode,rDat,pDat) = getDataParsed(data)
if self.snrCode == None or rDat == None or pDat == None:
err = "Bad format"
else:
err = "OK"
print "err:%s"%(err) # debug print to show flow control
self.err = err
self.transport.write(self.snrCode)
self.transport.write(self.err)
self.transport.write(rDat)
self.transport.write(pDat)
self.transport.loseConnection()
if self.snrCode != None and rDat != None and pDat != None:
self.saveRealTimeData(rcs, rDat)
self.savePeriodData(dbpool, pDat, conf)
# Removing yield before DB ops
print "err2:%s"%(err) # debug print to show flow control
#defer.inlineCallbacks
def saveRealTimeData(self, rc, dat):
print "saveRedis"
key = "somekey"
val = "somedata"
yield rc.set(key,val)
yield rc.expire(key,30)
#defer.inlineCallbacks
def savePeriodData(self,rc,dat,conf):
print "save SQL"
query = "some SQL statement"
yield rc.runQuery(query)
If we keep #defer.inlineCallbacks and yield in dataReceived. The connection is closed before second DB op. Therefore no data is output to connection. Maybe is caused by inlineCallbacks decorator.
By removing this, the flow control is simple and straightforward.
However, I still can get why I can not add inlineCallbacks if there are two deferred DB ops. This time they don't need deferred?

strange error when receiving udp datagram

I have a file, mc_send.py, that send mcast messages and shall receive a unicastmessage back from the program that received the mcast message, mc_recv.py. mcast work but when receiving the unicast message back a strange error appear: ValueError: maximum length of data to be read cannot be negative The error is att line 14 in this file mc_send.py:
I have struggled with this many hours on windows7 with python2.7.2 and pyqt4 v4.9 but can't find what I'm doing wrong. This programs is based on the broadcast examples from pyqt4.
""" to see all ports on windows: netstat -ap udp | find "4545" """
from PyQt4 import QtCore, QtGui, QtNetwork
unicast_addr = "127.0.0.1"
unicast_port = 45455
mcast_addr = "239.255.43.21"
mcast_port = 45454
class Sender(QtGui.QDialog):
def processPendingDatagrams(self):
while self.udpServer.hasPendingDatagrams():
datagram, host, port = self.udpServer.readDatagram(self.udpSocket.pendingDatagramSize())
print "got msg:", datagram
def __init__(self, parent=None):
super(Sender, self).__init__(parent)
self.groupAddress = QtNetwork.QHostAddress(mcast_addr)
self.unicastAddress = QtNetwork.QHostAddress(unicast_addr)
self.statusLabel = QtGui.QLabel("Ready to multicast datagrams to group %s on port 45454" %
self.groupAddress.toString())
# setup socket for listening on incomming datagrams
self.udpServer = QtNetwork.QUdpSocket(self)
self.udpServer.bind(unicast_port)
self.udpServer.readyRead.connect(self.processPendingDatagrams)
self.startButton = QtGui.QPushButton("&Start")
self.quitButton = QtGui.QPushButton("&Quit")
buttonBox = QtGui.QDialogButtonBox()
buttonBox.addButton(self.startButton, QtGui.QDialogButtonBox.ActionRole)
buttonBox.addButton(self.quitButton, QtGui.QDialogButtonBox.RejectRole)
self.timer = QtCore.QTimer(self)
self.udpSocket = QtNetwork.QUdpSocket(self)
self.messageNo = 1
self.startButton.clicked.connect(self.startSending)
self.quitButton.clicked.connect(self.close)
self.timer.timeout.connect(self.send_mc_msg)
mainLayout = QtGui.QVBoxLayout()
mainLayout.addWidget(self.statusLabel)
mainLayout.addWidget(buttonBox)
self.setLayout(mainLayout)
self.setWindowTitle("WSim")
def startSending(self):
self.startButton.setEnabled(False)
self.timer.start(1000)
def send_mc_msg(self):
self.udpSocket.writeDatagram("hello %d" %(self.messageNo), self.groupAddress, mcast_port)
self.messageNo += 1
if __name__ == '__main__':
import sys
app = QtGui.QApplication(sys.argv)
sender = Sender()
sender.show()
sys.exit(sender.exec_())
The multicast receiver that also send the unicast response back looks like this mc_recv.py:
from PyQt4 import QtGui, QtNetwork
mcast_addr = "239.255.43.21"
mcast_port = 45454
answer_addr = "127.0.0.1"
answer_port = 45455
class Receiver(QtGui.QDialog):
def __init__(self, parent=None):
super(Receiver, self).__init__(parent)
self.groupAddress = QtNetwork.QHostAddress(mcast_addr)
self.udpSocket = QtNetwork.QUdpSocket(self)
self.udpSocket.bind(mcast_port, QtNetwork.QUdpSocket.ReuseAddressHint)
self.udpSocket.joinMulticastGroup(self.groupAddress)
self.udpSocket.readyRead.connect(self.processPendingDatagrams)
# Use this socket to send unicast messages to back
self.answerSocket = QtNetwork.QUdpSocket(self)
self.answerAddress = QtNetwork.QHostAddress(answer_addr)
quitButton = QtGui.QPushButton("&Quit")
quitButton.clicked.connect(self.close)
buttonLayout = QtGui.QHBoxLayout()
buttonLayout.addStretch(1)
buttonLayout.addWidget(quitButton)
buttonLayout.addStretch(1)
self.statusLabel = QtGui.QLabel("Listening for multicasted messages on %s" % mcast_addr)
mainLayout = QtGui.QVBoxLayout()
mainLayout.addWidget(self.statusLabel)
mainLayout.addLayout(buttonLayout)
self.setLayout(mainLayout)
self.setWindowTitle("mrecv")
def processPendingDatagrams(self):
"""receive and decode multicast messages and send a response message on the return address"""
while self.udpSocket.hasPendingDatagrams():
datagram, host, port = self.udpSocket.readDatagram(self.udpSocket.pendingDatagramSize())
self.statusLabel.setText("received mcast msg '%s'" % datagram)
# send a response back to msend
self.answerSocket.writeDatagram("hi back", self.answerAddress, answer_port)
if __name__ == '__main__':
import sys
app = QtGui.QApplication(sys.argv)
receiver = Receiver()
receiver.show()
sys.exit(receiver.exec_())
found the cause, embarrassingly simple error, had written self.udpSocket.pendingDatagramSize()
instead of self.udpServer.pendingDatagramSize() when I was reading the data...