robofish/CM_DOG
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

12个电机读写正常

Browse Source
This commit is contained in:
robofish 2025-05-16 21:45:58 +08:00
parent 138be4f159
commit d0630a82c7
30 changed files with 205 additions and 2048 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
.vscode/settings.json vendored
View File

@ -1,5 +1,5 @@
{ {
"files.associations": { "files.associations": {
"functional": "cpp" "chrono": "cpp"
} }
} }

8
src/commands/joystick_input/src/JoystickInput.cpp
View File

@ -25,9 +25,9 @@ void JoystickInput::joy_callback(sensor_msgs::msg::Joy::SharedPtr msg) {
inputs_.command = 5; // LT + B inputs_.command = 5; // LT + B
} else if (msg->axes[2] != 1 && msg->buttons[0]) { } else if (msg->axes[2] != 1 && msg->buttons[0]) {
inputs_.command = 6; // LT + A 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]) { } 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 inputs_.command = 8; // LT + Y
} else if (msg->buttons[7]) { } else if (msg->buttons[7]) {
inputs_.command = 9; // START inputs_.command = 9; // START
@ -35,8 +35,8 @@ void JoystickInput::joy_callback(sensor_msgs::msg::Joy::SharedPtr msg) {
inputs_.command = 0; inputs_.command = 0;
inputs_.lx = -msg->axes[0]; inputs_.lx = -msg->axes[0];
inputs_.ly = msg->axes[1]; inputs_.ly = msg->axes[1];
inputs_.rx = -msg->axes[3]; inputs_.rx = -msg->axes[2];
inputs_.ry = msg->axes[4]; inputs_.ry = msg->axes[3];
} }
publisher_->publish(inputs_); publisher_->publish(inputs_);
} }

89
src/controllers/mdog_simple_controller/CMakeLists.txt
View File

@ -6,58 +6,19 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
endif() endif()
# find dependencies # 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(ament_cmake REQUIRED)
find_package(generate_parameter_library REQUIRED) # uncomment the following section in order to fill in
find_package(ament_cmake_gmock REQUIRED) # further dependencies manually.
find_package(controller_manager REQUIRED) # find_package(<dependency> 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
"$<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>"
"$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
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)
install( install(
TARGETS DIRECTORY launch
mdog_simple_controller DESTINATION share/${PROJECT_NAME}/
RUNTIME DESTINATION bin
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
) )
install( install(
DIRECTORY include/ DIRECTORY include/
DESTINATION include/${PROJECT_NAME} DESTINATION include
) )
if(BUILD_TESTING) if(BUILD_TESTING)
@ -69,43 +30,7 @@ if(BUILD_TESTING)
# comment the line when this package is in a git repo and when # comment the line when this package is in a git repo and when
# a copyright and license is added to all source files # a copyright and license is added to all source files
set(ament_cmake_cpplint_FOUND TRUE) set(ament_cmake_cpplint_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
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
)
endif() endif()
ament_export_include_directories(
include
)
ament_export_dependencies(
${THIS_PACKAGE_INCLUDE_DEPENDS}
)
ament_export_libraries(
mdog_simple_controller
)
ament_package() ament_package()

117
src/controllers/mdog_simple_controller/include/mdog_simple_controller/mdog_simple_controller.hpp
View File

@ -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 <memory>
#include <string>
#include <vector>
#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<mdog_simple_controller::ParamListener> param_listener_;
mdog_simple_controller::Params params_;
std::vector<std::string> state_joints_;
// Command subscribers and Controller State publisher
rclcpp::Subscription<ControllerReferenceMsg>::SharedPtr ref_subscriber_ = nullptr;
realtime_tools::RealtimeBuffer<std::shared_ptr<ControllerReferenceMsg>> input_ref_;
rclcpp::Service<ControllerModeSrvType>::SharedPtr set_slow_control_mode_service_;
realtime_tools::RealtimeBuffer<control_mode_type> control_mode_;
using ControllerStatePublisher = realtime_tools::RealtimePublisher<ControllerStateMsg>;
rclcpp::Publisher<ControllerStateMsg>::SharedPtr s_publisher_;
std::unique_ptr<ControllerStatePublisher> state_publisher_;
private:
// callback for topic interface
MDOG_SIMPLE_CONTROLLER__VISIBILITY_LOCAL
void reference_callback(const std::shared_ptr<ControllerReferenceMsg> msg);
};
} // namespace mdog_simple_controller
#endif // MDOG_SIMPLE_CONTROLLER__MDOG_SIMPLE_CONTROLLER_HPP_

43
src/controllers/mdog_simple_controller/include/mdog_simple_controller/validate_mdog_simple_controller_parameters.hpp
View File

