search

BehaviorTree / Groot2

37 stars 0 forks source link

Groot2 crashes with std::vector<double> in Blackboard #55

Open b-adkins opened 2 months ago

b-adkins commented 2 months ago

Versions:

Platform: Ubuntu 22.04

Behavior

I put a C++ vector on the Blackboard. I tried to visualize it in Groot, but Groot crashed when I clicked the checkmark to visualize the tree.

The exception is

terminate called after throwing an instance of 'nlohmann::json_abi_v3_11_2::detail::invalid_iterator'
  what():  [json.exception.invalid_iterator.207] cannot use key() for non-object iterators

(Groot2 was able to visualize a primitive - a double - or a more complex ROS2 type, a geometry_msgs/msg/Point, without problem.)

Hypothesized Cause

I am guessing that the Groot2 JSON parser assumes that the root any element will be a JSON object or primitive. If that is the case, then it needs to also handle a JSON array.

To reproduce

  1. Register a JsonDefinition for a vector.

    BT::RegisterJsonDefinition<std::vector<double>>();

  2. Create a behavior with the following output ports that writes a vector to them:

    class VectorBehavior
{
...
  static BT::PortsList providedPorts()
  {
   return{
    BT::OutputPort<std::vector<double>>("offset")
   };
  }
...
};

BT::NodeStatus VectorBehavior::onRunning()
{
  std::vector<double> offset{-0.111, 0, 0};
  setOutput("offset", offset);
}

  3. Use a behavior tree runner with a Groot2Publisher (I used the BT::TreeExecutionServer from behaviortree_ros2)

  4. XML for the tree:

    <?xml version="1.0" encoding="UTF-8"?>
<root BTCPP_format="4"
      main_tree_to_execute="main">
    <BehaviorTree ID="main">
        <Sequence>
            <VectorBehavior offset={offset}"/>
        </Sequence>
    </BehaviorTree>

    <TreeNodesModel>
        <Action ID="VectorBehavior">
            <output_port name="offset" type="std::vector<double>>"/>
        </Action>
    </TreeNodesModel>
</root>

    I experienced the same behavior when using the auto-generated <TreeNodesModel>

            <Action ID="VectorBehavior">
            <output_port name="offset" type="std::vector&lt;double, std::allocator&lt;double&gt; &gt;"/>
        </Action>

Groot2 console log:

Warning: Thu Sep 12 10:34:03 2024 - Disconnected with error:  "Reply timeout" ,
 Reconnecting...
"Reply timeout"
socket disconnected
Warning: Thu Sep 12 10:34:04 2024 - Disconnected with error:  "Reply timeout" ,
 Reconnecting...
subscribe finished
"Reply timeout"
socket disconnected
Warning: Thu Sep 12 10:34:04 2024 - Disconnected with error:  "Reply timeout" ,
 Reconnecting...
subscribe finished
Info: Thu Sep 12 10:34:04 2024 - "Error loading  model" "The model 'LoopDouble' is builtin model and is being overriden...."
Info: Thu Sep 12 10:34:04 2024 - "Error loading  model" "The model 'LoopString' is builtin model and is being overriden...."
Info: Thu Sep 12 10:34:04 2024 - "Error loading  model" "The model 'Script' is builtin model and is being overriden...."
Info: Thu Sep 12 10:34:04 2024 - "Error loading  model" "The model 'ScriptCondition' is builtin model and is being overriden...."
Info: Thu Sep 12 10:34:04 2024 - "Error loading  model" "The model 'SetBlackboard' is builtin model and is being overriden...."
Info: Thu Sep 12 10:34:04 2024 - "Error loading  model" "The model 'Switch2' is builtin model and is being overriden...."
Info: Thu Sep 12 10:34:04 2024 - "Error loading  model" "The model 'Switch3' is builtin model and is being overriden...."
Info: Thu Sep 12 10:34:04 2024 - "Error loading  model" "The model 'Switch4' is builtin model and is being overriden...."
Info: Thu Sep 12 10:34:04 2024 - "Error loading  model" "The model 'Switch5' is builtin model and is being overriden...."
Info: Thu Sep 12 10:34:04 2024 - "Error loading  model" "The model 'Switch6' is builtin model and is being overriden...."
terminate called after throwing an instance of 'nlohmann::json_abi_v3_11_2::detail::invalid_iterator'
  what():  [json.exception.invalid_iterator.207] cannot use key() for non-object iterators
