add armor tester controller

.gitignore

@@ -2,4 +2,5 @@ build
 install
 log
 __pycache__/
-.cache
+.cache
+.vscode

decision/armor_tester/CMakeLists.txt

@@ -1,26 +1,44 @@
-cmake_minimum_required(VERSION 3.8)
-project(armor_tester)
+cmake_minimum_required(VERSION 3.8)  # the mininal version
+project(armor_tester)  # define the project name here, and it influence the variable ${PROJECT_NAME}
 
+# only acts for GCC and Clang
 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)
-find_package(ament_cmake_auto REQUIRED)
-ament_auto_find_build_dependencies()
+find_package(ament_cmake REQUIRED)  # ament_cmake is mandatory
+find_package(ament_cmake_auto REQUIRED)  # ament_cmake_auto is also mandatory
+ament_auto_find_build_dependencies()  # scan the package.xml to get the dependencies
 
+# construct controller shared library to complile the .so lib
 ament_auto_add_library(
-  ${PROJECT_NAME} SHARED
+  ${PROJECT_NAME}
+  SHARED
   src/armor_tester.cpp
-  )
+)
 
+# register a class in library as plugin, here, the ArmorTester
 rclcpp_components_register_node(
   ${PROJECT_NAME}
   PLUGIN "ArmorTester"
   EXECUTABLE ${PROJECT_NAME}_node
-  )
+)
+# rclcpp_components_register_node(
+#     <library_target>  # existing library name
+#     PLUGIN "<class_name>"  # existing class name
+#     EXECUTABLE <executable_name>  # self-designed
+# )
+
+# add the include file
+target_include_directories(
+  ${PROJECT_NAME}
+  PUBLIC
+  "$<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>"
+  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
+)
 
+# test for building
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   # the following line skips the linter which checks for copyrights

decision/armor_tester/package.xml

@@ -4,16 +4,25 @@
   <name>armor_tester</name>
   <version>0.0.0</version>
   <description>TODO: Package description</description>
-  <maintainer email="mcerztn@gmail.com">meta</maintainer>
+  <maintainer email="hanbinzheng54@gmail.com">hanbinzheng</maintainer>
   <license>TODO: License declaration</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
 
+  <build_depend>generate_parameter_library</build_depend>
+
   <depend>operation_interface</depend>
   <depend>geometry_msgs</depend>
   <depend>behavior_interface</depend>
+  <depend>control_msgs</depend>
+  <depend>control_toolbox</depend>
+  <depend>controller_interface</depend>
+  <depend>hardware_interface</depend>
+  <depend>realtime_tools</depend>
+  <depend>pluginlib</depend>
   <depend>rclcpp</depend>
+  <depend>rclcpp_lifecycle</depend>
   <depend>rclcpp_components</depend>
 
   <test_depend>ament_lint_auto</test_depend>

decision/armor_tester/src/armor_tester.cpp

@@ -1,10 +1,12 @@
 #include "rclcpp/rclcpp.hpp"
 #include <geometry_msgs/msg/twist.hpp>
 #include <operation_interface/msg/dbus_control.hpp>
+#include "behavior_interface/msg/armor.hpp"
 #include <std_msgs/msg/float64_multi_array.hpp>
 #include <memory>
 
 #define PUB_RATE 15 // ms
