working prototype with 8 items + some indentation

examples/iiqka_moveit_task_constructor_example/kuka_moveit_task_constructor/CMakeLists.txt

@@ -28,4 +28,4 @@ find_package(ament_lint_auto REQUIRED)
 ament_lint_auto_find_test_dependencies()
 endif()
 
-ament_package()
+ament_package()

examples/iiqka_moveit_task_constructor_example/kuka_moveit_task_constructor/include/kuka_moveit_task_constructor/kuka_moveit_task_constructor.hpp

@@ -1,32 +1,49 @@
-#ifndef KUKA_MOVEIT_TASK_CONSTRUCTOR_HPP
-#define KUKA_MOVEIT_TASK_CONSTRUCTOR_HPP
+// Copyright 2024 Ádám Pető
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef KUKA_MOVEIT_TASK_CONSTRUCTOR__KUKA_MOVEIT_TASK_CONSTRUCTOR_HPP_
+#define KUKA_MOVEIT_TASK_CONSTRUCTOR__KUKA_MOVEIT_TASK_CONSTRUCTOR_HPP_
 
-#include <rclcpp/rclcpp.hpp>
 #include <moveit/planning_scene/planning_scene.h>
 #include <moveit/planning_scene_interface/planning_scene_interface.h>
-#include <moveit/task_constructor/task.h>
 #include <moveit/task_constructor/solvers.h>
 #include <moveit/task_constructor/stages.h>
-#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#include <moveit/task_constructor/task.h>
+
+#include <string>
+
+#include <rclcpp/rclcpp.hpp>
 #include <tf2_eigen/tf2_eigen.hpp>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 
 namespace mtc = moveit::task_constructor;
 
 class MTCTaskNode
 {
 public:
-  MTCTaskNode(const rclcpp::NodeOptions& options);
+  explicit MTCTaskNode(const rclcpp::NodeOptions & options);
 
   rclcpp::node_interfaces::NodeBaseInterface::SharedPtr getNodeBaseInterface();
   void setupPlanningScene();
   mtc::Task createDepalletizingTask();
-  bool doTask(mtc::Task& task);
+  bool doTask(mtc::Task & task);
 
 private:
   rclcpp::Node::SharedPtr node_;
   void addPalletObjects();
-  void attachObject(const std::string& object_id);
-  void detachAndRemoveObject(const std::string& object_id);
+  void attachObject(const std::string & object_id);
+  void detachAndRemoveObject(const std::string & object_id);
 };
 
-#endif // KUKA_MOVEIT_TASK_CONSTRUCTOR_HPP
+#endif  // KUKA_MOVEIT_TASK_CONSTRUCTOR__KUKA_MOVEIT_TASK_CONSTRUCTOR_HPP_

examples/iiqka_moveit_task_constructor_example/kuka_moveit_task_constructor/launch/startup.launch.py

@@ -1,14 +1,25 @@
+# Copyright 2024 Ádám Pető
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
 from launch import LaunchDescription
 from launch_ros.actions import Node
 from ament_index_python.packages import get_package_share_directory
 from moveit_configs_utils import MoveItConfigsBuilder
-from launch.actions.include_launch_description import IncludeLaunchDescription
 from launch.actions import DeclareLaunchArgument, OpaqueFunction
-from launch.launch_description_sources.python_launch_description_source import (
-    PythonLaunchDescriptionSource,
-)
 from launch.substitutions import LaunchConfiguration
 
+
 def launch_setup(context, *args, **kwargs):
     robot_model = LaunchConfiguration("robot_model")
     ns = LaunchConfiguration("namespace")
@@ -45,10 +56,13 @@ def launch_setup(context, *args, **kwargs):
         )
         .robot_description_kinematics(
             file_path=get_package_share_directory("kuka_lbr_iisy_moveit_config")
-            + "/config/kinematics.yaml")
-        .trajectory_execution(file_path=get_package_share_directory("kuka_lbr_iisy_moveit_config")
-            + "/config/moveit_controllers.yaml")
-        .planning_pipelines(pipelines=["ompl"])
+            + "/config/kinematics.yaml"
+        )
+        .trajectory_execution(
+            file_path=get_package_share_directory("kuka_lbr_iisy_moveit_config")
+            + "/config/moveit_controllers.yaml"
+        )
+        .planning_pipelines(pipelines=["ompl", "pilz_industrial_motion_planner"])
         .planning_scene_monitor(
             publish_robot_description=True, publish_robot_description_semantic=True
         )
