336 create joystick

.devcontainer/dev/Dockerfile

@@ -46,6 +46,8 @@ RUN echo "source /.dev_bashrc" >> /home/$REMOTE_USER/.bashrc
 
 # Copy repo packages:
 COPY alliander_core/src/ /alliander/ros/src
+COPY alliander_franka/src/ /alliander/ros/src
+COPY alliander_joystick/src/ /alliander/ros/src
 COPY alliander_moveit/src/ /alliander/ros/src
 
 # Build repo packages:

.github/workflows/docker.yml

@@ -227,7 +227,7 @@ jobs:
     strategy:
       matrix: ${{ fromJson(needs.detect-changes.outputs.matrix_ubuntu) }}
     needs: [detect-changes, build-ubuntu-images]
-    if: needs.detect-changes.outputs.has_changes_ubuntu == 'true'
+    if: always() && needs.detect-changes.outputs.has_changes_ubuntu == 'true'
     runs-on: ubuntu-24.04
     steps:
       - name: Docker login
@@ -346,7 +346,7 @@ jobs:
     strategy:
       matrix: ${{ fromJson(needs.detect-changes.outputs.matrix_cuda) }}
     needs: [detect-changes, build-cuda-images]
-    if: needs.detect-changes.outputs.has_changes_cuda == 'true'
+    if: always() && needs.detect-changes.outputs.has_changes_cuda == 'true'
     runs-on: ubuntu-24.04
     steps:
       - name: Docker login
@@ -371,7 +371,7 @@ jobs:
 
   linting:
     runs-on: ubuntu-24.04
-    needs: [build-ubuntu-images, build-cuda-images]
+    needs: [create-manifests-ubuntu, create-manifests-cuda]
     if: always()
     steps:
       - uses: actions/checkout@v4
@@ -391,7 +391,7 @@ jobs:
       matrix:
         runner: [ubuntu-24.04, ubuntu-24.04-arm]
     runs-on: ${{ matrix.runner }}
-    needs: [build-ubuntu-images, build-cuda-images]
+    needs: [create-manifests-ubuntu, create-manifests-cuda]
     steps:
       - uses: actions/checkout@v4
       - name: Free disk space

alliander_joystick/alliander_joystick.Dockerfile

@@ -0,0 +1,25 @@
+# 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 repo packages:
+WORKDIR /$WORKDIR/ros
+COPY alliander_core/src/ /$WORKDIR/ros/src
+COPY alliander_joystick/src/ /$WORKDIR/ros/src
+RUN /$WORKDIR/colcon_build.sh
+
+# Install python dependencies:
+WORKDIR $WORKDIR
+COPY pyproject.toml /$WORKDIR/pyproject.toml
+RUN uv sync \
+  && 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
+
+WORKDIR /$WORKDIR
+ENTRYPOINT ["/entrypoint.sh"]
+CMD ["sleep", "infinity"]

alliander_joystick/docker-compose.yml

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

alliander_joystick/src/rcdt_joystick/CMakeLists.txt

