Skip phase issue (#418)

Posted by @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"
    }
    

Posted by @mxgrey:

I think the bug is on this line: rmf_ros2/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp at efccf11088055e283a3a29071ee7a0364f405af6 · open-rmf/rmf_ros2 · GitHub

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.

Posted by @cwrx777:

Hi,

Could it be this line?

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