Public Member Functions
void	abortPushingGoal (std::string msg)
ShapeLocation	chooseFixedGoalPushStartLoc (ProtoObject &cur_obj, PushTrackerState &cur_state, bool new_object, int num_start_loc_pushes_per_sample, int num_start_loc_sample_locs, std::string trial_id)
ShapeLocation	chooseLearnedPushStartLoc (ProtoObject &cur_obj, PushTrackerState &cur_state, std::string param_path, float &chosen_score, bool previous_position_worked, bool rotate_push)
ShapeLocation	choosePushStartLoc (ProtoObject &cur_obj, PushTrackerState &cur_state, bool new_object, int num_clusters)
ShapeLocation	chooseRandomPushStartLoc (ProtoObject &cur_obj, PushTrackerState &cur_state, bool rotate_push)
void	displayGoalHeading (cv::Mat &img, PointStamped &centroid, double theta, double goal_theta, bool force_no_write=false)
void	displayInitialPushVector (cv::Mat &img, PointStamped &start_point, PointStamped &end_point, PointStamped &centroid)
void	displayLearnedPushLocScores (std::vector< double > &push_scores, XYZPointCloud &locs, pcl16::PointXYZ selected, bool rotate_push)
void	displayPushVector (cv::Mat &img, PointStamped &start_point, PointStamped &end_point, std::string display_name="goal_vector", bool force_no_write=false)
void	displayRobotGripperPoses (cv::Mat &img)
void	evaluateGoalAndAbortConditions (PushTrackerState &tracker_state)
std::vector< pcl16::PointXYZ >	findAxisAlignedBoundingBox (PushTrackerState &cur_state, ProtoObject &cur_obj)
Pose2D	generateStartLocLearningGoalPose (PushTrackerState &cur_state, ProtoObject &cur_obj, ShapeLocation &chosen_loc, float &new_push_angle, bool rotate_push=false)
void	getObjectPose (LearnPush::Response &res)
LearnPush::Response	getPushStartPose (LearnPush::Request &req)
float	getRotatePushHeading (PushTrackerState &cur_state, ShapeLocation &chosen_loc, ProtoObject &cur_obj)
ShapeLocation	getStartLocDescriptor (ProtoObject &cur_obj, PushTrackerState &cur_state, geometry_msgs::Point start_pt)
bool	getTableLocation (LocateTable::Request &req, LocateTable::Response &res)
void	getTablePlane (XYZPointCloud &cloud, PoseStamped &p)
bool	goalPoseValid (Pose2D goal_pose) const
bool	gripperNotMoving ()
void	lArmStateCartCB (const pr2_manipulation_controllers::JTTaskControllerState l_arm_state)
void	lArmStateVelCB (const pr2_manipulation_controllers::JinvTeleopControllerState l_arm_state)
bool	learnPushCallback (LearnPush::Request &req, LearnPush::Response &res)
bool	objectDisappeared (PushTrackerState &tracker_state)
bool	objectNotBetweenGoalAndGripper (Pose2D &obj_state)
bool	objectNotMoving (PushTrackerState &tracker_state)
pcl16::PointXYZ	objectPointInWorldFrame (pcl16::PointXYZ obj_pt, PushTrackerState &cur_state)
bool	objectTooFarFromGripper (Pose2D &obj_state)
bool	pointIsBetweenOthers (geometry_msgs::Point pt, Pose2D &x1, Pose2D &x2, double epsilon=0.0)
bool	pushingTimeUp ()
void	pushTrackerGoalCB ()
void	pushTrackerPreemptCB ()
void	rArmStateCartCB (const pr2_manipulation_controllers::JTTaskControllerState r_arm_state)
void	rArmStateVelCB (const pr2_manipulation_controllers::JinvTeleopControllerState r_arm_state)
void	sensorCallback (const sensor_msgs::ImageConstPtr &img_msg, const sensor_msgs::ImageConstPtr &depth_msg, const sensor_msgs::ImageConstPtr &mask_msg, const sensor_msgs::PointCloud2ConstPtr &cloud_msg)
void	spin ()
void	startTracking (PushTrackerState &state, bool start_swap=false)
	TabletopPushingPerceptionNode (ros::NodeHandle &n)
