remove code

kroshu · Oct 25, 2023 · 2221673 · 2221673
1 parent 7cd25aa
commit 2221673
Show file tree

Hide file tree

Showing 4 changed files with 17 additions and 62 deletions.
diff --git a/kuka_driver_examples/eci_demo/include/moveit_example.h b/kuka_driver_examples/eci_demo/include/moveit_example.h
@@ -181,23 +181,10 @@ class MoveitExample : public rclcpp::Node
     moveit_msgs::msg::CollisionObject collision_object;
     collision_object.header.frame_id = move_group_interface_->getPlanningFrame();
     collision_object.id = "collision_box";
-    shape_msgs::msg::SolidPrimitive primitive;
-    primitive.type = primitive.BOX;
-    primitive.dimensions.resize(3);
-    primitive.dimensions[primitive.BOX_X] = box_msg->size.x;
-    primitive.dimensions[primitive.BOX_Y] = box_msg->size.y;
-    primitive.dimensions[primitive.BOX_Z] = box_msg->size.z;
+    // TODO: add collision box based on CollisionBox message (position + size)
+    //////////////////////////////////////////////
 
-    // Define a pose for the box (specified relative to frame_id).
-    geometry_msgs::msg::Pose stand_pose;
-    stand_pose.orientation.w = 1.0;
-    stand_pose.position.x = box_msg->position.x;
-    stand_pose.position.y = box_msg->position.y;
-    stand_pose.position.z = box_msg->position.z;
-
-    collision_object.primitives.push_back(primitive);
-    collision_object.primitive_poses.push_back(stand_pose);
-    collision_object.operation = collision_object.ADD;
+    //////////////////////////////////////////////
 
     AddObject(collision_object);
   }

diff --git a/kuka_driver_examples/eci_demo/src/MoveitCollisionAvoidanceExample.cpp b/kuka_driver_examples/eci_demo/src/MoveitCollisionAvoidanceExample.cpp
@@ -38,21 +38,10 @@ int main(int argc, char * argv[])
   example_node->addRobotPlatform();
   example_node->addBreakPoint();
 
-  auto standing_pose = Eigen::Isometry3d(
-    Eigen::Translation3d(
-      0.1, 0,
-      0.8) *
-    Eigen::Quaterniond::Identity());
-
-  // Plan with collision avoidance
-  auto planned_trajectory = example_node->planToPoint(
-    standing_pose, "ompl",
-    "RRTConnectkConfigDefault");
-  if (planned_trajectory != nullptr) {
-    example_node->drawTrajectory(*planned_trajectory);
-    example_node->addBreakPoint();
-    example_node->moveGroupInterface()->execute(*planned_trajectory);
-  }
+  // TODO: plan to 0.1, 0, 0.8 with identity quaternion (same as last PTP)
+  //////////////////////////////////////////////
+
+  //////////////////////////////////////////////
 
   // Shutdown ROS
   rclcpp::shutdown();

diff --git a/kuka_driver_examples/eci_demo/src/MoveitConstrainedPlanningExample.cpp b/kuka_driver_examples/eci_demo/src/MoveitConstrainedPlanningExample.cpp
@@ -45,10 +45,12 @@ int main(int argc, char * argv[])
     Eigen::Quaterniond::Identity());
 
   geometry_msgs::msg::Quaternion q;
-  q.x = 0;
-  q.y = 0;
-  q.z = 0;
-  q.w = 1;
+
+  // TODO: fill in constraint
+  //////////////////////////////////////////////
+
+  //////////////////////////////////////////////
+
 
   example_node->setOrientationConstraint(q);
   // Plan with collision avoidance

diff --git a/kuka_driver_examples/eci_demo/src/MoveitDepalletizingExample.cpp b/kuka_driver_examples/eci_demo/src/MoveitDepalletizingExample.cpp
@@ -26,40 +26,17 @@ class Depalletizer : public MoveitExample
     for (int k = 0; k < 3; k++) {
       for (int j = 0; j < 3; j++) {
         for (int i = 0; i < 3; i++) {
-          std::string object_name = "pallet_" + std::to_string(9 * k + 3 * j + i);
-          RCLCPP_INFO(LOGGER, "Going for object %s", object_name.c_str());
-
+          // TODO: depalletize
+          //////////////////////////////////////////////
           // Go to pickup
-          Eigen::Isometry3d pose = Eigen::Isometry3d(
-            Eigen::Translation3d(
-              0.3 + i * 0.1, j * 0.1 - 0.1,
-              0.35 - 0.1 * k) *
-            Eigen::Quaterniond(0, 1, 0, 0));
-          auto planned_trajectory =
-            planToPointUntilSuccess(pose, "ompl", "RRTConnectkConfigDefault");
-          if (planned_trajectory != nullptr) {
-            move_group_interface_->execute(*planned_trajectory);
-          } else {
-            RCLCPP_ERROR(LOGGER, "Planning failed");
-          }
 
           // Attach object
-          AttachObject(object_name);
 
           // Drop off to -0.3, 0.0, 0.35 pointing down
-          Eigen::Isometry3d dropoff_pose = Eigen::Isometry3d(
-            Eigen::Translation3d(-0.3, 0.0, 0.35) * Eigen::Quaterniond(0, 1, 0, 0));
-          auto drop_trajectory = planToPointUntilSuccess(
-            dropoff_pose, "ompl",
-            "RRTConnectkConfigDefault");
-          if (drop_trajectory != nullptr) {
-            move_group_interface_->execute(*drop_trajectory);
-          } else {
-            RCLCPP_ERROR(LOGGER, "Planning failed");
-          }
 
           // Detach
-          DetachAndRemoveObject(object_name);
+
+          //////////////////////////////////////////////
         }
       }
     }