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