nav2_simple_commander: improve goal request rejection error reporting (…

…ros-navigation#4341)

Signed-off-by: Mike Wake <michael.wake@aosgrp.com.au>

nav2_simple_commander/nav2_simple_commander/robot_navigator.py

@@ -169,7 +169,9 @@ def goThroughPoses(self, poses, behavior_tree=''):
  self.goal_handle = send_goal_future.result()
 
  if not self.goal_handle.accepted:
- self.error(f'Goal with {len(poses)} poses was rejected!')
+ msg = f'NavigateThroughPoses request with {len(poses)} was rejected!'
+ self.setTaskError(NavigateThroughPoses.UNKNOWN, msg)
+ self.error(msg)
  return False
 
  self.result_future = self.goal_handle.get_result_async()
@@ -200,13 +202,15 @@ def goToPose(self, pose, behavior_tree=''):
  self.goal_handle = send_goal_future.result()
 
  if not self.goal_handle.accepted:
- self.error(
- 'Goal to '
+ msg = (
+ 'NavigateToPose goal to '
  + str(pose.pose.position.x)
  + ' '
  + str(pose.pose.position.y)
  + ' was rejected!'
  )
+ self.setTaskError(NavigateToPose.UNKNOWN, msg)
+ self.error(msg)
  return False
 
  self.result_future = self.goal_handle.get_result_async()
@@ -230,7 +234,9 @@ def followWaypoints(self, poses):
  self.goal_handle = send_goal_future.result()
 
  if not self.goal_handle.accepted:
- self.error(f'Following {len(poses)} waypoints request was rejected!')
+ msg = f'Following {len(poses)} waypoints request was rejected!'
+ self.setTaskError(FollowWaypoints.UNKNOWN, msg)
+ self.error(msg)
  return False
 
  self.result_future = self.goal_handle.get_result_async()
@@ -254,9 +260,9 @@ def followGpsWaypoints(self, gps_poses):
  self.goal_handle = send_goal_future.result()
 
  if not self.goal_handle.accepted:
- self.error(
-  f'Following {len(gps_poses)} gps waypoints request was rejected!'
- )
+ msg = f'Following {len(gps_poses)} gps waypoints request was rejected!'
+ self.setTaskError(FollowGPSWaypoints.UNKNOWN, msg)
+ self.error(msg)
  return False
 
  self.result_future = self.goal_handle.get_result_async()
@@ -279,7 +285,9 @@ def spin(self, spin_dist=1.57, time_allowance=10):
  self.goal_handle = send_goal_future.result()
 
  if not self.goal_handle.accepted:
- self.error('Spin request was rejected!')
+ msg = 'Spin request was rejected!'
+ self.setTaskError(Spin.UNKNOWN, msg)
+ self.error(msg)
  return False
 
  self.result_future = self.goal_handle.get_result_async()
@@ -303,7 +311,9 @@ def backup(self, backup_dist=0.15, backup_speed=0.025, time_allowance=10):
  self.goal_handle = send_goal_future.result()
 
  if not self.goal_handle.accepted:
- self.error('Backup request was rejected!')
+ msg = 'Backup request was rejected!'
+ self.setTaskError(BackUp.UNKNOWN, msg)
+ self.error(msg)
  return False
 
  self.result_future = self.goal_handle.get_result_async()
@@ -327,7 +337,9 @@ def driveOnHeading(self, dist=0.15, speed=0.025, time_allowance=10):
  self.goal_handle = send_goal_future.result()
 
  if not self.goal_handle.accepted:
- self.error('Drive On Heading request was rejected!')
+ msg = 'Drive On Heading request was rejected!'
+ self.setTaskError(DriveOnHeading.UNKNOWN, msg)
+ self.error(msg)
  return False
 
  self.result_future = self.goal_handle.get_result_async()
@@ -349,7 +361,9 @@ def assistedTeleop(self, time_allowance=30):
  self.goal_handle = send_goal_future.result()
 
  if not self.goal_handle.accepted:
- self.error('Assisted Teleop request was rejected!')
+ msg = 'Assisted Teleop request was rejected!'
+ self.setTaskError(AssistedTeleop.UNKNOWN, msg)
+ self.error(msg)
  return False
 
  self.result_future = self.goal_handle.get_result_async()
@@ -375,7 +389,9 @@ def followPath(self, path, controller_id='', goal_checker_id=''):
  self.goal_handle = send_goal_future.result()
 
  if not self.goal_handle.accepted:
- self.error('Follow path was rejected!')
+ msg = 'FollowPath goal was rejected!'
+ self.setTaskError(FollowPath.UNKNOWN, msg)
+ self.error(msg)
  return False
 
  self.result_future = self.goal_handle.get_result_async()
@@ -401,8 +417,9 @@ def dockRobotByPose(self, dock_pose, dock_type, nav_to_dock=True):
  self.goal_handle = send_goal_future.result()
 
  if not self.goal_handle.accepted:
- self.setTaskError(DockRobot.UNKNOWN, 'Docking request was rejected')
- self.info('Docking request was rejected!')
+ msg = 'DockRobot request was rejected!'
+ self.setTaskError(DockRobot.UNKNOWN, msg)
+ self.error(msg)
  return False
 
  self.result_future = self.goal_handle.get_result_async()
@@ -427,8 +444,9 @@ def dockRobotByID(self, dock_id, nav_to_dock=True):
  self.goal_handle = send_goal_future.result()
 
  if not self.goal_handle.accepted:
- self.setTaskError(DockRobot.UNKNOWN, 'Docking request was rejected')
- self.info('Docking request was rejected!')
+ msg = 'DockRobot request was rejected!'
+ self.setTaskError(DockRobot.UNKNOWN, msg)
+ self.error(msg)
  return False
 
  self.result_future = self.goal_handle.get_result_async()
@@ -451,8 +469,9 @@ def undockRobot(self, dock_type=''):
  self.goal_handle = send_goal_future.result()
 
  if not self.goal_handle.accepted:
- self.setTaskError(UndockRobot.UNKNOWN, 'Undocking request was rejected')
- self.info('Undocking request was rejected!')
+ msg = 'UndockRobot request was rejected!'
+ self.setTaskError(UndockRobot.UNKNOWN, msg)
+ self.error(msg)
  return False
 
  self.result_future = self.goal_handle.get_result_async()
@@ -553,7 +572,7 @@ def _getPathImpl(
  self.error('Get path was rejected!')
  self.status = GoalStatus.UNKNOWN
  result = ComputePathToPose.Result()
- result.error_code = ComputePathToPose.NONE
+ result.error_code = ComputePathToPose.UNKNOWN
  result.error_msg = 'Get path was rejected'
  return result