Requesting to callback for already at goal. (#523)

Open-RMF Open-RMF General
1

Posted by @anandharaj-dotworld:

Hi @mxgrey ,

I am using the submit direct request call for all tasks. When I provide the same location, the “go to place” function prints a distance of 0. As a result, the “follow new path” function does not receive any callbacks. Is there a way to implement a callback to capture that the robot is already at the goal?

Also i want to know, is that possible to use skip_rotation_command (fleet config) on the command handler py wrapper.

Edited by @anandharaj-dotworld at 2024-09-06T04:21:54Z

2

Posted by @mxgrey:

I can imagine some legitimate use cases for wanting to be notified when the robot is being commanded to the location that RMF believes it is already on, so I’m open to adding an API that lets you hook a callback into that event. I would put it in the unstable API for now since we’re trying to wind down official feature development for these APIs so we can direct attention to the next generation.

Are you able to share any details about what you want to do inside your callback? That would help me understand what kind of arguments to pass to the callback. For example which of the following best describes your purpose for wanting the callback:

  1. I just want to know that a task is expecting the robot to still be at its current location.
  2. I want to trigger something to happen when the robot is already at the location that a task needs it to be at.
  3. I may need to have the robot move (e.g. redo a docking maneuver) even if RMF thinks it is already where it needs to be.
  4. Something else.

is that possible to use skip_rotation_command (fleet config) on the command handler py wrapper.

This is exclusively for the EasyFullControl API [C++][Python]. If you use the basic command handle then you’ll need to filter out these points yourself. The simplest way is to iterate through the path as soon as you receive it and identify whenever you have two sequential waypoints with the same graph index, then delete the earlier of the two waypoints before you begin following the path.

3

Posted by @anandharaj-dotworld:

Hi! @mxgrey

[fleet_adapter-15] [INFO] [1726206853.354875190] [floor_fleet_adapter]: Direct request [go_to_place_1/apex/436e8f90-0126-4d55-bb31-6973e5ebf31b] successfully queued for robot [apex]
[fleet_adapter-15] [INFO] [1726206853.358533051] [fleet_adapter]: [2024-09-13T05:54:13.356264Z] [apex] Received task planner response: {'state': {'assigned_to': {'group': 'floor', 'name': 'apex'}, 'booking': {'id': 'go_to_place_1/apex/436e8f90-0126-4d55-bb31-6973e5ebf31b', 'unix_millis_earliest_start_time': 1726206853350}, 'detail': {'category': 'go_to_place', 'phases': [{'activity': {'category': 'sequence', 'description': {'activities': [{'category': 'go_to_place', 'description': {'nav_through': False, 'waypoint': 'two'}}]}}}]}, 'dispatch': {'status': 'queued'}, 'status': 'queued'}, 'success': True}
[api_server-11] [INFO] [1726206853.574462436] [dispatch_client]: [2024-09-13T05:54:13.573974Z] | Received command handler response: apex/dispatch_task_response, {'floor': [{'robot': 'apex', 'robot_availability': 'AVAILABLE', 'battery_percent': 11.842105865478516}]}
[api_server-11] [INFO] [1726206853.574974649] [dispatch_client]: Received command handler response: apex/dispatch_task_response
[fleet_adapter-15] [INFO] [1726206853.676055292] [floor_fleet_adapter]: Requesting new schedule update because update timed out
[fleet_adapter-15] [INFO] [1726206853.676263943] [floor_fleet_adapter]: [rmf_traffic_ros2::MirrorManager::request_update] Requesting changes for query ID [1] since beginning of recorded history
[schedule_visualizer_node-4] [INFO] [1726206853.676415135] [schedule_data_node]: Requesting new schedule update because update timed out
[schedule_visualizer_node-4] [INFO] [1726206853.676513310] [schedule_data_node]: [rmf_traffic_ros2::MirrorManager::request_update] Requesting changes for query ID [1] since beginning of recorded history
[rmf_traffic_schedule-1] [INFO] [1726206853.685989198] [rmf_traffic_schedule_primary]: [ScheduleNode::update_mirrors] Sending remedial update starting from the beginning going to 3 for query 1
[fleet_adapter-15] [INFO] [1726206853.773983554] [floor_fleet_adapter]: Beginning next task [go_to_place_1/apex/436e8f90-0126-4d55-bb31-6973e5ebf31b] for robot [floor/apex]
[fleet_adapter-15] [INFO] [1726206853.775141794] [floor_fleet_adapter]: Selecting a new go_to_place location from [1] choices for robot [floor/apex]
[fleet_adapter-15] [INFO] [1726206853.775277468] [floor_fleet_adapter]: Got distance from [1] as 0.000000
[fleet_adapter-15] [INFO] [1726206853.775456440] [floor_fleet_adapter]: Planning for [floor/apex] to [two] from one of these locations:
[fleet_adapter-15]  -- l1 < 8.62284 -7.27995> [two] | on waypoint | orientation -175.089
[fleet_adapter-15] [INFO] [1726206853.775846380] [floor_fleet_adapter]: Beginning new task [go_to_place_1/apex/436e8f90-0126-4d55-bb31-6973e5ebf31b] for [floor/apex]. Remaining queue size: 0
[fleet_adapter-15] [INFO] [1726206853.776926362] [floor_fleet_adapter]: Robot [floor/apex] is already at its goal [1]
[api_server-11] [INFO] [1726206853.782784893] [dispatch_client]: [2024-09-13T05:54:13.782281Z] | [apex] Time of end to get result: 2024-09-13T05:54:13.781750Z::2024-09-13T05:54:13.782258Z
[api_server-11] [INFO] [1726206853.850467529] [dispatch_client]: Returning Task response: True

In this log I got the lines from the fleet adapter node: “Got distance from [1] as 0.000000” “Robot [floor/apex] is already at its goal [1]”

I was using a Command Handler Template for the task given. As per the free fleet template, it’s not suitable for my application.

The “Received task planner response” is the task_response for a direct request submission. The follow_new_path function is not receiving any waypoints for already_at_goal. If possible, I need at least the robot’s location (already at a place) in the task_response callback.

def submit_task_request(self, payload):
        task_uuid = str(uuid.uuid4())
        request_id = f"{payload['id']}/{self.robot_name}/{task_uuid}"
        self.update_handle.submit_direct_request(payload["payload"], request_id, self.task_planner_callback_response)
    
  def task_planner_callback_response(self, message):
      self.log_message(self.node.get_logger(), "info", f"Received task planner response: {message}")
      self._process_task_activity(message)

This is direct request i was using from command handler

Edited by @anandharaj-dotworld at 2024-09-18T12:43:17Z