def handle_navigate_to(self, msg):
"""ROS service handler to run mission of the robot. The robot will replay a mission"""
# create thread to periodically publish feedback
feedback_thraed = threading.Thread(target = self.handle_navigate_to_feedback, args = ())
self.run_navigate_to = True
feedback_thraed.start()
# run navigate_to
resp = self.spot_wrapper.navigate_to(upload_path = msg.upload_path,
navigate_to = msg.navigate_to,
initial_localization_fiducial = msg.initial_localization_fiducial,
initial_localization_waypoint = msg.initial_localization_waypoint)
self.run_navigate_to = False
feedback_thraed.join()
# check status
if resp[0]:
self.navigate_as.set_succeeded(NavigateToResult(resp[0], resp[1]))
else:
self.navigate_as.set_aborted(NavigateToResult(resp[0], resp[1]))
def handle_navigate_to(self, msg):
"""ROS service handler to run mission of the robot. The robot will replay a mission"""
# create thread to periodically publish feedback
feedback_thread = threading.Thread(target = self.handle_navigate_to_feedback, args=())
preemption_thread = threading.Thread(target=self.handle_navigate_to_preemption, args=())
self.run_navigate_to = True
feedback_thread.start()
preemption_thread.start()
# run navigate_to
resp = self.spot_wrapper.navigate_to(id_navigate_to=msg.id_navigate_to)
self.run_navigate_to = False
feedback_thread.join()
preemption_thread.join()
# check status
if resp[0]:
self.navigate_as.set_succeeded(NavigateToResult(resp[0], resp[1]))
elif not resp[0] and resp[2] == 'preempted':
self.navigate_as.set_preempted(NavigateToResult(resp[0], resp[1]))
else:
self.navigate_as.set_aborted(NavigateToResult(resp[0], resp[1]))