Current.cpp File Reference

#include <ros/ros.h>
#include <iostream>
#include <fstream>
#include <ias_drawer_executive/Approach.h>
#include <ias_drawer_executive/RobotDriver.h>
#include <ias_drawer_executive/Gripper.h>
#include <ias_drawer_executive/Geometry.h>
#include <ias_drawer_executive/Pressure.h>
#include <ias_drawer_executive/RobotArm.h>
#include <ias_drawer_executive/Torso.h>
#include <ias_drawer_executive/Head.h>
#include <ias_drawer_executive/OperateHandleController.h>
#include <ias_drawer_executive/Current.h>
#include <ias_drawer_executive/ObjectLocalizer.h>
#include <pcl/common/common.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/point_types.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/features/boundary.h>
#include <pcl/features/normal_3d.h>
#include <pcl/ros/conversions.h>
#include <pcl_ros/point_cloud.h>
#include <pcl_ros/transforms.h>
#include <boost/thread.hpp>
#include <visualization_msgs/Marker.h>
#include "rosbag/bag.h"
#include "rosbag/query.h"
#include "rosbag/view.h"
#include <ias_drawer_executive/Kinect.h>

Include dependency graph for Current.cpp:

Go to the source code of this file.

Functions
void	announce (const std::string text, const tf::Stamped< tf::Pose > &poseStamped)
void	announce_box (const std::string text, const btVector3 min, const btVector3 max)
void	calcBigBowlGraspPoses (const tf::Stamped< tf::Pose > &bowl, tf::Stamped< tf::Pose > &l_pregrasp, tf::Stamped< tf::Pose > &l_grasp, tf::Stamped< tf::Pose > &r_pregrasp, tf::Stamped< tf::Pose > &r_grasp)
void	calcPotGraspPoints (tf::Stamped< tf::Pose > det, tf::Stamped< tf::Pose > &leftpreg, tf::Stamped< tf::Pose > &leftg, tf::Stamped< tf::Pose > &rightpreg, tf::Stamped< tf::Pose > &rightg)
int	closeDrawer (int jump)
int	current (int argc, char **argv)
int	development_tools (int argc, char **argv)
btVector3	diff (-.12, 0, 0)
void	discardKinectFrame ()
void	findBoundary1 (pcl::PointCloud< pcl::PointXYZRGB > &in, pcl::PointCloud< pcl::PointXYZRGB > &edges, pcl::PointCloud< pcl::PointXYZRGB > &other, std::vector< int > &edge_indices)
void	findBoundary2 (pcl::PointCloud< pcl::PointXYZRGB > &in, pcl::PointCloud< pcl::PointXYZRGB > &edges, pcl::PointCloud< pcl::PointXYZRGB > &other, std::vector< int > &edge_indices)
int	fotopourReadyPopcorn_ (double base_x, double base_y, double base_oz, double base_ow, double height_, double angle, bool do_exit=false)
void	getBigBowlOut ()
tf::Stamped< tf::Pose >	getBigBowlPose (const btVector3 &search)
int	getBowlFromDrawer (int jump)
bool	getCircle (const btVector3 min, const btVector3 max, btVector3 &center, double &radius)
bool	getCircleFromCloud (pcl::PointCloud< pcl::PointXYZRGB > &pot_cyl_cloud, btVector3 &center, double &radius)
	get circle2d in xy plane within searchspace min->max, from pointcloud in parameter
bool	getCirclesFromCloud (pcl::PointCloud< pcl::PointXYZRGB > &pot_cyl_cloud, double radius_goal, double radius_tolerance, std::vector< btVector3 > &center, std::vector< double > &radius, std::vector< int > &numinliers, size_t iterations=5)
void	getLidAndBowl (int jump)
bool	getLidPose (tf::Stamped< tf::Pose > &lid, btVector3 min=btVector3(-2.134, 2.625, 0.955), btVector3 max=btVector3(-1.663, 3.215, 0.972))
void	getPointsInBox (pcl::PointCloud< pcl::PointXYZRGB >::Ptr cloud, btVector3 min, btVector3 max, pcl::PointCloud< pcl::PointXYZRGB >::Ptr inBox)
void	getPotOut (int jump)
bool	getPotPose (tf::Stamped< tf::Pose > &potPose, tf::Stamped< tf::Pose > &leftpreg, tf::Stamped< tf::Pose > &leftg, tf::Stamped< tf::Pose > &rightpreg, tf::Stamped< tf::Pose > &rightg, bool via_lid=true)
	pot detector based on pcl