+#define PI 3.1415926
 
 class ArmorTester : public rclcpp::Node
 {
@@ -14,7 +16,8 @@ class ArmorTester : public rclcpp::Node
         // reset
         ls_x = ls_y = rs_x = rs_y = wheel = 0;
         lsw = rsw = "";
-        motor_pub_ = this->create_publisher<std_msgs::msg::Float64MultiArray>("/forward_position_controller/commands", 10);
+        velocity_pub_ = this->create_publisher<behavior_interface::msg::Armor>(
+            "/armor_tester_controller/reference", 10);
 
         dbus_sub_ = this->create_subscription<operation_interface::msg::DbusControl>(
             "dbus_control", 10,
@@ -31,7 +34,7 @@ class ArmorTester : public rclcpp::Node
 
 private:
     rclcpp::Subscription<operation_interface::msg::DbusControl>::SharedPtr dbus_sub_;
-    rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr motor_pub_;
+    rclcpp::Publisher<behavior_interface::msg::Armor>::SharedPtr velocity_pub_;
     rclcpp::TimerBase::SharedPtr timer_;
     double ls_x, ls_y, rs_x, rs_y, wheel;
     std::string lsw, rsw;
@@ -52,14 +55,14 @@ class ArmorTester : public rclcpp::Node
     {
         if (lsw != "MID") return;
 
-        std_msgs::msg::Float64MultiArray tmp_motor_velocity;
-        std::vector<double> motor_velocity;
-        motor_velocity.push_back(ls_x*6.28*2);
-        RCLCPP_INFO(this->get_logger(), "speed:%lf", ls_x*6.28*2);
-        tmp_motor_velocity.data = motor_velocity;
+        behavior_interface::msg::Armor armor_tester_velocity;
+        armor_tester_velocity.unitree_vel = ls_y * 4 * PI;
+        armor_tester_velocity.dji_vel = rs_y * 4 * PI;
 
-        motor_pub_->publish(tmp_motor_velocity);
+        RCLCPP_INFO(this->get_logger(), "unitree_vel:%lf, dji_vel: %lf",
+                    ls_y * 4 * PI, rs_y * 4 * PI);
+        velocity_pub_->publish(armor_tester_velocity);
     }
 };
 #include <rclcpp_components/register_node_macro.hpp>
-RCLCPP_COMPONENTS_REGISTER_NODE(ArmorTester)
+RCLCPP_COMPONENTS_REGISTER_NODE(ArmorTester)

decomposition/meta_utils_controller/CMakeLists.txt

@@ -0,0 +1,84 @@
+cmake_minimum_required(VERSION 3.8)  # the mininal version
+project(meta_utils_controller)  # define the project name here, and it influence the variable ${PROJECT_NAME}
+
+# only acts for GCC and Clang
+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)  # ament_cmake is mandatory
+find_package(ament_cmake_auto REQUIRED)  # ament_cmake_auto is also mandatory
+ament_auto_find_build_dependencies()  # scan the package.xml to get the dependencies
+
+# generate parameter library for controller
+generate_parameter_library(
+  armor_tester_controller_parameters
+  src/armor_tester_controller.yaml
+)
+
+# generate_parameter_lib (
+#     name_of_target, -> to generate target_name.cpp
+#     yaml_file, -> generate the parameter listeners based on which yaml file
+# )
+
+ament_auto_add_library(
+  armor_tester_controller
+  SHARED
+  src/armor_tester_controller.cpp
+)
+
+# ament_auto_add_library (
+#     target_package_name
+#     SHARED/STATIC/OBJECT
+#     sorce file
+# )
+
+target_include_directories(
+  armor_tester_controller
+  PUBLIC
+  "$<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>"
+  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
+)
+
+# target_include_directories(
+#     cmake-target
+#     PUBLIC
+#     <BUILD_INTERFACE:.....> # used during build
+#     <INSTALL_INTERFACE:...> # used during install
+# )
+
+# link the the parameter listeners
+target_link_libraries(
+  armor_tester_controller
+  armor_tester_controller_parameters
+)
+# target_link_libraries(
+#     cmake-target-package
+#     cmake-target that cmake-target-package depends on
+# )
+
+# where to find the description of the controller
+pluginlib_export_plugin_description_file(
+  controller_interface
+  armor_tester_controller.xml
+)
+# pluginlib_export_plugin_description_file(
+#     base-class
+#     description file
+# )
+
+# test for building
+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_auto_package()

decomposition/meta_utils_controller/armor_tester_controller.xml

