Skip phase issue (#418)

Open-RMF Open-RMF General
1

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"
}
2

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.

3

Posted by @cwrx777:

Hi,

Could it be this line?

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