From 065a987c7bd6eaf24695b3994407364286756995 Mon Sep 17 00:00:00 2001
From: Niko <nikolaus.feith@unileoben.ac.at>
Date: Thu, 21 Mar 2024 13:03:03 +0100
Subject: [PATCH] urdf and srdf are not passed as dicts but as
 {'robot_description': <launch.substitutions.command.Command object at
 0x7358a27fb6d0>} -> maybe the mistake

---
 .../config/initial_positions.yaml             |  9 +++++
 .../launch/ur3e_robotiq.launch.py             | 36 ++++++++++---------
 .../srdf/ur_robotiq.srdf                      | 13 +++++++
 3 files changed, 41 insertions(+), 17 deletions(-)
 create mode 100644 src/ur_robotiq_moveit_config/config/initial_positions.yaml
 create mode 100644 src/ur_robotiq_moveit_config/srdf/ur_robotiq.srdf

diff --git a/src/ur_robotiq_moveit_config/config/initial_positions.yaml b/src/ur_robotiq_moveit_config/config/initial_positions.yaml
new file mode 100644
index 0000000..3a72ceb
--- /dev/null
+++ b/src/ur_robotiq_moveit_config/config/initial_positions.yaml
@@ -0,0 +1,9 @@
+# Default initial positions for ur_robotiq's ros2_control fake system
+
+initial_positions:
+  elbow_joint: 0
+  shoulder_lift_joint: 0
+  shoulder_pan_joint: 0
+  wrist_1_joint: 0
+  wrist_2_joint: 0
+  wrist_3_joint: 0
\ No newline at end of file
diff --git a/src/ur_robotiq_moveit_config/launch/ur3e_robotiq.launch.py b/src/ur_robotiq_moveit_config/launch/ur3e_robotiq.launch.py
index 97ad952..d1ed76e 100644
--- a/src/ur_robotiq_moveit_config/launch/ur3e_robotiq.launch.py
+++ b/src/ur_robotiq_moveit_config/launch/ur3e_robotiq.launch.py
@@ -38,6 +38,7 @@ def generate_launch_description():
 
     # File and package names
     ur_description_package = "ur_description"
+    ur_moveit_config_package = "ur_moveit_config"
     ur_robotiq_description_package = "ur_robotiq_description"
     ur_robotiq_description_file = "ur_robotiq.urdf.xacro"
     moveit_config_package = "ur_robotiq_moveit_config"
@@ -125,9 +126,9 @@ def generate_launch_description():
 
     robot_description_kinematics = load_yaml(moveit_config_package, "config/kinematics.yaml")
 
-    # robot_description_planning = {
-    # "robot_description_planning": load_yaml_abs(str(joint_limit_params.perform(context)))
-    # }
+    robot_description_planning = {
+        "robot_description_planning": load_yaml(ur_moveit_config_package, "config/joint_limits.yaml")
+    }
 
     # Planning Configuration
     ompl_planning_pipeline_config = {
@@ -185,7 +186,7 @@ def generate_launch_description():
             robot_description,
             robot_description_semantic,
             robot_description_kinematics,
-            # robot_description_planning,
+            robot_description_planning,
             ompl_planning_pipeline_config,
             trajectory_execution,
             moveit_controllers,
@@ -194,9 +195,10 @@ def generate_launch_description():
         ],
     )
 
-    robot_state_publisher_node = launch_ros.actions.Node(
+    robot_state_publisher_node = Node(
         package="robot_state_publisher",
         executable="robot_state_publisher",
+        output="both",
         parameters=[robot_description],
     )
 
@@ -216,7 +218,7 @@ def generate_launch_description():
             robot_description_semantic,
             ompl_planning_pipeline_config,
             robot_description_kinematics,
-            # robot_description_planning,
+            robot_description_planning,
         ],
     )
 
@@ -224,17 +226,17 @@ def generate_launch_description():
 
     servo_yaml = load_yaml(moveit_config_package, "config/ur_servo.yaml")
     servo_params = {"moveit_servo": servo_yaml}
-    # servo_node = Node(
-    #     package="moveit_servo",
-    #     condition=IfCondition(launch_servo),
-    #     executable="servo_node_main",
-    #     parameters=[
-    #         servo_params,
-    #         robot_description,
-    #         robot_description_semantic,
-    #     ],
-    #     output="screen",
-    # )
+    servo_node = Node(
+        package="moveit_servo",
+        condition=IfCondition(launch_servo),
+        executable="servo_node_main",
+        parameters=[
+            servo_params,
+            robot_description,
+            robot_description_semantic,
+        ],
+        output="screen",
+    )
 
     # Declare the Launch arguments
     # UR specific arguments
diff --git a/src/ur_robotiq_moveit_config/srdf/ur_robotiq.srdf b/src/ur_robotiq_moveit_config/srdf/ur_robotiq.srdf
new file mode 100644
index 0000000..53b6aa4
--- /dev/null
+++ b/src/ur_robotiq_moveit_config/srdf/ur_robotiq.srdf
@@ -0,0 +1,13 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<robot xmlns:xacro="http://wiki.ros.org/xacro" name="ur_robotiq_srdf">
+
+  <!-- robot name parameter -->
+  <xacro:arg name="name" default="ur"/>
+  <!-- parameters -->
+  <xacro:arg name="prefix" default="" />
+
+  <xacro:include filename="$(find ur_robotiq_moveit_config)/srdf/ur_robotiq_macro.srdf.xacro"/>
+
+  <xacro:ur_robotiq_srdf name="$(arg name)" prefix="$(arg prefix)"/>
+
+</robot>