@@ -0,0 +1,28 @@
+<!--
+Copyright (c) 2024, 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.
+
+
+Source of this file are templates in
+[RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository.
+-->
+
+<library path="armor_tester_controller">
+  <class name="armor_tester_controller/ArmorTesterController"
+         type="armor_tester_controller::ArmorTesterController" base_class_type="controller_interface::ChainableControllerInterface">
+  <description>
+    An Armor Tester Controller.
+  </description>
+  </class>
+</library>

decomposition/meta_utils_controller/include/meta_utils_controller/armor_tester_controller.hpp

@@ -0,0 +1,87 @@
+#ifndef ARMOR_TESTER_CONTROLLER_HPP_
+#define ARMOR_TESTER_CONTROLLER_HPP_
+
+#include <memory>
+#include <string>
+#include <vector>
+#include <control_toolbox/pid_ros.hpp>
+#include <rclcpp/subscription.hpp>
+
+#include "controller_interface/controller_interface.hpp"
+#include "controller_interface/chainable_controller_interface.hpp"
+#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
+#include "rclcpp/duration.hpp"
+#include "rclcpp_lifecycle/state.hpp"
+#include "realtime_tools/realtime_buffer.hpp"
+#include "realtime_tools/realtime_publisher.hpp"
+
+#include <meta_utils_controller/armor_tester_controller_parameters.hpp>
+#include "behavior_interface/msg/armor.hpp" // dji_vel, unitree_vel
+
+namespace armor_tester_controller // namespace begin here
+{
+
+class ArmorTesterController : public controller_interface::ChainableControllerInterface
+{
+    public:
+        ArmorTesterController() = default;
+
+        // ControllerInterfaceBase and ChainableControllerInterface, a little strange
+        ~ArmorTesterController() = default;
+
+        // override method from ControllerInterfaceBase (done?)
+        controller_interface::InterfaceConfiguration command_interface_configuration() const override;
+
+        // override method from ControllerInterfaceBase (done?)
+        controller_interface::InterfaceConfiguration state_interface_configuration() const override;
+
+        // override method from ControllerInterfaceBase (done)
+        controller_interface::CallbackReturn on_init() override;
+
+        // override method from ControllerInterfaceBase (done)
+        controller_interface::CallbackReturn on_configure(
+            const rclcpp_lifecycle::State & previous_state) override;
+
+        // override method from ControllerInterfaceBase (done)
+        controller_interface::CallbackReturn on_activate(
+            const rclcpp_lifecycle::State & previous_state) override;
+
+        // override method from ControllerInterfaceBase (done)
+        controller_interface::CallbackReturn on_deactivate(
+            const rclcpp_lifecycle::State & previous_state) override;
+
+        // override method from ChainableControllerInterface
+        controller_interface::return_type update_and_write_commands(
+            const rclcpp::Time & time, const rclcpp::Duration & period) override;
+
+    protected:
+        // override method from ChainableControllerInterface
+        #if RCLCPP_VERSION_MAJOR >= 28 // Ros2 Jazzy or latter
+                controller_interface::return_type update_reference_from_subscribers(
+                        const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override;
+        #else
+                controller_interface::return_type update_reference_from_subscribers() override;
+        #endif
+
+        // parameters
+        armor_tester_controller::Params params_;
+        std::shared_ptr<armor_tester_controller::ParamListener> param_listener_;
+
+        // pid for dji motor
+        std::shared_ptr<control_toolbox::PidROS> dji_pid_;
+
+        // for subscriber
+        rclcpp::Subscription<behavior_interface::msg::Armor>::SharedPtr vel_subscriber_ = nullptr;
+        realtime_tools::RealtimeBuffer<std::shared_ptr<behavior_interface::msg::Armor>> vel_buffer_;
+
+        // override method from ChainableControllerInterface (done)
+        std::vector<hardware_interface::CommandInterface> on_export_reference_interfaces() override;
+
+    private:
+        // callback function for subscriber (done)
+        void velocity_callback(const std::shared_ptr<behavior_interface::msg::Armor> msg);
+}; // class definition ends here
+
+} // namespace ends here
+
+#endif // ARMOR_TESTER_CONTROLLER_HPP_

decomposition/meta_utils_controller/package.xml

@@ -0,0 +1,34 @@
+<?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>meta_utils_controller</name>
+  <version>0.0.0</version>
+  <description>Package for Meta Utils</description>
+  <maintainer email="hanbinzheng54@gmail.com">hanbinzheng</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <build_depend>generate_parameter_library</build_depend>
+
+  <depend>operation_interface</depend>
+  <depend>geometry_msgs</depend>
+  <depend>behavior_interface</depend>
+  <depend>control_msgs</depend>
+  <depend>control_toolbox</depend>
+  <depend>controller_interface</depend>
+  <depend>hardware_interface</depend>
+  <depend>realtime_tools</depend>
+  <depend>pluginlib</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_lifecycle</depend>
+  <depend>rclcpp_components</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>