Public Types | Public Member Functions | Private Member Functions | Private Attributes | List of all members
GeneralCommander Class Reference

#include <pr2_teleop_general_commander.h>

Public Types

enum  ArmControlMode { ARM_NO_CONTROLLER, ARM_MANNEQUIN_MODE, ARM_POSITION_CONTROL }
 
enum  HeadControlMode { HEAD_JOYSTICK, HEAD_TRACK_LEFT_HAND, HEAD_TRACK_RIGHT_HAND, HEAD_MANNEQUIN }
 
enum  HeadSequence { HEAD_NOD, HEAD_SHAKE }
 
enum  LaserControlMode { LASER_TILT_OFF, LASER_TILT_SLOW, LASER_TILT_FAST }
 
enum  WhichArm { ARMS_LEFT, ARMS_RIGHT, ARMS_BOTH }
 

Public Member Functions

 GeneralCommander (bool control_body, bool control_head, bool control_rarm, bool control_larm, bool control_prosilica, std::string arm_controller_name=default_arm_controller_name)
 
ArmControlMode getArmMode (WhichArm which)
 
HeadControlMode getHeadMode ()
 
bool getJointPosition (const std::string &name, double &pos) const
 
bool getJointVelocity (const std::string &name, double &vel) const
 
LaserControlMode getLaserMode ()
 
bool initWalkAlong ()
 
bool isWalkAlongOk ()
 
bool moveToWalkAlongArmPose ()
 
void requestProsilicaImage (std::string ns)
 
void sendArmVelCommands (double r_x_vel, double r_y_vel, double r_z_vel, double r_roll_vel, double r_pitch_vel, double r_yaw_vel, double l_x_vel, double l_y_vel, double l_z_vel, double l_roll_vel, double l_pitch_vel, double l_yaw_vel, double hz)
 
void sendBaseCommand (double vx, double vy, double vw)
 
void sendGripperCommand (WhichArm which, bool close)
 
void sendHeadCommand (double req_pan, double req_tilt)
 
void sendHeadSequence (HeadSequence seq)
 
void sendHeadTrackCommand ()
 
void sendProjectorStartStop (bool start)
 
void sendTorsoCommand (double pos, double vel)
 
void sendWalkAlongCommand (double thresh, double x_dist_max, double x_speed_scale, double y_dist_max, double y_speed_scale, double rot_scale)
 
void sendWristVelCommands (double right_wrist_vel, double left_wrist_vel, double hz)
 
void setArmMode (WhichArm which, ArmControlMode mode)
 
void setHeadMode (HeadControlMode mode)
 
void setLaserMode (LaserControlMode mode)
 
void switchControllers (const std::vector< std::string > &start_controllers, const std::vector< std::string > &stop_controllers)
 
void tuckArms (WhichArm arm)
 
void turnOffWalkAlong ()
 
void untuckArms (WhichArm arm)
 
void updateCurrentWristPositions ()
 
 ~GeneralCommander ()
 

Private Member Functions

double calcAverage (const std::list< double > &av_list) const
 
void clampDesiredArmPositionsToActual (double max_dist)
 
void composeWristRotGoal (const std::string pref, pr2_controllers_msgs::JointTrajectoryGoal &goal, std::vector< double > &des_joints, double des_vel, double hz) const
 
geometry_msgs::Pose getPositionFromJointsPose (ros::ServiceClient &service_client, std::string fk_link, const std::vector< std::string > &joint_names, const std::vector< double > &joint_pos)
 
void jointStateCallback (const sensor_msgs::JointStateConstPtr &jointState)
 
void powerBoardCallback (const pr2_msgs::PowerBoardStateConstPtr &powerBoardState)
 
void unnormalizeTrajectory (trajectory_msgs::JointTrajectory &traj) const
 
void updateWalkAlongAverages ()
 

Private Attributes

ros::Publisher base_pub_
 
bool control_body_
 
bool control_head_
 
bool control_larm_
 
bool control_prosilica_
 
bool control_rarm_
 
geometry_msgs::Pose des_l_wrist_roll_pose_
 
