3d_navigation: 3d_nav_executive | 3d_nav_gazebo | pose_follower_3d | sbpl_lattice_planner_3d

Package Summary

This stack provides navigation capabilities in complex environments with 3D collision checks. This allows for navigation in any robot configuration (e.g. with untucked arms) and for various mobile manipulation tasks.

• Author: Armin Hornung, Mike Philips
• License: BSD

• Repository: https://github.com/ros-planning/3d_navigation

• Issue report: https://github.com/ros-planning/3d_navigation/issues

Overview

Related news blog post

Details are available in the corresponding publication Navigation in Three-Dimensional Cluttered Environments for Mobile Manipulation by A. Hornung, M. Phillips, E. G. Jones, M. Bennewitz, M. Likhachev, and S. Chitta (ICRA 2012).

@INPROCEEDINGS{hornung12icra_pr2,
  author = {Armin Hornung and Mike Phillips and E. Gil Jones and 
  Maren Bennewitz and Maxim Likhachev and Sachin Chitta},
  title = {Navigation in Three-Dimensional Cluttered Environments 
  for Mobile Manipulation},
  booktitle = {Proc.~of the IEEE International Conference on Robotics and 
  Automation (ICRA)},
  year = {2012},
  month = {May}
}

3d_navigation aims extend the navigation stack to navigate in complex unstructured environments. octomap is used as 3D world model to allow collision checks of the robot's full body configuration. The map is incrementally built from sensor data in octomap_server. This enables navigation with untucked arms for various mobile manipulation tasks, such as maximizing the reach for tabletop manipulation, carrying trays, or pick and place tasks.

sbpl_lattice_planner_3d provides the global planner plugin, while pose_follower_3d provides the local one. Launch files and configuration are provided in 3d_nav_executive. A simulation environment can be set up in Gazebo with 3d_nav_gazebo.

Setup

In addition to the default stacks which get installed with the Ubuntu package ros-diamondback-desktop-full, you need the stacks installed that 3d_navigation depends on. This can be done on Ubuntu by installing

ros-diamondback-pr2-simulator ros-diamondback-navigation-experimental ros-diamondback-motion-planners ros-diamondback-pr2-common-actions ros-diamondback-pr2-apps ros-diamondback-point-cloud-perception ros-diamondback-pr2-arm-navigation

Then use the following rosinstall to overlay your diamondback installation with the development versions of additional stacks and the 3d_navigation code:

- svn:
    uri: https://code.ros.org/svn/wg-ros-pkg/branches/trunk_diamondback/sandbox/3d_navigation/
    local-name: stacks/3d_navigation
- svn:
    uri: http://alufr-ros-pkg.googlecode.com/svn/branches/octomap_mapping-diamondback/
    local-name: stacks/octomap_mapping
- svn:
    uri: https://code.ros.org/svn/wg-ros-pkg/stacks/motion_planning_common/branches/arm_navigation_metrics
    local-name: stacks/motion_planning_common

After all packages are checked out and in your ROS_PACKAGE_PATH, compile with

rosmake --rosdep-install 3d_navigation

Running 3d_navigation

Startup in simulation

Simply run

roslaunch 3d_nav_gazebo 3d_nav_gazebo_complete.launch

See 3d_nav_gazebo for details.

Startup on the robot (PR2)

You need to increase the timeout parameter of pr2_base_controller to run the local planner without stopping due to timeouts. The configured 0.2s are not always enough, 0.3-0.4 s work fine. For this, you need to create an overlay of pr2_controller_configuration so the parameter gets loaded properly when running robot start. In your overlay, change the parameter base_controller/timeout in pr2_base_controller2.yaml.

Start up 2D localization (amcl) first with

roslaunch 3d_nav_executive wg_2dnav_amcl.launch

In RViz, set the estimated robot pose in the 2D map used for localization and drive the robot teleoperated for a short distance so that the particle filter converges.

Then, start up the 3D map integration with

roslaunch 3d_nav_executive 3d_nav_environment.launch

and after you acquired a few scans in the map (see below) start 3D navigation with

roslaunch 3d_nav_executive move_base_sbpl_3d.launch

Don't forget to turn off the gamepad teleoperaton at this points, as it might interfere with the robot's commands.

Planning

Use RViz with the config file provided at 3d_nav_executive/3d_nav_view to set a 2D navigation goal. If the goal configuration is valid, a green robot will appear at the goal pose (published as Marker in the topic /planning_scene_visualizer). Otherwise, detected collisions at start or goal are published as red spheres.

Current limitations

There is no automatic map acquisition when starting up. It is recommended to teleoperate the robot at the beginning. Just rotate the robot on the spot when looking downwards to build a map of the immediate surroundings.

Only the textured narrow stereo is integrated in the 3D map currently, so keep the narrow field of view in mind when navigating.