Merge main

joint_state_broadcaster/src/joint_state_broadcaster.cpp

@@ -24,6 +24,7 @@
 #include "hardware_interface/types/hardware_interface_return_values.hpp"
 #include "hardware_interface/types/hardware_interface_type_values.hpp"
 #include "rclcpp/clock.hpp"
+#include "rclcpp/event_handler.hpp"
 #include "rclcpp/qos.hpp"
 #include "rclcpp/time.hpp"
 #include "rclcpp_lifecycle/lifecycle_node.hpp"

joint_trajectory_controller/CMakeLists.txt

@@ -11,14 +11,20 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
   control_toolbox
   controller_interface
   generate_parameter_library
+  joint_limits
   hardware_interface
+  joint_limits
+  nav_msgs
   pluginlib
   rclcpp
   rclcpp_lifecycle
   realtime_tools
   rsl
+  ruckig
   tl_expected
   trajectory_msgs
+  tf2
+  tf2_geometry_msgs
   urdf
 )
 
@@ -44,22 +50,38 @@ target_include_directories(joint_trajectory_controller PUBLIC
 )
 target_link_libraries(joint_trajectory_controller PUBLIC
   joint_trajectory_controller_parameters
+  ruckig::ruckig
 )
 ament_target_dependencies(joint_trajectory_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS})
 
 # Causes the visibility macros to use dllexport rather than dllimport,
 # which is appropriate when building the dll but not consuming it.
 target_compile_definitions(joint_trajectory_controller PRIVATE "JOINT_TRAJECTORY_CONTROLLER_BUILDING_DLL" "_USE_MATH_DEFINES")
+
+add_library(cartesian_trajectory_generator SHARED
+  src/cartesian_trajectory_generator.cpp
+)
+target_compile_features(cartesian_trajectory_generator PUBLIC cxx_std_17)
+target_include_directories(cartesian_trajectory_generator PUBLIC
+  $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include/joint_trajectory_controller>
+)
+target_link_libraries(cartesian_trajectory_generator PUBLIC
+  joint_trajectory_controller
+)
+ament_target_dependencies(cartesian_trajectory_generator PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS})
+
 pluginlib_export_plugin_description_file(controller_interface joint_trajectory_plugin.xml)
 
 if(BUILD_TESTING)
   find_package(ament_cmake_gmock REQUIRED)
   find_package(controller_manager REQUIRED)
   find_package(ros2_control_test_assets REQUIRED)
 
-  ament_add_gmock(test_trajectory test/test_trajectory.cpp)
-  target_link_libraries(test_trajectory joint_trajectory_controller)
-  target_compile_definitions(test_trajectory PRIVATE _USE_MATH_DEFINES)
+  # TODO(henrygerardmoore): re-enable
+  # ament_add_gmock(test_trajectory test/test_trajectory.cpp)
+  # target_link_libraries(test_trajectory joint_trajectory_controller)
+  # target_compile_definitions(test_trajectory PRIVATE _USE_MATH_DEFINES)
 
   ament_add_gmock(test_trajectory_controller
     test/test_trajectory_controller.cpp)
@@ -82,6 +104,19 @@ if(BUILD_TESTING)
     ros2_control_test_assets
   )
 
+ament_add_gmock(test_load_cartesian_trajectory_generator
+  test/test_load_cartesian_trajectory_generator.cpp
+)
+target_link_libraries(test_load_cartesian_trajectory_generator
+  cartesian_trajectory_generator
+)
+ament_target_dependencies(test_load_cartesian_trajectory_generator
+  controller_manager
+  control_toolbox
+  realtime_tools
+  ros2_control_test_assets
+)
+
   ament_add_gmock(test_trajectory_actions
     test/test_trajectory_actions.cpp
   )
