refactor and added new launch configs

examples/iiqka_moveit_task_constructor_example/kuka_moveit_task_constructor/CMakeLists.txt

@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.8)
-project(kuka_moveit_task_constructor)
+project(kuka_moveit_task_constructor_depalletizing)
 
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
 add_compile_options(-Wall -Wextra -Wpedantic)
@@ -10,16 +10,16 @@ find_package(ament_cmake REQUIRED)
 find_package(moveit_task_constructor_core REQUIRED)
 find_package(rclcpp REQUIRED)
 
-add_executable(kuka_moveit_task_constructor src/kuka_moveit_task_constructor.cpp)
-ament_target_dependencies(kuka_moveit_task_constructor moveit_task_constructor_core rclcpp)
-target_include_directories(kuka_moveit_task_constructor PUBLIC
+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 PUBLIC c_std_99 cxx_std_17)  # Require C99 and C++17
+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
+install(TARGETS kuka_moveit_task_constructor_depalletizing
 DESTINATION lib/${PROJECT_NAME})
 
 if(BUILD_TESTING)

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/kuka_moveit_task_constructor.hpp → examples/iiqka_moveit_task_constructor_example/kuka_moveit_task_constructor/include/kuka_moveit_task_constructor/mtc_depalletizing_task_node.hpp

@@ -12,38 +12,36 @@
 // 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_
+#ifndef KUKA_MOVEIT_TASK_CONSTRUCTOR__MTC_DEPALLETIZING_TASK_NODE_HPP_
+#define KUKA_MOVEIT_TASK_CONSTRUCTOR__MTC_DEPALLETIZING_TASK_NODE_HPP_
 
+#include "kuka_moveit_task_constructor/imtc_task.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 <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
+class MTCDepalletizingTaskNode : public IMTCTask
 {
 public:
-  explicit MTCTaskNode(const rclcpp::NodeOptions & options);
+  explicit MTCDepalletizingTaskNode(const rclcpp::NodeOptions & options);
 
   rclcpp::node_interfaces::NodeBaseInterface::SharedPtr getNodeBaseInterface();
-  void setupPlanningScene();
-  mtc::Task createDepalletizingTask();
-  bool doTask(mtc::Task & task);
+  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();
-  void attachObject(const std::string & object_id);
-  void detachAndRemoveObject(const std::string & object_id);
 };
 
-#endif  // KUKA_MOVEIT_TASK_CONSTRUCTOR__KUKA_MOVEIT_TASK_CONSTRUCTOR_HPP_
+#endif  // KUKA_MOVEIT_TASK_CONSTRUCTOR__MTC_DEPALLETIZING_TASK_NODE_HPP_

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

@@ -16,7 +16,11 @@
 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
 
 
@@ -82,17 +86,25 @@ def launch_setup(context, *args, **kwargs):
         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",
-        executable="kuka_moveit_task_constructor",
+        package="kuka_moveit_task_constructor_depalletizing",
+        executable="kuka_moveit_task_constructor_depalletizing",
         output="screen",
         parameters=[
             moveit_config.to_dict(),
         ],
     )
 
-    to_start = [mtc_demo, move_group_server]
+    to_start = [move_group_server, startup_launch, mtc_demo]
 
     return to_start
 

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

@@ -0,0 +1,114 @@
+# 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],
+    )
+
+    # 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)])

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

@@ -16,7 +16,11 @@
 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
 
 
@@ -74,7 +78,7 @@ def launch_setup(context, *args, **kwargs):
     )
 
     rviz_config_file = (
-        get_package_share_directory("kuka_moveit_task_constructor") + "/rviz/mtc.rviz"
+        get_package_share_directory("kuka_resources") + "/config/view_6_axis_planning_scene.rviz"
     )
 
     move_group_capabilities = {"capabilities": "move_group/ExecuteTaskSolutionCapability"}
@@ -100,17 +104,25 @@ def launch_setup(context, *args, **kwargs):
         ],
     )
 
+    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",
-        executable="kuka_moveit_task_constructor",
+        package="kuka_moveit_task_constructor_depalletizing",
+        executable="kuka_moveit_task_constructor_depalletizing",
         output="screen",
         parameters=[
             moveit_config.to_dict(),
         ],
     )
 
-    to_start = [rviz, mtc_demo, move_group_server]
+    to_start = [move_group_server, rviz, startup_launch, mtc_demo]
 
     return to_start