@ -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 <string>
#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_

54
src/controllers/mdog_simple_controller/include/mdog_simple_controller/visibility_control.h
View File

@ -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_

28
src/controllers/mdog_simple_controller/mdog_simple_controller.xml
View File

@ -1,28 +0,0 @@
<!--
Copyright (c) 2024, Stogl Robotics Consulting UG (haftungsbeschränkt)
Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at
http://www.apache.org/licenses/LICENSE-2.0
Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
Source of this file are templates in
[RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository.
-->
<library path="mdog_simple_controller">
<class name="mdog_simple_controller/MdogSimpleController"
type="mdog_simple_controller::MdogSimpleController" base_class_type="controller_interface::ControllerInterface">
<description>
MdogSimpleController ros2_control controller.
</description>
</class>
</library>

23
src/controllers/mdog_simple_controller/package.xml
View File

@ -9,29 +9,8 @@
<buildtool_depend>ament_cmake</buildtool_depend> <buildtool_depend>ament_cmake</buildtool_depend>
<build_depend>generate_parameter_library</build_depend>
<depend>control_msgs</depend>
<depend>controller_interface</depend>
<depend>hardware_interface</depend>
<depend>pluginlib</depend>
<depend>rclcpp</depend>
<depend>rclcpp_lifecycle</depend>
<depend>realtime_tools</depend>
<depend>std_srvs</depend>
<test_depend>ament_lint_auto</test_depend> <test_depend>ament_lint_auto</test_depend>
<test_depend>ament_cmake_gmock</test_depend> <test_depend>ament_lint_common</test_depend>
<test_depend>controller_manager</test_depend>
<test_depend>hardware_interface</test_depend>
<test_depend>ros2_control_test_assets</test_depend>
<export> <export>
<build_type>ament_cmake</build_type> <build_type>ament_cmake</build_type>

265
src/controllers/mdog_simple_controller/src/mdog_simple_controller.cpp
View File

@ -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 <limits>
#include <memory>
#include <string>
#include <vector>
#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<ControllerReferenceMsg> & msg, const std::vector<std::string> & joint_names)
{
msg->joint_names = joint_names;
msg->displacements.resize(joint_names.size(), std::numeric_limits<double>::quiet_NaN());
msg->velocities.resize(joint_names.size(), std::numeric_limits<double>::quiet_NaN());
msg->duration = std::numeric_limits<double>::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<mdog_simple_controller::ParamListener>(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<ControllerReferenceMsg>(
"~/reference", subscribers_qos,
std::bind(&MdogSimpleController::reference_callback, this, std::placeholders::_1));
std::shared_ptr<ControllerReferenceMsg> msg = std::make_shared<ControllerReferenceMsg>();
reset_controller_reference_msg(msg, params_.joints);
input_ref_.writeFromNonRT(msg);
auto set_slow_mode_service_callback =
[&](
const std::shared_ptr<ControllerModeSrvType::Request> request,
std::shared_ptr<ControllerModeSrvType::Response> 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<ControllerModeSrvType>(
"~/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<ControllerStateMsg>("~/state", rclcpp::SystemDefaultsQoS());
state_publisher_ = std::make_unique<ControllerStatePublisher>(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<ControllerReferenceMsg> 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<double>::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<double>::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)

50
src/controllers/mdog_simple_controller/src/mdog_simple_controller.yaml
View File

@ -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
}
}

26
src/controllers/mdog_simple_controller/test/mdog_simple_controller_params.yaml
View File

@ -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

28
src/controllers/mdog_simple_controller/test/mdog_simple_controller_preceeding_params.yaml
View File

@ -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

44
src/controllers/mdog_simple_controller/test/test_load_mdog_simple_controller.cpp
View File

@ -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 <gmock/gmock.h>
#include <memory>
#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<rclcpp::Executor> executor =
std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
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();
}

277
src/controllers/mdog_simple_controller/test/test_mdog_simple_controller.cpp
View File

@ -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 <limits>
#include <memory>
#include <string>
#include <utility>
#include <vector>
#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<TestableMdogSimpleController>
{
};
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<ControllerReferenceMsg> msg = std::make_shared<ControllerReferenceMsg>();
msg->joint_names = joint_names_;
msg->displacements.resize(joint_names_.size(), TEST_DISPLACEMENT);
msg->velocities.resize(joint_names_.size(), std::numeric_limits<double>::quiet_NaN());
msg->duration = std::numeric_limits<double>::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<ControllerReferenceMsg> msg = std::make_shared<ControllerReferenceMsg>();
// 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<double>::quiet_NaN());
msg->duration = std::numeric_limits<double>::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;
}

274
src/controllers/mdog_simple_controller/test/test_mdog_simple_controller.hpp
View File

@ -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 <chrono>
#include <limits>
#include <memory>
#include <string>
#include <tuple>
#include <utility>
#include <vector>
#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 <typename CtrlType>
class MdogSimpleControllerFixture : public ::testing::Test
{
public:
static void SetUpTestCase() {}
void SetUp()
{
// initialize controller
controller_ = std::make_unique<CtrlType>();
command_publisher_node_ = std::make_shared<rclcpp::Node>("command_publisher");
command_publisher_ = command_publisher_node_->create_publisher<ControllerReferenceMsg>(
"/test_mdog_simple_controller/commands", rclcpp::SystemDefaultsQoS());
service_caller_node_ = std::make_shared<rclcpp::Node>("service_caller");
slow_control_service_client_ = service_caller_node_->create_client<ControllerModeSrvType>(
"/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<hardware_interface::LoanedCommandInterface> 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<hardware_interface::LoanedStateInterface> 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<ControllerStateMsg>(
"/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<double> & displacements = {0.45},
const std::vector<double> & 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<ControllerModeSrvType::Response> call_service(
const bool slow_control, rclcpp::Executor & executor)
{
auto request = std::make_shared<ControllerModeSrvType::Request>();
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<std::string> joint_names_ = {"joint1"};
std::vector<std::string> state_joint_names_ = {"joint1state"};
std::string interface_name_ = "acceleration";
std::array<double, 1> joint_state_values_ = {1.1};
std::array<double, 1> joint_command_values_ = {101.101};
std::vector<hardware_interface::StateInterface> state_itfs_;
std::vector<hardware_interface::CommandInterface> command_itfs_;
// Test related parameters
std::unique_ptr<TestableMdogSimpleController> controller_;
rclcpp::Node::SharedPtr command_publisher_node_;
rclcpp::Publisher<ControllerReferenceMsg>::SharedPtr command_publisher_;
rclcpp::Node::SharedPtr service_caller_node_;
rclcpp::Client<ControllerModeSrvType>::SharedPtr slow_control_service_client_;
};
#endif // TEMPLATES__ROS2_CONTROL__CONTROLLER__TEST_MDOG_SIMPLE_CONTROLLER_HPP_

80
src/controllers/mdog_simple_controller/test/test_mdog_simple_controller_preceeding.cpp
View File

@ -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 <limits>
#include <memory>
#include <string>
#include <utility>
#include <vector>
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<TestableMdogSimpleController>
{
};
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;
}

0
src/controllers/trot.py
View File

93
src/hardware/mdog_hardware/CMakeLists.txt
View File

@ -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()

79
src/hardware/mdog_hardware/include/mdog_hardware/mdog_hardware_interface.hpp
View File

@ -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 <string>
#include <vector>
#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<hardware_interface::StateInterface> export_state_interfaces() override;
TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC
std::vector<hardware_interface::CommandInterface> 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<double> hw_commands_;
std::vector<double> hw_states_;
std::vector<double> imu_states_;
rclcpp::Node::SharedPtr node_;
rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imu_subscription_;
void imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg);
};
} // namespace mdog_hardware
#endif // MDOG_HARDWARE__MDOG_HARDWARE_INTERFACE_HPP_

