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)