Automatic Target-Oriented Mapping of Precariously Balanced Rocks

This repository contains a ROS-based automatic target-oriented mapping system for locating and mapping sparse geologic features with an unpiloted aircraft system (UAS). Its primary application is the automated detection, localization, and three-dimensional mapping of precariously balanced rocks (PBRs).

PBRs are sparse, spatially distributed geologic features whose locations and geometries support studies of past earthquake ground motions and seismic hazard. Instead of applying the same dense survey pattern everywhere, this method first detects candidate targets and then directs the UAS to collect target-specific observations. The goal is to make field mapping of PBR populations more scalable and efficient.

System capabilities

• YOLO-based PBR detection in aerial imagery
• Online bounding-box tracking
• Three-dimensional target localization from UAS observations
• Target-oriented motion planning
• RGB-D SLAM and target point-cloud construction
• Assembly and visualization of mapped targets

Research publication

This repository accompanies:

Z. Chen, S. Bearman, J. Ramon Arrowsmith, and J. Das, "Localization and Mapping of Sparse Geologic Features with Unpiloted Aircraft Systems," RSS Workshop on Robots in the Wild, 2020.

See the GitHub Wiki for additional documentation.

Apache License 2.0 (c) 2020 Distributed Robotic Exploration and Mapping Systems Laboratory, Arizona State University.

System overview

Perception subsystem workflow

Motion state diagram

Video demos

1. Granite Dells

2. Blender field

Dependencies

darknet_ros: ROS implementation of YOLO real-time detection
rtabmap_ros: RGBD SLAM
uav_motion: minimum-snap trajectory generation and attitude control for PX4-based rotary wing drones ros_iou_tracking: ROS implementation of simple online realtime tracking (SORT)

ROS Nodes

1. iou_tracker.py

Tracking bounding boxes using SORT.
Publications:

• /bbox_tracker/bounding_boxes [darknet_ros_msgs/BoundingBoxes]
• /iou_tracker/detection_image [sensor_msgs/Image]
• /rosout [rosgraph_msgs/Log]

Subscriptions:

• /clock [rosgraph_msgs/Clock]
• /darknet_ros/bounding_boxes [darknet_ros_msgs/BoundingBoxes]
• /darknet_ros/detection_image [sensor_msgs/Image]

2. target_tracker.py

3D points tracker Publications:

• /path_planner/target_plan/cancel [actionlib_msgs/GoalID]
• /path_planner/target_plan/goal [target_mapping/TargetPlanActionGoal]
• /rosout [rosgraph_msgs/Log]
• /target_tracker/detection_image [sensor_msgs/Image]
• /target_tracker/ellipsoids [visualization_msgs/MarkerArray]
• /target_tracker/points [sensor_msgs/PointCloud2]

Subscriptions:

• /bbox_tracker/bounding_boxes_drop [unknown type]
• /clock [rosgraph_msgs/Clock]
• /darknet_ros/detection_image [sensor_msgs/Image]
• /mavros/local_position/pose [geometry_msgs/PoseStamped]
• /path_planner/target_plan/feedback [unknown type]
• /path_planner/target_plan/result [unknown type]
• /path_planner/target_plan/status [unknown type]

3. path_planner.py

Planning the lawnmower search, b-cylinder estimation motion, and mapping motion.
Publications:

• /local_path [nav_msgs/Path]
• /path_planner/cylinder_marker [visualization_msgs/Marker]
• /path_planner/target_plan/feedback [target_mapping/TargetPlanActionFeedback]
• /path_planner/target_plan/result [target_mapping/TargetPlanActionResult]
• /path_planner/target_plan/status [actionlib_msgs/GoalStatusArray]
• /rosout [rosgraph_msgs/Log]
• /waypoints/cancel [actionlib_msgs/GoalID]
• /waypoints/goal [uav_motion/waypointsActionGoal]

Subscriptions:

• /clock [rosgraph_msgs/Clock]
• /mavros/local_position/pose [geometry_msgs/PoseStamped]
• /path_planner/target_plan/cancel [actionlib_msgs/GoalID]
• /path_planner/target_plan/goal [target_mapping/TargetPlanActionGoal]
• /rtabmap/cloud_map [sensor_msgs/PointCloud2]
• /waypoints/feedback [uav_motion/waypointsActionFeedback]
• /waypoints/result [uav_motion/waypointsActionResult]
• /waypoints/status [actionlib_msgs/GoalStatusArray]

4. rtabmap.launch

Mapping targets.

roslaunch target_mapping rtabmap.launch

5. darknet_ros_pbr.launch

Detecting targets, e.g. precariously balanced rocks (pbrs). The weights and network configurations need to be customized.

roslaunch target_mapping sort_tracker.launch

6. uav_motion.launch

UAV motion system.

roslaunch uav_motion uav_motion.launch

Simulations

1. Models

gazebo_sim_models: Download .dae files of models

2. Launch environments

(1) Granite Dells world

roslaunch gazebo_sim_models mavros_iris_granite_dell_v2.launch

(2) Blender field world

roslaunch gazebo_sim_models mavros_iris_blender_terrain.launch

3. Launch target mapping

roslaunch gazebo_sim_models mavros_iris_granite_dell_v2.launch
roslaunch target_mapping sort_tracker.launch
roslaunch uav_motion uav_motion.launch
rosrun target_mapping target_tracker.py
roslaunch target_mapping rtabmap.launch
rosrun target_mapping path_planner.py

Results Visualization

You can run map_assembler.py to merge all mapped targets and display them in rviz, or use the APIs in map_assembler.py to display point cloud with open3d.

Configurations

Parameters are defined in config/target_mapping.yaml. E.g.,

half_vfov: 0.3490658503988659 #20/180.*np.pi # half vertical fov for mapping
trans_threshold: 1.5  # keyframe selection (trans)
rot_threshold: 0.5235987755982988  # 30/180.*np.pi # keyframe selection (rot)
nm: 2000  # number of 3D points per target

# granite dell
z_min: 3 
z_max: 18
trigger_converging_DE: 3. 
alpha: 0.6981317007977318  # 40. / 180 * np.pi  # z axis for mapping
bcem_alpha: 0.6981317007977318  # z axis for b-cylinder estimation motion

Citation

Please cite this paper if you use the code or algorithms from this work ;)

@article{chen2020localization,
  title={Localization and Mapping of Sparse Geologic Features with Unpiloted Aircraft Systems},
  author={Chen, Zhiang and Bearman, Sarah and Arrowsmith, J Ramon and Das, Jnaneshwar},
  journal={RSS Workshop on Robots in the Wild},
  year={2020}
}