$search

PR2MarkerControl Class Reference

A class for controlling the PR2 using interactive markers. More...

#include <marker_control.h>

List of all members.

Classes

struct  ControlState
 A struct to keep the state of the overall robot control. More...
struct  GripperState
 A struct to store the state of the gripper controls. More...

Public Member Functions

void cancelBaseMovement ()
 Cancel any base movement.
void fastUpdate ()
 For fast updates, like changes to control and mesh marker poses.
double getJointPosition (std::string name, const arm_navigation_msgs::RobotState &robot_state)
 Finds the state of a given joint in the robot state.
void initAllMarkers (bool apply_immediately=false)
 Re-initialize all interactive markers.
void initControlMarkers ()
 Re-initialize only the control markers (like the gripper-dragging frame).
void initMeshMarkers ()
 Re-initialize only the mesh markers.
 PR2MarkerControl ()
 Constructor.
void slowUpdate ()
 Used for lower frequency state updates.
geometry_msgs::PoseStamped toolToWrist (const geometry_msgs::PoseStamped &ps)
 Translate the tool pose to the wrist.
geometry_msgs::PoseStamped wristToTool (const geometry_msgs::PoseStamped &ps)
 Translate the wrist pose to the tool frame.
 ~PR2MarkerControl ()
 Destructor.

Protected Member Functions

