Skip to content

Commit fb0f602

Browse files
anla-xuanla-xu
committed
Add a Bluetooth server of pi version
1 parent d4f09d5 commit fb0f602

File tree

10 files changed

+1733
-0
lines changed

10 files changed

+1733
-0
lines changed

‎demo/mycobot_pi_bluetooth/README.md‎

Lines changed: 17 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,17 @@
1+
# Used for mycobot pi and mycobot_app
2+
3+
## Usage method
4+
5+
### 1.Make sure raspberry pi Bluetooth is available
6+
7+
The Ubuntu 20.04 system needs to restart the Bluetooth service first.
8+
9+
```shell
10+
sudo systemctl restart bluetooth
11+
```
12+
13+
### 2.Run Bluetooth server
14+
15+
```shell
16+
python uart_peripheral_serial.py
17+
```

‎demo/mycobot_pi_bluetooth/__init__.py‎

Whitespace-only changes.

‎demo/mycobot_pi_bluetooth/app.py‎

Lines changed: 74 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,74 @@
1+
import sys, time, threading, logging
2+
sys.path.append('.')
3+
4+
from ai import singleton, parameters, functools, coloredlogging
5+
6+
logger = coloredlogging.get_logger("AS", coloredlogging.BLUE)
7+
logger.addHandler(logging.NullHandler())
8+
9+
class AppStates(metaclass=singleton.Singleton):
10+
happiness = 4
11+
hungry = 4
12+
energy = 4
13+
comfort = 4
14+
counter = 0
15+
16+
def __init__(self) -> None:
17+
...
18+
19+
@classmethod
20+
def recover(cls, genre):
21+
if genre == 'energy':
22+
cls.energy += 1
23+
cls.energy = min(4, cls.energy)
24+
elif genre == 'happiness':
25+
cls.happiness += 1
26+
cls.happiness = min(4, cls.happiness)
27+
elif genre == 'hungry':
28+
cls.happiness += 1
29+
cls.happiness = min(4, cls.happiness)
30+
elif genre == 'comfort':
31+
cls.comfort += 1
32+
cls.comfort = min(4, cls.comfort)
33+
34+
@classmethod
35+
def drop(cls, genre: str):
36+
if genre == 'energy':
37+
cls.energy -= 1
38+
cls.energy = max(0, cls.energy)
39+
elif genre == 'happiness':
40+
cls.happiness -= 1
41+
cls.happiness = max(0, cls.happiness)
42+
elif genre == 'hungry':
43+
cls.happiness -= 1
44+
cls.happiness = max(0, cls.happiness)
45+
elif genre == 'comfort':
46+
cls.comfort -= 1
47+
cls.comfort = max(0, cls.comfort)
48+
49+
@classmethod
50+
def recover_by_action(cls, action: str):
51+
if action == 'eat':
52+
cls.recover('hungry')
53+
elif action.startswith('touch'):
54+
cls.recover('comfort')
55+
elif action in parameters.BEHAVIOURS['relax']:
56+
cls.recover('energy')
57+
elif action.startswith('voice') or action.startswith(
58+
'stare') or action in ['ball', 'attack', 'flap']:
59+
cls.recover('happiness')
60+
61+
@classmethod
62+
@functools.thread_run
63+
def async_run(cls):
64+
logger.debug(f'Starting Stats recorder')
65+
while ...:
66+
time.sleep(60 * 10)
67+
AppStates.counter += 1
68+
cls.drop('energy')
69+
if AppStates.counter % 2 == 0:
70+
cls.drop('happiness')
71+
cls.drop('comfort')
72+
elif AppStates.counter % 12 == 0:
73+
cls.drop('hungry')
74+

‎demo/mycobot_pi_bluetooth/bt.py‎