@@ -97,6 +132,7 @@ install(
   DESTINATION include/joint_trajectory_controller
 )
 install(TARGETS
+    cartesian_trajectory_generator
     joint_trajectory_controller
     joint_trajectory_controller_parameters
   EXPORT export_joint_trajectory_controller

joint_trajectory_controller/include/joint_trajectory_controller/cartesian_trajectory_generator.hpp

@@ -0,0 +1,97 @@
+// Copyright (c) 2023 Stogl Robotics Consulting UG (haftungsbeschränkt)
+//
+// 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.
+
+#ifndef JOINT_TRAJECTORY_CONTROLLER__CARTESIAN_TRAJECTORY_GENERATOR_HPP_
+#define JOINT_TRAJECTORY_CONTROLLER__CARTESIAN_TRAJECTORY_GENERATOR_HPP_
+
+#include <memory>
+#include <string>
+#include <unordered_map>
+#include <vector>
+
+#include "control_msgs/msg/cartesian_trajectory_generator_state.hpp"
+#include "geometry_msgs/msg/pose.hpp"
+#include "geometry_msgs/msg/transform_stamped.hpp"
+#include "joint_limits/joint_limits.hpp"
+#include "joint_trajectory_controller/joint_trajectory_controller.hpp"
+#include "nav_msgs/msg/odometry.hpp"
+#include "trajectory_msgs/msg/multi_dof_joint_trajectory_point.hpp"
+
+using namespace std::chrono_literals;  // NOLINT
+
+namespace cartesian_trajectory_generator
+{
+class CartesianTrajectoryGenerator : public joint_trajectory_controller::JointTrajectoryController
+{
+public:
+  JOINT_TRAJECTORY_CONTROLLER_PUBLIC
+  CartesianTrajectoryGenerator();
+
+  /**
+   * @brief command_interface_configuration This controller requires the position and velocity
+   * state interfaces for the controlled joints
+   */
+  JOINT_TRAJECTORY_CONTROLLER_PUBLIC
+  controller_interface::InterfaceConfiguration state_interface_configuration() const override;
+
+  JOINT_TRAJECTORY_CONTROLLER_PUBLIC
+  controller_interface::CallbackReturn on_configure(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+  JOINT_TRAJECTORY_CONTROLLER_PUBLIC
+  controller_interface::CallbackReturn on_activate(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+  using ControllerReferenceMsg = trajectory_msgs::msg::MultiDOFJointTrajectoryPoint;
+  using ControllerFeedbackMsg = nav_msgs::msg::Odometry;
+
+protected:
+  bool read_state_from_hardware(JointTrajectoryPoint & state) override;
+
+  using JointTrajectoryPoint = trajectory_msgs::msg::JointTrajectoryPoint;
+  JOINT_TRAJECTORY_CONTROLLER_PUBLIC
+  void publish_state(
+    const rclcpp::Time & time, const JointTrajectoryPoint & desired_state,
+    const JointTrajectoryPoint & current_state, const JointTrajectoryPoint & state_error,
+    const JointTrajectoryPoint & splines_output, const JointTrajectoryPoint & ruckig_input_target,
+    const JointTrajectoryPoint & ruckig_input) override;
+
+  // Command subscribers and Controller State publisher
+  rclcpp::Subscription<ControllerReferenceMsg>::SharedPtr ref_subscriber_ = nullptr;
+  rclcpp::Subscription<ControllerReferenceMsg>::SharedPtr ref_subscriber_reliable_ = nullptr;
+  realtime_tools::RealtimeBuffer<std::shared_ptr<ControllerReferenceMsg>> reference_world_;
+
+  rclcpp::Subscription<ControllerFeedbackMsg>::SharedPtr feedback_subscriber_ = nullptr;
+  realtime_tools::RealtimeBuffer<std::shared_ptr<ControllerFeedbackMsg>> feedback_;
+
+  trajectory_msgs::msg::JointTrajectoryPoint control_output_local_;
+
+private:
+  void reference_callback(const std::shared_ptr<ControllerReferenceMsg> msg);
+
+  using CartControllerStateMsg = control_msgs::msg::CartesianTrajectoryGeneratorState;
+  using CartStatePublisher = realtime_tools::RealtimePublisher<CartControllerStateMsg>;
+  using CartStatePublisherPtr = std::unique_ptr<CartStatePublisher>;
+  rclcpp::Publisher<CartControllerStateMsg>::SharedPtr cart_publisher_;
+  CartStatePublisherPtr cart_state_publisher_;
+
+  std::vector<joint_limits::JointLimits> configured_joint_limits_;
+
+  // storage of last received measured position to
+  geometry_msgs::msg::Pose last_received_measured_position_;
+};
+
+}  // namespace cartesian_trajectory_generator
+
+#endif  // JOINT_TRAJECTORY_CONTROLLER__CARTESIAN_TRAJECTORY_GENERATOR_HPP_

joint_trajectory_controller/include/joint_trajectory_controller/interpolation_methods.hpp

@@ -30,13 +30,16 @@ namespace interpolation_methods
 enum class InterpolationMethod
 {
   NONE,
-  VARIABLE_DEGREE_SPLINE
+  VARIABLE_DEGREE_SPLINE,
+  RUCKIG
 };
 
 const InterpolationMethod DEFAULT_INTERPOLATION = InterpolationMethod::VARIABLE_DEGREE_SPLINE;
 
 const std::unordered_map<InterpolationMethod, std::string> InterpolationMethodMap(
-  {{InterpolationMethod::NONE, "none"}, {InterpolationMethod::VARIABLE_DEGREE_SPLINE, "splines"}});
+  {{InterpolationMethod::NONE, "none"},
+   {InterpolationMethod::VARIABLE_DEGREE_SPLINE, "splines"},
+   {InterpolationMethod::RUCKIG, "ruckig"}});
 
 [[nodiscard]] inline InterpolationMethod from_string(const std::string & interpolation_method)
 {
@@ -50,6 +53,11 @@ const std::unordered_map<InterpolationMethod, std::string> InterpolationMethodMa
   {
     return InterpolationMethod::VARIABLE_DEGREE_SPLINE;
   }
+  else if (
+    interpolation_method.compare(InterpolationMethodMap.at(InterpolationMethod::RUCKIG)) == 0)
+  {
+    return InterpolationMethod::RUCKIG;
+  }
   // Default
   else
   {