435 integrate meta (#437)

* Add Meta platform to our system.

Signed-off-by: Jelmer de Wolde <jelmer.de.wolde@alliander.com>

* First teleoperation.

Signed-off-by: Jelmer de Wolde <jelmer.de.wolde@alliander.com>

* Rewrite teleoperation python node to CPP meta manager node.

Signed-off-by: Jelmer de Wolde <jelmer.de.wolde@alliander.com>

* Use meta as flag, not as platform.

Signed-off-by: Jelmer de Wolde <jelmer.de.wolde@alliander.com>

* Add support for GUI debugging using dev container.

Signed-off-by: Jelmer de Wolde <jelmer.de.wolde@alliander.com>

* Tune servo params.

Signed-off-by: Jelmer de Wolde <jelmer.de.wolde@alliander.com>

* Switch to meta_quest_reader package.

Signed-off-by: Jelmer de Wolde <jelmer.de.wolde@alliander.com>

* Switch to ppadb-reborn that includes fixes for escape sequences.

Signed-off-by: Jelmer de Wolde <jelmer.de.wolde@alliander.com>

* Add scrcpy to stream quest.

Signed-off-by: Jelmer de Wolde <jelmer.de.wolde@alliander.com>

* Start other containers only when meta launches correctly.

Signed-off-by: Jelmer de Wolde <jelmer.de.wolde@alliander.com>

* Fix linting.

Signed-off-by: Jelmer de Wolde <jelmer.de.wolde@alliander.com>

* Fix configuration, to support Meta, Joystick and GUI.

Signed-off-by: Jelmer de Wolde <jelmer.de.wolde@alliander.com>

* Set servo command type from joystick manager and meta manager.

Signed-off-by: Jelmer de Wolde <jelmer.de.wolde@alliander.com>

* Add documentation.

Signed-off-by: Jelmer de Wolde <jelmer.de.wolde@alliander.com>

* Undo scale changes in servo params.

Signed-off-by: Jelmer de Wolde <jelmer.de.wolde@alliander.com>

* Apply feedback.

Signed-off-by: Jelmer de Wolde <jelmer.de.wolde@alliander.com>

---------

Signed-off-by: Jelmer de Wolde <jelmer.de.wolde@alliander.com>
Signed-off-by: Peter Geurts <peter.geurts@proton.me>

Author: Jelmerdw

.devcontainer/dev/Dockerfile

@@ -43,6 +43,11 @@ RUN git clone --depth=1 --filter=blob:none -b v3.1.1 \
     && cd src/franka_ros2 \
     && git sparse-checkout set franka_msgs
 
+# Install debug tools:
+RUN apt update && apt install -y --no-install-recommends \
+    ros-$ROS_DISTRO-plotjuggler-ros \
+    ros-$ROS_DISTRO-rqt-tf-tree
+
 # Setup configuration files:
 COPY .devcontainer/dev/.dev_bashrc /.dev_bashrc
 RUN echo "source /.dev_bashrc" >> /home/$REMOTE_USER/.bashrc
@@ -52,6 +57,7 @@ COPY alliander_core/src/ /alliander/ros/src
 COPY alliander_diagnostics/src/ /alliander/ros/src
 COPY alliander_franka/src/ /alliander/ros/src
 COPY alliander_joystick/src/ /alliander/ros/src
+COPY alliander_meta/src/ /alliander/ros/src
 COPY alliander_moveit/src/ /alliander/ros/src
 COPY alliander_nav2/src/ /alliander/ros/src
 COPY alliander_xsens/src/ /alliander/ros/src

.devcontainer/dev/devcontainer.json

@@ -14,8 +14,21 @@
   "workspaceMount": "source=${localWorkspaceFolder},target=/alliander_robotics,type=bind",
   "workspaceFolder": "/alliander_robotics",
   "overrideCommand": true,
+  "mounts": [
+    "source=/tmp/.X11-unix,target=/tmp/.X11-unix,type=bind,consistency=cached"
+  ],
   "runArgs": [
-    "--rm"
+    "--rm",
+    "--privileged",
+    "--network=host",
+    "-e",
+    "DISPLAY",
+    "-e",
+    "NVIDIA_VISIBLE_DEVICES=all",
+    "-e",
+    "NVIDIA_DRIVER_CAPABILITIES=all",
+    "-e",
+    "RMW_IMPLEMENTATION=rmw_cyclonedds_cpp"
   ],
   "customizations": {
     "vscode": {

alliander_core/src/alliander_description/franka/urdf/franka.urdf.xacro

@@ -11,7 +11,7 @@ SPDX-License-Identifier: Apache-2.0
   <xacro:arg name="xyz_ee" default="0 0 0" />
   <xacro:arg name="rpy_ee" default="0 0 ${-pi/4}" />
   <xacro:arg name="tcp_xyz" default="0 0 0.1034" />
-  <xacro:arg name="tcp_rpy" default="0 0 0" />
+  <xacro:arg name="tcp_rpy" default="3.1415 0 0" />
 
   <!-- Custom arguments: -->
   <xacro:arg name="namespace" default="" />

alliander_joystick/alliander_joystick.Dockerfile

@@ -7,6 +7,9 @@ FROM $BASE_IMAGE
 ARG COLCON_BUILD_SEQUENTIAL
 ENV ROS_DISTRO=jazzy
 
+# Install ROS depenencies:
+RUN apt update && apt install -y ros-$ROS_DISTRO-moveit-msgs
+
 # Install repo packages:
 WORKDIR /$WORKDIR/ros
 COPY alliander_core/src/ /$WORKDIR/ros/src

alliander_joystick/src/alliander_joystick/CMakeLists.txt

@@ -13,12 +13,13 @@ find_package(rclcpp REQUIRED)
 find_package(rclcpp_action REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(sensor_msgs REQUIRED)
+find_package(moveit_msgs REQUIRED)
 find_package(std_srvs REQUIRED)
 find_package(alliander_interfaces REQUIRED)
 
 # C++ executables:
 include_directories(include)
-add_executable(joystick_manager 
+add_executable(joystick_manager
   src/main.cpp
   src/joystick_manager.cpp
 )
@@ -30,6 +31,7 @@ ament_target_dependencies(joystick_manager
   rclcpp_action
   geometry_msgs
   sensor_msgs
+  moveit_msgs
   std_srvs
   alliander_interfaces
 )

alliander_joystick/src/alliander_joystick/include/joystick_manager.hpp

@@ -9,6 +9,7 @@
 #include <alliander_interfaces/srv/string_srv.hpp>
 #include <cstdint>
 #include <geometry_msgs/msg/twist_stamped.hpp>
+#include <moveit_msgs/srv/servo_command_type.hpp>
 #include <rclcpp/client.hpp>
 #include <rclcpp/node.hpp>
 #include <rclcpp/node_options.hpp>
@@ -22,6 +23,7 @@
 using std::placeholders::_1;
 using std::placeholders::_2;
 typedef alliander_interfaces::action::TriggerAction TriggerAction;
+typedef moveit_msgs::srv::ServoCommandType ServoCommandType;
 
 enum Button {
   A = 0,   // Switch between platform modes
@@ -62,28 +64,19 @@ class JoystickManager {
   /// Client to move the arm back to home position
   rclcpp::Client<alliander_interfaces::srv::StringSrv>::SharedPtr
       srv_client_arm_home;
+  /// Client to change the command type of the servo node
+  rclcpp::Client<ServoCommandType>::SharedPtr
+      srv_client_switch_servo_command_type;
   /// Client to trigger the arm's gripper to open
   rclcpp_action::Client<TriggerAction>::SharedPtr action_client_gripper_open;
   /// Client to trigger the arm's gripper to close
   rclcpp_action::Client<TriggerAction>::SharedPtr action_client_gripper_close;
 
   // ROS2 get parameter variables:
-  /// The arm twist topic
-  std::string arm_topic;
-  /// Frame ID of the arm
-  std::string arm_frame_id;
-  /// Name of the arm's gripper
-  std::string arm_gripper_name;
-  /// Service name with which the arm can be moved back to its home position
-  std::string arm_home_service;
-  /// Service name with which the arm's servo node can be paused
-  std::string arm_pause_servo_service;
-  /// The vehicle twist topic
-  std::string vehicle_topic;
-  /// Service name for resetting the vehicle's E-stop
-  std::string vehicle_estop_reset_service;
-  /// Service name for triggering the vehicle's E-stop
-  std::string vehicle_estop_trigger_service;
+  /// The namespace of the arm
+  std::string namespace_arm;
+  /// The namespace of the vehicle
+  std::string namespace_vehicle;
 
   // Other variables:
   /// Previous input values of the Joy message
@@ -182,6 +175,11 @@ class JoystickManager {
    */
   void move_arm_to_home();
 
+  /**
+   * @brief Call the service to switch the command type of the servo node
+   */
+  void switch_servo_command_type();
+
   /**
    * @brief (un)pause the servo node via a service request.
    * @param pause indication whether the servo node should be paused (true) or

alliander_joystick/src/alliander_joystick/launch/joystick.launch.py

@@ -29,15 +29,15 @@ def launch_setup(context: LaunchContext) -> list:
     platforms = PlatformList.from_str(platform_list_arg.string_value(context)).platforms
 
     # Create default namespaces which can be ignored when left unchanged
-    arm_namespace = "/arm"
-    vehicle_namespace = "/vehicle"
+    arm_namespace = "arm"
+    vehicle_namespace = "vehicle"
 
     # Find arm / vehicle platforms in the platform list
     for platform in platforms:
         match platform.platform_type:
             case "Arm":
-                if arm_namespace == "/arm":
-                    arm_namespace = f"/{platform.namespace}"
+                if arm_namespace == "arm":
+                    arm_namespace = platform.namespace
                 else:
                     warnings.warn(
                         "No support for multiple arms yet, only accepting the first arm.",
@@ -46,8 +46,8 @@ def launch_setup(context: LaunchContext) -> list:
                     )
 
             case "Vehicle":
-                if vehicle_namespace == "/vehicle":
-                    vehicle_namespace = f"/{platform.namespace}"
+                if vehicle_namespace == "vehicle":
+                    vehicle_namespace = platform.namespace
                 else:
                     warnings.warn(
                         "No support for multiple vehicles yet, only accepting the first vehicle.",
@@ -58,7 +58,7 @@ def launch_setup(context: LaunchContext) -> list:
             case _:
                 pass
 
-    if arm_namespace == "/arm" and vehicle_namespace == "/vehicle":
+    if arm_namespace == "arm" and vehicle_namespace == "vehicle":
         raise RuntimeError(
             "No arm/vehicle platform present, cancelling the joystick manager."
         )
@@ -68,16 +68,8 @@ def launch_setup(context: LaunchContext) -> list:
         executable="joystick_manager",
         name="joystick_manager",
         parameters=[
-            {"arm_cmd_topic": f"{arm_namespace}/servo_node/delta_twist_cmds"},
-            {"arm_frame_id": f"{arm_namespace[1:]}/fr3_link1"},
-            {"arm_gripper_name": f"{arm_namespace}/gripper"},
-            {
-                "arm_home_service": f"{arm_namespace}/moveit_manager/move_to_configuration"
-            },
-            {"arm_pause_servo_service": f"{arm_namespace}/servo_node/pause_servo"},
-            {"vehicle_cmd_topic": f"{vehicle_namespace}/cmd_vel_joy"},
-            {"vehicle_estop_reset": f"{vehicle_namespace}/hardware/e_stop_reset"},
-            {"vehicle_estop_trigger": f"{vehicle_namespace}/hardware/e_stop_trigger"},
+            {"namespace_arm": arm_namespace},
+            {"namespace_vehicle": vehicle_namespace},
         ],
     )
 

alliander_joystick/src/alliander_joystick/src/joystick_manager.cpp

@@ -5,17 +5,8 @@
 #include "joystick_manager.hpp"
 
 JoystickManager::JoystickManager(rclcpp::Node::SharedPtr node) : node(node) {
-  arm_topic = node->get_parameter("arm_cmd_topic").as_string();
-  arm_frame_id = node->get_parameter("arm_frame_id").as_string();
-  arm_gripper_name = node->get_parameter("arm_gripper_name").as_string();
-  arm_home_service = node->get_parameter("arm_home_service").as_string();
-  arm_pause_servo_service =
-      node->get_parameter("arm_pause_servo_service").as_string();
-  vehicle_topic = node->get_parameter("vehicle_cmd_topic").as_string();
-  vehicle_estop_reset_service =
-      node->get_parameter("vehicle_estop_reset").as_string();
-  vehicle_estop_trigger_service =
-      node->get_parameter("vehicle_estop_trigger").as_string();
+  namespace_arm = node->get_parameter("namespace_arm").as_string();
+  namespace_vehicle = node->get_parameter("namespace_vehicle").as_string();
 
   initialize_joystick_manager();
 
@@ -36,27 +27,29 @@ void JoystickManager::initialize_joystick_manager() {
       std::bind(&JoystickManager::joy_cb, this, _1));
 
   // Publishers
-  pub_arm_vel =
-      node->create_publisher<geometry_msgs::msg::TwistStamped>(arm_topic, 10);
+  pub_arm_vel = node->create_publisher<geometry_msgs::msg::TwistStamped>(
+      "/" + namespace_arm + "/servo_node/delta_twist_cmds", 10);
   pub_vehicle_vel = node->create_publisher<geometry_msgs::msg::TwistStamped>(
-      vehicle_topic, 10);
+      "/" + namespace_vehicle + "/cmd_vel_joy", 10);
 
   // Service clients
   srv_client_estop_trigger = node->create_client<std_srvs::srv::Trigger>(
-      vehicle_estop_trigger_service);
-  srv_client_estop_reset =
-      node->create_client<std_srvs::srv::Trigger>(vehicle_estop_reset_service);
-  srv_client_pause_servo =
-      node->create_client<std_srvs::srv::SetBool>(arm_pause_servo_service);
+      "/" + namespace_vehicle + "/hardware/e_stop_trigger");
+  srv_client_estop_reset = node->create_client<std_srvs::srv::Trigger>(
+      "/" + namespace_vehicle + "/hardware/e_stop_reset");
+  srv_client_pause_servo = node->create_client<std_srvs::srv::SetBool>(
+      "/" + namespace_arm + "/servo_node/pause_servo");
   srv_client_arm_home =
       node->create_client<alliander_interfaces::srv::StringSrv>(
-          arm_home_service);
+          "/" + namespace_arm + "/moveit_manager/move_to_configuration");
+  srv_client_switch_servo_command_type = node->create_client<ServoCommandType>(
+      "/" + namespace_arm + "/servo_node/switch_command_type");
 
   // Action clients
   action_client_gripper_open = rclcpp_action::create_client<TriggerAction>(
-      node, arm_gripper_name + "/open");
+      node, "/" + namespace_arm + "/open");
   action_client_gripper_close = rclcpp_action::create_client<TriggerAction>(
-      node, arm_gripper_name + "/close");
+      node, "/" + namespace_arm + "/close");
 
   // Log initial mode
   switch (current_mode) {
@@ -125,11 +118,13 @@ void JoystickManager::handle_button_input(const std::vector<int32_t>& buttons) {
         return;
       case vehicle_mode:
         RCLCPP_INFO(node->get_logger(), "Switch to ARM mode.");
+        switch_servo_command_type();
         current_mode = arm_mode;
         pub_vehicle_vel->publish(geometry_msgs::msg::TwistStamped{});
         return;
       case no_mode:
         RCLCPP_INFO(node->get_logger(), "Switch to ARM mode.");
+        switch_servo_command_type();
         current_mode = arm_mode;
         return;
       default:
@@ -222,7 +217,7 @@ void JoystickManager::handle_arm_movement(const float& x, const float& y,
                                           const float& rotation) {
   geometry_msgs::msg::TwistStamped twist;
   twist.header.stamp = node->now();
-  twist.header.frame_id = arm_frame_id;
+  twist.header.frame_id = namespace_arm + "/" + "fr3_link1";
 
   twist.twist.linear.x = (std::abs(x) > dead_axis_zone)
                              ? (x * arm_speed_scale)
@@ -283,6 +278,35 @@ void JoystickManager::move_arm_to_home() {
       });
 }
 
+// Switch the servo command type to TWIST:
+void JoystickManager::switch_servo_command_type() {
+  auto request = std::make_shared<ServoCommandType::Request>();
+  request->command_type = 1;
+
+  if (!srv_client_switch_servo_command_type->service_is_ready()) {
+    RCLCPP_WARN(node->get_logger(),
+                "'Switch servo command type' service not available.");
+  }
+
+  srv_client_switch_servo_command_type->async_send_request(
+      request, [this](rclcpp::Client<ServoCommandType>::SharedFuture future) {
+        try {
+          auto response = future.get();
+
+          if (response->success) {
+            RCLCPP_INFO(node->get_logger(), "Servo command type is TWIST.");
+          } else {
+            RCLCPP_WARN(node->get_logger(),
+                        "Failed to set servo command type.");
+          }
+        } catch (const std::exception& e) {
+          RCLCPP_ERROR(node->get_logger(),
+                       "'Switch servo command type' service call failed: %s",
+                       e.what());
+        }
+      });
+}
+
 void JoystickManager::pause_servo_node(bool pause) {
   auto request = std::make_shared<std_srvs::srv::SetBool::Request>();
   request->data = pause;

alliander_meta/alliander_meta.Dockerfile

@@ -0,0 +1,40 @@
+# SPDX-FileCopyrightText: Alliander N. V.
+#
+# SPDX-License-Identifier: Apache-2.0
+ARG BASE_IMAGE=ubuntu:latest
+FROM $BASE_IMAGE 
+
+ARG COLCON_BUILD_SEQUENTIAL
+ENV ROS_DISTRO=jazzy
+
+# Install adb:
+RUN apt update && apt install -y android-tools-adb
+
+# Install srcpy:
+RUN apt update && apt install -y \
+  ffmpeg libsdl2-2.0-0 adb wget gcc git pkg-config meson ninja-build libsdl2-dev libavcodec-dev \
+  libavdevice-dev libavformat-dev libavutil-dev libswresample-dev libusb-1.0-0 libusb-1.0-0-dev \
+  && git clone https://github.com/Genymobile/scrcpy \
+  && cd scrcpy \
+  && ./install_release.sh
+
+# Install ROS depenencies:
+RUN apt update && apt install -y ros-$ROS_DISTRO-moveit-msgs
+
+# Install repo packages:
+WORKDIR /$WORKDIR/ros
+COPY alliander_core/src/ /$WORKDIR/ros/src
+COPY alliander_meta/src/ /$WORKDIR/ros/src
+RUN /$WORKDIR/colcon_build.sh
+
+# Install python dependencies:
+WORKDIR $WORKDIR
+COPY pyproject.toml/ /$WORKDIR/pyproject.toml
+RUN uv sync --group alliander-meta \
+  && echo "export PYTHONPATH=\"$(dirname $(dirname $(uv python find)))/lib/python3.12/site-packages:\$PYTHONPATH\"" >> /root/.bashrc \
+  && echo "export PATH=\"$(dirname $(dirname $(uv python find)))/bin:\$PATH\"" >> /root/.bashrc
+
+# Finalize
+WORKDIR /$WORKDIR
+ENTRYPOINT ["/entrypoint.sh"]
+CMD ["sleep", "infinity"]

alliander_meta/docker-compose.yml

@@ -0,0 +1,17 @@
+# SPDX-FileCopyrightText: Alliander N. V.
+#
+# SPDX-License-Identifier: Apache-2.0
+services:
+  alliander_meta:
+    image: allianderrobotics/meta
+    container_name: alliander_meta
+    network_mode: host
+    privileged: true
+    mem_limit: 6gb
+    tty: true
+    volumes:
+      - "/tmp/.X11-unix:/tmp/.X11-unix"
+      - "/dev:/dev"
+    env_file:
+      - .env
+    command: ["/bin/bash", "-c", "ros2 launch alliander_meta meta.launch.py"]