enable FCI and unlock Franka programmatically (#189)

MaxWaterhout · Max Waterhout · web-flow · commit d6f2db6e2925 · 2025-06-25T10:12:27.000+02:00
Add franka_lock_unlock script
---------

Signed-off-by: Max Waterhout &lt;max.waterhout@soprasteria.com&gt;
Signed-off-by: Max Waterhout &lt;max.waterhout@hotmail.nl&gt;
Co-authored-by: Max Waterhout &lt;max.waterhout@soprasteria.com&gt;
diff --git a/.devcontainer/.env b/.devcontainer/.env
@@ -1,5 +1,14 @@
 # SPDX-FileCopyrightText: Alliander N. V.
-#
+# 
 # SPDX-License-Identifier: Apache-2.0
 
-PRE_COMMIT_HOME=/home/rcdt/rcdt_robotics/.cache/pre-commit
+# Define IGN_IP to use Gazebo with firewall enabled:
+IGN_IP=127.0.0.1
+
+# Set QT_API to pyside6 for PyFlow to work:
+QT_API=pyside6
+
+PRE_COMMIT_HOME=/home/rcdt/rcdt_robotics/.cache/pre-commit
+
+# Set the RMW implementation to Cyclone DDS for communication with the Panther:
+RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
diff --git a/.env b/.env
diff --git a/.env.example b/.env.example
@@ -0,0 +1,7 @@
+# SPDX-FileCopyrightText: Alliander N. V.
+# 
+# SPDX-License-Identifier: Apache-2.0
+
+FRANKA_HOSTNAME=10.15.20.4
+FRANKA_USERNAME=...
+FRANKA_PASSWORD=...
diff --git a/.github/workflows/reusable-workspace.yml b/.github/workflows/reusable-workspace.yml
@@ -40,7 +40,6 @@ jobs:
         shell: bash -ieo pipefail {0}
         run: |
           source /home/rcdt/.bashrc
-          source .env
           cd ros2_ws
           source install/setup.bash
           echo "GZ_SIM_RESOURCE_PATH: ${GZ_SIM_RESOURCE_PATH}"
diff --git a/.gitignore b/.gitignore
@@ -20,5 +20,8 @@ ignore/
 .vscode-server
 
 build/
+
+.env
+!.devcontainer/.env
 .cache
-.venv
+.venv
diff --git a/dockerfiles/install_scripts/franka_lock_unlock.sh b/dockerfiles/install_scripts/franka_lock_unlock.sh
@@ -0,0 +1,16 @@
+#!/bin/bash -i
+
+# SPDX-FileCopyrightText: Alliander N. V.
+#
+# SPDX-License-Identifier: Apache-2.0
+
+mkdir -p /home/$UNAME/franka_lock_unlock/src
+cd /home/$UNAME/franka_lock_unlock
+git clone https://github.com/alliander-opensource/franka_lock_unlock.git
+
+rosdep update
+rosdep install --from-paths src --rosdistro humble -y -i
+
+source /home/$UNAME/.bashrc
+colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release
+echo "source /home/$UNAME/franka_lock_unlock/install/setup.bash" >>/home/$UNAME/.bashrc
diff --git a/dockerfiles/rcdt_robotics.Dockerfile b/dockerfiles/rcdt_robotics.Dockerfile
@@ -36,6 +36,9 @@ RUN ./sensors.sh
 COPY ./install_scripts/sphinx.sh .
 RUN ./sphinx.sh
 
+COPY ./install_scripts/franka_lock_unlock.sh .
+RUN ./franka_lock_unlock.sh
+
 COPY ./install_scripts/dev_packages.sh .
 RUN ./dev_packages.sh
 
diff --git a/docs/content/franka.md b/docs/content/franka.md
@@ -53,19 +53,38 @@ ros2 run rosboard rosboard_node
 
 Running the brick pickup demo can be done in the same way.
 
-## Setup & FCI activation of pysical robot
 
-First plug an ehternet cable into your laptop, and connect it to the control box's LAN port (this is not the LAN port on the arm itself).  
-Additionally, change your ethernet ipv4 settings to Manual and set the following:  
-Address: `172.16.0.1`\
-Netmask: `255.255.255.0`
+## Setup & FCI activation of physical robot
 
-![pyflow](../img/franka/network.png)
+### Programmatic unlock via `.env`
+
+You can unlock the robot automatically without using the browser by setting environment variables in a `.env` file at the root of your workspace:
+
+```dotenv
+# .env
+FRANKA_HOSTNAME=your-hostname
+FRANKA_USERNAME=your-username
+FRANKA_PASSWORD=your-password
+```
+
+When you run the launch command, the robot will automatically unlock and activate the FCI.
+
+This feature is based on the [jk-ethz/franka\_lock\_unlock](https://github.com/jk-ethz/franka_lock_unlock) repository. We maintain a fork with specific enhancements for our robot at [alliander-opensource/franka\_lock\_unlock](https://github.com/alliander-opensource/franka_lock_unlock.git).
+
+### Manual unlock via web interface
+
+You can also manually lock the robot via the web interface, plug an Ethernet cable from your laptop into the control box’s LAN port (this is not the LAN port on the arm itself).  Additionally, change your Ethernet IPv4 settings to **Manual** and set the following:
+
+```text
+Address: 172.16.0.1
+Netmask: 255.255.255.0
+```
+
+![Network Setup](../img/franka/network.png)
 
 You may also want to consider making this a separate profile for future convenience when moving to different networks.
 
-If the robot's power switch is turned on, you should now be able to go to [https://172.16.0.2](https://172.16.0.2).  
-Fill in the username and password, and click unlock.  
-After that, click `My Franka Robot> Activate FCI`. The robot is now ready to be controlled by the user, and the commands in quickstart can be run.
+If the robot's power switch is turned on, you should now be able to go to [https://172.16.0.2](https://172.16.0.2).
+Fill in the username and password, and click **Unlock**. After that, click **My Franka Robot → Activate FCI**. The robot is now ready to be controlled by the user, and the commands in Quickstart can be run.
 
 ---TODO: screenshot here----
diff --git a/ros2_ws/src/rcdt_franka/launch/core.launch.py b/ros2_ws/src/rcdt_franka/launch/core.launch.py
@@ -18,6 +18,7 @@
 load_gazebo_ui_arg = LaunchArgument("load_gazebo_ui", False, [True, False])
 world_arg = LaunchArgument("world", "empty_camera.sdf")
 use_realsense_arg = LaunchArgument("realsense", False, [True, False])
+enable_lock_unlock_arg = LaunchArgument("franka_lock_unlock", True, [True, False])
 
 
 def launch_setup(context: LaunchContext) -> list:
@@ -34,6 +35,7 @@ def launch_setup(context: LaunchContext) -> list:
     load_gazebo_ui = load_gazebo_ui_arg.bool_value(context)
     world = str(world_arg.string_value(context))
     use_realsense = use_realsense_arg.bool_value(context)
+    enable_lock_unlock = enable_lock_unlock_arg.bool_value(context)
 
     namespace = "franka"
     frame_prefix = namespace + "/" if namespace else ""
@@ -66,7 +68,10 @@ def launch_setup(context: LaunchContext) -> list:
         )
     else:
         robot = RegisteredLaunchDescription(
-            get_file_path("rcdt_franka", ["launch"], "robot.launch.py")
+            get_file_path("rcdt_franka", ["launch"], "robot.launch.py"),
+            launch_arguments={
+                "franka_lock_unlock": str(enable_lock_unlock),
+            },
         )
 
     # Create a tf frame called 'base', required for the MotionPlanning plugin in Rviz:
@@ -102,6 +107,7 @@ def generate_launch_description() -> LaunchDescription:
     """
     return LaunchDescription(
         [
+            enable_lock_unlock_arg.declaration,
             use_sim_arg.declaration,
             parent_arg.declaration,
             load_gazebo_ui_arg.declaration,
diff --git a/ros2_ws/src/rcdt_franka/launch/franka.launch.py b/ros2_ws/src/rcdt_franka/launch/franka.launch.py
@@ -15,6 +15,7 @@
     "world", "table_with_1_brick.sdf", ["table_with_1_brick.sdf", "empty_camera.sdf"]
 )
 use_realsense_arg = LaunchArgument("realsense", False, [True, False])
+enable_lock_unlock_arg = LaunchArgument("franka_lock_unlock", True, [True, False])
 
 
 def launch_setup(context: LaunchContext) -> list:
@@ -31,6 +32,7 @@ def launch_setup(context: LaunchContext) -> list:
     use_rviz = use_rviz_arg.bool_value(context)
     world = str(world_arg.string_value(context))
     use_realsense = use_realsense_arg.bool_value(context)
+    enable_lock_unlock = enable_lock_unlock_arg.bool_value(context)
 
     namespace = "franka"
     ns = f"/{namespace}" if namespace else ""
@@ -42,6 +44,7 @@ def launch_setup(context: LaunchContext) -> list:
             "load_gazebo_ui": str(load_gazebo_ui),
             "realsense": str(use_realsense),
             "world": world,
+            "franka_lock_unlock": str(enable_lock_unlock),
         },
     )
 
@@ -110,6 +113,7 @@ def generate_launch_description() -> LaunchDescription:
     """
     return LaunchDescription(
         [
+            enable_lock_unlock_arg.declaration,
             use_sim_arg.declaration,
             load_gazebo_ui_arg.declaration,
             use_rviz_arg.declaration,
diff --git a/ros2_ws/src/rcdt_franka/launch/robot.launch.py b/ros2_ws/src/rcdt_franka/launch/robot.launch.py
@@ -2,53 +2,101 @@
 #
 # SPDX-License-Identifier: Apache-2.0
 
-from launch import LaunchDescription
-from launch.actions import Shutdown
+import os
+
+from launch import LaunchContext, LaunchDescription
+from launch.actions import OpaqueFunction, Shutdown
 from launch_ros.actions import Node
-from rcdt_utilities.launch_utils import get_file_path
-
-namespace = "franka"
-ns = f"/{namespace}" if namespace else ""
-
-franka_controllers = param_file = get_file_path(
-    "rcdt_franka", ["config"], "controllers.yaml"
-)
-
-ros2_control_node = Node(
-    package="controller_manager",
-    executable="ros2_control_node",
-    parameters=[
-        franka_controllers,
-        {"arm_id": "fr3"},
-    ],
-    remappings=[
-        (f"{ns}/controller_manager/robot_description", f"{ns}/robot_description"),
-        (f"{ns}/joint_states", f"{ns}/fr3_arm/joint_states"),
-    ],
-    namespace=namespace,
-    on_exit=Shutdown(),
-)
-
-settings_setter = Node(
-    package="rcdt_franka",
-    executable="settings_setter.py",
-)
-
-joint_state_publisher = Node(
-    package="joint_state_publisher",
-    executable="joint_state_publisher",
-    name="joint_state_publisher",
-    parameters=[
-        {
-            "source_list": [
-                f"{ns}/fr3_arm/joint_states",
-                f"{ns}/fr3_gripper/joint_states",
-            ],
-            "rate": 30,
-        }
-    ],
-    namespace=namespace,
-)
+from rcdt_utilities.launch_utils import SKIP, LaunchArgument, get_file_path
+from rcdt_utilities.register import Register
+
+enable_lock_unlock_arg = LaunchArgument("franka_lock_unlock", True, [True, False])
+
+
+def launch_setup(context: LaunchContext) -> list:
+    """Setup the launch description for the Franka robot controllers.
+
+    Args:
+        context (LaunchContext): The launch context.
+
+    Raises:
+        RuntimeError: If the required environment variables are not set.
+
+    Returns:
+        list: A list of actions to be executed in the launch description.
+    """
+    enable_lock_unlock = enable_lock_unlock_arg.bool_value(context)
+
+    namespace = "franka"
+    ns = f"/{namespace}" if namespace else ""
+
+    hostname = os.getenv("FRANKA_HOSTNAME", "")
+    username = os.getenv("FRANKA_USERNAME", "")
+    password = os.getenv("FRANKA_PASSWORD", "")
+
+    if not hostname or not username or not password:
+        raise RuntimeError(
+            """You must set FRANKA_HOSTNAME, FRANKA_USERNAME and FRANKA_PASSWORD
+            in your environment if you want to enable the Franka Lock/Unlock
+            node programmatically, otherwise set franka_lock_unlock:=False ."""
+        )
+
+    # only include the node if we actually have a hostname and password
+    franka_lock_unlock = Node(
+        name="franka_lock_unlock",
+        package="franka_lock_unlock",
+        executable="franka_lock_unlock.py",
+        output="screen",
+        arguments=[hostname, username, password, "-u", "-l", "-w", "-r", "-p", "-c"],
+        respawn=True,
+    )
+
+    franka_controllers = get_file_path("rcdt_franka", ["config"], "controllers.yaml")
+
+    ros2_control_node = Node(
+        package="controller_manager",
+        executable="ros2_control_node",
+        parameters=[
+            franka_controllers,
+            {"arm_id": "fr3"},
+        ],
+        remappings=[
+            (f"{ns}/controller_manager/robot_description", f"{ns}/robot_description"),
+            (f"{ns}/joint_states", f"{ns}/fr3_arm/joint_states"),
+        ],
+        namespace=namespace,
+        on_exit=Shutdown(),
+    )
+
+    settings_setter = Node(
+        package="rcdt_franka",
+        executable="settings_setter.py",
+    )
+
+    joint_state_publisher = Node(
+        package="joint_state_publisher",
+        executable="joint_state_publisher",
+        name="joint_state_publisher",
+        parameters=[
+            {
+                "source_list": [
+                    f"{ns}/fr3_arm/joint_states",
+                    f"{ns}/fr3_gripper/joint_states",
+                ],
+                "rate": 30,
+            }
+        ],
+        namespace=namespace,
+    )
+
+    return [
+        Register.on_log(franka_lock_unlock, "Keeping persistent connection...", context)
+        if enable_lock_unlock
+        else SKIP,
+        Register.on_start(settings_setter, context),
+        Register.on_start(ros2_control_node, context),
+        Register.on_start(joint_state_publisher, context),
+    ]
 
 
 def generate_launch_description() -> LaunchDescription:
@@ -59,8 +107,7 @@ def generate_launch_description() -> LaunchDescription:
     """
     return LaunchDescription(
         [
-            settings_setter,
-            ros2_control_node,
-            joint_state_publisher,
+            enable_lock_unlock_arg.declaration,
+            OpaqueFunction(function=launch_setup),
         ]
     )
