def __process_goal(self, goal):
self.client.send_goal(goal)
r = rospy.Rate(self.check_frequency)
timed_out = False
start_time = rospy.Time.now()
while not rospy.is_shutdown() and not self.__is_done(
self.client.get_state()) and not timed_out:
if rospy.Time.now(
) > start_time + goal.timeout + self.timeout_padding:
timed_out = True
break
r.sleep()
#This shouldn't happen, but could in rare cases where the server hangs
if timed_out:
self.client.cancel_goal()
raise tf2.TimeoutException(
"The LookupTransform goal sent to the BufferServer did not come back in the specified time. Something is likely wrong with the server"
)
if self.client.get_state() != GoalStatus.SUCCEEDED:
raise tf2.TimeoutException(
"The LookupTransform goal sent to the BufferServer did not come back with SUCCEEDED status. Something is likely wrong with the server."
)
return self.__process_result(self.client.get_result())
def __process_goal(self, goal):
# TODO(sloretz) why is this an action client? Service seems more appropriate.
if not self.action_client.server_is_ready():
raise tf2.TimeoutException("The BufferServer is not ready")
event = threading.Event()
def unblock(future):
nonlocal event
event.set()
send_goal_future = self.action_client.send_goal_async(goal)
send_goal_future.add_done_callback(unblock)
def unblock_by_timeout():
nonlocal send_goal_future, goal, event
clock = Clock()
start_time = clock.now()
timeout = Duration.from_msg(goal.timeout)
timeout_padding = Duration(seconds=self.timeout_padding)
while not send_goal_future.done() and not event.is_set():
if clock.now() > start_time + timeout + timeout_padding:
break
# TODO(vinnamkim): rclpy.Rate is not ready
# See https://github.com/ros2/rclpy/issues/186
#r = rospy.Rate(self.check_frequency)
sleep(1.0 / self.check_frequency)
event.set()
t = threading.Thread(target=unblock_by_timeout)
t.start()
event.wait()
#This shouldn't happen, but could in rare cases where the server hangs
if not send_goal_future.done():
raise tf2.TimeoutException(
"The LookupTransform goal sent to the BufferServer did not come back in the specified time. Something is likely wrong with the server"
)
# Raises if future was given an exception
goal_handle = send_goal_future.result()
if not goal_handle.accepted:
raise tf2.TimeoutException(
"The LookupTransform goal sent to the BufferServer did not come back with accepted status. Something is likely wrong with the server."
)
response = self.action_client._get_result(goal_handle)
return self.__process_result(response.result)
def __process_goal(self, goal: LookupTransformGoal) -> TransformStamped:
# TODO(sloretz) why is this an action client? Service seems more appropriate.
if not self.action_client.server_is_ready():
raise tf2.TimeoutException("The BufferServer is not ready.")
event = threading.Event()
def unblock(future):
nonlocal event
event.set()
send_goal_future = self.action_client.send_goal_async(goal)
send_goal_future.add_done_callback(unblock)
def unblock_by_timeout():
nonlocal send_goal_future, goal, event
clock = Clock()
start_time = clock.now()
timeout = Duration.from_msg(goal.timeout)
timeout_padding = Duration(seconds=self.timeout_padding)
while not send_goal_future.done() and not event.is_set():
if clock.now() > start_time + timeout + timeout_padding:
break
# TODO(Anyone): We can't use Rate here because it would never expire
# with a single-threaded executor.
# See https://github.com/ros2/geometry2/issues/327 for ideas on
# how to timeout waiting for transforms that don't block the executor.
sleep(1.0 / self.check_frequency)
event.set()
t = threading.Thread(target=unblock_by_timeout)
t.start()
event.wait()
#This shouldn't happen, but could in rare cases where the server hangs
if not send_goal_future.done():
raise tf2.TimeoutException("The LookupTransform goal sent to the BufferServer did not come back in the specified time. Something is likely wrong with the server.")
# Raises if future was given an exception
goal_handle = send_goal_future.result()
if not goal_handle.accepted:
raise tf2.TimeoutException("The LookupTransform goal sent to the BufferServer did not come back with accepted status. Something is likely wrong with the server.")
response = self.action_client._get_result(goal_handle)
return self.__process_result(response.result)
def __process_goal(self, goal):
self.client.send_goal(goal)
if not self.client.wait_for_result(goal.timeout +
self.timeout_padding):
#This shouldn't happen, but could in rare cases where the server hangs
raise tf2.TimeoutException(
"The LookupTransform goal sent to the BufferServer did not come back in the specified time. Something is likely wrong with the server"
)
if self.client.get_state() != GoalStatus.SUCCEEDED:
raise tf2.TimeoutException(
"The LookupTransform goal sent to the BufferServer did not come back with SUCCEEDED status. Something is likely wrong with the server."
)
return self.__process_result(self.client.get_result())
def __process_result(self, result):
if result == None or result.error == None:
raise tf2.TransformException("The BufferServer returned None for result or result.error! Something is likely wrong with the server.")
if result.error.error != result.error.NO_ERROR:
if result.error.error == result.error.LOOKUP_ERROR:
raise tf2.LookupException(result.error.error_string)
if result.error.error == result.error.CONNECTIVITY_ERROR:
raise tf2.ConnectivityException(result.error.error_string)
if result.error.error == result.error.EXTRAPOLATION_ERROR:
raise tf2.ExtrapolationException(result.error.error_string)
if result.error.error == result.error.INVALID_ARGUMENT_ERROR:
raise tf2.InvalidArgumentException(result.error.error_string)
if result.error.error == result.error.TIMEOUT_ERROR:
raise tf2.TimeoutException(result.error.error_string)
raise tf2.TransformException(result.error.error_string)
return result.transform