Added package moteus_ros2_control

.gitignore

@@ -3,6 +3,8 @@ logs/
 build/
 bin/
 lib/
+log/
+install/
 msg_gen/
 srv_gen/
 msg/*Action.msg
@@ -49,3 +51,7 @@ qtcreator-*
 
 # Catkin custom files
 CATKIN_IGNORE
+
+
+#vscode
+.vscode/

moteus_ros2_control/CMakeLists.txt

@@ -0,0 +1,26 @@
+cmake_minimum_required(VERSION 3.8)
+project(moteus_ros2_control)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # comment the line when a copyright and license is added to all source files
+  set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # comment the line when this package is in a git repo and when
+  # a copyright and license is added to all source files
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

moteus_ros2_control/include/moteus_ros2_control/MoteusControllerInterface.hpp

@@ -0,0 +1,106 @@
+#ifndef MOTEUS_CONTROLLER_INTERFACE_HPP_
+#define MOTEUS_CONTROLLER_INTERFACE_HPP_
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "control_msgs/msg/multi_dof_command.hpp"
+#include "control_msgs/msg/multi_dof_state_stamped.hpp"
+#include "controller_interface/chainable_controller_interface.hpp"
+#include "ros2_moteus_position_controller/visibility_control.h"
+#include "rclcpp_lifecycle/state.hpp"
+#include "realtime_tools/realtime_buffer.h"
+#include "realtime_tools/realtime_publisher.h"
+#include "std_srvs/srv/set_bool.hpp"
+
+namespace moteus_controller_ros
+{
+
+  class MoteusControllerInterface : public controller_interface::ChainableControllerInterface
+  {
+  public:
+    MOTEUS_CONTROLLER_INTERFACE__VISIBILITY_PUBLIC 
+    MoteusControllerInterface();
+
+    MOTEUS_CONTROLLER_INTERFACE__VISIBILITY_PUBLIC 
+    controller_interface::CallbackReturn on_init() override;
+
+    MOTEUS_CONTROLLER_INTERFACE__VISIBILITY_PUBLIC 
+    controller_interface::InterfaceConfiguration command_interface_configuration() const override;
+
+    MOTEUS_CONTROLLER_INTERFACE__VISIBILITY_PUBLIC 
+    controller_interface::InterfaceConfiguration state_interface_configuration() const override;
+
+    MOTEUS_CONTROLLER_INTERFACE__VISIBILITY_PUBLIC 
+    controller_interface::CallbackReturn on_cleanup(
+      const rclcpp_lifecycle::State & previous_state) override;
+
+    MOTEUS_CONTROLLER_INTERFACE__VISIBILITY_PUBLIC 
+    controller_interface::CallbackReturn on_configure(
+      const rclcpp_lifecycle::State & previous_state) override;
+
+    MOTEUS_CONTROLLER_INTERFACE__VISIBILITY_PUBLIC 
+    controller_interface::CallbackReturn on_activate(
+      const rclcpp_lifecycle::State & previous_state) override;
+
+    MOTEUS_CONTROLLER_INTERFACE__VISIBILITY_PUBLIC 
+    controller_interface::CallbackReturn on_deactivate(
+      const rclcpp_lifecycle::State & previous_state) override;
+
+    MOTEUS_CONTROLLER_INTERFACE__VISIBILITY_PUBLIC 
+    controller_interface::return_type update_reference_from_subscribers(
+      const rclcpp::Time & time, const rclcpp::Duration & period) override;
+
+    MOTEUS_CONTROLLER_INTERFACE__VISIBILITY_PUBLIC 
+    controller_interface::return_type update_and_write_commands(
+      const rclcpp::Time & time, const rclcpp::Duration & period) override;
+
+  protected:
+    std::shared_ptr<position_controller::ParamListener> param_listener_; // TODO DODAJ TO
+    position_controller::Params params_;
+
+    std::vector<std::string> reference_and_state_dof_names_;
+    size_t dof_;
+    std::vector<double> measured_state_values_;
+
+    using PidPtr = std::shared_ptr<control_toolbox::PidROS>;
+    std::vector<PidPtr> pids_;
+    // Feed-forward velocity weight factor when calculating closed loop pid adapter's command
+    std::vector<double> feedforward_gain_;
+
+    // Command subscribers and Controller State publisher
+    rclcpp::Subscription<ControllerReferenceMsg>::SharedPtr ref_subscriber_ = nullptr;
+    realtime_tools::RealtimeBuffer<std::shared_ptr<ControllerReferenceMsg>> input_ref_;
+
+    rclcpp::Subscription<ControllerMeasuredStateMsg>::SharedPtr measured_state_subscriber_ = nullptr;
+    realtime_tools::RealtimeBuffer<std::shared_ptr<ControllerMeasuredStateMsg>> measured_state_;
+
+    rclcpp::Service<ControllerModeSrvType>::SharedPtr set_feedforward_control_service_;
+    realtime_tools::RealtimeBuffer<feedforward_mode_type> control_mode_;
+
+    using ControllerStatePublisher = realtime_tools::RealtimePublisher<ControllerStateMsg>;
+
+    rclcpp::Publisher<ControllerStateMsg>::SharedPtr s_publisher_;
+    std::unique_ptr<ControllerStatePublisher> state_publisher_;
+
+    // override methods from ChainableControllerInterface
+    std::vector<hardware_interface::CommandInterface> on_export_reference_interfaces() override;
+
+    std::vector<hardware_interface::StateInterface> on_export_state_interfaces() override;
+
+    bool on_set_chained_mode(bool chained_mode) override;
+
+    // internal methods
+    void update_parameters();
+    controller_interface::CallbackReturn configure_parameters();
+
+  private:
+    // callback for topic interface
+    PID_CONTROLLER__VISIBILITY_LOCAL
+    void reference_callback(const std::shared_ptr<ControllerReferenceMsg> msg);
+  };
+
+}  // namespace pid_controller
+
+#endif  // PID_CONTROLLER__PID_CONTROLLER_HPP_

moteus_ros2_control/include/moteus_ros2_control/position_controller/PositionController.hpp

@@ -0,0 +1,53 @@
+#ifndef _POSITION_CONTROLLER_HPP_
+#define _POSITION_CONTROLLER_HPP_
+
+namespace position_controller
+{
+    struct PID_Parameters
+    {
+        /* Classic PID coefficients */
+        double proportional_coef_;
+        double derivative_coef_;
+        double integral_coef_;
+
+        /* Limit for integration term */
+        double integration_limit_;
+    };
+
+    struct PID_Scales
+    {
+        /* Proportional and derivative scales, can be changed in every control call */
+        double proportional_scale_ = 1;
+        double derivative_scale_ = 1;
+    };
+
+    class PositionController
+    {
+        private:
+
+        PID_Parameters pid_params_;
+
+        PID_Scales pid_scales_;
+
+        /* Position integrator, updated in every control call */
+        double position_integrator_;
+
+        /* Time step (inverse of frequency)*/
+        double time_step_;
+
+        double calculateProportionalOutput(double _position_error); // Biorą bledy juz z wartosci wyzej
+        double calculateIntegralOutput(double _position_error);
+        double calculateDerivativeOutput(double _velocity_error);
+
+
+        public:
+
+        PositionController(PID_Parameters _pid_params, double _frequency);
+        PositionController(const PositionController& other) = default;
+        PositionController(PositionController&& other) = default;
+
+        double calculateOutput(double _position_error, double _velocity_error,
+         double _feedforward_input, PID_Scales _pid_scales); //  Tutaj liczymy wszystko
+    };
+};
+#endif

