Make a ros wrapper.

Signed-off-by: Jelmer de Wolde <[email protected]>

Author: Jelmerdw

dockerfiles/install_scripts/dev_packages.sh

@@ -7,4 +7,3 @@
 set -e
 source /home/$UNAME/.bashrc
 apt update
-

pyproject.toml

@@ -22,7 +22,6 @@ dependencies = [
     "mashumaro>=3.16",
     "myst-parser>=4.0.1",
     "numpy<2.0",
-    "opencv-python>=4.11.0.86",
     "pillow>=11.3.0",
     "pre-commit>=4.2.0",
     "psutil>=7.0.0",
@@ -42,9 +41,9 @@ dependencies = [
     "tornado>=6.5.1",
     "transforms3d>=0.4.2",
     "waitress>=3.0.2",
-    "xmltodict>=1.0.2",
     "pandas>=2.2.0",
     "trimesh>=4.8.2",
+    "xmltodict>=1.0.2",
 ]
 
 [dependency-groups]

ros2_ws/src/rcdt_gazebo/launch/gazebo_robot.launch.py

@@ -3,6 +3,7 @@
 # SPDX-License-Identifier: Apache-2.0
 
 
+import math
 import xml.etree.ElementTree as ET
 
 from launch import LaunchContext, LaunchDescription
@@ -16,6 +17,7 @@
 world_arg = LaunchArgument("world", "walls.sdf")
 robots_arg = LaunchArgument("robots", "")
 positions_arg = LaunchArgument("positions", "")
+orientation_arg = LaunchArgument("orientation", "0,0,0,1")
 bridge_topics_arg = LaunchArgument("bridge_topics", "")
 
 
@@ -35,6 +37,7 @@ def launch_setup(context: LaunchContext) -> list:
     world = world_arg.string_value(context)
     robots = robots_arg.string_value(context).split()
     positions = positions_arg.string_value(context).split()
+    orientations = orientation_arg.string_value(context).split()
     bridge_topics = bridge_topics_arg.string_value(context).split()
 
     sdf_file = get_file_path("rcdt_gazebo", ["worlds"], world)
@@ -61,9 +64,21 @@ def launch_setup(context: LaunchContext) -> list:
     )
 
     spawn_robots: list[Node] = []
-    for robot, position in zip(robots, positions, strict=False):
+    for robot, position, orientation in zip(
+        robots, positions, orientations, strict=False
+    ):
         namespace = "" if not robot else f"/{robot}"
         x, y, z = position.split(",")
+        qx, qy, qz, qw = orientation.split(",")
+        roll, pitch, yaw = 0, 0, 0
+        if robot in {"realsense1", "realsense"}:
+            roll = 0
+            pitch = math.radians(90)
+            yaw = 0
+
+        print(
+            f"Spawning robot '{robot}' at position {position} with orientation {roll, pitch, yaw} in world '{world_name}'"
+        )
         spawn_robot = Node(
             package="ros_gz_sim",
             executable="create",
@@ -78,6 +93,12 @@ def launch_setup(context: LaunchContext) -> list:
                 str(y),
                 "-z",
                 str(z),
+                "-P",
+                str(pitch),
+                "-R",
+                str(roll),
+                "-Y",
+                str(yaw),
             ],
             output="screen",
         )
@@ -125,6 +146,7 @@ def generate_launch_description() -> LaunchDescription:
             world_arg.declaration,
             robots_arg.declaration,
             positions_arg.declaration,
+            orientation_arg.declaration,
             bridge_topics_arg.declaration,
             OpaqueFunction(function=launch_setup),
         ]

ros2_ws/src/rcdt_gazebo/worlds/table_with_1_brick.sdf

@@ -69,7 +69,7 @@ SPDX-License-Identifier: Apache-2.0
     </model>
 
     <model name='brick0'>
