Skip to content

Commit

Permalink
Merge branch 'main' into init_nav_fix
Browse files Browse the repository at this point in the history
  • Loading branch information
@fred-labs
fred-labs authored Jun 21, 2024
2 parents f05c1ca + 56fbddc commit 956c082
Show file tree
Hide file tree
Showing 2 changed files with 53 additions and 167 deletions.
176 changes: 23 additions & 153 deletions scenario_execution_ros/scenario_execution_ros/actions/nav_to_pose.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,172 +14,42 @@
#
# SPDX-License-Identifier: Apache-2.0

from enum import Enum

from rclpy.node import Node
from rclpy.duration import Duration
from rclpy.action import ActionClient

import py_trees

from .nav2_common import get_pose_stamped
from .ros_action_call import RosActionCall, ActionCallActionState
from nav2_msgs.action import NavigateToPose
from action_msgs.msg import GoalStatus


class NavToPoseState(Enum):
"""
States for executing a nav-to-pose with nav2
"""
IDLE = 1
NAV_TO_GOAL_REQUESTED = 2
NAV_TO_GOAL = 3
DONE = 4
FAILURE = 5


class NavToPose(py_trees.behaviour.Behaviour):
class NavToPose(RosActionCall):
"""
Class to navigate to a pose
"""

def __init__(self, name: str, associated_actor, goal_pose: list, monitor_progress: bool, action_topic: str, namespace_override: str) -> None:
super().__init__(name)
self.goal_pose = goal_pose
self.namespace = associated_actor["namespace"]
if namespace_override:
self.namespace = namespace_override
self.goal_pose = goal_pose
super().__init__(name, self.namespace + '/' + action_topic, "nav2_msgs.action.NavigateToPose", "")

self.monitor_progress = monitor_progress
self.action_topic = action_topic
self.node = None
self.future = None
self.current_state = NavToPoseState.IDLE
self.nav_to_pose_client = None
self.result_future = None
self.goal_handle = None
self.feedback = None
self.request_result = None
self.status = None

def setup(self, **kwargs):
"""
Setup ROS2 node and service client
"""
try:
self.node: Node = kwargs['node']
except KeyError as e:
error_message = "didn't find 'node' in setup's kwargs [{}][{}]".format(
self.name, self.__class__.__name__)
raise KeyError(error_message) from e

self.nav_to_pose_client = ActionClient(
self.node, NavigateToPose, self.namespace + '/' + self.action_topic)

self.logger.debug("Waiting for 'NavigateToPose' action server")
wait_for_action_server_time = 30
try:
while wait_for_action_server_time > 0 and not self.nav_to_pose_client.wait_for_server(timeout_sec=1.0):
self.logger.info(f"'NavigateToPose' action server not available, waiting {wait_for_action_server_time}s...")
wait_for_action_server_time -= 1
except KeyboardInterrupt:
pass
if wait_for_action_server_time == 0:
raise ValueError(f"{self.name}: Timeout while waiting for action server.")

def update(self) -> py_trees.common.Status:
"""
Execute states
"""
self.logger.debug(f"Current State {self.current_state}")
result = py_trees.common.Status.FAILURE
if self.current_state == NavToPoseState.IDLE:
goal_pose = get_pose_stamped(self.node.get_clock().now().to_msg(), self.goal_pose)
self.feedback_message = "Executing navigation." # pylint: disable= attribute-defined-outside-init

result = py_trees.common.Status.RUNNING
self.current_state = NavToPoseState.NAV_TO_GOAL_REQUESTED
self.navigate_to_pose(goal_pose)
elif self.current_state == NavToPoseState.NAV_TO_GOAL_REQUESTED:
if self.goal_handle:
if not self.goal_handle.accepted:
self.current_state = NavToPoseState.FAILURE
self.feedback_message = 'Goal was rejected!' # pylint: disable= attribute-defined-outside-init
else:
if self.monitor_progress:
result = py_trees.common.Status.RUNNING
self.current_state = NavToPoseState.NAV_TO_GOAL

