Interface and Behavior Tree

From a high-level standpoint, the planner consists of a core planning module (evaluate.py script) and ROS-dependent environment wrappers (rotors_wrapper.py for Gazebo-RotorS simulator, flightmare_wrapper.py for Flightmare simulator, and realtime_ros_wrapper_infogain_ardupilot.py for running in LMF hardware)

Input Interface

The list of waypoints is specified in WAYPOINT_FILE in config.py. We provide some example waypoint files in the waypoints folder.

The following services can be used to interact with the planner:

• start_planner (type std_srvs/Empty): start the planner
• stop_planner (type std_srvs/Empty): stop the planner
• set_goal_dir (custom type): if the waypoint list is not specified (or empty), use this service to change the desired goal direction of the robot (unit: rad)

Subscribed topics in sim (specified in config.py)

• SIM_DEPTH_TOPIC: the topic of the depth image in the simulator, type sensor_msgs/Image, unit: meters
• SIM_ODOM_TOPIC: the topic of the robot's odometry in the world frame in the simulator (for calculating the unit goal vector and checking if the robot has reached the waypoints), type nav_msgs/Odometry
• SIM_IMU_TOPIC: the IMU topic in the simulator, type sensor_msgs/Imu
• SIM_MASK_TOPIC: the interestingness mask topic in the simulator for visually attentive navigation task, type sensor_msgs/Image. This mask is used only when PLANNING_TYPE >= 2. The detection mask is such that each pixel of the depth image is associated with a value from 0 (lowest) to 255 (highest) based on its interestingness.
• SIM_LATENT_TOPIC: the latent_vector topic in the simulator when using PLANNING_TYPE = 1, type std_msgs/Float32MultiArray

Subscribed topics in the real robot (specified in config.py)

• ROBOT_DEPTH_TOPIC: the topic of the depth image, type sensor_msgs/Image, depth unit: mm
• ROBOT_ODOM_TOPIC: the topic of the robot's odometry in the world frame (for calculating the unit goal vector and checking if the robot has reached the waypoints), type nav_msgs/Odometry
• ROBOT_MASK_TOPIC: the interestingness mask topic for visually attentive navigation task, type sensor_msgs/Image. This mask is used only when PLANNING_TYPE >= 2. The detection mask is such that each pixel of the depth image is associated with a value from 0 (lowest) to 255 (highest) based on its interestingness.
• ROBOT_LATENT_TOPIC: the latent_vector topic when using PLANNING_TYPE = 1, type std_msgs/Float32MultiArray

Output Interface

The planner published the following topics (specified in config.py) to interface with the low-level controller:

• SIM_CMD_TOPIC: the command topic in the simulator, type geometry_msgs/Twist, containing: 3D velocity command in vehicle frame (yaw-rotated world frame) + reference yaw angle (Note: we use angular.z field to save the reference yaw angle!).
• ROBOT_CMD_TOPIC: the command topic in the real robot, its type and content are mentioned above.

You can use this node to convert the [SIM/ROBOT]_CMD_TOPIC message to RollPitchYawrateThrust message or this node to convert the [SIM/ROBOT]_CMD_TOPIC message to /mavros/setpoint_velocity/cmd_vel_unstamped message.

Additionally, for visualization purposes, the following topics are also published:

• TRAJECTORY_TOPIC (specified in config.py): visualization of the trajectory endpoints, type visualization_msgs/MarkerArray, estimated roughly from the motion primitives library, the robot's initial state, and the first-order dynamics models.
• filtered_image: depth image after pre-processing step to fill in missing depth pixels (only when USE_D455_HOLE_FILLING = False)

Behavior Tree of The Planner

We realized a very naive implementation of the Behavior Tree for the navigation task in the config.py. The overview of this Behaviour Tree is illustrated in the figure below.

Deadend action

Reach-goal action

Goal_dir_service action

Timeout action