void baseButtonCB (const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
bool checkStateValidity (std::string arm_name)
 Checks if the arm pose is currently in collision.
void clearLocalCostmap (const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
void commandGripperPose (const geometry_msgs::PoseStamped &ps, int arm_id, bool use_offset)
void gripperButtonCB (const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback, int id)
 Turns the gripper controls on and off.
void gripperClosureCB (const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback, const float &command)
void gripperResetControlCB (const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
void gripperToggleControlCB (const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
void gripperToggleFixedCB (const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
void gripperToggleModeCB (const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
void initMenus ()
void moveArm (const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback, const std::string &position, bool planner)
void moveArmThread (std::string arm_name, std::string position, bool collision, bool planner)
void processGripperPoseFeedback (const pr2_object_manipulation_msgs::GetGripperPoseFeedbackConstPtr &result, const std::string &arm_name)
void processGripperPoseResult (const actionlib::SimpleClientGoalState &state, const pr2_object_manipulation_msgs::GetGripperPoseResultConstPtr &result, const std::string &arm_name)
void processNavGoal (const actionlib::SimpleClientGoalState &state, const pr2_object_manipulation_msgs::GetNavPoseResultConstPtr &result, const bool &collision_aware)
void projectorMenuCB (const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
void refreshPosture (const std::string &arm_name)
void requestGripperPose (const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback, int arm_id)
void requestNavGoal (const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback, const bool &collision_aware)
void sendLastNavGoal ()
 This is useful so we can bind it to a callback.
void snapshotCB ()
void startDualGrippers (const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback, bool active)
void switchToCartesian ()
 Activates the cartesian controllers for the grippers.
void switchToJoint ()
 Activates the joint controllers for the grippers.
void targetPointMenuCB (const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
void torsoMenuCB (const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
void tuckArmsCB (bool tuck_left, bool tuck_right)
void updateBase (const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
void updateDualGripper (const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
void updateGripper (const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback, int arm_id)
void updateHeadGoal (const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback, int arm_id)
void updatePosture (const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback, int arm_id)
void updateTorso (const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
void upperArmButtonCB (const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback, int id)

Protected Attributes

pr2_wrappers::BaseClient base_client_
geometry_msgs::PoseStamped base_goal_pose_
object_manipulator::ActionWrapper
< pr2_object_manipulation_msgs::GetNavPoseAction
base_pose_client_
 Client for getting a base pose.
double cartesian_clip_angle_
double cartesian_clip_distance_
object_manipulator::ServiceWrapper
< arm_navigation_msgs::GetStateValidity
check_state_validity_client_
 Client for service that checks state validity.
object_manipulator::ServiceWrapper
< std_srvs::Empty > 
collider_node_reset_srv_
ControlState control_state_
bool double_menu_
std::vector< tf::Transformdual_gripper_offsets_
geometry_msgs::PoseStamped dual_grippers_frame_
interactive_markers::MenuHandler::EntryHandle gripper_6dof_handle_
pr2_wrappers::GripperController gripper_client_
double gripper_control_angular_deadband_
double gripper_control_linear_deadband_
interactive_markers::MenuHandler::EntryHandle gripper_edit_control_handle_
interactive_markers::MenuHandler::EntryHandle gripper_fixed_control_handle_
object_manipulator::ActionWrapper
< pr2_object_manipulation_msgs::GetGripperPoseAction
gripper_pose_client_
 Client for getting a gripper pose.
interactive_markers::MenuHandler::EntryHandle gripper_reset_control_handle_
interactive_markers::MenuHandler::EntryHandle gripper_view_facing_handle_
geometry_msgs::PoseStamped head_goal_pose_
std::string head_pointing_frame_
interactive_markers::MenuHandler::EntryHandle head_target_handle_
bool in_collision_l_
bool in_collision_r_
int interface_number_
 For HRI user study.
interactive_markers::MenuHandler::EntryHandle joint_handle_
interactive_markers::MenuHandler::EntryHandle jtranspose_handle_
double max_direct_nav_radius_
object_manipulator::MechanismInterface mechanism_
interactive_markers::MenuHandler menu_arms_
 Menu handles for the robot.
interactive_markers::MenuHandler menu_base_
interactive_markers::MenuHandler menu_dual_grippers_
interactive_markers::MenuHandler menu_gripper_close_
interactive_markers::MenuHandler menu_grippers_
interactive_markers::MenuHandler menu_head_
interactive_markers::MenuHandler menu_laser_
interactive_markers::MenuHandler menu_torso_
std::string move_base_node_name_
 Name of the move base node.
ros::NodeHandle nh_
ros::NodeHandle pnh_
std::vector< tf::Transformpose_offset_
interactive_markers::MenuHandler::EntryHandle posture_handle_
interactive_markers::MenuHandler::EntryHandle projector_handle_
interactive_markers::InteractiveMarkerServer server_
ros::Timer slow_sync_timer_
CloudHandler snapshot_client_
ros::Timer spin_timer_
boost::shared_ptr< boost::thread > sys_thread_
int task_number_
 For HRI user study.
tf::TransformListener tfl_
geometry_msgs::Pose tool_frame_offset_
pr2_wrappers::TorsoClient torso_client_
pr2_wrappers::TuckArmsClient tuck_arms_client_
interactive_markers::MenuHandler::EntryHandle tuck_handle_
 Menu Entry handles for some important ones.
double update_period_
bool use_left_arm_
bool use_right_arm_
bool use_state_validator_
bool using_3d_nav_
 Are we using 3D navigation?

Detailed Description

A class for controlling the PR2 using interactive markers.

Definition at line 71 of file marker_control.h.


Constructor & Destructor Documentation

PR2MarkerControl::PR2MarkerControl (  ) 

Constructor.

Definition at line 104 of file marker_control.cpp.

PR2MarkerControl::~PR2MarkerControl (  )  [inline]

Destructor.

Definition at line 79 of file marker_control.h.


Member Function Documentation

void PR2MarkerControl::baseButtonCB ( const visualization_msgs::InteractiveMarkerFeedbackConstPtr feedback  )  [inline, protected]

Definition at line 159 of file marker_control.h.

void PR2MarkerControl::cancelBaseMovement (  )  [inline]

Cancel any base movement.

Definition at line 118 of file marker_control.h.

bool PR2MarkerControl::checkStateValidity ( std::string  arm_name  )  [protected]

Checks if the arm pose is currently in collision.

Definition at line 763 of file marker_control.cpp.

void PR2MarkerControl::clearLocalCostmap ( const visualization_msgs::InteractiveMarkerFeedbackConstPtr feedback  )  [protected]
void PR2MarkerControl::commandGripperPose ( const geometry_msgs::PoseStamped ps,
int  arm_id,
bool  use_offset 
) [protected]

Definition at line 964 of file marker_control.cpp.

void PR2MarkerControl::fastUpdate (  ) 

For fast updates, like changes to control and mesh marker poses.

Definition at line 180 of file marker_control.cpp.

double PR2MarkerControl::getJointPosition ( std::string  name,
const arm_navigation_msgs::RobotState robot_state 
)

Finds the state of a given joint in the robot state.

Definition at line 328 of file marker_control.cpp.

void PR2MarkerControl::gripperButtonCB ( const visualization_msgs::InteractiveMarkerFeedbackConstPtr feedback,
int  id 
) [protected]

Turns the gripper controls on and off.

Definition at line 1109 of file marker_control.cpp.

void PR2MarkerControl::gripperClosureCB ( const visualization_msgs::InteractiveMarkerFeedbackConstPtr feedback,
const float &  command 
) [protected]

Definition at line 1182 of file marker_control.cpp.

void PR2MarkerControl::gripperResetControlCB ( const visualization_msgs::InteractiveMarkerFeedbackConstPtr feedback  )  [protected]

Definition at line 912 of file marker_control.cpp.

void PR2MarkerControl::gripperToggleControlCB ( const visualization_msgs::InteractiveMarkerFeedbackConstPtr feedback  )  [protected]

Definition at line 898 of file marker_control.cpp.

void PR2MarkerControl::gripperToggleFixedCB ( const visualization_msgs::InteractiveMarkerFeedbackConstPtr feedback  )  [protected]

Definition at line 884 of file marker_control.cpp.

void PR2MarkerControl::gripperToggleModeCB ( const visualization_msgs::InteractiveMarkerFeedbackConstPtr feedback  )  [protected]

Definition at line 860 of file marker_control.cpp.

void PR2MarkerControl::initAllMarkers ( bool  apply_immediately = false  )  [inline]

Re-initialize all interactive markers.

Definition at line 99 of file marker_control.h.

void PR2MarkerControl::initControlMarkers (  ) 

Re-initialize only the control markers (like the gripper-dragging frame).

Definition at line 343 of file marker_control.cpp.

void PR2MarkerControl::initMenus (  )  [protected]

Definition at line 1512 of file marker_control.cpp.

void PR2MarkerControl::initMeshMarkers (  ) 

Re-initialize only the mesh markers.

Definition at line 602 of file marker_control.cpp.

void PR2MarkerControl::moveArm ( const visualization_msgs::InteractiveMarkerFeedbackConstPtr feedback,
const std::string &  position,
bool  planner 
) [protected]

Definition at line 1258 of file marker_control.cpp.

void PR2MarkerControl::moveArmThread ( std::string  arm_name,
std::string  position,
bool  collision,
bool  planner 
) [protected]

Definition at line 1275 of file marker_control.cpp.

void PR2MarkerControl::processGripperPoseFeedback ( const pr2_object_manipulation_msgs::GetGripperPoseFeedbackConstPtr result,
const std::string &  arm_name 
) [protected]

Definition at line 1238 of file marker_control.cpp.

void PR2MarkerControl::processGripperPoseResult ( const actionlib::SimpleClientGoalState state,
const pr2_object_manipulation_msgs::GetGripperPoseResultConstPtr result,
const std::string &  arm_name 
) [protected]

Definition at line 1248 of file marker_control.cpp.

void PR2MarkerControl::processNavGoal ( const actionlib::SimpleClientGoalState state,
const pr2_object_manipulation_msgs::GetNavPoseResultConstPtr result,
const bool &  collision_aware 
) [protected]

Definition at line 1475 of file marker_control.cpp.

void PR2MarkerControl::projectorMenuCB ( const visualization_msgs::InteractiveMarkerFeedbackConstPtr feedback  )  [protected]

Definition at line 828 of file marker_control.cpp.

void PR2MarkerControl::refreshPosture ( const std::string &  arm_name  )  [inline, protected]

Definition at line 165 of file marker_control.h.

void PR2MarkerControl::requestGripperPose ( const visualization_msgs::InteractiveMarkerFeedbackConstPtr feedback,
int  arm_id 
) [protected]

Definition at line 1205 of file marker_control.cpp.

void PR2MarkerControl::requestNavGoal ( const visualization_msgs::InteractiveMarkerFeedbackConstPtr feedback,
const bool &  collision_aware 
) [protected]

Definition at line 1420 of file marker_control.cpp.

void PR2MarkerControl::sendLastNavGoal (  )  [protected]

This is useful so we can bind it to a callback.

Definition at line 1491 of file marker_control.cpp.

void PR2MarkerControl::slowUpdate (  ) 

Used for lower frequency state updates.

Definition at line 229 of file marker_control.cpp.

void PR2MarkerControl::snapshotCB (  )  [inline, protected]

Definition at line 138 of file marker_control.h.

void PR2MarkerControl::startDualGrippers ( const visualization_msgs::InteractiveMarkerFeedbackConstPtr feedback,
bool  active 
) [protected]

Definition at line 928 of file marker_control.cpp.

void PR2MarkerControl::switchToCartesian (  )  [protected]

Activates the cartesian controllers for the grippers.

Definition at line 701 of file marker_control.cpp.

void PR2MarkerControl::switchToJoint (  )  [protected]

Activates the joint controllers for the grippers.

Definition at line 736 of file marker_control.cpp.

void PR2MarkerControl::targetPointMenuCB ( const visualization_msgs::InteractiveMarkerFeedbackConstPtr feedback  )  [protected]

Definition at line 815 of file marker_control.cpp.

geometry_msgs::PoseStamped PR2MarkerControl::toolToWrist ( const geometry_msgs::PoseStamped ps  ) 

Translate the tool pose to the wrist.

Translate the control pose to the wrist.

Definition at line 76 of file marker_control.cpp.

void PR2MarkerControl::torsoMenuCB ( const visualization_msgs::InteractiveMarkerFeedbackConstPtr feedback  )  [protected]

Definition at line 1368 of file marker_control.cpp.

void PR2MarkerControl::tuckArmsCB ( bool  tuck_left,
bool  tuck_right 
) [inline, protected]

Definition at line 172 of file marker_control.h.

void PR2MarkerControl::updateBase ( const visualization_msgs::InteractiveMarkerFeedbackConstPtr feedback  )  [protected]

Definition at line 1378 of file marker_control.cpp.

void PR2MarkerControl::updateDualGripper ( const visualization_msgs::InteractiveMarkerFeedbackConstPtr feedback  )  [protected]

Definition at line 1035 of file marker_control.cpp.

void PR2MarkerControl::updateGripper ( const visualization_msgs::InteractiveMarkerFeedbackConstPtr feedback,
int  arm_id 
) [protected]

Definition at line 992 of file marker_control.cpp.

void PR2MarkerControl::updateHeadGoal ( const visualization_msgs::InteractiveMarkerFeedbackConstPtr feedback,
int  arm_id 
) [protected]

Definition at line 789 of file marker_control.cpp.

void PR2MarkerControl::updatePosture ( const visualization_msgs::InteractiveMarkerFeedbackConstPtr feedback,
int  arm_id 
) [protected]

Definition at line 1151 of file marker_control.cpp.

void PR2MarkerControl::updateTorso ( const visualization_msgs::InteractiveMarkerFeedbackConstPtr feedback  )  [protected]

Definition at line 1344 of file marker_control.cpp.

void PR2MarkerControl::upperArmButtonCB ( const visualization_msgs::InteractiveMarkerFeedbackConstPtr feedback,
int  id 
) [protected]

Definition at line 1130 of file marker_control.cpp.

geometry_msgs::PoseStamped PR2MarkerControl::wristToTool ( const geometry_msgs::PoseStamped ps  ) 

Translate the wrist pose to the tool frame.

Translate to the control pose.

Definition at line 89 of file marker_control.cpp.


Member Data Documentation

Definition at line 309 of file marker_control.h.

Definition at line 269 of file marker_control.h.

Client for getting a base pose.

Definition at line 334 of file marker_control.h.

Definition at line 320 of file marker_control.h.

Definition at line 319 of file marker_control.h.

Client for service that checks state validity.

Definition at line 329 of file marker_control.h.

Definition at line 330 of file marker_control.h.

Definition at line 306 of file marker_control.h.

Definition at line 345 of file marker_control.h.

Definition at line 267 of file marker_control.h.

Definition at line 266 of file marker_control.h.

Definition at line 255 of file marker_control.h.

Definition at line 307 of file marker_control.h.

Definition at line 317 of file marker_control.h.

Definition at line 317 of file marker_control.h.

Definition at line 257 of file marker_control.h.

Definition at line 256 of file marker_control.h.

Client for getting a gripper pose.

Definition at line 337 of file marker_control.h.

Definition at line 258 of file marker_control.h.

Definition at line 254 of file marker_control.h.

Definition at line 264 of file marker_control.h.

std::string PR2MarkerControl::head_pointing_frame_ [protected]

Definition at line 344 of file marker_control.h.

Definition at line 253 of file marker_control.h.

Definition at line 324 of file marker_control.h.

Definition at line 324 of file marker_control.h.

For HRI user study.

Definition at line 340 of file marker_control.h.

Definition at line 249 of file marker_control.h.

Definition at line 250 of file marker_control.h.

Definition at line 322 of file marker_control.h.

Definition at line 312 of file marker_control.h.

Menu handles for the robot.

Definition at line 238 of file marker_control.h.

Definition at line 245 of file marker_control.h.

Definition at line 243 of file marker_control.h.

Definition at line 244 of file marker_control.h.

Definition at line 242 of file marker_control.h.

Definition at line 239 of file marker_control.h.

Definition at line 241 of file marker_control.h.

Definition at line 240 of file marker_control.h.

std::string PR2MarkerControl::move_base_node_name_ [protected]

Name of the move base node.

Definition at line 348 of file marker_control.h.

Definition at line 230 of file marker_control.h.

Definition at line 231 of file marker_control.h.

Definition at line 262 of file marker_control.h.

Definition at line 251 of file marker_control.h.

Definition at line 252 of file marker_control.h.

Definition at line 234 of file marker_control.h.

Definition at line 233 of file marker_control.h.

Definition at line 331 of file marker_control.h.

Definition at line 232 of file marker_control.h.

boost::shared_ptr< boost::thread > PR2MarkerControl::sys_thread_ [protected]

Definition at line 326 of file marker_control.h.

For HRI user study.

Definition at line 342 of file marker_control.h.

Definition at line 260 of file marker_control.h.

Definition at line 263 of file marker_control.h.

Definition at line 308 of file marker_control.h.

Definition at line 310 of file marker_control.h.

Menu Entry handles for some important ones.

Definition at line 248 of file marker_control.h.

Definition at line 318 of file marker_control.h.

Definition at line 316 of file marker_control.h.

Definition at line 315 of file marker_control.h.

Definition at line 314 of file marker_control.h.

Are we using 3D navigation?

Definition at line 351 of file marker_control.h.


The documentation for this class was generated from the following files:
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


pr2_marker_control
Author(s): Adam Leeper
autogenerated on Tue Mar 5 14:40:33 2013