pcl16::PointXYZ	worldPointInObjectFrame (pcl16::PointXYZ world_pt, PushTrackerState &cur_state)
void	worldPointsInObjectFrame (XYZPointCloud &world_cloud, PushTrackerState &cur_state, XYZPointCloud &object_cloud)
void	writePredictedScoreToDisk (XYZPointCloud &hull_cloud, ShapeDescriptors &sds, std::vector< double > &pred_scores)
Protected Attributes
shared_ptr< ArmObjSegmentation >	arm_obj_segmenter_
actionlib::SimpleActionServer < PushTrackerAction >	as_
std::string	base_output_path_
std::string	behavior_primitive_
sensor_msgs::CameraInfo	cam_info_
std::string	cam_info_topic_
std::string	camera_frame_
bool	camera_initialized_
message_filters::Subscriber < sensor_msgs::PointCloud2 >	cloud_sub_
std::string	controller_name_
std_msgs::Header	cur_camera_header_
cv::Mat	cur_color_frame_
cv::Mat	cur_depth_frame_
XYZPointCloud	cur_point_cloud_
XYZPointCloud	cur_self_filtered_cloud_
cv::Mat	cur_self_mask_
std::string	current_file_id_
message_filters::Subscriber < sensor_msgs::Image >	depth_sub_
int	display_wait_ms_
int	footprint_count_
bool	force_swap_
int	frame_callback_count_
int	frame_set_count_
int	goal_heading_count_
int	goal_out_count_
int	gripper_not_moving_count_
int	gripper_not_moving_count_limit_
double	gripper_not_moving_thresh_
double	gripper_spread_
bool	have_depth_data_
double	hull_alpha_
message_filters::Subscriber < sensor_msgs::Image >	image_sub_
ros::Subscriber	jinv_l_arm_subscriber_
ros::Subscriber	jinv_r_arm_subscriber_
ros::Subscriber	jtteleop_l_arm_subscriber_
ros::Subscriber	jtteleop_r_arm_subscriber_
bool	just_spun_
PoseStamped	l_arm_pose_
Twist	l_arm_vel_
int	learn_callback_count_
double	major_axis_spin_pos_scale_
int	mask_dilate_size_
message_filters::Subscriber < sensor_msgs::Image >	mask_sub_
double	max_goal_x_
double	max_goal_y_
double	max_object_gripper_dist_
double	min_goal_x_
double	min_goal_y_
ros::NodeHandle	n_
ros::NodeHandle	n_private_
int	num_downsamples_
int	num_position_failures_
shared_ptr< ObjectTracker25D >	obj_tracker_
int	object_not_between_count_
int	object_not_between_count_limit_
double	object_not_between_epsilon_
int	object_not_detected_count_
int	object_not_detected_count_limit_
int	object_not_moving_count_
int	object_not_moving_count_limit_
double	object_not_moving_thresh_
int	object_too_far_count_
int	object_too_far_count_limit_
shared_ptr < PointCloudSegmentation >	pcl_segmenter_
double	point_cloud_hist_res_
std::string	proxy_name_
ros::ServiceServer	push_pose_server_
double	push_start_time_
std::string	pushing_arm_
PoseStamped	r_arm_pose_
Twist	r_arm_vel_
int	record_count_
bool	recording_input_
double	start_loc_arc_length_percent_
ShapeLocations	start_loc_history_
ProtoObject	start_loc_obj_
double	start_loc_push_dist_
int	start_loc_push_sample_count_
double	start_loc_push_time_
bool	start_loc_use_fixed_goal_
bool	start_tracking_on_push_call_
message_filters::Synchronizer < MySyncPolicy >	sync_
PoseStamped	table_centroid_
ros::ServiceServer	table_location_server_
shared_ptr< tf::TransformListener >	tf_
bool	timing_push_
double	tracker_angle_thresh_
double	tracker_dist_thresh_
Pose2D	tracker_goal_pose_
bool	use_center_pointing_shape_context_
bool	use_displays_
bool	use_graphcut_arm_seg_
bool	use_mps_segmentation_
std::string	workspace_frame_
bool	write_input_to_disk_
bool	write_to_disk_