49
src/hardware/mdog_hardware/include/mdog_hardware/visibility_control.h
View File

@ -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_

43
src/hardware/mdog_hardware/launch/mdog_hardware.launch.py
View File

@ -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
])

9
src/hardware/mdog_hardware/mdog_hardware.xml
View File

@ -1,9 +0,0 @@
<library path="mdog_hardware">
<class name="mdog_hardware/MDogHardwareInterface"
type="mdog_hardware::MDogHardwareInterface"
base_class_type="hardware_interface::SystemInterface">
<description>
ros2_control hardware interface.
</description>
</class>
</library>

30
src/hardware/mdog_hardware/package.xml
View File

@ -1,30 +0,0 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>mdog_hardware</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="1683502971@qq.com">robofish</maintainer>
<license>TODO: License declaration</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>hardware_interface</depend>
<depend>pluginlib</depend>
<depend>rclcpp_lifecycle</depend>
<depend>rclcpp</depend>
<depend>std_msgs</depend>
<depend>sensor_msgs</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<test_depend>ament_cmake_gmock</test_depend>
<test_depend>ros2_control_test_assets</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

165
src/hardware/mdog_hardware/src/mdog_hardware_interface.cpp
View File

@ -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 <limits>
#include <vector>
#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<double>::quiet_NaN());
hw_commands_.resize(info_.joints.size() * 5, std::numeric_limits<double>::quiet_NaN());
imu_states_.resize(10, std::numeric_limits<double>::quiet_NaN());
// 创建 ROS 2 节点和订阅器
node_ = std::make_shared<rclcpp::Node>("mdog_hardware_interface");
imu_subscription_ = node_->create_subscription<sensor_msgs::msg::Imu>(
"/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<hardware_interface::StateInterface> MDogHardwareInterface::export_state_interfaces()
{
std::vector<hardware_interface::StateInterface> 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<hardware_interface::CommandInterface> MDogHardwareInterface::export_command_interfaces()
{
std::vector<hardware_interface::CommandInterface> 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)

57
src/hardware/mdog_hardware/test/test_mdog_hardware_interface.cpp
View File

@ -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 <gmock/gmock.h>
#include <string>
#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"(
<ros2_control name="MDogHardwareInterface2dof" type="system">
<hardware>
<plugin>mdog_hardware/MDogHardwareInterface</plugin>
</hardware>
<joint name="joint1">
<command_interface name="position"/>
<state_interface name="position"/>
<param name="initial_position">1.57</param>
</joint>
<joint name="joint2">
<command_interface name="position"/>
<state_interface name="position"/>
<param name="initial_position">0.7854</param>
</joint>
</ros2_control>
)";
}
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));
}