bool	graspBigBowl (const tf::Stamped< tf::Pose > &bowl, bool grasp=true)
int	openFavouriteDrawer (int jump)
int	pourReadyPopcorn_ (double base_x, double base_y, double base_oz, double base_ow, double height_=0.0)
int	pourReadyPopcornTable (int jump)
void	printPose (const char title[], tf::Pose pose)
void	printPose (const char title[], tf::Stamped< tf::Pose > pose)
void	printPoseSimple (tf::Stamped< tf::Pose > pose)
void	pubCloud (pcl::PointCloud< pcl::PointXYZRGB >::Ptr cloud)
void	pubMarker (tf::Stamped< tf::Pose > pose, double radius)
void	putBigBowlOntoIsland ()
void	putBigBowlOntoIsland_heater ()
void	putBigBowlOntoTable (btVector3 search, double base_x, double base_y, double base_oz, double base_ow)
void	returnPotToHeater ()
void	returnPotToSink ()
void	salt_it (btVector3 saltloc, double base_x=-1.087042+.1, double base_y=1.707556-.025, double base_oz=0.998003, double base_ow=-0.063155)
void	shake_it ()
void	takepot (int jump)
Variables
ros::Publisher	cloud_pub
bool	cloud_pub_initialized = false
tf::Stamped< tf::Pose >	hack_l
tf::Stamped< tf::Pose >	hack_r
int	marker_id = 1
ros::Publisher	perc_announcer
bool	perc_init = false

Function Documentation

void announce	(	const std::string	text,
		const tf::Stamped< tf::Pose > &	poseStamped
	)

Definition at line 54 of file Current.cpp.

void announce_box	(	const std::string	text,
		const btVector3	min,
		const btVector3	max
	)

Definition at line 115 of file Current.cpp.

void calcBigBowlGraspPoses	(	const tf::Stamped< tf::Pose > &	bowl,
		tf::Stamped< tf::Pose > &	l_pregrasp,
		tf::Stamped< tf::Pose > &	l_grasp,
		tf::Stamped< tf::Pose > &	r_pregrasp,
		tf::Stamped< tf::Pose > &	r_grasp
	)

Definition at line 3423 of file Current.cpp.

void calcPotGraspPoints	(	tf::Stamped< tf::Pose >	det,
		tf::Stamped< tf::Pose > &	leftpreg,
		tf::Stamped< tf::Pose > &	leftg,
		tf::Stamped< tf::Pose > &	rightpreg,
		tf::Stamped< tf::Pose > &	rightg
	)

Definition at line 902 of file Current.cpp.

int closeDrawer ( int jump )

Definition at line 3650 of file Current.cpp.

int current	(	int	argc,
		char **	argv
	)

bowl detection test?

Definition at line 4008 of file Current.cpp.

int development_tools	(	int	argc,
		char **	argv
	)

Definition at line 212 of file Current.cpp.

btVector3 diff	(	-.	12,
		0	,
		0
	)

void discardKinectFrame ( )

Definition at line 546 of file Current.cpp.

void findBoundary1	(	pcl::PointCloud< pcl::PointXYZRGB > &	in,
		pcl::PointCloud< pcl::PointXYZRGB > &	edges,
		pcl::PointCloud< pcl::PointXYZRGB > &	other,
		std::vector< int > &	edge_indices
	)

Definition at line 1207 of file Current.cpp.

void findBoundary2	(	pcl::PointCloud< pcl::PointXYZRGB > &	in,
		pcl::PointCloud< pcl::PointXYZRGB > &	edges,
		pcl::PointCloud< pcl::PointXYZRGB > &	other,
		std::vector< int > &	edge_indices
	)

Definition at line 1257 of file Current.cpp.

int fotopourReadyPopcorn_	(	double	base_x,
		double	base_y,
		double	base_oz,
		double	base_ow,
		double	height_,
		double	angle,
		bool	do_exit = `false`
	)

pour out the popcorn

Definition at line 2790 of file Current.cpp.

void getBigBowlOut ( )

Definition at line 3559 of file Current.cpp.

tf::Stamped< tf::Pose > getBigBowlPose ( const btVector3 & search )

