katana_openloop_grasping: katana_openloop_grasping: pick_and_place

00001 /*
00002  * UOS-ROS packages - Robot Operating System code by the University of Osnabrück
00003  * Copyright (C) 2010  University of Osnabrück
00004  *
00005  * This program is free software; you can redistribute it and/or
00006  * modify it under the terms of the GNU General Public License
00007  * as published by the Free Software Foundation; either version 2
00008  * of the License, or (at your option) any later version.
00009  *
00010  * This program is distributed in the hope that it will be useful,
00011  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00012  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00013  * GNU General Public License for more details.
00014  *
00015  * You should have received a copy of the GNU General Public License
00016  * along with this program; if not, write to the Free Software
00017  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
00018  *
00019  * pick_and_place_demo.cpp
00020  *
00021  *  Created on: 31.01.2011
00022  *      Author: Martin Günther <mguenthe@uos.de>
00023  */
00024 
00025 #include <katana_openloop_grasping/pick_and_place_demo.h>
00026 
00027 namespace katana_openloop_grasping
00028 {
00029 
00030 PickAndPlaceDemo::PickAndPlaceDemo() :
00031   move_arm_("move_arm", true), gripper_("gripper_grasp_posture_controller", true),
00032       make_static_collision_map_("make_static_collision_map", true)
00033 {
00034   ros::NodeHandle nh;
00035 
00036   move_arm_.waitForServer();
00037   ROS_INFO("Connected to move arm action server");
00038 
00039   gripper_.waitForServer();
00040   ROS_INFO("Connected to gripper action server");
00041 
00042   // make_static_collision_map_.waitForServer();
00043   // ROS_INFO("Connected to make_static_collision_map action server");
00044 
00045   grasp_status_client_ = nh.serviceClient<object_manipulation_msgs::GraspStatus> ("gripper_grasp_status");
00046   if (!grasp_status_client_.waitForExistence(ros::Duration(10.0)))
00047   {
00048     ROS_FATAL("Could not connect to gripper grasp status service");
00049     return;
00050   }
00051   else
00052   {
00053     ROS_INFO("Connected to gripper grasp status service");
00054   }
00055 
00056 }
00057 
00058 PickAndPlaceDemo::~PickAndPlaceDemo()
00059 {
00060 }
00061 
00062 void PickAndPlaceDemo::loop()
00063 {
00064 
00065   std::vector<std::string> names(NUM_JOINTS);
00066   names[0] = "katana_motor1_pan_joint";
00067   names[1] = "katana_motor2_lift_joint";
00068   names[2] = "katana_motor3_lift_joint";
00069   names[3] = "katana_motor4_lift_joint";
00070   names[4] = "katana_motor5_wrist_roll_joint";
00071 
00072   std::vector<motion_planning_msgs::JointConstraint> joint_constraints(NUM_JOINTS);
00073 
00074   for (size_t i = 0; i < NUM_JOINTS; ++i)
00075   {
00076     joint_constraints[i].joint_name = names[i];
00077     joint_constraints[i].tolerance_below = 0.1;
00078     joint_constraints[i].tolerance_above = 0.1;
00079   }
00080 
00081   while (nh_.ok())
00082   {
00083     bool success;
00084 
00085     // TODO: To make the grasping work, we need to add a known object to the environment and attach
00086     // it to the gripper, or else disable the collision checks for the fingers
00087 
00088     ROS_INFO("Sending PRE_GRASP");
00089     success = send_gripper_action(GHPEG::PRE_GRASP);
00090     success &= query_grasp_status();
00091     if (!success)
00092       break;
00093 
00094     //success = make_static_collision_map();
00095     //if (!success)
00096     //  break;
00097 
00098     //  == position 1 ==
00099     joint_constraints[0].position = 0.029225487499999758;
00100     joint_constraints[1].position = 0.11312164000000005;
00101     joint_constraints[2].position = -0.61444545070000012;
00102     joint_constraints[3].position = 1.3272473432000003;
00103     joint_constraints[4].position = 0.13708330500000043;
00104 
00105     ROS_INFO("Moving to goal 1");
00106     success = move_to_joint_goal(joint_constraints);
00107     if (!success)
00108       break;
00109 
00110     ros::Duration(1.0).sleep();
00111     ROS_INFO("Sending GRASP");
00112     success = send_gripper_action(GHPEG::GRASP);
00113     success &= query_grasp_status();
00114     if (!success)
00115       break;
00116 
00117     //success = make_static_collision_map();
00118     //if (!success)
00119     //  break;
00120 
00121     //  == position 2 ==
00122     joint_constraints[0].position = 1.0;
00123     joint_constraints[1].position = 0.11312164000000005;
00124     joint_constraints[2].position = -0.61444545070000012;
00125     joint_constraints[3].position = 1.3272473432000003;
00126     joint_constraints[4].position = -0.038843444999999921;
00127 
00128     ROS_INFO("Moving to goal 2");
00129     success = move_to_joint_goal(joint_constraints);
00130     if (!success)
00131       break;
00132 
00133     ros::Duration(1.0).sleep();
00134     ROS_INFO("Sending RELEASE");
00135     success = send_gripper_action(GHPEG::RELEASE);
00136     success &= query_grasp_status();
00137     if (!success)
00138       break;
00139   }
00140 }
00141 
00142 bool PickAndPlaceDemo::move_to_joint_goal(std::vector<motion_planning_msgs::JointConstraint> joint_constraints)
00143 {
00144   move_arm_msgs::MoveArmGoal goal;
00145 
00146   goal.motion_plan_request.group_name = "arm";
00147   goal.motion_plan_request.num_planning_attempts = 1;
00148   goal.motion_plan_request.allowed_planning_time = ros::Duration(5.0);
00149 
00150   goal.motion_plan_request.planner_id = std::string("");
00151   goal.planner_service_name = std::string("ompl_planning/plan_kinematic_path");
00152 
00153   goal.motion_plan_request.goal_constraints.joint_constraints = joint_constraints;
00154 
00155   bool finished_within_time = false;
00156   move_arm_.sendGoal(goal);
00157   finished_within_time = move_arm_.waitForResult(ros::Duration(40.0));
00158   if (!finished_within_time)
00159   {
00160     move_arm_.cancelGoal();
00161     ROS_INFO("Timed out achieving goal!");
00162     return false;
00163   }
00164   else
00165   {
00166     actionlib::SimpleClientGoalState state = move_arm_.getState();
00167     bool success = (state == actionlib::SimpleClientGoalState::SUCCEEDED);
00168     if (success)
00169       ROS_INFO("Action finished: %s", state.toString().c_str());
00170     else
00171       ROS_INFO("Action failed: %s", state.toString().c_str());
00172 
00173     return success;
00174   }
00175 }
00176 
00177 bool PickAndPlaceDemo::send_gripper_action(int32_t goal_type)
00178 {
00179 
00180   GHPEG goal;
00181 
00182   switch (goal_type)
00183   {
00184     case GHPEG::GRASP:
00185       goal.grasp.grasp_posture.name.push_back("dummy_name");
00186       goal.grasp.grasp_posture.position.push_back(GRIPPER_CLOSED_ANGLE);
00187       // leave velocity and effort empty
00188       break;
00189 
00190     case GHPEG::PRE_GRASP:
00191       goal.grasp.pre_grasp_posture.name.push_back("dummy_name");
00192       goal.grasp.pre_grasp_posture.position.push_back(GRIPPER_OPEN_ANGLE);
00193       // leave velocity and effort empty
00194       break;
00195 
00196     case GHPEG::RELEASE:
00197       break;
00198 
00199     default:
00200       ROS_ERROR("unknown goal code (%d)", goal_type);
00201       return false;
00202   }
00203 
00204   goal.goal = goal_type;
00205 
00206   bool finished_within_time = false;
00207   gripper_.sendGoal(goal);
00208   finished_within_time = gripper_.waitForResult(ros::Duration(40.0));
00209   if (!finished_within_time)
00210   {
00211     gripper_.cancelGoal();
00212     ROS_WARN("Timed out achieving goal!");
00213     return false;
00214   }
00215   else
00216   {
00217     actionlib::SimpleClientGoalState state = gripper_.getState();
00218     bool success = (state == actionlib::SimpleClientGoalState::SUCCEEDED);
00219     if (success)
00220       ROS_INFO("Action finished: %s",state.toString().c_str());
00221     else
00222       ROS_WARN("Action failed: %s",state.toString().c_str());
00223 
00224     return success;
00225   }
00226 }
00227 
00228 bool PickAndPlaceDemo::make_static_collision_map() {
00229   ROS_INFO("Making static collision map");
00230 
00231   collision_environment_msgs::MakeStaticCollisionMapGoal goal;
00232   goal.cloud_source = "tilt_scan_cloud_filtered";
00233   goal.number_of_clouds = 1;
00234 
00235   make_static_collision_map_.sendGoal(goal);
00236 
00237   if (!make_static_collision_map_.waitForResult(ros::Duration(30.0)))
00238   {
00239     ROS_ERROR("Timeout when waiting on make_static_collision_map");
00240     return false;
00241   }
00242 
00243   if (make_static_collision_map_.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
00244   {
00245     return true;
00246   }else {
00247     ROS_ERROR("make_static_collision_map action failed!");
00248     return false;
00249   }
00250 }
00251 
00252 
00253 bool PickAndPlaceDemo::query_grasp_status()
00254 {
00255   object_manipulation_msgs::GraspStatus srv;
00256 
00257   bool success = grasp_status_client_.call(srv);
00258   if (success)
00259     if (srv.response.is_hand_occupied)
00260       ROS_INFO("Gripper is holding something");
00261     else
00262       ROS_INFO("Gripper is empty");
00263   else
00264     ROS_ERROR("Failed to call service gripper_grasp_status");
00265 
00266   return success;
00267 }
00268 
00269 } // namespace katana_openloop_grasping
00270 
00271 int main(int argc, char **argv)
00272 {
00273   // TODO: rosservice call /collision_map_self_occ_node/reset
00274 
00275   ros::init(argc, argv, "pick_and_place_demo");
00276   katana_openloop_grasping::PickAndPlaceDemo demo;
00277 
00278   demo.loop();
00279 
00280   ros::shutdown();
00281 }