Public Member Functions
	book_stacking ()
double	calculateHowMuchToPush (book_stacking_msgs::ObjectInfo objInfo)
bool	callQueryFK (kinematics_msgs::GetKinematicSolverInfo::Response &response)
bool	callQueryIK (kinematics_msgs::GetKinematicSolverInfo::Response &response)
bool	checkIfRightGripperHoldsObject ()
void	CloseLeftGripper ()
void	CloseRightGripper ()
void	commandCallback (const std_msgs::String::ConstPtr &msg)
pr2_controllers_msgs::JointTrajectoryGoal	createRightArmHomingTrajectory ()
pr2_controllers_msgs::JointTrajectoryGoal	createRightArmTrajectoryFromAngles (std::vector< arm_config_7DOF > qs, std::vector< double > ts)
double	DetermineDragDistance (book_stacking_msgs::ObjectInfo objInfo)
bool	DragObject (book_stacking_msgs::DragRequest drag_req, double &output_dist)
void	drawHull (pcl::PointCloud< Point > hull_pts, const ros::Publisher &plane_pub, float r, float g, float b)
bool	ExecuteDragAction (geometry_msgs::Vector3Stamped desired_dir, double desired_dist, DragActionTemplate drag_act)
bool	ExecuteGraspAction (DragActionTemplate drag_act)
void	ExecuteNavWaypoints (book_stacking_msgs::NavWaypoints goals)
bool	ExecutePlaceAction (DragActionTemplate drag_act)
bool	ExploreDragAction (book_stacking_msgs::ObjectInfo objInfo, geometry_msgs::Vector3Stamped desired_dir, double desired_dist, double &output_dist, DragActionTemplate &drag_act, geometry_msgs::PoseStamped &queried_pose_wrt_torso_lift_link)
bool	ExploreGraspAction (book_stacking_msgs::ObjectInfo objInfo, double &output_dist, DragActionTemplate &grasp_act, geometry_msgs::PoseStamped &queried_pose_wrt_torso_lift_link)
bool	ExploreLinearMoveAction (double dx, double dy, double dz, arm_config_7DOF q_seed, bool move_as_much_you_can, double &path_distance, arm_config_7DOF &q_solution)
bool	ExplorePlaceAction (book_stacking_msgs::ObjectInfo objInfo, double &output_dist, DragActionTemplate &grasp_act, geometry_msgs::PoseStamped &queried_pose_wrt_torso_lift_link)
void	FingertipLeftCallback (const pr2_msgs::PressureState::ConstPtr &msg)
void	FingertipRightCallback (const pr2_msgs::PressureState::ConstPtr &msg)
arm_config_7DOF	GetCurrentRightArmJointAngles ()
double	getEuclidianDist (double x1, double y1, double x2, double y2)
arm_config_7DOF	GetExampleRightArmJointAngles ()
bool	getInitTable (bool refreshScan)
bool	getOdomPose (double &x, double &y, double &yaw, const ros::Time &t)
int	getShortestObjectIndex (book_stacking_msgs::ObjectInfos allObjectInfos)
bool	getTablePlane (XYZPointCloud &cloud, book_stacking_msgs::PlaneInfo &pl_info, bool draw_marker)
int	getTallestObjectIndex (book_stacking_msgs::ObjectInfos allObjectInfos)
bool	GetTiltingPointCloud (bool refreshScan)
bool	GraspObject (int object_index, book_stacking_msgs::ObjectInfos allObjectInfos, double &output_dist)
void	HomeRightArm ()
void	InitializeRobot ()
void	KinectCallback (const sensor_msgs::PointCloud2ConstPtr &cloud_msg)
void	LoadParameters ()
void	log_IK ()
void	lookAt (std::string frame_id, double x, double y, double z)
bool	MoveBasePosition (move_base_msgs::MoveBaseGoal goal)
void	moveTorsoToPosition (double d)
bool	NavigateToObject (book_stacking_msgs::ObjectInfo objInfo)
bool	OmnixAdjustBaseForOptimalWorkspace (geometry_msgs::PoseStamped queried_pose_wrt_torso_lift_link)
bool	OmnixAdjustBaseForOptimalWorkspaceForGrasping (geometry_msgs::PoseStamped queried_pose_wrt_torso_lift_link)
bool	OmnixMoveBasePosition (move_base_msgs::MoveBaseGoal goal)
void	OpenLeftGripper ()
void	OpenRightGripper ()
bool	PickUpBook (int object_index)
bool	PlaceEndEffector (bool use_right_arm, ArmActionClient *arm_ac_client_, arm_navigation_msgs::SimplePoseConstraint &desired_pose, bool disable_gripper)
bool	PlaceObject (int object_index, double &output_dist)
bool	plan0 ()
bool	run_fk (arm_config_7DOF q, geometry_msgs::PoseStamped &solution, kinematics_msgs::GetKinematicSolverInfo::Response fk_solver_info)
bool	run_ik (geometry_msgs::PoseStamped pose, arm_config_7DOF q_seed, arm_config_7DOF &q_solution, std::string link_name, kinematics_msgs::GetKinematicSolverInfo::Response ik_solver_info)
void	SaveAllObjectsInOdomCombined (book_stacking_msgs::ObjectInfos allObjectInfos)
unsigned int	ScanAndGetTableAndObjects (book_stacking_msgs::PlaneInfo &latest_table_plane_info, book_stacking_msgs::ObjectInfos &allObjectInfos, bool refreshScan)
void	SendLeftGripperCommand (double pos, double effort)
bool	SendRightEndEffectorTrajectory (pr2_controllers_msgs::JointTrajectoryGoal traj, bool stop_when_contact)
void	SendRightGripperCommand (double pos, double effort)
bool	SetMoveArmGoal (double x, double y, double z)
void	shakeHead (int n)
void	TestArm ()
void	TestArm2 ()
void	TestArm3 ()
void	TestCollision ()
void	TiltTimingCallback (const pr2_msgs::LaserScannerSignal::ConstPtr &msg)
bool	TrackObject (int object_index, book_stacking_msgs::ObjectInfos allObjectInfos, double max_matching_dist)
Public Attributes
ros::Subscriber	command_subscriber_
ros::Publisher	filtered_cloud_pub_
ros::NodeHandle	n_
ros::Publisher	obj_marker_pub_
ros::Publisher	plane_marker_pub_
ros::Publisher	vis_pub_
Private Attributes
book_stacking_msgs::ObjectInfos	all_objects_wrt_odom_combined
std::vector< double >	all_objects_x_wrt_odom_combined
std::vector< double >	all_objects_y_wrt_odom_combined
std::vector< double >	all_objects_z_wrt_odom_combined
std::string	base_frame_tf
ros::ServiceClient	collision_processing_srv
bool	concave_hull_mode_
double	diff_drag_force
bool	downsample_cloud
double	downsample_grid_size_
bool	enable_nav
bool	filter_outliers
int	filter_outliers_meank
double	filter_outliers_stddev_thresh
bool	filter_spatial
double	filter_spatial_xmax
double	filter_spatial_xmin
double	filter_spatial_ymax
double	filter_spatial_ymin
double	filter_spatial_zmax
double	filter_spatial_zmin
double	FINGERTIP_CONTACT_THRESHOLD
ros::ServiceClient	fk_client_right
kinematics_msgs::GetKinematicSolverInfo::Response	fk_solver_info
bool	got_init_table
double	grasp_offset_from_com
ros::ServiceClient	ik_client_right
kinematics_msgs::GetKinematicSolverInfo::Response	ik_solver_info
geometry_msgs::Point32	init_table_centroid
pcl::PointCloud< Point >	init_table_hull
book_stacking_msgs::PlaneInfo	init_table_plane_info
double	init_torso_position
ros::Subscriber	l_fingertip_sub_
ros::ServiceClient	laser_assembler_client
XYZPointCloud	latest_cloud
bool	latest_pt_cloud_ready
bool	left_arm_fingertips_sensing
GripperClient *	left_gripper_client_
std::ofstream	logFile
int	max_planes_
int	max_sac_iterations_
int	min_plane_inliers_
MoveBaseClient *	move_base_client_
ArmActionClient *	move_left_arm_client_
ArmActionClient *	move_right_arm_client_
book_stacking_msgs::NavWaypoints	nav_waypoint_goals
double	nav_waypoint_offset
pcl::PointCloud< Point >	nav_waypoints
double	normal_search_radius_
MoveBaseClient *	omnix_move_base_client_
double	optimal_workspace_wrt_torso_x
double	optimal_workspace_wrt_torso_x_grasping
double	optimal_workspace_wrt_torso_y
double	optimal_workspace_wrt_torso_y_grasping
double	optimal_workspace_wrt_torso_z
double	optimal_workspace_wrt_torso_z_grasping
PickupClient *	pickup_client_
double	plane_distance_thresh_
ros::Subscriber	point_cloud_sub_
PointHeadClient *	point_head_client_
double	predrag_dist
double	pregrasp_dist
double	pregrasp_dist_vertical
double	preplace_dist
ros::Subscriber	r_fingertip_sub_
bool	right_arm_end_eff_goal_resulted
bool	right_arm_fingertips_contacted
bool	right_arm_fingertips_nominals
bool	right_arm_fingertips_sensing
std::vector< short int >	right_arm_l_finger_tip_nominals
std::vector< short int >	right_arm_r_finger_tip_nominals
GripperClient *	right_gripper_client_
pr2_controllers_msgs::JointTrajectoryGoal	rightArmHomingTrajectory
bool	robot_initialized
ros::NodeHandle	root_handle_
double	sac_probability_
double	table_obj_detector_lower_z
double	table_obj_detector_upper_z
bool	test_arms
tf::TransformListener	tf_listener
pr2_msgs::LaserScannerSignal	tilt_maxima_msg
pr2_msgs::LaserScannerSignal	tilt_minima_msg
double	tilt_period
ros::Subscriber	tilt_timing_sub_
ros::Publisher	tilting_pt_cloud_pub_
TorsoClient *	torso_client_
TrajClient *	traj_right_arm_client_
bool	use_normal_seg_
bool	use_omp_
std::string	workspace_frame

