#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

enum GeneralCommander::ArmControlMode

Enumerator
ARM_NO_CONTROLLER
ARM_MANNEQUIN_MODE
ARM_POSITION_CONTROL

Definition at line 64 of file pr2_teleop_general_commander.h.

enum GeneralCommander::HeadControlMode

Enumerator
HEAD_JOYSTICK
HEAD_TRACK_LEFT_HAND
HEAD_TRACK_RIGHT_HAND
HEAD_MANNEQUIN

Definition at line 70 of file pr2_teleop_general_commander.h.

enum GeneralCommander::HeadSequence

Enumerator
HEAD_NOD
HEAD_SHAKE

Definition at line 83 of file pr2_teleop_general_commander.h.

enum GeneralCommander::LaserControlMode

Enumerator
LASER_TILT_OFF
LASER_TILT_SLOW
LASER_TILT_FAST

Definition at line 77 of file pr2_teleop_general_commander.h.

enum GeneralCommander::WhichArm

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 1484 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 1495 of file pr2_teleop_general_commander.cpp.

bool GeneralCommander::initWalkAlong ( )

Definition at line 1303 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 1155 of file pr2_teleop_general_commander.cpp.

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

private

Definition at line 1537 of file pr2_teleop_general_commander.cpp.

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

Definition at line 1546 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 903 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 1520 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 1406 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 1556 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 1579 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 1366 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:

Public Types

Public Member Functions

Private Member Functions

Private Attributes

Detailed Description

Member Enumeration Documentation

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation