sbpl_3dnav_planner is a ROS wrapper for SBPL Full-Body Planner library to provide 3D navigation with full-body collision checking to move_base. It adheres to the nav_core::BaseGlobalPlanner interface specified in nav_core.

Source of SBPL Full-Body Planner:

(for ROS Electric only)

git clone https://sbpl.pc.cs.cmu.edu/redmine/people/mike/full-body-nav.git

Overview

The sbpl_3dnav_planner::Sbpl3DNavPlanner is a global planner that adheres to the nav_core::BaseGlobalPlanner interface found in the nav_core package and can be used as a global planner plugin for the move_base node. It performs three-dimensional full-body collision checking to compute paths that are collision-free with the environment.

The New Collision Model

The PR2 makes dancing look easy...

Sbpl3DNavPlanner

API Stability

• The ROS API is unreviewed.
• The C++ API is unreviewed.

ROS Topics

Subscribed Topics

collision_map_occ (arm_navigation_msgs/CollisionMap)

• The 3D collision map needed for collision checking against the environment; required since the Costmap2D supplied by move_base is ignored

joint_states (sensor_msgs/JointState)

• The current status of all the joints of the robot

collision_object (arm_navigation_msgs/CollisionObject)

• Interface to allow individual insertion of collision objects into the 3D collision map

sbpl_attached_collision_object (arm_navigation_msgs/AttachedCollisionObject)

• Interface to allow insertion of attached collision objects into the robot's collision model

ROS Services

Services

/sbpl_full_body_planning/collision_check (sbpl_3dnav_planner/FullBodyCollisionCheck)

• Service to perform collision checking using the robot's collision model and current view of the world

/sbpl_full_body_planning/find_base_poses (sbpl_3dnav_planner/GetBasePoses)

• Service to compute a set of collision-free robot poses from which the robot can reach a specified object

Services Called

<left_fk_service_name> (see params) (kinematics_msgs/GetPositionFK)

• The service called to compute forward kinematics for the left arm

<right_fk_service_name> (see params) (kinematics_msgs/GetPositionFK)

• The service called to compute forward kinematics for the right arm

ROS Parameters

~planner/search_mode (bool, default: true)

• Whether to stop after finding the first solution

~planner/allocated_time (double, default: 60.0)

• Amount of time to allow for planning (in seconds)

~planner/object_radius (double, default: 0.10)

• TODO

~planner/left_arm_description_file (string, default: "")

• File describing the left arm; see package://sbpl_arm_planner/config/pr2_left_arm.cfg for details.

~planner/right_arm_description_file (string, default: "")

• File describing the right arm; see package://sbpl_arm_planner/config/pr2_right_arm.cfg for details.

~planner/motion_primitive_file (string, default: "")

• File describing the motion primitives allowed by the robot's arms

~planner/base_motion_primitive_file (string, default: "")

• File describing the motion primitives allowed by the robot's base

~planner/use_shortened_path (bool, default: false)

• TODO

~debug/print_out_path (bool, default: true)

• Whether to print the returned path to ROS_INFO; prints out joint angles and end effector poses for both arms and the (x, y, torso, theta) positions of the base

~debug/succesors_logger_level (string, default: "info")

• Unused

~robot/waypoint_time (double, default: 0.2)

• The time it takes to move between waypoints on the returned path

~robot/arm_name (string, default: "right_arm")

• Unused

robot_description (string, default: None (required))

• The URDF file for the robot

~robot/num_joints (int, default: 7)

• The number of joints per arm

~reference_frame (string, default: "map")

• The frame for the returned path and visualizations

~left_fk_service_name (string, default: "pr2_left_arm_kinematics/get_fk")

• Name of the service call used to compute forward kinematics for the left arm

~left_ik_service_name (string, default: "pr2_left_arm_kinematics/get_ik")

• Name of the service call used to compute inverse kinematics for the left arm

~right_fk_service_name (string, default: "pr2_right_arm_kinematics/get_fk")

• Name of the service call used to compute forward kinematics for the right arm

~right_ik_service_name (string, default: "pr2_right_arm_kinematics/get_ik")

• Name of the service call used to compute inverse kinematics for the right arm

~use_collision_map_from_sensors (bool, default: true)

• Whether to update the occupancy grid from the collision_map_occ topic

~right_arm_pose_on_object_x (double, default: 0.0)

• The x offset from the right gripper's location to the object's location

~right_arm_pose_on_object_y (double, default: -0.15)

• The y offset from the right gripper's location to the object's location

~right_arm_pose_on_object_z (double, default: 0.0)

• The z offset from the right gripper's location to the object's location

~left_arm_pose_on_object_x (double, default: 0.0)

• The x offset from the left gripper's location to the object's location

~left_arm_pose_on_object_y (double, default: 0.15)

• The y offset from the left gripper's location to the object's location

~left_arm_pose_on_object_z (double, default: 0.0)

• The z offset from the left gripper's location to the object's location

~minimum_working_distance (double, default: 0.3 (in meters))

• The minimum distance away the robot will be from the object when using the /sbpl_full_body_planning/find_base_poses service

~maximum_working_distance (double, default: 0.7)

• The maximum distance away the robot will be from the object when using the /sbpl_full_body_planning/find_base_poses service (in meters)

~yaw_steps (int, default: 16)

• The number of angles to sample when using the /sbpl_full_body_planning/find_base_poses service; samples will be made uniformly from 0 to 2*pi

~radii_steps (int, default: 16)

• The number of radii to sample when using the /sbpl_full_body_planning/find_base_poses service; samples will be made uniformly along the available workspace of the robot (obeying the maximum and minimum working distances parameters)

~collision_space/collision_map_topic (string, default: collision_map_occ)

• Unused

~visualizations/goal (bool, default: true)

• Whether to visualize the goal using PViz when the planner is called

~visualizations/expanded_states (bool, default: true)

• Whether to visualize the state expansions using AViz when the planner is called

~visualizations/heuristic (bool, default: true)

• Whether to visualize the heuristic (the shortest path to the goal) using AViz when the planner is called

~visualizations/voxel_size (double, default: 0.02)

• Unused

~visualizations/trajectory (bool, default: false)

• Whether to visualize the trajectory using PViz when the planner is called

~visualizations/end_effector_path (bool, default: false)

• Whether to visualize the path the end effector takes using AViz when the planner is called

~visualizations/collision_model_trajectory (bool, default: false)

• Unused

~visualizations/collision_model (bool, default: false)

• Whether to visualize the collision model of the robot

~visualizations/trajectory_throttle (int, default: 4)

• Throttle for trajectory visualizations; e.g., a trajectory_throttle of 4 will make it so only every 4th pose will be displayed. Raise this to increase performance

~visualizations/heuristic_grid (bool, default: false)

• Whether to visualize the heuristic grid using AViz when the planner is called; no actual planning will be done if this is set