Measures.cpp
Go to the documentation of this file.
00001 #include <ros/ros.h>
00002 
00003 #include <iostream>
00004 #include <fstream>
00005 using namespace std;
00006 
00007 #include <ias_drawer_executive/Approach.h>
00008 #include <ias_drawer_executive/RobotDriver.h>
00009 #include <ias_drawer_executive/Gripper.h>
00010 #include <ias_drawer_executive/Pressure.h>
00011 #include <ias_drawer_executive/RobotArm.h>
00012 #include <ias_drawer_executive/Torso.h>
00013 #include <ias_drawer_executive/Head.h>
00014 #include <ias_drawer_executive/OperateHandleController.h>
00015 #include <ias_drawer_executive/Keywords.h>
00016 #include <ias_drawer_executive/ObjectLocalizer.h>
00017 
00018 
00019 #include <ias_drawer_executive/Current.h>
00020 
00021 #include <pcl/common/common.h>
00022 #include <pcl/filters/extract_indices.h>
00023 #include <pcl/ModelCoefficients.h>
00024 #include <pcl/point_types.h>
00025 #include <pcl/sample_consensus/method_types.h>
00026 #include <pcl/sample_consensus/model_types.h>
00027 #include <pcl/segmentation/sac_segmentation.h>
00028 #include <pcl/features/boundary.h>
00029 #include <pcl/features/normal_3d.h>
00030 
00031 #include <pcl/ros/conversions.h>
00032 #include <pcl_ros/point_cloud.h>
00033 #include <pcl_ros/transforms.h>
00034 
00035 #include <boost/thread.hpp>
00036 
00037 #include <visualization_msgs/Marker.h>
00038 
00039 #include <map>
00040 
00041 #include <stdio.h>
00042 #include <stdarg.h>
00043 
00044 
00045 
00046 void test_func(Keywords &kp)
00047 {
00048     ROS_INFO("string %s",kp.lookup_s("blub").c_str());
00049     ROS_INFO("double %f",kp.lookup_d("blub"));
00050     ROS_INFO("vec %f %f %f",kp.lookup_v("blic").x(),kp.lookup_v("blic").y(),kp.lookup_v("blic").z());
00051 
00052     ROS_INFO("double %f",(double)kp.lookup("blub"));
00053 }
00054 
00055 //std::map<std::string, double
00056 
00057 int measures(int argc, char** argv)
00058 {
00059     if (atoi(argv[1]) == -800)
00060     {
00061         ROS_INFO("Dip");
00062         PotLocalizer *pot = new PotLocalizer();
00063 
00064         tf::Stamped<tf::Pose> pose;
00065 
00066         ObjectLocalizer::localize("bla",&pose, 1);
00067         ObjectLocalizer::localize("Pot",&pose, 1);
00068 
00069         //Keywords kp = Keywords("bla", 1).contains()("blub", "blib")("tic", 5).contains();
00070         //kp.contains();
00071         //ROS_INFO("string lookup : %s", kp.lookup_s("blub").c_str());
00072         //ROS_INFO("double lookup : %f    ", kp.lookup_d("bla"));
00073 
00074         test_func(Keywords("bla",1) ("blub","blib") ("tic", 5) ("blub",7)("blic",tf::Vector3(0,0,1)) );
00075 
00076     }
00077 
00078     if (atoi(argv[1]) == -801)
00079     {
00080         tf::Stamped<tf::Pose> rTool = RobotArm::getInstance(0)->getToolPose("/base_link");
00081 
00082         for (double forward = 0; forward < 10; forward+=.25)
00083         {
00084 
00085             tf::Stamped<tf::Pose> act = rTool;
00086             act.getOrigin() += tf::Vector3(forward,0,0);
00087 
00088             {
00089                 tf::Stamped<tf::Pose> result;
00090                 std::vector<int> arm;
00091                 std::vector<tf::Stamped<tf::Pose> > goal;
00092                 arm.push_back(0);
00093                 goal.push_back(act);
00094                 //bool RobotArm::findBaseMovement(tf::Stamped<tf::Pose> &result, std::vector<int> arm, std::vector<tf::Stamped<tf::Pose> > goal, bool drive, bool reach)
00095 
00096                 RobotArm::findBaseMovement(result, arm, goal, false, false);
00097             }
00098 
00099         }
00100 
00101     }
00102 
00103 
00104     if (atoi(argv[1]) == -802)
00105     {
00106         tf::Stamped<tf::Pose> rTool = RobotArm::getInstance(0)->getToolPose("/base_link");
00107 
00108         for (double forward = 0; forward > -10; forward-=.25)
00109         {
00110 
00111             tf::Stamped<tf::Pose> act = rTool;
00112             act.getOrigin() += tf::Vector3(forward,0,0);
00113 
00114             {
00115                 tf::Stamped<tf::Pose> result;
00116                 std::vector<int> arm;
00117                 std::vector<tf::Stamped<tf::Pose> > goal;
00118                 arm.push_back(0);
00119                 goal.push_back(act);
00120                 //bool RobotArm::findBaseMovement(tf::Stamped<tf::Pose> &result, std::vector<int> arm, std::vector<tf::Stamped<tf::Pose> > goal, bool drive, bool reach)
00121 
00122                 RobotArm::findBaseMovement(result, arm, goal, false, false);
00123             }
00124 
00125         }
00126 
00127     }
00128 
00129 
00130     // -1.77 2.15 0.88
00131 
00132     return 0;
00133 
00134 }
00135 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Defines


ias_drawer_executive
Author(s): Thomas Ruehr
autogenerated on Thu May 23 2013 09:44:57