def move(data):
global q4
new_angles_pos = list(temp)
commands = data
rospy.loginfo(commands)
unit = 4
pos = q4
if commands.linear.z == 2.0:
# q4[2] = q4[2] + unit
pos[2] = pos[2] + unit
if commands.linear.z == -2.0:
# q4[2] = q4[2] - unit
pos[2] = pos[2] - unit
if commands.angular.x == -2.0:
# q4[0] = q4[0] - unit
pos[0] = pos[0] - unit
if commands.angular.x == 2.0:
# q4[0] = q4[0] + unit
pos[0] = pos[0] + unit
if commands.angular.y == -2.0:
# q4[1] = q4[1] - unit
pos[1] = pos[1] - unit
if commands.angular.y == 2.0:
# q4[1] = q4[1] + unit
pos[1] = pos[1] + unit
new_angles = joints_angles(pos[0], pos[1], pos[2])
new_angles_pos[0] = new_angles[0] * RAD2POS + ZERO_OFFSET
new_angles_pos[1] = new_angles[1] * RAD2POS + ZERO_OFFSET
new_angles_pos[2] = new_angles[2] * RAD2POS + ZERO_OFFSET
angles_msg.data = new_angles_pos
nw_angles_pub.publish(angles_msg)
rospy.loginfo(q4)
def calc_scan_points(angles):
[a04, q4, q5] = sensor_location(angles[0], angles[1], angles[2], angles[3],
angles[4])
d = 2
plain_offset = -31
x7 = np.array(range(-20, 22, d))
y7 = np.array(range(-20, 22, d))
z71 = plain_offset * np.ones(len(y7))
z72 = (plain_offset - 0.8) * np.ones(len(y7))
size = len(y7)**2
scan_matrix = list(
np.array([np.ones(size),
np.ones(size),
np.ones(size),
np.ones(size)]))
i = 0
j = 0
index = 0
while index < size:
scan_matrix[0][index] = x7[i]
scan_matrix[1][index] = y7[j]
if index <= int(size / 2):
scan_matrix[2][index] = z71[i]
elif index > int(size / 2):
scan_matrix[2][index] = z72[i]
i += 1
index += 1
if x7[i] == -20 or x7[i] == 20:
scan_matrix[0][index] = x7[i]
scan_matrix[1][index] = y7[j]
if index < int(size / 2):
scan_matrix[2][index] = z71[i]
elif index >= int(size / 2):
scan_matrix[2][index] = z72[i]
x7 = np.flipud(x7)
j += 1
index += 1
i = 0 # ??????
world_point_mat = a04.dot(scan_matrix)
# print(world_point_mat)
# calc World's points
h = 0
world_angles = (np.array([np.zeros(size), np.zeros(size), np.zeros(size)]))
while h < len(world_point_mat[0]):
some_angles = joints_angles(world_point_mat[0][h],
world_point_mat[1][h],
world_point_mat[2][h])
world_angles[0][h] = some_angles[0]
world_angles[1][h] = some_angles[1]
world_angles[2][h] = some_angles[2]
h += 1
# world_angles = np.transpose(world_angles)
return world_angles
def calc_scan_points(angles):
[a04, q5, q7] = sensor_location(angles[0], angles[1], angles[2], angles[3],
angles[4])
plain_offset = -l4
x7 = np.array(range(-25, 27, 2))
y7 = np.array(range(-25, 27, 2))
# xx7, yy7 = np.meshgrid(x7, y7)
z7 = plain_offset * np.ones(len(y7))
size = len(y7)**2
scan_matrix = list(
np.array([np.ones(size),
np.ones(size),
np.ones(size),
np.ones(size)]))
scan_matrix[-1][-1] = 1
i = 0
j = 0
index = 0
while index < size:
scan_matrix[0][index] = x7[i]
scan_matrix[1][index] = y7[j]
scan_matrix[2][index] = z7[i]
i += 1
index += 1
if x7[i] == -25 or x7[i] == 25:
scan_matrix[0][index] = x7[i]
scan_matrix[1][index] = y7[j]
scan_matrix[2][index] = z7[i]
x7 = np.flipud(x7)
j += 1
index += 1
i = 0 # ??????
world_point_mat = a04.dot(scan_matrix)
# print(world_point_mat)
# calc World's points
h = 0
world_angles = (np.array([np.zeros(size), np.zeros(size), np.zeros(size)]))
while h < len(world_point_mat[0]):
some_angles = joints_angles(world_point_mat[0][h],
world_point_mat[1][h],
world_point_mat[2][h])
world_angles[0][h] = some_angles[0]
world_angles[1][h] = some_angles[1]
world_angles[2][h] = some_angles[2]
h += 1
# world_angles = np.transpose(world_angles)
return world_angles
scan_matrix[2][index] = z72[i]
i += 1
index += 1
if x7[i] == -20 or x7[i] == 20:
scan_matrix[0][index] = x7[i]
scan_matrix[1][index] = y7[j]
if index < int(size / 2):
scan_matrix[2][index] = z71[i]
elif index >= int(size / 2):
scan_matrix[2][index] = z72[i]
x7 = np.flipud(x7)
j += 1
index += 1
i = 0 # ??????
world_point_mat = a04.dot(scan_matrix)
#print(world_point_mat)
# calc World's points
h = 0
world_angles = (np.array([np.zeros(size), np.zeros(size), np.zeros(size)]))
while h < len(world_point_mat[0]):
some_angles = joints_angles(world_point_mat[0][h], world_point_mat[1][h], world_point_mat[2][h])
world_angles[0][h] = some_angles[0]
world_angles[1][h] = some_angles[1]
world_angles[2][h] = some_angles[2]
h += 1
# world_anglesT = np.transpose(world_angles)