@@ -82,6 +96,7 @@ def launch_setup(context, *args, **kwargs):
 
     return to_start
 
+
 def generate_launch_description():
     launch_arguments = []
     launch_arguments.append(DeclareLaunchArgument("robot_model", default_value="lbr_iisy3_r760"))
@@ -92,4 +107,4 @@ def generate_launch_description():
     launch_arguments.append(DeclareLaunchArgument("roll", default_value="0"))
     launch_arguments.append(DeclareLaunchArgument("pitch", default_value="0"))
     launch_arguments.append(DeclareLaunchArgument("yaw", default_value="0"))
-    return LaunchDescription(launch_arguments + [OpaqueFunction(function=launch_setup)])
+    return LaunchDescription(launch_arguments + [OpaqueFunction(function=launch_setup)])

examples/iiqka_moveit_task_constructor_example/kuka_moveit_task_constructor/launch/startup_with_rviz.launch.py

@@ -1,14 +1,25 @@
+# Copyright 2024 Ádám Pető
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
 from launch import LaunchDescription
 from launch_ros.actions import Node
 from ament_index_python.packages import get_package_share_directory
 from moveit_configs_utils import MoveItConfigsBuilder
-from launch.actions.include_launch_description import IncludeLaunchDescription
 from launch.actions import DeclareLaunchArgument, OpaqueFunction
-from launch.launch_description_sources.python_launch_description_source import (
-    PythonLaunchDescriptionSource,
-)
 from launch.substitutions import LaunchConfiguration
 
+
 def launch_setup(context, *args, **kwargs):
     robot_model = LaunchConfiguration("robot_model")
     ns = LaunchConfiguration("namespace")
@@ -45,9 +56,12 @@ def launch_setup(context, *args, **kwargs):
         )
         .robot_description_kinematics(
             file_path=get_package_share_directory("kuka_lbr_iisy_moveit_config")
-            + "/config/kinematics.yaml")
-        .trajectory_execution(file_path=get_package_share_directory("kuka_lbr_iisy_moveit_config")
-            + "/config/moveit_controllers.yaml")
+            + "/config/kinematics.yaml"
+        )
+        .trajectory_execution(
+            file_path=get_package_share_directory("kuka_lbr_iisy_moveit_config")
+            + "/config/moveit_controllers.yaml"
+        )
         .planning_pipelines(pipelines=["ompl"])
         .planning_scene_monitor(
             publish_robot_description=True, publish_robot_description_semantic=True
@@ -83,7 +97,7 @@ def launch_setup(context, *args, **kwargs):
             moveit_config.robot_description_semantic,
             moveit_config.robot_description_kinematics,
             moveit_config.planning_pipelines,
-        ]
+        ],
     )
 
     # MTC Demo node
@@ -100,6 +114,7 @@ def launch_setup(context, *args, **kwargs):
 
     return to_start
 
+
 def generate_launch_description():
     launch_arguments = []
     launch_arguments.append(DeclareLaunchArgument("robot_model", default_value="lbr_iisy3_r760"))
@@ -110,4 +125,4 @@ def generate_launch_description():
     launch_arguments.append(DeclareLaunchArgument("roll", default_value="0"))
     launch_arguments.append(DeclareLaunchArgument("pitch", default_value="0"))
     launch_arguments.append(DeclareLaunchArgument("yaw", default_value="0"))
-    return LaunchDescription(launch_arguments + [OpaqueFunction(function=launch_setup)])
+    return LaunchDescription(launch_arguments + [OpaqueFunction(function=launch_setup)])

examples/iiqka_moveit_task_constructor_example/kuka_moveit_task_constructor/package.xml

@@ -10,7 +10,7 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
 
   <depend>moveit_task_constructor_core</depend>
-  <depend>rclcpp</depend> 
+  <depend>rclcpp</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

examples/iiqka_moveit_task_constructor_example/kuka_moveit_task_constructor/rviz/mtc.rviz

@@ -268,4 +268,4 @@ Window Geometry:
     collapsed: false
   Width: 1920
   X: 0
-  Y: 0
+  Y: 0