Reactive Navigation with Semantic Feedback Using ROS
This package can be used for doubly reactive navigation with semantic feedback, using C++ and ROS.
It has been tested with Ubuntu 18.04 and ROS Melodic, on three different robots: Turtlebot, Ghost Robotics Minitaur™ and Ghost Robotics Spirit™.
Maintainer: Vasileios Vasilopoulos email@example.com
Relevant publications and packages
The code included in this package has been used in the following papers:
- V. Vasilopoulos, G. Pavlakos, S. L. Bowman, J. D. Caporale, K. Daniilidis, G. J. Pappas, D. E. Koditschek, "Reactive Semantic Planning in Unexplored Semantic Environments Using Deep Perceptual Feedback" (under review).
- V. Vasilopoulos, G. Pavlakos, K. Schmeckpeper, K. Daniilidis, D. E. Koditschek, "Reactive Navigation in Partially Familiar Planar Environments Using Semantic Perceptual Feedback", arXiv: 2002.08946.
The doubly-reactive operations in the model space are based on the papers:
- Arslan, O., and Koditschek, D. E., "Exact Robot Navigation using Power Diagrams", IEEE International Conference on Robotics and Automation (ICRA '16), 2016, pp. 1-8.
- Arslan, O., and Koditschek, D. E., "Sensor-based Reactive Navigation in Unknown Convex Sphere Worlds", The 12th International Workshop on the Algorithmic Foundations of Robotics (WAFR), 2016.
The package assumes that the robot possesses:
- a LIDAR sensor, for estimating distance to unknown obstacles.
- a way of generating a semantic map of its surroundings with familiar obstacles (see details in Semantic SLAM interfaces below).
- a way of generating its own odometry estimate.
These three inputs are given as topics in the
navigation_* launch files (see below).
- For our experiments, we use the ZED Mini stereo camera. A resolution of 720HD@60Hz works well with an NVIDIA TX2 or an NVIDIA Xavier (make sure to run
sudo nvpmodel -m 0to enable maximum performance first).
- For reading a Hokuyo LIDAR sensor, we use the urg_node package.
- For LIDAR downsampling, we use the (forked and modified) laser_scan_sparsifier package, included in scan_tools. This package depends on csm which must be installed first.
- We use the robot_localization package for fusing odometry inputs from multiple sources.
- We use Boost Geometry for basic operations with planar polygons, which must be already installed in your system.
- For more advanced computational geometry operations, we use the CGAL library. See here for installation instructions.
- We implement the ear clipping triangulation method in C++ using the earcut.hpp package, included here. For the Python implementation, we use the tripy package.
- Except for the ROS Python packages (already included with ROS), the following Python packages are also needed:
- For properly using the visualization functionalities in visualization.py, we need the Python modules
- For benchmark experiments with Vicon, the motion_capture_system is used.
You can install all the prerequisites, by first independently installing the ZED SDK on your machine, and then running the following commands:
sudo apt-get install ros-melodic-urg-node ros-melodic-robot-localization python-shapely python-scipy python-numpy libcgal-dev cd ~/catkin_ws/src git clone https://github.com/stereolabs/zed-ros-wrapper.git git clone https://github.com/AndreaCensi/csm.git git clone https://github.com/vvasilo/scan_tools.git git clone https://github.com/KumarRobotics/motion_capture_system.git catkin build csm catkin build pip install tripy
Once all the prerequisites above are satisfied, install with
cd ~/catkin_ws/src git clone https://github.com/vvasilo/semnav.git cp -r semnav/extras/object_pose_interface_msgs . catkin build
Semantic SLAM interfaces
This package needs an external Semantic SLAM engine, not included here. However, any such engine can be used. The only restriction is associated with the type of messages used, i.e., the semantic map has to be given in a specific way.
In our implementation, these messages are included in a separate package called
object_pose_interface_msgs. We include pointers to the necessary message formats in the extras folder.
We provide the semantic map in the form of a
SemanticMapObjectArray message. Each
SemanticMapObject in the array has a
pose element, as well as a number of 3D
Using semslam_polygon_publisher.py, we project those
keypoints on the horizontal plane of motion, and republish the semantic map object with a CCW-oriented
polygon2d element (i.e., the projection of this 3D object on the 2D plane). To do so, we use a pre-defined object mesh, given in the form of a .mat file. The
mesh_location for all objects is defined in the associated
tracking_* launch file (see below), and we include examples for different objects here.
Note: If the user knows the 2D polygon directly, the above procedure is not necessary - only the
polygon2d element of each
SemanticMapObject is used for navigation.
Types of files and libraries
- The main reactive planning library is reactive_planner_lib.cpp (in Python: reactive_planner_lib.py), which uses functions from polygeom_lib.cpp (in Python: polygeom_lib.py). This file includes the functionality for the diffeomorphism construction using either the ear clipping algorithm (see the functions
polygonDiffeoTriangulation), or convex decomposition (see the functions
polygonDiffeoConvex). In the C++ implementation, reactive_planner_lib.cpp and polygeom_lib.cpp are built together into a single library.
- We include several navigation nodes, depending on each geometric or semantic task:
- navigation.cpp is the basic navigation node. The user has to specify a geometric target that the robot needs to reach.
- navigation_humans.cpp also uses moving humans as obstacles. It is assumed that the robot detects humans with the standard 24-keypoint interface (ROS message
- navigation_semantic.cpp lets the robot navigate to a predefined geometric target, until it sees a desired object. Then, it tries to reach an obstacle free side in front or behind that object.
- human_following.cpp is the basic human following node. The robot navigates to a predefined geometric target, until it sees a human. Then, it follows the closest human, or navigates to the last position it saw a human.
- human_following_fallen.cpp lets the robot navigate to a predefined geometric target, until it sees a human falling down - then it proceeds to approach him.
- human_following_signal.cpp lets the robot navigate to a predefined geometric target, until it sees a human. Then it follows the closest human (or navigates to the last position it saw one), until a stop gesture is given (left or right hand raised) - after that, it navigates back to its starting position.
- The visualization.py script visualizes properties of the diffeomorphism construction, using the Python implementations (reactive_planner_lib.py and polygeom_lib.py).
To use the code on a real robot, you need to launch one of each type of launch files below:
- The files with name
bringup_*launch the sensors for each corresponding robot. For example, the file bringup_turtlebot.launch launches:
- The files with name
tracking_*launch the tracking files needed for semantic navigation. For example, the file tracking_turtlebot_semslam_onboard.launch launches:
- the corresponding semantic SLAM launch file from the semantic_slam package.
- the necessary tf transforms (e.g., between the camera and the robot and between the LIDAR and the robot) for this particular robot.
- the semslam_polygon_publisher.py node that subscribes to the output of the semantic SLAM and publishes 2D polygons on the plane.
- The files with name
navigation_*launch the reactive controller. For example, the file navigation_turtlebot_onboard.launch launches the main navigation node for Turtlebot, which subscribes to:
- the local odometry node (in this case provided directly by the ZED stereo camera).
- the LIDAR data, after downsampling.
- the 2D polygons from semslam_polygon_publisher.py.
- necessary tf updates to correct local odometry as new updates from the semantic SLAM pipeline become available.