giskard will cancel actions servers that take too long to execute

src/giskardpy/exceptions.py

@@ -1,6 +1,7 @@
 class GiskardException(Exception):
  pass
 
+
 # solver exceptions-----------------------------------------------------------------------------------------------------
 # int64 QP_SOLVER_ERROR=5 # if no solver code fits
 class QPSolverException(GiskardException):
@@ -11,22 +12,27 @@ class QPSolverException(GiskardException):
 class MAX_NWSR_REACHEDException(QPSolverException):
  pass
 
+
 class InfeasibleException(QPSolverException):
  pass
 
+
 # int64 OUT_OF_JOINT_LIMITS=3
 class OutOfJointLimitsException(InfeasibleException):
  pass
 
+
 # int64 HARD_CONSTRAINTS_VIOLATED=4 # conflicting hard constraints, prob because of collision avoidance
 class HardConstraintsViolatedException(InfeasibleException):
  pass
 
+
 # world state exceptions------------------------------------------------------------------------------------------------
 # int64 WORLD_ERROR=7 # if no world error fits
 class PhysicsWorldException(GiskardException):
  pass
 
+
 # int64 UNKNOWN_OBJECT=6
 class UnknownBodyException(PhysicsWorldException, KeyError):
  pass
@@ -48,48 +54,78 @@ class CorruptShapeException(PhysicsWorldException):
  pass
 
 
-
 # error during motion problem building phase----------------------------------------------------------------------------
 # int64 CONSTRAINT_ERROR # if no constraint code fits
 class ConstraintException(GiskardException):
  pass
 
+
 # int64 UNKNOWN_CONSTRAINT
 class UnknownConstraintException(ConstraintException, KeyError):
  pass
 
+
 # int64 CONSTRAINT_INITIALIZATION_ERROR
 class ConstraintInitalizationException(ConstraintException):
  pass
 
+
 # int64 INVALID_GOAL
 class InvalidGoalException(ConstraintException):
  pass
 
+
 # errors during planning------------------------------------------------------------------------------------------------
 # int64 PLANNING_ERROR=13 # if no planning code fits
 class PlanningException(GiskardException):
  pass
 
+
 # int64 SHAKING # Planning was stopped because the trajectory contains a shaky velocity profile. Detection parameters can be tuned in config file
 class ShakingException(PlanningException):
  pass
 
+
 # int64 UNREACHABLE # if reachability check fails
 class UnreachableException(PlanningException):
  pass
 
+
 # errors during execution
 # int64 EXECUTION_ERROR # if no execution code fits
 # int64 PREEMPTED # goal got canceled via action server interface
 class ExecutionException(GiskardException):
  pass
 
+
+class FollowJointTrajectory_INVALID_GOAL(ExecutionException):
+ pass
+
+
+class FollowJointTrajectory_INVALID_JOINTS(ExecutionException):
+ pass
+
+
+class FollowJointTrajectory_OLD_HEADER_TIMESTAMP(ExecutionException):
+ pass
+
+
+class FollowJointTrajectory_PATH_TOLERANCE_VIOLATED(ExecutionException):
+ pass
+
+
+class FollowJointTrajectory_GOAL_TOLERANCE_VIOLATED(ExecutionException):
+ pass
+
+
 class PreemptedException(ExecutionException):
  pass
 
 
+class ExecutionTimeoutException(ExecutionException):
+ pass
+
+
 # -----------------------------------------------------------------------------------------------------------------------
 class ImplementationException(GiskardException):
  pass
-

src/giskardpy/garden.py

@@ -216,10 +216,11 @@ def grow_tree():
  # ----------------------------------------------
  execute_canceled = Sequence('execute canceled')
  execute_canceled.add_child(GoalCanceled('goal canceled', action_server_name))
- execute_canceled.add_child(SetErrorCode('set error code'))
+ execute_canceled.add_child(SetErrorCode('set error code', 'Execution'))
  publish_result = failure_is_success(Selector)('monitor execution')
  publish_result.add_child(execute_canceled)
  publish_result.add_child(execution_action_server)
+ publish_result.add_child(SetErrorCode('set error code', 'Execution'))
  # ----------------------------------------------
  # ----------------------------------------------
  planning_2 = failure_is_success(Selector)('planning II')
@@ -256,7 +257,7 @@ def grow_tree():
  process_move_cmd = success_is_failure(Sequence)('Process move commands')
  process_move_cmd.add_child(SetCmd('set move cmd', action_server_name))
  process_move_cmd.add_child(planning)