geometry_msgs::Pose des_r_wrist_roll_pose_
 
HeadControlMode head_control_mode_
 
trajectory_msgs::JointTrajectory head_nod_traj_
 
ros::Publisher head_pub_
 
trajectory_msgs::JointTrajectory head_shake_traj_
 
actionlib::SimpleActionClient< pr2_controllers_msgs::PointHeadAction > * head_track_hand_client_
 
std::map< std::string, double > joint_state_position_map_
 
ros::Subscriber joint_state_sub_
 
std::map< std::string, double > joint_state_velocity_map_
 
std::string l_arm_controller_name_
 
LaserControlMode laser_control_mode_
 
double laser_fast_amplitude_
 
double laser_fast_offset_
 
double laser_fast_period_
 
double laser_slow_amplitude_
 
double laser_slow_offset_
 
double laser_slow_period_
 
ros::Time last_left_wrist_goal_stamp_
 
ros::Time last_right_wrist_goal_stamp_
 
double last_torso_vel_
 
ArmControlMode left_arm_control_mode_
 
ros::ServiceClient left_arm_kinematics_forward_client_
 
ros::ServiceClient left_arm_kinematics_inverse_client_
 
ros::Publisher left_arm_traj_pub_
 
actionlib::SimpleActionClient< pr2_controllers_msgs::JointTrajectoryAction > * left_arm_trajectory_client_
 
std::vector< double > left_des_joint_states_
 
actionlib::SimpleActionClient< pr2_controllers_msgs::Pr2GripperCommandAction > * left_gripper_client_
 
std::vector< double > left_walk_along_pose_
 
geometry_msgs::Pose left_wrist_roll_pose_
 
ros::NodeHandle n_
 
ros::Subscriber power_board_sub_
 
ros::ServiceClient prosilica_polling_client_
 
std::string r_arm_controller_name_
 
ArmControlMode right_arm_control_mode_
 
ros::ServiceClient right_arm_kinematics_forward_client_
 
ros::ServiceClient right_arm_kinematics_inverse_client_
 
ros::Publisher right_arm_traj_pub_
 
actionlib::SimpleActionClient< pr2_controllers_msgs::JointTrajectoryAction > * right_arm_trajectory_client_
 
std::vector< double > right_des_joint_states_
 
actionlib::SimpleActionClient< pr2_controllers_msgs::Pr2GripperCommandAction > * right_gripper_client_
 
std::vector< double > right_walk_along_pose_
 
geometry_msgs::Pose right_wrist_roll_pose_
 
urdf::Model robot_model_
 A model of the robot to see which joints wrap around. More...
 
bool robot_model_initialized_
 Flag that tells us if the robot model was initialized successfully. More...
 
bool status_projector_on
 
ros::ServiceClient switch_controllers_service_
 
ros::ServiceClient tilt_laser_service_
 
ros::Publisher torso_pub_
 
actionlib::SimpleActionClient< pr2_common_action_msgs::TuckArmsAction > * tuck_arms_client_
 
geometry_msgs::Pose walk_along_left_des_pose_
 
bool walk_along_ok_
 
geometry_msgs::Pose walk_along_right_des_pose_
 
std::list< double > walk_ldx_vals_
 
std::list< double > walk_ldy_vals_
 
std::list< double > walk_rdx_vals_
 
std::list< double > walk_rdy_vals_
 

Detailed Description

Definition at line 54 of file pr2_teleop_general_commander.h.

Member Enumeration Documentation

Enumerator
ARM_NO_CONTROLLER 
ARM_MANNEQUIN_MODE 
ARM_POSITION_CONTROL 

Definition at line 64 of file pr2_teleop_general_commander.h.

Enumerator
HEAD_JOYSTICK 
HEAD_TRACK_LEFT_HAND 
HEAD_TRACK_RIGHT_HAND 
HEAD_MANNEQUIN 

Definition at line 70 of file pr2_teleop_general_commander.h.

Enumerator
HEAD_NOD 
HEAD_SHAKE 

Definition at line 83 of file pr2_teleop_general_commander.h.

