#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>
Go to the source code of this file.
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.
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.
Definition at line 1343 of file ias_drawer_executive.cpp.
Definition at line 1345 of file ias_drawer_executive.cpp.