Add new moveit examples (#109)

* start from home in mock

* createPalletObjects + attach skeleton

* update planning freq

* rework attachobj

* readd coll objs

* working attach

* fix addobject

* rework to global vars

* change plantopoint parametrization

* rework collision object handling

* detachobject

* 3*3*3 + ori constraint

* add planning failed msg

* depalletize

* remove object removal

* remove collision object vector

* planToPointUntilSuccess

* separate into two examples - add basic planners ex

* fix palletizing example

* contstraints

* add common base

* fix source name

* set scaling factor for pilz lin

* fix scaling setting

* change default planner

* add collision box subscriber

* separate examples

* fix scaling

* fix depallet speed

* remove coll objects

* remove trigger

* remove comments

* fixes

* add breakpoint at end of basic ex

* better constrained planning

* add pos1 for before coll avoid

* clean up example

* go to init poses

* reset dummy pub yaml

* lint

* fix build

---------

Co-authored-by: Aron Svastits <svastits1@gmail.com>
Co-authored-by: kovacsge11 <kovacsge11@gmail.com>

kuka_driver_examples/eci_demo/CMakeLists.txt

@@ -18,16 +18,51 @@ find_package(rclcpp REQUIRED)
 find_package(moveit_visual_tools REQUIRED)
 find_package(rviz_visual_tools REQUIRED)
 find_package(moveit_msgs REQUIRED)
+find_package(kuka_driver_interfaces REQUIRED)
 
-add_executable(moveit_basic_plan src/MoveitBasicPlan.cpp)
-ament_target_dependencies(moveit_basic_plan
+include_directories(include)
+
+add_executable(moveit_basic_planners_example src/MoveitBasicPlannersExample.cpp)
+ament_target_dependencies(moveit_basic_planners_example
+  moveit_ros_planning_interface
+  rclcpp
+  rviz_visual_tools
+  moveit_visual_tools
+  kuka_driver_interfaces
+)
+
+add_executable(moveit_collision_avoidance_example src/MoveitCollisionAvoidanceExample.cpp)
+ament_target_dependencies(moveit_collision_avoidance_example
+  moveit_ros_planning_interface
+  rclcpp
+  rviz_visual_tools
+  moveit_visual_tools
+  kuka_driver_interfaces
+)
+
+add_executable(moveit_constrained_planning_example src/MoveitConstrainedPlanningExample.cpp)
+ament_target_dependencies(moveit_constrained_planning_example
+  moveit_ros_planning_interface
+  rclcpp
+  rviz_visual_tools
+  moveit_visual_tools
+  kuka_driver_interfaces
+)
+
+add_executable(moveit_depalletizing_example src/MoveitDepalletizingExample.cpp)
+ament_target_dependencies(moveit_depalletizing_example
   moveit_ros_planning_interface
   rclcpp
   rviz_visual_tools
   moveit_visual_tools
+  kuka_driver_interfaces
 )
 
-install(TARGETS moveit_basic_plan
+install(TARGETS
+  moveit_basic_planners_example
+  moveit_collision_avoidance_example
+  moveit_constrained_planning_example
+  moveit_depalletizing_example
   EXPORT export_${PROJECT_NAME}
   DESTINATION lib/${PROJECT_NAME}
 )

kuka_driver_examples/eci_demo/config/dummy_publisher.yaml

@@ -5,10 +5,14 @@
     wait_sec_between_publish: 6
 
     goal_names: ["pos1", "pos2", "pos3", "pos4"]
-    pos1: [0.0, -1.57, 0.0, 0.0, 0.0, 0.0]
-    pos2: [0.0, -1.37, 0.0, 0.0, 0.0, 0.0]
-    pos3: [0.2, -1.57, 0.2, 0.2, 0.2, 0.2]
-    pos4: [-0.2, -1.57, -0.2, -0.2, -0.2, -0.2]
+    pos1: 
+      positions: [0.0, -1.57, 0.0, 0.0, 0.0, 0.0]
+    pos2: 
+      positions: [0.0, -1.37, 0.0, 0.0, 0.0, 0.0]
+    pos3: 
+      positions: [0.2, -1.57, 0.2, 0.2, 0.2, 0.2]
+    pos4: 
+      positions: [-0.2, -1.57, -0.2, -0.2, -0.2, -0.2]
 
     joints:
       - joint_a1
@@ -18,4 +22,4 @@
       - joint_a5
       - joint_a6
 
-    check_starting_point: false
+    check_starting_point: false