- process_move_cmd.add_child(SetErrorCode('set error code'))
+ process_move_cmd.add_child(SetErrorCode('set error code', 'Planning'))
 
  process_move_goal = failure_is_success(Selector)('Process goal')
  process_move_goal.add_child(process_move_cmd)

src/giskardpy/tree/send_trajectory.py

@@ -3,6 +3,11 @@
 from rostopic import ROSTopicException
 from sensor_msgs.msg import JointState
 
+from giskardpy.exceptions import ExecutionException, FollowJointTrajectory_INVALID_JOINTS, \
+ FollowJointTrajectory_INVALID_GOAL, FollowJointTrajectory_OLD_HEADER_TIMESTAMP, \
+ FollowJointTrajectory_PATH_TOLERANCE_VIOLATED, FollowJointTrajectory_GOAL_TOLERANCE_VIOLATED, \
+ ExecutionTimeoutException, PreemptedException
+
 try:
  import pr2_controllers_msgs.msg
 except ImportError:
@@ -21,6 +26,7 @@
 
 
 class SendFollowJointTrajectory(ActionClient, GiskardBehavior):
+ deadline: rospy.Time
  error_code_to_str = {value: name for name, value in vars(FollowJointTrajectoryResult).items() if
  isinstance(value, int)}
 
@@ -43,6 +49,8 @@ def __init__(self, name, namespace, state_topic, fill_velocity_values=True):
  while not action_msg_type:
  try:
  action_msg_type, _, _ = rostopic.get_topic_class('{}/goal'.format(self.action_namespace))
+ if action_msg_type is None:
+ raise ROSTopicException()
  try:
  action_msg_type = eval(action_msg_type._type.replace('/', '.msg.')[:-4])
  if action_msg_type not in self.supported_action_types:
@@ -62,6 +70,8 @@ def __init__(self, name, namespace, state_topic, fill_velocity_values=True):
  while not msg:
  try:
  status_msg_type, _, _ = rostopic.get_topic_class(state_topic)
+ if status_msg_type is None:
+ raise ROSTopicException()
  if status_msg_type not in self.supported_state_types:
  raise TypeError('State topic of type \'{}\' is not supported. '
  'Must be one of: {}'.format(status_msg_type, self.supported_state_types))
@@ -84,6 +94,10 @@ def initialise(self):
  sample_period = self.get_god_map().get_data(identifier.sample_period)
  goal.trajectory = trajectory.to_msg(sample_period, self.controlled_joints, self.fill_velocity_values)
  self.action_goal = goal
