removing debugging

iri_trick_or_treat/src/trick_or_treat_alg_node.cpp

@@ -83,7 +83,7 @@ void TrickOrTreatAlgNode::mainNodeThread(void)
   {
     case 0:
     {
-      //initialize the pick location
+      //initialize the pick and place location
       int pick_location = 1;
       int place_location = 8;
 
@@ -92,12 +92,9 @@ void TrickOrTreatAlgNode::mainNodeThread(void)
       double pick_grasp_block_z_ax = -0.02;
       double pick_pregrasp_block_z_ax = 0.1;
       double pick_postgrasp_block_z_ax = 0.2;
-
       double place_grasp_block_z_ax = 0.2;
       double place_pregrasp_block_z_ax = 0.1;
       double place_postgrasp_block_z_ax = 0.25;
-      ROS_INFO("1");
-
       tfTransformation2BoardFrame(
               pick_location,
               board_frame,
@@ -105,9 +102,7 @@ void TrickOrTreatAlgNode::mainNodeThread(void)
               pick_grasp_block_z_ax,
               pick_pregrasp_block_z_ax,
               pick_postgrasp_block_z_ax);
-      ROS_INFO("2");
       pickOrPlaceBlock(pregrasp_point, grasp_point, postgrasp_point, ini_grasp_EF_rpy, end_grasp_EF_rpy, 1, true);
-      ROS_INFO("3");
       tfTransformation2BoardFrame(
               place_location,
               board_frame,
@@ -115,7 +110,6 @@ void TrickOrTreatAlgNode::mainNodeThread(void)
               place_grasp_block_z_ax,
               place_pregrasp_block_z_ax,
               place_postgrasp_block_z_ax);
-      ROS_INFO("4");
 
       //wait for the action to return
       bool finished_before_timeout = pick_or_place_client_.waitForResult(ros::Duration(50.0));
@@ -137,14 +131,244 @@ void TrickOrTreatAlgNode::mainNodeThread(void)
 
       break;
     case 1:
+    {
+      //initialize the pick and place location
+      int pick_location = 1;
+      int place_location = 8;
+
+      geometry_msgs::PoseStamped board_frame;
+      geometry_msgs::PoseStamped iri_wam_link_base_frame;
+      double pick_grasp_block_z_ax = -0.02;
+      double pick_pregrasp_block_z_ax = 0.1;
+      double pick_postgrasp_block_z_ax = 0.2;
+      double place_grasp_block_z_ax = 0.2;
+      double place_pregrasp_block_z_ax = 0.1;
+      double place_postgrasp_block_z_ax = 0.25;
+      tfTransformation2BoardFrame(
+              pick_location,
+              board_frame,
+              iri_wam_link_base_frame,
+              pick_grasp_block_z_ax,
+              pick_pregrasp_block_z_ax,
+              pick_postgrasp_block_z_ax);
+      pickOrPlaceBlock(pregrasp_point, grasp_point, postgrasp_point, ini_grasp_EF_rpy, end_grasp_EF_rpy, 1, true);
+      tfTransformation2BoardFrame(
+              place_location,
+              board_frame,
+              iri_wam_link_base_frame,
+              place_grasp_block_z_ax,
+              place_pregrasp_block_z_ax,
+              place_postgrasp_block_z_ax);
+
+      //wait for the action to return
+      bool finished_before_timeout = pick_or_place_client_.waitForResult(ros::Duration(50.0));
+
+      //place
+      if(finished_before_timeout)
+      {
+        ROS_DEBUG("Action finished: %s",pick_or_place_client_.getState());
+        pickOrPlaceBlock(pregrasp_point, grasp_point, postgrasp_point, ini_grasp_EF_rpy, end_grasp_EF_rpy, 1, false);
+
+      }
+      else
+      {
+        ROS_DEBUG("Action did not finish before the time out.");
+      }
+
+
+    }
       break;
     case 2:
+    {
+      //initialize the pick and place location
+      int pick_location = 1;
+      int place_location = 8;
+
+      geometry_msgs::PoseStamped board_frame;
+      geometry_msgs::PoseStamped iri_wam_link_base_frame;
+      double pick_grasp_block_z_ax = -0.02;
+      double pick_pregrasp_block_z_ax = 0.1;
+      double pick_postgrasp_block_z_ax = 0.2;
+      double place_grasp_block_z_ax = 0.2;
+      double place_pregrasp_block_z_ax = 0.1;
+      double place_postgrasp_block_z_ax = 0.25;
+      tfTransformation2BoardFrame(
+              pick_location,
+              board_frame,
+              iri_wam_link_base_frame,
+              pick_grasp_block_z_ax,
+              pick_pregrasp_block_z_ax,
+              pick_postgrasp_block_z_ax);
+      pickOrPlaceBlock(pregrasp_point, grasp_point, postgrasp_point, ini_grasp_EF_rpy, end_grasp_EF_rpy, 1, true);
+      tfTransformation2BoardFrame(
+              place_location,
+              board_frame,
+              iri_wam_link_base_frame,
+              place_grasp_block_z_ax,
+              place_pregrasp_block_z_ax,
+              place_postgrasp_block_z_ax);
+
+      //wait for the action to return
+      bool finished_before_timeout = pick_or_place_client_.waitForResult(ros::Duration(50.0));
+
+      //place
+      if(finished_before_timeout)
+      {
+        ROS_DEBUG("Action finished: %s",pick_or_place_client_.getState());
+        pickOrPlaceBlock(pregrasp_point, grasp_point, postgrasp_point, ini_grasp_EF_rpy, end_grasp_EF_rpy, 1, false);
+
+      }
+      else
+      {
+        ROS_DEBUG("Action did not finish before the time out.");
+      }
+
+
+    }
       break;
     case 3:
+    {
+      //initialize the pick and place location
+      int pick_location = 1;
+      int place_location = 8;
+
+      geometry_msgs::PoseStamped board_frame;
+      geometry_msgs::PoseStamped iri_wam_link_base_frame;
+      double pick_grasp_block_z_ax = -0.02;
+      double pick_pregrasp_block_z_ax = 0.1;
+      double pick_postgrasp_block_z_ax = 0.2;
+      double place_grasp_block_z_ax = 0.2;
+      double place_pregrasp_block_z_ax = 0.1;
+      double place_postgrasp_block_z_ax = 0.25;
+      tfTransformation2BoardFrame(
+              pick_location,
+              board_frame,
+              iri_wam_link_base_frame,
+              pick_grasp_block_z_ax,
+              pick_pregrasp_block_z_ax,
+              pick_postgrasp_block_z_ax);
+      pickOrPlaceBlock(pregrasp_point, grasp_point, postgrasp_point, ini_grasp_EF_rpy, end_grasp_EF_rpy, 1, true);
+      tfTransformation2BoardFrame(
+              place_location,
+              board_frame,
+              iri_wam_link_base_frame,
+              place_grasp_block_z_ax,
+              place_pregrasp_block_z_ax,
+              place_postgrasp_block_z_ax);
+
+      //wait for the action to return
+      bool finished_before_timeout = pick_or_place_client_.waitForResult(ros::Duration(50.0));
+
+      //place
+      if(finished_before_timeout)
+      {
+        ROS_DEBUG("Action finished: %s",pick_or_place_client_.getState());
+        pickOrPlaceBlock(pregrasp_point, grasp_point, postgrasp_point, ini_grasp_EF_rpy, end_grasp_EF_rpy, 1, false);
+
+      }
+      else
+      {
+        ROS_DEBUG("Action did not finish before the time out.");
+      }
+
+
+    }
       break;
     case 4:
+    {
+      //initialize the pick and place location
+      int pick_location = 1;
+      int place_location = 8;
+
+      geometry_msgs::PoseStamped board_frame;
+      geometry_msgs::PoseStamped iri_wam_link_base_frame;
+      double pick_grasp_block_z_ax = -0.02;
+      double pick_pregrasp_block_z_ax = 0.1;
+      double pick_postgrasp_block_z_ax = 0.2;
+      double place_grasp_block_z_ax = 0.2;
+      double place_pregrasp_block_z_ax = 0.1;
+      double place_postgrasp_block_z_ax = 0.25;
+      tfTransformation2BoardFrame(
+              pick_location,
+              board_frame,
+              iri_wam_link_base_frame,
+              pick_grasp_block_z_ax,
+              pick_pregrasp_block_z_ax,
+              pick_postgrasp_block_z_ax);
+      pickOrPlaceBlock(pregrasp_point, grasp_point, postgrasp_point, ini_grasp_EF_rpy, end_grasp_EF_rpy, 1, true);
+      tfTransformation2BoardFrame(
+              place_location,
+              board_frame,
+              iri_wam_link_base_frame,
+              place_grasp_block_z_ax,
+              place_pregrasp_block_z_ax,
+              place_postgrasp_block_z_ax);
+
+      //wait for the action to return
+      bool finished_before_timeout = pick_or_place_client_.waitForResult(ros::Duration(50.0));
+
+      //place
+      if(finished_before_timeout)
+      {
+        ROS_DEBUG("Action finished: %s",pick_or_place_client_.getState());
+        pickOrPlaceBlock(pregrasp_point, grasp_point, postgrasp_point, ini_grasp_EF_rpy, end_grasp_EF_rpy, 1, false);
+
+      }
+      else
+      {
+        ROS_DEBUG("Action did not finish before the time out.");
+      }
+
+
+    }
       break;
     case 5:
+    {
+      //initialize the pick and place location
+      int pick_location = 1;
+      int place_location = 8;
+
+      geometry_msgs::PoseStamped board_frame;
+      geometry_msgs::PoseStamped iri_wam_link_base_frame;
+      double pick_grasp_block_z_ax = -0.02;
+      double pick_pregrasp_block_z_ax = 0.1;
+      double pick_postgrasp_block_z_ax = 0.2;
+      double place_grasp_block_z_ax = 0.2;
+      double place_pregrasp_block_z_ax = 0.1;
+      double place_postgrasp_block_z_ax = 0.25;
+      tfTransformation2BoardFrame(
+              pick_location,
+              board_frame,
+              iri_wam_link_base_frame,
+              pick_grasp_block_z_ax,
+              pick_pregrasp_block_z_ax,
+              pick_postgrasp_block_z_ax);
+      pickOrPlaceBlock(pregrasp_point, grasp_point, postgrasp_point, ini_grasp_EF_rpy, end_grasp_EF_rpy, 1, true);
+      tfTransformation2BoardFrame(
+              place_location,
+              board_frame,
+              iri_wam_link_base_frame,
+              place_grasp_block_z_ax,
+              place_pregrasp_block_z_ax,
+              place_postgrasp_block_z_ax);
+
+      //wait for the action to return
+      bool finished_before_timeout = pick_or_place_client_.waitForResult(ros::Duration(50.0));
+
+      //place
+      if(finished_before_timeout)
+      {
+        ROS_DEBUG("Action finished: %s",pick_or_place_client_.getState());
+        pickOrPlaceBlock(pregrasp_point, grasp_point, postgrasp_point, ini_grasp_EF_rpy, end_grasp_EF_rpy, 1, false);
+
+      }
+      else
+      {
+        ROS_DEBUG("Action did not finish before the time out.");
+      }
+
+
+    }
       break;
 
   }