SegmentFurnitureInteractive.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2010, Thomas Ruehr <ruehr@cs.tum.edu>
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 #include <ros/ros.h>
00031 #include <actionlib/client/simple_action_client.h>
00032 #include <actionlib/client/terminal_state.h>
00033 #include <ias_drawer_executive/OpenContainerAction.h>
00034 #include <ias_drawer_executive/CloseContainerAction.h>
00035 #include <ias_drawer_executive/OperateHandleController.h>
00036 #include <ias_drawer_executive/RobotDriver.h>
00037 #include <ias_drawer_executive/RobotArm.h>
00038 #include <ias_drawer_executive/Torso.h>
00039 #include <ias_drawer_executive/Perception3d.h>
00040 
00041 //for trajectory publishing
00042 #include <visualization_msgs/MarkerArray.h>
00043 
00044 bool getParam(std::string param_name)
00045 {
00046   ros::NodeHandle nh;
00047   bool segment;
00048   nh.getParam(param_name, segment);
00049   return segment;
00050 }
00051 
00052 void setParam(std::string param_name, bool segment)
00053 {
00054   ros::NodeHandle nh;
00055   nh.setParam(param_name, segment);
00056 }
00057 
00058 
00059 void test_open(int arm_, double x, double y, double z, double ox, double oy, double oz, double ow)
00060 {
00061   //publish trajectory as arrow marker array
00062   ros::Publisher trajectory_marker_array_publisher, trajectory_marker_publisher;
00063   ros::NodeHandle nh;
00064   //advertise as latched
00065   trajectory_marker_publisher=nh.advertise<visualization_msgs::Marker>("trajectory_marker", 100, true);
00066   trajectory_marker_array_publisher = nh.advertise<visualization_msgs::MarkerArray>("trajectory_marker_array", 100, true);
00067 
00068     OperateHandleController::plateTuckPose();
00069 
00070 
00071     //boost::thread tors(boost::bind(&Torso::pos,Torso::getInstance(),(z < 0.5) ? 0.1f : 0.29f));
00072     //   Torso::down();
00073 
00074     //go to initial scan pose
00075     RobotDriver::getInstance()->moveBaseP(x -0.5,y + (arm_  ? -.2 : .2),0,1);
00076     // sleep(10);
00077     // //this is to get the first cloud into segment_differences node
00078     // setParam("/segment_difference_interactive/take_first_cloud", true);
00079     // while (getParam("/segment_difference_interactive/take_first_cloud"))
00080     //   {
00081     //  sleep(1);
00082     //   }
00083 
00084     RobotDriver::getInstance()->moveBaseP(x -.6,y + (arm_  ? -.2 : .2),0,1);
00085     //tors.join();
00086 
00087   //open the drawer
00088     actionlib::SimpleActionClient<ias_drawer_executive::OpenContainerAction> ac("open_container_action", true);
00089 
00090     ROS_INFO("Waiting for action server to start.");
00091     // wait for the action server to start
00092     std::cerr << "before action" << std::endl;
00093     ac.waitForServer(); //will wait for infinite time
00094     std::cerr << "after action" << std::endl;
00095 
00096     ROS_INFO("Action server started, sending goal.");
00097 
00098 
00099     tf::Stamped<tf::Pose> handleHint;
00100     handleHint.setOrigin(btVector3( x, y, z));
00101     handleHint.frame_id_ = "/map";
00102     tf::Stamped<tf::Pose> handlePos = Perception3d::getHandlePoseFromLaser(handleHint);
00103     //handlePos = OperateHandleController::getHandlePoseFromLaser(handleHint);
00104     handlePos.setRotation(btQuaternion(ox, oy, oz, ow));
00105 
00106     // send a goal to the action
00107     ias_drawer_executive::OpenContainerGoal goal;
00108     goal.arm = arm_;
00109     goal.position.header.frame_id = "map";
00110     goal.position.pose.position.x = handlePos.getOrigin().x();
00111     goal.position.pose.position.y = handlePos.getOrigin().y();
00112     goal.position.pose.position.z = handlePos.getOrigin().z();
00113     goal.position.pose.orientation.x = handlePos.getRotation().x();
00114     goal.position.pose.orientation.y = handlePos.getRotation().y();
00115     goal.position.pose.orientation.z = handlePos.getRotation().z();
00116     goal.position.pose.orientation.w = handlePos.getRotation().w();
00117 
00118     ac.sendGoal(goal);
00119 
00120     //wait if openning of drawer succeeded
00121     ROS_INFO("waiting for openning of drawer");
00122     bool finished_before_timeout = ac.waitForResult(ros::Duration(1000.0));
00123 
00124 
00125     //move base backward
00126     ROS_INFO("moving base backward");
00127     double target[4];
00128     target[0] = -0.1;
00129     target[1] = -0.1;
00130     target[2] = 0;
00131     target[3] = 1;
00132     ROS_INFO("POSE IN BASE %f %f %f", target[0], target[1], target[2]);
00133     RobotDriver::getInstance()->driveInOdom(target, 1);
00134 
00135     //get arm pose
00136     tf::Stamped<tf::Pose> p0 = RobotArm::getInstance(1)->getToolPose("base_link");
00137 
00138     //park the arm to the side
00139     ROS_INFO("parking the arm");
00140     OperateHandleController::plateTuckPose();
00141 
00142     //got to the same initial position and scan
00143     RobotDriver::getInstance()->moveBaseP(x -1.0,y + (arm_  ? -.2 : .2),0,1);
00144     //sleep(11);
00145 
00146     //call "segment differences" service
00147     ROS_INFO("calling the segment-difference service");
00148     setParam("/segment_difference_interactive/segment", true);
00149     // while (getParam("/segment_difference_interactive/segment"))
00150     // {
00151     //   sleep(1);
00152     // }
00153 
00154     //publish the marker
00155     visualization_msgs::MarkerArray marker;
00156     boost::shared_ptr<const ias_drawer_executive::OpenContainerResult> result = ac.getResult();
00157     ROS_INFO("Trajectory: %i Poses", (unsigned int) result->trajectory.poses.size() );
00158     marker.markers.resize(result->trajectory.poses.size());
00159     for (size_t j=0; j < result->trajectory.poses.size(); ++j)
00160       {
00161             ROS_INFO(" Pos %f %f %f, %f %f %f %f", result->trajectory.poses[j].position.x,result->trajectory.poses[j].position.y,result->trajectory.poses[j].position.z,
00162                 result->trajectory.poses[j].orientation.x,result->trajectory.poses[j].orientation.y,result->trajectory.poses[j].orientation.z,result->trajectory.poses[j].orientation.w);
00163             marker.markers[j].header.frame_id = result->trajectory.header.frame_id;
00164         marker.markers[j].header.stamp=ros::Time::now();
00165         marker.markers[j].type = visualization_msgs::Marker::ARROW;
00166         marker.markers[j].action = visualization_msgs::Marker::ADD;
00167         marker.markers[j].ns="trajectory";
00168         marker.markers[j].pose= result->trajectory.poses[j];
00169         marker.markers[j].scale.x=0.1;
00170         marker.markers[j].scale.y=1.0;
00171         marker.markers[j].id = j;
00172         marker.markers[j].color.r = 0.0f;
00173         marker.markers[j].color.g = 1.0f;
00174         marker.markers[j].color.b = 0.0f;
00175         marker.markers[j].color.a = 1.0f;
00176         marker.markers[j].lifetime = ros::Duration::Duration();
00177       }
00178     ROS_INFO("publishing marker array for goto poses");
00179     trajectory_marker_array_publisher.publish(marker);
00180 
00181     //move arm to pose
00182     //RobotArm::getInstance(0)->move_toolframe_ik_pose(p0);
00183     //int handle = OperateHandleController::operateHandle(0,p0);
00184 
00185     //move base forwward
00186     // ROS_INFO("moving base forward");
00187     // target[0] = -target[0];
00188     // ROS_INFO("POSE IN BASE %f %f %f", target[0], target[1], target[2]);
00189     // RobotDriver::getInstance()->driveInOdom(target, 1);
00190 
00191     //close the drawer
00192     ROS_INFO("closing the drawer");
00193     if (finished_before_timeout)
00194     {
00195         actionlib::SimpleClientGoalState state = ac.getState();
00196         ROS_INFO("Action finished: %s",state.toString().c_str());
00197         //boost::shared_ptr<const ias_drawer_executive::OpenContainerResult> result = ac.getResult();
00198         //ias_drawer_executive::OpenContainerResult result = ac.getResult();
00199         ROS_INFO("sucess %i", result->success);
00200         ROS_INFO("Trajectory: %i Poses", (unsigned int) result->trajectory.poses.size() );
00201         for (size_t j=0; j < result->trajectory.poses.size(); ++j)
00202         {
00203             ROS_INFO(" Pos %f %f %f, %f %f %f %f", result->trajectory.poses[j].position.x,result->trajectory.poses[j].position.y,result->trajectory.poses[j].position.z,
00204                      result->trajectory.poses[j].orientation.x,result->trajectory.poses[j].orientation.y,result->trajectory.poses[j].orientation.z,result->trajectory.poses[j].orientation.w);
00205         }
00206 
00207         ROS_INFO("TRYING NOW TO CLOSE");
00208 
00209         actionlib::SimpleActionClient<ias_drawer_executive::CloseContainerAction> ac_close("close_container_action", true);
00210 
00211         ROS_INFO("Waiting for action server to start.");
00212         // wait for the action server to start
00213         ac_close.waitForServer(); //will wait for infinite time
00214 
00215         ROS_INFO("Action server started, sending goal.");
00216 
00217         ias_drawer_executive::CloseContainerGoal closeGoal;
00218         closeGoal.arm = arm_;
00219         closeGoal.opening_trajectory = result->trajectory;
00220 
00221         ac_close.sendGoal(closeGoal);
00222 
00223         bool finished_before_timeout = ac_close.waitForResult(ros::Duration(1000.0));
00224 
00225         if (finished_before_timeout)
00226         {
00227             actionlib::SimpleClientGoalState state = ac_close.getState();
00228             ROS_INFO("Action finished: %s",state.toString().c_str());
00229         }
00230 
00231         OperateHandleController::plateTuckPose();
00232     }
00233     else
00234         ROS_INFO("Action did not finish before the time out.");
00235 }
00236 
00237 geometry_msgs::Pose *getPose(double x, double y, double z, double ox, double oy, double oz, double ow)
00238 {
00239         geometry_msgs::Pose *act = new geometry_msgs::Pose();
00240         act->position.x = x;
00241         act->position.y = y;
00242         act->position.z = z;
00243         act->orientation.x = ox;
00244         act->orientation.y = oy;
00245         act->orientation.z = oz;
00246         act->orientation.w = ow;
00247         return act;
00248 }
00249 
00250 
00251 void closeonly() {
00252    actionlib::SimpleActionClient<ias_drawer_executive::CloseContainerAction> ac_close("close_container_action", true);
00253 
00254      ROS_INFO("Waiting for action server to start.");
00255         // wait for the action server to start
00256         ac_close.waitForServer(); //will wait for infinite time
00257 
00258         ROS_INFO("Action server started, sending goal.");
00259 
00260         ias_drawer_executive::CloseContainerGoal closeGoal;
00261         closeGoal.arm = 0;
00262 
00263         closeGoal.opening_trajectory.poses.push_back(*getPose( 0.346634, 1.118203, 0.763804, -0.708801, -0.055698, 0.039966, 0.702069));
00264         closeGoal.opening_trajectory.poses.push_back(*getPose( 0.509877, 1.121905, 0.761193, -0.705817, -0.039952, 0.034062, 0.706447));
00265         closeGoal.opening_trajectory.poses.push_back(*getPose( 0.710390, 1.128074, 0.761932, -0.705345, -0.038818, 0.035855, 0.706892));
00266 
00267         ac_close.sendGoal(closeGoal);
00268 
00269         bool finished_before_timeout = ac_close.waitForResult(ros::Duration(1000.0));
00270 
00271         if (finished_before_timeout)
00272         {
00273             actionlib::SimpleClientGoalState state = ac_close.getState();
00274             ROS_INFO("Action finished: %s",state.toString().c_str());
00275         }
00276 }
00277 
00278 void test_open(int arm, btTransform &t)
00279 {
00280     test_open(arm, t.getOrigin().x(), t.getOrigin().y(), t.getOrigin().z(), t.getRotation().x(),t.getRotation().y(),t.getRotation().z(),t.getRotation().w());
00281 }
00282 
00283 int main (int argc, char **argv)
00284 {
00285 
00286     ros::init(argc, argv, "segment_difference_interactive");
00287 
00288     //closeonly();
00289 
00290     std::vector<btTransform> hints(100);
00291     //hints.push_back(btTransform(btQuaternion(1.000, 0.008, -0.024, -0.004) ,btVector3(0.762, 2.574, 0.791)); // 00
00292 
00293     hints[0] = btTransform(btQuaternion(1.000, 0.008, -0.024, -0.004), btVector3(0.762, 2.574, 0.791)); // 0
00294     hints[10] = btTransform(btQuaternion(-0.691, -0.039, 0.040, 0.721), btVector3(0.799, 2.137, 1.320)); // 10
00295     hints[11] = btTransform(btQuaternion(-0.715, -0.017, -0.003, 0.699), btVector3(0.777, 2.126, 0.804)); // 11
00296     hints[12] = btTransform(btQuaternion(-0.741, -0.029, -0.020, 0.671), btVector3(0.777, 2.125, 0.662)); // 12
00297     hints[20] = btTransform(btQuaternion(0.025, -0.002, 0.039, 0.999), btVector3(0.798, 1.678, 0.805)); // 20
00298     hints[30] = btTransform(btQuaternion(0.712, 0.003, 0.025, 0.702), btVector3(0.629, 1.126, 0.756)); // 30
00299     hints[31] = btTransform(btQuaternion(0.712, 0.027, 0.019, 0.701), btVector3(0.627, 1.140, 0.614)); // 31
00300     hints[32] = btTransform(btQuaternion(0.718, 0.031, 0.019, 0.695), btVector3(0.623, 1.135, 0.331)); // 32
00301     hints[40] = btTransform(btQuaternion(0.698, 0.004, 0.056, 0.714) ,btVector3(0.654, 0.442, 0.766)); // 40
00302     hints[50] = btTransform(btQuaternion(0.704, -0.035, 0.027, 0.709) ,btVector3(0.705, -0.171, 0.614)); // 50
00303     hints[60] = btTransform(btQuaternion(-0.029, -0.012, 0.020, 0.999) ,btVector3(0.882, -0.520, 0.985)); // 60
00304     hints[61] = btTransform(btQuaternion(-0.688, -0.015, 0.045, 0.724) ,btVector3(0.881, -0.776, 0.437)); // 61
00305 
00306     //test_open(atoi(argv[1]),atof(argv[2]),atof(argv[3]),atof(argv[4]),atof(argv[5]),atof(argv[6]),atof(argv[7]),atof(argv[8]));
00307     test_open(0, hints[atoi(argv[1])]);
00308 
00309     // create the action client
00310     // true causes the client to spin it's own thread
00311     //exit
00312     return 0;
00313 }
00314 
00315 
00316 /*
00317 */
 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