Detailed Description

Definition at line 104 of file book_stacking.cpp.

Constructor & Destructor Documentation

book_stacking::book_stacking ( ) [inline]

Definition at line 201 of file book_stacking.cpp.

Member Function Documentation

double book_stacking::calculateHowMuchToPush ( book_stacking_msgs::ObjectInfo objInfo ) [inline]

Definition at line 553 of file book_stacking.cpp.

bool book_stacking::callQueryFK ( kinematics_msgs::GetKinematicSolverInfo::Response & response ) [inline]

Definition at line 1301 of file book_stacking.cpp.

bool book_stacking::callQueryIK ( kinematics_msgs::GetKinematicSolverInfo::Response & response ) [inline]

Definition at line 1326 of file book_stacking.cpp.

bool book_stacking::checkIfRightGripperHoldsObject ( ) [inline]

Definition at line 1670 of file book_stacking.cpp.

void book_stacking::CloseLeftGripper ( ) [inline]

Definition at line 1828 of file book_stacking.cpp.

void book_stacking::CloseRightGripper ( ) [inline]

Definition at line 1851 of file book_stacking.cpp.

void book_stacking::commandCallback ( const std_msgs::String::ConstPtr & msg ) [inline]

Definition at line 220 of file book_stacking.cpp.

pr2_controllers_msgs::JointTrajectoryGoal book_stacking::createRightArmHomingTrajectory ( ) [inline]