+ self.deadline = self.action_goal.trajectory.header.stamp + \
+ self.action_goal.trajectory.points[-1].time_from_start + \
+ self.action_goal.goal_time_tolerance
+ self.cancel_tries = 0
 
  def update(self):
  """
@@ -107,12 +121,48 @@ def update(self):
  if self.action_client.get_state() == GoalStatus.ABORTED:
  result = self.action_client.get_result()
  self.feedback_message = self.error_code_to_str[result.error_code]
- logging.logerr('fail {}'.format(self.feedback_message))
+ msg = '\'{}\' failed to execute goal. Error: \'{}\''.format(self.action_namespace,
+ self.error_code_to_str[result.error_code])
+ logging.logerr(msg)
+ if result.error_code == FollowJointTrajectoryResult.INVALID_GOAL:
+ e = FollowJointTrajectory_INVALID_GOAL(msg)
+ elif result.error_code == FollowJointTrajectoryResult.INVALID_JOINTS:
+ e = FollowJointTrajectory_INVALID_JOINTS(msg)
+ elif result.error_code == FollowJointTrajectoryResult.OLD_HEADER_TIMESTAMP:
+ e = FollowJointTrajectory_OLD_HEADER_TIMESTAMP(msg)
+ elif result.error_code == FollowJointTrajectoryResult.PATH_TOLERANCE_VIOLATED:
+ e = FollowJointTrajectory_PATH_TOLERANCE_VIOLATED(msg)
+ elif result.error_code == FollowJointTrajectoryResult.GOAL_TOLERANCE_VIOLATED:
+ e = FollowJointTrajectory_GOAL_TOLERANCE_VIOLATED(msg)
+ else:
+ e = ExecutionException(msg)
+ self.raise_to_blackboard(e)
+ return py_trees.Status.FAILURE
+ if self.action_client.get_state() in [GoalStatus.PREEMPTED, GoalStatus.PREEMPTING]:
+ if rospy.get_rostime() > self.deadline:
+ msg = '\'{}\' preempted, ' \
+ 'probably because it took to long to execute the goal.'.format(self.action_namespace)
+ self.raise_to_blackboard(ExecutionTimeoutException(msg))
+ else:
+ msg = '\'{}\' preempted. Stopping execution.'.format(self.action_namespace)
+ self.raise_to_blackboard(PreemptedException(msg))
+ logging.logerr(msg)
  return py_trees.Status.FAILURE
+ if rospy.get_rostime() > self.deadline:
+ self.action_client.cancel_goal()
+ msg = 'Cancelling \'{}\' because it took to long to execute the goal.'.format(self.action_namespace)
+ logging.logerr(msg)
+ self.cancel_tries += 1
+ if self.cancel_tries > 5:
+ logging.logwarn('\'{}\' didn\'t cancel execution after 5 tries.'.format(self.action_namespace))
+ self.raise_to_blackboard(ExecutionTimeoutException(msg))
+ return py_trees.Status.FAILURE
+ return py_trees.Status.RUNNING
+
  result = self.action_client.get_result()
  if result:
  self.feedback_message = "goal reached"
- logging.loginfo('{} successfully executed the trajectory.'.format(self.action_namespace))
+ logging.loginfo('\'{}\' successfully executed the trajectory.'.format(self.action_namespace))
  return py_trees.Status.SUCCESS
  else:
  self.feedback_message = "moving"

src/giskardpy/tree/set_error_code.py

@@ -2,20 +2,17 @@
 
 import giskardpy.identifier as identifier
 from giskard_msgs.msg import MoveResult
-from giskardpy.exceptions import UnreachableException, MAX_NWSR_REACHEDException, \
- QPSolverException, UnknownBodyException, ImplementationException, OutOfJointLimitsException, \
- HardConstraintsViolatedException, PhysicsWorldException, ConstraintException, UnknownConstraintException, \
- ConstraintInitalizationException, PlanningException, ShakingException, ExecutionException, InvalidGoalException, \
- PreemptedException
+from giskardpy.exceptions import *
 from giskardpy.tree.plugin import GiskardBehavior
 from giskardpy.utils import logging
 
 
 class SetErrorCode(GiskardBehavior):
 
- def __init__(self, name, print=True):
+ def __init__(self, name, context, print=True):
  self.reachability_threshold = 0.001
  self.print = print
+ self.context = context
  super(SetErrorCode, self).__init__(name)
 
  @profile
@@ -36,9 +33,9 @@ def update(self):
  else:
  if self.print:
  if error_code == MoveResult.SUCCESS:
- logging.loginfo('Planning succeeded.')
+ logging.loginfo('{} succeeded.'.format(self.context))
  else:
- logging.logwarn('Planning failed: {}.'.format(error_message))
+ logging.logwarn('{} failed: {}.'.format(self.context, error_message))
  self.get_god_map().set_data(identifier.result_message, result)
  return Status.SUCCESS
 
@@ -91,6 +88,18 @@ def exception_to_error_code(self, exception):
  error_code = MoveResult.EXECUTION_ERROR
  if isinstance(exception, PreemptedException):
  error_code = MoveResult.PREEMPTED
+ elif isinstance(exception, ExecutionTimeoutException):
+ error_code = MoveResult.EXECUTION_TIMEOUT
+ elif isinstance(exception, FollowJointTrajectory_INVALID_GOAL):
+ error_code = MoveResult.FollowJointTrajectory_INVALID_GOAL
+ elif isinstance(exception, FollowJointTrajectory_INVALID_JOINTS):
+ error_code = MoveResult.FollowJointTrajectory_INVALID_JOINTS
+ elif isinstance(exception, FollowJointTrajectory_OLD_HEADER_TIMESTAMP):
+ error_code = MoveResult.FollowJointTrajectory_OLD_HEADER_TIMESTAMP
+ elif isinstance(exception, FollowJointTrajectory_PATH_TOLERANCE_VIOLATED):
+ error_code = MoveResult.FollowJointTrajectory_PATH_TOLERANCE_VIOLATED
+ elif isinstance(exception, FollowJointTrajectory_GOAL_TOLERANCE_VIOLATED):
+ error_code = MoveResult.FollowJointTrajectory_GOAL_TOLERANCE_VIOLATED
 
  elif isinstance(exception, ImplementationException):
  print(exception)

src/giskardpy/tree/world_updater.py

@@ -1,31 +1,22 @@
 import traceback
-from multiprocessing import Lock
+from queue import Queue
 from xml.etree.ElementTree import ParseError
 
-from giskardpy.tree.sync_localization import SyncLocalization
-
-try:
- # Python 2
- from Queue import Empty, Queue
-except ImportError:
- # Python 3
- from queue import Queue, Empty
-
 import rospy
-from giskard_msgs.srv import UpdateWorld, UpdateWorldResponse, UpdateWorldRequest, GetObjectNames, \
- GetObjectNamesResponse, GetObjectInfo, GetObjectInfoResponse, GetAttachedObjects, GetAttachedObjectsResponse
 from py_trees import Status
-from tf2_py import InvalidArgumentException, ExtrapolationException, TransformException
+from tf2_py import TransformException
 from visualization_msgs.msg import MarkerArray, Marker
 
 import giskardpy.identifier as identifier
-from giskardpy import RobotPrefix, RobotName
+from giskard_msgs.srv import UpdateWorld, UpdateWorldResponse, UpdateWorldRequest, GetObjectNames, \
+ GetObjectNamesResponse, GetObjectInfo, GetObjectInfoResponse, GetAttachedObjects, GetAttachedObjectsResponse
 from giskardpy.data_types import PrefixName
 from giskardpy.exceptions import CorruptShapeException, UnknownBodyException, \
  UnsupportedOptionException, DuplicateNameException
 from giskardpy.model.world import SubWorldTree
 from giskardpy.tree.plugin import GiskardBehavior
 from giskardpy.tree.sync_configuration import SyncConfiguration
+from giskardpy.tree.sync_localization import SyncLocalization
 from giskardpy.tree.tree_manager import TreeManager
 from giskardpy.utils import logging
 from giskardpy.utils.tfwrapper import transform_pose
@@ -37,6 +28,7 @@ def error_in_list(error, list_of_errors):
  for x in list_of_errors:
  result |= isinstance(error, x)
  return result
+
  if error_in_list(e, [CorruptShapeException, ParseError]):
  traceback.print_exc()
  if req.body.type == req.body.MESH_BODY:
@@ -59,7 +51,7 @@ def error_in_list(error, list_of_errors):
  traceback.print_exc()
  return UpdateWorldResponse(UpdateWorldResponse.ERROR,
  '{}: {}'.format(e.__class__.__name__,
-  str(e)))
+ str(e)))
 
 
 class WorldUpdater(GiskardBehavior):

test/joint_trajectory_splitter/fake_servers.py

@@ -3,13 +3,16 @@
 import actionlib
 import control_msgs.msg
 import rospy
+from control_msgs.msg import FollowJointTrajectoryResult
 
 
-class SuccessfulActionServer(object):
+class FakeActionServer(object):
 
  def __init__(self):
  self.name_space = rospy.get_param('~name_space')
  self.joint_names = rospy.get_param('~joint_names')
+ self.sleep_percent = rospy.get_param('~sleep_factor')
+ self.result = rospy.get_param('~result')
  self.state = {j:0 for j in self.joint_names}
  self.pub = rospy.Publisher('{}/state'.format(self.name_space), control_msgs.msg.JointTrajectoryControllerState,
  queue_size=10)
@@ -30,24 +33,17 @@ def preempt_requested(self):
  self._as.set_preempted()
 
  def execute_cb(self, goal):
- rospy.sleep(goal.trajectory.points[-1].time_from_start)
+ rospy.sleep(self.sleep_percent * goal.trajectory.points[-1].time_from_start)
  if self._as.is_active():
- self._as.set_succeeded()
+ result = control_msgs.msg.FollowJointTrajectoryResult()
+ result.error_code = self.result
+ if self.result == FollowJointTrajectoryResult.SUCCESSFUL:
+ self._as.set_succeeded(result)
+ else:
+ self._as.set_aborted(result)
 
 
-class FailingActionServer(SuccessfulActionServer):
- def execute_cb(self, goal):
- rospy.sleep(goal.trajectory.points[1].time_from_start)
- result = control_msgs.msg.FollowJointTrajectoryResult()
- result.error_code = control_msgs.msg.FollowJointTrajectoryResult.GOAL_TOLERANCE_VIOLATED
- if self._as.is_active():
- self._as.set_aborted(result)
-
 if __name__ == '__main__':
  rospy.init_node('SuccessfulActionServer')
- action_sever_type = rospy.get_param('~type')
- if action_sever_type == 'SuccessfulActionServer':
- server = SuccessfulActionServer()
- else:
- server = FailingActionServer()
+ server = FakeActionServer()
  rospy.spin()