From df8401df7be3047e36e8442d60566472d8c6bf44 Mon Sep 17 00:00:00 2001
From: Sebastian Jahr <sebastian.jahr@picknik.ai>
Date: Thu, 21 Dec 2023 10:13:00 +0100
Subject: [PATCH] Update ros2 control usage (#192) (#193)

* Update ros2_control usage

* Update dual_arm_panda_moveit_config/config/panda_hand.ros2_control.xacro

Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com>

---------

Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com>
# Conflicts:
#	dual_arm_panda_moveit_config/config/panda.ros2_control.xacro
#	dual_arm_panda_moveit_config/config/panda_hand.ros2_control.xacro
#	dual_arm_panda_moveit_config/launch/demo.launch.py
---
 .../config/fanuc.ros2_control.xacro           | 24 ++++++++++++----
 fanuc_moveit_config/launch/demo.launch.py     |  5 +++-
 .../config/panda.ros2_control.xacro           | 28 ++++++++++++++-----
 .../config/panda_hand.ros2_control.xacro      |  8 ++++--
 panda_moveit_config/launch/demo.launch.py     |  5 +++-
 5 files changed, 53 insertions(+), 17 deletions(-)

diff --git a/fanuc_moveit_config/config/fanuc.ros2_control.xacro b/fanuc_moveit_config/config/fanuc.ros2_control.xacro
index 607f9199..e79fb2e6 100644
--- a/fanuc_moveit_config/config/fanuc.ros2_control.xacro
+++ b/fanuc_moveit_config/config/fanuc.ros2_control.xacro
@@ -13,42 +13,54 @@
                 <state_interface name="position">
                   <param name="initial_value">${initial_positions['joint_1']}</param>
                 </state_interface>
-                <state_interface name="velocity"/>
+                <state_interface name="velocity">
+                    <param name="initial_value">0.0</param>
+                </state_interface>
             </joint>
             <joint name="joint_2">
                 <command_interface name="position"/>
                 <state_interface name="position">
                   <param name="initial_value">${initial_positions['joint_2']}</param>
                 </state_interface>
-                <state_interface name="velocity"/>
+                <state_interface name="velocity">
+                    <param name="initial_value">0.0</param>
+                </state_interface>
             </joint>
             <joint name="joint_3">
                 <command_interface name="position"/>
                 <state_interface name="position">
                   <param name="initial_value">${initial_positions['joint_3']}</param>
                 </state_interface>
-                <state_interface name="velocity"/>
+                <state_interface name="velocity">
+                    <param name="initial_value">0.0</param>
+                </state_interface>
             </joint>
             <joint name="joint_4">
                 <command_interface name="position"/>
                 <state_interface name="position">
                   <param name="initial_value">${initial_positions['joint_4']}</param>
                 </state_interface>
-                <state_interface name="velocity"/>
+                <state_interface name="velocity">
+                    <param name="initial_value">0.0</param>
+                </state_interface>
             </joint>
             <joint name="joint_5">
                 <command_interface name="position"/>
                 <state_interface name="position">
                   <param name="initial_value">${initial_positions['joint_5']}</param>
                 </state_interface>
-                <state_interface name="velocity"/>
+                <state_interface name="velocity">
+                    <param name="initial_value">0.0</param>
+                </state_interface>
             </joint>
             <joint name="joint_6">
                 <command_interface name="position"/>
                 <state_interface name="position">
                   <param name="initial_value">${initial_positions['joint_6']}</param>
                 </state_interface>
-                <state_interface name="velocity"/>
+                <state_interface name="velocity">
+                    <param name="initial_value">0.0</param>
+                </state_interface>
             </joint>
         </ros2_control>
     </xacro:macro>
diff --git a/fanuc_moveit_config/launch/demo.launch.py b/fanuc_moveit_config/launch/demo.launch.py
index 6becbcdf..80ec9541 100644
--- a/fanuc_moveit_config/launch/demo.launch.py
+++ b/fanuc_moveit_config/launch/demo.launch.py
@@ -79,7 +79,10 @@ def generate_launch_description():
     ros2_control_node = Node(
         package="controller_manager",
         executable="ros2_control_node",
-        parameters=[moveit_config.robot_description, ros2_controllers_path],
+        parameters=[ros2_controllers_path],
+        remappings=[
+            ("/controller_manager/robot_description", "/robot_description"),
+        ],
         output="both",
     )
 
diff --git a/panda_moveit_config/config/panda.ros2_control.xacro b/panda_moveit_config/config/panda.ros2_control.xacro
index addc3384..d66294c9 100644
--- a/panda_moveit_config/config/panda.ros2_control.xacro
+++ b/panda_moveit_config/config/panda.ros2_control.xacro
@@ -20,49 +20,63 @@
                 <state_interface name="position">
                   <param name="initial_value">${initial_positions['panda_joint1']}</param>
                 </state_interface>
-                <state_interface name="velocity"/>
+                <state_interface name="velocity">
+                  <param name="initial_value">0.0</param>
+                </state_interface>
             </joint>
             <joint name="panda_joint2">
                 <command_interface name="position"/>
                 <state_interface name="position">
                   <param name="initial_value">${initial_positions['panda_joint2']}</param>
                 </state_interface>
-                <state_interface name="velocity"/>
+                <state_interface name="velocity">
+                  <param name="initial_value">0.0</param>
+                </state_interface>
             </joint>
             <joint name="panda_joint3">
                 <command_interface name="position"/>
                 <state_interface name="position">
                   <param name="initial_value">${initial_positions['panda_joint3']}</param>
                 </state_interface>
-                <state_interface name="velocity"/>
+                <state_interface name="velocity">
+                  <param name="initial_value">0.0</param>
+                </state_interface>
             </joint>
             <joint name="panda_joint4">
                 <command_interface name="position"/>
                 <state_interface name="position">
                   <param name="initial_value">${initial_positions['panda_joint4']}</param>
                 </state_interface>
-                <state_interface name="velocity"/>
+                <state_interface name="velocity">
+                  <param name="initial_value">0.0</param>
+                </state_interface>
             </joint>
             <joint name="panda_joint5">
                 <command_interface name="position"/>
                 <state_interface name="position">
                   <param name="initial_value">${initial_positions['panda_joint5']}</param>
                 </state_interface>
-                <state_interface name="velocity"/>
+                <state_interface name="velocity">
+                  <param name="initial_value">0.0</param>
+                </state_interface>
             </joint>
             <joint name="panda_joint6">
                 <command_interface name="position"/>
                 <state_interface name="position">
                   <param name="initial_value">${initial_positions['panda_joint6']}</param>
                 </state_interface>
-                <state_interface name="velocity"/>
+                <state_interface name="velocity">
+                  <param name="initial_value">0.0</param>
+                </state_interface>
             </joint>
             <joint name="panda_joint7">
                 <command_interface name="position"/>
                 <state_interface name="position">
                   <param name="initial_value">${initial_positions['panda_joint7']}</param>
                 </state_interface>
-                <state_interface name="velocity"/>
+                <state_interface name="velocity">
+                  <param name="initial_value">0.0</param>
+                </state_interface>
             </joint>
         </ros2_control>
     </xacro:macro>
diff --git a/panda_moveit_config/config/panda_hand.ros2_control.xacro b/panda_moveit_config/config/panda_hand.ros2_control.xacro
index 06d4d177..9adc4f6c 100644
--- a/panda_moveit_config/config/panda_hand.ros2_control.xacro
+++ b/panda_moveit_config/config/panda_hand.ros2_control.xacro
@@ -18,7 +18,9 @@
                 <state_interface name="position">
                   <param name="initial_value">0.0</param>
                 </state_interface>
-                <state_interface name="velocity"/>
+                <state_interface name="velocity">
+                    <param name="initial_value">0.0</param>
+                </state_interface>
             </joint>
             <joint name="panda_finger_joint2">
                 <param name="mimic">panda_finger_joint1</param>
@@ -27,7 +29,9 @@
                 <state_interface name="position">
                   <param name="initial_value">0.0</param>
                 </state_interface>
-                <state_interface name="velocity"/>
+                <state_interface name="velocity">
+                    <param name="initial_value">0.0</param>
+                </state_interface>
             </joint>
         </ros2_control>
     </xacro:macro>
diff --git a/panda_moveit_config/launch/demo.launch.py b/panda_moveit_config/launch/demo.launch.py
index bdbc12cf..9e7cf8e3 100644
--- a/panda_moveit_config/launch/demo.launch.py
+++ b/panda_moveit_config/launch/demo.launch.py
@@ -116,7 +116,10 @@ def generate_launch_description():
     ros2_control_node = Node(
         package="controller_manager",
         executable="ros2_control_node",
-        parameters=[moveit_config.robot_description, ros2_controllers_path],
+        parameters=[ros2_controllers_path],
+        remappings=[
+            ("/controller_manager/robot_description", "/robot_description"),
+        ],
         output="screen",
     )