From e56255a675553c5745ee782682978e2220bfdf03 Mon Sep 17 00:00:00 2001 From: ilithebutterfly Date: Fri, 31 May 2024 14:06:17 -0400 Subject: [PATCH 01/12] Added rove_tracks package --- src/rove_tracks/CMakeLists.txt | 26 ++++++++++++++++++++++++++ src/rove_tracks/LICENSE | 17 +++++++++++++++++ src/rove_tracks/package.xml | 18 ++++++++++++++++++ 3 files changed, 61 insertions(+) create mode 100644 src/rove_tracks/CMakeLists.txt create mode 100644 src/rove_tracks/LICENSE create mode 100644 src/rove_tracks/package.xml diff --git a/src/rove_tracks/CMakeLists.txt b/src/rove_tracks/CMakeLists.txt new file mode 100644 index 0000000..e1670c1 --- /dev/null +++ b/src/rove_tracks/CMakeLists.txt @@ -0,0 +1,26 @@ +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 +find_package(ament_cmake REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/src/rove_tracks/LICENSE b/src/rove_tracks/LICENSE new file mode 100644 index 0000000..30e8e2e --- /dev/null +++ b/src/rove_tracks/LICENSE @@ -0,0 +1,17 @@ +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/package.xml b/src/rove_tracks/package.xml new file mode 100644 index 0000000..0f59c79 --- /dev/null +++ b/src/rove_tracks/package.xml @@ -0,0 +1,18 @@ + + + + rove_tracks + 0.0.0 + Controller chain to allow diffdrive to control a single track with two motors + capra + MIT + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + From 47407d61c5e33186c5f660976bfe21b4371e3e3a Mon Sep 17 00:00:00 2001 From: ilithebutterfly Date: Wed, 5 Jun 2024 14:58:58 -0400 Subject: [PATCH 02/12] Working with hardware --- src/rove_tracks/CMakeLists.txt | 93 ++++- .../rove_tracks/chained_controller.hpp | 121 ++++++ ...validate_chained_controller_parameters.hpp | 39 ++ .../include/rove_tracks/visibility_control.h | 50 +++ src/rove_tracks/package.xml | 23 +- src/rove_tracks/rove_tracks.xml | 8 + src/rove_tracks/src/chained_controller.cpp | 297 ++++++++++++++ src/rove_tracks/src/chained_controller.yaml | 31 ++ .../test/chained_controller_params.yaml | 7 + .../chained_controller_preceeding_params.yaml | 9 + .../test/test_chained_controller.cpp | 387 ++++++++++++++++++ .../test/test_chained_controller.hpp | 277 +++++++++++++ .../test_chained_controller_preceeding.cpp | 89 ++++ .../test/test_load_chained_controller.cpp | 42 ++ 14 files changed, 1468 insertions(+), 5 deletions(-) create mode 100644 src/rove_tracks/include/rove_tracks/chained_controller.hpp create mode 100644 src/rove_tracks/include/rove_tracks/validate_chained_controller_parameters.hpp create mode 100644 src/rove_tracks/include/rove_tracks/visibility_control.h create mode 100644 src/rove_tracks/rove_tracks.xml create mode 100644 src/rove_tracks/src/chained_controller.cpp create mode 100644 src/rove_tracks/src/chained_controller.yaml create mode 100644 src/rove_tracks/test/chained_controller_params.yaml create mode 100644 src/rove_tracks/test/chained_controller_preceeding_params.yaml create mode 100644 src/rove_tracks/test/test_chained_controller.cpp create mode 100644 src/rove_tracks/test/test_chained_controller.hpp create mode 100644 src/rove_tracks/test/test_chained_controller_preceeding.cpp create mode 100644 src/rove_tracks/test/test_load_chained_controller.cpp diff --git a/src/rove_tracks/CMakeLists.txt b/src/rove_tracks/CMakeLists.txt index e1670c1..85993ea 100644 --- a/src/rove_tracks/CMakeLists.txt +++ b/src/rove_tracks/CMakeLists.txt @@ -6,10 +6,55 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") endif() # find dependencies +set(THIS_PACKAGE_INCLUDE_DEPENDS + control_msgs + controller_interface + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + std_srvs +) + find_package(ament_cmake REQUIRED) -# uncomment the following section in order to fill in -# further dependencies manually. -# find_package( 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( + chained_controller + SHARED + src/chained_controller.cpp +) +target_include_directories(chained_controller PUBLIC + "$" + "$") +target_link_libraries(chained_controller chained_controller_parameters) +ament_target_dependencies(chained_controller ${THIS_PACKAGE_INCLUDE_DEPENDS}) +target_compile_definitions(chained_controller PRIVATE "CHAINED_CONTROLLER_BUILDING_DLL") + +pluginlib_export_plugin_description_file( + controller_interface rove_tracks.xml) + +install( + TARGETS + chained_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) @@ -20,7 +65,47 @@ if(BUILD_TESTING) # comment the line when this package is in a git repo and when # a copyright and license is added to all source files set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() + 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_gmock(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( + chained_controller +) + ament_package() diff --git a/src/rove_tracks/include/rove_tracks/chained_controller.hpp b/src/rove_tracks/include/rove_tracks/chained_controller.hpp new file mode 100644 index 0000000..b600803 --- /dev/null +++ b/src/rove_tracks/include/rove_tracks/chained_controller.hpp @@ -0,0 +1,121 @@ +// 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 +{ +// 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 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_; + chained_controller::Params params_; + + std::vector state_joints_; + + // Command subscribers and Controller State publisher + rclcpp::Subscription::SharedPtr ref_subscriber_ = nullptr; + realtime_tools::RealtimeBuffer> input_ref_; + + rclcpp::Service::SharedPtr set_slow_control_mode_service_; + realtime_tools::RealtimeBuffer control_mode_; + + using ControllerStatePublisher = realtime_tools::RealtimePublisher; + + rclcpp::Publisher::SharedPtr s_publisher_; + std::unique_ptr state_publisher_; + + // 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); +}; + +} // 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 new file mode 100644 index 0000000..caf9569 --- /dev/null +++ b/src/rove_tracks/include/rove_tracks/validate_chained_controller_parameters.hpp @@ -0,0 +1,39 @@ +// 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 "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 new file mode 100644 index 0000000..2c5668f --- /dev/null +++ b/src/rove_tracks/include/rove_tracks/visibility_control.h @@ -0,0 +1,50 @@ +// 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 index 0f59c79..f428394 100644 --- a/src/rove_tracks/package.xml +++ b/src/rove_tracks/package.xml @@ -9,8 +9,29 @@ ament_cmake + generate_parameter_library + + control_msgs + + controller_interface + + hardware_interface + + pluginlib + + rclcpp + + rclcpp_lifecycle + + realtime_tools + + std_srvs + ament_lint_auto - ament_lint_common + 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 new file mode 100644 index 0000000..0c9446d --- /dev/null +++ b/src/rove_tracks/rove_tracks.xml @@ -0,0 +1,8 @@ + + + + ChainedController ros2_control controller. + + + diff --git a/src/rove_tracks/src/chained_controller.cpp b/src/rove_tracks/src/chained_controller.cpp new file mode 100644 index 0000000..2a3d9ad --- /dev/null +++ b/src/rove_tracks/src/chained_controller.cpp @@ -0,0 +1,297 @@ +// 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 "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() +{ + control_mode_.initRT(control_mode_type::FAST); + + try + { + param_listener_ = std::make_shared(get_node()); + } + catch (const std::exception & e) + { + fprintf(stderr, "Exception thrown during controller's init with message: %s \n", e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn ChainedController::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + params_ = param_listener_->get_params(); + + if (!params_.state_joints.empty()) + { + state_joints_ = params_.state_joints; + } + else + { + state_joints_ = params_.joints; + } + + if (params_.joints.size() != state_joints_.size()) + { + RCLCPP_FATAL( + get_node()->get_logger(), + "Size of 'joints' (%zu) and 'state_joints' (%zu) parameters has to be the same!", + params_.joints.size(), state_joints_.size()); + return CallbackReturn::FAILURE; + } + + // topics QoS + auto subscribers_qos = rclcpp::SystemDefaultsQoS(); + subscribers_qos.keep_last(1); + subscribers_qos.best_effort(); + + // Reference Subscriber + ref_subscriber_ = get_node()->create_subscription( + "~/reference", subscribers_qos, + std::bind(&ChainedController::reference_callback, this, std::placeholders::_1)); + + std::shared_ptr msg = std::make_shared(); + reset_controller_reference_msg(msg, params_.joints); + input_ref_.writeFromNonRT(msg); + + auto set_slow_mode_service_callback = + [&]( + const std::shared_ptr request, + std::shared_ptr response) + { + if (request->data) + { + control_mode_.writeFromNonRT(control_mode_type::SLOW); + } + else + { + control_mode_.writeFromNonRT(control_mode_type::FAST); + } + response->success = true; + }; + + set_slow_control_mode_service_ = get_node()->create_service( + "~/set_slow_control_mode", set_slow_mode_service_callback, + rmw_qos_profile_services_hist_keep_all); + + try + { + // State publisher + s_publisher_ = + get_node()->create_publisher("~/state", rclcpp::SystemDefaultsQoS()); + state_publisher_ = std::make_unique(s_publisher_); + } + catch (const std::exception & e) + { + fprintf( + stderr, "Exception thrown during publisher creation at configure stage with message : %s \n", + e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + // TODO(anyone): Reserve memory in state publisher depending on the message type + state_publisher_->lock(); + state_publisher_->msg_.header.frame_id = params_.joints[0]; + state_publisher_->unlock(); + + RCLCPP_INFO(get_node()->get_logger(), "configure successful"); + return controller_interface::CallbackReturn::SUCCESS; +} + +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(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 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(state_joints_.size()); + 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() == 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()); + } +} + +std::vector ChainedController::on_export_reference_interfaces() +{ + reference_interfaces_.resize(state_joints_.size(), std::numeric_limits::quiet_NaN()); + + std::vector reference_interfaces; + reference_interfaces.reserve(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) +{ + // 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()), state_joints_); + + 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*/) +{ + // 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(reference_interfaces_[i])) + { + if (*(control_mode_.readFromRT()) == control_mode_type::SLOW) + { + reference_interfaces_[i] /= 2; + } + command_interfaces_[i].set_value(reference_interfaces_[i]); + + reference_interfaces_[i] = std::numeric_limits::quiet_NaN(); + } + } + + if (state_publisher_ && state_publisher_->trylock()) + { + state_publisher_->msg_.header.stamp = time; + state_publisher_->msg_.set_point = command_interfaces_[CMD_MY_ITFS].get_value(); + + state_publisher_->unlockAndPublish(); + } + + return controller_interface::return_type::OK; +} + +} // namespace 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 new file mode 100644 index 0000000..aa92704 --- /dev/null +++ b/src/rove_tracks/src/chained_controller.yaml @@ -0,0 +1,31 @@ +chained_controller: + joints: { + type: string_array, + default_value: [], + description: "Specifies joints used by the controller. If state joints parameter is defined, then only command joints are defined with this parameter.", + read_only: true, + validation: { + unique<>: null, + not_empty<>: null, + } + } + state_joints: { + type: string_array, + default_value: [], + description: "(optional) Specifies joints for reading states. This parameter is only relevant when state joints are different then command joint, i.e., when a following controller is used.", + read_only: true, + validation: { + unique<>: null, + } + } + interface_name: { + type: string, + default_value: "", + description: "Name of the interface used by the controller on joints and command_joints.", + read_only: true, + validation: { + not_empty<>: null, + one_of<>: [["position", "velocity", "acceleration", "effort",]], + forbidden_interface_name_prefix: null + } + } diff --git a/src/rove_tracks/test/chained_controller_params.yaml b/src/rove_tracks/test/chained_controller_params.yaml new file mode 100644 index 0000000..a2acc4c --- /dev/null +++ b/src/rove_tracks/test/chained_controller_params.yaml @@ -0,0 +1,7 @@ +test_chained_controller: + ros__parameters: + + joints: + - joint1 + + interface_name: acceleration diff --git a/src/rove_tracks/test/chained_controller_preceeding_params.yaml b/src/rove_tracks/test/chained_controller_preceeding_params.yaml new file mode 100644 index 0000000..dc08f7a --- /dev/null +++ b/src/rove_tracks/test/chained_controller_preceeding_params.yaml @@ -0,0 +1,9 @@ +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 new file mode 100644 index 0000000..d980b7e --- /dev/null +++ b/src/rove_tracks/test/test_chained_controller.cpp @@ -0,0 +1,387 @@ +// 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_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 new file mode 100644 index 0000000..b715f41 --- /dev/null +++ b/src/rove_tracks/test/test_chained_controller.hpp @@ -0,0 +1,277 @@ +// 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 new file mode 100644 index 0000000..225e984 --- /dev/null +++ b/src/rove_tracks/test/test_chained_controller_preceeding.cpp @@ -0,0 +1,89 @@ +// 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 new file mode 100644 index 0000000..dd92b23 --- /dev/null +++ b/src/rove_tracks/test/test_load_chained_controller.cpp @@ -0,0 +1,42 @@ +// 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(); +} From 0dcecd1c4ccee5093e8f1a368a2565ffd3bc2014 Mon Sep 17 00:00:00 2001 From: ilithebutterfly Date: Wed, 5 Jun 2024 14:59:40 -0400 Subject: [PATCH 03/12] Joy or cmd_vel are lagging --- src/rove_bringup/CMakeLists.txt | 3 +- .../config/teleop_joy_params.yaml | 7 +- src/rove_bringup/launch/common.launch.py | 26 +-- src/rove_bringup/launch/real.launch.py | 107 ++++++++- .../rove_controller_bluetooth.launch.py | 4 +- .../launch/rove_controller_usb.launch.py | 6 +- src/rove_bringup/package.xml | 2 + src/rove_bringup/src/rove_controller_node.cpp | 38 ++- .../config/test_controllers.yaml | 14 ++ .../config/tracks_controllers.yaml | 87 +++++++ .../urdf/rove.ros2_control.urdf.xacro | 111 +++++++++ src/rove_description/urdf/rove.urdf.xacro | 12 +- src/rove_description/urdf/track.urdf.xacro | 34 +++ src/rove_tracks/CMakeLists.txt | 24 +- .../rove_tracks/chained_controller.hpp | 53 +++-- ...validate_chained_controller_parameters.hpp | 1 + src/rove_tracks/src/chained_controller.cpp | 219 +++++++++++------- src/rove_tracks/src/chained_controller.yaml | 45 ++-- .../test/chained_controller_params.yaml | 7 +- .../test/test_chained_controller.cpp | 7 +- src/rove_tracks/urdf/rove_track.urdf.xacro | 37 +++ 21 files changed, 671 insertions(+), 173 deletions(-) create mode 100644 src/rove_description/config/test_controllers.yaml create mode 100644 src/rove_description/config/tracks_controllers.yaml create mode 100644 src/rove_description/urdf/rove.ros2_control.urdf.xacro create mode 100644 src/rove_tracks/urdf/rove_track.urdf.xacro diff --git a/src/rove_bringup/CMakeLists.txt b/src/rove_bringup/CMakeLists.txt index fcf4c58..8e81165 100644 --- a/src/rove_bringup/CMakeLists.txt +++ b/src/rove_bringup/CMakeLists.txt @@ -6,12 +6,13 @@ project(rove_bringup) find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(sensor_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) find_package(Qt5 COMPONENTS Widgets REQUIRED) add_executable(rove_controller_node src/rove_controller_node.cpp) # Link dependencies -ament_target_dependencies(rove_controller_node rclcpp sensor_msgs) +ament_target_dependencies(rove_controller_node rclcpp sensor_msgs geometry_msgs) # Install executable install(TARGETS diff --git a/src/rove_bringup/config/teleop_joy_params.yaml b/src/rove_bringup/config/teleop_joy_params.yaml index 2b37113..c5aff47 100644 --- a/src/rove_bringup/config/teleop_joy_params.yaml +++ b/src/rove_bringup/config/teleop_joy_params.yaml @@ -14,16 +14,17 @@ scale_angular: pitch: 0.0 roll: 0.0 - yaw: 0.5 + yaw: 4.0 scale_angular_turbo: pitch: 0.0 roll: 0.0 yaw: 1.0 scale_linear: - x: 0.5 + x: 15.0 y: 0.0 z: 0.0 scale_linear_turbo: x: 1.0 y: 0.0 - z: 0.0 \ No newline at end of file + z: 0.0 + publish_stamped_twist: true \ No newline at end of file diff --git a/src/rove_bringup/launch/common.launch.py b/src/rove_bringup/launch/common.launch.py index b88bcd7..6b18040 100644 --- a/src/rove_bringup/launch/common.launch.py +++ b/src/rove_bringup/launch/common.launch.py @@ -15,24 +15,12 @@ def generate_launch_description(): pkg_rove_bringup = get_package_share_directory('rove_bringup') pkg_rove_description = get_package_share_directory('rove_description') pkg_rove_slam = get_package_share_directory('rove_slam') - bringup_pkg_path = get_package_share_directory('rove_bringup') # Get the URDF file urdf_path = os.path.join(pkg_rove_description, 'urdf', 'rove.urdf.xacro') robot_desc = ParameterValue(Command(['xacro ', urdf_path]), value_type=str) - # Takes the description and joint angles as inputs and publishes - # the 3D poses of the robot links - robot_state_publisher = Node( - package='robot_state_publisher', - executable='robot_state_publisher', - name='robot_state_publisher', - output='both', - parameters=[ - {'robot_description': robot_desc}, - {"use_sim_time": True, } - ] - ) + # Visualize in RViz rviz = Node( @@ -83,7 +71,7 @@ def generate_launch_description(): teleop = IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(bringup_pkg_path, "launch", "rove_controller_usb.launch.py"), + os.path.join(pkg_rove_bringup, "launch", "rove_controller_usb.launch.py"), ), ) @@ -99,11 +87,11 @@ def generate_launch_description(): ) return LaunchDescription([ - robot_state_publisher, - robot_localization_node_local, - robot_localization_node_global, - navsat_transform, + # robot_state_publisher, + # robot_localization_node_local, + # robot_localization_node_global, + # navsat_transform, rviz, teleop, - autonomy, + # autonomy, ]) diff --git a/src/rove_bringup/launch/real.launch.py b/src/rove_bringup/launch/real.launch.py index ef14e0b..f08fa88 100644 --- a/src/rove_bringup/launch/real.launch.py +++ b/src/rove_bringup/launch/real.launch.py @@ -2,17 +2,60 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import IncludeLaunchDescription +from launch.actions import IncludeLaunchDescription, RegisterEventHandler from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import Command -from launch_ros.actions import Node +from launch.substitutions import Command, PathJoinSubstitution, FindExecutable +from launch_ros.actions import Node, SetRemap from launch_ros.parameter_descriptions import ParameterValue - +from launch.event_handlers import OnProcessExit def generate_launch_description(): # Get the launch directory pkg_rove_bringup = get_package_share_directory('rove_bringup') + pkg_rove_description = get_package_share_directory('rove_description') + + # Get the URDF file + urdf_path = os.path.join(pkg_rove_description, 'urdf', 'rove.urdf.xacro') + robot_desc = ParameterValue(Command(['xacro ', urdf_path]), value_type=str) + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [urdf_path] + ), +# " ", +# "use_mock_hardware:=", +# use_mock_hardware, + ] + ) + robot_description = {"robot_description": robot_description_content} + + # Takes the description and joint angles as inputs and publishes + # the 3D poses of the robot links + robot_state_pub_node = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + # name='robot_state_publisher', + output='both', + parameters=[ + {'robot_description': robot_desc}, + {"use_sim_time": True, } + ], + remappings=[ + # ("/diff_drive_controller/cmd_vel_unstamped", "/cmd_vel"), + # ("/diff_drive_controller/cmd_vel", "/cmd_vel"), + # ("/cmd_vel", "/diff_drive_controller/cmd_vel_unstamped"), + # ("/cmd_vel", "/diff_drive_controller/cmd_vel"), + ], + ) + + # Controllers + # controller_nodes = ["left_track_controller", "right_track_controller", "diff_drive_controller"] + controller_nodes = ["diff_drive_controller"] + # controller_nodes = ["velocity_controller"] + # controller_nodes = ["forward_controller"] common = IncludeLaunchDescription( PythonLaunchDescriptionSource( @@ -20,6 +63,55 @@ def generate_launch_description(): ), ) + ###### ROS2 control ###### + controllers = PathJoinSubstitution( + [ + pkg_rove_description, + "config", + "tracks_controllers.yaml", + # "test_controllers.yaml", + ] + ) + + control_node = Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[ + # robot_desc, + robot_description, + controllers, + ], + # remappings=[ + # ('/cmd_vel', '/diff_drive_controller/cmd_vel_unstamped'), + # ], + output="both", + ) + + joint_state_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"], + ) + + + def create_controller_node(node_name:str): + robot_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=[node_name, "--controller-manager", "/controller_manager"], + ) + + # Delay start of robot_controller after `joint_state_broadcaster` + delay_robot_controller_spawner_after_joint_state_broadcaster_spawner = RegisterEventHandler( + event_handler=OnProcessExit( + target_action=joint_state_broadcaster_spawner, + on_exit=[robot_controller_spawner], + ) + ) + return delay_robot_controller_spawner_after_joint_state_broadcaster_spawner + + delayed_controller_nodes = list([create_controller_node(node_name) for node_name in controller_nodes]) + ###### Sensor ###### vectornav = IncludeLaunchDescription( @@ -27,8 +119,13 @@ def generate_launch_description(): os.path.join(pkg_rove_bringup, "launch", "vectornav.launch.py"), ), ) + return LaunchDescription([ + robot_state_pub_node, + control_node, common, - vectornav, + joint_state_broadcaster_spawner, + *delayed_controller_nodes, + # vectornav, ]) diff --git a/src/rove_bringup/launch/rove_controller_bluetooth.launch.py b/src/rove_bringup/launch/rove_controller_bluetooth.launch.py index ddf043c..43dc67e 100644 --- a/src/rove_bringup/launch/rove_controller_bluetooth.launch.py +++ b/src/rove_bringup/launch/rove_controller_bluetooth.launch.py @@ -7,7 +7,7 @@ def generate_launch_description(): dir = get_package_share_directory(package_name) # Get params files joy_params_file = dir + '/config/joy_params.yaml' - teleope_joy_params_file = dir + '/config/teleop_joy_params.yaml' + teleop_joy_params_file = dir + '/config/teleop_joy_params.yaml' bluetooth_mapping_file = dir + '/config/bluetooth_mapping.yaml' return LaunchDescription([ @@ -22,7 +22,7 @@ def generate_launch_description(): package='teleop_twist_joy', executable='teleop_node', name='teleop_twist_joy_node', - parameters=[teleope_joy_params_file], + parameters=[teleop_joy_params_file], remappings=[ ('/joy', '/joy'), # ('/cmd_vel', '/model/rove/cmd_vel') diff --git a/src/rove_bringup/launch/rove_controller_usb.launch.py b/src/rove_bringup/launch/rove_controller_usb.launch.py index 846e720..7241cb8 100644 --- a/src/rove_bringup/launch/rove_controller_usb.launch.py +++ b/src/rove_bringup/launch/rove_controller_usb.launch.py @@ -7,7 +7,7 @@ def generate_launch_description(): dir = get_package_share_directory(package_name) # Get params files joy_params_file = dir + '/config/joy_params.yaml' - teleope_joy_params_file = dir + '/config/teleop_joy_params.yaml' + teleop_joy_params_file = dir + '/config/teleop_joy_params.yaml' usb_mapping_file = dir + '/config/usb_mapping.yaml' return LaunchDescription([ @@ -22,10 +22,10 @@ def generate_launch_description(): package='teleop_twist_joy', executable='teleop_node', name='teleop_twist_joy_node', - parameters=[teleope_joy_params_file], + parameters=[teleop_joy_params_file], remappings=[ ('/joy', '/joy'), - # ('/cmd_vel', '/model/rove/cmd_vel') + ('/cmd_vel', '/diff_drive_controller/cmd_vel_unstamped') ], ), Node( diff --git a/src/rove_bringup/package.xml b/src/rove_bringup/package.xml index 140cc49..808c303 100644 --- a/src/rove_bringup/package.xml +++ b/src/rove_bringup/package.xml @@ -9,6 +9,8 @@ joy teleop_twist_joy + std_msgs + geometry_msgs joy teleop_twist_joy diff --git a/src/rove_bringup/src/rove_controller_node.cpp b/src/rove_bringup/src/rove_controller_node.cpp index 8baedae..7ea6902 100644 --- a/src/rove_bringup/src/rove_controller_node.cpp +++ b/src/rove_bringup/src/rove_controller_node.cpp @@ -1,6 +1,9 @@ // rove_controller.cpp #include "rclcpp/rclcpp.hpp" #include "sensor_msgs/msg/joy.hpp" +#include "geometry_msgs/msg/twist.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "std_msgs/msg/float64_multi_array.hpp" enum ControllerMode{ FLIPPER_MODE = 0, @@ -39,8 +42,14 @@ class RoveController : public rclcpp::Node { joy_sub_ = create_subscription( "/joy", 10, std::bind(&RoveController::joyCallback, this, std::placeholders::_1) ); + // cmd_vel_sub_ = create_subscription("/cmd_vel", 1, std::bind(&RoveController::cmdvelCallback, this, std::placeholders::_1)); - joy_pub_ = create_publisher("/rove/joy", 10); + joy_pub_ = create_publisher("/rove/joy", 1); + // cmd_vel_stamped_pub_ = create_publisher("/diff_drive_controller/cmd_vel", 1); + + // auto x = create_wall_timer( + // std::chrono::milliseconds(1000/20), std::bind(&RoveController::cmdvelTimerCallback, this, std::placeholders::_1) + // ); previous_msg_ = sensor_msgs::msg::Joy(); previous_msg_.axes.resize(6, 0.0); @@ -49,6 +58,9 @@ class RoveController : public rclcpp::Node { teleop_msg_ = sensor_msgs::msg::Joy(); teleop_msg_.axes.resize(2,0); teleop_msg_.buttons.resize(1,0); + + // cmd_vel_msg_ = geometry_msgs::msg::Twist(); + // cmd_vel_stamped_msg_ = geometry_msgs::msg::TwistStamped(); } private: @@ -77,6 +89,7 @@ class RoveController : public rclcpp::Node { } void arm_action(const sensor_msgs::msg::Joy::SharedPtr joy_msg) { + log("Arm"); if(button_pressed(joy_msg, B)){ log("Arm B pressed"); } @@ -88,6 +101,7 @@ class RoveController : public rclcpp::Node { if(buttton_down(joy_msg, X)){ log("Arm X down"); } + } void flipper_action(const sensor_msgs::msg::Joy::SharedPtr joy_msg) { @@ -109,6 +123,11 @@ class RoveController : public rclcpp::Node { } void log(const char *text){ + static rclcpp::Clock clk{}; + static const char* last = nullptr; + if (last == text) return; + last = text; + // RCLCPP_INFO_THROTTLE(get_logger(), clk, 2000, text); RCLCPP_INFO(get_logger(), text); } @@ -126,11 +145,26 @@ class RoveController : public rclcpp::Node { previous_msg_ = *joy_msg; } + // void cmdvelCallback(const geometry_msgs::msg::Twist::SharedPtr cmd_vel_msg) { + // cmd_vel_msg_ = *cmd_vel_msg; + // cmd_vel_stamped_msg_.twist = cmd_vel_msg_; + // auto header = std_msgs::msg::Header(); + // header.stamp = this->get_clock()->now(); + // cmd_vel_stamped_msg_.header = header; + // } + + // void cmdvelTimerCallback() { + // cmd_vel_stamped_pub_->publish(cmd_vel_stamped_msg_); + // } + rclcpp::Subscription::SharedPtr joy_sub_; + // rclcpp::Subscription::SharedPtr cmd_vel_sub_; rclcpp::Publisher::SharedPtr joy_pub_; + // rclcpp::Publisher::SharedPtr cmd_vel_stamped_pub_; sensor_msgs::msg::Joy previous_msg_; sensor_msgs::msg::Joy teleop_msg_; - + // geometry_msgs::msg::Twist cmd_vel_msg_; + // geometry_msgs::msg::TwistStamped cmd_vel_stamped_msg_; }; int main(int argc, char **argv) { diff --git a/src/rove_description/config/test_controllers.yaml b/src/rove_description/config/test_controllers.yaml new file mode 100644 index 0000000..43b5fdd --- /dev/null +++ b/src/rove_description/config/test_controllers.yaml @@ -0,0 +1,14 @@ +controller_manager: + ros__parameters: + update_rate: 10 # Hz + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + velocity_controller: + type: velocity_controllers/JointGroupVelocityController + +velocity_controller: + ros__parameters: + joints: + - track_fl_j \ No newline at end of file diff --git a/src/rove_description/config/tracks_controllers.yaml b/src/rove_description/config/tracks_controllers.yaml new file mode 100644 index 0000000..92d4dd3 --- /dev/null +++ b/src/rove_description/config/tracks_controllers.yaml @@ -0,0 +1,87 @@ +# This config file is used by ros2_control +controller_manager: + ros__parameters: + update_rate: 50 # Hz + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + # left_track_controller: + # type: rove_tracks/ChainedController + + # right_track_controller: + # type: rove_tracks/ChainedController + + diff_drive_controller: + type: diff_drive_controller/DiffDriveController + # forward_controller: + # type: forward_command_controller/ForwardCommandController + +# forward_controller: +# ros__parameters: +# joints: +# - track_fl_j +# interface_name: velocity + +diff_drive_controller: + ros__parameters: + # left_wheel_names: ["track_l_j"] + right_wheel_names: ["track_rr_j", "track_fr_j"] + left_wheel_names: ["track_fl_j", "track_rl_j"] + # right_wheel_names: ["track_fr_j"] + + wheel_separation: 0.419 + wheel_radius: 0.0855 + + wheel_separation_multiplier: 1.0 + left_wheel_radius_multiplier: -1.0 + right_wheel_radius_multiplier: 1.0 + + publish_rate: 50.0 + odom_frame_id: odom + # allow_multiple_cmd_vel_publishers: true + base_frame_id: base_link + pose_covariance_diagonal : [0.001, 0.001, 0.001, 0.001, 0.001, 0.01] + twist_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.01] + + open_loop: true + enable_odom_tf: true + + cmd_vel_timeout: 0.5 + # publish_limited_velocity: true + # publish_cmd: true + # velocity_rolling_window_size: 10 + use_stamped_vel: false + + # Velocity and acceleration limits + # Whenever a min_* is unspecified, default to -max_* + linear.x.has_velocity_limits: true + linear.x.has_acceleration_limits: true + linear.x.has_jerk_limits: false + linear.x.max_velocity: 100.0 + # linear.x.min_velocity: -1.0 + linear.x.max_acceleration: 100.0 + linear.x.max_jerk: 0.0 + linear.x.min_jerk: 0.0 + + angular.z.has_velocity_limits: true + angular.z.has_acceleration_limits: true + angular.z.has_jerk_limits: false + angular.z.max_velocity: 100.0 + # angular.z.min_velocity: -1.0 + angular.z.max_acceleration: 1.0 + # angular.z.min_acceleration: -1.0 + angular.z.max_jerk: 0.0 + angular.z.min_jerk: 0.0 + +# left_track_controller: +# ros__parameters: +# track_joint_name: track_l_j +# front_joint: track_fl_j +# back_joint: track_rl_j + +# right_track_controller: +# ros__parameters: +# track_joint_name: track_r_j +# front_joint: track_fr_j +# back_joint: track_rr_j \ No newline at end of file diff --git a/src/rove_description/urdf/rove.ros2_control.urdf.xacro b/src/rove_description/urdf/rove.ros2_control.urdf.xacro new file mode 100644 index 0000000..882deec --- /dev/null +++ b/src/rove_description/urdf/rove.ros2_control.urdf.xacro @@ -0,0 +1,111 @@ + + + + + + + + + + + + + + + + + + + + odrive_ros2_control_plugin/ODriveHardwareInterface + can0 + + + 23 + + + + + + + 24 + + + + + + + + 21 + + + + + + + 22 + + + + + + + + + + + mock_components/GenericSystem + true + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/rove_description/urdf/rove.urdf.xacro b/src/rove_description/urdf/rove.urdf.xacro index 48a80b8..0248842 100644 --- a/src/rove_description/urdf/rove.urdf.xacro +++ b/src/rove_description/urdf/rove.urdf.xacro @@ -16,8 +16,10 @@ + - + + @@ -48,10 +50,14 @@ + + + + - + diff --git a/src/rove_description/urdf/track.urdf.xacro b/src/rove_description/urdf/track.urdf.xacro index bb8c100..328db4d 100644 --- a/src/rove_description/urdf/track.urdf.xacro +++ b/src/rove_description/urdf/track.urdf.xacro @@ -19,6 +19,24 @@ + + + + + + + + + + + + + + @@ -52,6 +70,22 @@ + + + + + + + + + + + + + + + + track_${suffix} diff --git a/src/rove_tracks/CMakeLists.txt b/src/rove_tracks/CMakeLists.txt index 85993ea..d045986 100644 --- a/src/rove_tracks/CMakeLists.txt +++ b/src/rove_tracks/CMakeLists.txt @@ -29,23 +29,23 @@ generate_parameter_library(chained_controller_parameters include/rove_tracks/validate_chained_controller_parameters.hpp ) add_library( - chained_controller + rove_tracks SHARED src/chained_controller.cpp ) -target_include_directories(chained_controller PUBLIC +target_include_directories(rove_tracks PUBLIC "$" "$") -target_link_libraries(chained_controller chained_controller_parameters) -ament_target_dependencies(chained_controller ${THIS_PACKAGE_INCLUDE_DEPENDS}) -target_compile_definitions(chained_controller PRIVATE "CHAINED_CONTROLLER_BUILDING_DLL") +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 - chained_controller + rove_tracks RUNTIME DESTINATION bin ARCHIVE DESTINATION lib LIBRARY DESTINATION lib @@ -56,7 +56,13 @@ install( DESTINATION include/${PROJECT_NAME} ) -if(BUILD_TESTING) +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 @@ -88,7 +94,7 @@ if(BUILD_TESTING) hardware_interface ) - add_rostest_with_parameters_gmock(test_chained_controller_preceeding test/test_chained_controller_preceeding.cpp ${CMAKE_CURRENT_SOURCE_DIR}/test/chained_controller_preceeding_params.yaml) + 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( @@ -105,7 +111,7 @@ ament_export_dependencies( ${THIS_PACKAGE_INCLUDE_DEPENDS} ) ament_export_libraries( - chained_controller + rove_tracks ) ament_package() diff --git a/src/rove_tracks/include/rove_tracks/chained_controller.hpp b/src/rove_tracks/include/rove_tracks/chained_controller.hpp index b600803..71b5a00 100644 --- a/src/rove_tracks/include/rove_tracks/chained_controller.hpp +++ b/src/rove_tracks/include/rove_tracks/chained_controller.hpp @@ -35,19 +35,39 @@ namespace rove_tracks { -// 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; +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, +}; -// TODO(anyone: example setup for control mode (usually you will use some enums defined in messages) -enum class control_mode_type : std::uint8_t +enum track_state_id : int { - FAST = 0, - SLOW = 1, + 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: @@ -88,18 +108,21 @@ class ChainedController : public controller_interface::ChainableControllerInterf using ControllerStateMsg = control_msgs::msg::JointControllerState; protected: - std::shared_ptr param_listener_; - chained_controller::Params params_; + std::shared_ptr param_listener_; + rove_tracks::Params params_; - std::vector state_joints_; + 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_; - rclcpp::Service::SharedPtr set_slow_control_mode_service_; - realtime_tools::RealtimeBuffer control_mode_; - using ControllerStatePublisher = realtime_tools::RealtimePublisher; rclcpp::Publisher::SharedPtr s_publisher_; @@ -114,6 +137,8 @@ class ChainedController : public controller_interface::ChainableControllerInterf // callback for topic interface ROVE_TRACKS__VISIBILITY_LOCAL void reference_callback(const std::shared_ptr msg); + + rclcpp::Logger logger() const; }; } // namespace rove_tracks 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 index caf9569..10a8db2 100644 --- a/src/rove_tracks/include/rove_tracks/validate_chained_controller_parameters.hpp +++ b/src/rove_tracks/include/rove_tracks/validate_chained_controller_parameters.hpp @@ -18,6 +18,7 @@ #include +#include #include "parameter_traits/parameter_traits.hpp" namespace parameter_traits diff --git a/src/rove_tracks/src/chained_controller.cpp b/src/rove_tracks/src/chained_controller.cpp index 2a3d9ad..4597465 100644 --- a/src/rove_tracks/src/chained_controller.cpp +++ b/src/rove_tracks/src/chained_controller.cpp @@ -20,6 +20,7 @@ #include #include +#include #include "controller_interface/helpers.hpp" namespace @@ -58,14 +59,13 @@ ChainedController::ChainedController() : controller_interface::ChainableControll controller_interface::CallbackReturn ChainedController::on_init() { - control_mode_.initRT(control_mode_type::FAST); - try { - param_listener_ = std::make_shared(get_node()); + 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; } @@ -78,58 +78,24 @@ controller_interface::CallbackReturn ChainedController::on_configure( { 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; - } + front_joint_ = params_.front_joint; + back_joint_ = params_.back_joint; + track_joint_ = params_.track_joint_name; - // topics QoS + // // topics QoS auto subscribers_qos = rclcpp::SystemDefaultsQoS(); subscribers_qos.keep_last(1); subscribers_qos.best_effort(); - // Reference Subscriber + // // 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_.joints); + reset_controller_reference_msg(msg, {params_.track_joint_name}); input_ref_.writeFromNonRT(msg); - auto set_slow_mode_service_callback = - [&]( - const std::shared_ptr request, - std::shared_ptr response) - { - if (request->data) - { - control_mode_.writeFromNonRT(control_mode_type::SLOW); - } - else - { - control_mode_.writeFromNonRT(control_mode_type::FAST); - } - response->success = true; - }; - - set_slow_control_mode_service_ = get_node()->create_service( - "~/set_slow_control_mode", set_slow_mode_service_callback, - rmw_qos_profile_services_hist_keep_all); - try { // State publisher @@ -147,7 +113,7 @@ controller_interface::CallbackReturn ChainedController::on_configure( // 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_->msg_.header.frame_id = params_.track_joint_name; state_publisher_->unlock(); RCLCPP_INFO(get_node()->get_logger(), "configure successful"); @@ -159,11 +125,16 @@ controller_interface::InterfaceConfiguration ChainedController::command_interfac 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); - } + // 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; } @@ -173,49 +144,71 @@ controller_interface::InterfaceConfiguration ChainedController::state_interface_ 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); - } + // 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() == params_.joints.size()) + if (msg->joint_names.size() == 1) { input_ref_.writeFromNonRT(msg); } else { RCLCPP_ERROR( - get_node()->get_logger(), + logger(), "Received %zu , but expected %zu joints in command. Ignoring message.", - msg->joint_names.size(), params_.joints.size()); + 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(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()); - 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])); - } + 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; } @@ -224,7 +217,7 @@ controller_interface::CallbackReturn ChainedController::on_activate( const rclcpp_lifecycle::State & /*previous_state*/) { // Set default value in command - reset_controller_reference_msg(*(input_ref_.readFromRT()), state_joints_); + reset_controller_reference_msg(*(input_ref_.readFromRT()), {track_joint_}); return controller_interface::CallbackReturn::SUCCESS; } @@ -262,34 +255,98 @@ controller_interface::return_type ChainedController::update_reference_from_subsc controller_interface::return_type ChainedController::update_and_write_commands( const rclcpp::Time & time, const rclcpp::Duration & /*period*/) { - // 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) + static rclcpp::Clock clk{}; + if (!std::isnan(reference_interfaces_[0])) { - if (!std::isnan(reference_interfaces_[i])) - { - if (*(control_mode_.readFromRT()) == control_mode_type::SLOW) - { - reference_interfaces_[i] /= 2; - } - command_interfaces_[i].set_value(reference_interfaces_[i]); + RCLCPP_INFO_STREAM_THROTTLE(logger(), clk, 1000, "Ref value: " << reference_interfaces_[0]); + direction_ = reference_interfaces_[0] == 0 ? 0 : ( + reference_interfaces_[0] < 0 ? -1 : 1 + ); + } - reference_interfaces_[i] = std::numeric_limits::quiet_NaN(); + 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 (state_publisher_ && state_publisher_->trylock()) + if (leading_st_effort != track_state_id::TRK_ST_NONE) { - state_publisher_->msg_.header.stamp = time; - state_publisher_->msg_.set_point = command_interfaces_[CMD_MY_ITFS].get_value(); + leading_torque = state_interfaces_[leading_st_effort].get_value(); + } - state_publisher_->unlockAndPublish(); + 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 +} // namespace rove_tracks #include "pluginlib/class_list_macros.hpp" diff --git a/src/rove_tracks/src/chained_controller.yaml b/src/rove_tracks/src/chained_controller.yaml index aa92704..f58782a 100644 --- a/src/rove_tracks/src/chained_controller.yaml +++ b/src/rove_tracks/src/chained_controller.yaml @@ -1,31 +1,30 @@ -chained_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.", +rove_tracks: + track_joint_name: { + type: string, + default_value: "", + description: "The name of the track joint.", read_only: true, - validation: { - unique<>: null, - not_empty<>: null, - } + # validation: { + # 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.", + front_joint: { + type: string, + default_value: "", + description: "Specifies the joint at the front of the track.", read_only: true, - validation: { - unique<>: null, - } + # validation: { + # not_empty<>: null, + # # unique<>: null, + # } } - interface_name: { + back_joint: { type: string, default_value: "", - description: "Name of the interface used by the controller on joints and command_joints.", + description: "Specifies the joint at the back of the track.", read_only: true, - validation: { - not_empty<>: null, - one_of<>: [["position", "velocity", "acceleration", "effort",]], - forbidden_interface_name_prefix: null - } + # 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 index a2acc4c..91fc7d6 100644 --- a/src/rove_tracks/test/chained_controller_params.yaml +++ b/src/rove_tracks/test/chained_controller_params.yaml @@ -1,7 +1,6 @@ test_chained_controller: ros__parameters: + track_joint_name: joint1 + front_joint: joint2 + back_joint: joint3 - joints: - - joint1 - - interface_name: acceleration diff --git a/src/rove_tracks/test/test_chained_controller.cpp b/src/rove_tracks/test/test_chained_controller.cpp index d980b7e..f3cb7f5 100644 --- a/src/rove_tracks/test/test_chained_controller.cpp +++ b/src/rove_tracks/test/test_chained_controller.cpp @@ -22,7 +22,6 @@ #include using rove_tracks::CMD_MY_ITFS; -using rove_tracks::control_mode_type; using rove_tracks::STATE_MY_ITFS; class ChainedControllerTest : public ChainedControllerFixture @@ -33,9 +32,9 @@ 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_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); diff --git a/src/rove_tracks/urdf/rove_track.urdf.xacro b/src/rove_tracks/urdf/rove_track.urdf.xacro new file mode 100644 index 0000000..06da19d --- /dev/null +++ b/src/rove_tracks/urdf/rove_track.urdf.xacro @@ -0,0 +1,37 @@ + + + + + + ${nodeid} + + + + + + + + + + + + + + + + + + + + mock_components/GenericSystem + + + + + + + + + + + From 207ad64a46da7e57a4c51252077ab80ec18934ec Mon Sep 17 00:00:00 2001 From: iliana Date: Sat, 8 Jun 2024 02:02:33 -0400 Subject: [PATCH 04/12] Not quite stable --- rove.repos | 2 +- .../config/teleop_joy_params.yaml | 10 ++++---- src/rove_bringup/launch/real.launch.py | 25 ++++++++++++++++--- .../config/tracks_controllers.yaml | 18 ++++++------- .../urdf/rove.ros2_control.urdf.xacro | 14 +++++------ 5 files changed, 44 insertions(+), 25 deletions(-) diff --git a/rove.repos b/rove.repos index 628e2b7..20ecf12 100644 --- a/rove.repos +++ b/rove.repos @@ -10,4 +10,4 @@ repositories: vectornav: type: git url: https://github.com/dawonn/vectornav.git - version: ros2 + version: ros2 \ No newline at end of file diff --git a/src/rove_bringup/config/teleop_joy_params.yaml b/src/rove_bringup/config/teleop_joy_params.yaml index c5aff47..6bdf944 100644 --- a/src/rove_bringup/config/teleop_joy_params.yaml +++ b/src/rove_bringup/config/teleop_joy_params.yaml @@ -14,17 +14,17 @@ scale_angular: pitch: 0.0 roll: 0.0 - yaw: 4.0 + yaw: 200.0 scale_angular_turbo: pitch: 0.0 roll: 0.0 - yaw: 1.0 + yaw: 300.0 scale_linear: - x: 15.0 + x: 50.0 y: 0.0 z: 0.0 scale_linear_turbo: - x: 1.0 + x: 100.0 y: 0.0 z: 0.0 - publish_stamped_twist: true \ No newline at end of file + # publish_stamped_twist: true \ No newline at end of file diff --git a/src/rove_bringup/launch/real.launch.py b/src/rove_bringup/launch/real.launch.py index f08fa88..73ee563 100644 --- a/src/rove_bringup/launch/real.launch.py +++ b/src/rove_bringup/launch/real.launch.py @@ -2,13 +2,13 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import IncludeLaunchDescription, RegisterEventHandler +from launch.actions import IncludeLaunchDescription, RegisterEventHandler, ExecuteProcess from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import Command, PathJoinSubstitution, FindExecutable from launch_ros.actions import Node, SetRemap from launch_ros.parameter_descriptions import ParameterValue -from launch.event_handlers import OnProcessExit +from launch.event_handlers import OnProcessExit, OnShutdown def generate_launch_description(): # Get the launch directory @@ -41,7 +41,7 @@ def generate_launch_description(): output='both', parameters=[ {'robot_description': robot_desc}, - {"use_sim_time": True, } + # {"use_sim_time": True, } ], remappings=[ # ("/diff_drive_controller/cmd_vel_unstamped", "/cmd_vel"), @@ -120,6 +120,25 @@ def create_controller_node(node_name:str): ), ) + # start_can_cmd = ExecuteProcess( + # cmd=[[ + # 'ip link set can0 type can bitrate 250000; ip link set up can0' + # ]], + # shell=True + # ) + + # stop_can_cmd = ExecuteProcess( + # cmd=[[ + # 'ip link set down can0' + # ]], + # shell=True + # ) + + # shutdown = RegisterEventHandler( + # event_handler=OnShutdown( + # on_shutdown=[stop_can_cmd] + # ) + # ) return LaunchDescription([ robot_state_pub_node, diff --git a/src/rove_description/config/tracks_controllers.yaml b/src/rove_description/config/tracks_controllers.yaml index 92d4dd3..c44c394 100644 --- a/src/rove_description/config/tracks_controllers.yaml +++ b/src/rove_description/config/tracks_controllers.yaml @@ -1,7 +1,7 @@ # This config file is used by ros2_control controller_manager: ros__parameters: - update_rate: 50 # Hz + update_rate: 20 # Hz joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster @@ -30,19 +30,19 @@ diff_drive_controller: left_wheel_names: ["track_fl_j", "track_rl_j"] # right_wheel_names: ["track_fr_j"] - wheel_separation: 0.419 - wheel_radius: 0.0855 + wheel_separation: 0.230 + wheel_radius: 0.220 wheel_separation_multiplier: 1.0 left_wheel_radius_multiplier: -1.0 right_wheel_radius_multiplier: 1.0 - publish_rate: 50.0 + publish_rate: 20.0 odom_frame_id: odom # allow_multiple_cmd_vel_publishers: true base_frame_id: base_link - pose_covariance_diagonal : [0.001, 0.001, 0.001, 0.001, 0.001, 0.01] - twist_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.01] + # pose_covariance_diagonal : [0.001, 0.001, 0.001, 0.001, 0.001, 0.01] + # twist_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.01] open_loop: true enable_odom_tf: true @@ -60,16 +60,16 @@ diff_drive_controller: linear.x.has_jerk_limits: false linear.x.max_velocity: 100.0 # linear.x.min_velocity: -1.0 - linear.x.max_acceleration: 100.0 + linear.x.max_acceleration: 200.0 linear.x.max_jerk: 0.0 linear.x.min_jerk: 0.0 angular.z.has_velocity_limits: true angular.z.has_acceleration_limits: true angular.z.has_jerk_limits: false - angular.z.max_velocity: 100.0 + angular.z.max_velocity: 500.0 # angular.z.min_velocity: -1.0 - angular.z.max_acceleration: 1.0 + angular.z.max_acceleration: 800.0 # angular.z.min_acceleration: -1.0 angular.z.max_jerk: 0.0 angular.z.min_jerk: 0.0 diff --git a/src/rove_description/urdf/rove.ros2_control.urdf.xacro b/src/rove_description/urdf/rove.ros2_control.urdf.xacro index 882deec..eb0752b 100644 --- a/src/rove_description/urdf/rove.ros2_control.urdf.xacro +++ b/src/rove_description/urdf/rove.ros2_control.urdf.xacro @@ -21,7 +21,7 @@ can0 - 23 + 19 @@ -57,12 +57,12 @@ --> - + - - + + --> From e25881021649dba01b3d39d3a0742556f6511503 Mon Sep 17 00:00:00 2001 From: iliana Date: Sun, 9 Jun 2024 00:40:29 -0400 Subject: [PATCH 08/12] adjust velocity and rotation settings --- .../config/teleop_joy_params.yaml | 8 ++-- src/rove_bringup/launch/real.launch.py | 4 +- .../config/tracks_controllers.yaml | 42 +++++++------------ 3 files changed, 20 insertions(+), 34 deletions(-) diff --git a/src/rove_bringup/config/teleop_joy_params.yaml b/src/rove_bringup/config/teleop_joy_params.yaml index 6bdf944..4195ba7 100644 --- a/src/rove_bringup/config/teleop_joy_params.yaml +++ b/src/rove_bringup/config/teleop_joy_params.yaml @@ -14,17 +14,17 @@ scale_angular: pitch: 0.0 roll: 0.0 - yaw: 200.0 + yaw: 1.0 scale_angular_turbo: pitch: 0.0 roll: 0.0 - yaw: 300.0 + yaw: 1.0 scale_linear: - x: 50.0 + x: 1.0 y: 0.0 z: 0.0 scale_linear_turbo: - x: 100.0 + x: 2.0 y: 0.0 z: 0.0 # publish_stamped_twist: true \ No newline at end of file diff --git a/src/rove_bringup/launch/real.launch.py b/src/rove_bringup/launch/real.launch.py index 793de80..e4ec4da 100644 --- a/src/rove_bringup/launch/real.launch.py +++ b/src/rove_bringup/launch/real.launch.py @@ -155,6 +155,6 @@ def create_controller_node(node_name:str): common, joint_state_broadcaster_spawner, *delayed_controller_nodes, - vectornav, - velodyne, + # vectornav, + # velodyne, ]) diff --git a/src/rove_description/config/tracks_controllers.yaml b/src/rove_description/config/tracks_controllers.yaml index c44c394..8fced65 100644 --- a/src/rove_description/config/tracks_controllers.yaml +++ b/src/rove_description/config/tracks_controllers.yaml @@ -31,18 +31,18 @@ diff_drive_controller: # right_wheel_names: ["track_fr_j"] wheel_separation: 0.230 - wheel_radius: 0.220 + wheel_radius: 0.11 - wheel_separation_multiplier: 1.0 - left_wheel_radius_multiplier: -1.0 - right_wheel_radius_multiplier: 1.0 + wheel_separation_multiplier: 10.0 + left_wheel_radius_multiplier: -0.0325 + right_wheel_radius_multiplier: 0.0325 publish_rate: 20.0 odom_frame_id: odom # allow_multiple_cmd_vel_publishers: true base_frame_id: base_link - # pose_covariance_diagonal : [0.001, 0.001, 0.001, 0.001, 0.001, 0.01] - # twist_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.01] + pose_covariance_diagonal : [0.001, 0.001, 0.001, 0.001, 0.001, 0.01] + twist_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.01] open_loop: true enable_odom_tf: true @@ -53,35 +53,21 @@ diff_drive_controller: # velocity_rolling_window_size: 10 use_stamped_vel: false - # Velocity and acceleration limits - # Whenever a min_* is unspecified, default to -max_* linear.x.has_velocity_limits: true linear.x.has_acceleration_limits: true linear.x.has_jerk_limits: false - linear.x.max_velocity: 100.0 - # linear.x.min_velocity: -1.0 - linear.x.max_acceleration: 200.0 + linear.x.max_velocity: 5.0 + linear.x.min_velocity: -5.0 + linear.x.max_acceleration: 2.0 linear.x.max_jerk: 0.0 linear.x.min_jerk: 0.0 angular.z.has_velocity_limits: true angular.z.has_acceleration_limits: true angular.z.has_jerk_limits: false - angular.z.max_velocity: 500.0 - # angular.z.min_velocity: -1.0 - angular.z.max_acceleration: 800.0 - # angular.z.min_acceleration: -1.0 + angular.z.max_velocity: 1.0 + angular.z.min_velocity: -1.0 + angular.z.max_acceleration: 1.0 + angular.z.min_acceleration: -1.0 angular.z.max_jerk: 0.0 - angular.z.min_jerk: 0.0 - -# left_track_controller: -# ros__parameters: -# track_joint_name: track_l_j -# front_joint: track_fl_j -# back_joint: track_rl_j - -# right_track_controller: -# ros__parameters: -# track_joint_name: track_r_j -# front_joint: track_fr_j -# back_joint: track_rr_j \ No newline at end of file + angular.z.min_jerk: 0.0 \ No newline at end of file From 582c7469592e6686e924298a68b6064f697dc7ce Mon Sep 17 00:00:00 2001 From: SimonR99 Date: Sun, 9 Jun 2024 00:44:49 -0400 Subject: [PATCH 09/12] remove tracks --- src/rove_tracks/CMakeLists.txt | 117 ------ src/rove_tracks/LICENSE | 17 - .../rove_tracks/chained_controller.hpp | 146 ------- ...validate_chained_controller_parameters.hpp | 40 -- .../include/rove_tracks/visibility_control.h | 50 --- src/rove_tracks/package.xml | 39 -- src/rove_tracks/rove_tracks.xml | 8 - src/rove_tracks/src/chained_controller.cpp | 354 ---------------- src/rove_tracks/src/chained_controller.yaml | 30 -- .../test/chained_controller_params.yaml | 6 - .../chained_controller_preceeding_params.yaml | 9 - .../test/test_chained_controller.cpp | 386 ------------------ .../test/test_chained_controller.hpp | 277 ------------- .../test_chained_controller_preceeding.cpp | 89 ---- .../test/test_load_chained_controller.cpp | 42 -- src/rove_tracks/urdf/rove_track.urdf.xacro | 37 -- 16 files changed, 1647 deletions(-) delete mode 100644 src/rove_tracks/CMakeLists.txt delete mode 100644 src/rove_tracks/LICENSE delete mode 100644 src/rove_tracks/include/rove_tracks/chained_controller.hpp delete mode 100644 src/rove_tracks/include/rove_tracks/validate_chained_controller_parameters.hpp delete mode 100644 src/rove_tracks/include/rove_tracks/visibility_control.h delete mode 100644 src/rove_tracks/package.xml delete mode 100644 src/rove_tracks/rove_tracks.xml delete mode 100644 src/rove_tracks/src/chained_controller.cpp delete mode 100644 src/rove_tracks/src/chained_controller.yaml delete mode 100644 src/rove_tracks/test/chained_controller_params.yaml delete mode 100644 src/rove_tracks/test/chained_controller_preceeding_params.yaml delete mode 100644 src/rove_tracks/test/test_chained_controller.cpp delete mode 100644 src/rove_tracks/test/test_chained_controller.hpp delete mode 100644 src/rove_tracks/test/test_chained_controller_preceeding.cpp delete mode 100644 src/rove_tracks/test/test_load_chained_controller.cpp delete mode 100644 src/rove_tracks/urdf/rove_track.urdf.xacro 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 - - - - - - - - - - - From dda876cee0b118589e1b56e7e51022cb54789e00 Mon Sep 17 00:00:00 2001 From: SimonR99 Date: Sun, 9 Jun 2024 00:49:51 -0400 Subject: [PATCH 10/12] add usb debug --- src/rove_bringup/src/rove_controller_node.cpp | 14 ----------- test.py => utils/odrive_can_test.py | 0 utils/odrive_usb_debug.py | 24 +++++++++++++++++++ 3 files changed, 24 insertions(+), 14 deletions(-) rename test.py => utils/odrive_can_test.py (100%) create mode 100644 utils/odrive_usb_debug.py diff --git a/src/rove_bringup/src/rove_controller_node.cpp b/src/rove_bringup/src/rove_controller_node.cpp index 7ea6902..3086ea0 100644 --- a/src/rove_bringup/src/rove_controller_node.cpp +++ b/src/rove_bringup/src/rove_controller_node.cpp @@ -45,7 +45,6 @@ class RoveController : public rclcpp::Node { // cmd_vel_sub_ = create_subscription("/cmd_vel", 1, std::bind(&RoveController::cmdvelCallback, this, std::placeholders::_1)); joy_pub_ = create_publisher("/rove/joy", 1); - // cmd_vel_stamped_pub_ = create_publisher("/diff_drive_controller/cmd_vel", 1); // auto x = create_wall_timer( // std::chrono::milliseconds(1000/20), std::bind(&RoveController::cmdvelTimerCallback, this, std::placeholders::_1) @@ -145,22 +144,9 @@ class RoveController : public rclcpp::Node { previous_msg_ = *joy_msg; } - // void cmdvelCallback(const geometry_msgs::msg::Twist::SharedPtr cmd_vel_msg) { - // cmd_vel_msg_ = *cmd_vel_msg; - // cmd_vel_stamped_msg_.twist = cmd_vel_msg_; - // auto header = std_msgs::msg::Header(); - // header.stamp = this->get_clock()->now(); - // cmd_vel_stamped_msg_.header = header; - // } - - // void cmdvelTimerCallback() { - // cmd_vel_stamped_pub_->publish(cmd_vel_stamped_msg_); - // } - rclcpp::Subscription::SharedPtr joy_sub_; // rclcpp::Subscription::SharedPtr cmd_vel_sub_; rclcpp::Publisher::SharedPtr joy_pub_; - // rclcpp::Publisher::SharedPtr cmd_vel_stamped_pub_; sensor_msgs::msg::Joy previous_msg_; sensor_msgs::msg::Joy teleop_msg_; // geometry_msgs::msg::Twist cmd_vel_msg_; diff --git a/test.py b/utils/odrive_can_test.py similarity index 100% rename from test.py rename to utils/odrive_can_test.py diff --git a/utils/odrive_usb_debug.py b/utils/odrive_usb_debug.py new file mode 100644 index 0000000..d52adbb --- /dev/null +++ b/utils/odrive_usb_debug.py @@ -0,0 +1,24 @@ +import odrive +import time +import math +from odrive.enums import * + + +odrv0 = odrive.find_any() +odrv = odrv0 + +#odrv.axis0.controller.config.spinout_electrical_power_threshold = 1000 +#odrv.axis0.controller.config.spinout_mechanical_power_threshold = -1000 +#odrv.axis0.config.can.error_msg_rate_ms = 100 +#odrv.axis0.config.can.node_id = 21 +#odrv.axis0.config.enable_watchdog = True + +print(odrv.axis0.config.can.node_id) +print(odrv.axis0.config.enable_watchdog) +print(odrv.axis0.config.watchdog_timeout) +print(odrive.utils.dump_errors(odrv0)) +print(odrv.axis0.disarm_reason) +print(odrv.axis0.active_errors) + +#odrv.axis0.config.can.node_id = 0 +#odrv.save_configuration() \ No newline at end of file From 7bfc763ccd2efb3a8d693d5647ddd176c899b8b6 Mon Sep 17 00:00:00 2001 From: SimonR99 Date: Sun, 9 Jun 2024 01:55:27 -0400 Subject: [PATCH 11/12] arrange mapping --- src/rove_bringup/launch/common.launch.py | 10 ++--- src/rove_bringup/launch/real.launch.py | 43 ------------------- .../launch/rove_controller_usb.launch.py | 1 - .../urdf/rove.ros2_control.urdf.xacro | 41 +----------------- 4 files changed, 6 insertions(+), 89 deletions(-) diff --git a/src/rove_bringup/launch/common.launch.py b/src/rove_bringup/launch/common.launch.py index 54429f5..87781ff 100644 --- a/src/rove_bringup/launch/common.launch.py +++ b/src/rove_bringup/launch/common.launch.py @@ -99,11 +99,11 @@ def generate_launch_description(): ) return LaunchDescription([ - # robot_state_publisher, - # robot_localization_node_local, - # robot_localization_node_global, - # navsat_transform, + robot_state_publisher, + robot_localization_node_local, + #robot_localization_node_global, + #navsat_transform, rviz, teleop, - # autonomy, + autonomy, ]) diff --git a/src/rove_bringup/launch/real.launch.py b/src/rove_bringup/launch/real.launch.py index e4ec4da..4205eff 100644 --- a/src/rove_bringup/launch/real.launch.py +++ b/src/rove_bringup/launch/real.launch.py @@ -17,7 +17,6 @@ def generate_launch_description(): # Get the URDF file urdf_path = os.path.join(pkg_rove_description, 'urdf', 'rove.urdf.xacro') - robot_desc = ParameterValue(Command(['xacro ', urdf_path]), value_type=str) robot_description_content = Command( [ PathJoinSubstitution([FindExecutable(name="xacro")]), @@ -32,30 +31,8 @@ def generate_launch_description(): ) robot_description = {"robot_description": robot_description_content} - # Takes the description and joint angles as inputs and publishes - # the 3D poses of the robot links - robot_state_pub_node = Node( - package='robot_state_publisher', - executable='robot_state_publisher', - # name='robot_state_publisher', - output='both', - parameters=[ - {'robot_description': robot_desc}, - # {"use_sim_time": True, } - ], - remappings=[ - # ("/diff_drive_controller/cmd_vel_unstamped", "/cmd_vel"), - # ("/diff_drive_controller/cmd_vel", "/cmd_vel"), - # ("/cmd_vel", "/diff_drive_controller/cmd_vel_unstamped"), - # ("/cmd_vel", "/diff_drive_controller/cmd_vel"), - ], - ) - # Controllers - # controller_nodes = ["left_track_controller", "right_track_controller", "diff_drive_controller"] controller_nodes = ["diff_drive_controller"] - # controller_nodes = ["velocity_controller"] - # controller_nodes = ["forward_controller"] common = IncludeLaunchDescription( PythonLaunchDescriptionSource( @@ -72,7 +49,6 @@ def generate_launch_description(): pkg_rove_description, "config", "tracks_controllers.yaml", - # "test_controllers.yaml", ] ) @@ -80,13 +56,9 @@ def generate_launch_description(): package="controller_manager", executable="ros2_control_node", parameters=[ - # robot_desc, robot_description, controllers, ], - # remappings=[ - # ('/cmd_vel', '/diff_drive_controller/cmd_vel_unstamped'), - # ], output="both", ) @@ -115,14 +87,6 @@ def create_controller_node(node_name:str): delayed_controller_nodes = list([create_controller_node(node_name) for node_name in controller_nodes]) - - ###### Sensor ###### - vectornav = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(pkg_rove_bringup, "launch", "vectornav.launch.py"), - ), - ) - # start_can_cmd = ExecuteProcess( # cmd=[[ # 'ip link set can0 type can bitrate 250000; ip link set up can0' @@ -143,14 +107,7 @@ def create_controller_node(node_name:str): # ) # ) - velodyne = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(pkg_rove_bringup, "launch", "velodyne.launch.py"), - ), - ) - return LaunchDescription([ - robot_state_pub_node, control_node, common, joint_state_broadcaster_spawner, diff --git a/src/rove_bringup/launch/rove_controller_usb.launch.py b/src/rove_bringup/launch/rove_controller_usb.launch.py index 7241cb8..61153ce 100644 --- a/src/rove_bringup/launch/rove_controller_usb.launch.py +++ b/src/rove_bringup/launch/rove_controller_usb.launch.py @@ -25,7 +25,6 @@ def generate_launch_description(): parameters=[teleop_joy_params_file], remappings=[ ('/joy', '/joy'), - ('/cmd_vel', '/diff_drive_controller/cmd_vel_unstamped') ], ), Node( diff --git a/src/rove_description/urdf/rove.ros2_control.urdf.xacro b/src/rove_description/urdf/rove.ros2_control.urdf.xacro index 8845a93..45461da 100644 --- a/src/rove_description/urdf/rove.ros2_control.urdf.xacro +++ b/src/rove_description/urdf/rove.ros2_control.urdf.xacro @@ -1,7 +1,5 @@ - - - + @@ -50,41 +48,6 @@ - - transmission_interface/SimpleTransmission - - - 30.76923077 - 0.0 - - - - - transmission_interface/SimpleTransmission - - - 30.76923077 - 0.0 - - - - - transmission_interface/SimpleTransmission - - - 30.76923077 - 0.0 - - - - - transmission_interface/SimpleTransmission - - - 30.76923077 - 0.0 - - @@ -141,7 +104,5 @@ - - \ No newline at end of file From a89fc55991d85dc76bd2c1c92b6b46f35a2e9b11 Mon Sep 17 00:00:00 2001 From: SimonR99 Date: Sun, 9 Jun 2024 02:14:56 -0400 Subject: [PATCH 12/12] remove track mass --- src/rove_description/config/test_controllers.yaml | 14 -------------- src/rove_description/urdf/track.urdf.xacro | 10 ---------- 2 files changed, 24 deletions(-) delete mode 100644 src/rove_description/config/test_controllers.yaml diff --git a/src/rove_description/config/test_controllers.yaml b/src/rove_description/config/test_controllers.yaml deleted file mode 100644 index 43b5fdd..0000000 --- a/src/rove_description/config/test_controllers.yaml +++ /dev/null @@ -1,14 +0,0 @@ -controller_manager: - ros__parameters: - update_rate: 10 # Hz - - joint_state_broadcaster: - type: joint_state_broadcaster/JointStateBroadcaster - - velocity_controller: - type: velocity_controllers/JointGroupVelocityController - -velocity_controller: - ros__parameters: - joints: - - track_fl_j \ No newline at end of file diff --git a/src/rove_description/urdf/track.urdf.xacro b/src/rove_description/urdf/track.urdf.xacro index 19d2502..e9fe3f3 100644 --- a/src/rove_description/urdf/track.urdf.xacro +++ b/src/rove_description/urdf/track.urdf.xacro @@ -23,18 +23,12 @@ - - @@ -74,15 +68,11 @@ - - - -