Helper functions for using image regions of PointCloud2s. More...
Namespaces | |
namespace | cluster_bounding_box_finder |
namespace | convert_functions |
namespace | draw_functions |
namespace | image_region_functions |
namespace | msg |
namespace | shapes |
Classes | |
class | ActionWrapper |
Wrapper class for SimpleActionClient that checks server availability on first use. More... | |
class | BadParamException |
Thrown when a needed parameter on the parameter server is found but does not have the right type. More... | |
class | CollisionMapException |
Thrown when the grasping pipeline has failed to communicate with the collision map. More... | |
class | GraspException |
General base class for all exceptions originating in the grasping pipeline. More... | |
class | GraspExecutor |
Base class, interface and common functionality for grasp execution. More... | |
class | GraspExecutorWithApproach |
Uses an interpolated IK approach from pregrasp to final grasp. More... | |
class | GraspMarkerPublisher |
Publishes and keeps track of debug grasp markers. More... | |
class | HandDescription |
class | IncompatibleRobotStateException |
Thrown if the current robot state prevents any grasp execution. More... | |
class | MechanismException |
Thrown when the grasping pipeline has failed to communicate with or issue commands to the robot. More... | |
class | MechanismInterface |
A collection of ROS service and action clients needed for grasp execution. More... | |
class | MissingParamException |
Thrown when a needed parameter is not found on the parameter server. More... | |
class | MoveArmStuckException |
Thrown if move arm can not plan out of the current arm state. More... | |
class | MultiArmActionWrapper |
A wrapper for multiple instances of a given action, one for each arm in the system. More... | |
class | MultiArmServiceWrapper |
A wrapper for multiple instances of a given service, one for each arm in the system. More... | |
class | ObjectManipulator |
Oversees the grasping app; bundles together functionality in higher level calls. More... | |
class | ObjectManipulatorNode |
Wraps the Object Manipulator in a ROS API. More... | |
class | PlaceExecutor |
Functionality for placing a grasped object at a given location. More... | |
class | ReactiveGraspExecutor |
class | ReactivePlaceExecutor |
Uses a reactive version of the move from pre-place to place. More... | |
class | ServiceNotFoundException |
Thrown when a service or action server was not found. More... | |
class | ServiceWrapper |
Wrapper class for service clients to perform initialization on first use. More... | |
class | SimpleGraspExecutor |
Simplest grasp executor, using only final grasp information. More... | |
Functions | |
motion_planning_msgs::OrderedCollisionOperations | concat (const motion_planning_msgs::OrderedCollisionOperations &o1, const motion_planning_msgs::OrderedCollisionOperations &o2) |
template<class T > | |
std::vector< T > | concat (const std::vector< T > &v1, const std::vector< T > &v2) |
void | drawBox (ros::Publisher &pub, const shapes::Box &shape, const char *ns="box", int id=0, const ros::Duration lifetime=ros::Duration(), const std_msgs::ColorRGBA color=msg::createColorMsg(0.5, 0.5, 0.5, 0.5), bool destroy=false, bool frame_locked=false) |
void | drawCylinder (ros::Publisher &pub, const shapes::Cylinder &shape, const char *ns="cylinder", int id=0, const ros::Duration lifetime=ros::Duration(), const std_msgs::ColorRGBA color=msg::createColorMsg(0.5, 0.5, 0.5, 0.5), bool destroy=false, bool frame_locked=false) |
void | drawMesh (ros::Publisher &pub, const shapes::Mesh &shape, const char *ns="mesh", int id=0, const ros::Duration lifetime=ros::Duration(), const std_msgs::ColorRGBA color=msg::createColorMsg(0.5, 0.5, 0.5, 0.5), bool destroy=false, bool frame_locked=false) |
void | drawSphere (ros::Publisher &pub, const shapes::Sphere &shape, const char *ns="sphere", int id=0, const ros::Duration lifetime=ros::Duration(), const std_msgs::ColorRGBA color=msg::createColorMsg(0.5, 0.5, 0.5, 0.5), bool destroy=false, bool frame_locked=false) |
HandDescription & | handDescription () |
Returns a hand description singleton. | |
MechanismInterface & | mechInterface () |
Returns a MechanismInterface singleton. | |
Variables | |
static const std::string | ATTACHED_COLLISION_TOPIC = "attached_collision_object" |
static const std::string | CARTESIAN_COMMAND_SUFFIX = "/cart/command_pose" |
static const std::string | CHECK_STATE_VALIDITY_NAME = "environment_server/get_state_validity" |
static const std::string | FK_SERVICE_SUFFIX = "/get_fk" |
static const std::string | GRASP_STATUS_SUFFIX = "/grasp_status" |
static const std::string | HAND_POSTURE_ACTION_SUFFIX = "/hand_posture_execution" |
static const std::string | IK_QUERY_SERVICE_SUFFIX = "/get_ik_solver_info" |
static const std::string | IK_SERVICE_SUFFIX = "/constraint_aware_ik" |
static const std::string | INTERPOLATED_IK_SERVICE_SUFFIX = "/interpolated_ik" |
static const std::string | INTERPOLATED_IK_SET_PARAMS_SERVICE_SUFFIX = "/interpolated_ik_set_params" |
static const std::string | LIST_CONTROLLERS_SERVICE_NAME = "/list_controllers" |
static const std::string | MARKERS_NAMESPACE = "grasp_markers" |
static const std::string | MARKERS_OUT_NAME = "grasp_execution_markers" |
static const std::string | MOVE_ARM_ACTION_SUFFIX = "/move_arm" |
static const std::string | MOVE_ARM_CONSTRAINED_PLANNER_SERVICE_NAME = "ompl_planning/plan_kinematic_path" |
static const std::string | MOVE_ARM_PLANNER_ID = "SBLkConfig1" |
static const std::string | MOVE_ARM_PLANNER_SERVICE_NAME = "ompl_planning/plan_kinematic_path" |
static const std::string | NORMALIZE_SERVICE_NAME = "trajectory_filter_unnormalizer/filter_trajectory" |
static const double | OBJECT_POSITION_TOLERANCE_X = 0.02 |
static const double | OBJECT_POSITION_TOLERANCE_Y = 0.02 |
static const double | OBJECT_POSITION_TOLERANCE_Z = 0.02 |
static const std::string | PICKUP_ACTION_NAME = "object_manipulator_pickup" |
static const std::string | PLACE_ACTION_NAME = "object_manipulator_place" |
static const std::string | POINT_HEAD_ACTION_TOPIC = "/head_traj_controller/point_head_action" |
static const std::string | REACTIVE_GRASP_ACTION_SUFFIX = "/reactive_grasp" |
static const std::string | REACTIVE_LIFT_ACTION_SUFFIX = "/reactive_lift" |
static const std::string | REACTIVE_PLACE_ACTION_SUFFIX = "/reactive_place" |
static const std::string | SWITCH_CONTROLLER_SERVICE_NAME = "/switch_controller" |
static const std::string | TRAJECTORY_ACTION_SUFFIX = "/joint_trajectory" |
Helper functions for using image regions of PointCloud2s.
motion_planning_msgs::OrderedCollisionOperations object_manipulator::concat | ( | const motion_planning_msgs::OrderedCollisionOperations & | o1, | |
const motion_planning_msgs::OrderedCollisionOperations & | o2 | |||
) |
Definition at line 41 of file vector_tools.cpp.
std::vector<T> object_manipulator::concat | ( | const std::vector< T > & | v1, | |
const std::vector< T > & | v2 | |||
) | [inline] |
Definition at line 42 of file vector_tools.h.
void object_manipulator::drawBox | ( | ros::Publisher & | pub, | |
const shapes::Box & | shape, | |||
const char * | ns = "box" , |
|||
int | id = 0 , |
|||
const ros::Duration | lifetime = ros::Duration() , |
|||
const std_msgs::ColorRGBA | color = msg::createColorMsg(0.5, 0.5, 0.5, 0.5) , |
|||
bool | destroy = false , |
|||
bool | frame_locked = false | |||
) |
Definition at line 60 of file shape_tools.cpp.
void object_manipulator::drawCylinder | ( | ros::Publisher & | pub, | |
const shapes::Cylinder & | shape, | |||
const char * | ns = "cylinder" , |
|||
int | id = 0 , |
|||
const ros::Duration | lifetime = ros::Duration() , |
|||
const std_msgs::ColorRGBA | color = msg::createColorMsg(0.5, 0.5, 0.5, 0.5) , |
|||
bool | destroy = false , |
|||
bool | frame_locked = false | |||
) |
Definition at line 43 of file shape_tools.cpp.
void object_manipulator::drawMesh | ( | ros::Publisher & | pub, | |
const shapes::Mesh & | shape, | |||
const char * | ns = "mesh" , |
|||
int | id = 0 , |
|||
const ros::Duration | lifetime = ros::Duration() , |
|||
const std_msgs::ColorRGBA | color = msg::createColorMsg(0.5, 0.5, 0.5, 0.5) , |
|||
bool | destroy = false , |
|||
bool | frame_locked = false | |||
) |
Definition at line 94 of file shape_tools.cpp.
void object_manipulator::drawSphere | ( | ros::Publisher & | pub, | |
const shapes::Sphere & | shape, | |||
const char * | ns = "sphere" , |
|||
int | id = 0 , |
|||
const ros::Duration | lifetime = ros::Duration() , |
|||
const std_msgs::ColorRGBA | color = msg::createColorMsg(0.5, 0.5, 0.5, 0.5) , |
|||
bool | destroy = false , |
|||
bool | frame_locked = false | |||
) |
Definition at line 77 of file shape_tools.cpp.
HandDescription& object_manipulator::handDescription | ( | ) | [inline] |
Returns a hand description singleton.
Definition at line 160 of file hand_description.h.
MechanismInterface& object_manipulator::mechInterface | ( | ) | [inline] |
Returns a MechanismInterface singleton.
Due to problems with waitForServer, it is not recommended to use multiple instances of the MechanismInterface in the same node. This function provides a singleton.
CAREFUL WITH MULTI-THREADED CODE!!!
Definition at line 408 of file mechanism_interface.h.
const std::string object_manipulator::ATTACHED_COLLISION_TOPIC = "attached_collision_object" [static] |
Definition at line 65 of file mechanism_interface.cpp.
const std::string object_manipulator::CARTESIAN_COMMAND_SUFFIX = "/cart/command_pose" [static] |
Definition at line 59 of file mechanism_interface.cpp.
const std::string object_manipulator::CHECK_STATE_VALIDITY_NAME = "environment_server/get_state_validity" [static] |
Definition at line 47 of file mechanism_interface.cpp.
const std::string object_manipulator::FK_SERVICE_SUFFIX = "/get_fk" [static] |
Definition at line 41 of file mechanism_interface.cpp.
const std::string object_manipulator::GRASP_STATUS_SUFFIX = "/grasp_status" [static] |
Definition at line 45 of file mechanism_interface.cpp.
const std::string object_manipulator::HAND_POSTURE_ACTION_SUFFIX = "/hand_posture_execution" [static] |
Definition at line 55 of file mechanism_interface.cpp.
const std::string object_manipulator::IK_QUERY_SERVICE_SUFFIX = "/get_ik_solver_info" [static] |
Definition at line 44 of file mechanism_interface.cpp.
const std::string object_manipulator::IK_SERVICE_SUFFIX = "/constraint_aware_ik" [static] |
Definition at line 40 of file mechanism_interface.cpp.
const std::string object_manipulator::INTERPOLATED_IK_SERVICE_SUFFIX = "/interpolated_ik" [static] |
Definition at line 42 of file mechanism_interface.cpp.
const std::string object_manipulator::INTERPOLATED_IK_SET_PARAMS_SERVICE_SUFFIX = "/interpolated_ik_set_params" [static] |
Definition at line 43 of file mechanism_interface.cpp.
const std::string object_manipulator::LIST_CONTROLLERS_SERVICE_NAME = "/list_controllers" [static] |
Definition at line 58 of file mechanism_interface.cpp.
const std::string object_manipulator::MARKERS_NAMESPACE = "grasp_markers" [static] |
Definition at line 42 of file grasp_marker_publisher.cpp.
const std::string object_manipulator::MARKERS_OUT_NAME = "grasp_execution_markers" [static] |
Definition at line 41 of file grasp_marker_publisher.cpp.
const std::string object_manipulator::MOVE_ARM_ACTION_SUFFIX = "/move_arm" [static] |
Definition at line 53 of file mechanism_interface.cpp.
const std::string object_manipulator::MOVE_ARM_CONSTRAINED_PLANNER_SERVICE_NAME = "ompl_planning/plan_kinematic_path" [static] |
Definition at line 63 of file mechanism_interface.cpp.
const std::string object_manipulator::MOVE_ARM_PLANNER_ID = "SBLkConfig1" [static] |
Definition at line 61 of file mechanism_interface.cpp.
const std::string object_manipulator::MOVE_ARM_PLANNER_SERVICE_NAME = "ompl_planning/plan_kinematic_path" [static] |
Definition at line 62 of file mechanism_interface.cpp.
const std::string object_manipulator::NORMALIZE_SERVICE_NAME = "trajectory_filter_unnormalizer/filter_trajectory" [static] |
Definition at line 48 of file mechanism_interface.cpp.
const double object_manipulator::OBJECT_POSITION_TOLERANCE_X = 0.02 [static] |
Definition at line 69 of file mechanism_interface.cpp.
const double object_manipulator::OBJECT_POSITION_TOLERANCE_Y = 0.02 [static] |
Definition at line 70 of file mechanism_interface.cpp.
const double object_manipulator::OBJECT_POSITION_TOLERANCE_Z = 0.02 [static] |
Definition at line 71 of file mechanism_interface.cpp.
const std::string object_manipulator::PICKUP_ACTION_NAME = "object_manipulator_pickup" [static] |
Definition at line 39 of file object_manipulator_node.cpp.
const std::string object_manipulator::PLACE_ACTION_NAME = "object_manipulator_place" [static] |
Definition at line 40 of file object_manipulator_node.cpp.
const std::string object_manipulator::POINT_HEAD_ACTION_TOPIC = "/head_traj_controller/point_head_action" [static] |
Definition at line 67 of file mechanism_interface.cpp.
const std::string object_manipulator::REACTIVE_GRASP_ACTION_SUFFIX = "/reactive_grasp" [static] |
Definition at line 50 of file mechanism_interface.cpp.
const std::string object_manipulator::REACTIVE_LIFT_ACTION_SUFFIX = "/reactive_lift" [static] |
Definition at line 51 of file mechanism_interface.cpp.
const std::string object_manipulator::REACTIVE_PLACE_ACTION_SUFFIX = "/reactive_place" [static] |
Definition at line 52 of file mechanism_interface.cpp.
const std::string object_manipulator::SWITCH_CONTROLLER_SERVICE_NAME = "/switch_controller" [static] |
Definition at line 57 of file mechanism_interface.cpp.
const std::string object_manipulator::TRAJECTORY_ACTION_SUFFIX = "/joint_trajectory" [static] |
Definition at line 54 of file mechanism_interface.cpp.