Definition at line 1488 of file book_stacking.cpp.

pr2_controllers_msgs::JointTrajectoryGoal book_stacking::createRightArmTrajectoryFromAngles	(	std::vector< arm_config_7DOF >	qs,
		std::vector< double >	ts
	)		`[inline]`

Definition at line 1452 of file book_stacking.cpp.

double book_stacking::DetermineDragDistance ( book_stacking_msgs::ObjectInfo objInfo ) [inline]

Definition at line 597 of file book_stacking.cpp.

bool book_stacking::DragObject	(	book_stacking_msgs::DragRequest	drag_req,
		double &	output_dist
	)		`[inline]`

Definition at line 2558 of file book_stacking.cpp.

void book_stacking::drawHull	(	pcl::PointCloud< Point >	hull_pts,
		const ros::Publisher &	plane_pub,
		float	r,
		float	g,
		float	b
	)		`[inline]`

Definition at line 399 of file book_stacking.cpp.

bool book_stacking::ExecuteDragAction	(	geometry_msgs::Vector3Stamped	desired_dir,
		double	desired_dist,
		DragActionTemplate	drag_act
	)		`[inline]`

Definition at line 2091 of file book_stacking.cpp.

bool book_stacking::ExecuteGraspAction ( DragActionTemplate drag_act ) [inline]

