Skill Manager goes down when pyrobosim finishes navigating

p-rosit commented 2 months ago

Steps to reproduce issue

Start the simulation with ros2 run delib_ws_worlds run --ros-args -p problem_number:=1
Start skiros with ros2 launch skiros2_pyrobosim_lib main.launch.py
Select the navigate skill and set TargetLocation to pantry_storage

Result

The robot starts navigating to the pantry and skiros is logging that it's waiting:

[skill_manager_node-4] [INFO] [1726123526.599017240] [skiros]: [VisitorExecute]    NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ]
[skill_manager_node-4] [INFO] [1726123526.638574698] [skiros]: [VisitorExecute]    NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ]
[skill_manager_node-4] [INFO] [1726123526.678669562] [skiros]: [VisitorExecute]    NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ]

Then the robot finishes the navigation, skiros recognizes that navigation finished and the skill manager goes down:

[skill_manager_node-4] [INFO] [1726123526.756687834] [skiros]: [VisitorExecute]    NavigateExecution-navigate_execution(State.Success)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ]
[skill_manager_node-4] [INFO] [1726123526.757624522] [skiros]: [VisitorStart]      WmSetRelation-wm_set_relation(State.Running)[Src: cora:Robot-1-robi:robot Relation:['skiros:at'] RelationState:[False] Robot: cora:Robot-1-robi:robot Dst: skiros:OpenableLocation-8-table ]
[world_model_server_node-2] [INFO] [1726123526.781403755] [skiros]: robi:robot/utility_primitives.py:79-><scene>[-] (cora:Robot-1) - (skiros:at) - (skiros:OpenableLocation-8) .
[world_model_server_node-2] [INFO] [1726123526.782990705] [skiros]: [WmModify]          robi:robot/utility_primitives.py:79 update ['cora:Robot-1']. Time: 0.016 secs
[rqt_gui-3] [INFO] [1726123529.111298598] [skiros]: [SkillLayerInterface]Skill manager skill_mgr went down.

Then the skill manager comes back, dies, repeat.

Sometimes navigation finishes and the bt finishes successfully, most of the time it is stuck running at the navigation execution

Full logs

``` [world_model_server_node-2] [INFO] [1726123522.066915481] [skiros]: [WmGet] Done get [skiros:OpenableLocation-5-]. Answer: skiros:OpenableLocation-5 . Time: 0.000 secs [skill_manager_node-4] [INFO] [1726123522.074262695] [skiros]: [SkillManager] Add task skiros:Navigate:navigate [skill_manager_node-4] StartLocation: skiros:Location- TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: sumo:Agent- [skill_manager_node-4] [INFO] [1726123522.075461138] [skiros]: [start] Starting task 9. [skill_manager_node-4] [INFO] [1726123522.076699088] [skiros]: [BtTicker] Execution starts. [skill_manager_node-4] [INFO] [1726123522.077075047] [skiros]: [VisitorStart] Root-task_9(State.Running)[] [world_model_server_node-2] [INFO] [1726123522.078206216] [skiros]: [WmGet] Done get [skiros:OpenableLocation-5-]. Answer: skiros:OpenableLocation-5 . Time: 0.000 secs [skill_manager_node-4] [INFO] [1726123522.084772077] [skiros]: [Autoparametrize] Resolving skiros:Navigate:['StartLocation', 'Robot'] [world_model_server_node-2] [INFO] [1726123522.088434216] [skiros]: [WmGet] Done resolve [skiros:Location-]. Answer: skiros:OpenableLocation-5 skiros:OpenableLocation-8 . Time: 0.003 secs [world_model_server_node-2] [INFO] [1726123522.095334685] [skiros]: [WmGet] Done resolve [sumo:Agent-]. Answer: cora:Robot-1 . Time: 0.000 secs [world_model_server_node-2] [INFO] [1726123522.110717219] [skiros]: [WmGet] Done get [cora:Robot-1-]. Answer: cora:Robot-1 . Time: 0.000 secs [world_model_server_node-2] [INFO] [1726123522.151067139] [skiros]: [WoQuery] Query: SELECT ?x WHERE { ?x rdfs:subPropertyOf* skiros:at . } . Context: [world_model_server_node-2] [INFO] [1726123522.153636315] [skiros]: [WoQuery] Answer: ['skiros:at']. Time: 0.002 sec [world_model_server_node-2] [INFO] [1726123522.159813715] [skiros]: [WmGet] Done get [skiros:OpenableLocation-8-]. Answer: skiros:OpenableLocation-8 . Time: 0.000 secs [skill_manager_node-4] [INFO] [1726123522.171059745] [skiros]: MatchWm skiros:Navigate:[Robot=cora:Robot-1-robi:robot][StartLocation=skiros:OpenableLocation-8-table] [skill_manager_node-4] [INFO] [1726123522.172790893] [skiros]: [VisitorStart] Navigate-navigate(State.Running)[StartLocation: skiros:OpenableLocation-8-table TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123522.178747744] [skiros]: [VisitorStart] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123522.180458865] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [INFO] [1726123522.181160134] [pyrobosim]: Executing action navigate with robot robot... [skill_manager_node-4] [INFO] [1726123522.181440592] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123522.197464170] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123522.237179548] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123522.276927827] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123522.316977068] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123522.356945296] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123522.397033288] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123522.438513669] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123522.477372480] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123522.517168126] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123522.557573620] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123522.596945954] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123522.636962690] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123522.676930486] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123522.716967324] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123522.757650619] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123522.796973211] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123522.836987615] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123522.876978324] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123522.916969340] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123522.957571556] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123522.996886620] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123523.036894579] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123523.076975349] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123523.116767800] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123523.157794919] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123523.196890406] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123523.236905945] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123523.276906210] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123523.316934761] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123523.358476148] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123523.396874344] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123523.437105145] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123523.477047150] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123523.516929248] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123523.558435395] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123523.597204607] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123523.636895151] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123523.676891511] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123523.716988667] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123523.757865944] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123523.796892186] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123523.836906305] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123523.876917172] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123523.916889661] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123523.957732178] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123523.996905291] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123524.037045322] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123524.077150921] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123524.117055109] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123524.158126774] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123524.197125938] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123524.236894128] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123524.276913236] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123524.316927108] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123524.358517037] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123524.396896401] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123524.436707655] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123524.476892286] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123524.516932435] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123524.559155841] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123524.597409092] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123524.636953078] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123524.676847367] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123524.716894431] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123524.757928622] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123524.796912935] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123524.837025932] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123524.880366248] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123524.917170021] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123524.957660242] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123524.996936726] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123525.036847767] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123525.076864726] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123525.116943591] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123525.157140597] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123525.197250440] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123525.236929891] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123525.277009868] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123525.316891068] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123525.358175534] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123525.397043752] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123525.436932037] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123525.476884864] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123525.516969423] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123525.558050570] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123525.597165731] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123525.637266952] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123525.676953843] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123525.716936799] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123525.757918591] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123525.796891100] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123525.836831641] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123525.876896169] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123525.916834105] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123525.957946760] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123525.996864505] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123526.036959274] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123526.076885875] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123526.116736994] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123526.158376220] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123526.196924298] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123526.237039162] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123526.276813181] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123526.316904623] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123526.357014815] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123526.397019451] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123526.438407336] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123526.478601037] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123526.518508685] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123526.558443549] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123526.599017240] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123526.638574698] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123526.678669562] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [robot] Action completed with result: SUCCESS [skill_manager_node-4] [INFO] [1726123526.717069044] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Running)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [INFO] [1726123526.732578943] [pyrobosim]: Action navigate finished with status: SUCCESS [skill_manager_node-4] [INFO] [1726123526.756687834] [skiros]: [VisitorExecute] NavigateExecution-navigate_execution(State.Success)[TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123526.757624522] [skiros]: [VisitorStart] WmSetRelation-wm_set_relation(State.Running)[Src: cora:Robot-1-robi:robot Relation:['skiros:at'] RelationState:[False] Robot: cora:Robot-1-robi:robot Dst: skiros:OpenableLocation-8-table ] [world_model_server_node-2] [INFO] [1726123526.781403755] [skiros]: robi:robot/utility_primitives.py:79->[-] (cora:Robot-1) - (skiros:at) - (skiros:OpenableLocation-8) . [world_model_server_node-2] [INFO] [1726123526.782990705] [skiros]: [WmModify] robi:robot/utility_primitives.py:79 update ['cora:Robot-1']. Time: 0.016 secs [rqt_gui-3] [INFO] [1726123529.111298598] [skiros]: [SkillLayerInterface]Skill manager skill_mgr went down. [rqt_gui-3] [INFO] [1726123530.026383582] [skiros]: [SkillLayerInterface]New skill manager detected: skill_mgr [world_model_server_node-2] [INFO] [1726123530.028312756] [skiros]: [WmModify] robi:robot/utility_primitives.py:80 update ['skiros:OpenableLocation-8']. Time: 0.001 secs [world_model_server_node-2] [INFO] [1726123530.051844455] [skiros]: [WmModify] robi:robot/primitive.py:58 update ['cora:Robot-1']. Time: 0.008 secs [world_model_server_node-2] [INFO] [1726123530.553287471] [skiros]: [WoQuery] Query: SELECT ?x { skiros:Navigate rdfs:subClassOf* ?x }. Context: [world_model_server_node-2] [INFO] [1726123530.555856446] [skiros]: [WoQuery] Answer: ['skiros:Navigate', 'skiros:PyrobosimSkills', 'skiros:Skiros2PyrobosimLib', 'skiros:Skill', 'sumo:Abstract', 'sumo:Entity']. Time: 0.002 sec [world_model_server_node-2] [INFO] [1726123530.562376247] [skiros]: [WoQuery] Query: SELECT ?x { skiros:Pick rdfs:subClassOf* ?x }. Context: [world_model_server_node-2] [INFO] [1726123530.569579008] [skiros]: [WoQuery] Answer: ['skiros:Pick', 'skiros:PyrobosimSkills', 'skiros:Skiros2PyrobosimLib', 'skiros:Skill', 'sumo:Abstract', 'sumo:Entity']. Time: 0.007 sec [world_model_server_node-2] [INFO] [1726123530.586509898] [skiros]: [WoQuery] Query: SELECT ?x { skiros:Place rdfs:subClassOf* ?x }. Context: [world_model_server_node-2] [INFO] [1726123530.590267176] [skiros]: [WoQuery] Answer: ['skiros:Place', 'skiros:PyrobosimSkills', 'skiros:Skiros2PyrobosimLib', 'skiros:Skill', 'sumo:Abstract', 'sumo:Entity']. Time: 0.003 sec [world_model_server_node-2] [INFO] [1726123530.617797230] [skiros]: [WoQuery] Query: SELECT ?x { skiros:NavigateExecution rdfs:subClassOf* ?x }. Context: [world_model_server_node-2] [INFO] [1726123530.621669312] [skiros]: [WoQuery] Answer: ['skiros:NavigateExecution', 'skiros:PyrobosimPrimitives', 'skiros:Skiros2PyrobosimLib', 'skiros:Skill', 'sumo:Abstract', 'sumo:Entity']. Time: 0.003 sec [world_model_server_node-2] [INFO] [1726123530.649240358] [skiros]: [WoQuery] Query: SELECT ?x { skiros:PickExecution rdfs:subClassOf* ?x }. Context: [world_model_server_node-2] [INFO] [1726123530.653002453] [skiros]: [WoQuery] Answer: ['skiros:PickExecution', 'skiros:PyrobosimPrimitives', 'skiros:Skiros2PyrobosimLib', 'skiros:Skill', 'sumo:Abstract', 'sumo:Entity']. Time: 0.003 sec [world_model_server_node-2] [INFO] [1726123530.676831644] [skiros]: [WoQuery] Query: SELECT ?x { skiros:PlaceExecution rdfs:subClassOf* ?x }. Context: [world_model_server_node-2] [INFO] [1726123530.682328092] [skiros]: [WoQuery] Answer: ['skiros:PlaceExecution', 'skiros:PyrobosimPrimitives', 'skiros:Skiros2PyrobosimLib', 'skiros:Skill', 'sumo:Abstract', 'sumo:Entity']. Time: 0.005 sec [world_model_server_node-2] [INFO] [1726123530.702854488] [skiros]: [WoQuery] Query: SELECT ?x { skiros:Problem1Solution rdfs:subClassOf* ?x }. Context: [world_model_server_node-2] [INFO] [1726123530.708233690] [skiros]: [WoQuery] Answer: ['skiros:Problem1Solution', 'skiros:PyrobosimSolutions', 'skiros:Skiros2PyrobosimLib', 'skiros:Skill', 'sumo:Abstract', 'sumo:Entity']. Time: 0.005 sec [world_model_server_node-2] [INFO] [1726123530.713847149] [skiros]: [WoQuery] Query: SELECT ?x { skiros:WmSetRelation rdfs:subClassOf* ?x }. Context: [world_model_server_node-2] [INFO] [1726123530.717510220] [skiros]: [WoQuery] Answer: ['skiros:WmSetRelation', 'skiros:UtilityPrimitives', 'skiros:Skiros2StdSkills', 'skiros:Skill', 'sumo:Abstract', 'sumo:Entity']. Time: 0.003 sec [world_model_server_node-2] [INFO] [1726123530.734826808] [skiros]: [WoQuery] Query: SELECT ?x { skiros:WmMoveObject rdfs:subClassOf* ?x }. Context: [world_model_server_node-2] [INFO] [1726123530.738494223] [skiros]: [WoQuery] Answer: ['skiros:WmMoveObject', 'skiros:UtilityPrimitives', 'skiros:Skiros2StdSkills', 'skiros:Skill', 'sumo:Abstract', 'sumo:Entity']. Time: 0.003 sec [world_model_server_node-2] [INFO] [1726123530.749524063] [skiros]: [WmGet] Done resolve [skiros:Location-]. Answer: skiros:OpenableLocation-5 skiros:OpenableLocation-8 . Time: 0.002 secs [rqt_gui-3] [INFO] [1726123532.110947481] [skiros]: [SkillLayerInterface]Skill manager skill_mgr went down. [rqt_gui-3] [INFO] [1726123533.270204395] [skiros]: [SkillLayerInterface]New skill manager detected: skill_mgr [world_model_server_node-2] [INFO] [1726123533.277596433] [skiros]: [WmModify] robi:robot/primitive.py:58 update ['skiros:OpenableLocation-8']. Time: 0.004 secs [skill_manager_node-4] [INFO] [1726123533.287766108] [skiros]: [VisitorExecute] WmSetRelation-wm_set_relation(State.Success)[Src: cora:Robot-1-robi:robot Relation:['skiros:at'] RelationState:[False] Robot: cora:Robot-1-robi:robot Dst: skiros:OpenableLocation-8-table ] [skill_manager_node-4] [INFO] [1726123533.289168429] [skiros]: [VisitorStart] WmSetRelation-wm_set_relation(State.Running)[Src: cora:Robot-1-robi:robot Relation:['skiros:at'] RelationState:[True] Robot: cora:Robot-1-robi:robot Dst: skiros:OpenableLocation-5-pantry_storage ] [world_model_server_node-2] [INFO] [1726123533.316392208] [skiros]: robi:robot/utility_primitives.py:79->[+] (cora:Robot-1) - (skiros:at) - (skiros:OpenableLocation-5) . [world_model_server_node-2] [INFO] [1726123533.317275542] [skiros]: [WmModify] robi:robot/utility_primitives.py:79 update ['cora:Robot-1']. Time: 0.026 secs [world_model_server_node-2] [INFO] [1726123533.334611928] [skiros]: [WmModify] robi:robot/utility_primitives.py:80 update ['skiros:OpenableLocation-5']. Time: 0.001 secs [world_model_server_node-2] [INFO] [1726123533.356038653] [skiros]: [WmModify] robi:robot/primitive.py:58 update ['cora:Robot-1']. Time: 0.009 secs [world_model_server_node-2] [INFO] [1726123533.552918923] [skiros]: [WoQuery] Query: SELECT ?x { skiros:Navigate rdfs:subClassOf* ?x }. Context: [world_model_server_node-2] [INFO] [1726123533.558306190] [skiros]: [WoQuery] Answer: ['skiros:Navigate', 'skiros:PyrobosimSkills', 'skiros:Skiros2PyrobosimLib', 'skiros:Skill', 'sumo:Abstract', 'sumo:Entity']. Time: 0.005 sec [world_model_server_node-2] [INFO] [1726123533.564626476] [skiros]: [WoQuery] Query: SELECT ?x { skiros:Pick rdfs:subClassOf* ?x }. Context: [world_model_server_node-2] [INFO] [1726123533.570189364] [skiros]: [WoQuery] Answer: ['skiros:Pick', 'skiros:PyrobosimSkills', 'skiros:Skiros2PyrobosimLib', 'skiros:Skill', 'sumo:Abstract', 'sumo:Entity']. Time: 0.005 sec [world_model_server_node-2] [INFO] [1726123533.586823934] [skiros]: [WoQuery] Query: SELECT ?x { skiros:Place rdfs:subClassOf* ?x }. Context: [world_model_server_node-2] [INFO] [1726123533.589732158] [skiros]: [WoQuery] Answer: ['skiros:Place', 'skiros:PyrobosimSkills', 'skiros:Skiros2PyrobosimLib', 'skiros:Skill', 'sumo:Abstract', 'sumo:Entity']. Time: 0.002 sec [world_model_server_node-2] [INFO] [1726123533.598180303] [skiros]: [WoQuery] Query: SELECT ?x { skiros:NavigateExecution rdfs:subClassOf* ?x }. Context: [world_model_server_node-2] [INFO] [1726123533.602220042] [skiros]: [WoQuery] Answer: ['skiros:NavigateExecution', 'skiros:PyrobosimPrimitives', 'skiros:Skiros2PyrobosimLib', 'skiros:Skill', 'sumo:Abstract', 'sumo:Entity']. Time: 0.004 sec [world_model_server_node-2] [INFO] [1726123533.620810783] [skiros]: [WoQuery] Query: SELECT ?x { skiros:PickExecution rdfs:subClassOf* ?x }. Context: [world_model_server_node-2] [INFO] [1726123533.629661766] [skiros]: [WoQuery] Answer: ['skiros:PickExecution', 'skiros:PyrobosimPrimitives', 'skiros:Skiros2PyrobosimLib', 'skiros:Skill', 'sumo:Abstract', 'sumo:Entity']. Time: 0.008 sec [world_model_server_node-2] [INFO] [1726123533.636988797] [skiros]: [WoQuery] Query: SELECT ?x { skiros:PlaceExecution rdfs:subClassOf* ?x }. Context: [world_model_server_node-2] [INFO] [1726123533.640972904] [skiros]: [WoQuery] Answer: ['skiros:PlaceExecution', 'skiros:PyrobosimPrimitives', 'skiros:Skiros2PyrobosimLib', 'skiros:Skill', 'sumo:Abstract', 'sumo:Entity']. Time: 0.004 sec [world_model_server_node-2] [INFO] [1726123533.643141424] [skiros]: [WoQuery] Query: SELECT ?x { skiros:Problem1Solution rdfs:subClassOf* ?x }. Context: [world_model_server_node-2] [INFO] [1726123533.647803619] [skiros]: [WoQuery] Answer: ['skiros:Problem1Solution', 'skiros:PyrobosimSolutions', 'skiros:Skiros2PyrobosimLib', 'skiros:Skill', 'sumo:Abstract', 'sumo:Entity']. Time: 0.004 sec [world_model_server_node-2] [INFO] [1726123533.654257604] [skiros]: [WoQuery] Query: SELECT ?x { skiros:WmSetRelation rdfs:subClassOf* ?x }. Context: [world_model_server_node-2] [INFO] [1726123533.659956263] [skiros]: [WoQuery] Answer: ['skiros:WmSetRelation', 'skiros:UtilityPrimitives', 'skiros:Skiros2StdSkills', 'skiros:Skill', 'sumo:Abstract', 'sumo:Entity']. Time: 0.005 sec [world_model_server_node-2] [INFO] [1726123533.666289083] [skiros]: [WoQuery] Query: SELECT ?x { skiros:WmMoveObject rdfs:subClassOf* ?x }. Context: [world_model_server_node-2] [INFO] [1726123533.668751484] [skiros]: [WoQuery] Answer: ['skiros:WmMoveObject', 'skiros:UtilityPrimitives', 'skiros:Skiros2StdSkills', 'skiros:Skill', 'sumo:Abstract', 'sumo:Entity']. Time: 0.002 sec [world_model_server_node-2] [INFO] [1726123533.680654678] [skiros]: [WmGet] Done resolve [skiros:Location-]. Answer: skiros:OpenableLocation-5 skiros:OpenableLocation-8 . Time: 0.001 secs [rqt_gui-3] [INFO] [1726123536.111212172] [skiros]: [SkillLayerInterface]Skill manager skill_mgr went down. [rqt_gui-3] [INFO] [1726123536.598522524] [skiros]: [SkillLayerInterface]New skill manager detected: skill_mgr [world_model_server_node-2] [INFO] [1726123536.602026186] [skiros]: [WmModify] robi:robot/primitive.py:58 update ['skiros:OpenableLocation-5']. Time: 0.001 secs [skill_manager_node-4] [INFO] [1726123536.612699028] [skiros]: [VisitorExecute] WmSetRelation-wm_set_relation(State.Success)[Src: cora:Robot-1-robi:robot Relation:['skiros:at'] RelationState:[True] Robot: cora:Robot-1-robi:robot Dst: skiros:OpenableLocation-5-pantry_storage ] [skill_manager_node-4] [INFO] [1726123536.614820311] [skiros]: [VisitorExecute] Navigate-navigate(State.Success)[StartLocation: skiros:OpenableLocation-8-table TargetLocation: skiros:OpenableLocation-5-pantry_storage Robot: cora:Robot-1-robi:robot ] [skill_manager_node-4] [INFO] [1726123536.616807845] [skiros]: [VisitorExecute] Root-task_9(State.Success)[] [skill_manager_node-4] [INFO] [1726123536.619252977] [skiros]: [BtTicker] Execution stops. [world_model_server_node-2] [INFO] [1726123537.054924386] [skiros]: [WoQuery] Query: SELECT ?x { skiros:Navigate rdfs:subClassOf* ?x }. Context: [world_model_server_node-2] [INFO] [1726123537.061991352] [skiros]: [WoQuery] Answer: ['skiros:Navigate', 'skiros:PyrobosimSkills', 'skiros:Skiros2PyrobosimLib', 'skiros:Skill', 'sumo:Abstract', 'sumo:Entity']. Time: 0.006 sec [world_model_server_node-2] [INFO] [1726123537.079329114] [skiros]: [WoQuery] Query: SELECT ?x { skiros:Pick rdfs:subClassOf* ?x }. Context: [world_model_server_node-2] [INFO] [1726123537.081774392] [skiros]: [WoQuery] Answer: ['skiros:Pick', 'skiros:PyrobosimSkills', 'skiros:Skiros2PyrobosimLib', 'skiros:Skill', 'sumo:Abstract', 'sumo:Entity']. Time: 0.002 sec [world_model_server_node-2] [INFO] [1726123537.088937810] [skiros]: [WoQuery] Query: SELECT ?x { skiros:Place rdfs:subClassOf* ?x }. Context: [world_model_server_node-2] [INFO] [1726123537.091578787] [skiros]: [WoQuery] Answer: ['skiros:Place', 'skiros:PyrobosimSkills', 'skiros:Skiros2PyrobosimLib', 'skiros:Skill', 'sumo:Abstract', 'sumo:Entity']. Time: 0.002 sec [world_model_server_node-2] [INFO] [1726123537.098230837] [skiros]: [WoQuery] Query: SELECT ?x { skiros:NavigateExecution rdfs:subClassOf* ?x }. Context: [world_model_server_node-2] [INFO] [1726123537.101085693] [skiros]: [WoQuery] Answer: ['skiros:NavigateExecution', 'skiros:PyrobosimPrimitives', 'skiros:Skiros2PyrobosimLib', 'skiros:Skill', 'sumo:Abstract', 'sumo:Entity']. Time: 0.003 sec [world_model_server_node-2] [INFO] [1726123537.107830701] [skiros]: [WoQuery] Query: SELECT ?x { skiros:PickExecution rdfs:subClassOf* ?x }. Context: [world_model_server_node-2] [INFO] [1726123537.110337609] [skiros]: [WoQuery] Answer: ['skiros:PickExecution', 'skiros:PyrobosimPrimitives', 'skiros:Skiros2PyrobosimLib', 'skiros:Skill', 'sumo:Abstract', 'sumo:Entity']. Time: 0.002 sec [world_model_server_node-2] [INFO] [1726123537.119343932] [skiros]: [WoQuery] Query: SELECT ?x { skiros:PlaceExecution rdfs:subClassOf* ?x }. Context: [world_model_server_node-2] [INFO] [1726123537.121977191] [skiros]: [WoQuery] Answer: ['skiros:PlaceExecution', 'skiros:PyrobosimPrimitives', 'skiros:Skiros2PyrobosimLib', 'skiros:Skill', 'sumo:Abstract', 'sumo:Entity']. Time: 0.002 sec [world_model_server_node-2] [INFO] [1726123537.131346901] [skiros]: [WoQuery] Query: SELECT ?x { skiros:Problem1Solution rdfs:subClassOf* ?x }. Context: [world_model_server_node-2] [INFO] [1726123537.133985786] [skiros]: [WoQuery] Answer: ['skiros:Problem1Solution', 'skiros:PyrobosimSolutions', 'skiros:Skiros2PyrobosimLib', 'skiros:Skill', 'sumo:Abstract', 'sumo:Entity']. Time: 0.002 sec [world_model_server_node-2] [INFO] [1726123537.147231996] [skiros]: [WoQuery] Query: SELECT ?x { skiros:WmSetRelation rdfs:subClassOf* ?x }. Context: [world_model_server_node-2] [INFO] [1726123537.149996618] [skiros]: [WoQuery] Answer: ['skiros:WmSetRelation', 'skiros:UtilityPrimitives', 'skiros:Skiros2StdSkills', 'skiros:Skill', 'sumo:Abstract', 'sumo:Entity']. Time: 0.002 sec [world_model_server_node-2] [INFO] [1726123537.168943649] [skiros]: [WoQuery] Query: SELECT ?x { skiros:WmMoveObject rdfs:subClassOf* ?x }. Context: [world_model_server_node-2] [INFO] [1726123537.171416839] [skiros]: [WoQuery] Answer: ['skiros:WmMoveObject', 'skiros:UtilityPrimitives', 'skiros:Skiros2StdSkills', 'skiros:Skill', 'sumo:Abstract', 'sumo:Entity']. Time: 0.002 sec [world_model_server_node-2] [INFO] [1726123537.182616490] [skiros]: [WmGet] Done resolve [skiros:Location-]. Answer: skiros:OpenableLocation-5 skiros:OpenableLocation-8 . Time: 0.001 secs ```

