ias_drawer_executive: ias_drawer_executive: ArticulationModelling.cpp Source File

00001 
00002 
00003 
00004 #include <ros/ros.h>
00005 #include <tf/transform_listener.h>
00006 
00007 #include <articulation_pr2/ArticulatedObjectAction.h>
00008 #include <actionlib/client/simple_action_client.h>
00009 
00010 
00011 #include <ias_drawer_executive/ArticulationModelling.h>
00012 #include <ias_drawer_executive/Gripper.h>
00013 #include <ias_drawer_executive/RobotArm.h>
00014 
00015 typedef actionlib::SimpleActionClient<articulation_pr2::ArticulatedObjectAction> Client;
00016 
00017 int articulate()
00018 {
00019     Client client("articulate_object", true); // true -> don't need ros::spin()
00020     client.waitForServer();
00021     {
00022         articulation_pr2::ArticulatedObjectGoal goal;
00023         goal.timeout = ros::Duration(20.00);
00024         goal.initial_pulling_direction.header.frame_id = "torso_lift_link";
00025         goal.initial_pulling_direction.vector.x = -1;// pull towards robot
00026         goal.desired_velocity = 0.04;
00027 
00028         // Fill in goal here
00029         client.sendGoal(goal);
00030         client.waitForResult(ros::Duration(5.0));
00031         while (client.getState() != actionlib::SimpleClientGoalState::SUCCEEDED)
00032         {
00033             //if (client.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
00034             printf("Current State: %s\n", client.getState().toString().c_str());
00035             ros::Duration(0.5).sleep();
00036         }
00037 
00038         articulation_pr2::ArticulatedObjectResultConstPtr result = client.getResult();
00039 
00040         ROS_INFO("FRAME : %s", result->posterior_model.track.header.frame_id.c_str());
00041         //std::vector<geometry_msgs::Pose>::iterator pose_it;
00042         //for (pose_it = result->posterior_model.track.pose_projected.begin(); pose_it != result->posterior_model.track.pose_projected.end(); ++pose_it) {
00043         tf::Stamped<tf::Pose> lastPose;
00044         lastPose.setOrigin(btVector3(0,0,10000));
00045         for (int pos_i = 0; pos_i < result->posterior_model.track.pose_resampled.size(); ++pos_i)
00046         {
00047             geometry_msgs::Pose act = result->posterior_model.track.pose_resampled[pos_i];
00048             ROS_INFO("%f %f %f  %f %f %f %f", act.position.x, act.position.y, act.position.z,
00049                      act.orientation.x,  act.orientation.y,  act.orientation.z,  act.orientation.w);
00050             tf::Pose actTF;
00051             tf::poseMsgToTF(act,actTF);
00052             tf::Stamped<tf::Pose> actSTF;
00053             actSTF.setOrigin(actTF.getOrigin());
00054             actSTF.setRotation(actTF.getRotation());
00055             actSTF.frame_id_ = result->posterior_model.track.header.frame_id.c_str();
00056             if (pos_i == 0)
00057                 Gripper::getInstance(0)->open();
00058             if (((actSTF.getOrigin() - lastPose.getOrigin()).length() >= 0.05) || (pos_i == result->posterior_model.track.pose_resampled.size() - 1))
00059             {
00060                 RobotArm::getInstance(0)->universal_move_toolframe_ik_pose(actSTF);
00061                 lastPose = actSTF;
00062             }
00063 
00064             if (pos_i == 0)
00065                 Gripper::getInstance(0)->close();
00066         }
00067 
00068         for (int pos_i = result->posterior_model.track.pose_resampled.size() - 1; pos_i >= 0 ; --pos_i)
00069         {
00070             geometry_msgs::Pose act = result->posterior_model.track.pose_resampled[pos_i];
00071             ROS_INFO("%f %f %f  %f %f %f %f", act.position.x, act.position.y, act.position.z,
00072                      act.orientation.x,  act.orientation.y,  act.orientation.z,  act.orientation.w);
00073             tf::Pose actTF;
00074             tf::poseMsgToTF(act,actTF);
00075             tf::Stamped<tf::Pose> actSTF;
00076             actSTF.setOrigin(actTF.getOrigin());
00077             actSTF.setRotation(actTF.getRotation());
00078             actSTF.frame_id_ = result->posterior_model.track.header.frame_id.c_str();
00079             if (((actSTF.getOrigin() - lastPose.getOrigin()).length() >= 0.05) || (pos_i == result->posterior_model.track.pose_resampled.size() - 1))
00080             {
00081                 RobotArm::getInstance(0)->universal_move_toolframe_ik_pose(actSTF);
00082                 lastPose = actSTF;
00083             }
00084 
00085             if (pos_i == 0)
00086                 Gripper::getInstance(0)->open();
00087         }
00088 
00089 
00090 
00091         printf("Yay! The dishes are now clean");
00092     }
00093     if (0)
00094     {
00095         articulation_pr2::ArticulatedObjectGoal goal;
00096         goal.timeout = ros::Duration(20.00);
00097         goal.initial_pulling_direction.header.frame_id = "torso_lift_link";
00098         goal.initial_pulling_direction.vector.x = 1;// pull towards robot
00099         goal.desired_velocity = 0.04;
00100 
00101         // Fill in goal here
00102         client.sendGoal(goal);
00103         client.waitForResult(ros::Duration(5.0));
00104         while (client.getState() != actionlib::SimpleClientGoalState::SUCCEEDED)
00105         {
00106             //if (client.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
00107             printf("Current State: %s\n", client.getState().toString().c_str());
00108             ros::Duration(0.5).sleep();
00109         }
00110         printf("Yay! The dishes are now clean");
00111     }
00112 
00113     return 0;
00114 }