能读和控制12个电机
This commit is contained in:
parent
7d2230b092
commit
138be4f159
111
src/controllers/mdog_simple_controller/CMakeLists.txt
Normal file
111
src/controllers/mdog_simple_controller/CMakeLists.txt
Normal file
@ -0,0 +1,111 @@
|
|||||||
|
cmake_minimum_required(VERSION 3.8)
|
||||||
|
project(mdog_simple_controller)
|
||||||
|
|
||||||
|
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
||||||
|
add_compile_options(-Wall -Wextra -Wpedantic)
|
||||||
|
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
|
||||||
|
"$<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(
|
||||||
|
TARGETS
|
||||||
|
mdog_simple_controller
|
||||||
|
RUNTIME DESTINATION bin
|
||||||
|
ARCHIVE DESTINATION lib
|
||||||
|
LIBRARY DESTINATION lib
|
||||||
|
)
|
||||||
|
|
||||||
|
install(
|
||||||
|
DIRECTORY include/
|
||||||
|
DESTINATION include/${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_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()
|
||||||
|
|
||||||
|
ament_export_include_directories(
|
||||||
|
include
|
||||||
|
)
|
||||||
|
ament_export_dependencies(
|
||||||
|
${THIS_PACKAGE_INCLUDE_DEPENDS}
|
||||||
|
)
|
||||||
|
ament_export_libraries(
|
||||||
|
mdog_simple_controller
|
||||||
|
)
|
||||||
|
|
||||||
|
ament_package()
|
||||||
@ -0,0 +1,117 @@
|
|||||||
|
// 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_
|
||||||
@ -0,0 +1,43 @@
|
|||||||
|
// 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_
|
||||||
@ -0,0 +1,54 @@
|
|||||||
|
// 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_
|
||||||
@ -0,0 +1,28 @@
|
|||||||
|
<!--
|
||||||
|
Copyright (c) 2024, Stogl Robotics Consulting UG (haftungsbeschränkt)
|
||||||
|
|
||||||
|
Licensed under the Apache License, Version 2.0 (the "License");
|
||||||
|
you may not use this file except in compliance with the License.
|
||||||
|
You may obtain a copy of the License at
|
||||||
|
|
||||||
|
http://www.apache.org/licenses/LICENSE-2.0
|
||||||
|
|
||||||
|
Unless required by applicable law or agreed to in writing, software
|
||||||
|
distributed under the License is distributed on an "AS IS" BASIS,
|
||||||
|
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||||
|
See the License for the specific language governing permissions and
|
||||||
|
limitations under the License.
|
||||||
|
|
||||||
|
|
||||||
|
Source of this file are templates in
|
||||||
|
[RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository.
|
||||||
|
-->
|
||||||
|
|
||||||
|
<library path="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>
|
||||||
39
src/controllers/mdog_simple_controller/package.xml
Normal file
39
src/controllers/mdog_simple_controller/package.xml
Normal file
@ -0,0 +1,39 @@
|
|||||||
|
<?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_simple_controller</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>
|
||||||
|
|
||||||
|
<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_cmake_gmock</test_depend>
|
||||||
|
<test_depend>controller_manager</test_depend>
|
||||||
|
<test_depend>hardware_interface</test_depend>
|
||||||
|
<test_depend>ros2_control_test_assets</test_depend>
|
||||||
|
|
||||||
|
<export>
|
||||||
|
<build_type>ament_cmake</build_type>
|
||||||
|
</export>
|
||||||
|
</package>
|
||||||
@ -0,0 +1,265 @@
|
|||||||
|
// 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)
|
||||||
@ -0,0 +1,50 @@
|
|||||||
|
# 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
|
||||||
|
}
|
||||||
|
}
|
||||||
@ -0,0 +1,26 @@
|
|||||||
|
# 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
|
||||||
@ -0,0 +1,28 @@
|
|||||||
|
# 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
|
||||||
@ -0,0 +1,44 @@
|
|||||||
|
// 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();
|
||||||
|
}
|
||||||
@ -0,0 +1,277 @@
|
|||||||
|
// 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;
|
||||||
|
}
|
||||||
@ -0,0 +1,274 @@
|
|||||||
|
// 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_
|
||||||
@ -0,0 +1,80 @@
|
|||||||
|
// 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;
|
||||||
|
}
|
||||||
Loading…
Reference in New Issue
Block a user