From ec4279db808d608e66f37ac287846922859a3232 Mon Sep 17 00:00:00 2001 From: Hanbin Zheng Date: Sat, 13 Sep 2025 23:39:48 +0800 Subject: [PATCH 1/8] add the armor_tester, but fail to build --- decision/armor_tester/CMakeLists.txt | 19 ++ .../armor_tester/armor_tester_controller.xml | 28 +++ .../armor_tester/armor_tester_controller.hpp | 79 +++++++ decision/armor_tester/package.xml | 11 +- decision/armor_tester/src/armor_tester.cpp | 21 +- .../src/armor_tester_controller.cpp | 214 ++++++++++++++++++ .../src/armor_tester_controller.yaml | 15 ++ interfaces/behavior_interface/CMakeLists.txt | 1 + interfaces/behavior_interface/msg/Armor.msg | 2 + 9 files changed, 380 insertions(+), 10 deletions(-) create mode 100644 decision/armor_tester/armor_tester_controller.xml create mode 100644 decision/armor_tester/include/armor_tester/armor_tester_controller.hpp create mode 100644 decision/armor_tester/src/armor_tester_controller.cpp create mode 100644 decision/armor_tester/src/armor_tester_controller.yaml create mode 100644 interfaces/behavior_interface/msg/Armor.msg diff --git a/decision/armor_tester/CMakeLists.txt b/decision/armor_tester/CMakeLists.txt index 24826c3..84a0420 100644 --- a/decision/armor_tester/CMakeLists.txt +++ b/decision/armor_tester/CMakeLists.txt @@ -10,17 +10,36 @@ find_package(ament_cmake REQUIRED) find_package(ament_cmake_auto REQUIRED) ament_auto_find_build_dependencies() +generate_parameter_library( + ${PROJECT_NAME}_parameters + src/armor_tester_controller.yaml +) + ament_auto_add_library( ${PROJECT_NAME} SHARED src/armor_tester.cpp + src/armor_tester_controller.cpp ) +target_include_directories(${PROJECT_NAME} PUBLIC + "$" + "$") + +target_link_libraries( + ${PROJECT_NAME} + ${PROJECT_NAME}_parameters +) + rclcpp_components_register_node( ${PROJECT_NAME} PLUGIN "ArmorTester" EXECUTABLE ${PROJECT_NAME}_node ) +pluginlib_export_plugin_description_file( + controller_interface armor_tester_controller.xml) + + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) # the following line skips the linter which checks for copyrights diff --git a/decision/armor_tester/armor_tester_controller.xml b/decision/armor_tester/armor_tester_controller.xml new file mode 100644 index 0000000..443e55c --- /dev/null +++ b/decision/armor_tester/armor_tester_controller.xml @@ -0,0 +1,28 @@ + + + + + + An Armor Tester Controller. + + + diff --git a/decision/armor_tester/include/armor_tester/armor_tester_controller.hpp b/decision/armor_tester/include/armor_tester/armor_tester_controller.hpp new file mode 100644 index 0000000..9e5787f --- /dev/null +++ b/decision/armor_tester/include/armor_tester/armor_tester_controller.hpp @@ -0,0 +1,79 @@ +#ifndef ARMOR_TESTER_CONTROLLER_HPP_ +#define ARMOR_TESTER_CONTROLLER_HPP_ + +#include +#include +#include +#include +#include + +#include "controller_interface/controller_interface.hpp" +#include "rclcpp_lifecycle/node_interfaces/life_cycle_node_interface.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include "realtime_tools/realtime_buffer.h" +#include "armor_tester_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 + controller_interface::return_type update_reference_from_subscribers() override; + + // parameters + armor_tester_controller::Params params_; + std::shared_ptr param_listener_; + + // pid for dji motor + std::shared_ptr dji_pid_; + + // for subscriber + rclcpp::Subscription::SharedPtr vel_subscriber_ = nullptr; + realtime_tools::RealtimeBuffer> vel_buffer_; + + // override method from ChainableControllerInterface (done) + std::vector on_export_reference_interfaces() override; + + private: + // callback function for subscriber (done) + void velocity_callback(const std::shared_ptr msg); +} // class definition ends here + +} // namespace ends here + +#endif // ARMOR_TESTER_CONTROLLER_HPP_ diff --git a/decision/armor_tester/package.xml b/decision/armor_tester/package.xml index ba5c9be..f8e8cf4 100644 --- a/decision/armor_tester/package.xml +++ b/decision/armor_tester/package.xml @@ -4,16 +4,25 @@ armor_tester 0.0.0 TODO: Package description - meta + hanbinzheng TODO: License declaration ament_cmake ament_cmake_auto + generate_parameter_library + operation_interface geometry_msgs behavior_interface + control_msgs + control_toolbox + controller_interface + hardware_interface + realtime_tools + pluginlib rclcpp + rclcpp_lifecycle rclcpp_components ament_lint_auto diff --git a/decision/armor_tester/src/armor_tester.cpp b/decision/armor_tester/src/armor_tester.cpp index 92bbad6..e2de1f8 100644 --- a/decision/armor_tester/src/armor_tester.cpp +++ b/decision/armor_tester/src/armor_tester.cpp @@ -1,10 +1,12 @@ #include "rclcpp/rclcpp.hpp" #include #include +#include "behavior_interface/msg/armor.hpp" #include #include #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("/forward_position_controller/commands", 10); + velocity_pub_ = this->create_publisher( + "~/armor_tester_velocity", 10); dbus_sub_ = this->create_subscription( "dbus_control", 10, @@ -31,7 +34,7 @@ class ArmorTester : public rclcpp::Node private: rclcpp::Subscription::SharedPtr dbus_sub_; - rclcpp::Publisher::SharedPtr motor_pub_; + rclcpp::Publisher::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 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_x * 2 * PI; + armor_tester_velocity.dji_vel = rs_x * 2 * PI; - motor_pub_->publish(tmp_motor_velocity); + RCLCPP_INFO(this->get_logger(), "unitree_vel:%lf, dji_vel: %lf", + ls_x * 2 * PI, rs_x * 2 * PI); + velocity_pub_->publish(armor_tester_velocity); } }; #include -RCLCPP_COMPONENTS_REGISTER_NODE(ArmorTester) \ No newline at end of file +RCLCPP_COMPONENTS_REGISTER_NODE(ArmorTester) diff --git a/decision/armor_tester/src/armor_tester_controller.cpp b/decision/armor_tester/src/armor_tester_controller.cpp new file mode 100644 index 0000000..9ee12f4 --- /dev/null +++ b/decision/armor_tester/src/armor_tester_controller.cpp @@ -0,0 +1,214 @@ +#include "armor_tester/armor_tester_controller.hpp" + +#include +#include +#include +#include +#include +#include "behavior_interface/msg/armor.hpp" // unitree_vel, dji_vel + + +constexpr double NaN = std::numeric_limits::quiet_NaN(); + +namespace armor_tester_controller { + controller_interface::CallbackReturn ArmorTesterController::on_init() { + // The first line usually calls the parent on_init() method + // Here is the best place to initialize the variables, reserve memory, + // and declare node parameters used by the controller + + // call father on_init() method + auto ret = ChainableControllerInterface::on_init(); + if (ret != controller_interface::CallbackReturn::SUCCESS) { + return ret; + } + + // declare and set node parameters + try { + param_listener_ = std::make_shared (get_node()); + } catch (const std::exception & e) { + std::cerr << "Exception thrown during controller's init with message: " + << e.what() << std::endl; + return controller_interface::CallbackReturn::ERROR; + } + + return controller_interface::CallbackReturn::SUCCESS; + } + + controller_interface::CallbackReturn + ArmorTesterController::on_configure(const rclcpp_lifecycle::State & /*previous_state*/) { + // parameters are usually read here + // everything is prepared so that the controller can be started. + + // read parameters + params_ = param_listener_->get_params(); + + // initialize dji pid + dji_pid_ = std::make_shared ( + get_node(), "gains." + params_.dji_joint.name + "_vel2eff", true); + if (!dji_pid_->initPID()) { + RCLCPP_ERROR(get_node()->get_logger(), "Failed to initialize PID for DJI Motor"); + return controller_interface::CallbackReturn::FAILURE; + } + + // topic QoS (Quality of Service) + auto vel_sub_qos = rclcpp::SystemDefaultQoS(); + vel_sub_qos.keep_last(1); + vel_sub_qos.best_effort(); + // create subscriber + vel_subscriber_ = get_node()->create_subscription( + "~/armor_tester_velocity", vel_sub_qos, + std::bind(& ArmorTesterController::velocity_callback, this, std::placeholders::_1)); + // create, initialize the message, and write it the real time buffer + auto vel_msg = std::make_shared(); + vel_msg->dji_vel = NaN; + vel_msg->unitree_vel = NaN; + vel_buffer_.writeFromNonRT(vel_msg); + + RCLCPP_INFO(get_node()->get_logger(), "configure successfully"); + return controller_interface::CallbackReturn::SUCCESS; + } + + + controller_interface::InterfaceConfiguration + ArmorTesterController::command_interface_configuration() const { + controller_interface::InterfaceConfiguration cmd_itf_config; + + cmd_itf_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + cmd_itf_config.names.reserve(2); + cmd_itf_config.names.push_back( + params_.unitree_joint.name + "/" + HW_IF_VELOCITY); + cmd_itf_config.names.push_back( + params_.dji_joint.name + "/" + HW_IF_EFFORT); + + return cmd_itf_config; + } + + controller_interface::InterfaceConfiguration + ArmorTesterController::state_interface_configuration() const { + controller_interface::InterfaceConfiguration state_itf_config; + + state_itf_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + state_itf_config.names.reserve(1); + + // only the dji velocity is required + state_itf_config.names.push_back(params_.dji_joint.name + "/" + HW_IF_VELOCITY); + + return state_itf_config; + } + + void ArmorTesterController::velocity_callback( + const std::shared_ptr msg) { + // shared_ptr<..>, and still writeFromNonRT(msg)? not writeFromNonRT(*msg)??? + vel_buffer_.writeFromNonRT(msg); + } + + controller_interface::CallbackReturn + ArmorTesterController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) { + // set the default value in command + auto msg_ptr = * (vel_buffer_.readFromRT()); + msg_ptr->unitree_vel = NaN; + msg_ptr->dji_vel = NaN; + + // initialize the reference_interfaces_, where write the control reference (tgt) + // in update, the controller implement the control via reference_interfaces_ + reference_interfaces_.assign(reference_interfaces_.size(), NaN); + + return controller_interface::CallbackReturn::SUCCESS; + } + + controller_interface::return_type + ArmorTesterController::update_reference_from_subscribers() { + // shared_ptr must be allocated immediately to avoid dangling + auto current_vel_msg = * (vel_buffer_.readFromRT()); + + if (!std::isnan(current_vel_msg->unitree_vel) && + !std::isnan(current_vel_msg->dji_vel)) { + // set the velocity into the reference_interfaces_ + reference_interfaces_[0] = current_vel_msg->unitree_vel; + reference_interfaces_[1] = current_vel_msg->dji_vel; + + current_vel_msg->unitree_vel = NaN; + current_vel_msg->dji_vel = NaN; + } + + return controller_interface::return_type::OK; + } + + controller_interface::CallbackReturn + ArmorTesterController::on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/) { + // we use C-c to stop the program, + // and stop the device in the deconstructor of the hardware interface + // on_deactivate() here is useless for us. + + return controller_interface::CallbackReturn::SUCCESS; + } + + std::vector + ArmorTesterController::on_export_reference_interfaces() { + reference_interfaces_.resize(2, NaN); + + std::vector ref_itf; + ref_itf.reserve(reference_interfaces_.size()); + + ref_itf.emplace_back(get_node()->get_name(), + params_.unitree_joint.name + "/" + HW_IF_VELOCITY, + &reference_interfaces_[0]); + ref_itf.emplace_back(get_node()->get_name(), + params_.dji_joint.name + "/" + HW_IF_VELOCITY, + &reference_interfaces_[1]); + return ref_itf; + } + + controller_interface::return_type + ArmorTesterController::update_and_write_commands(const rclcpp::Time & time, + const rclcpp::Duration & period) { + if (command_interfaces_.size() != 2 || reference_interfaces_.size() != 2) { + RCLCPP_WARN(get_node()->get_logger(), "ref_itf or cmd_itf has wrong size"); + // return controller_interface::return_type::ERROR; + } + + auto ref_unitree_vel = reference_interfaces_[0]; + auto ref_dji_vel = reference_interfaces_[1]; + + // write to the command interface for unitree + if (!std::isnan(ref_unitree_vel)) { + command_interfaces_[0].set_value(ref_unitree_vel); + } else { + RCLCPP_WARN(get_node()->get_logger(), "the reference of unitree velocity is nan"); + // return controller_interface::return_type::ERROR; + } + + // write to the command interface for dji with pid + if (!std::isnan(ref_dji_vel)) { + double current_dji_vel = std::numeric_limits::quiet_NaN(); + + for (auto & state_itf : state_interfaces_) { + if (state_itf.get_name() == params_.dji_joint.name() && + state_itf.get_interface_name() == HW_IF_VELOCITY) { + current_dji_vel = state_itf.get_value(); + break; + } + } + + if (!std::isnan(current_dji_vel)) { + double dji_vel_err = ref_dji_vel - current_dji_vel; + double dji_vel_eff = dji_pid_.computeCommand(dji_vel_err, period); + command_interfaces_[1].set_value(dji_vel_eff); + } else { + RCLCPP_WARN(get_node()->get_logger(), "the current dji velocity is nan"); + // return controller_interface::return_type::ERROR; + } + } else { + RCLCPP_WARN(get_node()->get_logger(), "the reference of dji velocity is nan"); + // return controller_interface::return_type::ERROR; + } + + return controller_interface::return_type::OK; + } + +} + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS(armor_tester_controller::ArmorTesterController, + controller_interface::ChainableControllerInterface) diff --git a/decision/armor_tester/src/armor_tester_controller.yaml b/decision/armor_tester/src/armor_tester_controller.yaml new file mode 100644 index 0000000..0265dd8 --- /dev/null +++ b/decision/armor_tester/src/armor_tester_controller.yaml @@ -0,0 +1,15 @@ +armor_tester_controller: + unitree_joint: + name: + { + type: string, + default_value: "unitree_motor", + description: "specify the name of the unitree motor", + } + dji_joint: + name: + { + type: string, + default_value: "dji_motor", + description: "specify the name of the dji motor", + } diff --git a/interfaces/behavior_interface/CMakeLists.txt b/interfaces/behavior_interface/CMakeLists.txt index 8f766ce..5dfa48e 100644 --- a/interfaces/behavior_interface/CMakeLists.txt +++ b/interfaces/behavior_interface/CMakeLists.txt @@ -12,6 +12,7 @@ find_package(ament_cmake REQUIRED) # find_package( REQUIRED) find_package(rosidl_default_generators REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} + "msg/Armor.msg" "msg/Aim.msg" "msg/Chassis.msg" "msg/Move.msg" diff --git a/interfaces/behavior_interface/msg/Armor.msg b/interfaces/behavior_interface/msg/Armor.msg new file mode 100644 index 0000000..e3f5830 --- /dev/null +++ b/interfaces/behavior_interface/msg/Armor.msg @@ -0,0 +1,2 @@ +float64 unitree_vel # in radians, for unitree motor +float64 dji_vel # in radians, for dji motor From 30dd2afae366857e3f2e7832550d5aed6fd16215 Mon Sep 17 00:00:00 2001 From: Hanbin Zheng Date: Mon, 22 Sep 2025 21:23:37 +0800 Subject: [PATCH 2/8] update .gitignore --- .gitignore | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/.gitignore b/.gitignore index e129ef2..fce3960 100644 --- a/.gitignore +++ b/.gitignore @@ -2,4 +2,5 @@ build install log __pycache__/ -.cache \ No newline at end of file +.cache +.vscode From 2ea6ac68009fc490fd28552e02874c4b0a6d55d9 Mon Sep 17 00:00:00 2001 From: Hanbin Zheng Date: Mon, 22 Sep 2025 21:27:17 +0800 Subject: [PATCH 3/8] add armor_tester --- decision/armor_tester/CMakeLists.txt | 94 +++++++++++++++---- .../armor_tester/armor_tester_controller.hpp | 13 ++- decision/armor_tester/src/armor_tester.cpp | 2 +- .../src/armor_tester_controller.cpp | 33 ++++--- 4 files changed, 104 insertions(+), 38 deletions(-) diff --git a/decision/armor_tester/CMakeLists.txt b/decision/armor_tester/CMakeLists.txt index 84a0420..fb573b1 100644 --- a/decision/armor_tester/CMakeLists.txt +++ b/decision/armor_tester/CMakeLists.txt @@ -1,45 +1,101 @@ -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 +# generate parameter library for controller generate_parameter_library( - ${PROJECT_NAME}_parameters + ${PROJECT_NAME}_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 +# ) + +# construct controller shared library to complile the .so lib ament_auto_add_library( - ${PROJECT_NAME} SHARED + ${PROJECT_NAME} + SHARED src/armor_tester.cpp - src/armor_tester_controller.cpp - ) - -target_include_directories(${PROJECT_NAME} PUBLIC - "$" - "$") +) -target_link_libraries( - ${PROJECT_NAME} - ${PROJECT_NAME}_parameters +ament_auto_add_library( + ${PROJECT_NAME}_controller + SHARED + src/armor_tester_controller.cpp ) +# ament_auto_add_library ( +# target_package_name +# SHARED/STATIC/OBJECT +# sorce file +# ) + +# 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( +# # existing library name +# PLUGIN "" # existing class name +# EXECUTABLE # self-designed +# ) -pluginlib_export_plugin_description_file( - controller_interface armor_tester_controller.xml) +# add the include file +target_include_directories( + ${PROJECT_NAME} + PUBLIC + "$" + "$" +) + +target_include_directories( + ${PROJECT_NAME}_controller + PUBLIC + "$" + "$" +) +# target_include_directories( +# cmake-target +# PUBLIC +# # used during build +# # used during install +# ) + +# link the the parameter listeners +target_link_libraries( + ${PROJECT_NAME}_controller + ${PROJECT_NAME}_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 diff --git a/decision/armor_tester/include/armor_tester/armor_tester_controller.hpp b/decision/armor_tester/include/armor_tester/armor_tester_controller.hpp index 9e5787f..4db12ee 100644 --- a/decision/armor_tester/include/armor_tester/armor_tester_controller.hpp +++ b/decision/armor_tester/include/armor_tester/armor_tester_controller.hpp @@ -8,12 +8,15 @@ #include #include "controller_interface/controller_interface.hpp" -#include "rclcpp_lifecycle/node_interfaces/life_cycle_node_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.h" -#include "armor_tester_controller/armor_tester_controller_parameters.hpp" -#include "behavior_interface/msg/armor.hpp" // dji_vel, unitree_vel +#include "realtime_tools/realtime_buffer.hpp" +#include "realtime_tools/realtime_publisher.hpp" +#include +#include "behavior_interface/msg/armor.hpp" // dji_vel, unitree_vel namespace armor_tester_controller // namespace begin here { @@ -72,7 +75,7 @@ class ArmorTesterController : public controller_interface::ChainableControllerIn private: // callback function for subscriber (done) void velocity_callback(const std::shared_ptr msg); -} // class definition ends here +}; // class definition ends here } // namespace ends here diff --git a/decision/armor_tester/src/armor_tester.cpp b/decision/armor_tester/src/armor_tester.cpp index e2de1f8..a3aeaa1 100644 --- a/decision/armor_tester/src/armor_tester.cpp +++ b/decision/armor_tester/src/armor_tester.cpp @@ -17,7 +17,7 @@ class ArmorTester : public rclcpp::Node ls_x = ls_y = rs_x = rs_y = wheel = 0; lsw = rsw = ""; velocity_pub_ = this->create_publisher( - "~/armor_tester_velocity", 10); + "/armor_tester_controller/reference", 10); dbus_sub_ = this->create_subscription( "dbus_control", 10, diff --git a/decision/armor_tester/src/armor_tester_controller.cpp b/decision/armor_tester/src/armor_tester_controller.cpp index 9ee12f4..704a695 100644 --- a/decision/armor_tester/src/armor_tester_controller.cpp +++ b/decision/armor_tester/src/armor_tester_controller.cpp @@ -6,8 +6,10 @@ #include #include #include "behavior_interface/msg/armor.hpp" // unitree_vel, dji_vel +#include "hardware_interface/types/hardware_interface_type_values.hpp" - +using hardware_interface::HW_IF_EFFORT; +using hardware_interface::HW_IF_VELOCITY; constexpr double NaN = std::numeric_limits::quiet_NaN(); namespace armor_tester_controller { @@ -17,10 +19,10 @@ namespace armor_tester_controller { // and declare node parameters used by the controller // call father on_init() method - auto ret = ChainableControllerInterface::on_init(); - if (ret != controller_interface::CallbackReturn::SUCCESS) { - return ret; - } + // auto ret = ChainableControllerInterface::on_init(); + // if (ret != controller_interface::CallbackReturn::SUCCESS) { + // return ret; + // } // declare and set node parameters try { @@ -43,20 +45,21 @@ namespace armor_tester_controller { params_ = param_listener_->get_params(); // initialize dji pid + RCLCPP_INFO(get_node()->get_logger(), "The params_.dji_joint.name is: %s", params_.dji_joint.name.c_str()); dji_pid_ = std::make_shared ( get_node(), "gains." + params_.dji_joint.name + "_vel2eff", true); - if (!dji_pid_->initPID()) { + if (!dji_pid_->initPid()) { RCLCPP_ERROR(get_node()->get_logger(), "Failed to initialize PID for DJI Motor"); return controller_interface::CallbackReturn::FAILURE; } // topic QoS (Quality of Service) - auto vel_sub_qos = rclcpp::SystemDefaultQoS(); + auto vel_sub_qos = rclcpp::SystemDefaultsQoS(); vel_sub_qos.keep_last(1); vel_sub_qos.best_effort(); // create subscriber vel_subscriber_ = get_node()->create_subscription( - "~/armor_tester_velocity", vel_sub_qos, + "~/reference", vel_sub_qos, std::bind(& ArmorTesterController::velocity_callback, this, std::placeholders::_1)); // create, initialize the message, and write it the real time buffer auto vel_msg = std::make_shared(); @@ -160,7 +163,7 @@ namespace armor_tester_controller { } controller_interface::return_type - ArmorTesterController::update_and_write_commands(const rclcpp::Time & time, + ArmorTesterController::update_and_write_commands(const rclcpp::Time & /*time*/, const rclcpp::Duration & period) { if (command_interfaces_.size() != 2 || reference_interfaces_.size() != 2) { RCLCPP_WARN(get_node()->get_logger(), "ref_itf or cmd_itf has wrong size"); @@ -169,7 +172,8 @@ namespace armor_tester_controller { auto ref_unitree_vel = reference_interfaces_[0]; auto ref_dji_vel = reference_interfaces_[1]; - + RCLCPP_INFO(get_node()->get_logger(), "REF:unitree_vel:%.2lf, dji_vel: %.2lf", + ref_unitree_vel, ref_dji_vel); // write to the command interface for unitree if (!std::isnan(ref_unitree_vel)) { command_interfaces_[0].set_value(ref_unitree_vel); @@ -183,16 +187,19 @@ namespace armor_tester_controller { double current_dji_vel = std::numeric_limits::quiet_NaN(); for (auto & state_itf : state_interfaces_) { - if (state_itf.get_name() == params_.dji_joint.name() && - state_itf.get_interface_name() == HW_IF_VELOCITY) { + // write to the command interface for unitree + if (state_itf.get_name() == params_.dji_joint.name + "/" + HW_IF_VELOCITY) { current_dji_vel = state_itf.get_value(); + RCLCPP_INFO(get_node()->get_logger(), "STATE: dji_vel: %lf", current_dji_vel); break; } } if (!std::isnan(current_dji_vel)) { double dji_vel_err = ref_dji_vel - current_dji_vel; - double dji_vel_eff = dji_pid_.computeCommand(dji_vel_err, period); + double dji_vel_eff = dji_pid_->computeCommand(dji_vel_err, period); + RCLCPP_INFO(get_node()->get_logger(), "PID: current_dji_vel: %.2lf, ref_dji_vel: %.2lf, dji_vel_err: %.2lf, dji_vel_eff: %.2lf", + current_dji_vel, ref_dji_vel, dji_vel_err, dji_vel_eff); command_interfaces_[1].set_value(dji_vel_eff); } else { RCLCPP_WARN(get_node()->get_logger(), "the current dji velocity is nan"); From bf75b0261cdf2453352bea3a213d4d9f770fe620 Mon Sep 17 00:00:00 2001 From: Hanbin Zheng Date: Mon, 22 Sep 2025 21:28:01 +0800 Subject: [PATCH 4/8] add the urdf, yaml and launch file for armor_tester --- .../urdf/playground/armor_tester.xacro | 46 +++++++ meta_bringup/config/armor_tester.yaml | 21 ++++ meta_bringup/launch/armor_tester.py | 119 ++++++++++++++++++ 3 files changed, 186 insertions(+) create mode 100644 decomposition/metav_description/urdf/playground/armor_tester.xacro create mode 100644 meta_bringup/config/armor_tester.yaml create mode 100644 meta_bringup/launch/armor_tester.py diff --git a/decomposition/metav_description/urdf/playground/armor_tester.xacro b/decomposition/metav_description/urdf/playground/armor_tester.xacro new file mode 100644 index 0000000..e83b53e --- /dev/null +++ b/decomposition/metav_description/urdf/playground/armor_tester.xacro @@ -0,0 +1,46 @@ + + + + + + + + + meta_hardware/MetaRobotUnitreeMotorInterface + /dev/tty_RS485_1 + + + + + + + + + + + meta_hardware/MetaRobotDjiMotorNetwork + can_1 + + + + + + + M3508 + 3 + 20.0 + 0.0 + + + + + + + + + + + diff --git a/meta_bringup/config/armor_tester.yaml b/meta_bringup/config/armor_tester.yaml new file mode 100644 index 0000000..71ae295 --- /dev/null +++ b/meta_bringup/config/armor_tester.yaml @@ -0,0 +1,21 @@ +controller_manager: + ros__parameters: + update_rate: 250 # Hz + armor_tester_controller: + type: armor_tester_controller/ArmorTesterController + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + +armor_tester_controller: + ros__parameters: + unitree_joint: + name: "motor1" + dji_joint: + name: "motor2" + gains: + motor2_vel2eff: + { p: 2.0e-2, i: 1.0e-4, d: 0.0, i_clamp_max: 1.0, i_clamp_min: -1.0, antiwindup: true } + +dbus_control_node: + ros__parameters: + dbus_port: "tty_DBUS" diff --git a/meta_bringup/launch/armor_tester.py b/meta_bringup/launch/armor_tester.py new file mode 100644 index 0000000..de098c1 --- /dev/null +++ b/meta_bringup/launch/armor_tester.py @@ -0,0 +1,119 @@ +# Copyright 2021 Open Source Robotics Foundation, Inc. +# +# 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. + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution +from launch.conditions import IfCondition, UnlessCondition + +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + +import os +import sys + +from ament_index_python.packages import get_package_share_directory +sys.path.append(os.path.join(get_package_share_directory('meta_bringup'), 'launch')) + +from launch_utils import load_controller, register_loading_order, register_sequential_loading + +ARGUMENTS = [ + DeclareLaunchArgument( + 'enable_simulation', + default_value='false', + description='If true, the simulation will be started', + ) +] + +def generate_launch_description(): + # get uddf via xacro + robot_description_content = Command([ + PathJoinSubstitution([FindExecutable(name='xacro')]), + ' ', + PathJoinSubstitution([FindPackageShare('metav_description'), 'urdf', 'playground', 'armor_tester.xacro']) + ]) + + # node for robot state publisher + node_robot_state_publisher = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + parameters=[ + {'robot_description': robot_description_content} + ], + output='both', + emulate_tty=True + ) + + # ros2 control related launch, to read the yaml configure file + robot_controller_config = PathJoinSubstitution( + [ + FindPackageShare('meta_bringup'), + 'config', + 'armor_tester.yaml' + ] + ) + + # controller manager node + controller_manager = Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[robot_controller_config], + remappings=[ + ("~/robot_description", "/robot_description"), + ], + output='both', + emulate_tty=True, + ) + + # node for dbus_controll, not a controller + dbus_control_node = Node( + package='dbus_control', + executable='dbus_control_node', + name='dbus_control_node', + parameters=[robot_controller_config], + output='both', + emulate_tty=True + ) + + # node for armor_tester, and the executable is armor_tester_node + armor_tester_node = Node( + package='armor_tester', + executable='armor_tester_node', + name='armor_tester', + output='both', + parameters=[robot_controller_config], + emulate_tty=True + ) + + load_joint_state_broadcaster = load_controller('joint_state_broadcaster') + + load_controllers = [ + load_controller('armor_tester_controller') + ] + + return LaunchDescription([ + # lauch arguments + *ARGUMENTS, + # load robot state publisher + dbus_control_node, + armor_tester_node, + node_robot_state_publisher, + # launch controller manager(if not in simulation) + controller_manager, + # load joint state broadcaster + load_joint_state_broadcaster, + # load controller + *register_sequential_loading(load_joint_state_broadcaster, *load_controllers), + ]) From 566bdd46e2202232ef072679000893eb18ac70ec Mon Sep 17 00:00:00 2001 From: Hanbin Zheng Date: Mon, 22 Sep 2025 22:13:48 +0800 Subject: [PATCH 5/8] remove the controller to keep it a node --- decision/armor_tester/CMakeLists.txt | 57 ----- .../armor_tester/armor_tester_controller.xml | 28 --- .../armor_tester/armor_tester_controller.hpp | 82 ------- .../src/armor_tester_controller.cpp | 221 ------------------ .../src/armor_tester_controller.yaml | 15 -- 5 files changed, 403 deletions(-) delete mode 100644 decision/armor_tester/armor_tester_controller.xml delete mode 100644 decision/armor_tester/include/armor_tester/armor_tester_controller.hpp delete mode 100644 decision/armor_tester/src/armor_tester_controller.cpp delete mode 100644 decision/armor_tester/src/armor_tester_controller.yaml diff --git a/decision/armor_tester/CMakeLists.txt b/decision/armor_tester/CMakeLists.txt index fb573b1..f49deb3 100644 --- a/decision/armor_tester/CMakeLists.txt +++ b/decision/armor_tester/CMakeLists.txt @@ -11,17 +11,6 @@ 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( - ${PROJECT_NAME}_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 -# ) - # construct controller shared library to complile the .so lib ament_auto_add_library( ${PROJECT_NAME} @@ -29,18 +18,6 @@ ament_auto_add_library( src/armor_tester.cpp ) -ament_auto_add_library( - ${PROJECT_NAME}_controller - SHARED - src/armor_tester_controller.cpp -) - -# ament_auto_add_library ( -# target_package_name -# SHARED/STATIC/OBJECT -# sorce file -# ) - # register a class in library as plugin, here, the ArmorTester rclcpp_components_register_node( ${PROJECT_NAME} @@ -61,40 +38,6 @@ target_include_directories( "$" ) -target_include_directories( - ${PROJECT_NAME}_controller - PUBLIC - "$" - "$" -) - -# target_include_directories( -# cmake-target -# PUBLIC -# # used during build -# # used during install -# ) - -# link the the parameter listeners -target_link_libraries( - ${PROJECT_NAME}_controller - ${PROJECT_NAME}_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) diff --git a/decision/armor_tester/armor_tester_controller.xml b/decision/armor_tester/armor_tester_controller.xml deleted file mode 100644 index 443e55c..0000000 --- a/decision/armor_tester/armor_tester_controller.xml +++ /dev/null @@ -1,28 +0,0 @@ - - - - - - An Armor Tester Controller. - - - diff --git a/decision/armor_tester/include/armor_tester/armor_tester_controller.hpp b/decision/armor_tester/include/armor_tester/armor_tester_controller.hpp deleted file mode 100644 index 4db12ee..0000000 --- a/decision/armor_tester/include/armor_tester/armor_tester_controller.hpp +++ /dev/null @@ -1,82 +0,0 @@ -#ifndef ARMOR_TESTER_CONTROLLER_HPP_ -#define ARMOR_TESTER_CONTROLLER_HPP_ - -#include -#include -#include -#include -#include - -#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 -#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 - controller_interface::return_type update_reference_from_subscribers() override; - - // parameters - armor_tester_controller::Params params_; - std::shared_ptr param_listener_; - - // pid for dji motor - std::shared_ptr dji_pid_; - - // for subscriber - rclcpp::Subscription::SharedPtr vel_subscriber_ = nullptr; - realtime_tools::RealtimeBuffer> vel_buffer_; - - // override method from ChainableControllerInterface (done) - std::vector on_export_reference_interfaces() override; - - private: - // callback function for subscriber (done) - void velocity_callback(const std::shared_ptr msg); -}; // class definition ends here - -} // namespace ends here - -#endif // ARMOR_TESTER_CONTROLLER_HPP_ diff --git a/decision/armor_tester/src/armor_tester_controller.cpp b/decision/armor_tester/src/armor_tester_controller.cpp deleted file mode 100644 index 704a695..0000000 --- a/decision/armor_tester/src/armor_tester_controller.cpp +++ /dev/null @@ -1,221 +0,0 @@ -#include "armor_tester/armor_tester_controller.hpp" - -#include -#include -#include -#include -#include -#include "behavior_interface/msg/armor.hpp" // unitree_vel, dji_vel -#include "hardware_interface/types/hardware_interface_type_values.hpp" - -using hardware_interface::HW_IF_EFFORT; -using hardware_interface::HW_IF_VELOCITY; -constexpr double NaN = std::numeric_limits::quiet_NaN(); - -namespace armor_tester_controller { - controller_interface::CallbackReturn ArmorTesterController::on_init() { - // The first line usually calls the parent on_init() method - // Here is the best place to initialize the variables, reserve memory, - // and declare node parameters used by the controller - - // call father on_init() method - // auto ret = ChainableControllerInterface::on_init(); - // if (ret != controller_interface::CallbackReturn::SUCCESS) { - // return ret; - // } - - // declare and set node parameters - try { - param_listener_ = std::make_shared (get_node()); - } catch (const std::exception & e) { - std::cerr << "Exception thrown during controller's init with message: " - << e.what() << std::endl; - return controller_interface::CallbackReturn::ERROR; - } - - return controller_interface::CallbackReturn::SUCCESS; - } - - controller_interface::CallbackReturn - ArmorTesterController::on_configure(const rclcpp_lifecycle::State & /*previous_state*/) { - // parameters are usually read here - // everything is prepared so that the controller can be started. - - // read parameters - params_ = param_listener_->get_params(); - - // initialize dji pid - RCLCPP_INFO(get_node()->get_logger(), "The params_.dji_joint.name is: %s", params_.dji_joint.name.c_str()); - dji_pid_ = std::make_shared ( - get_node(), "gains." + params_.dji_joint.name + "_vel2eff", true); - if (!dji_pid_->initPid()) { - RCLCPP_ERROR(get_node()->get_logger(), "Failed to initialize PID for DJI Motor"); - return controller_interface::CallbackReturn::FAILURE; - } - - // topic QoS (Quality of Service) - auto vel_sub_qos = rclcpp::SystemDefaultsQoS(); - vel_sub_qos.keep_last(1); - vel_sub_qos.best_effort(); - // create subscriber - vel_subscriber_ = get_node()->create_subscription( - "~/reference", vel_sub_qos, - std::bind(& ArmorTesterController::velocity_callback, this, std::placeholders::_1)); - // create, initialize the message, and write it the real time buffer - auto vel_msg = std::make_shared(); - vel_msg->dji_vel = NaN; - vel_msg->unitree_vel = NaN; - vel_buffer_.writeFromNonRT(vel_msg); - - RCLCPP_INFO(get_node()->get_logger(), "configure successfully"); - return controller_interface::CallbackReturn::SUCCESS; - } - - - controller_interface::InterfaceConfiguration - ArmorTesterController::command_interface_configuration() const { - controller_interface::InterfaceConfiguration cmd_itf_config; - - cmd_itf_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; - cmd_itf_config.names.reserve(2); - cmd_itf_config.names.push_back( - params_.unitree_joint.name + "/" + HW_IF_VELOCITY); - cmd_itf_config.names.push_back( - params_.dji_joint.name + "/" + HW_IF_EFFORT); - - return cmd_itf_config; - } - - controller_interface::InterfaceConfiguration - ArmorTesterController::state_interface_configuration() const { - controller_interface::InterfaceConfiguration state_itf_config; - - state_itf_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; - state_itf_config.names.reserve(1); - - // only the dji velocity is required - state_itf_config.names.push_back(params_.dji_joint.name + "/" + HW_IF_VELOCITY); - - return state_itf_config; - } - - void ArmorTesterController::velocity_callback( - const std::shared_ptr msg) { - // shared_ptr<..>, and still writeFromNonRT(msg)? not writeFromNonRT(*msg)??? - vel_buffer_.writeFromNonRT(msg); - } - - controller_interface::CallbackReturn - ArmorTesterController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) { - // set the default value in command - auto msg_ptr = * (vel_buffer_.readFromRT()); - msg_ptr->unitree_vel = NaN; - msg_ptr->dji_vel = NaN; - - // initialize the reference_interfaces_, where write the control reference (tgt) - // in update, the controller implement the control via reference_interfaces_ - reference_interfaces_.assign(reference_interfaces_.size(), NaN); - - return controller_interface::CallbackReturn::SUCCESS; - } - - controller_interface::return_type - ArmorTesterController::update_reference_from_subscribers() { - // shared_ptr must be allocated immediately to avoid dangling - auto current_vel_msg = * (vel_buffer_.readFromRT()); - - if (!std::isnan(current_vel_msg->unitree_vel) && - !std::isnan(current_vel_msg->dji_vel)) { - // set the velocity into the reference_interfaces_ - reference_interfaces_[0] = current_vel_msg->unitree_vel; - reference_interfaces_[1] = current_vel_msg->dji_vel; - - current_vel_msg->unitree_vel = NaN; - current_vel_msg->dji_vel = NaN; - } - - return controller_interface::return_type::OK; - } - - controller_interface::CallbackReturn - ArmorTesterController::on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/) { - // we use C-c to stop the program, - // and stop the device in the deconstructor of the hardware interface - // on_deactivate() here is useless for us. - - return controller_interface::CallbackReturn::SUCCESS; - } - - std::vector - ArmorTesterController::on_export_reference_interfaces() { - reference_interfaces_.resize(2, NaN); - - std::vector ref_itf; - ref_itf.reserve(reference_interfaces_.size()); - - ref_itf.emplace_back(get_node()->get_name(), - params_.unitree_joint.name + "/" + HW_IF_VELOCITY, - &reference_interfaces_[0]); - ref_itf.emplace_back(get_node()->get_name(), - params_.dji_joint.name + "/" + HW_IF_VELOCITY, - &reference_interfaces_[1]); - return ref_itf; - } - - controller_interface::return_type - ArmorTesterController::update_and_write_commands(const rclcpp::Time & /*time*/, - const rclcpp::Duration & period) { - if (command_interfaces_.size() != 2 || reference_interfaces_.size() != 2) { - RCLCPP_WARN(get_node()->get_logger(), "ref_itf or cmd_itf has wrong size"); - // return controller_interface::return_type::ERROR; - } - - auto ref_unitree_vel = reference_interfaces_[0]; - auto ref_dji_vel = reference_interfaces_[1]; - RCLCPP_INFO(get_node()->get_logger(), "REF:unitree_vel:%.2lf, dji_vel: %.2lf", - ref_unitree_vel, ref_dji_vel); - // write to the command interface for unitree - if (!std::isnan(ref_unitree_vel)) { - command_interfaces_[0].set_value(ref_unitree_vel); - } else { - RCLCPP_WARN(get_node()->get_logger(), "the reference of unitree velocity is nan"); - // return controller_interface::return_type::ERROR; - } - - // write to the command interface for dji with pid - if (!std::isnan(ref_dji_vel)) { - double current_dji_vel = std::numeric_limits::quiet_NaN(); - - for (auto & state_itf : state_interfaces_) { - // write to the command interface for unitree - if (state_itf.get_name() == params_.dji_joint.name + "/" + HW_IF_VELOCITY) { - current_dji_vel = state_itf.get_value(); - RCLCPP_INFO(get_node()->get_logger(), "STATE: dji_vel: %lf", current_dji_vel); - break; - } - } - - if (!std::isnan(current_dji_vel)) { - double dji_vel_err = ref_dji_vel - current_dji_vel; - double dji_vel_eff = dji_pid_->computeCommand(dji_vel_err, period); - RCLCPP_INFO(get_node()->get_logger(), "PID: current_dji_vel: %.2lf, ref_dji_vel: %.2lf, dji_vel_err: %.2lf, dji_vel_eff: %.2lf", - current_dji_vel, ref_dji_vel, dji_vel_err, dji_vel_eff); - command_interfaces_[1].set_value(dji_vel_eff); - } else { - RCLCPP_WARN(get_node()->get_logger(), "the current dji velocity is nan"); - // return controller_interface::return_type::ERROR; - } - } else { - RCLCPP_WARN(get_node()->get_logger(), "the reference of dji velocity is nan"); - // return controller_interface::return_type::ERROR; - } - - return controller_interface::return_type::OK; - } - -} - -#include "pluginlib/class_list_macros.hpp" - -PLUGINLIB_EXPORT_CLASS(armor_tester_controller::ArmorTesterController, - controller_interface::ChainableControllerInterface) diff --git a/decision/armor_tester/src/armor_tester_controller.yaml b/decision/armor_tester/src/armor_tester_controller.yaml deleted file mode 100644 index 0265dd8..0000000 --- a/decision/armor_tester/src/armor_tester_controller.yaml +++ /dev/null @@ -1,15 +0,0 @@ -armor_tester_controller: - unitree_joint: - name: - { - type: string, - default_value: "unitree_motor", - description: "specify the name of the unitree motor", - } - dji_joint: - name: - { - type: string, - default_value: "dji_motor", - description: "specify the name of the dji motor", - } From 41bae7f4884f28921a5be101e249a79afb1eb4f9 Mon Sep 17 00:00:00 2001 From: Hanbin Zheng Date: Mon, 22 Sep 2025 22:14:30 +0800 Subject: [PATCH 6/8] add armor_tester_controller for meta_utils_controller --- .../meta_utils_controller/CMakeLists.txt | 84 +++++++ .../armor_tester_controller.xml | 28 +++ .../armor_tester_controller.hpp | 82 +++++++ .../meta_utils_controller/package.xml | 34 +++ .../src/armor_tester_controller.cpp | 221 ++++++++++++++++++ .../src/armor_tester_controller.yaml | 15 ++ 6 files changed, 464 insertions(+) create mode 100644 decomposition/meta_utils_controller/CMakeLists.txt create mode 100644 decomposition/meta_utils_controller/armor_tester_controller.xml create mode 100644 decomposition/meta_utils_controller/include/meta_utils_controller/armor_tester_controller.hpp create mode 100644 decomposition/meta_utils_controller/package.xml create mode 100644 decomposition/meta_utils_controller/src/armor_tester_controller.cpp create mode 100644 decomposition/meta_utils_controller/src/armor_tester_controller.yaml diff --git a/decomposition/meta_utils_controller/CMakeLists.txt b/decomposition/meta_utils_controller/CMakeLists.txt new file mode 100644 index 0000000..8586c60 --- /dev/null +++ b/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 + "$" + "$" +) + +# target_include_directories( +# cmake-target +# PUBLIC +# # used during build +# # 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() diff --git a/decomposition/meta_utils_controller/armor_tester_controller.xml b/decomposition/meta_utils_controller/armor_tester_controller.xml new file mode 100644 index 0000000..443e55c --- /dev/null +++ b/decomposition/meta_utils_controller/armor_tester_controller.xml @@ -0,0 +1,28 @@ + + + + + + An Armor Tester Controller. + + + diff --git a/decomposition/meta_utils_controller/include/meta_utils_controller/armor_tester_controller.hpp b/decomposition/meta_utils_controller/include/meta_utils_controller/armor_tester_controller.hpp new file mode 100644 index 0000000..d856179 --- /dev/null +++ b/decomposition/meta_utils_controller/include/meta_utils_controller/armor_tester_controller.hpp @@ -0,0 +1,82 @@ +#ifndef ARMOR_TESTER_CONTROLLER_HPP_ +#define ARMOR_TESTER_CONTROLLER_HPP_ + +#include +#include +#include +#include +#include + +#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 +#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 + controller_interface::return_type update_reference_from_subscribers() override; + + // parameters + armor_tester_controller::Params params_; + std::shared_ptr param_listener_; + + // pid for dji motor + std::shared_ptr dji_pid_; + + // for subscriber + rclcpp::Subscription::SharedPtr vel_subscriber_ = nullptr; + realtime_tools::RealtimeBuffer> vel_buffer_; + + // override method from ChainableControllerInterface (done) + std::vector on_export_reference_interfaces() override; + + private: + // callback function for subscriber (done) + void velocity_callback(const std::shared_ptr msg); +}; // class definition ends here + +} // namespace ends here + +#endif // ARMOR_TESTER_CONTROLLER_HPP_ diff --git a/decomposition/meta_utils_controller/package.xml b/decomposition/meta_utils_controller/package.xml new file mode 100644 index 0000000..43b5808 --- /dev/null +++ b/decomposition/meta_utils_controller/package.xml @@ -0,0 +1,34 @@ + + + + meta_utils_controller + 0.0.0 + Package for Meta Utils + hanbinzheng + TODO: License declaration + + ament_cmake + ament_cmake_auto + + generate_parameter_library + + operation_interface + geometry_msgs + behavior_interface + control_msgs + control_toolbox + controller_interface + hardware_interface + realtime_tools + pluginlib + rclcpp + rclcpp_lifecycle + rclcpp_components + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/decomposition/meta_utils_controller/src/armor_tester_controller.cpp b/decomposition/meta_utils_controller/src/armor_tester_controller.cpp new file mode 100644 index 0000000..a19ada7 --- /dev/null +++ b/decomposition/meta_utils_controller/src/armor_tester_controller.cpp @@ -0,0 +1,221 @@ +#include "meta_utils_controller/armor_tester_controller.hpp" + +#include +#include +#include +#include +#include +#include "behavior_interface/msg/armor.hpp" // unitree_vel, dji_vel +#include "hardware_interface/types/hardware_interface_type_values.hpp" + +using hardware_interface::HW_IF_EFFORT; +using hardware_interface::HW_IF_VELOCITY; +constexpr double NaN = std::numeric_limits::quiet_NaN(); + +namespace armor_tester_controller { + controller_interface::CallbackReturn ArmorTesterController::on_init() { + // The first line usually calls the parent on_init() method + // Here is the best place to initialize the variables, reserve memory, + // and declare node parameters used by the controller + + // call father on_init() method + // auto ret = ChainableControllerInterface::on_init(); + // if (ret != controller_interface::CallbackReturn::SUCCESS) { + // return ret; + // } + + // declare and set node parameters + try { + param_listener_ = std::make_shared (get_node()); + } catch (const std::exception & e) { + std::cerr << "Exception thrown during controller's init with message: " + << e.what() << std::endl; + return controller_interface::CallbackReturn::ERROR; + } + + return controller_interface::CallbackReturn::SUCCESS; + } + + controller_interface::CallbackReturn + ArmorTesterController::on_configure(const rclcpp_lifecycle::State & /*previous_state*/) { + // parameters are usually read here + // everything is prepared so that the controller can be started. + + // read parameters + params_ = param_listener_->get_params(); + + // initialize dji pid + RCLCPP_INFO(get_node()->get_logger(), "The params_.dji_joint.name is: %s", params_.dji_joint.name.c_str()); + dji_pid_ = std::make_shared ( + get_node(), "gains." + params_.dji_joint.name + "_vel2eff", true); + if (!dji_pid_->initPid()) { + RCLCPP_ERROR(get_node()->get_logger(), "Failed to initialize PID for DJI Motor"); + return controller_interface::CallbackReturn::FAILURE; + } + + // topic QoS (Quality of Service) + auto vel_sub_qos = rclcpp::SystemDefaultsQoS(); + vel_sub_qos.keep_last(1); + vel_sub_qos.best_effort(); + // create subscriber + vel_subscriber_ = get_node()->create_subscription( + "~/reference", vel_sub_qos, + std::bind(& ArmorTesterController::velocity_callback, this, std::placeholders::_1)); + // create, initialize the message, and write it the real time buffer + auto vel_msg = std::make_shared(); + vel_msg->dji_vel = NaN; + vel_msg->unitree_vel = NaN; + vel_buffer_.writeFromNonRT(vel_msg); + + RCLCPP_INFO(get_node()->get_logger(), "configure successfully"); + return controller_interface::CallbackReturn::SUCCESS; + } + + + controller_interface::InterfaceConfiguration + ArmorTesterController::command_interface_configuration() const { + controller_interface::InterfaceConfiguration cmd_itf_config; + + cmd_itf_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + cmd_itf_config.names.reserve(2); + cmd_itf_config.names.push_back( + params_.unitree_joint.name + "/" + HW_IF_VELOCITY); + cmd_itf_config.names.push_back( + params_.dji_joint.name + "/" + HW_IF_EFFORT); + + return cmd_itf_config; + } + + controller_interface::InterfaceConfiguration + ArmorTesterController::state_interface_configuration() const { + controller_interface::InterfaceConfiguration state_itf_config; + + state_itf_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + state_itf_config.names.reserve(1); + + // only the dji velocity is required + state_itf_config.names.push_back(params_.dji_joint.name + "/" + HW_IF_VELOCITY); + + return state_itf_config; + } + + void ArmorTesterController::velocity_callback( + const std::shared_ptr msg) { + // shared_ptr<..>, and still writeFromNonRT(msg)? not writeFromNonRT(*msg)??? + vel_buffer_.writeFromNonRT(msg); + } + + controller_interface::CallbackReturn + ArmorTesterController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) { + // set the default value in command + auto msg_ptr = * (vel_buffer_.readFromRT()); + msg_ptr->unitree_vel = NaN; + msg_ptr->dji_vel = NaN; + + // initialize the reference_interfaces_, where write the control reference (tgt) + // in update, the controller implement the control via reference_interfaces_ + reference_interfaces_.assign(reference_interfaces_.size(), NaN); + + return controller_interface::CallbackReturn::SUCCESS; + } + + controller_interface::return_type + ArmorTesterController::update_reference_from_subscribers() { + // shared_ptr must be allocated immediately to avoid dangling + auto current_vel_msg = * (vel_buffer_.readFromRT()); + + if (!std::isnan(current_vel_msg->unitree_vel) && + !std::isnan(current_vel_msg->dji_vel)) { + // set the velocity into the reference_interfaces_ + reference_interfaces_[0] = current_vel_msg->unitree_vel; + reference_interfaces_[1] = current_vel_msg->dji_vel; + + current_vel_msg->unitree_vel = NaN; + current_vel_msg->dji_vel = NaN; + } + + return controller_interface::return_type::OK; + } + + controller_interface::CallbackReturn + ArmorTesterController::on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/) { + // we use C-c to stop the program, + // and stop the device in the deconstructor of the hardware interface + // on_deactivate() here is useless for us. + + return controller_interface::CallbackReturn::SUCCESS; + } + + std::vector + ArmorTesterController::on_export_reference_interfaces() { + reference_interfaces_.resize(2, NaN); + + std::vector ref_itf; + ref_itf.reserve(reference_interfaces_.size()); + + ref_itf.emplace_back(get_node()->get_name(), + params_.unitree_joint.name + "/" + HW_IF_VELOCITY, + &reference_interfaces_[0]); + ref_itf.emplace_back(get_node()->get_name(), + params_.dji_joint.name + "/" + HW_IF_VELOCITY, + &reference_interfaces_[1]); + return ref_itf; + } + + controller_interface::return_type + ArmorTesterController::update_and_write_commands(const rclcpp::Time & /*time*/, + const rclcpp::Duration & period) { + if (command_interfaces_.size() != 2 || reference_interfaces_.size() != 2) { + RCLCPP_WARN(get_node()->get_logger(), "ref_itf or cmd_itf has wrong size"); + // return controller_interface::return_type::ERROR; + } + + auto ref_unitree_vel = reference_interfaces_[0]; + auto ref_dji_vel = reference_interfaces_[1]; + RCLCPP_INFO(get_node()->get_logger(), "REF:unitree_vel:%.2lf, dji_vel: %.2lf", + ref_unitree_vel, ref_dji_vel); + // write to the command interface for unitree + if (!std::isnan(ref_unitree_vel)) { + command_interfaces_[0].set_value(ref_unitree_vel); + } else { + RCLCPP_WARN(get_node()->get_logger(), "the reference of unitree velocity is nan"); + // return controller_interface::return_type::ERROR; + } + + // write to the command interface for dji with pid + if (!std::isnan(ref_dji_vel)) { + double current_dji_vel = std::numeric_limits::quiet_NaN(); + + for (auto & state_itf : state_interfaces_) { + // write to the command interface for unitree + if (state_itf.get_name() == params_.dji_joint.name + "/" + HW_IF_VELOCITY) { + current_dji_vel = state_itf.get_value(); + RCLCPP_INFO(get_node()->get_logger(), "STATE: dji_vel: %lf", current_dji_vel); + break; + } + } + + if (!std::isnan(current_dji_vel)) { + double dji_vel_err = ref_dji_vel - current_dji_vel; + double dji_vel_eff = dji_pid_->computeCommand(dji_vel_err, period); + RCLCPP_INFO(get_node()->get_logger(), "PID: current_dji_vel: %.2lf, ref_dji_vel: %.2lf, dji_vel_err: %.2lf, dji_vel_eff: %.2lf", + current_dji_vel, ref_dji_vel, dji_vel_err, dji_vel_eff); + command_interfaces_[1].set_value(dji_vel_eff); + } else { + RCLCPP_WARN(get_node()->get_logger(), "the current dji velocity is nan"); + // return controller_interface::return_type::ERROR; + } + } else { + RCLCPP_WARN(get_node()->get_logger(), "the reference of dji velocity is nan"); + // return controller_interface::return_type::ERROR; + } + + return controller_interface::return_type::OK; + } + +} + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS(armor_tester_controller::ArmorTesterController, + controller_interface::ChainableControllerInterface) diff --git a/decomposition/meta_utils_controller/src/armor_tester_controller.yaml b/decomposition/meta_utils_controller/src/armor_tester_controller.yaml new file mode 100644 index 0000000..0265dd8 --- /dev/null +++ b/decomposition/meta_utils_controller/src/armor_tester_controller.yaml @@ -0,0 +1,15 @@ +armor_tester_controller: + unitree_joint: + name: + { + type: string, + default_value: "unitree_motor", + description: "specify the name of the unitree motor", + } + dji_joint: + name: + { + type: string, + default_value: "dji_motor", + description: "specify the name of the dji motor", + } From 792dbb364f9741956068797253d00389e66f578b Mon Sep 17 00:00:00 2001 From: Hanbin Zheng Date: Fri, 26 Sep 2025 19:00:45 +0800 Subject: [PATCH 7/8] revise the velocity --- decision/armor_tester/src/armor_tester.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/decision/armor_tester/src/armor_tester.cpp b/decision/armor_tester/src/armor_tester.cpp index a3aeaa1..e934630 100644 --- a/decision/armor_tester/src/armor_tester.cpp +++ b/decision/armor_tester/src/armor_tester.cpp @@ -56,11 +56,11 @@ class ArmorTester : public rclcpp::Node if (lsw != "MID") return; behavior_interface::msg::Armor armor_tester_velocity; - armor_tester_velocity.unitree_vel = ls_x * 2 * PI; - armor_tester_velocity.dji_vel = rs_x * 2 * PI; + armor_tester_velocity.unitree_vel = ls_y * 4 * PI; + armor_tester_velocity.dji_vel = rs_y * 4 * PI; RCLCPP_INFO(this->get_logger(), "unitree_vel:%lf, dji_vel: %lf", - ls_x * 2 * PI, rs_x * 2 * PI); + ls_y * 4 * PI, rs_y * 4 * PI); velocity_pub_->publish(armor_tester_velocity); } }; From 66b3007755c7e5f47eb4fea6b2df9147452feb92 Mon Sep 17 00:00:00 2001 From: Hanbin Zheng Date: Fri, 26 Sep 2025 19:01:29 +0800 Subject: [PATCH 8/8] update for jazzy version --- .../armor_tester_controller.hpp | 7 ++- .../src/armor_tester_controller.cpp | 44 +++++++++---------- 2 files changed, 28 insertions(+), 23 deletions(-) diff --git a/decomposition/meta_utils_controller/include/meta_utils_controller/armor_tester_controller.hpp b/decomposition/meta_utils_controller/include/meta_utils_controller/armor_tester_controller.hpp index d856179..a36eee3 100644 --- a/decomposition/meta_utils_controller/include/meta_utils_controller/armor_tester_controller.hpp +++ b/decomposition/meta_utils_controller/include/meta_utils_controller/armor_tester_controller.hpp @@ -56,7 +56,12 @@ class ArmorTesterController : public controller_interface::ChainableControllerIn protected: // override method from ChainableControllerInterface - controller_interface::return_type update_reference_from_subscribers() override; + #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_; diff --git a/decomposition/meta_utils_controller/src/armor_tester_controller.cpp b/decomposition/meta_utils_controller/src/armor_tester_controller.cpp index a19ada7..564a194 100644 --- a/decomposition/meta_utils_controller/src/armor_tester_controller.cpp +++ b/decomposition/meta_utils_controller/src/armor_tester_controller.cpp @@ -45,7 +45,6 @@ namespace armor_tester_controller { params_ = param_listener_->get_params(); // initialize dji pid - RCLCPP_INFO(get_node()->get_logger(), "The params_.dji_joint.name is: %s", params_.dji_joint.name.c_str()); dji_pid_ = std::make_shared ( get_node(), "gains." + params_.dji_joint.name + "_vel2eff", true); if (!dji_pid_->initPid()) { @@ -119,23 +118,29 @@ namespace armor_tester_controller { return controller_interface::CallbackReturn::SUCCESS; } - controller_interface::return_type - ArmorTesterController::update_reference_from_subscribers() { - // shared_ptr must be allocated immediately to avoid dangling - auto current_vel_msg = * (vel_buffer_.readFromRT()); - - if (!std::isnan(current_vel_msg->unitree_vel) && - !std::isnan(current_vel_msg->dji_vel)) { - // set the velocity into the reference_interfaces_ - reference_interfaces_[0] = current_vel_msg->unitree_vel; - reference_interfaces_[1] = current_vel_msg->dji_vel; - - current_vel_msg->unitree_vel = NaN; - current_vel_msg->dji_vel = NaN; - } + #if RCLCPP_VERSION_MAJOR >= 28 // Ros2 Jazzy or later + controller_interface::return_type + ArmorTesterController::update_reference_from_subscribers( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { + #else + controller_interface::return_type + ArmorTesterController::update_reference_from_subscribers() { + #endif + // shared_ptr must be allocated immediately to avoid dangling + auto current_vel_msg = * (vel_buffer_.readFromRT()); + + if (!std::isnan(current_vel_msg->unitree_vel) && + !std::isnan(current_vel_msg->dji_vel)) { + // set the velocity into the reference_interfaces_ + reference_interfaces_[0] = current_vel_msg->unitree_vel; + reference_interfaces_[1] = current_vel_msg->dji_vel; + + current_vel_msg->unitree_vel = NaN; + current_vel_msg->dji_vel = NaN; + } - return controller_interface::return_type::OK; - } + return controller_interface::return_type::OK; + } controller_interface::CallbackReturn ArmorTesterController::on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/) { @@ -172,8 +177,6 @@ namespace armor_tester_controller { auto ref_unitree_vel = reference_interfaces_[0]; auto ref_dji_vel = reference_interfaces_[1]; - RCLCPP_INFO(get_node()->get_logger(), "REF:unitree_vel:%.2lf, dji_vel: %.2lf", - ref_unitree_vel, ref_dji_vel); // write to the command interface for unitree if (!std::isnan(ref_unitree_vel)) { command_interfaces_[0].set_value(ref_unitree_vel); @@ -190,7 +193,6 @@ namespace armor_tester_controller { // write to the command interface for unitree if (state_itf.get_name() == params_.dji_joint.name + "/" + HW_IF_VELOCITY) { current_dji_vel = state_itf.get_value(); - RCLCPP_INFO(get_node()->get_logger(), "STATE: dji_vel: %lf", current_dji_vel); break; } } @@ -198,8 +200,6 @@ namespace armor_tester_controller { if (!std::isnan(current_dji_vel)) { double dji_vel_err = ref_dji_vel - current_dji_vel; double dji_vel_eff = dji_pid_->computeCommand(dji_vel_err, period); - RCLCPP_INFO(get_node()->get_logger(), "PID: current_dji_vel: %.2lf, ref_dji_vel: %.2lf, dji_vel_err: %.2lf, dji_vel_eff: %.2lf", - current_dji_vel, ref_dji_vel, dji_vel_err, dji_vel_eff); command_interfaces_[1].set_value(dji_vel_eff); } else { RCLCPP_WARN(get_node()->get_logger(), "the current dji velocity is nan");