Skip to content

Commit c29c1d9

Browse files
jimmyyhwujimmyyhwu
committed
Move constants into own file
1 parent fb411d5 commit c29c1d9

25 files changed

+171
-79
lines changed

‎README.md‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -165,7 +165,7 @@ python controller.py --debug
165165

166166
### Mobile Base Accuracy
167167

168-
The marker detection setup should output 2D robot pose estimates with centimeter-level accuracy. For instance, our setup can reliably pick up small Lego Duplo blocks (32 mm x 32 mm) from the floor. Inaccurate marker detection can be due to many reasons, such as inaccurate camera alignment or suboptimal camera settings (see `get_video_cap` in [`utils.py`](server/utils.py)). Also note that the mobile base motors should be calibrated (`.motor_cal.txt`) for more accurate movement.
168+
The marker detection setup should output 2D robot pose estimates with centimeter-level accuracy. For instance, our setup can reliably pick up small Lego Duplo blocks (32 mm x 32 mm) from the floor. Inaccurate marker detection can be due to many reasons, such as inaccurate camera alignment or suboptimal camera settings (e.g., exposure and gain, see `get_video_cap` in [`utils.py`](server/utils.py)). Also note that the mobile base motors should be calibrated (`.motor_cal.txt`) for more accurate movement.
169169

170170
### Arm Accuracy
171171

