Public Member Functions | Protected Member Functions | Protected Attributes
GripperPoseAction Class Reference

An action for getting a gripper pose using interactive markers. More...

List of all members.

Public Member Functions

void eraseAllGraspMarkers ()
geometry_msgs::PoseStamped fromWrist (const geometry_msgs::PoseStamped &ps)
 Translate to the control pose.
geometry_msgs::PoseStamped getDefaultPose (std::string arm_name)
void goalCB ()
 Callback to accept a new action goal.
 GripperPoseAction ()
void initButtonMarker ()
void initGraspMarkers ()
void initGripperControl ()
void initGripperMarker ()
void initMarkers ()
 Re-initializes all markers.
void initObjectMarker ()
void preemptCB ()
 Callback to allow this action to get preempted by backend.
void selectGrasp (const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
void selectGraspIndex (int index)
void selectNextGrasp ()
void setIdle ()
 Remove the markers.
void setSeed (const geometry_msgs::PoseStampedConstPtr &seed)
void setSeedPoint (const geometry_msgs::PointStampedConstPtr &seed_point)
geometry_msgs::PoseStamped toWrist (const geometry_msgs::PoseStamped &ps)
 Translate the control pose to the wrist.
bool transformGripperPose (const std::string frame_id="/base_link")
void updateGripperAngle ()
void updateGripperOpening ()
void updatePoses ()
 Update the pose of certain markers.
 ~GripperPoseAction ()

Protected Member Functions

void acceptCB ()
 Return with the gripper pose if the pose is valid, otherwise do nothing.
void alternativesCB ()
 Find alternatives that are valid grasps.
void cancelCB ()
 Cancel this action call.
void cycleGrasps (const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
 Cycle through planned grasps and initialize the grasp(n/m) button for switching.
void focusCB ()
 Sends a request for the 3D camera to focus on the ghosted gripper.
std::vector
< geometry_msgs::PoseStamped > 
generatePosesAlongDir (tf::Pose pose, tf::Vector3 axis, double min_shift, double max_shift, double resolution, std::string frame_id)
 Generate poses that are shifted along axis, by amounts from min_shift to max_shift with spacing resolution.
std::vector
< geometry_msgs::PoseStamped > 
generateRotatedPoses (tf::Pose pose, tf::Vector3 axis, double min_rot, double max_rot, double resolution, std::string frame_id)
 Generate poses that are rotated about axis, by amounts from min_rot to max_rot with spacing resolution.
bool getModelMesh (int model_id, shape_msgs::Mesh &mesh)
 Get the mesh corresponding to a database model_id.
void graspPlanCB ()
 Call the grasp planner to find grasps near the seed.
void graspPlanResultCB (const actionlib::SimpleClientGoalState &state, const manipulation_msgs::GraspPlanningResultConstPtr &result)
 Callback that receives the result of a TestGripperPose action.
void gripperClickCB (const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
 Called when the gripper is clicked; each call cycles through gripper opening values.
void initMenus ()
 Initialize the menus for all markers.
visualization_msgs::InteractiveMarker makeCloudMarker (const char *name, const geometry_msgs::PoseStamped &stamped, float point_size, std_msgs::ColorRGBA color)
 Create an interactive marker from a point cloud.
void slowSync ()
void spinOnce ()
 ROS spin update callback.
void testGripperResultCallback (const actionlib::SimpleClientGoalState &state, const pr2_object_manipulation_msgs::TestGripperPoseResultConstPtr &result)
 Callback that receives the result of a TestGripperPose action.
void testPose (geometry_msgs::PoseStamped pose, float opening)
 Test one grasp pose.
void testPoses (std::vector< geometry_msgs::PoseStamped > poses, float opening)
 Test a list of grasp poses.
void updateGripper (const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
 Callback for pose updates from the controls.

Protected Attributes

MenuHandler::EntryHandle accept_handle_
bool active_
MenuHandler::EntryHandle alternatives_handle_
double alternatives_search_angle_
double alternatives_search_angle_resolution_
double alternatives_search_dist_
double alternatives_search_dist_resolution_
bool always_call_planner_
bool always_find_alternatives_
geometry_msgs::PoseStamped button_marker_pose_
MenuHandler::EntryHandle cancel_handle_
object_manipulator::ActionWrapper
< point_cloud_server::StoreCloudAction
cloud_server_client_
geometry_msgs::PoseStamped control_offset_
MenuHandler::EntryHandle focus_handle_
ros::ServiceClient get_model_mesh_client_
std::string get_pose_name_
actionlib::SimpleActionServer
< pr2_object_manipulation_msgs::GetGripperPoseAction
get_pose_server_
object_manipulator::ActionWrapper
< manipulation_msgs::GraspPlanningAction > 
grasp_plan_client_
double grasp_plan_region_len_x_
double grasp_plan_region_len_y_
double grasp_plan_region_len_z_
float gripper_angle_
float gripper_opening_
bool gripper_opening_cycling_
geometry_msgs::PoseStamped gripper_pose_
int interface_number_
object_manipulator::MechanismInterface mechanism_
MenuHandler menu_gripper_
ros::NodeHandle nh_
pcl::PointCloud< PointT >::Ptr object_cloud_
bool object_model_
std::vector< int > planner_grasp_types_
int planner_index_
std::vector
< geometry_msgs::PoseStamped > 
planner_poses_
std::vector< PoseStateplanner_states_
ros::NodeHandle pnh_
std::string point_cloud_topic_
PoseState pose_state_
ros::Publisher pub_cloud_
ros::Publisher pub_focus_
InteractiveMarkerServer server_
bool show_invalid_grasps_
ros::Timer slow_sync_timer_
ros::Timer spin_timer_
ros::Subscriber sub_point_seed_
ros::Subscriber sub_seed_
int task_number_
object_manipulator::ActionWrapper
< pr2_object_manipulation_msgs::TestGripperPoseAction
test_pose_client_
int tested_grasp_index_
bool testing_alternatives_
bool testing_current_grasp_
bool testing_planned_grasp_
tf::TransformListener tfl_

Detailed Description

An action for getting a gripper pose using interactive markers.

Definition at line 83 of file pr2_interactive_gripper_pose_action.cpp.


Constructor & Destructor Documentation

Definition at line 156 of file pr2_interactive_gripper_pose_action.cpp.

Definition at line 227 of file pr2_interactive_gripper_pose_action.cpp.


Member Function Documentation

void GripperPoseAction::acceptCB ( ) [inline, protected]

Return with the gripper pose if the pose is valid, otherwise do nothing.

Definition at line 956 of file pr2_interactive_gripper_pose_action.cpp.

void GripperPoseAction::alternativesCB ( ) [inline, protected]

Find alternatives that are valid grasps.

Definition at line 1038 of file pr2_interactive_gripper_pose_action.cpp.

void GripperPoseAction::cancelCB ( ) [inline, protected]

Cancel this action call.

Definition at line 974 of file pr2_interactive_gripper_pose_action.cpp.

void GripperPoseAction::cycleGrasps ( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &  feedback) [inline, protected]

Cycle through planned grasps and initialize the grasp(n/m) button for switching.

Definition at line 949 of file pr2_interactive_gripper_pose_action.cpp.

Definition at line 590 of file pr2_interactive_gripper_pose_action.cpp.

void GripperPoseAction::focusCB ( ) [inline, protected]

Sends a request for the 3D camera to focus on the ghosted gripper.

Definition at line 986 of file pr2_interactive_gripper_pose_action.cpp.

geometry_msgs::PoseStamped GripperPoseAction::fromWrist ( const geometry_msgs::PoseStamped &  ps) [inline]

Translate to the control pose.

Definition at line 521 of file pr2_interactive_gripper_pose_action.cpp.

std::vector<geometry_msgs::PoseStamped> GripperPoseAction::generatePosesAlongDir ( tf::Pose  pose,
tf::Vector3  axis,
double  min_shift,
double  max_shift,
double  resolution,
std::string  frame_id 
) [inline, protected]

Generate poses that are shifted along axis, by amounts from min_shift to max_shift with spacing resolution.

Definition at line 995 of file pr2_interactive_gripper_pose_action.cpp.

std::vector<geometry_msgs::PoseStamped> GripperPoseAction::generateRotatedPoses ( tf::Pose  pose,
tf::Vector3  axis,
double  min_rot,
double  max_rot,
double  resolution,
std::string  frame_id 
) [inline, protected]

Generate poses that are rotated about axis, by amounts from min_rot to max_rot with spacing resolution.

Definition at line 1016 of file pr2_interactive_gripper_pose_action.cpp.

geometry_msgs::PoseStamped GripperPoseAction::getDefaultPose ( std::string  arm_name) [inline]

Definition at line 542 of file pr2_interactive_gripper_pose_action.cpp.

bool GripperPoseAction::getModelMesh ( int  model_id,
shape_msgs::Mesh &  mesh 
) [inline, protected]

Get the mesh corresponding to a database model_id.

Definition at line 1199 of file pr2_interactive_gripper_pose_action.cpp.

void GripperPoseAction::goalCB ( ) [inline]

Callback to accept a new action goal.

Definition at line 334 of file pr2_interactive_gripper_pose_action.cpp.

void GripperPoseAction::graspPlanCB ( ) [inline, protected]

Call the grasp planner to find grasps near the seed.

Definition at line 848 of file pr2_interactive_gripper_pose_action.cpp.

void GripperPoseAction::graspPlanResultCB ( const actionlib::SimpleClientGoalState state,
const manipulation_msgs::GraspPlanningResultConstPtr &  result 
) [inline, protected]

Callback that receives the result of a TestGripperPose action.

Definition at line 894 of file pr2_interactive_gripper_pose_action.cpp.

void GripperPoseAction::gripperClickCB ( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &  feedback) [inline, protected]

Called when the gripper is clicked; each call cycles through gripper opening values.

Definition at line 1107 of file pr2_interactive_gripper_pose_action.cpp.

Definition at line 649 of file pr2_interactive_gripper_pose_action.cpp.

Definition at line 575 of file pr2_interactive_gripper_pose_action.cpp.

Definition at line 697 of file pr2_interactive_gripper_pose_action.cpp.

Definition at line 676 of file pr2_interactive_gripper_pose_action.cpp.

void GripperPoseAction::initMarkers ( ) [inline]

Re-initializes all markers.

Definition at line 568 of file pr2_interactive_gripper_pose_action.cpp.

void GripperPoseAction::initMenus ( ) [inline, protected]

Initialize the menus for all markers.

Definition at line 1186 of file pr2_interactive_gripper_pose_action.cpp.

Definition at line 667 of file pr2_interactive_gripper_pose_action.cpp.

visualization_msgs::InteractiveMarker GripperPoseAction::makeCloudMarker ( const char *  name,
const geometry_msgs::PoseStamped &  stamped,
float  point_size,
std_msgs::ColorRGBA  color 
) [inline, protected]

Create an interactive marker from a point cloud.

Definition at line 1223 of file pr2_interactive_gripper_pose_action.cpp.

void GripperPoseAction::preemptCB ( ) [inline]

Callback to allow this action to get preempted by backend.

Definition at line 498 of file pr2_interactive_gripper_pose_action.cpp.

void GripperPoseAction::selectGrasp ( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &  feedback) [inline]

Definition at line 642 of file pr2_interactive_gripper_pose_action.cpp.

void GripperPoseAction::selectGraspIndex ( int  index) [inline]

Definition at line 623 of file pr2_interactive_gripper_pose_action.cpp.

Definition at line 607 of file pr2_interactive_gripper_pose_action.cpp.

void GripperPoseAction::setIdle ( ) [inline]

Remove the markers.

Definition at line 322 of file pr2_interactive_gripper_pose_action.cpp.

void GripperPoseAction::setSeed ( const geometry_msgs::PoseStampedConstPtr &  seed) [inline]

Definition at line 267 of file pr2_interactive_gripper_pose_action.cpp.

void GripperPoseAction::setSeedPoint ( const geometry_msgs::PointStampedConstPtr &  seed_point) [inline]

Definition at line 256 of file pr2_interactive_gripper_pose_action.cpp.

void GripperPoseAction::slowSync ( ) [inline, protected]

Definition at line 717 of file pr2_interactive_gripper_pose_action.cpp.

void GripperPoseAction::spinOnce ( ) [inline, protected]

ROS spin update callback.

Definition at line 711 of file pr2_interactive_gripper_pose_action.cpp.

Callback that receives the result of a TestGripperPose action.

Definition at line 722 of file pr2_interactive_gripper_pose_action.cpp.

void GripperPoseAction::testPose ( geometry_msgs::PoseStamped  pose,
float  opening 
) [inline, protected]

Test one grasp pose.

Definition at line 1130 of file pr2_interactive_gripper_pose_action.cpp.

void GripperPoseAction::testPoses ( std::vector< geometry_msgs::PoseStamped >  poses,
float  opening 
) [inline, protected]

Test a list of grasp poses.

Definition at line 1140 of file pr2_interactive_gripper_pose_action.cpp.

geometry_msgs::PoseStamped GripperPoseAction::toWrist ( const geometry_msgs::PoseStamped &  ps) [inline]

Translate the control pose to the wrist.

Definition at line 508 of file pr2_interactive_gripper_pose_action.cpp.

bool GripperPoseAction::transformGripperPose ( const std::string  frame_id = "/base_link") [inline]

Definition at line 231 of file pr2_interactive_gripper_pose_action.cpp.

void GripperPoseAction::updateGripper ( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &  feedback) [inline, protected]

Callback for pose updates from the controls.

Definition at line 1152 of file pr2_interactive_gripper_pose_action.cpp.

Definition at line 251 of file pr2_interactive_gripper_pose_action.cpp.

Definition at line 246 of file pr2_interactive_gripper_pose_action.cpp.

void GripperPoseAction::updatePoses ( ) [inline]

Update the pose of certain markers.

Definition at line 536 of file pr2_interactive_gripper_pose_action.cpp.


Member Data Documentation

Definition at line 138 of file pr2_interactive_gripper_pose_action.cpp.

bool GripperPoseAction::active_ [protected]

Definition at line 100 of file pr2_interactive_gripper_pose_action.cpp.

Definition at line 141 of file pr2_interactive_gripper_pose_action.cpp.

Definition at line 116 of file pr2_interactive_gripper_pose_action.cpp.

Definition at line 117 of file pr2_interactive_gripper_pose_action.cpp.

Definition at line 114 of file pr2_interactive_gripper_pose_action.cpp.

Definition at line 115 of file pr2_interactive_gripper_pose_action.cpp.

Definition at line 102 of file pr2_interactive_gripper_pose_action.cpp.

Definition at line 104 of file pr2_interactive_gripper_pose_action.cpp.

geometry_msgs::PoseStamped GripperPoseAction::button_marker_pose_ [protected]

Definition at line 93 of file pr2_interactive_gripper_pose_action.cpp.

Definition at line 139 of file pr2_interactive_gripper_pose_action.cpp.

Definition at line 149 of file pr2_interactive_gripper_pose_action.cpp.

geometry_msgs::PoseStamped GripperPoseAction::control_offset_ [protected]

Definition at line 96 of file pr2_interactive_gripper_pose_action.cpp.

Definition at line 140 of file pr2_interactive_gripper_pose_action.cpp.

Definition at line 129 of file pr2_interactive_gripper_pose_action.cpp.

std::string GripperPoseAction::get_pose_name_ [protected]

Definition at line 151 of file pr2_interactive_gripper_pose_action.cpp.

Definition at line 152 of file pr2_interactive_gripper_pose_action.cpp.

object_manipulator::ActionWrapper<manipulation_msgs::GraspPlanningAction> GripperPoseAction::grasp_plan_client_ [protected]

Definition at line 148 of file pr2_interactive_gripper_pose_action.cpp.

Definition at line 118 of file pr2_interactive_gripper_pose_action.cpp.

Definition at line 119 of file pr2_interactive_gripper_pose_action.cpp.

Definition at line 120 of file pr2_interactive_gripper_pose_action.cpp.

Definition at line 98 of file pr2_interactive_gripper_pose_action.cpp.

Definition at line 97 of file pr2_interactive_gripper_pose_action.cpp.

Definition at line 105 of file pr2_interactive_gripper_pose_action.cpp.

geometry_msgs::PoseStamped GripperPoseAction::gripper_pose_ [protected]

Definition at line 95 of file pr2_interactive_gripper_pose_action.cpp.

Definition at line 107 of file pr2_interactive_gripper_pose_action.cpp.

Definition at line 145 of file pr2_interactive_gripper_pose_action.cpp.

Definition at line 137 of file pr2_interactive_gripper_pose_action.cpp.

Definition at line 125 of file pr2_interactive_gripper_pose_action.cpp.

Definition at line 99 of file pr2_interactive_gripper_pose_action.cpp.

Definition at line 101 of file pr2_interactive_gripper_pose_action.cpp.

std::vector<int> GripperPoseAction::planner_grasp_types_ [protected]

Definition at line 91 of file pr2_interactive_gripper_pose_action.cpp.

Definition at line 92 of file pr2_interactive_gripper_pose_action.cpp.

std::vector<geometry_msgs::PoseStamped> GripperPoseAction::planner_poses_ [protected]

Definition at line 89 of file pr2_interactive_gripper_pose_action.cpp.

Definition at line 90 of file pr2_interactive_gripper_pose_action.cpp.

Definition at line 126 of file pr2_interactive_gripper_pose_action.cpp.

std::string GripperPoseAction::point_cloud_topic_ [protected]

Definition at line 122 of file pr2_interactive_gripper_pose_action.cpp.

Definition at line 124 of file pr2_interactive_gripper_pose_action.cpp.

Definition at line 134 of file pr2_interactive_gripper_pose_action.cpp.

Definition at line 135 of file pr2_interactive_gripper_pose_action.cpp.

Definition at line 132 of file pr2_interactive_gripper_pose_action.cpp.

Definition at line 103 of file pr2_interactive_gripper_pose_action.cpp.

Definition at line 131 of file pr2_interactive_gripper_pose_action.cpp.

Definition at line 130 of file pr2_interactive_gripper_pose_action.cpp.

Definition at line 128 of file pr2_interactive_gripper_pose_action.cpp.

Definition at line 127 of file pr2_interactive_gripper_pose_action.cpp.

Definition at line 108 of file pr2_interactive_gripper_pose_action.cpp.

Definition at line 147 of file pr2_interactive_gripper_pose_action.cpp.

Definition at line 110 of file pr2_interactive_gripper_pose_action.cpp.

Definition at line 112 of file pr2_interactive_gripper_pose_action.cpp.

Definition at line 111 of file pr2_interactive_gripper_pose_action.cpp.

Definition at line 109 of file pr2_interactive_gripper_pose_action.cpp.

Definition at line 143 of file pr2_interactive_gripper_pose_action.cpp.


The documentation for this class was generated from the following file:


pr2_interactive_gripper_pose_action
Author(s): Adam Leeper
autogenerated on Mon Oct 6 2014 12:40:33