Definition at line 3371 of file Current.cpp.

int getBowlFromDrawer ( int jump )

Definition at line 3642 of file Current.cpp.

bool getCircle	(	const btVector3	min,
		const btVector3	max,
		btVector3 &	center,
		double &	radius
	)

Definition at line 592 of file Current.cpp.

bool getCircleFromCloud	(	pcl::PointCloud< pcl::PointXYZRGB > &	pot_cyl_cloud,
		btVector3 &	center,
		double &	radius
	)

get circle2d in xy plane within searchspace min->max, from pointcloud in parameter

Definition at line 707 of file Current.cpp.

bool getCirclesFromCloud	(	pcl::PointCloud< pcl::PointXYZRGB > &	pot_cyl_cloud,
		double	radius_goal,
		double	radius_tolerance,
		std::vector< btVector3 > &	center,
		std::vector< double > &	radius,
		std::vector< int > &	numinliers,
		size_t	iterations = `5`
	)

Definition at line 768 of file Current.cpp.

void getLidAndBowl ( int jump )

Definition at line 2244 of file Current.cpp.

bool getLidPose	(	tf::Stamped< tf::Pose > &	lid,
		btVector3	min = `btVector3(-2.134, 2.625,0.955)`,
		btVector3	max = `btVector3(-1.663, 3.215,0.972)`
	)

Definition at line 863 of file Current.cpp.

void getPointsInBox	(	pcl::PointCloud< pcl::PointXYZRGB >::Ptr	cloud,
		btVector3	min,
		btVector3	max,
		pcl::PointCloud< pcl::PointXYZRGB >::Ptr	inBox
	)

Definition at line 554 of file Current.cpp.

void getPotOut ( int jump )

Definition at line 1633 of file Current.cpp.

bool getPotPose	(	tf::Stamped< tf::Pose > &	potPose,
		tf::Stamped< tf::Pose > &	leftpreg,
		tf::Stamped< tf::Pose > &	leftg,
		tf::Stamped< tf::Pose > &	rightpreg,
		tf::Stamped< tf::Pose > &	rightg,
		bool	via_lid = `true`
	)

pot detector based on pcl

find circle from pot base cylinder

calc grasp poses

Definition at line 945 of file Current.cpp.

bool graspBigBowl	(	const tf::Stamped< tf::Pose > &	bowl,
		bool	grasp = `true`
	)

Definition at line 3446 of file Current.cpp.

int openFavouriteDrawer ( int jump )

Definition at line 3575 of file Current.cpp.

int pourReadyPopcorn_	(	double	base_x,
		double	base_y,
		double	base_oz,
		double	base_ow,
		double	height_ = `0.0`
	)

pour out the popcorn

Definition at line 2721 of file Current.cpp.

int pourReadyPopcornTable ( int jump )

Definition at line 3969 of file Current.cpp.

void printPose	(	const char	title[],
		tf::Pose	pose
	)

Definition at line 2563 of file Current.cpp.

void printPose	(	const char	title[],
		tf::Stamped< tf::Pose >	pose
	)

Definition at line 199 of file Current.cpp.

void printPoseSimple ( tf::Stamped< tf::Pose > pose )

Definition at line 205 of file Current.cpp.

void pubCloud ( pcl::PointCloud< pcl::PointXYZRGB >::Ptr cloud )

Definition at line 174 of file Current.cpp.

void pubMarker	(	tf::Stamped< tf::Pose >	pose,
		double	radius
	)

Definition at line 445 of file Current.cpp.

void putBigBowlOntoIsland ( )

Definition at line 3704 of file Current.cpp.

void putBigBowlOntoIsland_heater ( )

Definition at line 3739 of file Current.cpp.

void putBigBowlOntoTable	(	btVector3	search,
		double	base_x,
		double	base_y,
		double	base_oz,
		double	base_ow
	)

Definition at line 3721 of file Current.cpp.

void returnPotToHeater ( )

Definition at line 2866 of file Current.cpp.

void returnPotToSink ( )

Definition at line 2906 of file Current.cpp.

void salt_it	(	btVector3	saltloc,
		double	base_x = `-1.087042 + .1`,
		double	base_y = `1.707556 - .025`,
		double	base_oz = `0.998003`,
		double	base_ow = `-0.063155`
	)