Detailed Description

Definition at line 190 of file tabletop_pushing_perception_node.cpp.

Constructor & Destructor Documentation

TabletopPushingPerceptionNode::TabletopPushingPerceptionNode ( ros::NodeHandle & n ) [inline]

Definition at line 193 of file tabletop_pushing_perception_node.cpp.

Member Function Documentation

void TabletopPushingPerceptionNode::abortPushingGoal ( std::string msg ) [inline]

Definition at line 784 of file tabletop_pushing_perception_node.cpp.

ShapeLocation TabletopPushingPerceptionNode::chooseFixedGoalPushStartLoc	(	ProtoObject &	cur_obj,
		PushTrackerState &	cur_state,
		bool	new_object,
		int	num_start_loc_pushes_per_sample,
		int	num_start_loc_sample_locs,
		std::string	trial_id
	)		`[inline]`

Method to choose an initial pushing location at a specified percentage around the object boundary with 0 distance on the boundary at the dominatnt orientation of the object.

Parameters:

cur_obj	object model of current frame
cur_state	state estimate of object
new_object	switch if we are initialzing on a new object
num_start_loc_pushes_per_sample	The number of samples to attempt at each push locations
num_start_loc_sample_locs	The number of pushing locations on the boundary to sample

Returns:: The location and shape descriptor on the boundary to place the hand

Definition at line 1200 of file tabletop_pushing_perception_node.cpp.

ShapeLocation TabletopPushingPerceptionNode::chooseLearnedPushStartLoc	(	ProtoObject &	cur_obj,
		PushTrackerState &	cur_state,
		std::string	param_path,
		float &	chosen_score,
		bool	previous_position_worked,
		bool	rotate_push
	)		`[inline]`

Definition at line 1343 of file tabletop_pushing_perception_node.cpp.

ShapeLocation TabletopPushingPerceptionNode::choosePushStartLoc	(	ProtoObject &	cur_obj,
		PushTrackerState &	cur_state,
		bool	new_object,
		int	num_clusters
	)		`[inline]`

Method to determine which pushing location to choose as a function of current object shape descriptors and history

Parameters:

cur_obj	Object for which we are choosing the push location
cur_state	Current state information of the object
new_object	Whether this object is new or has a history
num_clusters	Number of push clusters to use in finding push locations

Returns:: The the location and descriptor of the push location

Definition at line 1108 of file tabletop_pushing_perception_node.cpp.

ShapeLocation TabletopPushingPerceptionNode::chooseRandomPushStartLoc	(	ProtoObject &	cur_obj,
		PushTrackerState &	cur_state,
		bool	rotate_push
	)		`[inline]`

Definition at line 1478 of file tabletop_pushing_perception_node.cpp.

void TabletopPushingPerceptionNode::displayGoalHeading	(	cv::Mat &	img,
		PointStamped &	centroid,
		double	theta,
		double	goal_theta,
		bool	force_no_write = `false`
	)		`[inline]`

Definition at line 2065 of file tabletop_pushing_perception_node.cpp.

void TabletopPushingPerceptionNode::displayInitialPushVector	(	cv::Mat &	img,
		PointStamped &	start_point,
		PointStamped &	end_point,
		PointStamped &	centroid
	)		`[inline]`

Definition at line 2046 of file tabletop_pushing_perception_node.cpp.

void TabletopPushingPerceptionNode::displayLearnedPushLocScores	(	std::vector< double > &	push_scores,
		XYZPointCloud &	locs,
		pcl16::PointXYZ	selected,
		bool	rotate_push
	)		`[inline]`

Definition at line 2145 of file tabletop_pushing_perception_node.cpp.

void TabletopPushingPerceptionNode::displayPushVector	(	cv::Mat &	img,
		PointStamped &	start_point,
		PointStamped &	end_point,
		std::string	display_name = `"goal_vector"`,
		bool	force_no_write = `false`
	)		`[inline]`

