Skip to content

Get the closest voltile to the scout

Carter Frost edited this page Aug 1, 2020 · 3 revisions
from gazebo_msgs.srv import GetModelState
import math
cur_pose = model_coordinates("scout_1", 'world').pose.position
all_vols_poses =[ model_coordinates("vol_"+str(num), "world").pose.position for num in range(0,29) ]
closest_vol_pose = min(all_vols_poses, key=lambda k: math.sqrt((k.x - cur_pose.x) ** 2 + (k.y - cur_pose.y) ** 2))

should yield something like this https://files.slack.com/files-pri/TNTGK1EUS-F018489HZM1/image.png

Clone this wiki locally