#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/Perception3d.h>
#include <ias_drawer_executive/Pressure.h>
#include <ias_drawer_executive/Poses.h>
#include <ias_drawer_executive/RobotArm.h>
#include <ias_drawer_executive/AverageTF.h>
#include <ias_drawer_executive/Torso.h>
#include <ias_drawer_executive/Head.h>
#include <ias_drawer_executive/OperateHandleController.h>
#include <ias_drawer_executive/yamlWriter.h>
#include <boost/thread.hpp>
#include <actionlib/client/simple_client_goal_state.h>
#include <visualization_msgs/Marker.h>
#include <ias_drawer_executive/DemoScripts.h>
#include <articulation_pr2/ArticulatedObjectAction.h>
#include <actionlib/client/simple_action_client.h>
#include <pcl/ros/conversions.h>
#include <pcl_ros/point_cloud.h>
#include <pcl_ros/transforms.h>
#include <pcl/kdtree/kdtree_flann.h>
#include "rosbag/bag.h"
#include "rosbag/query.h"
#include "rosbag/view.h"
#include <boost/foreach.hpp>
#include <limits>
#include <stdio.h>
#include <ctype.h>

Include dependency graph for ias_drawer_executive.cpp:

Typedefs
typedef actionlib::SimpleActionClient < articulation_pr2::ArticulatedObjectAction >	Client
Functions
void	addState (double a, double b, double *tar, int num=7)
void	addStateDiscounted (double a, double b, double discount, double *tar, int num=7)
int	articulate (const string &filename, double desired_length, const string &kinect_rgb_optical_frame="/openni_rgb_optical_frame", const string &arm="r", const string &kinect_cloud_topic="/kinect/cloud_throttled", bool model=true)
void	calc_better_base_pose (articulation_pr2::ArticulatedObjectResultConstPtr result, double desired_length)
void	cart_err_monit (int side)
void	copyState (double src, double tar, int num=7)
int	current (int argc, char **argv)
void	findBothArms (tf::Stamped< tf::Pose > leftArm, tf::Stamped< tf::Pose > rightArm)
pcl::KdTree< pcl::PointXYZRGB > ::Ptr	getTree (pcl::PointCloud< pcl::PointXYZRGB > &cloudb)
int	main (int argc, char **argv)
void	mangle (const char filename[])
int	measures (int argc, char **argv)
double	pointDist (pcl::PointXYZRGB &p, pcl::KdTree< pcl::PointXYZRGB >::Ptr &tree)
void	printPose (const char title[], tf::Stamped< tf::Pose > pose)
void	printPose (const char title[], tf::Pose pose)
void	pubPose (tf::Stamped< tf::Pose > &t)
void	pubPose (tf::Stamped< tf::Pose > &t, ros::Publisher vis_pub)
double	reachable_distance (tf::Stamped< tf::Pose > secondlast, tf::Stamped< tf::Pose > last)
void	releaseWhenPulled (int side)
std::string	removeWhitespace (const std::string in)
pcl::PointCloud< pcl::PointXYZRGB >	substractPC (pcl::PointCloud< pcl::PointXYZRGB > clouda, pcl::PointCloud< pcl::PointXYZRGB > cloudb, double distance_threshold=0.01)
pcl::PointCloud< pcl::PointXYZRGB >	substractPC (pcl::PointCloud< pcl::PointXYZRGB > clouda, pcl::KdTree< pcl::PointXYZRGB >::Ptr &tree, double distance_threshold=0.01)
tf::Stamped< tf::Pose >	toPose (double x, double y, double z, double ox, double oy, double oz, double ow, const char fixed_frame[])
tf::Stamped< tf::Pose >	toPose (const char text[], const char fixed_frame[])
tf::Stamped< tf::Pose >	turnBowlStraight (int currside, tf::Stamped< tf::Pose > &bowlPose)
tf::Stamped< tf::Pose >	turnToSide (int side, tf::Stamped< tf::Pose > toolPose, tf::Stamped< tf::Pose > bowlPose)
Variables
bool	have_vis_pub = false
ros::NodeHandle *	node_handle_ = 0
ros::Publisher	vis_pub_

Typedef Documentation

typedef actionlib::SimpleActionClient<articulation_pr2::ArticulatedObjectAction> Client

Definition at line 855 of file ias_drawer_executive.cpp.

Function Documentation

void addState	(	double *	a,
		double *	b,
		double *	tar,
		int	num = `7`
	)

Definition at line 1298 of file ias_drawer_executive.cpp.

void addStateDiscounted	(	double *	a,
		double *	b,
		double	discount,
		double *	tar,
		int	num = `7`
	)

Definition at line 1304 of file ias_drawer_executive.cpp.

int articulate	(	const string &	filename,
		double	desired_length,
		const string &	kinect_rgb_optical_frame = `"/openni_rgb_optical_frame"`,
		const string &	arm = `"r"`,
		const string &	kinect_cloud_topic = `"/kinect/cloud_throttled"`,
		bool	model = `true`
	)

aim the head at the middle of the trajectory

Definition at line 997 of file ias_drawer_executive.cpp.

void calc_better_base_pose	(	articulation_pr2::ArticulatedObjectResultConstPtr	result,
		double	desired_length
	)

Definition at line 893 of file ias_drawer_executive.cpp.

