def __init__(self, x, y, theta, timeout):
super().__init__('nav_demo')
#self.create_timer(.1, self.timer_callback)
latching_qos = QoSProfile(depth=1,
durability=QoSDurabilityPolicy.
RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL)
self.create_subscription(OccupancyGrid,
'map',
self.map_callback,
qos_profile=latching_qos)
# Create the action client and wait until it is active
#self.ac = ActionClient(self, NavigateToPose, '/NavigateToPose')
self.ac = ActionClient(self, NavigateToPose, '/navigate_to_pose')
# Set up the goal message
self.goal = NavigateToPose.Goal()
self.goal.pose.header.frame_id = 'map' # SEEMS TO BE IGNORED!
self.goal.pose.pose.position.x = x
self.goal.pose.pose.position.y = y
# We need to convert theta to a quaternion....
quaternion = transformations.quaternion_from_euler(0, 0, theta, 'rxyz')
self.goal.pose.pose.orientation.x = quaternion[0]
self.goal.pose.pose.orientation.y = quaternion[1]
self.goal.pose.pose.orientation.z = quaternion[2]
self.goal.pose.pose.orientation.w = quaternion[3]
self.timeout = timeout
def __init__(self, node, ns, check_frequency = 10.0, timeout_padding = Duration(seconds=2.0)):
"""
.. function:: __init__(ns, check_frequency = 10.0, timeout_padding = rospy.Duration.from_sec(2.0))
Constructor.
:param node: The ROS2 node.
:param ns: The namespace in which to look for a BufferServer.
:param check_frequency: How frequently to check for updates to known transforms.
:param timeout_padding: A constant timeout to add to blocking calls.
"""
tf2_ros.BufferInterface.__init__(self)
self.node = node
self.action_client = ActionClient(node, LookupTransform, action_name=ns)
self.check_frequency = check_frequency
self.timeout_padding = timeout_padding
class BufferClient(tf2_ros.BufferInterface):
"""
Action client-based implementation of BufferInterface.
"""
def __init__(self, node, ns, check_frequency = 10.0, timeout_padding = Duration(seconds=2.0)):
"""
.. function:: __init__(ns, check_frequency = 10.0, timeout_padding = rospy.Duration.from_sec(2.0))
Constructor.
:param node: The ROS2 node.
:param ns: The namespace in which to look for a BufferServer.
:param check_frequency: How frequently to check for updates to known transforms.
:param timeout_padding: A constant timeout to add to blocking calls.
"""
tf2_ros.BufferInterface.__init__(self)
self.node = node
self.action_client = ActionClient(node, LookupTransform, action_name=ns)
self.check_frequency = check_frequency
self.timeout_padding = timeout_padding
# lookup, simple api
def lookup_transform(self, target_frame, source_frame, time, timeout=Duration()):
"""
Get the transform from the source frame to the target frame.
:param target_frame: Name of the frame to transform into.
:param source_frame: Name of the input frame.
:param time: The time at which to get the transform. (0 will get the latest)
:param timeout: (Optional) Time to wait for the target frame to become available.
:return: The transform between the frames.
:rtype: :class:`geometry_msgs.msg.TransformStamped`
"""
goal = LookupTransform.Goal()
goal.target_frame = target_frame
goal.source_frame = source_frame
goal.source_time = time.to_msg()
goal.timeout = timeout.to_msg()
goal.advanced = False
return self.__process_goal(goal)
# lookup, advanced api
def lookup_transform_full(self, target_frame, target_time, source_frame, source_time, fixed_frame, timeout=Duration()):
"""
Get the transform from the source frame to the target frame using the advanced API.
:param target_frame: Name of the frame to transform into.
:param target_time: The time to transform to. (0 will get the latest)
:param source_frame: Name of the input frame.
:param source_time: The time at which source_frame will be evaluated. (0 will get the latest)
:param fixed_frame: Name of the frame to consider constant in time.
:param timeout: (Optional) Time to wait for the target frame to become available.
:return: The transform between the frames.
:rtype: :class:`geometry_msgs.msg.TransformStamped`
"""
goal = LookupTransform.Goal()
goal.target_frame = target_frame
goal.source_frame = source_frame
goal.source_time = source_time.to_msg()
goal.timeout = timeout.to_msg()
goal.target_time = target_time.to_msg()
goal.fixed_frame = fixed_frame
goal.advanced = True
return self.__process_goal(goal)
# can, simple api
def can_transform(self, target_frame, source_frame, time, timeout=Duration()):
"""
Check if a transform from the source frame to the target frame is possible.
:param target_frame: Name of the frame to transform into.
:param source_frame: Name of the input frame.
:param time: The time at which to get the transform. (0 will get the latest)
:param timeout: (Optional) Time to wait for the target frame to become available.
:param return_debug_type: (Optional) If true, return a tuple representing debug information.
:return: True if the transform is possible, false otherwise.
:rtype: bool
"""
try:
self.lookup_transform(target_frame, source_frame, time, timeout)
return True
except tf2.TransformException:
return False
# can, advanced api
def can_transform_full(self, target_frame, target_time, source_frame, source_time, fixed_frame, timeout=Duration()):
"""
Check if a transform from the source frame to the target frame is possible (advanced API).
Must be implemented by a subclass of BufferInterface.
:param target_frame: Name of the frame to transform into.
:param target_time: The time to transform to. (0 will get the latest)
:param source_frame: Name of the input frame.
:param source_time: The time at which source_frame will be evaluated. (0 will get the latest)
:param fixed_frame: Name of the frame to consider constant in time.
:param timeout: (Optional) Time to wait for the target frame to become available.
:param return_debug_type: (Optional) If true, return a tuple representing debug information.
:return: True if the transform is possible, false otherwise.
:rtype: bool
"""
try:
self.lookup_transform_full(target_frame, target_time, source_frame, source_time, fixed_frame, timeout)
return True
except tf2.TransformException:
return False
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_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
class NavNode(rclpy.node.Node):
def __init__(self, x, y, theta, timeout):
super().__init__('nav_demo')
#self.create_timer(.1, self.timer_callback)
latching_qos = QoSProfile(depth=1,
durability=QoSDurabilityPolicy.
RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL)
self.create_subscription(OccupancyGrid,
'map',
self.map_callback,
qos_profile=latching_qos)
# Create the action client and wait until it is active
#self.ac = ActionClient(self, NavigateToPose, '/NavigateToPose')
self.ac = ActionClient(self, NavigateToPose, '/navigate_to_pose')
# Set up the goal message
self.goal = NavigateToPose.Goal()
self.goal.pose.header.frame_id = 'map' # SEEMS TO BE IGNORED!
self.goal.pose.pose.position.x = x
self.goal.pose.pose.position.y = y
# We need to convert theta to a quaternion....
quaternion = transformations.quaternion_from_euler(0, 0, theta, 'rxyz')
self.goal.pose.pose.orientation.x = quaternion[0]
self.goal.pose.pose.orientation.y = quaternion[1]
self.goal.pose.pose.orientation.z = quaternion[2]
self.goal.pose.pose.orientation.w = quaternion[3]
self.timeout = timeout
def send_goal(self):
self.get_logger().info("WAITING FOR NAVIGATION SERVER...")
self.ac.wait_for_server()
self.get_logger().info("NAVIGATION SERVER AVAILABLE...")
self.get_logger().info("SENDING GOAL TO NAVIGATION SERVER...")
self.start_time = time.time()
self.cancel_future = None
self.is_done = False
self.map = None
self.goal_future = self.ac.send_goal_async(self.goal)
self.create_timer(.1, self.timer_callback)
self.future_event = Future()
return self.future_event
def map_callback(self, map_msg):
"""Process the map message. This doesn't really do anything useful, it is
purely intended as an illustration of the Map class.
"""
if self.map is None: # No need to do this every time map is published.
self.map = map_utils.Map(map_msg)
# Use numpy to calculate some statistics about the map:
total_cells = self.map.width * self.map.height
pct_occupied = np.count_nonzero(
self.map.grid == 100) / total_cells * 100
pct_unknown = np.count_nonzero(
self.map.grid == -1) / total_cells * 100
pct_free = np.count_nonzero(self.map.grid == 0) / total_cells * 100
map_str = "Map Statistics: occupied: {:.1f}% free: {:.1f}% unknown: {:.1f}%"
self.get_logger().info(
map_str.format(pct_occupied, pct_free, pct_unknown))
# Here is how to access map cells to see if they are free:
# x = 2.06
# y = -1.2
x = self.goal.pose.pose.position.x
y = self.goal.pose.pose.position.y
val = self.map.get_cell(x, y)
if val == 100:
free = "occupied"
elif val == 0:
free = "free"
else:
free = "unknown"
self.get_logger().info(
"HEY! Map position ({:.2f}, {:.2f}) is {}".format(x, y, free))
def done(self):
""" This allows the node to act as a future object. """
return self.is_done
def timer_callback(self):
""" Periodically check in on the progress of navigation. """
if not self.goal_future.done():
self.get_logger().info("NAVIGATION GOAL NOT YET ACCEPTED")
elif self.cancel_future is not None: # We've cancelled and are waiting for ack.
if self.cancel_future.done():
self.ac.destroy()
self.is_done = True
self.future_event.set_result(None)
else:
if self.goal_future.result().status == GoalStatus.STATUS_SUCCEEDED:
self.get_logger().info(
"NAVIGATION SERVER REPORTS SUCCESS. EXITING!")
self.ac.destroy()
self.is_done = True
self.future_event.set_result(None)
if self.goal_future.result().status == GoalStatus.STATUS_ABORTED:
self.get_logger().info(
"NAVIGATION SERVER HAS ABORTED. EXITING!")
self.ac.destroy()
self.is_done = True
self.future_event.set_result(None)
elif time.time() - self.start_time > self.timeout:
self.get_logger().info("TAKING TOO LONG. CANCELLING GOAL!")
self.cancel_future = self.goal_future.result(
).cancel_goal_async()
self.future_event.set_result(None)