Behavior Tree for Gesture and Face Detection Sensor - slgrobotics/articubot_one GitHub Wiki

Back to Wiki

Behavior Tree for Gesture and Face Detection Sensor

Contents:

To connect the sensor driver to ROS2/Nav2 Behavior Trees (BTs), the system uses two components:

The perception_adapter converts the driver’s raw output (/face_gesture_detections, a Detection2DArray) into a small set of BT-friendly topics:

  • /bt/face_detected (Bool) published continuously while a face is in view
  • /bt/face_yaw_error (Float32) radians from camera center; positive means “turn left”
  • /bt/gesture_command (String) "STOP", "OK", "YES", "LIKE", "SIX"
  • /bt/face_gesture_detect (Illuminance) - a combination of the above. Yes, using sensor_msgs::msg::Illuminance is a hack, but it works.

These topics are consumed by custom BT Condition and Action plugins.

This design cleanly separates perception, decision-making, and motion control, allowing gesture and face awareness to be added without modifying Nav2 itself.

Note: There are two ways to compile the slg_bt_plugins package, depending on the CMakeLists.txt USE_RCLCPP_SUBSCRIPTIONS definition:

  • if defined (not recommended), each plugin subscribes to its corresponding topic and reacts by returning FAILURE, SUCCESS or managing RUNNING state.
  • if not defined (default) a single FgsTopicToBlackboard BT node (a.k.a."Data Pump") is responsible for pumping data on the BT Blackboard, making the information available to all BT nodes of the tree at runtime. Other BT Nodes use Input Ports to get this data.

(The BT Blackboard is a central key-value store, a shared memory space accessible by all nodes in the behavior tree)

The "Data Pump" configuration is more architecturally sound and robust and simplifies plugin code. It requires use of the "Parallel" pattern in the Behavior Tree code.

The "USE_RCLCPP_SUBSCRIPTIONS" configuration is more fragile but works with simple no-pump trees.

Example Behavior Tree

The file robots/seggy/behavior_trees/face_aware_nav.xml demonstrates how these plugins are used to implement face-aware navigation behavior.

What this tree does:

  • No face detected:
  • Face detected:
  • Turns the robot toward the detected person (keeps turning towards the face as long as a face is detected)
  • Navigation resumes when the face goes out of view
  • STOP gesture:
  • Immediately cancels the current Nav2 goal. Robot stops until a new goal is set
  • Works even while the robot is actively moving/navigating
  • When robot reaches the goal:
  • it stays there, not reacting to a face

Note:

  • Gestures are recognized by the device only when a face is detected
  • Behavior is identical to stock Nav2 when no gestures or faces are detected

Below is a cropped version of the tree (rendered using Nav2 bt2img.py --behavior_tree face_aware_nav.xml --image_out face_aware).

The Nav2 default Subtree is cropped out, leaving the Pump and Face-reacting nodes:

Watch a short video of my Seggy robot running an earlier version of this tree during debugging.

Here are some ChatGPT.com thoughts about this example tree.

  • BT Tree explanation
  • Why is there a need to use failure_count="2" success_count="2" in the Parallel node of that tree? answer
  • Could you suggest improvements to that code? answer

Let me know in issues if you have any suggestions.

Useful Links

Back to Wiki or Docs Folder