self.result_future = self.goal_handle.get_result_async()
else:
result = py_trees.common.Status.SUCCESS
self.current_state = NavToPoseState.DONE
else:
result = py_trees.common.Status.RUNNING
elif self.current_state == NavToPoseState.NAV_TO_GOAL:
if not self.is_task_complete():
if self.feedback:
self.feedback_message = 'Estimated time of arrival: ' + '{0:.0f}'.format( # pylint: disable= attribute-defined-outside-init
Duration.from_msg(self.feedback.estimated_time_remaining).nanoseconds / 1e9) + ' seconds.'

if Duration.from_msg(self.feedback.navigation_time) > Duration(seconds=600):
self.cancel_task()
result = py_trees.common.Status.RUNNING
else:
self.current_state = NavToPoseState.DONE
result = self.status
if result == GoalStatus.STATUS_SUCCEEDED:
self.feedback_message = 'Goal succeeded!' # pylint: disable= attribute-defined-outside-init
result = py_trees.common.Status.SUCCESS
elif result == GoalStatus.STATUS_CANCELED:
self.feedback_message = 'Goal was canceled!' # pylint: disable= attribute-defined-outside-init
result = py_trees.common.Status.FAILURE
elif result == GoalStatus.STATUS_ABORTED:
self.feedback_message = 'Goal failed!' # pylint: disable= attribute-defined-outside-init
result = py_trees.common.Status.FAILURE
elif self.current_state == NavToPoseState.DONE:
self.logger.debug("Nothing to do!")
else:
self.logger.error(f"Invalid state {self.current_state}")

return result

def navigate_to_pose(self, pose, behavior_tree=''):
def get_goal_msg(self):
goal_msg = NavigateToPose.Goal()
goal_msg.pose = pose
goal_msg.behavior_tree = behavior_tree

self.logger.info(
f'Navigating to goal: {pose.pose.position.x:.2f} {pose.pose.position.y:.2f}...')
future = self.nav_to_pose_client.send_goal_async(goal_msg, self.feedback_callback)
future.add_done_callback(self.send_goal_done_callback)

def send_goal_done_callback(self, future):
self.goal_handle = future.result()

def is_task_complete(self):
if not self.result_future:
return True
if self.result_future.result():
self.status = self.result_future.result().status
if self.status != GoalStatus.STATUS_SUCCEEDED:
self.feedback_message = f'Task failed with status code: {self.status}' # pylint: disable= attribute-defined-outside-init
return True
else:
# Timed out, still processing, not complete yet
return False

self.feedback_message = "Navigation executed." # pylint: disable= attribute-defined-outside-init
return True

def cancel_task(self):
if self.result_future:
self.goal_handle.cancel_goal()

def feedback_callback(self, msg):
self.feedback = msg.feedback

def shutdown(self):
self.cancel_task()
self.nav_to_pose_client.destroy()
goal_msg.pose = get_pose_stamped(self.node.get_clock().now().to_msg(), self.goal_pose)
return goal_msg

def get_feedback_message(self, current_state):
feedback_message = super().get_feedback_message(current_state)

if self.current_state == ActionCallActionState.IDLE:
feedback_message = "Waiting for navigation"
elif self.current_state == ActionCallActionState.ACTION_CALLED:
if self.received_feedback:
feedback_message = 'Estimated time of arrival: ' + \
'{0:.0f}'.format(Duration.from_msg(self.received_feedback.estimated_time_remaining).nanoseconds / 1e9) + ' seconds.'
else:
goal_msg = self.get_goal_msg()
feedback_message = f"Executing navigation to ({goal_msg.pose.pose.position.x}, {goal_msg.pose.pose.position.y})."
elif current_state == ActionCallActionState.DONE:
feedback_message = f"Goal reached."
return feedback_message
44 changes: 30 additions & 14 deletions scenario_execution_ros/scenario_execution_ros/actions/ros_action_call.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -49,11 +49,13 @@ def __init__(self, name, action_name: str, action_type: str, data: str):
self.goal_handle = None
self.action_type = action_type
self.action_name = action_name
try:
trimmed_data = data.encode('utf-8').decode('unicode_escape')
self.data = literal_eval(trimmed_data)
except Exception as e: # pylint: disable=broad-except
raise ValueError(f"Error while parsing sevice call data:") from e
self.received_feedback = None
if data:
try:
trimmed_data = data.encode('utf-8').decode('unicode_escape')
self.data = literal_eval(trimmed_data)
except Exception as e: # pylint: disable=broad-except
raise ValueError(f"Error while parsing sevice call data:") from e
self.current_state = ActionCallActionState.IDLE
self.cb_group = ReentrantCallbackGroup()

