Merge pull request #44 from kroshu/fix/code_duplication

combine moveit planning examples

kuka_driver_examples/eci_demo/CMakeLists.txt

@@ -19,14 +19,6 @@ find_package(moveit_visual_tools REQUIRED)
 find_package(rviz_visual_tools REQUIRED)
 find_package(moveit_msgs REQUIRED)
 
-add_executable(moveit_circular src/MoveitCircular.cpp)
-ament_target_dependencies(moveit_circular
-  moveit_ros_planning_interface
-  rclcpp
-  rviz_visual_tools
-  moveit_visual_tools
-)
-
 add_executable(moveit_basic_plan src/MoveitBasicPlan.cpp)
 ament_target_dependencies(moveit_basic_plan
   moveit_ros_planning_interface
@@ -35,10 +27,6 @@ ament_target_dependencies(moveit_basic_plan
   moveit_visual_tools
 )
 
-install(TARGETS moveit_circular
-  EXPORT export_${PROJECT_NAME}
-  DESTINATION lib/${PROJECT_NAME}
-)
 install(TARGETS moveit_basic_plan
   EXPORT export_${PROJECT_NAME}
   DESTINATION lib/${PROJECT_NAME}

kuka_driver_examples/eci_demo/src/MoveitBasicPlan.cpp

@@ -23,6 +23,8 @@
 #include "moveit_msgs/msg/collision_object.hpp"
 #include "moveit_visual_tools/moveit_visual_tools.h"
 
+#define CIRCLE false
+
 
 int main(int argc, char * argv[])
 {
@@ -77,27 +79,76 @@ int main(int argc, char * argv[])
   planning_scene_interface.addCollisionObjects(collision_objects);
   // End Collision Objects define
 
-
   std::vector<geometry_msgs::msg::Pose> waypoints;
-
   moveit_msgs::msg::RobotTrajectory trajectory;
 
-  // Motion start
-
-  // First circle faceing down
-  // Move to origin point
-  geometry_msgs::msg::Pose msg;
-  msg.orientation.x = 0.0;
-  msg.orientation.y = -sqrt(2.0) / 2.0;
-  msg.orientation.z = 0.0;
-  msg.orientation.w = sqrt(2.0) / 2.0;
-  msg.position.x = 0.1;
-  msg.position.y = 0.0;
-  msg.position.z = 0.8;
-  waypoints.push_back(msg);
+  // Add waypoints for planning
+  if (!CIRCLE) {
+    // Move to point near candle
+    geometry_msgs::msg::Pose msg;
+    msg.orientation.x = 0.0;
+    msg.orientation.y = -sqrt(2.0) / 2.0;
+    msg.orientation.z = 0.0;
+    msg.orientation.w = sqrt(2.0) / 2.0;
+    msg.position.x = 0.1;
+    msg.position.y = 0.0;
+    msg.position.z = 0.8;
+    waypoints.push_back(msg);
+  } else {
+    // First circle faceing down
+    // Move to origin point
+    geometry_msgs::msg::Pose msg;
+    msg.orientation.x = 0.0;
+    msg.orientation.y = 0.0;
+    msg.orientation.z = 0.0;
+    msg.orientation.w = 1.0;
+    msg.position.x = 0.35;
+    msg.position.y = 0.0;
+    msg.position.z = 0.4;
+    waypoints.push_back(msg);
+    // Move circle
+    for (int i = 1; i < 63; i++) {
+      msg.position.y = 0.0 + sin(0.1 * i) * 0.18;
+      msg.position.x = 0.35 + cos(0.1 * i) * 0.18;
+      waypoints.push_back(msg);
+    }
+
+    // Second circle faceing forward
+    // Move to origin point
+    msg.orientation.x = 0.0;
+    msg.orientation.y = -sqrt(2.0) / 2.0;
+    msg.orientation.z = 0.0;
+    msg.orientation.w = sqrt(2.0) / 2.0;
+    msg.position.x = 0.4;
+    msg.position.y = -0.2;
+    msg.position.z = 0.2;
+    waypoints.push_back(msg);
+    // Move circle
+    for (int i = 63; i > 0; i--) {
+      msg.position.y = -0.2 + sin(0.1 * i) * 0.15;
+      msg.position.z = 0.2 + cos(0.1 * i) * 0.15;
+      waypoints.push_back(msg);
+    }
+
+    // Third cirlce faceing right
+    // Move to origin point
+    msg.orientation.x = sqrt(2.0) / 2.0;
+    msg.orientation.y = 0.0;
+    msg.orientation.z = 0.0;
+    msg.orientation.w = sqrt(2.0) / 2.0;
+    msg.position.x = 0.2;
+    msg.position.y = 0.2;
+    msg.position.z = 0.38;
+    waypoints.push_back(msg);
+    // Move circle
+    for (int i = 63; i > 0; i--) {
+      msg.position.x = 0.2 + sin(0.1 * i) * 0.18;
+      msg.position.z = 0.38 + cos(0.1 * i) * 0.25;
+      waypoints.push_back(msg);
+    }
+  }
 
   RCLCPP_INFO(logger, "Start planning");
-
   // Planning
   // move_group_interface.setPlannerId("");
   double fraction = move_group_interface.computeCartesianPath(waypoints, 0.005, 0.0, trajectory);