Enumerator
LASER_TILT_OFF 
LASER_TILT_SLOW 
LASER_TILT_FAST 

Definition at line 77 of file pr2_teleop_general_commander.h.

Enumerator
ARMS_LEFT 
ARMS_RIGHT 
ARMS_BOTH 

Definition at line 58 of file pr2_teleop_general_commander.h.

Constructor & Destructor Documentation

GeneralCommander::GeneralCommander ( bool  control_body,
bool  control_head,
bool  control_rarm,
bool  control_larm,
bool  control_prosilica,
std::string  arm_controller_name = default_arm_controller_name 
)

Definition at line 68 of file pr2_teleop_general_commander.cpp.

GeneralCommander::~GeneralCommander ( )

Definition at line 262 of file pr2_teleop_general_commander.cpp.

Member Function Documentation

double GeneralCommander::calcAverage ( const std::list< double > &  av_list) const
private

Definition at line 1480 of file pr2_teleop_general_commander.cpp.

void GeneralCommander::clampDesiredArmPositionsToActual ( double  max_dist)
private

Definition at line 770 of file pr2_teleop_general_commander.cpp.

void GeneralCommander::composeWristRotGoal ( const std::string  pref,
pr2_controllers_msgs::JointTrajectoryGoal &  goal,
std::vector< double > &  des_joints,
double  des_vel,
double  hz 
) const
private

Definition at line 660 of file pr2_teleop_general_commander.cpp.

GeneralCommander::ArmControlMode GeneralCommander::getArmMode ( WhichArm  which)

Definition at line 319 of file pr2_teleop_general_commander.cpp.

GeneralCommander::HeadControlMode GeneralCommander::getHeadMode ( )

Definition at line 315 of file pr2_teleop_general_commander.cpp.

bool GeneralCommander::getJointPosition ( const std::string &  name,
double &  pos 
) const

Definition at line 295 of file pr2_teleop_general_commander.cpp.

bool GeneralCommander::getJointVelocity ( const std::string &  name,
double &  vel 
) const

Definition at line 303 of file pr2_teleop_general_commander.cpp.

GeneralCommander::LaserControlMode GeneralCommander::getLaserMode ( )

Definition at line 311 of file pr2_teleop_general_commander.cpp.

geometry_msgs::Pose GeneralCommander::getPositionFromJointsPose ( ros::ServiceClient service_client,
std::string  fk_link,
const std::vector< std::string > &  joint_names,
const std::vector< double > &  joint_pos 
)
private

Definition at line 1491 of file pr2_teleop_general_commander.cpp.

bool GeneralCommander::initWalkAlong ( )

Definition at line 1299 of file pr2_teleop_general_commander.cpp.

bool GeneralCommander::isWalkAlongOk ( )
inline

Definition at line 153 of file pr2_teleop_general_commander.h.

void GeneralCommander::jointStateCallback ( const sensor_msgs::JointStateConstPtr &  jointState)
private

Definition at line 284 of file pr2_teleop_general_commander.cpp.

bool GeneralCommander::moveToWalkAlongArmPose ( )

Definition at line 1151 of file pr2_teleop_general_commander.cpp.

void GeneralCommander::powerBoardCallback ( const pr2_msgs::PowerBoardStateConstPtr &  powerBoardState)
private

Definition at line 1533 of file pr2_teleop_general_commander.cpp.

void GeneralCommander::requestProsilicaImage ( std::string  ns)

Definition at line 1542 of file pr2_teleop_general_commander.cpp.

void GeneralCommander::sendArmVelCommands ( double  r_x_vel,
double  r_y_vel,
double  r_z_vel,
double  r_roll_vel,
double  r_pitch_vel,
double  r_yaw_vel,
double  l_x_vel,
double  l_y_vel,
double  l_z_vel,
double  l_roll_vel,
double  l_pitch_vel,
double  l_yaw_vel,
double  hz 
)

Definition at line 899 of file pr2_teleop_general_commander.cpp.

void GeneralCommander::sendBaseCommand ( double  vx,
double  vy,
double  vw 
)

