Skip to content

Commit 5ccc08b

Browse files
mergify[bot]tpoignonecThibault Poignonec
authored
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>
1 parent 01aa352 commit 5ccc08b

22 files changed

+2602
-1
lines changed

‎cartesian_control_msgs/CMakeLists.txt

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -19,12 +19,18 @@ find_package(geometry_msgs REQUIRED)
1919
find_package(std_msgs REQUIRED)
2020

2121
set(msg_files
22+
# General cartesian msgs
2223
msg/CartesianState.msg
2324
msg/CartesianTrajectoryPoint.msg
2425
msg/CartesianTrajectory.msg
2526
msg/CartesianCompliance.msg
27+
# VIC msgs
2628
msg/CompliantFrameTrajectory.msg
2729
msg/VicControllerState.msg
30+
# Teleoperation msgs
31+
msg/TeleopControllerState.msg
32+
msg/TeleopCompliance.msg
33+
# Debug msgs
2834
msg/KeyValues.msg
2935
)
3036

‎cartesian_control_msgs/msg/TeleopCompliance.msg

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,8 @@
1+
# Note: all matrices are expressed in Follower frame of reference!!!
2+
std_msgs/Float64MultiArray leader_desired_inertia
3+
std_msgs/Float64MultiArray leader_desired_stiffness
4+
std_msgs/Float64MultiArray leader_desired_damping
5+
6+
std_msgs/Float64MultiArray follower_desired_inertia
7+
std_msgs/Float64MultiArray follower_desired_stiffness
8+
std_msgs/Float64MultiArray follower_desired_damping

‎cartesian_control_msgs/msg/TeleopControllerState.msg

Lines changed: 44 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,44 @@
1+
std_msgs/Header header
2+
3+
# Teleop controller input data
4+
std_msgs/Bool workspace_is_engaged
5+
6+
geometry_msgs/Pose x_1
7+
geometry_msgs/Twist x_1_dot
8+
geometry_msgs/Accel x_1_ddot
9+
geometry_msgs/Wrench f_1
10+
11+
std_msgs/Float64MultiArray desired_inertia_1
12+
std_msgs/Float64MultiArray desired_damping_1
13+
std_msgs/Float64MultiArray desired_stiffness_1
14+
15+
geometry_msgs/Pose x_2
16+
geometry_msgs/Twist x_2_dot
17+
geometry_msgs/Accel x_2_ddot
18+
geometry_msgs/Wrench f_2
19+
20+
std_msgs/Float64MultiArray desired_inertia_2
21+
std_msgs/Float64MultiArray desired_damping_2
22+
std_msgs/Float64MultiArray desired_stiffness_2
23+
24+
# Teleop controller ref
25+
geometry_msgs/Pose x_1_d
26+
geometry_msgs/Twist x_1_dot_d
27+
geometry_msgs/Accel x_1_ddot_d
28+
geometry_msgs/Wrench f_1_d
29+
30+
std_msgs/Float64MultiArray rendered_inertia_1
31+
std_msgs/Float64MultiArray rendered_damping_1
32+
std_msgs/Float64MultiArray rendered_stiffness_1
33+
34+
geometry_msgs/Pose x_2_d
35+
geometry_msgs/Twist x_2_dot_d
36+
geometry_msgs/Accel x_2_ddot_d
37+
geometry_msgs/Wrench f_2_d
38+
39+
std_msgs/Float64MultiArray rendered_inertia_2
40+
std_msgs/Float64MultiArray rendered_damping_2
41+
std_msgs/Float64MultiArray rendered_stiffness_2
42+
43+
# Misc.
44+
cartesian_control_msgs/KeyValues diagnostic_data