Lines changed: 195 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,195 @@
1+
import bluetooth
2+
import pygatt
3+
import pathlib
4+
import time
5+
import json
6+
import hashlib
7+
from func_timeout import func_timeout, FunctionTimedOut
8+
import os
9+
import sys
10+
sys.path.append('.')
11+
12+
from typing import Tuple
13+
14+
import ai.action.move.movement
15+
16+
mars = ai.action.move.movement.Movements()
17+
18+
19+
class BluetoothService:
20+
pass
21+
22+
23+
def list_devices() -> Tuple[str, str]:
24+
nearby_devices = bluetooth.discover_devices(lookup_names=True)
25+
return nearby_devices
26+
27+
28+
def filter_marscat(bluetooth_devices: Tuple[str, str]) -> Tuple[str, str]:
29+
marscats = []
30+
for addr, name in bluetooth_devices:
31+
name.encode('utf-8', 'replace')
32+
if name == 'marscat':
33+
marscats.append((addr, name))
34+
return marscats
35+
36+
37+
def start_marscat_bt() -> None:
38+
server_sock = bluetooth.BluetoothSocket(bluetooth.L2CAP)
39+
server_sock.bind(("", bluetooth.PORT_ANY))
40+
server_sock.listen(1)
41+
port = server_sock.getsockname()[1]
42+
uuid = '26754beb-1bd0-4017-b341-154bed30b71a'
43+
name = 'marscat_bt'
44+
bluetooth.advertise_service(
45+
server_sock,
46+
name,
47+
service_id=uuid,
48+
service_classes=[uuid, bluetooth.SERIAL_PORT_CLASS],
49+
profiles=[bluetooth.SERIAL_PORT_PROFILE],
50+
provider='',
51+
description='',
52+
protocols=[bluetooth.OBEX_UUID])
53+
print("Waiting for connection on RFCOMM channel {}".format(port))
54+
55+
client_sock, client_info = server_sock.accept()
56+
57+
58+
print("Accepted connection from", client_info)
59+
60+
while True:
61+
time.sleep(0.1)
62+
try:
63+
data = client_sock.recv(1024)
64+
except OSError:
65+
print("Remote client disconnected")
66+
data = None
67+
68+
if not data:
69+
client_sock.close()
70+
print("Close Client Socket")
71+
print("Waiting for connection on RFCOMM channel {}".format(port))
72+
client_sock, client_info = server_sock.accept()
73+
print("Accepted connection from", client_info)
74+
continue
75+
76+
if data == b'run':
77+
pass
78+
#mars.set_run()
79+
elif data == b'walk':
80+
pass
81+
#mars.set_walk()
82+
elif data == b'stand':
83+
pass
84+
#mars.set_stand()
85+
elif data == b'sit':
86+
pass
87+
#mars.set_sit()
88+
elif data == b'left':
89+
pass
90+
#mars.set_turn()
91+
elif data == b'right':
92+
pass
93+
#mars.set_turn(-1)
94+
elif data == b'send_update_start':
95+
print('start receive')
96+
97+
# recv file information
98+
data = client_sock.recv(1024)
99+
file_len = int(str(data, encoding='utf8'))
100+
print('file size : ', file_len)
101+
102+
data = client_sock.recv(1024)
103+
file_md5 = str(data, encoding='utf8')
104+
print(file_md5)
105+
106+
# receive marsai.zip
107+
if os.path.exists(str(pathlib.Path.home()) + '/marsai.zip'):
108+
os.system(
109+
'rm {}'.format(str(pathlib.Path.home()) + '/marsai.zip'))
110+
os.system(
111+
'touch {}'.format(str(pathlib.Path.home()) + '/marsai.zip'))
112+
113+
i = 0
114+
buffer = 4096
115+
116+
def recv_data(buffer):
117+
print('start recv_data')
118+
data = client_sock.recv(buffer)
119+
return data
120+
121+
with open(str(pathlib.Path.home()) + '/marsai.zip', 'ab+') as f:
122+
while file_len != 0:
123+
time.sleep(0.1)
124+
data = recv_data(buffer)
125+
#while True:
126+
# try:
127+
# time.sleep(0.1)
128+
# data = func_timeout(1, recv_data, (buffer, ))
129+
# except FunctionTimedOut:
130+
# print('time out')
131+
# client_sock.send(str(file_len).encode('utf-8'))
132+
# client_sock.close()
133+
# print('client closed')
134+
# client_sock, client_info = server_sock.accept()
135+
# print('accepted')
136+
# except bluetooth.BluetoothError as e:
137+
# print("error")
138+
# else:
139+
# break
140+
f.write(data)
141+
print('write', i, len(data))
142+
i += 1
143+
file_len -= len(data)
144+
print(file_len)
145+
if file_len < buffer:
146+
buffer = file_len
147+
print('write finished')
148+
print('receive over')
149+
150+
# recv over flag and update marsai
151+
while True:
152+
data = client_sock.recv(4096)
153+
if data == b'over_and_update':
154+
md5 = get_md5(str(pathlib.Path.home()) + '/marsai.zip')
155+
if file_md5 == md5:
156+
os.system('sh ~/marsai/tools/stop-systemd-services.sh')
157+
time.sleep(3)
158+
os.system('unzip -o ~/marsai.zip -d ~/marsai')
159+
time.sleep(1)
160+
os.system('sh ~/marsai/tools/start-systemd-services.sh')
161+
client_sock.send(b'update_over')
162+
print('over')
163+
break
164+
else:
165+
print('md5 error')
166+
break
167+
else:
168+
pass
169+
170+
171+
172+
173+
else:
174+
pass
175+
176+
print("Received " + str(data))
177+
178+
client_sock.close()
179+
print("Close Client Socket")
180+
181+
server_sock.close()
182+
print("Stop Server")
183+
184+
def get_md5(_file):
185+
m = hashlib.md5()
186+
with open(_file, 'rb') as f:
187+
for line in f:
188+
m.update(line)
189+
md5_code = m.hexdigest()
190+
print(md5_code)
191+
return md5_code
192+
193+
194+
if __name__ == "__main__":
195+
start_marscat_bt()

‎demo/mycobot_pi_bluetooth/bt_client.py‎

Lines changed: 39 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,39 @@
1+
import sys
2+
import bluetooth
3+
4+
5+
if __name__ == "__main__":
6+
addr = None
7+
8+
if len(sys.argv) < 2:
9+
print("No device specified. Searching all nearby bluetooth devices for the marscat_bt service...")
10+
else:
11+
addr = sys.argv[1]
12+
print("Searching for marscat_bt on {}...".format(addr))
13+
14+
uuid = '26754beb-1bd0-4017-b341-154bed30b71a'
15+
service_matches = bluetooth.find_service(uuid=uuid, address=addr)
16+
17+
if len(service_matches) == 0:
18+
print("Couldn't find the marscat_bt service.")
19+
sys.exit(0)
20+
21+
first_match = service_matches[0]
22+
port = first_match["port"]
23+
name = first_match["name"]
24+
host = first_match["host"]
25+
26+
print("Connecting to \"{}\" on {}".format(name, host))
27+
28+
# Create the client socket
29+
sock = bluetooth.BluetoothSocket(bluetooth.RFCOMM)
30+
sock.connect((host, port))
31+
32+
print("Connected. Type something...")
33+
while True:
34+
data = input()
35+
if not data:
36+
break
37+
sock.send(data)
38+
39+
sock.close()

0 commit comments

Comments
 (0)