def callback(data):
# rospy.loginfo(rospy.get_caller_id() + "I heard")
print("callback")
coordinates = data.data
if (point_approximator.update((coordinates[0] / 10, coordinates[1] / 10))):
if (host_arrived(point_approximator.get_last_approximated_position(),
100)):
print("HOST ARRIVED")
path_holder.clean_path()
publish_steering(Helper.generate_stopping_frames(20))
sub_handler.unsubscribe()
rospy.signal_shutdown("Host finished the route")
elif (host_left_path(
point_approximator.get_last_approximated_position(),
(coordinates[0] / 10, coordinates[1] / 10), 100)):
print("HOST LEFT PATH")
last_pos = point_approximator.get_last_n_approximated_positions(2)
host_theta = Helper.angle_between_two_points(
last_pos[0], last_pos[1])
recalculate_path(int(coordinates[0]), int(coordinates[1]),
host_theta)
calculate_curvatures_and_send_path()
def starting_procedure() -> (tuple, float):
"""Get starting position and angle by measuring positions for x seconds,
then moving platform, and measuring position once again
Returns:
[tuple] -- [initial position]
[float] -- [initial angle]
"""
print("STARTING PROCEDURE")
try:
rospy.init_node("uwb_pos_getter")
except:
print("initialized earlier")
time_to_drive = 100
how_many_frames = 40
print("STARTING")
starting_pos = get_UWB_position()
# starting_pos = [0, 0]
print("ZERO POSITION: ", starting_pos)
starting_run = Helper.generate_straight_run_frames(how_many_frames)
publish_steering(starting_run)
print("FORWARD RUN")
# forward_roll_run()
print("END FORWARD RUN")
time.sleep(5)
next_pos = get_UWB_position()
# next_pos = [0, 20]
# next_pos = starting_pos
# next_pos[0] += 100
print("next post", next_pos)
angle = Helper.angle_between_two_points(starting_pos, next_pos)
print("AFTER FORWARD ROLL: ", next_pos, " ANGLE: ", angle)
return next_pos, Helper.angle_between_two_points(starting_pos, next_pos)