Definition at line 2265 of file book_stacking.cpp.

void book_stacking::ExecuteNavWaypoints ( book_stacking_msgs::NavWaypoints goals ) [inline]

Definition at line 566 of file book_stacking.cpp.

bool book_stacking::ExecutePlaceAction ( DragActionTemplate drag_act ) [inline]

Definition at line 2218 of file book_stacking.cpp.

bool book_stacking::ExploreDragAction	(	book_stacking_msgs::ObjectInfo	objInfo,
		geometry_msgs::Vector3Stamped	desired_dir,
		double	desired_dist,
		double &	output_dist,
		DragActionTemplate &	drag_act,
		geometry_msgs::PoseStamped &	queried_pose_wrt_torso_lift_link
	)		`[inline]`

Definition at line 2168 of file book_stacking.cpp.

bool book_stacking::ExploreGraspAction	(	book_stacking_msgs::ObjectInfo	objInfo,
		double &	output_dist,
		DragActionTemplate &	grasp_act,
		geometry_msgs::PoseStamped &	queried_pose_wrt_torso_lift_link
	)		`[inline]`

Definition at line 2377 of file book_stacking.cpp.

bool book_stacking::ExploreLinearMoveAction	(	double	dx,
		double	dy,
		double	dz,
		arm_config_7DOF	q_seed,
		bool	move_as_much_you_can,
		double &	path_distance,
		arm_config_7DOF &	q_solution
	)		`[inline]`

Definition at line 1696 of file book_stacking.cpp.

bool book_stacking::ExplorePlaceAction	(	book_stacking_msgs::ObjectInfo	objInfo,
		double &	output_dist,
		DragActionTemplate &	grasp_act,
		geometry_msgs::PoseStamped &	queried_pose_wrt_torso_lift_link
	)		`[inline]`

Definition at line 2327 of file book_stacking.cpp.

void book_stacking::FingertipLeftCallback ( const pr2_msgs::PressureState::ConstPtr & msg ) [inline]

Definition at line 1857 of file book_stacking.cpp.

void book_stacking::FingertipRightCallback ( const pr2_msgs::PressureState::ConstPtr & msg ) [inline]

Definition at line 1868 of file book_stacking.cpp.

arm_config_7DOF book_stacking::GetCurrentRightArmJointAngles ( ) [inline]

Definition at line 1554 of file book_stacking.cpp.

double book_stacking::getEuclidianDist	(	double	x1,
		double	y1,
		double	x2,
		double	y2
	)		`[inline]`

Definition at line 559 of file book_stacking.cpp.

arm_config_7DOF book_stacking::GetExampleRightArmJointAngles ( ) [inline]

Definition at line 1543 of file book_stacking.cpp.

bool book_stacking::getInitTable ( bool refreshScan ) [inline]

Definition at line 445 of file book_stacking.cpp.

bool book_stacking::getOdomPose	(	double &	x,
		double &	y,
		double &	yaw,
		const ros::Time &	t
	)		`[inline]`

Definition at line 786 of file book_stacking.cpp.

int book_stacking::getShortestObjectIndex ( book_stacking_msgs::ObjectInfos allObjectInfos ) [inline]

Definition at line 1117 of file book_stacking.cpp.

bool book_stacking::getTablePlane	(	XYZPointCloud &	cloud,
		book_stacking_msgs::PlaneInfo &	pl_info,
		bool	draw_marker
	)		`[inline]`

Definition at line 3022 of file book_stacking.cpp.

int book_stacking::getTallestObjectIndex ( book_stacking_msgs::ObjectInfos allObjectInfos ) [inline]

Definition at line 1093 of file book_stacking.cpp.

bool book_stacking::GetTiltingPointCloud ( bool refreshScan ) [inline]

Definition at line 1912 of file book_stacking.cpp.

