277 make a ros wrapper (#285)

* Make a ros wrapper.

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

* Undo unnecessary changes.

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

* Introduce gui.

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

* Visualize gripper pose in Rviz.

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

* Correct gripper visualization with offset between wrist and tcp.

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

* Add animated visualization of plan.

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

* Clean up code.

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

---------

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

Author: MaxWaterhout

pyproject.toml

@@ -47,6 +47,7 @@ dependencies = [
     "xmltodict>=1.0.2",
     "pandas>=2.2.0",
     "trimesh>=4.8.2",
+    "nicegui>=3.0.4",
 ]
 
 [dependency-groups]
@@ -104,4 +105,4 @@ select = "DOC"
 style = "google"
 skip-checking-short-docstrings = false
 allow-init-docstring = true
-check-style-mismatch = true
+check-style-mismatch = true

ros2_ws/src/rcdt_franka_moveit_config/config/fr3.srdf

@@ -28,7 +28,7 @@ are defined
     group-->
     <!--SUBGROUPS:
     Groups can also be formed by referencing to already defined group names-->
-    <group name="fr3_arm">
+    <group name="arm">
         <joint name="fr3_joint1" />
         <joint name="fr3_joint2" />
         <joint name="fr3_joint3" />
@@ -37,10 +37,20 @@ are defined
         <joint name="fr3_joint6" />
         <joint name="fr3_joint7" />
     </group>
+    <group name="hand">
+        <joint name="fr3_joint8" />
+        <joint name="fr3_hand_joint" />
+        <joint name="fr3_hand_tcp_joint" />
+        <joint name="fr3_finger_joint1" />
+        <joint name="fr3_finger_joint2" />
+    </group>
+    <group name="tcp">
+        <link name="fr3_hand_tcp" />
+    </group>
     <!--GROUP_STATES:
     Purpose: Define a named state for a particular group, in terms of joint values. This is
     useful to define states like 'folded arms'-->
-    <group_state name="home" group="fr3_arm">
+    <group_state name="home" group="arm">
         <joint name="fr3_joint1" value="0" />
         <joint name="fr3_joint2" value="-0.78539816339" />
         <joint name="fr3_joint3" value="0" />
@@ -49,7 +59,7 @@ are defined
         <joint name="fr3_joint6" value="1.57079632679" />
         <joint name="fr3_joint7" value="0.78539816339" />
     </group_state>
-    <group_state name="drop" group="fr3_arm">
+    <group_state name="drop" group="arm">
         <joint name="fr3_joint1" value="-1.57079632679" />
         <joint name="fr3_joint2" value="-0.65" />
         <joint name="fr3_joint3" value="0" />
@@ -61,6 +71,7 @@ are defined
 
     <!--END_EFFECTOR:
     Purpose: Represent information about an end effector.-->
+    <end_effector name="hand" parent_link="fr3_link7" group="hand" parent_group="arm" />
     <!--DISABLE_COLLISIONS:
     By default it is assumed that any link of the robot could potentially come into
     collision with any other link in the robot. This tag disables collision checking between a

ros2_ws/src/rcdt_franka_moveit_config/config/kinematics.yaml

@@ -2,7 +2,7 @@
 #
 # SPDX-License-Identifier: Apache-2.0
 
-fr3_arm:
+arm:
   kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
   kinematics_solver_search_resolution: 0.0050000000000000001
   kinematics_solver_timeout: 0.0050000000000000001

ros2_ws/src/rcdt_franka_moveit_config/config/servo_params.yaml

@@ -7,7 +7,7 @@
 thread_priority: 40
 publish_period: 0.001
 max_expected_latency: 0.1
-move_group_name: fr3_arm
+move_group_name: arm
 active_subgroup: ""
 
 ############################# INCOMING COMMAND SETTINGS ########################

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

@@ -123,6 +123,7 @@ def generate_launch_description() -> LaunchDescription:
             world_arg.declaration,
             platforms_arg.declaration,
             positions_arg.declaration,
+            orientations_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,42 @@
+# 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.register import Register
+
+
+def launch_setup(context: LaunchContext) -> list:
+    """Setup the launch description for the node.
+
+    Args:
+        context (LaunchContext): The launch context.
+
+    Returns:
+        list: A list of actions to be executed in the launch description.
+    """
+    graspnet_node = Node(
+        package="rcdt_grasping",
+        executable="generate_grasp.py",
+    )
+
+    return [
+        Register.on_log(
+            graspnet_node, "GraspNet model initialized and weights loaded", 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(
+        [
+            OpaqueFunction(function=launch_setup),
+        ]
+    )