Stack trace (most recent call last):
#0  | Source "./nptl/pthread_kill.c", line 89, in __pthread_kill_internal
    | Source "./nptl/pthread_kill.c", line 78, in __pthread_kill_implementation
      Source "./nptl/pthread_kill.c", line 44, in __pthread_kill [0x732af40969fc]
#1    Source "../sysdeps/posix/raise.c", line 26, in raise [0x732af4042475]
#2    Source "./stdlib/abort.c", line 79, in abort [0x732af40287f2]
#3    Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.30", at 0x732af44a2b9d, in 
#4    Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.30", at 0x732af44ae20b, in 
#5    Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.30", at 0x732af44ae276, in std::terminate()
#6    Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.30", at 0x732af44ae4d7, in __cxa_throw
#7  | Source "/home/davide/ws_behaviortree/src/Groot2/src_gui/monitor/widgets/blackboard_widget.cpp", line 82, in key
      Source "/home/davide/.conan2/p/nlohm1bed1ddc0a2fa/p/include/nlohmann/detail/iterators/iter_impl.hpp", line 731, in operator() [0x60c3e9966ba3]
#8  | Source "/home/davide/ws_behaviortree/src/Groot2/src_gui/monitor/widgets/blackboard_widget.cpp", line 102, in operator()
      Source "/usr/include/c++/9/bits/std_function.h", line 688, in operator() [0x60c3e9966f8c]
#9  | Source "/home/davide/ws_behaviortree/src/Groot2/src_gui/monitor/widgets/blackboard_widget.cpp", line 102, in operator()
      Source "/usr/include/c++/9/bits/std_function.h", line 688, in operator() [0x60c3e9966f8c]
#10 | Source "/home/davide/ws_behaviortree/src/Groot2/src_gui/monitor/widgets/blackboard_widget.cpp", line 192, in setBlackboard
    | Source "/home/davide/ws_behaviortree/src/Groot2/src_gui/monitor/widgets/blackboard_widget.cpp", line 128, in operator()
      Source "/usr/include/c++/9/bits/std_function.h", line 688, in onBlackboardReady [0x60c3e9965ba7]
#11 | Source "/home/davide/QtDev/6.7.2/gcc_64/include/QtCore/qobjectdefs_impl.h", line 553, in call<QtPrivate::List<nlohmann::json_abi_v3_11_2::basic_json<std::map, std::vector, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, bool, long int, long unsigned int, double, std::allocator, nlohmann::json_abi_v3_11_2::adl_serializer, std::vector<unsigned char, std::allocator<unsigned char> > > >, void>
    | Source "/home/davide/QtDev/6.7.2/gcc_64/include/QtCore/qobjectdefs_impl.h", line 182, in call
      Source "/home/davide/QtDev/6.7.2/gcc_64/include/QtCore/qobjectdefs_impl.h", line 145, in impl [0x60c3e9946916]
#12   Object "/home/myname/.local/opt/Groot2/lib/libQt6Core.so.6", at 0x732af49b4ed2, in QObject::event(QEvent*)
#13   Object "/home/myname/.local/opt/Groot2/lib/libQt6Widgets.so.6", at 0x732af5d844f1, in QApplicationPrivate::notify_helper(QObject*, QEvent*)
#14   Object "/home/myname/.local/opt/Groot2/lib/libQt6Core.so.6", at 0x732af4963189, in QCoreApplication::notifyInternal2(QObject*, QEvent*)
#15   Object "/home/myname/.local/opt/Groot2/lib/libQt6Core.so.6", at 0x732af496663c, in QCoreApplicationPrivate::sendPostedEvents(QObject*, int, QThreadData*)

Saving backlog in: /home/myname/.cache/AurynRobotics/Groot2/crash_log.txtAborted (Signal sent by tkill() 15699 1000)
Aborted (core dumped)

The contents of crash_log.txt:

version: 1.6.0
Stack trace (most recent call last):
#0  | Source "./nptl/pthread_kill.c", line 89, in __pthread_kill_internal
    | Source "./nptl/pthread_kill.c", line 78, in __pthread_kill_implementation
      Source "./nptl/pthread_kill.c", line 44, in __pthread_kill [0x732af40969fc]