Definition at line 2020 of file tabletop_pushing_perception_node.cpp.

void TabletopPushingPerceptionNode::displayRobotGripperPoses ( cv::Mat & img ) [inline]

Definition at line 2119 of file tabletop_pushing_perception_node.cpp.

void TabletopPushingPerceptionNode::evaluateGoalAndAbortConditions ( PushTrackerState & tracker_state ) [inline]

Definition at line 706 of file tabletop_pushing_perception_node.cpp.

std::vector<pcl16::PointXYZ> TabletopPushingPerceptionNode::findAxisAlignedBoundingBox	(	PushTrackerState &	cur_state,
		ProtoObject &	cur_obj
	)		`[inline]`

Definition at line 1519 of file tabletop_pushing_perception_node.cpp.

Pose2D TabletopPushingPerceptionNode::generateStartLocLearningGoalPose	(	PushTrackerState &	cur_state,
		ProtoObject &	cur_obj,
		ShapeLocation &	chosen_loc,
		float &	new_push_angle,
		bool	rotate_push = `false`
	)		`[inline]`

Definition at line 1618 of file tabletop_pushing_perception_node.cpp.

void TabletopPushingPerceptionNode::getObjectPose ( LearnPush::Response & res ) [inline]

Definition at line 1717 of file tabletop_pushing_perception_node.cpp.

LearnPush::Response TabletopPushingPerceptionNode::getPushStartPose ( LearnPush::Request & req ) [inline]

Definition at line 900 of file tabletop_pushing_perception_node.cpp.

float TabletopPushingPerceptionNode::getRotatePushHeading	(	PushTrackerState &	cur_state,
		ShapeLocation &	chosen_loc,
		ProtoObject &	cur_obj
	)		`[inline]`

Definition at line 1556 of file tabletop_pushing_perception_node.cpp.

ShapeLocation TabletopPushingPerceptionNode::getStartLocDescriptor	(	ProtoObject &	cur_obj,
		PushTrackerState &	cur_state,
		geometry_msgs::Point	start_pt
	)		`[inline]`

Definition at line 1651 of file tabletop_pushing_perception_node.cpp.

bool TabletopPushingPerceptionNode::getTableLocation	(	LocateTable::Request &	req,
		LocateTable::Response &	res
	)		`[inline]`

ROS Service callback method for determining the location of a table in the scene

Parameters:

req	The service request
res	The service response

Returns:: true if successfull, false otherwise

Definition at line 1970 of file tabletop_pushing_perception_node.cpp.

void TabletopPushingPerceptionNode::getTablePlane	(	XYZPointCloud &	cloud,
		PoseStamped &	p
	)		`[inline]`

Calculate the location of the dominant plane (table) in a point cloud

Parameters:

cloud The point cloud containing a table

Returns:: The estimated 3D centroid of the table

Definition at line 2003 of file tabletop_pushing_perception_node.cpp.

bool TabletopPushingPerceptionNode::goalPoseValid ( Pose2D goal_pose ) const [inline]

Definition at line 1645 of file tabletop_pushing_perception_node.cpp.

bool TabletopPushingPerceptionNode::gripperNotMoving ( ) [inline]

Definition at line 1855 of file tabletop_pushing_perception_node.cpp.

void TabletopPushingPerceptionNode::lArmStateCartCB ( const pr2_manipulation_controllers::JTTaskControllerState l_arm_state ) [inline]

Definition at line 1771 of file tabletop_pushing_perception_node.cpp.

void TabletopPushingPerceptionNode::lArmStateVelCB ( const pr2_manipulation_controllers::JinvTeleopControllerState l_arm_state ) [inline]

Definition at line 1781 of file tabletop_pushing_perception_node.cpp.

bool TabletopPushingPerceptionNode::learnPushCallback	(	LearnPush::Request &	req,
		LearnPush::Response &	res
	)		`[inline]`

Service request callback method to return a location and orientation for the robot to push.

Parameters:

req	The service request
res	The service response

Returns:: true if successfull, false otherwise

