1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
|
import numpy as np
def rotate_vector(vector, angle):
"""
将向量旋转一定的角度
"""
angle_rad = np.deg2rad(angle)
rotation_matrix = np.array([[np.cos(angle_rad), -np.sin(angle_rad)],
[np.sin(angle_rad), np.cos(angle_rad)]])
rotated_vector = np.dot(rotation_matrix, vector)
rotated_vector = np.around(rotated_vector, decimals=3)
return rotated_vector
def radians_to_vector(theta):
"""
将theta转化为向量
"""
x = np.cos(theta)*1
y = np.sin(theta)*1
x = np.around(x, decimals=3)
y = np.around(y, decimals=3)
return np.array([x, y])
def is_angle_less_than_X(x, v1, v2):
dot_product = np.dot(v1, v2)
v1_norm = np.linalg.norm(v1)
v2_norm = np.linalg.norm(v2)
cos_angle = dot_product / (v1_norm * v2_norm)
cos_x_deg = np.cos(np.deg2rad(x))
return cos_angle > cos_x_deg
def is_angle_greater_than_90(v1, v2):
"""
判断两向量夹角是否大于90度，大于返回True
"""
dot_product = np.dot(v1, v2)
return dot_product < 0
class BoardTrue:
"""
用于判断并筛选出有效的板子
"""
def __init__(self, my_list, car_x, car_y, car_theta, cv_po):
self.my_list = my_list
self.car_x = np.around(car_x, decimals=3)
self.car_y = np.around(car_y, decimals=3)
self.car_theta = np.around(car_theta, decimals=3)
self.cv_po = np.around(cv_po, decimals=3)
def chose_board(self):
if len(self.my_list) == 0:
return None
else:
n = len(self.my_list)
print(n)
for i in range(n):
if 0.2 < self.my_list[i][2] < 0.7:
x = self.my_list[i][0] - self.car_x
y = self.my_list[i][1] - self.car_y
v1 = np.array([x, y])
v2 = radians_to_vector(self.car_theta)
print("i为" + str(i))
print(is_angle_less_than_X(70, v1, v2))
if is_angle_less_than_X(70, v1, v2):
print("dfg")
mapped_value = (self.cv_po - (0)) * (70 - (-10)) / (640 - 0) + (-70)
v3 = rotate_vector(v2, mapped_value)
if is_angle_less_than_X(25, v1, v3):
print("asd")
return self.my_list[i]
else:
continue
else:
continue
return None
class BoardWithCv:
"""
使用此类时，请确保传入的板子是一个且有效,他会返回一个导航点x,y,theta
"""
def __init__(self, car_x, car_y, car_theta, board_x, board_y, board_theta):
self.car_x = np.around(car_x, decimals=3)
self.car_y = np.around(car_y, decimals=3)
self.car_theta = np.around(car_theta, decimals=3)
self.board_x = np.around(board_x, decimals=3)
self.board_y = np.around(board_y, decimals=3)
self.board_theta = np.around(board_theta, decimals=3)
def is_board_theta_ture(self):
"""
判断并执行板子的角度是否需要反转,并返回导航点,弧度，是否有效
:return:
"""
v1 = np.array([self.board_x - self.car_x, self.board_y - self.car_y])
v2 = radians_to_vector(self.board_theta)
if is_angle_greater_than_90(v1, v2):
self.board_theta = self.board_theta + 3.141
v2 = radians_to_vector(self.board_theta)
goal_x = np.around(self.board_x - 0.3 * v2[0], decimals=3)
goal_y = np.around(self.board_y - 0.3 * v2[1], decimals=3)
return goal_x, goal_y, self.board_theta
if __name__ == "__main__":
board = [-1.6258577108383179, 1.445590853691101, 0.456392377614975, 0.011698315851390362, 1.518639469495104]
pos = BoardWithCv(-2, 0, 1.57, board[0], board[1], board[4])
print(pos.is_board_theta_ture())
my_list = [[-1, 1,0.5,0.02, 0.785], [-3, 1,0.5,0.02,3.14-0.785], [-2, 1,0.5,0.02,1.3]]
my_list2 =[]
my_list3 = [[-1.1039749383926392, -1.6410338878631592, 0.459942102432251, 0.006066703703254461, 2.404021134944642], [-1.0234383344650269, 1.4331644773483276, 0.6236299276351929, 0.0975717082619667, 0.6774194980518585], [-1.6258577108383179, 1.445590853691101, 0.456392377614975, 0.011698315851390362, 1.518639469495104], [-2.016007423400879, 1.6973320245742798, 0.17067547142505646, 0.008529485203325748, 1.4121627729088593], [-2.3203864097595215, 1.3803951740264893, 0.4648244082927704, 0.011016763746738434, 2.12274152271989]]
my_list4 = [[-2.2005648612976074, -1.9698346853256226, 0.26301005482673645, 0.02629818394780159, 1.2184761268334674], [-1.6092170476913452, -1.8576725721359253, 0.3533917963504791, 0.029181374236941338, 1.9172304414045462], [-1.0438616275787354, -1.4628329277038574, 0.47950631380081177, 0.013515759259462357, 2.4409537863628334], [-1.6358827352523804, 1.4406025409698486, 1.5764230489730835, 0.26953673362731934, 1.4236640668400438]]
my_list5 = [[-2.2020256519317627, -1.9928362369537354, 0.2039794772863388, 0.025548739358782768, 1.1120026801359906], [-1.6541303396224976, -1.898160457611084, 0.3227234482765198, 0.01702151633799076, 1.7969596897315867], [-1.4612840414047241, -1.1711866855621338, 0.4812791049480438, 0.05817915499210358, 2.159874044377892], [-1.0457868576049805, 1.2947330474853516, 0.44938141107559204, 0.009451523423194885, 0.8893285575700086], [-2.283306121826172, 1.518304705619812, 0.4721873700618744, 0.007294466253370047, 2.0108069978910965]]
posa = BoardTrue(my_list5, -2, 0, 0.8, 331)
print(posa.chose_board())
"""
[00:15:08.34] 前往pose_3
{u'spontoon': 284.0}
targetspontoon
first_1{u'spontoon': 284.0}
boards[[-2.20574951171875, -1.9964897632598877, 0.19374234974384308, 0.0202349741011858, 1.0713866771931375], [-1.6379033327102661, -1.9009284973144531, 0.34384506940841675, 0.010412354953587055, 1.8343551319682068], [-1.4544870853424072, -1.176705002784729, 0.4698032736778259, 0.053842246532440186, 2.147697938715995], [-1.02494215965271, 1.2817209959030151, 0.468691885471344, 0.005473896395415068, 0.8983430915438178], [-2.271810293197632, 1.5118887424468994, 0.46807199716567993, 0.00690682977437973, 2.0043066211443765]]
[-1.02494215965271, 1.2817209959030151, 0.468691885471344, 0.005473896395415068, 0.8983430915438178]
quat=[0, 0, 0.434, 0.901]
send the goal
"""
|