#1    Source "../sysdeps/posix/raise.c", line 26, in raise [0x732af4042475]
#2    Source "./stdlib/abort.c", line 79, in abort [0x732af40287f2]
#3    Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.30", at 0x732af44a2b9d, in 
#4    Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.30", at 0x732af44ae20b, in 
#5    Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.30", at 0x732af44ae276, in std::terminate()
#6    Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.30", at 0x732af44ae4d7, in __cxa_throw
#7  | Source "/home/davide/ws_behaviortree/src/Groot2/src_gui/monitor/widgets/blackboard_widget.cpp", line 82, in key
      Source "/home/davide/.conan2/p/nlohm1bed1ddc0a2fa/p/include/nlohmann/detail/iterators/iter_impl.hpp", line 731, in operator() [0x60c3e9966ba3]
#8  | Source "/home/davide/ws_behaviortree/src/Groot2/src_gui/monitor/widgets/blackboard_widget.cpp", line 102, in operator()
      Source "/usr/include/c++/9/bits/std_function.h", line 688, in operator() [0x60c3e9966f8c]
#9  | Source "/home/davide/ws_behaviortree/src/Groot2/src_gui/monitor/widgets/blackboard_widget.cpp", line 102, in operator()
      Source "/usr/include/c++/9/bits/std_function.h", line 688, in operator() [0x60c3e9966f8c]
#10 | Source "/home/davide/ws_behaviortree/src/Groot2/src_gui/monitor/widgets/blackboard_widget.cpp", line 192, in setBlackboard
    | Source "/home/davide/ws_behaviortree/src/Groot2/src_gui/monitor/widgets/blackboard_widget.cpp", line 128, in operator()
      Source "/usr/include/c++/9/bits/std_function.h", line 688, in onBlackboardReady [0x60c3e9965ba7]
#11 | Source "/home/davide/QtDev/6.7.2/gcc_64/include/QtCore/qobjectdefs_impl.h", line 553, in call<QtPrivate::List<nlohmann::json_abi_v3_11_2::basic_json<std::map, std::vector, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, bool, long int, long unsigned int, double, std::allocator, nlohmann::json_abi_v3_11_2::adl_serializer, std::vector<unsigned char, std::allocator<unsigned char> > > >, void>
    | Source "/home/davide/QtDev/6.7.2/gcc_64/include/QtCore/qobjectdefs_impl.h", line 182, in call
      Source "/home/davide/QtDev/6.7.2/gcc_64/include/QtCore/qobjectdefs_impl.h", line 145, in impl [0x60c3e9946916]
#12   Object "/home/myname/.local/opt/Groot2/lib/libQt6Core.so.6", at 0x732af49b4ed2, in QObject::event(QEvent*)
#13   Object "/home/myname/.local/opt/Groot2/lib/libQt6Widgets.so.6", at 0x732af5d844f1, in QApplicationPrivate::notify_helper(QObject*, QEvent*)
#14   Object "/home/myname/.local/opt/Groot2/lib/libQt6Core.so.6", at 0x732af4963189, in QCoreApplication::notifyInternal2(QObject*, QEvent*)
#15   Object "/home/myname/.local/opt/Groot2/lib/libQt6Core.so.6", at 0x732af496663c, in QCoreApplicationPrivate::sendPostedEvents(QObject*, int, QThreadData*)
-----------------------------
b-adkins commented 2 months ago

I had assumed that Groot was only considering that the root-level JSON was an object or primitive. But I tried a workaround where I added a trivial key to the JSON serializer:

namespace std
{
void to_json(nlohmann::json& j, const std::vector<double>& x)
{
  j["vector"] = nlohmann::json::array();
  for(const double& element : x)
  {
    j["vector"].push_back(element);
  }
  std::cout << j.dump(2) << std::endl;
}
void from_json(const nlohmann::json&, std::vector<double>&)
{
  // Not implemented, but it's not used for Groot
  throw std::runtime_error("Not implemented");
}
}

Console output showing it was run:

[INFO] [1726172162.754222982] [bt_action_server]: Tree finished with status: SUCCESS
{
  "vector": [
    -0.111,
    0.0,
    0.0
  ]
}

Groot2 crashed with the same error. I am guessing then that it will crash on JSON arrays anywhere. This severely limits the data types that can go in the Blackboard.

b-adkins commented 2 months ago

A workaround: add trivial keys for the array elements

  void to_json(nlohmann::json& j, const std::vector<double>& x)
  {
    for(unsigned i = 0; i < x.size(); i++)
    {
      std::stringstream key;
      key << i;
      j[key.str()] = x[i];
    }
  }

It looks pretty good, like this is just how Groot shows arrays. Screenshot from 2024-09-12 13-59-15