Add Example for Depalletizing Using MoveIt Task Constructor

.github/workflows/industrial_ci.yml

@@ -33,7 +33,7 @@ jobs:
       BUILDER: colcon
       ANALYZER: sonarqube
       TEST_COVERAGE: true
-      UPSTREAM_WORKSPACE: 'github:kroshu/kuka_robot_descriptions#master github:kroshu/kuka-external-control-sdk#master'
+      UPSTREAM_WORKSPACE: 'github:kroshu/kuka_robot_descriptions#master github:kroshu/kuka-external-control-sdk#master github:moveit/moveit_task_constructor#ros2'
       ROS_DISTRO: jazzy
       CCACHE_DIR: /github/home/.ccache # Directory for ccache (and how we enable ccache in industrial_ci)
       EVENT_NAME: ${{ github.event_name }}

examples/iiqka_moveit_task_constructor_example/kuka_moveit_task_constructor/CMakeLists.txt

@@ -0,0 +1,29 @@
+cmake_minimum_required(VERSION 3.8)
+project(kuka_moveit_task_constructor_depalletizing)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(moveit_task_constructor_core REQUIRED)
+find_package(rclcpp REQUIRED)
+
+add_executable(kuka_moveit_task_constructor_depalletizing src/mtc_depalletizing_task_node.cpp)
+ament_target_dependencies(kuka_moveit_task_constructor_depalletizing moveit_task_constructor_core rclcpp)
+target_include_directories(kuka_moveit_task_constructor_depalletizing PUBLIC
+$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+$<INSTALL_INTERFACE:include>)
+target_compile_features(kuka_moveit_task_constructor_depalletizing PUBLIC c_std_99 cxx_std_17)  # Require C99 and C++17
+
+
+install(DIRECTORY launch rviz DESTINATION share/${PROJECT_NAME})
+install(TARGETS kuka_moveit_task_constructor_depalletizing
+DESTINATION lib/${PROJECT_NAME})
+
+if(BUILD_TESTING)
+
+endif()
+
+ament_package()

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

@@ -0,0 +1,34 @@
+// 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__IMTC_TASK_HPP_
+#define KUKA_MOVEIT_TASK_CONSTRUCTOR__IMTC_TASK_HPP_
+
+#include <moveit/task_constructor/task.h>
+
+#include <string>
+
+namespace mtc = moveit::task_constructor;
+
+class IMTCTask
+{
+public:
+  virtual ~IMTCTask() = default;
+
+  virtual void setupPlanningScene() = 0;
+  virtual mtc::Task createTask() = 0;
+  virtual bool doTask(mtc::Task & task) = 0;
+};
+
+#endif  // KUKA_MOVEIT_TASK_CONSTRUCTOR__IMTC_TASK_HPP_

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

@@ -0,0 +1,50 @@
+// 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__MTC_DEPALLETIZING_TASK_NODE_HPP_
+#define KUKA_MOVEIT_TASK_CONSTRUCTOR__MTC_DEPALLETIZING_TASK_NODE_HPP_
+
+#include <moveit/planning_scene/planning_scene.h>
+#include <moveit/planning_scene_interface/planning_scene_interface.h>
+#include <moveit/task_constructor/solvers.h>
+#include <moveit/task_constructor/stages.h>
+
+#include <string>
+
+#include "kuka_moveit_task_constructor/imtc_task.hpp"
+
+#include <rclcpp/rclcpp.hpp>
+#include <tf2_eigen/tf2_eigen.hpp>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+
+namespace mtc = moveit::task_constructor;
+
+class MTCDepalletizingTaskNode : public IMTCTask
+{
+public:
+  explicit MTCDepalletizingTaskNode(const rclcpp::NodeOptions & options);
+
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr getNodeBaseInterface();
+  void attachObject(const std::string & object_id);
+  void detachObject(const std::string & object_id);
+  void setupPlanningScene() override;
+  mtc::Task createTask() override;
+  bool doTask(mtc::Task & task) override;
+
+private:
+  rclcpp::Node::SharedPtr node_;
+  void addPalletObjects();
+};
+
+#endif  // KUKA_MOVEIT_TASK_CONSTRUCTOR__MTC_DEPALLETIZING_TASK_NODE_HPP_

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