sea-bass commented 4 weeks ago

I also noticed this in my testing. Also, sometimes I click on a navigation skill and I get

[skill_manager_node-4] [INFO] [1729038870.698472933] [skiros]: [BtTicker]          Execution starts.

But the action has not actually started on the PyRoboSim side. No other errors/warnings/log messages.

matthias-mayr commented 3 weeks ago

About what Pontus reported:

Then the skill manager comes back, dies, repeat.

Technically it does not die, but only fails to report that it's alive for some time. This does not happen for me, but apparently at least for Pontus when the world model is updated. It should be better now that we moved two updates into a single one.

matthias-mayr commented 3 weeks ago

I also noticed this in my testing. Also, sometimes I click on a navigation skill and I get
[skill_manager_node-4] [INFO] [1729038870.698472933] [skiros]: [BtTicker]          Execution starts.
But the action has not actually started on the PyRoboSim side. No other errors/warnings/log messages.

That one I haven't seen yet. @sea-bass: Which problem did you load and did you start Navigate to Location or NavigationExecution? How does it look like in the GUI when it does not report anything more? Can you repeat it when you run SkiROS2 with verbose:=True?

sea-bass commented 3 weeks ago

That one I haven't seen yet. @sea-bass: Which problem did you load and did you start Navigate to Location or NavigationExecution? How does it look like in the GUI when it does not report anything more? Can you repeat it when you run SkiROS2 with verbose:=True?