-      <pose>0.5 0 0 0 0 0</pose>
+      <pose>0.5 0 0.03 0 0 0</pose>
       <include>
         <uri>model://rcdt_gazebo/models/brick</uri>
       </include>

ros2_ws/src/rcdt_grasping/CMakeLists.txt

@@ -0,0 +1,36 @@
+# SPDX-FileCopyrightText: Alliander N. V.
+#
+# SPDX-License-Identifier: Apache-2.0
+
+cmake_minimum_required(VERSION 3.5)
+project(rcdt_grasping)
+
+# CMake dependencies:
+find_package(ament_cmake REQUIRED)
+find_package(ament_cmake_python REQUIRED)
+
+# Other dependencies:
+find_package(rclpy REQUIRED)
+find_package(rcdt_utilities REQUIRED)
+
+ament_python_install_package(${PROJECT_NAME})
+
+# Python executables:
+install(
+  DIRECTORY src_py/
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+# Shared folders:
+install(
+  DIRECTORY launch 
+  DESTINATION share/${PROJECT_NAME}
+)
+
+# Default test:
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

ros2_ws/src/rcdt_grasping/checkpoint/checkpoint.tar

@@ -0,0 +1,3 @@
+SPDX-FileCopyrightText: Alliander N. V.
+
+SPDX-License-Identifier: Apache-2.0

ros2_ws/src/rcdt_grasping/checkpoint/checkpoint.tar.license

@@ -0,0 +1,60 @@
+# SPDX-FileCopyrightText: Alliander N. V.
+#
+# SPDX-License-Identifier: Apache-2.0
+
+from launch import LaunchContext, LaunchDescription
+from launch.actions import OpaqueFunction
+from launch_ros.actions import Node
+from rcdt_utilities.launch_utils import LaunchArgument
+from rcdt_utilities.register import Register
+
+namespace_arm_arg = LaunchArgument("namespace_arm", "")
+namespace_camera_arg = LaunchArgument("namespace_camera", "")
+
+
+def launch_setup(context: LaunchContext) -> list:
+    """Setup the launch description for the grasping node.
+
+    Args:
+        context (LaunchContext): The launch context.
+
+    Returns:
+        list: A list of actions to be executed in the launch description.
+    """
+    namespace_arm = namespace_arm_arg.string_value(context)
+    namespace_camera = namespace_camera_arg.string_value(context)
+
+    graspnet_node = Node(
+        package="rcdt_grasping",
+        executable="generate_grasp.py",
+    )
+
+    grasping_logic = Node(
+        package="rcdt_grasping",
+        executable="grasp_logic.py",
+        parameters=[
+            {"namespace_arm": namespace_arm, "namespace_camera": namespace_camera}
+        ],
+    )
+
+    return [
+        Register.on_log(
+            graspnet_node, "GraspNet model initialized and weights loaded", context
+        ),
+        Register.on_log(grasping_logic, "GraspLogicNode initialized!", context),
+    ]
+
+
+def generate_launch_description() -> LaunchDescription:
+    """Generate the launch description for the grasping node.
+
+    Returns:
+        LaunchDescription: The launch description containing the grasping node.
+    """
+    return LaunchDescription(
+        [
+            namespace_arm_arg.declaration,
+            namespace_camera_arg.declaration,
+            OpaqueFunction(function=launch_setup),
+        ]
+    )

ros2_ws/src/rcdt_grasping/launch/grasping.launch.py

@@ -0,0 +1,27 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+
+<!--
+SPDX-FileCopyrightText: Alliander N. V.
+
+SPDX-License-Identifier: Apache-2.0
+-->
+
+<package format="3">
+  <name>rcdt_grasping</name>
+  <version>0.1.0</version>
+  <description>Package that contains functionality for grasping objects.</description>
+  <maintainer email="[email protected]">RCDT</maintainer>
+  <license>Apache 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>ament_cmake_python</buildtool_depend>
+
+  <depend>rcdt_utilities</depend>
+
+  <exec_depend>rclpy</exec_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>