add teleop controller (backport #35) (#37)

* add teleop controller (#35)

* import teleoperation msgs

* import and refractor teleoperation controller
(from https://github.com/tpoignonec/passive_vic_teleop_ros2 / commit SHA = de3d28f0d4564299251f8d38f36908fe1d762b5c)

* fix teleop controller name

* fix rule plugin export

* remove testing for now

* misc fixes

* #IF guard for humble

* fix deps

* comment out humble-specific code

(cherry picked from commit 5d12107)

* reintroduce humble specific precautions in teleop ctrl

---------

Co-authored-by: Thibault Poignonec <79221188+tpoignonec@users.noreply.github.com>
Co-authored-by: Thibault Poignonec <tpoignonec@unistra.fr>

cartesian_control_msgs/CMakeLists.txt

@@ -19,12 +19,18 @@ find_package(geometry_msgs REQUIRED)
 find_package(std_msgs REQUIRED)
 
 set(msg_files
+# General cartesian msgs
   msg/CartesianState.msg
   msg/CartesianTrajectoryPoint.msg
   msg/CartesianTrajectory.msg
   msg/CartesianCompliance.msg
+  # VIC msgs
   msg/CompliantFrameTrajectory.msg
   msg/VicControllerState.msg
+  # Teleoperation msgs
+  msg/TeleopControllerState.msg
+  msg/TeleopCompliance.msg
+  # Debug msgs
   msg/KeyValues.msg
 )
 

cartesian_control_msgs/msg/TeleopCompliance.msg

@@ -0,0 +1,8 @@
+# Note: all matrices are expressed in Follower frame of reference!!!
+std_msgs/Float64MultiArray leader_desired_inertia
+std_msgs/Float64MultiArray leader_desired_stiffness
+std_msgs/Float64MultiArray leader_desired_damping
+
+std_msgs/Float64MultiArray follower_desired_inertia
+std_msgs/Float64MultiArray follower_desired_stiffness
+std_msgs/Float64MultiArray follower_desired_damping

cartesian_control_msgs/msg/TeleopControllerState.msg

@@ -0,0 +1,44 @@
+std_msgs/Header  header
+
+# Teleop controller input data
+std_msgs/Bool workspace_is_engaged
+
+geometry_msgs/Pose x_1
+geometry_msgs/Twist x_1_dot
+geometry_msgs/Accel x_1_ddot
+geometry_msgs/Wrench f_1
+
+std_msgs/Float64MultiArray desired_inertia_1
+std_msgs/Float64MultiArray desired_damping_1
+std_msgs/Float64MultiArray desired_stiffness_1
+
+geometry_msgs/Pose x_2
+geometry_msgs/Twist x_2_dot
+geometry_msgs/Accel x_2_ddot
+geometry_msgs/Wrench f_2
+
+std_msgs/Float64MultiArray desired_inertia_2
+std_msgs/Float64MultiArray desired_damping_2
+std_msgs/Float64MultiArray desired_stiffness_2
+
+# Teleop controller ref
+geometry_msgs/Pose x_1_d
+geometry_msgs/Twist x_1_dot_d
+geometry_msgs/Accel x_1_ddot_d
+geometry_msgs/Wrench f_1_d
+
+std_msgs/Float64MultiArray rendered_inertia_1
+std_msgs/Float64MultiArray rendered_damping_1
+std_msgs/Float64MultiArray rendered_stiffness_1
+
+geometry_msgs/Pose x_2_d
+geometry_msgs/Twist x_2_dot_d
+geometry_msgs/Accel x_2_ddot_d
+geometry_msgs/Wrench f_2_d
+
+std_msgs/Float64MultiArray rendered_inertia_2
+std_msgs/Float64MultiArray rendered_damping_2
+std_msgs/Float64MultiArray rendered_stiffness_2
+
+# Misc.
+cartesian_control_msgs/KeyValues diagnostic_data

cartesian_vic_controller/CMakeLists.txt

@@ -82,7 +82,7 @@ target_link_libraries(cartesian_vic_rules PUBLIC
 ament_target_dependencies(cartesian_vic_rules PUBLIC ${DEPENDENCIES})
 pluginlib_export_plugin_description_file(cartesian_vic_controller cartesian_vic_rules.xml)
 
-if(BUILD_TESTING)
+if(0)  # (BUILD_TESTING)
   find_package(ament_cmake_gmock REQUIRED)
   find_package(controller_manager REQUIRED)
   find_package(ros2_control_test_assets REQUIRED)

cartesian_vic_teleop_controller/CMakeLists.txt

@@ -0,0 +1,107 @@
+cmake_minimum_required(VERSION 3.16)
+project(cartesian_vic_teleop_controller LANGUAGES CXX)
+
+if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+
+# Use CMake to pass the current ROS_DISTRO via variables into a preprocessor template.
+# We then include this file and switch between the different APIs.
+if($ENV{ROS_DISTRO} STREQUAL "rolling")
+  set(CARTESIAN_CONTROLLERS_ROLLING TRUE)
+elseif($ENV{ROS_DISTRO} STREQUAL "jazzy")
+        set(CARTESIAN_CONTROLLERS_JAZZY TRUE)
+elseif($ENV{ROS_DISTRO} STREQUAL "humble")
+        set(CARTESIAN_CONTROLLERS_HUMBLE TRUE)
+else()
+        message(WARNING "ROS2 version must be {rolling|jazzy|humble}")
+endif()
+configure_file(include/cartesian_vic_teleop_controller/Ros2VersionConfig.h.in include/cartesian_vic_teleop_controller/Ros2VersionConfig.h)
+
+set(DEPENDENCIES
+  Eigen3
+  pluginlib
+  rclcpp
+  rclcpp_lifecycle
+  realtime_tools
+  tf2
+  tf2_eigen
+  tf2_geometry_msgs
+  tf2_kdl
+  tf2_ros
+  trajectory_msgs
+  cartesian_control_msgs
+  cartesian_vic_controller
+)
+
+find_package(ament_cmake REQUIRED)
+find_package(backward_ros REQUIRED)
+foreach(Dependency IN ITEMS ${DEPENDENCIES})
+  find_package(${Dependency} REQUIRED)
+endforeach()
+
+add_library(${PROJECT_NAME} SHARED
+  # ROS2 controller impl.
+  src/cartesian_vic_teleop_controller.cpp
+  # Logic
+  src/cartesian_vic_teleop_logic.cpp
+  src/mapping_manager.cpp
+  src/teleop_data.cpp
+  src/teleop_rule.cpp
+  # Async node impl.
+  # TODO
+)
+target_compile_features(${PROJECT_NAME} PUBLIC cxx_std_17)
+target_include_directories(${PROJECT_NAME} PUBLIC
+  $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
+  # $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/src/rules>  # LP solvers
+  $<BUILD_INTERFACE:${CMAKE_BINARY_DIR}/include>  # ROS2VersionConfig.h
+  $<INSTALL_INTERFACE:include/${PROJECT_NAME}>
+)
+ament_target_dependencies(${PROJECT_NAME} PUBLIC ${DEPENDENCIES})
+
+# Causes the visibility macros to use dllexport rather than dllimport,
+# which is appropriate when building the dll but not consuming it.
+target_compile_definitions(${PROJECT_NAME} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")
+target_compile_definitions(${PROJECT_NAME} PRIVATE "cartesian_vic_teleop_controller_BUILDING_DLL")
+
+pluginlib_export_plugin_description_file(controller_interface cartesian_vic_teleop_controller.xml)
+
+# VIC-based teleoperation rule plugins library
+add_library(cartesian_vic_teleop_rules SHARED
+  src/rules/vanilla_teleop_rule.cpp
+)
+target_compile_features(cartesian_vic_teleop_rules PUBLIC cxx_std_17)
+target_include_directories(cartesian_vic_teleop_rules PUBLIC
+  $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include/cartesian_vic_teleop_controller>
+)
+target_link_libraries(cartesian_vic_teleop_rules PUBLIC
+  cartesian_vic_teleop_controller
+)
+ament_target_dependencies(
+  cartesian_vic_teleop_rules
+  PUBLIC
+    ${DEPENDENCIES}
+)
+pluginlib_export_plugin_description_file(cartesian_vic_teleop_controller cartesian_vic_teleop_rules.xml)
+
+install(
+  DIRECTORY include/
+  DESTINATION include/${PROJECT_NAME}
+)
+
+install(
+  TARGETS
+    ${PROJECT_NAME}
+    cartesian_vic_teleop_rules
+  EXPORT export_cartesian_vic_teleop_controller
+  RUNTIME DESTINATION bin
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+)
+
+ament_export_targets(export_cartesian_vic_teleop_controller HAS_LIBRARY_TARGET)
+ament_export_dependencies(${DEPENDENCIES})
+ament_package()

cartesian_vic_teleop_controller/cartesian_vic_teleop_controller.xml

@@ -0,0 +1,9 @@
+<library path="cartesian_vic_teleop_controller">
+  <class name="cartesian_vic_teleop_controller/CartesianVicTeleopController"
+         type="cartesian_vic_teleop_controller::CartesianVicTeleopController"
+         base_class_type="controller_interface::ControllerInterface">
+    <description>
+      Cartesian teleoperation controller (leader-side only!) based on the cartesian_vic_controller package.
+    </description>
+  </class>
+</library>

cartesian_vic_teleop_controller/cartesian_vic_teleop_rules.xml

@@ -0,0 +1,9 @@
+<library path="cartesian_vic_teleop_rules">
+  <class name="cartesian_vic_teleop_controller/VanillaTeleopRule"
+         type="cartesian_vic_teleop_controller::VanillaTeleopRule"
+         base_class_type="cartesian_vic_teleop_controller::TeleopRule">
+    <description>
+      Basic position-position with force feedback.
+    </description>
+  </class>
+</library>

cartesian_vic_teleop_controller/include/cartesian_vic_teleop_controller/Ros2VersionConfig.h.in

@@ -0,0 +1,3 @@
+#cmakedefine CARTESIAN_VIC_TELEOP_CONTROLLER_ROLLING ${CARTESIAN_VIC_TELEOP_CONTROLLER_ROLLING}
+#cmakedefine CARTESIAN_VIC_TELEOP_CONTROLLER_JAZZY ${CARTESIAN_VIC_TELEOP_CONTROLLER_JAZZY}
+#cmakedefine CARTESIAN_VIC_TELEOP_CONTROLLER_HUMBLE ${CARTESIAN_VIC_TELEOP_CONTROLLER_HUMBLE}

cartesian_vic_teleop_controller/include/cartesian_vic_teleop_controller/cartesian_vic_teleop_controller.hpp

@@ -0,0 +1,129 @@
+// Copyright 2024 ICUBE Laboratory, University of Strasbourg
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+//
+/// \authors: Thibault Poignonec <thibault.poignonec@gmail.com>
+
+#ifndef CARTESIAN_VIC_TELEOP_CONTROLLER__CARTESIAN_VIC_TELEOP_CONTROLLER_HPP_
+#define CARTESIAN_VIC_TELEOP_CONTROLLER__CARTESIAN_VIC_TELEOP_CONTROLLER_HPP_
+
+#include <chrono>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "cartesian_vic_teleop_controller/visibility_control.h"
+#include "cartesian_vic_teleop_controller/mapping_manager.hpp"
+#include "cartesian_vic_teleop_controller/cartesian_vic_teleop_logic.hpp"
+
+#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
+#include "rclcpp_lifecycle/state.hpp"
+#include "realtime_tools/realtime_buffer.h"
+#include "realtime_tools/realtime_publisher.h"
+
+// Base class for VIC controller
+#include "cartesian_vic_controller/cartesian_vic_controller.hpp"
+
+// ROS2 standard msgs
+#include "std_msgs/msg/bool.hpp"
+
+// Custom msgs
+#include "cartesian_control_msgs/msg/vic_controller_state.hpp"
+#include "cartesian_control_msgs/msg/cartesian_trajectory.hpp"
+#include "cartesian_control_msgs/msg/compliant_frame_trajectory.hpp"
+#include "cartesian_control_msgs/msg/teleop_controller_state.hpp"
+#include "cartesian_control_msgs/msg/teleop_compliance.hpp"
+
+namespace cartesian_vic_teleop_controller
+{
+class CartesianVicTeleopController : public cartesian_vic_controller::CartesianVicController
+{
+public:
+  CartesianVicTeleopController();
+  virtual ~CartesianVicTeleopController() = default;
+
+  CARTESIAN_VIC_TELEOP_CONTROLLER_PUBLIC
+  controller_interface::CallbackReturn on_init() override;
+
+  CARTESIAN_VIC_TELEOP_CONTROLLER_PUBLIC
+  controller_interface::return_type update(
+    const rclcpp::Time & time, const rclcpp::Duration & period) override;
+
+  CARTESIAN_VIC_TELEOP_CONTROLLER_PUBLIC
+  controller_interface::CallbackReturn on_configure(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+  CARTESIAN_VIC_TELEOP_CONTROLLER_PUBLIC
+  controller_interface::CallbackReturn on_activate(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+  CARTESIAN_VIC_TELEOP_CONTROLLER_PUBLIC
+  controller_interface::CallbackReturn on_deactivate(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+  CARTESIAN_VIC_TELEOP_CONTROLLER_PUBLIC
+  controller_interface::CallbackReturn on_cleanup(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+  CARTESIAN_VIC_TELEOP_CONTROLLER_PUBLIC
+  controller_interface::CallbackReturn on_error(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+  using Base = cartesian_vic_controller::CartesianVicController;
+
+protected:
+  PassiveVicTeleopLogic teleop_logic_;
+
+  // Publisher teleop controller state (for logging purposes only)
+  rclcpp::Publisher<cartesian_control_msgs::msg::TeleopControllerState>::SharedPtr
+    non_rt_teleop_state_publisher_;
+  std::unique_ptr<realtime_tools::RealtimePublisher<
+      cartesian_control_msgs::msg::TeleopControllerState>> teleop_state_publisher_;
+  cartesian_control_msgs::msg::TeleopControllerState teleop_state_msg_;
+
+  // Publisher follower robot VIC reference trajectory
+  rclcpp::Publisher<cartesian_control_msgs::msg::CompliantFrameTrajectory>::SharedPtr
+    non_rt_follower_vic_ref_publisher_;
+  std::unique_ptr<realtime_tools::RealtimePublisher<
+      cartesian_control_msgs::msg::CompliantFrameTrajectory>> follower_vic_ref_publisher_;
+
+  // Subscriber to follower VIC state
+  // TODO(tpoignonec): remove follower_vic_state_msg_
+  cartesian_control_msgs::msg::VicControllerState follower_vic_state_msg_;
+  std::shared_ptr<cartesian_control_msgs::msg::VicControllerState>
+  follower_vic_state_msg_ptr_;
+  rclcpp::Subscription<cartesian_control_msgs::msg::VicControllerState>::SharedPtr
+    follower_vic_state_subscriber_;
+  realtime_tools::RealtimeBuffer<std::shared_ptr<
+      cartesian_control_msgs::msg::VicControllerState>> input_follower_vic_state_msg_;
+
+  // Subscriber to teleoperation compliance ref.
+  rclcpp::Subscription<
+    cartesian_control_msgs::msg::TeleopCompliance>::SharedPtr teleop_compliance_subscriber_;
+  realtime_tools::RealtimeBuffer<std::shared_ptr<
+      cartesian_control_msgs::msg::TeleopCompliance>> input_teleop_compliance_msg_;
+  std::shared_ptr<cartesian_control_msgs::msg::TeleopCompliance> teleop_compliance_msg_ptr_;
+
+  // Subscriber to follower VIC state
+  rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr is_clutched_subscriber_;
+  realtime_tools::RealtimeBuffer<std::shared_ptr<std_msgs::msg::Bool>> input_is_clutched_msg_;
+  std::shared_ptr<std_msgs::msg::Bool> is_clutched_msg_ptr_;
+
+  // Storage
+  cartesian_control_msgs::msg::CompliantFrameTrajectory leader_vic_ref_;
+  cartesian_control_msgs::msg::CompliantFrameTrajectory follower_vic_ref_;
+};
+
+}  // namespace cartesian_vic_teleop_controller
+
+#endif  // CARTESIAN_VIC_TELEOP_CONTROLLER__CARTESIAN_VIC_TELEOP_CONTROLLER_HPP_