I just recreated it.

Started Problem 1
Navigate to Location to Desk (successful)
Pick Part on Waste-26 (got stuck)
Stopped running the skill
Navigate to Location to elsewhere (got stuck)

The logs just spam this:

[skill_manager_node-4] [INFO] [1729125258.381700269] [skiros]: [VisitorStart]      Root-task_31(State.Running)[]
[skill_manager_node-4] [INFO] [1729125258.382631651] [skiros]: [VisitorExecute]    Root-task_31(State.Running)[]
[skill_manager_node-4] [INFO] [1729125258.420613862] [skiros]: [VisitorExecute]    Root-task_31(State.Running)[]
[skill_manager_node-4] [INFO] [1729125258.460906466] [skiros]: [VisitorExecute]    Root-task_31(State.Running)[]
... repeats forever...

Noting that when the skill succeeds, it does not show this VisitorStart / VisitorExecute, but rather:

[skill_manager_node-4] [INFO] [1729125418.181004528] [skiros]: [VisitorStart]      Pick-Pick Part(State.Running)[Object: skiros:Waste-26-waste Container: skiros:Location-16-desk Gripper: rparts:GripperEffector-15-Robi robot gripper Robot: cora:Robot-14-robi:robot ]
[skill_manager_node-4] [INFO] [1729125418.185653030] [skiros]: [VisitorStart]      PickExecution-Pick Execution(State.Running)[Object: skiros:Waste-26-waste Robot: cora:Robot-14-robi:robot ]
[skill_manager_node-4] [INFO] [1729125418.186128953] [skiros]: [VisitorExecute]    PickExecution-Pick Execution(State.Running)[Object: skiros:Waste-26-waste Robot: cora:Robot-14-robi:robot ]
[skill_manager_node-4] [INFO] [1729125418.189997920] [skiros]: [VisitorExecute]    PickExecution-Pick Execution(State.Running)[Object: skiros:Waste-26-waste Robot: cora:Robot-14-robi:robot ]
[skill_manager_node-4] [INFO] [1729125418.190916019] [skiros]: [VisitorExecute]    PickExecution-Pick Execution(State.Running)[Object: skiros:Waste-26-waste Robot: cora:Robot-14-robi:robot ]
[run-6] [INFO] [1729125418.192586757] [pyrobosim_world]: Executing action pick with robot robot...
[skill_manager_node-4] [INFO] [1729125418.230779990] [skiros]: [VisitorExecute]    PickExecution-Pick Execution(State.Running)[Object: skiros:Waste-26-waste Robot: cora:Robot-14-robi:robot ]
[skill_manager_node-4] [INFO] [1729125418.270842556] [skiros]: [VisitorExecute]    PickExecution-Pick Execution(State.Running)[Object: skiros:Waste-26-waste Robot: cora:Robot-14-robi:robot ]
[skill_manager_node-4] [INFO] [1729125418.310713098] [skiros]: [VisitorExecute]    PickExecution-Pick Execution(State.Running)[Object: skiros:Waste-26-waste Robot: cora:Robot-14-robi:robot ]
[run-6] [INFO] [1729125418.332644477] [pyrobosim_world]: Action pick finished with status: SUCCESS
[skill_manager_node-4] [INFO] [1729125418.350746886] [skiros]: [VisitorExecute]    PickExecution-Pick Execution(State.Success)[Object: skiros:Waste-26-waste Robot: cora:Robot-14-robi:robot ]

matthias-mayr commented 3 weeks ago

@sea-bass: thanks for reproducing it

When I run the Problem 1 solution skill, this works:

Then I repeated the steps by step instructions to:

Navigate to the desk
Pick up Waste 26
And then navigated to the table that also succeeds

Do your reproduction steps say that you interrupted the picking by pressing the stop button?
I also tried to interrupt the picking by pressing the stop button, but I could still navigate after that. But even if I interrupt a pick or place, it finishes the placement. Could that be the difference between our setups?

Maybe @p-rosit has an easier time reproducing this, since for him it also seems to run slower. For me wm_set_relation still takes a second or two, but I think for others it's slower.

matthias-mayr / skiros2_pyrobosim_lib

Skill Manager goes down when pyrobosim finishes navigating #3

Steps to reproduce issue

Result