moteus_ros2_control/include/moteus_ros2_control/visibility_control.hpp

@@ -0,0 +1,35 @@
+#ifndef MOTEUS_CONTROLLER_INTERFACE__VISIBILITY_CONTROL_H_
+#define MOTEUS_CONTROLLER_INTERFACE__VISIBILITY_CONTROL_H_
+
+// This logic was borrowed (then namespaced) from the examples on the gcc wiki:
+//     https://gcc.gnu.org/wiki/Visibility
+
+#if defined _WIN32 || defined __CYGWIN__
+#ifdef __GNUC__
+#define MOTEUS_CONTROLLER_INTERFACE__VISIBILITY_EXPORT __attribute__((dllexport))
+#define MOTEUS_CONTROLLER_INTERFACE__VISIBILITY_IMPORT __attribute__((dllimport))
+#else
+#define MOTEUS_CONTROLLER_INTERFACE__VISIBILITY_EXPORT __declspec(dllexport)
+#define MOTEUS_CONTROLLER_INTERFACE__VISIBILITY_IMPORT __declspec(dllimport)
+#endif
+#ifdef MOTEUS_CONTROLLER_INTERFACE__VISIBILITY_BUILDING_DLL
+#define MOTEUS_CONTROLLER_INTERFACE__VISIBILITY_PUBLIC MOTEUS_CONTROLLER_INTERFACE__VISIBILITY_EXPORT
+#else
+#define MOTEUS_CONTROLLER_INTERFACE__VISIBILITY_PUBLIC MOTEUS_CONTROLLER_INTERFACE__VISIBILITY_IMPORT
+#endif
+#define MOTEUS_CONTROLLER_INTERFACE__VISIBILITY_PUBLIC_TYPE MOTEUS_CONTROLLER_INTERFACE__VISIBILITY_PUBLIC
+#define MOTEUS_CONTROLLER_INTERFACE__VISIBILITY_LOCAL
+#else
+#define MOTEUS_CONTROLLER_INTERFACE__VISIBILITY_EXPORT __attribute__((visibility("default")))
+#define MOTEUS_CONTROLLER_INTERFACE__VISIBILITY_IMPORT
+#if __GNUC__ >= 4
+#define MOTEUS_CONTROLLER_INTERFACE__VISIBILITY_PUBLIC __attribute__((visibility("default")))
+#define MOTEUS_CONTROLLER_INTERFACE__VISIBILITY_LOCAL __attribute__((visibility("hidden")))
+#else
+#define MOTEUS_CONTROLLER_INTERFACE__VISIBILITY_PUBLIC
+#define MOTEUS_CONTROLLER_INTERFACE__VISIBILITY_LOCAL
+#endif
+#define MOTEUS_CONTROLLER_INTERFACE__VISIBILITY_PUBLIC_TYPE
+#endif
+
+#endif  // MOTEUS_CONTROLLER_INTERFACE__VISIBILITY_CONTROL_H_

moteus_ros2_control/package.xml

@@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>moteus_ros2_control</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="[email protected]">bartek</maintainer>
+  <license>GPL-3.0-only</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

moteus_ros2_control/src/position_controller/PositionController.cpp

@@ -0,0 +1,3 @@
+#include "ros2_moteus_position_controller/position_controller/PositionController.hpp"
+
+using namespace position_controller;