diff --git a/src/rove_tracks/CMakeLists.txt b/src/rove_tracks/CMakeLists.txt deleted file mode 100644 index d045986..0000000 --- a/src/rove_tracks/CMakeLists.txt +++ /dev/null @@ -1,117 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(rove_tracks) - -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) -foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) - find_package(${Dependency} REQUIRED) -endforeach() - -# Add chained_controller library related compile commands -generate_parameter_library(chained_controller_parameters - src/chained_controller.yaml - include/rove_tracks/validate_chained_controller_parameters.hpp -) -add_library( - rove_tracks - SHARED - src/chained_controller.cpp -) -target_include_directories(rove_tracks PUBLIC - "$" - "$") -target_link_libraries(rove_tracks chained_controller_parameters) -ament_target_dependencies(rove_tracks ${THIS_PACKAGE_INCLUDE_DEPENDS}) -target_compile_definitions(rove_tracks PRIVATE "CHAINED_CONTROLLER_BUILDING_DLL") - -pluginlib_export_plugin_description_file( - controller_interface rove_tracks.xml) - -install( - TARGETS - rove_tracks - RUNTIME DESTINATION bin - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib -) - -install( - DIRECTORY include/ - DESTINATION include/${PROJECT_NAME} -) - -install( - DIRECTORY urdf - DESTINATION share/${PROJECT_NAME} -) - -# if(BUILD_TESTING) -if(0) - 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) - find_package(ament_cmake_gmock REQUIRED) - find_package(controller_manager REQUIRED) - find_package(hardware_interface REQUIRED) - find_package(ros2_control_test_assets REQUIRED) - - ament_add_gmock(test_load_chained_controller test/test_load_chained_controller.cpp) - target_include_directories(test_load_chained_controller PRIVATE include) - ament_target_dependencies( - test_load_chained_controller - controller_manager - hardware_interface - ros2_control_test_assets - ) - - add_rostest_with_parameters_gmock(test_chained_controller test/test_chained_controller.cpp ${CMAKE_CURRENT_SOURCE_DIR}/test/chained_controller_params.yaml) - target_include_directories(test_chained_controller PRIVATE include) - target_link_libraries(test_chained_controller chained_controller) - ament_target_dependencies( - test_chained_controller - controller_interface - hardware_interface - ) - - add_rostest_with_parameters_gchained_controllermock(test_chained_controller_preceeding test/test_chained_controller_preceeding.cpp ${CMAKE_CURRENT_SOURCE_DIR}/test/chained_controller_preceeding_params.yaml) - target_include_directories(test_chained_controller_preceeding PRIVATE include) - target_link_libraries(test_chained_controller_preceeding chained_controller) - ament_target_dependencies( - test_chained_controller_preceeding - controller_interface - hardware_interface - ) -endif() - -ament_export_include_directories( - include -) -ament_export_dependencies( - ${THIS_PACKAGE_INCLUDE_DEPENDS} -) -ament_export_libraries( - rove_tracks -) - -ament_package() diff --git a/src/rove_tracks/LICENSE b/src/rove_tracks/LICENSE deleted file mode 100644 index 30e8e2e..0000000 --- a/src/rove_tracks/LICENSE +++ /dev/null @@ -1,17 +0,0 @@ -Permission is hereby granted, free of charge, to any person obtaining a copy -of this software and associated documentation files (the "Software"), to deal -in the Software without restriction, including without limitation the rights -to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -copies of the Software, and to permit persons to whom the Software is -furnished to do so, subject to the following conditions: - -The above copyright notice and this permission notice shall be included in -all copies or substantial portions of the Software. - -THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -THE SOFTWARE. diff --git a/src/rove_tracks/include/rove_tracks/chained_controller.hpp b/src/rove_tracks/include/rove_tracks/chained_controller.hpp deleted file mode 100644 index 71b5a00..0000000 --- a/src/rove_tracks/include/rove_tracks/chained_controller.hpp +++ /dev/null @@ -1,146 +0,0 @@ -// Copyright (c) 2024, capra -// 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. - -#ifndef ROVE_TRACKS__CHAINED_CONTROLLER_HPP_ -#define ROVE_TRACKS__CHAINED_CONTROLLER_HPP_ - -#include -#include -#include - -#include "controller_interface/chainable_controller_interface.hpp" -#include "chained_controller_parameters.hpp" -#include "rove_tracks/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 rove_tracks -{ - -enum track_cmd_id : int -{ - TRK_CMD_FRONT_VELOCITY = 0, - TRK_CMD_BACK_VELOCITY, - TRK_CMD_FRONT_EFFORT, - TRK_CMD_BACK_EFFORT, - TRK_CMD_COUNT, - TRK_CMD_TRACK_VELOCITY = TRK_CMD_COUNT, - TRK_CMD_TOTAL_COUNT, - TRK_CMD_NONE = -1, -}; - -enum track_state_id : int -{ - TRK_ST_FRONT_POSITION = 0, - TRK_ST_BACK_POSITION, - TRK_ST_FRONT_VELOCITY, - TRK_ST_BACK_VELOCITY, - TRK_ST_FRONT_EFFORT, - TRK_ST_BACK_EFFORT, - TRK_ST_COUNT, - TRK_ST_TRACK_VELOCITY = TRK_ST_COUNT, - TRK_ST_TOTAL_COUNT, - TRK_ST_NONE = -1, -}; - -// name constants for command interfaces -static constexpr size_t CMD_MY_ITFS = TRK_CMD_COUNT; - -// name constants for state interfaces -static constexpr size_t STATE_MY_ITFS = TRK_ST_COUNT; - -class ChainedController : public controller_interface::ChainableControllerInterface -{ -public: - ROVE_TRACKS__VISIBILITY_PUBLIC - ChainedController(); - - ROVE_TRACKS__VISIBILITY_PUBLIC - controller_interface::CallbackReturn on_init() override; - - ROVE_TRACKS__VISIBILITY_PUBLIC - controller_interface::InterfaceConfiguration command_interface_configuration() const override; - - ROVE_TRACKS__VISIBILITY_PUBLIC - controller_interface::InterfaceConfiguration state_interface_configuration() const override; - - ROVE_TRACKS__VISIBILITY_PUBLIC - controller_interface::CallbackReturn on_configure( - const rclcpp_lifecycle::State & previous_state) override; - - ROVE_TRACKS__VISIBILITY_PUBLIC - controller_interface::CallbackReturn on_activate( - const rclcpp_lifecycle::State & previous_state) override; - - ROVE_TRACKS__VISIBILITY_PUBLIC - controller_interface::CallbackReturn on_deactivate( - const rclcpp_lifecycle::State & previous_state) override; - - ROVE_TRACKS__VISIBILITY_PUBLIC - controller_interface::return_type update_reference_from_subscribers() override; - - ROVE_TRACKS__VISIBILITY_PUBLIC - controller_interface::return_type update_and_write_commands( - const rclcpp::Time & time, const rclcpp::Duration & period) override; - - // TODO(anyone): replace the state and command message types - using ControllerReferenceMsg = control_msgs::msg::JointJog; - using ControllerModeSrvType = std_srvs::srv::SetBool; - using ControllerStateMsg = control_msgs::msg::JointControllerState; - -protected: - std::shared_ptr param_listener_; - rove_tracks::Params params_; - - std::string front_joint_; - std::string back_joint_; - std::string track_joint_; - int direction_ = 0; - int coasting_direction_ = 0; - const double inrush_torque_ = 0; - bool caught_up_ = false; - - // Command subscribers and Controller State publisher - rclcpp::Subscription::SharedPtr ref_subscriber_ = nullptr; - realtime_tools::RealtimeBuffer> input_ref_; - - using ControllerStatePublisher = realtime_tools::RealtimePublisher; - - rclcpp::Publisher::SharedPtr s_publisher_; - std::unique_ptr state_publisher_; - - // override methods from ChainableControllerInterface - std::vector on_export_reference_interfaces() override; - - bool on_set_chained_mode(bool chained_mode) override; - -private: - // callback for topic interface - ROVE_TRACKS__VISIBILITY_LOCAL - void reference_callback(const std::shared_ptr msg); - - rclcpp::Logger logger() const; -}; - -} // namespace rove_tracks - -#endif // ROVE_TRACKS__CHAINED_CONTROLLER_HPP_ diff --git a/src/rove_tracks/include/rove_tracks/validate_chained_controller_parameters.hpp b/src/rove_tracks/include/rove_tracks/validate_chained_controller_parameters.hpp deleted file mode 100644 index 10a8db2..0000000 --- a/src/rove_tracks/include/rove_tracks/validate_chained_controller_parameters.hpp +++ /dev/null @@ -1,40 +0,0 @@ -// Copyright (c) 2024, capra -// 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. - -#ifndef ROVE_TRACKS__VALIDATE_CHAINED_CONTROLLER_PARAMETERS_HPP_ -#define ROVE_TRACKS__VALIDATE_CHAINED_CONTROLLER_PARAMETERS_HPP_ - -#include - -#include -#include "parameter_traits/parameter_traits.hpp" - -namespace parameter_traits -{ -Result forbidden_interface_name_prefix(rclcpp::Parameter const & parameter) -{ - auto const & interface_name = parameter.as_string(); - - if (interface_name.rfind("blup_", 0) == 0) - { - return ERROR("'interface_name' parameter can not start with 'blup_'"); - } - - return OK; -} - -} // namespace parameter_traits - -#endif // ROVE_TRACKS__VALIDATE_CHAINED_CONTROLLER_PARAMETERS_HPP_ diff --git a/src/rove_tracks/include/rove_tracks/visibility_control.h b/src/rove_tracks/include/rove_tracks/visibility_control.h deleted file mode 100644 index 2c5668f..0000000 --- a/src/rove_tracks/include/rove_tracks/visibility_control.h +++ /dev/null @@ -1,50 +0,0 @@ -// Copyright (c) 2024, capra -// 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. - -#ifndef ROVE_TRACKS__VISIBILITY_CONTROL_H_ -#define ROVE_TRACKS__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 ROVE_TRACKS__VISIBILITY_EXPORT __attribute__((dllexport)) -#define ROVE_TRACKS__VISIBILITY_IMPORT __attribute__((dllimport)) -#else -#define ROVE_TRACKS__VISIBILITY_EXPORT __declspec(dllexport) -#define ROVE_TRACKS__VISIBILITY_IMPORT __declspec(dllimport) -#endif -#ifdef ROVE_TRACKS__VISIBILITY_BUILDING_DLL -#define ROVE_TRACKS__VISIBILITY_PUBLIC ROVE_TRACKS__VISIBILITY_EXPORT -#else -#define ROVE_TRACKS__VISIBILITY_PUBLIC ROVE_TRACKS__VISIBILITY_IMPORT -#endif -#define ROVE_TRACKS__VISIBILITY_PUBLIC_TYPE ROVE_TRACKS__VISIBILITY_PUBLIC -#define ROVE_TRACKS__VISIBILITY_LOCAL -#else -#define ROVE_TRACKS__VISIBILITY_EXPORT __attribute__((visibility("default"))) -#define ROVE_TRACKS__VISIBILITY_IMPORT -#if __GNUC__ >= 4 -#define ROVE_TRACKS__VISIBILITY_PUBLIC __attribute__((visibility("default"))) -#define ROVE_TRACKS__VISIBILITY_LOCAL __attribute__((visibility("hidden"))) -#else -#define ROVE_TRACKS__VISIBILITY_PUBLIC -#define ROVE_TRACKS__VISIBILITY_LOCAL -#endif -#define ROVE_TRACKS__VISIBILITY_PUBLIC_TYPE -#endif - -#endif // ROVE_TRACKS__VISIBILITY_CONTROL_H_ diff --git a/src/rove_tracks/package.xml b/src/rove_tracks/package.xml deleted file mode 100644 index f428394..0000000 --- a/src/rove_tracks/package.xml +++ /dev/null @@ -1,39 +0,0 @@ - - - - rove_tracks - 0.0.0 - Controller chain to allow diffdrive to control a single track with two motors - capra - MIT - - ament_cmake - - generate_parameter_library - - control_msgs - - controller_interface - - hardware_interface - - pluginlib - - rclcpp - - rclcpp_lifecycle - - realtime_tools - - std_srvs - - ament_lint_auto - ament_cmake_gmock - controller_manager - hardware_interface - ros2_control_test_assets - - - ament_cmake - - diff --git a/src/rove_tracks/rove_tracks.xml b/src/rove_tracks/rove_tracks.xml deleted file mode 100644 index 0c9446d..0000000 --- a/src/rove_tracks/rove_tracks.xml +++ /dev/null @@ -1,8 +0,0 @@ - - - - ChainedController ros2_control controller. - - - diff --git a/src/rove_tracks/src/chained_controller.cpp b/src/rove_tracks/src/chained_controller.cpp deleted file mode 100644 index 4597465..0000000 --- a/src/rove_tracks/src/chained_controller.cpp +++ /dev/null @@ -1,354 +0,0 @@ -// Copyright (c) 2024, capra -// 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. - -#include "rove_tracks/chained_controller.hpp" - -#include -#include -#include -#include - -#include -#include "controller_interface/helpers.hpp" - -namespace -{ // utility - -// TODO(destogl): remove this when merged upstream -// Changed services history QoS to keep all so we don't lose any client service calls -static constexpr rmw_qos_profile_t rmw_qos_profile_services_hist_keep_all = { - RMW_QOS_POLICY_HISTORY_KEEP_ALL, - 1, // message queue depth - RMW_QOS_POLICY_RELIABILITY_RELIABLE, - RMW_QOS_POLICY_DURABILITY_VOLATILE, - RMW_QOS_DEADLINE_DEFAULT, - RMW_QOS_LIFESPAN_DEFAULT, - RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT, - RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT, - false}; - -using ControllerReferenceMsg = rove_tracks::ChainedController::ControllerReferenceMsg; - -// called from RT control loop -void reset_controller_reference_msg( - const std::shared_ptr & msg, const std::vector & joint_names) -{ - msg->joint_names = joint_names; - msg->displacements.resize(joint_names.size(), std::numeric_limits::quiet_NaN()); - msg->velocities.resize(joint_names.size(), std::numeric_limits::quiet_NaN()); - msg->duration = std::numeric_limits::quiet_NaN(); -} - -} // namespace - -namespace rove_tracks -{ -ChainedController::ChainedController() : controller_interface::ChainableControllerInterface() {} - -controller_interface::CallbackReturn ChainedController::on_init() -{ - try - { - param_listener_ = std::make_shared(get_node()); - } - catch (const std::exception & e) - { - - fprintf(stderr, "Exception thrown during controller's init with message: %s \n", e.what()); - return controller_interface::CallbackReturn::ERROR; - } - - return controller_interface::CallbackReturn::SUCCESS; -} - -controller_interface::CallbackReturn ChainedController::on_configure( - const rclcpp_lifecycle::State & /*previous_state*/) -{ - params_ = param_listener_->get_params(); - - front_joint_ = params_.front_joint; - back_joint_ = params_.back_joint; - track_joint_ = params_.track_joint_name; - - // // topics QoS - auto subscribers_qos = rclcpp::SystemDefaultsQoS(); - subscribers_qos.keep_last(1); - subscribers_qos.best_effort(); - - // // Reference Subscriber - ref_subscriber_ = get_node()->create_subscription( - "~/reference", subscribers_qos, - std::bind(&ChainedController::reference_callback, this, std::placeholders::_1)); - - std::shared_ptr msg = std::make_shared(); - reset_controller_reference_msg(msg, {params_.track_joint_name}); - input_ref_.writeFromNonRT(msg); - - try - { - // State publisher - s_publisher_ = - get_node()->create_publisher("~/state", rclcpp::SystemDefaultsQoS()); - state_publisher_ = std::make_unique(s_publisher_); - } - catch (const std::exception & e) - { - fprintf( - stderr, "Exception thrown during publisher creation at configure stage with message : %s \n", - e.what()); - return controller_interface::CallbackReturn::ERROR; - } - - // TODO(anyone): Reserve memory in state publisher depending on the message type - state_publisher_->lock(); - state_publisher_->msg_.header.frame_id = params_.track_joint_name; - state_publisher_->unlock(); - - RCLCPP_INFO(get_node()->get_logger(), "configure successful"); - return controller_interface::CallbackReturn::SUCCESS; -} - -controller_interface::InterfaceConfiguration ChainedController::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(TRK_CMD_TOTAL_COUNT); - command_interfaces_config.names.reserve(TRK_CMD_COUNT); - // command_interfaces_config.names.push_back(front_joint_ + "/" + hardware_interface::HW_IF_POSITION); - // command_interfaces_config.names.push_back(back_joint_ + "/" + hardware_interface::HW_IF_POSITION); - command_interfaces_config.names.push_back(front_joint_ + "/" + hardware_interface::HW_IF_VELOCITY); - command_interfaces_config.names.push_back(back_joint_ + "/" + hardware_interface::HW_IF_VELOCITY); - command_interfaces_config.names.push_back(front_joint_ + "/" + hardware_interface::HW_IF_EFFORT); - command_interfaces_config.names.push_back(back_joint_ + "/" + hardware_interface::HW_IF_EFFORT); - // command_interfaces_config.names.push_back(track_joint_ + "/" + hardware_interface::HW_IF_VELOCITY); - // command_interfaces_config.names.push_back(track_joint_ + "/" + hardware_interface::HW_IF_VELOCITY); - - return command_interfaces_config; -} - -controller_interface::InterfaceConfiguration ChainedController::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(TRK_ST_TOTAL_COUNT); - state_interfaces_config.names.reserve(TRK_ST_COUNT); - state_interfaces_config.names.push_back(front_joint_ + "/" + hardware_interface::HW_IF_POSITION); - state_interfaces_config.names.push_back(back_joint_ + "/" + hardware_interface::HW_IF_POSITION); - state_interfaces_config.names.push_back(front_joint_ + "/" + hardware_interface::HW_IF_VELOCITY); - state_interfaces_config.names.push_back(back_joint_ + "/" + hardware_interface::HW_IF_VELOCITY); - state_interfaces_config.names.push_back(front_joint_ + "/" + hardware_interface::HW_IF_EFFORT); - state_interfaces_config.names.push_back(back_joint_ + "/" + hardware_interface::HW_IF_EFFORT); - // state_interfaces_config.names.push_back(track_joint_ + "/" + hardware_interface::HW_IF_VELOCITY); - // for (const auto & joint : state_joints_) - // { - // state_interfaces_config.names.push_back(joint + "/" + params_.interface_name); - // } - - return state_interfaces_config; -} - -void ChainedController::reference_callback(const std::shared_ptr msg) -{ - if (msg->joint_names.size() == 1) - { - input_ref_.writeFromNonRT(msg); - } - else - { - RCLCPP_ERROR( - logger(), - "Received %zu , but expected %zu joints in command. Ignoring message.", - msg->joint_names.size(), 1); - } -} - - -rclcpp::Logger ChainedController::logger() const -{ - return get_node()->get_logger(); -} - -std::vector ChainedController::on_export_reference_interfaces() -{ - // reference_interfaces_.resize(state_joints_.size(), std::numeric_limits::quiet_NaN()); - reference_interfaces_.resize(TRK_CMD_TOTAL_COUNT - TRK_CMD_COUNT, std::numeric_limits::quiet_NaN()); - - - std::vector reference_interfaces; - reference_interfaces.reserve(reference_interfaces_.size()); - RCLCPP_INFO(logger(), "Interface count: %d", reference_interfaces_.size()); - - reference_interfaces.push_back(hardware_interface::CommandInterface(get_node()->get_name(), track_joint_ + "/" + hardware_interface::HW_IF_VELOCITY, - &reference_interfaces_[0])); - RCLCPP_INFO(logger(), "Interface exported: %d", reference_interfaces.size()); - - // for (size_t i = 0; i < reference_interfaces_.size(); ++i) - // { - // reference_interfaces.push_back(hardware_interface::CommandInterface( - // get_node()->get_name(), state_joints_[i] + "/" + params_.interface_name, - // &reference_interfaces_[i])); - // } - - return reference_interfaces; -} - -bool ChainedController::on_set_chained_mode(bool chained_mode) -{ - RCLCPP_INFO(logger(), "Set chained mode to: %d", chained_mode); - // Always accept switch to/from chained mode - return true || chained_mode; -} - -controller_interface::CallbackReturn ChainedController::on_activate( - const rclcpp_lifecycle::State & /*previous_state*/) -{ - // Set default value in command - reset_controller_reference_msg(*(input_ref_.readFromRT()), {track_joint_}); - - return controller_interface::CallbackReturn::SUCCESS; -} - -controller_interface::CallbackReturn ChainedController::on_deactivate( - const rclcpp_lifecycle::State & /*previous_state*/) -{ - // TODO(anyone): depending on number of interfaces, use definitions, e.g., `CMD_MY_ITFS`, - // instead of a loop - for (size_t i = 0; i < command_interfaces_.size(); ++i) - { - command_interfaces_[i].set_value(std::numeric_limits::quiet_NaN()); - } - return controller_interface::CallbackReturn::SUCCESS; -} - -controller_interface::return_type ChainedController::update_reference_from_subscribers() -{ - 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 < reference_interfaces_.size(); ++i) - { - if (!std::isnan((*current_ref)->displacements[i])) - { - reference_interfaces_[i] = (*current_ref)->displacements[i]; - - (*current_ref)->displacements[i] = std::numeric_limits::quiet_NaN(); - } - } - return controller_interface::return_type::OK; -} - -controller_interface::return_type ChainedController::update_and_write_commands( - const rclcpp::Time & time, const rclcpp::Duration & /*period*/) -{ - static rclcpp::Clock clk{}; - if (!std::isnan(reference_interfaces_[0])) - { - RCLCPP_INFO_STREAM_THROTTLE(logger(), clk, 1000, "Ref value: " << reference_interfaces_[0]); - direction_ = reference_interfaces_[0] == 0 ? 0 : ( - reference_interfaces_[0] < 0 ? -1 : 1 - ); - } - - track_cmd_id leading_cmd = track_cmd_id::TRK_CMD_NONE; - track_cmd_id trailing_cmd = track_cmd_id::TRK_CMD_NONE; - track_state_id leading_st_velocity = track_state_id::TRK_ST_NONE; - track_state_id leading_st_effort = track_state_id::TRK_ST_NONE; - - switch (direction_) - { - case -1: - leading_cmd = track_cmd_id::TRK_CMD_BACK_VELOCITY; - trailing_cmd = track_cmd_id::TRK_CMD_FRONT_EFFORT; - leading_st_velocity = track_state_id::TRK_ST_BACK_VELOCITY; - leading_st_effort = track_state_id::TRK_ST_BACK_EFFORT; - break; - case 0: - if (coasting_direction_ < 0) - { - leading_cmd = track_cmd_id::TRK_CMD_BACK_VELOCITY; - trailing_cmd = track_cmd_id::TRK_CMD_FRONT_EFFORT; - leading_st_velocity = track_state_id::TRK_ST_BACK_VELOCITY; - leading_st_effort = track_state_id::TRK_ST_BACK_EFFORT; - } - if (coasting_direction_ > 0) - { - leading_cmd = track_cmd_id::TRK_CMD_FRONT_VELOCITY; - trailing_cmd = track_cmd_id::TRK_CMD_BACK_EFFORT; - leading_st_velocity = track_state_id::TRK_ST_FRONT_VELOCITY; - leading_st_effort = track_state_id::TRK_ST_FRONT_EFFORT; - } - break; - case 1: - leading_cmd = track_cmd_id::TRK_CMD_FRONT_VELOCITY; - trailing_cmd = track_cmd_id::TRK_CMD_BACK_EFFORT; - leading_st_velocity = track_state_id::TRK_ST_FRONT_VELOCITY; - leading_st_effort = track_state_id::TRK_ST_FRONT_EFFORT; - break; - default: - leading_cmd = track_cmd_id::TRK_CMD_NONE; - trailing_cmd = track_cmd_id::TRK_CMD_NONE; - leading_st_velocity = track_state_id::TRK_ST_NONE; - leading_st_effort = track_state_id::TRK_ST_NONE; - break; - } - - if (!std::isnan(reference_interfaces_[0])) - { - command_interfaces_[leading_cmd].set_value(reference_interfaces_[0]); - reference_interfaces_[0] = std::numeric_limits::quiet_NaN(); - } - - double leading_vel = 0; - double leading_torque = 0; - if (leading_st_velocity != track_state_id::TRK_ST_NONE) - { - leading_vel = state_interfaces_[leading_st_velocity].get_value(); - coasting_direction_ = /*TODO close to*/leading_vel == 0 ? 0 : ( - leading_vel > 0 ? 1 : -1 - ); - } - - if (leading_st_effort != track_state_id::TRK_ST_NONE) - { - leading_torque = state_interfaces_[leading_st_effort].get_value(); - } - - if (coasting_direction_ < direction_ || coasting_direction_ > direction_) caught_up_ = false; - if (abs(leading_torque) > inrush_torque_) caught_up_ = true; - - double torque = caught_up_ ? leading_torque : (std::min(inrush_torque_, abs(leading_torque)) * coasting_direction_); - - if (coasting_direction_ != 0) - { - command_interfaces_[trailing_cmd].set_value(torque); - } - RCLCPP_INFO_STREAM_THROTTLE(logger(), clk, 1000, "Ref value: " << reference_interfaces_[0] << - " Direction: " << direction_ << - " Leading vel: " << leading_vel << - " Leading torque: " << leading_torque << - " Coasting direction: " << coasting_direction_); - - return controller_interface::return_type::OK; -} - -} // namespace rove_tracks - -#include "pluginlib/class_list_macros.hpp" - -PLUGINLIB_EXPORT_CLASS( - rove_tracks::ChainedController, controller_interface::ChainableControllerInterface) diff --git a/src/rove_tracks/src/chained_controller.yaml b/src/rove_tracks/src/chained_controller.yaml deleted file mode 100644 index f58782a..0000000 --- a/src/rove_tracks/src/chained_controller.yaml +++ /dev/null @@ -1,30 +0,0 @@ -rove_tracks: - track_joint_name: { - type: string, - default_value: "", - description: "The name of the track joint.", - read_only: true, - # validation: { - # not_empty<>: null, - # } - } - front_joint: { - type: string, - default_value: "", - description: "Specifies the joint at the front of the track.", - read_only: true, - # validation: { - # not_empty<>: null, - # # unique<>: null, - # } - } - back_joint: { - type: string, - default_value: "", - description: "Specifies the joint at the back of the track.", - read_only: true, - # validation: { - # not_empty<>: null, - # # unique<>: null, - # } - } diff --git a/src/rove_tracks/test/chained_controller_params.yaml b/src/rove_tracks/test/chained_controller_params.yaml deleted file mode 100644 index 91fc7d6..0000000 --- a/src/rove_tracks/test/chained_controller_params.yaml +++ /dev/null @@ -1,6 +0,0 @@ -test_chained_controller: - ros__parameters: - track_joint_name: joint1 - front_joint: joint2 - back_joint: joint3 - diff --git a/src/rove_tracks/test/chained_controller_preceeding_params.yaml b/src/rove_tracks/test/chained_controller_preceeding_params.yaml deleted file mode 100644 index dc08f7a..0000000 --- a/src/rove_tracks/test/chained_controller_preceeding_params.yaml +++ /dev/null @@ -1,9 +0,0 @@ -test_chained_controller: - ros__parameters: - joints: - - joint1 - - interface_name: acceleration - - state_joints: - - joint1state diff --git a/src/rove_tracks/test/test_chained_controller.cpp b/src/rove_tracks/test/test_chained_controller.cpp deleted file mode 100644 index f3cb7f5..0000000 --- a/src/rove_tracks/test/test_chained_controller.cpp +++ /dev/null @@ -1,386 +0,0 @@ -// Copyright (c) 2024, capra -// 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. - -#include "test_chained_controller.hpp" - -#include -#include -#include -#include -#include - -using rove_tracks::CMD_MY_ITFS; -using rove_tracks::STATE_MY_ITFS; - -class ChainedControllerTest : public ChainedControllerFixture -{ -}; - -TEST_F(ChainedControllerTest, all_parameters_set_configure_success) -{ - SetUpController(); - - ASSERT_TRUE(controller_->params_.front_joint == ""); - ASSERT_TRUE(controller_->params_.back_joint == ""); - ASSERT_TRUE(controller_->params_.track_joint_name == ""); - - 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(ChainedControllerTest, 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_); - } - - // check ref itfs - auto reference_interfaces = controller_->export_reference_interfaces(); - ASSERT_EQ(reference_interfaces.size(), joint_names_.size()); - for (size_t i = 0; i < joint_names_.size(); ++i) - { - const std::string ref_itf_name = std::string(controller_->get_node()->get_name()) + "/" + - joint_names_[i] + "/" + interface_name_; - EXPECT_EQ(reference_interfaces[i].get_name(), ref_itf_name); - EXPECT_EQ(reference_interfaces[i].get_prefix_name(), controller_->get_node()->get_name()); - EXPECT_EQ( - reference_interfaces[i].get_interface_name(), joint_names_[i] + "/" + interface_name_); - } -} - -TEST_F(ChainedControllerTest, 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)); - - EXPECT_EQ(controller_->reference_interfaces_.size(), joint_names_.size()); - for (const auto & interface : controller_->reference_interfaces_) - { - EXPECT_TRUE(std::isnan(interface)); - } -} - -TEST_F(ChainedControllerTest, 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(ChainedControllerTest, 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(ChainedControllerTest, 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(ChainedControllerTest, 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(ChainedControllerTest, 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); - controller_->set_chained_mode(false); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_FALSE(controller_->is_in_chained_mode()); - - // set command statically - static constexpr double TEST_DISPLACEMENT = 23.24; - std::shared_ptr msg = std::make_shared(); - msg->joint_names = joint_names_; - msg->displacements.resize(joint_names_.size(), TEST_DISPLACEMENT); - msg->velocities.resize(joint_names_.size(), std::numeric_limits::quiet_NaN()); - msg->duration = std::numeric_limits::quiet_NaN(); - controller_->input_ref_.writeFromNonRT(msg); - - ASSERT_EQ( - controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - - EXPECT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::FAST); - EXPECT_EQ(joint_command_values_[STATE_MY_ITFS], TEST_DISPLACEMENT); - EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->displacements[0])); - EXPECT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::FAST); - EXPECT_EQ(controller_->reference_interfaces_.size(), joint_names_.size()); - for (const auto & interface : controller_->reference_interfaces_) - { - EXPECT_TRUE(std::isnan(interface)); - } -} - -TEST_F(ChainedControllerTest, 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); - controller_->set_chained_mode(false); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_FALSE(controller_->is_in_chained_mode()); - - // set command statically - static constexpr double TEST_DISPLACEMENT = 23.24; - std::shared_ptr msg = std::make_shared(); - // When slow mode is enabled command ends up being half of the value - msg->joint_names = joint_names_; - msg->displacements.resize(joint_names_.size(), TEST_DISPLACEMENT); - msg->velocities.resize(joint_names_.size(), std::numeric_limits::quiet_NaN()); - msg->duration = std::numeric_limits::quiet_NaN(); - controller_->input_ref_.writeFromNonRT(msg); - controller_->control_mode_.writeFromNonRT(control_mode_type::SLOW); - - EXPECT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::SLOW); - ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT); - ASSERT_EQ( - controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - - EXPECT_EQ(joint_command_values_[STATE_MY_ITFS], TEST_DISPLACEMENT / 2); - EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->displacements[0])); - EXPECT_EQ(controller_->reference_interfaces_.size(), joint_names_.size()); - for (const auto & interface : controller_->reference_interfaces_) - { - EXPECT_TRUE(std::isnan(interface)); - } -} - -TEST_F(ChainedControllerTest, test_update_logic_chainable_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); - controller_->set_chained_mode(true); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_TRUE(controller_->is_in_chained_mode()); - - // set command statically - static constexpr double TEST_DISPLACEMENT = 23.24; - std::shared_ptr msg = std::make_shared(); - msg->joint_names = joint_names_; - msg->displacements.resize(joint_names_.size(), TEST_DISPLACEMENT); - msg->velocities.resize(joint_names_.size(), std::numeric_limits::quiet_NaN()); - msg->duration = std::numeric_limits::quiet_NaN(); - controller_->input_ref_.writeFromNonRT(msg); - // this is input source in chained mode - controller_->reference_interfaces_[STATE_MY_ITFS] = TEST_DISPLACEMENT * 2; - - 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); - // reference_interfaces is directly applied - EXPECT_EQ(joint_command_values_[STATE_MY_ITFS], TEST_DISPLACEMENT * 2); - // message is not touched in chained mode - EXPECT_EQ((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT); - EXPECT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::FAST); - EXPECT_EQ(controller_->reference_interfaces_.size(), joint_names_.size()); - for (const auto & interface : controller_->reference_interfaces_) - { - EXPECT_TRUE(std::isnan(interface)); - } -} - -TEST_F(ChainedControllerTest, test_update_logic_chainable_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); - controller_->set_chained_mode(true); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_TRUE(controller_->is_in_chained_mode()); - - // set command statically - static constexpr double TEST_DISPLACEMENT = 23.24; - std::shared_ptr msg = std::make_shared(); - // When slow mode is enabled command ends up being half of the value - msg->joint_names = joint_names_; - msg->displacements.resize(joint_names_.size(), TEST_DISPLACEMENT); - msg->velocities.resize(joint_names_.size(), std::numeric_limits::quiet_NaN()); - msg->duration = std::numeric_limits::quiet_NaN(); - controller_->input_ref_.writeFromNonRT(msg); - controller_->control_mode_.writeFromNonRT(control_mode_type::SLOW); - // this is input source in chained mode - controller_->reference_interfaces_[STATE_MY_ITFS] = TEST_DISPLACEMENT * 4; - - 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); - - // reference_interfaces is directly applied - EXPECT_EQ(joint_command_values_[STATE_MY_ITFS], TEST_DISPLACEMENT * 2); - // message is not touched in chained mode - EXPECT_EQ((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT); - EXPECT_EQ(controller_->reference_interfaces_.size(), joint_names_.size()); - for (const auto & interface : controller_->reference_interfaces_) - { - EXPECT_TRUE(std::isnan(interface)); - } -} - -TEST_F(ChainedControllerTest, 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(ChainedControllerTest, receive_message_and_publish_updated_status) -{ - SetUpController(); - rclcpp::executors::MultiThreadedExecutor executor; - executor.add_node(controller_->get_node()->get_node_base_interface()); - - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - - ASSERT_EQ( - controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - - ControllerStateMsg msg; - subscribe_and_get_messages(msg); - - ASSERT_EQ(msg.set_point, 101.101); - - publish_commands(); - ASSERT_TRUE(controller_->wait_for_commands(executor)); - - ASSERT_EQ( - controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - - EXPECT_EQ(joint_command_values_[CMD_MY_ITFS], 0.45); - - subscribe_and_get_messages(msg); - - ASSERT_EQ(msg.set_point, 0.45); -} - -int main(int argc, char ** argv) -{ - ::testing::InitGoogleTest(&argc, argv); - rclcpp::init(argc, argv); - int result = RUN_ALL_TESTS(); - rclcpp::shutdown(); - return result; -} diff --git a/src/rove_tracks/test/test_chained_controller.hpp b/src/rove_tracks/test/test_chained_controller.hpp deleted file mode 100644 index b715f41..0000000 --- a/src/rove_tracks/test/test_chained_controller.hpp +++ /dev/null @@ -1,277 +0,0 @@ -// Copyright (c) 2024, capra -// 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. - -#ifndef TEMPLATES__ROS2_CONTROL__CONTROLLER__TEST_CHAINED_CONTROLLER_HPP_ -#define TEMPLATES__ROS2_CONTROL__CONTROLLER__TEST_CHAINED_CONTROLLER_HPP_ - -#include -#include -#include -#include -#include -#include -#include - -#include "rove_tracks/chained_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/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 = rove_tracks::ChainedController::ControllerStateMsg; -using ControllerReferenceMsg = rove_tracks::ChainedController::ControllerReferenceMsg; -using ControllerModeSrvType = rove_tracks::ChainedController::ControllerModeSrvType; - -namespace -{ -constexpr auto NODE_SUCCESS = controller_interface::CallbackReturn::SUCCESS; -constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR; -} // namespace -// namespace - -// subclassing and friending so we can access member variables -class TestableChainedController : public rove_tracks::ChainedController -{ - FRIEND_TEST(ChainedControllerTest, all_parameters_set_configure_success); - FRIEND_TEST(ChainedControllerTest, activate_success); - FRIEND_TEST(ChainedControllerTest, reactivate_success); - FRIEND_TEST(ChainedControllerTest, test_setting_slow_mode_service); - FRIEND_TEST(ChainedControllerTest, test_update_logic_fast); - FRIEND_TEST(ChainedControllerTest, test_update_logic_slow); - FRIEND_TEST(ChainedControllerTest, test_update_logic_chainable_fast); - FRIEND_TEST(ChainedControllerTest, test_update_logic_chainable_slow); - -public: - controller_interface::CallbackReturn on_configure( - const rclcpp_lifecycle::State & previous_state) override - { - auto ret = rove_tracks::ChainedController::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; - } - - controller_interface::CallbackReturn on_activate( - const rclcpp_lifecycle::State & previous_state) override - { - auto ref_itfs = on_export_reference_interfaces(); - return rove_tracks::ChainedController::on_activate(previous_state); - } - - /** - * @brief wait_for_command blocks until a new ControllerReferenceMsg is received. - * Requires that the executor is not spinned elsewhere between the - * message publication and the call to this function. - * - * @return true if new ControllerReferenceMsg msg was received, false if timeout. - */ - bool wait_for_command( - rclcpp::Executor & executor, rclcpp::WaitSet & subscriber_wait_set, - const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) - { - bool success = subscriber_wait_set.wait(timeout).kind() == rclcpp::WaitResultKind::Ready; - if (success) - { - executor.spin_some(); - } - return success; - } - - bool wait_for_commands( - rclcpp::Executor & executor, - const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) - { - return wait_for_command(executor, ref_subscriber_wait_set_, timeout); - } - - // TODO(anyone): add implementation of any methods of your controller is needed - -private: - rclcpp::WaitSet ref_subscriber_wait_set_; -}; - -// We are using template class here for easier reuse of Fixture in specializations of controllers -template -class ChainedControllerFixture : public ::testing::Test -{ -public: - static void SetUpTestCase() {} - - void SetUp() - { - // initialize controller - controller_ = std::make_unique(); - - command_publisher_node_ = std::make_shared("command_publisher"); - command_publisher_ = command_publisher_node_->create_publisher( - "/test_chained_controller/commands", rclcpp::SystemDefaultsQoS()); - - service_caller_node_ = std::make_shared("service_caller"); - slow_control_service_client_ = service_caller_node_->create_client( - "/test_chained_controller/set_slow_control_mode"); - } - - static void TearDownTestCase() {} - - void TearDown() { controller_.reset(nullptr); } - -protected: - void SetUpController(const std::string controller_name = "test_chained_controller") - { - ASSERT_EQ(controller_->init(controller_name), controller_interface::return_type::OK); - - std::vector command_ifs; - command_itfs_.reserve(joint_command_values_.size()); - command_ifs.reserve(joint_command_values_.size()); - - for (size_t i = 0; i < joint_command_values_.size(); ++i) - { - command_itfs_.emplace_back(hardware_interface::CommandInterface( - joint_names_[i], interface_name_, &joint_command_values_[i])); - command_ifs.emplace_back(command_itfs_.back()); - } - // TODO(anyone): Add other command interfaces, if any - - std::vector state_ifs; - state_itfs_.reserve(joint_state_values_.size()); - state_ifs.reserve(joint_state_values_.size()); - - for (size_t i = 0; i < joint_state_values_.size(); ++i) - { - state_itfs_.emplace_back(hardware_interface::StateInterface( - joint_names_[i], interface_name_, &joint_state_values_[i])); - state_ifs.emplace_back(state_itfs_.back()); - } - // TODO(anyone): Add other state interfaces, if any - - controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); - } - - void subscribe_and_get_messages(ControllerStateMsg & msg) - { - // create a new subscriber - rclcpp::Node test_subscription_node("test_subscription_node"); - auto subs_callback = [&](const ControllerStateMsg::SharedPtr) {}; - auto subscription = test_subscription_node.create_subscription( - "/test_chained_controller/state", 10, subs_callback); - - // call update to publish the test value - ASSERT_EQ( - controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - - // call update to publish the test value - // since update doesn't guarantee a published message, republish until received - int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop - rclcpp::WaitSet wait_set; // block used to wait on message - wait_set.add_subscription(subscription); - while (max_sub_check_loop_count--) - { - controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); - // check if message has been received - if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) - { - break; - } - } - ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " - "controller/broadcaster update loop"; - - // take message from subscription - rclcpp::MessageInfo msg_info; - ASSERT_TRUE(subscription->take(msg, msg_info)); - } - - // TODO(anyone): add/remove arguments as it suites your command message type - void publish_commands( - const std::vector & displacements = {0.45}, - const std::vector & velocities = {0.0}, const double duration = 1.25) - { - auto wait_for_topic = [&](const auto topic_name) - { - size_t wait_count = 0; - while (command_publisher_node_->count_subscribers(topic_name) == 0) - { - if (wait_count >= 5) - { - auto error_msg = - std::string("publishing to ") + topic_name + " but no node subscribes to it"; - throw std::runtime_error(error_msg); - } - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - ++wait_count; - } - }; - - wait_for_topic(command_publisher_->get_topic_name()); - - ControllerReferenceMsg msg; - msg.joint_names = joint_names_; - msg.displacements = displacements; - msg.velocities = velocities; - msg.duration = duration; - - command_publisher_->publish(msg); - } - - std::shared_ptr call_service( - const bool slow_control, rclcpp::Executor & executor) - { - auto request = std::make_shared(); - request->data = slow_control; - - bool wait_for_service_ret = - slow_control_service_client_->wait_for_service(std::chrono::milliseconds(500)); - EXPECT_TRUE(wait_for_service_ret); - if (!wait_for_service_ret) - { - throw std::runtime_error("Services is not available!"); - } - auto result = slow_control_service_client_->async_send_request(request); - EXPECT_EQ(executor.spin_until_future_complete(result), rclcpp::FutureReturnCode::SUCCESS); - - return result.get(); - } - -protected: - // TODO(anyone): adjust the members as needed - - // Controller-related parameters - std::vector joint_names_ = {"joint1"}; - std::vector state_joint_names_ = {"joint1state"}; - std::string interface_name_ = "acceleration"; - std::array joint_state_values_ = {1.1}; - std::array joint_command_values_ = {101.101}; - - std::vector state_itfs_; - std::vector command_itfs_; - - // Test related parameters - std::unique_ptr controller_; - rclcpp::Node::SharedPtr command_publisher_node_; - rclcpp::Publisher::SharedPtr command_publisher_; - rclcpp::Node::SharedPtr service_caller_node_; - rclcpp::Client::SharedPtr slow_control_service_client_; -}; - -#endif // TEMPLATES__ROS2_CONTROL__CONTROLLER__TEST_CHAINED_CONTROLLER_HPP_ diff --git a/src/rove_tracks/test/test_chained_controller_preceeding.cpp b/src/rove_tracks/test/test_chained_controller_preceeding.cpp deleted file mode 100644 index 225e984..0000000 --- a/src/rove_tracks/test/test_chained_controller_preceeding.cpp +++ /dev/null @@ -1,89 +0,0 @@ -// Copyright (c) 2024, capra -// 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. - -#include "test_chained_controller.hpp" - -#include -#include -#include -#include -#include - -using rove_tracks::CMD_MY_ITFS; -using rove_tracks::control_mode_type; -using rove_tracks::STATE_MY_ITFS; - -class ChainedControllerTest : public ChainedControllerFixture -{ -}; - -TEST_F(ChainedControllerTest, 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(ChainedControllerTest, 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_); - } - - // check ref itfs - auto reference_interfaces = controller_->export_reference_interfaces(); - ASSERT_EQ(reference_interfaces.size(), joint_names_.size()); - for (size_t i = 0; i < joint_names_.size(); ++i) - { - const std::string ref_itf_name = std::string(controller_->get_node()->get_name()) + "/" + - state_joint_names_[i] + "/" + interface_name_; - EXPECT_EQ(reference_interfaces[i].get_name(), ref_itf_name); - EXPECT_EQ(reference_interfaces[i].get_prefix_name(), controller_->get_node()->get_name()); - EXPECT_EQ( - reference_interfaces[i].get_interface_name(), state_joint_names_[i] + "/" + interface_name_); - } -} - -int main(int argc, char ** argv) -{ - ::testing::InitGoogleTest(&argc, argv); - rclcpp::init(argc, argv); - int result = RUN_ALL_TESTS(); - rclcpp::shutdown(); - return result; -} diff --git a/src/rove_tracks/test/test_load_chained_controller.cpp b/src/rove_tracks/test/test_load_chained_controller.cpp deleted file mode 100644 index dd92b23..0000000 --- a/src/rove_tracks/test/test_load_chained_controller.cpp +++ /dev/null @@ -1,42 +0,0 @@ -// Copyright (c) 2024, capra -// 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. - -#include -#include - -#include "controller_manager/controller_manager.hpp" -#include "hardware_interface/resource_manager.hpp" -#include "rclcpp/executor.hpp" -#include "rclcpp/executors/single_threaded_executor.hpp" -#include "rclcpp/utilities.hpp" -#include "ros2_control_test_assets/descriptions.hpp" - -TEST(TestLoadChainedController, load_controller) -{ - rclcpp::init(0, nullptr); - - std::shared_ptr executor = - std::make_shared(); - - controller_manager::ControllerManager cm( - std::make_unique( - ros2_control_test_assets::minimal_robot_urdf), - executor, "test_controller_manager"); - - ASSERT_NO_THROW( - cm.load_controller("test_rove_tracks", "rove_tracks/ChainedController")); - - rclcpp::shutdown(); -} diff --git a/src/rove_tracks/urdf/rove_track.urdf.xacro b/src/rove_tracks/urdf/rove_track.urdf.xacro deleted file mode 100644 index 06da19d..0000000 --- a/src/rove_tracks/urdf/rove_track.urdf.xacro +++ /dev/null @@ -1,37 +0,0 @@ - - - - - - ${nodeid} - - - - - - - - - - - - - - - - - - - - mock_components/GenericSystem - - - - - - - - - - -