‎cartesian_vic_controller/CMakeLists.txt

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -82,7 +82,7 @@ target_link_libraries(cartesian_vic_rules PUBLIC
8282
ament_target_dependencies(cartesian_vic_rules PUBLIC ${DEPENDENCIES})
8383
pluginlib_export_plugin_description_file(cartesian_vic_controller cartesian_vic_rules.xml)
8484

85-
if(BUILD_TESTING)
85+
if(0) # (BUILD_TESTING)
8686
find_package(ament_cmake_gmock REQUIRED)
8787
find_package(controller_manager REQUIRED)
8888
find_package(ros2_control_test_assets REQUIRED)

‎cartesian_vic_teleop_controller/CMakeLists.txt

Lines changed: 107 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,107 @@
1+
cmake_minimum_required(VERSION 3.16)
2+
project(cartesian_vic_teleop_controller LANGUAGES CXX)
3+
4+
if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)")
5+
add_compile_options(-Wall -Wextra -Wpedantic)
6+
endif()
7+
8+
9+
# Use CMake to pass the current ROS_DISTRO via variables into a preprocessor template.
10+
# We then include this file and switch between the different APIs.
11+
if($ENV{ROS_DISTRO} STREQUAL "rolling")
12+
set(CARTESIAN_CONTROLLERS_ROLLING TRUE)
13+
elseif($ENV{ROS_DISTRO} STREQUAL "jazzy")
14+
set(CARTESIAN_CONTROLLERS_JAZZY TRUE)
15+
elseif($ENV{ROS_DISTRO} STREQUAL "humble")
16+
set(CARTESIAN_CONTROLLERS_HUMBLE TRUE)
17+
else()
18+
message(WARNING "ROS2 version must be {rolling|jazzy|humble}")
19+
endif()
20+
configure_file(include/cartesian_vic_teleop_controller/Ros2VersionConfig.h.in include/cartesian_vic_teleop_controller/Ros2VersionConfig.h)
21+
22+
set(DEPENDENCIES
23+
Eigen3
24+
pluginlib
25+
rclcpp
26+
rclcpp_lifecycle
27+
realtime_tools
28+
tf2
29+
tf2_eigen
30+
tf2_geometry_msgs
31+
tf2_kdl
32+
tf2_ros
33+
trajectory_msgs
34+
cartesian_control_msgs
35+
cartesian_vic_controller
36+
)
37+
38+
find_package(ament_cmake REQUIRED)
39+
find_package(backward_ros REQUIRED)
40+
foreach(Dependency IN ITEMS ${DEPENDENCIES})
41+
find_package(${Dependency} REQUIRED)
42+
endforeach()
43+
44+
add_library(${PROJECT_NAME} SHARED
45+
# ROS2 controller impl.
46+
src/cartesian_vic_teleop_controller.cpp
47+
# Logic
48+
src/cartesian_vic_teleop_logic.cpp
49+
src/mapping_manager.cpp
50+
src/teleop_data.cpp
51+
src/teleop_rule.cpp
52+
# Async node impl.
53+
# TODO
54+
)
55+
target_compile_features(${PROJECT_NAME} PUBLIC cxx_std_17)
56+
target_include_directories(${PROJECT_NAME} PUBLIC
57+
$<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
58+
# $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/src/rules> # LP solvers
59+
$<BUILD_INTERFACE:${CMAKE_BINARY_DIR}/include> # ROS2VersionConfig.h
60+
$<INSTALL_INTERFACE:include/${PROJECT_NAME}>
61+
)
62+
ament_target_dependencies(${PROJECT_NAME} PUBLIC ${DEPENDENCIES})
63+
64+
# Causes the visibility macros to use dllexport rather than dllimport,
65+
# which is appropriate when building the dll but not consuming it.
66+
target_compile_definitions(${PROJECT_NAME} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")
67+
target_compile_definitions(${PROJECT_NAME} PRIVATE "cartesian_vic_teleop_controller_BUILDING_DLL")
68+
69+
pluginlib_export_plugin_description_file(controller_interface cartesian_vic_teleop_controller.xml)
70+
71+
# VIC-based teleoperation rule plugins library
72+
add_library(cartesian_vic_teleop_rules SHARED
73+
src/rules/vanilla_teleop_rule.cpp
74+
)
75+
target_compile_features(cartesian_vic_teleop_rules PUBLIC cxx_std_17)
76+
target_include_directories(cartesian_vic_teleop_rules PUBLIC
77+
$<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
78+
$<INSTALL_INTERFACE:include/cartesian_vic_teleop_controller>
79+
)
80+
target_link_libraries(cartesian_vic_teleop_rules PUBLIC
81+
cartesian_vic_teleop_controller
82+
)
83+
ament_target_dependencies(
84+
cartesian_vic_teleop_rules
85+
PUBLIC
86+
${DEPENDENCIES}
87+
)
88+
pluginlib_export_plugin_description_file(cartesian_vic_teleop_controller cartesian_vic_teleop_rules.xml)
89+
90+
install(
91+
DIRECTORY include/
92+
DESTINATION include/${PROJECT_NAME}
93+
)
94+
95+
install(
96+
TARGETS
97+
${PROJECT_NAME}
98+
cartesian_vic_teleop_rules
99+
EXPORT export_cartesian_vic_teleop_controller
100+
RUNTIME DESTINATION bin
101+
ARCHIVE DESTINATION lib
102+
LIBRARY DESTINATION lib
103+
)
104+
105+
ament_export_targets(export_cartesian_vic_teleop_controller HAS_LIBRARY_TARGET)
106+
ament_export_dependencies(${DEPENDENCIES})
107+
ament_package()

‎cartesian_vic_teleop_controller/cartesian_vic_teleop_controller.xml

Lines changed: 9 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,9 @@
1+
<library path="cartesian_vic_teleop_controller">
2+
<class name="cartesian_vic_teleop_controller/CartesianVicTeleopController"
3+
type="cartesian_vic_teleop_controller::CartesianVicTeleopController"
4+
base_class_type="controller_interface::ControllerInterface">
5+
<description>
6+
Cartesian teleoperation controller (leader-side only!) based on the cartesian_vic_controller package.
7+
</description>
8+
</class>
9+
</library>

‎cartesian_vic_teleop_controller/cartesian_vic_teleop_rules.xml

Lines changed: 9 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,9 @@
1+
<library path="cartesian_vic_teleop_rules">
2+
<class name="cartesian_vic_teleop_controller/VanillaTeleopRule"
3+
type="cartesian_vic_teleop_controller::VanillaTeleopRule"
4+
base_class_type="cartesian_vic_teleop_controller::TeleopRule">
5+
<description>
6+
Basic position-position with force feedback.
7+
</description>
8+
</class>
9+
</library>

‎cartesian_vic_teleop_controller/include/cartesian_vic_teleop_controller/Ros2VersionConfig.h.in

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,3 @@
1+
#cmakedefine CARTESIAN_VIC_TELEOP_CONTROLLER_ROLLING ${CARTESIAN_VIC_TELEOP_CONTROLLER_ROLLING}
2+
#cmakedefine CARTESIAN_VIC_TELEOP_CONTROLLER_JAZZY ${CARTESIAN_VIC_TELEOP_CONTROLLER_JAZZY}
3+
#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

Lines changed: 129 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,129 @@
1+
// Copyright 2024 ICUBE Laboratory, University of Strasbourg
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
//
15+
/// \authors: Thibault Poignonec <thibault.poignonec@gmail.com>
16+
17+
#ifndef CARTESIAN_VIC_TELEOP_CONTROLLER__CARTESIAN_VIC_TELEOP_CONTROLLER_HPP_
18+
#define CARTESIAN_VIC_TELEOP_CONTROLLER__CARTESIAN_VIC_TELEOP_CONTROLLER_HPP_
19+
20+
#include <chrono>
21+
#include <memory>
22+
#include <string>
23+
#include <vector>
24+
25+
#include "cartesian_vic_teleop_controller/visibility_control.h"
26+
#include "cartesian_vic_teleop_controller/mapping_manager.hpp"
27+
#include "cartesian_vic_teleop_controller/cartesian_vic_teleop_logic.hpp"
28+
29+
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
30+
#include "rclcpp_lifecycle/state.hpp"
31+
#include "realtime_tools/realtime_buffer.h"
32+
#include "realtime_tools/realtime_publisher.h"
33+
34+
// Base class for VIC controller
35+
#include "cartesian_vic_controller/cartesian_vic_controller.hpp"
36+
37+
// ROS2 standard msgs
38+
#include "std_msgs/msg/bool.hpp"
39+
40+
// Custom msgs
41+
#include "cartesian_control_msgs/msg/vic_controller_state.hpp"
42+
#include "cartesian_control_msgs/msg/cartesian_trajectory.hpp"
43+
#include "cartesian_control_msgs/msg/compliant_frame_trajectory.hpp"
44+
#include "cartesian_control_msgs/msg/teleop_controller_state.hpp"
45+
#include "cartesian_control_msgs/msg/teleop_compliance.hpp"
46+
47+
namespace cartesian_vic_teleop_controller
48+
{
49+
class CartesianVicTeleopController : public cartesian_vic_controller::CartesianVicController
50+
{
51+
public:
52+
CartesianVicTeleopController();
53+
virtual ~CartesianVicTeleopController() = default;
54+
55+
CARTESIAN_VIC_TELEOP_CONTROLLER_PUBLIC
56+
controller_interface::CallbackReturn on_init() override;
57+
58+
CARTESIAN_VIC_TELEOP_CONTROLLER_PUBLIC
59+
controller_interface::return_type update(
60+
const rclcpp::Time & time, const rclcpp::Duration & period) override;
61+
62+
CARTESIAN_VIC_TELEOP_CONTROLLER_PUBLIC
63+
controller_interface::CallbackReturn on_configure(
64+
const rclcpp_lifecycle::State & previous_state) override;
65+
66+
CARTESIAN_VIC_TELEOP_CONTROLLER_PUBLIC
67+
controller_interface::CallbackReturn on_activate(
68+
const rclcpp_lifecycle::State & previous_state) override;
69+
70+
CARTESIAN_VIC_TELEOP_CONTROLLER_PUBLIC
71+
controller_interface::CallbackReturn on_deactivate(
72+
const rclcpp_lifecycle::State & previous_state) override;
73+
74+
CARTESIAN_VIC_TELEOP_CONTROLLER_PUBLIC
75+
controller_interface::CallbackReturn on_cleanup(
76+
const rclcpp_lifecycle::State & previous_state) override;
77+
78+
CARTESIAN_VIC_TELEOP_CONTROLLER_PUBLIC
79+
controller_interface::CallbackReturn on_error(
80+
const rclcpp_lifecycle::State & previous_state) override;
81+
82+
using Base = cartesian_vic_controller::CartesianVicController;
83+
84+
protected:
85+
PassiveVicTeleopLogic teleop_logic_;
86+
87+
// Publisher teleop controller state (for logging purposes only)
88+
rclcpp::Publisher<cartesian_control_msgs::msg::TeleopControllerState>::SharedPtr
89+
non_rt_teleop_state_publisher_;
90+
std::unique_ptr<realtime_tools::RealtimePublisher<
91+
cartesian_control_msgs::msg::TeleopControllerState>> teleop_state_publisher_;
92+
cartesian_control_msgs::msg::TeleopControllerState teleop_state_msg_;
93+
94+
// Publisher follower robot VIC reference trajectory
95+
rclcpp::Publisher<cartesian_control_msgs::msg::CompliantFrameTrajectory>::SharedPtr
96+
non_rt_follower_vic_ref_publisher_;
97+
std::unique_ptr<realtime_tools::RealtimePublisher<
98+
cartesian_control_msgs::msg::CompliantFrameTrajectory>> follower_vic_ref_publisher_;
99+
100+
// Subscriber to follower VIC state
101+
// TODO(tpoignonec): remove follower_vic_state_msg_
102+
cartesian_control_msgs::msg::VicControllerState follower_vic_state_msg_;
103+
std::shared_ptr<cartesian_control_msgs::msg::VicControllerState>
104+
follower_vic_state_msg_ptr_;
105+
rclcpp::Subscription<cartesian_control_msgs::msg::VicControllerState>::SharedPtr
106+
follower_vic_state_subscriber_;
107+
realtime_tools::RealtimeBuffer<std::shared_ptr<
108+
cartesian_control_msgs::msg::VicControllerState>> input_follower_vic_state_msg_;
109+
110+
// Subscriber to teleoperation compliance ref.
111+
rclcpp::Subscription<
112+
cartesian_control_msgs::msg::TeleopCompliance>::SharedPtr teleop_compliance_subscriber_;
113+
realtime_tools::RealtimeBuffer<std::shared_ptr<
114+
cartesian_control_msgs::msg::TeleopCompliance>> input_teleop_compliance_msg_;
115+
std::shared_ptr<cartesian_control_msgs::msg::TeleopCompliance> teleop_compliance_msg_ptr_;
116+
117+
// Subscriber to follower VIC state
118+
rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr is_clutched_subscriber_;
119+
realtime_tools::RealtimeBuffer<std::shared_ptr<std_msgs::msg::Bool>> input_is_clutched_msg_;
120+
std::shared_ptr<std_msgs::msg::Bool> is_clutched_msg_ptr_;
121+
122+
// Storage
123+
cartesian_control_msgs::msg::CompliantFrameTrajectory leader_vic_ref_;
124+
cartesian_control_msgs::msg::CompliantFrameTrajectory follower_vic_ref_;
125+
};
126+
127+
} // namespace cartesian_vic_teleop_controller
128+
129+
#endif // CARTESIAN_VIC_TELEOP_CONTROLLER__CARTESIAN_VIC_TELEOP_CONTROLLER_HPP_

0 commit comments

Comments
 (0)