skip phase issue

[cwrx777]

Hi,
I'm trying to understand how skip phase works and encountered error in the fleet adapter after skip phase request is submitted.

Logs

After task request is submitted:

[fleet_adapter-15] [tinyRobot_fleet_adapter][INFO] Handling [dispatch_task_request] request_id: [direct_cd0cf889-f801-4f55-9b07-e044951d73d5]
[fleet_adapter-15] [tinyRobot_fleet_adapter][INFO] Handling [dispatch_task_request] request_id: [direct_cd0cf889-f801-4f55-9b07-e044951d73d5]
[fleet_adapter-15] [tinyRobot_fleet_adapter][INFO] Requesting new schedule update because update timed out
[fleet_adapter-15] [tinyRobot_fleet_adapter][INFO] [rmf_traffic_ros2::MirrorManager::request_update] Requesting changes for query ID [1] since version [14]
[rmf_task_dispatcher-13] [rmf_dispatcher_node][INFO] Add Task [compose.dispatch-0] to a bidding queue
[rmf_task_dispatcher-13] [rmf_dispatcher_node][INFO]  - Start new bidding task: compose.dispatch-0
[fleet_adapter-15] [tinyRobot_fleet_adapter][INFO] [Bidder] Received Bidding notice for task_id [compose.dispatch-0]
[fleet_adapter-15] [tinyRobot_fleet_adapter][INFO] Planning for [2] robot(s) and [1] request(s)
[fleet_adapter-15] [tinyRobot_fleet_adapter][INFO] Submitted BidProposal to accommodate task [compose.dispatch-0] by robot [tinyRobot1] with new cost [35.974384]
[rmf_task_dispatcher-13] [rmf_dispatcher_node][INFO] Determined winning Fleet Adapter: [tinyRobot], from 1 responses
[rmf_task_dispatcher-13] [rmf_dispatcher_node][INFO] Dispatcher Bidding Result: task [compose.dispatch-0] is awarded to fleet adapter [tinyRobot], with expected robot [tinyRobot1].
[fleet_adapter-15] [tinyRobot_fleet_adapter][INFO] Bid for task_id [compose.dispatch-0] awarded to fleet [tinyRobot]. Processing request...
[fleet_adapter-15] [tinyRobot_fleet_adapter][INFO] Beginning next task [compose.dispatch-0] for robot [tinyRobot/tinyRobot1]
[fleet_adapter-15] [tinyRobot_fleet_adapter][INFO] Beginning new task [compose.dispatch-0] for [tinyRobot/tinyRobot1]. Remaining queue size: 0
[fleet_adapter-15] [tinyRobot_fleet_adapter][INFO] TaskAssignments updated for robots in fleet [tinyRobot] to accommodate task_id [compose.dispatch-0]

After task skip phase request is submitted:

[fleet_adapter-15] [tinyRobot_fleet_adapter][INFO] Selecting a new go_to_place location from [1] choices for robot [tinyRobot/tinyRobot1]
[fleet_adapter-15] [tinyRobot_fleet_adapter][INFO] Got distance from [24] as 21.280834
[fleet_adapter-15] [tinyRobot_fleet_adapter][INFO] Planning for [tinyRobot/tinyRobot1] to [coe] from one of these locations:
[fleet_adapter-15]  -- L1 <10.4331 -5.5751> [tinyRobot1_charger] | on waypoint | orientation 76.1012
[fleet_adapter-15] [tinyRobot_fleet_adapter][INFO] Handling [skip_phase_request] request_id: [compose.dispatch-0]
[fleet_adapter-15] [tinyRobot_fleet_adapter][INFO] Executing go_to_place [coe] for robot [tinyRobot/tinyRobot1]
[fleet_adapter-15] [tinyRobot_command_handle][INFO] Commanding [tinyRobot1] to navigate to [10.08614619 -6.97943654  2.54466843] on map [L1]: cmd_id 2

[fleet_adapter-15] terminate called after throwing an instance of 'nlohmann::detail::type_error'
[fleet_adapter-15]   what():  [json.exception.type_error.305] cannot use operator[] with a numeric argument with object

Steps to replicate:

1. Edit tinyRobot_config.yaml in office demo:
   actions: ["custom_action"]

2. Launch office simulation (after rebuilding rmf_demos workspace).

   ros2 launch rmf_demos_gz_classic office.launch.xml

3. Submit a task request with the following content:

   dispatch_task_request

   {
     "type": "dispatch_task_request",
     "request": {
       "unix_millis_earliest_start_time": 0,
       "category": "compose",
       "description": {
         "detail": "Text to describe the task.",
         "category": "this_can_be_anything",
         "phases": [
           {
             "activity": {
               "category": "perform_action",
               "description": {
                 "category": "custom_action",
                 "description": {},
                 "unix_millis_duration_estimate": 60000
               }
             }
           },
           {
             "activity": {
               "category": "go_to_place",
               "description": {
                 "waypoint": "coe"
               }
             }
           }
         ]
       }
     }
   }

   response

   {
    "state": {
      "booking": {
        "id": "compose.dispatch-0",
        "unix_millis_earliest_start_time": 0
      },
      "category": "compose",
      "detail": {
        "category": "this_can_be_anything",
        "detail": "Text to describe the task.",
        "phases": [
          {
            "activity": {
              "category": "perform_action",
              "description": {
                "category": "custom_action",
                "description": {},
                "unix_millis_duration_estimate": 60000
              }
            }
          },
          {
            "activity": {
              "category": "go_to_place",
              "description": {
                "waypoint": "coe"
              }
            }
          }
        ]
      },
      "dispatch": {
        "errors": [],
        "status": "queued"
      },
      "status": "queued",
      "unix_millis_start_time": 0
    },
    "success": true
   }

4. Submit Skip Phase Request:

   skip phase request

   {
     "type": "skip_phase_request",
     "task_id": "compose.dispatch-0",
     "phase_id": 1,
     "labels": [
       "test skip phase"
      ]
   }

   response

   {
    "success": true, 
    "token": "0"
   }

[mxgrey]

I think the bug is on this line: https://github.com/open-rmf/rmf_ros2/blob/efccf11088055e283a3a29071ee7a0364f405af6/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp#L747

If you're building from source you could try changing that line to

auto& skip_info = _skip_info_map[std::to_string(phase_id)];

and then running the test again.

I'll try to find time early this week to reproduce the bug and test this possible fix.

[cwrx777]

Hi,

Could it be this line?
https://github.com/open-rmf/rmf_ros2/blob/efccf11088055e283a3a29071ee7a0364f405af6/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp#L559

 auto& skip_requests = phases[std::to_string(phase)]["skip_requests"];