Definition at line 593 of file pr2_teleop_general_commander.cpp.

void GeneralCommander::sendGripperCommand ( WhichArm  which,
bool  close 
)

Definition at line 536 of file pr2_teleop_general_commander.cpp.

void GeneralCommander::sendHeadCommand ( double  req_pan,
double  req_tilt 
)

Definition at line 477 of file pr2_teleop_general_commander.cpp.

void GeneralCommander::sendHeadSequence ( HeadSequence  seq)

Definition at line 1516 of file pr2_teleop_general_commander.cpp.

void GeneralCommander::sendHeadTrackCommand ( )

Definition at line 497 of file pr2_teleop_general_commander.cpp.

void GeneralCommander::sendProjectorStartStop ( bool  start)

Definition at line 327 of file pr2_teleop_general_commander.cpp.

void GeneralCommander::sendTorsoCommand ( double  pos,
double  vel 
)

Definition at line 575 of file pr2_teleop_general_commander.cpp.

void GeneralCommander::sendWalkAlongCommand ( double  thresh,
double  x_dist_max,
double  x_speed_scale,
double  y_dist_max,
double  y_speed_scale,
double  rot_scale 
)

Definition at line 1402 of file pr2_teleop_general_commander.cpp.

void GeneralCommander::sendWristVelCommands ( double  right_wrist_vel,
double  left_wrist_vel,
double  hz 
)

Definition at line 627 of file pr2_teleop_general_commander.cpp.

void GeneralCommander::setArmMode ( WhichArm  which,
ArmControlMode  mode 
)

Definition at line 403 of file pr2_teleop_general_commander.cpp.

void GeneralCommander::setHeadMode ( HeadControlMode  mode)

Definition at line 380 of file pr2_teleop_general_commander.cpp.

void GeneralCommander::setLaserMode ( LaserControlMode  mode)

Definition at line 346 of file pr2_teleop_general_commander.cpp.

void GeneralCommander::switchControllers ( const std::vector< std::string > &  start_controllers,
const std::vector< std::string > &  stop_controllers 
)

Definition at line 603 of file pr2_teleop_general_commander.cpp.

void GeneralCommander::tuckArms ( WhichArm  arm)

Definition at line 1552 of file pr2_teleop_general_commander.cpp.

void GeneralCommander::turnOffWalkAlong ( )
inline

Definition at line 157 of file pr2_teleop_general_commander.h.

void GeneralCommander::unnormalizeTrajectory ( trajectory_msgs::JointTrajectory &  traj) const
private

Definition at line 845 of file pr2_teleop_general_commander.cpp.

void GeneralCommander::untuckArms ( WhichArm  arm)

Definition at line 1575 of file pr2_teleop_general_commander.cpp.

void GeneralCommander::updateCurrentWristPositions ( )

Definition at line 695 of file pr2_teleop_general_commander.cpp.

void GeneralCommander::updateWalkAlongAverages ( )
private

Definition at line 1362 of file pr2_teleop_general_commander.cpp.

Member Data Documentation

ros::Publisher GeneralCommander::base_pub_
private

Definition at line 226 of file pr2_teleop_general_commander.h.

bool GeneralCommander::control_body_
private

Definition at line 184 of file pr2_teleop_general_commander.h.

bool GeneralCommander::control_head_
private

Definition at line 185 of file pr2_teleop_general_commander.h.

bool GeneralCommander::control_larm_
private

Definition at line 187 of file pr2_teleop_general_commander.h.

bool GeneralCommander::control_prosilica_
private

Definition at line 188 of file pr2_teleop_general_commander.h.

bool GeneralCommander::control_rarm_
private

Definition at line 186 of file pr2_teleop_general_commander.h.

geometry_msgs::Pose GeneralCommander::des_l_wrist_roll_pose_
private

Definition at line 205 of file pr2_teleop_general_commander.h.

geometry_msgs::Pose GeneralCommander::des_r_wrist_roll_pose_
private

Definition at line 205 of file pr2_teleop_general_commander.h.