Definition at line 802 of file tabletop_pushing_perception_node.cpp.

bool TabletopPushingPerceptionNode::objectDisappeared ( PushTrackerState & tracker_state ) [inline]

Definition at line 1842 of file tabletop_pushing_perception_node.cpp.

bool TabletopPushingPerceptionNode::objectNotBetweenGoalAndGripper ( Pose2D & obj_state ) [inline]

Definition at line 1878 of file tabletop_pushing_perception_node.cpp.

bool TabletopPushingPerceptionNode::objectNotMoving ( PushTrackerState & tracker_state ) [inline]

Definition at line 1828 of file tabletop_pushing_perception_node.cpp.

pcl16::PointXYZ TabletopPushingPerceptionNode::objectPointInWorldFrame	(	pcl16::PointXYZ	obj_pt,
		PushTrackerState &	cur_state
	)		`[inline]`

Definition at line 1700 of file tabletop_pushing_perception_node.cpp.

bool TabletopPushingPerceptionNode::objectTooFarFromGripper ( Pose2D & obj_state ) [inline]

Definition at line 1908 of file tabletop_pushing_perception_node.cpp.

bool TabletopPushingPerceptionNode::pointIsBetweenOthers	(	geometry_msgs::Point	pt,
		Pose2D &	x1,
		Pose2D &	x2,
		double	epsilon = `0.0`
	)		`[inline]`

Definition at line 1940 of file tabletop_pushing_perception_node.cpp.

bool TabletopPushingPerceptionNode::pushingTimeUp ( ) [inline]

Definition at line 1931 of file tabletop_pushing_perception_node.cpp.

void TabletopPushingPerceptionNode::pushTrackerGoalCB ( ) [inline]

Definition at line 1792 of file tabletop_pushing_perception_node.cpp.

void TabletopPushingPerceptionNode::pushTrackerPreemptCB ( ) [inline]

Definition at line 1820 of file tabletop_pushing_perception_node.cpp.

void TabletopPushingPerceptionNode::rArmStateCartCB ( const pr2_manipulation_controllers::JTTaskControllerState r_arm_state ) [inline]

Definition at line 1776 of file tabletop_pushing_perception_node.cpp.

void TabletopPushingPerceptionNode::rArmStateVelCB ( const pr2_manipulation_controllers::JinvTeleopControllerState r_arm_state ) [inline]

Definition at line 1786 of file tabletop_pushing_perception_node.cpp.

void TabletopPushingPerceptionNode::sensorCallback	(	const sensor_msgs::ImageConstPtr &	img_msg,
		const sensor_msgs::ImageConstPtr &	depth_msg,
		const sensor_msgs::ImageConstPtr &	mask_msg,
		const sensor_msgs::PointCloud2ConstPtr &	cloud_msg
	)		`[inline]`

Definition at line 353 of file tabletop_pushing_perception_node.cpp.

void TabletopPushingPerceptionNode::spin ( ) [inline]

Executive control function for launching the node.

Definition at line 2215 of file tabletop_pushing_perception_node.cpp.

void TabletopPushingPerceptionNode::startTracking	(	PushTrackerState &	state,
		bool	start_swap = `false`
	)		`[inline]`

Definition at line 1751 of file tabletop_pushing_perception_node.cpp.

pcl16::PointXYZ TabletopPushingPerceptionNode::worldPointInObjectFrame	(	pcl16::PointXYZ	world_pt,
		PushTrackerState &	cur_state
	)		`[inline]`

Definition at line 1683 of file tabletop_pushing_perception_node.cpp.

void TabletopPushingPerceptionNode::worldPointsInObjectFrame	(	XYZPointCloud &	world_cloud,
		PushTrackerState &	cur_state,
		XYZPointCloud &	object_cloud
	)		`[inline]`

Definition at line 1671 of file tabletop_pushing_perception_node.cpp.

void TabletopPushingPerceptionNode::writePredictedScoreToDisk	(	XYZPointCloud &	hull_cloud,
		ShapeDescriptors &	sds,
		std::vector< double > &	pred_scores
	)		`[inline]`