48
src/hardware/unitree_leg_serial_driver/include/unitree_leg_serial_driver/unitree_leg_serial.hpp
View File

@ -5,10 +5,9 @@
#include <rclcpp/rclcpp.hpp> #include <rclcpp/rclcpp.hpp>
#include <string> #include <string>
#include <thread> #include <thread>
#include <array>
#include "unitree_leg_serial_driver/crc_ccitt.hpp" #include "unitree_leg_serial_driver/crc_ccitt.hpp"
#include "unitree_leg_serial_driver/gom_protocol.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 namespace unitree_leg_serial
{ {
@ -18,41 +17,44 @@ public:
explicit UnitreeLegSerial(const rclcpp::NodeOptions &options); explicit UnitreeLegSerial(const rclcpp::NodeOptions &options);
~UnitreeLegSerial() override; ~UnitreeLegSerial() override;
void write(const std::array<MotorCmd_t, 12>& cmds);
std::array<MotorData_t, 12> read();
private: private:
void receive_data(); static constexpr int PORT_NUM = 4;
void reopen_port(); static constexpr int MOTOR_NUM = 3;
void send_motor_data(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_; void receive_data(int port_idx);
int recv_count_; void reopen_port(int port_idx);
void send_motor_data(int port_idx, const MotorCmd_t &cmd);
void motor_update();
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::Time last_freq_time_;
rclcpp::Publisher<rc_msgs::msg::GoMotorFdb>::SharedPtr motor_fdb_pub_; std::array<std::unique_ptr<serial::Serial>, PORT_NUM> serial_port_;
rclcpp::Subscription<rc_msgs::msg::GoMotorCmd>::SharedPtr motor_cmd_sub_;
std::unique_ptr<serial::Serial> serial_port_;
rclcpp::TimerBase::SharedPtr timer_; rclcpp::TimerBase::SharedPtr timer_;
std::thread read_thread_; std::array<std::thread, PORT_NUM> read_thread_;
std::atomic<bool> running_{false}; std::atomic<bool> running_{false};
// 状态管理 // 状态管理
enum StatusFlag : int8_t { OFFLINE = 0, ERROR = 1, CONTROLED = 2 }; enum StatusFlag : int8_t { OFFLINE = 0, ERROR = 1, CONTROLED = 2 };
std::atomic<StatusFlag> status_flag_{OFFLINE}; std::atomic<StatusFlag> status_flag_{OFFLINE};
std::atomic<int8_t> tick_{0}; std::array<std::atomic<int8_t>, PORT_NUM> tick_;
// 串口参数 // 串口参数
std::string serial_port_name_; std::array<std::string, PORT_NUM> serial_port_name_;
int baud_rate_; std::array<int, PORT_NUM> baud_rate_;
// 电机数据 // 电机数据
MotorCmd_t motor_cmd_; std::array<std::array<MotorCmd_t, MOTOR_NUM>, PORT_NUM> motor_cmd_;
MotorData_t motor_fbk_; std::array<std::array<MotorData_t, MOTOR_NUM>, PORT_NUM> motor_fbk_;
std::array<int, PORT_NUM> current_motor_idx_{};
std::array<std::array<int, MOTOR_NUM>, PORT_NUM> send_id_count_{};
}; };
} // namespace unitree_leg_serial } // namespace unitree_leg_serial

