PNP_Vacuum: Adjust rotation, rename variable

CommonplaceRobotics · May 4, 2023 · 96d62ca · 96d62ca
1 parent 219692e
commit 96d62ca
Showing 1 changed file with 16 additions and 16 deletions.
diff --git a/irc_ros_examples/src/pick_and_place_vacuum.cpp b/irc_ros_examples/src/pick_and_place_vacuum.cpp
@@ -66,7 +66,7 @@ class PickAndPlaceVacuum : public PickAndPlaceBase
     const std::chrono::duration<int64_t, std::milli> timeout = std::chrono::milliseconds(1000);
 
     while (std::chrono::steady_clock::now() - start < timeout) {
-      if (result.future.valid()) {  //  == rclcpp::FutureReturnCode::SUCCESS) {
+      if (result.future.valid()) {
         return (result.get()->gripped == state);
       }
       std::this_thread::yield();
@@ -88,12 +88,12 @@ class PickAndPlaceVacuum : public PickAndPlaceBase
     posestamped.header.frame_id = frame_id;
     posestamped.pose.position.x = 0 * slot_offset_;
     posestamped.pose.position.y = 0 * slot_offset_;
-    posestamped.pose.position.z = height_offsetheight_offset_;
+    posestamped.pose.position.z = height_offset_;
 
     // Always point downwards for all movements
-    posestamped.pose.orientation.w = 0;
+    posestamped.pose.orientation.w = 0.707;
     posestamped.pose.orientation.x = 0;
-    posestamped.pose.orientation.y = 1;
+    posestamped.pose.orientation.y = 0.707;
     posestamped.pose.orientation.z = 0;
 
     geometry_msgs::msg::PoseStamped target_pose;
@@ -130,12 +130,12 @@ class PickAndPlaceVacuum : public PickAndPlaceBase
     posestamped.header.frame_id = frame_id;
     posestamped.pose.position.x = x * slot_offset_;
     posestamped.pose.position.y = y * slot_offset_;
-    posestamped.pose.position.z = height_offsetheight_offset_;
+    posestamped.pose.position.z = height_offset_;
 
     // Always point downwards for all movements
-    posestamped.pose.orientation.w = 0;
+    posestamped.pose.orientation.w = 0.707;
     posestamped.pose.orientation.x = 0;
-    posestamped.pose.orientation.y = 1;
+    posestamped.pose.orientation.y = 0.707;
     posestamped.pose.orientation.z = 0;
 
     geometry_msgs::msg::PoseStamped target_pose;
@@ -145,14 +145,14 @@ class PickAndPlaceVacuum : public PickAndPlaceBase
     lin(target_pose);
 
     // Slowly touch the object
-    posestamped.pose.position.z -= height_offsetheight_offset_;
+    posestamped.pose.position.z -= height_offset_;
     target_pose = buffer_->transform(posestamped, planning_frame_);
     lin(target_pose, 0.1, 0.1);
 
     set_gripper(true);
 
     // Lift the object
-    posestamped.pose.position.z += height_offsetheight_offset_;
+    posestamped.pose.position.z += height_offset_;
     target_pose = buffer_->transform(posestamped, planning_frame_);
     lin(target_pose);
   }
@@ -169,12 +169,12 @@ class PickAndPlaceVacuum : public PickAndPlaceBase
     posestamped.header.frame_id = frame_id;
     posestamped.pose.position.x = x * slot_offset_;
     posestamped.pose.position.y = y * slot_offset_;
-    posestamped.pose.position.z = height_offsetheight_offset_;
+    posestamped.pose.position.z = height_offset_;
 
     // Always point downwards for all movements
-    posestamped.pose.orientation.w = 0;
+    posestamped.pose.orientation.w = 0.707;
     posestamped.pose.orientation.x = 0;
-    posestamped.pose.orientation.y = 1;
+    posestamped.pose.orientation.y = 0.707;
     posestamped.pose.orientation.z = 0;
 
     geometry_msgs::msg::PoseStamped target_pose;
@@ -184,7 +184,7 @@ class PickAndPlaceVacuum : public PickAndPlaceBase
     lin(target_pose);
 
     // Slowly touch the object
-    posestamped.pose.position.z -= height_offsetheight_offset_;
+    posestamped.pose.position.z -= height_offset_;
     target_pose = buffer_->transform(posestamped, planning_frame_);
     lin(target_pose, 0.1, 0.1);
 
@@ -194,7 +194,7 @@ class PickAndPlaceVacuum : public PickAndPlaceBase
     std::this_thread::sleep_for(std::chrono::milliseconds(1500));
 
     //Move back up again
-    posestamped.pose.position.z += height_offsetheight_offset_;
+    posestamped.pose.position.z += height_offset_;
     target_pose = buffer_->transform(posestamped, planning_frame_);
     lin(target_pose);
   }
@@ -205,7 +205,7 @@ class PickAndPlaceVacuum : public PickAndPlaceBase
     const int objects_y = 3;
     const int objects_x = 5;
 
-    //Make sure the gripper is empty
+    // Make sure the gripper is empty
     set_gripper(false);
 
     // Move over the outlines of the trays to make sure they are at the right position
@@ -250,7 +250,7 @@ class PickAndPlaceVacuum : public PickAndPlaceBase
   std::shared_ptr<tf2_ros::TransformListener> listener_;
 
   // Scene parameters
-  const double height_offsetheight_offset_ =
+  const double height_offset_ =
     0.03;                             // z offset of how much to lift the objects out of the trays
   const double slot_offset_ = 0.035;  // x and y offset between different slots in the tray
 };