Expand Down Expand Up @@ -86,33 +88,35 @@ def update(self) -> py_trees.common.Status:
self.logger.debug(f"Current State {self.current_state}")
result = py_trees.common.Status.FAILURE
if self.current_state == ActionCallActionState.IDLE:
self.feedback_message = f"Waiting for action server {self.action_name}" # pylint: disable= attribute-defined-outside-init
if self.client.wait_for_server(0.0):
self.current_state = ActionCallActionState.ACTION_SERVER_AVAILABLE
result = py_trees.common.Status.RUNNING
elif self.current_state == ActionCallActionState.ACTION_SERVER_AVAILABLE:
self.current_state = ActionCallActionState.ACTION_CALLED
if self.send_goal_future:
self.send_goal_future.cancel()
req = self.action_type.Goal()
set_message_fields(req, self.data)
self.send_goal_future = self.client.send_goal_async(req, feedback_callback=self.feedback_callback)
self.send_goal_future = self.client.send_goal_async(self.get_goal_msg(), feedback_callback=self.feedback_callback)
self.send_goal_future.add_done_callback(self.goal_response_callback)
self.feedback_message = f"action {self.action_name} called." # pylint: disable= attribute-defined-outside-init
result = py_trees.common.Status.RUNNING
elif self.current_state == ActionCallActionState.ACTION_CALLED:
result = py_trees.common.Status.RUNNING
elif self.current_state == ActionCallActionState.DONE:
result = py_trees.common.Status.SUCCESS
else:
self.logger.error(f"Invalid state {self.current_state}")
feedback_msg = self.get_feedback_message(self.current_state)
if feedback_msg is not None:
self.feedback_message = feedback_msg # pylint: disable= attribute-defined-outside-init

return result

def get_goal_msg(self):
req = self.action_type.Goal()
set_message_fields(req, self.data)
return req

def feedback_callback(self, msg):
if self.current_state not in [ActionCallActionState.DONE, ActionCallActionState.ERROR]:
print(msg.feedback)
self.feedback_message = f"Current: {msg.feedback}" # pylint: disable= attribute-defined-outside-init
self.received_feedback = msg.feedback

def goal_response_callback(self, future):
self.goal_handle = future.result()
Expand All @@ -133,7 +137,6 @@ def get_result_callback(self, future):
if self.current_state == ActionCallActionState.ACTION_CALLED:
if status == GoalStatus.STATUS_SUCCEEDED:
self.current_state = ActionCallActionState.DONE
self.feedback_message = f"action successfully finished." # pylint: disable= attribute-defined-outside-init
self.goal_handle = None
elif status == GoalStatus.STATUS_CANCELED:
self.current_state = ActionCallActionState.ERROR
Expand All @@ -149,3 +152,16 @@ def get_result_callback(self, future):
def shutdown(self):
if self.goal_handle:
self.goal_handle.cancel_goal()

def get_feedback_message(self, current_state):
feedback_message = None
if current_state == ActionCallActionState.IDLE:
feedback_message = f"Waiting for action server {self.action_name}"
elif current_state == ActionCallActionState.ACTION_CALLED:
if self.received_feedback is not None:
feedback_message = f"Current: {self.received_feedback}"
else:
feedback_message = f"Action {self.action_name} called."
elif current_state == ActionCallActionState.DONE:
feedback_message = f"Action successfully finished."
return feedback_message

0 comments on commit 956c082

Please sign in to comment.