void cart_err_monit ( int side )

Definition at line 1312 of file ias_drawer_executive.cpp.

void copyState	(	double *	src,
		double *	tar,
		int	num = `7`
	)

Definition at line 1292 of file ias_drawer_executive.cpp.

int current	(	int	argc,
		char **	argv
	)

bowl detection test?

Definition at line 4008 of file Current.cpp.

void findBothArms	(	tf::Stamped< tf::Pose >	leftArm,
		tf::Stamped< tf::Pose >	rightArm
	)

Definition at line 1327 of file ias_drawer_executive.cpp.

pcl::KdTree<pcl::PointXYZRGB>::Ptr getTree ( pcl::PointCloud< pcl::PointXYZRGB > & cloudb )

Definition at line 143 of file ias_drawer_executive.cpp.

int main	(	int	argc,
		char **	argv
	)

drive in map x y oz ow

put gripper to some pose in map

put gripper to some pose in base

put gripper to some pose in frame

get plate from open drawer

drive relative to base

print plate pose

get bottle out of open fridge

move both grippers uniformly in direction x y z in base

look at a pos in frame

open both grippers at the same time

print bottle pose

doesnt work

put down the bottle at the island

bring plate to the island next to the pancake heater and present it so that the pancake can be pushed on top

deprecated: get the lego piece, now implemented in another package

put plate or silverware into carrying pose

serve pancake / bring silverware over to the pillar table

open drawer no. 4 in one pull, for video comparison with haptic approach

open handle assuming the right gripper surrounds it already, it takes the grippers pose and makes a haptic approach + opens

get silverware, bring to pillar like plate with -35

open drawer for plate picking with fixed map pose -> a little quicker

get plate back from island

arm->startTrajectory(arm->goalTraj(pose));

open handle at map coordinate, expects base to be put more or less frontal to drawer surface and almost in reach

pick a plate from map coords

in RPY [-0.850, 1.543, 0.732]

Definition at line 1519 of file ias_drawer_executive.cpp.

void mangle ( const char filename[] )

Definition at line 388 of file ias_drawer_executive.cpp.

int measures	(	int	argc,
		char **	argv
	)

Definition at line 57 of file Measures.cpp.

double pointDist	(	pcl::PointXYZRGB &	p,
		pcl::KdTree< pcl::PointXYZRGB >::Ptr &	tree
	)

Definition at line 193 of file ias_drawer_executive.cpp.

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

Definition at line 82 of file ias_drawer_executive.cpp.

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

Definition at line 88 of file ias_drawer_executive.cpp.

void pubPose ( tf::Stamped< tf::Pose > & t )

Definition at line 1347 of file ias_drawer_executive.cpp.

void pubPose	(	tf::Stamped< tf::Pose > &	t,
		ros::Publisher	vis_pub
	)

Definition at line 1378 of file ias_drawer_executive.cpp.

double reachable_distance	(	tf::Stamped< tf::Pose >	secondlast,
		tf::Stamped< tf::Pose >	last
	)

Definition at line 857 of file ias_drawer_executive.cpp.

void releaseWhenPulled ( int side )

Definition at line 1483 of file ias_drawer_executive.cpp.

std::string removeWhitespace ( const std::string in )

Definition at line 377 of file ias_drawer_executive.cpp.

pcl::PointCloud<pcl::PointXYZRGB> substractPC	(	pcl::PointCloud< pcl::PointXYZRGB >	clouda,
		pcl::PointCloud< pcl::PointXYZRGB >	cloudb,
		double	distance_threshold = `0.01`
	)

Definition at line 94 of file ias_drawer_executive.cpp.

pcl::PointCloud<pcl::PointXYZRGB> substractPC	(	pcl::PointCloud< pcl::PointXYZRGB >	clouda,
		pcl::KdTree< pcl::PointXYZRGB >::Ptr &	tree,
		double	distance_threshold = `0.01`
	)

Definition at line 152 of file ias_drawer_executive.cpp.

tf::Stamped<tf::Pose> toPose	(	double	x,
		double	y,
		double	z,
		double	ox,
		double	oy,
		double	oz,
		double	ow,
		const char	fixed_frame[]
	)

Definition at line 1275 of file ias_drawer_executive.cpp.

tf::Stamped<tf::Pose> toPose	(	const char	text[],
		const char	fixed_frame[]
	)

Definition at line 1285 of file ias_drawer_executive.cpp.

tf::Stamped<tf::Pose> turnBowlStraight	(	int	currside,
		tf::Stamped< tf::Pose > &	bowlPose
	)

Definition at line 1443 of file ias_drawer_executive.cpp.

tf::Stamped<tf::Pose> turnToSide	(	int	side,
		tf::Stamped< tf::Pose >	toolPose,
		tf::Stamped< tf::Pose >	bowlPose
	)

Definition at line 1400 of file ias_drawer_executive.cpp.

Variable Documentation

bool have_vis_pub = false

Definition at line 1344 of file ias_drawer_executive.cpp.

ros::NodeHandle* node_handle_ = 0

Definition at line 1343 of file ias_drawer_executive.cpp.

ros::Publisher vis_pub_

Definition at line 1345 of file ias_drawer_executive.cpp.

Typedefs

Functions

Variables

Typedef Documentation

Function Documentation

Variable Documentation