HeadControlMode GeneralCommander::head_control_mode_
private

Definition at line 244 of file pr2_teleop_general_commander.h.

trajectory_msgs::JointTrajectory GeneralCommander::head_nod_traj_
private

Definition at line 215 of file pr2_teleop_general_commander.h.

ros::Publisher GeneralCommander::head_pub_
private

Definition at line 224 of file pr2_teleop_general_commander.h.

trajectory_msgs::JointTrajectory GeneralCommander::head_shake_traj_
private

Definition at line 215 of file pr2_teleop_general_commander.h.

actionlib::SimpleActionClient<pr2_controllers_msgs::PointHeadAction>* GeneralCommander::head_track_hand_client_
private

Definition at line 248 of file pr2_teleop_general_commander.h.

std::map<std::string, double> GeneralCommander::joint_state_position_map_
private

Definition at line 201 of file pr2_teleop_general_commander.h.

ros::Subscriber GeneralCommander::joint_state_sub_
private

Definition at line 229 of file pr2_teleop_general_commander.h.

std::map<std::string, double> GeneralCommander::joint_state_velocity_map_
private

Definition at line 202 of file pr2_teleop_general_commander.h.

std::string GeneralCommander::l_arm_controller_name_
private

Definition at line 199 of file pr2_teleop_general_commander.h.

LaserControlMode GeneralCommander::laser_control_mode_
private

Definition at line 243 of file pr2_teleop_general_commander.h.

double GeneralCommander::laser_fast_amplitude_
private

Definition at line 195 of file pr2_teleop_general_commander.h.

double GeneralCommander::laser_fast_offset_
private

Definition at line 196 of file pr2_teleop_general_commander.h.

double GeneralCommander::laser_fast_period_
private

Definition at line 194 of file pr2_teleop_general_commander.h.

double GeneralCommander::laser_slow_amplitude_
private

Definition at line 191 of file pr2_teleop_general_commander.h.

double GeneralCommander::laser_slow_offset_
private

Definition at line 192 of file pr2_teleop_general_commander.h.

double GeneralCommander::laser_slow_period_
private

Definition at line 190 of file pr2_teleop_general_commander.h.

ros::Time GeneralCommander::last_left_wrist_goal_stamp_
private

Definition at line 233 of file pr2_teleop_general_commander.h.

ros::Time GeneralCommander::last_right_wrist_goal_stamp_
private

Definition at line 232 of file pr2_teleop_general_commander.h.

double GeneralCommander::last_torso_vel_
private

Definition at line 235 of file pr2_teleop_general_commander.h.

ArmControlMode GeneralCommander::left_arm_control_mode_
private

Definition at line 246 of file pr2_teleop_general_commander.h.

ros::ServiceClient GeneralCommander::left_arm_kinematics_forward_client_
private

Definition at line 221 of file pr2_teleop_general_commander.h.

ros::ServiceClient GeneralCommander::left_arm_kinematics_inverse_client_
private

Definition at line 222 of file pr2_teleop_general_commander.h.

ros::Publisher GeneralCommander::left_arm_traj_pub_
private

Definition at line 228 of file pr2_teleop_general_commander.h.

actionlib::SimpleActionClient<pr2_controllers_msgs::JointTrajectoryAction>* GeneralCommander::left_arm_trajectory_client_
private

Definition at line 252 of file pr2_teleop_general_commander.h.

std::vector<double> GeneralCommander::left_des_joint_states_
private

Definition at line 210 of file pr2_teleop_general_commander.h.

actionlib::SimpleActionClient<pr2_controllers_msgs::Pr2GripperCommandAction>* GeneralCommander::left_gripper_client_
private

Definition at line 250 of file pr2_teleop_general_commander.h.

std::vector<double> GeneralCommander::left_walk_along_pose_
private

Definition at line 208 of file pr2_teleop_general_commander.h.

geometry_msgs::Pose GeneralCommander::left_wrist_roll_pose_
private

Definition at line 204 of file pr2_teleop_general_commander.h.

ros::NodeHandle GeneralCommander::n_
private

