Skip to content
GitLab
Explore
Sign in
Register
Primary navigation
Search or go to…
Project
R
Robobin
Manage
Activity
Members
Labels
Plan
Issues
Issue boards
Milestones
Wiki
Code
Merge requests
Repository
Branches
Commits
Tags
Repository graph
Compare revisions
Snippets
Deploy
Releases
Package registry
Model registry
Operate
Terraform modules
Monitor
Incidents
Analyze
Value stream analytics
Contributor analytics
Repository analytics
Model experiments
Help
Help
Support
GitLab documentation
Compare GitLab plans
GitLab community forum
Contribute to GitLab
Provide feedback
Keyboard shortcuts
?
Snippets
Groups
Projects
Show more breadcrumbs
plw1g21
Robobin
Commits
5b338480
Commit
5b338480
authored
7 months ago
by
jlKronos01
Browse files
Options
Downloads
Patches
Plain Diff
Moved graph maker and rtl cli into ros2 folder
parent
bbfaceac
No related branches found
No related tags found
No related merge requests found
Changes
2
Show whitespace changes
Inline
Side-by-side
Showing
2 changed files
ros2/src/robobin/robobin/helpers/graph_maker.py
+1
-1
1 addition, 1 deletion
ros2/src/robobin/robobin/helpers/graph_maker.py
ros2/src/robobin/robobin/helpers/realtime_location_cli_only.py
+75
-59
75 additions, 59 deletions
...src/robobin/robobin/helpers/realtime_location_cli_only.py
with
76 additions
and
60 deletions
ros2/src/robobin/robobin/helpers/graph_maker.py
+
1
−
1
View file @
5b338480
...
@@ -73,7 +73,7 @@ class GraphMaker:
...
@@ -73,7 +73,7 @@ class GraphMaker:
print
(
f
"
Node
{
new_node_index
+
1
}
added at (
{
tag1_x
:
.
2
f
}
,
{
tag1_y
:
.
2
f
}
).
"
)
print
(
f
"
Node
{
new_node_index
+
1
}
added at (
{
tag1_x
:
.
2
f
}
,
{
tag1_y
:
.
2
f
}
).
"
)
def
save_graph
(
self
):
def
save_graph
(
self
):
directory
=
r
"
D:\
Git
h
ub
\
robobin
\ros2\src\robobin\robobin\graphs
"
#Replace with whereever your graphs folder is in the ros2 workspace
directory
=
"
/Users/paulwinpenny/Documents/
Git
H
ub
/
robobin
/Wireless_Communication/UWB/Beacons_tag_position/output
"
os
.
makedirs
(
directory
,
exist_ok
=
True
)
# Create the directory if it doesn't exist
os
.
makedirs
(
directory
,
exist_ok
=
True
)
# Create the directory if it doesn't exist
file_path
=
os
.
path
.
join
(
directory
,
"
graph.json
"
)
file_path
=
os
.
path
.
join
(
directory
,
"
graph.json
"
)
with
open
(
file_path
,
"
w
"
)
as
f
:
with
open
(
file_path
,
"
w
"
)
as
f
:
...
...
This diff is collapsed.
Click to expand it.
ros2/src/robobin/robobin/helpers/realtime_location_cli_only.py
+
75
−
59
View file @
5b338480
...
@@ -70,6 +70,7 @@ class AnchorTagCLI:
...
@@ -70,6 +70,7 @@ class AnchorTagCLI:
self
.
tag2_distances
=
{
"
A1
"
:
0.0
,
"
A2
"
:
0.0
,
"
A3
"
:
0.0
,
"
A4
"
:
0.0
}
self
.
tag2_distances
=
{
"
A1
"
:
0.0
,
"
A2
"
:
0.0
,
"
A3
"
:
0.0
,
"
A4
"
:
0.0
}
self
.
anchors
=
{}
self
.
anchors
=
{}
self
.
anchorHeight
=
250
self
.
anchors_coords_known
=
False
self
.
anchors_coords_known
=
False
if
self
.
testing
:
if
self
.
testing
:
...
@@ -86,25 +87,30 @@ class AnchorTagCLI:
...
@@ -86,25 +87,30 @@ class AnchorTagCLI:
def
determine_anchor_coords
(
self
):
def
determine_anchor_coords
(
self
):
try
:
try
:
measured_distances
=
np
.
array
(
self
.
measured_distances
)
measured_distances
=
np
.
array
(
self
.
measured_distances
)
noise_level
=
0.0
y_A
=
measured_distances
[
2
]
# Distance from A to D
measured_distances_noisy
=
measured_distances
+
np
.
random
.
uniform
(
-
noise_level
,
noise_level
,
size
=
len
(
measured_distances
))
# Variables: x_B, y_B, x_C, y_C, y_A
# Guess for B based on distance A-B and B-D
initial_guess
=
[
120
,
100
,
150
,
200
,
50
]
# [x_B, y_B, x_C, y_C, y_A]
x_B
=
measured_distances
[
0
]
/
2
maxBounds
=
30000
y_B
=
measured_distances
[
4
]
/
2
+
200
# Start y_B above y_C
bounds
=
([
0
,
0
,
0
,
0
,
0
],
[
maxBounds
]
*
5
)
# Guess for C with symmetrical logic
x_C
=
-
measured_distances
[
5
]
/
2
# Allow for negative x_C
y_C
=
-
measured_distances
[
1
]
/
2
# Allow for negative y_C
initial_guess
=
[
x_B
,
y_B
,
x_C
,
y_C
,
y_A
]
min_dist
=
min
(
measured_distances
)
max_dist
=
max
(
measured_distances
)
lower_bounds
=
[
-
max_dist
,
-
max_dist
,
-
max_dist
,
-
max_dist
,
min_dist
/
2
]
upper_bounds
=
[
max_dist
*
1.5
for
i
in
range
(
5
)]
def
error_function
(
variables
,
measured
):
def
error_function
(
variables
,
measured
):
x_B
,
y_B
,
x_C
,
y_C
,
y_A
=
variables
x_B
,
y_B
,
x_C
,
y_C
,
y_A
=
variables
# measured: [A, E, D, B, F, C]
a_measured
=
measured
[
0
]
# Map measured distances to a, e, d, b, f, c
e_measured
=
measured
[
1
]
a_measured
,
e_measured
,
d_measured
,
b_measured
,
f_measured
,
c_measured
=
measured
d_measured
=
measured
[
2
]
b_measured
=
measured
[
3
]
# Compute each distance
f_measured
=
measured
[
4
]
c_measured
=
measured
[
5
]
# A=(0,y_A), B=(x_B,y_B), C=(x_C,y_C), D=(0,0)
a_calc
=
np
.
sqrt
((
x_B
-
0
)
**
2
+
(
y_B
-
y_A
)
**
2
)
# A-B
a_calc
=
np
.
sqrt
((
x_B
-
0
)
**
2
+
(
y_B
-
y_A
)
**
2
)
# A-B
b_calc
=
np
.
sqrt
((
x_C
-
x_B
)
**
2
+
(
y_C
-
y_B
)
**
2
)
# B-C
b_calc
=
np
.
sqrt
((
x_C
-
x_B
)
**
2
+
(
y_C
-
y_B
)
**
2
)
# B-C
c_calc
=
np
.
sqrt
(
x_C
**
2
+
y_C
**
2
)
# C-D
c_calc
=
np
.
sqrt
(
x_C
**
2
+
y_C
**
2
)
# C-D
...
@@ -112,30 +118,27 @@ class AnchorTagCLI:
...
@@ -112,30 +118,27 @@ class AnchorTagCLI:
e_calc
=
np
.
sqrt
(
x_C
**
2
+
(
y_C
-
y_A
)
**
2
)
# A-C
e_calc
=
np
.
sqrt
(
x_C
**
2
+
(
y_C
-
y_A
)
**
2
)
# A-C
f_calc
=
np
.
sqrt
(
x_B
**
2
+
y_B
**
2
)
# B-D
f_calc
=
np
.
sqrt
(
x_B
**
2
+
y_B
**
2
)
# B-D
return
[
# Residuals
a_calc
-
a_measured
,
r_a
=
a_calc
-
a_measured
b_calc
-
b_measured
,
r_b
=
b_calc
-
b_measured
c_calc
-
c_measured
,
r_c
=
c_calc
-
c_measured
d_calc
-
d_measured
,
r_d
=
d_calc
-
d_measured
e_calc
-
e_measured
,
r_e
=
e_calc
-
e_measured
f_calc
-
f_measured
r_f
=
f_calc
-
f_measured
]
# Run least squares optimization
result_noisy
=
least_squares
(
error_function
,
initial_guess
,
args
=
(
measured_distances_noisy
,),
bounds
=
bounds
,
loss
=
'
soft_l1
'
)
optimized_coords_noisy
=
result_noisy
.
x
# Add a smoother penalty if y_B <= y_C
penalty
=
1e3
*
max
(
0
,
y_C
-
y_B
+
10
)
# Soft penalty to enforce constraint
return
[
r_a
,
r_b
,
r_c
,
r_d
,
r_e
,
r_f
,
penalty
]
result
=
least_squares
(
error_function
,
initial_guess
,
args
=
(
measured_distances
,),
bounds
=
(
lower_bounds
,
upper_bounds
),
loss
=
'
soft_l1
'
)
x_B
,
y_B
,
x_C
,
y_C
,
y_A
=
result
.
x
self
.
anchors
=
{
self
.
anchors
=
{
"
A1
"
:
(
0
,
optimized_coords_noisy
[
4
]
),
"
A1
"
:
(
0
,
y_A
,
self
.
anchorHeight
),
"
A2
"
:
(
optimized_coords_noisy
[
0
],
optimized_coords_noisy
[
1
]
),
"
A2
"
:
(
x_B
,
y_B
,
self
.
anchorHeight
),
"
A3
"
:
(
optimized_coords_noisy
[
2
],
optimized_coords_noisy
[
3
]
),
"
A3
"
:
(
x_C
,
y_C
,
self
.
anchorHeight
),
"
A4
"
:
(
0
,
0
)
,
"
A4
"
:
(
0
,
0
,
self
.
anchorHeight
)
}
}
return
{
k
:
(
round
(
v
[
0
],
2
),
round
(
v
[
1
],
2
))
for
k
,
v
in
self
.
anchors
.
items
()}
return
{
k
:
(
round
(
v
[
0
],
2
),
round
(
v
[
1
],
2
))
for
k
,
v
in
self
.
anchors
.
items
()}
except
Exception
as
e
:
except
Exception
as
e
:
...
@@ -155,27 +158,39 @@ class AnchorTagCLI:
...
@@ -155,27 +158,39 @@ class AnchorTagCLI:
if
len
(
available_beacons
)
<
3
:
if
len
(
available_beacons
)
<
3
:
return
None
,
None
return
None
,
None
def
error_function
(
vars
):
heights
=
[]
x
,
y
=
vars
for
(
bx
,
by
,
bz
),
d_measured
in
zip
(
available_beacons
,
available_distances
):
residuals
=
[]
horizontal_distance
=
np
.
sqrt
((
bx
-
0
)
**
2
+
(
by
-
0
)
**
2
)
# Assume tag starts at (0, 0)
for
(
bx
,
by
),
d_measured
in
zip
(
available_beacons
,
available_distances
):
estimated_z
=
np
.
sqrt
(
max
(
0
,
d_measured
**
2
-
horizontal_distance
**
2
))
# Ensure non-negative
d_computed
=
np
.
sqrt
((
x
-
bx
)
**
2
+
(
y
-
by
)
**
2
)
heights
.
append
(
bz
-
estimated_z
)
# Estimate tag height relative to beacon
residuals
.
append
(
d_computed
-
d_measured
)
return
residuals
initial_z
=
max
(
0
,
min
(
self
.
anchorHeight
,
np
.
mean
(
heights
)))
# Constrain height to [0, 200]
beacon_xs
=
[
b
[
0
]
for
b
in
available_beacons
]
beacon_xs
=
[
b
[
0
]
for
b
in
available_beacons
]
beacon_ys
=
[
b
[
1
]
for
b
in
available_beacons
]
beacon_ys
=
[
b
[
1
]
for
b
in
available_beacons
]
initial_guess
=
[
np
.
mean
(
beacon_xs
),
np
.
mean
(
beacon_ys
)]
initial_guess
=
[
np
.
mean
(
beacon_xs
),
np
.
mean
(
beacon_ys
)
,
initial_z
]
x_min
=
min
(
beacon_xs
)
-
100
# Define bounds for the tag position
x_max
=
max
(
beacon_xs
)
+
100
x_min
=
min
(
beacon_xs
)
-
1000
# Add a small margin around the beacons
y_min
=
min
(
beacon_ys
)
-
100
x_max
=
max
(
beacon_xs
)
+
1000
y_max
=
max
(
beacon_ys
)
+
100
y_min
=
min
(
beacon_ys
)
-
1000
bounds
=
([
x_min
,
y_min
],
[
x_max
,
y_max
])
y_max
=
max
(
beacon_ys
)
+
1000
z_min
=
0.0
# Tag's height cannot be below the ground
z_max
=
self
.
anchorHeight
# Tag's height cannot exceed the beacon height
bounds
=
([
x_min
,
y_min
,
z_min
],
[
x_max
,
y_max
,
z_max
])
def
error_function
(
vars
):
x
,
y
,
z
=
vars
residuals
=
[]
for
(
bx
,
by
,
bz
),
d_measured
in
zip
(
available_beacons
,
available_distances
):
d_computed
=
np
.
sqrt
((
x
-
bx
)
**
2
+
(
y
-
by
)
**
2
+
(
z
-
bz
)
**
2
)
residuals
.
append
(
d_computed
-
d_measured
)
return
residuals
result
=
least_squares
(
error_function
,
initial_guess
,
bounds
=
bounds
,
loss
=
'
soft_l1
'
)
result
=
least_squares
(
error_function
,
initial_guess
,
bounds
=
bounds
,
loss
=
'
soft_l1
'
)
x_tag
,
y_tag
=
result
.
x
# self.get_logger().info(f"Optimization result: {result.x}")
return
x_tag
,
y_tag
return
tuple
(
result
.
x
)
def
call_bpm
(
self
):
def
call_bpm
(
self
):
if
self
.
testing
:
if
self
.
testing
:
...
@@ -216,24 +231,24 @@ class AnchorTagCLI:
...
@@ -216,24 +231,24 @@ class AnchorTagCLI:
self
.
tag2_distances
[
anchor
]
=
distance
self
.
tag2_distances
[
anchor
]
=
distance
# Now calculate both tags
# Now calculate both tags
tag1_x
,
tag1_y
=
self
.
calculate_tag_coordinates
(
self
.
tag_distances
)
tag1_x
,
tag1_y
,
tag1_z
=
self
.
calculate_tag_coordinates
(
self
.
tag_distances
)
valid_tag2_distances
=
[
dist
for
dist
in
self
.
tag2_distances
.
values
()
if
dist
>
0
]
valid_tag2_distances
=
[
dist
for
dist
in
self
.
tag2_distances
.
values
()
if
dist
>
0
]
# Check if there are enough valid distances for Tag 2
# Check if there are enough valid distances for Tag 2
if
len
(
valid_tag2_distances
)
<
3
:
if
len
(
valid_tag2_distances
)
<
3
:
print
(
f
"
Insufficient valid distances for Tag 2:
{
len
(
valid_tag2_distances
)
}
provided.
"
)
print
(
f
"
Insufficient valid distances for Tag 2:
{
len
(
valid_tag2_distances
)
}
provided.
"
)
tag2_x
,
tag2_y
=
None
,
None
tag2_x
,
tag2_y
,
tag2_z
=
None
,
None
,
None
else
:
else
:
tag2_x
,
tag2_y
=
self
.
calculate_tag_coordinates
(
self
.
tag2_distances
)
tag2_x
,
tag2_y
,
tag2_z
=
self
.
calculate_tag_coordinates
(
self
.
tag2_distances
)
print
(
"
Tag Positions:
"
)
print
(
"
Tag Positions:
"
)
if
tag1_x
is
not
None
and
tag1_y
is
not
None
:
if
tag1_x
is
not
None
and
tag1_y
is
not
None
:
print
(
f
"
Tag 1: (
{
tag1_x
:
.
2
f
}
,
{
tag1_y
:
.
2
f
}
)
"
)
print
(
f
"
Tag 1: (
{
tag1_x
:
.
2
f
}
,
{
tag1_y
:
.
2
f
}
,
{
tag1_z
:
.
2
f
}
)
"
)
else
:
else
:
print
(
"
Tag 1: Not enough data
"
)
print
(
"
Tag 1: Not enough data
"
)
if
tag2_x
is
not
None
and
tag2_y
is
not
None
:
if
tag2_x
is
not
None
and
tag2_y
is
not
None
:
print
(
f
"
Tag 2: (
{
tag2_x
:
.
2
f
}
,
{
tag2_y
:
.
2
f
}
)
"
)
print
(
f
"
Tag 2: (
{
tag2_x
:
.
2
f
}
,
{
tag2_y
:
.
2
f
}
,
{
tag2_z
:
.
2
f
}
)
"
)
else
:
else
:
print
(
"
Tag 2: Not enough data
"
)
print
(
"
Tag 2: Not enough data
"
)
...
@@ -241,7 +256,8 @@ class AnchorTagCLI:
...
@@ -241,7 +256,8 @@ class AnchorTagCLI:
if
tag1_x
is
not
None
and
tag1_y
is
not
None
and
tag2_x
is
not
None
and
tag2_y
is
not
None
:
if
tag1_x
is
not
None
and
tag1_y
is
not
None
and
tag2_x
is
not
None
and
tag2_y
is
not
None
:
dx
=
tag2_x
-
tag1_x
dx
=
tag2_x
-
tag1_x
dy
=
tag2_y
-
tag1_y
dy
=
tag2_y
-
tag1_y
displacement
=
np
.
sqrt
(
dx
**
2
+
dy
**
2
)
dz
=
tag2_z
-
tag1_z
displacement
=
np
.
sqrt
(
dx
**
2
+
dy
**
2
+
dz
**
2
)
angle_deg
=
np
.
degrees
(
np
.
arctan2
(
dy
,
dx
))
angle_deg
=
np
.
degrees
(
np
.
arctan2
(
dy
,
dx
))
if
angle_deg
<
0
:
if
angle_deg
<
0
:
angle_deg
+=
360
angle_deg
+=
360
...
...
This diff is collapsed.
Click to expand it.
Preview
0%
Loading
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Save comment
Cancel
Please
register
or
sign in
to comment