Implement https://github.com/team-vigir/flexbe_behavior_engine/pull/161 here in both ROS 1 and ROS 2 versions