From d0630a82c79776f434c5b0b834bd12217d19a329 Mon Sep 17 00:00:00 2001 From: robofish <1683502971@qq.com> Date: Fri, 16 May 2025 21:45:58 +0800 Subject: [PATCH] =?UTF-8?q?12=E4=B8=AA=E7=94=B5=E6=9C=BA=E8=AF=BB=E5=86=99?= =?UTF-8?q?=E6=AD=A3=E5=B8=B8?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .vscode/settings.json | 2 +- .../joystick_input/src/JoystickInput.cpp | 8 +- .../mdog_simple_controller/CMakeLists.txt | 89 +----- .../mdog_simple_controller.hpp | 117 -------- ...date_mdog_simple_controller_parameters.hpp | 43 --- .../visibility_control.h | 54 ---- .../mdog_simple_controller.xml | 28 -- .../mdog_simple_controller/package.xml | 23 +- .../src/mdog_simple_controller.cpp | 265 ----------------- .../src/mdog_simple_controller.yaml | 50 ---- .../test/mdog_simple_controller_params.yaml | 26 -- ...g_simple_controller_preceeding_params.yaml | 28 -- .../test/test_load_mdog_simple_controller.cpp | 44 --- .../test/test_mdog_simple_controller.cpp | 277 ------------------ .../test/test_mdog_simple_controller.hpp | 274 ----------------- ...test_mdog_simple_controller_preceeding.cpp | 80 ----- src/controllers/trot.py | 0 src/hardware/mdog_hardware/CMakeLists.txt | 93 ------ .../mdog_hardware/mdog_hardware_interface.hpp | 79 ----- .../mdog_hardware/visibility_control.h | 49 ---- .../launch/mdog_hardware.launch.py | 43 --- src/hardware/mdog_hardware/mdog_hardware.xml | 9 - src/hardware/mdog_hardware/package.xml | 30 -- .../src/mdog_hardware_interface.cpp | 165 ----------- .../test/test_mdog_hardware_interface.cpp | 57 ---- .../unitree_leg_serial.hpp | 48 +-- .../launch/unitree_leg_serial.launch.py | 10 +- .../src/unitree_leg_serial.cpp | 242 +++++++++------ .../qut/mdog_description/urdf/robot.urdf | 18 +- .../mdog_description/xacro/ros2_control.xacro | 2 +- 30 files changed, 205 insertions(+), 2048 deletions(-) delete mode 100644 src/controllers/mdog_simple_controller/include/mdog_simple_controller/mdog_simple_controller.hpp delete mode 100644 src/controllers/mdog_simple_controller/include/mdog_simple_controller/validate_mdog_simple_controller_parameters.hpp delete mode 100644 src/controllers/mdog_simple_controller/include/mdog_simple_controller/visibility_control.h delete mode 100644 src/controllers/mdog_simple_controller/mdog_simple_controller.xml delete mode 100644 src/controllers/mdog_simple_controller/src/mdog_simple_controller.cpp delete mode 100644 src/controllers/mdog_simple_controller/src/mdog_simple_controller.yaml delete mode 100644 src/controllers/mdog_simple_controller/test/mdog_simple_controller_params.yaml delete mode 100644 src/controllers/mdog_simple_controller/test/mdog_simple_controller_preceeding_params.yaml delete mode 100644 src/controllers/mdog_simple_controller/test/test_load_mdog_simple_controller.cpp delete mode 100644 src/controllers/mdog_simple_controller/test/test_mdog_simple_controller.cpp delete mode 100644 src/controllers/mdog_simple_controller/test/test_mdog_simple_controller.hpp delete mode 100644 src/controllers/mdog_simple_controller/test/test_mdog_simple_controller_preceeding.cpp delete mode 100644 src/controllers/trot.py delete mode 100644 src/hardware/mdog_hardware/CMakeLists.txt delete mode 100644 src/hardware/mdog_hardware/include/mdog_hardware/mdog_hardware_interface.hpp delete mode 100644 src/hardware/mdog_hardware/include/mdog_hardware/visibility_control.h delete mode 100644 src/hardware/mdog_hardware/launch/mdog_hardware.launch.py delete mode 100644 src/hardware/mdog_hardware/mdog_hardware.xml delete mode 100644 src/hardware/mdog_hardware/package.xml delete mode 100644 src/hardware/mdog_hardware/src/mdog_hardware_interface.cpp delete mode 100644 src/hardware/mdog_hardware/test/test_mdog_hardware_interface.cpp diff --git a/.vscode/settings.json b/.vscode/settings.json index 1507357..5e09a03 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -1,5 +1,5 @@ { "files.associations": { - "functional": "cpp" + "chrono": "cpp" } } \ No newline at end of file diff --git a/src/commands/joystick_input/src/JoystickInput.cpp b/src/commands/joystick_input/src/JoystickInput.cpp index 1aa18f8..b282738 100644 --- a/src/commands/joystick_input/src/JoystickInput.cpp +++ b/src/commands/joystick_input/src/JoystickInput.cpp @@ -25,9 +25,9 @@ void JoystickInput::joy_callback(sensor_msgs::msg::Joy::SharedPtr msg) { inputs_.command = 5; // LT + B } else if (msg->axes[2] != 1 && msg->buttons[0]) { inputs_.command = 6; // LT + A - } else if (msg->axes[2] != 1 && msg->buttons[2]) { - inputs_.command = 7; // LT + X } else if (msg->axes[2] != 1 && msg->buttons[3]) { + inputs_.command = 7; // LT + X + } else if (msg->axes[2] != 1 && msg->buttons[4]) { inputs_.command = 8; // LT + Y } else if (msg->buttons[7]) { inputs_.command = 9; // START @@ -35,8 +35,8 @@ void JoystickInput::joy_callback(sensor_msgs::msg::Joy::SharedPtr msg) { inputs_.command = 0; inputs_.lx = -msg->axes[0]; inputs_.ly = msg->axes[1]; - inputs_.rx = -msg->axes[3]; - inputs_.ry = msg->axes[4]; + inputs_.rx = -msg->axes[2]; + inputs_.ry = msg->axes[3]; } publisher_->publish(inputs_); } diff --git a/src/controllers/mdog_simple_controller/CMakeLists.txt b/src/controllers/mdog_simple_controller/CMakeLists.txt index f11d6cf..deb36c0 100644 --- a/src/controllers/mdog_simple_controller/CMakeLists.txt +++ b/src/controllers/mdog_simple_controller/CMakeLists.txt @@ -6,58 +6,19 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") endif() # find dependencies -set(THIS_PACKAGE_INCLUDE_DEPENDS - control_msgs - controller_interface - hardware_interface - pluginlib - rclcpp - rclcpp_lifecycle - realtime_tools - std_srvs -) - find_package(ament_cmake REQUIRED) -find_package(generate_parameter_library REQUIRED) -find_package(ament_cmake_gmock REQUIRED) -find_package(controller_manager REQUIRED) -find_package(hardware_interface REQUIRED) -find_package(ros2_control_test_assets REQUIRED) -foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) - find_package(${Dependency} REQUIRED) -endforeach() - -# Add mdog_simple_controller library related compile commands -generate_parameter_library(mdog_simple_controller_parameters - src/mdog_simple_controller.yaml - include/mdog_simple_controller/validate_mdog_simple_controller_parameters.hpp -) -add_library( - mdog_simple_controller - SHARED - src/mdog_simple_controller.cpp -) -target_include_directories(mdog_simple_controller PUBLIC - "$" - "$") -target_link_libraries(mdog_simple_controller mdog_simple_controller_parameters) -ament_target_dependencies(mdog_simple_controller ${THIS_PACKAGE_INCLUDE_DEPENDS}) -target_compile_definitions(mdog_simple_controller PRIVATE "MDOG_SIMPLE_CONTROLLER_BUILDING_DLL") - -pluginlib_export_plugin_description_file( - controller_interface mdog_simple_controller.xml) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) install( - TARGETS - mdog_simple_controller - RUNTIME DESTINATION bin - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib + DIRECTORY launch + DESTINATION share/${PROJECT_NAME}/ ) install( DIRECTORY include/ - DESTINATION include/${PROJECT_NAME} + DESTINATION include ) if(BUILD_TESTING) @@ -69,43 +30,7 @@ if(BUILD_TESTING) # 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_add_gmock(test_load_mdog_simple_controller test/test_load_mdog_simple_controller.cpp) - target_include_directories(test_load_mdog_simple_controller PRIVATE include) - ament_target_dependencies( - test_load_mdog_simple_controller - controller_manager - hardware_interface - ros2_control_test_assets - ) - - add_rostest_with_parameters_gmock(test_mdog_simple_controller test/test_mdog_simple_controller.cpp ${CMAKE_CURRENT_SOURCE_DIR}/test/mdog_simple_controller_params.yaml) - target_include_directories(test_mdog_simple_controller PRIVATE include) - target_link_libraries(test_mdog_simple_controller mdog_simple_controller) - ament_target_dependencies( - test_mdog_simple_controller - controller_interface - hardware_interface - ) - - add_rostest_with_parameters_gmock(test_mdog_simple_controller_preceeding test/test_mdog_simple_controller_preceeding.cpp ${CMAKE_CURRENT_SOURCE_DIR}/test/mdog_simple_controller_preceeding_params.yaml) - target_include_directories(test_mdog_simple_controller_preceeding PRIVATE include) - target_link_libraries(test_mdog_simple_controller_preceeding mdog_simple_controller) - ament_target_dependencies( - test_mdog_simple_controller_preceeding - controller_interface - hardware_interface - ) + ament_lint_auto_find_test_dependencies() endif() -ament_export_include_directories( - include -) -ament_export_dependencies( - ${THIS_PACKAGE_INCLUDE_DEPENDS} -) -ament_export_libraries( - mdog_simple_controller -) - ament_package() diff --git a/src/controllers/mdog_simple_controller/include/mdog_simple_controller/mdog_simple_controller.hpp b/src/controllers/mdog_simple_controller/include/mdog_simple_controller/mdog_simple_controller.hpp deleted file mode 100644 index 0cb7113..0000000 --- a/src/controllers/mdog_simple_controller/include/mdog_simple_controller/mdog_simple_controller.hpp +++ /dev/null @@ -1,117 +0,0 @@ -// Copyright (c) 2024, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) -// -// 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. -// - -#ifndef MDOG_SIMPLE_CONTROLLER__MDOG_SIMPLE_CONTROLLER_HPP_ -#define MDOG_SIMPLE_CONTROLLER__MDOG_SIMPLE_CONTROLLER_HPP_ - -#include -#include -#include - -#include "controller_interface/controller_interface.hpp" -#include "mdog_simple_controller_parameters.hpp" -#include "mdog_simple_controller/visibility_control.h" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" -#include "rclcpp_lifecycle/state.hpp" -#include "realtime_tools/realtime_buffer.h" -#include "realtime_tools/realtime_publisher.h" -#include "std_srvs/srv/set_bool.hpp" - -// TODO(anyone): Replace with controller specific messages -#include "control_msgs/msg/joint_controller_state.hpp" -#include "control_msgs/msg/joint_jog.hpp" - -namespace mdog_simple_controller -{ -// name constants for state interfaces -static constexpr size_t STATE_MY_ITFS = 0; - -// name constants for command interfaces -static constexpr size_t CMD_MY_ITFS = 0; - -// TODO(anyone: example setup for control mode (usually you will use some enums defined in messages) -enum class control_mode_type : std::uint8_t -{ - FAST = 0, - SLOW = 1, -}; - -class MdogSimpleController : public controller_interface::ControllerInterface -{ -public: - MDOG_SIMPLE_CONTROLLER__VISIBILITY_PUBLIC - MdogSimpleController(); - - MDOG_SIMPLE_CONTROLLER__VISIBILITY_PUBLIC - controller_interface::CallbackReturn on_init() override; - - MDOG_SIMPLE_CONTROLLER__VISIBILITY_PUBLIC - controller_interface::InterfaceConfiguration command_interface_configuration() const override; - - MDOG_SIMPLE_CONTROLLER__VISIBILITY_PUBLIC - controller_interface::InterfaceConfiguration state_interface_configuration() const override; - - MDOG_SIMPLE_CONTROLLER__VISIBILITY_PUBLIC - controller_interface::CallbackReturn on_configure( - const rclcpp_lifecycle::State & previous_state) override; - - MDOG_SIMPLE_CONTROLLER__VISIBILITY_PUBLIC - controller_interface::CallbackReturn on_activate( - const rclcpp_lifecycle::State & previous_state) override; - - MDOG_SIMPLE_CONTROLLER__VISIBILITY_PUBLIC - controller_interface::CallbackReturn on_deactivate( - const rclcpp_lifecycle::State & previous_state) override; - - MDOG_SIMPLE_CONTROLLER__VISIBILITY_PUBLIC - controller_interface::return_type update( - const rclcpp::Time & time, const rclcpp::Duration & period) override; - - // TODO(anyone): replace the state and command message types - using ControllerReferenceMsg = control_msgs::msg::JointJog; - using ControllerModeSrvType = std_srvs::srv::SetBool; - using ControllerStateMsg = control_msgs::msg::JointControllerState; - -protected: - std::shared_ptr param_listener_; - mdog_simple_controller::Params params_; - - std::vector state_joints_; - - // Command subscribers and Controller State publisher - rclcpp::Subscription::SharedPtr ref_subscriber_ = nullptr; - realtime_tools::RealtimeBuffer> input_ref_; - - rclcpp::Service::SharedPtr set_slow_control_mode_service_; - realtime_tools::RealtimeBuffer control_mode_; - - using ControllerStatePublisher = realtime_tools::RealtimePublisher; - - rclcpp::Publisher::SharedPtr s_publisher_; - std::unique_ptr state_publisher_; - -private: - // callback for topic interface - MDOG_SIMPLE_CONTROLLER__VISIBILITY_LOCAL - void reference_callback(const std::shared_ptr msg); -}; - -} // namespace mdog_simple_controller - -#endif // MDOG_SIMPLE_CONTROLLER__MDOG_SIMPLE_CONTROLLER_HPP_ diff --git a/src/controllers/mdog_simple_controller/include/mdog_simple_controller/validate_mdog_simple_controller_parameters.hpp b/src/controllers/mdog_simple_controller/include/mdog_simple_controller/validate_mdog_simple_controller_parameters.hpp deleted file mode 100644 index 1e09499..0000000 --- a/src/controllers/mdog_simple_controller/include/mdog_simple_controller/validate_mdog_simple_controller_parameters.hpp +++ /dev/null @@ -1,43 +0,0 @@ -// Copyright (c) 2024, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) -// -// 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. -// - -#ifndef MDOG_SIMPLE_CONTROLLER__VALIDATE_MDOG_SIMPLE_CONTROLLER_PARAMETERS_HPP_ -#define MDOG_SIMPLE_CONTROLLER__VALIDATE_MDOG_SIMPLE_CONTROLLER_PARAMETERS_HPP_ - -#include - -#include "parameter_traits/parameter_traits.hpp" - -namespace parameter_traits -{ -Result forbidden_interface_name_prefix(rclcpp::Parameter const & parameter) -{ - auto const & interface_name = parameter.as_string(); - - if (interface_name.rfind("blup_", 0) == 0) - { - return ERROR("'interface_name' parameter can not start with 'blup_'"); - } - - return OK; -} - -} // namespace parameter_traits - -#endif // MDOG_SIMPLE_CONTROLLER__VALIDATE_MDOG_SIMPLE_CONTROLLER_PARAMETERS_HPP_ diff --git a/src/controllers/mdog_simple_controller/include/mdog_simple_controller/visibility_control.h b/src/controllers/mdog_simple_controller/include/mdog_simple_controller/visibility_control.h deleted file mode 100644 index eb6a7a1..0000000 --- a/src/controllers/mdog_simple_controller/include/mdog_simple_controller/visibility_control.h +++ /dev/null @@ -1,54 +0,0 @@ -// Copyright (c) 2024, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) -// -// 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. -// - -#ifndef MDOG_SIMPLE_CONTROLLER__VISIBILITY_CONTROL_H_ -#define MDOG_SIMPLE_CONTROLLER__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 MDOG_SIMPLE_CONTROLLER__VISIBILITY_EXPORT __attribute__((dllexport)) -#define MDOG_SIMPLE_CONTROLLER__VISIBILITY_IMPORT __attribute__((dllimport)) -#else -#define MDOG_SIMPLE_CONTROLLER__VISIBILITY_EXPORT __declspec(dllexport) -#define MDOG_SIMPLE_CONTROLLER__VISIBILITY_IMPORT __declspec(dllimport) -#endif -#ifdef MDOG_SIMPLE_CONTROLLER__VISIBILITY_BUILDING_DLL -#define MDOG_SIMPLE_CONTROLLER__VISIBILITY_PUBLIC MDOG_SIMPLE_CONTROLLER__VISIBILITY_EXPORT -#else -#define MDOG_SIMPLE_CONTROLLER__VISIBILITY_PUBLIC MDOG_SIMPLE_CONTROLLER__VISIBILITY_IMPORT -#endif -#define MDOG_SIMPLE_CONTROLLER__VISIBILITY_PUBLIC_TYPE MDOG_SIMPLE_CONTROLLER__VISIBILITY_PUBLIC -#define MDOG_SIMPLE_CONTROLLER__VISIBILITY_LOCAL -#else -#define MDOG_SIMPLE_CONTROLLER__VISIBILITY_EXPORT __attribute__((visibility("default"))) -#define MDOG_SIMPLE_CONTROLLER__VISIBILITY_IMPORT -#if __GNUC__ >= 4 -#define MDOG_SIMPLE_CONTROLLER__VISIBILITY_PUBLIC __attribute__((visibility("default"))) -#define MDOG_SIMPLE_CONTROLLER__VISIBILITY_LOCAL __attribute__((visibility("hidden"))) -#else -#define MDOG_SIMPLE_CONTROLLER__VISIBILITY_PUBLIC -#define MDOG_SIMPLE_CONTROLLER__VISIBILITY_LOCAL -#endif -#define MDOG_SIMPLE_CONTROLLER__VISIBILITY_PUBLIC_TYPE -#endif - -#endif // MDOG_SIMPLE_CONTROLLER__VISIBILITY_CONTROL_H_ diff --git a/src/controllers/mdog_simple_controller/mdog_simple_controller.xml b/src/controllers/mdog_simple_controller/mdog_simple_controller.xml deleted file mode 100644 index 654bf58..0000000 --- a/src/controllers/mdog_simple_controller/mdog_simple_controller.xml +++ /dev/null @@ -1,28 +0,0 @@ - - - - - - MdogSimpleController ros2_control controller. - - - diff --git a/src/controllers/mdog_simple_controller/package.xml b/src/controllers/mdog_simple_controller/package.xml index 787dbc0..2440eb3 100644 --- a/src/controllers/mdog_simple_controller/package.xml +++ b/src/controllers/mdog_simple_controller/package.xml @@ -9,29 +9,8 @@ ament_cmake - generate_parameter_library - - control_msgs - - controller_interface - - hardware_interface - - pluginlib - - rclcpp - - rclcpp_lifecycle - - realtime_tools - - std_srvs - ament_lint_auto - ament_cmake_gmock - controller_manager - hardware_interface - ros2_control_test_assets + ament_lint_common ament_cmake diff --git a/src/controllers/mdog_simple_controller/src/mdog_simple_controller.cpp b/src/controllers/mdog_simple_controller/src/mdog_simple_controller.cpp deleted file mode 100644 index 4cd34b9..0000000 --- a/src/controllers/mdog_simple_controller/src/mdog_simple_controller.cpp +++ /dev/null @@ -1,265 +0,0 @@ -// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) -// -// 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. -// - -#include "mdog_simple_controller/mdog_simple_controller.hpp" - -#include -#include -#include -#include - -#include "controller_interface/helpers.hpp" - -namespace -{ // utility - -// TODO(destogl): remove this when merged upstream -// Changed services history QoS to keep all so we don't lose any client service calls -static constexpr rmw_qos_profile_t rmw_qos_profile_services_hist_keep_all = { - RMW_QOS_POLICY_HISTORY_KEEP_ALL, - 1, // message queue depth - RMW_QOS_POLICY_RELIABILITY_RELIABLE, - RMW_QOS_POLICY_DURABILITY_VOLATILE, - RMW_QOS_DEADLINE_DEFAULT, - RMW_QOS_LIFESPAN_DEFAULT, - RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT, - RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT, - false}; - -using ControllerReferenceMsg = mdog_simple_controller::MdogSimpleController::ControllerReferenceMsg; - -// called from RT control loop -void reset_controller_reference_msg( - std::shared_ptr & msg, const std::vector & joint_names) -{ - msg->joint_names = joint_names; - msg->displacements.resize(joint_names.size(), std::numeric_limits::quiet_NaN()); - msg->velocities.resize(joint_names.size(), std::numeric_limits::quiet_NaN()); - msg->duration = std::numeric_limits::quiet_NaN(); -} - -} // namespace - -namespace mdog_simple_controller -{ -MdogSimpleController::MdogSimpleController() : controller_interface::ControllerInterface() {} - -controller_interface::CallbackReturn MdogSimpleController::on_init() -{ - control_mode_.initRT(control_mode_type::FAST); - - try - { - param_listener_ = std::make_shared(get_node()); - } - catch (const std::exception & e) - { - fprintf(stderr, "Exception thrown during controller's init with message: %s \n", e.what()); - return controller_interface::CallbackReturn::ERROR; - } - - return controller_interface::CallbackReturn::SUCCESS; -} - -controller_interface::CallbackReturn MdogSimpleController::on_configure( - const rclcpp_lifecycle::State & /*previous_state*/) -{ - params_ = param_listener_->get_params(); - - if (!params_.state_joints.empty()) - { - state_joints_ = params_.state_joints; - } - else - { - state_joints_ = params_.joints; - } - - if (params_.joints.size() != state_joints_.size()) - { - RCLCPP_FATAL( - get_node()->get_logger(), - "Size of 'joints' (%zu) and 'state_joints' (%zu) parameters has to be the same!", - params_.joints.size(), state_joints_.size()); - return CallbackReturn::FAILURE; - } - - // topics QoS - auto subscribers_qos = rclcpp::SystemDefaultsQoS(); - subscribers_qos.keep_last(1); - subscribers_qos.best_effort(); - - // Reference Subscriber - ref_subscriber_ = get_node()->create_subscription( - "~/reference", subscribers_qos, - std::bind(&MdogSimpleController::reference_callback, this, std::placeholders::_1)); - - std::shared_ptr msg = std::make_shared(); - reset_controller_reference_msg(msg, params_.joints); - input_ref_.writeFromNonRT(msg); - - auto set_slow_mode_service_callback = - [&]( - const std::shared_ptr request, - std::shared_ptr response) - { - if (request->data) - { - control_mode_.writeFromNonRT(control_mode_type::SLOW); - } - else - { - control_mode_.writeFromNonRT(control_mode_type::FAST); - } - response->success = true; - }; - - set_slow_control_mode_service_ = get_node()->create_service( - "~/set_slow_control_mode", set_slow_mode_service_callback, - rmw_qos_profile_services_hist_keep_all); - - try - { - // State publisher - s_publisher_ = - get_node()->create_publisher("~/state", rclcpp::SystemDefaultsQoS()); - state_publisher_ = std::make_unique(s_publisher_); - } - catch (const std::exception & e) - { - fprintf( - stderr, "Exception thrown during publisher creation at configure stage with message : %s \n", - e.what()); - return controller_interface::CallbackReturn::ERROR; - } - - // TODO(anyone): Reserve memory in state publisher depending on the message type - state_publisher_->lock(); - state_publisher_->msg_.header.frame_id = params_.joints[0]; - state_publisher_->unlock(); - - RCLCPP_INFO(get_node()->get_logger(), "configure successful"); - return controller_interface::CallbackReturn::SUCCESS; -} - -void MdogSimpleController::reference_callback(const std::shared_ptr msg) -{ - if (msg->joint_names.size() == params_.joints.size()) - { - input_ref_.writeFromNonRT(msg); - } - else - { - RCLCPP_ERROR( - get_node()->get_logger(), - "Received %zu , but expected %zu joints in command. Ignoring message.", - msg->joint_names.size(), params_.joints.size()); - } -} - -controller_interface::InterfaceConfiguration MdogSimpleController::command_interface_configuration() const -{ - controller_interface::InterfaceConfiguration command_interfaces_config; - command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; - - command_interfaces_config.names.reserve(params_.joints.size()); - for (const auto & joint : params_.joints) - { - command_interfaces_config.names.push_back(joint + "/" + params_.interface_name); - } - - return command_interfaces_config; -} - -controller_interface::InterfaceConfiguration MdogSimpleController::state_interface_configuration() const -{ - controller_interface::InterfaceConfiguration state_interfaces_config; - state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; - - state_interfaces_config.names.reserve(state_joints_.size()); - for (const auto & joint : state_joints_) - { - state_interfaces_config.names.push_back(joint + "/" + params_.interface_name); - } - - return state_interfaces_config; -} - -controller_interface::CallbackReturn MdogSimpleController::on_activate( - const rclcpp_lifecycle::State & /*previous_state*/) -{ - // TODO(anyone): if you have to manage multiple interfaces that need to be sorted check - // `on_activate` method in `JointTrajectoryController` for exemplary use of - // `controller_interface::get_ordered_interfaces` helper function - - // Set default value in command - reset_controller_reference_msg(*(input_ref_.readFromRT)(), params_.joints); - - return controller_interface::CallbackReturn::SUCCESS; -} - -controller_interface::CallbackReturn MdogSimpleController::on_deactivate( - const rclcpp_lifecycle::State & /*previous_state*/) -{ - // TODO(anyone): depending on number of interfaces, use definitions, e.g., `CMD_MY_ITFS`, - // instead of a loop - for (size_t i = 0; i < command_interfaces_.size(); ++i) - { - command_interfaces_[i].set_value(std::numeric_limits::quiet_NaN()); - } - return controller_interface::CallbackReturn::SUCCESS; -} - -controller_interface::return_type MdogSimpleController::update( - const rclcpp::Time & time, const rclcpp::Duration & /*period*/) -{ - auto current_ref = input_ref_.readFromRT(); - - // TODO(anyone): depending on number of interfaces, use definitions, e.g., `CMD_MY_ITFS`, - // instead of a loop - for (size_t i = 0; i < command_interfaces_.size(); ++i) - { - if (!std::isnan((*current_ref)->displacements[i])) - { - if (*(control_mode_.readFromRT()) == control_mode_type::SLOW) - { - (*current_ref)->displacements[i] /= 2; - } - command_interfaces_[i].set_value((*current_ref)->displacements[i]); - - (*current_ref)->displacements[i] = std::numeric_limits::quiet_NaN(); - } - } - - if (state_publisher_ && state_publisher_->trylock()) - { - state_publisher_->msg_.header.stamp = time; - state_publisher_->msg_.set_point = command_interfaces_[CMD_MY_ITFS].get_value(); - state_publisher_->unlockAndPublish(); - } - - return controller_interface::return_type::OK; -} - -} // namespace mdog_simple_controller - -#include "pluginlib/class_list_macros.hpp" - -PLUGINLIB_EXPORT_CLASS( - mdog_simple_controller::MdogSimpleController, controller_interface::ControllerInterface) diff --git a/src/controllers/mdog_simple_controller/src/mdog_simple_controller.yaml b/src/controllers/mdog_simple_controller/src/mdog_simple_controller.yaml deleted file mode 100644 index 75843c1..0000000 --- a/src/controllers/mdog_simple_controller/src/mdog_simple_controller.yaml +++ /dev/null @@ -1,50 +0,0 @@ -# Copyright (c) 20224 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. -# - -mdog_simple_controller: - joints: { - type: string_array, - default_value: [], - description: "Specifies joints used by the controller. If state joints parameter is defined, then only command joints are defined with this parameter.", - read_only: true, - validation: { - unique<>: null, - not_empty<>: null, - } - } - state_joints: { - type: string_array, - default_value: [], - description: "(optional) Specifies joints for reading states. This parameter is only relevant when state joints are different then command joint, i.e., when a following controller is used.", - read_only: true, - validation: { - unique<>: null, - } - } - interface_name: { - type: string, - default_value: "", - description: "Name of the interface used by the controller on joints and command_joints.", - read_only: true, - validation: { - not_empty<>: null, - one_of<>: [["position", "velocity", "acceleration", "effort",]], - forbidden_interface_name_prefix: null - } - } diff --git a/src/controllers/mdog_simple_controller/test/mdog_simple_controller_params.yaml b/src/controllers/mdog_simple_controller/test/mdog_simple_controller_params.yaml deleted file mode 100644 index a0d1615..0000000 --- a/src/controllers/mdog_simple_controller/test/mdog_simple_controller_params.yaml +++ /dev/null @@ -1,26 +0,0 @@ -# Copyright (c) 20224 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. -# - -test_mdog_simple_controller: - ros__parameters: - - joints: - - joint1 - - interface_name: acceleration diff --git a/src/controllers/mdog_simple_controller/test/mdog_simple_controller_preceeding_params.yaml b/src/controllers/mdog_simple_controller/test/mdog_simple_controller_preceeding_params.yaml deleted file mode 100644 index 06e227c..0000000 --- a/src/controllers/mdog_simple_controller/test/mdog_simple_controller_preceeding_params.yaml +++ /dev/null @@ -1,28 +0,0 @@ -# Copyright (c) 20224 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. -# - -test_mdog_simple_controller: - ros__parameters: - joints: - - joint1 - - interface_name: acceleration - - state_joints: - - joint1state diff --git a/src/controllers/mdog_simple_controller/test/test_load_mdog_simple_controller.cpp b/src/controllers/mdog_simple_controller/test/test_load_mdog_simple_controller.cpp deleted file mode 100644 index 2b61393..0000000 --- a/src/controllers/mdog_simple_controller/test/test_load_mdog_simple_controller.cpp +++ /dev/null @@ -1,44 +0,0 @@ -// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) -// -// 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. -// - -#include -#include - -#include "controller_manager/controller_manager.hpp" -#include "hardware_interface/resource_manager.hpp" -#include "rclcpp/executor.hpp" -#include "rclcpp/executors/single_threaded_executor.hpp" -#include "rclcpp/utilities.hpp" -#include "ros2_control_test_assets/descriptions.hpp" - -TEST(TestLoadMdogSimpleController, load_controller) -{ - rclcpp::init(0, nullptr); - - std::shared_ptr executor = - std::make_shared(); - - controller_manager::ControllerManager cm( - executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); - - ASSERT_NO_THROW( - cm.load_controller("test_mdog_simple_controller", "mdog_simple_controller/MdogSimpleController")); - - rclcpp::shutdown(); -} diff --git a/src/controllers/mdog_simple_controller/test/test_mdog_simple_controller.cpp b/src/controllers/mdog_simple_controller/test/test_mdog_simple_controller.cpp deleted file mode 100644 index 73d4c7d..0000000 --- a/src/controllers/mdog_simple_controller/test/test_mdog_simple_controller.cpp +++ /dev/null @@ -1,277 +0,0 @@ -// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) -// -// 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. -// - -#include -#include -#include -#include -#include - -#include "rclcpp/rclcpp.hpp" -#include "test_mdog_simple_controller.hpp" - -using mdog_simple_controller::CMD_MY_ITFS; -using mdog_simple_controller::control_mode_type; -using mdog_simple_controller::STATE_MY_ITFS; - -class MdogSimpleControllerTest : public MdogSimpleControllerFixture -{ -}; - -TEST_F(MdogSimpleControllerTest, all_parameters_set_configure_success) -{ - SetUpController(); - - ASSERT_TRUE(controller_->params_.joints.empty()); - ASSERT_TRUE(controller_->params_.state_joints.empty()); - ASSERT_TRUE(controller_->params_.interface_name.empty()); - - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - - ASSERT_THAT(controller_->params_.joints, testing::ElementsAreArray(joint_names_)); - ASSERT_TRUE(controller_->params_.state_joints.empty()); - ASSERT_THAT(controller_->state_joints_, testing::ElementsAreArray(joint_names_)); - ASSERT_EQ(controller_->params_.interface_name, interface_name_); -} - -TEST_F(MdogSimpleControllerTest, check_exported_intefaces) -{ - SetUpController(); - - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - - auto command_intefaces = controller_->command_interface_configuration(); - ASSERT_EQ(command_intefaces.names.size(), joint_command_values_.size()); - for (size_t i = 0; i < command_intefaces.names.size(); ++i) - { - EXPECT_EQ(command_intefaces.names[i], joint_names_[i] + "/" + interface_name_); - } - - auto state_intefaces = controller_->state_interface_configuration(); - ASSERT_EQ(state_intefaces.names.size(), joint_state_values_.size()); - for (size_t i = 0; i < state_intefaces.names.size(); ++i) - { - EXPECT_EQ(state_intefaces.names[i], joint_names_[i] + "/" + interface_name_); - } -} - -TEST_F(MdogSimpleControllerTest, activate_success) -{ - SetUpController(); - - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - - // check that the message is reset - auto msg = controller_->input_ref_.readFromNonRT(); - EXPECT_EQ((*msg)->displacements.size(), joint_names_.size()); - for (const auto & cmd : (*msg)->displacements) - { - EXPECT_TRUE(std::isnan(cmd)); - } - EXPECT_EQ((*msg)->velocities.size(), joint_names_.size()); - for (const auto & cmd : (*msg)->velocities) - { - EXPECT_TRUE(std::isnan(cmd)); - } - - ASSERT_TRUE(std::isnan((*msg)->duration)); -} - -TEST_F(MdogSimpleControllerTest, update_success) -{ - SetUpController(); - - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - - ASSERT_EQ( - controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); -} - -TEST_F(MdogSimpleControllerTest, deactivate_success) -{ - SetUpController(); - - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); -} - -TEST_F(MdogSimpleControllerTest, reactivate_success) -{ - SetUpController(); - - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_EQ(controller_->command_interfaces_[CMD_MY_ITFS].get_value(), 101.101); - ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_TRUE(std::isnan(controller_->command_interfaces_[CMD_MY_ITFS].get_value())); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_TRUE(std::isnan(controller_->command_interfaces_[CMD_MY_ITFS].get_value())); - - ASSERT_EQ( - controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); -} - -TEST_F(MdogSimpleControllerTest, test_setting_slow_mode_service) -{ - SetUpController(); - - rclcpp::executors::MultiThreadedExecutor executor; - executor.add_node(controller_->get_node()->get_node_base_interface()); - executor.add_node(service_caller_node_->get_node_base_interface()); - - // initially set to false - ASSERT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::FAST); - - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - - // should stay false - ASSERT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::FAST); - - // set to true - ASSERT_NO_THROW(call_service(true, executor)); - ASSERT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::SLOW); - - // set back to false - ASSERT_NO_THROW(call_service(false, executor)); - ASSERT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::FAST); -} - -TEST_F(MdogSimpleControllerTest, test_update_logic_fast) -{ - SetUpController(); - rclcpp::executors::MultiThreadedExecutor executor; - executor.add_node(controller_->get_node()->get_node_base_interface()); - executor.add_node(service_caller_node_->get_node_base_interface()); - - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - - // set command statically - static constexpr double TEST_DISPLACEMENT = 23.24; - std::shared_ptr msg = std::make_shared(); - msg->joint_names = joint_names_; - msg->displacements.resize(joint_names_.size(), TEST_DISPLACEMENT); - msg->velocities.resize(joint_names_.size(), std::numeric_limits::quiet_NaN()); - msg->duration = std::numeric_limits::quiet_NaN(); - controller_->input_ref_.writeFromNonRT(msg); - - ASSERT_EQ( - controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - - EXPECT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::FAST); - EXPECT_EQ(joint_command_values_[STATE_MY_ITFS], TEST_DISPLACEMENT); - EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->displacements[0])); - EXPECT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::FAST); -} - -TEST_F(MdogSimpleControllerTest, test_update_logic_slow) -{ - SetUpController(); - rclcpp::executors::MultiThreadedExecutor executor; - executor.add_node(controller_->get_node()->get_node_base_interface()); - executor.add_node(service_caller_node_->get_node_base_interface()); - - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - - // set command statically - static constexpr double TEST_DISPLACEMENT = 23.24; - std::shared_ptr msg = std::make_shared(); - // When slow mode is enabled command ends up being half of the value - msg->joint_names = joint_names_; - msg->displacements.resize(joint_names_.size(), TEST_DISPLACEMENT); - msg->velocities.resize(joint_names_.size(), std::numeric_limits::quiet_NaN()); - msg->duration = std::numeric_limits::quiet_NaN(); - controller_->input_ref_.writeFromNonRT(msg); - controller_->control_mode_.writeFromNonRT(control_mode_type::SLOW); - - EXPECT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::SLOW); - ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT); - ASSERT_EQ( - controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - - EXPECT_EQ(joint_command_values_[STATE_MY_ITFS], TEST_DISPLACEMENT / 2); - EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->displacements[0])); -} - -TEST_F(MdogSimpleControllerTest, publish_status_success) -{ - SetUpController(); - - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - - ASSERT_EQ( - controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - - ControllerStateMsg msg; - subscribe_and_get_messages(msg); - - ASSERT_EQ(msg.set_point, 101.101); -} - -TEST_F(MdogSimpleControllerTest, receive_message_and_publish_updated_status) -{ - SetUpController(); - rclcpp::executors::MultiThreadedExecutor executor; - executor.add_node(controller_->get_node()->get_node_base_interface()); - - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - - ASSERT_EQ( - controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - - ControllerStateMsg msg; - subscribe_and_get_messages(msg); - - ASSERT_EQ(msg.set_point, 101.101); - - publish_commands(); - ASSERT_TRUE(controller_->wait_for_commands(executor)); - - ASSERT_EQ( - controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - - EXPECT_EQ(joint_command_values_[CMD_MY_ITFS], 0.45); - - subscribe_and_get_messages(msg); - - ASSERT_EQ(msg.set_point, 0.45); -} - -int main(int argc, char ** argv) -{ - ::testing::InitGoogleTest(&argc, argv); - rclcpp::init(argc, argv); - int result = RUN_ALL_TESTS(); - rclcpp::shutdown(); - return result; -} diff --git a/src/controllers/mdog_simple_controller/test/test_mdog_simple_controller.hpp b/src/controllers/mdog_simple_controller/test/test_mdog_simple_controller.hpp deleted file mode 100644 index 274f3c2..0000000 --- a/src/controllers/mdog_simple_controller/test/test_mdog_simple_controller.hpp +++ /dev/null @@ -1,274 +0,0 @@ -// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) -// -// 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. -// - -#ifndef TEMPLATES__ROS2_CONTROL__CONTROLLER__TEST_MDOG_SIMPLE_CONTROLLER_HPP_ -#define TEMPLATES__ROS2_CONTROL__CONTROLLER__TEST_MDOG_SIMPLE_CONTROLLER_HPP_ - -#include -#include -#include -#include -#include -#include -#include - -#include "mdog_simple_controller/mdog_simple_controller.hpp" -#include "gmock/gmock.h" -#include "hardware_interface/loaned_command_interface.hpp" -#include "hardware_interface/loaned_state_interface.hpp" -#include "hardware_interface/types/hardware_interface_return_values.hpp" -#include "rclcpp/executor.hpp" -#include "rclcpp/parameter_value.hpp" -#include "rclcpp/time.hpp" -#include "rclcpp/utilities.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" - -// TODO(anyone): replace the state and command message types -using ControllerStateMsg = mdog_simple_controller::MdogSimpleController::ControllerStateMsg; -using ControllerReferenceMsg = mdog_simple_controller::MdogSimpleController::ControllerReferenceMsg; -using ControllerModeSrvType = mdog_simple_controller::MdogSimpleController::ControllerModeSrvType; - -namespace -{ -constexpr auto NODE_SUCCESS = controller_interface::CallbackReturn::SUCCESS; -constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR; -} // namespace - -// subclassing and friending so we can access member variables -class TestableMdogSimpleController : public mdog_simple_controller::MdogSimpleController -{ - FRIEND_TEST(MdogSimpleControllerTest, all_parameters_set_configure_success); - FRIEND_TEST(MdogSimpleControllerTest, activate_success); - FRIEND_TEST(MdogSimpleControllerTest, reactivate_success); - FRIEND_TEST(MdogSimpleControllerTest, test_setting_slow_mode_service); - FRIEND_TEST(MdogSimpleControllerTest, test_update_logic_fast); - FRIEND_TEST(MdogSimpleControllerTest, test_update_logic_slow); - -public: - controller_interface::CallbackReturn on_configure( - const rclcpp_lifecycle::State & previous_state) override - { - auto ret = mdog_simple_controller::MdogSimpleController::on_configure(previous_state); - // Only if on_configure is successful create subscription - if (ret == CallbackReturn::SUCCESS) - { - ref_subscriber_wait_set_.add_subscription(ref_subscriber_); - } - return ret; - } - - /** - * @brief wait_for_command blocks until a new ControllerReferenceMsg is received. - * Requires that the executor is not spinned elsewhere between the - * message publication and the call to this function. - * - * @return true if new ControllerReferenceMsg msg was received, false if timeout. - */ - bool wait_for_command( - rclcpp::Executor & executor, rclcpp::WaitSet & subscriber_wait_set, - const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) - { - bool success = subscriber_wait_set.wait(timeout).kind() == rclcpp::WaitResultKind::Ready; - if (success) - { - executor.spin_some(); - } - return success; - } - - bool wait_for_commands( - rclcpp::Executor & executor, - const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) - { - return wait_for_command(executor, ref_subscriber_wait_set_, timeout); - } - - // TODO(anyone): add implementation of any methods of your controller is needed - -private: - rclcpp::WaitSet ref_subscriber_wait_set_; -}; - -// We are using template class here for easier reuse of Fixture in specializations of controllers -template -class MdogSimpleControllerFixture : public ::testing::Test -{ -public: - static void SetUpTestCase() {} - - void SetUp() - { - // initialize controller - controller_ = std::make_unique(); - - command_publisher_node_ = std::make_shared("command_publisher"); - command_publisher_ = command_publisher_node_->create_publisher( - "/test_mdog_simple_controller/commands", rclcpp::SystemDefaultsQoS()); - - service_caller_node_ = std::make_shared("service_caller"); - slow_control_service_client_ = service_caller_node_->create_client( - "/test_mdog_simple_controller/set_slow_control_mode"); - } - - static void TearDownTestCase() {} - - void TearDown() { controller_.reset(nullptr); } - -protected: - void SetUpController(const std::string controller_name = "test_mdog_simple_controller") - { - ASSERT_EQ( - controller_->init(controller_name, "", 0, "", controller_->define_custom_node_options()), - controller_interface::return_type::OK); - - std::vector command_ifs; - command_itfs_.reserve(joint_command_values_.size()); - command_ifs.reserve(joint_command_values_.size()); - - for (size_t i = 0; i < joint_command_values_.size(); ++i) - { - command_itfs_.emplace_back(hardware_interface::CommandInterface( - joint_names_[i], interface_name_, &joint_command_values_[i])); - command_ifs.emplace_back(command_itfs_.back()); - } - // TODO(anyone): Add other command interfaces, if any - - std::vector state_ifs; - state_itfs_.reserve(joint_state_values_.size()); - state_ifs.reserve(joint_state_values_.size()); - - for (size_t i = 0; i < joint_state_values_.size(); ++i) - { - state_itfs_.emplace_back(hardware_interface::StateInterface( - joint_names_[i], interface_name_, &joint_state_values_[i])); - state_ifs.emplace_back(state_itfs_.back()); - } - // TODO(anyone): Add other state interfaces, if any - - controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); - } - - void subscribe_and_get_messages(ControllerStateMsg & msg) - { - // create a new subscriber - rclcpp::Node test_subscription_node("test_subscription_node"); - auto subs_callback = [&](const ControllerStateMsg::SharedPtr) {}; - auto subscription = test_subscription_node.create_subscription( - "/test_mdog_simple_controller/state", 10, subs_callback); - - // call update to publish the test value - ASSERT_EQ( - controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - - // call update to publish the test value - // since update doesn't guarantee a published message, republish until received - int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop - rclcpp::WaitSet wait_set; // block used to wait on message - wait_set.add_subscription(subscription); - while (max_sub_check_loop_count--) - { - controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); - // check if message has been received - if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) - { - break; - } - } - ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " - "controller/broadcaster update loop"; - - // take message from subscription - rclcpp::MessageInfo msg_info; - ASSERT_TRUE(subscription->take(msg, msg_info)); - } - - // TODO(anyone): add/remove arguments as it suites your command message type - void publish_commands( - const std::vector & displacements = {0.45}, - const std::vector & velocities = {0.0}, const double duration = 1.25) - { - auto wait_for_topic = [&](const auto topic_name) - { - size_t wait_count = 0; - while (command_publisher_node_->count_subscribers(topic_name) == 0) - { - if (wait_count >= 5) - { - auto error_msg = - std::string("publishing to ") + topic_name + " but no node subscribes to it"; - throw std::runtime_error(error_msg); - } - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - ++wait_count; - } - }; - - wait_for_topic(command_publisher_->get_topic_name()); - - ControllerReferenceMsg msg; - msg.joint_names = joint_names_; - msg.displacements = displacements; - msg.velocities = velocities; - msg.duration = duration; - - command_publisher_->publish(msg); - } - - std::shared_ptr call_service( - const bool slow_control, rclcpp::Executor & executor) - { - auto request = std::make_shared(); - request->data = slow_control; - - bool wait_for_service_ret = - slow_control_service_client_->wait_for_service(std::chrono::milliseconds(500)); - EXPECT_TRUE(wait_for_service_ret); - if (!wait_for_service_ret) - { - throw std::runtime_error("Services is not available!"); - } - auto result = slow_control_service_client_->async_send_request(request); - EXPECT_EQ(executor.spin_until_future_complete(result), rclcpp::FutureReturnCode::SUCCESS); - - return result.get(); - } - -protected: - // TODO(anyone): adjust the members as needed - - // Controller-related parameters - std::vector joint_names_ = {"joint1"}; - std::vector state_joint_names_ = {"joint1state"}; - std::string interface_name_ = "acceleration"; - std::array joint_state_values_ = {1.1}; - std::array joint_command_values_ = {101.101}; - - std::vector state_itfs_; - std::vector command_itfs_; - - // Test related parameters - std::unique_ptr controller_; - rclcpp::Node::SharedPtr command_publisher_node_; - rclcpp::Publisher::SharedPtr command_publisher_; - rclcpp::Node::SharedPtr service_caller_node_; - rclcpp::Client::SharedPtr slow_control_service_client_; -}; - -#endif // TEMPLATES__ROS2_CONTROL__CONTROLLER__TEST_MDOG_SIMPLE_CONTROLLER_HPP_ diff --git a/src/controllers/mdog_simple_controller/test/test_mdog_simple_controller_preceeding.cpp b/src/controllers/mdog_simple_controller/test/test_mdog_simple_controller_preceeding.cpp deleted file mode 100644 index 39117ee..0000000 --- a/src/controllers/mdog_simple_controller/test/test_mdog_simple_controller_preceeding.cpp +++ /dev/null @@ -1,80 +0,0 @@ -// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) -// -// 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. -// - -#include "test_mdog_simple_controller.hpp" - -#include -#include -#include -#include -#include - -using mdog_simple_controller::CMD_MY_ITFS; -using mdog_simple_controller::control_mode_type; -using mdog_simple_controller::STATE_MY_ITFS; - -class MdogSimpleControllerTest : public MdogSimpleControllerFixture -{ -}; - -TEST_F(MdogSimpleControllerTest, all_parameters_set_configure_success) -{ - SetUpController(); - - ASSERT_TRUE(controller_->params_.joints.empty()); - ASSERT_TRUE(controller_->params_.state_joints.empty()); - ASSERT_TRUE(controller_->params_.interface_name.empty()); - - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - - ASSERT_THAT(controller_->params_.joints, testing::ElementsAreArray(joint_names_)); - ASSERT_THAT(controller_->params_.state_joints, testing::ElementsAreArray(state_joint_names_)); - ASSERT_THAT(controller_->state_joints_, testing::ElementsAreArray(state_joint_names_)); - ASSERT_EQ(controller_->params_.interface_name, interface_name_); -} - -TEST_F(MdogSimpleControllerTest, check_exported_intefaces) -{ - SetUpController(); - - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - - auto command_intefaces = controller_->command_interface_configuration(); - ASSERT_EQ(command_intefaces.names.size(), joint_command_values_.size()); - for (size_t i = 0; i < command_intefaces.names.size(); ++i) - { - EXPECT_EQ(command_intefaces.names[i], joint_names_[i] + "/" + interface_name_); - } - - auto state_intefaces = controller_->state_interface_configuration(); - ASSERT_EQ(state_intefaces.names.size(), joint_state_values_.size()); - for (size_t i = 0; i < state_intefaces.names.size(); ++i) - { - EXPECT_EQ(state_intefaces.names[i], state_joint_names_[i] + "/" + interface_name_); - } -} - -int main(int argc, char ** argv) -{ - ::testing::InitGoogleTest(&argc, argv); - rclcpp::init(argc, argv); - int result = RUN_ALL_TESTS(); - rclcpp::shutdown(); - return result; -} diff --git a/src/controllers/trot.py b/src/controllers/trot.py deleted file mode 100644 index e69de29..0000000 diff --git a/src/hardware/mdog_hardware/CMakeLists.txt b/src/hardware/mdog_hardware/CMakeLists.txt deleted file mode 100644 index 19448ab..0000000 --- a/src/hardware/mdog_hardware/CMakeLists.txt +++ /dev/null @@ -1,93 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(mdog_hardware) - -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(hardware_interface REQUIRED) -find_package(pluginlib REQUIRED) -find_package(rclcpp_lifecycle REQUIRED) -find_package(rclcpp REQUIRED) -find_package(std_msgs REQUIRED) -find_package(sensor_msgs REQUIRED) - -add_library( - mdog_hardware - SHARED - src/mdog_hardware_interface.cpp -) -target_include_directories( - mdog_hardware - PUBLIC - include -) -ament_target_dependencies( - mdog_hardware - hardware_interface - rclcpp - rclcpp_lifecycle -) -# prevent pluginlib from using boost -target_compile_definitions(mdog_hardware PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") - -pluginlib_export_plugin_description_file( - hardware_interface mdog_hardware.xml) - -install( - TARGETS - mdog_hardware - RUNTIME DESTINATION bin - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib -) - -install( - DIRECTORY include/ - DESTINATION include -) - -install( - DIRECTORY launch - DESTINATION share/${PROJECT_NAME} -) - -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() - find_package(ament_cmake_gmock REQUIRED) - find_package(ros2_control_test_assets REQUIRED) - - ament_add_gmock(test_mdog_hardware_interface test/test_mdog_hardware_interface.cpp) - target_include_directories(test_mdog_hardware_interface PRIVATE include) - ament_target_dependencies( - test_mdog_hardware_interface - hardware_interface - pluginlib - ros2_control_test_assets - ) -endif() - -ament_export_include_directories( - include -) -ament_export_libraries( - mdog_hardware -) -ament_export_dependencies( - hardware_interface - pluginlib - rclcpp - rclcpp_lifecycle -) - -ament_package() diff --git a/src/hardware/mdog_hardware/include/mdog_hardware/mdog_hardware_interface.hpp b/src/hardware/mdog_hardware/include/mdog_hardware/mdog_hardware_interface.hpp deleted file mode 100644 index b66f801..0000000 --- a/src/hardware/mdog_hardware/include/mdog_hardware/mdog_hardware_interface.hpp +++ /dev/null @@ -1,79 +0,0 @@ -// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef MDOG_HARDWARE__MDOG_HARDWARE_INTERFACE_HPP_ -#define MDOG_HARDWARE__MDOG_HARDWARE_INTERFACE_HPP_ - -#include -#include - -#include "mdog_hardware/visibility_control.h" -#include "hardware_interface/system_interface.hpp" -#include "hardware_interface/handle.hpp" -#include "hardware_interface/hardware_info.hpp" -#include "hardware_interface/types/hardware_interface_return_values.hpp" -#include "rclcpp/macros.hpp" -#include "rclcpp_lifecycle/state.hpp" -#include "rclcpp/rclcpp.hpp" -#include "sensor_msgs/msg/imu.hpp" - -namespace mdog_hardware -{ -class MDogHardwareInterface : public hardware_interface::SystemInterface -{ -public: - TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC - hardware_interface::CallbackReturn on_init( - const hardware_interface::HardwareInfo & info) override; - - TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC - hardware_interface::CallbackReturn on_configure( - const rclcpp_lifecycle::State & previous_state) override; - - TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC - std::vector export_state_interfaces() override; - - TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC - std::vector export_command_interfaces() override; - - TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC - hardware_interface::CallbackReturn on_activate( - const rclcpp_lifecycle::State & previous_state) override; - - TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC - hardware_interface::CallbackReturn on_deactivate( - const rclcpp_lifecycle::State & previous_state) override; - - TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC - hardware_interface::return_type read( - const rclcpp::Time & time, const rclcpp::Duration & period) override; - - TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC - hardware_interface::return_type write( - const rclcpp::Time & time, const rclcpp::Duration & period) override; - -private: - std::vector hw_commands_; - std::vector hw_states_; - std::vector imu_states_; - - rclcpp::Node::SharedPtr node_; - rclcpp::Subscription::SharedPtr imu_subscription_; - - void imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg); -}; - -} // namespace mdog_hardware - -#endif // MDOG_HARDWARE__MDOG_HARDWARE_INTERFACE_HPP_ diff --git a/src/hardware/mdog_hardware/include/mdog_hardware/visibility_control.h b/src/hardware/mdog_hardware/include/mdog_hardware/visibility_control.h deleted file mode 100644 index 255b7fb..0000000 --- a/src/hardware/mdog_hardware/include/mdog_hardware/visibility_control.h +++ /dev/null @@ -1,49 +0,0 @@ -// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef MDOG_HARDWARE__VISIBILITY_CONTROL_H_ -#define MDOG_HARDWARE__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 TEMPLATES__ROS2_CONTROL__VISIBILITY_EXPORT __attribute__((dllexport)) -#define TEMPLATES__ROS2_CONTROL__VISIBILITY_IMPORT __attribute__((dllimport)) -#else -#define TEMPLATES__ROS2_CONTROL__VISIBILITY_EXPORT __declspec(dllexport) -#define TEMPLATES__ROS2_CONTROL__VISIBILITY_IMPORT __declspec(dllimport) -#endif -#ifdef TEMPLATES__ROS2_CONTROL__VISIBILITY_BUILDING_DLL -#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC TEMPLATES__ROS2_CONTROL__VISIBILITY_EXPORT -#else -#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC TEMPLATES__ROS2_CONTROL__VISIBILITY_IMPORT -#endif -#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC_TYPE TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC -#define TEMPLATES__ROS2_CONTROL__VISIBILITY_LOCAL -#else -#define TEMPLATES__ROS2_CONTROL__VISIBILITY_EXPORT __attribute__((visibility("default"))) -#define TEMPLATES__ROS2_CONTROL__VISIBILITY_IMPORT -#if __GNUC__ >= 4 -#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC __attribute__((visibility("default"))) -#define TEMPLATES__ROS2_CONTROL__VISIBILITY_LOCAL __attribute__((visibility("hidden"))) -#else -#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC -#define TEMPLATES__ROS2_CONTROL__VISIBILITY_LOCAL -#endif -#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC_TYPE -#endif - -#endif // MDOG_HARDWARE__VISIBILITY_CONTROL_H_ diff --git a/src/hardware/mdog_hardware/launch/mdog_hardware.launch.py b/src/hardware/mdog_hardware/launch/mdog_hardware.launch.py deleted file mode 100644 index f82d709..0000000 --- a/src/hardware/mdog_hardware/launch/mdog_hardware.launch.py +++ /dev/null @@ -1,43 +0,0 @@ -import os - -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch_ros.actions import Node - -import xacro # 添加 xacro 模块 - - -def process_xacro(): - # 获取 xacro 文件路径 - pkg_path = os.path.join(get_package_share_directory('mdog_description')) - xacro_file = os.path.join(pkg_path, 'xacro', 'robot.xacro') - # 处理 xacro 文件 - robot_description_config = xacro.process_file(xacro_file) - return robot_description_config.toxml() - - -def generate_launch_description(): - # 生成机器人描述 - robot_description = {'robot_description': process_xacro()} - - # 启动机器人状态发布器 - robot_state_publisher_node = Node( - package='robot_state_publisher', - executable='robot_state_publisher', - output='screen', - parameters=[robot_description] - ) - - # 启动控制器管理器 - controller_manager_node = Node( - package='controller_manager', - executable='ros2_control_node', - output='screen', - parameters=[robot_description], - arguments=['--ros-args', '--log-level', 'info'] - ) - - return LaunchDescription([ - robot_state_publisher_node, - controller_manager_node - ]) \ No newline at end of file diff --git a/src/hardware/mdog_hardware/mdog_hardware.xml b/src/hardware/mdog_hardware/mdog_hardware.xml deleted file mode 100644 index 78fc04d..0000000 --- a/src/hardware/mdog_hardware/mdog_hardware.xml +++ /dev/null @@ -1,9 +0,0 @@ - - - - ros2_control hardware interface. - - - diff --git a/src/hardware/mdog_hardware/package.xml b/src/hardware/mdog_hardware/package.xml deleted file mode 100644 index 7e4d806..0000000 --- a/src/hardware/mdog_hardware/package.xml +++ /dev/null @@ -1,30 +0,0 @@ - - - - mdog_hardware - 0.0.0 - TODO: Package description - robofish - TODO: License declaration - - ament_cmake - - hardware_interface - - pluginlib - - rclcpp_lifecycle - - rclcpp - std_msgs - sensor_msgs - - ament_lint_auto - ament_lint_common - ament_cmake_gmock - ros2_control_test_assets - - - ament_cmake - - diff --git a/src/hardware/mdog_hardware/src/mdog_hardware_interface.cpp b/src/hardware/mdog_hardware/src/mdog_hardware_interface.cpp deleted file mode 100644 index d80ada8..0000000 --- a/src/hardware/mdog_hardware/src/mdog_hardware_interface.cpp +++ /dev/null @@ -1,165 +0,0 @@ -// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) -// -// 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. - -#include -#include - -#include "mdog_hardware/mdog_hardware_interface.hpp" -#include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "rclcpp/rclcpp.hpp" -#include "sensor_msgs/msg/imu.hpp" - -namespace mdog_hardware -{ - -void MDogHardwareInterface::imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg) -{ - imu_states_[0] = msg->orientation.w; - imu_states_[1] = msg->orientation.x; - imu_states_[2] = msg->orientation.y; - imu_states_[3] = msg->orientation.z; - - imu_states_[4] = msg->angular_velocity.x; - imu_states_[5] = msg->angular_velocity.y; - imu_states_[6] = msg->angular_velocity.z; - - imu_states_[7] = msg->linear_acceleration.x; - imu_states_[8] = msg->linear_acceleration.y; - imu_states_[9] = msg->linear_acceleration.z; -} - -hardware_interface::CallbackReturn MDogHardwareInterface::on_init( - const hardware_interface::HardwareInfo & info) -{ - if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) - { - return CallbackReturn::ERROR; - } - - // 初始化硬件状态和命令 - hw_states_.resize(info_.joints.size() * 3, std::numeric_limits::quiet_NaN()); - hw_commands_.resize(info_.joints.size() * 5, std::numeric_limits::quiet_NaN()); - imu_states_.resize(10, std::numeric_limits::quiet_NaN()); - - // 创建 ROS 2 节点和订阅器 - node_ = std::make_shared("mdog_hardware_interface"); - imu_subscription_ = node_->create_subscription( - "/imu", 10, std::bind(&MDogHardwareInterface::imu_callback, this, std::placeholders::_1)); - - return CallbackReturn::SUCCESS; -} - -hardware_interface::CallbackReturn MDogHardwareInterface::on_configure( - const rclcpp_lifecycle::State & /*previous_state*/) -{ - // TODO(anyone): prepare the robot to be ready for read calls and write calls of some interfaces - - return CallbackReturn::SUCCESS; -} - -std::vector MDogHardwareInterface::export_state_interfaces() -{ - std::vector state_interfaces; - for (size_t i = 0; i < info_.joints.size(); ++i) - { - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_states_[i * 3])); - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_states_[i * 3 + 1])); - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_EFFORT, &hw_states_[i * 3 + 2])); - - } - - state_interfaces.emplace_back(hardware_interface::StateInterface( - "imu_sensor", "orientation.w", &imu_states_[0])); - state_interfaces.emplace_back(hardware_interface::StateInterface( - "imu_sensor", "orientation.x", &imu_states_[1])); - state_interfaces.emplace_back(hardware_interface::StateInterface( - "imu_sensor", "orientation.y", &imu_states_[2])); - state_interfaces.emplace_back(hardware_interface::StateInterface( - "imu_sensor", "orientation.z", &imu_states_[3])); - state_interfaces.emplace_back(hardware_interface::StateInterface( - "imu_sensor", "angular_velocity.x", &imu_states_[4])); - state_interfaces.emplace_back(hardware_interface::StateInterface( - "imu_sensor", "angular_velocity.y", &imu_states_[5])); - state_interfaces.emplace_back(hardware_interface::StateInterface( - "imu_sensor", "angular_velocity.z", &imu_states_[6])); - state_interfaces.emplace_back(hardware_interface::StateInterface( - "imu_sensor", "linear_acceleration.x", &imu_states_[7])); - state_interfaces.emplace_back(hardware_interface::StateInterface( - "imu_sensor", "linear_acceleration.y", &imu_states_[8])); - state_interfaces.emplace_back(hardware_interface::StateInterface( - "imu_sensor", "linear_acceleration.z", &imu_states_[9])); - - return state_interfaces; -} - -std::vector MDogHardwareInterface::export_command_interfaces() -{ - std::vector command_interfaces; - for (size_t i = 0; i < info_.joints.size(); ++i) - { - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_commands_[i * 5])); - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_commands_[i * 5 + 1])); - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[i].name, hardware_interface::HW_IF_EFFORT, &hw_commands_[i * 5 + 2])); - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[i].name, "kp", &hw_commands_[i * 5 + 3])); - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[i].name, "kd", &hw_commands_[i * 5 + 4])); - } - return command_interfaces; -} - -hardware_interface::CallbackReturn MDogHardwareInterface::on_activate( - const rclcpp_lifecycle::State & /*previous_state*/) -{ - // TODO(anyone): prepare the robot to receive commands - - return CallbackReturn::SUCCESS; -} - -hardware_interface::CallbackReturn MDogHardwareInterface::on_deactivate( - const rclcpp_lifecycle::State & /*previous_state*/) -{ - // TODO(anyone): prepare the robot to stop receiving commands - - return CallbackReturn::SUCCESS; -} - -hardware_interface::return_type MDogHardwareInterface::read( - const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) -{ - // TODO(anyone): read robot states - - return hardware_interface::return_type::OK; -} - -hardware_interface::return_type MDogHardwareInterface::write( - const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) -{ - // TODO(anyone): write robot's commands' - - return hardware_interface::return_type::OK; -} - -} // namespace mdog_hardware - -#include "pluginlib/class_list_macros.hpp" - -PLUGINLIB_EXPORT_CLASS( - mdog_hardware::MDogHardwareInterface, hardware_interface::SystemInterface) diff --git a/src/hardware/mdog_hardware/test/test_mdog_hardware_interface.cpp b/src/hardware/mdog_hardware/test/test_mdog_hardware_interface.cpp deleted file mode 100644 index 2262ac4..0000000 --- a/src/hardware/mdog_hardware/test/test_mdog_hardware_interface.cpp +++ /dev/null @@ -1,57 +0,0 @@ -// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) -// -// 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. - -#include - -#include - -#include "hardware_interface/resource_manager.hpp" -#include "ros2_control_test_assets/components_urdfs.hpp" -#include "ros2_control_test_assets/descriptions.hpp" - -class TestMDogHardwareInterface : public ::testing::Test -{ -protected: - void SetUp() override - { - // TODO(anyone): Extend this description to your robot - mdog_hardware_interface_2dof_ = - R"( - - - mdog_hardware/MDogHardwareInterface - - - - - 1.57 - - - - - 0.7854 - - - )"; - } - - std::string mdog_hardware_interface_2dof_; -}; - -TEST_F(TestMDogHardwareInterface, load_mdog_hardware_interface_2dof) -{ - auto urdf = ros2_control_test_assets::urdf_head + mdog_hardware_interface_2dof_ + - ros2_control_test_assets::urdf_tail; - ASSERT_NO_THROW(hardware_interface::ResourceManager rm(urdf)); -} diff --git a/src/hardware/unitree_leg_serial_driver/include/unitree_leg_serial_driver/unitree_leg_serial.hpp b/src/hardware/unitree_leg_serial_driver/include/unitree_leg_serial_driver/unitree_leg_serial.hpp index 260137b..0fb3d1a 100644 --- a/src/hardware/unitree_leg_serial_driver/include/unitree_leg_serial_driver/unitree_leg_serial.hpp +++ b/src/hardware/unitree_leg_serial_driver/include/unitree_leg_serial_driver/unitree_leg_serial.hpp @@ -5,10 +5,9 @@ #include #include #include +#include #include "unitree_leg_serial_driver/crc_ccitt.hpp" #include "unitree_leg_serial_driver/gom_protocol.hpp" -#include "rc_msgs/msg/go_motor_cmd.hpp" -#include "rc_msgs/msg/go_motor_fdb.hpp" namespace unitree_leg_serial { @@ -18,41 +17,44 @@ public: explicit UnitreeLegSerial(const rclcpp::NodeOptions &options); ~UnitreeLegSerial() override; + void write(const std::array& cmds); + std::array read(); private: - void receive_data(); - void reopen_port(); - void send_motor_data(const MotorCmd_t &cmd); + static constexpr int PORT_NUM = 4; + static constexpr int MOTOR_NUM = 3; + + void receive_data(int port_idx); + void reopen_port(int port_idx); + void send_motor_data(int port_idx, const MotorCmd_t &cmd); void motor_update(); - void motor_cmd_reset(); - bool open_serial_port(); - void close_serial_port(); - void motor_cmd_callback(const rc_msgs::msg::GoMotorCmd::SharedPtr msg); - - int send_count_; - int recv_count_; + void motor_cmd_reset(int port_idx, int idx); + bool open_serial_port(int port_idx); + void close_serial_port(int port_idx); + + int send_count_[PORT_NUM]{}; + int recv_count_[PORT_NUM]{}; + bool if_debug_{true}; rclcpp::Time last_freq_time_; - rclcpp::Publisher::SharedPtr motor_fdb_pub_; - rclcpp::Subscription::SharedPtr motor_cmd_sub_; - - - std::unique_ptr serial_port_; + std::array, PORT_NUM> serial_port_; rclcpp::TimerBase::SharedPtr timer_; - std::thread read_thread_; + std::array read_thread_; std::atomic running_{false}; // 状态管理 enum StatusFlag : int8_t { OFFLINE = 0, ERROR = 1, CONTROLED = 2 }; std::atomic status_flag_{OFFLINE}; - std::atomic tick_{0}; + std::array, PORT_NUM> tick_; // 串口参数 - std::string serial_port_name_; - int baud_rate_; + std::array serial_port_name_; + std::array baud_rate_; // 电机数据 - MotorCmd_t motor_cmd_; - MotorData_t motor_fbk_; + std::array, PORT_NUM> motor_cmd_; + std::array, PORT_NUM> motor_fbk_; + std::array current_motor_idx_{}; + std::array, PORT_NUM> send_id_count_{}; }; } // namespace unitree_leg_serial \ No newline at end of file diff --git a/src/hardware/unitree_leg_serial_driver/launch/unitree_leg_serial.launch.py b/src/hardware/unitree_leg_serial_driver/launch/unitree_leg_serial.launch.py index 241d684..cff6744 100644 --- a/src/hardware/unitree_leg_serial_driver/launch/unitree_leg_serial.launch.py +++ b/src/hardware/unitree_leg_serial_driver/launch/unitree_leg_serial.launch.py @@ -1,9 +1,16 @@ from launch import LaunchDescription from launch_ros.actions import ComposableNodeContainer from launch_ros.descriptions import ComposableNode +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration def generate_launch_description(): return LaunchDescription([ + DeclareLaunchArgument( + "if_debug", + default_value="true", + description="If true, use ROS topics for communication" + ), ComposableNodeContainer( name="component_container", namespace="", @@ -13,7 +20,8 @@ def generate_launch_description(): ComposableNode( package="unitree_leg_serial_driver", plugin="unitree_leg_serial::UnitreeLegSerial", - name="unitree_leg_serial" + name="unitree_leg_serial", + parameters=[{"if_debug": LaunchConfiguration("if_debug")}] ) ], output="screen" diff --git a/src/hardware/unitree_leg_serial_driver/src/unitree_leg_serial.cpp b/src/hardware/unitree_leg_serial_driver/src/unitree_leg_serial.cpp index 7af8360..70438d5 100644 --- a/src/hardware/unitree_leg_serial_driver/src/unitree_leg_serial.cpp +++ b/src/hardware/unitree_leg_serial_driver/src/unitree_leg_serial.cpp @@ -1,120 +1,170 @@ #include "unitree_leg_serial_driver/unitree_leg_serial.hpp" #include "rclcpp_components/register_node_macro.hpp" -#include "rc_msgs/msg/go_motor_cmd.hpp" -#include "rc_msgs/msg/go_motor_fdb.hpp" namespace unitree_leg_serial { UnitreeLegSerial::UnitreeLegSerial(const rclcpp::NodeOptions &options) : Node("unitree_leg_serial", options) { - serial_port_name_ = "/dev/ttyACM0"; - baud_rate_ = 4000000; + // 串口名和波特率初始化 + serial_port_name_ = {"/dev/ttyACM1", "/dev/ttyACM2", "/dev/ttyACM3", "/dev/ttyACM4"}; + baud_rate_ = {4000000, 4000000, 4000000, 4000000}; - send_count_ = 0; - recv_count_ = 0; last_freq_time_ = this->now(); - // 发布器 - motor_fdb_pub_ = this->create_publisher("motor_fdb", 10); - // 订阅器 - motor_cmd_sub_ = this->create_subscription( - "motor_cmd", 10, - std::bind(&UnitreeLegSerial::motor_cmd_callback, this, std::placeholders::_1)); + for (int p = 0; p < PORT_NUM; ++p) { + send_count_[p] = 0; + recv_count_[p] = 0; + tick_[p] = 0; + current_motor_idx_[p] = 0; + send_id_count_[p].fill(0); - if (!open_serial_port()) { - RCLCPP_ERROR(this->get_logger(), "Failed to open serial port: %s", serial_port_name_.c_str()); - return; + // 初始化每个串口的3个电机命令 + for (int i = 0; i < MOTOR_NUM; ++i) { + motor_cmd_[p][i] = MotorCmd_t{}; + motor_cmd_[p][i].id = i; + motor_cmd_[p][i].mode = 1; + } + + if (!open_serial_port(p)) { + RCLCPP_ERROR(this->get_logger(), "Failed to open serial port: %s", serial_port_name_[p].c_str()); + continue; + } } + status_flag_ = OFFLINE; running_ = true; - status_flag_ = OFFLINE; - tick_ = 0; - read_thread_ = std::thread(&UnitreeLegSerial::receive_data, this); + // 启动每个串口的接收线程 + for (int p = 0; p < PORT_NUM; ++p) { + if (serial_port_[p] && serial_port_[p]->isOpen()) { + read_thread_[p] = std::thread(&UnitreeLegSerial::receive_data, this, p); + } + } + // 定时器，轮询所有串口 timer_ = this->create_wall_timer( - std::chrono::microseconds(500), + std::chrono::microseconds(333), [this]() { motor_update(); }); } UnitreeLegSerial::~UnitreeLegSerial() { running_ = false; - if (read_thread_.joinable()) { - read_thread_.join(); + for (int p = 0; p < PORT_NUM; ++p) { + if (read_thread_[p].joinable()) { + read_thread_[p].join(); + } + close_serial_port(p); } - close_serial_port(); } -void UnitreeLegSerial::motor_cmd_callback(const rc_msgs::msg::GoMotorCmd::SharedPtr msg) +void UnitreeLegSerial::write(const std::array& cmds) { - // 填充motor_cmd_结构体 - motor_cmd_.T = msg->torque_des; - motor_cmd_.W = msg->speed_des; - motor_cmd_.Pos = msg->pos_des; - motor_cmd_.K_P = msg->kp; - motor_cmd_.K_W = msg->kd; + for (int p = 0; p < PORT_NUM; ++p) { + for (int m = 0; m < MOTOR_NUM; ++m) { + int idx = p * MOTOR_NUM + m; + if (idx < 12) { + motor_cmd_[p][m] = cmds[idx]; + } + } + } status_flag_ = CONTROLED; - tick_ = 0; } -bool UnitreeLegSerial::open_serial_port() +std::array UnitreeLegSerial::read() +{ + std::array result; + for (int p = 0; p < PORT_NUM; ++p) { + for (int m = 0; m < MOTOR_NUM; ++m) { + int idx = p * MOTOR_NUM + m; + if (idx < 12) { + result[idx] = motor_fbk_[p][m]; + } + } + } + return result; +} + +bool UnitreeLegSerial::open_serial_port(int port_idx) { try { - serial_port_ = std::make_unique(serial_port_name_, baud_rate_, serial::Timeout::simpleTimeout(1000)); - return serial_port_->isOpen(); + serial_port_[port_idx] = std::make_unique( + serial_port_name_[port_idx], baud_rate_[port_idx], serial::Timeout::simpleTimeout(1000)); + return serial_port_[port_idx]->isOpen(); } catch (const std::exception &e) { - RCLCPP_ERROR(this->get_logger(), "Serial open exception: %s", e.what()); + RCLCPP_ERROR(this->get_logger(), "Serial open exception [%d]: %s", port_idx, e.what()); return false; } } -void UnitreeLegSerial::close_serial_port() +void UnitreeLegSerial::close_serial_port(int port_idx) { - if (serial_port_ && serial_port_->isOpen()) { - serial_port_->close(); + if (serial_port_[port_idx] && serial_port_[port_idx]->isOpen()) { + serial_port_[port_idx]->close(); } } void UnitreeLegSerial::motor_update() { - if (tick_ < 10) { - ++tick_; - } else { - status_flag_ = OFFLINE; + for (int p = 0; p < PORT_NUM; ++p) { + if (tick_[p] < 500) { + ++tick_[p]; + } else { + status_flag_ = OFFLINE; + } + + if (status_flag_ != CONTROLED) { + for (int i = 0; i < MOTOR_NUM; ++i) { + motor_cmd_reset(p, i); + } + } + // 每次只发送一个电机命令，轮流发送 + send_motor_data(p, motor_cmd_[p][current_motor_idx_[p]]); + send_count_[p]++; + send_id_count_[p][current_motor_idx_[p]]++; + current_motor_idx_[p] = (current_motor_idx_[p] + 1) % MOTOR_NUM; } - if (status_flag_ != CONTROLED) { - motor_cmd_reset(); - } - send_motor_data(motor_cmd_); - send_count_++; - - // 每秒打印一次频率 + // 每秒打印一次频率和所有电机状态 auto now = this->now(); if ((now - last_freq_time_).seconds() >= 1.0) { - RCLCPP_INFO(this->get_logger(), "发送频率: %d Hz, 接收频率: %d Hz", send_count_, recv_count_); - send_count_ = 0; - recv_count_ = 0; + for (int p = 0; p < PORT_NUM; ++p) { + RCLCPP_INFO(this->get_logger(), "[Port%d] 发送频率: %d Hz, 接收频率: %d Hz", p, send_count_[p], recv_count_[p]); + for (int i = 0; i < MOTOR_NUM; ++i) { + RCLCPP_INFO(this->get_logger(), + "[Port%d] 电机%d: send_count=%d, Pos=%.3f, W=%.3f, T=%.3f, Temp=%d, Mode=%d, Correct=%d", + p, i, send_id_count_[p][i], + motor_fbk_[p][i].Pos, + motor_fbk_[p][i].W, + motor_fbk_[p][i].T, + motor_fbk_[p][i].Temp, + motor_fbk_[p][i].mode, + motor_fbk_[p][i].correct + ); + send_id_count_[p][i] = 0; // 重置 + } + send_count_[p] = 0; + recv_count_[p] = 0; + } last_freq_time_ = now; } } -void UnitreeLegSerial::motor_cmd_reset() +void UnitreeLegSerial::motor_cmd_reset(int port_idx, int idx) { - motor_cmd_ = MotorCmd_t{}; - motor_cmd_.id = 2; - motor_cmd_.mode = 1; + motor_cmd_[port_idx][idx] = MotorCmd_t{}; + motor_cmd_[port_idx][idx].id = idx; + motor_cmd_[port_idx][idx].mode = 1; } -void UnitreeLegSerial::send_motor_data(const MotorCmd_t &cmd) +void UnitreeLegSerial::send_motor_data(int port_idx, const MotorCmd_t &cmd) { - if (!serial_port_ || !serial_port_->isOpen()) { - RCLCPP_ERROR_THROTTLE(this->get_logger(), *this->get_clock(), 2000, "Serial port is not open."); + if (!serial_port_[port_idx] || !serial_port_[port_idx]->isOpen()) { + RCLCPP_ERROR_THROTTLE(this->get_logger(), *this->get_clock(), 2000, "[Port%d] Serial port is not open.", port_idx); return; } - auto &out = motor_cmd_.motor_send_data; + RIS_ControlData_t out; out.head[0] = 0xFE; out.head[1] = 0xEE; @@ -142,10 +192,10 @@ void UnitreeLegSerial::send_motor_data(const MotorCmd_t &cmd) out.CRC16 = crc_ccitt::crc_ccitt( 0, (uint8_t *)&out, sizeof(RIS_ControlData_t) - sizeof(out.CRC16)); - serial_port_->write(reinterpret_cast(&out), sizeof(RIS_ControlData_t)); + serial_port_[port_idx]->write(reinterpret_cast(&out), sizeof(RIS_ControlData_t)); } -void UnitreeLegSerial::receive_data() +void UnitreeLegSerial::receive_data(int port_idx) { size_t packet_size = sizeof(RIS_MotorData_t); std::vector buffer(packet_size * 2); @@ -153,7 +203,7 @@ void UnitreeLegSerial::receive_data() while (running_) { try { - size_t bytes_read = serial_port_->read(buffer.data() + buffer_offset, buffer.size() - buffer_offset); + size_t bytes_read = serial_port_[port_idx]->read(buffer.data() + buffer_offset, buffer.size() - buffer_offset); if (bytes_read == 0) continue; buffer_offset += bytes_read; @@ -172,53 +222,57 @@ void UnitreeLegSerial::receive_data() } if (buffer_offset < packet_size) continue; - std::memcpy(&motor_fbk_.motor_recv_data, buffer.data(), packet_size); + RIS_MotorData_t recv_data; + std::memcpy(&recv_data, buffer.data(), packet_size); - if (motor_fbk_.motor_recv_data.head[0] != 0xFD || motor_fbk_.motor_recv_data.head[1] != 0xEE) { - motor_fbk_.correct = 0; + int id = recv_data.mode.id; + if (id < 0 || id >= MOTOR_NUM) { + buffer_offset -= packet_size; + std::memmove(buffer.data(), buffer.data() + packet_size, buffer_offset); + continue; + } + + if (recv_data.head[0] != 0xFD || recv_data.head[1] != 0xEE) { + motor_fbk_[port_idx][id].correct = 0; } else { - motor_fbk_.calc_crc = crc_ccitt::crc_ccitt( - 0, (uint8_t *)&motor_fbk_.motor_recv_data, sizeof(RIS_MotorData_t) - sizeof(motor_fbk_.motor_recv_data.CRC16)); - if (motor_fbk_.motor_recv_data.CRC16 != motor_fbk_.calc_crc) { - memset(&motor_fbk_.motor_recv_data, 0, sizeof(RIS_MotorData_t)); - motor_fbk_.correct = 0; - motor_fbk_.bad_msg++; + motor_fbk_[port_idx][id].calc_crc = crc_ccitt::crc_ccitt( + 0, (uint8_t *)&recv_data, sizeof(RIS_MotorData_t) - sizeof(recv_data.CRC16)); + if (recv_data.CRC16 != motor_fbk_[port_idx][id].calc_crc) { + memset(&motor_fbk_[port_idx][id].motor_recv_data, 0, sizeof(RIS_MotorData_t)); + motor_fbk_[port_idx][id].correct = 0; + motor_fbk_[port_idx][id].bad_msg++; } else { - motor_fbk_.motor_id = motor_fbk_.motor_recv_data.mode.id; - motor_fbk_.mode = motor_fbk_.motor_recv_data.mode.status; - motor_fbk_.Temp = motor_fbk_.motor_recv_data.fbk.temp; - motor_fbk_.MError = motor_fbk_.motor_recv_data.fbk.MError; - motor_fbk_.W = ((float)motor_fbk_.motor_recv_data.fbk.speed / 256.0f) * 6.28318f; - motor_fbk_.T = ((float)motor_fbk_.motor_recv_data.fbk.torque) / 256.0f; - motor_fbk_.Pos = 6.28318f * ((float)motor_fbk_.motor_recv_data.fbk.pos) / 32768.0f; - motor_fbk_.footForce = motor_fbk_.motor_recv_data.fbk.force; - motor_fbk_.correct = 1; + std::memcpy(&motor_fbk_[port_idx][id].motor_recv_data, &recv_data, packet_size); + motor_fbk_[port_idx][id].motor_id = recv_data.mode.id; + motor_fbk_[port_idx][id].mode = recv_data.mode.status; + motor_fbk_[port_idx][id].Temp = recv_data.fbk.temp; + motor_fbk_[port_idx][id].MError = recv_data.fbk.MError; + motor_fbk_[port_idx][id].W = ((float)recv_data.fbk.speed / 256.0f) * 6.28318f; + motor_fbk_[port_idx][id].T = ((float)recv_data.fbk.torque) / 256.0f; + motor_fbk_[port_idx][id].Pos = 6.28318f * ((float)recv_data.fbk.pos) / 32768.0f; + motor_fbk_[port_idx][id].footForce = recv_data.fbk.force; + motor_fbk_[port_idx][id].correct = 1; } } - if (motor_fbk_.correct) { - // 发布反馈消息 - rc_msgs::msg::GoMotorFdb msg; - msg.torque = motor_fbk_.T; - msg.speed = motor_fbk_.W; - msg.pos = motor_fbk_.Pos; - motor_fdb_pub_->publish(msg); - recv_count_++; - RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "Motor ID: %d, Position: %f", motor_fbk_.motor_id, motor_fbk_.Pos); + if (motor_fbk_[port_idx][id].correct) { + recv_count_[port_idx]++; + RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 1000, + "[Port%d] Motor ID: %d, Position: %f", port_idx, motor_fbk_[port_idx][id].motor_id, motor_fbk_[port_idx][id].Pos); } std::memmove(buffer.data(), buffer.data() + packet_size, buffer_offset - packet_size); buffer_offset -= packet_size; } catch (const std::exception &e) { - RCLCPP_ERROR(this->get_logger(), "Serial read exception: %s", e.what()); - reopen_port(); + RCLCPP_ERROR(this->get_logger(), "Serial read exception [%d]: %s", port_idx, e.what()); + reopen_port(port_idx); } } } -void UnitreeLegSerial::reopen_port() +void UnitreeLegSerial::reopen_port(int port_idx) { - close_serial_port(); + close_serial_port(port_idx); rclcpp::sleep_for(std::chrono::milliseconds(100)); - open_serial_port(); + open_serial_port(port_idx); } } // namespace unitree_leg_serial diff --git a/src/robot_descriptions/qut/mdog_description/urdf/robot.urdf b/src/robot_descriptions/qut/mdog_description/urdf/robot.urdf index 5d72935..3b56330 100644 --- a/src/robot_descriptions/qut/mdog_description/urdf/robot.urdf +++ b/src/robot_descriptions/qut/mdog_description/urdf/robot.urdf @@ -39,7 +39,7 @@ - hardware_mdog/hardware_mdog_real + mdog_hardware/MDogHardwareInterface @@ -246,7 +246,7 @@ - + @@ -274,7 +274,7 @@ - + @@ -330,7 +330,7 @@ - + @@ -358,7 +358,7 @@ - + @@ -414,7 +414,7 @@ - + @@ -442,7 +442,7 @@ - + @@ -498,7 +498,7 @@ - + @@ -526,7 +526,7 @@ - + diff --git a/src/robot_descriptions/qut/mdog_description/xacro/ros2_control.xacro b/src/robot_descriptions/qut/mdog_description/xacro/ros2_control.xacro index e21aa01..5485541 100644 --- a/src/robot_descriptions/qut/mdog_description/xacro/ros2_control.xacro +++ b/src/robot_descriptions/qut/mdog_description/xacro/ros2_control.xacro @@ -4,7 +4,7 @@ - mdog_hardware/mdog_hardware_interface + mdog_hardware/MDogHardwareInterface