@@ -0,0 +1,121 @@
+# 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")
+    x = LaunchConfiguration("x")
+    y = LaunchConfiguration("y")
+    z = LaunchConfiguration("z")
+    roll = LaunchConfiguration("roll")
+    pitch = LaunchConfiguration("pitch")
+    yaw = LaunchConfiguration("yaw")
+
+    if ns.perform(context) == "":
+        tf_prefix = ""
+    else:
+        tf_prefix = ns.perform(context) + "_"
+
+    moveit_config = (
+        MoveItConfigsBuilder("kuka_lbr_iisy")
+        .robot_description(
+            file_path=get_package_share_directory("kuka_lbr_iisy_support")
+            + f"/urdf/{robot_model.perform(context)}.urdf.xacro",
+            mappings={
+                "x": x.perform(context),
+                "y": y.perform(context),
+                "z": z.perform(context),
+                "roll": roll.perform(context),
+                "pitch": pitch.perform(context),
+                "yaw": yaw.perform(context),
+                "prefix": tf_prefix,
+            },
+        )
+        .robot_description_semantic(
+            file_path=get_package_share_directory("kuka_lbr_iisy_moveit_config")
+            + f"/urdf/{robot_model.perform(context)}.srdf"
+        )
+        .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", "pilz_industrial_motion_planner"])
+        .planning_scene_monitor(
+            publish_robot_description=True, publish_robot_description_semantic=True
+        )
+        .joint_limits(
+            file_path=get_package_share_directory("kuka_lbr_iisy_support")
+            + f"/config/{robot_model.perform(context)}_joint_limits.yaml"
+        )
+        .to_moveit_configs()
+    )
+
+    move_group_capabilities = {"capabilities": "move_group/ExecuteTaskSolutionCapability"}
+
+    move_group_server = Node(
+        package="moveit_ros_move_group",
+        executable="move_group",
+        output="screen",
+        parameters=[moveit_config.to_dict(), move_group_capabilities],
+    )
+
+    startup_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            [get_package_share_directory("kuka_iiqka_eac_driver"), "/launch/startup.launch.py"]
+        ),
+        launch_arguments={"use_fake_hardware": "true"}.items(),
+    )
+
+    # MTC Demo node
+    mtc_demo = Node(
+        package="kuka_moveit_task_constructor_depalletizing",
+        executable="kuka_moveit_task_constructor_depalletizing",
+        output="screen",
+        parameters=[
+            moveit_config.to_dict(),
+        ],
+    )
+
+    to_start = [move_group_server, startup_launch, mtc_demo]
+
+    return to_start
+
+
+def generate_launch_description():
+    launch_arguments = []
+    launch_arguments.append(DeclareLaunchArgument("robot_model", default_value="lbr_iisy3_r760"))
+    launch_arguments.append(DeclareLaunchArgument("namespace", default_value=""))
+    launch_arguments.append(DeclareLaunchArgument("x", default_value="0"))
+    launch_arguments.append(DeclareLaunchArgument("y", default_value="0"))
+    launch_arguments.append(DeclareLaunchArgument("z", default_value="0"))
+    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)])

examples/iiqka_moveit_task_constructor_example/kuka_moveit_task_constructor/launch/startup_standalone.launch.py

@@ -0,0 +1,110 @@
+# 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 import DeclareLaunchArgument, OpaqueFunction
+from launch.substitutions import LaunchConfiguration
+
+
+def launch_setup(context, *args, **kwargs):
+    robot_model = LaunchConfiguration("robot_model")
+    ns = LaunchConfiguration("namespace")
+    x = LaunchConfiguration("x")
+    y = LaunchConfiguration("y")
+    z = LaunchConfiguration("z")
+    roll = LaunchConfiguration("roll")
+    pitch = LaunchConfiguration("pitch")
+    yaw = LaunchConfiguration("yaw")
+
+    if ns.perform(context) == "":
+        tf_prefix = ""
+    else:
+        tf_prefix = ns.perform(context) + "_"
+
+    moveit_config = (
+        MoveItConfigsBuilder("kuka_lbr_iisy")
+        .robot_description(
+            file_path=get_package_share_directory("kuka_lbr_iisy_support")
+            + f"/urdf/{robot_model.perform(context)}.urdf.xacro",
+            mappings={
+                "x": x.perform(context),
+                "y": y.perform(context),
+                "z": z.perform(context),
+                "roll": roll.perform(context),
+                "pitch": pitch.perform(context),
+                "yaw": yaw.perform(context),
+                "prefix": tf_prefix,
+            },
+        )
+        .robot_description_semantic(
+            file_path=get_package_share_directory("kuka_lbr_iisy_moveit_config")
+            + f"/urdf/{robot_model.perform(context)}.srdf"
+        )
+        .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", "pilz_industrial_motion_planner"])
+        .planning_scene_monitor(
+            publish_robot_description=True, publish_robot_description_semantic=True
+        )
+        .joint_limits(
+            file_path=get_package_share_directory("kuka_lbr_iisy_support")
+            + f"/config/{robot_model.perform(context)}_joint_limits.yaml"
+        )
+        .to_moveit_configs()
+    )
+
+    move_group_capabilities = {"capabilities": "move_group/ExecuteTaskSolutionCapability"}
+
+    move_group_server = Node(
+        package="moveit_ros_move_group",
+        executable="move_group",
+        output="screen",
+        parameters=[moveit_config.to_dict(), move_group_capabilities],
+    )
+
+    # MTC Demo node
+    mtc_demo = Node(
+        package="kuka_moveit_task_constructor_depalletizing",
+        executable="kuka_moveit_task_constructor_depalletizing",
+        output="screen",
+        parameters=[
+            moveit_config.to_dict(),
+        ],
+    )
+
+    to_start = [move_group_server, mtc_demo]
+
+    return to_start
+
+
+def generate_launch_description():
+    launch_arguments = []
+    launch_arguments.append(DeclareLaunchArgument("robot_model", default_value="lbr_iisy3_r760"))
+    launch_arguments.append(DeclareLaunchArgument("namespace", default_value=""))
+    launch_arguments.append(DeclareLaunchArgument("x", default_value="0"))
+    launch_arguments.append(DeclareLaunchArgument("y", default_value="0"))
+    launch_arguments.append(DeclareLaunchArgument("z", default_value="0"))
+    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)])