Skip to content

Commit 584b70a

Browse files
anathananathan
committed
adds a simple tool to do baseline device configuration over ip
1 parent f22894f commit 584b70a

1 file changed

Lines changed: 340 additions & 0 deletions

File tree

‎bin/config_wizard.py‎

Lines changed: 340 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,340 @@
1+
#!/usr/bin/env python3
2+
3+
"""
4+
Interactive configuration wizard for Point One devices.
5+
6+
This wizard guides users through configuring key device parameters:
7+
- IMU to body lever arm (X, Y, Z)
8+
- GPS to body lever arm (X, Y, Z)
9+
- Device orientation (Z axis direction, X axis direction)
10+
"""
11+
12+
import os
13+
import socket
14+
import sys
15+
from urllib.parse import urlparse
16+
17+
from fusion_engine_client.messages import *
18+
19+
# Add the parent directory to the search path to enable p1_runner imports.
20+
repo_root = os.path.normpath(os.path.join(os.path.dirname(__file__), '..'))
21+
sys.path.append(repo_root)
22+
sys.path.append(os.path.dirname(__file__))
23+
24+
from p1_runner import trace as logging
25+
from p1_runner.data_source import SocketDataSource
26+
from p1_runner.device_interface import DeviceInterface
27+
28+
logger = logging.getLogger('point_one.config_wizard')
29+
30+
DEFAULT_TCP_PORT = 30200
31+
32+
# Direction options for orientation
33+
DIRECTION_OPTIONS = {
34+
'forward': Direction.FORWARD,
35+
'backward': Direction.BACKWARD,
36+
'left': Direction.LEFT,
37+
'right': Direction.RIGHT,
38+
'up': Direction.UP,
39+
'down': Direction.DOWN,
40+
}
41+
42+
DIRECTION_NAMES = {v: k for k, v in DIRECTION_OPTIONS.items()}
43+
44+
45+
def get_direction_name(direction: Direction) -> str:
46+
"""Convert Direction enum to human-readable name."""
47+
return DIRECTION_NAMES.get(direction, str(direction))
48+
49+
50+
def connect_to_device(ip_address: str) -> tuple:
51+
"""
52+
Connect to device via TCP.
53+
54+
Returns:
55+
Tuple of (data_source, config_interface) or (None, None) on failure.
56+
"""
57+
try:
58+
# Parse the address
59+
if '://' not in ip_address:
60+
ip_address = f'tcp://{ip_address}'
61+
62+
parts = urlparse(ip_address)
63+
address = parts.hostname
64+
port = parts.port if parts.port is not None else DEFAULT_TCP_PORT
65+
66+
if address is None:
67+
print(f"Error: Invalid IP address format.")
68+
return None, None
69+
70+
print(f"Connecting to {address}:{port}...")
71+
s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
72+
s.settimeout(5.0)
73+
s.connect((address, port))
74+
s.settimeout(None)
75+
76+
data_source = SocketDataSource(s)
77+
config_interface = DeviceInterface(data_source)
78+
79+
print("Connected successfully.")
80+
return data_source, config_interface
81+
82+
except Exception as e:
83+
print(f"Error connecting to device: {e}")
84+
return None, None
85+
86+
87+
def query_config(config_interface: DeviceInterface, config_type) -> object:
88+
"""Query a configuration value from the device."""
89+
config_interface.get_config(ConfigurationSource.ACTIVE, config_type.GetType())
90+
resp = config_interface.wait_for_message(ConfigResponseMessage.MESSAGE_TYPE)
91+
92+
if resp is None:
93+
return None
94+
if resp.response != Response.OK:
95+
return None
96+
97+
return resp.config_object
98+
99+
100+
def apply_config(config_interface: DeviceInterface, config_object, save: bool = False) -> bool:
101+
"""Apply a configuration value to the device."""
102+
config_interface.set_config(config_object, save=save)
103+
resp = config_interface.wait_for_message(CommandResponseMessage.MESSAGE_TYPE)
104+
105+
if not isinstance(resp, CommandResponseMessage):
106+
return False
107+
if resp.response != Response.OK:
108+
print(f"Error: {resp.response}")
109+
return False
110+
111+
return True
112+
113+
114+
def save_all_config(config_interface: DeviceInterface) -> bool:
115+
"""Save all configuration to persistent storage."""
116+
config_interface.send_save(SaveAction.SAVE)
117+
resp = config_interface.wait_for_message(CommandResponseMessage.MESSAGE_TYPE)
118+
119+
if not isinstance(resp, CommandResponseMessage):
120+
return False
121+
if resp.response != Response.OK:
122+
print(f"Error saving: {resp.response}")
123+
return False
124+
125+
return True
126+
127+
128+
def prompt_float(prompt: str, current_value: float) -> float:
129+
"""Prompt for a float value, showing current value as default."""
130+
while True:
131+
response = input(f"{prompt} [{current_value:.3f}]: ").strip()
132+
if response == '':
133+
return current_value
134+
try:
135+
return float(response)
136+
except ValueError:
137+
print("Please enter a valid number.")
138+
139+
140+
def prompt_direction(prompt: str, current_value: Direction, valid_options: list = None) -> Direction:
141+
"""Prompt for a direction value."""
142+
if valid_options is None:
143+
valid_options = list(DIRECTION_OPTIONS.keys())
144+
145+
current_name = get_direction_name(current_value)
146+
options_str = ', '.join(valid_options)
147+
148+
while True:
149+
response = input(f"{prompt} ({options_str}) [{current_name}]: ").strip().lower()
150+
if response == '':
151+
return current_value
152+
if response in DIRECTION_OPTIONS and response in valid_options:
153+
return DIRECTION_OPTIONS[response]
154+
print(f"Please enter one of: {options_str}")
155+
156+
157+
def prompt_yes_no(prompt: str, default: bool = True) -> bool:
158+
"""Prompt for a yes/no response."""
159+
default_str = "Y/n" if default else "y/N"
160+
while True:
161+
response = input(f"{prompt} [{default_str}]: ").strip().lower()
162+
if response == '':
163+
return default
164+
if response in ('y', 'yes'):
165+
return True
166+
if response in ('n', 'no'):
167+
return False
168+
print("Please enter y or n.")
169+
170+
171+
def print_section(title: str):
172+
"""Print a section header."""
173+
print()
174+
print("=" * 60)
175+
print(f" {title}")
176+
print("=" * 60)
177+
178+
179+
def print_lever_arm(name: str, config):
180+
"""Print lever arm values."""
181+
print(f" {name}: X={config.x:.3f}m, Y={config.y:.3f}m, Z={config.z:.3f}m")
182+
183+
184+
def print_orientation(config):
185+
"""Print orientation values."""
186+
x_name = get_direction_name(config.x_direction)
187+
z_name = get_direction_name(config.z_direction)
188+
print(f" Orientation: X-axis={x_name}, Z-axis={z_name}")
189+
190+
191+
def main():
192+
logging.basicConfig(
193+
level=logging.WARNING,
194+
format='%(message)s',
195+
stream=sys.stdout
196+
)
197+
198+
print()
199+
print("=" * 60)
200+
print(" Point One Device Configuration Wizard")
201+
print("=" * 60)
202+
print()
203+
204+
# Get device IP
205+
ip_address = input("Enter device IP address [192.168.0.1]: ").strip()
206+
if ip_address == '':
207+
ip_address = "192.168.0.1"
208+
209+
# Connect to device
210+
data_source, config_interface = connect_to_device(ip_address)
211+
if config_interface is None:
212+
sys.exit(1)
213+
214+
try:
215+
# Query current values
216+
print()
217+
print("Querying current configuration...")
218+
219+
device_lever_arm = query_config(config_interface, DeviceLeverArmConfig)
220+
gnss_lever_arm = query_config(config_interface, GNSSLeverArmConfig)
221+
orientation = query_config(config_interface, DeviceCourseOrientationConfig)
222+
223+
if device_lever_arm is None or gnss_lever_arm is None or orientation is None:
224+
print("Error: Failed to query current configuration.")
225+
sys.exit(1)
226+
227+
# Display current values
228+
print_section("Current Configuration")
229+
print_lever_arm("IMU Lever Arm", device_lever_arm)
230+
print_lever_arm("GPS Lever Arm", gnss_lever_arm)
231+
print_orientation(orientation)
232+
233+
# Track what changed
234+
changes = []
235+
236+
# IMU Lever Arm
237+
print_section("IMU to Body Lever Arm (meters)")
238+
print("Enter the offset from the vehicle body origin to the IMU.")
239+
print(" +X = Forward, +Y = Left, +Z = Up")
240+
print()
241+
242+
new_device_x = prompt_float(" X (forward)", device_lever_arm.x)
243+
new_device_y = prompt_float(" Y (left)", device_lever_arm.y)
244+
new_device_z = prompt_float(" Z (up)", device_lever_arm.z)
245+
246+
if (new_device_x != device_lever_arm.x or
247+
new_device_y != device_lever_arm.y or
248+
new_device_z != device_lever_arm.z):
249+
new_device_lever_arm = DeviceLeverArmConfig(new_device_x, new_device_y, new_device_z)
250+
changes.append(('IMU Lever Arm', new_device_lever_arm))
251+
252+
# GPS Lever Arm
253+
print_section("GPS Antenna to Body Lever Arm (meters)")
254+
print("Enter the offset from the vehicle body origin to the GPS antenna.")
255+
print(" +X = Forward, +Y = Left, +Z = Up")
256+
print()
257+
258+
new_gnss_x = prompt_float(" X (forward)", gnss_lever_arm.x)
259+
new_gnss_y = prompt_float(" Y (left)", gnss_lever_arm.y)
260+
new_gnss_z = prompt_float(" Z (up)", gnss_lever_arm.z)
261+
262+
if (new_gnss_x != gnss_lever_arm.x or
263+
new_gnss_y != gnss_lever_arm.y or
264+
new_gnss_z != gnss_lever_arm.z):
265+
new_gnss_lever_arm = GNSSLeverArmConfig(new_gnss_x, new_gnss_y, new_gnss_z)
266+
changes.append(('GPS Lever Arm', new_gnss_lever_arm))
267+
268+
# Orientation
269+
print_section("Device Orientation")
270+
print("Specify how the device is mounted relative to the vehicle body.")
271+
print(" Vehicle body: +X = Forward, +Y = Left, +Z = Up")
272+
print()
273+
274+
new_z_dir = prompt_direction(
275+
" Device Z-axis points",
276+
orientation.z_direction,
277+
['up', 'down', 'forward', 'backward', 'left', 'right']
278+
)
279+
new_x_dir = prompt_direction(
280+
" Device X-axis points",
281+
orientation.x_direction,
282+
['forward', 'backward', 'left', 'right', 'up', 'down']
283+
)
284+
285+
if new_z_dir != orientation.z_direction or new_x_dir != orientation.x_direction:
286+
new_orientation = DeviceCourseOrientationConfig(new_x_dir, new_z_dir)
287+
changes.append(('Orientation', new_orientation))
288+
289+
# Summary and confirmation
290+
if not changes:
291+
print()
292+
print("No changes made.")
293+
sys.exit(0)
294+
295+
print_section("Summary of Changes")
296+
for name, config in changes:
297+
if isinstance(config, DeviceCourseOrientationConfig):
298+
x_name = get_direction_name(config.x_direction)
299+
z_name = get_direction_name(config.z_direction)
300+
print(f" {name}: X-axis={x_name}, Z-axis={z_name}")
301+
else:
302+
print(f" {name}: X={config.x:.3f}m, Y={config.y:.3f}m, Z={config.z:.3f}m")
303+
304+
print()
305+
if not prompt_yes_no("Apply these changes?", default=True):
306+
print("Changes cancelled.")
307+
sys.exit(0)
308+
309+
# Apply changes
310+
print()
311+
print("Applying changes...")
312+
for name, config in changes:
313+
print(f" Setting {name}...", end=' ')
314+
if apply_config(config_interface, config):
315+
print("OK")
316+
else:
317+
print("FAILED")
318+
sys.exit(1)
319+
320+
# Save to persistent storage
321+
print()
322+
if prompt_yes_no("Save changes to persistent storage?", default=True):
323+
print("Saving configuration...", end=' ')
324+
if save_all_config(config_interface):
325+
print("OK")
326+
else:
327+
print("FAILED")
328+
sys.exit(1)
329+
else:
330+
print("Changes applied but NOT saved. They will be lost on reboot.")
331+
332+
print()
333+
print("Configuration complete!")
334+
335+
finally:
336+
data_source.stop()
337+
338+
339+
if __name__ == "__main__":
340+
main()

0 commit comments

Comments
 (0)