10
src/hardware/unitree_leg_serial_driver/launch/unitree_leg_serial.launch.py
View File

@ -1,9 +1,16 @@
from launch import LaunchDescription from launch import LaunchDescription
from launch_ros.actions import ComposableNodeContainer from launch_ros.actions import ComposableNodeContainer
from launch_ros.descriptions import ComposableNode from launch_ros.descriptions import ComposableNode
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
def generate_launch_description(): def generate_launch_description():
return LaunchDescription([ return LaunchDescription([
DeclareLaunchArgument(
"if_debug",
default_value="true",
description="If true, use ROS topics for communication"
),
ComposableNodeContainer( ComposableNodeContainer(
name="component_container", name="component_container",
namespace="", namespace="",
@ -13,7 +20,8 @@ def generate_launch_description():
ComposableNode( ComposableNode(
package="unitree_leg_serial_driver", package="unitree_leg_serial_driver",
plugin="unitree_leg_serial::UnitreeLegSerial", plugin="unitree_leg_serial::UnitreeLegSerial",
name="unitree_leg_serial" name="unitree_leg_serial",
parameters=[{"if_debug": LaunchConfiguration("if_debug")}]
) )
], ],
output="screen" output="screen"

242
src/hardware/unitree_leg_serial_driver/src/unitree_leg_serial.cpp
View File

@ -1,120 +1,170 @@
#include "unitree_leg_serial_driver/unitree_leg_serial.hpp" #include "unitree_leg_serial_driver/unitree_leg_serial.hpp"
#include "rclcpp_components/register_node_macro.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 namespace unitree_leg_serial
{ {
UnitreeLegSerial::UnitreeLegSerial(const rclcpp::NodeOptions &options) UnitreeLegSerial::UnitreeLegSerial(const rclcpp::NodeOptions &options)
: Node("unitree_leg_serial", 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(); last_freq_time_ = this->now();
// 发布器 for (int p = 0; p < PORT_NUM; ++p) {
motor_fdb_pub_ = this->create_publisher<rc_msgs::msg::GoMotorFdb>("motor_fdb", 10); send_count_[p] = 0;
// 订阅器 recv_count_[p] = 0;
motor_cmd_sub_ = this->create_subscription<rc_msgs::msg::GoMotorCmd>( tick_[p] = 0;
"motor_cmd", 10, current_motor_idx_[p] = 0;
std::bind(&UnitreeLegSerial::motor_cmd_callback, this, std::placeholders::_1)); send_id_count_[p].fill(0);
if (!open_serial_port()) { // 初始化每个串口的3个电机命令
RCLCPP_ERROR(this->get_logger(), "Failed to open serial port: %s", serial_port_name_.c_str()); for (int i = 0; i < MOTOR_NUM; ++i) {
return; 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; running_ = true;
status_flag_ = OFFLINE; // 启动每个串口的接收线程
tick_ = 0; for (int p = 0; p < PORT_NUM; ++p) {
read_thread_ = std::thread(&UnitreeLegSerial::receive_data, this); if (serial_port_[p] && serial_port_[p]->isOpen()) {
read_thread_[p] = std::thread(&UnitreeLegSerial::receive_data, this, p);
}
}
// 定时器，轮询所有串口
timer_ = this->create_wall_timer( timer_ = this->create_wall_timer(
std::chrono::microseconds(500), std::chrono::microseconds(333),
[this]() { motor_update(); }); [this]() { motor_update(); });
} }
UnitreeLegSerial::~UnitreeLegSerial() UnitreeLegSerial::~UnitreeLegSerial()
{ {
running_ = false; running_ = false;
if (read_thread_.joinable()) { for (int p = 0; p < PORT_NUM; ++p) {
read_thread_.join(); 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<MotorCmd_t, 12>& cmds)
{ {
// 填充motor_cmd_结构体 for (int p = 0; p < PORT_NUM; ++p) {
motor_cmd_.T = msg->torque_des; for (int m = 0; m < MOTOR_NUM; ++m) {
motor_cmd_.W = msg->speed_des; int idx = p * MOTOR_NUM + m;
motor_cmd_.Pos = msg->pos_des; if (idx < 12) {
motor_cmd_.K_P = msg->kp; motor_cmd_[p][m] = cmds[idx];
motor_cmd_.K_W = msg->kd; }
}
}
status_flag_ = CONTROLED; status_flag_ = CONTROLED;
tick_ = 0;
} }
bool UnitreeLegSerial::open_serial_port() std::array<MotorData_t, 12> UnitreeLegSerial::read()
{
std::array<MotorData_t, 12> 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 { try {
serial_port_ = std::make_unique<serial::Serial>(serial_port_name_, baud_rate_, serial::Timeout::simpleTimeout(1000)); serial_port_[port_idx] = std::make_unique<serial::Serial>(
return serial_port_->isOpen(); serial_port_name_[port_idx], baud_rate_[port_idx], serial::Timeout::simpleTimeout(1000));
return serial_port_[port_idx]->isOpen();
} catch (const std::exception &e) { } 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; return false;
} }
} }
void UnitreeLegSerial::close_serial_port() void UnitreeLegSerial::close_serial_port(int port_idx)
{ {
if (serial_port_ && serial_port_->isOpen()) { if (serial_port_[port_idx] && serial_port_[port_idx]->isOpen()) {
serial_port_->close(); serial_port_[port_idx]->close();
} }
} }
void UnitreeLegSerial::motor_update() void UnitreeLegSerial::motor_update()
{ {
if (tick_ < 10) { for (int p = 0; p < PORT_NUM; ++p) {
++tick_; if (tick_[p] < 500) {
} else { ++tick_[p];
status_flag_ = OFFLINE; } 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(); auto now = this->now();
if ((now - last_freq_time_).seconds() >= 1.0) { if ((now - last_freq_time_).seconds() >= 1.0) {
RCLCPP_INFO(this->get_logger(), "发送频率: %d Hz, 接收频率: %d Hz", send_count_, recv_count_); for (int p = 0; p < PORT_NUM; ++p) {
send_count_ = 0; RCLCPP_INFO(this->get_logger(), "[Port%d] 发送频率: %d Hz, 接收频率: %d Hz", p, send_count_[p], recv_count_[p]);
recv_count_ = 0; 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; 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_[port_idx][idx] = MotorCmd_t{};
motor_cmd_.id = 2; motor_cmd_[port_idx][idx].id = idx;
motor_cmd_.mode = 1; 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()) { if (!serial_port_[port_idx] || !serial_port_[port_idx]->isOpen()) {
RCLCPP_ERROR_THROTTLE(this->get_logger(), *this->get_clock(), 2000, "Serial port is not open."); RCLCPP_ERROR_THROTTLE(this->get_logger(), *this->get_clock(), 2000, "[Port%d] Serial port is not open.", port_idx);
return; return;
} }
auto &out = motor_cmd_.motor_send_data; RIS_ControlData_t out;
out.head[0] = 0xFE; out.head[0] = 0xFE;
out.head[1] = 0xEE; out.head[1] = 0xEE;
@ -142,10 +192,10 @@ void UnitreeLegSerial::send_motor_data(const MotorCmd_t &cmd)
out.CRC16 = crc_ccitt::crc_ccitt( out.CRC16 = crc_ccitt::crc_ccitt(
0, (uint8_t *)&out, sizeof(RIS_ControlData_t) - sizeof(out.CRC16)); 0, (uint8_t *)&out, sizeof(RIS_ControlData_t) - sizeof(out.CRC16));
serial_port_->write(reinterpret_cast<const uint8_t *>(&out), sizeof(RIS_ControlData_t)); serial_port_[port_idx]->write(reinterpret_cast<const uint8_t *>(&out), sizeof(RIS_ControlData_t));
} }
void UnitreeLegSerial::receive_data() void UnitreeLegSerial::receive_data(int port_idx)
{ {
size_t packet_size = sizeof(RIS_MotorData_t); size_t packet_size = sizeof(RIS_MotorData_t);
std::vector<uint8_t> buffer(packet_size * 2); std::vector<uint8_t> buffer(packet_size * 2);
@ -153,7 +203,7 @@ void UnitreeLegSerial::receive_data()
while (running_) { while (running_) {
try { 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; if (bytes_read == 0) continue;
buffer_offset += bytes_read; buffer_offset += bytes_read;
@ -172,53 +222,57 @@ void UnitreeLegSerial::receive_data()
} }
if (buffer_offset < packet_size) continue; 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) { int id = recv_data.mode.id;
motor_fbk_.correct = 0; 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 { } else {
motor_fbk_.calc_crc = crc_ccitt::crc_ccitt( motor_fbk_[port_idx][id].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)); 0, (uint8_t *)&recv_data, sizeof(RIS_MotorData_t) - sizeof(recv_data.CRC16));
if (motor_fbk_.motor_recv_data.CRC16 != motor_fbk_.calc_crc) { if (recv_data.CRC16 != motor_fbk_[port_idx][id].calc_crc) {
memset(&motor_fbk_.motor_recv_data, 0, sizeof(RIS_MotorData_t)); memset(&motor_fbk_[port_idx][id].motor_recv_data, 0, sizeof(RIS_MotorData_t));
motor_fbk_.correct = 0; motor_fbk_[port_idx][id].correct = 0;
motor_fbk_.bad_msg++; motor_fbk_[port_idx][id].bad_msg++;
} else { } else {
motor_fbk_.motor_id = motor_fbk_.motor_recv_data.mode.id; std::memcpy(&motor_fbk_[port_idx][id].motor_recv_data, &recv_data, packet_size);
motor_fbk_.mode = motor_fbk_.motor_recv_data.mode.status; motor_fbk_[port_idx][id].motor_id = recv_data.mode.id;
motor_fbk_.Temp = motor_fbk_.motor_recv_data.fbk.temp; motor_fbk_[port_idx][id].mode = recv_data.mode.status;
motor_fbk_.MError = motor_fbk_.motor_recv_data.fbk.MError; motor_fbk_[port_idx][id].Temp = recv_data.fbk.temp;
motor_fbk_.W = ((float)motor_fbk_.motor_recv_data.fbk.speed / 256.0f) * 6.28318f; motor_fbk_[port_idx][id].MError = recv_data.fbk.MError;
motor_fbk_.T = ((float)motor_fbk_.motor_recv_data.fbk.torque) / 256.0f; motor_fbk_[port_idx][id].W = ((float)recv_data.fbk.speed / 256.0f) * 6.28318f;
motor_fbk_.Pos = 6.28318f * ((float)motor_fbk_.motor_recv_data.fbk.pos) / 32768.0f; motor_fbk_[port_idx][id].T = ((float)recv_data.fbk.torque) / 256.0f;
motor_fbk_.footForce = motor_fbk_.motor_recv_data.fbk.force; motor_fbk_[port_idx][id].Pos = 6.28318f * ((float)recv_data.fbk.pos) / 32768.0f;
motor_fbk_.correct = 1; motor_fbk_[port_idx][id].footForce = recv_data.fbk.force;
motor_fbk_[port_idx][id].correct = 1;
} }
} }
if (motor_fbk_.correct) { if (motor_fbk_[port_idx][id].correct) {
// 发布反馈消息 recv_count_[port_idx]++;
rc_msgs::msg::GoMotorFdb msg; RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 1000,
msg.torque = motor_fbk_.T; "[Port%d] Motor ID: %d, Position: %f", port_idx, motor_fbk_[port_idx][id].motor_id, motor_fbk_[port_idx][id].Pos);
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);
} }
std::memmove(buffer.data(), buffer.data() + packet_size, buffer_offset - packet_size); std::memmove(buffer.data(), buffer.data() + packet_size, buffer_offset - packet_size);
buffer_offset -= packet_size; buffer_offset -= packet_size;
} catch (const std::exception &e) { } catch (const std::exception &e) {
RCLCPP_ERROR(this->get_logger(), "Serial read exception: %s", e.what()); RCLCPP_ERROR(this->get_logger(), "Serial read exception [%d]: %s", port_idx, e.what());
reopen_port(); 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)); rclcpp::sleep_for(std::chrono::milliseconds(100));
open_serial_port(); open_serial_port(port_idx);
} }
} // namespace unitree_leg_serial } // namespace unitree_leg_serial

18
src/robot_descriptions/qut/mdog_description/urdf/robot.urdf
View File

@ -39,7 +39,7 @@
</material> </material>
<ros2_control name="MdogSystem" type="system"> <ros2_control name="MdogSystem" type="system">
<hardware> <hardware>
<plugin>hardware_mdog/hardware_mdog_real</plugin> <plugin>mdog_hardware/MDogHardwareInterface</plugin>
</hardware> </hardware>
<joint name="FR_hip_joint"> <joint name="FR_hip_joint">
<command_interface name="position"/> <command_interface name="position"/>
@ -246,7 +246,7 @@
</link> </link>
<!-- 定义thigh_joint --> <!-- 定义thigh_joint -->
<joint name="FR_thigh_joint" type="revolute"> <joint name="FR_thigh_joint" type="revolute">
<origin rpy="0 -1.5708 0" xyz="0 0.05 0"/> <origin rpy="0 -0.7854 0" xyz="0 0.05 0"/>
<parent link="FR_hip"/> <parent link="FR_hip"/>
<child link="FR_thigh"/> <child link="FR_thigh"/>
<axis xyz="0 1 0"/> <axis xyz="0 1 0"/>
@ -274,7 +274,7 @@
</link> </link>
<!-- 定义calf_joint --> <!-- 定义calf_joint -->
<joint name="FR_calf_joint" type="revolute"> <joint name="FR_calf_joint" type="revolute">
<origin rpy="0 0 0" xyz="-0.25 0 0"/> <origin rpy="0 -1.5708 0" xyz="-0.25 0 0"/>
<parent link="FR_thigh"/> <parent link="FR_thigh"/>
<child link="FR_calf"/> <child link="FR_calf"/>
<axis xyz="0 1 0"/> <axis xyz="0 1 0"/>
@ -330,7 +330,7 @@
</link> </link>
<!-- 定义thigh_joint --> <!-- 定义thigh_joint -->
<joint name="FL_thigh_joint" type="revolute"> <joint name="FL_thigh_joint" type="revolute">
<origin rpy="0 -1.5708 0" xyz="0 -0.05 0"/> <origin rpy="0 -0.7854 0" xyz="0 -0.05 0"/>
<parent link="FL_hip"/> <parent link="FL_hip"/>
<child link="FL_thigh"/> <child link="FL_thigh"/>
<axis xyz="0 1 0"/> <axis xyz="0 1 0"/>
@ -358,7 +358,7 @@
</link> </link>
<!-- 定义calf_joint --> <!-- 定义calf_joint -->
<joint name="FL_calf_joint" type="revolute"> <joint name="FL_calf_joint" type="revolute">
<origin rpy="0 0 0" xyz="-0.25 0 0"/> <origin rpy="0 -1.5708 0" xyz="-0.25 0 0"/>
<parent link="FL_thigh"/> <parent link="FL_thigh"/>
<child link="FL_calf"/> <child link="FL_calf"/>
<axis xyz="0 1 0"/> <axis xyz="0 1 0"/>
@ -414,7 +414,7 @@
</link> </link>
<!-- 定义thigh_joint --> <!-- 定义thigh_joint -->
<joint name="RR_thigh_joint" type="revolute"> <joint name="RR_thigh_joint" type="revolute">
<origin rpy="0 -1.5708 0" xyz="0 0.05 0"/> <origin rpy="0 -0.7854 0" xyz="0 0.05 0"/>
<parent link="RR_hip"/> <parent link="RR_hip"/>
<child link="RR_thigh"/> <child link="RR_thigh"/>
<axis xyz="0 1 0"/> <axis xyz="0 1 0"/>
@ -442,7 +442,7 @@
</link> </link>
<!-- 定义calf_joint --> <!-- 定义calf_joint -->
<joint name="RR_calf_joint" type="revolute"> <joint name="RR_calf_joint" type="revolute">
<origin rpy="0 0 0" xyz="-0.25 0 0"/> <origin rpy="0 -1.5708 0" xyz="-0.25 0 0"/>
<parent link="RR_thigh"/> <parent link="RR_thigh"/>
<child link="RR_calf"/> <child link="RR_calf"/>
<axis xyz="0 1 0"/> <axis xyz="0 1 0"/>
@ -498,7 +498,7 @@
</link> </link>
<!-- 定义thigh_joint --> <!-- 定义thigh_joint -->
<joint name="RL_thigh_joint" type="revolute"> <joint name="RL_thigh_joint" type="revolute">
<origin rpy="0 -1.5708 0" xyz="0 -0.05 0"/> <origin rpy="0 -0.7854 0" xyz="0 -0.05 0"/>
<parent link="RL_hip"/> <parent link="RL_hip"/>
<child link="RL_thigh"/> <child link="RL_thigh"/>
<axis xyz="0 1 0"/> <axis xyz="0 1 0"/>
@ -526,7 +526,7 @@
</link> </link>
<!-- 定义calf_joint --> <!-- 定义calf_joint -->
<joint name="RL_calf_joint" type="revolute"> <joint name="RL_calf_joint" type="revolute">
<origin rpy="0 0 0" xyz="-0.25 0 0"/> <origin rpy="0 -1.5708 0" xyz="-0.25 0 0"/>
<parent link="RL_thigh"/> <parent link="RL_thigh"/>
<child link="RL_calf"/> <child link="RL_calf"/>
<axis xyz="0 1 0"/> <axis xyz="0 1 0"/>

2
src/robot_descriptions/qut/mdog_description/xacro/ros2_control.xacro
View File

@ -4,7 +4,7 @@
<ros2_control name="MdogSystem" type="system"> <ros2_control name="MdogSystem" type="system">
<hardware> <hardware>
<plugin>mdog_hardware/mdog_hardware_interface</plugin> <plugin>mdog_hardware/MDogHardwareInterface</plugin>
</hardware> </hardware>
<joint name="FR_hip_joint"> <joint name="FR_hip_joint">