bool book_stacking::GraspObject	(	int	object_index,
		book_stacking_msgs::ObjectInfos	allObjectInfos,
		double &	output_dist
	)		`[inline]`

Definition at line 2492 of file book_stacking.cpp.

void book_stacking::HomeRightArm ( ) [inline]

Definition at line 1152 of file book_stacking.cpp.

void book_stacking::InitializeRobot ( ) [inline]

Definition at line 1351 of file book_stacking.cpp.

void book_stacking::KinectCallback ( const sensor_msgs::PointCloud2ConstPtr & cloud_msg ) [inline]

Definition at line 2981 of file book_stacking.cpp.

void book_stacking::LoadParameters ( ) [inline]

Definition at line 1994 of file book_stacking.cpp.

void book_stacking::log_IK ( ) [inline]

Definition at line 1268 of file book_stacking.cpp.

void book_stacking::lookAt	(	std::string	frame_id,
		double	x,
		double	y,
		double	z
	)		`[inline]`

Definition at line 2050 of file book_stacking.cpp.

bool book_stacking::MoveBasePosition ( move_base_msgs::MoveBaseGoal goal ) [inline]

Definition at line 1159 of file book_stacking.cpp.

void book_stacking::moveTorsoToPosition ( double d ) [inline]

Definition at line 1983 of file book_stacking.cpp.

bool book_stacking::NavigateToObject ( book_stacking_msgs::ObjectInfo objInfo ) [inline]

Definition at line 669 of file book_stacking.cpp.

bool book_stacking::OmnixAdjustBaseForOptimalWorkspace ( geometry_msgs::PoseStamped queried_pose_wrt_torso_lift_link ) [inline]

Definition at line 1197 of file book_stacking.cpp.

bool book_stacking::OmnixAdjustBaseForOptimalWorkspaceForGrasping ( geometry_msgs::PoseStamped queried_pose_wrt_torso_lift_link ) [inline]

Definition at line 1172 of file book_stacking.cpp.

bool book_stacking::OmnixMoveBasePosition ( move_base_msgs::MoveBaseGoal goal ) [inline]

Definition at line 1223 of file book_stacking.cpp.

void book_stacking::OpenLeftGripper ( ) [inline]

Definition at line 1823 of file book_stacking.cpp.

void book_stacking::OpenRightGripper ( ) [inline]

Definition at line 1846 of file book_stacking.cpp.

bool book_stacking::PickUpBook ( int object_index ) [inline]

Definition at line 910 of file book_stacking.cpp.

bool book_stacking::PlaceEndEffector	(	bool	use_right_arm,
		ArmActionClient *	arm_ac_client_,
		arm_navigation_msgs::SimplePoseConstraint &	desired_pose,
		bool	disable_gripper
	)		`[inline]`

Definition at line 2612 of file book_stacking.cpp.

bool book_stacking::PlaceObject	(	int	object_index,
		double &	output_dist
	)		`[inline]`

Definition at line 2426 of file book_stacking.cpp.

bool book_stacking::plan0 ( ) [inline]

Definition at line 1036 of file book_stacking.cpp.

bool book_stacking::run_fk	(	arm_config_7DOF	q,
		geometry_msgs::PoseStamped &	solution,
		kinematics_msgs::GetKinematicSolverInfo::Response	fk_solver_info
	)		`[inline]`

Definition at line 1579 of file book_stacking.cpp.

bool book_stacking::run_ik	(	geometry_msgs::PoseStamped	pose,
		arm_config_7DOF	q_seed,
		arm_config_7DOF &	q_solution,
		std::string	link_name,
		kinematics_msgs::GetKinematicSolverInfo::Response	ik_solver_info
	)		`[inline]`

Definition at line 1626 of file book_stacking.cpp.

void book_stacking::SaveAllObjectsInOdomCombined ( book_stacking_msgs::ObjectInfos allObjectInfos ) [inline]

Definition at line 837 of file book_stacking.cpp.

unsigned int book_stacking::ScanAndGetTableAndObjects	(	book_stacking_msgs::PlaneInfo &	latest_table_plane_info,
		book_stacking_msgs::ObjectInfos &	allObjectInfos,
		bool	refreshScan
	)		`[inline]`