def shutdown(self):
self.info_msg('Shutting down')
self.action_client.destroy()
transition_service = 'lifecycle_manager_navigation/manage_nodes'
mgr_client = self.create_client(ManageLifecycleNodes, transition_service)
while not mgr_client.wait_for_service(timeout_sec=1.0):
self.info_msg(transition_service + ' service not available, waiting...')
req = ManageLifecycleNodes.Request()
req.command = ManageLifecycleNodes.Request().SHUTDOWN
future = mgr_client.call_async(req)
try:
self.info_msg('Shutting down navigation lifecycle manager...')
rclpy.spin_until_future_complete(self, future)
future.result()
self.info_msg('Shutting down navigation lifecycle manager complete.')
except Exception as e: # noqa: B902
self.error_msg('Service call failed %r' % (e,))
transition_service = 'lifecycle_manager_localization/manage_nodes'
mgr_client = self.create_client(ManageLifecycleNodes, transition_service)
while not mgr_client.wait_for_service(timeout_sec=1.0):
self.info_msg(transition_service + ' service not available, waiting...')
req = ManageLifecycleNodes.Request()
req.command = ManageLifecycleNodes.Request().SHUTDOWN
future = mgr_client.call_async(req)
try:
self.info_msg('Shutting down localization lifecycle manager...')
rclpy.spin_until_future_complete(self, future)
future.result()
self.info_msg('Shutting down localization lifecycle manager complete')
except Exception as e: # noqa: B902
self.error_msg('Service call failed %r' % (e,))
def lifecycleStartup(self):
"""Startup nav2 lifecycle system."""
self.info('Starting up lifecycle nodes based on lifecycle_manager.')
for srv_name, srv_type in self.get_service_names_and_types():
if srv_type[0] == 'nav2_msgs/srv/ManageLifecycleNodes':
self.info(f'Starting up {srv_name}')
mgr_client = self.create_client(ManageLifecycleNodes, srv_name)
while not mgr_client.wait_for_service(timeout_sec=1.0):
self.info(f'{srv_name} service not available, waiting...')
req = ManageLifecycleNodes.Request()
req.command = ManageLifecycleNodes.Request().STARTUP
future = mgr_client.call_async(req)
# starting up requires a full map->odom->base_link TF tree
# so if we're not successful, try forwarding the initial pose
while True:
rclpy.spin_until_future_complete(self,
future,
timeout_sec=0.10)
if not future:
self._waitForInitialPose()
else:
break
self.info('Nav2 is ready for use!')
return
def lifecycleShutdown(self):
"""Shutdown nav2 lifecycle system."""
self.info('Shutting down lifecycle nodes based on lifecycle_manager.')
for srv_name, srv_type in self.get_service_names_and_types():
if srv_type[0] == 'nav2_msgs/srv/ManageLifecycleNodes':
self.info(f'Shutting down {srv_name}')
mgr_client = self.create_client(ManageLifecycleNodes, srv_name)
while not mgr_client.wait_for_service(timeout_sec=1.0):
self.info(f'{srv_name} service not available, waiting...')
req = ManageLifecycleNodes.Request()
req.command = ManageLifecycleNodes.Request().SHUTDOWN
future = mgr_client.call_async(req)
rclpy.spin_until_future_complete(self, future)
future.result()
return
def shutdown(self):
self.info_msg('Shutting down')
self.action_client.destroy()
self.info_msg('Destroyed FollowWaypoints action client')
transition_service = 'lifecycle_manager_navigation/manage_nodes'
mgr_client = self.create_client(ManageLifecycleNodes,
transition_service)
while not mgr_client.wait_for_service(timeout_sec=1.0):
self.info_msg(transition_service +
' service not available, waiting...')
req = ManageLifecycleNodes.Request()
req.command = ManageLifecycleNodes.Request().SHUTDOWN
future = mgr_client.call_async(req)
try:
rclpy.spin_until_future_complete(self, future)
future.result()
except Exception as e:
self.error_msg('%s service call failed %r' % (
transition_service,
e,
))
self.info_msg('{} finished'.format(transition_service))
transition_service = 'lifecycle_manager_localization/manage_nodes'
mgr_client = self.create_client(ManageLifecycleNodes,
transition_service)
while not mgr_client.wait_for_service(timeout_sec=1.0):
self.info_msg(transition_service +
' service not available, waiting...')
req = ManageLifecycleNodes.Request()
req.command = ManageLifecycleNodes.Request().SHUTDOWN
future = mgr_client.call_async(req)
try:
rclpy.spin_until_future_complete(self, future)
future.result()
except Exception as e:
self.error_msg('%s service call failed %r' % (
transition_service,
e,
))
self.info_msg('{} finished'.format(transition_service))
def start_run(self):
print_info("preparing to start run")
# wait to receive sensor data from the environment (e.g., a simulator may need time to startup)
waiting_time = 0.0
waiting_period = 0.5
while not self.received_first_scan and rclpy.ok():
time.sleep(waiting_period)
rclpy.spin_once(self)
waiting_time += waiting_period
if waiting_time > 5.0:
self.get_logger().warning(
'still waiting to receive first sensor message from environment'
)
waiting_time = 0.0
# get the parameter robot_radius from the global costmap
parameters_request = GetParameters.Request(names=['robot_radius'])
parameters_response = self.call_service(
self.global_costmap_get_parameters_service_client,
parameters_request)
self.robot_radius = parameters_response.values[0].double_value
print_info("got robot radius")
# get deleaved reduced Voronoi graph from ground truth map
voronoi_graph = self.ground_truth_map.deleaved_reduced_voronoi_graph(
minimum_radius=2 * self.robot_radius).copy()
minimum_length_paths = nx.all_pairs_dijkstra_path(
voronoi_graph, weight='voronoi_path_distance')
minimum_length_costs = dict(
nx.all_pairs_dijkstra_path_length(voronoi_graph,
weight='voronoi_path_distance'))
costs = defaultdict(dict)
for i, paths_dict in minimum_length_paths:
for j in paths_dict.keys():
if i != j:
costs[i][j] = minimum_length_costs[i][j]
# in case the graph has multiple unconnected components, remove the components with less than two nodes
too_small_voronoi_graph_components = list(
filter(lambda component: len(component) < 2,
nx.connected_components(voronoi_graph)))
for graph_component in too_small_voronoi_graph_components:
voronoi_graph.remove_nodes_from(graph_component)
if len(voronoi_graph.nodes) < 2:
self.write_event(
self.get_clock().now(),
'insufficient_number_of_nodes_in_deleaved_reduced_voronoi_graph'
)
raise RunFailException(
"insufficient number of nodes in deleaved_reduced_voronoi_graph, can not generate traversal path"
)
# get greedy path traversing the whole graph starting from a random node
traversal_path_indices = list()
current_node = random.choice(list(voronoi_graph.nodes))
nodes_queue = set(
nx.node_connected_component(voronoi_graph, current_node))
while len(nodes_queue):
candidates = list(
filter(lambda node_cost: node_cost[0] in nodes_queue,
costs[current_node].items()))
candidate_nodes, candidate_costs = zip(*candidates)
next_node = candidate_nodes[int(np.argmin(candidate_costs))]
traversal_path_indices.append(next_node)
current_node = next_node
nodes_queue.remove(next_node)
# convert path of nodes to list of poses (as deque so they can be popped)
self.traversal_path_poses = deque()
for node_index in traversal_path_indices:
pose = Pose()
pose.position.x, pose.position.y = voronoi_graph.nodes[node_index][
'vertex']
q = pyquaternion.Quaternion(axis=[0, 0, 1],
radians=np.random.uniform(
-np.pi, np.pi))
pose.orientation = Quaternion(w=q.w, x=q.x, y=q.y, z=q.z)
self.traversal_path_poses.append(pose)
# publish the traversal path for visualization
traversal_path_msg = Path()
traversal_path_msg.header.frame_id = self.fixed_frame
traversal_path_msg.header.stamp = self.get_clock().now().to_msg()
for traversal_pose in self.traversal_path_poses:
traversal_pose_stamped = PoseStamped()
traversal_pose_stamped.header = traversal_path_msg.header
traversal_pose_stamped.pose = traversal_pose
traversal_path_msg.poses.append(traversal_pose_stamped)
self.traversal_path_publisher.publish(traversal_path_msg)
# pop the first pose from traversal_path_poses and set it as initial pose
self.initial_pose = PoseWithCovarianceStamped()
self.initial_pose.header.frame_id = self.fixed_frame
self.initial_pose.header.stamp = self.get_clock().now().to_msg()
self.initial_pose.pose.pose = self.traversal_path_poses.popleft()
self.initial_pose.pose.covariance = list(
self.initial_pose_covariance_matrix.flat)
self.num_goals = len(self.traversal_path_poses)
# set the position of the robot in the simulator
self.call_service(self.pause_physics_service_client, Empty.Request())
print_info("called pause_physics_service")
time.sleep(1.0)
robot_entity_state = EntityState(name=self.robot_entity_name,
pose=self.initial_pose.pose.pose)
set_entity_state_response = self.call_service(
self.set_entity_state_service_client,
SetEntityState.Request(state=robot_entity_state))
if not set_entity_state_response.success:
self.write_event(self.get_clock().now(),
'failed_to_set_entity_state')
raise RunFailException("could not set robot entity state")
print_info("called set_entity_state_service")
time.sleep(1.0)
self.call_service(self.unpause_physics_service_client, Empty.Request())
print_info("called unpause_physics_service")
time.sleep(1.0)
# ask lifecycle_manager to startup all its managed nodes
startup_request = ManageLifecycleNodes.Request(
command=ManageLifecycleNodes.Request.STARTUP)
startup_response: ManageLifecycleNodes.Response = self.call_service(
self.lifecycle_manager_service_client, startup_request)
if not startup_response.success:
self.write_event(self.get_clock().now(), 'failed_to_startup_nodes')
raise RunFailException("lifecycle manager could not startup nodes")
self.write_event(self.get_clock().now(), 'run_start')
self.run_started = True
self.send_goal()