@@ -0,0 +1,54 @@
+# SPDX-FileCopyrightText: Alliander N. V.
+#
+# SPDX-License-Identifier: Apache-2.0
+
+cmake_minimum_required(VERSION 3.5)
+project(alliander_joystick)
+
+# CMake dependencies:
+find_package(ament_cmake REQUIRED)
+
+# Other dependencies:
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_action REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(std_srvs REQUIRED)
+find_package(alliander_interfaces REQUIRED)
+
+# C++ executables:
+include_directories(include)
+add_executable(joystick_manager 
+  src/main.cpp
+  src/joystick_manager.cpp
+)
+target_include_directories(joystick_manager PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>)
+ament_target_dependencies(joystick_manager
+  rclcpp
+  rclcpp_action
+  geometry_msgs
+  sensor_msgs
+  std_srvs
+  alliander_interfaces
+)
+
+install(
+  TARGETS joystick_manager
+  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()

alliander_joystick/src/rcdt_joystick/include/joystick_manager.hpp

@@ -0,0 +1,234 @@
+// # SPDX-FileCopyrightText: Alliander N. V.
+//
+// # SPDX-License-Identifier: Apache-2.0
+
+#ifndef JOYSTICK_MANAGER_HPP_
+#define JOYSTICK_MANAGER_HPP_
+
+#include <alliander_interfaces/action/trigger_action.hpp>
+#include <alliander_interfaces/srv/string_srv.hpp>
+#include <cstdint>
+#include <geometry_msgs/msg/twist_stamped.hpp>
+#include <rclcpp/client.hpp>
+#include <rclcpp/node.hpp>
+#include <rclcpp/node_options.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp_action/client.hpp>
+#include <rclcpp_action/rclcpp_action.hpp>
+#include <sensor_msgs/msg/joy.hpp>
+#include <std_srvs/srv/set_bool.hpp>
+#include <std_srvs/srv/trigger.hpp>
+
+using std::placeholders::_1;
+using std::placeholders::_2;
+typedef alliander_interfaces::action::TriggerAction TriggerAction;
+
+enum Button {
+  A = 0,   // Switch between platform modes
+  B = 1,   // Move arm back to home position
+  X = 2,   // Trigger E-stop
+  Y = 3,   // Reset E-stop
+  LT = 9,  // Open gripper
+  RT = 10  // Close gripper
+};
+
+/// Class to interact with the joystick.
+class JoystickManager {
+ public:
+  /**
+   * @brief constructor for the JoystickManager class.
+   * @param node The ROS2 node to attach to.
+   */
+  JoystickManager(rclcpp::Node::SharedPtr node);
+  ~JoystickManager();
+
+ private:
+  // ROS2 communication variables:
+  /// The ROS2 node
+  rclcpp::Node::SharedPtr node;
+  /// Subsciber for the Joy topic
+  rclcpp::Subscription<sensor_msgs::msg::Joy>::SharedPtr sub_joy;
+  /// Publisher for the arm movement velocities
+  rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr pub_arm_vel;
+  /// Publisher for the vehicle driving velocities
+  rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr
+      pub_vehicle_vel;
+  /// Client to trigger the vehicle's E-stop
+  rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr srv_client_estop_trigger;
+  /// Client to reset the vehicle's E-stop
+  rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr srv_client_estop_reset;
+  /// Client to pause and unpause the servo node
+  rclcpp::Client<std_srvs::srv::SetBool>::SharedPtr srv_client_pause_servo;
+  /// Client to move the arm back to home position
+  rclcpp::Client<alliander_interfaces::srv::StringSrv>::SharedPtr
+      srv_client_arm_home;
+  /// 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;
+
+  // Other variables:
+  /// Previous input values of the Joy message
+  sensor_msgs::msg::Joy::SharedPtr prev_joy_input;
+  /// Current platform mode of the joystick
+  int current_mode = no_mode;
+  /// Indication whether the arm is following a trajectory or not
+  bool arm_busy = false;
+  /// Indication whether the gripper is busy or not
+  bool gripper_busy = false;
+
+  // constant variables:
+  /// Arm platform mode code
+  static constexpr int arm_mode = 0;
+  /// Vehicle platform mode code
+  static constexpr int vehicle_mode = 1;
+  /// No platform mode code
+  static constexpr int no_mode = 99;
+  /// Makes response to changes in joystick values less sensitive with a 'dead
+  /// zone'.
+  const float dead_axis_zone = 0.2;
+  /// Scale the arm's speed, max possible value provided by the joystick: 1.0
+  /// m/s
+  const float arm_speed_scale = 0.2;
+  /// Scale the vehicle's speed, max possible value provided by the
+  /// joystick: 1.0 m/s
+  const float vehicle_speed_scale = 0.4;
+
+  /**
+   * @brief initialize the joystick manager.
+   */
+  void initialize_joystick_manager();
+
+  /**
+   * @brief callback method that handles the data received from the joystick.
+   * @param msg joystick sensor message.
+   */
+  void joy_cb(const sensor_msgs::msg::Joy::SharedPtr msg);
+
+  /**
+   * @brief based on the button input, activate corresponding behaviour.
+   * @param buttons all current button input values as received on the joystick
+   * topic.
+   */
+  void handle_button_input(const std::vector<int32_t>& buttons);
+
+  /**
+   * @brief matches buttons with the corresponding behaviour as expected for an
+   * arm platform.
+   * @param buttons all current button input values as received on the joystick
+   * topic.
+   */
+  void handle_buttons_arm(const std::vector<int32_t>& buttons);
+
+  /**
+   * @brief matches buttons with the corresponding behaviour as expected for an
+   * vehicle platform.
+   * @param buttons all current button input values as received on the joystick
+   * topic.
+   */
+  void handle_buttons_vehicle(const std::vector<int32_t>& buttons);
+
+  /**
+   * @brief Check whether a button has been pressed or not.
+   * @param idx desired index value of the button.
+   * @param curr all current button input values as received on the joystick
+   * topic.
+   * @param prev all previous button input values as received on the joystick
+   * topic.
+   * @return boolean indicating whether the button is pressed (True) or not
+   * (False).
+   */
+  bool check_btn_pressed(size_t idx, const std::vector<int32_t>& curr,
+                         const std::vector<int32_t>& prev);
+
+  /**
+   * @brief publish TwistStamped message based on linear and angular value.
+   * @param linear linear velocity of the vehicle (m/s).
+   * @param angular angular velocity of the vehicle (m/s).
+   */
+  void handle_driving(const float& linear, const float& angular);
+
+  /**
+   * @brief publish TwistStamped message based on [x, y, z] translation and a
+   * rotation value.
+   * @param x velocity of the arm moving forwards/backwards.
+   * @param y velocity of the arm moving left/right.
+   * @param z velocity of the arm moving up/down.
+   * @param rotation velocity of the arm rotating the gripper.
+   */
+  void handle_arm_movement(const float& x, const float& y, const float& z,
+                           const float& rotation);
+
+  /**
+   * @brief send service request to move the arm back to its home position.
+   */
+  void move_arm_to_home();
+
+  /**
+   * @brief (un)pause the servo node via a service request.
+   * @param pause indication whether the servo node should be paused (true) or
+   * unpaused (false).
+   */
+  void pause_servo_node(bool pause);
+
+  /**
+   * @brief send a trigger service request with the provided service client.
+   * @param client service client with which the request will be send.
+   */
+  void send_trigger_request(
+      const rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr& client);
+
+  /**
+   * @brief send the gripper the request to either open or close and mark the
+   * gripper as "busy" via a variable value.
+   * @param client ...
+   */
+  void send_gripper_goal(
+      const rclcpp_action::Client<TriggerAction>::SharedPtr& client);
+
+  /**
+   * @brief log whether the gripper task was accepted or not. If not, the
+   * gripper is marked as free again via variable value.
+   * @param goal_handle variable that contains information about the client's
+   * goal (status/result).
+   */
+  void gripper_goal_response_callback(
+      std::shared_ptr<rclcpp_action::ClientGoalHandle<TriggerAction>>
+          goal_handle);
+
+  /**
+   * @brief log the status of the gripper task.
+   * @param feedback the feedback received from the action server.
+   */
+  void gripper_feedback_callback(
+      std::shared_ptr<rclcpp_action::ClientGoalHandle<TriggerAction>>,
+      const std::shared_ptr<const TriggerAction::Feedback> feedback);
+
+  /**
+   * @brief log the result of the gripper task and mark the gripper as free to
+   * use again via variable value.
+   * @param result the result received from the action server.
+   */
+  void gripper_result_callback(
+      rclcpp_action::ClientGoalHandle<TriggerAction>::WrappedResult& result);
+};
+
+#endif  // JOYSTICK_MANAGER_HPP_