Classes | Public Member Functions | Protected Member Functions | Protected Attributes
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.
virtual ~PR2MarkerControl ()
 Destructor.

Protected Member Functions

void baseButtonCB (const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
void centerHeadCB ()
bool checkStateValidity (std::string arm_name)
 Checks if the arm pose is currently in collision.
void clearLocalCostmap ()
void commandGripperPose (const geometry_msgs::PoseStamped &ps, int arm_id, bool use_offset)
void dualGripperResetControlCB (const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
void dualGripperToggleControlCB (const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
void dualGripperToggleFixedCB (const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
void gripperButtonCB (const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback, std::string action)
 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 inHandObjectLeftCallback (const sensor_msgs::PointCloud2ConstPtr &cloud)
void inHandObjectRightCallback (const sensor_msgs::PointCloud2ConstPtr &cloud)
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 processBasePoseEstimate (const actionlib::SimpleClientGoalState &state, const pr2_object_manipulation_msgs::GetNavPoseResultConstPtr &result)
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 publishAlignedOdom ()
void refreshPosture (const std::string &arm_name)
void requestBasePoseEstimate ()
void requestGripperPose (const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback, int arm_id)
void requestNavGoal (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

tf::StampedTransform alignedOdom_
boost::mutex alignedOdomMutex_
boost::thread alignedOdomThread_
bool alignedOdomValid_
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_
interactive_markers::MenuHandler::EntryHandle dual_gripper_edit_control_handle_
interactive_markers::MenuHandler::EntryHandle dual_gripper_fixed_control_handle_
std::vector< tf::Transformdual_gripper_offsets_
interactive_markers::MenuHandler::EntryHandle dual_gripper_reset_control_handle_
geometry_msgs::PoseStamped dual_grippers_frame_
tf::Transform dual_pose_offset_
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_
std::string l_gripper_type_
 What type of gripper we have.
std::string manipulator_base_frame_
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_
CloudHandler object_cloud_left_
ros::Subscriber object_cloud_left_sub_
 Subscribers for in-hand object clouds.
CloudHandler object_cloud_right_
ros::Subscriber object_cloud_right_sub_
boost::mutex planner_lock_
ros::NodeHandle pnh_
std::vector< tf::Transformpose_offset_
interactive_markers::MenuHandler::EntryHandle posture_handle_
interactive_markers::MenuHandler::EntryHandle projector_handle_
std::string r_gripper_type_
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::TransformBroadcaster tfb_
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 73 of file marker_control.h.


Constructor & Destructor Documentation

Constructor.

Definition at line 121 of file marker_control.cpp.

virtual PR2MarkerControl::~PR2MarkerControl ( ) [inline, virtual]

Destructor.

Definition at line 81 of file marker_control.h.


Member Function Documentation

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

Definition at line 167 of file marker_control.h.

Cancel any base movement.

Definition at line 120 of file marker_control.h.

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

Definition at line 192 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 980 of file marker_control.cpp.

Definition at line 1915 of file marker_control.cpp.

void PR2MarkerControl::commandGripperPose ( const geometry_msgs::PoseStamped &  ps,
int  arm_id,
bool  use_offset 
) [protected]

Definition at line 1227 of file marker_control.cpp.

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

Definition at line 1171 of file marker_control.cpp.

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

Definition at line 1141 of file marker_control.cpp.

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

Definition at line 1115 of file marker_control.cpp.

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

Definition at line 225 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 373 of file marker_control.cpp.

void PR2MarkerControl::gripperButtonCB ( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &  feedback,
std::string  action 
) [protected]

Turns the gripper controls on and off.

Turns the gripper controls on and off or toggles them.

Definition at line 1410 of file marker_control.cpp.

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

Definition at line 1596 of file marker_control.cpp.

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

Definition at line 1155 of file marker_control.cpp.

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

Definition at line 1127 of file marker_control.cpp.

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

Definition at line 1101 of file marker_control.cpp.

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

Definition at line 1077 of file marker_control.cpp.

void PR2MarkerControl::inHandObjectLeftCallback ( const sensor_msgs::PointCloud2ConstPtr &  cloud) [protected]

Definition at line 1927 of file marker_control.cpp.

void PR2MarkerControl::inHandObjectRightCallback ( const sensor_msgs::PointCloud2ConstPtr &  cloud) [protected]

Definition at line 1921 of file marker_control.cpp.

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

Re-initialize all interactive markers.

Definition at line 101 of file marker_control.h.

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

Definition at line 388 of file marker_control.cpp.

void PR2MarkerControl::initMenus ( ) [protected]

Definition at line 1933 of file marker_control.cpp.

Re-initialize only the mesh markers.

Definition at line 704 of file marker_control.cpp.

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

Definition at line 1668 of file marker_control.cpp.

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

Definition at line 1686 of file marker_control.cpp.

Definition at line 1874 of file marker_control.cpp.

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

Definition at line 1648 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 1658 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 1850 of file marker_control.cpp.

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

Definition at line 1045 of file marker_control.cpp.

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

Definition at line 402 of file marker_control.h.

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

Definition at line 173 of file marker_control.h.

Definition at line 1866 of file marker_control.cpp.

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

Definition at line 1615 of file marker_control.cpp.

void PR2MarkerControl::requestNavGoal ( const bool collision_aware) [protected]

Definition at line 1796 of file marker_control.cpp.

void PR2MarkerControl::sendLastNavGoal ( ) [protected]

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

Definition at line 1898 of file marker_control.cpp.

Used for lower frequency state updates.

Definition at line 274 of file marker_control.cpp.

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

Definition at line 140 of file marker_control.h.

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

Definition at line 1179 of file marker_control.cpp.

Activates the cartesian controllers for the grippers.

Definition at line 918 of file marker_control.cpp.

void PR2MarkerControl::switchToJoint ( ) [protected]

Activates the joint controllers for the grippers.

Definition at line 953 of file marker_control.cpp.

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

Definition at line 1032 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 1585 of file marker_control.cpp.

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

Definition at line 180 of file marker_control.h.

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

Definition at line 1543 of file marker_control.cpp.

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

Definition at line 1313 of file marker_control.cpp.

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

Definition at line 1261 of file marker_control.cpp.

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

Definition at line 1006 of file marker_control.cpp.

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

Definition at line 1466 of file marker_control.cpp.

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

Definition at line 1518 of file marker_control.cpp.

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

Definition at line 1441 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 396 of file marker_control.h.

boost::mutex PR2MarkerControl::alignedOdomMutex_ [protected]

Definition at line 395 of file marker_control.h.

boost::thread PR2MarkerControl::alignedOdomThread_ [protected]

Definition at line 394 of file marker_control.h.

Definition at line 393 of file marker_control.h.

Definition at line 341 of file marker_control.h.

geometry_msgs::PoseStamped PR2MarkerControl::base_goal_pose_ [protected]

Definition at line 300 of file marker_control.h.

Client for getting a base pose.

Definition at line 374 of file marker_control.h.

Definition at line 354 of file marker_control.h.

Definition at line 353 of file marker_control.h.

Client for service that checks state validity.

Definition at line 363 of file marker_control.h.

Definition at line 364 of file marker_control.h.

Definition at line 338 of file marker_control.h.

Definition at line 283 of file marker_control.h.

Definition at line 281 of file marker_control.h.

Definition at line 296 of file marker_control.h.

Definition at line 285 of file marker_control.h.

geometry_msgs::PoseStamped PR2MarkerControl::dual_grippers_frame_ [protected]

Definition at line 295 of file marker_control.h.

Definition at line 291 of file marker_control.h.

Definition at line 279 of file marker_control.h.

Definition at line 339 of file marker_control.h.

Definition at line 351 of file marker_control.h.

Definition at line 351 of file marker_control.h.

Definition at line 282 of file marker_control.h.

Definition at line 280 of file marker_control.h.

Client for getting a gripper pose.

Definition at line 377 of file marker_control.h.

Definition at line 284 of file marker_control.h.

Definition at line 278 of file marker_control.h.

geometry_msgs::PoseStamped PR2MarkerControl::head_goal_pose_ [protected]

Definition at line 293 of file marker_control.h.

std::string PR2MarkerControl::head_pointing_frame_ [protected]

Definition at line 384 of file marker_control.h.

Definition at line 277 of file marker_control.h.

Definition at line 358 of file marker_control.h.

Definition at line 358 of file marker_control.h.

For HRI user study.

Definition at line 380 of file marker_control.h.

Definition at line 273 of file marker_control.h.

Definition at line 274 of file marker_control.h.

std::string PR2MarkerControl::l_gripper_type_ [protected]

What type of gripper we have.

For now, "pr2" or "lcg"

Definition at line 400 of file marker_control.h.

Definition at line 298 of file marker_control.h.

Definition at line 356 of file marker_control.h.

Definition at line 346 of file marker_control.h.

Menu handles for the robot.

Definition at line 262 of file marker_control.h.

Definition at line 269 of file marker_control.h.

Definition at line 267 of file marker_control.h.

Definition at line 268 of file marker_control.h.

Definition at line 266 of file marker_control.h.

Definition at line 263 of file marker_control.h.

Definition at line 265 of file marker_control.h.

Definition at line 264 of file marker_control.h.

std::string PR2MarkerControl::move_base_node_name_ [protected]

Name of the move base node.

Definition at line 387 of file marker_control.h.

Definition at line 254 of file marker_control.h.

Definition at line 366 of file marker_control.h.

Subscribers for in-hand object clouds.

Definition at line 370 of file marker_control.h.

Definition at line 367 of file marker_control.h.

Definition at line 371 of file marker_control.h.

boost::mutex PR2MarkerControl::planner_lock_ [protected]

Definition at line 344 of file marker_control.h.

Definition at line 255 of file marker_control.h.

Definition at line 290 of file marker_control.h.

Definition at line 275 of file marker_control.h.

Definition at line 276 of file marker_control.h.

std::string PR2MarkerControl::r_gripper_type_ [protected]

Definition at line 400 of file marker_control.h.

Definition at line 258 of file marker_control.h.

Definition at line 257 of file marker_control.h.

Definition at line 365 of file marker_control.h.

Definition at line 256 of file marker_control.h.

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

Definition at line 360 of file marker_control.h.

For HRI user study.

Definition at line 382 of file marker_control.h.

Definition at line 288 of file marker_control.h.

Definition at line 287 of file marker_control.h.

Definition at line 292 of file marker_control.h.

Definition at line 340 of file marker_control.h.

Definition at line 342 of file marker_control.h.

Menu Entry handles for some important ones.

Definition at line 272 of file marker_control.h.

Definition at line 352 of file marker_control.h.

Definition at line 350 of file marker_control.h.

Definition at line 349 of file marker_control.h.

Definition at line 348 of file marker_control.h.

Are we using 3D navigation?

Definition at line 390 of file marker_control.h.


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


pr2_marker_control
Author(s): Adam Leeper
autogenerated on Fri Jan 3 2014 12:07:28