@@ -186,8 +186,8 @@ To kill all processes using the occupied ports, you can use the [`clear-ports.sh
186186
```
187187

188188
For reference, here are all of the ports used by this codebase:
189-
* `6000`: Camera server (serial: `E4298F4E`)
190-
* `6001`: Camera server (serial: `099A11EE`)
189+
* `6000`: Camera server (top camera)
190+
* `6001`: Camera server (bottom camera)
191191
* `6002`: Marker detector server
192192
* `6003`: Object detector server
193193
* `6004`: Robot 1 controller server

‎robot/README.md‎

Lines changed: 16 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -33,7 +33,22 @@ The following steps should be run on the mobile base computer:
3333
sudo udevadm trigger
3434
```
3535

36-
5. See the [`driver`](driver) directory for mobile base driver setup
36+
5. Get the serial number of the camera using either of the following commands:
37+
38+
```bash
39+
lsusb -v -d 046d:0843 | grep iSerial
40+
udevadm info -n video0 | grep ID_SERIAL_SHORT
41+
```
42+
43+
6. Modify [`constants.py`](constants.py) to reflect your setup. These values may potentially need to be modified:
44+
45+
* `SERVER_HOSTNAME`
46+
* `ROBOT_HOSTNAME_PREFIX`
47+
* `REDIS_PASSWORD`
48+
* `ARM_HEADING_COMPENSATION`
49+
* `CAMERA_SERIALS`
50+
51+
7. See the [`driver`](driver) directory for mobile base driver setup
3752

3853
## Camera Setup
3954

‎robot/camera.py‎

Lines changed: 17 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -2,6 +2,8 @@
22
from multiprocessing.connection import Listener
33
from pathlib import Path
44
import cv2 as cv
5+
from constants import ROBOT_HOSTNAME_PREFIX, CONN_AUTHKEY
6+
from constants import CAMERA_SERIALS, CAMERA_FOCUS, CAMERA_TEMPERATURE, CAMERA_EXPOSURE, CAMERA_GAIN
57

68
def get_camera_params(serial):
79
camera_params_file_path = Path('camera_params') / f'{serial}.yml'
@@ -29,10 +31,10 @@ def get_video_cap(serial, frame_width, frame_height):
2931
# Read several frames to let settings (especially gain/exposure) stabilize
3032
for _ in range(30):
3133
cap.read()
32-
cap.set(cv.CAP_PROP_FOCUS, 0) # Fixed focus
33-
cap.set(cv.CAP_PROP_TEMPERATURE, 3900) # Fixed white balance
34-
cap.set(cv.CAP_PROP_GAIN, 10) # Fixed gain
35-
cap.set(cv.CAP_PROP_EXPOSURE, 156) # Fixed exposure
34+
cap.set(cv.CAP_PROP_FOCUS, CAMERA_FOCUS) # Fixed focus
35+
cap.set(cv.CAP_PROP_TEMPERATURE, CAMERA_TEMPERATURE) # Fixed white balance
36+
cap.set(cv.CAP_PROP_EXPOSURE, CAMERA_EXPOSURE) # Fixed exposure
37+
cap.set(cv.CAP_PROP_GAIN, CAMERA_GAIN) # Fixed gain
3638

3739
# Check all settings match expected
3840
assert cap.get(cv.CAP_PROP_FRAME_WIDTH) == frame_width
@@ -41,26 +43,26 @@ def get_video_cap(serial, frame_width, frame_height):
4143
assert cap.get(cv.CAP_PROP_AUTOFOCUS) == 0
4244
assert cap.get(cv.CAP_PROP_AUTO_WB) == 0
4345
assert cap.get(cv.CAP_PROP_AUTO_EXPOSURE) == 1
44-
assert cap.get(cv.CAP_PROP_FOCUS) == 0
45-
assert cap.get(cv.CAP_PROP_TEMPERATURE) == 3900
46-
assert cap.get(cv.CAP_PROP_GAIN) == 10
47-
assert cap.get(cv.CAP_PROP_EXPOSURE) == 156
46+
assert cap.get(cv.CAP_PROP_FOCUS) == CAMERA_FOCUS
47+
assert cap.get(cv.CAP_PROP_TEMPERATURE) == CAMERA_TEMPERATURE
48+
assert cap.get(cv.CAP_PROP_EXPOSURE) == CAMERA_EXPOSURE
49+
assert cap.get(cv.CAP_PROP_GAIN) == CAMERA_GAIN
4850

4951
return cap
5052

5153
class Camera:
5254
def __init__(self, robot_idx):
53-
serial = {0: '634093BE', 1: '44251E9E', 2:'7E841E9E'}[robot_idx]
55+
serial = CAMERA_SERIALS[robot_idx]
5456
image_width, image_height, camera_matrix, dist_coeffs = get_camera_params(serial)
5557
self.cap = get_video_cap(serial, image_width, image_height)
5658
self.map_x, self.map_y = cv.initUndistortRectifyMap(camera_matrix, dist_coeffs, None, camera_matrix, (image_width, image_height), cv.CV_32FC1)
5759
self.image_size = (image_width // 2, image_height // 2)
5860

5961
def get_encoded_image(self):
60-
assert self.cap.get(cv.CAP_PROP_EXPOSURE) == 156
61-
assert self.cap.get(cv.CAP_PROP_GAIN) == 10
62-
assert self.cap.get(cv.CAP_PROP_TEMPERATURE) == 3900
63-
assert self.cap.get(cv.CAP_PROP_FOCUS) == 0
62+
assert self.cap.get(cv.CAP_PROP_FOCUS) == CAMERA_FOCUS
63+
assert self.cap.get(cv.CAP_PROP_TEMPERATURE) == CAMERA_TEMPERATURE
64+
assert self.cap.get(cv.CAP_PROP_EXPOSURE) == CAMERA_EXPOSURE
65+
assert self.cap.get(cv.CAP_PROP_GAIN) == CAMERA_GAIN
6466

6567
# Read new frame
6668
image = None
@@ -85,7 +87,7 @@ def disconnect(self):
8587
class CameraServer:
8688
def __init__(self, camera, hostname='0.0.0.0', port=6010):
8789
self.camera = camera
88-
self.listener = Listener((hostname, port), authkey=b'secret password')
90+
self.listener = Listener((hostname, port), authkey=CONN_AUTHKEY)
8991

9092
def run(self):
9193
address, port = self.listener.address
@@ -102,7 +104,7 @@ def run(self):
102104

103105
def main():
104106
hostname = socket.gethostname()
105-
assert hostname.startswith('iprl-bot')
107+
assert hostname.startswith(ROBOT_HOSTNAME_PREFIX)
106108
robot_num = int(hostname[-1])
107109
robot_idx = robot_num - 1
108110
port = 6010 + robot_idx

‎robot/constants.py‎

Lines changed: 27 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,27 @@
1+
SERVER_HOSTNAME = 'bohg-ws-14'
2+
ROBOT_HOSTNAME_PREFIX = 'iprl-bot'
3+
CONN_AUTHKEY = b'secret password'
4+
REDIS_PASSWORD = 'secret password'
5+
6+
################################################################################
7+
# Arm
8+
9+
# Arm-dependent heading compensation (set to 0 if unsure)
10+
ARM_HEADING_COMPENSATION = {
11+
0: -0.7, # Robot 1 (asset tag: none)
12+
1: 0.2, # Robot 2 (asset tag: 000007 402760)
13+
2: 0.7, # Robot 3 (asset tag: 000007 402746)
14+
}
15+
16+
################################################################################
17+
# Camera
18+
19+
CAMERA_SERIALS = {
20+
0: '634093BE', # Robot 1
21+
1: '44251E9E', # Robot 2
22+
2: '7E841E9E', # Robot 3
23+
}
24+
CAMERA_FOCUS = 0
25+
CAMERA_TEMPERATURE = 3900
26+
CAMERA_EXPOSURE = 156
27+
CAMERA_GAIN = 10

‎robot/controller.py‎

Lines changed: 9 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -16,6 +16,8 @@
1616
from kortex_api.Exceptions.KServerException import KServerException
1717
from redis import Redis
1818
from camera import Camera
19+
from constants import SERVER_HOSTNAME, ROBOT_HOSTNAME_PREFIX, CONN_AUTHKEY, REDIS_PASSWORD
20+
from constants import ARM_HEADING_COMPENSATION
1921
from kinova import KinovaArm
2022

2123
def distance(pt1, pt2):
@@ -47,9 +49,9 @@ def intersect(d, f, r, use_t1=False):
4749
class RedisClient:
4850
def __init__(self):
4951
hostname = socket.gethostname()
50-
assert hostname.startswith('iprl-bot')
52+
assert hostname.startswith(ROBOT_HOSTNAME_PREFIX)
5153
self.bot_num = int(hostname[-1])
52-
self.client = Redis(f'iprl-bot{self.bot_num}', password='secret password', decode_responses=True)
54+
self.client = Redis(f'{ROBOT_HOSTNAME_PREFIX}{self.bot_num}', password=REDIS_PASSWORD, decode_responses=True)
5355

5456
def get_pose(self):
5557
return tuple(map(float, self.client.get(f'mmp::bot{self.bot_num}::veh::sensor::x').split(' ')))
@@ -185,7 +187,7 @@ def run(self):
185187
self.running = True
186188

187189
# Robot pose from marker detection
188-
marker_detector_conn = Client(('bohg-ws-14', 6002), authkey=b'secret password')
190+
marker_detector_conn = Client((SERVER_HOSTNAME, 6002), authkey=CONN_AUTHKEY)
189191
marker_detector_conn.send(None)
190192

191193
goal_reached_steps = 0
@@ -555,8 +557,8 @@ def run(self):
555557

556558
arm_command = self.queue.get()
557559

558-
# Arm-dependent heading compensation (arm asset tags - {0: 'none', 1: '000007 402760', 2: '000007 402746'})
559-
arm_command['target_arm_heading'] += {0: -0.7, 1: 0.2, 2: 0.7}[self.robot_idx]
560+
# Arm-dependent heading compensation
561+
arm_command['target_arm_heading'] += ARM_HEADING_COMPENSATION[self.robot_idx]
560562

561563
# Execute command
562564
self.target_ee_pos = arm_command['target_ee_pos']
@@ -652,7 +654,7 @@ def __init__(self, debug=False):
652654
self.categories = []
653655
try:
654656
self.camera = Camera(robot_idx)
655-
self.object_detector_conn = Client(('bohg-ws-14', 6003), authkey=b'secret password')
657+
self.object_detector_conn = Client((SERVER_HOSTNAME, 6003), authkey=CONN_AUTHKEY)
656658
self.queue = Queue()
657659
self.object_detector_conn.send({'encoded_image': self.camera.get_encoded_image(), 'categories': self.categories}) # Warm up the server
658660
self.object_detector_conn.recv()
@@ -663,7 +665,7 @@ def __init__(self, debug=False):
663665
print('Could not set up object detection, falling back to random selection from object categories')
664666

665667
# Controller server
666-
self.server_conn = Client(('bohg-ws-14', 6007 + robot_idx), authkey=b'secret password')
668+
self.server_conn = Client((SERVER_HOSTNAME, 6007 + robot_idx), authkey=CONN_AUTHKEY)
667669

668670
# Logging
669671
log_dir = 'logs'

‎robot/kinova.py‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -433,7 +433,7 @@ def _run_cyclic(self):
433433
bins = np.r_[np.arange(MoveJController.CYCLIC_STEP_SIZE, 11 * MoveJController.CYCLIC_STEP_SIZE, MoveJController.CYCLIC_STEP_SIZE), np.inf]
434434
print('Cyclic step times:', np.histogram(step_times, bins=bins))
435435

436-
# import pickle, socket
436+
# import pickle
437437
# output_path = f'joint-states.pkl'
438438
# with open(output_path, 'wb') as f:
439439
# pickle.dump(data, f)

‎server/README.md‎

Lines changed: 17 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -35,18 +35,29 @@ The following steps should be run on the workstation:
3535
pip install gsutil==5.14 easydict==1.10 pyyaml==6.0 jupyter==1.0.0
3636
```
3737

38+
4. Get the serial numbers of the two overhead cameras using either of the following commands:
39+
40+
```bash
41+
lsusb -v -d 046d:0843 | grep iSerial
42+
udevadm info -n video0 | grep ID_SERIAL_SHORT
43+
```
44+
45+
5. Modify [`constants.py`](constants.py) to reflect your setup. These values may potentially need to be modified:
46+
47+
* ``ROBOT_HOSTNAME_PREFIX``
48+
* ``CAMERA_SERIALS``
49+
* ``CAMERA_HEIGHT``
50+
* ``ROBOT_WIDTH``
51+
* ``ROBOT_HEIGHT``
52+
* ``ROBOT_DIAG``
53+
3854
## Usage
3955

4056
The following sections describe the process of setting up marker detection and object detection from scratch. To get started with a setup that already works, see the "TidyBot Quickstart" section of the main [`README`](../README.md).
4157

4258
We use two ceiling-mounted overhead cameras to detect fiducial markers installed on robots in the scene to get real-time 2D robot poses with centimeter-level precision. The camera we use is the [Logitech C930e webcam](https://www.amazon.com/Logitech-C930e-1080P-Video-Webcam/dp/B00CRJWW2G). Note that even though the C930e supports 1080p, we run it at 720p, as we found that 720p is faster and was sufficient to get centimeter-level accuracy.
4359

44-
The code uses the serial number of your camera as an identifier for `cv.VideoCapture` and for saving and loading camera parameters. On Ubuntu 20, you can get the serial number of your camera using either of the following commands:
45-
46-
```bash
47-
lsusb -v -d 046d:0843 | grep iSerial
48-
udevadm info -n video0 | grep ID_SERIAL_SHORT
49-
```
60+
The code uses the serial number of your camera as an identifier for `cv.VideoCapture` and for saving and loading camera parameters.
5061

5162
### Camera Calibration
5263

‎server/camera_alignment.py‎

Lines changed: 3 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -7,6 +7,7 @@
77
import cv2 as cv
88
import numpy as np
99
import utils
10+
from constants import CAMERA_SERIALS
1011

1112
def compute_transformation_matrix(src_points):
1213
dst_points = np.array([[0, 1], [1, 1], [1, 0], [0, 0]], dtype=np.float32)
@@ -120,7 +121,7 @@ def run(self):
120121
args = parser.parse_args()
121122
if args.serial is None:
122123
if args.camera2:
123-
args.serial = '099A11EE'
124+
args.serial = CAMERA_SERIALS[1]
124125
else:
125-
args.serial = 'E4298F4E'
126+
args.serial = CAMERA_SERIALS[0]
126127
Annotator(args.serial, center=args.center).run()

‎server/camera_client.py‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -7,6 +7,7 @@
77
from multiprocessing.connection import Client
88
import cv2 as cv
99
import numpy as np
10+
from constants import CONN_AUTHKEY
1011

1112
# See https://bugs.python.org/issue38119
1213
def remove_shm_from_resource_tracker():
@@ -29,7 +30,7 @@ def fix_unregister(name, rtype):
2930
class CameraClient:
3031
def __init__(self, port):
3132
# Connect to camera server
32-
self.conn = Client(('localhost', port), authkey=b'secret password')
33+
self.conn = Client(('localhost', port), authkey=CONN_AUTHKEY)
3334

3435
# Set up shared memory block for reading camera images
3536
self.conn.send(None)

‎server/camera_server.py‎

Lines changed: 7 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -9,6 +9,7 @@
99
import cv2 as cv
1010
import numpy as np
1111
import utils
12+
from constants import CAMERA_SERIALS, CAMERA_FOCUS, CAMERA_TEMPERATURE, CAMERA_EXPOSURE, CAMERA_GAIN
1213
from publisher import Publisher
1314

1415
class CameraServer(Publisher):
@@ -46,10 +47,10 @@ def camera_worker(self):
4647
if self.queue.empty():
4748
image = self.get_image()
4849
self.queue.put((time.time(), image))
49-
assert self.cap.get(cv.CAP_PROP_EXPOSURE) == 77
50-
assert self.cap.get(cv.CAP_PROP_GAIN) == 50
51-
assert self.cap.get(cv.CAP_PROP_TEMPERATURE) == 3900
52-
assert self.cap.get(cv.CAP_PROP_FOCUS) == 0
50+
assert self.cap.get(cv.CAP_PROP_FOCUS) == CAMERA_FOCUS
51+
assert self.cap.get(cv.CAP_PROP_TEMPERATURE) == CAMERA_TEMPERATURE
52+
assert self.cap.get(cv.CAP_PROP_EXPOSURE) == CAMERA_EXPOSURE
53+
assert self.cap.get(cv.CAP_PROP_GAIN) == CAMERA_GAIN
5354
time.sleep(0.0001)
5455

5556
def get_data(self):
@@ -73,9 +74,9 @@ def clean_up(self):
7374
def main(args):
7475
if args.serial is None:
7576
if args.camera2:
76-
args.serial = '099A11EE'
77+
args.serial = CAMERA_SERIALS[1]
7778
else:
78-
args.serial = 'E4298F4E'
79+
args.serial = CAMERA_SERIALS[0]
7980
CameraServer(args.serial, port=args.port).run()
8081

8182
if __name__ == '__main__':

0 commit comments

Comments
 (0)