Definition at line 182 of file pr2_teleop_general_commander.h.

ros::Subscriber GeneralCommander::power_board_sub_
private

Definition at line 230 of file pr2_teleop_general_commander.h.

ros::ServiceClient GeneralCommander::prosilica_polling_client_
private

Definition at line 223 of file pr2_teleop_general_commander.h.

std::string GeneralCommander::r_arm_controller_name_
private

Definition at line 198 of file pr2_teleop_general_commander.h.

ArmControlMode GeneralCommander::right_arm_control_mode_
private

Definition at line 245 of file pr2_teleop_general_commander.h.

ros::ServiceClient GeneralCommander::right_arm_kinematics_forward_client_
private

Definition at line 219 of file pr2_teleop_general_commander.h.

ros::ServiceClient GeneralCommander::right_arm_kinematics_inverse_client_
private

Definition at line 220 of file pr2_teleop_general_commander.h.

ros::Publisher GeneralCommander::right_arm_traj_pub_
private

Definition at line 227 of file pr2_teleop_general_commander.h.

actionlib::SimpleActionClient<pr2_controllers_msgs::JointTrajectoryAction>* GeneralCommander::right_arm_trajectory_client_
private

Definition at line 251 of file pr2_teleop_general_commander.h.

std::vector<double> GeneralCommander::right_des_joint_states_
private

Definition at line 210 of file pr2_teleop_general_commander.h.

actionlib::SimpleActionClient<pr2_controllers_msgs::Pr2GripperCommandAction>* GeneralCommander::right_gripper_client_
private

Definition at line 249 of file pr2_teleop_general_commander.h.

std::vector<double> GeneralCommander::right_walk_along_pose_
private

Definition at line 208 of file pr2_teleop_general_commander.h.

geometry_msgs::Pose GeneralCommander::right_wrist_roll_pose_
private

Definition at line 204 of file pr2_teleop_general_commander.h.

urdf::Model GeneralCommander::robot_model_
private

A model of the robot to see which joints wrap around.

Definition at line 238 of file pr2_teleop_general_commander.h.

bool GeneralCommander::robot_model_initialized_
private

Flag that tells us if the robot model was initialized successfully.

Definition at line 240 of file pr2_teleop_general_commander.h.

bool GeneralCommander::status_projector_on
private

Definition at line 242 of file pr2_teleop_general_commander.h.

ros::ServiceClient GeneralCommander::switch_controllers_service_
private

Definition at line 218 of file pr2_teleop_general_commander.h.

ros::ServiceClient GeneralCommander::tilt_laser_service_
private

Definition at line 217 of file pr2_teleop_general_commander.h.

ros::Publisher GeneralCommander::torso_pub_
private

Definition at line 225 of file pr2_teleop_general_commander.h.

actionlib::SimpleActionClient<pr2_common_action_msgs::TuckArmsAction>* GeneralCommander::tuck_arms_client_
private

Definition at line 253 of file pr2_teleop_general_commander.h.

geometry_msgs::Pose GeneralCommander::walk_along_left_des_pose_
private

Definition at line 207 of file pr2_teleop_general_commander.h.

bool GeneralCommander::walk_along_ok_
private

Definition at line 213 of file pr2_teleop_general_commander.h.

geometry_msgs::Pose GeneralCommander::walk_along_right_des_pose_
private

Definition at line 207 of file pr2_teleop_general_commander.h.

std::list<double> GeneralCommander::walk_ldx_vals_
private

Definition at line 212 of file pr2_teleop_general_commander.h.

std::list<double> GeneralCommander::walk_ldy_vals_
private

Definition at line 212 of file pr2_teleop_general_commander.h.

std::list<double> GeneralCommander::walk_rdx_vals_
private

Definition at line 212 of file pr2_teleop_general_commander.h.

std::list<double> GeneralCommander::walk_rdy_vals_
private

Definition at line 212 of file pr2_teleop_general_commander.h.


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


pr2_teleop_general
Author(s): Gil Jones
autogenerated on Sat Feb 27 2021 04:01:05