From 2c744de71b9c6206389a98596af31b3c0c6d8fc4 Mon Sep 17 00:00:00 2001 From: GiridharBukka Date: Sun, 13 Nov 2022 08:06:23 +0100 Subject: [PATCH 001/186] created pkg and setup controller from templates rtw --- ackermann_steering_controller/CMakeLists.txt | 119 ++++++ ackermann_steering_controller/LICENSE | 17 + .../ackermann_steering_controller.xml | 8 + .../ackermann_steering_controller.hpp | 119 ++++++ ...kermann_steering_controller_parameters.hpp | 37 ++ .../visibility_control.h | 49 +++ ackermann_steering_controller/package.xml | 39 ++ .../src/ackermann_steering_controller.cpp | 269 +++++++++++++ .../src/ackermann_steering_controller.yaml | 31 ++ .../ackermann_steering_controller_params.yaml | 7 + ...steering_controller_preceeding_params.yaml | 9 + .../test_ackermann_steering_controller.cpp | 376 ++++++++++++++++++ .../test_ackermann_steering_controller.hpp | 266 +++++++++++++ ...kermann_steering_controller_preceeding.cpp | 85 ++++ ...est_load_ackermann_steering_controller.cpp | 41 ++ 15 files changed, 1472 insertions(+) create mode 100644 ackermann_steering_controller/CMakeLists.txt create mode 100644 ackermann_steering_controller/LICENSE create mode 100644 ackermann_steering_controller/ackermann_steering_controller.xml create mode 100644 ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp create mode 100644 ackermann_steering_controller/include/ackermann_steering_controller/validate_ackermann_steering_controller_parameters.hpp create mode 100644 ackermann_steering_controller/include/ackermann_steering_controller/visibility_control.h create mode 100644 ackermann_steering_controller/package.xml create mode 100644 ackermann_steering_controller/src/ackermann_steering_controller.cpp create mode 100644 ackermann_steering_controller/src/ackermann_steering_controller.yaml create mode 100644 ackermann_steering_controller/test/ackermann_steering_controller_params.yaml create mode 100644 ackermann_steering_controller/test/ackermann_steering_controller_preceeding_params.yaml create mode 100644 ackermann_steering_controller/test/test_ackermann_steering_controller.cpp create mode 100644 ackermann_steering_controller/test/test_ackermann_steering_controller.hpp create mode 100644 ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp create mode 100644 ackermann_steering_controller/test/test_load_ackermann_steering_controller.cpp diff --git a/ackermann_steering_controller/CMakeLists.txt b/ackermann_steering_controller/CMakeLists.txt new file mode 100644 index 0000000000..eaf6b0c61d --- /dev/null +++ b/ackermann_steering_controller/CMakeLists.txt @@ -0,0 +1,119 @@ +cmake_minimum_required(VERSION 3.8) +project(ackermann_steering_controller) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +set(THIS_PACKAGE_INCLUDE_DEPENDS + control_msgs + controller_interface + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + std_srvs +) + +find_package(ament_cmake REQUIRED) +find_package(generate_parameter_library REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +cmake_minimum_required(VERSION 3.8) +project(ackermann_steering_controller) + +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) + +# Add ackermann_steering_controller library related compile commands +generate_parameter_library(ackermann_steering_controller_parameters + src/ackermann_steering_controller.yaml + include/ackermann_steering_controller/validate_ackermann_steering_controller_parameters.hpp +) +add_library( + ackermann_steering_controller + SHARED + src/ackermann_steering_controller.cpp +) +target_include_directories(ackermann_steering_controller PRIVATE include) +target_link_libraries(ackermann_steering_controller ackermann_steering_controller_parameters) +ament_target_dependencies(ackermann_steering_controller ${THIS_PACKAGE_INCLUDE_DEPENDS}) +target_compile_definitions(ackermann_steering_controller PRIVATE "ACKERMANN_STEERING_CONTROLLER_BUILDING_DLL") + +pluginlib_export_plugin_description_file( + controller_interface ackermann_steering_controller.xml) + +install( + TARGETS + ackermann_steering_controller + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib +) + +install( + DIRECTORY include/ + DESTINATION include +) + +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) + 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_ackermann_steering_controller test/test_load_ackermann_steering_controller.cpp) + target_include_directories(test_load_ackermann_steering_controller PRIVATE include) + ament_target_dependencies( + test_load_ackermann_steering_controller + controller_manager + hardware_interface + ros2_control_test_assets + ) + + add_rostest_with_parameters_gmock(test_ackermann_steering_controller test/test_ackermann_steering_controller.cpp ${CMAKE_CURRENT_SOURCE_DIR}/test/ackermann_steering_controller_params.yaml) + target_include_directories(test_ackermann_steering_controller PRIVATE include) + target_link_libraries(test_ackermann_steering_controller ackermann_steering_controller) + ament_target_dependencies( + test_ackermann_steering_controller + controller_interface + hardware_interface + ) + + add_rostest_with_parameters_gmock(test_ackermann_steering_controller_preceeding test/test_ackermann_steering_controller_preceeding.cpp ${CMAKE_CURRENT_SOURCE_DIR}/test/ackermann_steering_controller_preceeding_params.yaml) + target_include_directories(test_ackermann_steering_controller_preceeding PRIVATE include) + target_link_libraries(test_ackermann_steering_controller_preceeding ackermann_steering_controller) + ament_target_dependencies( + test_ackermann_steering_controller_preceeding + controller_interface + hardware_interface + ) +endif() + +ament_export_include_directories( + include +) +ament_export_dependencies( + +) +ament_export_libraries( + ackermann_steering_controller +) + +ament_package() diff --git a/ackermann_steering_controller/LICENSE b/ackermann_steering_controller/LICENSE new file mode 100644 index 0000000000..30e8e2ece5 --- /dev/null +++ b/ackermann_steering_controller/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/ackermann_steering_controller/ackermann_steering_controller.xml b/ackermann_steering_controller/ackermann_steering_controller.xml new file mode 100644 index 0000000000..de128d6911 --- /dev/null +++ b/ackermann_steering_controller/ackermann_steering_controller.xml @@ -0,0 +1,8 @@ + + + + AckermannSteeringController ros2_control controller. + + + diff --git a/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp b/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp new file mode 100644 index 0000000000..3d9946451a --- /dev/null +++ b/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp @@ -0,0 +1,119 @@ +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef ACKERMANN_STEERING_CONTROLLER__ACKERMANN_STEERING_CONTROLLER_HPP_ +#define ACKERMANN_STEERING_CONTROLLER__ACKERMANN_STEERING_CONTROLLER_HPP_ + +#include +#include +#include + +#include "controller_interface/chainable_controller_interface.hpp" +#include "ackermann_steering_controller_parameters.hpp" +#include "ackermann_steering_controller/visibility_control.h" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include "realtime_tools/realtime_buffer.h" +#include "realtime_tools/realtime_publisher.h" +#include "std_srvs/srv/set_bool.hpp" + +// TODO(anyone): Replace with controller specific messages +#include "control_msgs/msg/joint_controller_state.hpp" +#include "control_msgs/msg/joint_jog.hpp" + +namespace ackermann_steering_controller +{ +// name constants for state interfaces +static constexpr size_t STATE_MY_ITFS = 0; + +// name constants for command interfaces +static constexpr size_t CMD_MY_ITFS = 0; + +// TODO(anyone: example setup for control mode (usually you will use some enums defined in messages) +enum class control_mode_type : std::uint8_t { + FAST = 0, + SLOW = 1, +}; + +class AckermannSteeringController : public controller_interface::ChainableControllerInterface +{ +public: + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + AckermannSteeringController(); + + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_init() override; + + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + controller_interface::return_type update_reference_from_subscribers() override; + + TEMPLATES__ROS2_CONTROL__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_; + ackermann_steering_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 + TEMPLATES__ROS2_CONTROL__VISIBILITY_LOCAL + void reference_callback(const std::shared_ptr msg); +}; + +} // namespace ackermann_steering_controller + +#endif // ACKERMANN_STEERING_CONTROLLER__ACKERMANN_STEERING_CONTROLLER_HPP_ diff --git a/ackermann_steering_controller/include/ackermann_steering_controller/validate_ackermann_steering_controller_parameters.hpp b/ackermann_steering_controller/include/ackermann_steering_controller/validate_ackermann_steering_controller_parameters.hpp new file mode 100644 index 0000000000..6e3187690a --- /dev/null +++ b/ackermann_steering_controller/include/ackermann_steering_controller/validate_ackermann_steering_controller_parameters.hpp @@ -0,0 +1,37 @@ +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef ACKERMANN_STEERING_CONTROLLER__VALIDATE_ACKERMANN_STEERING_CONTROLLER_PARAMETERS_HPP_ +#define ACKERMANN_STEERING_CONTROLLER__VALIDATE_ACKERMANN_STEERING_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 // ACKERMANN_STEERING_CONTROLLER__VALIDATE_ACKERMANN_STEERING_CONTROLLER_PARAMETERS_HPP_ diff --git a/ackermann_steering_controller/include/ackermann_steering_controller/visibility_control.h b/ackermann_steering_controller/include/ackermann_steering_controller/visibility_control.h new file mode 100644 index 0000000000..b0fe7a41d9 --- /dev/null +++ b/ackermann_steering_controller/include/ackermann_steering_controller/visibility_control.h @@ -0,0 +1,49 @@ +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef ACKERMANN_STEERING_CONTROLLER__VISIBILITY_CONTROL_H_ +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_EXPORT __attribute__((dllexport)) +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_IMPORT __attribute__((dllimport)) +#else +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_EXPORT __declspec(dllexport) +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_IMPORT __declspec(dllimport) +#endif +#ifdef TEMPLATES__ROS2_CONTROL__VISIBILITY_BUILDING_DLL +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC TEMPLATES__ROS2_CONTROL__VISIBILITY_EXPORT +#else +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC TEMPLATES__ROS2_CONTROL__VISIBILITY_IMPORT +#endif +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC_TYPE TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_LOCAL +#else +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_EXPORT __attribute__((visibility("default"))) +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_IMPORT +#if __GNUC__ >= 4 +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC __attribute__((visibility("default"))) +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_LOCAL __attribute__((visibility("hidden"))) +#else +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_LOCAL +#endif +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC_TYPE +#endif + +#endif // ACKERMANN_STEERING_CONTROLLER__VISIBILITY_CONTROL_H_ diff --git a/ackermann_steering_controller/package.xml b/ackermann_steering_controller/package.xml new file mode 100644 index 0000000000..cb4486ea7a --- /dev/null +++ b/ackermann_steering_controller/package.xml @@ -0,0 +1,39 @@ + + + + ackermann_steering_controller + 0.0.0 + DESCRIPTION + giridharvb + 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/ackermann_steering_controller/src/ackermann_steering_controller.cpp b/ackermann_steering_controller/src/ackermann_steering_controller.cpp new file mode 100644 index 0000000000..d3d7a71d36 --- /dev/null +++ b/ackermann_steering_controller/src/ackermann_steering_controller.cpp @@ -0,0 +1,269 @@ +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "ackermann_steering_controller/ackermann_steering_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 = ackermann_steering_controller::AckermannSteeringController::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 ackermann_steering_controller +{ +AckermannSteeringController::AckermannSteeringController() : controller_interface::ChainableControllerInterface() {} + +controller_interface::CallbackReturn AckermannSteeringController::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 AckermannSteeringController::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' (%d) and 'state_joints' (%d) 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(&AckermannSteeringController::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 AckermannSteeringController::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 AckermannSteeringController::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 AckermannSteeringController::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 AckermannSteeringController::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 AckermannSteeringController::on_set_chained_mode(bool chained_mode) +{ + // Always accept switch to/from chained mode + return true || chained_mode; +} + +controller_interface::CallbackReturn AckermannSteeringController::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 AckermannSteeringController::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 AckermannSteeringController::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 AckermannSteeringController::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 ackermann_steering_controller + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + ackermann_steering_controller::AckermannSteeringController, controller_interface::ChainableControllerInterface) diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.yaml b/ackermann_steering_controller/src/ackermann_steering_controller.yaml new file mode 100644 index 0000000000..b9d40ab673 --- /dev/null +++ b/ackermann_steering_controller/src/ackermann_steering_controller.yaml @@ -0,0 +1,31 @@ +ackermann_steering_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/ackermann_steering_controller/test/ackermann_steering_controller_params.yaml b/ackermann_steering_controller/test/ackermann_steering_controller_params.yaml new file mode 100644 index 0000000000..d09ee30a97 --- /dev/null +++ b/ackermann_steering_controller/test/ackermann_steering_controller_params.yaml @@ -0,0 +1,7 @@ +test_ackermann_steering_controller: + ros__parameters: + + joints: + - joint1 + + interface_name: acceleration diff --git a/ackermann_steering_controller/test/ackermann_steering_controller_preceeding_params.yaml b/ackermann_steering_controller/test/ackermann_steering_controller_preceeding_params.yaml new file mode 100644 index 0000000000..0fe973bb8d --- /dev/null +++ b/ackermann_steering_controller/test/ackermann_steering_controller_preceeding_params.yaml @@ -0,0 +1,9 @@ +test_ackermann_steering_controller: + ros__parameters: + joints: + - joint1 + + interface_name: acceleration + + state_joints: + - joint1state diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp new file mode 100644 index 0000000000..d4b896eefb --- /dev/null +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp @@ -0,0 +1,376 @@ +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "test_ackermann_steering_controller.hpp" + +#include +#include +#include +#include +#include + +using ackermann_steering_controller::CMD_MY_ITFS; +using ackermann_steering_controller::control_mode_type; +using ackermann_steering_controller::STATE_MY_ITFS; + +class AckermannSteeringControllerTest : public AckermannSteeringControllerFixture +{ +}; + +TEST_F(AckermannSteeringControllerTest, 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(AckermannSteeringControllerTest, 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(AckermannSteeringControllerTest, 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(AckermannSteeringControllerTest, 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(AckermannSteeringControllerTest, 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(AckermannSteeringControllerTest, 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(AckermannSteeringControllerTest, 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(AckermannSteeringControllerTest, 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(AckermannSteeringControllerTest, 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(AckermannSteeringControllerTest, 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(AckermannSteeringControllerTest, 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(AckermannSteeringControllerTest, 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(AckermannSteeringControllerTest, 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/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp new file mode 100644 index 0000000000..0f9e56f8d9 --- /dev/null +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp @@ -0,0 +1,266 @@ +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TEMPLATES__ROS2_CONTROL__CONTROLLER__TEST_ACKERMANN_STEERING_CONTROLLER_HPP_ +#define TEMPLATES__ROS2_CONTROL__CONTROLLER__TEST_ACKERMANN_STEERING_CONTROLLER_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "ackermann_steering_controller/ackermann_steering_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 = ackermann_steering_controller::AckermannSteeringController::ControllerStateMsg; +using ControllerReferenceMsg = ackermann_steering_controller::AckermannSteeringController::ControllerReferenceMsg; +using ControllerModeSrvType = ackermann_steering_controller::AckermannSteeringController::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 TestableAckermannSteeringController : public ackermann_steering_controller::AckermannSteeringController +{ + FRIEND_TEST(AckermannSteeringControllerTest, all_parameters_set_configure_success); + FRIEND_TEST(AckermannSteeringControllerTest, activate_success); + FRIEND_TEST(AckermannSteeringControllerTest, reactivate_success); + FRIEND_TEST(AckermannSteeringControllerTest, test_setting_slow_mode_service); + FRIEND_TEST(AckermannSteeringControllerTest, test_update_logic_fast); + FRIEND_TEST(AckermannSteeringControllerTest, test_update_logic_slow); + FRIEND_TEST(AckermannSteeringControllerTest, test_update_logic_chainable_fast); + FRIEND_TEST(AckermannSteeringControllerTest, test_update_logic_chainable_slow); + +public: + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override + { + auto ret = ackermann_steering_controller::AckermannSteeringController::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 ackermann_steering_controller::AckermannSteeringController::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 AckermannSteeringControllerFixture : 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_ackermann_steering_controller/commands", rclcpp::SystemDefaultsQoS()); + + service_caller_node_ = std::make_shared("service_caller"); + slow_control_service_client_ = service_caller_node_->create_client( + "/test_ackermann_steering_controller/set_slow_control_mode"); + } + + static void TearDownTestCase() {} + + void TearDown() { controller_.reset(nullptr); } + +protected: + void SetUpController(const std::string controller_name = "test_ackermann_steering_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_ackermann_steering_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_ACKERMANN_STEERING_CONTROLLER_HPP_ diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp b/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp new file mode 100644 index 0000000000..f5d65a8272 --- /dev/null +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp @@ -0,0 +1,85 @@ +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "test_ackermann_steering_controller.hpp" + +#include +#include +#include +#include +#include + +using ackermann_steering_controller::CMD_MY_ITFS; +using ackermann_steering_controller::control_mode_type; +using ackermann_steering_controller::STATE_MY_ITFS; + +class AckermannSteeringControllerTest : public AckermannSteeringControllerFixture +{ +}; + +TEST_F(AckermannSteeringControllerTest, 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(AckermannSteeringControllerTest, 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/ackermann_steering_controller/test/test_load_ackermann_steering_controller.cpp b/ackermann_steering_controller/test/test_load_ackermann_steering_controller.cpp new file mode 100644 index 0000000000..a794fc3da0 --- /dev/null +++ b/ackermann_steering_controller/test/test_load_ackermann_steering_controller.cpp @@ -0,0 +1,41 @@ +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "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(TestLoadAckermannSteeringController, 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_ackermann_steering_controller", "ackermann_steering_controller/AckermannSteeringController")); + + rclcpp::shutdown(); +} From e3be21f294c66a714c9ab5496692770a230bcbe3 Mon Sep 17 00:00:00 2001 From: GiridharBukka Date: Sun, 13 Nov 2022 08:46:28 +0100 Subject: [PATCH 002/186] recreated controller setup outside docker to use rtw branch with ref_timeout updated template/ updated .hpp of controller --- .../ackermann_steering_controller.hpp | 88 +++- .../src/ackermann_steering_controller.cpp | 93 ++-- .../src/ackermann_steering_controller.yaml | 8 + .../ackermann_steering_controller_params.yaml | 2 + ...steering_controller_preceeding_params.yaml | 2 + .../test_ackermann_steering_controller.cpp | 409 ++++++++++++++---- .../test_ackermann_steering_controller.hpp | 32 +- 7 files changed, 499 insertions(+), 135 deletions(-) diff --git a/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp b/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp index 3d9946451a..cbd78e4ab7 100644 --- a/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp +++ b/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp @@ -15,11 +15,17 @@ #ifndef ACKERMANN_STEERING_CONTROLLER__ACKERMANN_STEERING_CONTROLLER_HPP_ #define ACKERMANN_STEERING_CONTROLLER__ACKERMANN_STEERING_CONTROLLER_HPP_ +#include +#include #include +#include #include #include +#include "ros2_ackermann_cont/odometry.h" + #include "controller_interface/chainable_controller_interface.hpp" +#include "hardware_interface/handle.hpp" #include "ackermann_steering_controller_parameters.hpp" #include "ackermann_steering_controller/visibility_control.h" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" @@ -29,22 +35,22 @@ #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" +#include "geometry_msgs/msg/twist.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "tf2_msgs/msg/tf_message.hpp" +#include "nav_msgs/msg/odometry.hpp" + namespace ackermann_steering_controller { // name constants for state interfaces -static constexpr size_t STATE_MY_ITFS = 0; +static constexpr size_t NR_STATE_ITFS = 2; // name constants for command interfaces -static constexpr size_t CMD_MY_ITFS = 0; +static constexpr size_t NR_CMD_ITFS = 2; -// 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, -}; +// name constants for reference interfaces +static constexpr size_t NR_REF_ITFS = 2; class AckermannSteeringController : public controller_interface::ChainableControllerInterface { @@ -74,44 +80,88 @@ class AckermannSteeringController : public controller_interface::ChainableContro const rclcpp_lifecycle::State & previous_state) override; TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC - controller_interface::return_type update_reference_from_subscribers() override; + controller_interface::return_type update_reference_from_subscribers( + const rclcpp::Time & time, const rclcpp::Duration & period) override; TEMPLATES__ROS2_CONTROL__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; + using ControllerReferenceMsg = geometry_msgs::msg::TwistStamped; + using ControllerStateMsgOdom = nav_msgs::msg::Odometry; + using ControllerStateMsgTf = tf2_msgs::msg::TFMessage; protected: std::shared_ptr param_listener_; ackermann_steering_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::Duration ref_timeout_ = rclcpp::Duration::from_seconds(0.0); // 0ms - rclcpp::Service::SharedPtr set_slow_control_mode_service_; - realtime_tools::RealtimeBuffer control_mode_; - using ControllerStatePublisher = realtime_tools::RealtimePublisher; + using ControllerStatePublisherOdom = realtime_tools::RealtimePublisher; + using ControllerStatePublisherTf = realtime_tools::RealtimePublisher; - rclcpp::Publisher::SharedPtr s_publisher_; - std::unique_ptr state_publisher_; + rclcpp::Publisher::SharedPtr odom_s_publisher_; + rclcpp::Publisher::SharedPtr tf_odom_s_publisher_; + + std::unique_ptr rt_odom_state_publisher_; + std::unique_ptr rt_tf_odom_state_publisher_; // override methods from ChainableControllerInterface std::vector on_export_reference_interfaces() override; bool on_set_chained_mode(bool chained_mode) override; + struct WheelHandle + { + std::reference_wrapper feedback; + // std::reference_wrapper velocity; + }; + + std::vector registered_rear_wheel_handles_; + std::vector registered_front_wheel_handles_; + + /// Odometry related: + rclcpp::Duration publish_period_ = rclcpp::Duration::from_nanoseconds(0); + bool open_loop_; + rclcpp::Time previous_publish_timestamp_{0}; + + /// Velocity command related: + struct Commands + { + double lin; + double ang; + rclcpp::Time stamp; + + Commands() : lin(0.0), ang(0.0), stamp(0.0) {} + }; + + // Odometry related: + Odometry odometry_; + private: // callback for topic interface TEMPLATES__ROS2_CONTROL__VISIBILITY_LOCAL void reference_callback(const std::shared_ptr msg); + + /// Frame to use for the robot base: + std::string base_frame_id_; + + /// Frame to use for odometry and odom tf: + std::string odom_frame_id_; + + /// Whether to publish odometry to tf or not: + bool enable_odom_tf_; + + // store last velocity + double last_linear_velocity_; + double last_angular_velocity_; + }; } // namespace ackermann_steering_controller diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.cpp b/ackermann_steering_controller/src/ackermann_steering_controller.cpp index d3d7a71d36..aee3b8e92e 100644 --- a/ackermann_steering_controller/src/ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/src/ackermann_steering_controller.cpp @@ -41,8 +41,10 @@ using ControllerReferenceMsg = ackermann_steering_controller::AckermannSteeringC // called from RT control loop void reset_controller_reference_msg( - const std::shared_ptr & msg, const std::vector & joint_names) + const std::shared_ptr & msg, const std::vector & joint_names, + const std::shared_ptr & node) { + msg->header.stamp = node->now(); 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()); @@ -94,12 +96,13 @@ controller_interface::CallbackReturn AckermannSteeringController::on_configure( subscribers_qos.best_effort(); // Reference Subscriber + ref_timeout_ = rclcpp::Duration::from_seconds(params_.reference_timeout); ref_subscriber_ = get_node()->create_subscription( - "~/reference", subscribers_qos, + "~/commands", subscribers_qos, std::bind(&AckermannSteeringController::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_.joints, get_node()); input_ref_.writeFromNonRT(msg); auto set_slow_mode_service_callback = @@ -139,6 +142,37 @@ controller_interface::CallbackReturn AckermannSteeringController::on_configure( return controller_interface::CallbackReturn::SUCCESS; } +void AckermannSteeringController::reference_callback(const std::shared_ptr msg) +{ + // if no timestamp provided use current time for command timestamp + if (msg->header.stamp.sec == 0 && msg->header.stamp.nanosec == 0u) { + RCLCPP_WARN( + get_node()->get_logger(), + "Timestamp in header is missing, using current time as command timestamp."); + msg->header.stamp = get_node()->now(); + } + const auto age_of_last_command = get_node()->now() - msg->header.stamp; + if (msg->joint_names.size() == params_.joints.size()) { + if (ref_timeout_ == rclcpp::Duration::from_seconds(0) || age_of_last_command <= ref_timeout_) { + input_ref_.writeFromNonRT(msg); + } else { + RCLCPP_ERROR( + get_node()->get_logger(), + "Received message has timestamp %.10f older for %.10f which is more then allowed timeout " + "(%.4f).", + rclcpp::Time(msg->header.stamp).seconds(), age_of_last_command.seconds(), + ref_timeout_.seconds()); + } + + } else { + RCLCPP_ERROR( + get_node()->get_logger(), + "Received %zu , but expected %zu joints in command. Ignoring message.", + msg->joint_names.size(), params_.joints.size()); + + } +} + controller_interface::InterfaceConfiguration AckermannSteeringController::command_interface_configuration() const { controller_interface::InterfaceConfiguration command_interfaces_config; @@ -165,17 +199,7 @@ controller_interface::InterfaceConfiguration AckermannSteeringController::state_ return state_interfaces_config; } -void AckermannSteeringController::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 AckermannSteeringController::on_export_reference_interfaces() { @@ -203,7 +227,7 @@ controller_interface::CallbackReturn AckermannSteeringController::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()), state_joints_, get_node()); return controller_interface::CallbackReturn::SUCCESS; } @@ -219,16 +243,27 @@ controller_interface::CallbackReturn AckermannSteeringController::on_deactivate( return controller_interface::CallbackReturn::SUCCESS; } -controller_interface::return_type AckermannSteeringController::update_reference_from_subscribers() +controller_interface::return_type AckermannSteeringController::update_reference_from_subscribers( + const rclcpp::Time & time, const rclcpp::Duration & /*period*/) { auto current_ref = input_ref_.readFromRT(); + const auto age_of_last_command = time - (*current_ref)->header.stamp; // 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]; - + // send message only if there is no timeout + if (age_of_last_command <= ref_timeout_ || ref_timeout_ == rclcpp::Duration::from_seconds(0)) { + if (!std::isnan((*current_ref)->displacements[i])) { + if (*(control_mode_.readFromRT()) == control_mode_type::SLOW) { + (*current_ref)->displacements[i] /= 2; + } + reference_interfaces_[i] = (*current_ref)->displacements[i]; + if (ref_timeout_ == rclcpp::Duration::from_seconds(0)){ + (*current_ref)->displacements[i] = std::numeric_limits::quiet_NaN(); + } + } + } else { (*current_ref)->displacements[i] = std::numeric_limits::quiet_NaN(); } } @@ -238,15 +273,24 @@ controller_interface::return_type AckermannSteeringController::update_reference_ controller_interface::return_type AckermannSteeringController::update_and_write_commands( const rclcpp::Time & time, const rclcpp::Duration & /*period*/) { + auto current_ref = input_ref_.readFromRT(); + const auto age_of_last_command = time - (*current_ref)->header.stamp; + // 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; + // send message only if there is no timeout + if (age_of_last_command <= ref_timeout_ || ref_timeout_ == rclcpp::Duration::from_seconds(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]); + if (ref_timeout_ == rclcpp::Duration::from_seconds(0)){ + reference_interfaces_[i] = std::numeric_limits::quiet_NaN(); + } } - command_interfaces_[i].set_value(reference_interfaces_[i]); - + } else { reference_interfaces_[i] = std::numeric_limits::quiet_NaN(); } } @@ -254,7 +298,6 @@ controller_interface::return_type AckermannSteeringController::update_and_write_ 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(); } diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.yaml b/ackermann_steering_controller/src/ackermann_steering_controller.yaml index b9d40ab673..ff6086863c 100644 --- a/ackermann_steering_controller/src/ackermann_steering_controller.yaml +++ b/ackermann_steering_controller/src/ackermann_steering_controller.yaml @@ -29,3 +29,11 @@ ackermann_steering_controller: forbidden_interface_name_prefix: null } } + reference_timeout: { + type: double, + default_value: 0.0, + description: "Timeout for controller references after which they will be reset. This is especially useful for controllers that can cause unwanted and dangerous behaviour if reference is not reset, e.g., velocity controllers. If value is 0 the reference is reset after each run.", +# validation: { +# gt_eq<>: 0.0, +# } + } diff --git a/ackermann_steering_controller/test/ackermann_steering_controller_params.yaml b/ackermann_steering_controller/test/ackermann_steering_controller_params.yaml index d09ee30a97..f9d9e7c159 100644 --- a/ackermann_steering_controller/test/ackermann_steering_controller_params.yaml +++ b/ackermann_steering_controller/test/ackermann_steering_controller_params.yaml @@ -5,3 +5,5 @@ test_ackermann_steering_controller: - joint1 interface_name: acceleration + + reference_timeout: 0.1 diff --git a/ackermann_steering_controller/test/ackermann_steering_controller_preceeding_params.yaml b/ackermann_steering_controller/test/ackermann_steering_controller_preceeding_params.yaml index 0fe973bb8d..f19ef15914 100644 --- a/ackermann_steering_controller/test/ackermann_steering_controller_preceeding_params.yaml +++ b/ackermann_steering_controller/test/ackermann_steering_controller_preceeding_params.yaml @@ -7,3 +7,5 @@ test_ackermann_steering_controller: state_joints: - joint1state + + reference_timeout: 0.1 diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp index d4b896eefb..1d89527dcf 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp @@ -35,6 +35,7 @@ TEST_F(AckermannSteeringControllerTest, all_parameters_set_configure_success) ASSERT_TRUE(controller_->params_.joints.empty()); ASSERT_TRUE(controller_->params_.state_joints.empty()); ASSERT_TRUE(controller_->params_.interface_name.empty()); + ASSERT_EQ(controller_->params_.reference_timeout, 0.0); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); @@ -42,6 +43,7 @@ TEST_F(AckermannSteeringControllerTest, all_parameters_set_configure_success) ASSERT_TRUE(controller_->params_.state_joints.empty()); ASSERT_THAT(controller_->state_joints_, testing::ElementsAreArray(joint_names_)); ASSERT_EQ(controller_->params_.interface_name, interface_name_); + ASSERT_EQ(controller_->params_.reference_timeout, 0.1); } TEST_F(AckermannSteeringControllerTest, check_exported_intefaces) @@ -109,7 +111,12 @@ TEST_F(AckermannSteeringControllerTest, update_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_->update_reference_from_subscribers( + controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + ASSERT_EQ( + controller_->update_and_write_commands( + controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); } @@ -135,7 +142,12 @@ TEST_F(AckermannSteeringControllerTest, reactivate_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_->update_reference_from_subscribers( + controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + ASSERT_EQ( + controller_->update_and_write_commands( + controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); } @@ -165,8 +177,8 @@ TEST_F(AckermannSteeringControllerTest, test_setting_slow_mode_service) ASSERT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::FAST); } -TEST_F(AckermannSteeringControllerTest, test_update_logic_fast) -{ +TEST_F(AckermannSteeringControllerTest, test_update_logic_chainable_fast) +{// 1. ageref_timeout SetUpController(); rclcpp::executors::MultiThreadedExecutor executor; executor.add_node(controller_->get_node()->get_node_base_interface()); @@ -179,29 +191,67 @@ TEST_F(AckermannSteeringControllerTest, test_update_logic_fast) // set command statically static constexpr double TEST_DISPLACEMENT = 23.24; + joint_command_values_[STATE_MY_ITFS] = 111; std::shared_ptr msg = std::make_shared(); + msg->header.stamp = controller_->get_node()->now() - controller_->ref_timeout_ - rclcpp::Duration::from_seconds(0.1); 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); + const auto age_of_last_command = controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; + EXPECT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::FAST); + ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_); + ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT); + ASSERT_EQ( + controller_->update_reference_from_subscribers( + controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); ASSERT_EQ( - controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_->update_and_write_commands( + controller_->get_node()->now(), 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()); + EXPECT_EQ(joint_command_values_[STATE_MY_ITFS], 111); + ASSERT_TRUE(std::isnan(controller_->reference_interfaces_[0])); for (const auto & interface : controller_->reference_interfaces_) { EXPECT_TRUE(std::isnan(interface)); } + + std::shared_ptr msg_2 = std::make_shared(); + msg_2->header.stamp = controller_->get_node()->now() - rclcpp::Duration::from_seconds(0.01); + msg_2->joint_names = joint_names_; + msg_2->displacements.resize(joint_names_.size(), TEST_DISPLACEMENT); + msg_2->velocities.resize(joint_names_.size(), std::numeric_limits::quiet_NaN()); + msg_2->duration = std::numeric_limits::quiet_NaN(); + controller_->input_ref_.writeFromNonRT(msg_2); + const auto age_of_last_command_2 = controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; + EXPECT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::FAST); + + ASSERT_TRUE(age_of_last_command_2 <= controller_->ref_timeout_); + ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT); + ASSERT_EQ( + controller_->update_reference_from_subscribers( + controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + ASSERT_EQ( + controller_->update_and_write_commands( + controller_->get_node()->now(), 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);//exact value + EXPECT_NE(joint_command_values_[STATE_MY_ITFS], 111); + ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT); + for (const auto & interface : controller_->reference_interfaces_) { + EXPECT_FALSE(std::isnan(interface)); + } } -TEST_F(AckermannSteeringControllerTest, test_update_logic_slow) -{ +TEST_F(AckermannSteeringControllerTest, test_update_logic_chainable_slow) +{// 1. ageref_timeout SetUpController(); rclcpp::executors::MultiThreadedExecutor executor; executor.add_node(controller_->get_node()->get_node_base_interface()); @@ -214,69 +264,215 @@ TEST_F(AckermannSteeringControllerTest, test_update_logic_slow) // set command statically static constexpr double TEST_DISPLACEMENT = 23.24; + joint_command_values_[STATE_MY_ITFS] = 111; std::shared_ptr msg = std::make_shared(); - // When slow mode is enabled command ends up being half of the value + msg->header.stamp = controller_->get_node()->now() - controller_->ref_timeout_ - rclcpp::Duration::from_seconds(0.1); 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); + const auto age_of_last_command = controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; controller_->control_mode_.writeFromNonRT(control_mode_type::SLOW); EXPECT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::SLOW); + //age_of_last_command > ref_timeout_ + ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_); ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT); ASSERT_EQ( - controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_->update_reference_from_subscribers( + controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + ASSERT_EQ( + controller_->update_and_write_commands( + controller_->get_node()->now(), 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()); + EXPECT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::SLOW); + EXPECT_EQ(joint_command_values_[STATE_MY_ITFS], 111); + ASSERT_TRUE(std::isnan(controller_->reference_interfaces_[0])); for (const auto & interface : controller_->reference_interfaces_) { EXPECT_TRUE(std::isnan(interface)); } + + std::shared_ptr msg_2 = std::make_shared(); + msg_2->header.stamp = controller_->get_node()->now() - rclcpp::Duration::from_seconds(0.01); + msg_2->joint_names = joint_names_; + msg_2->displacements.resize(joint_names_.size(), TEST_DISPLACEMENT); + msg_2->velocities.resize(joint_names_.size(), std::numeric_limits::quiet_NaN()); + msg_2->duration = std::numeric_limits::quiet_NaN(); + controller_->input_ref_.writeFromNonRT(msg_2); + const auto age_of_last_command_2 = controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; + //age_of_last_command_2 < ref_timeout_ + ASSERT_TRUE(age_of_last_command_2 <= controller_->ref_timeout_); + ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT); + ASSERT_EQ( + controller_->update_reference_from_subscribers( + controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + ASSERT_EQ( + controller_->update_and_write_commands( + controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::SLOW); + EXPECT_EQ(joint_command_values_[STATE_MY_ITFS], TEST_DISPLACEMENT/4);//exact value + EXPECT_NE(joint_command_values_[STATE_MY_ITFS], 111); + ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT/2); + for (const auto & interface : controller_->reference_interfaces_) { + EXPECT_FALSE(std::isnan(interface)); + } } -TEST_F(AckermannSteeringControllerTest, test_update_logic_chainable_fast) +TEST_F(AckermannSteeringControllerTest, 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_reference_from_subscribers( + controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + ASSERT_EQ( + controller_->update_and_write_commands( + controller_->get_node()->now(), 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(AckermannSteeringControllerTest, receive_message_and_publish_updated_status) { 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_reference_from_subscribers( + controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + ASSERT_EQ( + controller_->update_and_write_commands( + controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); +//here + ControllerStateMsg msg; + subscribe_and_get_messages(msg); + ASSERT_EQ(msg.set_point, 101.101); + + publish_commands(controller_->get_node()->now()); + ASSERT_TRUE(controller_->wait_for_commands(executor)); + + ASSERT_EQ( + controller_->update_reference_from_subscribers( + controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); ASSERT_EQ( - controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_->update_and_write_commands( + controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); +//here + EXPECT_EQ(joint_command_values_[CMD_MY_ITFS], 0.45); - 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)); - } + subscribe_and_get_messages(msg); + + ASSERT_EQ(msg.set_point, 0.45); } -TEST_F(AckermannSteeringControllerTest, test_update_logic_chainable_slow) +TEST_F(AckermannSteeringControllerTest, test_message_timeout) +{ + 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); + +// try to set command with time before timeout - command is not updated + auto reference = controller_->input_ref_.readFromNonRT(); + auto old_timestamp = (*reference)->header.stamp; + EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->joint_names.size(), joint_names_.size()); + EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->joint_names[0], joint_names_[0]); + EXPECT_TRUE(std::isnan((*reference)->displacements[0])); + EXPECT_TRUE(std::isnan((*reference)->velocities[0])); + EXPECT_TRUE(std::isnan((*reference)->duration)); + publish_commands(controller_->get_node()->now() - controller_->ref_timeout_ - rclcpp::Duration::from_seconds(0.1)); + ASSERT_TRUE(controller_->wait_for_commands(executor)); + ASSERT_EQ(old_timestamp, (*(controller_->input_ref_.readFromNonRT()))->header.stamp); + EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->joint_names.size(), joint_names_.size()); + EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->joint_names[0], joint_names_[0]); + EXPECT_TRUE(std::isnan((*reference)->displacements[0])); + EXPECT_TRUE(std::isnan((*reference)->velocities[0])); + EXPECT_TRUE(std::isnan((*reference)->duration)); +} + + + +TEST_F(AckermannSteeringControllerTest, test_message_wrong_num_joints) +{ + 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); + + // try to set command with time before timeout - command is not updated + auto reference = controller_->input_ref_.readFromNonRT(); + auto old_timestamp = (*reference)->header.stamp; + EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->joint_names.size(), joint_names_.size()); + EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->joint_names[0], joint_names_[0]); + EXPECT_TRUE(std::isnan((*reference)->displacements[0])); + EXPECT_TRUE(std::isnan((*reference)->velocities[0])); + EXPECT_TRUE(std::isnan((*reference)->duration)); + publish_commands(controller_->get_node()->now(), {"joint1","joint2"}); + ASSERT_TRUE(controller_->wait_for_commands(executor)); + EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->joint_names.size(), joint_names_.size()); + EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->joint_names[0], joint_names_[0]); + EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->displacements[0])); + EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->velocities[0])); + EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->duration)); +} + +TEST_F(AckermannSteeringControllerTest, test_message_accepted) +{ + 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); + + // try to set command with time before timeout - command is not updated + auto reference = controller_->input_ref_.readFromNonRT(); + EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->joint_names.size(), joint_names_.size()); + EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->joint_names[0], joint_names_[0]); + EXPECT_TRUE(std::isnan((*reference)->displacements[0])); + EXPECT_TRUE(std::isnan((*reference)->velocities[0])); + EXPECT_TRUE(std::isnan((*reference)->duration)); + publish_commands(controller_->get_node()->now()); + ASSERT_TRUE(controller_->wait_for_commands(executor)); + EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->joint_names.size(), joint_names_.size()); + EXPECT_NE((*(controller_->input_ref_.readFromNonRT()))->joint_names[0], joint_names_[0]); + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->displacements[0])); + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->velocities[0])); + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->duration)); + EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->displacements[0], 0.45); + EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->velocities[0], 0.0); + EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->duration, 1.25); + +} + +TEST_F(AckermannSteeringControllerTest, test_update_logic) { SetUpController(); rclcpp::executors::MultiThreadedExecutor executor; @@ -284,88 +480,131 @@ TEST_F(AckermannSteeringControllerTest, test_update_logic_chainable_slow) 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()); + + auto reference = controller_->input_ref_.readFromNonRT(); // set command statically static constexpr double TEST_DISPLACEMENT = 23.24; + joint_command_values_[STATE_MY_ITFS] = 111; std::shared_ptr msg = std::make_shared(); - // When slow mode is enabled command ends up being half of the value + msg->header.stamp = controller_->get_node()->now() - controller_->ref_timeout_ - rclcpp::Duration::from_seconds(0.1); 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; + const auto age_of_last_command = controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; - EXPECT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::SLOW); + ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_); ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT); ASSERT_EQ( - controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_->update_reference_from_subscribers( + controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + ASSERT_EQ( + controller_->update_and_write_commands( + controller_->get_node()->now(), 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)); - } + EXPECT_EQ(joint_command_values_[STATE_MY_ITFS], 111); + ASSERT_TRUE(std::isnan(controller_->reference_interfaces_[0])); + + std::shared_ptr msg_2 = std::make_shared(); + msg_2->header.stamp = controller_->get_node()->now(); + msg_2->joint_names = joint_names_; + msg_2->displacements.resize(joint_names_.size(), TEST_DISPLACEMENT); + msg_2->velocities.resize(joint_names_.size(), std::numeric_limits::quiet_NaN()); + msg_2->duration = std::numeric_limits::quiet_NaN(); + controller_->input_ref_.writeFromNonRT(msg_2); + const auto age_of_last_command_2 = controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; + + ASSERT_TRUE(age_of_last_command_2 <= controller_->ref_timeout_); + ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT); + ASSERT_EQ( + controller_->update_reference_from_subscribers( + controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + ASSERT_EQ( + controller_->update_and_write_commands( + controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_EQ(joint_command_values_[STATE_MY_ITFS], TEST_DISPLACEMENT);//exact value + EXPECT_NE(joint_command_values_[STATE_MY_ITFS], 111); + ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT); } -TEST_F(AckermannSteeringControllerTest, publish_status_success) + +TEST_F(AckermannSteeringControllerTest, test_ref_timeout_zero_for_update) { SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + executor.add_node(service_caller_node_->get_node_base_interface()); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + auto reference = controller_->input_ref_.readFromNonRT(); + + // set command statically + static constexpr double TEST_DISPLACEMENT = 23.24; + controller_->ref_timeout_= rclcpp::Duration::from_seconds(0.0); + std::shared_ptr msg = std::make_shared(); + msg->header.stamp = controller_->get_node()->now() - rclcpp::Duration::from_seconds(0.0); + 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); + const auto age_of_last_command = controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; + + ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT); ASSERT_EQ( - controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_->update_reference_from_subscribers( + controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + ASSERT_EQ( + controller_->update_and_write_commands( + controller_->get_node()->now(), 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); + EXPECT_EQ(joint_command_values_[STATE_MY_ITFS], TEST_DISPLACEMENT); + ASSERT_NE((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT); + ASSERT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->displacements[0])); } -TEST_F(AckermannSteeringControllerTest, receive_message_and_publish_updated_status) +TEST_F(AckermannSteeringControllerTest, test_ref_timeout_zero_for_reference_callback) { SetUpController(); rclcpp::executors::MultiThreadedExecutor executor; executor.add_node(controller_->get_node()->get_node_base_interface()); + executor.add_node(service_caller_node_->get_node_base_interface()); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - 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(); + EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->joint_names.size(), joint_names_.size()); + EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->joint_names[0], joint_names_[0]); + EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->displacements[0])); + EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->velocities[0])); + EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->duration)); + controller_->ref_timeout_= rclcpp::Duration::from_seconds(0.0); + //reference_callback() is called implicitly when publish_commands() is called. + publish_commands(controller_->get_node()->now()); 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); + EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->joint_names.size(), joint_names_.size()); + EXPECT_NE((*(controller_->input_ref_.readFromNonRT()))->joint_names[0], joint_names_[0]); + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->displacements[0])); + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->velocities[0])); + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->duration)); + EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->displacements[0], 0.45); + EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->velocities[0], 0.0); + EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->duration, 1.25); } + int main(int argc, char ** argv) { ::testing::InitGoogleTest(&argc, argv); diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp index 0f9e56f8d9..ff3c61f097 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp @@ -49,13 +49,22 @@ constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR; class TestableAckermannSteeringController : public ackermann_steering_controller::AckermannSteeringController { FRIEND_TEST(AckermannSteeringControllerTest, all_parameters_set_configure_success); + FRIEND_TEST(AckermannSteeringControllerTest, check_exported_intefaces); FRIEND_TEST(AckermannSteeringControllerTest, activate_success); + FRIEND_TEST(AckermannSteeringControllerTest, update_success); + FRIEND_TEST(AckermannSteeringControllerTest, deactivate_success); FRIEND_TEST(AckermannSteeringControllerTest, reactivate_success); FRIEND_TEST(AckermannSteeringControllerTest, test_setting_slow_mode_service); - FRIEND_TEST(AckermannSteeringControllerTest, test_update_logic_fast); - FRIEND_TEST(AckermannSteeringControllerTest, test_update_logic_slow); FRIEND_TEST(AckermannSteeringControllerTest, test_update_logic_chainable_fast); FRIEND_TEST(AckermannSteeringControllerTest, test_update_logic_chainable_slow); + FRIEND_TEST(AckermannSteeringControllerTest, publish_status_success); + FRIEND_TEST(AckermannSteeringControllerTest, receive_message_and_publish_updated_status); + FRIEND_TEST(AckermannSteeringControllerTest, test_message_timeout); + FRIEND_TEST(AckermannSteeringControllerTest, test_message_wrong_num_joints); + FRIEND_TEST(AckermannSteeringControllerTest, test_message_accepted); + FRIEND_TEST(AckermannSteeringControllerTest, test_update_logic); + FRIEND_TEST(AckermannSteeringControllerTest, test_ref_timeout_zero_for_update); + FRIEND_TEST(AckermannSteeringControllerTest, test_ref_timeout_zero_for_reference_callback); public: controller_interface::CallbackReturn on_configure( @@ -172,7 +181,12 @@ class AckermannSteeringControllerFixture : public ::testing::Test // call update to publish the test value ASSERT_EQ( - controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_->update_reference_from_subscribers( + controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + ASSERT_EQ( + controller_->update_and_write_commands( + controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); // call update to publish the test value @@ -181,7 +195,10 @@ class AckermannSteeringControllerFixture : public ::testing::Test 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)); + controller_->update_reference_from_subscribers( + controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)); + controller_->update_and_write_commands( + controller_->get_node()->now(), 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; @@ -197,7 +214,7 @@ class AckermannSteeringControllerFixture : public ::testing::Test // TODO(anyone): add/remove arguments as it suites your command message type void publish_commands( - const std::vector & displacements = {0.45}, + const rclcpp::Time & stamp, const std::vector & joint_names = {"joint1_test"}, 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) { @@ -216,7 +233,8 @@ class AckermannSteeringControllerFixture : public ::testing::Test wait_for_topic(command_publisher_->get_topic_name()); ControllerReferenceMsg msg; - msg.joint_names = joint_names_; + msg.header.stamp = stamp; + msg.joint_names = joint_names; msg.displacements = displacements; msg.velocities = velocities; msg.duration = duration; @@ -255,6 +273,8 @@ class AckermannSteeringControllerFixture : public ::testing::Test std::vector state_itfs_; std::vector command_itfs_; + double ref_timeout_ = 0.2; + // Test related parameters std::unique_ptr controller_; rclcpp::Node::SharedPtr command_publisher_node_; From c9b1dad8da8e307ccc91b3db8a7df73711b67f0a Mon Sep 17 00:00:00 2001 From: GiridharBukka Date: Sun, 13 Nov 2022 09:22:20 +0100 Subject: [PATCH 003/186] modified .yaml and controller methods content --- .../src/ackermann_steering_controller.cpp | 265 +++++++++++------- .../src/ackermann_steering_controller.yaml | 170 +++++++++-- 2 files changed, 299 insertions(+), 136 deletions(-) diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.cpp b/ackermann_steering_controller/src/ackermann_steering_controller.cpp index aee3b8e92e..27f29d7539 100644 --- a/ackermann_steering_controller/src/ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/src/ackermann_steering_controller.cpp @@ -20,6 +20,10 @@ #include #include "controller_interface/helpers.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "tf2/transform_datatypes.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" namespace { // utility @@ -41,14 +45,16 @@ using ControllerReferenceMsg = ackermann_steering_controller::AckermannSteeringC // called from RT control loop void reset_controller_reference_msg( - const std::shared_ptr & msg, const std::vector & joint_names, + const std::shared_ptr & msg, const std::shared_ptr & node) { msg->header.stamp = node->now(); - 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(); + msg->twist.linear.x = std::numeric_limits::quiet_NaN(); + msg->twist.linear.y = std::numeric_limits::quiet_NaN(); + msg->twist.linear.z = std::numeric_limits::quiet_NaN(); + msg->twist.angular.x = std::numeric_limits::quiet_NaN(); + msg->twist.angular.y = std::numeric_limits::quiet_NaN(); + msg->twist.angular.z = std::numeric_limits::quiet_NaN(); } } // namespace @@ -59,7 +65,6 @@ AckermannSteeringController::AckermannSteeringController() : controller_interfac controller_interface::CallbackReturn AckermannSteeringController::on_init() { - control_mode_.initRT(control_mode_type::FAST); try { param_listener_ = std::make_shared(get_node()); @@ -76,19 +81,10 @@ controller_interface::CallbackReturn AckermannSteeringController::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' (%d) and 'state_joints' (%d) parameters has to be the same!", - params_.joints.size(), state_joints_.size()); - return CallbackReturn::FAILURE; - } + const double wheel_seperation = params_.wheel_separation_multiplier * params_.wheel_separation; + const double wheel_radius = params_.wheel_radius_multiplier * params_.wheel_radius; + odometry_.setWheelParams(wheel_seperation, wheel_radius); + odometry_.setVelocityRollingWindowSize(params_.velocity_rolling_window_size); // topics QoS auto subscribers_qos = rclcpp::SystemDefaultsQoS(); @@ -98,34 +94,18 @@ controller_interface::CallbackReturn AckermannSteeringController::on_configure( // Reference Subscriber ref_timeout_ = rclcpp::Duration::from_seconds(params_.reference_timeout); ref_subscriber_ = get_node()->create_subscription( - "~/commands", subscribers_qos, + "~/reference", subscribers_qos, std::bind(&AckermannSteeringController::reference_callback, this, std::placeholders::_1)); std::shared_ptr msg = std::make_shared(); - reset_controller_reference_msg(msg, params_.joints, get_node()); + reset_controller_reference_msg(msg,get_node()); 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_); + odom_s_publisher_ = + get_node()->create_publisher("~/odometry", rclcpp::SystemDefaultsQoS()); + rt_odom_state_publisher_ = std::make_unique(odom_s_publisher_); } catch (const std::exception & e) { fprintf( stderr, "Exception thrown during publisher creation at configure stage with message : %s \n", @@ -134,9 +114,45 @@ controller_interface::CallbackReturn AckermannSteeringController::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_->unlock(); + rt_odom_state_publisher_->lock(); + rt_odom_state_publisher_->msg_.header.frame_id = params_.rear_wheel_name; + rt_odom_state_publisher_->msg_.header.frame_id = params_.odom_frame_id; + rt_odom_state_publisher_->msg_.child_frame_id = params_.base_frame_id; + rt_odom_state_publisher_->msg_.pose.pose.position.z = 0; + + auto & covariance = rt_odom_state_publisher_->msg_.twist.covariance; + constexpr size_t NUM_DIMENSIONS = 6; + for (size_t index = 0; index < 6; ++index) + { + // 0, 7, 14, 21, 28, 35 + const size_t diagonal_index = NUM_DIMENSIONS * index + index; + covariance[diagonal_index] = params_.pose_covariance_diagonal[index]; + covariance[diagonal_index] = params_.twist_covariance_diagonal[index]; + } + rt_odom_state_publisher_->unlock(); + + try { + // Tf State publisher + tf_odom_s_publisher_ = get_node()->create_publisher( + "~/tf_odometry", rclcpp::SystemDefaultsQoS()); + rt_tf_odom_state_publisher_ = std::make_unique(tf_odom_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; + } + + rt_tf_odom_state_publisher_->lock(); + rt_tf_odom_state_publisher_->msg_.transforms.resize(1); + rt_tf_odom_state_publisher_->msg_.transforms[0].header.frame_id = params_.rear_wheel_name; + rt_tf_odom_state_publisher_->msg_.transforms[0].header.frame_id = params_.odom_frame_id; + rt_tf_odom_state_publisher_->msg_.transforms[0].child_frame_id = params_.base_frame_id; + rt_tf_odom_state_publisher_->msg_.transforms[0].transform.translation.z = 0.0; + rt_tf_odom_state_publisher_->unlock(); + + // calculation publication period of odometry and tf odometry messages + publish_period_ = rclcpp::Duration::from_seconds(1.0 / params_.publish_rate); RCLCPP_INFO(get_node()->get_logger(), "configure successful"); return controller_interface::CallbackReturn::SUCCESS; @@ -152,7 +168,7 @@ void AckermannSteeringController::reference_callback(const std::shared_ptrheader.stamp = get_node()->now(); } const auto age_of_last_command = get_node()->now() - msg->header.stamp; - if (msg->joint_names.size() == params_.joints.size()) { + if (ref_timeout_ == rclcpp::Duration::from_seconds(0) || age_of_last_command <= ref_timeout_) { input_ref_.writeFromNonRT(msg); } else { @@ -164,13 +180,6 @@ void AckermannSteeringController::reference_callback(const std::shared_ptrget_logger(), - "Received %zu , but expected %zu joints in command. Ignoring message.", - msg->joint_names.size(), params_.joints.size()); - - } } controller_interface::InterfaceConfiguration AckermannSteeringController::command_interface_configuration() const @@ -178,10 +187,12 @@ controller_interface::InterfaceConfiguration AckermannSteeringController::comman 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(NR_CMD_ITFS); + command_interfaces_config.names.push_back( + params_.rear_wheel_name + "/" + hardware_interface::HW_IF_VELOCITY); + command_interfaces_config.names.push_back( + params_.front_steer_name + "/" + hardware_interface::HW_IF_POSITION); return command_interfaces_config; } @@ -191,10 +202,11 @@ controller_interface::InterfaceConfiguration AckermannSteeringController::state_ 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(NR_STATE_ITFS); + state_interfaces_config.names.push_back( + params_.rear_wheel_name + "/" + hardware_interface::HW_IF_POSITION); + state_interfaces_config.names.push_back( + params_.front_steer_name + "/" + hardware_interface::HW_IF_POSITION); return state_interfaces_config; } @@ -203,16 +215,18 @@ controller_interface::InterfaceConfiguration AckermannSteeringController::state_ std::vector AckermannSteeringController::on_export_reference_interfaces() { - reference_interfaces_.resize(state_joints_.size(), std::numeric_limits::quiet_NaN()); + reference_interfaces_.resize(NR_REF_ITFS, std::numeric_limits::quiet_NaN()); std::vector reference_interfaces; - reference_interfaces.reserve(reference_interfaces_.size()); + reference_interfaces.reserve(NR_REF_ITFS); - 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(), std::string("linear/") + hardware_interface::HW_IF_VELOCITY, + &reference_interfaces_[0])); + + reference_interfaces.push_back(hardware_interface::CommandInterface( + get_node()->get_name(), std::string("angular/") + hardware_interface::HW_IF_VELOCITY, + &reference_interfaces_[1])); return reference_interfaces; } @@ -227,7 +241,7 @@ controller_interface::CallbackReturn AckermannSteeringController::on_activate( const rclcpp_lifecycle::State & /*previous_state*/) { // Set default value in command - reset_controller_reference_msg(*(input_ref_.readFromRT()), state_joints_, get_node()); + reset_controller_reference_msg(*(input_ref_.readFromRT()), get_node()); return controller_interface::CallbackReturn::SUCCESS; } @@ -237,7 +251,7 @@ controller_interface::CallbackReturn AckermannSteeringController::on_deactivate( { // 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) { + for (size_t i = 0; i < NR_CMD_ITFS; ++i) { command_interfaces_[i].set_value(std::numeric_limits::quiet_NaN()); } return controller_interface::CallbackReturn::SUCCESS; @@ -251,56 +265,97 @@ controller_interface::return_type AckermannSteeringController::update_reference_ // 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) { // send message only if there is no timeout - if (age_of_last_command <= ref_timeout_ || ref_timeout_ == rclcpp::Duration::from_seconds(0)) { - if (!std::isnan((*current_ref)->displacements[i])) { - if (*(control_mode_.readFromRT()) == control_mode_type::SLOW) { - (*current_ref)->displacements[i] /= 2; - } - reference_interfaces_[i] = (*current_ref)->displacements[i]; - if (ref_timeout_ == rclcpp::Duration::from_seconds(0)){ - (*current_ref)->displacements[i] = std::numeric_limits::quiet_NaN(); - } - } - } else { - (*current_ref)->displacements[i] = std::numeric_limits::quiet_NaN(); + if (age_of_last_command <= ref_timeout_ || ref_timeout_ == rclcpp::Duration::from_seconds(0)) { + if (!std::isnan(current_ref->twist.linear.x) && !std::isnan(current_ref->twist.angular.z)) { + reference_interfaces_[0] = current_ref->twist.linear.x; + reference_interfaces_[1] = current_ref->twist.angular.z; } - } + } else { + reference_interfaces_[0] = std::numeric_limits::quiet_NaN(); + reference_interfaces_[1] = std::numeric_limits::quiet_NaN();} return controller_interface::return_type::OK; } controller_interface::return_type AckermannSteeringController::update_and_write_commands( const rclcpp::Time & time, const rclcpp::Duration & /*period*/) { - auto current_ref = input_ref_.readFromRT(); - const auto age_of_last_command = time - (*current_ref)->header.stamp; - - // 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) { - // send message only if there is no timeout - if (age_of_last_command <= ref_timeout_ || ref_timeout_ == rclcpp::Duration::from_seconds(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]); - if (ref_timeout_ == rclcpp::Duration::from_seconds(0)){ - reference_interfaces_[i] = std::numeric_limits::quiet_NaN(); - } - } - } else { - reference_interfaces_[i] = std::numeric_limits::quiet_NaN(); + if (params_.open_loop) + { + odometry_.updateOpenLoop(last_linear_velocity_, last_angular_velocity_, time); + } + else + { + const double wheel_position = state_interfaces_[0].get_value(); + const double steer_position = state_interfaces_[1].get_value() * params_.steer_pos_multiplier; + + if (std::isnan(wheel_position) || std::isnan(steer_position)) + { + return controller_interface::return_type::OK; } + + // Estimate linear and angular velocity using joint information + odometry_.update(wheel_position, steer_position, time); } - 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(); + // MOVE ROBOT + + // Limit velocities and accelerations: + // TODO(destogl): add limiter for the velocities + + if (std::isnan(reference_interfaces_[0]) || std::isnan(reference_interfaces_[1])) + { + return controller_interface::return_type::OK; + } + + // store and set commands + last_linear_velocity_ = reference_interfaces_[0]; + last_angular_velocity_ = reference_interfaces_[1]; + + // omega = linear_vel / radius + command_interfaces_[0].set_value(last_linear_velocity_ / params_.wheel_radius); + command_interfaces_[0].set_value(last_angular_velocity_); + + if (ref_timeout_ == rclcpp::Duration::from_seconds(0) || is_in_chained_mode()) + { + reference_interfaces_[0] = std::numeric_limits::quiet_NaN(); + reference_interfaces_[1] = std::numeric_limits::quiet_NaN(); } + // Publish odometry message + if (previous_publish_timestamp_ + publish_period_ < time) + { + previous_publish_timestamp_ += publish_period_; + // Compute and store orientation info + tf2::Quaternion orientation; + orientation.setRPY(0.0, 0.0, odometry_.getHeading()); + + // Populate odom message and publish + if (rt_odom_state_publisher_->trylock()) + { + auto & odometry_message = rt_odom_state_publisher_->msg_; + odometry_message.header.stamp = time; + odometry_message.pose.pose.position.x = odometry_.getX(); + odometry_message.pose.pose.position.y = odometry_.getY(); + odometry_message.pose.pose.orientation = tf2::toMsg(orientation); + odometry_message.twist.twist.linear.x = odometry_.getLinear(); + odometry_message.twist.twist.angular.z = odometry_.getAngular(); + rt_odom_state_publisher_->unlockAndPublish(); + } + + // Publish tf /odom frame + if (params_.enable_odom_tf && rt_tf_odom_state_publisher_->trylock()) + { + auto & transform = rt_tf_odom_state_publisher_->msg_.transforms[0]; + transform.header.stamp = time; + transform.transform.translation.x = odometry_.getX(); + transform.transform.translation.y = odometry_.getY(); + transform.transform.rotation = tf2::toMsg(orientation); + rt_tf_odom_state_publisher_->unlockAndPublish(); + } + } + + return controller_interface::return_type::OK; } diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.yaml b/ackermann_steering_controller/src/ackermann_steering_controller.yaml index ff6086863c..4c4b50d50e 100644 --- a/ackermann_steering_controller/src/ackermann_steering_controller.yaml +++ b/ackermann_steering_controller/src/ackermann_steering_controller.yaml @@ -1,34 +1,4 @@ -ackermann_steering_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 - } - } +ackermann_steering_controller_ros2: reference_timeout: { type: double, default_value: 0.0, @@ -37,3 +7,141 @@ ackermann_steering_controller: # gt_eq<>: 0.0, # } } + + rear_wheel_name: { + type: string, + default_value: "rear_wheel_joint", + description: "Name of the rear wheel.", + read_only: true, + + } + + front_steer_name: { + type: string, + default_value: "front_steer_joint", + description: "Name of the front steer wheel.", + read_only: true, + + } + + + publish_rate: { + type: double, + default_value: 50.0, + description: "In Hz. Controller state will be published at this rate.", + read_only: true, + + } + + open_loop: { + type: bool, + default_value: false, + description: "bool parameter decides if open oop or not (feedback).", + read_only: false, + + } + + wheel_separation_multiplier: { + type: double, + default_value: 1.0, + description: "Wheel separation height will be multiplied by value of the wheel_separation_h_multiplier.", + read_only: false, + + } + + wheel_separation: { + type: double, + default_value: 0.0, + description: "Wheel separation.", + read_only: false, + + } + + wheel_radius: { + type: double, + default_value: 0.0, + description: "Wheel radius.", + read_only: false, + + } + + wheel_radius_multiplier: { + type: double, + default_value: 1.0, + description: "Wheel radius will be multiplied by value of wheel_radius_multiplier.", + read_only: false, + + } + + steer_pos_multiplier: { + type: double, + default_value: 1.0, + description: "Steer pos will be multiplied by value of steer_pos_multiplier.", + read_only: false, + + } + + velocity_rolling_window_size: { + type: int, + default_value: 10, + description: "The number of velocity samples to average together to compute the odometry twist.linear.x and twist.angular.z velocities.", + read_only: false, + + } + + allow_multiple_cmd_vel_publishers: { + type: bool, + default_value: true, + description: "Allow multiple cmd_vel publishers is enabled or disabled?.", + read_only: false, + + } + + base_frame_id: { + type: string, + default_value: "base_link", + description: "Base frame_id set to value of base_frame_id.", + read_only: false, + + } + + odom_frame_id: { + type: string, + default_value: "odom", + description: "Odometry frame_id set to value of odom_frame_id.", + read_only: false, + + } + + + enable_odom_tf: { + type: bool, + default_value: true, + description: "Publishing to tf is enabled or disabled ?.", + read_only: false, + + } + + twist_covariance_diagonal: { + type: double_array, + default_value: [0, 7, 14, 21, 28, 35], + description: "diagonal values of twist covariance matrix.", + read_only: false, + + } + + pose_covariance_diagonal: { + type: double_array, + default_value: [0, 7, 14, 21, 28, 35], + description: "diagonal values of pose covariance matrix.", + read_only: false, + + } + + + + + + + + From 568f7234d56e876408e272f25385862e9d75b274 Mon Sep 17 00:00:00 2001 From: GiridharBukka Date: Sun, 13 Nov 2022 09:25:32 +0100 Subject: [PATCH 004/186] added odometry.hpp --- .../include/odometry.hpp | 193 ++++++++++++++++++ 1 file changed, 193 insertions(+) create mode 100644 ackermann_steering_controller/include/odometry.hpp diff --git a/ackermann_steering_controller/include/odometry.hpp b/ackermann_steering_controller/include/odometry.hpp new file mode 100644 index 0000000000..7c39782ed4 --- /dev/null +++ b/ackermann_steering_controller/include/odometry.hpp @@ -0,0 +1,193 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, PAL Robotics, S.L. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the PAL Robotics nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* + * Author: Luca Marchionni + * Author: Bence Magyar + * Author: Enrique Fernández + * Author: Paul Mathieu + * Author: Masaru Morita + */ + +#pragma once + +#include "realtime_tools/realtime_buffer.h" +#include "realtime_tools/realtime_publisher.h" + +//#include +#include +#include +#include +#include + +namespace ros2_ackermann_cont +{ + +namespace bacc = boost::accumulators; + +/** + * \brief The Odometry class handles odometry readings + * (2D pose and velocity with related timestamp) + */ +class Odometry +{ +public: + /// Integration function, used to integrate the odometry: + typedef boost::function IntegrationFunction; + + /** + * \brief Constructor + * Timestamp will get the current time value + * Value will be set to zero + * \param velocity_rolling_window_size Rolling window size used to compute the velocity mean + * + */ + // ackermann_steering_controller_ros2::Params params; + explicit Odometry(size_t velocity_rolling_window_size = 10); + + /** + * \brief Initialize the odometry + * \param time Current time + */ + void init(const rclcpp::Time & time); + + /** + * \brief Updates the odometry class with latest wheels position + * \param rear_wheel_pos Rear wheel position [rad] + * \param front_steer_pos Front Steer position [rad] + * \param time Current time + * \return true if the odometry is actually updated + */ + bool update(double rear_wheel_pos, double front_steer_pos, const rclcpp::Time & time); + + /** + * \brief Updates the odometry class with latest velocity command + * \param linear Linear velocity [m/s] + * \param angular Angular velocity [rad/s] + * \param time Current time + */ + void updateOpenLoop(double linear, double angular, const rclcpp::Time & time); + + /** + * \brief heading getter + * \return heading [rad] + */ + double getHeading() const { return heading_; } + + /** + * \brief x position getter + * \return x position [m] + */ + double getX() const { return x_; } + + /** + * \brief y position getter + * \return y position [m] + */ + double getY() const { return y_; } + + /** + * \brief linear velocity getter + * \return linear velocity [m/s] + */ + double getLinear() const { return linear_; } + + /** + * \brief angular velocity getter + * \return angular velocity [rad/s] + */ + double getAngular() const { return angular_; } + + /** + * \brief Sets the wheel parameters: radius and separation + */ + void setWheelParams(double wheel_reparation_h, double wheel_radius); + + /** + * \brief Velocity rolling window size setter + * \param velocity_rolling_window_size Velocity rolling window size + */ + void setVelocityRollingWindowSize(size_t velocity_rolling_window_size); + +private: + /// Rolling mean accumulator and window: + typedef bacc::accumulator_set > RollingMeanAcc; + typedef bacc::tag::rolling_window RollingWindow; + + /** + * \brief Integrates the velocities (linear and angular) using 2nd order Runge-Kutta + * \param linear Linear velocity [m] (linear displacement, i.e. m/s * dt) computed by encoders + * \param angular Angular velocity [rad] (angular displacement, i.e. m/s * dt) computed by encoders + */ + void integrateRungeKutta2(double linear, double angular); + + /** + * \brief Integrates the velocities (linear and angular) using exact method + * \param linear Linear velocity [m] (linear displacement, i.e. m/s * dt) computed by encoders + * \param angular Angular velocity [rad] (angular displacement, i.e. m/s * dt) computed by encoders + */ + void integrateExact(double linear, double angular); + + /** + * \brief Reset linear and angular accumulators + */ + void resetAccumulators(); + + /// Current timestamp: + rclcpp::Time timestamp_; + + /// Current pose: + double x_; // [m] + double y_; // [m] + double heading_; // [rad] + + /// Current velocity: + double linear_; // [m/s] + double angular_; // [rad/s] + + double wheel_separation_; + double wheel_radius_; + + /// Previous wheel position/state [rad]: + double rear_wheel_old_pos_; + + /// Rolling mean accumulators for the linar and angular velocities: + int velocity_rolling_window_size_; + RollingMeanAcc linear_acc_; + RollingMeanAcc angular_acc_; + + /// Integration funcion, used to integrate the odometry: + IntegrationFunction integrate_fun_; +}; +} From 29ef5f8003aa90832812301180799b99fb693446 Mon Sep 17 00:00:00 2001 From: GiridharBukka Date: Sun, 13 Nov 2022 09:31:55 +0100 Subject: [PATCH 005/186] modified namespace of odometry.hpp and added odometry.cpp --- .../include/odometry.hpp | 2 +- .../src/odometry.cpp | 177 ++++++++++++++++++ 2 files changed, 178 insertions(+), 1 deletion(-) create mode 100644 ackermann_steering_controller/src/odometry.cpp diff --git a/ackermann_steering_controller/include/odometry.hpp b/ackermann_steering_controller/include/odometry.hpp index 7c39782ed4..abf6155a48 100644 --- a/ackermann_steering_controller/include/odometry.hpp +++ b/ackermann_steering_controller/include/odometry.hpp @@ -51,7 +51,7 @@ #include #include -namespace ros2_ackermann_cont +namespace ackermann_steering_controller { namespace bacc = boost::accumulators; diff --git a/ackermann_steering_controller/src/odometry.cpp b/ackermann_steering_controller/src/odometry.cpp new file mode 100644 index 0000000000..f086bd888c --- /dev/null +++ b/ackermann_steering_controller/src/odometry.cpp @@ -0,0 +1,177 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, PAL Robotics, S.L. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the PAL Robotics nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* + * Author: Luca Marchionni + * Author: Bence Magyar + * Author: Enrique Fernández + * Author: Paul Mathieu + * Author: Masaru Morita + */ + +#include + +namespace ackermann_steering_controller +{ + namespace bacc = boost::accumulators; + // using namespace ackermann_steering_controller_ros2; + // ackermann_steering_controller_ros2::Params params; + + Odometry::Odometry(size_t velocity_rolling_window_size) + : timestamp_(0.0) + , x_(0.0) + , y_(0.0) + , heading_(0.0) + , linear_(0.0) + , angular_(0.0) + , wheel_separation_(0.0) + , wheel_radius_(0.0) + , rear_wheel_old_pos_(0.0) + , velocity_rolling_window_size_(0.0) + , linear_acc_(RollingWindow::window_size = velocity_rolling_window_size) + , angular_acc_(RollingWindow::window_size = velocity_rolling_window_size) + , integrate_fun_(std::bind(&Odometry::integrateExact, this, std::placeholders::_1, std::placeholders::_2)) + + { + } + + void Odometry::init(const rclcpp::Time& time) + { + // Reset accumulators and timestamp: + resetAccumulators(); + timestamp_ = time; + } + + // TODO(destogl): enalbe also velocity interface to update using velocity from the rear wheel + bool Odometry::update(double rear_wheel_pos, double front_steer_pos, const rclcpp::Time &time) + { + /// Get current wheel joint positions: + const double rear_wheel_cur_pos = rear_wheel_pos * wheel_radius_; + + /// Estimate velocity of wheels using old and current position: + //const double left_wheel_est_vel = left_wheel_cur_pos - left_wheel_old_pos_; + //const double right_wheel_est_vel = right_wheel_cur_pos - right_wheel_old_pos_; + + const double rear_wheel_est_vel = rear_wheel_cur_pos - rear_wheel_old_pos_; + + /// Update old position with current: + rear_wheel_old_pos_ = rear_wheel_cur_pos; + + /// Compute linear and angular diff: + const double linear = rear_wheel_est_vel;//(right_wheel_est_vel + left_wheel_est_vel) * 0.5; + //const double angular = (right_wheel_est_vel - left_wheel_est_vel) / wheel_separation_w_; + const double angular = tan(front_steer_pos) * linear / wheel_separation_; + + /// Integrate odometry: + integrate_fun_(linear, angular); + + /// We cannot estimate the speed with very small time intervals: + const double dt = time.seconds() - timestamp_.seconds(); + if (dt < 0.0001) + return false; // Interval too small to integrate with + + timestamp_ = time; + + /// Estimate speeds using a rolling mean to filter them out: + linear_acc_(linear/dt); + angular_acc_(angular/dt); + + linear_ = bacc::rolling_mean(linear_acc_); + angular_ = bacc::rolling_mean(angular_acc_); + + return true; + } + + void Odometry::updateOpenLoop(double linear, double angular, const rclcpp::Time &time) + { + /// Save last linear and angular velocity: + linear_ = linear; + angular_ = angular; + + /// Integrate odometry: + const double dt = time.seconds() - timestamp_.seconds(); + timestamp_ = time; + integrate_fun_(linear * dt, angular * dt); + } + + void Odometry::setWheelParams(double wheel_separation, double wheel_radius) + { + wheel_separation_ = wheel_separation; + wheel_radius_ = wheel_radius; + } + + void Odometry::setVelocityRollingWindowSize(size_t velocity_rolling_window_size) + { + velocity_rolling_window_size_ = velocity_rolling_window_size; + + resetAccumulators(); + } + + void Odometry::integrateRungeKutta2(double linear, double angular) + { + const double direction = heading_ + angular * 0.5; + + /// Runge-Kutta 2nd order integration: + x_ += linear * cos(direction); + y_ += linear * sin(direction); + heading_ += angular; + } + + /** + * \brief Other possible integration method provided by the class + * \param linear + * \param angular + */ + void Odometry::integrateExact(double linear, double angular) + { + if (fabs(angular) < 1e-6) + integrateRungeKutta2(linear, angular); + else + { + /// Exact integration (should solve problems when angular is zero): + const double heading_old = heading_; + const double r = linear/angular; + heading_ += angular; + x_ += r * (sin(heading_) - sin(heading_old)); + y_ += -r * (cos(heading_) - cos(heading_old)); + } + } + + void Odometry::resetAccumulators() + { + linear_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_); + angular_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_); + } + +} // namespace ackermann_steering_controller \ No newline at end of file From 2671377ca2ff0719d2cc70b7d0d31501eead8400 Mon Sep 17 00:00:00 2001 From: GiridharBukka Date: Sun, 13 Nov 2022 10:15:22 +0100 Subject: [PATCH 006/186] spotted issue with namespace naming (has xyz_ros2 when namespace everyhere is xyz) inside generated parameters.hpp, removed _ros2 from the namespace name, pkg compiles without any error, --- ackermann_steering_controller/CMakeLists.txt | 57 ++++++++++--------- .../ackermann_steering_controller.hpp | 2 +- .../odometry.hpp | 0 ackermann_steering_controller/package.xml | 13 ++--- .../src/ackermann_steering_controller.cpp | 4 +- 5 files changed, 40 insertions(+), 36 deletions(-) rename ackermann_steering_controller/include/{ => ackermann_steering_controller}/odometry.hpp (100%) diff --git a/ackermann_steering_controller/CMakeLists.txt b/ackermann_steering_controller/CMakeLists.txt index eaf6b0c61d..1bf44b62d9 100644 --- a/ackermann_steering_controller/CMakeLists.txt +++ b/ackermann_steering_controller/CMakeLists.txt @@ -9,12 +9,17 @@ endif() set(THIS_PACKAGE_INCLUDE_DEPENDS control_msgs controller_interface + geometry_msgs hardware_interface + nav_msgs pluginlib rclcpp rclcpp_lifecycle realtime_tools std_srvs + tf2 + tf2_msgs + tf2_geometry_msgs ) find_package(ament_cmake REQUIRED) @@ -78,32 +83,32 @@ if(BUILD_TESTING) find_package(hardware_interface REQUIRED) find_package(ros2_control_test_assets REQUIRED) - ament_add_gmock(test_load_ackermann_steering_controller test/test_load_ackermann_steering_controller.cpp) - target_include_directories(test_load_ackermann_steering_controller PRIVATE include) - ament_target_dependencies( - test_load_ackermann_steering_controller - controller_manager - hardware_interface - ros2_control_test_assets - ) - - add_rostest_with_parameters_gmock(test_ackermann_steering_controller test/test_ackermann_steering_controller.cpp ${CMAKE_CURRENT_SOURCE_DIR}/test/ackermann_steering_controller_params.yaml) - target_include_directories(test_ackermann_steering_controller PRIVATE include) - target_link_libraries(test_ackermann_steering_controller ackermann_steering_controller) - ament_target_dependencies( - test_ackermann_steering_controller - controller_interface - hardware_interface - ) - - add_rostest_with_parameters_gmock(test_ackermann_steering_controller_preceeding test/test_ackermann_steering_controller_preceeding.cpp ${CMAKE_CURRENT_SOURCE_DIR}/test/ackermann_steering_controller_preceeding_params.yaml) - target_include_directories(test_ackermann_steering_controller_preceeding PRIVATE include) - target_link_libraries(test_ackermann_steering_controller_preceeding ackermann_steering_controller) - ament_target_dependencies( - test_ackermann_steering_controller_preceeding - controller_interface - hardware_interface - ) + # ament_add_gmock(test_load_ackermann_steering_controller test/test_load_ackermann_steering_controller.cpp) + # target_include_directories(test_load_ackermann_steering_controller PRIVATE include) + # ament_target_dependencies( + # test_load_ackermann_steering_controller + # controller_manager + # hardware_interface + # ros2_control_test_assets + # ) + + # add_rostest_with_parameters_gmock(test_ackermann_steering_controller test/test_ackermann_steering_controller.cpp ${CMAKE_CURRENT_SOURCE_DIR}/test/ackermann_steering_controller_params.yaml) + # target_include_directories(test_ackermann_steering_controller PRIVATE include) + # target_link_libraries(test_ackermann_steering_controller ackermann_steering_controller) + # ament_target_dependencies( + # test_ackermann_steering_controller + # controller_interface + # hardware_interface + # ) + + # add_rostest_with_parameters_gmock(test_ackermann_steering_controller_preceeding test/test_ackermann_steering_controller_preceeding.cpp ${CMAKE_CURRENT_SOURCE_DIR}/test/ackermann_steering_controller_preceeding_params.yaml) + # target_include_directories(test_ackermann_steering_controller_preceeding PRIVATE include) + # target_link_libraries(test_ackermann_steering_controller_preceeding ackermann_steering_controller) + # ament_target_dependencies( + # test_ackermann_steering_controller_preceeding + # controller_interface + # hardware_interface + # ) endif() ament_export_include_directories( diff --git a/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp b/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp index cbd78e4ab7..0083a63720 100644 --- a/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp +++ b/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp @@ -22,7 +22,7 @@ #include #include -#include "ros2_ackermann_cont/odometry.h" +#include "ackermann_steering_controller/odometry.hpp" #include "controller_interface/chainable_controller_interface.hpp" #include "hardware_interface/handle.hpp" diff --git a/ackermann_steering_controller/include/odometry.hpp b/ackermann_steering_controller/include/ackermann_steering_controller/odometry.hpp similarity index 100% rename from ackermann_steering_controller/include/odometry.hpp rename to ackermann_steering_controller/include/ackermann_steering_controller/odometry.hpp diff --git a/ackermann_steering_controller/package.xml b/ackermann_steering_controller/package.xml index cb4486ea7a..872b0376c8 100644 --- a/ackermann_steering_controller/package.xml +++ b/ackermann_steering_controller/package.xml @@ -12,20 +12,19 @@ generate_parameter_library control_msgs - controller_interface - + geometry_msgs hardware_interface - + nav_msgs pluginlib - rclcpp - rclcpp_lifecycle - realtime_tools - + rcpputils std_srvs + tf2 + tf2_msgs + tf2_geometry_msgs ament_lint_auto ament_cmake_gmock diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.cpp b/ackermann_steering_controller/src/ackermann_steering_controller.cpp index 27f29d7539..d858185d49 100644 --- a/ackermann_steering_controller/src/ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/src/ackermann_steering_controller.cpp @@ -260,8 +260,8 @@ controller_interface::CallbackReturn AckermannSteeringController::on_deactivate( controller_interface::return_type AckermannSteeringController::update_reference_from_subscribers( const rclcpp::Time & time, const rclcpp::Duration & /*period*/) { - auto current_ref = input_ref_.readFromRT(); - const auto age_of_last_command = time - (*current_ref)->header.stamp; + auto current_ref = *(input_ref_.readFromRT()); + const auto age_of_last_command = time - (current_ref)->header.stamp; // TODO(anyone): depending on number of interfaces, use definitions, e.g., `CMD_MY_ITFS`, // instead of a loop From 9181e18e34062be8bec7952d967b132c1da1a3ee Mon Sep 17 00:00:00 2001 From: GiridharBukka Date: Sun, 13 Nov 2022 10:45:29 +0100 Subject: [PATCH 007/186] corrected the copy paste error in the .yaml file which created the _ros2 addition in the namespace naming of the parameters.hpp file --- .../src/ackermann_steering_controller.yaml | 2 +- .../ackermann_steering_controller_params.yaml | 34 ++++++++++++++++--- 2 files changed, 31 insertions(+), 5 deletions(-) diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.yaml b/ackermann_steering_controller/src/ackermann_steering_controller.yaml index 4c4b50d50e..bc23b13d82 100644 --- a/ackermann_steering_controller/src/ackermann_steering_controller.yaml +++ b/ackermann_steering_controller/src/ackermann_steering_controller.yaml @@ -1,4 +1,4 @@ -ackermann_steering_controller_ros2: +ackermann_steering_controller: reference_timeout: { type: double, default_value: 0.0, diff --git a/ackermann_steering_controller/test/ackermann_steering_controller_params.yaml b/ackermann_steering_controller/test/ackermann_steering_controller_params.yaml index f9d9e7c159..cf1ff4995a 100644 --- a/ackermann_steering_controller/test/ackermann_steering_controller_params.yaml +++ b/ackermann_steering_controller/test/ackermann_steering_controller_params.yaml @@ -1,9 +1,35 @@ test_ackermann_steering_controller: ros__parameters: - joints: - - joint1 + reference_timeout: 0.1 - interface_name: acceleration + rear_wheel_name: "rear_wheel_joint" + + front_steer_name: "front_steer_joint" + + publish_rate: 50.0 + + open_loop: false + + wheel_separation_multiplier: 1.0 + + wheel_separation: 0.0 + + wheel_radius: 1.0 + + steer_pos_multiplier: 1.0 + + velocity_rolling_window_size: 10 + + allow_multiple_cmd_vel_publishers: true + + base_frame_id: "base_link" + + odom_frame_id: "odom" + + enable_odom_tf: true + + twist_covariance_diagonal: [0, 7, 14, 21, 28, 35] + + pose_covariance_diagonal: [0, 7, 14, 21, 28, 35] - reference_timeout: 0.1 From ff4c3b9767ce64283d408e62e211933ce9cf7851 Mon Sep 17 00:00:00 2001 From: GiridharBukka Date: Mon, 14 Nov 2022 14:49:18 +0100 Subject: [PATCH 008/186] added tests yet to debug few tests --- ackermann_steering_controller/CMakeLists.txt | 19 +- .../ackermann_steering_controller.hpp | 11 +- .../src/ackermann_steering_controller.cpp | 50 +- .../src/odometry.cpp | 1 + ackermann_steering_controller/test.txt | 125 +++ .../ackermann_steering_controller_params.yaml | 4 +- .../test_ackermann_steering_controller.cpp | 713 +++++++++--------- .../test_ackermann_steering_controller.hpp | 107 ++- 8 files changed, 561 insertions(+), 469 deletions(-) create mode 100644 ackermann_steering_controller/test.txt diff --git a/ackermann_steering_controller/CMakeLists.txt b/ackermann_steering_controller/CMakeLists.txt index 1bf44b62d9..079329a3b4 100644 --- a/ackermann_steering_controller/CMakeLists.txt +++ b/ackermann_steering_controller/CMakeLists.txt @@ -41,12 +41,13 @@ find_package(ament_cmake REQUIRED) # Add ackermann_steering_controller library related compile commands generate_parameter_library(ackermann_steering_controller_parameters src/ackermann_steering_controller.yaml - include/ackermann_steering_controller/validate_ackermann_steering_controller_parameters.hpp + # include/ackermann_steering_controller/validate_ackermann_steering_controller_parameters.hpp ) add_library( ackermann_steering_controller SHARED src/ackermann_steering_controller.cpp + src/odometry.cpp ) target_include_directories(ackermann_steering_controller PRIVATE include) target_link_libraries(ackermann_steering_controller ackermann_steering_controller_parameters) @@ -92,14 +93,14 @@ if(BUILD_TESTING) # ros2_control_test_assets # ) - # add_rostest_with_parameters_gmock(test_ackermann_steering_controller test/test_ackermann_steering_controller.cpp ${CMAKE_CURRENT_SOURCE_DIR}/test/ackermann_steering_controller_params.yaml) - # target_include_directories(test_ackermann_steering_controller PRIVATE include) - # target_link_libraries(test_ackermann_steering_controller ackermann_steering_controller) - # ament_target_dependencies( - # test_ackermann_steering_controller - # controller_interface - # hardware_interface - # ) + add_rostest_with_parameters_gmock(test_ackermann_steering_controller test/test_ackermann_steering_controller.cpp ${CMAKE_CURRENT_SOURCE_DIR}/test/ackermann_steering_controller_params.yaml) + target_include_directories(test_ackermann_steering_controller PRIVATE include) + target_link_libraries(test_ackermann_steering_controller ackermann_steering_controller) + ament_target_dependencies( + test_ackermann_steering_controller + controller_interface + hardware_interface + ) # add_rostest_with_parameters_gmock(test_ackermann_steering_controller_preceeding test/test_ackermann_steering_controller_preceeding.cpp ${CMAKE_CURRENT_SOURCE_DIR}/test/ackermann_steering_controller_preceeding_params.yaml) # target_include_directories(test_ackermann_steering_controller_preceeding PRIVATE include) diff --git a/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp b/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp index 0083a63720..25f0f6c43d 100644 --- a/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp +++ b/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp @@ -21,10 +21,10 @@ #include #include #include - -#include "ackermann_steering_controller/odometry.hpp" +#include #include "controller_interface/chainable_controller_interface.hpp" +#include "ackermann_steering_controller/odometry.hpp" #include "hardware_interface/handle.hpp" #include "ackermann_steering_controller_parameters.hpp" #include "ackermann_steering_controller/visibility_control.h" @@ -127,10 +127,9 @@ class AckermannSteeringController : public controller_interface::ChainableContro std::vector registered_front_wheel_handles_; /// Odometry related: - rclcpp::Duration publish_period_ = rclcpp::Duration::from_nanoseconds(0); + rclcpp::Duration publish_period_ = rclcpp::Duration::from_nanoseconds(0);; bool open_loop_; rclcpp::Time previous_publish_timestamp_{0}; - /// Velocity command related: struct Commands { @@ -159,8 +158,8 @@ class AckermannSteeringController : public controller_interface::ChainableContro bool enable_odom_tf_; // store last velocity - double last_linear_velocity_; - double last_angular_velocity_; + double last_linear_velocity_ = 0.0; + double last_angular_velocity_ = 0.0; }; diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.cpp b/ackermann_steering_controller/src/ackermann_steering_controller.cpp index d858185d49..40af25ede9 100644 --- a/ackermann_steering_controller/src/ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/src/ackermann_steering_controller.cpp @@ -18,6 +18,8 @@ #include #include #include +#include +#include #include "controller_interface/helpers.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" @@ -115,7 +117,7 @@ controller_interface::CallbackReturn AckermannSteeringController::on_configure( // TODO(anyone): Reserve memory in state publisher depending on the message type rt_odom_state_publisher_->lock(); - rt_odom_state_publisher_->msg_.header.frame_id = params_.rear_wheel_name; + rt_odom_state_publisher_->msg_.header.stamp = get_node()->now(); rt_odom_state_publisher_->msg_.header.frame_id = params_.odom_frame_id; rt_odom_state_publisher_->msg_.child_frame_id = params_.base_frame_id; rt_odom_state_publisher_->msg_.pose.pose.position.z = 0; @@ -145,14 +147,15 @@ controller_interface::CallbackReturn AckermannSteeringController::on_configure( rt_tf_odom_state_publisher_->lock(); rt_tf_odom_state_publisher_->msg_.transforms.resize(1); - rt_tf_odom_state_publisher_->msg_.transforms[0].header.frame_id = params_.rear_wheel_name; + rt_tf_odom_state_publisher_->msg_.transforms[0].header.stamp = get_node()->now(); rt_tf_odom_state_publisher_->msg_.transforms[0].header.frame_id = params_.odom_frame_id; rt_tf_odom_state_publisher_->msg_.transforms[0].child_frame_id = params_.base_frame_id; rt_tf_odom_state_publisher_->msg_.transforms[0].transform.translation.z = 0.0; rt_tf_odom_state_publisher_->unlock(); // calculation publication period of odometry and tf odometry messages - publish_period_ = rclcpp::Duration::from_seconds(1.0 / params_.publish_rate); + publish_period_ = rclcpp::Durationf::from_seconds(1.0 / params_.publish_rate); + fprintf(stderr, "publish_period_ = %d \n", publish_period_); RCLCPP_INFO(get_node()->get_logger(), "configure successful"); return controller_interface::CallbackReturn::SUCCESS; @@ -192,7 +195,7 @@ controller_interface::InterfaceConfiguration AckermannSteeringController::comman command_interfaces_config.names.push_back( params_.rear_wheel_name + "/" + hardware_interface::HW_IF_VELOCITY); command_interfaces_config.names.push_back( - params_.front_steer_name + "/" + hardware_interface::HW_IF_POSITION); + params_.front_steer_name + "/" + hardware_interface::HW_IF_VELOCITY); return command_interfaces_config; } @@ -291,11 +294,11 @@ controller_interface::return_type AckermannSteeringController::update_and_write_ if (std::isnan(wheel_position) || std::isnan(steer_position)) { - return controller_interface::return_type::OK; + return controller_interface::return_type::OK; } // Estimate linear and angular velocity using joint information - odometry_.update(wheel_position, steer_position, time); + odometry_.update(wheel_position, steer_position,time); } // MOVE ROBOT @@ -312,19 +315,30 @@ controller_interface::return_type AckermannSteeringController::update_and_write_ last_linear_velocity_ = reference_interfaces_[0]; last_angular_velocity_ = reference_interfaces_[1]; + previous_publish_timestamp_ = time; + + // omega = linear_vel / radius command_interfaces_[0].set_value(last_linear_velocity_ / params_.wheel_radius); - command_interfaces_[0].set_value(last_angular_velocity_); + command_interfaces_[1].set_value(last_angular_velocity_); if (ref_timeout_ == rclcpp::Duration::from_seconds(0) || is_in_chained_mode()) { reference_interfaces_[0] = std::numeric_limits::quiet_NaN(); reference_interfaces_[1] = std::numeric_limits::quiet_NaN(); } + fprintf(stderr, "previous_publish_timestamp_ + publish_period_ < time = %d, %d, %d, %d\n", time, + previous_publish_timestamp_, publish_period_, (previous_publish_timestamp_ + publish_period_) < time); + // fprintf(stderr, "Exception thrown during controller's init with message: %s \n", e.what()); // Publish odometry message if (previous_publish_timestamp_ + publish_period_ < time) { + fprintf(stderr, "debuggin2 \n"); + fprintf(stderr, "previous_publish_timestamp_ + publish_period_ < time = %d, %d, %d, %d\n", time, + previous_publish_timestamp_, publish_period_, (previous_publish_timestamp_ + publish_period_) < time); + + previous_publish_timestamp_ += publish_period_; // Compute and store orientation info tf2::Quaternion orientation; @@ -333,24 +347,22 @@ controller_interface::return_type AckermannSteeringController::update_and_write_ // Populate odom message and publish if (rt_odom_state_publisher_->trylock()) { - auto & odometry_message = rt_odom_state_publisher_->msg_; - odometry_message.header.stamp = time; - odometry_message.pose.pose.position.x = odometry_.getX(); - odometry_message.pose.pose.position.y = odometry_.getY(); - odometry_message.pose.pose.orientation = tf2::toMsg(orientation); - odometry_message.twist.twist.linear.x = odometry_.getLinear(); - odometry_message.twist.twist.angular.z = odometry_.getAngular(); + rt_odom_state_publisher_->msg_.header.stamp = time; + rt_odom_state_publisher_->msg_.pose.pose.position.x = odometry_.getX(); + rt_odom_state_publisher_->msg_.pose.pose.position.y = odometry_.getY(); + rt_odom_state_publisher_->msg_.pose.pose.orientation = tf2::toMsg(orientation); + rt_odom_state_publisher_->msg_.twist.twist.linear.x = odometry_.getLinear(); + rt_odom_state_publisher_->msg_.twist.twist.angular.z = odometry_.getAngular(); rt_odom_state_publisher_->unlockAndPublish(); } // Publish tf /odom frame if (params_.enable_odom_tf && rt_tf_odom_state_publisher_->trylock()) { - auto & transform = rt_tf_odom_state_publisher_->msg_.transforms[0]; - transform.header.stamp = time; - transform.transform.translation.x = odometry_.getX(); - transform.transform.translation.y = odometry_.getY(); - transform.transform.rotation = tf2::toMsg(orientation); + rt_tf_odom_state_publisher_->msg_.transforms.front().header.stamp = time; + rt_tf_odom_state_publisher_->msg_.transforms.front().transform.translation.x = odometry_.getX(); + rt_tf_odom_state_publisher_->msg_.transforms.front().transform.translation.y = odometry_.getY(); + rt_tf_odom_state_publisher_->msg_.transforms.front().transform.rotation = tf2::toMsg(orientation); rt_tf_odom_state_publisher_->unlockAndPublish(); } } diff --git a/ackermann_steering_controller/src/odometry.cpp b/ackermann_steering_controller/src/odometry.cpp index f086bd888c..6e5bd99088 100644 --- a/ackermann_steering_controller/src/odometry.cpp +++ b/ackermann_steering_controller/src/odometry.cpp @@ -41,6 +41,7 @@ */ #include +#include namespace ackermann_steering_controller { diff --git a/ackermann_steering_controller/test.txt b/ackermann_steering_controller/test.txt new file mode 100644 index 0000000000..3f71fab599 --- /dev/null +++ b/ackermann_steering_controller/test.txt @@ -0,0 +1,125 @@ +cmake_minimum_required(VERSION 3.8) +project(ros2_ackermann_cont) + +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 + geometry_msgs + hardware_interface + nav_msgs + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + std_srvs + tf2 + tf2_msgs + tf2_geometry_msgs +) + +find_package(ament_cmake REQUIRED) +find_package(generate_parameter_library REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +cmake_minimum_required(VERSION 3.8) +project(ros2_ackermann_cont) + +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) + +# Add ackermann_steering_controller_ros2 library related compile commands +generate_parameter_library(ackermann_steering_controller_ros2_parameters + src/ackermann_steering_controller_ros2.yaml + # include/ros2_ackermann_cont/validate_ackermann_steering_controller_ros2_parameters.hpp +) +add_library( + ackermann_steering_controller_ros2 + SHARED + src/ackermann_steering_controller_ros2.cpp + src/odometry.cpp +) +target_include_directories(ackermann_steering_controller_ros2 PRIVATE include) +target_link_libraries(ackermann_steering_controller_ros2 ackermann_steering_controller_ros2_parameters) +ament_target_dependencies(ackermann_steering_controller_ros2 ${THIS_PACKAGE_INCLUDE_DEPENDS}) +target_compile_definitions(ackermann_steering_controller_ros2 PRIVATE "ACKERMANN_STEERING_CONTROLLER_ROS2_BUILDING_DLL") + +pluginlib_export_plugin_description_file( + controller_interface ros2_ackermann_cont.xml) + +install( + TARGETS + ackermann_steering_controller_ros2 + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib +) + +install( + DIRECTORY include/ + DESTINATION include +) + +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) + 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_ackermann_steering_controller_ros2 test/test_load_ackermann_steering_controller_ros2.cpp) + target_include_directories(test_load_ackermann_steering_controller_ros2 PRIVATE include) + ament_target_dependencies( + test_load_ackermann_steering_controller_ros2 + controller_manager + hardware_interface + ros2_control_test_assets + ) + + # add_rostest_with_parameters_gmock(test_ackermann_steering_controller_ros2 test/# # test_ackermann_steering_controller_ros2.cpp ${CMAKE_CURRENT_SOURCE_DIR}/test/ackermann_steering_controller_ros2_params.yaml) + # target_include_directories(test_ackermann_steering_controller_ros2 PRIVATE include) + # target_link_libraries(test_ackermann_steering_controller_ros2 ackermann_steering_controller_ros2) + #ament_target_dependencies( + # test_ackermann_steering_controller_ros2 + # controller_interface + # hardware_interface + #) + + # add_rostest_with_parameters_gmock(test_ackermann_steering_controller_ros2_preceeding test/test_ackermann_steering_controller_ros2_preceeding.cpp ${CMAKE_CURRENT_SOURCE_DIR}/test/ackermann_steering_controller_ros2_preceeding_params.yaml) + # target_include_directories(test_ackermann_steering_controller_ros2_preceeding PRIVATE include) + # target_link_libraries(test_ackermann_steering_controller_ros2_preceeding ackermann_steering_controller_ros2) + # ament_target_dependencies( + # test_ackermann_steering_controller_ros2_preceeding + # controller_interface + # hardware_interface + # ) +endif() + +ament_export_include_directories( + include +) +ament_export_dependencies( + +) +ament_export_libraries( + ackermann_steering_controller_ros2 +) + +ament_package() diff --git a/ackermann_steering_controller/test/ackermann_steering_controller_params.yaml b/ackermann_steering_controller/test/ackermann_steering_controller_params.yaml index cf1ff4995a..4a71d933ee 100644 --- a/ackermann_steering_controller/test/ackermann_steering_controller_params.yaml +++ b/ackermann_steering_controller/test/ackermann_steering_controller_params.yaml @@ -29,7 +29,7 @@ test_ackermann_steering_controller: enable_odom_tf: true - twist_covariance_diagonal: [0, 7, 14, 21, 28, 35] + twist_covariance_diagonal: [0.0, 7.0, 14.0, 21.0, 28.0, 35.0] - pose_covariance_diagonal: [0, 7, 14, 21, 28, 35] + pose_covariance_diagonal: [0.0, 7.0, 14.0, 21.0, 28.0, 35.0] diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp index 1d89527dcf..46e6ec7c76 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp @@ -13,6 +13,8 @@ // limitations under the License. #include "test_ackermann_steering_controller.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" + #include #include @@ -20,9 +22,10 @@ #include #include -using ackermann_steering_controller::CMD_MY_ITFS; -using ackermann_steering_controller::control_mode_type; -using ackermann_steering_controller::STATE_MY_ITFS; +using ackermann_steering_controller::NR_CMD_ITFS; +using ackermann_steering_controller::NR_STATE_ITFS; +using ackermann_steering_controller::NR_REF_ITFS; + class AckermannSteeringControllerTest : public AckermannSteeringControllerFixture { @@ -32,17 +35,17 @@ TEST_F(AckermannSteeringControllerTest, 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_.joints.empty()); + // ASSERT_TRUE(controller_->params_.state_joints.empty()); + // ASSERT_TRUE(controller_->params_.interface_name.empty()); ASSERT_EQ(controller_->params_.reference_timeout, 0.0); 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_); + // 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_); ASSERT_EQ(controller_->params_.reference_timeout, 0.1); } @@ -54,27 +57,33 @@ TEST_F(AckermannSteeringControllerTest, check_exported_intefaces) 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_); - } + EXPECT_EQ(command_intefaces.names[0], controller_->params_.rear_wheel_name + "/" + hardware_interface::HW_IF_VELOCITY); + EXPECT_EQ(command_intefaces.names[1], controller_->params_.front_steer_name + "/" + hardware_interface::HW_IF_VELOCITY); + 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_); - } + EXPECT_EQ(state_intefaces.names[0], controller_->params_.rear_wheel_name + "/" + hardware_interface::HW_IF_POSITION); + EXPECT_EQ(state_intefaces.names[1], controller_->params_.front_steer_name + "/" + hardware_interface::HW_IF_POSITION); + - // check ref itfs + // // 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_); - } + ASSERT_EQ(reference_interfaces.size(), NR_REF_ITFS); + + const std::string ref_itf_name_0 = std::string(controller_->get_node()->get_name()) + "/" + + "linear" + "/" + hardware_interface::HW_IF_VELOCITY; + EXPECT_EQ(reference_interfaces[0].get_name(), ref_itf_name_0); + EXPECT_EQ(reference_interfaces[0].get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ( + reference_interfaces[0].get_interface_name(), std::string("linear") + "/" +hardware_interface::HW_IF_VELOCITY); + + const std::string ref_itf_name_1 = std::string(controller_->get_node()->get_name()) + "/" + + "angular" + "/" + hardware_interface::HW_IF_VELOCITY; + EXPECT_EQ(reference_interfaces[1].get_name(), ref_itf_name_1); + EXPECT_EQ(reference_interfaces[1].get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ( + reference_interfaces[1].get_interface_name(), std::string("angular") + "/" + hardware_interface::HW_IF_VELOCITY); } TEST_F(AckermannSteeringControllerTest, activate_success) @@ -86,23 +95,17 @@ TEST_F(AckermannSteeringControllerTest, activate_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)); - } + EXPECT_TRUE(std::isnan((*msg)->twist.linear.x)); - ASSERT_TRUE(std::isnan((*msg)->duration)); + ASSERT_TRUE(std::isnan((*msg)->twist.angular.z)); - EXPECT_EQ(controller_->reference_interfaces_.size(), joint_names_.size()); + EXPECT_EQ(controller_->reference_interfaces_.size(), NR_REF_ITFS); for (const auto & interface : controller_->reference_interfaces_) { EXPECT_TRUE(std::isnan(interface)); } } + TEST_F(AckermannSteeringControllerTest, update_success) { SetUpController(); @@ -135,75 +138,12 @@ TEST_F(AckermannSteeringControllerTest, reactivate_success) 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_->command_interfaces_[NR_CMD_ITFS-2].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_reference_from_subscribers( - controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - ASSERT_EQ( - controller_->update_and_write_commands( - controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); -} - -TEST_F(AckermannSteeringControllerTest, 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_TRUE(std::isnan(controller_->command_interfaces_[NR_CMD_ITFS-2].get_value())); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(std::isnan(controller_->command_interfaces_[NR_CMD_ITFS-2].get_value())); - // 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(AckermannSteeringControllerTest, test_update_logic_chainable_fast) -{// 1. ageref_timeout - 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; - joint_command_values_[STATE_MY_ITFS] = 111; - std::shared_ptr msg = std::make_shared(); - msg->header.stamp = controller_->get_node()->now() - controller_->ref_timeout_ - rclcpp::Duration::from_seconds(0.1); - 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); - const auto age_of_last_command = controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; - EXPECT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::FAST); - - ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_); - ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT); ASSERT_EQ( controller_->update_reference_from_subscribers( controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), @@ -212,117 +152,181 @@ TEST_F(AckermannSteeringControllerTest, test_update_logic_chainable_fast) controller_->update_and_write_commands( controller_->get_node()->now(), 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], 111); - ASSERT_TRUE(std::isnan(controller_->reference_interfaces_[0])); - for (const auto & interface : controller_->reference_interfaces_) { - EXPECT_TRUE(std::isnan(interface)); - } - - std::shared_ptr msg_2 = std::make_shared(); - msg_2->header.stamp = controller_->get_node()->now() - rclcpp::Duration::from_seconds(0.01); - msg_2->joint_names = joint_names_; - msg_2->displacements.resize(joint_names_.size(), TEST_DISPLACEMENT); - msg_2->velocities.resize(joint_names_.size(), std::numeric_limits::quiet_NaN()); - msg_2->duration = std::numeric_limits::quiet_NaN(); - controller_->input_ref_.writeFromNonRT(msg_2); - const auto age_of_last_command_2 = controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; - EXPECT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::FAST); - - ASSERT_TRUE(age_of_last_command_2 <= controller_->ref_timeout_); - ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT); - ASSERT_EQ( - controller_->update_reference_from_subscribers( - controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - ASSERT_EQ( - controller_->update_and_write_commands( - controller_->get_node()->now(), 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);//exact value - EXPECT_NE(joint_command_values_[STATE_MY_ITFS], 111); - ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT); - for (const auto & interface : controller_->reference_interfaces_) { - EXPECT_FALSE(std::isnan(interface)); - } } -TEST_F(AckermannSteeringControllerTest, test_update_logic_chainable_slow) -{// 1. ageref_timeout - 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; - joint_command_values_[STATE_MY_ITFS] = 111; - std::shared_ptr msg = std::make_shared(); - msg->header.stamp = controller_->get_node()->now() - controller_->ref_timeout_ - rclcpp::Duration::from_seconds(0.1); - 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); - const auto age_of_last_command = controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; - controller_->control_mode_.writeFromNonRT(control_mode_type::SLOW); - - EXPECT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::SLOW); - //age_of_last_command > ref_timeout_ - ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_); - ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT); - ASSERT_EQ( - controller_->update_reference_from_subscribers( - controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - ASSERT_EQ( - controller_->update_and_write_commands( - controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - - EXPECT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::SLOW); - EXPECT_EQ(joint_command_values_[STATE_MY_ITFS], 111); - ASSERT_TRUE(std::isnan(controller_->reference_interfaces_[0])); - for (const auto & interface : controller_->reference_interfaces_) { - EXPECT_TRUE(std::isnan(interface)); - } - - std::shared_ptr msg_2 = std::make_shared(); - msg_2->header.stamp = controller_->get_node()->now() - rclcpp::Duration::from_seconds(0.01); - msg_2->joint_names = joint_names_; - msg_2->displacements.resize(joint_names_.size(), TEST_DISPLACEMENT); - msg_2->velocities.resize(joint_names_.size(), std::numeric_limits::quiet_NaN()); - msg_2->duration = std::numeric_limits::quiet_NaN(); - controller_->input_ref_.writeFromNonRT(msg_2); - const auto age_of_last_command_2 = controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; - //age_of_last_command_2 < ref_timeout_ - ASSERT_TRUE(age_of_last_command_2 <= controller_->ref_timeout_); - ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT); - ASSERT_EQ( - controller_->update_reference_from_subscribers( - controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - ASSERT_EQ( - controller_->update_and_write_commands( - controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - - EXPECT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::SLOW); - EXPECT_EQ(joint_command_values_[STATE_MY_ITFS], TEST_DISPLACEMENT/4);//exact value - EXPECT_NE(joint_command_values_[STATE_MY_ITFS], 111); - ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT/2); - for (const auto & interface : controller_->reference_interfaces_) { - EXPECT_FALSE(std::isnan(interface)); - } -} +// TEST_F(AckermannSteeringControllerTest, 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(AckermannSteeringControllerTest, test_update_logic_chainable_fast) +// {// 1. ageref_timeout +// 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; +// joint_command_values_[STATE_MY_ITFS] = 111; +// std::shared_ptr msg = std::make_shared(); +// msg->header.stamp = controller_->get_node()->now() - controller_->ref_timeout_ - rclcpp::Duration::from_seconds(0.1); +// 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); +// const auto age_of_last_command = controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; +// EXPECT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::FAST); + +// ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_); +// ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT); +// ASSERT_EQ( +// controller_->update_reference_from_subscribers( +// controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), +// controller_interface::return_type::OK); +// ASSERT_EQ( +// controller_->update_and_write_commands( +// controller_->get_node()->now(), 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], 111); +// ASSERT_TRUE(std::isnan(controller_->reference_interfaces_[0])); +// for (const auto & interface : controller_->reference_interfaces_) { +// EXPECT_TRUE(std::isnan(interface)); +// } + +// std::shared_ptr msg_2 = std::make_shared(); +// msg_2->header.stamp = controller_->get_node()->now() - rclcpp::Duration::from_seconds(0.01); +// msg_2->joint_names = joint_names_; +// msg_2->displacements.resize(joint_names_.size(), TEST_DISPLACEMENT); +// msg_2->velocities.resize(joint_names_.size(), std::numeric_limits::quiet_NaN()); +// msg_2->duration = std::numeric_limits::quiet_NaN(); +// controller_->input_ref_.writeFromNonRT(msg_2); +// const auto age_of_last_command_2 = controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; +// EXPECT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::FAST); + +// ASSERT_TRUE(age_of_last_command_2 <= controller_->ref_timeout_); +// ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT); +// ASSERT_EQ( +// controller_->update_reference_from_subscribers( +// controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), +// controller_interface::return_type::OK); +// ASSERT_EQ( +// controller_->update_and_write_commands( +// controller_->get_node()->now(), 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);//exact value +// EXPECT_NE(joint_command_values_[STATE_MY_ITFS], 111); +// ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT); +// for (const auto & interface : controller_->reference_interfaces_) { +// EXPECT_FALSE(std::isnan(interface)); +// } +// } + +// TEST_F(AckermannSteeringControllerTest, test_update_logic_chainable_slow) +// {// 1. ageref_timeout +// 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; +// joint_command_values_[STATE_MY_ITFS] = 111; +// std::shared_ptr msg = std::make_shared(); +// msg->header.stamp = controller_->get_node()->now() - controller_->ref_timeout_ - rclcpp::Duration::from_seconds(0.1); +// 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); +// const auto age_of_last_command = controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; +// controller_->control_mode_.writeFromNonRT(control_mode_type::SLOW); + +// EXPECT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::SLOW); +// //age_of_last_command > ref_timeout_ +// ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_); +// ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT); +// ASSERT_EQ( +// controller_->update_reference_from_subscribers( +// controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), +// controller_interface::return_type::OK); +// ASSERT_EQ( +// controller_->update_and_write_commands( +// controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), +// controller_interface::return_type::OK); + +// EXPECT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::SLOW); +// EXPECT_EQ(joint_command_values_[STATE_MY_ITFS], 111); +// ASSERT_TRUE(std::isnan(controller_->reference_interfaces_[0])); +// for (const auto & interface : controller_->reference_interfaces_) { +// EXPECT_TRUE(std::isnan(interface)); +// } + +// std::shared_ptr msg_2 = std::make_shared(); +// msg_2->header.stamp = controller_->get_node()->now() - rclcpp::Duration::from_seconds(0.01); +// msg_2->joint_names = joint_names_; +// msg_2->displacements.resize(joint_names_.size(), TEST_DISPLACEMENT); +// msg_2->velocities.resize(joint_names_.size(), std::numeric_limits::quiet_NaN()); +// msg_2->duration = std::numeric_limits::quiet_NaN(); +// controller_->input_ref_.writeFromNonRT(msg_2); +// const auto age_of_last_command_2 = controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; +// //age_of_last_command_2 < ref_timeout_ +// ASSERT_TRUE(age_of_last_command_2 <= controller_->ref_timeout_); +// ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT); +// ASSERT_EQ( +// controller_->update_reference_from_subscribers( +// controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), +// controller_interface::return_type::OK); +// ASSERT_EQ( +// controller_->update_and_write_commands( +// controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), +// controller_interface::return_type::OK); + +// EXPECT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::SLOW); +// EXPECT_EQ(joint_command_values_[STATE_MY_ITFS], TEST_DISPLACEMENT/4);//exact value +// EXPECT_NE(joint_command_values_[STATE_MY_ITFS], 111); +// ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT/2); +// for (const auto & interface : controller_->reference_interfaces_) { +// EXPECT_FALSE(std::isnan(interface)); +// } +// } TEST_F(AckermannSteeringControllerTest, publish_status_success) { @@ -330,20 +334,22 @@ TEST_F(AckermannSteeringControllerTest, publish_status_success) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - - ASSERT_EQ( - controller_->update_reference_from_subscribers( - controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); + // auto current_ref = *(input_ref_.readFromRT()); + // ASSERT_EQ( + // controller_->update_reference_from_subscribers( + // controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + // controller_interface::return_type::OK); + controller_->reference_interfaces_[0] = 2.3; + controller_->reference_interfaces_[1] = 4.4; ASSERT_EQ( controller_->update_and_write_commands( controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - ControllerStateMsg msg; + ControllerStateMsgOdom msg; subscribe_and_get_messages(msg); - ASSERT_EQ(msg.set_point, 101.101); + ASSERT_EQ(msg.pose.pose.position.z, 0); } TEST_F(AckermannSteeringControllerTest, receive_message_and_publish_updated_status) @@ -364,10 +370,10 @@ TEST_F(AckermannSteeringControllerTest, receive_message_and_publish_updated_stat controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); //here - ControllerStateMsg msg; + ControllerStateMsgOdom msg; subscribe_and_get_messages(msg); - ASSERT_EQ(msg.set_point, 101.101); + ASSERT_EQ(msg.pose.pose.position.z, 0); publish_commands(controller_->get_node()->now()); ASSERT_TRUE(controller_->wait_for_commands(executor)); @@ -381,68 +387,41 @@ TEST_F(AckermannSteeringControllerTest, receive_message_and_publish_updated_stat controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); //here - EXPECT_EQ(joint_command_values_[CMD_MY_ITFS], 0.45); + EXPECT_EQ(joint_command_values_[NR_CMD_ITFS-2], 0.45); subscribe_and_get_messages(msg); - ASSERT_EQ(msg.set_point, 0.45); -} - -TEST_F(AckermannSteeringControllerTest, test_message_timeout) -{ - 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); - -// try to set command with time before timeout - command is not updated - auto reference = controller_->input_ref_.readFromNonRT(); - auto old_timestamp = (*reference)->header.stamp; - EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->joint_names.size(), joint_names_.size()); - EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->joint_names[0], joint_names_[0]); - EXPECT_TRUE(std::isnan((*reference)->displacements[0])); - EXPECT_TRUE(std::isnan((*reference)->velocities[0])); - EXPECT_TRUE(std::isnan((*reference)->duration)); - publish_commands(controller_->get_node()->now() - controller_->ref_timeout_ - rclcpp::Duration::from_seconds(0.1)); - ASSERT_TRUE(controller_->wait_for_commands(executor)); - ASSERT_EQ(old_timestamp, (*(controller_->input_ref_.readFromNonRT()))->header.stamp); - EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->joint_names.size(), joint_names_.size()); - EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->joint_names[0], joint_names_[0]); - EXPECT_TRUE(std::isnan((*reference)->displacements[0])); - EXPECT_TRUE(std::isnan((*reference)->velocities[0])); - EXPECT_TRUE(std::isnan((*reference)->duration)); + ASSERT_EQ(msg.pose.pose.position.z, 0); } +// TEST_F(AckermannSteeringControllerTest, test_message_timeout) +// { +// 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); + +// // try to set command with time before timeout - command is not updated +// auto reference = controller_->input_ref_.readFromNonRT(); +// auto old_timestamp = (*reference)->header.stamp; +// EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->joint_names.size(), joint_names_.size()); +// EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->joint_names[0], joint_names_[0]); +// EXPECT_TRUE(std::isnan((*reference)->displacements[0])); +// EXPECT_TRUE(std::isnan((*reference)->velocities[0])); +// EXPECT_TRUE(std::isnan((*reference)->duration)); +// publish_commands(controller_->get_node()->now() - controller_->ref_timeout_ - rclcpp::Duration::from_seconds(0.1)); +// ASSERT_TRUE(controller_->wait_for_commands(executor)); +// ASSERT_EQ(old_timestamp, (*(controller_->input_ref_.readFromNonRT()))->header.stamp); +// EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->joint_names.size(), joint_names_.size()); +// EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->joint_names[0], joint_names_[0]); +// EXPECT_TRUE(std::isnan((*reference)->displacements[0])); +// EXPECT_TRUE(std::isnan((*reference)->velocities[0])); +// EXPECT_TRUE(std::isnan((*reference)->duration)); +// } -TEST_F(AckermannSteeringControllerTest, test_message_wrong_num_joints) -{ - 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); - - // try to set command with time before timeout - command is not updated - auto reference = controller_->input_ref_.readFromNonRT(); - auto old_timestamp = (*reference)->header.stamp; - EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->joint_names.size(), joint_names_.size()); - EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->joint_names[0], joint_names_[0]); - EXPECT_TRUE(std::isnan((*reference)->displacements[0])); - EXPECT_TRUE(std::isnan((*reference)->velocities[0])); - EXPECT_TRUE(std::isnan((*reference)->duration)); - publish_commands(controller_->get_node()->now(), {"joint1","joint2"}); - ASSERT_TRUE(controller_->wait_for_commands(executor)); - EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->joint_names.size(), joint_names_.size()); - EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->joint_names[0], joint_names_[0]); - EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->displacements[0])); - EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->velocities[0])); - EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->duration)); -} - TEST_F(AckermannSteeringControllerTest, test_message_accepted) { SetUpController(); @@ -454,86 +433,79 @@ TEST_F(AckermannSteeringControllerTest, test_message_accepted) // try to set command with time before timeout - command is not updated auto reference = controller_->input_ref_.readFromNonRT(); - EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->joint_names.size(), joint_names_.size()); - EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->joint_names[0], joint_names_[0]); - EXPECT_TRUE(std::isnan((*reference)->displacements[0])); - EXPECT_TRUE(std::isnan((*reference)->velocities[0])); - EXPECT_TRUE(std::isnan((*reference)->duration)); + EXPECT_TRUE(std::isnan((*reference)->twist.linear.x)); + EXPECT_TRUE(std::isnan((*reference)->twist.angular.z)); publish_commands(controller_->get_node()->now()); ASSERT_TRUE(controller_->wait_for_commands(executor)); - EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->joint_names.size(), joint_names_.size()); - EXPECT_NE((*(controller_->input_ref_.readFromNonRT()))->joint_names[0], joint_names_[0]); - EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->displacements[0])); - EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->velocities[0])); - EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->duration)); - EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->displacements[0], 0.45); - EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->velocities[0], 0.0); - EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->duration, 1.25); + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x)); + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z)); + EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x,0.45); + EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z,0.0); } -TEST_F(AckermannSteeringControllerTest, test_update_logic) -{ - SetUpController(); - rclcpp::executors::MultiThreadedExecutor executor; - executor.add_node(controller_->get_node()->get_node_base_interface()); - executor.add_node(service_caller_node_->get_node_base_interface()); - - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - - auto reference = controller_->input_ref_.readFromNonRT(); - - // set command statically - static constexpr double TEST_DISPLACEMENT = 23.24; - joint_command_values_[STATE_MY_ITFS] = 111; - std::shared_ptr msg = std::make_shared(); - msg->header.stamp = controller_->get_node()->now() - controller_->ref_timeout_ - rclcpp::Duration::from_seconds(0.1); - 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); - const auto age_of_last_command = controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; - - ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_); - ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT); - ASSERT_EQ( - controller_->update_reference_from_subscribers( - controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - ASSERT_EQ( - controller_->update_and_write_commands( - controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - - EXPECT_EQ(joint_command_values_[STATE_MY_ITFS], 111); - ASSERT_TRUE(std::isnan(controller_->reference_interfaces_[0])); +// TEST_F(AckermannSteeringControllerTest, test_update_logic) +// { +// SetUpController(); +// rclcpp::executors::MultiThreadedExecutor executor; +// executor.add_node(controller_->get_node()->get_node_base_interface()); +// executor.add_node(service_caller_node_->get_node_base_interface()); + +// ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); +// ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + +// auto reference = controller_->input_ref_.readFromNonRT(); + +// // set command statically +// static constexpr double TEST_DISPLACEMENT = 23.24; +// joint_command_values_[STATE_MY_ITFS] = 111; +// std::shared_ptr msg = std::make_shared(); +// msg->header.stamp = controller_->get_node()->now() - controller_->ref_timeout_ - rclcpp::Duration::from_seconds(0.1); +// 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); +// const auto age_of_last_command = controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; + +// ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_); +// ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT); +// ASSERT_EQ( +// controller_->update_reference_from_subscribers( +// controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), +// controller_interface::return_type::OK); +// ASSERT_EQ( +// controller_->update_and_write_commands( +// controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), +// controller_interface::return_type::OK); + +// EXPECT_EQ(joint_command_values_[STATE_MY_ITFS], 111); +// ASSERT_TRUE(std::isnan(controller_->reference_interfaces_[0])); - std::shared_ptr msg_2 = std::make_shared(); - msg_2->header.stamp = controller_->get_node()->now(); - msg_2->joint_names = joint_names_; - msg_2->displacements.resize(joint_names_.size(), TEST_DISPLACEMENT); - msg_2->velocities.resize(joint_names_.size(), std::numeric_limits::quiet_NaN()); - msg_2->duration = std::numeric_limits::quiet_NaN(); - controller_->input_ref_.writeFromNonRT(msg_2); - const auto age_of_last_command_2 = controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; +// std::shared_ptr msg_2 = std::make_shared(); +// msg_2->header.stamp = controller_->get_node()->now(); +// msg_2->joint_names = joint_names_; +// msg_2->displacements.resize(joint_names_.size(), TEST_DISPLACEMENT); +// msg_2->velocities.resize(joint_names_.size(), std::numeric_limits::quiet_NaN()); +// msg_2->duration = std::numeric_limits::quiet_NaN(); +// controller_->input_ref_.writeFromNonRT(msg_2); +// const auto age_of_last_command_2 = controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; - ASSERT_TRUE(age_of_last_command_2 <= controller_->ref_timeout_); - ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT); - ASSERT_EQ( - controller_->update_reference_from_subscribers( - controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - ASSERT_EQ( - controller_->update_and_write_commands( - controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - - EXPECT_EQ(joint_command_values_[STATE_MY_ITFS], TEST_DISPLACEMENT);//exact value - EXPECT_NE(joint_command_values_[STATE_MY_ITFS], 111); - ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT); -} +// ASSERT_TRUE(age_of_last_command_2 <= controller_->ref_timeout_); +// ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT); +// ASSERT_EQ( +// controller_->update_reference_from_subscribers( +// controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), +// controller_interface::return_type::OK); +// ASSERT_EQ( +// controller_->update_and_write_commands( +// controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), +// controller_interface::return_type::OK); + +// EXPECT_EQ(joint_command_values_[STATE_MY_ITFS], TEST_DISPLACEMENT);//exact value +// EXPECT_NE(joint_command_values_[STATE_MY_ITFS], 111); +// ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT); +// } TEST_F(AckermannSteeringControllerTest, test_ref_timeout_zero_for_update) @@ -541,26 +513,29 @@ TEST_F(AckermannSteeringControllerTest, test_ref_timeout_zero_for_update) SetUpController(); rclcpp::executors::MultiThreadedExecutor executor; executor.add_node(controller_->get_node()->get_node_base_interface()); - executor.add_node(service_caller_node_->get_node_base_interface()); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); auto reference = controller_->input_ref_.readFromNonRT(); // set command statically - static constexpr double TEST_DISPLACEMENT = 23.24; + static constexpr double TEST_LINEAR_VELOCITY_X = 1.5; + static constexpr double TEST_ANGULAR_VELOCITY_Z = 1.1; + controller_->ref_timeout_= rclcpp::Duration::from_seconds(0.0); std::shared_ptr msg = std::make_shared(); + msg->header.stamp = controller_->get_node()->now() - rclcpp::Duration::from_seconds(0.0); - 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(); + msg->twist.linear.x = TEST_LINEAR_VELOCITY_X; + msg->twist.linear.y = std::numeric_limits::quiet_NaN(); + msg->twist.linear.z = std::numeric_limits::quiet_NaN(); + msg->twist.angular.x = std::numeric_limits::quiet_NaN(); + msg->twist.angular.y = std::numeric_limits::quiet_NaN(); + msg->twist.angular.z = TEST_ANGULAR_VELOCITY_Z; controller_->input_ref_.writeFromNonRT(msg); const auto age_of_last_command = controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; - ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT); + ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); ASSERT_EQ( controller_->update_reference_from_subscribers( controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), @@ -570,9 +545,9 @@ TEST_F(AckermannSteeringControllerTest, test_ref_timeout_zero_for_update) controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - EXPECT_EQ(joint_command_values_[STATE_MY_ITFS], TEST_DISPLACEMENT); - ASSERT_NE((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT); - ASSERT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->displacements[0])); + EXPECT_EQ(joint_command_values_[NR_STATE_ITFS-2], TEST_LINEAR_VELOCITY_X); + ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); + ASSERT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x)); } TEST_F(AckermannSteeringControllerTest, test_ref_timeout_zero_for_reference_callback) @@ -580,28 +555,20 @@ TEST_F(AckermannSteeringControllerTest, test_ref_timeout_zero_for_reference_call SetUpController(); rclcpp::executors::MultiThreadedExecutor executor; executor.add_node(controller_->get_node()->get_node_base_interface()); - executor.add_node(service_caller_node_->get_node_base_interface()); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->joint_names.size(), joint_names_.size()); - EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->joint_names[0], joint_names_[0]); - EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->displacements[0])); - EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->velocities[0])); - EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->duration)); + EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x)); + EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z)); controller_->ref_timeout_= rclcpp::Duration::from_seconds(0.0); //reference_callback() is called implicitly when publish_commands() is called. publish_commands(controller_->get_node()->now()); ASSERT_TRUE(controller_->wait_for_commands(executor)); - EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->joint_names.size(), joint_names_.size()); - EXPECT_NE((*(controller_->input_ref_.readFromNonRT()))->joint_names[0], joint_names_[0]); - EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->displacements[0])); - EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->velocities[0])); - EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->duration)); - EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->displacements[0], 0.45); - EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->velocities[0], 0.0); - EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->duration, 1.25); + + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x)); + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z)); + EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x, 0.45); + EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z, 0.0); } diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp index ff3c61f097..49c494fae5 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp @@ -15,6 +15,8 @@ #ifndef TEMPLATES__ROS2_CONTROL__CONTROLLER__TEST_ACKERMANN_STEERING_CONTROLLER_HPP_ #define TEMPLATES__ROS2_CONTROL__CONTROLLER__TEST_ACKERMANN_STEERING_CONTROLLER_HPP_ +#include "gmock/gmock.h" + #include #include #include @@ -24,19 +26,19 @@ #include #include "ackermann_steering_controller/ackermann_steering_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 "hardware_interface/types/hardware_interface_type_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 = ackermann_steering_controller::AckermannSteeringController::ControllerStateMsg; +using ControllerStateMsgOdom = ackermann_steering_controller::AckermannSteeringController::ControllerStateMsgOdom; +using ControllerStateMsgTf = ackermann_steering_controller::AckermannSteeringController::ControllerStateMsgTf; using ControllerReferenceMsg = ackermann_steering_controller::AckermannSteeringController::ControllerReferenceMsg; -using ControllerModeSrvType = ackermann_steering_controller::AckermannSteeringController::ControllerModeSrvType; namespace { @@ -54,15 +56,14 @@ class TestableAckermannSteeringController : public ackermann_steering_controller FRIEND_TEST(AckermannSteeringControllerTest, update_success); FRIEND_TEST(AckermannSteeringControllerTest, deactivate_success); FRIEND_TEST(AckermannSteeringControllerTest, reactivate_success); - FRIEND_TEST(AckermannSteeringControllerTest, test_setting_slow_mode_service); - FRIEND_TEST(AckermannSteeringControllerTest, test_update_logic_chainable_fast); - FRIEND_TEST(AckermannSteeringControllerTest, test_update_logic_chainable_slow); + // FRIEND_TEST(AckermannSteeringControllerTest, test_setting_slow_mode_service); + // FRIEND_TEST(AckermannSteeringControllerTest, test_update_logic_chainable_fast); + // FRIEND_TEST(AckermannSteeringControllerTest, test_update_logic_chainable_slow); FRIEND_TEST(AckermannSteeringControllerTest, publish_status_success); FRIEND_TEST(AckermannSteeringControllerTest, receive_message_and_publish_updated_status); - FRIEND_TEST(AckermannSteeringControllerTest, test_message_timeout); - FRIEND_TEST(AckermannSteeringControllerTest, test_message_wrong_num_joints); + // FRIEND_TEST(AckermannSteeringControllerTest, test_message_timeout); FRIEND_TEST(AckermannSteeringControllerTest, test_message_accepted); - FRIEND_TEST(AckermannSteeringControllerTest, test_update_logic); + // FRIEND_TEST(AckermannSteeringControllerTest, test_update_logic); FRIEND_TEST(AckermannSteeringControllerTest, test_ref_timeout_zero_for_update); FRIEND_TEST(AckermannSteeringControllerTest, test_ref_timeout_zero_for_reference_callback); @@ -130,11 +131,8 @@ class AckermannSteeringControllerFixture : public ::testing::Test command_publisher_node_ = std::make_shared("command_publisher"); command_publisher_ = command_publisher_node_->create_publisher( - "/test_ackermann_steering_controller/commands", rclcpp::SystemDefaultsQoS()); + "/test_ackermann_steering_controller/reference", rclcpp::SystemDefaultsQoS()); - service_caller_node_ = std::make_shared("service_caller"); - slow_control_service_client_ = service_caller_node_->create_client( - "/test_ackermann_steering_controller/set_slow_control_mode"); } static void TearDownTestCase() {} @@ -150,34 +148,38 @@ class AckermannSteeringControllerFixture : public ::testing::Test 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()); - } + command_itfs_.emplace_back(hardware_interface::CommandInterface( + rear_wheel_name, hardware_interface::HW_IF_VELOCITY, &joint_command_values_[0])); + command_ifs.emplace_back(command_itfs_.back()); + + command_itfs_.emplace_back(hardware_interface::CommandInterface( + front_steer_name, hardware_interface::HW_IF_VELOCITY, &joint_command_values_[1])); + 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()); - } + state_itfs_.emplace_back(hardware_interface::StateInterface( + rear_wheel_name, hardware_interface::HW_IF_POSITION, &joint_state_values_[0])); + state_ifs.emplace_back(state_itfs_.back()); + + state_itfs_.emplace_back(hardware_interface::StateInterface( + front_steer_name, hardware_interface::HW_IF_POSITION, &joint_state_values_[1])); + 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) + void subscribe_and_get_messages(ControllerStateMsgOdom & msg_odom) { // 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_ackermann_steering_controller/state", 10, subs_callback); + auto subs_callback_odom = [&](const ControllerStateMsgOdom::SharedPtr) {}; + auto subscription_odom = test_subscription_node.create_subscription( + "/test_ackermann_steering_controller/odometry", 10, subs_callback_odom); // call update to publish the test value ASSERT_EQ( @@ -193,7 +195,7 @@ class AckermannSteeringControllerFixture : public ::testing::Test // 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); + wait_set.add_subscription(subscription_odom); while (max_sub_check_loop_count--) { controller_->update_reference_from_subscribers( controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)); @@ -204,18 +206,19 @@ class AckermannSteeringControllerFixture : public ::testing::Test 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)); + rclcpp::MessageInfo msg_odom_info; + ASSERT_TRUE(subscription_odom->take(msg_odom, msg_odom_info)); } // TODO(anyone): add/remove arguments as it suites your command message type void publish_commands( - const rclcpp::Time & stamp, const std::vector & joint_names = {"joint1_test"}, const std::vector & displacements = {0.45}, - const std::vector & velocities = {0.0}, const double duration = 1.25) + const rclcpp::Time & stamp, const double & twist_linear_x = 0.45, + const double & twist_angular_z = 0.0) { auto wait_for_topic = [&](const auto topic_name) { size_t wait_count = 0; @@ -233,42 +236,27 @@ class AckermannSteeringControllerFixture : public ::testing::Test wait_for_topic(command_publisher_->get_topic_name()); ControllerReferenceMsg msg; + msg.header.stamp = stamp; - msg.joint_names = joint_names; - msg.displacements = displacements; - msg.velocities = velocities; - msg.duration = duration; + msg.twist.linear.x = twist_linear_x; + msg.twist.linear.y = std::numeric_limits::quiet_NaN(); + msg.twist.linear.z = std::numeric_limits::quiet_NaN(); + msg.twist.angular.x = std::numeric_limits::quiet_NaN(); + msg.twist.angular.y = std::numeric_limits::quiet_NaN(); + msg.twist.angular.z = twist_angular_z; 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 + std::string rear_wheel_name = "rear_wheel_joint"; + std::string front_steer_name = "front_steer_joint"; // 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::array joint_state_values_ = {1.1, 2.2}; + std::array joint_command_values_ = {101.101, 202.202}; std::vector state_itfs_; std::vector command_itfs_; @@ -279,8 +267,7 @@ class AckermannSteeringControllerFixture : public ::testing::Test 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_ACKERMANN_STEERING_CONTROLLER_HPP_ From a1cb0bcfd1b1a902ed430bf45e542b2704fdfc65 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Mon, 14 Nov 2022 18:40:06 +0100 Subject: [PATCH 009/186] Corrected formatting. --- .../ackermann_steering_controller.hpp | 39 +- .../visibility_control.h | 35 +- .../src/ackermann_steering_controller.cpp | 134 +++-- .../test_ackermann_steering_controller.cpp | 520 +++++++----------- .../test_ackermann_steering_controller.hpp | 71 +-- 5 files changed, 368 insertions(+), 431 deletions(-) diff --git a/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp b/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp index 25f0f6c43d..0feafdc612 100644 --- a/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp +++ b/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp @@ -20,14 +20,14 @@ #include #include #include -#include #include +#include -#include "controller_interface/chainable_controller_interface.hpp" #include "ackermann_steering_controller/odometry.hpp" -#include "hardware_interface/handle.hpp" -#include "ackermann_steering_controller_parameters.hpp" #include "ackermann_steering_controller/visibility_control.h" +#include "ackermann_steering_controller_parameters.hpp" +#include "controller_interface/chainable_controller_interface.hpp" +#include "hardware_interface/handle.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/realtime_buffer.h" @@ -37,9 +37,8 @@ // TODO(anyone): Replace with controller specific messages #include "geometry_msgs/msg/twist.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" -#include "tf2_msgs/msg/tf_message.hpp" #include "nav_msgs/msg/odometry.hpp" - +#include "tf2_msgs/msg/tf_message.hpp" namespace ackermann_steering_controller { @@ -55,35 +54,33 @@ static constexpr size_t NR_REF_ITFS = 2; class AckermannSteeringController : public controller_interface::ChainableControllerInterface { public: - TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC AckermannSteeringController(); - TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC controller_interface::CallbackReturn on_init() override; - TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC controller_interface::InterfaceConfiguration command_interface_configuration() const override; - TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC controller_interface::InterfaceConfiguration state_interface_configuration() const override; - TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC controller_interface::CallbackReturn on_configure( const rclcpp_lifecycle::State & previous_state) override; - TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC controller_interface::CallbackReturn on_activate( const rclcpp_lifecycle::State & previous_state) override; - TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC controller_interface::CallbackReturn on_deactivate( const rclcpp_lifecycle::State & previous_state) override; - TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC - controller_interface::return_type update_reference_from_subscribers( - const rclcpp::Time & time, const rclcpp::Duration & period) override; + ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC - TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC controller_interface::return_type update_and_write_commands( const rclcpp::Time & time, const rclcpp::Duration & period) override; @@ -96,13 +93,11 @@ class AckermannSteeringController : public controller_interface::ChainableContro std::shared_ptr param_listener_; ackermann_steering_controller::Params params_; - // Command subscribers and Controller State publisher rclcpp::Subscription::SharedPtr ref_subscriber_ = nullptr; realtime_tools::RealtimeBuffer> input_ref_; rclcpp::Duration ref_timeout_ = rclcpp::Duration::from_seconds(0.0); // 0ms - using ControllerStatePublisherOdom = realtime_tools::RealtimePublisher; using ControllerStatePublisherTf = realtime_tools::RealtimePublisher; @@ -127,7 +122,8 @@ class AckermannSteeringController : public controller_interface::ChainableContro std::vector registered_front_wheel_handles_; /// Odometry related: - rclcpp::Duration publish_period_ = rclcpp::Duration::from_nanoseconds(0);; + rclcpp::Duration publish_period_ = rclcpp::Duration::from_nanoseconds(0); + ; bool open_loop_; rclcpp::Time previous_publish_timestamp_{0}; /// Velocity command related: @@ -145,7 +141,7 @@ class AckermannSteeringController : public controller_interface::ChainableContro private: // callback for topic interface - TEMPLATES__ROS2_CONTROL__VISIBILITY_LOCAL + ACKERMANN_STEERING_CONTROLLER__VISIBILITY_LOCAL void reference_callback(const std::shared_ptr msg); /// Frame to use for the robot base: @@ -160,7 +156,6 @@ class AckermannSteeringController : public controller_interface::ChainableContro // store last velocity double last_linear_velocity_ = 0.0; double last_angular_velocity_ = 0.0; - }; } // namespace ackermann_steering_controller diff --git a/ackermann_steering_controller/include/ackermann_steering_controller/visibility_control.h b/ackermann_steering_controller/include/ackermann_steering_controller/visibility_control.h index b0fe7a41d9..c494ba3bb4 100644 --- a/ackermann_steering_controller/include/ackermann_steering_controller/visibility_control.h +++ b/ackermann_steering_controller/include/ackermann_steering_controller/visibility_control.h @@ -20,30 +20,33 @@ #if defined _WIN32 || defined __CYGWIN__ #ifdef __GNUC__ -#define TEMPLATES__ROS2_CONTROL__VISIBILITY_EXPORT __attribute__((dllexport)) -#define TEMPLATES__ROS2_CONTROL__VISIBILITY_IMPORT __attribute__((dllimport)) +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_EXPORT __attribute__((dllexport)) +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_IMPORT __attribute__((dllimport)) #else -#define TEMPLATES__ROS2_CONTROL__VISIBILITY_EXPORT __declspec(dllexport) -#define TEMPLATES__ROS2_CONTROL__VISIBILITY_IMPORT __declspec(dllimport) +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_EXPORT __declspec(dllexport) +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_IMPORT __declspec(dllimport) #endif -#ifdef TEMPLATES__ROS2_CONTROL__VISIBILITY_BUILDING_DLL -#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC TEMPLATES__ROS2_CONTROL__VISIBILITY_EXPORT +#ifdef ACKERMANN_STEERING_CONTROLLER__VISIBILITY_BUILDING_DLL +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC \ + ACKERMANN_STEERING_CONTROLLER__VISIBILITY_EXPORT #else -#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC TEMPLATES__ROS2_CONTROL__VISIBILITY_IMPORT +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC \ + ACKERMANN_STEERING_CONTROLLER__VISIBILITY_IMPORT #endif -#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC_TYPE TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC -#define TEMPLATES__ROS2_CONTROL__VISIBILITY_LOCAL +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC_TYPE \ + ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_LOCAL #else -#define TEMPLATES__ROS2_CONTROL__VISIBILITY_EXPORT __attribute__((visibility("default"))) -#define TEMPLATES__ROS2_CONTROL__VISIBILITY_IMPORT +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_EXPORT __attribute__((visibility("default"))) +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_IMPORT #if __GNUC__ >= 4 -#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC __attribute__((visibility("default"))) -#define TEMPLATES__ROS2_CONTROL__VISIBILITY_LOCAL __attribute__((visibility("hidden"))) +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC __attribute__((visibility("default"))) +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_LOCAL __attribute__((visibility("hidden"))) #else -#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC -#define TEMPLATES__ROS2_CONTROL__VISIBILITY_LOCAL +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_LOCAL #endif -#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC_TYPE +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC_TYPE #endif #endif // ACKERMANN_STEERING_CONTROLLER__VISIBILITY_CONTROL_H_ diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.cpp b/ackermann_steering_controller/src/ackermann_steering_controller.cpp index 40af25ede9..02f36ecb16 100644 --- a/ackermann_steering_controller/src/ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/src/ackermann_steering_controller.cpp @@ -16,10 +16,10 @@ #include #include +#include #include -#include #include -#include +#include #include "controller_interface/helpers.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" @@ -43,7 +43,8 @@ static constexpr rmw_qos_profile_t rmw_qos_profile_services_hist_keep_all = { RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT, false}; -using ControllerReferenceMsg = ackermann_steering_controller::AckermannSteeringController::ControllerReferenceMsg; +using ControllerReferenceMsg = + ackermann_steering_controller::AckermannSteeringController::ControllerReferenceMsg; // called from RT control loop void reset_controller_reference_msg( @@ -51,7 +52,7 @@ void reset_controller_reference_msg( const std::shared_ptr & node) { msg->header.stamp = node->now(); - msg->twist.linear.x = std::numeric_limits::quiet_NaN(); + msg->twist.linear.x = std::numeric_limits::quiet_NaN(); msg->twist.linear.y = std::numeric_limits::quiet_NaN(); msg->twist.linear.z = std::numeric_limits::quiet_NaN(); msg->twist.angular.x = std::numeric_limits::quiet_NaN(); @@ -63,14 +64,19 @@ void reset_controller_reference_msg( namespace ackermann_steering_controller { -AckermannSteeringController::AckermannSteeringController() : controller_interface::ChainableControllerInterface() {} +AckermannSteeringController::AckermannSteeringController() +: controller_interface::ChainableControllerInterface() +{ +} controller_interface::CallbackReturn AckermannSteeringController::on_init() { - - try { + try + { param_listener_ = std::make_shared(get_node()); - } catch (const std::exception & e) { + } + catch (const std::exception & e) + { fprintf(stderr, "Exception thrown during controller's init with message: %s \n", e.what()); return controller_interface::CallbackReturn::ERROR; } @@ -100,15 +106,18 @@ controller_interface::CallbackReturn AckermannSteeringController::on_configure( std::bind(&AckermannSteeringController::reference_callback, this, std::placeholders::_1)); std::shared_ptr msg = std::make_shared(); - reset_controller_reference_msg(msg,get_node()); + reset_controller_reference_msg(msg, get_node()); input_ref_.writeFromNonRT(msg); - try { + try + { // State publisher - odom_s_publisher_ = - get_node()->create_publisher("~/odometry", rclcpp::SystemDefaultsQoS()); + odom_s_publisher_ = get_node()->create_publisher( + "~/odometry", rclcpp::SystemDefaultsQoS()); rt_odom_state_publisher_ = std::make_unique(odom_s_publisher_); - } catch (const std::exception & e) { + } + catch (const std::exception & e) + { fprintf( stderr, "Exception thrown during publisher creation at configure stage with message : %s \n", e.what()); @@ -130,15 +139,19 @@ controller_interface::CallbackReturn AckermannSteeringController::on_configure( const size_t diagonal_index = NUM_DIMENSIONS * index + index; covariance[diagonal_index] = params_.pose_covariance_diagonal[index]; covariance[diagonal_index] = params_.twist_covariance_diagonal[index]; - } + } rt_odom_state_publisher_->unlock(); - try { + try + { // Tf State publisher tf_odom_s_publisher_ = get_node()->create_publisher( "~/tf_odometry", rclcpp::SystemDefaultsQoS()); - rt_tf_odom_state_publisher_ = std::make_unique(tf_odom_s_publisher_); - } catch (const std::exception & e) { + rt_tf_odom_state_publisher_ = + std::make_unique(tf_odom_s_publisher_); + } + catch (const std::exception & e) + { fprintf( stderr, "Exception thrown during publisher creation at configure stage with message : %s \n", e.what()); @@ -154,17 +167,19 @@ controller_interface::CallbackReturn AckermannSteeringController::on_configure( rt_tf_odom_state_publisher_->unlock(); // calculation publication period of odometry and tf odometry messages - publish_period_ = rclcpp::Durationf::from_seconds(1.0 / params_.publish_rate); + publish_period_ = rclcpp::Duration::from_seconds(1.0 / params_.publish_rate); fprintf(stderr, "publish_period_ = %d \n", publish_period_); RCLCPP_INFO(get_node()->get_logger(), "configure successful"); return controller_interface::CallbackReturn::SUCCESS; } -void AckermannSteeringController::reference_callback(const std::shared_ptr msg) +void AckermannSteeringController::reference_callback( + const std::shared_ptr msg) { // if no timestamp provided use current time for command timestamp - if (msg->header.stamp.sec == 0 && msg->header.stamp.nanosec == 0u) { + if (msg->header.stamp.sec == 0 && msg->header.stamp.nanosec == 0u) + { RCLCPP_WARN( get_node()->get_logger(), "Timestamp in header is missing, using current time as command timestamp."); @@ -172,25 +187,27 @@ void AckermannSteeringController::reference_callback(const std::shared_ptrnow() - msg->header.stamp; - if (ref_timeout_ == rclcpp::Duration::from_seconds(0) || age_of_last_command <= ref_timeout_) { + if (ref_timeout_ == rclcpp::Duration::from_seconds(0) || age_of_last_command <= ref_timeout_) + { input_ref_.writeFromNonRT(msg); - } else { + } + else + { RCLCPP_ERROR( get_node()->get_logger(), "Received message has timestamp %.10f older for %.10f which is more then allowed timeout " "(%.4f).", rclcpp::Time(msg->header.stamp).seconds(), age_of_last_command.seconds(), ref_timeout_.seconds()); - } - + } } -controller_interface::InterfaceConfiguration AckermannSteeringController::command_interface_configuration() const +controller_interface::InterfaceConfiguration +AckermannSteeringController::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(NR_CMD_ITFS); command_interfaces_config.names.push_back( params_.rear_wheel_name + "/" + hardware_interface::HW_IF_VELOCITY); @@ -200,7 +217,8 @@ controller_interface::InterfaceConfiguration AckermannSteeringController::comman return command_interfaces_config; } -controller_interface::InterfaceConfiguration AckermannSteeringController::state_interface_configuration() const +controller_interface::InterfaceConfiguration +AckermannSteeringController::state_interface_configuration() const { controller_interface::InterfaceConfiguration state_interfaces_config; state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; @@ -209,14 +227,13 @@ controller_interface::InterfaceConfiguration AckermannSteeringController::state_ state_interfaces_config.names.push_back( params_.rear_wheel_name + "/" + hardware_interface::HW_IF_POSITION); state_interfaces_config.names.push_back( - params_.front_steer_name + "/" + hardware_interface::HW_IF_POSITION); + params_.front_steer_name + "/" + hardware_interface::HW_IF_POSITION); return state_interfaces_config; } - - -std::vector AckermannSteeringController::on_export_reference_interfaces() +std::vector +AckermannSteeringController::on_export_reference_interfaces() { reference_interfaces_.resize(NR_REF_ITFS, std::numeric_limits::quiet_NaN()); @@ -254,7 +271,8 @@ controller_interface::CallbackReturn AckermannSteeringController::on_deactivate( { // TODO(anyone): depending on number of interfaces, use definitions, e.g., `CMD_MY_ITFS`, // instead of a loop - for (size_t i = 0; i < NR_CMD_ITFS; ++i) { + for (size_t i = 0; i < NR_CMD_ITFS; ++i) + { command_interfaces_[i].set_value(std::numeric_limits::quiet_NaN()); } return controller_interface::CallbackReturn::SUCCESS; @@ -268,15 +286,20 @@ controller_interface::return_type AckermannSteeringController::update_reference_ // TODO(anyone): depending on number of interfaces, use definitions, e.g., `CMD_MY_ITFS`, // instead of a loop - // send message only if there is no timeout - if (age_of_last_command <= ref_timeout_ || ref_timeout_ == rclcpp::Duration::from_seconds(0)) { - if (!std::isnan(current_ref->twist.linear.x) && !std::isnan(current_ref->twist.angular.z)) { + // send message only if there is no timeout + if (age_of_last_command <= ref_timeout_ || ref_timeout_ == rclcpp::Duration::from_seconds(0)) + { + if (!std::isnan(current_ref->twist.linear.x) && !std::isnan(current_ref->twist.angular.z)) + { reference_interfaces_[0] = current_ref->twist.linear.x; - reference_interfaces_[1] = current_ref->twist.angular.z; + reference_interfaces_[1] = current_ref->twist.angular.z; } - } else { + } + else + { reference_interfaces_[0] = std::numeric_limits::quiet_NaN(); - reference_interfaces_[1] = std::numeric_limits::quiet_NaN();} + reference_interfaces_[1] = std::numeric_limits::quiet_NaN(); + } return controller_interface::return_type::OK; } @@ -294,11 +317,11 @@ controller_interface::return_type AckermannSteeringController::update_and_write_ if (std::isnan(wheel_position) || std::isnan(steer_position)) { - return controller_interface::return_type::OK; + return controller_interface::return_type::OK; } // Estimate linear and angular velocity using joint information - odometry_.update(wheel_position, steer_position,time); + odometry_.update(wheel_position, steer_position, time); } // MOVE ROBOT @@ -315,8 +338,7 @@ controller_interface::return_type AckermannSteeringController::update_and_write_ last_linear_velocity_ = reference_interfaces_[0]; last_angular_velocity_ = reference_interfaces_[1]; - previous_publish_timestamp_ = time; - + previous_publish_timestamp_ = time; // omega = linear_vel / radius command_interfaces_[0].set_value(last_linear_velocity_ / params_.wheel_radius); @@ -327,18 +349,21 @@ controller_interface::return_type AckermannSteeringController::update_and_write_ reference_interfaces_[0] = std::numeric_limits::quiet_NaN(); reference_interfaces_[1] = std::numeric_limits::quiet_NaN(); } - fprintf(stderr, "previous_publish_timestamp_ + publish_period_ < time = %d, %d, %d, %d\n", time, - previous_publish_timestamp_, publish_period_, (previous_publish_timestamp_ + publish_period_) < time); - // fprintf(stderr, "Exception thrown during controller's init with message: %s \n", e.what()); + fprintf( + stderr, "previous_publish_timestamp_ + publish_period_ < time = %d, %d, %d, %d\n", time, + previous_publish_timestamp_, publish_period_, + (previous_publish_timestamp_ + publish_period_) < time); + // fprintf(stderr, "Exception thrown during controller's init with message: %s \n", e.what()); // Publish odometry message if (previous_publish_timestamp_ + publish_period_ < time) { fprintf(stderr, "debuggin2 \n"); - fprintf(stderr, "previous_publish_timestamp_ + publish_period_ < time = %d, %d, %d, %d\n", time, - previous_publish_timestamp_, publish_period_, (previous_publish_timestamp_ + publish_period_) < time); + fprintf( + stderr, "previous_publish_timestamp_ + publish_period_ < time = %d, %d, %d, %d\n", time, + previous_publish_timestamp_, publish_period_, + (previous_publish_timestamp_ + publish_period_) < time); - previous_publish_timestamp_ += publish_period_; // Compute and store orientation info tf2::Quaternion orientation; @@ -360,14 +385,16 @@ controller_interface::return_type AckermannSteeringController::update_and_write_ if (params_.enable_odom_tf && rt_tf_odom_state_publisher_->trylock()) { rt_tf_odom_state_publisher_->msg_.transforms.front().header.stamp = time; - rt_tf_odom_state_publisher_->msg_.transforms.front().transform.translation.x = odometry_.getX(); - rt_tf_odom_state_publisher_->msg_.transforms.front().transform.translation.y = odometry_.getY(); - rt_tf_odom_state_publisher_->msg_.transforms.front().transform.rotation = tf2::toMsg(orientation); + rt_tf_odom_state_publisher_->msg_.transforms.front().transform.translation.x = + odometry_.getX(); + rt_tf_odom_state_publisher_->msg_.transforms.front().transform.translation.y = + odometry_.getY(); + rt_tf_odom_state_publisher_->msg_.transforms.front().transform.rotation = + tf2::toMsg(orientation); rt_tf_odom_state_publisher_->unlockAndPublish(); } } - return controller_interface::return_type::OK; } @@ -376,4 +403,5 @@ controller_interface::return_type AckermannSteeringController::update_and_write_ #include "pluginlib/class_list_macros.hpp" PLUGINLIB_EXPORT_CLASS( - ackermann_steering_controller::AckermannSteeringController, controller_interface::ChainableControllerInterface) + ackermann_steering_controller::AckermannSteeringController, + controller_interface::ChainableControllerInterface) diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp index 46e6ec7c76..d1de3e700e 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp @@ -12,22 +12,21 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "test_ackermann_steering_controller.hpp" -#include "hardware_interface/types/hardware_interface_type_values.hpp" - - #include #include #include #include #include +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "test_ackermann_steering_controller.hpp" + using ackermann_steering_controller::NR_CMD_ITFS; -using ackermann_steering_controller::NR_STATE_ITFS; using ackermann_steering_controller::NR_REF_ITFS; +using ackermann_steering_controller::NR_STATE_ITFS; - -class AckermannSteeringControllerTest : public AckermannSteeringControllerFixture +class AckermannSteeringControllerTest +: public AckermannSteeringControllerFixture { }; @@ -57,33 +56,41 @@ TEST_F(AckermannSteeringControllerTest, check_exported_intefaces) auto command_intefaces = controller_->command_interface_configuration(); ASSERT_EQ(command_intefaces.names.size(), joint_command_values_.size()); - EXPECT_EQ(command_intefaces.names[0], controller_->params_.rear_wheel_name + "/" + hardware_interface::HW_IF_VELOCITY); - EXPECT_EQ(command_intefaces.names[1], controller_->params_.front_steer_name + "/" + hardware_interface::HW_IF_VELOCITY); - + EXPECT_EQ( + command_intefaces.names[0], + controller_->params_.rear_wheel_name + "/" + hardware_interface::HW_IF_VELOCITY); + EXPECT_EQ( + command_intefaces.names[1], + controller_->params_.front_steer_name + "/" + hardware_interface::HW_IF_VELOCITY); auto state_intefaces = controller_->state_interface_configuration(); ASSERT_EQ(state_intefaces.names.size(), joint_state_values_.size()); - EXPECT_EQ(state_intefaces.names[0], controller_->params_.rear_wheel_name + "/" + hardware_interface::HW_IF_POSITION); - EXPECT_EQ(state_intefaces.names[1], controller_->params_.front_steer_name + "/" + hardware_interface::HW_IF_POSITION); - + EXPECT_EQ( + state_intefaces.names[0], + controller_->params_.rear_wheel_name + "/" + hardware_interface::HW_IF_POSITION); + EXPECT_EQ( + state_intefaces.names[1], + controller_->params_.front_steer_name + "/" + hardware_interface::HW_IF_POSITION); - // // check ref itfs + // // check ref itfs auto reference_interfaces = controller_->export_reference_interfaces(); ASSERT_EQ(reference_interfaces.size(), NR_REF_ITFS); const std::string ref_itf_name_0 = std::string(controller_->get_node()->get_name()) + "/" + - "linear" + "/" + hardware_interface::HW_IF_VELOCITY; + "linear" + "/" + hardware_interface::HW_IF_VELOCITY; EXPECT_EQ(reference_interfaces[0].get_name(), ref_itf_name_0); EXPECT_EQ(reference_interfaces[0].get_prefix_name(), controller_->get_node()->get_name()); EXPECT_EQ( - reference_interfaces[0].get_interface_name(), std::string("linear") + "/" +hardware_interface::HW_IF_VELOCITY); + reference_interfaces[0].get_interface_name(), + std::string("linear") + "/" + hardware_interface::HW_IF_VELOCITY); const std::string ref_itf_name_1 = std::string(controller_->get_node()->get_name()) + "/" + - "angular" + "/" + hardware_interface::HW_IF_VELOCITY; + "angular" + "/" + hardware_interface::HW_IF_VELOCITY; EXPECT_EQ(reference_interfaces[1].get_name(), ref_itf_name_1); EXPECT_EQ(reference_interfaces[1].get_prefix_name(), controller_->get_node()->get_name()); EXPECT_EQ( - reference_interfaces[1].get_interface_name(), std::string("angular") + "/" + hardware_interface::HW_IF_VELOCITY); + reference_interfaces[1].get_interface_name(), + std::string("angular") + "/" + hardware_interface::HW_IF_VELOCITY); } TEST_F(AckermannSteeringControllerTest, activate_success) @@ -100,12 +107,12 @@ TEST_F(AckermannSteeringControllerTest, activate_success) ASSERT_TRUE(std::isnan((*msg)->twist.angular.z)); EXPECT_EQ(controller_->reference_interfaces_.size(), NR_REF_ITFS); - for (const auto & interface : controller_->reference_interfaces_) { + for (const auto & interface : controller_->reference_interfaces_) + { EXPECT_TRUE(std::isnan(interface)); } } - TEST_F(AckermannSteeringControllerTest, update_success) { SetUpController(); @@ -114,9 +121,7 @@ TEST_F(AckermannSteeringControllerTest, update_success) ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ( - controller_->update_reference_from_subscribers( - controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); + controller_->update_reference_from_subscribers(), controller_interface::return_type::OK); ASSERT_EQ( controller_->update_and_write_commands( controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), @@ -138,11 +143,11 @@ TEST_F(AckermannSteeringControllerTest, reactivate_success) 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_[NR_CMD_ITFS-2].get_value(), 101.101); + ASSERT_EQ(controller_->command_interfaces_[NR_CMD_ITFS - 2].get_value(), 101.101); ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_TRUE(std::isnan(controller_->command_interfaces_[NR_CMD_ITFS-2].get_value())); + ASSERT_TRUE(std::isnan(controller_->command_interfaces_[NR_CMD_ITFS - 2].get_value())); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_TRUE(std::isnan(controller_->command_interfaces_[NR_CMD_ITFS-2].get_value())); + ASSERT_TRUE(std::isnan(controller_->command_interfaces_[NR_CMD_ITFS - 2].get_value())); ASSERT_EQ( controller_->update_reference_from_subscribers( @@ -154,193 +159,14 @@ TEST_F(AckermannSteeringControllerTest, reactivate_success) controller_interface::return_type::OK); } - -// TEST_F(AckermannSteeringControllerTest, 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(AckermannSteeringControllerTest, test_update_logic_chainable_fast) -// {// 1. ageref_timeout -// 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; -// joint_command_values_[STATE_MY_ITFS] = 111; -// std::shared_ptr msg = std::make_shared(); -// msg->header.stamp = controller_->get_node()->now() - controller_->ref_timeout_ - rclcpp::Duration::from_seconds(0.1); -// 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); -// const auto age_of_last_command = controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; -// EXPECT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::FAST); - -// ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_); -// ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT); -// ASSERT_EQ( -// controller_->update_reference_from_subscribers( -// controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), -// controller_interface::return_type::OK); -// ASSERT_EQ( -// controller_->update_and_write_commands( -// controller_->get_node()->now(), 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], 111); -// ASSERT_TRUE(std::isnan(controller_->reference_interfaces_[0])); -// for (const auto & interface : controller_->reference_interfaces_) { -// EXPECT_TRUE(std::isnan(interface)); -// } - -// std::shared_ptr msg_2 = std::make_shared(); -// msg_2->header.stamp = controller_->get_node()->now() - rclcpp::Duration::from_seconds(0.01); -// msg_2->joint_names = joint_names_; -// msg_2->displacements.resize(joint_names_.size(), TEST_DISPLACEMENT); -// msg_2->velocities.resize(joint_names_.size(), std::numeric_limits::quiet_NaN()); -// msg_2->duration = std::numeric_limits::quiet_NaN(); -// controller_->input_ref_.writeFromNonRT(msg_2); -// const auto age_of_last_command_2 = controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; -// EXPECT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::FAST); - -// ASSERT_TRUE(age_of_last_command_2 <= controller_->ref_timeout_); -// ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT); -// ASSERT_EQ( -// controller_->update_reference_from_subscribers( -// controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), -// controller_interface::return_type::OK); -// ASSERT_EQ( -// controller_->update_and_write_commands( -// controller_->get_node()->now(), 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);//exact value -// EXPECT_NE(joint_command_values_[STATE_MY_ITFS], 111); -// ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT); -// for (const auto & interface : controller_->reference_interfaces_) { -// EXPECT_FALSE(std::isnan(interface)); -// } -// } - -// TEST_F(AckermannSteeringControllerTest, test_update_logic_chainable_slow) -// {// 1. ageref_timeout -// 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; -// joint_command_values_[STATE_MY_ITFS] = 111; -// std::shared_ptr msg = std::make_shared(); -// msg->header.stamp = controller_->get_node()->now() - controller_->ref_timeout_ - rclcpp::Duration::from_seconds(0.1); -// 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); -// const auto age_of_last_command = controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; -// controller_->control_mode_.writeFromNonRT(control_mode_type::SLOW); - -// EXPECT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::SLOW); -// //age_of_last_command > ref_timeout_ -// ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_); -// ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT); -// ASSERT_EQ( -// controller_->update_reference_from_subscribers( -// controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), -// controller_interface::return_type::OK); -// ASSERT_EQ( -// controller_->update_and_write_commands( -// controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), -// controller_interface::return_type::OK); - -// EXPECT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::SLOW); -// EXPECT_EQ(joint_command_values_[STATE_MY_ITFS], 111); -// ASSERT_TRUE(std::isnan(controller_->reference_interfaces_[0])); -// for (const auto & interface : controller_->reference_interfaces_) { -// EXPECT_TRUE(std::isnan(interface)); -// } - -// std::shared_ptr msg_2 = std::make_shared(); -// msg_2->header.stamp = controller_->get_node()->now() - rclcpp::Duration::from_seconds(0.01); -// msg_2->joint_names = joint_names_; -// msg_2->displacements.resize(joint_names_.size(), TEST_DISPLACEMENT); -// msg_2->velocities.resize(joint_names_.size(), std::numeric_limits::quiet_NaN()); -// msg_2->duration = std::numeric_limits::quiet_NaN(); -// controller_->input_ref_.writeFromNonRT(msg_2); -// const auto age_of_last_command_2 = controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; -// //age_of_last_command_2 < ref_timeout_ -// ASSERT_TRUE(age_of_last_command_2 <= controller_->ref_timeout_); -// ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT); -// ASSERT_EQ( -// controller_->update_reference_from_subscribers( -// controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), -// controller_interface::return_type::OK); -// ASSERT_EQ( -// controller_->update_and_write_commands( -// controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), -// controller_interface::return_type::OK); - -// EXPECT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::SLOW); -// EXPECT_EQ(joint_command_values_[STATE_MY_ITFS], TEST_DISPLACEMENT/4);//exact value -// EXPECT_NE(joint_command_values_[STATE_MY_ITFS], 111); -// ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT/2); -// for (const auto & interface : controller_->reference_interfaces_) { -// EXPECT_FALSE(std::isnan(interface)); -// } -// } - TEST_F(AckermannSteeringControllerTest, publish_status_success) { SetUpController(); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - // auto current_ref = *(input_ref_.readFromRT()); - // ASSERT_EQ( - // controller_->update_reference_from_subscribers( - // controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), - // controller_interface::return_type::OK); - controller_->reference_interfaces_[0] = 2.3; - controller_->reference_interfaces_[1] = 4.4; + controller_->reference_interfaces_[0] = 2.3; + controller_->reference_interfaces_[1] = 4.4; ASSERT_EQ( controller_->update_and_write_commands( controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), @@ -369,7 +195,6 @@ TEST_F(AckermannSteeringControllerTest, receive_message_and_publish_updated_stat controller_->update_and_write_commands( controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); -//here ControllerStateMsgOdom msg; subscribe_and_get_messages(msg); @@ -386,41 +211,41 @@ TEST_F(AckermannSteeringControllerTest, receive_message_and_publish_updated_stat controller_->update_and_write_commands( controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); -//here - EXPECT_EQ(joint_command_values_[NR_CMD_ITFS-2], 0.45); + EXPECT_EQ(joint_command_values_[NR_CMD_ITFS - 2], 0.45); subscribe_and_get_messages(msg); ASSERT_EQ(msg.pose.pose.position.z, 0); } -// TEST_F(AckermannSteeringControllerTest, test_message_timeout) -// { -// 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); - -// // try to set command with time before timeout - command is not updated -// auto reference = controller_->input_ref_.readFromNonRT(); -// auto old_timestamp = (*reference)->header.stamp; -// EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->joint_names.size(), joint_names_.size()); -// EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->joint_names[0], joint_names_[0]); -// EXPECT_TRUE(std::isnan((*reference)->displacements[0])); -// EXPECT_TRUE(std::isnan((*reference)->velocities[0])); -// EXPECT_TRUE(std::isnan((*reference)->duration)); -// publish_commands(controller_->get_node()->now() - controller_->ref_timeout_ - rclcpp::Duration::from_seconds(0.1)); -// ASSERT_TRUE(controller_->wait_for_commands(executor)); -// ASSERT_EQ(old_timestamp, (*(controller_->input_ref_.readFromNonRT()))->header.stamp); -// EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->joint_names.size(), joint_names_.size()); -// EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->joint_names[0], joint_names_[0]); -// EXPECT_TRUE(std::isnan((*reference)->displacements[0])); -// EXPECT_TRUE(std::isnan((*reference)->velocities[0])); -// EXPECT_TRUE(std::isnan((*reference)->duration)); -// } +TEST_F(AckermannSteeringControllerTest, test_message_timeout) +{ + 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); + // try to set command with time before timeout - command is not updated + auto reference = controller_->input_ref_.readFromNonRT(); + auto old_timestamp = (*reference)->header.stamp; + EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->joint_names.size(), joint_names_.size()); + EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->joint_names[0], joint_names_[0]); + EXPECT_TRUE(std::isnan((*reference)->displacements[0])); + EXPECT_TRUE(std::isnan((*reference)->velocities[0])); + EXPECT_TRUE(std::isnan((*reference)->duration)); + publish_commands( + controller_->get_node()->now() - controller_->ref_timeout_ - + rclcpp::Duration::from_seconds(0.1)); + ASSERT_TRUE(controller_->wait_for_commands(executor)); + ASSERT_EQ(old_timestamp, (*(controller_->input_ref_.readFromNonRT()))->header.stamp); + EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->joint_names.size(), joint_names_.size()); + EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->joint_names[0], joint_names_[0]); + EXPECT_TRUE(std::isnan((*reference)->displacements[0])); + EXPECT_TRUE(std::isnan((*reference)->velocities[0])); + EXPECT_TRUE(std::isnan((*reference)->duration)); +} TEST_F(AckermannSteeringControllerTest, test_message_accepted) { @@ -439,74 +264,154 @@ TEST_F(AckermannSteeringControllerTest, test_message_accepted) ASSERT_TRUE(controller_->wait_for_commands(executor)); EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x)); EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z)); - EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x,0.45); - EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z,0.0); + EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x, 0.45); + EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z, 0.0); +} + +TEST_F(AckermannSteeringControllerTest, test_update_logic_chainable) +{ + // 1. ageref_timeout + 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; + joint_command_values_[STATE_MY_ITFS] = 111; + std::shared_ptr msg = std::make_shared(); + msg->header.stamp = controller_->get_node()->now() - controller_->ref_timeout_ - + rclcpp::Duration::from_seconds(0.1); + 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); + const auto age_of_last_command = + controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; + + // age_of_last_command > ref_timeout_ + ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_); + ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT); + ASSERT_EQ( + controller_->update_reference_from_subscribers( + controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + ASSERT_EQ( + controller_->update_and_write_commands( + controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::SLOW); + EXPECT_EQ(joint_command_values_[STATE_MY_ITFS], 111); + ASSERT_TRUE(std::isnan(controller_->reference_interfaces_[0])); + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } + + std::shared_ptr msg_2 = std::make_shared(); + msg_2->header.stamp = controller_->get_node()->now() - rclcpp::Duration::from_seconds(0.01); + msg_2->joint_names = joint_names_; + msg_2->displacements.resize(joint_names_.size(), TEST_DISPLACEMENT); + msg_2->velocities.resize(joint_names_.size(), std::numeric_limits::quiet_NaN()); + msg_2->duration = std::numeric_limits::quiet_NaN(); + controller_->input_ref_.writeFromNonRT(msg_2); + const auto age_of_last_command_2 = + controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; + + // age_of_last_command_2 < ref_timeout_ + ASSERT_TRUE(age_of_last_command_2 <= controller_->ref_timeout_); + ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT); + ASSERT_EQ( + controller_->update_reference_from_subscribers( + controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + ASSERT_EQ( + controller_->update_and_write_commands( + controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + EXPECT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::SLOW); + EXPECT_EQ(joint_command_values_[STATE_MY_ITFS], TEST_DISPLACEMENT / 4); + EXPECT_NE(joint_command_values_[STATE_MY_ITFS], 111); + ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT / 2); + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_FALSE(std::isnan(interface)); + } } -// TEST_F(AckermannSteeringControllerTest, test_update_logic) -// { -// SetUpController(); -// rclcpp::executors::MultiThreadedExecutor executor; -// executor.add_node(controller_->get_node()->get_node_base_interface()); -// executor.add_node(service_caller_node_->get_node_base_interface()); - -// ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); -// ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - -// auto reference = controller_->input_ref_.readFromNonRT(); - -// // set command statically -// static constexpr double TEST_DISPLACEMENT = 23.24; -// joint_command_values_[STATE_MY_ITFS] = 111; -// std::shared_ptr msg = std::make_shared(); -// msg->header.stamp = controller_->get_node()->now() - controller_->ref_timeout_ - rclcpp::Duration::from_seconds(0.1); -// 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); -// const auto age_of_last_command = controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; - -// ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_); -// ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT); -// ASSERT_EQ( -// controller_->update_reference_from_subscribers( -// controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), -// controller_interface::return_type::OK); -// ASSERT_EQ( -// controller_->update_and_write_commands( -// controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), -// controller_interface::return_type::OK); - -// EXPECT_EQ(joint_command_values_[STATE_MY_ITFS], 111); -// ASSERT_TRUE(std::isnan(controller_->reference_interfaces_[0])); - -// std::shared_ptr msg_2 = std::make_shared(); -// msg_2->header.stamp = controller_->get_node()->now(); -// msg_2->joint_names = joint_names_; -// msg_2->displacements.resize(joint_names_.size(), TEST_DISPLACEMENT); -// msg_2->velocities.resize(joint_names_.size(), std::numeric_limits::quiet_NaN()); -// msg_2->duration = std::numeric_limits::quiet_NaN(); -// controller_->input_ref_.writeFromNonRT(msg_2); -// const auto age_of_last_command_2 = controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; - -// ASSERT_TRUE(age_of_last_command_2 <= controller_->ref_timeout_); -// ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT); -// ASSERT_EQ( -// controller_->update_reference_from_subscribers( -// controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), -// controller_interface::return_type::OK); -// ASSERT_EQ( -// controller_->update_and_write_commands( -// controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), -// controller_interface::return_type::OK); - -// EXPECT_EQ(joint_command_values_[STATE_MY_ITFS], TEST_DISPLACEMENT);//exact value -// EXPECT_NE(joint_command_values_[STATE_MY_ITFS], 111); -// ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT); -// } +TEST_F(AckermannSteeringControllerTest, test_update_logic) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + executor.add_node(service_caller_node_->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto reference = controller_->input_ref_.readFromNonRT(); + + // set command statically + static constexpr double TEST_DISPLACEMENT = 23.24; + joint_command_values_[STATE_MY_ITFS] = 111; + std::shared_ptr msg = std::make_shared(); + msg->header.stamp = controller_->get_node()->now() - controller_->ref_timeout_ - + rclcpp::Duration::from_seconds(0.1); + 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); + const auto age_of_last_command = + controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; + + ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_); + ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT); + ASSERT_EQ( + controller_->update_reference_from_subscribers( + controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + ASSERT_EQ( + controller_->update_and_write_commands( + controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + EXPECT_EQ(joint_command_values_[STATE_MY_ITFS], 111); + ASSERT_TRUE(std::isnan(controller_->reference_interfaces_[0])); + + std::shared_ptr msg_2 = std::make_shared(); + msg_2->header.stamp = controller_->get_node()->now(); + msg_2->joint_names = joint_names_; + msg_2->displacements.resize(joint_names_.size(), TEST_DISPLACEMENT); + msg_2->velocities.resize(joint_names_.size(), std::numeric_limits::quiet_NaN()); + msg_2->duration = std::numeric_limits::quiet_NaN(); + controller_->input_ref_.writeFromNonRT(msg_2); + const auto age_of_last_command_2 = + controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; + + ASSERT_TRUE(age_of_last_command_2 <= controller_->ref_timeout_); + ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT); + ASSERT_EQ( + controller_->update_reference_from_subscribers( + controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + ASSERT_EQ( + controller_->update_and_write_commands( + controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_EQ(joint_command_values_[STATE_MY_ITFS], TEST_DISPLACEMENT); + EXPECT_NE(joint_command_values_[STATE_MY_ITFS], 111); + ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT); +} TEST_F(AckermannSteeringControllerTest, test_ref_timeout_zero_for_update) { @@ -522,19 +427,20 @@ TEST_F(AckermannSteeringControllerTest, test_ref_timeout_zero_for_update) static constexpr double TEST_LINEAR_VELOCITY_X = 1.5; static constexpr double TEST_ANGULAR_VELOCITY_Z = 1.1; - controller_->ref_timeout_= rclcpp::Duration::from_seconds(0.0); + controller_->ref_timeout_ = rclcpp::Duration::from_seconds(0.0); std::shared_ptr msg = std::make_shared(); msg->header.stamp = controller_->get_node()->now() - rclcpp::Duration::from_seconds(0.0); - msg->twist.linear.x = TEST_LINEAR_VELOCITY_X; + msg->twist.linear.x = TEST_LINEAR_VELOCITY_X; msg->twist.linear.y = std::numeric_limits::quiet_NaN(); msg->twist.linear.z = std::numeric_limits::quiet_NaN(); msg->twist.angular.x = std::numeric_limits::quiet_NaN(); msg->twist.angular.y = std::numeric_limits::quiet_NaN(); msg->twist.angular.z = TEST_ANGULAR_VELOCITY_Z; controller_->input_ref_.writeFromNonRT(msg); - const auto age_of_last_command = controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; - + const auto age_of_last_command = + controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; + ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); ASSERT_EQ( controller_->update_reference_from_subscribers( @@ -545,7 +451,7 @@ TEST_F(AckermannSteeringControllerTest, test_ref_timeout_zero_for_update) controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - EXPECT_EQ(joint_command_values_[NR_STATE_ITFS-2], TEST_LINEAR_VELOCITY_X); + EXPECT_EQ(joint_command_values_[NR_STATE_ITFS - 2], TEST_LINEAR_VELOCITY_X); ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); ASSERT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x)); } @@ -560,8 +466,7 @@ TEST_F(AckermannSteeringControllerTest, test_ref_timeout_zero_for_reference_call EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x)); EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z)); - controller_->ref_timeout_= rclcpp::Duration::from_seconds(0.0); - //reference_callback() is called implicitly when publish_commands() is called. + controller_->ref_timeout_ = rclcpp::Duration::from_seconds(0.0); publish_commands(controller_->get_node()->now()); ASSERT_TRUE(controller_->wait_for_commands(executor)); @@ -571,7 +476,6 @@ TEST_F(AckermannSteeringControllerTest, test_ref_timeout_zero_for_reference_call EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z, 0.0); } - int main(int argc, char ** argv) { ::testing::InitGoogleTest(&argc, argv); diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp index 49c494fae5..1fe76f6d61 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp @@ -12,10 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TEMPLATES__ROS2_CONTROL__CONTROLLER__TEST_ACKERMANN_STEERING_CONTROLLER_HPP_ -#define TEMPLATES__ROS2_CONTROL__CONTROLLER__TEST_ACKERMANN_STEERING_CONTROLLER_HPP_ - -#include "gmock/gmock.h" +#ifndef TEST_ACKERMANN_STEERING_CONTROLLER_HPP_ +#define TEST_ACKERMANN_STEERING_CONTROLLER_HPP_ #include #include @@ -25,6 +23,8 @@ #include #include +#include "gmock/gmock.h" + #include "ackermann_steering_controller/ackermann_steering_controller.hpp" #include "hardware_interface/loaned_command_interface.hpp" #include "hardware_interface/loaned_state_interface.hpp" @@ -36,9 +36,12 @@ #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" // TODO(anyone): replace the state and command message types -using ControllerStateMsgOdom = ackermann_steering_controller::AckermannSteeringController::ControllerStateMsgOdom; -using ControllerStateMsgTf = ackermann_steering_controller::AckermannSteeringController::ControllerStateMsgTf; -using ControllerReferenceMsg = ackermann_steering_controller::AckermannSteeringController::ControllerReferenceMsg; +using ControllerStateMsgOdom = + ackermann_steering_controller::AckermannSteeringController::ControllerStateMsgOdom; +using ControllerStateMsgTf = + ackermann_steering_controller::AckermannSteeringController::ControllerStateMsgTf; +using ControllerReferenceMsg = + ackermann_steering_controller::AckermannSteeringController::ControllerReferenceMsg; namespace { @@ -48,7 +51,8 @@ constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR; // namespace // subclassing and friending so we can access member variables -class TestableAckermannSteeringController : public ackermann_steering_controller::AckermannSteeringController +class TestableAckermannSteeringController +: public ackermann_steering_controller::AckermannSteeringController { FRIEND_TEST(AckermannSteeringControllerTest, all_parameters_set_configure_success); FRIEND_TEST(AckermannSteeringControllerTest, check_exported_intefaces); @@ -56,14 +60,12 @@ class TestableAckermannSteeringController : public ackermann_steering_controller FRIEND_TEST(AckermannSteeringControllerTest, update_success); FRIEND_TEST(AckermannSteeringControllerTest, deactivate_success); FRIEND_TEST(AckermannSteeringControllerTest, reactivate_success); - // FRIEND_TEST(AckermannSteeringControllerTest, test_setting_slow_mode_service); - // FRIEND_TEST(AckermannSteeringControllerTest, test_update_logic_chainable_fast); - // FRIEND_TEST(AckermannSteeringControllerTest, test_update_logic_chainable_slow); FRIEND_TEST(AckermannSteeringControllerTest, publish_status_success); FRIEND_TEST(AckermannSteeringControllerTest, receive_message_and_publish_updated_status); - // FRIEND_TEST(AckermannSteeringControllerTest, test_message_timeout); + FRIEND_TEST(AckermannSteeringControllerTest, test_message_timeout); FRIEND_TEST(AckermannSteeringControllerTest, test_message_accepted); - // FRIEND_TEST(AckermannSteeringControllerTest, test_update_logic); + FRIEND_TEST(AckermannSteeringControllerTest, test_update_logic); + FRIEND_TEST(AckermannSteeringControllerTest, test_update_logic_chainable); FRIEND_TEST(AckermannSteeringControllerTest, test_ref_timeout_zero_for_update); FRIEND_TEST(AckermannSteeringControllerTest, test_ref_timeout_zero_for_reference_callback); @@ -71,9 +73,11 @@ class TestableAckermannSteeringController : public ackermann_steering_controller controller_interface::CallbackReturn on_configure( const rclcpp_lifecycle::State & previous_state) override { - auto ret = ackermann_steering_controller::AckermannSteeringController::on_configure(previous_state); + auto ret = + ackermann_steering_controller::AckermannSteeringController::on_configure(previous_state); // Only if on_configure is successful create subscription - if (ret == CallbackReturn::SUCCESS) { + if (ret == CallbackReturn::SUCCESS) + { ref_subscriber_wait_set_.add_subscription(ref_subscriber_); } return ret; @@ -98,7 +102,8 @@ class TestableAckermannSteeringController : public ackermann_steering_controller const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) { bool success = subscriber_wait_set.wait(timeout).kind() == rclcpp::WaitResultKind::Ready; - if (success) { + if (success) + { executor.spin_some(); } return success; @@ -132,7 +137,6 @@ class AckermannSteeringControllerFixture : public ::testing::Test command_publisher_node_ = std::make_shared("command_publisher"); command_publisher_ = command_publisher_node_->create_publisher( "/test_ackermann_steering_controller/reference", rclcpp::SystemDefaultsQoS()); - } static void TearDownTestCase() {} @@ -153,7 +157,7 @@ class AckermannSteeringControllerFixture : public ::testing::Test command_ifs.emplace_back(command_itfs_.back()); command_itfs_.emplace_back(hardware_interface::CommandInterface( - front_steer_name, hardware_interface::HW_IF_VELOCITY, &joint_command_values_[1])); + front_steer_name, hardware_interface::HW_IF_VELOCITY, &joint_command_values_[1])); command_ifs.emplace_back(command_itfs_.back()); // TODO(anyone): Add other command interfaces, if any @@ -183,12 +187,12 @@ class AckermannSteeringControllerFixture : public ::testing::Test // call update to publish the test value ASSERT_EQ( - controller_->update_reference_from_subscribers( - controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); + controller_->update_reference_from_subscribers( + controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); ASSERT_EQ( - controller_->update_and_write_commands( - controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_->update_and_write_commands( + controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); // call update to publish the test value @@ -196,13 +200,15 @@ class AckermannSteeringControllerFixture : public ::testing::Test 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_odom); - while (max_sub_check_loop_count--) { + while (max_sub_check_loop_count--) + { controller_->update_reference_from_subscribers( controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)); controller_->update_and_write_commands( controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)); // check if message has been received - if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) { + if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) + { break; } } @@ -220,10 +226,13 @@ class AckermannSteeringControllerFixture : public ::testing::Test const rclcpp::Time & stamp, const double & twist_linear_x = 0.45, const double & twist_angular_z = 0.0) { - auto wait_for_topic = [&](const auto topic_name) { + 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) { + 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); @@ -238,7 +247,7 @@ class AckermannSteeringControllerFixture : public ::testing::Test ControllerReferenceMsg msg; msg.header.stamp = stamp; - msg.twist.linear.x = twist_linear_x; + msg.twist.linear.x = twist_linear_x; msg.twist.linear.y = std::numeric_limits::quiet_NaN(); msg.twist.linear.z = std::numeric_limits::quiet_NaN(); msg.twist.angular.x = std::numeric_limits::quiet_NaN(); @@ -248,7 +257,6 @@ class AckermannSteeringControllerFixture : public ::testing::Test command_publisher_->publish(msg); } - protected: // TODO(anyone): adjust the members as needed std::string rear_wheel_name = "rear_wheel_joint"; @@ -267,7 +275,6 @@ class AckermannSteeringControllerFixture : public ::testing::Test std::unique_ptr controller_; rclcpp::Node::SharedPtr command_publisher_node_; rclcpp::Publisher::SharedPtr command_publisher_; - }; -#endif // TEMPLATES__ROS2_CONTROL__CONTROLLER__TEST_ACKERMANN_STEERING_CONTROLLER_HPP_ +#endif // TEST_ACKERMANN_STEERING_CONTROLLER_HPP_ From afe50063255d79b80a23b31d2649d725eb8e79ac Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Mon, 14 Nov 2022 19:02:33 +0100 Subject: [PATCH 010/186] Delete services and comment out to compile. --- .../test_ackermann_steering_controller.cpp | 260 +++++++++--------- 1 file changed, 127 insertions(+), 133 deletions(-) diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp index d1de3e700e..aed1fb408d 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp @@ -230,21 +230,21 @@ TEST_F(AckermannSteeringControllerTest, test_message_timeout) // try to set command with time before timeout - command is not updated auto reference = controller_->input_ref_.readFromNonRT(); auto old_timestamp = (*reference)->header.stamp; - EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->joint_names.size(), joint_names_.size()); - EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->joint_names[0], joint_names_[0]); - EXPECT_TRUE(std::isnan((*reference)->displacements[0])); - EXPECT_TRUE(std::isnan((*reference)->velocities[0])); - EXPECT_TRUE(std::isnan((*reference)->duration)); - publish_commands( - controller_->get_node()->now() - controller_->ref_timeout_ - - rclcpp::Duration::from_seconds(0.1)); - ASSERT_TRUE(controller_->wait_for_commands(executor)); - ASSERT_EQ(old_timestamp, (*(controller_->input_ref_.readFromNonRT()))->header.stamp); - EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->joint_names.size(), joint_names_.size()); - EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->joint_names[0], joint_names_[0]); - EXPECT_TRUE(std::isnan((*reference)->displacements[0])); - EXPECT_TRUE(std::isnan((*reference)->velocities[0])); - EXPECT_TRUE(std::isnan((*reference)->duration)); + // EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->joint_names.size(), joint_names_.size()); + // EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->joint_names[0], joint_names_[0]); + // EXPECT_TRUE(std::isnan((*reference)->displacements[0])); + // EXPECT_TRUE(std::isnan((*reference)->velocities[0])); + // EXPECT_TRUE(std::isnan((*reference)->duration)); + // publish_commands( + // controller_->get_node()->now() - controller_->ref_timeout_ - + // rclcpp::Duration::from_seconds(0.1)); + // ASSERT_TRUE(controller_->wait_for_commands(executor)); + // ASSERT_EQ(old_timestamp, (*(controller_->input_ref_.readFromNonRT()))->header.stamp); + // EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->joint_names.size(), joint_names_.size()); + // EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->joint_names[0], joint_names_[0]); + // EXPECT_TRUE(std::isnan((*reference)->displacements[0])); + // EXPECT_TRUE(std::isnan((*reference)->velocities[0])); + // EXPECT_TRUE(std::isnan((*reference)->duration)); } TEST_F(AckermannSteeringControllerTest, test_message_accepted) @@ -274,77 +274,74 @@ TEST_F(AckermannSteeringControllerTest, test_update_logic_chainable) 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; - joint_command_values_[STATE_MY_ITFS] = 111; - std::shared_ptr msg = std::make_shared(); - msg->header.stamp = controller_->get_node()->now() - controller_->ref_timeout_ - - rclcpp::Duration::from_seconds(0.1); - 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); - const auto age_of_last_command = - controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; - - // age_of_last_command > ref_timeout_ - ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_); - ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT); - ASSERT_EQ( - controller_->update_reference_from_subscribers( - controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - ASSERT_EQ( - controller_->update_and_write_commands( - controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - - EXPECT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::SLOW); - EXPECT_EQ(joint_command_values_[STATE_MY_ITFS], 111); - ASSERT_TRUE(std::isnan(controller_->reference_interfaces_[0])); - for (const auto & interface : controller_->reference_interfaces_) - { - EXPECT_TRUE(std::isnan(interface)); - } - - std::shared_ptr msg_2 = std::make_shared(); - msg_2->header.stamp = controller_->get_node()->now() - rclcpp::Duration::from_seconds(0.01); - msg_2->joint_names = joint_names_; - msg_2->displacements.resize(joint_names_.size(), TEST_DISPLACEMENT); - msg_2->velocities.resize(joint_names_.size(), std::numeric_limits::quiet_NaN()); - msg_2->duration = std::numeric_limits::quiet_NaN(); - controller_->input_ref_.writeFromNonRT(msg_2); - const auto age_of_last_command_2 = - controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; - - // age_of_last_command_2 < ref_timeout_ - ASSERT_TRUE(age_of_last_command_2 <= controller_->ref_timeout_); - ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT); - ASSERT_EQ( - controller_->update_reference_from_subscribers( - controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - ASSERT_EQ( - controller_->update_and_write_commands( - controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - - EXPECT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::SLOW); - EXPECT_EQ(joint_command_values_[STATE_MY_ITFS], TEST_DISPLACEMENT / 4); - EXPECT_NE(joint_command_values_[STATE_MY_ITFS], 111); - ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT / 2); - for (const auto & interface : controller_->reference_interfaces_) - { - EXPECT_FALSE(std::isnan(interface)); - } + // // set command statically + // static constexpr double TEST_DISPLACEMENT = 23.24; + // joint_command_values_[STATE_MY_ITFS] = 111; + // std::shared_ptr msg = std::make_shared(); + // msg->header.stamp = controller_->get_node()->now() - controller_->ref_timeout_ - + // rclcpp::Duration::from_seconds(0.1); + // 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); + // const auto age_of_last_command = + // controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; + // + // // age_of_last_command > ref_timeout_ + // ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_); + // ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT); + // ASSERT_EQ( + // controller_->update_reference_from_subscribers(), + // controller_interface::return_type::OK); + // ASSERT_EQ( + // controller_->update_and_write_commands( + // controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + // controller_interface::return_type::OK); + // + // EXPECT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::SLOW); + // EXPECT_EQ(joint_command_values_[STATE_MY_ITFS], 111); + // ASSERT_TRUE(std::isnan(controller_->reference_interfaces_[0])); + // for (const auto & interface : controller_->reference_interfaces_) + // { + // EXPECT_TRUE(std::isnan(interface)); + // } + // + // std::shared_ptr msg_2 = std::make_shared(); + // msg_2->header.stamp = controller_->get_node()->now() - rclcpp::Duration::from_seconds(0.01); + // msg_2->joint_names = joint_names_; + // msg_2->displacements.resize(joint_names_.size(), TEST_DISPLACEMENT); + // msg_2->velocities.resize(joint_names_.size(), std::numeric_limits::quiet_NaN()); + // msg_2->duration = std::numeric_limits::quiet_NaN(); + // controller_->input_ref_.writeFromNonRT(msg_2); + // const auto age_of_last_command_2 = + // controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; + // + // // age_of_last_command_2 < ref_timeout_ + // ASSERT_TRUE(age_of_last_command_2 <= controller_->ref_timeout_); + // ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT); + // ASSERT_EQ( + // controller_->update_reference_from_subscribers(), + // controller_interface::return_type::OK); + // ASSERT_EQ( + // controller_->update_and_write_commands( + // controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + // controller_interface::return_type::OK); + // + // EXPECT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::SLOW); + // EXPECT_EQ(joint_command_values_[STATE_MY_ITFS], TEST_DISPLACEMENT / 4); + // EXPECT_NE(joint_command_values_[STATE_MY_ITFS], 111); + // ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT / 2); + // for (const auto & interface : controller_->reference_interfaces_) + // { + // EXPECT_FALSE(std::isnan(interface)); + // } } TEST_F(AckermannSteeringControllerTest, test_update_logic) @@ -352,65 +349,62 @@ TEST_F(AckermannSteeringControllerTest, test_update_logic) SetUpController(); rclcpp::executors::MultiThreadedExecutor executor; executor.add_node(controller_->get_node()->get_node_base_interface()); - executor.add_node(service_caller_node_->get_node_base_interface()); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); auto reference = controller_->input_ref_.readFromNonRT(); - // set command statically - static constexpr double TEST_DISPLACEMENT = 23.24; - joint_command_values_[STATE_MY_ITFS] = 111; - std::shared_ptr msg = std::make_shared(); - msg->header.stamp = controller_->get_node()->now() - controller_->ref_timeout_ - - rclcpp::Duration::from_seconds(0.1); - 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); - const auto age_of_last_command = - controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; - - ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_); - ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT); - ASSERT_EQ( - controller_->update_reference_from_subscribers( - controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - ASSERT_EQ( - controller_->update_and_write_commands( - controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - - EXPECT_EQ(joint_command_values_[STATE_MY_ITFS], 111); - ASSERT_TRUE(std::isnan(controller_->reference_interfaces_[0])); - - std::shared_ptr msg_2 = std::make_shared(); - msg_2->header.stamp = controller_->get_node()->now(); - msg_2->joint_names = joint_names_; - msg_2->displacements.resize(joint_names_.size(), TEST_DISPLACEMENT); - msg_2->velocities.resize(joint_names_.size(), std::numeric_limits::quiet_NaN()); - msg_2->duration = std::numeric_limits::quiet_NaN(); - controller_->input_ref_.writeFromNonRT(msg_2); - const auto age_of_last_command_2 = - controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; - - ASSERT_TRUE(age_of_last_command_2 <= controller_->ref_timeout_); - ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT); - ASSERT_EQ( - controller_->update_reference_from_subscribers( - controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - ASSERT_EQ( - controller_->update_and_write_commands( - controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - - EXPECT_EQ(joint_command_values_[STATE_MY_ITFS], TEST_DISPLACEMENT); - EXPECT_NE(joint_command_values_[STATE_MY_ITFS], 111); - ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT); + // // set command statically + // static constexpr double TEST_DISPLACEMENT = 23.24; + // joint_command_values_[STATE_MY_ITFS] = 111; + // std::shared_ptr msg = std::make_shared(); + // msg->header.stamp = controller_->get_node()->now() - controller_->ref_timeout_ - + // rclcpp::Duration::from_seconds(0.1); + // 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); + // const auto age_of_last_command = + // controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; + // + // ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_); + // ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT); + // ASSERT_EQ( + // controller_->update_reference_from_subscribers(), + // controller_interface::return_type::OK); + // ASSERT_EQ( + // controller_->update_and_write_commands( + // controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + // controller_interface::return_type::OK); + // + // EXPECT_EQ(joint_command_values_[STATE_MY_ITFS], 111); + // ASSERT_TRUE(std::isnan(controller_->reference_interfaces_[0])); + // + // std::shared_ptr msg_2 = std::make_shared(); + // msg_2->header.stamp = controller_->get_node()->now(); + // msg_2->joint_names = joint_names_; + // msg_2->displacements.resize(joint_names_.size(), TEST_DISPLACEMENT); + // msg_2->velocities.resize(joint_names_.size(), std::numeric_limits::quiet_NaN()); + // msg_2->duration = std::numeric_limits::quiet_NaN(); + // controller_->input_ref_.writeFromNonRT(msg_2); + // const auto age_of_last_command_2 = + // controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; + // + // ASSERT_TRUE(age_of_last_command_2 <= controller_->ref_timeout_); + // ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT); + // ASSERT_EQ( + // controller_->update_reference_from_subscribers(), + // controller_interface::return_type::OK); + // ASSERT_EQ( + // controller_->update_and_write_commands( + // controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + // controller_interface::return_type::OK); + // + // EXPECT_EQ(joint_command_values_[STATE_MY_ITFS], TEST_DISPLACEMENT); + // EXPECT_NE(joint_command_values_[STATE_MY_ITFS], 111); + // ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT); } TEST_F(AckermannSteeringControllerTest, test_ref_timeout_zero_for_update) From 222a40d3e8123e6c1f597f26421245c52058a674 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Mon, 14 Nov 2022 19:03:15 +0100 Subject: [PATCH 011/186] Make compilable on humble. --- .../ackermann_steering_controller.hpp | 1 + .../src/ackermann_steering_controller.cpp | 5 ++--- .../test/test_ackermann_steering_controller.cpp | 16 ++++------------ .../test/test_ackermann_steering_controller.hpp | 7 ++----- 4 files changed, 9 insertions(+), 20 deletions(-) diff --git a/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp b/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp index 0feafdc612..9d7a1ec8a0 100644 --- a/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp +++ b/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp @@ -79,6 +79,7 @@ class AckermannSteeringController : public controller_interface::ChainableContro const rclcpp_lifecycle::State & previous_state) override; ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::return_type update_reference_from_subscribers() override; ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC controller_interface::return_type update_and_write_commands( diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.cpp b/ackermann_steering_controller/src/ackermann_steering_controller.cpp index 02f36ecb16..66e5946e14 100644 --- a/ackermann_steering_controller/src/ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/src/ackermann_steering_controller.cpp @@ -278,11 +278,10 @@ controller_interface::CallbackReturn AckermannSteeringController::on_deactivate( return controller_interface::CallbackReturn::SUCCESS; } -controller_interface::return_type AckermannSteeringController::update_reference_from_subscribers( - const rclcpp::Time & time, const rclcpp::Duration & /*period*/) +controller_interface::return_type AckermannSteeringController::update_reference_from_subscribers() { auto current_ref = *(input_ref_.readFromRT()); - const auto age_of_last_command = time - (current_ref)->header.stamp; + const auto age_of_last_command = get_node()->now() - (current_ref)->header.stamp; // TODO(anyone): depending on number of interfaces, use definitions, e.g., `CMD_MY_ITFS`, // instead of a loop diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp index aed1fb408d..65120b27e2 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp @@ -150,9 +150,7 @@ TEST_F(AckermannSteeringControllerTest, reactivate_success) ASSERT_TRUE(std::isnan(controller_->command_interfaces_[NR_CMD_ITFS - 2].get_value())); ASSERT_EQ( - controller_->update_reference_from_subscribers( - controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); + controller_->update_reference_from_subscribers(), controller_interface::return_type::OK); ASSERT_EQ( controller_->update_and_write_commands( controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), @@ -188,9 +186,7 @@ TEST_F(AckermannSteeringControllerTest, receive_message_and_publish_updated_stat ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ( - controller_->update_reference_from_subscribers( - controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); + controller_->update_reference_from_subscribers(), controller_interface::return_type::OK); ASSERT_EQ( controller_->update_and_write_commands( controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), @@ -204,9 +200,7 @@ TEST_F(AckermannSteeringControllerTest, receive_message_and_publish_updated_stat ASSERT_TRUE(controller_->wait_for_commands(executor)); ASSERT_EQ( - controller_->update_reference_from_subscribers( - controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); + controller_->update_reference_from_subscribers(), controller_interface::return_type::OK); ASSERT_EQ( controller_->update_and_write_commands( controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), @@ -437,9 +431,7 @@ TEST_F(AckermannSteeringControllerTest, test_ref_timeout_zero_for_update) ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); ASSERT_EQ( - controller_->update_reference_from_subscribers( - controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); + controller_->update_reference_from_subscribers(), controller_interface::return_type::OK); ASSERT_EQ( controller_->update_and_write_commands( controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp index 1fe76f6d61..45682cbb69 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp @@ -187,9 +187,7 @@ class AckermannSteeringControllerFixture : public ::testing::Test // call update to publish the test value ASSERT_EQ( - controller_->update_reference_from_subscribers( - controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); + controller_->update_reference_from_subscribers(), controller_interface::return_type::OK); ASSERT_EQ( controller_->update_and_write_commands( controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), @@ -202,8 +200,7 @@ class AckermannSteeringControllerFixture : public ::testing::Test wait_set.add_subscription(subscription_odom); while (max_sub_check_loop_count--) { - controller_->update_reference_from_subscribers( - controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)); + controller_->update_reference_from_subscribers(); controller_->update_and_write_commands( controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)); // check if message has been received From 72c3ca19f70a104e93888fa4f51816053559b1ea Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Tue, 15 Nov 2022 08:45:56 +0100 Subject: [PATCH 012/186] Correct interfaces in ackerman controllers. --- .../src/ackermann_steering_controller.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.cpp b/ackermann_steering_controller/src/ackermann_steering_controller.cpp index 66e5946e14..c40bff3f3e 100644 --- a/ackermann_steering_controller/src/ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/src/ackermann_steering_controller.cpp @@ -212,7 +212,7 @@ AckermannSteeringController::command_interface_configuration() const command_interfaces_config.names.push_back( params_.rear_wheel_name + "/" + hardware_interface::HW_IF_VELOCITY); command_interfaces_config.names.push_back( - params_.front_steer_name + "/" + hardware_interface::HW_IF_VELOCITY); + params_.front_steer_name + "/" + hardware_interface::HW_IF_POSITION); return command_interfaces_config; } @@ -245,7 +245,7 @@ AckermannSteeringController::on_export_reference_interfaces() &reference_interfaces_[0])); reference_interfaces.push_back(hardware_interface::CommandInterface( - get_node()->get_name(), std::string("angular/") + hardware_interface::HW_IF_VELOCITY, + get_node()->get_name(), std::string("angular/") + hardware_interface::HW_IF_POSITION, &reference_interfaces_[1])); return reference_interfaces; From d81513c550b53ebf46b903dfcf829d6c69baefef Mon Sep 17 00:00:00 2001 From: GiridharBukka Date: Tue, 15 Nov 2022 13:35:00 +0100 Subject: [PATCH 013/186] all tests pass --- .../src/ackermann_steering_controller.cpp | 42 ++- .../ackermann_steering_controller_params.yaml | 2 +- .../test_ackermann_steering_controller.cpp | 293 ++++++++++-------- 3 files changed, 185 insertions(+), 152 deletions(-) diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.cpp b/ackermann_steering_controller/src/ackermann_steering_controller.cpp index c40bff3f3e..c015cf56f6 100644 --- a/ackermann_steering_controller/src/ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/src/ackermann_steering_controller.cpp @@ -168,8 +168,8 @@ controller_interface::CallbackReturn AckermannSteeringController::on_configure( // calculation publication period of odometry and tf odometry messages publish_period_ = rclcpp::Duration::from_seconds(1.0 / params_.publish_rate); - fprintf(stderr, "publish_period_ = %d \n", publish_period_); - + // fprintf(stderr, "publish_period_, publish_rate = %f,%f \n", + // publish_period_.seconds(), params_.publish_rate); RCLCPP_INFO(get_node()->get_logger(), "configure successful"); return controller_interface::CallbackReturn::SUCCESS; } @@ -348,20 +348,32 @@ controller_interface::return_type AckermannSteeringController::update_and_write_ reference_interfaces_[0] = std::numeric_limits::quiet_NaN(); reference_interfaces_[1] = std::numeric_limits::quiet_NaN(); } - fprintf( - stderr, "previous_publish_timestamp_ + publish_period_ < time = %d, %d, %d, %d\n", time, - previous_publish_timestamp_, publish_period_, - (previous_publish_timestamp_ + publish_period_) < time); - // fprintf(stderr, "Exception thrown during controller's init with message: %s \n", e.what()); - + // fprintf(stderr, "Exception thrown during controller's init with message: %s + // \n", e.what()); + // fprintf(stderr, "outside \n"); + // fprintf( + // stderr, + // "all in sec time, previous_publish_timestamp_, publish_period_, " + // "previous_publish_timestamp_ + publish_period_) < time = %f, %f, %f, " + // "%d\n", + // time.seconds(), previous_publish_timestamp_.seconds(), + // publish_period_.seconds(), + // (previous_publish_timestamp_.seconds() + publish_period_.seconds()) < + // time.seconds()); // Publish odometry message - if (previous_publish_timestamp_ + publish_period_ < time) - { - fprintf(stderr, "debuggin2 \n"); - fprintf( - stderr, "previous_publish_timestamp_ + publish_period_ < time = %d, %d, %d, %d\n", time, - previous_publish_timestamp_, publish_period_, - (previous_publish_timestamp_ + publish_period_) < time); + if (previous_publish_timestamp_.seconds() + publish_period_.seconds() < + time.seconds()) { + // fprintf(stderr, "inside \n"); + // fprintf( + // stderr, + // "all in sec time, previous_publish_timestamp_, publish_period_, " + // "previous_publish_timestamp_ + publish_period_) < time = %f, %f, %f, + // " + // "%d\n", + // time.seconds(), previous_publish_timestamp_.seconds(), + // publish_period_.seconds(), + // (previous_publish_timestamp_.seconds() + publish_period_.seconds()) < + // time.seconds()); previous_publish_timestamp_ += publish_period_; // Compute and store orientation info diff --git a/ackermann_steering_controller/test/ackermann_steering_controller_params.yaml b/ackermann_steering_controller/test/ackermann_steering_controller_params.yaml index 4a71d933ee..bec88133ac 100644 --- a/ackermann_steering_controller/test/ackermann_steering_controller_params.yaml +++ b/ackermann_steering_controller/test/ackermann_steering_controller_params.yaml @@ -1,7 +1,7 @@ test_ackermann_steering_controller: ros__parameters: - reference_timeout: 0.1 + reference_timeout: 0.2 rear_wheel_name: "rear_wheel_joint" diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp index 65120b27e2..5feb3971dc 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp @@ -45,7 +45,7 @@ TEST_F(AckermannSteeringControllerTest, all_parameters_set_configure_success) // ASSERT_TRUE(controller_->params_.state_joints.empty()); // ASSERT_THAT(controller_->state_joints_, testing::ElementsAreArray(joint_names_)); // ASSERT_EQ(controller_->params_.interface_name, interface_name_); - ASSERT_EQ(controller_->params_.reference_timeout, 0.1); + ASSERT_EQ(controller_->params_.reference_timeout, 0.2); } TEST_F(AckermannSteeringControllerTest, check_exported_intefaces) @@ -184,9 +184,8 @@ TEST_F(AckermannSteeringControllerTest, receive_message_and_publish_updated_stat ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - - ASSERT_EQ( - controller_->update_reference_from_subscribers(), controller_interface::return_type::OK); + controller_->reference_interfaces_[0] = 2.3; + controller_->reference_interfaces_[1] = 4.4; ASSERT_EQ( controller_->update_and_write_commands( controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), @@ -224,21 +223,15 @@ TEST_F(AckermannSteeringControllerTest, test_message_timeout) // try to set command with time before timeout - command is not updated auto reference = controller_->input_ref_.readFromNonRT(); auto old_timestamp = (*reference)->header.stamp; - // EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->joint_names.size(), joint_names_.size()); - // EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->joint_names[0], joint_names_[0]); - // EXPECT_TRUE(std::isnan((*reference)->displacements[0])); - // EXPECT_TRUE(std::isnan((*reference)->velocities[0])); - // EXPECT_TRUE(std::isnan((*reference)->duration)); - // publish_commands( - // controller_->get_node()->now() - controller_->ref_timeout_ - - // rclcpp::Duration::from_seconds(0.1)); - // ASSERT_TRUE(controller_->wait_for_commands(executor)); - // ASSERT_EQ(old_timestamp, (*(controller_->input_ref_.readFromNonRT()))->header.stamp); - // EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->joint_names.size(), joint_names_.size()); - // EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->joint_names[0], joint_names_[0]); - // EXPECT_TRUE(std::isnan((*reference)->displacements[0])); - // EXPECT_TRUE(std::isnan((*reference)->velocities[0])); - // EXPECT_TRUE(std::isnan((*reference)->duration)); + EXPECT_TRUE(std::isnan((*reference)->twist.linear.x)); + EXPECT_TRUE(std::isnan((*reference)->twist.angular.z)); + publish_commands(controller_->get_node()->now() - controller_->ref_timeout_ - + rclcpp::Duration::from_seconds(0.1)); + ASSERT_TRUE(controller_->wait_for_commands(executor)); + ASSERT_EQ(old_timestamp, + (*(controller_->input_ref_.readFromNonRT()))->header.stamp); + EXPECT_TRUE(std::isnan((*reference)->twist.linear.x)); + EXPECT_TRUE(std::isnan((*reference)->twist.angular.z)); } TEST_F(AckermannSteeringControllerTest, test_message_accepted) @@ -262,9 +255,8 @@ TEST_F(AckermannSteeringControllerTest, test_message_accepted) EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z, 0.0); } -TEST_F(AckermannSteeringControllerTest, test_update_logic_chainable) -{ - // 1. ageref_timeout +TEST_F(AckermannSteeringControllerTest, test_update_logic_chainable) { + // 1. age>ref_timeout 2. ageget_node()->get_node_base_interface()); @@ -274,68 +266,80 @@ TEST_F(AckermannSteeringControllerTest, test_update_logic_chainable) 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; - // joint_command_values_[STATE_MY_ITFS] = 111; - // std::shared_ptr msg = std::make_shared(); - // msg->header.stamp = controller_->get_node()->now() - controller_->ref_timeout_ - - // rclcpp::Duration::from_seconds(0.1); - // 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); - // const auto age_of_last_command = - // controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; - // - // // age_of_last_command > ref_timeout_ - // ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_); - // ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT); - // ASSERT_EQ( - // controller_->update_reference_from_subscribers(), - // controller_interface::return_type::OK); - // ASSERT_EQ( - // controller_->update_and_write_commands( - // controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), - // controller_interface::return_type::OK); - // - // EXPECT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::SLOW); - // EXPECT_EQ(joint_command_values_[STATE_MY_ITFS], 111); - // ASSERT_TRUE(std::isnan(controller_->reference_interfaces_[0])); - // for (const auto & interface : controller_->reference_interfaces_) - // { - // EXPECT_TRUE(std::isnan(interface)); - // } - // - // std::shared_ptr msg_2 = std::make_shared(); - // msg_2->header.stamp = controller_->get_node()->now() - rclcpp::Duration::from_seconds(0.01); - // msg_2->joint_names = joint_names_; - // msg_2->displacements.resize(joint_names_.size(), TEST_DISPLACEMENT); - // msg_2->velocities.resize(joint_names_.size(), std::numeric_limits::quiet_NaN()); - // msg_2->duration = std::numeric_limits::quiet_NaN(); - // controller_->input_ref_.writeFromNonRT(msg_2); - // const auto age_of_last_command_2 = - // controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; - // - // // age_of_last_command_2 < ref_timeout_ - // ASSERT_TRUE(age_of_last_command_2 <= controller_->ref_timeout_); - // ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT); - // ASSERT_EQ( - // controller_->update_reference_from_subscribers(), - // controller_interface::return_type::OK); - // ASSERT_EQ( - // controller_->update_and_write_commands( - // controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), - // controller_interface::return_type::OK); - // - // EXPECT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::SLOW); - // EXPECT_EQ(joint_command_values_[STATE_MY_ITFS], TEST_DISPLACEMENT / 4); - // EXPECT_NE(joint_command_values_[STATE_MY_ITFS], 111); - // ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT / 2); - // for (const auto & interface : controller_->reference_interfaces_) - // { - // EXPECT_FALSE(std::isnan(interface)); - // } + // set command statically + static constexpr double TEST_LINEAR_VELOCITY_X = 1.5; + static constexpr double TEST_ANGULAR_VELOCITY_Z = 1.1; + joint_command_values_[NR_CMD_ITFS - 2] = 111; + std::shared_ptr msg = + std::make_shared(); + + msg->header.stamp = controller_->get_node()->now() - + controller_->ref_timeout_ - + rclcpp::Duration::from_seconds(0.1); + msg->twist.linear.x = TEST_LINEAR_VELOCITY_X; + msg->twist.linear.y = std::numeric_limits::quiet_NaN(); + msg->twist.linear.z = std::numeric_limits::quiet_NaN(); + msg->twist.angular.x = std::numeric_limits::quiet_NaN(); + msg->twist.angular.y = std::numeric_limits::quiet_NaN(); + msg->twist.angular.z = TEST_ANGULAR_VELOCITY_Z; + controller_->input_ref_.writeFromNonRT(msg); + const auto age_of_last_command = + controller_->get_node()->now() - + (*(controller_->input_ref_.readFromNonRT()))->header.stamp; + + // age_of_last_command > ref_timeout_ + ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_); + ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, + TEST_LINEAR_VELOCITY_X); + ASSERT_EQ( + controller_->update_reference_from_subscribers( + controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + ASSERT_EQ( + controller_->update_and_write_commands( + controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_EQ(joint_command_values_[NR_CMD_ITFS - 2], 111); + ASSERT_TRUE(std::isnan(controller_->reference_interfaces_[0])); + for (const auto& interface : controller_->reference_interfaces_) { + EXPECT_TRUE(std::isnan(interface)); + } + + std::shared_ptr msg_2 = + std::make_shared(); + msg_2->header.stamp = + controller_->get_node()->now() - rclcpp::Duration::from_seconds(0.01); + msg_2->twist.linear.x = TEST_LINEAR_VELOCITY_X; + msg_2->twist.linear.y = std::numeric_limits::quiet_NaN(); + msg_2->twist.linear.z = std::numeric_limits::quiet_NaN(); + msg_2->twist.angular.x = std::numeric_limits::quiet_NaN(); + msg_2->twist.angular.y = std::numeric_limits::quiet_NaN(); + msg_2->twist.angular.z = TEST_ANGULAR_VELOCITY_Z; + controller_->input_ref_.writeFromNonRT(msg_2); + const auto age_of_last_command_2 = + controller_->get_node()->now() - + (*(controller_->input_ref_.readFromNonRT()))->header.stamp; + + // age_of_last_command_2 < ref_timeout_ + ASSERT_TRUE(age_of_last_command_2 <= controller_->ref_timeout_); + ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, + TEST_LINEAR_VELOCITY_X); + ASSERT_EQ( + controller_->update_reference_from_subscribers( + controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + ASSERT_EQ( + controller_->update_and_write_commands( + controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_NE(joint_command_values_[NR_CMD_ITFS - 2], 111); + ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, + TEST_LINEAR_VELOCITY_X); + for (const auto& interface : controller_->reference_interfaces_) { + EXPECT_FALSE(std::isnan(interface)); + } } TEST_F(AckermannSteeringControllerTest, test_update_logic) @@ -349,56 +353,71 @@ TEST_F(AckermannSteeringControllerTest, test_update_logic) auto reference = controller_->input_ref_.readFromNonRT(); - // // set command statically - // static constexpr double TEST_DISPLACEMENT = 23.24; - // joint_command_values_[STATE_MY_ITFS] = 111; - // std::shared_ptr msg = std::make_shared(); - // msg->header.stamp = controller_->get_node()->now() - controller_->ref_timeout_ - - // rclcpp::Duration::from_seconds(0.1); - // 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); - // const auto age_of_last_command = - // controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; - // - // ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_); - // ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT); - // ASSERT_EQ( - // controller_->update_reference_from_subscribers(), - // controller_interface::return_type::OK); - // ASSERT_EQ( - // controller_->update_and_write_commands( - // controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), - // controller_interface::return_type::OK); - // - // EXPECT_EQ(joint_command_values_[STATE_MY_ITFS], 111); - // ASSERT_TRUE(std::isnan(controller_->reference_interfaces_[0])); - // - // std::shared_ptr msg_2 = std::make_shared(); - // msg_2->header.stamp = controller_->get_node()->now(); - // msg_2->joint_names = joint_names_; - // msg_2->displacements.resize(joint_names_.size(), TEST_DISPLACEMENT); - // msg_2->velocities.resize(joint_names_.size(), std::numeric_limits::quiet_NaN()); - // msg_2->duration = std::numeric_limits::quiet_NaN(); - // controller_->input_ref_.writeFromNonRT(msg_2); - // const auto age_of_last_command_2 = - // controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; - // - // ASSERT_TRUE(age_of_last_command_2 <= controller_->ref_timeout_); - // ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT); - // ASSERT_EQ( - // controller_->update_reference_from_subscribers(), - // controller_interface::return_type::OK); - // ASSERT_EQ( - // controller_->update_and_write_commands( - // controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), - // controller_interface::return_type::OK); - // - // EXPECT_EQ(joint_command_values_[STATE_MY_ITFS], TEST_DISPLACEMENT); - // EXPECT_NE(joint_command_values_[STATE_MY_ITFS], 111); - // ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT); + // set command statically + static constexpr double TEST_LINEAR_VELOCITY_X = 1.5; + static constexpr double TEST_ANGULAR_VELOCITY_Z = 1.1; + joint_command_values_[NR_CMD_ITFS - 2] = 111; + std::shared_ptr msg = + std::make_shared(); + msg->header.stamp = controller_->get_node()->now() - + controller_->ref_timeout_ - + rclcpp::Duration::from_seconds(0.1); + msg->twist.linear.x = TEST_LINEAR_VELOCITY_X; + msg->twist.linear.y = std::numeric_limits::quiet_NaN(); + msg->twist.linear.z = std::numeric_limits::quiet_NaN(); + msg->twist.angular.x = std::numeric_limits::quiet_NaN(); + msg->twist.angular.y = std::numeric_limits::quiet_NaN(); + msg->twist.angular.z = TEST_ANGULAR_VELOCITY_Z; + controller_->input_ref_.writeFromNonRT(msg); + const auto age_of_last_command = + controller_->get_node()->now() - + (*(controller_->input_ref_.readFromNonRT()))->header.stamp; + + ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_); + ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, + TEST_LINEAR_VELOCITY_X); + ASSERT_EQ( + controller_->update_reference_from_subscribers( + controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + ASSERT_EQ( + controller_->update_and_write_commands( + controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_EQ(joint_command_values_[NR_CMD_ITFS - 2], 111); + ASSERT_TRUE(std::isnan(controller_->reference_interfaces_[0])); + + std::shared_ptr msg_2 = + std::make_shared(); + msg_2->header.stamp = controller_->get_node()->now(); + msg_2->twist.linear.x = TEST_LINEAR_VELOCITY_X; + msg_2->twist.linear.y = std::numeric_limits::quiet_NaN(); + msg_2->twist.linear.z = std::numeric_limits::quiet_NaN(); + msg_2->twist.angular.x = std::numeric_limits::quiet_NaN(); + msg_2->twist.angular.y = std::numeric_limits::quiet_NaN(); + msg_2->twist.angular.z = TEST_ANGULAR_VELOCITY_Z; + controller_->input_ref_.writeFromNonRT(msg_2); + const auto age_of_last_command_2 = + controller_->get_node()->now() - + (*(controller_->input_ref_.readFromNonRT()))->header.stamp; + + ASSERT_TRUE(age_of_last_command_2 <= controller_->ref_timeout_); + ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, + TEST_LINEAR_VELOCITY_X); + ASSERT_EQ( + controller_->update_reference_from_subscribers( + controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + ASSERT_EQ( + controller_->update_and_write_commands( + controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_EQ(joint_command_values_[NR_CMD_ITFS - 2], TEST_LINEAR_VELOCITY_X); + EXPECT_NE(joint_command_values_[NR_CMD_ITFS - 2], 111); + ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, + TEST_LINEAR_VELOCITY_X); } TEST_F(AckermannSteeringControllerTest, test_ref_timeout_zero_for_update) @@ -438,8 +457,10 @@ TEST_F(AckermannSteeringControllerTest, test_ref_timeout_zero_for_update) controller_interface::return_type::OK); EXPECT_EQ(joint_command_values_[NR_STATE_ITFS - 2], TEST_LINEAR_VELOCITY_X); - ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); - ASSERT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x)); + ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, + TEST_LINEAR_VELOCITY_X); + ASSERT_FALSE( + std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x)); } TEST_F(AckermannSteeringControllerTest, test_ref_timeout_zero_for_reference_callback) From 82e7cf239e23e9d9df9f4ee831eebeb1a50c85df Mon Sep 17 00:00:00 2001 From: GiridharBukka Date: Tue, 15 Nov 2022 17:47:29 +0100 Subject: [PATCH 014/186] added test_time_stamp_zero test --- .../test_ackermann_steering_controller.cpp | 29 +++ .../test_ackermann_steering_controller.hpp | 179 +++++++++--------- 2 files changed, 123 insertions(+), 85 deletions(-) diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp index 5feb3971dc..ff0411239b 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp @@ -232,6 +232,35 @@ TEST_F(AckermannSteeringControllerTest, test_message_timeout) (*(controller_->input_ref_.readFromNonRT()))->header.stamp); EXPECT_TRUE(std::isnan((*reference)->twist.linear.x)); EXPECT_TRUE(std::isnan((*reference)->twist.angular.z)); + // EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x, + // 0.45); + // EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z, + // 0.0); +} + +TEST_F(AckermannSteeringControllerTest, test_time_stamp_zero) { + 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); + + // try to set command with time before timeout - command is not updated + auto reference = controller_->input_ref_.readFromNonRT(); + auto old_timestamp = (*reference)->header.stamp; + EXPECT_TRUE(std::isnan((*reference)->twist.linear.x)); + EXPECT_TRUE(std::isnan((*reference)->twist.angular.z)); + publish_commands(rclcpp::Time(0)); + ASSERT_TRUE(controller_->wait_for_commands(executor)); + ASSERT_EQ(old_timestamp.sec, + (*(controller_->input_ref_.readFromNonRT()))->header.stamp.sec); + EXPECT_FALSE( + std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x)); + EXPECT_FALSE(std::isnan( + (*(controller_->input_ref_.readFromNonRT()))->twist.angular.z)); + EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x, 0.45); + EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z, 0.0); } TEST_F(AckermannSteeringControllerTest, test_message_accepted) diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp index 45682cbb69..00d6bf5114 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp @@ -1,4 +1,5 @@ -// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) +// (template) // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -23,9 +24,8 @@ #include #include -#include "gmock/gmock.h" - #include "ackermann_steering_controller/ackermann_steering_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" @@ -36,15 +36,14 @@ #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" // TODO(anyone): replace the state and command message types -using ControllerStateMsgOdom = - ackermann_steering_controller::AckermannSteeringController::ControllerStateMsgOdom; -using ControllerStateMsgTf = - ackermann_steering_controller::AckermannSteeringController::ControllerStateMsgTf; -using ControllerReferenceMsg = - ackermann_steering_controller::AckermannSteeringController::ControllerReferenceMsg; - -namespace -{ +using ControllerStateMsgOdom = ackermann_steering_controller:: + AckermannSteeringController::ControllerStateMsgOdom; +using ControllerStateMsgTf = ackermann_steering_controller:: + AckermannSteeringController::ControllerStateMsgTf; +using ControllerReferenceMsg = ackermann_steering_controller:: + AckermannSteeringController::ControllerReferenceMsg; + +namespace { constexpr auto NODE_SUCCESS = controller_interface::CallbackReturn::SUCCESS; constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR; } // namespace @@ -52,112 +51,120 @@ constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR; // subclassing and friending so we can access member variables class TestableAckermannSteeringController -: public ackermann_steering_controller::AckermannSteeringController -{ - FRIEND_TEST(AckermannSteeringControllerTest, all_parameters_set_configure_success); + : public ackermann_steering_controller::AckermannSteeringController { + FRIEND_TEST(AckermannSteeringControllerTest, + all_parameters_set_configure_success); FRIEND_TEST(AckermannSteeringControllerTest, check_exported_intefaces); FRIEND_TEST(AckermannSteeringControllerTest, activate_success); FRIEND_TEST(AckermannSteeringControllerTest, update_success); FRIEND_TEST(AckermannSteeringControllerTest, deactivate_success); FRIEND_TEST(AckermannSteeringControllerTest, reactivate_success); FRIEND_TEST(AckermannSteeringControllerTest, publish_status_success); - FRIEND_TEST(AckermannSteeringControllerTest, receive_message_and_publish_updated_status); + FRIEND_TEST(AckermannSteeringControllerTest, + receive_message_and_publish_updated_status); FRIEND_TEST(AckermannSteeringControllerTest, test_message_timeout); + FRIEND_TEST(AckermannSteeringControllerTest, test_time_stamp_zero); FRIEND_TEST(AckermannSteeringControllerTest, test_message_accepted); FRIEND_TEST(AckermannSteeringControllerTest, test_update_logic); FRIEND_TEST(AckermannSteeringControllerTest, test_update_logic_chainable); - FRIEND_TEST(AckermannSteeringControllerTest, test_ref_timeout_zero_for_update); - FRIEND_TEST(AckermannSteeringControllerTest, test_ref_timeout_zero_for_reference_callback); + FRIEND_TEST(AckermannSteeringControllerTest, + test_ref_timeout_zero_for_update); + FRIEND_TEST(AckermannSteeringControllerTest, + test_ref_timeout_zero_for_reference_callback); -public: + public: controller_interface::CallbackReturn on_configure( - const rclcpp_lifecycle::State & previous_state) override - { - auto ret = - ackermann_steering_controller::AckermannSteeringController::on_configure(previous_state); + const rclcpp_lifecycle::State& previous_state) override { + auto ret = ackermann_steering_controller::AckermannSteeringController:: + on_configure(previous_state); // Only if on_configure is successful create subscription - if (ret == CallbackReturn::SUCCESS) - { + 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 - { + const rclcpp_lifecycle::State& previous_state) override { auto ref_itfs = on_export_reference_interfaces(); - return ackermann_steering_controller::AckermannSteeringController::on_activate(previous_state); + return ackermann_steering_controller::AckermannSteeringController:: + 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 + * @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. + * @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) - { + 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}) - { + 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 + // TODO(anyone): add implementation of any methods of your controller is + // needed -private: + private: rclcpp::WaitSet ref_subscriber_wait_set_; }; -// We are using template class here for easier reuse of Fixture in specializations of controllers +// We are using template class here for easier reuse of Fixture in +// specializations of controllers template -class AckermannSteeringControllerFixture : public ::testing::Test -{ -public: +class AckermannSteeringControllerFixture : public ::testing::Test { + public: static void SetUpTestCase() {} - void SetUp() - { + void SetUp() { // initialize controller controller_ = std::make_unique(); - command_publisher_node_ = std::make_shared("command_publisher"); - command_publisher_ = command_publisher_node_->create_publisher( - "/test_ackermann_steering_controller/reference", rclcpp::SystemDefaultsQoS()); + command_publisher_node_ = + std::make_shared("command_publisher"); + command_publisher_ = + command_publisher_node_->create_publisher( + "/test_ackermann_steering_controller/reference", + rclcpp::SystemDefaultsQoS()); } static void TearDownTestCase() {} void TearDown() { controller_.reset(nullptr); } -protected: - void SetUpController(const std::string controller_name = "test_ackermann_steering_controller") - { - ASSERT_EQ(controller_->init(controller_name), controller_interface::return_type::OK); + protected: + void SetUpController(const std::string controller_name = + "test_ackermann_steering_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()); command_itfs_.emplace_back(hardware_interface::CommandInterface( - rear_wheel_name, hardware_interface::HW_IF_VELOCITY, &joint_command_values_[0])); + rear_wheel_name, hardware_interface::HW_IF_VELOCITY, + &joint_command_values_[0])); command_ifs.emplace_back(command_itfs_.back()); command_itfs_.emplace_back(hardware_interface::CommandInterface( - front_steer_name, hardware_interface::HW_IF_VELOCITY, &joint_command_values_[1])); + front_steer_name, hardware_interface::HW_IF_VELOCITY, + &joint_command_values_[1])); command_ifs.emplace_back(command_itfs_.back()); // TODO(anyone): Add other command interfaces, if any @@ -166,24 +173,28 @@ class AckermannSteeringControllerFixture : public ::testing::Test state_ifs.reserve(joint_state_values_.size()); state_itfs_.emplace_back(hardware_interface::StateInterface( - rear_wheel_name, hardware_interface::HW_IF_POSITION, &joint_state_values_[0])); + rear_wheel_name, hardware_interface::HW_IF_POSITION, + &joint_state_values_[0])); state_ifs.emplace_back(state_itfs_.back()); state_itfs_.emplace_back(hardware_interface::StateInterface( - front_steer_name, hardware_interface::HW_IF_POSITION, &joint_state_values_[1])); + front_steer_name, hardware_interface::HW_IF_POSITION, + &joint_state_values_[1])); 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)); + controller_->assign_interfaces(std::move(command_ifs), + std::move(state_ifs)); } - void subscribe_and_get_messages(ControllerStateMsgOdom & msg_odom) - { + void subscribe_and_get_messages(ControllerStateMsgOdom& msg_odom) { // create a new subscriber rclcpp::Node test_subscription_node("test_subscription_node"); auto subs_callback_odom = [&](const ControllerStateMsgOdom::SharedPtr) {}; - auto subscription_odom = test_subscription_node.create_subscription( - "/test_ackermann_steering_controller/odometry", 10, subs_callback_odom); + auto subscription_odom = + test_subscription_node.create_subscription( + "/test_ackermann_steering_controller/odometry", 10, + subs_callback_odom); // call update to publish the test value ASSERT_EQ( @@ -194,7 +205,8 @@ class AckermannSteeringControllerFixture : public ::testing::Test controller_interface::return_type::OK); // call update to publish the test value - // since update doesn't guarantee a published message, republish until received + // 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_odom); @@ -202,16 +214,17 @@ class AckermannSteeringControllerFixture : public ::testing::Test { controller_->update_reference_from_subscribers(); controller_->update_and_write_commands( - controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)); + controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)); // check if message has been received - if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) - { + 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"; + 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_odom_info; @@ -219,19 +232,15 @@ class AckermannSteeringControllerFixture : public ::testing::Test } // TODO(anyone): add/remove arguments as it suites your command message type - void publish_commands( - const rclcpp::Time & stamp, const double & twist_linear_x = 0.45, - const double & twist_angular_z = 0.0) - { - auto wait_for_topic = [&](const auto topic_name) - { + void publish_commands(const rclcpp::Time& stamp, + const double& twist_linear_x = 0.45, + const double& twist_angular_z = 0.0) { + 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"; + 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)); @@ -254,7 +263,7 @@ class AckermannSteeringControllerFixture : public ::testing::Test command_publisher_->publish(msg); } -protected: + protected: // TODO(anyone): adjust the members as needed std::string rear_wheel_name = "rear_wheel_joint"; std::string front_steer_name = "front_steer_joint"; From 4d7fa9900a266e969ec42d63a044d2c25d4e8507 Mon Sep 17 00:00:00 2001 From: petkovich Date: Thu, 17 Nov 2022 16:46:22 +0100 Subject: [PATCH 015/186] ackermann odometry --- .../src/ackermann_steering_controller.cpp | 48 ++++++++----------- 1 file changed, 20 insertions(+), 28 deletions(-) diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.cpp b/ackermann_steering_controller/src/ackermann_steering_controller.cpp index c015cf56f6..b79558b128 100644 --- a/ackermann_steering_controller/src/ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/src/ackermann_steering_controller.cpp @@ -131,6 +131,8 @@ controller_interface::CallbackReturn AckermannSteeringController::on_configure( rt_odom_state_publisher_->msg_.child_frame_id = params_.base_frame_id; rt_odom_state_publisher_->msg_.pose.pose.position.z = 0; + previous_publish_timestamp_ = rclcpp::Time(0); + auto & covariance = rt_odom_state_publisher_->msg_.twist.covariance; constexpr size_t NUM_DIMENSIONS = 6; for (size_t index = 0; index < 6; ++index) @@ -168,8 +170,8 @@ controller_interface::CallbackReturn AckermannSteeringController::on_configure( // calculation publication period of odometry and tf odometry messages publish_period_ = rclcpp::Duration::from_seconds(1.0 / params_.publish_rate); - // fprintf(stderr, "publish_period_, publish_rate = %f,%f \n", - // publish_period_.seconds(), params_.publish_rate); + fprintf(stderr, "publish_period_ = %d \n", publish_period_); + RCLCPP_INFO(get_node()->get_logger(), "configure successful"); return controller_interface::CallbackReturn::SUCCESS; } @@ -337,7 +339,7 @@ controller_interface::return_type AckermannSteeringController::update_and_write_ last_linear_velocity_ = reference_interfaces_[0]; last_angular_velocity_ = reference_interfaces_[1]; - previous_publish_timestamp_ = time; + //previous_publish_timestamp_ = time; // omega = linear_vel / radius command_interfaces_[0].set_value(last_linear_velocity_ / params_.wheel_radius); @@ -348,34 +350,24 @@ controller_interface::return_type AckermannSteeringController::update_and_write_ reference_interfaces_[0] = std::numeric_limits::quiet_NaN(); reference_interfaces_[1] = std::numeric_limits::quiet_NaN(); } - // fprintf(stderr, "Exception thrown during controller's init with message: %s - // \n", e.what()); - // fprintf(stderr, "outside \n"); // fprintf( - // stderr, - // "all in sec time, previous_publish_timestamp_, publish_period_, " - // "previous_publish_timestamp_ + publish_period_) < time = %f, %f, %f, " - // "%d\n", - // time.seconds(), previous_publish_timestamp_.seconds(), - // publish_period_.seconds(), - // (previous_publish_timestamp_.seconds() + publish_period_.seconds()) < - // time.seconds()); + // stderr, "previous_publish_timestamp_ + publish_period_ < time = %d, %d, %d, %d\n", time, + // previous_publish_timestamp_, publish_period_, + // (previous_publish_timestamp_ + publish_period_) < time); + // fprintf(stderr, "Exception thrown during controller's init with message: %s \n", e.what()); + // Publish odometry message - if (previous_publish_timestamp_.seconds() + publish_period_.seconds() < - time.seconds()) { - // fprintf(stderr, "inside \n"); + // if (previous_publish_timestamp_ + publish_period_ < time) + if ((previous_publish_timestamp_ + publish_period_).nanoseconds() < time.nanoseconds()) + { + // fprintf(stderr, "debuggin2 \n"); // fprintf( - // stderr, - // "all in sec time, previous_publish_timestamp_, publish_period_, " - // "previous_publish_timestamp_ + publish_period_) < time = %f, %f, %f, - // " - // "%d\n", - // time.seconds(), previous_publish_timestamp_.seconds(), - // publish_period_.seconds(), - // (previous_publish_timestamp_.seconds() + publish_period_.seconds()) < - // time.seconds()); - - previous_publish_timestamp_ += publish_period_; + // stderr, "previous_publish_timestamp_ + publish_period_ < time = %d, %d, %d, %d\n", time, + // previous_publish_timestamp_, publish_period_, + // (previous_publish_timestamp_ + publish_period_) < time); + previous_publish_timestamp_ = rclcpp::Time(time.nanoseconds()); + + //previous_publish_timestamp_ += publish_period_; // Compute and store orientation info tf2::Quaternion orientation; orientation.setRPY(0.0, 0.0, odometry_.getHeading()); From 6e9a5190b5f5e0ab283e78f353a5226999003339 Mon Sep 17 00:00:00 2001 From: petkovich Date: Thu, 17 Nov 2022 17:28:38 +0100 Subject: [PATCH 016/186] template state publisher --- ackermann_steering_controller/CMakeLists.txt | 1 + .../ackermann_steering_controller.hpp | 5 +++++ ackermann_steering_controller/package.xml | 1 + .../src/ackermann_steering_controller.cpp | 4 +++- 4 files changed, 10 insertions(+), 1 deletion(-) diff --git a/ackermann_steering_controller/CMakeLists.txt b/ackermann_steering_controller/CMakeLists.txt index 079329a3b4..b41f72eede 100644 --- a/ackermann_steering_controller/CMakeLists.txt +++ b/ackermann_steering_controller/CMakeLists.txt @@ -20,6 +20,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS tf2 tf2_msgs tf2_geometry_msgs + ackermann_msgs ) find_package(ament_cmake REQUIRED) diff --git a/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp b/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp index 9d7a1ec8a0..aaf4608f69 100644 --- a/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp +++ b/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp @@ -35,6 +35,7 @@ #include "std_srvs/srv/set_bool.hpp" // TODO(anyone): Replace with controller specific messages +#include "ackermann_msgs/msg/ackermann_drive.hpp" #include "geometry_msgs/msg/twist.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" @@ -140,6 +141,10 @@ class AckermannSteeringController : public controller_interface::ChainableContro // Odometry related: Odometry odometry_; + using AckermannDrive = ackermann_msgs::msg::AckermannDrive; + ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC + void publish_state(const AckermannDrive & state); + private: // callback for topic interface ACKERMANN_STEERING_CONTROLLER__VISIBILITY_LOCAL diff --git a/ackermann_steering_controller/package.xml b/ackermann_steering_controller/package.xml index 872b0376c8..c081699356 100644 --- a/ackermann_steering_controller/package.xml +++ b/ackermann_steering_controller/package.xml @@ -25,6 +25,7 @@ tf2 tf2_msgs tf2_geometry_msgs + ackermann_msgs ament_lint_auto ament_cmake_gmock diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.cpp b/ackermann_steering_controller/src/ackermann_steering_controller.cpp index b79558b128..aef2c00734 100644 --- a/ackermann_steering_controller/src/ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/src/ackermann_steering_controller.cpp @@ -170,7 +170,7 @@ controller_interface::CallbackReturn AckermannSteeringController::on_configure( // calculation publication period of odometry and tf odometry messages publish_period_ = rclcpp::Duration::from_seconds(1.0 / params_.publish_rate); - fprintf(stderr, "publish_period_ = %d \n", publish_period_); + //fprintf(stderr, "publish_period_ = %d \n", publish_period_); RCLCPP_INFO(get_node()->get_logger(), "configure successful"); return controller_interface::CallbackReturn::SUCCESS; @@ -401,6 +401,8 @@ controller_interface::return_type AckermannSteeringController::update_and_write_ return controller_interface::return_type::OK; } +void AckermannSteeringController::publish_state(const AckermannDrive & state) { return; } + } // namespace ackermann_steering_controller #include "pluginlib/class_list_macros.hpp" From 8544e3ccdc4aef9ef2f3c9d9bd8627b5a630ac32 Mon Sep 17 00:00:00 2001 From: GiridharBukka Date: Thu, 17 Nov 2022 17:55:48 +0100 Subject: [PATCH 017/186] interfaces check --- .../test_ackermann_steering_controller.cpp | 18 +++++++----------- 1 file changed, 7 insertions(+), 11 deletions(-) diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp index ff0411239b..eb2b3b0744 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp @@ -61,7 +61,7 @@ TEST_F(AckermannSteeringControllerTest, check_exported_intefaces) controller_->params_.rear_wheel_name + "/" + hardware_interface::HW_IF_VELOCITY); EXPECT_EQ( command_intefaces.names[1], - controller_->params_.front_steer_name + "/" + hardware_interface::HW_IF_VELOCITY); + controller_->params_.front_steer_name + "/" + hardware_interface::HW_IF_POSITION); auto state_intefaces = controller_->state_interface_configuration(); ASSERT_EQ(state_intefaces.names.size(), joint_state_values_.size()); @@ -85,12 +85,12 @@ TEST_F(AckermannSteeringControllerTest, check_exported_intefaces) std::string("linear") + "/" + hardware_interface::HW_IF_VELOCITY); const std::string ref_itf_name_1 = std::string(controller_->get_node()->get_name()) + "/" + - "angular" + "/" + hardware_interface::HW_IF_VELOCITY; + "angular" + "/" + hardware_interface::HW_IF_POSITION; EXPECT_EQ(reference_interfaces[1].get_name(), ref_itf_name_1); EXPECT_EQ(reference_interfaces[1].get_prefix_name(), controller_->get_node()->get_name()); EXPECT_EQ( reference_interfaces[1].get_interface_name(), - std::string("angular") + "/" + hardware_interface::HW_IF_VELOCITY); + std::string("angular") + "/" + hardware_interface::HW_IF_POSITION); } TEST_F(AckermannSteeringControllerTest, activate_success) @@ -321,8 +321,7 @@ TEST_F(AckermannSteeringControllerTest, test_update_logic_chainable) { ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); ASSERT_EQ( - controller_->update_reference_from_subscribers( - controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_->update_reference_from_subscribers(), controller_interface::return_type::OK); ASSERT_EQ( controller_->update_and_write_commands( @@ -355,8 +354,7 @@ TEST_F(AckermannSteeringControllerTest, test_update_logic_chainable) { ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); ASSERT_EQ( - controller_->update_reference_from_subscribers( - controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_->update_reference_from_subscribers(), controller_interface::return_type::OK); ASSERT_EQ( controller_->update_and_write_commands( @@ -406,8 +404,7 @@ TEST_F(AckermannSteeringControllerTest, test_update_logic) ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); ASSERT_EQ( - controller_->update_reference_from_subscribers( - controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_->update_reference_from_subscribers(), controller_interface::return_type::OK); ASSERT_EQ( controller_->update_and_write_commands( @@ -435,8 +432,7 @@ TEST_F(AckermannSteeringControllerTest, test_update_logic) ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); ASSERT_EQ( - controller_->update_reference_from_subscribers( - controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_->update_reference_from_subscribers(), controller_interface::return_type::OK); ASSERT_EQ( controller_->update_and_write_commands( From 96a587974466d599b66e10ee1d45697ca34e7343 Mon Sep 17 00:00:00 2001 From: GiridharBukka Date: Thu, 17 Nov 2022 09:41:01 +0100 Subject: [PATCH 018/186] removed publish_period related code --- .../ackermann_steering_controller.hpp | 1 - .../src/ackermann_steering_controller.cpp | 71 +++++++------------ .../src/ackermann_steering_controller.yaml | 9 --- .../ackermann_steering_controller_params.yaml | 2 - 4 files changed, 25 insertions(+), 58 deletions(-) diff --git a/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp b/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp index aaf4608f69..31893c17fc 100644 --- a/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp +++ b/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp @@ -127,7 +127,6 @@ class AckermannSteeringController : public controller_interface::ChainableContro rclcpp::Duration publish_period_ = rclcpp::Duration::from_nanoseconds(0); ; bool open_loop_; - rclcpp::Time previous_publish_timestamp_{0}; /// Velocity command related: struct Commands { diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.cpp b/ackermann_steering_controller/src/ackermann_steering_controller.cpp index aef2c00734..a1ae99db50 100644 --- a/ackermann_steering_controller/src/ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/src/ackermann_steering_controller.cpp @@ -168,10 +168,6 @@ controller_interface::CallbackReturn AckermannSteeringController::on_configure( rt_tf_odom_state_publisher_->msg_.transforms[0].transform.translation.z = 0.0; rt_tf_odom_state_publisher_->unlock(); - // calculation publication period of odometry and tf odometry messages - publish_period_ = rclcpp::Duration::from_seconds(1.0 / params_.publish_rate); - //fprintf(stderr, "publish_period_ = %d \n", publish_period_); - RCLCPP_INFO(get_node()->get_logger(), "configure successful"); return controller_interface::CallbackReturn::SUCCESS; } @@ -350,52 +346,35 @@ controller_interface::return_type AckermannSteeringController::update_and_write_ reference_interfaces_[0] = std::numeric_limits::quiet_NaN(); reference_interfaces_[1] = std::numeric_limits::quiet_NaN(); } - // fprintf( - // stderr, "previous_publish_timestamp_ + publish_period_ < time = %d, %d, %d, %d\n", time, - // previous_publish_timestamp_, publish_period_, - // (previous_publish_timestamp_ + publish_period_) < time); - // fprintf(stderr, "Exception thrown during controller's init with message: %s \n", e.what()); // Publish odometry message - // if (previous_publish_timestamp_ + publish_period_ < time) - if ((previous_publish_timestamp_ + publish_period_).nanoseconds() < time.nanoseconds()) - { - // fprintf(stderr, "debuggin2 \n"); - // fprintf( - // stderr, "previous_publish_timestamp_ + publish_period_ < time = %d, %d, %d, %d\n", time, - // previous_publish_timestamp_, publish_period_, - // (previous_publish_timestamp_ + publish_period_) < time); - previous_publish_timestamp_ = rclcpp::Time(time.nanoseconds()); - - //previous_publish_timestamp_ += publish_period_; - // Compute and store orientation info - tf2::Quaternion orientation; - orientation.setRPY(0.0, 0.0, odometry_.getHeading()); - - // Populate odom message and publish - if (rt_odom_state_publisher_->trylock()) - { - rt_odom_state_publisher_->msg_.header.stamp = time; - rt_odom_state_publisher_->msg_.pose.pose.position.x = odometry_.getX(); - rt_odom_state_publisher_->msg_.pose.pose.position.y = odometry_.getY(); - rt_odom_state_publisher_->msg_.pose.pose.orientation = tf2::toMsg(orientation); - rt_odom_state_publisher_->msg_.twist.twist.linear.x = odometry_.getLinear(); - rt_odom_state_publisher_->msg_.twist.twist.angular.z = odometry_.getAngular(); - rt_odom_state_publisher_->unlockAndPublish(); - } + // Compute and store orientation info + tf2::Quaternion orientation; + orientation.setRPY(0.0, 0.0, odometry_.getHeading()); + + // Populate odom message and publish + if (rt_odom_state_publisher_->trylock()) { + rt_odom_state_publisher_->msg_.header.stamp = time; + rt_odom_state_publisher_->msg_.pose.pose.position.x = odometry_.getX(); + rt_odom_state_publisher_->msg_.pose.pose.position.y = odometry_.getY(); + rt_odom_state_publisher_->msg_.pose.pose.orientation = + tf2::toMsg(orientation); + rt_odom_state_publisher_->msg_.twist.twist.linear.x = odometry_.getLinear(); + rt_odom_state_publisher_->msg_.twist.twist.angular.z = + odometry_.getAngular(); + rt_odom_state_publisher_->unlockAndPublish(); + } - // Publish tf /odom frame - if (params_.enable_odom_tf && rt_tf_odom_state_publisher_->trylock()) - { - rt_tf_odom_state_publisher_->msg_.transforms.front().header.stamp = time; - rt_tf_odom_state_publisher_->msg_.transforms.front().transform.translation.x = - odometry_.getX(); - rt_tf_odom_state_publisher_->msg_.transforms.front().transform.translation.y = - odometry_.getY(); - rt_tf_odom_state_publisher_->msg_.transforms.front().transform.rotation = + // Publish tf /odom frame + if (params_.enable_odom_tf && rt_tf_odom_state_publisher_->trylock()) { + rt_tf_odom_state_publisher_->msg_.transforms.front().header.stamp = time; + rt_tf_odom_state_publisher_->msg_.transforms.front() + .transform.translation.x = odometry_.getX(); + rt_tf_odom_state_publisher_->msg_.transforms.front() + .transform.translation.y = odometry_.getY(); + rt_tf_odom_state_publisher_->msg_.transforms.front().transform.rotation = tf2::toMsg(orientation); - rt_tf_odom_state_publisher_->unlockAndPublish(); - } + rt_tf_odom_state_publisher_->unlockAndPublish(); } return controller_interface::return_type::OK; diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.yaml b/ackermann_steering_controller/src/ackermann_steering_controller.yaml index bc23b13d82..69b3a8b755 100644 --- a/ackermann_steering_controller/src/ackermann_steering_controller.yaml +++ b/ackermann_steering_controller/src/ackermann_steering_controller.yaml @@ -24,15 +24,6 @@ ackermann_steering_controller: } - - publish_rate: { - type: double, - default_value: 50.0, - description: "In Hz. Controller state will be published at this rate.", - read_only: true, - - } - open_loop: { type: bool, default_value: false, diff --git a/ackermann_steering_controller/test/ackermann_steering_controller_params.yaml b/ackermann_steering_controller/test/ackermann_steering_controller_params.yaml index bec88133ac..fa9a29a2dc 100644 --- a/ackermann_steering_controller/test/ackermann_steering_controller_params.yaml +++ b/ackermann_steering_controller/test/ackermann_steering_controller_params.yaml @@ -7,8 +7,6 @@ test_ackermann_steering_controller: front_steer_name: "front_steer_joint" - publish_rate: 50.0 - open_loop: false wheel_separation_multiplier: 1.0 From 29df37484281aefe187c13158105cbb44a7b9e63 Mon Sep 17 00:00:00 2001 From: GiridharBukka Date: Thu, 17 Nov 2022 18:20:34 +0100 Subject: [PATCH 019/186] compiles, tests passed, publish_period removed --- .../src/ackermann_steering_controller.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.cpp b/ackermann_steering_controller/src/ackermann_steering_controller.cpp index a1ae99db50..71be46bfba 100644 --- a/ackermann_steering_controller/src/ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/src/ackermann_steering_controller.cpp @@ -131,8 +131,6 @@ controller_interface::CallbackReturn AckermannSteeringController::on_configure( rt_odom_state_publisher_->msg_.child_frame_id = params_.base_frame_id; rt_odom_state_publisher_->msg_.pose.pose.position.z = 0; - previous_publish_timestamp_ = rclcpp::Time(0); - auto & covariance = rt_odom_state_publisher_->msg_.twist.covariance; constexpr size_t NUM_DIMENSIONS = 6; for (size_t index = 0; index < 6; ++index) From 7f36236472d6bb8a7f40b0d1fe700a00e9dc440b Mon Sep 17 00:00:00 2001 From: petkovich Date: Fri, 18 Nov 2022 15:08:28 +0100 Subject: [PATCH 020/186] state publisher --- .../ackermann_steering_controller.hpp | 10 +++- .../src/ackermann_steering_controller.cpp | 48 ++++++++++++++----- 2 files changed, 44 insertions(+), 14 deletions(-) diff --git a/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp b/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp index 31893c17fc..36462d2e4c 100644 --- a/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp +++ b/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp @@ -142,7 +142,15 @@ class AckermannSteeringController : public controller_interface::ChainableContro using AckermannDrive = ackermann_msgs::msg::AckermannDrive; ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC - void publish_state(const AckermannDrive & state); + void publish_state(const AckermannDrive & desired_state, const AckermannDrive & current_state); + AckermannDrive state_current_; + AckermannDrive state_desired_; + + // using ControllerStateMsg = control_msgs::msg::JointTrajectoryControllerState; + // using StatePublisher = realtime_tools::RealtimePublisher; + // using StatePublisherPtr = std::unique_ptr; + // rclcpp::Publisher::SharedPtr publisher_; + // StatePublisherPtr state_publisher_; private: // callback for topic interface diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.cpp b/ackermann_steering_controller/src/ackermann_steering_controller.cpp index 71be46bfba..1888dfdb0a 100644 --- a/ackermann_steering_controller/src/ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/src/ackermann_steering_controller.cpp @@ -309,6 +309,7 @@ controller_interface::return_type AckermannSteeringController::update_and_write_ { const double wheel_position = state_interfaces_[0].get_value(); const double steer_position = state_interfaces_[1].get_value() * params_.steer_pos_multiplier; + state_current_.steering_angle = steer_position; if (std::isnan(wheel_position) || std::isnan(steer_position)) { @@ -351,35 +352,56 @@ controller_interface::return_type AckermannSteeringController::update_and_write_ orientation.setRPY(0.0, 0.0, odometry_.getHeading()); // Populate odom message and publish - if (rt_odom_state_publisher_->trylock()) { + if (rt_odom_state_publisher_->trylock()) + { rt_odom_state_publisher_->msg_.header.stamp = time; rt_odom_state_publisher_->msg_.pose.pose.position.x = odometry_.getX(); rt_odom_state_publisher_->msg_.pose.pose.position.y = odometry_.getY(); - rt_odom_state_publisher_->msg_.pose.pose.orientation = - tf2::toMsg(orientation); + rt_odom_state_publisher_->msg_.pose.pose.orientation = tf2::toMsg(orientation); rt_odom_state_publisher_->msg_.twist.twist.linear.x = odometry_.getLinear(); - rt_odom_state_publisher_->msg_.twist.twist.angular.z = - odometry_.getAngular(); + rt_odom_state_publisher_->msg_.twist.twist.angular.z = odometry_.getAngular(); rt_odom_state_publisher_->unlockAndPublish(); } // Publish tf /odom frame - if (params_.enable_odom_tf && rt_tf_odom_state_publisher_->trylock()) { + if (params_.enable_odom_tf && rt_tf_odom_state_publisher_->trylock()) + { rt_tf_odom_state_publisher_->msg_.transforms.front().header.stamp = time; - rt_tf_odom_state_publisher_->msg_.transforms.front() - .transform.translation.x = odometry_.getX(); - rt_tf_odom_state_publisher_->msg_.transforms.front() - .transform.translation.y = odometry_.getY(); + rt_tf_odom_state_publisher_->msg_.transforms.front().transform.translation.x = odometry_.getX(); + rt_tf_odom_state_publisher_->msg_.transforms.front().transform.translation.y = odometry_.getY(); rt_tf_odom_state_publisher_->msg_.transforms.front().transform.rotation = - tf2::toMsg(orientation); + tf2::toMsg(orientation); rt_tf_odom_state_publisher_->unlockAndPublish(); } + //publish_state(state_current_, state_desired); return controller_interface::return_type::OK; } -void AckermannSteeringController::publish_state(const AckermannDrive & state) { return; } - +void AckermannSteeringController::publish_state( + const AckermannDrive & desired_state, const AckermannDrive & current_state) +{ + // if (state_publisher_ && state_publisher_->trylock()) + // { + // state_publisher_->msg_.header.stamp = get_node()->now(); + // state_publisher_->msg_.desired.positions = desired_state.positions; + // state_publisher_->msg_.desired.velocities = desired_state.velocities; + // state_publisher_->msg_.desired.accelerations = desired_state.accelerations; + // state_publisher_->msg_.actual.positions = current_state.positions; + // state_publisher_->msg_.error.positions = state_error.positions; + // if (has_velocity_state_interface_) + // { + // state_publisher_->msg_.actual.velocities = current_state.velocities; + // state_publisher_->msg_.error.velocities = state_error.velocities; + // } + // if (has_acceleration_state_interface_) + // { + // state_publisher_->msg_.actual.accelerations = current_state.accelerations; + // state_publisher_->msg_.error.accelerations = state_error.accelerations; + // } + + // state_publisher_->unlockAndPublish(); +} } // namespace ackermann_steering_controller #include "pluginlib/class_list_macros.hpp" From 65c69bc1200d7989c8b3796d43e497348b57532d Mon Sep 17 00:00:00 2001 From: petkovich Date: Fri, 18 Nov 2022 23:45:18 +0100 Subject: [PATCH 021/186] new message published --- ackermann_steering_controller/CMakeLists.txt | 6 ++ .../ackermann_steering_controller.hpp | 17 ++--- ackermann_steering_controller/package.xml | 1 + .../src/ackermann_steering_controller.cpp | 73 ++++++++++++------- custom_messages/CMakeLists.txt | 37 ++++++++++ .../msg/AckermanControllerState.msg | 4 + custom_messages/package.xml | 24 ++++++ 7 files changed, 126 insertions(+), 36 deletions(-) create mode 100644 custom_messages/CMakeLists.txt create mode 100644 custom_messages/msg/AckermanControllerState.msg create mode 100644 custom_messages/package.xml diff --git a/ackermann_steering_controller/CMakeLists.txt b/ackermann_steering_controller/CMakeLists.txt index b41f72eede..75b2f328a5 100644 --- a/ackermann_steering_controller/CMakeLists.txt +++ b/ackermann_steering_controller/CMakeLists.txt @@ -21,6 +21,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS tf2_msgs tf2_geometry_msgs ackermann_msgs + custom_messages ) find_package(ament_cmake REQUIRED) @@ -39,6 +40,10 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) +set(msg_files + msg/AckermanControllerState.msg +) + # Add ackermann_steering_controller library related compile commands generate_parameter_library(ackermann_steering_controller_parameters src/ackermann_steering_controller.yaml @@ -116,6 +121,7 @@ endif() ament_export_include_directories( include ) + ament_export_dependencies( ) diff --git a/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp b/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp index 36462d2e4c..b9da94cfe8 100644 --- a/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp +++ b/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp @@ -36,6 +36,7 @@ // TODO(anyone): Replace with controller specific messages #include "ackermann_msgs/msg/ackermann_drive.hpp" +#include "custom_messages/msg/ackerman_controller_state.hpp" #include "geometry_msgs/msg/twist.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" @@ -140,17 +141,13 @@ class AckermannSteeringController : public controller_interface::ChainableContro // Odometry related: Odometry odometry_; - using AckermannDrive = ackermann_msgs::msg::AckermannDrive; + using AckermanControllerState = custom_messages::msg::AckermanControllerState; ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC - void publish_state(const AckermannDrive & desired_state, const AckermannDrive & current_state); - AckermannDrive state_current_; - AckermannDrive state_desired_; - - // using ControllerStateMsg = control_msgs::msg::JointTrajectoryControllerState; - // using StatePublisher = realtime_tools::RealtimePublisher; - // using StatePublisherPtr = std::unique_ptr; - // rclcpp::Publisher::SharedPtr publisher_; - // StatePublisherPtr state_publisher_; + AckermanControllerState published_state_; + + using ControllerStatePublisher = realtime_tools::RealtimePublisher; + rclcpp::Publisher::SharedPtr controller_s_publisher_; + std::unique_ptr controller_state_publisher_; private: // callback for topic interface diff --git a/ackermann_steering_controller/package.xml b/ackermann_steering_controller/package.xml index c081699356..087545941e 100644 --- a/ackermann_steering_controller/package.xml +++ b/ackermann_steering_controller/package.xml @@ -26,6 +26,7 @@ tf2_msgs tf2_geometry_msgs ackermann_msgs + custom_messages ament_lint_auto ament_cmake_gmock diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.cpp b/ackermann_steering_controller/src/ackermann_steering_controller.cpp index 1888dfdb0a..bb5c178168 100644 --- a/ackermann_steering_controller/src/ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/src/ackermann_steering_controller.cpp @@ -111,7 +111,7 @@ controller_interface::CallbackReturn AckermannSteeringController::on_configure( try { - // State publisher + // Odom state publisher odom_s_publisher_ = get_node()->create_publisher( "~/odometry", rclcpp::SystemDefaultsQoS()); rt_odom_state_publisher_ = std::make_unique(odom_s_publisher_); @@ -166,6 +166,37 @@ controller_interface::CallbackReturn AckermannSteeringController::on_configure( rt_tf_odom_state_publisher_->msg_.transforms[0].transform.translation.z = 0.0; rt_tf_odom_state_publisher_->unlock(); + try + { + // State publisher + controller_s_publisher_ = get_node()->create_publisher( + "~/controller_state", rclcpp::SystemDefaultsQoS()); + controller_state_publisher_ = + std::make_unique(controller_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 + controller_state_publisher_->lock(); + controller_state_publisher_->msg_.header.stamp = get_node()->now(); + controller_state_publisher_->msg_.header.frame_id = params_.odom_frame_id; + controller_state_publisher_->msg_.odom.pose.pose.position.z = 0; + controller_state_publisher_->msg_.odom.child_frame_id = params_.base_frame_id; + controller_state_publisher_->unlock(); + auto & covariance_controller = controller_state_publisher_->msg_.odom.twist.covariance; + for (size_t index = 0; index < 6; ++index) + { + // 0, 7, 14, 21, 28, 35 + const size_t diagonal_index = NUM_DIMENSIONS * index + index; + covariance_controller[diagonal_index] = params_.pose_covariance_diagonal[index]; + covariance_controller[diagonal_index] = params_.twist_covariance_diagonal[index]; + } RCLCPP_INFO(get_node()->get_logger(), "configure successful"); return controller_interface::CallbackReturn::SUCCESS; } @@ -309,7 +340,6 @@ controller_interface::return_type AckermannSteeringController::update_and_write_ { const double wheel_position = state_interfaces_[0].get_value(); const double steer_position = state_interfaces_[1].get_value() * params_.steer_pos_multiplier; - state_current_.steering_angle = steer_position; if (std::isnan(wheel_position) || std::isnan(steer_position)) { @@ -374,34 +404,25 @@ controller_interface::return_type AckermannSteeringController::update_and_write_ rt_tf_odom_state_publisher_->unlockAndPublish(); } + if (controller_state_publisher_->trylock()) + { + controller_state_publisher_->msg_.header.stamp = time; + controller_state_publisher_->msg_.odom.pose.pose.position.x = odometry_.getX(); + controller_state_publisher_->msg_.odom.pose.pose.position.y = odometry_.getY(); + controller_state_publisher_->msg_.odom.pose.pose.orientation = tf2::toMsg(orientation); + controller_state_publisher_->msg_.odom.twist.twist.linear.x = odometry_.getLinear(); + controller_state_publisher_->msg_.odom.twist.twist.angular.z = odometry_.getAngular(); + controller_state_publisher_->msg_.wheel_position = state_interfaces_[0].get_value(); + controller_state_publisher_->msg_.steer_position = + state_interfaces_[1].get_value() * params_.steer_pos_multiplier; + + controller_state_publisher_->unlockAndPublish(); + } + //publish_state(state_current_, state_desired); return controller_interface::return_type::OK; } -void AckermannSteeringController::publish_state( - const AckermannDrive & desired_state, const AckermannDrive & current_state) -{ - // if (state_publisher_ && state_publisher_->trylock()) - // { - // state_publisher_->msg_.header.stamp = get_node()->now(); - // state_publisher_->msg_.desired.positions = desired_state.positions; - // state_publisher_->msg_.desired.velocities = desired_state.velocities; - // state_publisher_->msg_.desired.accelerations = desired_state.accelerations; - // state_publisher_->msg_.actual.positions = current_state.positions; - // state_publisher_->msg_.error.positions = state_error.positions; - // if (has_velocity_state_interface_) - // { - // state_publisher_->msg_.actual.velocities = current_state.velocities; - // state_publisher_->msg_.error.velocities = state_error.velocities; - // } - // if (has_acceleration_state_interface_) - // { - // state_publisher_->msg_.actual.accelerations = current_state.accelerations; - // state_publisher_->msg_.error.accelerations = state_error.accelerations; - // } - - // state_publisher_->unlockAndPublish(); -} } // namespace ackermann_steering_controller #include "pluginlib/class_list_macros.hpp" diff --git a/custom_messages/CMakeLists.txt b/custom_messages/CMakeLists.txt new file mode 100644 index 0000000000..bd66703e9e --- /dev/null +++ b/custom_messages/CMakeLists.txt @@ -0,0 +1,37 @@ +cmake_minimum_required(VERSION 3.8) +project(custom_messages) + +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) +find_package(rosidl_default_generators REQUIRED) +find_package(std_msgs REQUIRED) +find_package(nav_msgs REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/AckermanControllerState.msg" + DEPENDENCIES + std_msgs + nav_msgs + ) + + +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/custom_messages/msg/AckermanControllerState.msg b/custom_messages/msg/AckermanControllerState.msg new file mode 100644 index 0000000000..14562b8375 --- /dev/null +++ b/custom_messages/msg/AckermanControllerState.msg @@ -0,0 +1,4 @@ +std_msgs/Header header +nav_msgs/Odometry odom +float64 wheel_position +float64 steer_position diff --git a/custom_messages/package.xml b/custom_messages/package.xml new file mode 100644 index 0000000000..8bd88672c7 --- /dev/null +++ b/custom_messages/package.xml @@ -0,0 +1,24 @@ + + + + custom_messages + 0.0.0 + TODO: Package description + tomislav + TODO: License declaration + + ament_cmake + + ament_lint_auto + ament_lint_common + rosidl_default_generators + std_msgs + nav_msgs + + rosidl_default_runtime + +rosidl_interface_packages + + ament_cmake + + From 6e520432e9debf38ab24b0d51022a2221a1110b0 Mon Sep 17 00:00:00 2001 From: GiridharBukka Date: Tue, 22 Nov 2022 12:11:40 +0100 Subject: [PATCH 022/186] added feedback_type() method to choose position or velocity hardware interface --- .../ackermann_steering_controller.hpp | 2 + .../odometry.hpp | 2 +- .../src/ackermann_steering_controller.cpp | 42 ++++++++-- .../src/ackermann_steering_controller.yaml | 9 ++ .../src/odometry.cpp | 84 +++++++++++++------ 5 files changed, 103 insertions(+), 36 deletions(-) diff --git a/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp b/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp index b9da94cfe8..b0fbaf8bcf 100644 --- a/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp +++ b/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp @@ -96,6 +96,8 @@ class AckermannSteeringController : public controller_interface::ChainableContro std::shared_ptr param_listener_; ackermann_steering_controller::Params params_; + const char * feedback_type() const; + // Command subscribers and Controller State publisher rclcpp::Subscription::SharedPtr ref_subscriber_ = nullptr; realtime_tools::RealtimeBuffer> input_ref_; diff --git a/ackermann_steering_controller/include/ackermann_steering_controller/odometry.hpp b/ackermann_steering_controller/include/ackermann_steering_controller/odometry.hpp index abf6155a48..e6cd0a2e62 100644 --- a/ackermann_steering_controller/include/ackermann_steering_controller/odometry.hpp +++ b/ackermann_steering_controller/include/ackermann_steering_controller/odometry.hpp @@ -89,7 +89,7 @@ class Odometry * \param time Current time * \return true if the odometry is actually updated */ - bool update(double rear_wheel_pos, double front_steer_pos, const rclcpp::Time & time); + bool update(double rear_wheel_pos, double front_steer_pos, double rear_wheel_vel, bool position_feedback, const rclcpp::Time & time); /** * \brief Updates the odometry class with latest velocity command diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.cpp b/ackermann_steering_controller/src/ackermann_steering_controller.cpp index bb5c178168..f89ee0fbe2 100644 --- a/ackermann_steering_controller/src/ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/src/ackermann_steering_controller.cpp @@ -69,6 +69,11 @@ AckermannSteeringController::AckermannSteeringController() { } +const char * AckermannSteeringController::feedback_type() const +{ + return params_.position_feedback ? hardware_interface::HW_IF_POSITION : hardware_interface::HW_IF_VELOCITY; +} + controller_interface::CallbackReturn AckermannSteeringController::on_init() { try @@ -252,7 +257,7 @@ AckermannSteeringController::state_interface_configuration() const state_interfaces_config.names.reserve(NR_STATE_ITFS); state_interfaces_config.names.push_back( - params_.rear_wheel_name + "/" + hardware_interface::HW_IF_POSITION); + params_.rear_wheel_name + "/" + feedback_type()); state_interfaces_config.names.push_back( params_.front_steer_name + "/" + hardware_interface::HW_IF_POSITION); @@ -338,16 +343,35 @@ controller_interface::return_type AckermannSteeringController::update_and_write_ } else { - const double wheel_position = state_interfaces_[0].get_value(); - const double steer_position = state_interfaces_[1].get_value() * params_.steer_pos_multiplier; - - if (std::isnan(wheel_position) || std::isnan(steer_position)) + if (params_.position_feedback) { - return controller_interface::return_type::OK; - } + const double wheel_position = state_interfaces_[0].get_value(); + const double steer_position = state_interfaces_[1].get_value() * params_.steer_pos_multiplier; + const double wheel_vel = 0.0; + + if (std::isnan(wheel_position) || std::isnan(steer_position)) + { + return controller_interface::return_type::OK; + } + + // Estimate linear and angular velocity using joint information + odometry_.update(wheel_position, steer_position, wheel_vel, params_.position_feedback, time); + + } else{ + + const double wheel_position = 0.0; + const double steer_position = state_interfaces_[1].get_value() * params_.steer_pos_multiplier; + const double wheel_vel = state_interfaces_[0].get_value(); + + if (std::isnan(wheel_position) || std::isnan(steer_position)) + { + return controller_interface::return_type::OK; + } + // Estimate linear and angular velocity using joint information + odometry_.update(wheel_position, steer_position, wheel_vel, params_.position_feedback, time); + + } - // Estimate linear and angular velocity using joint information - odometry_.update(wheel_position, steer_position, time); } // MOVE ROBOT diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.yaml b/ackermann_steering_controller/src/ackermann_steering_controller.yaml index 69b3a8b755..c9a2d9b663 100644 --- a/ackermann_steering_controller/src/ackermann_steering_controller.yaml +++ b/ackermann_steering_controller/src/ackermann_steering_controller.yaml @@ -129,6 +129,15 @@ ackermann_steering_controller: } + position_feedback: { + type: bool, + default_value: false, + description: "choice of feedbacktype, if position_feedback is false then HW_IF_VELOCITY is taken as interface type, if + position_feedback is true then HW_IF_POSITION is taken as interface type", + read_only: false, + + } + diff --git a/ackermann_steering_controller/src/odometry.cpp b/ackermann_steering_controller/src/odometry.cpp index 6e5bd99088..cec736bd9b 100644 --- a/ackermann_steering_controller/src/odometry.cpp +++ b/ackermann_steering_controller/src/odometry.cpp @@ -75,43 +75,75 @@ namespace ackermann_steering_controller } // TODO(destogl): enalbe also velocity interface to update using velocity from the rear wheel - bool Odometry::update(double rear_wheel_pos, double front_steer_pos, const rclcpp::Time &time) + bool Odometry::update(double rear_wheel_pos, double front_steer_pos, double rear_wheel_vel, bool position_feedback, const rclcpp::Time &time) { - /// Get current wheel joint positions: - const double rear_wheel_cur_pos = rear_wheel_pos * wheel_radius_; + if (position_feedback) + { + /// Get current wheel joint positions: + const double rear_wheel_cur_pos = rear_wheel_pos * wheel_radius_; - /// Estimate velocity of wheels using old and current position: - //const double left_wheel_est_vel = left_wheel_cur_pos - left_wheel_old_pos_; - //const double right_wheel_est_vel = right_wheel_cur_pos - right_wheel_old_pos_; + /// Estimate velocity of wheels using old and current position: + //const double left_wheel_est_vel = left_wheel_cur_pos - left_wheel_old_pos_; + //const double right_wheel_est_vel = right_wheel_cur_pos - right_wheel_old_pos_; - const double rear_wheel_est_vel = rear_wheel_cur_pos - rear_wheel_old_pos_; + const double rear_wheel_est_vel = rear_wheel_cur_pos - rear_wheel_old_pos_; - /// Update old position with current: - rear_wheel_old_pos_ = rear_wheel_cur_pos; + /// Update old position with current: + rear_wheel_old_pos_ = rear_wheel_cur_pos; - /// Compute linear and angular diff: - const double linear = rear_wheel_est_vel;//(right_wheel_est_vel + left_wheel_est_vel) * 0.5; - //const double angular = (right_wheel_est_vel - left_wheel_est_vel) / wheel_separation_w_; - const double angular = tan(front_steer_pos) * linear / wheel_separation_; + /// Compute linear and angular diff: + const double linear = rear_wheel_est_vel;//(right_wheel_est_vel + left_wheel_est_vel) * 0.5; + //const double angular = (right_wheel_est_vel - left_wheel_est_vel) / wheel_separation_w_; + const double angular = tan(front_steer_pos) * linear / wheel_separation_; - /// Integrate odometry: - integrate_fun_(linear, angular); + /// Integrate odometry: + integrate_fun_(linear, angular); - /// We cannot estimate the speed with very small time intervals: - const double dt = time.seconds() - timestamp_.seconds(); - if (dt < 0.0001) - return false; // Interval too small to integrate with + /// We cannot estimate the speed with very small time intervals: + const double dt = time.seconds() - timestamp_.seconds(); + if (dt < 0.0001) + return false; // Interval too small to integrate with - timestamp_ = time; + timestamp_ = time; + + /// Estimate speeds using a rolling mean to filter them out: + linear_acc_(linear/dt); + angular_acc_(angular/dt); + + linear_ = bacc::rolling_mean(linear_acc_); + angular_ = bacc::rolling_mean(angular_acc_); + + return true; + + } else { - /// Estimate speeds using a rolling mean to filter them out: - linear_acc_(linear/dt); - angular_acc_(angular/dt); + /// Compute linear and angular diff: + const double linear = rear_wheel_vel;//(right_wheel_est_vel + left_wheel_est_vel) * 0.5; + //const double angular = (right_wheel_est_vel - left_wheel_est_vel) / wheel_separation_w_; + const double angular = tan(front_steer_pos) * linear / wheel_separation_; - linear_ = bacc::rolling_mean(linear_acc_); - angular_ = bacc::rolling_mean(angular_acc_); + /// Integrate odometry: + integrate_fun_(linear, angular); + + /// We cannot estimate the speed with very small time intervals: + const double dt = time.seconds() - timestamp_.seconds(); + if (dt < 0.0001) + return false; // Interval too small to integrate with + + timestamp_ = time; + + /// Estimate speeds using a rolling mean to filter them out: + linear_acc_(linear/dt); + angular_acc_(angular/dt); + + linear_ = bacc::rolling_mean(linear_acc_); + angular_ = bacc::rolling_mean(angular_acc_); + + return true; + + + } - return true; } void Odometry::updateOpenLoop(double linear, double angular, const rclcpp::Time &time) From c8cbd281222da5d57315647b18502767ce9f77f8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Thu, 24 Nov 2022 16:28:02 +0100 Subject: [PATCH 023/186] Code optimization. --- .../odometry.hpp | 13 +- .../src/ackermann_steering_controller.cpp | 84 +++--- .../src/odometry.cpp | 241 ++++++++---------- .../msg/AckermanControllerState.msg | 5 +- 4 files changed, 157 insertions(+), 186 deletions(-) diff --git a/ackermann_steering_controller/include/ackermann_steering_controller/odometry.hpp b/ackermann_steering_controller/include/ackermann_steering_controller/odometry.hpp index e6cd0a2e62..e5cff66bd7 100644 --- a/ackermann_steering_controller/include/ackermann_steering_controller/odometry.hpp +++ b/ackermann_steering_controller/include/ackermann_steering_controller/odometry.hpp @@ -53,7 +53,6 @@ namespace ackermann_steering_controller { - namespace bacc = boost::accumulators; /** @@ -71,7 +70,7 @@ class Odometry * Timestamp will get the current time value * Value will be set to zero * \param velocity_rolling_window_size Rolling window size used to compute the velocity mean - * + * */ // ackermann_steering_controller_ros2::Params params; explicit Odometry(size_t velocity_rolling_window_size = 10); @@ -89,7 +88,9 @@ class Odometry * \param time Current time * \return true if the odometry is actually updated */ - bool update(double rear_wheel_pos, double front_steer_pos, double rear_wheel_vel, bool position_feedback, const rclcpp::Time & time); + bool update( + const double rear_wheel_pos, const double front_steer_pos, const double rear_wheel_vel, + const bool position_feedback, const double dt); /** * \brief Updates the odometry class with latest velocity command @@ -97,7 +98,7 @@ class Odometry * \param angular Angular velocity [rad/s] * \param time Current time */ - void updateOpenLoop(double linear, double angular, const rclcpp::Time & time); + void updateOpenLoop(const double linear, const double angular, const double dt); /** * \brief heading getter @@ -187,7 +188,7 @@ class Odometry RollingMeanAcc linear_acc_; RollingMeanAcc angular_acc_; - /// Integration funcion, used to integrate the odometry: + /// Integration function, used to integrate the odometry: IntegrationFunction integrate_fun_; }; -} +} // namespace ackermann_steering_controller diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.cpp b/ackermann_steering_controller/src/ackermann_steering_controller.cpp index f89ee0fbe2..78f0a6a0b2 100644 --- a/ackermann_steering_controller/src/ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/src/ackermann_steering_controller.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -69,11 +69,6 @@ AckermannSteeringController::AckermannSteeringController() { } -const char * AckermannSteeringController::feedback_type() const -{ - return params_.position_feedback ? hardware_interface::HW_IF_POSITION : hardware_interface::HW_IF_VELOCITY; -} - controller_interface::CallbackReturn AckermannSteeringController::on_init() { try @@ -256,8 +251,9 @@ AckermannSteeringController::state_interface_configuration() const state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; state_interfaces_config.names.reserve(NR_STATE_ITFS); - state_interfaces_config.names.push_back( - params_.rear_wheel_name + "/" + feedback_type()); + const auto rear_wheel_feedback = params_.position_feedback ? hardware_interface::HW_IF_POSITION + : hardware_interface::HW_IF_VELOCITY; + state_interfaces_config.names.push_back(params_.rear_wheel_name + "/" + feedback_type()); state_interfaces_config.names.push_back( params_.front_steer_name + "/" + hardware_interface::HW_IF_POSITION); @@ -335,43 +331,32 @@ controller_interface::return_type AckermannSteeringController::update_reference_ } controller_interface::return_type AckermannSteeringController::update_and_write_commands( - const rclcpp::Time & time, const rclcpp::Duration & /*period*/) + const rclcpp::Time & time, const rclcpp::Duration & period) { if (params_.open_loop) { - odometry_.updateOpenLoop(last_linear_velocity_, last_angular_velocity_, time); + odometry_.updateOpenLoop(last_linear_velocity_, last_angular_velocity_, period.seconds()); } else { - if (params_.position_feedback) - { - const double wheel_position = state_interfaces_[0].get_value(); - const double steer_position = state_interfaces_[1].get_value() * params_.steer_pos_multiplier; - const double wheel_vel = 0.0; + const double rear_wheel_value = state_interfaces_[0].get_value(); + const double steer_position = state_interfaces_[1].get_value() * params_.steer_pos_multiplier; - if (std::isnan(wheel_position) || std::isnan(steer_position)) + if (!std::isnan(rear_wheel_value) && !std::isnan(steer_position)) + { + if (params_.position_feedback) { - return controller_interface::return_type::OK; - } - // Estimate linear and angular velocity using joint information - odometry_.update(wheel_position, steer_position, wheel_vel, params_.position_feedback, time); - - } else{ - - const double wheel_position = 0.0; - const double steer_position = state_interfaces_[1].get_value() * params_.steer_pos_multiplier; - const double wheel_vel = state_interfaces_[0].get_value(); - - if (std::isnan(wheel_position) || std::isnan(steer_position)) - { - return controller_interface::return_type::OK; - } + odometry_.update( + rear_wheel_value, steer_position, 0.0, params_.position_feedback, period.seconds()); + } + else + { // Estimate linear and angular velocity using joint information - odometry_.update(wheel_position, steer_position, wheel_vel, params_.position_feedback, time); - + odometry_.update( + 0.0, steer_position, rear_wheel_value, params_.position_feedback, period.seconds()); } - + } } // MOVE ROBOT @@ -379,20 +364,18 @@ controller_interface::return_type AckermannSteeringController::update_and_write_ // Limit velocities and accelerations: // TODO(destogl): add limiter for the velocities - if (std::isnan(reference_interfaces_[0]) || std::isnan(reference_interfaces_[1])) + if (!std::isnan(reference_interfaces_[0]) && !std::isnan(reference_interfaces_[1])) { - return controller_interface::return_type::OK; - } - - // store and set commands - last_linear_velocity_ = reference_interfaces_[0]; - last_angular_velocity_ = reference_interfaces_[1]; + // store and set commands + last_linear_velocity_ = reference_interfaces_[0]; + last_angular_velocity_ = reference_interfaces_[1]; - //previous_publish_timestamp_ = time; + //previous_publish_timestamp_ = time; - // omega = linear_vel / radius - command_interfaces_[0].set_value(last_linear_velocity_ / params_.wheel_radius); - command_interfaces_[1].set_value(last_angular_velocity_); + // omega = linear_vel / radius + command_interfaces_[0].set_value(last_linear_velocity_ / params_.wheel_radius); + command_interfaces_[1].set_value(last_angular_velocity_); + } if (ref_timeout_ == rclcpp::Duration::from_seconds(0) || is_in_chained_mode()) { @@ -436,9 +419,18 @@ controller_interface::return_type AckermannSteeringController::update_and_write_ controller_state_publisher_->msg_.odom.pose.pose.orientation = tf2::toMsg(orientation); controller_state_publisher_->msg_.odom.twist.twist.linear.x = odometry_.getLinear(); controller_state_publisher_->msg_.odom.twist.twist.angular.z = odometry_.getAngular(); - controller_state_publisher_->msg_.wheel_position = state_interfaces_[0].get_value(); + if (params_.position_feedback) + { + controller_state_publisher_->msg_.rear_wheel_position = state_interfaces_[0].get_value(); + } + else + { + controller_state_publisher_->msg_.rear_wheel_velocity = state_interfaces_[0].get_value(); + } controller_state_publisher_->msg_.steer_position = state_interfaces_[1].get_value() * params_.steer_pos_multiplier; + controller_state_publisher_->msg_.linear_velocity_command = command_interfaces_[0].get_value(); + controller_state_publisher_->msg_.steering_angle_command = command_interfaces_[1].get_value(); controller_state_publisher_->unlockAndPublish(); } diff --git a/ackermann_steering_controller/src/odometry.cpp b/ackermann_steering_controller/src/odometry.cpp index cec736bd9b..1dd674da63 100644 --- a/ackermann_steering_controller/src/odometry.cpp +++ b/ackermann_steering_controller/src/odometry.cpp @@ -45,166 +45,141 @@ namespace ackermann_steering_controller { - namespace bacc = boost::accumulators; - // using namespace ackermann_steering_controller_ros2; - // ackermann_steering_controller_ros2::Params params; - - Odometry::Odometry(size_t velocity_rolling_window_size) - : timestamp_(0.0) - , x_(0.0) - , y_(0.0) - , heading_(0.0) - , linear_(0.0) - , angular_(0.0) - , wheel_separation_(0.0) - , wheel_radius_(0.0) - , rear_wheel_old_pos_(0.0) - , velocity_rolling_window_size_(0.0) - , linear_acc_(RollingWindow::window_size = velocity_rolling_window_size) - , angular_acc_(RollingWindow::window_size = velocity_rolling_window_size) - , integrate_fun_(std::bind(&Odometry::integrateExact, this, std::placeholders::_1, std::placeholders::_2)) +namespace bacc = boost::accumulators; +// using namespace ackermann_steering_controller_ros2; +// ackermann_steering_controller_ros2::Params params; + +Odometry::Odometry(size_t velocity_rolling_window_size) +: timestamp_(0.0), + x_(0.0), + y_(0.0), + heading_(0.0), + linear_(0.0), + angular_(0.0), + wheel_separation_(0.0), + wheel_radius_(0.0), + rear_wheel_old_pos_(0.0), + velocity_rolling_window_size_(0.0), + linear_acc_(RollingWindow::window_size = velocity_rolling_window_size), + angular_acc_(RollingWindow::window_size = velocity_rolling_window_size), + integrate_fun_( + std::bind(&Odometry::integrateExact, this, std::placeholders::_1, std::placeholders::_2)) - { - } +{ +} - void Odometry::init(const rclcpp::Time& time) - { - // Reset accumulators and timestamp: - resetAccumulators(); - timestamp_ = time; - } +void Odometry::init(const rclcpp::Time & time) +{ + // Reset accumulators and timestamp: + resetAccumulators(); + timestamp_ = time; +} + +// TODO(destogl): enable also velocity interface to update using velocity from the rear wheel +bool Odometry::update( + const double rear_wheel_pos, const double front_steer_pos, const double rear_wheel_vel, + const bool position_feedback, const double dt) +{ + // (right_wheel_est_vel + left_wheel_est_vel) * 0.5; + double linear = rear_wheel_vel; - // TODO(destogl): enalbe also velocity interface to update using velocity from the rear wheel - bool Odometry::update(double rear_wheel_pos, double front_steer_pos, double rear_wheel_vel, bool position_feedback, const rclcpp::Time &time) + if (position_feedback) { - if (position_feedback) - { - /// Get current wheel joint positions: - const double rear_wheel_cur_pos = rear_wheel_pos * wheel_radius_; - - /// Estimate velocity of wheels using old and current position: - //const double left_wheel_est_vel = left_wheel_cur_pos - left_wheel_old_pos_; - //const double right_wheel_est_vel = right_wheel_cur_pos - right_wheel_old_pos_; - - const double rear_wheel_est_vel = rear_wheel_cur_pos - rear_wheel_old_pos_; - - /// Update old position with current: - rear_wheel_old_pos_ = rear_wheel_cur_pos; + /// Get current wheel joint positions: + const double rear_wheel_cur_pos = rear_wheel_pos * wheel_radius_; - /// Compute linear and angular diff: - const double linear = rear_wheel_est_vel;//(right_wheel_est_vel + left_wheel_est_vel) * 0.5; - //const double angular = (right_wheel_est_vel - left_wheel_est_vel) / wheel_separation_w_; - const double angular = tan(front_steer_pos) * linear / wheel_separation_; + /// Estimate velocity of wheels using old and current position: + //const double left_wheel_est_vel = left_wheel_cur_pos - left_wheel_old_pos_; + //const double right_wheel_est_vel = right_wheel_cur_pos - right_wheel_old_pos_; - /// Integrate odometry: - integrate_fun_(linear, angular); + const double rear_wheel_est_vel = rear_wheel_cur_pos - rear_wheel_old_pos_; - /// We cannot estimate the speed with very small time intervals: - const double dt = time.seconds() - timestamp_.seconds(); - if (dt < 0.0001) - return false; // Interval too small to integrate with + /// Update old position with current: + rear_wheel_old_pos_ = rear_wheel_cur_pos; - timestamp_ = time; - - /// Estimate speeds using a rolling mean to filter them out: - linear_acc_(linear/dt); - angular_acc_(angular/dt); - - linear_ = bacc::rolling_mean(linear_acc_); - angular_ = bacc::rolling_mean(angular_acc_); - - return true; - - } else { - - /// Compute linear and angular diff: - const double linear = rear_wheel_vel;//(right_wheel_est_vel + left_wheel_est_vel) * 0.5; - //const double angular = (right_wheel_est_vel - left_wheel_est_vel) / wheel_separation_w_; - const double angular = tan(front_steer_pos) * linear / wheel_separation_; - - /// Integrate odometry: - integrate_fun_(linear, angular); - - /// We cannot estimate the speed with very small time intervals: - const double dt = time.seconds() - timestamp_.seconds(); - if (dt < 0.0001) - return false; // Interval too small to integrate with + /// Compute linear and angular diff: + double linear = rear_wheel_est_vel; //(right_wheel_est_vel + left_wheel_est_vel) * 0.5; + } - timestamp_ = time; + //const double angular = (right_wheel_est_vel - left_wheel_est_vel) / wheel_separation_w_; + const double angular = tan(front_steer_pos) * linear / wheel_separation_; - /// Estimate speeds using a rolling mean to filter them out: - linear_acc_(linear/dt); - angular_acc_(angular/dt); + /// Integrate odometry: + integrate_fun_(linear, angular); - linear_ = bacc::rolling_mean(linear_acc_); - angular_ = bacc::rolling_mean(angular_acc_); + /// We cannot estimate the speed with very small time intervals: + if (dt < 0.0001) + { + return false; // Interval too small to integrate with + } - return true; + /// Estimate speeds using a rolling mean to filter them out: + linear_acc_(linear / dt); + angular_acc_(angular / dt); + linear_ = bacc::rolling_mean(linear_acc_); + angular_ = bacc::rolling_mean(angular_acc_); - } + return true; +} - } +void Odometry::updateOpenLoop(const double linear, const double angular, const double dt) +{ + /// Save last linear and angular velocity: + linear_ = linear; + angular_ = angular; - void Odometry::updateOpenLoop(double linear, double angular, const rclcpp::Time &time) - { - /// Save last linear and angular velocity: - linear_ = linear; - angular_ = angular; - - /// Integrate odometry: - const double dt = time.seconds() - timestamp_.seconds(); - timestamp_ = time; - integrate_fun_(linear * dt, angular * dt); - } + /// Integrate odometry: + integrate_fun_(linear * dt, angular * dt); +} - void Odometry::setWheelParams(double wheel_separation, double wheel_radius) - { - wheel_separation_ = wheel_separation; - wheel_radius_ = wheel_radius; - } +void Odometry::setWheelParams(double wheel_separation, double wheel_radius) +{ + wheel_separation_ = wheel_separation; + wheel_radius_ = wheel_radius; +} - void Odometry::setVelocityRollingWindowSize(size_t velocity_rolling_window_size) - { - velocity_rolling_window_size_ = velocity_rolling_window_size; +void Odometry::setVelocityRollingWindowSize(size_t velocity_rolling_window_size) +{ + velocity_rolling_window_size_ = velocity_rolling_window_size; - resetAccumulators(); - } + resetAccumulators(); +} - void Odometry::integrateRungeKutta2(double linear, double angular) - { - const double direction = heading_ + angular * 0.5; +void Odometry::integrateRungeKutta2(double linear, double angular) +{ + const double direction = heading_ + angular * 0.5; - /// Runge-Kutta 2nd order integration: - x_ += linear * cos(direction); - y_ += linear * sin(direction); - heading_ += angular; - } + /// Runge-Kutta 2nd order integration: + x_ += linear * cos(direction); + y_ += linear * sin(direction); + heading_ += angular; +} - /** +/** * \brief Other possible integration method provided by the class * \param linear * \param angular */ - void Odometry::integrateExact(double linear, double angular) +void Odometry::integrateExact(double linear, double angular) +{ + if (fabs(angular) < 1e-6) + integrateRungeKutta2(linear, angular); + else { - if (fabs(angular) < 1e-6) - integrateRungeKutta2(linear, angular); - else - { - /// Exact integration (should solve problems when angular is zero): - const double heading_old = heading_; - const double r = linear/angular; - heading_ += angular; - x_ += r * (sin(heading_) - sin(heading_old)); - y_ += -r * (cos(heading_) - cos(heading_old)); - } + /// Exact integration (should solve problems when angular is zero): + const double heading_old = heading_; + const double r = linear / angular; + heading_ += angular; + x_ += r * (sin(heading_) - sin(heading_old)); + y_ += -r * (cos(heading_) - cos(heading_old)); } +} - void Odometry::resetAccumulators() - { - linear_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_); - angular_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_); - } +void Odometry::resetAccumulators() +{ + linear_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_); + angular_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_); +} -} // namespace ackermann_steering_controller \ No newline at end of file +} // namespace ackermann_steering_controller diff --git a/custom_messages/msg/AckermanControllerState.msg b/custom_messages/msg/AckermanControllerState.msg index 14562b8375..95c29ebf3d 100644 --- a/custom_messages/msg/AckermanControllerState.msg +++ b/custom_messages/msg/AckermanControllerState.msg @@ -1,4 +1,7 @@ std_msgs/Header header nav_msgs/Odometry odom -float64 wheel_position +float64 rear_wheel_position +float64 rear_wheel_velocity float64 steer_position +float64 linear_velocity_command +float64 steering_angle_command From 4d6dced4b3433eb39dd78f120ec83d7588420609 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Thu, 24 Nov 2022 18:35:29 +0100 Subject: [PATCH 024/186] Make controller to compile again. --- .../ackermann_steering_controller.hpp | 2 -- .../src/ackermann_steering_controller.cpp | 5 ++--- 2 files changed, 2 insertions(+), 5 deletions(-) diff --git a/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp b/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp index b0fbaf8bcf..b9da94cfe8 100644 --- a/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp +++ b/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp @@ -96,8 +96,6 @@ class AckermannSteeringController : public controller_interface::ChainableContro std::shared_ptr param_listener_; ackermann_steering_controller::Params params_; - const char * feedback_type() const; - // Command subscribers and Controller State publisher rclcpp::Subscription::SharedPtr ref_subscriber_ = nullptr; realtime_tools::RealtimeBuffer> input_ref_; diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.cpp b/ackermann_steering_controller/src/ackermann_steering_controller.cpp index 78f0a6a0b2..8966ff7b7d 100644 --- a/ackermann_steering_controller/src/ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/src/ackermann_steering_controller.cpp @@ -253,7 +253,7 @@ AckermannSteeringController::state_interface_configuration() const state_interfaces_config.names.reserve(NR_STATE_ITFS); const auto rear_wheel_feedback = params_.position_feedback ? hardware_interface::HW_IF_POSITION : hardware_interface::HW_IF_VELOCITY; - state_interfaces_config.names.push_back(params_.rear_wheel_name + "/" + feedback_type()); + state_interfaces_config.names.push_back(params_.rear_wheel_name + "/" + rear_wheel_feedback); state_interfaces_config.names.push_back( params_.front_steer_name + "/" + hardware_interface::HW_IF_POSITION); @@ -370,7 +370,7 @@ controller_interface::return_type AckermannSteeringController::update_and_write_ last_linear_velocity_ = reference_interfaces_[0]; last_angular_velocity_ = reference_interfaces_[1]; - //previous_publish_timestamp_ = time; + // previous_publish_timestamp_ = time; // omega = linear_vel / radius command_interfaces_[0].set_value(last_linear_velocity_ / params_.wheel_radius); @@ -435,7 +435,6 @@ controller_interface::return_type AckermannSteeringController::update_and_write_ controller_state_publisher_->unlockAndPublish(); } - //publish_state(state_current_, state_desired); return controller_interface::return_type::OK; } From 4ab177b08aea924389a407877ed11f854f9bfc5f Mon Sep 17 00:00:00 2001 From: GiridharBukka Date: Mon, 28 Nov 2022 08:57:31 +0100 Subject: [PATCH 025/186] added unstamped twist topic -clean commit -functional changes --- .pre-commit-config.yaml | 90 +++++++++--------- ackermann_steering_controller/CMakeLists.txt | 18 ++-- .../ackermann_steering_controller.hpp | 3 + .../src/ackermann_steering_controller.cpp | 95 +++++++++++++++---- .../src/ackermann_steering_controller.yaml | 45 ++++----- 5 files changed, 155 insertions(+), 96 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 58fe46dd90..e77e9e84dc 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -44,12 +44,12 @@ repos: - id: black args: ["--line-length=99"] - # PEP 257 - - repo: https://github.com/FalconSocial/pre-commit-mirrors-pep257 - rev: v0.3.3 - hooks: - - id: pep257 - args: ["--ignore=D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D404"] + # # PEP 257 + # - repo: https://github.com/FalconSocial/pre-commit-mirrors-pep257 + # rev: v0.3.3 + # hooks: + # - id: pep257 + # args: ["--ignore=D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D404"] - repo: https://github.com/pycqa/flake8 rev: 5.0.4 @@ -74,48 +74,48 @@ repos: #- id: cppcheck #args: ['--error-exitcode=1', '-f', '--inline-suppr', '-q', '-rp', '--suppress=internalAstError', '--suppress=unknownMacro', '--verbose'] - - repo: local - hooks: - - id: ament_cppcheck - name: ament_cppcheck - description: Static code analysis of C/C++ files. - stages: [commit] - entry: ament_cppcheck - language: system - files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$ + # - repo: local + # hooks: + # - id: ament_cppcheck + # name: ament_cppcheck + # description: Static code analysis of C/C++ files. + # stages: [commit] + # entry: ament_cppcheck + # language: system + # files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$ - # Maybe use https://github.com/cpplint/cpplint instead - - repo: local - hooks: - - id: ament_cpplint - name: ament_cpplint - description: Static code analysis of C/C++ files. - stages: [commit] - entry: ament_cpplint - language: system - files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$ - args: ["--linelength=100", "--filter=-whitespace/newline"] + # # Maybe use https://github.com/cpplint/cpplint instead + # - repo: local + # hooks: + # - id: ament_cpplint + # name: ament_cpplint + # description: Static code analysis of C/C++ files. + # stages: [commit] + # entry: ament_cpplint + # language: system + # files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$ + # args: ["--linelength=100", "--filter=-whitespace/newline"] - # Cmake hooks - - repo: local - hooks: - - id: ament_lint_cmake - name: ament_lint_cmake - description: Check format of CMakeLists.txt files. - stages: [commit] - entry: ament_lint_cmake - language: system - files: CMakeLists\.txt$ + # # Cmake hooks + # - repo: local + # hooks: + # - id: ament_lint_cmake + # name: ament_lint_cmake + # description: Check format of CMakeLists.txt files. + # stages: [commit] + # entry: ament_lint_cmake + # language: system + # files: CMakeLists\.txt$ - # Copyright - - repo: local - hooks: - - id: ament_copyright - name: ament_copyright - description: Check if copyright notice is available in all files. - stages: [commit] - entry: ament_copyright - language: system + # # Copyright + # - repo: local + # hooks: + # - id: ament_copyright + # name: ament_copyright + # description: Check if copyright notice is available in all files. + # stages: [commit] + # entry: ament_copyright + # language: system # Docs - RestructuredText hooks - repo: https://github.com/PyCQA/doc8 diff --git a/ackermann_steering_controller/CMakeLists.txt b/ackermann_steering_controller/CMakeLists.txt index 75b2f328a5..48cd88df6f 100644 --- a/ackermann_steering_controller/CMakeLists.txt +++ b/ackermann_steering_controller/CMakeLists.txt @@ -99,14 +99,14 @@ if(BUILD_TESTING) # ros2_control_test_assets # ) - add_rostest_with_parameters_gmock(test_ackermann_steering_controller test/test_ackermann_steering_controller.cpp ${CMAKE_CURRENT_SOURCE_DIR}/test/ackermann_steering_controller_params.yaml) - target_include_directories(test_ackermann_steering_controller PRIVATE include) - target_link_libraries(test_ackermann_steering_controller ackermann_steering_controller) - ament_target_dependencies( - test_ackermann_steering_controller - controller_interface - hardware_interface - ) + # add_rostest_with_parameters_gmock(test_ackermann_steering_controller test/test_ackermann_steering_controller.cpp ${CMAKE_CURRENT_SOURCE_DIR}/test/ackermann_steering_controller_params.yaml) + # target_include_directories(test_ackermann_steering_controller PRIVATE include) + # target_link_libraries(test_ackermann_steering_controller ackermann_steering_controller) + # ament_target_dependencies( + # test_ackermann_steering_controller + # controller_interface + # hardware_interface + # ) # add_rostest_with_parameters_gmock(test_ackermann_steering_controller_preceeding test/test_ackermann_steering_controller_preceeding.cpp ${CMAKE_CURRENT_SOURCE_DIR}/test/ackermann_steering_controller_preceeding_params.yaml) # target_include_directories(test_ackermann_steering_controller_preceeding PRIVATE include) @@ -123,7 +123,7 @@ ament_export_include_directories( ) ament_export_dependencies( - + ) ament_export_libraries( ackermann_steering_controller diff --git a/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp b/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp index b9da94cfe8..e0fb32e435 100644 --- a/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp +++ b/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp @@ -98,7 +98,9 @@ class AckermannSteeringController : public controller_interface::ChainableContro // Command subscribers and Controller State publisher rclcpp::Subscription::SharedPtr ref_subscriber_ = nullptr; + rclcpp::Subscription::SharedPtr ref_subscriber_unstamped_ = nullptr; realtime_tools::RealtimeBuffer> input_ref_; + realtime_tools::RealtimeBuffer> input_ref_unstamped_; rclcpp::Duration ref_timeout_ = rclcpp::Duration::from_seconds(0.0); // 0ms using ControllerStatePublisherOdom = realtime_tools::RealtimePublisher; @@ -153,6 +155,7 @@ class AckermannSteeringController : public controller_interface::ChainableContro // callback for topic interface ACKERMANN_STEERING_CONTROLLER__VISIBILITY_LOCAL void reference_callback(const std::shared_ptr msg); + void reference_callback_unstamped(const std::shared_ptr msg); /// Frame to use for the robot base: std::string base_frame_id_; diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.cpp b/ackermann_steering_controller/src/ackermann_steering_controller.cpp index 8966ff7b7d..1c590933dd 100644 --- a/ackermann_steering_controller/src/ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/src/ackermann_steering_controller.cpp @@ -60,6 +60,19 @@ void reset_controller_reference_msg( msg->twist.angular.z = std::numeric_limits::quiet_NaN(); } +// called from RT control loop +void reset_controller_reference_msg_unstamped( + const std::shared_ptr & msg, + const std::shared_ptr & node) +{ + msg->linear.x = std::numeric_limits::quiet_NaN(); + msg->linear.y = std::numeric_limits::quiet_NaN(); + msg->linear.z = std::numeric_limits::quiet_NaN(); + msg->angular.x = std::numeric_limits::quiet_NaN(); + msg->angular.y = std::numeric_limits::quiet_NaN(); + msg->angular.z = std::numeric_limits::quiet_NaN(); +} + } // namespace namespace ackermann_steering_controller @@ -101,13 +114,27 @@ controller_interface::CallbackReturn AckermannSteeringController::on_configure( // Reference Subscriber ref_timeout_ = rclcpp::Duration::from_seconds(params_.reference_timeout); - ref_subscriber_ = get_node()->create_subscription( - "~/reference", subscribers_qos, - std::bind(&AckermannSteeringController::reference_callback, this, std::placeholders::_1)); + if (params_.use_stamped_vel) + { + ref_subscriber_ = get_node()->create_subscription( + "~/reference", subscribers_qos, + std::bind(&AckermannSteeringController::reference_callback, this, std::placeholders::_1)); - std::shared_ptr msg = std::make_shared(); - reset_controller_reference_msg(msg, get_node()); - input_ref_.writeFromNonRT(msg); + std::shared_ptr msg = std::make_shared(); + reset_controller_reference_msg(msg, get_node()); + input_ref_.writeFromNonRT(msg); + } + else + { + ref_subscriber_unstamped_ = get_node()->create_subscription( + "~/reference_unstamped", subscribers_qos, + std::bind( + &AckermannSteeringController::reference_callback_unstamped, this, std::placeholders::_1)); + + std::shared_ptr msg = std::make_shared(); + reset_controller_reference_msg_unstamped(msg, get_node()); + input_ref_unstamped_.writeFromNonRT(msg); + } try { @@ -229,6 +256,12 @@ void AckermannSteeringController::reference_callback( } } +void AckermannSteeringController::reference_callback_unstamped( + const std::shared_ptr msg) +{ + input_ref_unstamped_.writeFromNonRT(msg); +} + controller_interface::InterfaceConfiguration AckermannSteeringController::command_interface_configuration() const { @@ -288,8 +321,16 @@ bool AckermannSteeringController::on_set_chained_mode(bool chained_mode) controller_interface::CallbackReturn AckermannSteeringController::on_activate( const rclcpp_lifecycle::State & /*previous_state*/) { - // Set default value in command - reset_controller_reference_msg(*(input_ref_.readFromRT()), get_node()); + if (params_.use_stamped_vel) + { + // Set default value in command + reset_controller_reference_msg(*(input_ref_.readFromRT()), get_node()); + } + else + { + // Set default value in command + reset_controller_reference_msg_unstamped(*(input_ref_unstamped_.readFromRT()), get_node()); + } return controller_interface::CallbackReturn::SUCCESS; } @@ -308,25 +349,39 @@ controller_interface::CallbackReturn AckermannSteeringController::on_deactivate( controller_interface::return_type AckermannSteeringController::update_reference_from_subscribers() { - auto current_ref = *(input_ref_.readFromRT()); - const auto age_of_last_command = get_node()->now() - (current_ref)->header.stamp; - - // TODO(anyone): depending on number of interfaces, use definitions, e.g., `CMD_MY_ITFS`, - // instead of a loop - // send message only if there is no timeout - if (age_of_last_command <= ref_timeout_ || ref_timeout_ == rclcpp::Duration::from_seconds(0)) + if (params_.use_stamped_vel) { - if (!std::isnan(current_ref->twist.linear.x) && !std::isnan(current_ref->twist.angular.z)) + auto current_ref = *(input_ref_.readFromRT()); + const auto age_of_last_command = get_node()->now() - (current_ref)->header.stamp; + + // TODO(anyone): depending on number of interfaces, use definitions, e.g., `CMD_MY_ITFS`, + // instead of a loop + // send message only if there is no timeout + if (age_of_last_command <= ref_timeout_ || ref_timeout_ == rclcpp::Duration::from_seconds(0)) { - reference_interfaces_[0] = current_ref->twist.linear.x; - reference_interfaces_[1] = current_ref->twist.angular.z; + if (!std::isnan(current_ref->twist.linear.x) && !std::isnan(current_ref->twist.angular.z)) + { + reference_interfaces_[0] = current_ref->twist.linear.x; + reference_interfaces_[1] = current_ref->twist.angular.z; + } + } + else + { + reference_interfaces_[0] = std::numeric_limits::quiet_NaN(); + reference_interfaces_[1] = std::numeric_limits::quiet_NaN(); } } else { - reference_interfaces_[0] = std::numeric_limits::quiet_NaN(); - reference_interfaces_[1] = std::numeric_limits::quiet_NaN(); + auto current_ref = *(input_ref_unstamped_.readFromRT()); + + if (!std::isnan(current_ref->linear.x) && !std::isnan(current_ref->angular.z)) + { + reference_interfaces_[0] = current_ref->linear.x; + reference_interfaces_[1] = current_ref->angular.z; + } } + return controller_interface::return_type::OK; } diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.yaml b/ackermann_steering_controller/src/ackermann_steering_controller.yaml index c9a2d9b663..8a4ee8c0b2 100644 --- a/ackermann_steering_controller/src/ackermann_steering_controller.yaml +++ b/ackermann_steering_controller/src/ackermann_steering_controller.yaml @@ -13,7 +13,7 @@ ackermann_steering_controller: default_value: "rear_wheel_joint", description: "Name of the rear wheel.", read_only: true, - + } front_steer_name: { @@ -21,7 +21,7 @@ ackermann_steering_controller: default_value: "front_steer_joint", description: "Name of the front steer wheel.", read_only: true, - + } open_loop: { @@ -29,7 +29,7 @@ ackermann_steering_controller: default_value: false, description: "bool parameter decides if open oop or not (feedback).", read_only: false, - + } wheel_separation_multiplier: { @@ -37,7 +37,7 @@ ackermann_steering_controller: default_value: 1.0, description: "Wheel separation height will be multiplied by value of the wheel_separation_h_multiplier.", read_only: false, - + } wheel_separation: { @@ -45,7 +45,7 @@ ackermann_steering_controller: default_value: 0.0, description: "Wheel separation.", read_only: false, - + } wheel_radius: { @@ -53,7 +53,7 @@ ackermann_steering_controller: default_value: 0.0, description: "Wheel radius.", read_only: false, - + } wheel_radius_multiplier: { @@ -61,7 +61,7 @@ ackermann_steering_controller: default_value: 1.0, description: "Wheel radius will be multiplied by value of wheel_radius_multiplier.", read_only: false, - + } steer_pos_multiplier: { @@ -69,7 +69,7 @@ ackermann_steering_controller: default_value: 1.0, description: "Steer pos will be multiplied by value of steer_pos_multiplier.", read_only: false, - + } velocity_rolling_window_size: { @@ -77,7 +77,7 @@ ackermann_steering_controller: default_value: 10, description: "The number of velocity samples to average together to compute the odometry twist.linear.x and twist.angular.z velocities.", read_only: false, - + } allow_multiple_cmd_vel_publishers: { @@ -85,7 +85,7 @@ ackermann_steering_controller: default_value: true, description: "Allow multiple cmd_vel publishers is enabled or disabled?.", read_only: false, - + } base_frame_id: { @@ -93,7 +93,7 @@ ackermann_steering_controller: default_value: "base_link", description: "Base frame_id set to value of base_frame_id.", read_only: false, - + } odom_frame_id: { @@ -101,7 +101,7 @@ ackermann_steering_controller: default_value: "odom", description: "Odometry frame_id set to value of odom_frame_id.", read_only: false, - + } @@ -110,7 +110,7 @@ ackermann_steering_controller: default_value: true, description: "Publishing to tf is enabled or disabled ?.", read_only: false, - + } twist_covariance_diagonal: { @@ -118,7 +118,7 @@ ackermann_steering_controller: default_value: [0, 7, 14, 21, 28, 35], description: "diagonal values of twist covariance matrix.", read_only: false, - + } pose_covariance_diagonal: { @@ -126,7 +126,7 @@ ackermann_steering_controller: default_value: [0, 7, 14, 21, 28, 35], description: "diagonal values of pose covariance matrix.", read_only: false, - + } position_feedback: { @@ -135,13 +135,14 @@ ackermann_steering_controller: description: "choice of feedbacktype, if position_feedback is false then HW_IF_VELOCITY is taken as interface type, if position_feedback is true then HW_IF_POSITION is taken as interface type", read_only: false, - - } - - - - - + } + use_stamped_vel: { + type: bool, + default_value: true, + description: "choice of vel type, if use_stamped_vel is false then geometry_msgs::msg::Twist is taken as vel msg type, if + use_stamped_vel is true then geometry_msgs::msg::TwistStamped is taken as vel msg type", + read_only: false, + } From 8335f9544f1158e541075354c336598c3d33cef8 Mon Sep 17 00:00:00 2001 From: GiridharBukka Date: Mon, 28 Nov 2022 14:36:41 +0100 Subject: [PATCH 026/186] using only one realtime buffer handle --- .../ackermann_steering_controller.hpp | 1 - .../src/ackermann_steering_controller.cpp | 101 ++++++++---------- 2 files changed, 44 insertions(+), 58 deletions(-) diff --git a/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp b/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp index e0fb32e435..794e1ff481 100644 --- a/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp +++ b/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp @@ -100,7 +100,6 @@ class AckermannSteeringController : public controller_interface::ChainableContro rclcpp::Subscription::SharedPtr ref_subscriber_ = nullptr; rclcpp::Subscription::SharedPtr ref_subscriber_unstamped_ = nullptr; realtime_tools::RealtimeBuffer> input_ref_; - realtime_tools::RealtimeBuffer> input_ref_unstamped_; rclcpp::Duration ref_timeout_ = rclcpp::Duration::from_seconds(0.0); // 0ms using ControllerStatePublisherOdom = realtime_tools::RealtimePublisher; diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.cpp b/ackermann_steering_controller/src/ackermann_steering_controller.cpp index 1c590933dd..f2f443449b 100644 --- a/ackermann_steering_controller/src/ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/src/ackermann_steering_controller.cpp @@ -60,19 +60,6 @@ void reset_controller_reference_msg( msg->twist.angular.z = std::numeric_limits::quiet_NaN(); } -// called from RT control loop -void reset_controller_reference_msg_unstamped( - const std::shared_ptr & msg, - const std::shared_ptr & node) -{ - msg->linear.x = std::numeric_limits::quiet_NaN(); - msg->linear.y = std::numeric_limits::quiet_NaN(); - msg->linear.z = std::numeric_limits::quiet_NaN(); - msg->angular.x = std::numeric_limits::quiet_NaN(); - msg->angular.y = std::numeric_limits::quiet_NaN(); - msg->angular.z = std::numeric_limits::quiet_NaN(); -} - } // namespace namespace ackermann_steering_controller @@ -119,10 +106,6 @@ controller_interface::CallbackReturn AckermannSteeringController::on_configure( ref_subscriber_ = get_node()->create_subscription( "~/reference", subscribers_qos, std::bind(&AckermannSteeringController::reference_callback, this, std::placeholders::_1)); - - std::shared_ptr msg = std::make_shared(); - reset_controller_reference_msg(msg, get_node()); - input_ref_.writeFromNonRT(msg); } else { @@ -130,12 +113,12 @@ controller_interface::CallbackReturn AckermannSteeringController::on_configure( "~/reference_unstamped", subscribers_qos, std::bind( &AckermannSteeringController::reference_callback_unstamped, this, std::placeholders::_1)); - - std::shared_ptr msg = std::make_shared(); - reset_controller_reference_msg_unstamped(msg, get_node()); - input_ref_unstamped_.writeFromNonRT(msg); } + std::shared_ptr msg = std::make_shared(); + reset_controller_reference_msg(msg, get_node()); + input_ref_.writeFromNonRT(msg); + try { // Odom state publisher @@ -259,7 +242,32 @@ void AckermannSteeringController::reference_callback( void AckermannSteeringController::reference_callback_unstamped( const std::shared_ptr msg) { - input_ref_unstamped_.writeFromNonRT(msg); + auto twist_stamped = *(input_ref_.readFromNonRT()); + twist_stamped->header.stamp = get_node()->now(); + // if no timestamp provided use current time for command timestamp + if (twist_stamped->header.stamp.sec == 0 && twist_stamped->header.stamp.nanosec == 0u) + { + RCLCPP_WARN( + get_node()->get_logger(), + "Timestamp in header is missing, using current time as command timestamp."); + twist_stamped->header.stamp = get_node()->now(); + } + + const auto age_of_last_command = get_node()->now() - twist_stamped->header.stamp; + + if (ref_timeout_ == rclcpp::Duration::from_seconds(0) || age_of_last_command <= ref_timeout_) + { + twist_stamped->twist = *msg; + } + else + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Received message has timestamp %.10f older for %.10f which is more then allowed timeout " + "(%.4f).", + rclcpp::Time(twist_stamped->header.stamp).seconds(), age_of_last_command.seconds(), + ref_timeout_.seconds()); + } } controller_interface::InterfaceConfiguration @@ -321,16 +329,8 @@ bool AckermannSteeringController::on_set_chained_mode(bool chained_mode) controller_interface::CallbackReturn AckermannSteeringController::on_activate( const rclcpp_lifecycle::State & /*previous_state*/) { - if (params_.use_stamped_vel) - { - // Set default value in command - reset_controller_reference_msg(*(input_ref_.readFromRT()), get_node()); - } - else - { - // Set default value in command - reset_controller_reference_msg_unstamped(*(input_ref_unstamped_.readFromRT()), get_node()); - } + // Set default value in command + reset_controller_reference_msg(*(input_ref_.readFromRT()), get_node()); return controller_interface::CallbackReturn::SUCCESS; } @@ -349,37 +349,24 @@ controller_interface::CallbackReturn AckermannSteeringController::on_deactivate( controller_interface::return_type AckermannSteeringController::update_reference_from_subscribers() { - if (params_.use_stamped_vel) - { - auto current_ref = *(input_ref_.readFromRT()); - const auto age_of_last_command = get_node()->now() - (current_ref)->header.stamp; + auto current_ref = *(input_ref_.readFromRT()); + const auto age_of_last_command = get_node()->now() - (current_ref)->header.stamp; - // TODO(anyone): depending on number of interfaces, use definitions, e.g., `CMD_MY_ITFS`, - // instead of a loop - // send message only if there is no timeout - if (age_of_last_command <= ref_timeout_ || ref_timeout_ == rclcpp::Duration::from_seconds(0)) - { - if (!std::isnan(current_ref->twist.linear.x) && !std::isnan(current_ref->twist.angular.z)) - { - reference_interfaces_[0] = current_ref->twist.linear.x; - reference_interfaces_[1] = current_ref->twist.angular.z; - } - } - else + // TODO(anyone): depending on number of interfaces, use definitions, e.g., `CMD_MY_ITFS`, + // instead of a loop + // send message only if there is no timeout + if (age_of_last_command <= ref_timeout_ || ref_timeout_ == rclcpp::Duration::from_seconds(0)) + { + if (!std::isnan(current_ref->twist.linear.x) && !std::isnan(current_ref->twist.angular.z)) { - reference_interfaces_[0] = std::numeric_limits::quiet_NaN(); - reference_interfaces_[1] = std::numeric_limits::quiet_NaN(); + reference_interfaces_[0] = current_ref->twist.linear.x; + reference_interfaces_[1] = current_ref->twist.angular.z; } } else { - auto current_ref = *(input_ref_unstamped_.readFromRT()); - - if (!std::isnan(current_ref->linear.x) && !std::isnan(current_ref->angular.z)) - { - reference_interfaces_[0] = current_ref->linear.x; - reference_interfaces_[1] = current_ref->angular.z; - } + reference_interfaces_[0] = std::numeric_limits::quiet_NaN(); + reference_interfaces_[1] = std::numeric_limits::quiet_NaN(); } return controller_interface::return_type::OK; From dc4a59003a910a7f6ddaaa76dfd1b6e238ebfe20 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Tue, 29 Nov 2022 11:33:15 +0100 Subject: [PATCH 027/186] Debuging ackermann controller. --- .../odometry.hpp | 17 +++-- .../src/ackermann_steering_controller.cpp | 6 +- .../src/odometry.cpp | 68 +++++++++++++------ 3 files changed, 61 insertions(+), 30 deletions(-) diff --git a/ackermann_steering_controller/include/ackermann_steering_controller/odometry.hpp b/ackermann_steering_controller/include/ackermann_steering_controller/odometry.hpp index e5cff66bd7..cf1cb64190 100644 --- a/ackermann_steering_controller/include/ackermann_steering_controller/odometry.hpp +++ b/ackermann_steering_controller/include/ackermann_steering_controller/odometry.hpp @@ -85,12 +85,21 @@ class Odometry * \brief Updates the odometry class with latest wheels position * \param rear_wheel_pos Rear wheel position [rad] * \param front_steer_pos Front Steer position [rad] - * \param time Current time + * \param dt time difference to last call * \return true if the odometry is actually updated */ - bool update( - const double rear_wheel_pos, const double front_steer_pos, const double rear_wheel_vel, - const bool position_feedback, const double dt); + bool updateFromPosition( + const double rear_wheel_pos, const double front_steer_pos, const double dt); + + /** + * \brief Updates the odometry class with latest wheels position + * \param rear_wheel_vel Rear wheel velocity [rad/s] + * \param front_steer_pos Front Steer position [rad] + * \param dt time difference to last call + * \return true if the odometry is actually updated + */ + bool updateFromVelocity( + const double rear_wheel_vel, const double front_steer_pos, const double dt); /** * \brief Updates the odometry class with latest velocity command diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.cpp b/ackermann_steering_controller/src/ackermann_steering_controller.cpp index f2f443449b..cc0aa4fac7 100644 --- a/ackermann_steering_controller/src/ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/src/ackermann_steering_controller.cpp @@ -389,14 +389,12 @@ controller_interface::return_type AckermannSteeringController::update_and_write_ if (params_.position_feedback) { // Estimate linear and angular velocity using joint information - odometry_.update( - rear_wheel_value, steer_position, 0.0, params_.position_feedback, period.seconds()); + odometry_.updateFromPosition(rear_wheel_value, steer_position, period.seconds()); } else { // Estimate linear and angular velocity using joint information - odometry_.update( - 0.0, steer_position, rear_wheel_value, params_.position_feedback, period.seconds()); + odometry_.updateFromVelocity(rear_wheel_value, steer_position, period.seconds()); } } } diff --git a/ackermann_steering_controller/src/odometry.cpp b/ackermann_steering_controller/src/odometry.cpp index 1dd674da63..7435af8502 100644 --- a/ackermann_steering_controller/src/odometry.cpp +++ b/ackermann_steering_controller/src/odometry.cpp @@ -40,8 +40,10 @@ * Author: Masaru Morita */ -#include +#include "ackermann_steering_controller/odometry.hpp" + #include +#include namespace ackermann_steering_controller { @@ -76,36 +78,58 @@ void Odometry::init(const rclcpp::Time & time) } // TODO(destogl): enable also velocity interface to update using velocity from the rear wheel -bool Odometry::update( - const double rear_wheel_pos, const double front_steer_pos, const double rear_wheel_vel, - const bool position_feedback, const double dt) +bool Odometry::updateFromPosition( + const double rear_wheel_pos, const double front_steer_pos, const double dt) { - // (right_wheel_est_vel + left_wheel_est_vel) * 0.5; - double linear = rear_wheel_vel; + /// Get current wheel joint positions: + const double rear_wheel_cur_pos = rear_wheel_pos * wheel_radius_; - if (position_feedback) - { - /// Get current wheel joint positions: - const double rear_wheel_cur_pos = rear_wheel_pos * wheel_radius_; + /// Estimate velocity of wheels using old and current position: + //const double left_wheel_est_vel = left_wheel_cur_pos - left_wheel_old_pos_; + //const double right_wheel_est_vel = right_wheel_cur_pos - right_wheel_old_pos_; - /// Estimate velocity of wheels using old and current position: - //const double left_wheel_est_vel = left_wheel_cur_pos - left_wheel_old_pos_; - //const double right_wheel_est_vel = right_wheel_cur_pos - right_wheel_old_pos_; + const double rear_wheel_est_pos_diff = rear_wheel_cur_pos - rear_wheel_old_pos_; - const double rear_wheel_est_vel = rear_wheel_cur_pos - rear_wheel_old_pos_; + /// Update old position with current: + rear_wheel_old_pos_ = rear_wheel_cur_pos; + + /// Compute linear and angular diff: + const double linear_velocity = + rear_wheel_est_pos_diff / dt; //(right_wheel_est_vel + left_wheel_est_vel) * 0.5; + + //const double angular = (right_wheel_est_vel - left_wheel_est_vel) / wheel_separation_w_; + const double angular = tan(front_steer_pos) * linear_velocity / wheel_separation_; - /// Update old position with current: - rear_wheel_old_pos_ = rear_wheel_cur_pos; + /// Integrate odometry: + integrate_fun_(linear_velocity * dt, angular); - /// Compute linear and angular diff: - double linear = rear_wheel_est_vel; //(right_wheel_est_vel + left_wheel_est_vel) * 0.5; + /// We cannot estimate the speed with very small time intervals: + if (dt < 0.0001) + { + return false; // Interval too small to integrate with } + /// Estimate speeds using a rolling mean to filter them out: + linear_acc_(linear_velocity); + angular_acc_(angular / dt); + + linear_ = bacc::rolling_mean(linear_acc_); + angular_ = bacc::rolling_mean(angular_acc_); + + return true; +} + +bool Odometry::updateFromVelocity( + const double rear_wheel_vel, const double front_steer_pos, const double dt) +{ + // (right_wheel_est_vel + left_wheel_est_vel) * 0.5; + double linear_velocity = rear_wheel_vel * wheel_radius_; + //const double angular = (right_wheel_est_vel - left_wheel_est_vel) / wheel_separation_w_; - const double angular = tan(front_steer_pos) * linear / wheel_separation_; + const double angular = tan(front_steer_pos) * linear_velocity / wheel_separation_; /// Integrate odometry: - integrate_fun_(linear, angular); + integrate_fun_(linear_velocity * dt, angular * dt); /// We cannot estimate the speed with very small time intervals: if (dt < 0.0001) @@ -114,8 +138,8 @@ bool Odometry::update( } /// Estimate speeds using a rolling mean to filter them out: - linear_acc_(linear / dt); - angular_acc_(angular / dt); + linear_acc_(linear_velocity); + angular_acc_(angular); linear_ = bacc::rolling_mean(linear_acc_); angular_ = bacc::rolling_mean(angular_acc_); From 0c54190831258b9c65f2c41cf50129630b1bee75 Mon Sep 17 00:00:00 2001 From: GiridharBukka Date: Tue, 29 Nov 2022 18:12:27 +0100 Subject: [PATCH 028/186] updated tests to work with twist and twist_stamped cases --- .../test_ackermann_steering_controller.cpp | 220 ++++++++++------- .../test_ackermann_steering_controller.hpp | 223 +++++++++++------- 2 files changed, 268 insertions(+), 175 deletions(-) diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp index eb2b3b0744..da93778310 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp @@ -37,7 +37,7 @@ TEST_F(AckermannSteeringControllerTest, all_parameters_set_configure_success) // ASSERT_TRUE(controller_->params_.joints.empty()); // ASSERT_TRUE(controller_->params_.state_joints.empty()); // ASSERT_TRUE(controller_->params_.interface_name.empty()); - ASSERT_EQ(controller_->params_.reference_timeout, 0.0); + ASSERT_EQ(controller_->params_.reference_timeout, 1); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); @@ -195,7 +195,14 @@ TEST_F(AckermannSteeringControllerTest, receive_message_and_publish_updated_stat ASSERT_EQ(msg.pose.pose.position.z, 0); - publish_commands(controller_->get_node()->now()); + if (controller_->params_.use_stamped_vel) + { + publish_commands(controller_->get_node()->now()); + } + else + { + publish_commands_unstamped(); + } ASSERT_TRUE(controller_->wait_for_commands(executor)); ASSERT_EQ( @@ -211,7 +218,7 @@ TEST_F(AckermannSteeringControllerTest, receive_message_and_publish_updated_stat ASSERT_EQ(msg.pose.pose.position.z, 0); } -TEST_F(AckermannSteeringControllerTest, test_message_timeout) +TEST_F(AckermannSteeringControllerTest, test_sending_too_old_message) { SetUpController(); rclcpp::executors::MultiThreadedExecutor executor; @@ -225,20 +232,33 @@ TEST_F(AckermannSteeringControllerTest, test_message_timeout) auto old_timestamp = (*reference)->header.stamp; EXPECT_TRUE(std::isnan((*reference)->twist.linear.x)); EXPECT_TRUE(std::isnan((*reference)->twist.angular.z)); - publish_commands(controller_->get_node()->now() - controller_->ref_timeout_ - - rclcpp::Duration::from_seconds(0.1)); - ASSERT_TRUE(controller_->wait_for_commands(executor)); - ASSERT_EQ(old_timestamp, - (*(controller_->input_ref_.readFromNonRT()))->header.stamp); - EXPECT_TRUE(std::isnan((*reference)->twist.linear.x)); - EXPECT_TRUE(std::isnan((*reference)->twist.angular.z)); + if (controller_->params_.use_stamped_vel) + { + publish_commands( + controller_->get_node()->now() - controller_->ref_timeout_ - + rclcpp::Duration::from_seconds(0.1)); + ASSERT_TRUE(controller_->wait_for_commands(executor)); + ASSERT_EQ(old_timestamp, (*(controller_->input_ref_.readFromNonRT()))->header.stamp); + EXPECT_TRUE(std::isnan((*reference)->twist.linear.x)); + EXPECT_TRUE(std::isnan((*reference)->twist.angular.z)); + } + else + { + publish_commands_unstamped(); + ASSERT_TRUE(controller_->wait_for_commands(executor)); + ASSERT_NE(old_timestamp, (*(controller_->input_ref_.readFromNonRT()))->header.stamp); + EXPECT_EQ((*reference)->twist.linear.x, 0.45); + EXPECT_EQ((*reference)->twist.angular.z, 0.0); + } + // EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x, // 0.45); // EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z, // 0.0); } -TEST_F(AckermannSteeringControllerTest, test_time_stamp_zero) { +TEST_F(AckermannSteeringControllerTest, test_time_stamp_zero) +{ SetUpController(); rclcpp::executors::MultiThreadedExecutor executor; executor.add_node(controller_->get_node()->get_node_base_interface()); @@ -251,14 +271,18 @@ TEST_F(AckermannSteeringControllerTest, test_time_stamp_zero) { auto old_timestamp = (*reference)->header.stamp; EXPECT_TRUE(std::isnan((*reference)->twist.linear.x)); EXPECT_TRUE(std::isnan((*reference)->twist.angular.z)); - publish_commands(rclcpp::Time(0)); + if (controller_->params_.use_stamped_vel) + { + publish_commands(rclcpp::Time(0)); + } + else + { + publish_commands_unstamped(); + } ASSERT_TRUE(controller_->wait_for_commands(executor)); - ASSERT_EQ(old_timestamp.sec, - (*(controller_->input_ref_.readFromNonRT()))->header.stamp.sec); - EXPECT_FALSE( - std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x)); - EXPECT_FALSE(std::isnan( - (*(controller_->input_ref_.readFromNonRT()))->twist.angular.z)); + ASSERT_EQ(old_timestamp.sec, (*(controller_->input_ref_.readFromNonRT()))->header.stamp.sec); + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x)); + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z)); EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x, 0.45); EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z, 0.0); } @@ -276,7 +300,14 @@ TEST_F(AckermannSteeringControllerTest, test_message_accepted) auto reference = controller_->input_ref_.readFromNonRT(); EXPECT_TRUE(std::isnan((*reference)->twist.linear.x)); EXPECT_TRUE(std::isnan((*reference)->twist.angular.z)); - publish_commands(controller_->get_node()->now()); + if (controller_->params_.use_stamped_vel) + { + publish_commands(controller_->get_node()->now()); + } + else + { + publish_commands_unstamped(); + } ASSERT_TRUE(controller_->wait_for_commands(executor)); EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x)); EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z)); @@ -284,7 +315,8 @@ TEST_F(AckermannSteeringControllerTest, test_message_accepted) EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z, 0.0); } -TEST_F(AckermannSteeringControllerTest, test_update_logic_chainable) { +TEST_F(AckermannSteeringControllerTest, test_update_logic_chainable) +{ // 1. age>ref_timeout 2. age msg = - std::make_shared(); + std::shared_ptr msg = std::make_shared(); - msg->header.stamp = controller_->get_node()->now() - - controller_->ref_timeout_ - + msg->header.stamp = controller_->get_node()->now() - controller_->ref_timeout_ - rclcpp::Duration::from_seconds(0.1); msg->twist.linear.x = TEST_LINEAR_VELOCITY_X; msg->twist.linear.y = std::numeric_limits::quiet_NaN(); @@ -313,31 +343,27 @@ TEST_F(AckermannSteeringControllerTest, test_update_logic_chainable) { msg->twist.angular.z = TEST_ANGULAR_VELOCITY_Z; controller_->input_ref_.writeFromNonRT(msg); const auto age_of_last_command = - controller_->get_node()->now() - - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; + controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; // age_of_last_command > ref_timeout_ ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_); - ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, - TEST_LINEAR_VELOCITY_X); + ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); ASSERT_EQ( - controller_->update_reference_from_subscribers(), - controller_interface::return_type::OK); + controller_->update_reference_from_subscribers(), controller_interface::return_type::OK); ASSERT_EQ( - controller_->update_and_write_commands( - controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); + controller_->update_and_write_commands( + controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); - EXPECT_EQ(joint_command_values_[NR_CMD_ITFS - 2], 111); - ASSERT_TRUE(std::isnan(controller_->reference_interfaces_[0])); - for (const auto& interface : controller_->reference_interfaces_) { - EXPECT_TRUE(std::isnan(interface)); + EXPECT_EQ(joint_command_values_[NR_CMD_ITFS - 2], 0); + ASSERT_EQ(controller_->reference_interfaces_[0], 0); + for (const auto & interface : controller_->reference_interfaces_) + { + ASSERT_EQ(interface, 0); } - std::shared_ptr msg_2 = - std::make_shared(); - msg_2->header.stamp = - controller_->get_node()->now() - rclcpp::Duration::from_seconds(0.01); + std::shared_ptr msg_2 = std::make_shared(); + msg_2->header.stamp = controller_->get_node()->now() - rclcpp::Duration::from_seconds(0.01); msg_2->twist.linear.x = TEST_LINEAR_VELOCITY_X; msg_2->twist.linear.y = std::numeric_limits::quiet_NaN(); msg_2->twist.linear.z = std::numeric_limits::quiet_NaN(); @@ -346,25 +372,22 @@ TEST_F(AckermannSteeringControllerTest, test_update_logic_chainable) { msg_2->twist.angular.z = TEST_ANGULAR_VELOCITY_Z; controller_->input_ref_.writeFromNonRT(msg_2); const auto age_of_last_command_2 = - controller_->get_node()->now() - - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; + controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; // age_of_last_command_2 < ref_timeout_ ASSERT_TRUE(age_of_last_command_2 <= controller_->ref_timeout_); - ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, - TEST_LINEAR_VELOCITY_X); + ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); ASSERT_EQ( - controller_->update_reference_from_subscribers(), - controller_interface::return_type::OK); + controller_->update_reference_from_subscribers(), controller_interface::return_type::OK); ASSERT_EQ( - controller_->update_and_write_commands( - controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); + controller_->update_and_write_commands( + controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); EXPECT_NE(joint_command_values_[NR_CMD_ITFS - 2], 111); - ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, - TEST_LINEAR_VELOCITY_X); - for (const auto& interface : controller_->reference_interfaces_) { + ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); + for (const auto & interface : controller_->reference_interfaces_) + { EXPECT_FALSE(std::isnan(interface)); } } @@ -384,10 +407,8 @@ TEST_F(AckermannSteeringControllerTest, test_update_logic) static constexpr double TEST_LINEAR_VELOCITY_X = 1.5; static constexpr double TEST_ANGULAR_VELOCITY_Z = 1.1; joint_command_values_[NR_CMD_ITFS - 2] = 111; - std::shared_ptr msg = - std::make_shared(); - msg->header.stamp = controller_->get_node()->now() - - controller_->ref_timeout_ - + std::shared_ptr msg = std::make_shared(); + msg->header.stamp = controller_->get_node()->now() - controller_->ref_timeout_ - rclcpp::Duration::from_seconds(0.1); msg->twist.linear.x = TEST_LINEAR_VELOCITY_X; msg->twist.linear.y = std::numeric_limits::quiet_NaN(); @@ -397,25 +418,21 @@ TEST_F(AckermannSteeringControllerTest, test_update_logic) msg->twist.angular.z = TEST_ANGULAR_VELOCITY_Z; controller_->input_ref_.writeFromNonRT(msg); const auto age_of_last_command = - controller_->get_node()->now() - - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; + controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_); - ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, - TEST_LINEAR_VELOCITY_X); + ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); ASSERT_EQ( - controller_->update_reference_from_subscribers(), - controller_interface::return_type::OK); + controller_->update_reference_from_subscribers(), controller_interface::return_type::OK); ASSERT_EQ( - controller_->update_and_write_commands( - controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); + controller_->update_and_write_commands( + controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); - EXPECT_EQ(joint_command_values_[NR_CMD_ITFS - 2], 111); - ASSERT_TRUE(std::isnan(controller_->reference_interfaces_[0])); + EXPECT_EQ(joint_command_values_[NR_CMD_ITFS - 2], 0); + ASSERT_EQ(controller_->reference_interfaces_[0], 0); - std::shared_ptr msg_2 = - std::make_shared(); + std::shared_ptr msg_2 = std::make_shared(); msg_2->header.stamp = controller_->get_node()->now(); msg_2->twist.linear.x = TEST_LINEAR_VELOCITY_X; msg_2->twist.linear.y = std::numeric_limits::quiet_NaN(); @@ -425,24 +442,20 @@ TEST_F(AckermannSteeringControllerTest, test_update_logic) msg_2->twist.angular.z = TEST_ANGULAR_VELOCITY_Z; controller_->input_ref_.writeFromNonRT(msg_2); const auto age_of_last_command_2 = - controller_->get_node()->now() - - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; + controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; ASSERT_TRUE(age_of_last_command_2 <= controller_->ref_timeout_); - ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, - TEST_LINEAR_VELOCITY_X); + ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); ASSERT_EQ( - controller_->update_reference_from_subscribers(), - controller_interface::return_type::OK); + controller_->update_reference_from_subscribers(), controller_interface::return_type::OK); ASSERT_EQ( - controller_->update_and_write_commands( - controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); + controller_->update_and_write_commands( + controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); EXPECT_EQ(joint_command_values_[NR_CMD_ITFS - 2], TEST_LINEAR_VELOCITY_X); EXPECT_NE(joint_command_values_[NR_CMD_ITFS - 2], 111); - ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, - TEST_LINEAR_VELOCITY_X); + ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); } TEST_F(AckermannSteeringControllerTest, test_ref_timeout_zero_for_update) @@ -482,10 +495,8 @@ TEST_F(AckermannSteeringControllerTest, test_ref_timeout_zero_for_update) controller_interface::return_type::OK); EXPECT_EQ(joint_command_values_[NR_STATE_ITFS - 2], TEST_LINEAR_VELOCITY_X); - ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, - TEST_LINEAR_VELOCITY_X); - ASSERT_FALSE( - std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x)); + ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); + ASSERT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x)); } TEST_F(AckermannSteeringControllerTest, test_ref_timeout_zero_for_reference_callback) @@ -499,7 +510,14 @@ TEST_F(AckermannSteeringControllerTest, test_ref_timeout_zero_for_reference_call EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x)); EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z)); controller_->ref_timeout_ = rclcpp::Duration::from_seconds(0.0); - publish_commands(controller_->get_node()->now()); + if (controller_->params_.use_stamped_vel) + { + publish_commands(controller_->get_node()->now()); + } + else + { + publish_commands_unstamped(); + } ASSERT_TRUE(controller_->wait_for_commands(executor)); EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x)); @@ -508,6 +526,36 @@ TEST_F(AckermannSteeringControllerTest, test_ref_timeout_zero_for_reference_call EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z, 0.0); } +//test is expected to fail in case of param use_stamped_vel is true + +TEST_F(AckermannSteeringControllerTest, test_age_of_last_command_zero_reference_callback_unstamped) +{ + fprintf( + stderr, + "-------------------------Expect to fail when use_stamped_vel is true---------------- \n"); + 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); + + EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x)); + EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z)); + publish_commands_unstamped(); + ASSERT_TRUE(controller_->wait_for_commands(executor)); + + const auto age_of_last_command = + controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; + ASSERT_TRUE( + age_of_last_command >= rclcpp::Duration::from_seconds(0) && + age_of_last_command <= rclcpp::Duration::from_seconds(0.00001)); + + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x)); + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z)); + EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x, 0.45); + EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z, 0.0); +} + int main(int argc, char ** argv) { ::testing::InitGoogleTest(&argc, argv); diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp index 00d6bf5114..13378e91a5 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp @@ -36,14 +36,15 @@ #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" // TODO(anyone): replace the state and command message types -using ControllerStateMsgOdom = ackermann_steering_controller:: - AckermannSteeringController::ControllerStateMsgOdom; -using ControllerStateMsgTf = ackermann_steering_controller:: - AckermannSteeringController::ControllerStateMsgTf; -using ControllerReferenceMsg = ackermann_steering_controller:: - AckermannSteeringController::ControllerReferenceMsg; - -namespace { +using ControllerStateMsgOdom = + ackermann_steering_controller::AckermannSteeringController::ControllerStateMsgOdom; +using ControllerStateMsgTf = + ackermann_steering_controller::AckermannSteeringController::ControllerStateMsgTf; +using ControllerReferenceMsg = + ackermann_steering_controller::AckermannSteeringController::ControllerReferenceMsg; + +namespace +{ constexpr auto NODE_SUCCESS = controller_interface::CallbackReturn::SUCCESS; constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR; } // namespace @@ -51,120 +52,130 @@ constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR; // subclassing and friending so we can access member variables class TestableAckermannSteeringController - : public ackermann_steering_controller::AckermannSteeringController { - FRIEND_TEST(AckermannSteeringControllerTest, - all_parameters_set_configure_success); +: public ackermann_steering_controller::AckermannSteeringController +{ + FRIEND_TEST(AckermannSteeringControllerTest, all_parameters_set_configure_success); FRIEND_TEST(AckermannSteeringControllerTest, check_exported_intefaces); FRIEND_TEST(AckermannSteeringControllerTest, activate_success); FRIEND_TEST(AckermannSteeringControllerTest, update_success); FRIEND_TEST(AckermannSteeringControllerTest, deactivate_success); FRIEND_TEST(AckermannSteeringControllerTest, reactivate_success); FRIEND_TEST(AckermannSteeringControllerTest, publish_status_success); - FRIEND_TEST(AckermannSteeringControllerTest, - receive_message_and_publish_updated_status); - FRIEND_TEST(AckermannSteeringControllerTest, test_message_timeout); + FRIEND_TEST(AckermannSteeringControllerTest, receive_message_and_publish_updated_status); + FRIEND_TEST(AckermannSteeringControllerTest, test_sending_too_old_message); FRIEND_TEST(AckermannSteeringControllerTest, test_time_stamp_zero); FRIEND_TEST(AckermannSteeringControllerTest, test_message_accepted); FRIEND_TEST(AckermannSteeringControllerTest, test_update_logic); FRIEND_TEST(AckermannSteeringControllerTest, test_update_logic_chainable); - FRIEND_TEST(AckermannSteeringControllerTest, - test_ref_timeout_zero_for_update); - FRIEND_TEST(AckermannSteeringControllerTest, - test_ref_timeout_zero_for_reference_callback); + FRIEND_TEST(AckermannSteeringControllerTest, test_ref_timeout_zero_for_update); + FRIEND_TEST(AckermannSteeringControllerTest, test_ref_timeout_zero_for_reference_callback); + FRIEND_TEST( + AckermannSteeringControllerTest, test_age_of_last_command_zero_reference_callback_unstamped); - public: +public: controller_interface::CallbackReturn on_configure( - const rclcpp_lifecycle::State& previous_state) override { - auto ret = ackermann_steering_controller::AckermannSteeringController:: - on_configure(previous_state); + const rclcpp_lifecycle::State & previous_state) override + { + auto ret = + ackermann_steering_controller::AckermannSteeringController::on_configure(previous_state); // Only if on_configure is successful create subscription - if (ret == CallbackReturn::SUCCESS) { - ref_subscriber_wait_set_.add_subscription(ref_subscriber_); + if (ret == CallbackReturn::SUCCESS) + { + if (params_.use_stamped_vel) + { + ref_subscriber_wait_set_.add_subscription(ref_subscriber_); + } + else + { + ref_subscriber_wait_set_.add_subscription(ref_subscriber_unstamped_); + } } return ret; } controller_interface::CallbackReturn on_activate( - const rclcpp_lifecycle::State& previous_state) override { + const rclcpp_lifecycle::State & previous_state) override + { auto ref_itfs = on_export_reference_interfaces(); - return ackermann_steering_controller::AckermannSteeringController:: - on_activate(previous_state); + return ackermann_steering_controller::AckermannSteeringController::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) { + * @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}) { + 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: +private: rclcpp::WaitSet ref_subscriber_wait_set_; }; // We are using template class here for easier reuse of Fixture in // specializations of controllers template -class AckermannSteeringControllerFixture : public ::testing::Test { - public: +class AckermannSteeringControllerFixture : public ::testing::Test +{ +public: static void SetUpTestCase() {} - void SetUp() { + void SetUp() + { // initialize controller controller_ = std::make_unique(); - command_publisher_node_ = - std::make_shared("command_publisher"); - command_publisher_ = - command_publisher_node_->create_publisher( - "/test_ackermann_steering_controller/reference", - rclcpp::SystemDefaultsQoS()); + command_publisher_node_ = std::make_shared("command_publisher"); + command_publisher_ = command_publisher_node_->create_publisher( + "/test_ackermann_steering_controller/reference", rclcpp::SystemDefaultsQoS()); + command_publisher_node_unstamped_ = + std::make_shared("command_publisher_unstamped"); + command_publisher_unstamped_ = + command_publisher_node_unstamped_->create_publisher( + "/test_ackermann_steering_controller/reference_unstamped", rclcpp::SystemDefaultsQoS()); } static void TearDownTestCase() {} void TearDown() { controller_.reset(nullptr); } - protected: - void SetUpController(const std::string controller_name = - "test_ackermann_steering_controller") { - ASSERT_EQ(controller_->init(controller_name), - controller_interface::return_type::OK); +protected: + void SetUpController(const std::string controller_name = "test_ackermann_steering_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()); command_itfs_.emplace_back(hardware_interface::CommandInterface( - rear_wheel_name, hardware_interface::HW_IF_VELOCITY, - &joint_command_values_[0])); + rear_wheel_name, hardware_interface::HW_IF_VELOCITY, &joint_command_values_[0])); command_ifs.emplace_back(command_itfs_.back()); command_itfs_.emplace_back(hardware_interface::CommandInterface( - front_steer_name, hardware_interface::HW_IF_VELOCITY, - &joint_command_values_[1])); + front_steer_name, hardware_interface::HW_IF_VELOCITY, &joint_command_values_[1])); command_ifs.emplace_back(command_itfs_.back()); // TODO(anyone): Add other command interfaces, if any @@ -173,28 +184,24 @@ class AckermannSteeringControllerFixture : public ::testing::Test { state_ifs.reserve(joint_state_values_.size()); state_itfs_.emplace_back(hardware_interface::StateInterface( - rear_wheel_name, hardware_interface::HW_IF_POSITION, - &joint_state_values_[0])); + rear_wheel_name, hardware_interface::HW_IF_POSITION, &joint_state_values_[0])); state_ifs.emplace_back(state_itfs_.back()); state_itfs_.emplace_back(hardware_interface::StateInterface( - front_steer_name, hardware_interface::HW_IF_POSITION, - &joint_state_values_[1])); + front_steer_name, hardware_interface::HW_IF_POSITION, &joint_state_values_[1])); 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)); + controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); } - void subscribe_and_get_messages(ControllerStateMsgOdom& msg_odom) { + void subscribe_and_get_messages(ControllerStateMsgOdom & msg_odom) + { // create a new subscriber rclcpp::Node test_subscription_node("test_subscription_node"); auto subs_callback_odom = [&](const ControllerStateMsgOdom::SharedPtr) {}; - auto subscription_odom = - test_subscription_node.create_subscription( - "/test_ackermann_steering_controller/odometry", 10, - subs_callback_odom); + auto subscription_odom = test_subscription_node.create_subscription( + "/test_ackermann_steering_controller/odometry", 10, subs_callback_odom); // call update to publish the test value ASSERT_EQ( @@ -214,17 +221,16 @@ class AckermannSteeringControllerFixture : public ::testing::Test { { controller_->update_reference_from_subscribers(); controller_->update_and_write_commands( - controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)); + controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)); // check if message has been received - if (wait_set.wait(std::chrono::milliseconds(2)).kind() == - rclcpp::WaitResultKind::Ready) { + 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"; + 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_odom_info; @@ -232,15 +238,19 @@ class AckermannSteeringControllerFixture : public ::testing::Test { } // TODO(anyone): add/remove arguments as it suites your command message type - void publish_commands(const rclcpp::Time& stamp, - const double& twist_linear_x = 0.45, - const double& twist_angular_z = 0.0) { - auto wait_for_topic = [&](const auto topic_name) { + void publish_commands( + const rclcpp::Time & stamp, const double & twist_linear_x = 0.45, + const double & twist_angular_z = 0.0) + { + 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"; + 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)); @@ -263,7 +273,40 @@ class AckermannSteeringControllerFixture : public ::testing::Test { command_publisher_->publish(msg); } - protected: + void publish_commands_unstamped( + const double & twist_linear_x = 0.45, const double & twist_angular_z = 0.0) + { + auto wait_for_topic = [&](const auto topic_name) + { + size_t wait_count = 0; + while (command_publisher_node_unstamped_->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_unstamped_->get_topic_name()); + + geometry_msgs::msg::Twist msg_unstamped; + + msg_unstamped.linear.x = twist_linear_x; + msg_unstamped.linear.y = std::numeric_limits::quiet_NaN(); + msg_unstamped.linear.z = std::numeric_limits::quiet_NaN(); + msg_unstamped.angular.x = std::numeric_limits::quiet_NaN(); + msg_unstamped.angular.y = std::numeric_limits::quiet_NaN(); + msg_unstamped.angular.z = twist_angular_z; + + command_publisher_unstamped_->publish(msg_unstamped); + } + +protected: // TODO(anyone): adjust the members as needed std::string rear_wheel_name = "rear_wheel_joint"; std::string front_steer_name = "front_steer_joint"; @@ -280,7 +323,9 @@ class AckermannSteeringControllerFixture : public ::testing::Test { // Test related parameters std::unique_ptr controller_; rclcpp::Node::SharedPtr command_publisher_node_; + rclcpp::Node::SharedPtr command_publisher_node_unstamped_; rclcpp::Publisher::SharedPtr command_publisher_; + rclcpp::Publisher::SharedPtr command_publisher_unstamped_; }; #endif // TEST_ACKERMANN_STEERING_CONTROLLER_HPP_ From 20f5319c6e6ef8e70573d58730296d5718783681 Mon Sep 17 00:00:00 2001 From: GiridharBukka Date: Tue, 29 Nov 2022 18:13:06 +0100 Subject: [PATCH 029/186] modifid config files --- .../src/ackermann_steering_controller.yaml | 4 ++-- .../test/ackermann_steering_controller_params.yaml | 3 +++ 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.yaml b/ackermann_steering_controller/src/ackermann_steering_controller.yaml index 8a4ee8c0b2..6e4b6dcba3 100644 --- a/ackermann_steering_controller/src/ackermann_steering_controller.yaml +++ b/ackermann_steering_controller/src/ackermann_steering_controller.yaml @@ -1,7 +1,7 @@ ackermann_steering_controller: reference_timeout: { type: double, - default_value: 0.0, + default_value: 1, description: "Timeout for controller references after which they will be reset. This is especially useful for controllers that can cause unwanted and dangerous behaviour if reference is not reset, e.g., velocity controllers. If value is 0 the reference is reset after each run.", # validation: { # gt_eq<>: 0.0, @@ -140,7 +140,7 @@ ackermann_steering_controller: use_stamped_vel: { type: bool, - default_value: true, + default_value: false, description: "choice of vel type, if use_stamped_vel is false then geometry_msgs::msg::Twist is taken as vel msg type, if use_stamped_vel is true then geometry_msgs::msg::TwistStamped is taken as vel msg type", read_only: false, diff --git a/ackermann_steering_controller/test/ackermann_steering_controller_params.yaml b/ackermann_steering_controller/test/ackermann_steering_controller_params.yaml index fa9a29a2dc..bc92244584 100644 --- a/ackermann_steering_controller/test/ackermann_steering_controller_params.yaml +++ b/ackermann_steering_controller/test/ackermann_steering_controller_params.yaml @@ -31,3 +31,6 @@ test_ackermann_steering_controller: pose_covariance_diagonal: [0.0, 7.0, 14.0, 21.0, 28.0, 35.0] + position_feedback: false + + use_stamped_vel: false From 0c84ca6d98774afb8cb44000533cdf9999eba41e Mon Sep 17 00:00:00 2001 From: GiridharBukka Date: Tue, 29 Nov 2022 18:13:37 +0100 Subject: [PATCH 030/186] modified cmakelists --- ackermann_steering_controller/CMakeLists.txt | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/ackermann_steering_controller/CMakeLists.txt b/ackermann_steering_controller/CMakeLists.txt index 48cd88df6f..eb72253762 100644 --- a/ackermann_steering_controller/CMakeLists.txt +++ b/ackermann_steering_controller/CMakeLists.txt @@ -99,14 +99,14 @@ if(BUILD_TESTING) # ros2_control_test_assets # ) - # add_rostest_with_parameters_gmock(test_ackermann_steering_controller test/test_ackermann_steering_controller.cpp ${CMAKE_CURRENT_SOURCE_DIR}/test/ackermann_steering_controller_params.yaml) - # target_include_directories(test_ackermann_steering_controller PRIVATE include) - # target_link_libraries(test_ackermann_steering_controller ackermann_steering_controller) - # ament_target_dependencies( - # test_ackermann_steering_controller - # controller_interface - # hardware_interface - # ) + add_rostest_with_parameters_gmock(test_ackermann_steering_controller test/test_ackermann_steering_controller.cpp ${CMAKE_CURRENT_SOURCE_DIR}/test/ackermann_steering_controller_params.yaml) + target_include_directories(test_ackermann_steering_controller PRIVATE include) + target_link_libraries(test_ackermann_steering_controller ackermann_steering_controller) + ament_target_dependencies( + test_ackermann_steering_controller + controller_interface + hardware_interface + ) # add_rostest_with_parameters_gmock(test_ackermann_steering_controller_preceeding test/test_ackermann_steering_controller_preceeding.cpp ${CMAKE_CURRENT_SOURCE_DIR}/test/ackermann_steering_controller_preceeding_params.yaml) # target_include_directories(test_ackermann_steering_controller_preceeding PRIVATE include) From c8bbf2cadf50645a2015262305b454aef239e9d2 Mon Sep 17 00:00:00 2001 From: GiridharBukka Date: Tue, 29 Nov 2022 18:14:10 +0100 Subject: [PATCH 031/186] removed file not needed --- ackermann_steering_controller/test.txt | 125 ------------------------- 1 file changed, 125 deletions(-) delete mode 100644 ackermann_steering_controller/test.txt diff --git a/ackermann_steering_controller/test.txt b/ackermann_steering_controller/test.txt deleted file mode 100644 index 3f71fab599..0000000000 --- a/ackermann_steering_controller/test.txt +++ /dev/null @@ -1,125 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(ros2_ackermann_cont) - -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 - geometry_msgs - hardware_interface - nav_msgs - pluginlib - rclcpp - rclcpp_lifecycle - realtime_tools - std_srvs - tf2 - tf2_msgs - tf2_geometry_msgs -) - -find_package(ament_cmake REQUIRED) -find_package(generate_parameter_library REQUIRED) -foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) - find_package(${Dependency} REQUIRED) -endforeach() - -cmake_minimum_required(VERSION 3.8) -project(ros2_ackermann_cont) - -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) - -# Add ackermann_steering_controller_ros2 library related compile commands -generate_parameter_library(ackermann_steering_controller_ros2_parameters - src/ackermann_steering_controller_ros2.yaml - # include/ros2_ackermann_cont/validate_ackermann_steering_controller_ros2_parameters.hpp -) -add_library( - ackermann_steering_controller_ros2 - SHARED - src/ackermann_steering_controller_ros2.cpp - src/odometry.cpp -) -target_include_directories(ackermann_steering_controller_ros2 PRIVATE include) -target_link_libraries(ackermann_steering_controller_ros2 ackermann_steering_controller_ros2_parameters) -ament_target_dependencies(ackermann_steering_controller_ros2 ${THIS_PACKAGE_INCLUDE_DEPENDS}) -target_compile_definitions(ackermann_steering_controller_ros2 PRIVATE "ACKERMANN_STEERING_CONTROLLER_ROS2_BUILDING_DLL") - -pluginlib_export_plugin_description_file( - controller_interface ros2_ackermann_cont.xml) - -install( - TARGETS - ackermann_steering_controller_ros2 - RUNTIME DESTINATION bin - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib -) - -install( - DIRECTORY include/ - DESTINATION include -) - -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) - 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_ackermann_steering_controller_ros2 test/test_load_ackermann_steering_controller_ros2.cpp) - target_include_directories(test_load_ackermann_steering_controller_ros2 PRIVATE include) - ament_target_dependencies( - test_load_ackermann_steering_controller_ros2 - controller_manager - hardware_interface - ros2_control_test_assets - ) - - # add_rostest_with_parameters_gmock(test_ackermann_steering_controller_ros2 test/# # test_ackermann_steering_controller_ros2.cpp ${CMAKE_CURRENT_SOURCE_DIR}/test/ackermann_steering_controller_ros2_params.yaml) - # target_include_directories(test_ackermann_steering_controller_ros2 PRIVATE include) - # target_link_libraries(test_ackermann_steering_controller_ros2 ackermann_steering_controller_ros2) - #ament_target_dependencies( - # test_ackermann_steering_controller_ros2 - # controller_interface - # hardware_interface - #) - - # add_rostest_with_parameters_gmock(test_ackermann_steering_controller_ros2_preceeding test/test_ackermann_steering_controller_ros2_preceeding.cpp ${CMAKE_CURRENT_SOURCE_DIR}/test/ackermann_steering_controller_ros2_preceeding_params.yaml) - # target_include_directories(test_ackermann_steering_controller_ros2_preceeding PRIVATE include) - # target_link_libraries(test_ackermann_steering_controller_ros2_preceeding ackermann_steering_controller_ros2) - # ament_target_dependencies( - # test_ackermann_steering_controller_ros2_preceeding - # controller_interface - # hardware_interface - # ) -endif() - -ament_export_include_directories( - include -) -ament_export_dependencies( - -) -ament_export_libraries( - ackermann_steering_controller_ros2 -) - -ament_package() From fcd3cb4aed8333a85ef1134e0ca81442bf9eebfa Mon Sep 17 00:00:00 2001 From: petkovich Date: Mon, 5 Dec 2022 12:51:02 +0100 Subject: [PATCH 032/186] odom lib creation --- ackermann_steering_controller/CMakeLists.txt | 17 ++ .../ackermann_odometry/ackermann_odometry.hpp | 182 +++++++++++++++++ ...kermann_steering_controller_parameters.hpp | 3 +- .../src/ackermann_odometry.cpp | 188 ++++++++++++++++++ ...kermann_steering_controller_preceeding.cpp | 12 +- ...est_load_ackermann_steering_controller.cpp | 5 +- 6 files changed, 400 insertions(+), 7 deletions(-) create mode 100644 ackermann_steering_controller/include/ackermann_odometry/ackermann_odometry.hpp create mode 100644 ackermann_steering_controller/src/ackermann_odometry.cpp diff --git a/ackermann_steering_controller/CMakeLists.txt b/ackermann_steering_controller/CMakeLists.txt index eb72253762..ffcc00ca73 100644 --- a/ackermann_steering_controller/CMakeLists.txt +++ b/ackermann_steering_controller/CMakeLists.txt @@ -71,6 +71,23 @@ install( LIBRARY DESTINATION lib ) + +add_library( + ackermann_odometry + SHARED + src/ackermann_odometry.cpp +) +target_include_directories(ackermann_odometry PRIVATE include) +ament_target_dependencies(ackermann_odometry ${THIS_PACKAGE_INCLUDE_DEPENDS}) + +install( + TARGETS + ackermann_odometry + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib +) + install( DIRECTORY include/ DESTINATION include diff --git a/ackermann_steering_controller/include/ackermann_odometry/ackermann_odometry.hpp b/ackermann_steering_controller/include/ackermann_odometry/ackermann_odometry.hpp new file mode 100644 index 0000000000..9d76b314fa --- /dev/null +++ b/ackermann_steering_controller/include/ackermann_odometry/ackermann_odometry.hpp @@ -0,0 +1,182 @@ +/********************************************************************* +* Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. + *********************************************************************/ + +/* + * Author: dr. sc. Tomislav Petkovic + * Author: Dr. Ing. Denis Stogl + */ + +#pragma once + +#include "realtime_tools/realtime_buffer.h" +#include "realtime_tools/realtime_publisher.h" + +//#include +#include +#include +#include +#include + +namespace ackermann_odometry +{ +namespace bacc = boost::accumulators; + +/** + * \brief The Odometry class handles odometry readings + * (2D pose and velocity with related timestamp) + */ +class AckermannOdometry +{ +public: + /// Integration function, used to integrate the odometry: + typedef boost::function IntegrationFunction; + + /** + * \brief Constructor + * Timestamp will get the current time value + * Value will be set to zero + * \param velocity_rolling_window_size Rolling window size used to compute the velocity mean + * + */ + // ackermann_steering_controller_ros2::Params params; + explicit AckermannOdometry(size_t velocity_rolling_window_size = 10); + + /** + * \brief Initialize the odometry + * \param time Current time + */ + void init(const rclcpp::Time & time); + + /** + * \brief Updates the odometry class with latest wheels position + * \param rear_wheel_pos Rear wheel position [rad] + * \param front_steer_pos Front Steer position [rad] + * \param dt time difference to last call + * \return true if the odometry is actually updated + */ + bool updateFromPosition( + const double rear_wheel_pos, const double front_steer_pos, const double dt); + + /** + * \brief Updates the odometry class with latest wheels position + * \param rear_wheel_vel Rear wheel velocity [rad/s] + * \param front_steer_pos Front Steer position [rad] + * \param dt time difference to last call + * \return true if the odometry is actually updated + */ + bool updateFromVelocity( + const double rear_wheel_vel, const double front_steer_pos, const double dt); + + /** + * \brief Updates the odometry class with latest velocity command + * \param linear Linear velocity [m/s] + * \param angular Angular velocity [rad/s] + * \param time Current time + */ + void updateOpenLoop(const double linear, const double angular, const double dt); + + /** + * \brief heading getter + * \return heading [rad] + */ + double getHeading() const { return heading_; } + + /** + * \brief x position getter + * \return x position [m] + */ + double getX() const { return x_; } + + /** + * \brief y position getter + * \return y position [m] + */ + double getY() const { return y_; } + + /** + * \brief linear velocity getter + * \return linear velocity [m/s] + */ + double getLinear() const { return linear_; } + + /** + * \brief angular velocity getter + * \return angular velocity [rad/s] + */ + double getAngular() const { return angular_; } + + /** + * \brief Sets the wheel parameters: radius and separation + */ + void setWheelParams(double wheel_reparation_h, double wheel_radius); + + /** + * \brief Velocity rolling window size setter + * \param velocity_rolling_window_size Velocity rolling window size + */ + void setVelocityRollingWindowSize(size_t velocity_rolling_window_size); + +private: + /// Rolling mean accumulator and window: + typedef bacc::accumulator_set > RollingMeanAcc; + typedef bacc::tag::rolling_window RollingWindow; + + /** + * \brief Integrates the velocities (linear and angular) using 2nd order Runge-Kutta + * \param linear Linear velocity [m] (linear displacement, i.e. m/s * dt) computed by encoders + * \param angular Angular velocity [rad] (angular displacement, i.e. m/s * dt) computed by encoders + */ + void integrateRungeKutta2(double linear, double angular); + + /** + * \brief Integrates the velocities (linear and angular) using exact method + * \param linear Linear velocity [m] (linear displacement, i.e. m/s * dt) computed by encoders + * \param angular Angular velocity [rad] (angular displacement, i.e. m/s * dt) computed by encoders + */ + void integrateExact(double linear, double angular); + + /** + * \brief Reset linear and angular accumulators + */ + void resetAccumulators(); + + /// Current timestamp: + rclcpp::Time timestamp_; + + /// Current pose: + double x_; // [m] + double y_; // [m] + double heading_; // [rad] + + /// Current velocity: + double linear_; // [m/s] + double angular_; // [rad/s] + + double wheel_separation_; + double wheel_radius_; + + /// Previous wheel position/state [rad]: + double rear_wheel_old_pos_; + + /// Rolling mean accumulators for the linar and angular velocities: + int velocity_rolling_window_size_; + RollingMeanAcc linear_acc_; + RollingMeanAcc angular_acc_; + + /// Integration function, used to integrate the odometry: + IntegrationFunction integrate_fun_; +}; +} // namespace ackermann_odometry diff --git a/ackermann_steering_controller/include/ackermann_steering_controller/validate_ackermann_steering_controller_parameters.hpp b/ackermann_steering_controller/include/ackermann_steering_controller/validate_ackermann_steering_controller_parameters.hpp index 6e3187690a..3003f6236d 100644 --- a/ackermann_steering_controller/include/ackermann_steering_controller/validate_ackermann_steering_controller_parameters.hpp +++ b/ackermann_steering_controller/include/ackermann_steering_controller/validate_ackermann_steering_controller_parameters.hpp @@ -25,7 +25,8 @@ Result forbidden_interface_name_prefix(rclcpp::Parameter const & parameter) { auto const & interface_name = parameter.as_string(); - if (interface_name.rfind("blup_", 0) == 0) { + if (interface_name.rfind("blup_", 0) == 0) + { return ERROR("'interface_name' parameter can not start with 'blup_'"); } diff --git a/ackermann_steering_controller/src/ackermann_odometry.cpp b/ackermann_steering_controller/src/ackermann_odometry.cpp new file mode 100644 index 0000000000..c5f9471937 --- /dev/null +++ b/ackermann_steering_controller/src/ackermann_odometry.cpp @@ -0,0 +1,188 @@ +/********************************************************************* +* Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. + *********************************************************************/ + +/* + * Author: dr. sc. Tomislav Petkovic + * Author: Dr. Ing. Denis Stogl + */ + +#include "ackermann_odometry/ackermann_odometry.hpp" + +#include +#include + +namespace ackermann_odometry +{ +namespace bacc = boost::accumulators; +// using namespace ackermann_steering_controller_ros2; +// ackermann_steering_controller_ros2::Params params; + +AckermannOdometry::AckermannOdometry(size_t velocity_rolling_window_size) +: timestamp_(0.0), + x_(0.0), + y_(0.0), + heading_(0.0), + linear_(0.0), + angular_(0.0), + wheel_separation_(0.0), + wheel_radius_(0.0), + rear_wheel_old_pos_(0.0), + velocity_rolling_window_size_(0.0), + linear_acc_(RollingWindow::window_size = velocity_rolling_window_size), + angular_acc_(RollingWindow::window_size = velocity_rolling_window_size), + integrate_fun_(std::bind( + &AckermannOdometry::integrateExact, this, std::placeholders::_1, std::placeholders::_2)) + +{ +} + +void AckermannOdometry::init(const rclcpp::Time & time) +{ + // Reset accumulators and timestamp: + resetAccumulators(); + timestamp_ = time; +} + +// TODO(destogl): enable also velocity interface to update using velocity from the rear wheel +bool AckermannOdometry::updateFromPosition( + const double rear_wheel_pos, const double front_steer_pos, const double dt) +{ + /// Get current wheel joint positions: + const double rear_wheel_cur_pos = rear_wheel_pos * wheel_radius_; + + /// Estimate velocity of wheels using old and current position: + //const double left_wheel_est_vel = left_wheel_cur_pos - left_wheel_old_pos_; + //const double right_wheel_est_vel = right_wheel_cur_pos - right_wheel_old_pos_; + + const double rear_wheel_est_pos_diff = rear_wheel_cur_pos - rear_wheel_old_pos_; + + /// Update old position with current: + rear_wheel_old_pos_ = rear_wheel_cur_pos; + + /// Compute linear and angular diff: + const double linear_velocity = + rear_wheel_est_pos_diff / dt; //(right_wheel_est_vel + left_wheel_est_vel) * 0.5; + + //const double angular = (right_wheel_est_vel - left_wheel_est_vel) / wheel_separation_w_; + const double angular = tan(front_steer_pos) * linear_velocity / wheel_separation_; + + /// Integrate odometry: + integrate_fun_(linear_velocity * dt, angular); + + /// We cannot estimate the speed with very small time intervals: + if (dt < 0.0001) + { + return false; // Interval too small to integrate with + } + + /// Estimate speeds using a rolling mean to filter them out: + linear_acc_(linear_velocity); + angular_acc_(angular / dt); + + linear_ = bacc::rolling_mean(linear_acc_); + angular_ = bacc::rolling_mean(angular_acc_); + + return true; +} + +bool AckermannOdometry::updateFromVelocity( + const double rear_wheel_vel, const double front_steer_pos, const double dt) +{ + // (right_wheel_est_vel + left_wheel_est_vel) * 0.5; + double linear_velocity = rear_wheel_vel * wheel_radius_; + + //const double angular = (right_wheel_est_vel - left_wheel_est_vel) / wheel_separation_w_; + const double angular = tan(front_steer_pos) * linear_velocity / wheel_separation_; + + /// Integrate odometry: + integrate_fun_(linear_velocity * dt, angular * dt); + + /// We cannot estimate the speed with very small time intervals: + if (dt < 0.0001) + { + return false; // Interval too small to integrate with + } + + /// Estimate speeds using a rolling mean to filter them out: + linear_acc_(linear_velocity); + angular_acc_(angular); + + linear_ = bacc::rolling_mean(linear_acc_); + angular_ = bacc::rolling_mean(angular_acc_); + + return true; +} + +void AckermannOdometry::updateOpenLoop(const double linear, const double angular, const double dt) +{ + /// Save last linear and angular velocity: + linear_ = linear; + angular_ = angular; + + /// Integrate odometry: + integrate_fun_(linear * dt, angular * dt); +} + +void AckermannOdometry::setWheelParams(double wheel_separation, double wheel_radius) +{ + wheel_separation_ = wheel_separation; + wheel_radius_ = wheel_radius; +} + +void AckermannOdometry::setVelocityRollingWindowSize(size_t velocity_rolling_window_size) +{ + velocity_rolling_window_size_ = velocity_rolling_window_size; + + resetAccumulators(); +} + +void AckermannOdometry::integrateRungeKutta2(double linear, double angular) +{ + const double direction = heading_ + angular * 0.5; + + /// Runge-Kutta 2nd order integration: + x_ += linear * cos(direction); + y_ += linear * sin(direction); + heading_ += angular; +} + +/** + * \brief Other possible integration method provided by the class + * \param linear + * \param angular + */ +void AckermannOdometry::integrateExact(double linear, double angular) +{ + if (fabs(angular) < 1e-6) + integrateRungeKutta2(linear, angular); + else + { + /// Exact integration (should solve problems when angular is zero): + const double heading_old = heading_; + const double r = linear / angular; + heading_ += angular; + x_ += r * (sin(heading_) - sin(heading_old)); + y_ += -r * (cos(heading_) - cos(heading_old)); + } +} + +void AckermannOdometry::resetAccumulators() +{ + linear_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_); + angular_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_); +} + +} // namespace ackermann_odometry diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp b/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp index f5d65a8272..79bb1a418a 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp @@ -24,7 +24,8 @@ using ackermann_steering_controller::CMD_MY_ITFS; using ackermann_steering_controller::control_mode_type; using ackermann_steering_controller::STATE_MY_ITFS; -class AckermannSteeringControllerTest : public AckermannSteeringControllerFixture +class AckermannSteeringControllerTest +: public AckermannSteeringControllerFixture { }; @@ -52,20 +53,23 @@ TEST_F(AckermannSteeringControllerTest, check_exported_intefaces) 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) { + 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) { + 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) { + 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); diff --git a/ackermann_steering_controller/test/test_load_ackermann_steering_controller.cpp b/ackermann_steering_controller/test/test_load_ackermann_steering_controller.cpp index a794fc3da0..27da4a4929 100644 --- a/ackermann_steering_controller/test/test_load_ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/test/test_load_ackermann_steering_controller.cpp @@ -34,8 +34,9 @@ TEST(TestLoadAckermannSteeringController, load_controller) ros2_control_test_assets::minimal_robot_urdf), executor, "test_controller_manager"); - ASSERT_NO_THROW( - cm.load_controller("test_ackermann_steering_controller", "ackermann_steering_controller/AckermannSteeringController")); + ASSERT_NO_THROW(cm.load_controller( + "test_ackermann_steering_controller", + "ackermann_steering_controller/AckermannSteeringController")); rclcpp::shutdown(); } From 41df037cbe5aeac8387c82e70d8cb73d71568e03 Mon Sep 17 00:00:00 2001 From: petkovich Date: Mon, 5 Dec 2022 13:15:23 +0100 Subject: [PATCH 033/186] refrence new library --- ackermann_steering_controller/CMakeLists.txt | 35 +-- .../ackermann_steering_controller.hpp | 4 +- .../odometry.hpp | 203 ----------------- .../src/odometry.cpp | 209 ------------------ 4 files changed, 21 insertions(+), 430 deletions(-) delete mode 100644 ackermann_steering_controller/include/ackermann_steering_controller/odometry.hpp delete mode 100644 ackermann_steering_controller/src/odometry.cpp diff --git a/ackermann_steering_controller/CMakeLists.txt b/ackermann_steering_controller/CMakeLists.txt index ffcc00ca73..c2b1750edc 100644 --- a/ackermann_steering_controller/CMakeLists.txt +++ b/ackermann_steering_controller/CMakeLists.txt @@ -49,23 +49,19 @@ generate_parameter_library(ackermann_steering_controller_parameters src/ackermann_steering_controller.yaml # include/ackermann_steering_controller/validate_ackermann_steering_controller_parameters.hpp ) + + add_library( - ackermann_steering_controller + ackermann_odometry SHARED - src/ackermann_steering_controller.cpp - src/odometry.cpp + src/ackermann_odometry.cpp ) -target_include_directories(ackermann_steering_controller PRIVATE include) -target_link_libraries(ackermann_steering_controller ackermann_steering_controller_parameters) -ament_target_dependencies(ackermann_steering_controller ${THIS_PACKAGE_INCLUDE_DEPENDS}) -target_compile_definitions(ackermann_steering_controller PRIVATE "ACKERMANN_STEERING_CONTROLLER_BUILDING_DLL") - -pluginlib_export_plugin_description_file( - controller_interface ackermann_steering_controller.xml) +target_include_directories(ackermann_odometry PRIVATE include) +ament_target_dependencies(ackermann_odometry ${THIS_PACKAGE_INCLUDE_DEPENDS}) install( TARGETS - ackermann_steering_controller + ackermann_odometry RUNTIME DESTINATION bin ARCHIVE DESTINATION lib LIBRARY DESTINATION lib @@ -73,21 +69,28 @@ install( add_library( - ackermann_odometry + ackermann_steering_controller SHARED - src/ackermann_odometry.cpp + src/ackermann_steering_controller.cpp ) -target_include_directories(ackermann_odometry PRIVATE include) -ament_target_dependencies(ackermann_odometry ${THIS_PACKAGE_INCLUDE_DEPENDS}) +target_include_directories(ackermann_steering_controller PRIVATE include) +target_link_libraries(ackermann_steering_controller ackermann_steering_controller_parameters ackermann_odometry) +ament_target_dependencies(ackermann_steering_controller ${THIS_PACKAGE_INCLUDE_DEPENDS}) +target_compile_definitions(ackermann_steering_controller PRIVATE "ACKERMANN_STEERING_CONTROLLER_BUILDING_DLL") + +pluginlib_export_plugin_description_file( + controller_interface ackermann_steering_controller.xml) install( TARGETS - ackermann_odometry + ackermann_steering_controller RUNTIME DESTINATION bin ARCHIVE DESTINATION lib LIBRARY DESTINATION lib ) + + install( DIRECTORY include/ DESTINATION include diff --git a/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp b/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp index 794e1ff481..835a82df6f 100644 --- a/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp +++ b/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp @@ -23,7 +23,7 @@ #include #include -#include "ackermann_steering_controller/odometry.hpp" +#include "ackermann_odometry/ackermann_odometry.hpp" #include "ackermann_steering_controller/visibility_control.h" #include "ackermann_steering_controller_parameters.hpp" #include "controller_interface/chainable_controller_interface.hpp" @@ -140,7 +140,7 @@ class AckermannSteeringController : public controller_interface::ChainableContro }; // Odometry related: - Odometry odometry_; + ackermann_odometry::AckermannOdometry odometry_; using AckermanControllerState = custom_messages::msg::AckermanControllerState; ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC diff --git a/ackermann_steering_controller/include/ackermann_steering_controller/odometry.hpp b/ackermann_steering_controller/include/ackermann_steering_controller/odometry.hpp deleted file mode 100644 index cf1cb64190..0000000000 --- a/ackermann_steering_controller/include/ackermann_steering_controller/odometry.hpp +++ /dev/null @@ -1,203 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2013, PAL Robotics, S.L. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the PAL Robotics nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/* - * Author: Luca Marchionni - * Author: Bence Magyar - * Author: Enrique Fernández - * Author: Paul Mathieu - * Author: Masaru Morita - */ - -#pragma once - -#include "realtime_tools/realtime_buffer.h" -#include "realtime_tools/realtime_publisher.h" - -//#include -#include -#include -#include -#include - -namespace ackermann_steering_controller -{ -namespace bacc = boost::accumulators; - -/** - * \brief The Odometry class handles odometry readings - * (2D pose and velocity with related timestamp) - */ -class Odometry -{ -public: - /// Integration function, used to integrate the odometry: - typedef boost::function IntegrationFunction; - - /** - * \brief Constructor - * Timestamp will get the current time value - * Value will be set to zero - * \param velocity_rolling_window_size Rolling window size used to compute the velocity mean - * - */ - // ackermann_steering_controller_ros2::Params params; - explicit Odometry(size_t velocity_rolling_window_size = 10); - - /** - * \brief Initialize the odometry - * \param time Current time - */ - void init(const rclcpp::Time & time); - - /** - * \brief Updates the odometry class with latest wheels position - * \param rear_wheel_pos Rear wheel position [rad] - * \param front_steer_pos Front Steer position [rad] - * \param dt time difference to last call - * \return true if the odometry is actually updated - */ - bool updateFromPosition( - const double rear_wheel_pos, const double front_steer_pos, const double dt); - - /** - * \brief Updates the odometry class with latest wheels position - * \param rear_wheel_vel Rear wheel velocity [rad/s] - * \param front_steer_pos Front Steer position [rad] - * \param dt time difference to last call - * \return true if the odometry is actually updated - */ - bool updateFromVelocity( - const double rear_wheel_vel, const double front_steer_pos, const double dt); - - /** - * \brief Updates the odometry class with latest velocity command - * \param linear Linear velocity [m/s] - * \param angular Angular velocity [rad/s] - * \param time Current time - */ - void updateOpenLoop(const double linear, const double angular, const double dt); - - /** - * \brief heading getter - * \return heading [rad] - */ - double getHeading() const { return heading_; } - - /** - * \brief x position getter - * \return x position [m] - */ - double getX() const { return x_; } - - /** - * \brief y position getter - * \return y position [m] - */ - double getY() const { return y_; } - - /** - * \brief linear velocity getter - * \return linear velocity [m/s] - */ - double getLinear() const { return linear_; } - - /** - * \brief angular velocity getter - * \return angular velocity [rad/s] - */ - double getAngular() const { return angular_; } - - /** - * \brief Sets the wheel parameters: radius and separation - */ - void setWheelParams(double wheel_reparation_h, double wheel_radius); - - /** - * \brief Velocity rolling window size setter - * \param velocity_rolling_window_size Velocity rolling window size - */ - void setVelocityRollingWindowSize(size_t velocity_rolling_window_size); - -private: - /// Rolling mean accumulator and window: - typedef bacc::accumulator_set > RollingMeanAcc; - typedef bacc::tag::rolling_window RollingWindow; - - /** - * \brief Integrates the velocities (linear and angular) using 2nd order Runge-Kutta - * \param linear Linear velocity [m] (linear displacement, i.e. m/s * dt) computed by encoders - * \param angular Angular velocity [rad] (angular displacement, i.e. m/s * dt) computed by encoders - */ - void integrateRungeKutta2(double linear, double angular); - - /** - * \brief Integrates the velocities (linear and angular) using exact method - * \param linear Linear velocity [m] (linear displacement, i.e. m/s * dt) computed by encoders - * \param angular Angular velocity [rad] (angular displacement, i.e. m/s * dt) computed by encoders - */ - void integrateExact(double linear, double angular); - - /** - * \brief Reset linear and angular accumulators - */ - void resetAccumulators(); - - /// Current timestamp: - rclcpp::Time timestamp_; - - /// Current pose: - double x_; // [m] - double y_; // [m] - double heading_; // [rad] - - /// Current velocity: - double linear_; // [m/s] - double angular_; // [rad/s] - - double wheel_separation_; - double wheel_radius_; - - /// Previous wheel position/state [rad]: - double rear_wheel_old_pos_; - - /// Rolling mean accumulators for the linar and angular velocities: - int velocity_rolling_window_size_; - RollingMeanAcc linear_acc_; - RollingMeanAcc angular_acc_; - - /// Integration function, used to integrate the odometry: - IntegrationFunction integrate_fun_; -}; -} // namespace ackermann_steering_controller diff --git a/ackermann_steering_controller/src/odometry.cpp b/ackermann_steering_controller/src/odometry.cpp deleted file mode 100644 index 7435af8502..0000000000 --- a/ackermann_steering_controller/src/odometry.cpp +++ /dev/null @@ -1,209 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2013, PAL Robotics, S.L. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the PAL Robotics nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/* - * Author: Luca Marchionni - * Author: Bence Magyar - * Author: Enrique Fernández - * Author: Paul Mathieu - * Author: Masaru Morita - */ - -#include "ackermann_steering_controller/odometry.hpp" - -#include -#include - -namespace ackermann_steering_controller -{ -namespace bacc = boost::accumulators; -// using namespace ackermann_steering_controller_ros2; -// ackermann_steering_controller_ros2::Params params; - -Odometry::Odometry(size_t velocity_rolling_window_size) -: timestamp_(0.0), - x_(0.0), - y_(0.0), - heading_(0.0), - linear_(0.0), - angular_(0.0), - wheel_separation_(0.0), - wheel_radius_(0.0), - rear_wheel_old_pos_(0.0), - velocity_rolling_window_size_(0.0), - linear_acc_(RollingWindow::window_size = velocity_rolling_window_size), - angular_acc_(RollingWindow::window_size = velocity_rolling_window_size), - integrate_fun_( - std::bind(&Odometry::integrateExact, this, std::placeholders::_1, std::placeholders::_2)) - -{ -} - -void Odometry::init(const rclcpp::Time & time) -{ - // Reset accumulators and timestamp: - resetAccumulators(); - timestamp_ = time; -} - -// TODO(destogl): enable also velocity interface to update using velocity from the rear wheel -bool Odometry::updateFromPosition( - const double rear_wheel_pos, const double front_steer_pos, const double dt) -{ - /// Get current wheel joint positions: - const double rear_wheel_cur_pos = rear_wheel_pos * wheel_radius_; - - /// Estimate velocity of wheels using old and current position: - //const double left_wheel_est_vel = left_wheel_cur_pos - left_wheel_old_pos_; - //const double right_wheel_est_vel = right_wheel_cur_pos - right_wheel_old_pos_; - - const double rear_wheel_est_pos_diff = rear_wheel_cur_pos - rear_wheel_old_pos_; - - /// Update old position with current: - rear_wheel_old_pos_ = rear_wheel_cur_pos; - - /// Compute linear and angular diff: - const double linear_velocity = - rear_wheel_est_pos_diff / dt; //(right_wheel_est_vel + left_wheel_est_vel) * 0.5; - - //const double angular = (right_wheel_est_vel - left_wheel_est_vel) / wheel_separation_w_; - const double angular = tan(front_steer_pos) * linear_velocity / wheel_separation_; - - /// Integrate odometry: - integrate_fun_(linear_velocity * dt, angular); - - /// We cannot estimate the speed with very small time intervals: - if (dt < 0.0001) - { - return false; // Interval too small to integrate with - } - - /// Estimate speeds using a rolling mean to filter them out: - linear_acc_(linear_velocity); - angular_acc_(angular / dt); - - linear_ = bacc::rolling_mean(linear_acc_); - angular_ = bacc::rolling_mean(angular_acc_); - - return true; -} - -bool Odometry::updateFromVelocity( - const double rear_wheel_vel, const double front_steer_pos, const double dt) -{ - // (right_wheel_est_vel + left_wheel_est_vel) * 0.5; - double linear_velocity = rear_wheel_vel * wheel_radius_; - - //const double angular = (right_wheel_est_vel - left_wheel_est_vel) / wheel_separation_w_; - const double angular = tan(front_steer_pos) * linear_velocity / wheel_separation_; - - /// Integrate odometry: - integrate_fun_(linear_velocity * dt, angular * dt); - - /// We cannot estimate the speed with very small time intervals: - if (dt < 0.0001) - { - return false; // Interval too small to integrate with - } - - /// Estimate speeds using a rolling mean to filter them out: - linear_acc_(linear_velocity); - angular_acc_(angular); - - linear_ = bacc::rolling_mean(linear_acc_); - angular_ = bacc::rolling_mean(angular_acc_); - - return true; -} - -void Odometry::updateOpenLoop(const double linear, const double angular, const double dt) -{ - /// Save last linear and angular velocity: - linear_ = linear; - angular_ = angular; - - /// Integrate odometry: - integrate_fun_(linear * dt, angular * dt); -} - -void Odometry::setWheelParams(double wheel_separation, double wheel_radius) -{ - wheel_separation_ = wheel_separation; - wheel_radius_ = wheel_radius; -} - -void Odometry::setVelocityRollingWindowSize(size_t velocity_rolling_window_size) -{ - velocity_rolling_window_size_ = velocity_rolling_window_size; - - resetAccumulators(); -} - -void Odometry::integrateRungeKutta2(double linear, double angular) -{ - const double direction = heading_ + angular * 0.5; - - /// Runge-Kutta 2nd order integration: - x_ += linear * cos(direction); - y_ += linear * sin(direction); - heading_ += angular; -} - -/** - * \brief Other possible integration method provided by the class - * \param linear - * \param angular - */ -void Odometry::integrateExact(double linear, double angular) -{ - if (fabs(angular) < 1e-6) - integrateRungeKutta2(linear, angular); - else - { - /// Exact integration (should solve problems when angular is zero): - const double heading_old = heading_; - const double r = linear / angular; - heading_ += angular; - x_ += r * (sin(heading_) - sin(heading_old)); - y_ += -r * (cos(heading_) - cos(heading_old)); - } -} - -void Odometry::resetAccumulators() -{ - linear_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_); - angular_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_); -} - -} // namespace ackermann_steering_controller From ce0556d996ae6f8ca2ee1f6753c91fe60fefbde6 Mon Sep 17 00:00:00 2001 From: petkovich Date: Mon, 5 Dec 2022 13:25:33 +0100 Subject: [PATCH 034/186] snake case --- .../ackermann_odometry/ackermann_odometry.hpp | 26 +++++++------- .../src/ackermann_odometry.cpp | 24 ++++++------- .../src/ackermann_steering_controller.cpp | 34 ++++++++++--------- 3 files changed, 43 insertions(+), 41 deletions(-) diff --git a/ackermann_steering_controller/include/ackermann_odometry/ackermann_odometry.hpp b/ackermann_steering_controller/include/ackermann_odometry/ackermann_odometry.hpp index 9d76b314fa..e39d658731 100644 --- a/ackermann_steering_controller/include/ackermann_odometry/ackermann_odometry.hpp +++ b/ackermann_steering_controller/include/ackermann_odometry/ackermann_odometry.hpp @@ -67,7 +67,7 @@ class AckermannOdometry * \param dt time difference to last call * \return true if the odometry is actually updated */ - bool updateFromPosition( + bool update_from_position( const double rear_wheel_pos, const double front_steer_pos, const double dt); /** @@ -77,7 +77,7 @@ class AckermannOdometry * \param dt time difference to last call * \return true if the odometry is actually updated */ - bool updateFromVelocity( + bool update_from_velocity( const double rear_wheel_vel, const double front_steer_pos, const double dt); /** @@ -86,48 +86,48 @@ class AckermannOdometry * \param angular Angular velocity [rad/s] * \param time Current time */ - void updateOpenLoop(const double linear, const double angular, const double dt); + void update_open_loop(const double linear, const double angular, const double dt); /** * \brief heading getter * \return heading [rad] */ - double getHeading() const { return heading_; } + double get_heading() const { return heading_; } /** * \brief x position getter * \return x position [m] */ - double getX() const { return x_; } + double get_x() const { return x_; } /** * \brief y position getter * \return y position [m] */ - double getY() const { return y_; } + double get_y() const { return y_; } /** * \brief linear velocity getter * \return linear velocity [m/s] */ - double getLinear() const { return linear_; } + double get_linear() const { return linear_; } /** * \brief angular velocity getter * \return angular velocity [rad/s] */ - double getAngular() const { return angular_; } + double get_angular() const { return angular_; } /** * \brief Sets the wheel parameters: radius and separation */ - void setWheelParams(double wheel_reparation_h, double wheel_radius); + void set_wheel_params(double wheel_reparation_h, double wheel_radius); /** * \brief Velocity rolling window size setter * \param velocity_rolling_window_size Velocity rolling window size */ - void setVelocityRollingWindowSize(size_t velocity_rolling_window_size); + void set_velocity_rolling_window_size(size_t velocity_rolling_window_size); private: /// Rolling mean accumulator and window: @@ -139,19 +139,19 @@ class AckermannOdometry * \param linear Linear velocity [m] (linear displacement, i.e. m/s * dt) computed by encoders * \param angular Angular velocity [rad] (angular displacement, i.e. m/s * dt) computed by encoders */ - void integrateRungeKutta2(double linear, double angular); + void integrate_runge_kutta_2(double linear, double angular); /** * \brief Integrates the velocities (linear and angular) using exact method * \param linear Linear velocity [m] (linear displacement, i.e. m/s * dt) computed by encoders * \param angular Angular velocity [rad] (angular displacement, i.e. m/s * dt) computed by encoders */ - void integrateExact(double linear, double angular); + void integrate_exact(double linear, double angular); /** * \brief Reset linear and angular accumulators */ - void resetAccumulators(); + void reset_accumulators(); /// Current timestamp: rclcpp::Time timestamp_; diff --git a/ackermann_steering_controller/src/ackermann_odometry.cpp b/ackermann_steering_controller/src/ackermann_odometry.cpp index c5f9471937..18996deeef 100644 --- a/ackermann_steering_controller/src/ackermann_odometry.cpp +++ b/ackermann_steering_controller/src/ackermann_odometry.cpp @@ -44,7 +44,7 @@ AckermannOdometry::AckermannOdometry(size_t velocity_rolling_window_size) linear_acc_(RollingWindow::window_size = velocity_rolling_window_size), angular_acc_(RollingWindow::window_size = velocity_rolling_window_size), integrate_fun_(std::bind( - &AckermannOdometry::integrateExact, this, std::placeholders::_1, std::placeholders::_2)) + &AckermannOdometry::integrate_exact, this, std::placeholders::_1, std::placeholders::_2)) { } @@ -52,12 +52,12 @@ AckermannOdometry::AckermannOdometry(size_t velocity_rolling_window_size) void AckermannOdometry::init(const rclcpp::Time & time) { // Reset accumulators and timestamp: - resetAccumulators(); + reset_accumulators(); timestamp_ = time; } // TODO(destogl): enable also velocity interface to update using velocity from the rear wheel -bool AckermannOdometry::updateFromPosition( +bool AckermannOdometry::update_from_position( const double rear_wheel_pos, const double front_steer_pos, const double dt) { /// Get current wheel joint positions: @@ -98,7 +98,7 @@ bool AckermannOdometry::updateFromPosition( return true; } -bool AckermannOdometry::updateFromVelocity( +bool AckermannOdometry::update_from_velocity( const double rear_wheel_vel, const double front_steer_pos, const double dt) { // (right_wheel_est_vel + left_wheel_est_vel) * 0.5; @@ -126,7 +126,7 @@ bool AckermannOdometry::updateFromVelocity( return true; } -void AckermannOdometry::updateOpenLoop(const double linear, const double angular, const double dt) +void AckermannOdometry::update_open_loop(const double linear, const double angular, const double dt) { /// Save last linear and angular velocity: linear_ = linear; @@ -136,20 +136,20 @@ void AckermannOdometry::updateOpenLoop(const double linear, const double angular integrate_fun_(linear * dt, angular * dt); } -void AckermannOdometry::setWheelParams(double wheel_separation, double wheel_radius) +void AckermannOdometry::set_wheel_params(double wheel_separation, double wheel_radius) { wheel_separation_ = wheel_separation; wheel_radius_ = wheel_radius; } -void AckermannOdometry::setVelocityRollingWindowSize(size_t velocity_rolling_window_size) +void AckermannOdometry::set_velocity_rolling_window_size(size_t velocity_rolling_window_size) { velocity_rolling_window_size_ = velocity_rolling_window_size; - resetAccumulators(); + reset_accumulators(); } -void AckermannOdometry::integrateRungeKutta2(double linear, double angular) +void AckermannOdometry::integrate_runge_kutta_2(double linear, double angular) { const double direction = heading_ + angular * 0.5; @@ -164,10 +164,10 @@ void AckermannOdometry::integrateRungeKutta2(double linear, double angular) * \param linear * \param angular */ -void AckermannOdometry::integrateExact(double linear, double angular) +void AckermannOdometry::integrate_exact(double linear, double angular) { if (fabs(angular) < 1e-6) - integrateRungeKutta2(linear, angular); + integrate_runge_kutta_2(linear, angular); else { /// Exact integration (should solve problems when angular is zero): @@ -179,7 +179,7 @@ void AckermannOdometry::integrateExact(double linear, double angular) } } -void AckermannOdometry::resetAccumulators() +void AckermannOdometry::reset_accumulators() { linear_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_); angular_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_); diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.cpp b/ackermann_steering_controller/src/ackermann_steering_controller.cpp index cc0aa4fac7..f7b6a4012e 100644 --- a/ackermann_steering_controller/src/ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/src/ackermann_steering_controller.cpp @@ -91,8 +91,8 @@ controller_interface::CallbackReturn AckermannSteeringController::on_configure( const double wheel_seperation = params_.wheel_separation_multiplier * params_.wheel_separation; const double wheel_radius = params_.wheel_radius_multiplier * params_.wheel_radius; - odometry_.setWheelParams(wheel_seperation, wheel_radius); - odometry_.setVelocityRollingWindowSize(params_.velocity_rolling_window_size); + odometry_.set_wheel_params(wheel_seperation, wheel_radius); + odometry_.set_velocity_rolling_window_size(params_.velocity_rolling_window_size); // topics QoS auto subscribers_qos = rclcpp::SystemDefaultsQoS(); @@ -377,7 +377,7 @@ controller_interface::return_type AckermannSteeringController::update_and_write_ { if (params_.open_loop) { - odometry_.updateOpenLoop(last_linear_velocity_, last_angular_velocity_, period.seconds()); + odometry_.update_open_loop(last_linear_velocity_, last_angular_velocity_, period.seconds()); } else { @@ -389,12 +389,12 @@ controller_interface::return_type AckermannSteeringController::update_and_write_ if (params_.position_feedback) { // Estimate linear and angular velocity using joint information - odometry_.updateFromPosition(rear_wheel_value, steer_position, period.seconds()); + odometry_.update_from_position(rear_wheel_value, steer_position, period.seconds()); } else { // Estimate linear and angular velocity using joint information - odometry_.updateFromVelocity(rear_wheel_value, steer_position, period.seconds()); + odometry_.update_from_velocity(rear_wheel_value, steer_position, period.seconds()); } } } @@ -426,17 +426,17 @@ controller_interface::return_type AckermannSteeringController::update_and_write_ // Publish odometry message // Compute and store orientation info tf2::Quaternion orientation; - orientation.setRPY(0.0, 0.0, odometry_.getHeading()); + orientation.setRPY(0.0, 0.0, odometry_.get_heading()); // Populate odom message and publish if (rt_odom_state_publisher_->trylock()) { rt_odom_state_publisher_->msg_.header.stamp = time; - rt_odom_state_publisher_->msg_.pose.pose.position.x = odometry_.getX(); - rt_odom_state_publisher_->msg_.pose.pose.position.y = odometry_.getY(); + rt_odom_state_publisher_->msg_.pose.pose.position.x = odometry_.get_x(); + rt_odom_state_publisher_->msg_.pose.pose.position.y = odometry_.get_y(); rt_odom_state_publisher_->msg_.pose.pose.orientation = tf2::toMsg(orientation); - rt_odom_state_publisher_->msg_.twist.twist.linear.x = odometry_.getLinear(); - rt_odom_state_publisher_->msg_.twist.twist.angular.z = odometry_.getAngular(); + rt_odom_state_publisher_->msg_.twist.twist.linear.x = odometry_.get_linear(); + rt_odom_state_publisher_->msg_.twist.twist.angular.z = odometry_.get_angular(); rt_odom_state_publisher_->unlockAndPublish(); } @@ -444,8 +444,10 @@ controller_interface::return_type AckermannSteeringController::update_and_write_ if (params_.enable_odom_tf && rt_tf_odom_state_publisher_->trylock()) { rt_tf_odom_state_publisher_->msg_.transforms.front().header.stamp = time; - rt_tf_odom_state_publisher_->msg_.transforms.front().transform.translation.x = odometry_.getX(); - rt_tf_odom_state_publisher_->msg_.transforms.front().transform.translation.y = odometry_.getY(); + rt_tf_odom_state_publisher_->msg_.transforms.front().transform.translation.x = + odometry_.get_x(); + rt_tf_odom_state_publisher_->msg_.transforms.front().transform.translation.y = + odometry_.get_y(); rt_tf_odom_state_publisher_->msg_.transforms.front().transform.rotation = tf2::toMsg(orientation); rt_tf_odom_state_publisher_->unlockAndPublish(); @@ -454,11 +456,11 @@ controller_interface::return_type AckermannSteeringController::update_and_write_ if (controller_state_publisher_->trylock()) { controller_state_publisher_->msg_.header.stamp = time; - controller_state_publisher_->msg_.odom.pose.pose.position.x = odometry_.getX(); - controller_state_publisher_->msg_.odom.pose.pose.position.y = odometry_.getY(); + controller_state_publisher_->msg_.odom.pose.pose.position.x = odometry_.get_x(); + controller_state_publisher_->msg_.odom.pose.pose.position.y = odometry_.get_y(); controller_state_publisher_->msg_.odom.pose.pose.orientation = tf2::toMsg(orientation); - controller_state_publisher_->msg_.odom.twist.twist.linear.x = odometry_.getLinear(); - controller_state_publisher_->msg_.odom.twist.twist.angular.z = odometry_.getAngular(); + controller_state_publisher_->msg_.odom.twist.twist.linear.x = odometry_.get_linear(); + controller_state_publisher_->msg_.odom.twist.twist.angular.z = odometry_.get_angular(); if (params_.position_feedback) { controller_state_publisher_->msg_.rear_wheel_position = state_interfaces_[0].get_value(); From 24802b5de93501ec923bb44aff83c14bf54a4d94 Mon Sep 17 00:00:00 2001 From: petkovich Date: Tue, 6 Dec 2022 20:00:30 +0100 Subject: [PATCH 035/186] steer names as arrays --- ackermann_steering_controller/CMakeLists.txt | 16 +++++++------- .../ackermann_odometry/ackermann_odometry.hpp | 3 ++- .../src/ackermann_odometry.cpp | 5 ++++- .../src/ackermann_steering_controller.cpp | 18 +++++++++++----- .../src/ackermann_steering_controller.yaml | 21 ++++++++++++++++--- 5 files changed, 45 insertions(+), 18 deletions(-) diff --git a/ackermann_steering_controller/CMakeLists.txt b/ackermann_steering_controller/CMakeLists.txt index c2b1750edc..a4699c9a3c 100644 --- a/ackermann_steering_controller/CMakeLists.txt +++ b/ackermann_steering_controller/CMakeLists.txt @@ -119,14 +119,14 @@ if(BUILD_TESTING) # ros2_control_test_assets # ) - add_rostest_with_parameters_gmock(test_ackermann_steering_controller test/test_ackermann_steering_controller.cpp ${CMAKE_CURRENT_SOURCE_DIR}/test/ackermann_steering_controller_params.yaml) - target_include_directories(test_ackermann_steering_controller PRIVATE include) - target_link_libraries(test_ackermann_steering_controller ackermann_steering_controller) - ament_target_dependencies( - test_ackermann_steering_controller - controller_interface - hardware_interface - ) + # add_rostest_with_parameters_gmock(test_ackermann_steering_controller test/test_ackermann_steering_controller.cpp ${CMAKE_CURRENT_SOURCE_DIR}/test/ackermann_steering_controller_params.yaml) + # target_include_directories(test_ackermann_steering_controller PRIVATE include) + # target_link_libraries(test_ackermann_steering_controller ackermann_steering_controller) + # ament_target_dependencies( + # test_ackermann_steering_controller + # controller_interface + # hardware_interface + # ) # add_rostest_with_parameters_gmock(test_ackermann_steering_controller_preceeding test/test_ackermann_steering_controller_preceeding.cpp ${CMAKE_CURRENT_SOURCE_DIR}/test/ackermann_steering_controller_preceeding_params.yaml) # target_include_directories(test_ackermann_steering_controller_preceeding PRIVATE include) diff --git a/ackermann_steering_controller/include/ackermann_odometry/ackermann_odometry.hpp b/ackermann_steering_controller/include/ackermann_odometry/ackermann_odometry.hpp index e39d658731..dd8e855bfb 100644 --- a/ackermann_steering_controller/include/ackermann_odometry/ackermann_odometry.hpp +++ b/ackermann_steering_controller/include/ackermann_odometry/ackermann_odometry.hpp @@ -121,7 +121,7 @@ class AckermannOdometry /** * \brief Sets the wheel parameters: radius and separation */ - void set_wheel_params(double wheel_reparation_h, double wheel_radius); + void set_wheel_params(double wheel_reparation_h, double wheel_radius, double wheelbase); /** * \brief Velocity rolling window size setter @@ -166,6 +166,7 @@ class AckermannOdometry double angular_; // [rad/s] double wheel_separation_; + double wheelbase_; double wheel_radius_; /// Previous wheel position/state [rad]: diff --git a/ackermann_steering_controller/src/ackermann_odometry.cpp b/ackermann_steering_controller/src/ackermann_odometry.cpp index 18996deeef..b3e5e2b689 100644 --- a/ackermann_steering_controller/src/ackermann_odometry.cpp +++ b/ackermann_steering_controller/src/ackermann_odometry.cpp @@ -38,6 +38,7 @@ AckermannOdometry::AckermannOdometry(size_t velocity_rolling_window_size) linear_(0.0), angular_(0.0), wheel_separation_(0.0), + wheelbase_(0.0), wheel_radius_(0.0), rear_wheel_old_pos_(0.0), velocity_rolling_window_size_(0.0), @@ -136,10 +137,12 @@ void AckermannOdometry::update_open_loop(const double linear, const double angul integrate_fun_(linear * dt, angular * dt); } -void AckermannOdometry::set_wheel_params(double wheel_separation, double wheel_radius) +void AckermannOdometry::set_wheel_params( + double wheel_separation, double wheel_radius, double wheelbase) { wheel_separation_ = wheel_separation; wheel_radius_ = wheel_radius; + wheelbase_ = wheelbase; } void AckermannOdometry::set_velocity_rolling_window_size(size_t velocity_rolling_window_size) diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.cpp b/ackermann_steering_controller/src/ackermann_steering_controller.cpp index f7b6a4012e..5ab917920e 100644 --- a/ackermann_steering_controller/src/ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/src/ackermann_steering_controller.cpp @@ -91,7 +91,8 @@ controller_interface::CallbackReturn AckermannSteeringController::on_configure( const double wheel_seperation = params_.wheel_separation_multiplier * params_.wheel_separation; const double wheel_radius = params_.wheel_radius_multiplier * params_.wheel_radius; - odometry_.set_wheel_params(wheel_seperation, wheel_radius); + const double wheelbase = params_.wheelbase_multiplier * params_.wheelbase; + odometry_.set_wheel_params(wheel_seperation, wheel_radius, wheelbase); odometry_.set_velocity_rolling_window_size(params_.velocity_rolling_window_size); // topics QoS @@ -279,9 +280,12 @@ AckermannSteeringController::command_interface_configuration() const command_interfaces_config.names.reserve(NR_CMD_ITFS); command_interfaces_config.names.push_back( params_.rear_wheel_name + "/" + hardware_interface::HW_IF_VELOCITY); - command_interfaces_config.names.push_back( - params_.front_steer_name + "/" + hardware_interface::HW_IF_POSITION); + for (int i = 0; i < params_.front_steer_names.size(); i++) + { + command_interfaces_config.names.push_back( + params_.front_steer_names[i] + "/" + hardware_interface::HW_IF_POSITION); + } return command_interfaces_config; } @@ -295,8 +299,12 @@ AckermannSteeringController::state_interface_configuration() const const auto rear_wheel_feedback = params_.position_feedback ? hardware_interface::HW_IF_POSITION : hardware_interface::HW_IF_VELOCITY; state_interfaces_config.names.push_back(params_.rear_wheel_name + "/" + rear_wheel_feedback); - state_interfaces_config.names.push_back( - params_.front_steer_name + "/" + hardware_interface::HW_IF_POSITION); + + for (int i = 0; i < params_.front_steer_names.size(); i++) + { + state_interfaces_config.names.push_back( + params_.front_steer_names[i] + "/" + hardware_interface::HW_IF_POSITION); + } return state_interfaces_config; } diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.yaml b/ackermann_steering_controller/src/ackermann_steering_controller.yaml index 6e4b6dcba3..191e2dbc0b 100644 --- a/ackermann_steering_controller/src/ackermann_steering_controller.yaml +++ b/ackermann_steering_controller/src/ackermann_steering_controller.yaml @@ -16,9 +16,9 @@ ackermann_steering_controller: } - front_steer_name: { - type: string, - default_value: "front_steer_joint", + front_steer_names: { + type: string_array, + default_value: ["front_steer_joint"], description: "Name of the front steer wheel.", read_only: true, @@ -48,6 +48,21 @@ ackermann_steering_controller: } + wheelbase_multiplier: { + type: double, + default_value: 1.0, + description: "Wheelbase will be multiplied by value of the wheelbase_multiplier.", + read_only: false, + + } + + wheelbase: { + type: double, + default_value: 0.0, + description: "Wheelbase length.", + read_only: false, + + } wheel_radius: { type: double, default_value: 0.0, From 6a17f951f401210597e82b06f955555d6197a740 Mon Sep 17 00:00:00 2001 From: petkovich Date: Wed, 7 Dec 2022 18:24:46 +0100 Subject: [PATCH 036/186] direct kinematics --- .../ackermann_odometry/ackermann_odometry.hpp | 15 +++++++++++ .../src/ackermann_odometry.cpp | 27 +++++++++++++++++++ .../src/ackermann_steering_controller.cpp | 25 ++++++++++------- .../src/ackermann_steering_controller.yaml | 11 ++++---- 4 files changed, 64 insertions(+), 14 deletions(-) diff --git a/ackermann_steering_controller/include/ackermann_odometry/ackermann_odometry.hpp b/ackermann_steering_controller/include/ackermann_odometry/ackermann_odometry.hpp index dd8e855bfb..e23e1b19c7 100644 --- a/ackermann_steering_controller/include/ackermann_odometry/ackermann_odometry.hpp +++ b/ackermann_steering_controller/include/ackermann_odometry/ackermann_odometry.hpp @@ -129,6 +129,21 @@ class AckermannOdometry */ void set_velocity_rolling_window_size(size_t velocity_rolling_window_size); + /** + * \brief TODO + * \param Vx TODO + * \param theta_dot TODO + * \param wheelbase TODO + */ + double convert_trans_rot_vel_to_steering_angle(double Vx, double theta_dot, double wheelbase); + + /** + * \brief TODO + * \param Vx TODO + * \param theta_dot TODO + */ + std::tuple twist_to_ackermann(double Vx, double theta_dot); + private: /// Rolling mean accumulator and window: typedef bacc::accumulator_set > RollingMeanAcc; diff --git a/ackermann_steering_controller/src/ackermann_odometry.cpp b/ackermann_steering_controller/src/ackermann_odometry.cpp index b3e5e2b689..810c015ced 100644 --- a/ackermann_steering_controller/src/ackermann_odometry.cpp +++ b/ackermann_steering_controller/src/ackermann_odometry.cpp @@ -152,6 +152,33 @@ void AckermannOdometry::set_velocity_rolling_window_size(size_t velocity_rolling reset_accumulators(); } +double AckermannOdometry::convert_trans_rot_vel_to_steering_angle( + double Vx, double theta_dot, double wheelbase) +{ + if (theta_dot == 0 || Vx == 0) + { + return 0; + } + return std::atan(theta_dot * wheelbase / Vx); +} + +std::tuple AckermannOdometry::twist_to_ackermann(double Vx, double theta_dot) +{ + // using naming convention in http://users.isr.ist.utl.pt/~mir/cadeiras/robmovel/Kinematics.pdf + double alpha, Ws; + + if (Vx == 0 && theta_dot != 0) + { // is spin action + alpha = theta_dot > 0 ? M_PI_2 : -M_PI_2; + Ws = abs(theta_dot) * wheel_separation_ / wheel_radius_; + return std::make_tuple(alpha, Ws); + } + + alpha = convert_trans_rot_vel_to_steering_angle(Vx, theta_dot, wheel_separation_); + Ws = Vx / (wheel_radius_ * std::cos(alpha)); + return std::make_tuple(alpha, Ws); +} + void AckermannOdometry::integrate_runge_kutta_2(double linear, double angular) { const double direction = heading_ + angular * 0.5; diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.cpp b/ackermann_steering_controller/src/ackermann_steering_controller.cpp index 5ab917920e..3ce204cecc 100644 --- a/ackermann_steering_controller/src/ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/src/ackermann_steering_controller.cpp @@ -276,11 +276,13 @@ AckermannSteeringController::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(NR_CMD_ITFS); - command_interfaces_config.names.push_back( - params_.rear_wheel_name + "/" + hardware_interface::HW_IF_VELOCITY); + for (int i = 0; i < params_.front_steer_names.size(); i++) + { + command_interfaces_config.names.push_back( + params_.rear_wheel_names[i] + "/" + hardware_interface::HW_IF_VELOCITY); + } for (int i = 0; i < params_.front_steer_names.size(); i++) { command_interfaces_config.names.push_back( @@ -298,7 +300,12 @@ AckermannSteeringController::state_interface_configuration() const state_interfaces_config.names.reserve(NR_STATE_ITFS); const auto rear_wheel_feedback = params_.position_feedback ? hardware_interface::HW_IF_POSITION : hardware_interface::HW_IF_VELOCITY; - state_interfaces_config.names.push_back(params_.rear_wheel_name + "/" + rear_wheel_feedback); + + for (int i = 0; i < params_.front_steer_names.size(); i++) + { + state_interfaces_config.names.push_back( + params_.rear_wheel_names[i] + "/" + rear_wheel_feedback); + } for (int i = 0; i < params_.front_steer_names.size(); i++) { @@ -415,14 +422,14 @@ controller_interface::return_type AckermannSteeringController::update_and_write_ if (!std::isnan(reference_interfaces_[0]) && !std::isnan(reference_interfaces_[1])) { // store and set commands - last_linear_velocity_ = reference_interfaces_[0]; - last_angular_velocity_ = reference_interfaces_[1]; - + const double linear_command = reference_interfaces_[0]; + const double angular_command = reference_interfaces_[1]; + auto [alpha_write, Ws_write] = odometry_.twist_to_ackermann(linear_command, angular_command); // previous_publish_timestamp_ = time; // omega = linear_vel / radius - command_interfaces_[0].set_value(last_linear_velocity_ / params_.wheel_radius); - command_interfaces_[1].set_value(last_angular_velocity_); + command_interfaces_[0].set_value(Ws_write); + command_interfaces_[1].set_value(alpha_write); } if (ref_timeout_ == rclcpp::Duration::from_seconds(0) || is_in_chained_mode()) diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.yaml b/ackermann_steering_controller/src/ackermann_steering_controller.yaml index 191e2dbc0b..bd33eb00af 100644 --- a/ackermann_steering_controller/src/ackermann_steering_controller.yaml +++ b/ackermann_steering_controller/src/ackermann_steering_controller.yaml @@ -8,10 +8,10 @@ ackermann_steering_controller: # } } - rear_wheel_name: { - type: string, - default_value: "rear_wheel_joint", - description: "Name of the rear wheel.", + rear_wheel_names: { + type: string_array, + default_value: ["rear_wheel_joint"], + description: "Name of rear wheels.", read_only: true, } @@ -19,7 +19,7 @@ ackermann_steering_controller: front_steer_names: { type: string_array, default_value: ["front_steer_joint"], - description: "Name of the front steer wheel.", + description: "Names of front steer wheels.", read_only: true, } @@ -63,6 +63,7 @@ ackermann_steering_controller: read_only: false, } + wheel_radius: { type: double, default_value: 0.0, From dd1df2cbdcc16fa5383688f4fe28616968764b0d Mon Sep 17 00:00:00 2001 From: petkovich Date: Thu, 8 Dec 2022 12:20:16 +0100 Subject: [PATCH 037/186] replaced bacc acumulator with rcpputils --- .../ackermann_odometry/ackermann_odometry.hpp | 16 ++++------ .../src/ackermann_odometry.cpp | 30 ++++++++++--------- 2 files changed, 21 insertions(+), 25 deletions(-) diff --git a/ackermann_steering_controller/include/ackermann_odometry/ackermann_odometry.hpp b/ackermann_steering_controller/include/ackermann_odometry/ackermann_odometry.hpp index e23e1b19c7..087f2e7996 100644 --- a/ackermann_steering_controller/include/ackermann_odometry/ackermann_odometry.hpp +++ b/ackermann_steering_controller/include/ackermann_odometry/ackermann_odometry.hpp @@ -24,16 +24,11 @@ #include "realtime_tools/realtime_buffer.h" #include "realtime_tools/realtime_publisher.h" -//#include -#include -#include -#include #include +#include "rcpputils/rolling_mean_accumulator.hpp" namespace ackermann_odometry { -namespace bacc = boost::accumulators; - /** * \brief The Odometry class handles odometry readings * (2D pose and velocity with related timestamp) @@ -146,8 +141,7 @@ class AckermannOdometry private: /// Rolling mean accumulator and window: - typedef bacc::accumulator_set > RollingMeanAcc; - typedef bacc::tag::rolling_window RollingWindow; + typedef rcpputils::RollingMeanAccumulator RollingMeanAccumulator; /** * \brief Integrates the velocities (linear and angular) using 2nd order Runge-Kutta @@ -188,9 +182,9 @@ class AckermannOdometry double rear_wheel_old_pos_; /// Rolling mean accumulators for the linar and angular velocities: - int velocity_rolling_window_size_; - RollingMeanAcc linear_acc_; - RollingMeanAcc angular_acc_; + size_t velocity_rolling_window_size_; + RollingMeanAccumulator linear_acc_; + RollingMeanAccumulator angular_acc_; /// Integration function, used to integrate the odometry: IntegrationFunction integrate_fun_; diff --git a/ackermann_steering_controller/src/ackermann_odometry.cpp b/ackermann_steering_controller/src/ackermann_odometry.cpp index 810c015ced..89dd5df043 100644 --- a/ackermann_steering_controller/src/ackermann_odometry.cpp +++ b/ackermann_steering_controller/src/ackermann_odometry.cpp @@ -26,7 +26,6 @@ namespace ackermann_odometry { -namespace bacc = boost::accumulators; // using namespace ackermann_steering_controller_ros2; // ackermann_steering_controller_ros2::Params params; @@ -41,9 +40,9 @@ AckermannOdometry::AckermannOdometry(size_t velocity_rolling_window_size) wheelbase_(0.0), wheel_radius_(0.0), rear_wheel_old_pos_(0.0), - velocity_rolling_window_size_(0.0), - linear_acc_(RollingWindow::window_size = velocity_rolling_window_size), - angular_acc_(RollingWindow::window_size = velocity_rolling_window_size), + velocity_rolling_window_size_(velocity_rolling_window_size), + linear_acc_(velocity_rolling_window_size), + angular_acc_(velocity_rolling_window_size), integrate_fun_(std::bind( &AckermannOdometry::integrate_exact, this, std::placeholders::_1, std::placeholders::_2)) @@ -90,11 +89,11 @@ bool AckermannOdometry::update_from_position( } /// Estimate speeds using a rolling mean to filter them out: - linear_acc_(linear_velocity); - angular_acc_(angular / dt); + linear_acc_.accumulate(linear_velocity); + angular_acc_.accumulate(angular / dt); - linear_ = bacc::rolling_mean(linear_acc_); - angular_ = bacc::rolling_mean(angular_acc_); + linear_ = linear_acc_.getRollingMean(); + angular_ = angular_acc_.getRollingMean(); return true; } @@ -118,11 +117,11 @@ bool AckermannOdometry::update_from_velocity( } /// Estimate speeds using a rolling mean to filter them out: - linear_acc_(linear_velocity); - angular_acc_(angular); + linear_acc_.accumulate(linear_velocity); + angular_acc_.accumulate(angular); - linear_ = bacc::rolling_mean(linear_acc_); - angular_ = bacc::rolling_mean(angular_acc_); + linear_ = linear_acc_.getRollingMean(); + angular_ = angular_acc_.getRollingMean(); return true; } @@ -152,6 +151,7 @@ void AckermannOdometry::set_velocity_rolling_window_size(size_t velocity_rolling reset_accumulators(); } +//TODO: change functions depending on fwd kinematics model double AckermannOdometry::convert_trans_rot_vel_to_steering_angle( double Vx, double theta_dot, double wheelbase) { @@ -162,6 +162,7 @@ double AckermannOdometry::convert_trans_rot_vel_to_steering_angle( return std::atan(theta_dot * wheelbase / Vx); } +//TODO: change functions depending on fwd kinematics model std::tuple AckermannOdometry::twist_to_ackermann(double Vx, double theta_dot) { // using naming convention in http://users.isr.ist.utl.pt/~mir/cadeiras/robmovel/Kinematics.pdf @@ -211,8 +212,9 @@ void AckermannOdometry::integrate_exact(double linear, double angular) void AckermannOdometry::reset_accumulators() { - linear_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_); - angular_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_); + linear_acc_ = RollingMeanAccumulator(velocity_rolling_window_size_); + angular_acc_ = RollingMeanAccumulator(velocity_rolling_window_size_); + // TODO: angular rolling window size? } } // namespace ackermann_odometry From 7583c998cb3190cd895bb1365e413fe1f6d6a227 Mon Sep 17 00:00:00 2001 From: petkovich Date: Thu, 8 Dec 2022 14:23:22 +0100 Subject: [PATCH 038/186] migrated to steering_controllers package --- .../CMakeLists.txt | 19 +----- .../ackermann_steering_controller}/LICENSE | 0 .../ackermann_steering_controller.xml | 0 .../ackermann_steering_controller.hpp | 4 +- ...kermann_steering_controller_parameters.hpp | 0 .../visibility_control.h | 0 .../package.xml | 1 + .../src/ackermann_steering_controller.cpp | 0 .../src/ackermann_steering_controller.yaml | 0 .../ackermann_steering_controller_params.yaml | 0 ...steering_controller_preceeding_params.yaml | 0 .../test_ackermann_steering_controller.cpp | 0 .../test_ackermann_steering_controller.hpp | 0 ...kermann_steering_controller_preceeding.cpp | 0 ...est_load_ackermann_steering_controller.cpp | 0 .../steering_odometry/CMakeLists.txt | 66 +++++++++++++++++++ .../steering_odometry/steering_odometry.hpp | 8 +-- .../steering_odometry/package.xml | 20 ++++++ .../src/steering_odometry.cpp | 35 +++++----- 19 files changed, 111 insertions(+), 42 deletions(-) rename {ackermann_steering_controller => steering_controllers/ackermann_steering_controller}/CMakeLists.txt (91%) rename {ackermann_steering_controller => steering_controllers/ackermann_steering_controller}/LICENSE (100%) rename {ackermann_steering_controller => steering_controllers/ackermann_steering_controller}/ackermann_steering_controller.xml (100%) rename {ackermann_steering_controller => steering_controllers/ackermann_steering_controller}/include/ackermann_steering_controller/ackermann_steering_controller.hpp (98%) rename {ackermann_steering_controller => steering_controllers/ackermann_steering_controller}/include/ackermann_steering_controller/validate_ackermann_steering_controller_parameters.hpp (100%) rename {ackermann_steering_controller => steering_controllers/ackermann_steering_controller}/include/ackermann_steering_controller/visibility_control.h (100%) rename {ackermann_steering_controller => steering_controllers/ackermann_steering_controller}/package.xml (97%) rename {ackermann_steering_controller => steering_controllers/ackermann_steering_controller}/src/ackermann_steering_controller.cpp (100%) rename {ackermann_steering_controller => steering_controllers/ackermann_steering_controller}/src/ackermann_steering_controller.yaml (100%) rename {ackermann_steering_controller => steering_controllers/ackermann_steering_controller}/test/ackermann_steering_controller_params.yaml (100%) rename {ackermann_steering_controller => steering_controllers/ackermann_steering_controller}/test/ackermann_steering_controller_preceeding_params.yaml (100%) rename {ackermann_steering_controller => steering_controllers/ackermann_steering_controller}/test/test_ackermann_steering_controller.cpp (100%) rename {ackermann_steering_controller => steering_controllers/ackermann_steering_controller}/test/test_ackermann_steering_controller.hpp (100%) rename {ackermann_steering_controller => steering_controllers/ackermann_steering_controller}/test/test_ackermann_steering_controller_preceeding.cpp (100%) rename {ackermann_steering_controller => steering_controllers/ackermann_steering_controller}/test/test_load_ackermann_steering_controller.cpp (100%) create mode 100644 steering_controllers/steering_odometry/CMakeLists.txt rename ackermann_steering_controller/include/ackermann_odometry/ackermann_odometry.hpp => steering_controllers/steering_odometry/include/steering_odometry/steering_odometry.hpp (97%) create mode 100644 steering_controllers/steering_odometry/package.xml rename ackermann_steering_controller/src/ackermann_odometry.cpp => steering_controllers/steering_odometry/src/steering_odometry.cpp (83%) diff --git a/ackermann_steering_controller/CMakeLists.txt b/steering_controllers/ackermann_steering_controller/CMakeLists.txt similarity index 91% rename from ackermann_steering_controller/CMakeLists.txt rename to steering_controllers/ackermann_steering_controller/CMakeLists.txt index a4699c9a3c..f5801b9c5f 100644 --- a/ackermann_steering_controller/CMakeLists.txt +++ b/steering_controllers/ackermann_steering_controller/CMakeLists.txt @@ -22,6 +22,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS tf2_geometry_msgs ackermann_msgs custom_messages + steering_odometry ) find_package(ament_cmake REQUIRED) @@ -51,22 +52,6 @@ generate_parameter_library(ackermann_steering_controller_parameters ) -add_library( - ackermann_odometry - SHARED - src/ackermann_odometry.cpp -) -target_include_directories(ackermann_odometry PRIVATE include) -ament_target_dependencies(ackermann_odometry ${THIS_PACKAGE_INCLUDE_DEPENDS}) - -install( - TARGETS - ackermann_odometry - RUNTIME DESTINATION bin - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib -) - add_library( ackermann_steering_controller @@ -74,7 +59,7 @@ add_library( src/ackermann_steering_controller.cpp ) target_include_directories(ackermann_steering_controller PRIVATE include) -target_link_libraries(ackermann_steering_controller ackermann_steering_controller_parameters ackermann_odometry) +target_link_libraries(ackermann_steering_controller ackermann_steering_controller_parameters) ament_target_dependencies(ackermann_steering_controller ${THIS_PACKAGE_INCLUDE_DEPENDS}) target_compile_definitions(ackermann_steering_controller PRIVATE "ACKERMANN_STEERING_CONTROLLER_BUILDING_DLL") diff --git a/ackermann_steering_controller/LICENSE b/steering_controllers/ackermann_steering_controller/LICENSE similarity index 100% rename from ackermann_steering_controller/LICENSE rename to steering_controllers/ackermann_steering_controller/LICENSE diff --git a/ackermann_steering_controller/ackermann_steering_controller.xml b/steering_controllers/ackermann_steering_controller/ackermann_steering_controller.xml similarity index 100% rename from ackermann_steering_controller/ackermann_steering_controller.xml rename to steering_controllers/ackermann_steering_controller/ackermann_steering_controller.xml diff --git a/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp b/steering_controllers/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp similarity index 98% rename from ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp rename to steering_controllers/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp index 835a82df6f..33ccc0c4d6 100644 --- a/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp +++ b/steering_controllers/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp @@ -23,7 +23,6 @@ #include #include -#include "ackermann_odometry/ackermann_odometry.hpp" #include "ackermann_steering_controller/visibility_control.h" #include "ackermann_steering_controller_parameters.hpp" #include "controller_interface/chainable_controller_interface.hpp" @@ -33,6 +32,7 @@ #include "realtime_tools/realtime_buffer.h" #include "realtime_tools/realtime_publisher.h" #include "std_srvs/srv/set_bool.hpp" +#include "steering_odometry/steering_odometry.hpp" // TODO(anyone): Replace with controller specific messages #include "ackermann_msgs/msg/ackermann_drive.hpp" @@ -140,7 +140,7 @@ class AckermannSteeringController : public controller_interface::ChainableContro }; // Odometry related: - ackermann_odometry::AckermannOdometry odometry_; + steering_odometry::SteeringOdometry odometry_; using AckermanControllerState = custom_messages::msg::AckermanControllerState; ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC diff --git a/ackermann_steering_controller/include/ackermann_steering_controller/validate_ackermann_steering_controller_parameters.hpp b/steering_controllers/ackermann_steering_controller/include/ackermann_steering_controller/validate_ackermann_steering_controller_parameters.hpp similarity index 100% rename from ackermann_steering_controller/include/ackermann_steering_controller/validate_ackermann_steering_controller_parameters.hpp rename to steering_controllers/ackermann_steering_controller/include/ackermann_steering_controller/validate_ackermann_steering_controller_parameters.hpp diff --git a/ackermann_steering_controller/include/ackermann_steering_controller/visibility_control.h b/steering_controllers/ackermann_steering_controller/include/ackermann_steering_controller/visibility_control.h similarity index 100% rename from ackermann_steering_controller/include/ackermann_steering_controller/visibility_control.h rename to steering_controllers/ackermann_steering_controller/include/ackermann_steering_controller/visibility_control.h diff --git a/ackermann_steering_controller/package.xml b/steering_controllers/ackermann_steering_controller/package.xml similarity index 97% rename from ackermann_steering_controller/package.xml rename to steering_controllers/ackermann_steering_controller/package.xml index 087545941e..603366d364 100644 --- a/ackermann_steering_controller/package.xml +++ b/steering_controllers/ackermann_steering_controller/package.xml @@ -26,6 +26,7 @@ tf2_msgs tf2_geometry_msgs ackermann_msgs + steering_odometry custom_messages ament_lint_auto diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.cpp b/steering_controllers/ackermann_steering_controller/src/ackermann_steering_controller.cpp similarity index 100% rename from ackermann_steering_controller/src/ackermann_steering_controller.cpp rename to steering_controllers/ackermann_steering_controller/src/ackermann_steering_controller.cpp diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.yaml b/steering_controllers/ackermann_steering_controller/src/ackermann_steering_controller.yaml similarity index 100% rename from ackermann_steering_controller/src/ackermann_steering_controller.yaml rename to steering_controllers/ackermann_steering_controller/src/ackermann_steering_controller.yaml diff --git a/ackermann_steering_controller/test/ackermann_steering_controller_params.yaml b/steering_controllers/ackermann_steering_controller/test/ackermann_steering_controller_params.yaml similarity index 100% rename from ackermann_steering_controller/test/ackermann_steering_controller_params.yaml rename to steering_controllers/ackermann_steering_controller/test/ackermann_steering_controller_params.yaml diff --git a/ackermann_steering_controller/test/ackermann_steering_controller_preceeding_params.yaml b/steering_controllers/ackermann_steering_controller/test/ackermann_steering_controller_preceeding_params.yaml similarity index 100% rename from ackermann_steering_controller/test/ackermann_steering_controller_preceeding_params.yaml rename to steering_controllers/ackermann_steering_controller/test/ackermann_steering_controller_preceeding_params.yaml diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp b/steering_controllers/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp similarity index 100% rename from ackermann_steering_controller/test/test_ackermann_steering_controller.cpp rename to steering_controllers/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp b/steering_controllers/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp similarity index 100% rename from ackermann_steering_controller/test/test_ackermann_steering_controller.hpp rename to steering_controllers/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp b/steering_controllers/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp similarity index 100% rename from ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp rename to steering_controllers/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp diff --git a/ackermann_steering_controller/test/test_load_ackermann_steering_controller.cpp b/steering_controllers/ackermann_steering_controller/test/test_load_ackermann_steering_controller.cpp similarity index 100% rename from ackermann_steering_controller/test/test_load_ackermann_steering_controller.cpp rename to steering_controllers/ackermann_steering_controller/test/test_load_ackermann_steering_controller.cpp diff --git a/steering_controllers/steering_odometry/CMakeLists.txt b/steering_controllers/steering_odometry/CMakeLists.txt new file mode 100644 index 0000000000..2c47326dfb --- /dev/null +++ b/steering_controllers/steering_odometry/CMakeLists.txt @@ -0,0 +1,66 @@ +cmake_minimum_required(VERSION 3.8) +project(steering_odometry) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +set(THIS_PACKAGE_INCLUDE_DEPENDS + rclcpp + realtime_tools +) +# find dependencies +find_package(ament_cmake REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +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() + +add_library( + steering_odometry + SHARED + src/steering_odometry.cpp +) +target_include_directories(steering_odometry PRIVATE include) +ament_target_dependencies(steering_odometry ${THIS_PACKAGE_INCLUDE_DEPENDS}) + +install( + TARGETS + steering_odometry + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib +) + +install( + DIRECTORY include/ + DESTINATION include +) + + +ament_export_include_directories( + include +) + +ament_export_dependencies( + ${THIS_PACKAGE_INCLUDE_DEPENDS} +) +ament_export_libraries( + steering_odometry +) + +ament_package() diff --git a/ackermann_steering_controller/include/ackermann_odometry/ackermann_odometry.hpp b/steering_controllers/steering_odometry/include/steering_odometry/steering_odometry.hpp similarity index 97% rename from ackermann_steering_controller/include/ackermann_odometry/ackermann_odometry.hpp rename to steering_controllers/steering_odometry/include/steering_odometry/steering_odometry.hpp index 087f2e7996..c0696d8aa9 100644 --- a/ackermann_steering_controller/include/ackermann_odometry/ackermann_odometry.hpp +++ b/steering_controllers/steering_odometry/include/steering_odometry/steering_odometry.hpp @@ -27,13 +27,13 @@ #include #include "rcpputils/rolling_mean_accumulator.hpp" -namespace ackermann_odometry +namespace steering_odometry { /** * \brief The Odometry class handles odometry readings * (2D pose and velocity with related timestamp) */ -class AckermannOdometry +class SteeringOdometry { public: /// Integration function, used to integrate the odometry: @@ -47,7 +47,7 @@ class AckermannOdometry * */ // ackermann_steering_controller_ros2::Params params; - explicit AckermannOdometry(size_t velocity_rolling_window_size = 10); + explicit SteeringOdometry(size_t velocity_rolling_window_size = 10); /** * \brief Initialize the odometry @@ -189,4 +189,4 @@ class AckermannOdometry /// Integration function, used to integrate the odometry: IntegrationFunction integrate_fun_; }; -} // namespace ackermann_odometry +} // namespace steering_odometry diff --git a/steering_controllers/steering_odometry/package.xml b/steering_controllers/steering_odometry/package.xml new file mode 100644 index 0000000000..fe32d2d1f6 --- /dev/null +++ b/steering_controllers/steering_odometry/package.xml @@ -0,0 +1,20 @@ + + + + steering_odometry + 0.0.0 + TODO: Package description + tomislav + TODO: License declaration + + ament_cmake + realtime_tools + rcpputils + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/ackermann_steering_controller/src/ackermann_odometry.cpp b/steering_controllers/steering_odometry/src/steering_odometry.cpp similarity index 83% rename from ackermann_steering_controller/src/ackermann_odometry.cpp rename to steering_controllers/steering_odometry/src/steering_odometry.cpp index 89dd5df043..9ae29f9175 100644 --- a/ackermann_steering_controller/src/ackermann_odometry.cpp +++ b/steering_controllers/steering_odometry/src/steering_odometry.cpp @@ -19,17 +19,14 @@ * Author: Dr. Ing. Denis Stogl */ -#include "ackermann_odometry/ackermann_odometry.hpp" +#include "steering_odometry/steering_odometry.hpp" #include #include -namespace ackermann_odometry +namespace steering_odometry { -// using namespace ackermann_steering_controller_ros2; -// ackermann_steering_controller_ros2::Params params; - -AckermannOdometry::AckermannOdometry(size_t velocity_rolling_window_size) +SteeringOdometry::SteeringOdometry(size_t velocity_rolling_window_size) : timestamp_(0.0), x_(0.0), y_(0.0), @@ -44,12 +41,12 @@ AckermannOdometry::AckermannOdometry(size_t velocity_rolling_window_size) linear_acc_(velocity_rolling_window_size), angular_acc_(velocity_rolling_window_size), integrate_fun_(std::bind( - &AckermannOdometry::integrate_exact, this, std::placeholders::_1, std::placeholders::_2)) + &SteeringOdometry::integrate_exact, this, std::placeholders::_1, std::placeholders::_2)) { } -void AckermannOdometry::init(const rclcpp::Time & time) +void SteeringOdometry::init(const rclcpp::Time & time) { // Reset accumulators and timestamp: reset_accumulators(); @@ -57,7 +54,7 @@ void AckermannOdometry::init(const rclcpp::Time & time) } // TODO(destogl): enable also velocity interface to update using velocity from the rear wheel -bool AckermannOdometry::update_from_position( +bool SteeringOdometry::update_from_position( const double rear_wheel_pos, const double front_steer_pos, const double dt) { /// Get current wheel joint positions: @@ -98,7 +95,7 @@ bool AckermannOdometry::update_from_position( return true; } -bool AckermannOdometry::update_from_velocity( +bool SteeringOdometry::update_from_velocity( const double rear_wheel_vel, const double front_steer_pos, const double dt) { // (right_wheel_est_vel + left_wheel_est_vel) * 0.5; @@ -126,7 +123,7 @@ bool AckermannOdometry::update_from_velocity( return true; } -void AckermannOdometry::update_open_loop(const double linear, const double angular, const double dt) +void SteeringOdometry::update_open_loop(const double linear, const double angular, const double dt) { /// Save last linear and angular velocity: linear_ = linear; @@ -136,7 +133,7 @@ void AckermannOdometry::update_open_loop(const double linear, const double angul integrate_fun_(linear * dt, angular * dt); } -void AckermannOdometry::set_wheel_params( +void SteeringOdometry::set_wheel_params( double wheel_separation, double wheel_radius, double wheelbase) { wheel_separation_ = wheel_separation; @@ -144,7 +141,7 @@ void AckermannOdometry::set_wheel_params( wheelbase_ = wheelbase; } -void AckermannOdometry::set_velocity_rolling_window_size(size_t velocity_rolling_window_size) +void SteeringOdometry::set_velocity_rolling_window_size(size_t velocity_rolling_window_size) { velocity_rolling_window_size_ = velocity_rolling_window_size; @@ -152,7 +149,7 @@ void AckermannOdometry::set_velocity_rolling_window_size(size_t velocity_rolling } //TODO: change functions depending on fwd kinematics model -double AckermannOdometry::convert_trans_rot_vel_to_steering_angle( +double SteeringOdometry::convert_trans_rot_vel_to_steering_angle( double Vx, double theta_dot, double wheelbase) { if (theta_dot == 0 || Vx == 0) @@ -163,7 +160,7 @@ double AckermannOdometry::convert_trans_rot_vel_to_steering_angle( } //TODO: change functions depending on fwd kinematics model -std::tuple AckermannOdometry::twist_to_ackermann(double Vx, double theta_dot) +std::tuple SteeringOdometry::twist_to_ackermann(double Vx, double theta_dot) { // using naming convention in http://users.isr.ist.utl.pt/~mir/cadeiras/robmovel/Kinematics.pdf double alpha, Ws; @@ -180,7 +177,7 @@ std::tuple AckermannOdometry::twist_to_ackermann(double Vx, doub return std::make_tuple(alpha, Ws); } -void AckermannOdometry::integrate_runge_kutta_2(double linear, double angular) +void SteeringOdometry::integrate_runge_kutta_2(double linear, double angular) { const double direction = heading_ + angular * 0.5; @@ -195,7 +192,7 @@ void AckermannOdometry::integrate_runge_kutta_2(double linear, double angular) * \param linear * \param angular */ -void AckermannOdometry::integrate_exact(double linear, double angular) +void SteeringOdometry::integrate_exact(double linear, double angular) { if (fabs(angular) < 1e-6) integrate_runge_kutta_2(linear, angular); @@ -210,11 +207,11 @@ void AckermannOdometry::integrate_exact(double linear, double angular) } } -void AckermannOdometry::reset_accumulators() +void SteeringOdometry::reset_accumulators() { linear_acc_ = RollingMeanAccumulator(velocity_rolling_window_size_); angular_acc_ = RollingMeanAccumulator(velocity_rolling_window_size_); // TODO: angular rolling window size? } -} // namespace ackermann_odometry +} // namespace steering_odometry From e7c1a838bd36499197d0972f1127ba7aaf99937e Mon Sep 17 00:00:00 2001 From: petkovich Date: Thu, 8 Dec 2022 14:26:37 +0100 Subject: [PATCH 039/186] cleaned warnings --- .../ackermann_steering_controller.hpp | 3 +-- .../src/ackermann_steering_controller.cpp | 8 ++++---- 2 files changed, 5 insertions(+), 6 deletions(-) diff --git a/steering_controllers/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp b/steering_controllers/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp index 33ccc0c4d6..256c4351e0 100644 --- a/steering_controllers/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp +++ b/steering_controllers/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp @@ -127,7 +127,7 @@ class AckermannSteeringController : public controller_interface::ChainableContro /// Odometry related: rclcpp::Duration publish_period_ = rclcpp::Duration::from_nanoseconds(0); - ; + bool open_loop_; /// Velocity command related: struct Commands @@ -143,7 +143,6 @@ class AckermannSteeringController : public controller_interface::ChainableContro steering_odometry::SteeringOdometry odometry_; using AckermanControllerState = custom_messages::msg::AckermanControllerState; - ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC AckermanControllerState published_state_; using ControllerStatePublisher = realtime_tools::RealtimePublisher; diff --git a/steering_controllers/ackermann_steering_controller/src/ackermann_steering_controller.cpp b/steering_controllers/ackermann_steering_controller/src/ackermann_steering_controller.cpp index 3ce204cecc..37700d924d 100644 --- a/steering_controllers/ackermann_steering_controller/src/ackermann_steering_controller.cpp +++ b/steering_controllers/ackermann_steering_controller/src/ackermann_steering_controller.cpp @@ -278,12 +278,12 @@ AckermannSteeringController::command_interface_configuration() const command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; command_interfaces_config.names.reserve(NR_CMD_ITFS); - for (int i = 0; i < params_.front_steer_names.size(); i++) + for (size_t i = 0; i < params_.front_steer_names.size(); i++) { command_interfaces_config.names.push_back( params_.rear_wheel_names[i] + "/" + hardware_interface::HW_IF_VELOCITY); } - for (int i = 0; i < params_.front_steer_names.size(); i++) + for (size_t i = 0; i < params_.front_steer_names.size(); i++) { command_interfaces_config.names.push_back( params_.front_steer_names[i] + "/" + hardware_interface::HW_IF_POSITION); @@ -301,13 +301,13 @@ AckermannSteeringController::state_interface_configuration() const const auto rear_wheel_feedback = params_.position_feedback ? hardware_interface::HW_IF_POSITION : hardware_interface::HW_IF_VELOCITY; - for (int i = 0; i < params_.front_steer_names.size(); i++) + for (size_t i = 0; i < params_.front_steer_names.size(); i++) { state_interfaces_config.names.push_back( params_.rear_wheel_names[i] + "/" + rear_wheel_feedback); } - for (int i = 0; i < params_.front_steer_names.size(); i++) + for (size_t i = 0; i < params_.front_steer_names.size(); i++) { state_interfaces_config.names.push_back( params_.front_steer_names[i] + "/" + hardware_interface::HW_IF_POSITION); From 931553d1623f1928c49915c3f1762a0923cc7da0 Mon Sep 17 00:00:00 2001 From: petkovich Date: Thu, 8 Dec 2022 16:08:04 +0100 Subject: [PATCH 040/186] move tricycle, edit ackermann cmakelists --- .../CMakeLists.txt | 24 +++----- .../tricycle_controller}/CHANGELOG.rst | 0 .../tricycle_controller}/CMakeLists.txt | 56 ++++++++----------- .../config/tricycle_drive_controller.yaml | 0 .../tricycle_controller}/doc/userdoc.rst | 0 .../include/tricycle_controller/odometry.hpp | 0 .../tricycle_controller/steering_limiter.hpp | 0 .../tricycle_controller/traction_limiter.hpp | 0 .../tricycle_controller.hpp | 0 .../tricycle_controller/visibility_control.h | 0 .../tricycle_controller}/package.xml | 0 .../tricycle_controller}/src/odometry.cpp | 0 .../src/steering_limiter.cpp | 0 .../src/traction_limiter.cpp | 0 .../src/tricycle_controller.cpp | 0 .../test/config/test_tricycle_controller.yaml | 0 .../test/test_load_tricycle_controller.cpp | 0 .../test/test_tricycle_controller.cpp | 0 .../tricycle_controller.xml | 0 19 files changed, 31 insertions(+), 49 deletions(-) rename {tricycle_controller => steering_controllers/tricycle_controller}/CHANGELOG.rst (100%) rename {tricycle_controller => steering_controllers/tricycle_controller}/CMakeLists.txt (78%) rename {tricycle_controller => steering_controllers/tricycle_controller}/config/tricycle_drive_controller.yaml (100%) rename {tricycle_controller => steering_controllers/tricycle_controller}/doc/userdoc.rst (100%) rename {tricycle_controller => steering_controllers/tricycle_controller}/include/tricycle_controller/odometry.hpp (100%) rename {tricycle_controller => steering_controllers/tricycle_controller}/include/tricycle_controller/steering_limiter.hpp (100%) rename {tricycle_controller => steering_controllers/tricycle_controller}/include/tricycle_controller/traction_limiter.hpp (100%) rename {tricycle_controller => steering_controllers/tricycle_controller}/include/tricycle_controller/tricycle_controller.hpp (100%) rename {tricycle_controller => steering_controllers/tricycle_controller}/include/tricycle_controller/visibility_control.h (100%) rename {tricycle_controller => steering_controllers/tricycle_controller}/package.xml (100%) rename {tricycle_controller => steering_controllers/tricycle_controller}/src/odometry.cpp (100%) rename {tricycle_controller => steering_controllers/tricycle_controller}/src/steering_limiter.cpp (100%) rename {tricycle_controller => steering_controllers/tricycle_controller}/src/traction_limiter.cpp (100%) rename {tricycle_controller => steering_controllers/tricycle_controller}/src/tricycle_controller.cpp (100%) rename {tricycle_controller => steering_controllers/tricycle_controller}/test/config/test_tricycle_controller.yaml (100%) rename {tricycle_controller => steering_controllers/tricycle_controller}/test/test_load_tricycle_controller.cpp (100%) rename {tricycle_controller => steering_controllers/tricycle_controller}/test/test_tricycle_controller.cpp (100%) rename {tricycle_controller => steering_controllers/tricycle_controller}/tricycle_controller.xml (100%) diff --git a/steering_controllers/ackermann_steering_controller/CMakeLists.txt b/steering_controllers/ackermann_steering_controller/CMakeLists.txt index f5801b9c5f..804de9ea02 100644 --- a/steering_controllers/ackermann_steering_controller/CMakeLists.txt +++ b/steering_controllers/ackermann_steering_controller/CMakeLists.txt @@ -31,16 +31,6 @@ foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() -cmake_minimum_required(VERSION 3.8) -project(ackermann_steering_controller) - -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) - set(msg_files msg/AckermanControllerState.msg ) @@ -54,21 +44,21 @@ generate_parameter_library(ackermann_steering_controller_parameters add_library( - ackermann_steering_controller + ${PROJECT_NAME} SHARED src/ackermann_steering_controller.cpp ) -target_include_directories(ackermann_steering_controller PRIVATE include) -target_link_libraries(ackermann_steering_controller ackermann_steering_controller_parameters) -ament_target_dependencies(ackermann_steering_controller ${THIS_PACKAGE_INCLUDE_DEPENDS}) -target_compile_definitions(ackermann_steering_controller PRIVATE "ACKERMANN_STEERING_CONTROLLER_BUILDING_DLL") +target_include_directories( ${PROJECT_NAME} PRIVATE include) +target_link_libraries(${PROJECT_NAME} ackermann_steering_controller_parameters) +ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS}) +target_compile_definitions(${PROJECT_NAME} PRIVATE "ACKERMANN_STEERING_CONTROLLER_BUILDING_DLL") pluginlib_export_plugin_description_file( controller_interface ackermann_steering_controller.xml) install( TARGETS - ackermann_steering_controller + ${PROJECT_NAME} RUNTIME DESTINATION bin ARCHIVE DESTINATION lib LIBRARY DESTINATION lib @@ -128,7 +118,7 @@ ament_export_include_directories( ) ament_export_dependencies( - + ${THIS_PACKAGE_INCLUDE_DEPENDS} ) ament_export_libraries( ackermann_steering_controller diff --git a/tricycle_controller/CHANGELOG.rst b/steering_controllers/tricycle_controller/CHANGELOG.rst similarity index 100% rename from tricycle_controller/CHANGELOG.rst rename to steering_controllers/tricycle_controller/CHANGELOG.rst diff --git a/tricycle_controller/CMakeLists.txt b/steering_controllers/tricycle_controller/CMakeLists.txt similarity index 78% rename from tricycle_controller/CMakeLists.txt rename to steering_controllers/tricycle_controller/CMakeLists.txt index c499bca983..5b1105af51 100644 --- a/tricycle_controller/CMakeLists.txt +++ b/steering_controllers/tricycle_controller/CMakeLists.txt @@ -6,20 +6,29 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") endif() # find dependencies +set(THIS_PACKAGE_INCLUDE_DEPENDS + controller_interface + hardware_interface + pluginlib + nav_msgs + std_srvs + geometry_msgs + rclcpp + rcpputils + rclcpp_lifecycle + realtime_tools + tf2 + tf2_msgs + ackermann_msgs + steering_odometry +) + find_package(ament_cmake REQUIRED) -find_package(controller_interface REQUIRED) -find_package(hardware_interface REQUIRED) -find_package(pluginlib REQUIRED) -find_package(nav_msgs REQUIRED) -find_package(std_srvs REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rcpputils REQUIRED) -find_package(rclcpp_lifecycle REQUIRED) -find_package(realtime_tools REQUIRED) -find_package(tf2 REQUIRED) -find_package(tf2_msgs REQUIRED) -find_package(ackermann_msgs REQUIRED) +find_package(generate_parameter_library REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + add_library( ${PROJECT_NAME} @@ -37,20 +46,7 @@ target_include_directories( ament_target_dependencies( ${PROJECT_NAME} - ackermann_msgs - builtin_interfaces - controller_interface - geometry_msgs - hardware_interface - nav_msgs - std_srvs - pluginlib - rclcpp - rclcpp_lifecycle - rcpputils - realtime_tools - tf2 - tf2_msgs + ${THIS_PACKAGE_INCLUDE_DEPENDS} ) pluginlib_export_plugin_description_file(controller_interface tricycle_controller.xml) @@ -108,11 +104,7 @@ ament_export_libraries( ${PROJECT_NAME} ) ament_export_dependencies( - controller_interface - geometry_msgs - hardware_interface - rclcpp - rclcpp_lifecycle + ${THIS_PACKAGE_INCLUDE_DEPENDS} ) ament_package() diff --git a/tricycle_controller/config/tricycle_drive_controller.yaml b/steering_controllers/tricycle_controller/config/tricycle_drive_controller.yaml similarity index 100% rename from tricycle_controller/config/tricycle_drive_controller.yaml rename to steering_controllers/tricycle_controller/config/tricycle_drive_controller.yaml diff --git a/tricycle_controller/doc/userdoc.rst b/steering_controllers/tricycle_controller/doc/userdoc.rst similarity index 100% rename from tricycle_controller/doc/userdoc.rst rename to steering_controllers/tricycle_controller/doc/userdoc.rst diff --git a/tricycle_controller/include/tricycle_controller/odometry.hpp b/steering_controllers/tricycle_controller/include/tricycle_controller/odometry.hpp similarity index 100% rename from tricycle_controller/include/tricycle_controller/odometry.hpp rename to steering_controllers/tricycle_controller/include/tricycle_controller/odometry.hpp diff --git a/tricycle_controller/include/tricycle_controller/steering_limiter.hpp b/steering_controllers/tricycle_controller/include/tricycle_controller/steering_limiter.hpp similarity index 100% rename from tricycle_controller/include/tricycle_controller/steering_limiter.hpp rename to steering_controllers/tricycle_controller/include/tricycle_controller/steering_limiter.hpp diff --git a/tricycle_controller/include/tricycle_controller/traction_limiter.hpp b/steering_controllers/tricycle_controller/include/tricycle_controller/traction_limiter.hpp similarity index 100% rename from tricycle_controller/include/tricycle_controller/traction_limiter.hpp rename to steering_controllers/tricycle_controller/include/tricycle_controller/traction_limiter.hpp diff --git a/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp b/steering_controllers/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp similarity index 100% rename from tricycle_controller/include/tricycle_controller/tricycle_controller.hpp rename to steering_controllers/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp diff --git a/tricycle_controller/include/tricycle_controller/visibility_control.h b/steering_controllers/tricycle_controller/include/tricycle_controller/visibility_control.h similarity index 100% rename from tricycle_controller/include/tricycle_controller/visibility_control.h rename to steering_controllers/tricycle_controller/include/tricycle_controller/visibility_control.h diff --git a/tricycle_controller/package.xml b/steering_controllers/tricycle_controller/package.xml similarity index 100% rename from tricycle_controller/package.xml rename to steering_controllers/tricycle_controller/package.xml diff --git a/tricycle_controller/src/odometry.cpp b/steering_controllers/tricycle_controller/src/odometry.cpp similarity index 100% rename from tricycle_controller/src/odometry.cpp rename to steering_controllers/tricycle_controller/src/odometry.cpp diff --git a/tricycle_controller/src/steering_limiter.cpp b/steering_controllers/tricycle_controller/src/steering_limiter.cpp similarity index 100% rename from tricycle_controller/src/steering_limiter.cpp rename to steering_controllers/tricycle_controller/src/steering_limiter.cpp diff --git a/tricycle_controller/src/traction_limiter.cpp b/steering_controllers/tricycle_controller/src/traction_limiter.cpp similarity index 100% rename from tricycle_controller/src/traction_limiter.cpp rename to steering_controllers/tricycle_controller/src/traction_limiter.cpp diff --git a/tricycle_controller/src/tricycle_controller.cpp b/steering_controllers/tricycle_controller/src/tricycle_controller.cpp similarity index 100% rename from tricycle_controller/src/tricycle_controller.cpp rename to steering_controllers/tricycle_controller/src/tricycle_controller.cpp diff --git a/tricycle_controller/test/config/test_tricycle_controller.yaml b/steering_controllers/tricycle_controller/test/config/test_tricycle_controller.yaml similarity index 100% rename from tricycle_controller/test/config/test_tricycle_controller.yaml rename to steering_controllers/tricycle_controller/test/config/test_tricycle_controller.yaml diff --git a/tricycle_controller/test/test_load_tricycle_controller.cpp b/steering_controllers/tricycle_controller/test/test_load_tricycle_controller.cpp similarity index 100% rename from tricycle_controller/test/test_load_tricycle_controller.cpp rename to steering_controllers/tricycle_controller/test/test_load_tricycle_controller.cpp diff --git a/tricycle_controller/test/test_tricycle_controller.cpp b/steering_controllers/tricycle_controller/test/test_tricycle_controller.cpp similarity index 100% rename from tricycle_controller/test/test_tricycle_controller.cpp rename to steering_controllers/tricycle_controller/test/test_tricycle_controller.cpp diff --git a/tricycle_controller/tricycle_controller.xml b/steering_controllers/tricycle_controller/tricycle_controller.xml similarity index 100% rename from tricycle_controller/tricycle_controller.xml rename to steering_controllers/tricycle_controller/tricycle_controller.xml From de0e531e0bee66763dfe7816a884de7ab7ee7be9 Mon Sep 17 00:00:00 2001 From: petkovich Date: Thu, 8 Dec 2022 16:37:12 +0100 Subject: [PATCH 041/186] tricycle uses steering odometry, keeping odom.cpp for comparison, TODO: keep tricycle as a separate package? --- .../steering_odometry/steering_odometry.hpp | 5 ++ .../src/steering_odometry.cpp | 9 ++- .../tricycle_controller.hpp | 7 +-- .../src/tricycle_controller.cpp | 55 +++++-------------- 4 files changed, 30 insertions(+), 46 deletions(-) diff --git a/steering_controllers/steering_odometry/include/steering_odometry/steering_odometry.hpp b/steering_controllers/steering_odometry/include/steering_odometry/steering_odometry.hpp index c0696d8aa9..2c0667d21c 100644 --- a/steering_controllers/steering_odometry/include/steering_odometry/steering_odometry.hpp +++ b/steering_controllers/steering_odometry/include/steering_odometry/steering_odometry.hpp @@ -139,6 +139,11 @@ class SteeringOdometry */ std::tuple twist_to_ackermann(double Vx, double theta_dot); + /** + * \brief Reset poses, heading, and accumulators + */ + void reset_odometry(); + private: /// Rolling mean accumulator and window: typedef rcpputils::RollingMeanAccumulator RollingMeanAccumulator; diff --git a/steering_controllers/steering_odometry/src/steering_odometry.cpp b/steering_controllers/steering_odometry/src/steering_odometry.cpp index 9ae29f9175..4aae3ee7da 100644 --- a/steering_controllers/steering_odometry/src/steering_odometry.cpp +++ b/steering_controllers/steering_odometry/src/steering_odometry.cpp @@ -177,6 +177,14 @@ std::tuple SteeringOdometry::twist_to_ackermann(double Vx, doubl return std::make_tuple(alpha, Ws); } +void SteeringOdometry::reset_odometry() +{ + x_ = 0.0; + y_ = 0.0; + heading_ = 0.0; + reset_accumulators(); +} + void SteeringOdometry::integrate_runge_kutta_2(double linear, double angular) { const double direction = heading_ + angular * 0.5; @@ -213,5 +221,4 @@ void SteeringOdometry::reset_accumulators() angular_acc_ = RollingMeanAccumulator(velocity_rolling_window_size_); // TODO: angular rolling window size? } - } // namespace steering_odometry diff --git a/steering_controllers/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp b/steering_controllers/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp index 2178008725..356d9daf8a 100644 --- a/steering_controllers/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp +++ b/steering_controllers/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp @@ -40,7 +40,8 @@ #include "realtime_tools/realtime_publisher.h" #include "std_srvs/srv/empty.hpp" #include "tf2_msgs/msg/tf_message.hpp" -#include "tricycle_controller/odometry.hpp" +//#include "tricycle_controller/odometry.hpp" +#include "steering_odometry/steering_odometry.hpp" #include "tricycle_controller/steering_limiter.hpp" #include "tricycle_controller/traction_limiter.hpp" #include "tricycle_controller/visibility_control.h" @@ -106,8 +107,6 @@ class TricycleController : public controller_interface::ControllerInterface const std::string & traction_joint_name, std::vector & joint); CallbackReturn get_steering( const std::string & steering_joint_name, std::vector & joint); - double convert_trans_rot_vel_to_steering_angle(double v, double omega, double wheelbase); - std::tuple twist_to_ackermann(double linear_command, double angular_command); std::string traction_joint_name_; std::string steering_joint_name_; @@ -138,7 +137,7 @@ class TricycleController : public controller_interface::ControllerInterface std::shared_ptr> realtime_ackermann_command_publisher_ = nullptr; - Odometry odometry_; + steering_odometry::SteeringOdometry odometry_; std::shared_ptr> odometry_publisher_ = nullptr; std::shared_ptr> diff --git a/steering_controllers/tricycle_controller/src/tricycle_controller.cpp b/steering_controllers/tricycle_controller/src/tricycle_controller.cpp index 114731242c..42f09cfc94 100644 --- a/steering_controllers/tricycle_controller/src/tricycle_controller.cpp +++ b/steering_controllers/tricycle_controller/src/tricycle_controller.cpp @@ -154,7 +154,7 @@ controller_interface::return_type TricycleController::update( if (odom_params_.open_loop) { - odometry_.updateOpenLoop(linear_command, angular_command, period); + odometry_.update_open_loop(linear_command, angular_command, period.seconds()); } else { @@ -163,11 +163,11 @@ controller_interface::return_type TricycleController::update( RCLCPP_ERROR(get_node()->get_logger(), "Could not read feedback value"); return controller_interface::return_type::ERROR; } - odometry_.update(Ws_read, alpha_read, period); + odometry_.update_from_velocity(Ws_read, alpha_read, period.seconds()); } tf2::Quaternion orientation; - orientation.setRPY(0.0, 0.0, odometry_.getHeading()); + orientation.setRPY(0.0, 0.0, odometry_.get_heading()); if (previous_publish_timestamp_ + publish_period_ < time) { @@ -179,15 +179,15 @@ controller_interface::return_type TricycleController::update( odometry_message.header.stamp = time; if (!odom_params_.odom_only_twist) { - odometry_message.pose.pose.position.x = odometry_.getX(); - odometry_message.pose.pose.position.y = odometry_.getY(); + odometry_message.pose.pose.position.x = odometry_.get_x(); + odometry_message.pose.pose.position.y = odometry_.get_y(); odometry_message.pose.pose.orientation.x = orientation.x(); odometry_message.pose.pose.orientation.y = orientation.y(); odometry_message.pose.pose.orientation.z = orientation.z(); odometry_message.pose.pose.orientation.w = orientation.w(); } - odometry_message.twist.twist.linear.x = odometry_.getLinear(); - odometry_message.twist.twist.angular.z = odometry_.getAngular(); + odometry_message.twist.twist.linear.x = odometry_.get_linear(); + odometry_message.twist.twist.angular.z = odometry_.get_angular(); realtime_odometry_publisher_->unlockAndPublish(); } @@ -195,8 +195,8 @@ controller_interface::return_type TricycleController::update( { auto & transform = realtime_odometry_transform_publisher_->msg_.transforms.front(); transform.header.stamp = time; - transform.transform.translation.x = odometry_.getX(); - transform.transform.translation.y = odometry_.getY(); + transform.transform.translation.x = odometry_.get_x(); + transform.transform.translation.y = odometry_.get_y(); transform.transform.rotation.x = orientation.x(); transform.transform.rotation.y = orientation.y(); transform.transform.rotation.z = orientation.z(); @@ -206,7 +206,7 @@ controller_interface::return_type TricycleController::update( } // Compute wheel velocity and angle - auto [alpha_write, Ws_write] = twist_to_ackermann(linear_command, angular_command); + auto [alpha_write, Ws_write] = odometry_.twist_to_ackermann(linear_command, angular_command); // Reduce wheel speed until the target angle has been reached double alpha_delta = abs(alpha_write - alpha_read); @@ -281,8 +281,8 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State & wheel_params_.wheelbase = get_node()->get_parameter("wheelbase").as_double(); wheel_params_.radius = get_node()->get_parameter("wheel_radius").as_double(); - odometry_.setWheelParams(wheel_params_.wheelbase, wheel_params_.radius); - odometry_.setVelocityRollingWindowSize( + odometry_.set_wheel_params(wheel_params_.wheelbase, wheel_params_.radius, 0.0); + odometry_.set_velocity_rolling_window_size( get_node()->get_parameter("velocity_rolling_window_size").as_int()); odom_params_.odom_frame_id = get_node()->get_parameter("odom_frame_id").as_string(); @@ -518,13 +518,13 @@ void TricycleController::reset_odometry( const std::shared_ptr /*req*/, std::shared_ptr /*res*/) { - odometry_.resetOdometry(); + odometry_.reset_odometry(); RCLCPP_INFO(get_node()->get_logger(), "Odometry successfully reset"); } bool TricycleController::reset() { - odometry_.resetOdometry(); + odometry_.reset_odometry(); // release the old queue std::queue empty_ackermann_drive; @@ -637,33 +637,6 @@ CallbackReturn TricycleController::get_steering( return CallbackReturn::SUCCESS; } -double TricycleController::convert_trans_rot_vel_to_steering_angle( - double Vx, double theta_dot, double wheelbase) -{ - if (theta_dot == 0 || Vx == 0) - { - return 0; - } - return std::atan(theta_dot * wheelbase / Vx); -} - -std::tuple TricycleController::twist_to_ackermann(double Vx, double theta_dot) -{ - // using naming convention in http://users.isr.ist.utl.pt/~mir/cadeiras/robmovel/Kinematics.pdf - double alpha, Ws; - - if (Vx == 0 && theta_dot != 0) - { // is spin action - alpha = theta_dot > 0 ? M_PI_2 : -M_PI_2; - Ws = abs(theta_dot) * wheel_params_.wheelbase / wheel_params_.radius; - return std::make_tuple(alpha, Ws); - } - - alpha = convert_trans_rot_vel_to_steering_angle(Vx, theta_dot, wheel_params_.wheelbase); - Ws = Vx / (wheel_params_.radius * std::cos(alpha)); - return std::make_tuple(alpha, Ws); -} - } // namespace tricycle_controller #include From 9f85d4b963969b634654b1b73e93a7df606d3846 Mon Sep 17 00:00:00 2001 From: petkovich Date: Mon, 12 Dec 2022 15:24:57 +0100 Subject: [PATCH 042/186] cleaned cmakelists, removed tricycle odometry --- .../steering_odometry/CMakeLists.txt | 10 +- .../tricycle_controller/CMakeLists.txt | 1 - .../include/tricycle_controller/odometry.hpp | 75 ----------- .../tricycle_controller/src/odometry.cpp | 121 ------------------ 4 files changed, 5 insertions(+), 202 deletions(-) delete mode 100644 steering_controllers/tricycle_controller/include/tricycle_controller/odometry.hpp delete mode 100644 steering_controllers/tricycle_controller/src/odometry.cpp diff --git a/steering_controllers/steering_odometry/CMakeLists.txt b/steering_controllers/steering_odometry/CMakeLists.txt index 2c47326dfb..f8b34c3b67 100644 --- a/steering_controllers/steering_odometry/CMakeLists.txt +++ b/steering_controllers/steering_odometry/CMakeLists.txt @@ -31,16 +31,16 @@ if(BUILD_TESTING) endif() add_library( - steering_odometry + ${PROJECT_NAME} SHARED src/steering_odometry.cpp ) -target_include_directories(steering_odometry PRIVATE include) -ament_target_dependencies(steering_odometry ${THIS_PACKAGE_INCLUDE_DEPENDS}) +target_include_directories(${PROJECT_NAME} PRIVATE include) +ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS}) install( TARGETS - steering_odometry + ${PROJECT_NAME} RUNTIME DESTINATION bin ARCHIVE DESTINATION lib LIBRARY DESTINATION lib @@ -60,7 +60,7 @@ ament_export_dependencies( ${THIS_PACKAGE_INCLUDE_DEPENDS} ) ament_export_libraries( - steering_odometry + ${PROJECT_NAME} ) ament_package() diff --git a/steering_controllers/tricycle_controller/CMakeLists.txt b/steering_controllers/tricycle_controller/CMakeLists.txt index 5b1105af51..77000e93be 100644 --- a/steering_controllers/tricycle_controller/CMakeLists.txt +++ b/steering_controllers/tricycle_controller/CMakeLists.txt @@ -34,7 +34,6 @@ add_library( ${PROJECT_NAME} SHARED src/tricycle_controller.cpp - src/odometry.cpp src/traction_limiter.cpp src/steering_limiter.cpp ) diff --git a/steering_controllers/tricycle_controller/include/tricycle_controller/odometry.hpp b/steering_controllers/tricycle_controller/include/tricycle_controller/odometry.hpp deleted file mode 100644 index 4a958a93c6..0000000000 --- a/steering_controllers/tricycle_controller/include/tricycle_controller/odometry.hpp +++ /dev/null @@ -1,75 +0,0 @@ -// Copyright 2022 Pixel Robotics. -// -// 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. - -/* - * Author: Tony Najjar - */ - -#ifndef TRICYCLE_CONTROLLER__ODOMETRY_HPP_ -#define TRICYCLE_CONTROLLER__ODOMETRY_HPP_ - -#include - -#include "rclcpp/time.hpp" -#include "rcppmath/rolling_mean_accumulator.hpp" - -namespace tricycle_controller -{ -class Odometry -{ -public: - explicit Odometry(size_t velocity_rolling_window_size = 10); - - bool update(double left_vel, double right_vel, const rclcpp::Duration & dt); - void updateOpenLoop(double linear, double angular, const rclcpp::Duration & dt); - void resetOdometry(); - - double getX() const { return x_; } - double getY() const { return y_; } - double getHeading() const { return heading_; } - double getLinear() const { return linear_; } - double getAngular() const { return angular_; } - - void setWheelParams(double wheel_separation, double wheel_radius); - void setVelocityRollingWindowSize(size_t velocity_rolling_window_size); - -private: - using RollingMeanAccumulator = rcppmath::RollingMeanAccumulator; - - void integrateRungeKutta2(double linear, double angular); - void integrateExact(double linear, double angular); - void resetAccumulators(); - - // Current pose: - double x_; // [m] - double y_; // [m] - double heading_; // [rad] - - // Current velocity: - double linear_; // [m/s] - double angular_; // [rad/s] - - // Wheel kinematic parameters [m]: - double wheelbase_; - double wheel_radius_; - - // Rolling mean accumulators for the linear and angular velocities: - size_t velocity_rolling_window_size_; - RollingMeanAccumulator linear_accumulator_; - RollingMeanAccumulator angular_accumulator_; -}; - -} // namespace tricycle_controller - -#endif // TRICYCLE_CONTROLLER__ODOMETRY_HPP_ diff --git a/steering_controllers/tricycle_controller/src/odometry.cpp b/steering_controllers/tricycle_controller/src/odometry.cpp deleted file mode 100644 index e0e670480a..0000000000 --- a/steering_controllers/tricycle_controller/src/odometry.cpp +++ /dev/null @@ -1,121 +0,0 @@ -// Copyright 2022 Pixel Robotics. -// -// 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. - -/* - * Author: Tony Najjar - */ - -#include "tricycle_controller/odometry.hpp" - -namespace tricycle_controller -{ -Odometry::Odometry(size_t velocity_rolling_window_size) -: x_(0.0), - y_(0.0), - heading_(0.0), - linear_(0.0), - angular_(0.0), - wheelbase_(0.0), - wheel_radius_(0.0), - velocity_rolling_window_size_(velocity_rolling_window_size), - linear_accumulator_(velocity_rolling_window_size), - angular_accumulator_(velocity_rolling_window_size) -{ -} - -bool Odometry::update(double Ws, double alpha, const rclcpp::Duration & dt) -{ - // using naming convention in http://users.isr.ist.utl.pt/~mir/cadeiras/robmovel/Kinematics.pdf - double Vs = Ws * wheel_radius_; - double Vx = Vs * std::cos(alpha); - double theta_dot = Vs * std::sin(alpha) / wheelbase_; - - // Integrate odometry: - integrateExact(Vx * dt.seconds(), theta_dot * dt.seconds()); - - // Estimate speeds using a rolling mean to filter them out: - linear_accumulator_.accumulate(Vx); - angular_accumulator_.accumulate(theta_dot); - - linear_ = linear_accumulator_.getRollingMean(); - angular_ = angular_accumulator_.getRollingMean(); - - return true; -} - -void Odometry::updateOpenLoop(double linear, double angular, const rclcpp::Duration & dt) -{ - /// Save last linear and angular velocity: - linear_ = linear; - angular_ = angular; - - /// Integrate odometry: - integrateExact(linear * dt.seconds(), angular * dt.seconds()); -} - -void Odometry::resetOdometry() -{ - x_ = 0.0; - y_ = 0.0; - heading_ = 0.0; - resetAccumulators(); -} - -void Odometry::setWheelParams(double wheelbase, double wheel_radius) -{ - wheelbase_ = wheelbase; - wheel_radius_ = wheel_radius; -} - -void Odometry::setVelocityRollingWindowSize(size_t velocity_rolling_window_size) -{ - velocity_rolling_window_size_ = velocity_rolling_window_size; - - resetAccumulators(); -} - -void Odometry::integrateRungeKutta2(double linear, double angular) -{ - const double direction = heading_ + angular * 0.5; - - /// Runge-Kutta 2nd order integration: - x_ += linear * cos(direction); - y_ += linear * sin(direction); - heading_ += angular; -} - -void Odometry::integrateExact(double linear, double angular) -{ - if (fabs(angular) < 1e-6) - { - integrateRungeKutta2(linear, angular); - } - else - { - /// Exact integration (should solve problems when angular is zero): - const double heading_old = heading_; - const double r = linear / angular; - heading_ += angular; - x_ += r * (sin(heading_) - sin(heading_old)); - y_ += -r * (cos(heading_) - cos(heading_old)); - } -} - -void Odometry::resetAccumulators() -{ - linear_accumulator_ = RollingMeanAccumulator(velocity_rolling_window_size_); - angular_accumulator_ = RollingMeanAccumulator(velocity_rolling_window_size_); -} - -} // namespace tricycle_controller From 7bce4112ec3a862738ea5357686f42473f6598e3 Mon Sep 17 00:00:00 2001 From: petkovich Date: Tue, 13 Dec 2022 12:29:52 +0100 Subject: [PATCH 043/186] affed steering_controllers package, new base class/package --- .../CMakeLists.txt | 5 +- .../ackermann_steering_controller/package.xml | 1 + .../steering_controllers/CMakeLists.txt | 92 ++++ .../steering_controllers.hpp | 174 ++++++ .../steering_controllers/visibility_control.h | 52 ++ .../steering_controllers/package.xml | 39 ++ .../src/steering_controllers.cpp | 504 ++++++++++++++++++ .../src/steering_controllers.yaml | 164 ++++++ 8 files changed, 1029 insertions(+), 2 deletions(-) create mode 100644 steering_controllers/steering_controllers/CMakeLists.txt create mode 100644 steering_controllers/steering_controllers/include/steering_controllers/steering_controllers.hpp create mode 100644 steering_controllers/steering_controllers/include/steering_controllers/visibility_control.h create mode 100644 steering_controllers/steering_controllers/package.xml create mode 100644 steering_controllers/steering_controllers/src/steering_controllers.cpp create mode 100644 steering_controllers/steering_controllers/src/steering_controllers.yaml diff --git a/steering_controllers/ackermann_steering_controller/CMakeLists.txt b/steering_controllers/ackermann_steering_controller/CMakeLists.txt index 804de9ea02..84c4053b14 100644 --- a/steering_controllers/ackermann_steering_controller/CMakeLists.txt +++ b/steering_controllers/ackermann_steering_controller/CMakeLists.txt @@ -23,6 +23,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS ackermann_msgs custom_messages steering_odometry + steering_controllers ) find_package(ament_cmake REQUIRED) @@ -48,7 +49,7 @@ add_library( SHARED src/ackermann_steering_controller.cpp ) -target_include_directories( ${PROJECT_NAME} PRIVATE include) +target_include_directories(${PROJECT_NAME} PRIVATE include) target_link_libraries(${PROJECT_NAME} ackermann_steering_controller_parameters) ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS}) target_compile_definitions(${PROJECT_NAME} PRIVATE "ACKERMANN_STEERING_CONTROLLER_BUILDING_DLL") @@ -121,7 +122,7 @@ ament_export_dependencies( ${THIS_PACKAGE_INCLUDE_DEPENDS} ) ament_export_libraries( - ackermann_steering_controller + ${PROJECT_NAME} ) ament_package() diff --git a/steering_controllers/ackermann_steering_controller/package.xml b/steering_controllers/ackermann_steering_controller/package.xml index 603366d364..4f07afb940 100644 --- a/steering_controllers/ackermann_steering_controller/package.xml +++ b/steering_controllers/ackermann_steering_controller/package.xml @@ -28,6 +28,7 @@ ackermann_msgs steering_odometry custom_messages + steering_controllers ament_lint_auto ament_cmake_gmock diff --git a/steering_controllers/steering_controllers/CMakeLists.txt b/steering_controllers/steering_controllers/CMakeLists.txt new file mode 100644 index 0000000000..24ed3ded46 --- /dev/null +++ b/steering_controllers/steering_controllers/CMakeLists.txt @@ -0,0 +1,92 @@ +cmake_minimum_required(VERSION 3.8) +project(steering_controllers) + +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 + geometry_msgs + hardware_interface + nav_msgs + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + std_srvs + tf2 + tf2_msgs + tf2_geometry_msgs + ackermann_msgs + custom_messages + steering_odometry +) + +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_library( + ${PROJECT_NAME} + SHARED + src/steering_controllers.cpp +) +generate_parameter_library(steering_controllers_parameters + src/steering_controllers.yaml +) + + +target_include_directories(${PROJECT_NAME} PRIVATE include) +target_link_libraries(${PROJECT_NAME} steering_controllers_parameters) +ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS}) +target_compile_definitions(${PROJECT_NAME} PRIVATE "STEERING_CONTROLLERS_BUILDING_DLL") + +# pluginlib_export_plugin_description_file( +# controller_interface ackermann_steering_controller.xml) + +install( + TARGETS + ${PROJECT_NAME} + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib +) + + + +install( + DIRECTORY include/ + DESTINATION include +) + + +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_export_include_directories( + include +) + +ament_export_dependencies( + ${THIS_PACKAGE_INCLUDE_DEPENDS} +) +ament_export_libraries( + ${PROJECT_NAME} +) + +ament_package() diff --git a/steering_controllers/steering_controllers/include/steering_controllers/steering_controllers.hpp b/steering_controllers/steering_controllers/include/steering_controllers/steering_controllers.hpp new file mode 100644 index 0000000000..bfd72bab8f --- /dev/null +++ b/steering_controllers/steering_controllers/include/steering_controllers/steering_controllers.hpp @@ -0,0 +1,174 @@ +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef ACKERMANN_STEERING_CONTROLLER__ACKERMANN_STEERING_CONTROLLER_HPP_ +#define ACKERMANN_STEERING_CONTROLLER__ACKERMANN_STEERING_CONTROLLER_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "controller_interface/chainable_controller_interface.hpp" +#include "hardware_interface/handle.hpp" +#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" +#include "steering_controllers/visibility_control.h" +#include "steering_controllers_parameters.hpp" +#include "steering_odometry/steering_odometry.hpp" + +// TODO(anyone): Replace with controller specific messages +#include "ackermann_msgs/msg/ackermann_drive.hpp" +#include "custom_messages/msg/ackerman_controller_state.hpp" +#include "geometry_msgs/msg/twist.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "tf2_msgs/msg/tf_message.hpp" + +namespace ackermann_steering_controller +{ +// name constants for state interfaces +static constexpr size_t NR_STATE_ITFS = 2; + +// name constants for command interfaces +static constexpr size_t NR_CMD_ITFS = 2; + +// name constants for reference interfaces +static constexpr size_t NR_REF_ITFS = 2; + +class AckermannSteeringController : public controller_interface::ChainableControllerInterface +{ +public: + ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC + AckermannSteeringController(); + + ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_init() override; + + ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + + ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::return_type update_reference_from_subscribers() override; + + ACKERMANN_STEERING_CONTROLLER__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 = geometry_msgs::msg::TwistStamped; + using ControllerStateMsgOdom = nav_msgs::msg::Odometry; + using ControllerStateMsgTf = tf2_msgs::msg::TFMessage; + +protected: + std::shared_ptr param_listener_; + ackermann_steering_controller::Params params_; + + // Command subscribers and Controller State publisher + rclcpp::Subscription::SharedPtr ref_subscriber_ = nullptr; + rclcpp::Subscription::SharedPtr ref_subscriber_unstamped_ = nullptr; + realtime_tools::RealtimeBuffer> input_ref_; + rclcpp::Duration ref_timeout_ = rclcpp::Duration::from_seconds(0.0); // 0ms + + using ControllerStatePublisherOdom = realtime_tools::RealtimePublisher; + using ControllerStatePublisherTf = realtime_tools::RealtimePublisher; + + rclcpp::Publisher::SharedPtr odom_s_publisher_; + rclcpp::Publisher::SharedPtr tf_odom_s_publisher_; + + std::unique_ptr rt_odom_state_publisher_; + std::unique_ptr rt_tf_odom_state_publisher_; + + // override methods from ChainableControllerInterface + std::vector on_export_reference_interfaces() override; + + bool on_set_chained_mode(bool chained_mode) override; + + struct WheelHandle + { + std::reference_wrapper feedback; + // std::reference_wrapper velocity; + }; + + std::vector registered_rear_wheel_handles_; + std::vector registered_front_wheel_handles_; + + /// Odometry related: + rclcpp::Duration publish_period_ = rclcpp::Duration::from_nanoseconds(0); + + bool open_loop_; + /// Velocity command related: + struct Commands + { + double lin; + double ang; + rclcpp::Time stamp; + + Commands() : lin(0.0), ang(0.0), stamp(0.0) {} + }; + + // Odometry related: + steering_odometry::SteeringOdometry odometry_; + + using AckermanControllerState = custom_messages::msg::AckermanControllerState; + AckermanControllerState published_state_; + + using ControllerStatePublisher = realtime_tools::RealtimePublisher; + rclcpp::Publisher::SharedPtr controller_s_publisher_; + std::unique_ptr controller_state_publisher_; + +private: + // callback for topic interface + ACKERMANN_STEERING_CONTROLLER__VISIBILITY_LOCAL + void reference_callback(const std::shared_ptr msg); + void reference_callback_unstamped(const std::shared_ptr msg); + + /// Frame to use for the robot base: + std::string base_frame_id_; + + /// Frame to use for odometry and odom tf: + std::string odom_frame_id_; + + /// Whether to publish odometry to tf or not: + bool enable_odom_tf_; + + // store last velocity + double last_linear_velocity_ = 0.0; + double last_angular_velocity_ = 0.0; +}; + +} // namespace ackermann_steering_controller + +#endif // ACKERMANN_STEERING_CONTROLLER__ACKERMANN_STEERING_CONTROLLER_HPP_ diff --git a/steering_controllers/steering_controllers/include/steering_controllers/visibility_control.h b/steering_controllers/steering_controllers/include/steering_controllers/visibility_control.h new file mode 100644 index 0000000000..c494ba3bb4 --- /dev/null +++ b/steering_controllers/steering_controllers/include/steering_controllers/visibility_control.h @@ -0,0 +1,52 @@ +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef ACKERMANN_STEERING_CONTROLLER__VISIBILITY_CONTROL_H_ +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_EXPORT __attribute__((dllexport)) +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_IMPORT __attribute__((dllimport)) +#else +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_EXPORT __declspec(dllexport) +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_IMPORT __declspec(dllimport) +#endif +#ifdef ACKERMANN_STEERING_CONTROLLER__VISIBILITY_BUILDING_DLL +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC \ + ACKERMANN_STEERING_CONTROLLER__VISIBILITY_EXPORT +#else +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC \ + ACKERMANN_STEERING_CONTROLLER__VISIBILITY_IMPORT +#endif +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC_TYPE \ + ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_LOCAL +#else +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_EXPORT __attribute__((visibility("default"))) +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_IMPORT +#if __GNUC__ >= 4 +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC __attribute__((visibility("default"))) +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_LOCAL __attribute__((visibility("hidden"))) +#else +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_LOCAL +#endif +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC_TYPE +#endif + +#endif // ACKERMANN_STEERING_CONTROLLER__VISIBILITY_CONTROL_H_ diff --git a/steering_controllers/steering_controllers/package.xml b/steering_controllers/steering_controllers/package.xml new file mode 100644 index 0000000000..0a5463d199 --- /dev/null +++ b/steering_controllers/steering_controllers/package.xml @@ -0,0 +1,39 @@ + + + + steering_controllers + 0.0.0 + TODO: Package description + tomislav + TODO: License declaration + + generate_parameter_library + + control_msgs + controller_interface + geometry_msgs + hardware_interface + nav_msgs + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + rcpputils + std_srvs + tf2 + tf2_msgs + tf2_geometry_msgs + ackermann_msgs + steering_odometry + custom_messages + + ament_lint_auto + ament_cmake_gmock + controller_manager + hardware_interface + ros2_control_test_assets + + + ament_cmake + + diff --git a/steering_controllers/steering_controllers/src/steering_controllers.cpp b/steering_controllers/steering_controllers/src/steering_controllers.cpp new file mode 100644 index 0000000000..1c4299eeb2 --- /dev/null +++ b/steering_controllers/steering_controllers/src/steering_controllers.cpp @@ -0,0 +1,504 @@ +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "steering_controllers/steering_controllers.hpp" + +#include +#include +#include +#include +#include +#include + +#include "controller_interface/helpers.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "tf2/transform_datatypes.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.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 = + ackermann_steering_controller::AckermannSteeringController::ControllerReferenceMsg; + +// called from RT control loop +void reset_controller_reference_msg( + const std::shared_ptr & msg, + const std::shared_ptr & node) +{ + msg->header.stamp = node->now(); + msg->twist.linear.x = std::numeric_limits::quiet_NaN(); + msg->twist.linear.y = std::numeric_limits::quiet_NaN(); + msg->twist.linear.z = std::numeric_limits::quiet_NaN(); + msg->twist.angular.x = std::numeric_limits::quiet_NaN(); + msg->twist.angular.y = std::numeric_limits::quiet_NaN(); + msg->twist.angular.z = std::numeric_limits::quiet_NaN(); +} + +} // namespace + +namespace ackermann_steering_controller +{ +AckermannSteeringController::AckermannSteeringController() +: controller_interface::ChainableControllerInterface() +{ +} + +controller_interface::CallbackReturn AckermannSteeringController::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 AckermannSteeringController::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + params_ = param_listener_->get_params(); + + const double wheel_seperation = params_.wheel_separation_multiplier * params_.wheel_separation; + const double wheel_radius = params_.wheel_radius_multiplier * params_.wheel_radius; + const double wheelbase = params_.wheelbase_multiplier * params_.wheelbase; + odometry_.set_wheel_params(wheel_seperation, wheel_radius, wheelbase); + odometry_.set_velocity_rolling_window_size(params_.velocity_rolling_window_size); + + // topics QoS + auto subscribers_qos = rclcpp::SystemDefaultsQoS(); + subscribers_qos.keep_last(1); + subscribers_qos.best_effort(); + + // Reference Subscriber + ref_timeout_ = rclcpp::Duration::from_seconds(params_.reference_timeout); + if (params_.use_stamped_vel) + { + ref_subscriber_ = get_node()->create_subscription( + "~/reference", subscribers_qos, + std::bind(&AckermannSteeringController::reference_callback, this, std::placeholders::_1)); + } + else + { + ref_subscriber_unstamped_ = get_node()->create_subscription( + "~/reference_unstamped", subscribers_qos, + std::bind( + &AckermannSteeringController::reference_callback_unstamped, this, std::placeholders::_1)); + } + + std::shared_ptr msg = std::make_shared(); + reset_controller_reference_msg(msg, get_node()); + input_ref_.writeFromNonRT(msg); + + try + { + // Odom state publisher + odom_s_publisher_ = get_node()->create_publisher( + "~/odometry", rclcpp::SystemDefaultsQoS()); + rt_odom_state_publisher_ = std::make_unique(odom_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 + rt_odom_state_publisher_->lock(); + rt_odom_state_publisher_->msg_.header.stamp = get_node()->now(); + rt_odom_state_publisher_->msg_.header.frame_id = params_.odom_frame_id; + rt_odom_state_publisher_->msg_.child_frame_id = params_.base_frame_id; + rt_odom_state_publisher_->msg_.pose.pose.position.z = 0; + + auto & covariance = rt_odom_state_publisher_->msg_.twist.covariance; + constexpr size_t NUM_DIMENSIONS = 6; + for (size_t index = 0; index < 6; ++index) + { + // 0, 7, 14, 21, 28, 35 + const size_t diagonal_index = NUM_DIMENSIONS * index + index; + covariance[diagonal_index] = params_.pose_covariance_diagonal[index]; + covariance[diagonal_index] = params_.twist_covariance_diagonal[index]; + } + rt_odom_state_publisher_->unlock(); + + try + { + // Tf State publisher + tf_odom_s_publisher_ = get_node()->create_publisher( + "~/tf_odometry", rclcpp::SystemDefaultsQoS()); + rt_tf_odom_state_publisher_ = + std::make_unique(tf_odom_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; + } + + rt_tf_odom_state_publisher_->lock(); + rt_tf_odom_state_publisher_->msg_.transforms.resize(1); + rt_tf_odom_state_publisher_->msg_.transforms[0].header.stamp = get_node()->now(); + rt_tf_odom_state_publisher_->msg_.transforms[0].header.frame_id = params_.odom_frame_id; + rt_tf_odom_state_publisher_->msg_.transforms[0].child_frame_id = params_.base_frame_id; + rt_tf_odom_state_publisher_->msg_.transforms[0].transform.translation.z = 0.0; + rt_tf_odom_state_publisher_->unlock(); + + try + { + // State publisher + controller_s_publisher_ = get_node()->create_publisher( + "~/controller_state", rclcpp::SystemDefaultsQoS()); + controller_state_publisher_ = + std::make_unique(controller_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 + controller_state_publisher_->lock(); + controller_state_publisher_->msg_.header.stamp = get_node()->now(); + controller_state_publisher_->msg_.header.frame_id = params_.odom_frame_id; + controller_state_publisher_->msg_.odom.pose.pose.position.z = 0; + controller_state_publisher_->msg_.odom.child_frame_id = params_.base_frame_id; + controller_state_publisher_->unlock(); + auto & covariance_controller = controller_state_publisher_->msg_.odom.twist.covariance; + for (size_t index = 0; index < 6; ++index) + { + // 0, 7, 14, 21, 28, 35 + const size_t diagonal_index = NUM_DIMENSIONS * index + index; + covariance_controller[diagonal_index] = params_.pose_covariance_diagonal[index]; + covariance_controller[diagonal_index] = params_.twist_covariance_diagonal[index]; + } + RCLCPP_INFO(get_node()->get_logger(), "configure successful"); + return controller_interface::CallbackReturn::SUCCESS; +} + +void AckermannSteeringController::reference_callback( + const std::shared_ptr msg) +{ + // if no timestamp provided use current time for command timestamp + if (msg->header.stamp.sec == 0 && msg->header.stamp.nanosec == 0u) + { + RCLCPP_WARN( + get_node()->get_logger(), + "Timestamp in header is missing, using current time as command timestamp."); + msg->header.stamp = get_node()->now(); + } + const auto age_of_last_command = get_node()->now() - msg->header.stamp; + + if (ref_timeout_ == rclcpp::Duration::from_seconds(0) || age_of_last_command <= ref_timeout_) + { + input_ref_.writeFromNonRT(msg); + } + else + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Received message has timestamp %.10f older for %.10f which is more then allowed timeout " + "(%.4f).", + rclcpp::Time(msg->header.stamp).seconds(), age_of_last_command.seconds(), + ref_timeout_.seconds()); + } +} + +void AckermannSteeringController::reference_callback_unstamped( + const std::shared_ptr msg) +{ + auto twist_stamped = *(input_ref_.readFromNonRT()); + twist_stamped->header.stamp = get_node()->now(); + // if no timestamp provided use current time for command timestamp + if (twist_stamped->header.stamp.sec == 0 && twist_stamped->header.stamp.nanosec == 0u) + { + RCLCPP_WARN( + get_node()->get_logger(), + "Timestamp in header is missing, using current time as command timestamp."); + twist_stamped->header.stamp = get_node()->now(); + } + + const auto age_of_last_command = get_node()->now() - twist_stamped->header.stamp; + + if (ref_timeout_ == rclcpp::Duration::from_seconds(0) || age_of_last_command <= ref_timeout_) + { + twist_stamped->twist = *msg; + } + else + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Received message has timestamp %.10f older for %.10f which is more then allowed timeout " + "(%.4f).", + rclcpp::Time(twist_stamped->header.stamp).seconds(), age_of_last_command.seconds(), + ref_timeout_.seconds()); + } +} + +controller_interface::InterfaceConfiguration +AckermannSteeringController::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(NR_CMD_ITFS); + + for (size_t i = 0; i < params_.front_steer_names.size(); i++) + { + command_interfaces_config.names.push_back( + params_.rear_wheel_names[i] + "/" + hardware_interface::HW_IF_VELOCITY); + } + for (size_t i = 0; i < params_.front_steer_names.size(); i++) + { + command_interfaces_config.names.push_back( + params_.front_steer_names[i] + "/" + hardware_interface::HW_IF_POSITION); + } + return command_interfaces_config; +} + +controller_interface::InterfaceConfiguration +AckermannSteeringController::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(NR_STATE_ITFS); + const auto rear_wheel_feedback = params_.position_feedback ? hardware_interface::HW_IF_POSITION + : hardware_interface::HW_IF_VELOCITY; + + for (size_t i = 0; i < params_.front_steer_names.size(); i++) + { + state_interfaces_config.names.push_back( + params_.rear_wheel_names[i] + "/" + rear_wheel_feedback); + } + + for (size_t i = 0; i < params_.front_steer_names.size(); i++) + { + state_interfaces_config.names.push_back( + params_.front_steer_names[i] + "/" + hardware_interface::HW_IF_POSITION); + } + + return state_interfaces_config; +} + +std::vector +AckermannSteeringController::on_export_reference_interfaces() +{ + reference_interfaces_.resize(NR_REF_ITFS, std::numeric_limits::quiet_NaN()); + + std::vector reference_interfaces; + reference_interfaces.reserve(NR_REF_ITFS); + + reference_interfaces.push_back(hardware_interface::CommandInterface( + get_node()->get_name(), std::string("linear/") + hardware_interface::HW_IF_VELOCITY, + &reference_interfaces_[0])); + + reference_interfaces.push_back(hardware_interface::CommandInterface( + get_node()->get_name(), std::string("angular/") + hardware_interface::HW_IF_POSITION, + &reference_interfaces_[1])); + + return reference_interfaces; +} + +bool AckermannSteeringController::on_set_chained_mode(bool chained_mode) +{ + // Always accept switch to/from chained mode + return true || chained_mode; +} + +controller_interface::CallbackReturn AckermannSteeringController::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + // Set default value in command + reset_controller_reference_msg(*(input_ref_.readFromRT()), get_node()); + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn AckermannSteeringController::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 < NR_CMD_ITFS; ++i) + { + command_interfaces_[i].set_value(std::numeric_limits::quiet_NaN()); + } + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::return_type AckermannSteeringController::update_reference_from_subscribers() +{ + auto current_ref = *(input_ref_.readFromRT()); + const auto age_of_last_command = get_node()->now() - (current_ref)->header.stamp; + + // TODO(anyone): depending on number of interfaces, use definitions, e.g., `CMD_MY_ITFS`, + // instead of a loop + // send message only if there is no timeout + if (age_of_last_command <= ref_timeout_ || ref_timeout_ == rclcpp::Duration::from_seconds(0)) + { + if (!std::isnan(current_ref->twist.linear.x) && !std::isnan(current_ref->twist.angular.z)) + { + reference_interfaces_[0] = current_ref->twist.linear.x; + reference_interfaces_[1] = current_ref->twist.angular.z; + } + } + else + { + reference_interfaces_[0] = std::numeric_limits::quiet_NaN(); + reference_interfaces_[1] = std::numeric_limits::quiet_NaN(); + } + + return controller_interface::return_type::OK; +} + +controller_interface::return_type AckermannSteeringController::update_and_write_commands( + const rclcpp::Time & time, const rclcpp::Duration & period) +{ + if (params_.open_loop) + { + odometry_.update_open_loop(last_linear_velocity_, last_angular_velocity_, period.seconds()); + } + else + { + const double rear_wheel_value = state_interfaces_[0].get_value(); + const double steer_position = state_interfaces_[1].get_value() * params_.steer_pos_multiplier; + + if (!std::isnan(rear_wheel_value) && !std::isnan(steer_position)) + { + if (params_.position_feedback) + { + // Estimate linear and angular velocity using joint information + odometry_.update_from_position(rear_wheel_value, steer_position, period.seconds()); + } + else + { + // Estimate linear and angular velocity using joint information + odometry_.update_from_velocity(rear_wheel_value, steer_position, period.seconds()); + } + } + } + + // MOVE ROBOT + + // Limit velocities and accelerations: + // TODO(destogl): add limiter for the velocities + + if (!std::isnan(reference_interfaces_[0]) && !std::isnan(reference_interfaces_[1])) + { + // store and set commands + const double linear_command = reference_interfaces_[0]; + const double angular_command = reference_interfaces_[1]; + auto [alpha_write, Ws_write] = odometry_.twist_to_ackermann(linear_command, angular_command); + // previous_publish_timestamp_ = time; + + // omega = linear_vel / radius + command_interfaces_[0].set_value(Ws_write); + command_interfaces_[1].set_value(alpha_write); + } + + if (ref_timeout_ == rclcpp::Duration::from_seconds(0) || is_in_chained_mode()) + { + reference_interfaces_[0] = std::numeric_limits::quiet_NaN(); + reference_interfaces_[1] = std::numeric_limits::quiet_NaN(); + } + + // Publish odometry message + // Compute and store orientation info + tf2::Quaternion orientation; + orientation.setRPY(0.0, 0.0, odometry_.get_heading()); + + // Populate odom message and publish + if (rt_odom_state_publisher_->trylock()) + { + rt_odom_state_publisher_->msg_.header.stamp = time; + rt_odom_state_publisher_->msg_.pose.pose.position.x = odometry_.get_x(); + rt_odom_state_publisher_->msg_.pose.pose.position.y = odometry_.get_y(); + rt_odom_state_publisher_->msg_.pose.pose.orientation = tf2::toMsg(orientation); + rt_odom_state_publisher_->msg_.twist.twist.linear.x = odometry_.get_linear(); + rt_odom_state_publisher_->msg_.twist.twist.angular.z = odometry_.get_angular(); + rt_odom_state_publisher_->unlockAndPublish(); + } + + // Publish tf /odom frame + if (params_.enable_odom_tf && rt_tf_odom_state_publisher_->trylock()) + { + rt_tf_odom_state_publisher_->msg_.transforms.front().header.stamp = time; + rt_tf_odom_state_publisher_->msg_.transforms.front().transform.translation.x = + odometry_.get_x(); + rt_tf_odom_state_publisher_->msg_.transforms.front().transform.translation.y = + odometry_.get_y(); + rt_tf_odom_state_publisher_->msg_.transforms.front().transform.rotation = + tf2::toMsg(orientation); + rt_tf_odom_state_publisher_->unlockAndPublish(); + } + + if (controller_state_publisher_->trylock()) + { + controller_state_publisher_->msg_.header.stamp = time; + controller_state_publisher_->msg_.odom.pose.pose.position.x = odometry_.get_x(); + controller_state_publisher_->msg_.odom.pose.pose.position.y = odometry_.get_y(); + controller_state_publisher_->msg_.odom.pose.pose.orientation = tf2::toMsg(orientation); + controller_state_publisher_->msg_.odom.twist.twist.linear.x = odometry_.get_linear(); + controller_state_publisher_->msg_.odom.twist.twist.angular.z = odometry_.get_angular(); + if (params_.position_feedback) + { + controller_state_publisher_->msg_.rear_wheel_position = state_interfaces_[0].get_value(); + } + else + { + controller_state_publisher_->msg_.rear_wheel_velocity = state_interfaces_[0].get_value(); + } + controller_state_publisher_->msg_.steer_position = + state_interfaces_[1].get_value() * params_.steer_pos_multiplier; + controller_state_publisher_->msg_.linear_velocity_command = command_interfaces_[0].get_value(); + controller_state_publisher_->msg_.steering_angle_command = command_interfaces_[1].get_value(); + + controller_state_publisher_->unlockAndPublish(); + } + + return controller_interface::return_type::OK; +} + +} // namespace ackermann_steering_controller + +// #include "pluginlib/class_list_macros.hpp" + +// PLUGINLIB_EXPORT_CLASS( +// ackermann_steering_controller::AckermannSteeringController, +// controller_interface::ChainableControllerInterface) diff --git a/steering_controllers/steering_controllers/src/steering_controllers.yaml b/steering_controllers/steering_controllers/src/steering_controllers.yaml new file mode 100644 index 0000000000..bd33eb00af --- /dev/null +++ b/steering_controllers/steering_controllers/src/steering_controllers.yaml @@ -0,0 +1,164 @@ +ackermann_steering_controller: + reference_timeout: { + type: double, + default_value: 1, + description: "Timeout for controller references after which they will be reset. This is especially useful for controllers that can cause unwanted and dangerous behaviour if reference is not reset, e.g., velocity controllers. If value is 0 the reference is reset after each run.", +# validation: { +# gt_eq<>: 0.0, +# } + } + + rear_wheel_names: { + type: string_array, + default_value: ["rear_wheel_joint"], + description: "Name of rear wheels.", + read_only: true, + + } + + front_steer_names: { + type: string_array, + default_value: ["front_steer_joint"], + description: "Names of front steer wheels.", + read_only: true, + + } + + open_loop: { + type: bool, + default_value: false, + description: "bool parameter decides if open oop or not (feedback).", + read_only: false, + + } + + wheel_separation_multiplier: { + type: double, + default_value: 1.0, + description: "Wheel separation height will be multiplied by value of the wheel_separation_h_multiplier.", + read_only: false, + + } + + wheel_separation: { + type: double, + default_value: 0.0, + description: "Wheel separation.", + read_only: false, + + } + + wheelbase_multiplier: { + type: double, + default_value: 1.0, + description: "Wheelbase will be multiplied by value of the wheelbase_multiplier.", + read_only: false, + + } + + wheelbase: { + type: double, + default_value: 0.0, + description: "Wheelbase length.", + read_only: false, + + } + + wheel_radius: { + type: double, + default_value: 0.0, + description: "Wheel radius.", + read_only: false, + + } + + wheel_radius_multiplier: { + type: double, + default_value: 1.0, + description: "Wheel radius will be multiplied by value of wheel_radius_multiplier.", + read_only: false, + + } + + steer_pos_multiplier: { + type: double, + default_value: 1.0, + description: "Steer pos will be multiplied by value of steer_pos_multiplier.", + read_only: false, + + } + + velocity_rolling_window_size: { + type: int, + default_value: 10, + description: "The number of velocity samples to average together to compute the odometry twist.linear.x and twist.angular.z velocities.", + read_only: false, + + } + + allow_multiple_cmd_vel_publishers: { + type: bool, + default_value: true, + description: "Allow multiple cmd_vel publishers is enabled or disabled?.", + read_only: false, + + } + + base_frame_id: { + type: string, + default_value: "base_link", + description: "Base frame_id set to value of base_frame_id.", + read_only: false, + + } + + odom_frame_id: { + type: string, + default_value: "odom", + description: "Odometry frame_id set to value of odom_frame_id.", + read_only: false, + + } + + + enable_odom_tf: { + type: bool, + default_value: true, + description: "Publishing to tf is enabled or disabled ?.", + read_only: false, + + } + + twist_covariance_diagonal: { + type: double_array, + default_value: [0, 7, 14, 21, 28, 35], + description: "diagonal values of twist covariance matrix.", + read_only: false, + + } + + pose_covariance_diagonal: { + type: double_array, + default_value: [0, 7, 14, 21, 28, 35], + description: "diagonal values of pose covariance matrix.", + read_only: false, + + } + + position_feedback: { + type: bool, + default_value: false, + description: "choice of feedbacktype, if position_feedback is false then HW_IF_VELOCITY is taken as interface type, if + position_feedback is true then HW_IF_POSITION is taken as interface type", + read_only: false, + + } + + use_stamped_vel: { + type: bool, + default_value: false, + description: "choice of vel type, if use_stamped_vel is false then geometry_msgs::msg::Twist is taken as vel msg type, if + use_stamped_vel is true then geometry_msgs::msg::TwistStamped is taken as vel msg type", + read_only: false, + + } From 1a1f9770423a5eb8e7a29a890a25dada371435b2 Mon Sep 17 00:00:00 2001 From: petkovich Date: Thu, 15 Dec 2022 13:26:24 +0100 Subject: [PATCH 044/186] namespace, visibility, parameters --- .../steering_controllers.hpp | 52 ++++++++----------- .../steering_controllers/visibility_control.h | 41 +++++++-------- .../src/steering_controllers.cpp | 50 ++++++++---------- .../src/steering_controllers.yaml | 2 +- 4 files changed, 65 insertions(+), 80 deletions(-) diff --git a/steering_controllers/steering_controllers/include/steering_controllers/steering_controllers.hpp b/steering_controllers/steering_controllers/include/steering_controllers/steering_controllers.hpp index bfd72bab8f..5243eb6fcb 100644 --- a/steering_controllers/steering_controllers/include/steering_controllers/steering_controllers.hpp +++ b/steering_controllers/steering_controllers/include/steering_controllers/steering_controllers.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef ACKERMANN_STEERING_CONTROLLER__ACKERMANN_STEERING_CONTROLLER_HPP_ -#define ACKERMANN_STEERING_CONTROLLER__ACKERMANN_STEERING_CONTROLLER_HPP_ +#ifndef STEERING_CONTROLLERS__steering_controllers_HPP_ +#define STEERING_CONTROLLERS__steering_controllers_HPP_ #include #include @@ -42,7 +42,7 @@ #include "nav_msgs/msg/odometry.hpp" #include "tf2_msgs/msg/tf_message.hpp" -namespace ackermann_steering_controller +namespace steering_controllers { // name constants for state interfaces static constexpr size_t NR_STATE_ITFS = 2; @@ -53,39 +53,33 @@ static constexpr size_t NR_CMD_ITFS = 2; // name constants for reference interfaces static constexpr size_t NR_REF_ITFS = 2; -class AckermannSteeringController : public controller_interface::ChainableControllerInterface +class SteeringControllers : public controller_interface::ChainableControllerInterface { public: - ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC - AckermannSteeringController(); + STEERING_CONTROLLERS__VISIBILITY_PUBLIC SteeringControllers(); - ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC - controller_interface::CallbackReturn on_init() override; + STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::CallbackReturn on_init() override; - ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC - controller_interface::InterfaceConfiguration command_interface_configuration() const override; + STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::InterfaceConfiguration + command_interface_configuration() const override; - ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC - controller_interface::InterfaceConfiguration state_interface_configuration() const override; + STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::InterfaceConfiguration + state_interface_configuration() const override; - ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC - controller_interface::CallbackReturn on_configure( + STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::CallbackReturn on_configure( const rclcpp_lifecycle::State & previous_state) override; - ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC - controller_interface::CallbackReturn on_activate( + STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::CallbackReturn on_activate( const rclcpp_lifecycle::State & previous_state) override; - ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC - controller_interface::CallbackReturn on_deactivate( + STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::CallbackReturn on_deactivate( const rclcpp_lifecycle::State & previous_state) override; - ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC - controller_interface::return_type update_reference_from_subscribers() override; + STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::return_type + update_reference_from_subscribers() override; - ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC - controller_interface::return_type update_and_write_commands( - const rclcpp::Time & time, const rclcpp::Duration & period) override; + STEERING_CONTROLLERS__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 = geometry_msgs::msg::TwistStamped; @@ -93,8 +87,8 @@ class AckermannSteeringController : public controller_interface::ChainableContro using ControllerStateMsgTf = tf2_msgs::msg::TFMessage; protected: - std::shared_ptr param_listener_; - ackermann_steering_controller::Params params_; + std::shared_ptr param_listener_; + steering_controllers::Params params_; // Command subscribers and Controller State publisher rclcpp::Subscription::SharedPtr ref_subscriber_ = nullptr; @@ -151,8 +145,8 @@ class AckermannSteeringController : public controller_interface::ChainableContro private: // callback for topic interface - ACKERMANN_STEERING_CONTROLLER__VISIBILITY_LOCAL - void reference_callback(const std::shared_ptr msg); + STEERING_CONTROLLERS__VISIBILITY_LOCAL void reference_callback( + const std::shared_ptr msg); void reference_callback_unstamped(const std::shared_ptr msg); /// Frame to use for the robot base: @@ -169,6 +163,6 @@ class AckermannSteeringController : public controller_interface::ChainableContro double last_angular_velocity_ = 0.0; }; -} // namespace ackermann_steering_controller +} // namespace steering_controllers -#endif // ACKERMANN_STEERING_CONTROLLER__ACKERMANN_STEERING_CONTROLLER_HPP_ +#endif // STEERING_CONTROLLERS__steering_controllers_HPP_ diff --git a/steering_controllers/steering_controllers/include/steering_controllers/visibility_control.h b/steering_controllers/steering_controllers/include/steering_controllers/visibility_control.h index c494ba3bb4..e1ffb83e71 100644 --- a/steering_controllers/steering_controllers/include/steering_controllers/visibility_control.h +++ b/steering_controllers/steering_controllers/include/steering_controllers/visibility_control.h @@ -12,41 +12,38 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef ACKERMANN_STEERING_CONTROLLER__VISIBILITY_CONTROL_H_ -#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_CONTROL_H_ +#ifndef STEERING_CONTROLLERS__VISIBILITY_CONTROL_H_ +#define STEERING_CONTROLLERS__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 ACKERMANN_STEERING_CONTROLLER__VISIBILITY_EXPORT __attribute__((dllexport)) -#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_IMPORT __attribute__((dllimport)) +#define STEERING_CONTROLLERS__VISIBILITY_EXPORT __attribute__((dllexport)) +#define STEERING_CONTROLLERS__VISIBILITY_IMPORT __attribute__((dllimport)) #else -#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_EXPORT __declspec(dllexport) -#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_IMPORT __declspec(dllimport) +#define STEERING_CONTROLLERS__VISIBILITY_EXPORT __declspec(dllexport) +#define STEERING_CONTROLLERS__VISIBILITY_IMPORT __declspec(dllimport) #endif -#ifdef ACKERMANN_STEERING_CONTROLLER__VISIBILITY_BUILDING_DLL -#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC \ - ACKERMANN_STEERING_CONTROLLER__VISIBILITY_EXPORT +#ifdef STEERING_CONTROLLERS__VISIBILITY_BUILDING_DLL +#define STEERING_CONTROLLERS__VISIBILITY_PUBLIC STEERING_CONTROLLERS__VISIBILITY_EXPORT #else -#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC \ - ACKERMANN_STEERING_CONTROLLER__VISIBILITY_IMPORT +#define STEERING_CONTROLLERS__VISIBILITY_PUBLIC STEERING_CONTROLLERS__VISIBILITY_IMPORT #endif -#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC_TYPE \ - ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC -#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_LOCAL +#define STEERING_CONTROLLERS__VISIBILITY_PUBLIC_TYPE STEERING_CONTROLLERS__VISIBILITY_PUBLIC +#define STEERING_CONTROLLERS__VISIBILITY_LOCAL #else -#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_EXPORT __attribute__((visibility("default"))) -#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_IMPORT +#define STEERING_CONTROLLERS__VISIBILITY_EXPORT __attribute__((visibility("default"))) +#define STEERING_CONTROLLERS__VISIBILITY_IMPORT #if __GNUC__ >= 4 -#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC __attribute__((visibility("default"))) -#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_LOCAL __attribute__((visibility("hidden"))) +#define STEERING_CONTROLLERS__VISIBILITY_PUBLIC __attribute__((visibility("default"))) +#define STEERING_CONTROLLERS__VISIBILITY_LOCAL __attribute__((visibility("hidden"))) #else -#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC -#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_LOCAL +#define STEERING_CONTROLLERS__VISIBILITY_PUBLIC +#define STEERING_CONTROLLERS__VISIBILITY_LOCAL #endif -#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC_TYPE +#define STEERING_CONTROLLERS__VISIBILITY_PUBLIC_TYPE #endif -#endif // ACKERMANN_STEERING_CONTROLLER__VISIBILITY_CONTROL_H_ +#endif // STEERING_CONTROLLERS__VISIBILITY_CONTROL_H_ diff --git a/steering_controllers/steering_controllers/src/steering_controllers.cpp b/steering_controllers/steering_controllers/src/steering_controllers.cpp index 1c4299eeb2..13e3546ef8 100644 --- a/steering_controllers/steering_controllers/src/steering_controllers.cpp +++ b/steering_controllers/steering_controllers/src/steering_controllers.cpp @@ -43,8 +43,7 @@ static constexpr rmw_qos_profile_t rmw_qos_profile_services_hist_keep_all = { RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT, false}; -using ControllerReferenceMsg = - ackermann_steering_controller::AckermannSteeringController::ControllerReferenceMsg; +using ControllerReferenceMsg = steering_controllers::SteeringControllers::ControllerReferenceMsg; // called from RT control loop void reset_controller_reference_msg( @@ -62,18 +61,15 @@ void reset_controller_reference_msg( } // namespace -namespace ackermann_steering_controller +namespace steering_controllers { -AckermannSteeringController::AckermannSteeringController() -: controller_interface::ChainableControllerInterface() -{ -} +SteeringControllers::SteeringControllers() : controller_interface::ChainableControllerInterface() {} -controller_interface::CallbackReturn AckermannSteeringController::on_init() +controller_interface::CallbackReturn SteeringControllers::on_init() { try { - param_listener_ = std::make_shared(get_node()); + param_listener_ = std::make_shared(get_node()); } catch (const std::exception & e) { @@ -84,7 +80,7 @@ controller_interface::CallbackReturn AckermannSteeringController::on_init() return controller_interface::CallbackReturn::SUCCESS; } -controller_interface::CallbackReturn AckermannSteeringController::on_configure( +controller_interface::CallbackReturn SteeringControllers::on_configure( const rclcpp_lifecycle::State & /*previous_state*/) { params_ = param_listener_->get_params(); @@ -106,14 +102,13 @@ controller_interface::CallbackReturn AckermannSteeringController::on_configure( { ref_subscriber_ = get_node()->create_subscription( "~/reference", subscribers_qos, - std::bind(&AckermannSteeringController::reference_callback, this, std::placeholders::_1)); + std::bind(&SteeringControllers::reference_callback, this, std::placeholders::_1)); } else { ref_subscriber_unstamped_ = get_node()->create_subscription( "~/reference_unstamped", subscribers_qos, - std::bind( - &AckermannSteeringController::reference_callback_unstamped, this, std::placeholders::_1)); + std::bind(&SteeringControllers::reference_callback_unstamped, this, std::placeholders::_1)); } std::shared_ptr msg = std::make_shared(); @@ -212,8 +207,7 @@ controller_interface::CallbackReturn AckermannSteeringController::on_configure( return controller_interface::CallbackReturn::SUCCESS; } -void AckermannSteeringController::reference_callback( - const std::shared_ptr msg) +void SteeringControllers::reference_callback(const std::shared_ptr msg) { // if no timestamp provided use current time for command timestamp if (msg->header.stamp.sec == 0 && msg->header.stamp.nanosec == 0u) @@ -240,7 +234,7 @@ void AckermannSteeringController::reference_callback( } } -void AckermannSteeringController::reference_callback_unstamped( +void SteeringControllers::reference_callback_unstamped( const std::shared_ptr msg) { auto twist_stamped = *(input_ref_.readFromNonRT()); @@ -271,8 +265,8 @@ void AckermannSteeringController::reference_callback_unstamped( } } -controller_interface::InterfaceConfiguration -AckermannSteeringController::command_interface_configuration() const +controller_interface::InterfaceConfiguration SteeringControllers::command_interface_configuration() + const { controller_interface::InterfaceConfiguration command_interfaces_config; command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; @@ -291,8 +285,8 @@ AckermannSteeringController::command_interface_configuration() const return command_interfaces_config; } -controller_interface::InterfaceConfiguration -AckermannSteeringController::state_interface_configuration() const +controller_interface::InterfaceConfiguration SteeringControllers::state_interface_configuration() + const { controller_interface::InterfaceConfiguration state_interfaces_config; state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; @@ -317,7 +311,7 @@ AckermannSteeringController::state_interface_configuration() const } std::vector -AckermannSteeringController::on_export_reference_interfaces() +SteeringControllers::on_export_reference_interfaces() { reference_interfaces_.resize(NR_REF_ITFS, std::numeric_limits::quiet_NaN()); @@ -335,13 +329,13 @@ AckermannSteeringController::on_export_reference_interfaces() return reference_interfaces; } -bool AckermannSteeringController::on_set_chained_mode(bool chained_mode) +bool SteeringControllers::on_set_chained_mode(bool chained_mode) { // Always accept switch to/from chained mode return true || chained_mode; } -controller_interface::CallbackReturn AckermannSteeringController::on_activate( +controller_interface::CallbackReturn SteeringControllers::on_activate( const rclcpp_lifecycle::State & /*previous_state*/) { // Set default value in command @@ -350,7 +344,7 @@ controller_interface::CallbackReturn AckermannSteeringController::on_activate( return controller_interface::CallbackReturn::SUCCESS; } -controller_interface::CallbackReturn AckermannSteeringController::on_deactivate( +controller_interface::CallbackReturn SteeringControllers::on_deactivate( const rclcpp_lifecycle::State & /*previous_state*/) { // TODO(anyone): depending on number of interfaces, use definitions, e.g., `CMD_MY_ITFS`, @@ -362,7 +356,7 @@ controller_interface::CallbackReturn AckermannSteeringController::on_deactivate( return controller_interface::CallbackReturn::SUCCESS; } -controller_interface::return_type AckermannSteeringController::update_reference_from_subscribers() +controller_interface::return_type SteeringControllers::update_reference_from_subscribers() { auto current_ref = *(input_ref_.readFromRT()); const auto age_of_last_command = get_node()->now() - (current_ref)->header.stamp; @@ -387,7 +381,7 @@ controller_interface::return_type AckermannSteeringController::update_reference_ return controller_interface::return_type::OK; } -controller_interface::return_type AckermannSteeringController::update_and_write_commands( +controller_interface::return_type SteeringControllers::update_and_write_commands( const rclcpp::Time & time, const rclcpp::Duration & period) { if (params_.open_loop) @@ -495,10 +489,10 @@ controller_interface::return_type AckermannSteeringController::update_and_write_ return controller_interface::return_type::OK; } -} // namespace ackermann_steering_controller +} // namespace steering_controllers // #include "pluginlib/class_list_macros.hpp" // PLUGINLIB_EXPORT_CLASS( -// ackermann_steering_controller::AckermannSteeringController, +// steering_controllers::SteeringControllers, // controller_interface::ChainableControllerInterface) diff --git a/steering_controllers/steering_controllers/src/steering_controllers.yaml b/steering_controllers/steering_controllers/src/steering_controllers.yaml index bd33eb00af..5e4021b76b 100644 --- a/steering_controllers/steering_controllers/src/steering_controllers.yaml +++ b/steering_controllers/steering_controllers/src/steering_controllers.yaml @@ -1,4 +1,4 @@ -ackermann_steering_controller: +steering_controllers: reference_timeout: { type: double, default_value: 1, From ad05e0b1da84464e6d3f93d9133ab365f990c8ef Mon Sep 17 00:00:00 2001 From: petkovich Date: Thu, 15 Dec 2022 14:49:50 +0100 Subject: [PATCH 045/186] inherited steering controller in ackermann src --- .../src/ackermann_steering_controller.cpp | 482 +----------------- 1 file changed, 2 insertions(+), 480 deletions(-) diff --git a/steering_controllers/ackermann_steering_controller/src/ackermann_steering_controller.cpp b/steering_controllers/ackermann_steering_controller/src/ackermann_steering_controller.cpp index 37700d924d..7aaf54a0f5 100644 --- a/steering_controllers/ackermann_steering_controller/src/ackermann_steering_controller.cpp +++ b/steering_controllers/ackermann_steering_controller/src/ackermann_steering_controller.cpp @@ -12,489 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "ackermann_steering_controller/ackermann_steering_controller.hpp" - -#include -#include -#include -#include -#include -#include - -#include "controller_interface/helpers.hpp" -#include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "lifecycle_msgs/msg/state.hpp" -#include "tf2/transform_datatypes.h" -#include "tf2_geometry_msgs/tf2_geometry_msgs.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 = - ackermann_steering_controller::AckermannSteeringController::ControllerReferenceMsg; - -// called from RT control loop -void reset_controller_reference_msg( - const std::shared_ptr & msg, - const std::shared_ptr & node) -{ - msg->header.stamp = node->now(); - msg->twist.linear.x = std::numeric_limits::quiet_NaN(); - msg->twist.linear.y = std::numeric_limits::quiet_NaN(); - msg->twist.linear.z = std::numeric_limits::quiet_NaN(); - msg->twist.angular.x = std::numeric_limits::quiet_NaN(); - msg->twist.angular.y = std::numeric_limits::quiet_NaN(); - msg->twist.angular.z = std::numeric_limits::quiet_NaN(); -} - -} // namespace +#include "steering_controllers/steering_controllers.hpp" namespace ackermann_steering_controller { -AckermannSteeringController::AckermannSteeringController() -: controller_interface::ChainableControllerInterface() -{ -} - -controller_interface::CallbackReturn AckermannSteeringController::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 AckermannSteeringController::on_configure( - const rclcpp_lifecycle::State & /*previous_state*/) -{ - params_ = param_listener_->get_params(); - - const double wheel_seperation = params_.wheel_separation_multiplier * params_.wheel_separation; - const double wheel_radius = params_.wheel_radius_multiplier * params_.wheel_radius; - const double wheelbase = params_.wheelbase_multiplier * params_.wheelbase; - odometry_.set_wheel_params(wheel_seperation, wheel_radius, wheelbase); - odometry_.set_velocity_rolling_window_size(params_.velocity_rolling_window_size); - - // topics QoS - auto subscribers_qos = rclcpp::SystemDefaultsQoS(); - subscribers_qos.keep_last(1); - subscribers_qos.best_effort(); - - // Reference Subscriber - ref_timeout_ = rclcpp::Duration::from_seconds(params_.reference_timeout); - if (params_.use_stamped_vel) - { - ref_subscriber_ = get_node()->create_subscription( - "~/reference", subscribers_qos, - std::bind(&AckermannSteeringController::reference_callback, this, std::placeholders::_1)); - } - else - { - ref_subscriber_unstamped_ = get_node()->create_subscription( - "~/reference_unstamped", subscribers_qos, - std::bind( - &AckermannSteeringController::reference_callback_unstamped, this, std::placeholders::_1)); - } - - std::shared_ptr msg = std::make_shared(); - reset_controller_reference_msg(msg, get_node()); - input_ref_.writeFromNonRT(msg); - - try - { - // Odom state publisher - odom_s_publisher_ = get_node()->create_publisher( - "~/odometry", rclcpp::SystemDefaultsQoS()); - rt_odom_state_publisher_ = std::make_unique(odom_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 - rt_odom_state_publisher_->lock(); - rt_odom_state_publisher_->msg_.header.stamp = get_node()->now(); - rt_odom_state_publisher_->msg_.header.frame_id = params_.odom_frame_id; - rt_odom_state_publisher_->msg_.child_frame_id = params_.base_frame_id; - rt_odom_state_publisher_->msg_.pose.pose.position.z = 0; - - auto & covariance = rt_odom_state_publisher_->msg_.twist.covariance; - constexpr size_t NUM_DIMENSIONS = 6; - for (size_t index = 0; index < 6; ++index) - { - // 0, 7, 14, 21, 28, 35 - const size_t diagonal_index = NUM_DIMENSIONS * index + index; - covariance[diagonal_index] = params_.pose_covariance_diagonal[index]; - covariance[diagonal_index] = params_.twist_covariance_diagonal[index]; - } - rt_odom_state_publisher_->unlock(); - - try - { - // Tf State publisher - tf_odom_s_publisher_ = get_node()->create_publisher( - "~/tf_odometry", rclcpp::SystemDefaultsQoS()); - rt_tf_odom_state_publisher_ = - std::make_unique(tf_odom_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; - } - - rt_tf_odom_state_publisher_->lock(); - rt_tf_odom_state_publisher_->msg_.transforms.resize(1); - rt_tf_odom_state_publisher_->msg_.transforms[0].header.stamp = get_node()->now(); - rt_tf_odom_state_publisher_->msg_.transforms[0].header.frame_id = params_.odom_frame_id; - rt_tf_odom_state_publisher_->msg_.transforms[0].child_frame_id = params_.base_frame_id; - rt_tf_odom_state_publisher_->msg_.transforms[0].transform.translation.z = 0.0; - rt_tf_odom_state_publisher_->unlock(); - - try - { - // State publisher - controller_s_publisher_ = get_node()->create_publisher( - "~/controller_state", rclcpp::SystemDefaultsQoS()); - controller_state_publisher_ = - std::make_unique(controller_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 - controller_state_publisher_->lock(); - controller_state_publisher_->msg_.header.stamp = get_node()->now(); - controller_state_publisher_->msg_.header.frame_id = params_.odom_frame_id; - controller_state_publisher_->msg_.odom.pose.pose.position.z = 0; - controller_state_publisher_->msg_.odom.child_frame_id = params_.base_frame_id; - controller_state_publisher_->unlock(); - auto & covariance_controller = controller_state_publisher_->msg_.odom.twist.covariance; - for (size_t index = 0; index < 6; ++index) - { - // 0, 7, 14, 21, 28, 35 - const size_t diagonal_index = NUM_DIMENSIONS * index + index; - covariance_controller[diagonal_index] = params_.pose_covariance_diagonal[index]; - covariance_controller[diagonal_index] = params_.twist_covariance_diagonal[index]; - } - RCLCPP_INFO(get_node()->get_logger(), "configure successful"); - return controller_interface::CallbackReturn::SUCCESS; -} - -void AckermannSteeringController::reference_callback( - const std::shared_ptr msg) -{ - // if no timestamp provided use current time for command timestamp - if (msg->header.stamp.sec == 0 && msg->header.stamp.nanosec == 0u) - { - RCLCPP_WARN( - get_node()->get_logger(), - "Timestamp in header is missing, using current time as command timestamp."); - msg->header.stamp = get_node()->now(); - } - const auto age_of_last_command = get_node()->now() - msg->header.stamp; - - if (ref_timeout_ == rclcpp::Duration::from_seconds(0) || age_of_last_command <= ref_timeout_) - { - input_ref_.writeFromNonRT(msg); - } - else - { - RCLCPP_ERROR( - get_node()->get_logger(), - "Received message has timestamp %.10f older for %.10f which is more then allowed timeout " - "(%.4f).", - rclcpp::Time(msg->header.stamp).seconds(), age_of_last_command.seconds(), - ref_timeout_.seconds()); - } -} - -void AckermannSteeringController::reference_callback_unstamped( - const std::shared_ptr msg) -{ - auto twist_stamped = *(input_ref_.readFromNonRT()); - twist_stamped->header.stamp = get_node()->now(); - // if no timestamp provided use current time for command timestamp - if (twist_stamped->header.stamp.sec == 0 && twist_stamped->header.stamp.nanosec == 0u) - { - RCLCPP_WARN( - get_node()->get_logger(), - "Timestamp in header is missing, using current time as command timestamp."); - twist_stamped->header.stamp = get_node()->now(); - } - - const auto age_of_last_command = get_node()->now() - twist_stamped->header.stamp; - - if (ref_timeout_ == rclcpp::Duration::from_seconds(0) || age_of_last_command <= ref_timeout_) - { - twist_stamped->twist = *msg; - } - else - { - RCLCPP_ERROR( - get_node()->get_logger(), - "Received message has timestamp %.10f older for %.10f which is more then allowed timeout " - "(%.4f).", - rclcpp::Time(twist_stamped->header.stamp).seconds(), age_of_last_command.seconds(), - ref_timeout_.seconds()); - } -} - -controller_interface::InterfaceConfiguration -AckermannSteeringController::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(NR_CMD_ITFS); - - for (size_t i = 0; i < params_.front_steer_names.size(); i++) - { - command_interfaces_config.names.push_back( - params_.rear_wheel_names[i] + "/" + hardware_interface::HW_IF_VELOCITY); - } - for (size_t i = 0; i < params_.front_steer_names.size(); i++) - { - command_interfaces_config.names.push_back( - params_.front_steer_names[i] + "/" + hardware_interface::HW_IF_POSITION); - } - return command_interfaces_config; -} - -controller_interface::InterfaceConfiguration -AckermannSteeringController::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(NR_STATE_ITFS); - const auto rear_wheel_feedback = params_.position_feedback ? hardware_interface::HW_IF_POSITION - : hardware_interface::HW_IF_VELOCITY; - - for (size_t i = 0; i < params_.front_steer_names.size(); i++) - { - state_interfaces_config.names.push_back( - params_.rear_wheel_names[i] + "/" + rear_wheel_feedback); - } - - for (size_t i = 0; i < params_.front_steer_names.size(); i++) - { - state_interfaces_config.names.push_back( - params_.front_steer_names[i] + "/" + hardware_interface::HW_IF_POSITION); - } - - return state_interfaces_config; -} - -std::vector -AckermannSteeringController::on_export_reference_interfaces() -{ - reference_interfaces_.resize(NR_REF_ITFS, std::numeric_limits::quiet_NaN()); - - std::vector reference_interfaces; - reference_interfaces.reserve(NR_REF_ITFS); - - reference_interfaces.push_back(hardware_interface::CommandInterface( - get_node()->get_name(), std::string("linear/") + hardware_interface::HW_IF_VELOCITY, - &reference_interfaces_[0])); - - reference_interfaces.push_back(hardware_interface::CommandInterface( - get_node()->get_name(), std::string("angular/") + hardware_interface::HW_IF_POSITION, - &reference_interfaces_[1])); - - return reference_interfaces; -} - -bool AckermannSteeringController::on_set_chained_mode(bool chained_mode) -{ - // Always accept switch to/from chained mode - return true || chained_mode; -} - -controller_interface::CallbackReturn AckermannSteeringController::on_activate( - const rclcpp_lifecycle::State & /*previous_state*/) -{ - // Set default value in command - reset_controller_reference_msg(*(input_ref_.readFromRT()), get_node()); - - return controller_interface::CallbackReturn::SUCCESS; -} - -controller_interface::CallbackReturn AckermannSteeringController::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 < NR_CMD_ITFS; ++i) - { - command_interfaces_[i].set_value(std::numeric_limits::quiet_NaN()); - } - return controller_interface::CallbackReturn::SUCCESS; -} - -controller_interface::return_type AckermannSteeringController::update_reference_from_subscribers() -{ - auto current_ref = *(input_ref_.readFromRT()); - const auto age_of_last_command = get_node()->now() - (current_ref)->header.stamp; - - // TODO(anyone): depending on number of interfaces, use definitions, e.g., `CMD_MY_ITFS`, - // instead of a loop - // send message only if there is no timeout - if (age_of_last_command <= ref_timeout_ || ref_timeout_ == rclcpp::Duration::from_seconds(0)) - { - if (!std::isnan(current_ref->twist.linear.x) && !std::isnan(current_ref->twist.angular.z)) - { - reference_interfaces_[0] = current_ref->twist.linear.x; - reference_interfaces_[1] = current_ref->twist.angular.z; - } - } - else - { - reference_interfaces_[0] = std::numeric_limits::quiet_NaN(); - reference_interfaces_[1] = std::numeric_limits::quiet_NaN(); - } - - return controller_interface::return_type::OK; -} - -controller_interface::return_type AckermannSteeringController::update_and_write_commands( - const rclcpp::Time & time, const rclcpp::Duration & period) -{ - if (params_.open_loop) - { - odometry_.update_open_loop(last_linear_velocity_, last_angular_velocity_, period.seconds()); - } - else - { - const double rear_wheel_value = state_interfaces_[0].get_value(); - const double steer_position = state_interfaces_[1].get_value() * params_.steer_pos_multiplier; - - if (!std::isnan(rear_wheel_value) && !std::isnan(steer_position)) - { - if (params_.position_feedback) - { - // Estimate linear and angular velocity using joint information - odometry_.update_from_position(rear_wheel_value, steer_position, period.seconds()); - } - else - { - // Estimate linear and angular velocity using joint information - odometry_.update_from_velocity(rear_wheel_value, steer_position, period.seconds()); - } - } - } - - // MOVE ROBOT - - // Limit velocities and accelerations: - // TODO(destogl): add limiter for the velocities - - if (!std::isnan(reference_interfaces_[0]) && !std::isnan(reference_interfaces_[1])) - { - // store and set commands - const double linear_command = reference_interfaces_[0]; - const double angular_command = reference_interfaces_[1]; - auto [alpha_write, Ws_write] = odometry_.twist_to_ackermann(linear_command, angular_command); - // previous_publish_timestamp_ = time; - - // omega = linear_vel / radius - command_interfaces_[0].set_value(Ws_write); - command_interfaces_[1].set_value(alpha_write); - } - - if (ref_timeout_ == rclcpp::Duration::from_seconds(0) || is_in_chained_mode()) - { - reference_interfaces_[0] = std::numeric_limits::quiet_NaN(); - reference_interfaces_[1] = std::numeric_limits::quiet_NaN(); - } - - // Publish odometry message - // Compute and store orientation info - tf2::Quaternion orientation; - orientation.setRPY(0.0, 0.0, odometry_.get_heading()); - - // Populate odom message and publish - if (rt_odom_state_publisher_->trylock()) - { - rt_odom_state_publisher_->msg_.header.stamp = time; - rt_odom_state_publisher_->msg_.pose.pose.position.x = odometry_.get_x(); - rt_odom_state_publisher_->msg_.pose.pose.position.y = odometry_.get_y(); - rt_odom_state_publisher_->msg_.pose.pose.orientation = tf2::toMsg(orientation); - rt_odom_state_publisher_->msg_.twist.twist.linear.x = odometry_.get_linear(); - rt_odom_state_publisher_->msg_.twist.twist.angular.z = odometry_.get_angular(); - rt_odom_state_publisher_->unlockAndPublish(); - } - - // Publish tf /odom frame - if (params_.enable_odom_tf && rt_tf_odom_state_publisher_->trylock()) - { - rt_tf_odom_state_publisher_->msg_.transforms.front().header.stamp = time; - rt_tf_odom_state_publisher_->msg_.transforms.front().transform.translation.x = - odometry_.get_x(); - rt_tf_odom_state_publisher_->msg_.transforms.front().transform.translation.y = - odometry_.get_y(); - rt_tf_odom_state_publisher_->msg_.transforms.front().transform.rotation = - tf2::toMsg(orientation); - rt_tf_odom_state_publisher_->unlockAndPublish(); - } - - if (controller_state_publisher_->trylock()) - { - controller_state_publisher_->msg_.header.stamp = time; - controller_state_publisher_->msg_.odom.pose.pose.position.x = odometry_.get_x(); - controller_state_publisher_->msg_.odom.pose.pose.position.y = odometry_.get_y(); - controller_state_publisher_->msg_.odom.pose.pose.orientation = tf2::toMsg(orientation); - controller_state_publisher_->msg_.odom.twist.twist.linear.x = odometry_.get_linear(); - controller_state_publisher_->msg_.odom.twist.twist.angular.z = odometry_.get_angular(); - if (params_.position_feedback) - { - controller_state_publisher_->msg_.rear_wheel_position = state_interfaces_[0].get_value(); - } - else - { - controller_state_publisher_->msg_.rear_wheel_velocity = state_interfaces_[0].get_value(); - } - controller_state_publisher_->msg_.steer_position = - state_interfaces_[1].get_value() * params_.steer_pos_multiplier; - controller_state_publisher_->msg_.linear_velocity_command = command_interfaces_[0].get_value(); - controller_state_publisher_->msg_.steering_angle_command = command_interfaces_[1].get_value(); - - controller_state_publisher_->unlockAndPublish(); - } - - return controller_interface::return_type::OK; -} - +using AckermannSteeringController = steering_controllers::SteeringControllers; } // namespace ackermann_steering_controller #include "pluginlib/class_list_macros.hpp" From 58c36b9527503156e1c3ae463bb218b55238d021 Mon Sep 17 00:00:00 2001 From: petkovich Date: Thu, 15 Dec 2022 21:41:33 +0100 Subject: [PATCH 046/186] template for modifying update function --- .../ackermann_steering_controller.hpp | 154 +----------------- .../src/ackermann_steering_controller.cpp | 13 +- .../steering_controllers.hpp | 20 +-- 3 files changed, 26 insertions(+), 161 deletions(-) diff --git a/steering_controllers/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp b/steering_controllers/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp index 256c4351e0..f0fec631ed 100644 --- a/steering_controllers/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp +++ b/steering_controllers/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp @@ -12,163 +12,21 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef ACKERMANN_STEERING_CONTROLLER__ACKERMANN_STEERING_CONTROLLER_HPP_ -#define ACKERMANN_STEERING_CONTROLLER__ACKERMANN_STEERING_CONTROLLER_HPP_ - -#include -#include -#include -#include -#include -#include -#include - -#include "ackermann_steering_controller/visibility_control.h" -#include "ackermann_steering_controller_parameters.hpp" -#include "controller_interface/chainable_controller_interface.hpp" -#include "hardware_interface/handle.hpp" -#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" -#include "steering_odometry/steering_odometry.hpp" - -// TODO(anyone): Replace with controller specific messages -#include "ackermann_msgs/msg/ackermann_drive.hpp" -#include "custom_messages/msg/ackerman_controller_state.hpp" -#include "geometry_msgs/msg/twist.hpp" -#include "geometry_msgs/msg/twist_stamped.hpp" -#include "nav_msgs/msg/odometry.hpp" -#include "tf2_msgs/msg/tf_message.hpp" +#ifndef ACKERMANN_STEERING_CONTROLLER__STEERING_CONTROLLERS_HPP_ +#define ACKERMANN_STEERING_CONTROLLER__STEERING_CONTROLLERS_HPP_ +#include "steering_controllers/steering_controllers.hpp" namespace ackermann_steering_controller { -// name constants for state interfaces -static constexpr size_t NR_STATE_ITFS = 2; - -// name constants for command interfaces -static constexpr size_t NR_CMD_ITFS = 2; - -// name constants for reference interfaces -static constexpr size_t NR_REF_ITFS = 2; - -class AckermannSteeringController : public controller_interface::ChainableControllerInterface +class AckermannSteeringController : public steering_controllers::SteeringControllers { public: - ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC AckermannSteeringController(); - ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC - controller_interface::CallbackReturn on_init() override; - - ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC - controller_interface::InterfaceConfiguration command_interface_configuration() const override; - - ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC - controller_interface::InterfaceConfiguration state_interface_configuration() const override; - - ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC - controller_interface::CallbackReturn on_configure( + STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::CallbackReturn on_configure( const rclcpp_lifecycle::State & previous_state) override; - - ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC - controller_interface::CallbackReturn on_activate( - const rclcpp_lifecycle::State & previous_state) override; - - ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC - controller_interface::CallbackReturn on_deactivate( - const rclcpp_lifecycle::State & previous_state) override; - - ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC - controller_interface::return_type update_reference_from_subscribers() override; - - ACKERMANN_STEERING_CONTROLLER__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 = geometry_msgs::msg::TwistStamped; - using ControllerStateMsgOdom = nav_msgs::msg::Odometry; - using ControllerStateMsgTf = tf2_msgs::msg::TFMessage; - -protected: - std::shared_ptr param_listener_; - ackermann_steering_controller::Params params_; - - // Command subscribers and Controller State publisher - rclcpp::Subscription::SharedPtr ref_subscriber_ = nullptr; - rclcpp::Subscription::SharedPtr ref_subscriber_unstamped_ = nullptr; - realtime_tools::RealtimeBuffer> input_ref_; - rclcpp::Duration ref_timeout_ = rclcpp::Duration::from_seconds(0.0); // 0ms - - using ControllerStatePublisherOdom = realtime_tools::RealtimePublisher; - using ControllerStatePublisherTf = realtime_tools::RealtimePublisher; - - rclcpp::Publisher::SharedPtr odom_s_publisher_; - rclcpp::Publisher::SharedPtr tf_odom_s_publisher_; - - std::unique_ptr rt_odom_state_publisher_; - std::unique_ptr rt_tf_odom_state_publisher_; - - // override methods from ChainableControllerInterface - std::vector on_export_reference_interfaces() override; - - bool on_set_chained_mode(bool chained_mode) override; - - struct WheelHandle - { - std::reference_wrapper feedback; - // std::reference_wrapper velocity; - }; - - std::vector registered_rear_wheel_handles_; - std::vector registered_front_wheel_handles_; - - /// Odometry related: - rclcpp::Duration publish_period_ = rclcpp::Duration::from_nanoseconds(0); - - bool open_loop_; - /// Velocity command related: - struct Commands - { - double lin; - double ang; - rclcpp::Time stamp; - - Commands() : lin(0.0), ang(0.0), stamp(0.0) {} - }; - - // Odometry related: - steering_odometry::SteeringOdometry odometry_; - - using AckermanControllerState = custom_messages::msg::AckermanControllerState; - AckermanControllerState published_state_; - - using ControllerStatePublisher = realtime_tools::RealtimePublisher; - rclcpp::Publisher::SharedPtr controller_s_publisher_; - std::unique_ptr controller_state_publisher_; - -private: - // callback for topic interface - ACKERMANN_STEERING_CONTROLLER__VISIBILITY_LOCAL - void reference_callback(const std::shared_ptr msg); - void reference_callback_unstamped(const std::shared_ptr msg); - - /// Frame to use for the robot base: - std::string base_frame_id_; - - /// Frame to use for odometry and odom tf: - std::string odom_frame_id_; - - /// Whether to publish odometry to tf or not: - bool enable_odom_tf_; - - // store last velocity - double last_linear_velocity_ = 0.0; - double last_angular_velocity_ = 0.0; }; } // namespace ackermann_steering_controller -#endif // ACKERMANN_STEERING_CONTROLLER__ACKERMANN_STEERING_CONTROLLER_HPP_ +#endif // STEERING_CONTROLLERS__steering_controllers_HPP_ diff --git a/steering_controllers/ackermann_steering_controller/src/ackermann_steering_controller.cpp b/steering_controllers/ackermann_steering_controller/src/ackermann_steering_controller.cpp index 7aaf54a0f5..746a32420b 100644 --- a/steering_controllers/ackermann_steering_controller/src/ackermann_steering_controller.cpp +++ b/steering_controllers/ackermann_steering_controller/src/ackermann_steering_controller.cpp @@ -12,11 +12,20 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "steering_controllers/steering_controllers.hpp" +#include "ackermann_steering_controller/ackermann_steering_controller.hpp" namespace ackermann_steering_controller { -using AckermannSteeringController = steering_controllers::SteeringControllers; +//using AckermannSteeringController = steering_controllers::SteeringControllers; +AckermannSteeringController::AckermannSteeringController() +: steering_controllers::SteeringControllers() +{ +} +controller_interface::CallbackReturn AckermannSteeringController::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + return controller_interface::CallbackReturn::ERROR; +} } // namespace ackermann_steering_controller #include "pluginlib/class_list_macros.hpp" diff --git a/steering_controllers/steering_controllers/include/steering_controllers/steering_controllers.hpp b/steering_controllers/steering_controllers/include/steering_controllers/steering_controllers.hpp index 5243eb6fcb..0ee6b68baf 100644 --- a/steering_controllers/steering_controllers/include/steering_controllers/steering_controllers.hpp +++ b/steering_controllers/steering_controllers/include/steering_controllers/steering_controllers.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef STEERING_CONTROLLERS__steering_controllers_HPP_ -#define STEERING_CONTROLLERS__steering_controllers_HPP_ +#ifndef STEERING_CONTROLLERS__STEERING_CONTROLLERS_HPP_ +#define STEERING_CONTROLLERS__STEERING_CONTROLLERS_HPP_ #include #include @@ -44,15 +44,6 @@ namespace steering_controllers { -// name constants for state interfaces -static constexpr size_t NR_STATE_ITFS = 2; - -// name constants for command interfaces -static constexpr size_t NR_CMD_ITFS = 2; - -// name constants for reference interfaces -static constexpr size_t NR_REF_ITFS = 2; - class SteeringControllers : public controller_interface::ChainableControllerInterface { public: @@ -157,6 +148,13 @@ class SteeringControllers : public controller_interface::ChainableControllerInte /// Whether to publish odometry to tf or not: bool enable_odom_tf_; + // name constants for state interfaces + size_t NR_STATE_ITFS = 2; + + size_t NR_CMD_ITFS = 2; + + // name constants for reference interfaces + size_t NR_REF_ITFS = 2; // store last velocity double last_linear_velocity_ = 0.0; From 067e4df3c05e469adbb17cedf04a9c3a126daa4c Mon Sep 17 00:00:00 2001 From: petkovich Date: Fri, 16 Dec 2022 11:56:04 +0100 Subject: [PATCH 047/186] odom configuration separated --- .../ackermann_steering_controller.hpp | 10 +++++----- .../src/ackermann_steering_controller.cpp | 16 +++++++++++++--- .../steering_controllers.hpp | 3 +++ .../src/steering_controllers.cpp | 11 +++++++++-- 4 files changed, 30 insertions(+), 10 deletions(-) diff --git a/steering_controllers/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp b/steering_controllers/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp index f0fec631ed..54a2ee5bd8 100644 --- a/steering_controllers/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp +++ b/steering_controllers/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef ACKERMANN_STEERING_CONTROLLER__STEERING_CONTROLLERS_HPP_ -#define ACKERMANN_STEERING_CONTROLLER__STEERING_CONTROLLERS_HPP_ +#ifndef ACKERMANN_STEERING_CONTROLLER__HPP_ +#define ACKERMANN_STEERING_CONTROLLER___HPP_ #include "steering_controllers/steering_controllers.hpp" namespace ackermann_steering_controller @@ -23,10 +23,10 @@ class AckermannSteeringController : public steering_controllers::SteeringControl public: AckermannSteeringController(); - STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::CallbackReturn on_configure( - const rclcpp_lifecycle::State & previous_state) override; + STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::CallbackReturn configure_odometry() + override; }; } // namespace ackermann_steering_controller -#endif // STEERING_CONTROLLERS__steering_controllers_HPP_ +#endif // ACKERMANN_STEERING_CONTROLLER__HPP_ diff --git a/steering_controllers/ackermann_steering_controller/src/ackermann_steering_controller.cpp b/steering_controllers/ackermann_steering_controller/src/ackermann_steering_controller.cpp index 746a32420b..9c59534e16 100644 --- a/steering_controllers/ackermann_steering_controller/src/ackermann_steering_controller.cpp +++ b/steering_controllers/ackermann_steering_controller/src/ackermann_steering_controller.cpp @@ -21,11 +21,21 @@ AckermannSteeringController::AckermannSteeringController() : steering_controllers::SteeringControllers() { } -controller_interface::CallbackReturn AckermannSteeringController::on_configure( - const rclcpp_lifecycle::State & /*previous_state*/) + +controller_interface::CallbackReturn AckermannSteeringController::configure_odometry() { - return controller_interface::CallbackReturn::ERROR; + params_ = param_listener_->get_params(); + + const double wheel_seperation = params_.wheel_separation_multiplier * params_.wheel_separation; + const double wheel_radius = params_.wheel_radius_multiplier * params_.wheel_radius; + const double wheelbase = params_.wheelbase_multiplier * params_.wheelbase; + odometry_.set_wheel_params(wheel_seperation, wheel_radius, wheelbase); + odometry_.set_velocity_rolling_window_size(params_.velocity_rolling_window_size); + + RCLCPP_INFO(get_node()->get_logger(), "odom configure successful INSIDE ACKERMANN"); + return controller_interface::CallbackReturn::SUCCESS; } + } // namespace ackermann_steering_controller #include "pluginlib/class_list_macros.hpp" diff --git a/steering_controllers/steering_controllers/include/steering_controllers/steering_controllers.hpp b/steering_controllers/steering_controllers/include/steering_controllers/steering_controllers.hpp index 0ee6b68baf..f59cd2cc83 100644 --- a/steering_controllers/steering_controllers/include/steering_controllers/steering_controllers.hpp +++ b/steering_controllers/steering_controllers/include/steering_controllers/steering_controllers.hpp @@ -57,6 +57,9 @@ class SteeringControllers : public controller_interface::ChainableControllerInte STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::InterfaceConfiguration state_interface_configuration() const override; + virtual STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::CallbackReturn + configure_odometry(); + STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::CallbackReturn on_configure( const rclcpp_lifecycle::State & previous_state) override; diff --git a/steering_controllers/steering_controllers/src/steering_controllers.cpp b/steering_controllers/steering_controllers/src/steering_controllers.cpp index 13e3546ef8..6b4b3a3f4a 100644 --- a/steering_controllers/steering_controllers/src/steering_controllers.cpp +++ b/steering_controllers/steering_controllers/src/steering_controllers.cpp @@ -80,8 +80,7 @@ controller_interface::CallbackReturn SteeringControllers::on_init() return controller_interface::CallbackReturn::SUCCESS; } -controller_interface::CallbackReturn SteeringControllers::on_configure( - const rclcpp_lifecycle::State & /*previous_state*/) +controller_interface::CallbackReturn SteeringControllers::configure_odometry() { params_ = param_listener_->get_params(); @@ -91,6 +90,14 @@ controller_interface::CallbackReturn SteeringControllers::on_configure( odometry_.set_wheel_params(wheel_seperation, wheel_radius, wheelbase); odometry_.set_velocity_rolling_window_size(params_.velocity_rolling_window_size); + RCLCPP_INFO(get_node()->get_logger(), "odom configure successful"); + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn SteeringControllers::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + auto odometry_conf_msg = configure_odometry(); // topics QoS auto subscribers_qos = rclcpp::SystemDefaultsQoS(); subscribers_qos.keep_last(1); From effee35070a3e20f591483aaa81e696f25c11a37 Mon Sep 17 00:00:00 2001 From: petkovich Date: Fri, 16 Dec 2022 12:15:14 +0100 Subject: [PATCH 048/186] function for changing intefrace number --- .../src/ackermann_steering_controller.cpp | 6 ++++++ .../steering_controllers.hpp | 17 +++++++++------- .../src/steering_controllers.cpp | 20 ++++++++++++++----- 3 files changed, 31 insertions(+), 12 deletions(-) diff --git a/steering_controllers/ackermann_steering_controller/src/ackermann_steering_controller.cpp b/steering_controllers/ackermann_steering_controller/src/ackermann_steering_controller.cpp index 9c59534e16..9ffb5959c8 100644 --- a/steering_controllers/ackermann_steering_controller/src/ackermann_steering_controller.cpp +++ b/steering_controllers/ackermann_steering_controller/src/ackermann_steering_controller.cpp @@ -32,6 +32,12 @@ controller_interface::CallbackReturn AckermannSteeringController::configure_odom odometry_.set_wheel_params(wheel_seperation, wheel_radius, wheelbase); odometry_.set_velocity_rolling_window_size(params_.velocity_rolling_window_size); + const size_t nr_state_itfs = 2; + const size_t nr_cmd_itfs = 2; + const size_t nr_ref_itfs = 2; + + set_interface_numbers(nr_state_itfs, nr_cmd_itfs, nr_ref_itfs); + RCLCPP_INFO(get_node()->get_logger(), "odom configure successful INSIDE ACKERMANN"); return controller_interface::CallbackReturn::SUCCESS; } diff --git a/steering_controllers/steering_controllers/include/steering_controllers/steering_controllers.hpp b/steering_controllers/steering_controllers/include/steering_controllers/steering_controllers.hpp index f59cd2cc83..a01432e87d 100644 --- a/steering_controllers/steering_controllers/include/steering_controllers/steering_controllers.hpp +++ b/steering_controllers/steering_controllers/include/steering_controllers/steering_controllers.hpp @@ -57,6 +57,9 @@ class SteeringControllers : public controller_interface::ChainableControllerInte STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::InterfaceConfiguration state_interface_configuration() const override; + STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::CallbackReturn + set_interface_numbers(size_t nr_state_itfs, size_t nr_cmd_itfs, size_t nr_ref_itfs); + virtual STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::CallbackReturn configure_odometry(); @@ -137,6 +140,13 @@ class SteeringControllers : public controller_interface::ChainableControllerInte rclcpp::Publisher::SharedPtr controller_s_publisher_; std::unique_ptr controller_state_publisher_; + // name constants for state interfaces + size_t nr_state_itfs_; + // name constants for command interfaces + size_t nr_cmd_itfs_; + // name constants for reference interfaces + size_t nr_ref_itfs_; + private: // callback for topic interface STEERING_CONTROLLERS__VISIBILITY_LOCAL void reference_callback( @@ -151,13 +161,6 @@ class SteeringControllers : public controller_interface::ChainableControllerInte /// Whether to publish odometry to tf or not: bool enable_odom_tf_; - // name constants for state interfaces - size_t NR_STATE_ITFS = 2; - - size_t NR_CMD_ITFS = 2; - - // name constants for reference interfaces - size_t NR_REF_ITFS = 2; // store last velocity double last_linear_velocity_ = 0.0; diff --git a/steering_controllers/steering_controllers/src/steering_controllers.cpp b/steering_controllers/steering_controllers/src/steering_controllers.cpp index 6b4b3a3f4a..a4b108e193 100644 --- a/steering_controllers/steering_controllers/src/steering_controllers.cpp +++ b/steering_controllers/steering_controllers/src/steering_controllers.cpp @@ -80,6 +80,14 @@ controller_interface::CallbackReturn SteeringControllers::on_init() return controller_interface::CallbackReturn::SUCCESS; } +controller_interface::CallbackReturn SteeringControllers::set_interface_numbers( + size_t nr_state_itfs = 2, size_t nr_cmd_itfs = 2, size_t nr_ref_itfs = 2) +{ + nr_state_itfs_ = nr_state_itfs; + nr_cmd_itfs_ = nr_cmd_itfs; + nr_ref_itfs_ = nr_ref_itfs; +} + controller_interface::CallbackReturn SteeringControllers::configure_odometry() { params_ = param_listener_->get_params(); @@ -90,6 +98,8 @@ controller_interface::CallbackReturn SteeringControllers::configure_odometry() odometry_.set_wheel_params(wheel_seperation, wheel_radius, wheelbase); odometry_.set_velocity_rolling_window_size(params_.velocity_rolling_window_size); + set_interface_numbers(); + RCLCPP_INFO(get_node()->get_logger(), "odom configure successful"); return controller_interface::CallbackReturn::SUCCESS; } @@ -277,7 +287,7 @@ controller_interface::InterfaceConfiguration SteeringControllers::command_interf { controller_interface::InterfaceConfiguration command_interfaces_config; command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; - command_interfaces_config.names.reserve(NR_CMD_ITFS); + command_interfaces_config.names.reserve(nr_cmd_itfs_); for (size_t i = 0; i < params_.front_steer_names.size(); i++) { @@ -298,7 +308,7 @@ controller_interface::InterfaceConfiguration SteeringControllers::state_interfac controller_interface::InterfaceConfiguration state_interfaces_config; state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; - state_interfaces_config.names.reserve(NR_STATE_ITFS); + state_interfaces_config.names.reserve(nr_state_itfs_); const auto rear_wheel_feedback = params_.position_feedback ? hardware_interface::HW_IF_POSITION : hardware_interface::HW_IF_VELOCITY; @@ -320,10 +330,10 @@ controller_interface::InterfaceConfiguration SteeringControllers::state_interfac std::vector SteeringControllers::on_export_reference_interfaces() { - reference_interfaces_.resize(NR_REF_ITFS, std::numeric_limits::quiet_NaN()); + reference_interfaces_.resize(nr_ref_itfs_, std::numeric_limits::quiet_NaN()); std::vector reference_interfaces; - reference_interfaces.reserve(NR_REF_ITFS); + reference_interfaces.reserve(nr_ref_itfs_); reference_interfaces.push_back(hardware_interface::CommandInterface( get_node()->get_name(), std::string("linear/") + hardware_interface::HW_IF_VELOCITY, @@ -356,7 +366,7 @@ controller_interface::CallbackReturn SteeringControllers::on_deactivate( { // TODO(anyone): depending on number of interfaces, use definitions, e.g., `CMD_MY_ITFS`, // instead of a loop - for (size_t i = 0; i < NR_CMD_ITFS; ++i) + for (size_t i = 0; i < nr_cmd_itfs_; ++i) { command_interfaces_[i].set_value(std::numeric_limits::quiet_NaN()); } From 59f4c466c6e19e49a7da66e5116bdd5cddbbc59a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Sat, 17 Dec 2022 15:50:21 +0100 Subject: [PATCH 049/186] Delete LICENSE --- .../ackermann_steering_controller/LICENSE | 17 ----------------- 1 file changed, 17 deletions(-) delete mode 100644 steering_controllers/ackermann_steering_controller/LICENSE diff --git a/steering_controllers/ackermann_steering_controller/LICENSE b/steering_controllers/ackermann_steering_controller/LICENSE deleted file mode 100644 index 30e8e2ece5..0000000000 --- a/steering_controllers/ackermann_steering_controller/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. From 8f6803cd40dd77242bdde34f66fc7b89630d2d84 Mon Sep 17 00:00:00 2001 From: petkovich Date: Sun, 18 Dec 2022 00:37:25 +0100 Subject: [PATCH 050/186] remove custom msg, integrate new msg from control msgs --- custom_messages/CMakeLists.txt | 37 ------------------- .../msg/AckermanControllerState.msg | 7 ---- custom_messages/package.xml | 24 ------------ .../steering_controllers/CMakeLists.txt | 1 - .../steering_controllers.hpp | 4 +- .../steering_controllers/package.xml | 1 - .../tricycle_controller/package.xml | 2 + 7 files changed, 4 insertions(+), 72 deletions(-) delete mode 100644 custom_messages/CMakeLists.txt delete mode 100644 custom_messages/msg/AckermanControllerState.msg delete mode 100644 custom_messages/package.xml diff --git a/custom_messages/CMakeLists.txt b/custom_messages/CMakeLists.txt deleted file mode 100644 index bd66703e9e..0000000000 --- a/custom_messages/CMakeLists.txt +++ /dev/null @@ -1,37 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(custom_messages) - -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) -find_package(rosidl_default_generators REQUIRED) -find_package(std_msgs REQUIRED) -find_package(nav_msgs REQUIRED) - -rosidl_generate_interfaces(${PROJECT_NAME} - "msg/AckermanControllerState.msg" - DEPENDENCIES - std_msgs - nav_msgs - ) - - -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/custom_messages/msg/AckermanControllerState.msg b/custom_messages/msg/AckermanControllerState.msg deleted file mode 100644 index 95c29ebf3d..0000000000 --- a/custom_messages/msg/AckermanControllerState.msg +++ /dev/null @@ -1,7 +0,0 @@ -std_msgs/Header header -nav_msgs/Odometry odom -float64 rear_wheel_position -float64 rear_wheel_velocity -float64 steer_position -float64 linear_velocity_command -float64 steering_angle_command diff --git a/custom_messages/package.xml b/custom_messages/package.xml deleted file mode 100644 index 8bd88672c7..0000000000 --- a/custom_messages/package.xml +++ /dev/null @@ -1,24 +0,0 @@ - - - - custom_messages - 0.0.0 - TODO: Package description - tomislav - TODO: License declaration - - ament_cmake - - ament_lint_auto - ament_lint_common - rosidl_default_generators - std_msgs - nav_msgs - - rosidl_default_runtime - -rosidl_interface_packages - - ament_cmake - - diff --git a/steering_controllers/steering_controllers/CMakeLists.txt b/steering_controllers/steering_controllers/CMakeLists.txt index 24ed3ded46..018101bcd8 100644 --- a/steering_controllers/steering_controllers/CMakeLists.txt +++ b/steering_controllers/steering_controllers/CMakeLists.txt @@ -21,7 +21,6 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS tf2_msgs tf2_geometry_msgs ackermann_msgs - custom_messages steering_odometry ) diff --git a/steering_controllers/steering_controllers/include/steering_controllers/steering_controllers.hpp b/steering_controllers/steering_controllers/include/steering_controllers/steering_controllers.hpp index a01432e87d..31847b6c44 100644 --- a/steering_controllers/steering_controllers/include/steering_controllers/steering_controllers.hpp +++ b/steering_controllers/steering_controllers/include/steering_controllers/steering_controllers.hpp @@ -36,7 +36,7 @@ // TODO(anyone): Replace with controller specific messages #include "ackermann_msgs/msg/ackermann_drive.hpp" -#include "custom_messages/msg/ackerman_controller_state.hpp" +#include "control_msgs/msg/steering_controller_status.hpp" #include "geometry_msgs/msg/twist.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" @@ -133,7 +133,7 @@ class SteeringControllers : public controller_interface::ChainableControllerInte // Odometry related: steering_odometry::SteeringOdometry odometry_; - using AckermanControllerState = custom_messages::msg::AckermanControllerState; + using AckermanControllerState = control_msgs::msg::SteeringControllerStatus; AckermanControllerState published_state_; using ControllerStatePublisher = realtime_tools::RealtimePublisher; diff --git a/steering_controllers/steering_controllers/package.xml b/steering_controllers/steering_controllers/package.xml index 0a5463d199..0fe0935669 100644 --- a/steering_controllers/steering_controllers/package.xml +++ b/steering_controllers/steering_controllers/package.xml @@ -25,7 +25,6 @@ tf2_geometry_msgs ackermann_msgs steering_odometry - custom_messages ament_lint_auto ament_cmake_gmock diff --git a/steering_controllers/tricycle_controller/package.xml b/steering_controllers/tricycle_controller/package.xml index bd5204c357..6ca950c0c5 100644 --- a/steering_controllers/tricycle_controller/package.xml +++ b/steering_controllers/tricycle_controller/package.xml @@ -23,6 +23,8 @@ tf2 tf2_msgs ackermann_msgs + steering_controllers + stering_odometry pluginlib From 7ba0580f81e3ace65db502e69445dc1d380eae6b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tomislav=20Petkovi=C4=87?= <51706321+petkovich@users.noreply.github.com> Date: Sun, 18 Dec 2022 00:39:34 +0100 Subject: [PATCH 051/186] Update steering_controllers/ackermann_steering_controller/CMakeLists.txt MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Denis Štogl --- .../ackermann_steering_controller/CMakeLists.txt | 2 -- 1 file changed, 2 deletions(-) diff --git a/steering_controllers/ackermann_steering_controller/CMakeLists.txt b/steering_controllers/ackermann_steering_controller/CMakeLists.txt index 84c4053b14..7cded33397 100644 --- a/steering_controllers/ackermann_steering_controller/CMakeLists.txt +++ b/steering_controllers/ackermann_steering_controller/CMakeLists.txt @@ -42,8 +42,6 @@ generate_parameter_library(ackermann_steering_controller_parameters # include/ackermann_steering_controller/validate_ackermann_steering_controller_parameters.hpp ) - - add_library( ${PROJECT_NAME} SHARED From ace1ccb1483dc0e40dc5de703111ea997db15947 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tomislav=20Petkovi=C4=87?= <51706321+petkovich@users.noreply.github.com> Date: Sun, 18 Dec 2022 00:39:46 +0100 Subject: [PATCH 052/186] Update steering_controllers/ackermann_steering_controller/CMakeLists.txt MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Denis Štogl --- .../ackermann_steering_controller/CMakeLists.txt | 2 -- 1 file changed, 2 deletions(-) diff --git a/steering_controllers/ackermann_steering_controller/CMakeLists.txt b/steering_controllers/ackermann_steering_controller/CMakeLists.txt index 7cded33397..f065ce46fe 100644 --- a/steering_controllers/ackermann_steering_controller/CMakeLists.txt +++ b/steering_controllers/ackermann_steering_controller/CMakeLists.txt @@ -63,8 +63,6 @@ install( LIBRARY DESTINATION lib ) - - install( DIRECTORY include/ DESTINATION include From 4e132fdad24260c1af568e34ad526d7c89b40463 Mon Sep 17 00:00:00 2001 From: petkovich Date: Sun, 18 Dec 2022 12:02:01 +0100 Subject: [PATCH 053/186] cmakelists build and install interfaces --- .../ackermann_steering_controller/CMakeLists.txt | 2 +- .../steering_controllers/CMakeLists.txt | 12 +++++------- 2 files changed, 6 insertions(+), 8 deletions(-) diff --git a/steering_controllers/ackermann_steering_controller/CMakeLists.txt b/steering_controllers/ackermann_steering_controller/CMakeLists.txt index f065ce46fe..63d6a84671 100644 --- a/steering_controllers/ackermann_steering_controller/CMakeLists.txt +++ b/steering_controllers/ackermann_steering_controller/CMakeLists.txt @@ -65,7 +65,7 @@ install( install( DIRECTORY include/ - DESTINATION include + DESTINATION include/${PROJECT_NAME} ) if(BUILD_TESTING) diff --git a/steering_controllers/steering_controllers/CMakeLists.txt b/steering_controllers/steering_controllers/CMakeLists.txt index 018101bcd8..9922830197 100644 --- a/steering_controllers/steering_controllers/CMakeLists.txt +++ b/steering_controllers/steering_controllers/CMakeLists.txt @@ -39,8 +39,10 @@ generate_parameter_library(steering_controllers_parameters src/steering_controllers.yaml ) - -target_include_directories(${PROJECT_NAME} PRIVATE include) +#target_include_directories(${PROJECT_NAME} PRIVATE include) +target_include_directories(${PROJECT_NAME} PUBLIC + "$" + "$") target_link_libraries(${PROJECT_NAME} steering_controllers_parameters) ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS}) target_compile_definitions(${PROJECT_NAME} PRIVATE "STEERING_CONTROLLERS_BUILDING_DLL") @@ -56,14 +58,11 @@ install( LIBRARY DESTINATION lib ) - - install( DIRECTORY include/ - DESTINATION include + DESTINATION include/${PROJECT_NAME} ) - if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) # the following line skips the linter which checks for copyrights @@ -76,7 +75,6 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() endif() - ament_export_include_directories( include ) From 8a4d7c0780272210082a916828757307b0699cf1 Mon Sep 17 00:00:00 2001 From: petkovich Date: Sun, 18 Dec 2022 12:07:32 +0100 Subject: [PATCH 054/186] license and maintainers --- steering_controllers/steering_controllers/package.xml | 8 +++++--- steering_controllers/steering_odometry/package.xml | 8 +++++--- 2 files changed, 10 insertions(+), 6 deletions(-) diff --git a/steering_controllers/steering_controllers/package.xml b/steering_controllers/steering_controllers/package.xml index 0fe0935669..4b65bc5b23 100644 --- a/steering_controllers/steering_controllers/package.xml +++ b/steering_controllers/steering_controllers/package.xml @@ -3,9 +3,11 @@ steering_controllers 0.0.0 - TODO: Package description - tomislav - TODO: License declaration + Package for steering robot configurations including odometry and interfaces. + Apache License 2.0 + dr. sc. Tomislav Petkovic + Bence Magyar + Dr.-Ing. Denis Štogl generate_parameter_library diff --git a/steering_controllers/steering_odometry/package.xml b/steering_controllers/steering_odometry/package.xml index fe32d2d1f6..a128597f06 100644 --- a/steering_controllers/steering_odometry/package.xml +++ b/steering_controllers/steering_odometry/package.xml @@ -3,9 +3,11 @@ steering_odometry 0.0.0 - TODO: Package description - tomislav - TODO: License declaration + Package containing odometry implementation for steering robot configurations. + Apache License 2.0 + dr. sc. Tomislav Petkovic + Bence Magyar + Dr.-Ing. Denis Štogl ament_cmake realtime_tools From 27332c578fde76e1082dbea50a6bdf0e52e2b733 Mon Sep 17 00:00:00 2001 From: petkovich Date: Sun, 18 Dec 2022 19:04:12 +0100 Subject: [PATCH 055/186] removed ackermann package, integrated with steering controllers --- .../CMakeLists.txt | 124 ---- ...kermann_steering_controller_parameters.hpp | 38 -- .../visibility_control.h | 52 -- .../ackermann_steering_controller/package.xml | 42 -- .../src/ackermann_steering_controller.yaml | 164 ----- .../ackermann_steering_controller_params.yaml | 36 -- ...steering_controller_preceeding_params.yaml | 11 - .../test_ackermann_steering_controller.cpp | 566 ------------------ .../test_ackermann_steering_controller.hpp | 331 ---------- ...kermann_steering_controller_preceeding.cpp | 89 --- ...est_load_ackermann_steering_controller.cpp | 42 -- .../steering_controllers/CMakeLists.txt | 5 +- .../ackermann_steering_controller.hpp | 0 .../src/ackermann_steering_controller.cpp | 2 +- .../steering_controllers.xml} | 2 +- 15 files changed, 5 insertions(+), 1499 deletions(-) delete mode 100644 steering_controllers/ackermann_steering_controller/CMakeLists.txt delete mode 100644 steering_controllers/ackermann_steering_controller/include/ackermann_steering_controller/validate_ackermann_steering_controller_parameters.hpp delete mode 100644 steering_controllers/ackermann_steering_controller/include/ackermann_steering_controller/visibility_control.h delete mode 100644 steering_controllers/ackermann_steering_controller/package.xml delete mode 100644 steering_controllers/ackermann_steering_controller/src/ackermann_steering_controller.yaml delete mode 100644 steering_controllers/ackermann_steering_controller/test/ackermann_steering_controller_params.yaml delete mode 100644 steering_controllers/ackermann_steering_controller/test/ackermann_steering_controller_preceeding_params.yaml delete mode 100644 steering_controllers/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp delete mode 100644 steering_controllers/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp delete mode 100644 steering_controllers/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp delete mode 100644 steering_controllers/ackermann_steering_controller/test/test_load_ackermann_steering_controller.cpp rename steering_controllers/{ackermann_steering_controller/include/ackermann_steering_controller => steering_controllers/include/steering_controllers}/ackermann_steering_controller.hpp (100%) rename steering_controllers/{ackermann_steering_controller => steering_controllers}/src/ackermann_steering_controller.cpp (96%) rename steering_controllers/{ackermann_steering_controller/ackermann_steering_controller.xml => steering_controllers/steering_controllers.xml} (87%) diff --git a/steering_controllers/ackermann_steering_controller/CMakeLists.txt b/steering_controllers/ackermann_steering_controller/CMakeLists.txt deleted file mode 100644 index 63d6a84671..0000000000 --- a/steering_controllers/ackermann_steering_controller/CMakeLists.txt +++ /dev/null @@ -1,124 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(ackermann_steering_controller) - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -# find dependencies -set(THIS_PACKAGE_INCLUDE_DEPENDS - control_msgs - controller_interface - geometry_msgs - hardware_interface - nav_msgs - pluginlib - rclcpp - rclcpp_lifecycle - realtime_tools - std_srvs - tf2 - tf2_msgs - tf2_geometry_msgs - ackermann_msgs - custom_messages - steering_odometry - steering_controllers -) - -find_package(ament_cmake REQUIRED) -find_package(generate_parameter_library REQUIRED) -foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) - find_package(${Dependency} REQUIRED) -endforeach() - -set(msg_files - msg/AckermanControllerState.msg -) - -# Add ackermann_steering_controller library related compile commands -generate_parameter_library(ackermann_steering_controller_parameters - src/ackermann_steering_controller.yaml - # include/ackermann_steering_controller/validate_ackermann_steering_controller_parameters.hpp -) - -add_library( - ${PROJECT_NAME} - SHARED - src/ackermann_steering_controller.cpp -) -target_include_directories(${PROJECT_NAME} PRIVATE include) -target_link_libraries(${PROJECT_NAME} ackermann_steering_controller_parameters) -ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS}) -target_compile_definitions(${PROJECT_NAME} PRIVATE "ACKERMANN_STEERING_CONTROLLER_BUILDING_DLL") - -pluginlib_export_plugin_description_file( - controller_interface ackermann_steering_controller.xml) - -install( - TARGETS - ${PROJECT_NAME} - RUNTIME DESTINATION bin - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib -) - -install( - DIRECTORY include/ - DESTINATION include/${PROJECT_NAME} -) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # comment the line when a copyright and license is added to all source files - set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # comment the line when this package is in a git repo and when - # a copyright and license is added to all source files - set(ament_cmake_cpplint_FOUND TRUE) - 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_ackermann_steering_controller test/test_load_ackermann_steering_controller.cpp) - # target_include_directories(test_load_ackermann_steering_controller PRIVATE include) - # ament_target_dependencies( - # test_load_ackermann_steering_controller - # controller_manager - # hardware_interface - # ros2_control_test_assets - # ) - - # add_rostest_with_parameters_gmock(test_ackermann_steering_controller test/test_ackermann_steering_controller.cpp ${CMAKE_CURRENT_SOURCE_DIR}/test/ackermann_steering_controller_params.yaml) - # target_include_directories(test_ackermann_steering_controller PRIVATE include) - # target_link_libraries(test_ackermann_steering_controller ackermann_steering_controller) - # ament_target_dependencies( - # test_ackermann_steering_controller - # controller_interface - # hardware_interface - # ) - - # add_rostest_with_parameters_gmock(test_ackermann_steering_controller_preceeding test/test_ackermann_steering_controller_preceeding.cpp ${CMAKE_CURRENT_SOURCE_DIR}/test/ackermann_steering_controller_preceeding_params.yaml) - # target_include_directories(test_ackermann_steering_controller_preceeding PRIVATE include) - # target_link_libraries(test_ackermann_steering_controller_preceeding ackermann_steering_controller) - # ament_target_dependencies( - # test_ackermann_steering_controller_preceeding - # controller_interface - # hardware_interface - # ) -endif() - -ament_export_include_directories( - include -) - -ament_export_dependencies( - ${THIS_PACKAGE_INCLUDE_DEPENDS} -) -ament_export_libraries( - ${PROJECT_NAME} -) - -ament_package() diff --git a/steering_controllers/ackermann_steering_controller/include/ackermann_steering_controller/validate_ackermann_steering_controller_parameters.hpp b/steering_controllers/ackermann_steering_controller/include/ackermann_steering_controller/validate_ackermann_steering_controller_parameters.hpp deleted file mode 100644 index 3003f6236d..0000000000 --- a/steering_controllers/ackermann_steering_controller/include/ackermann_steering_controller/validate_ackermann_steering_controller_parameters.hpp +++ /dev/null @@ -1,38 +0,0 @@ -// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef ACKERMANN_STEERING_CONTROLLER__VALIDATE_ACKERMANN_STEERING_CONTROLLER_PARAMETERS_HPP_ -#define ACKERMANN_STEERING_CONTROLLER__VALIDATE_ACKERMANN_STEERING_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 // ACKERMANN_STEERING_CONTROLLER__VALIDATE_ACKERMANN_STEERING_CONTROLLER_PARAMETERS_HPP_ diff --git a/steering_controllers/ackermann_steering_controller/include/ackermann_steering_controller/visibility_control.h b/steering_controllers/ackermann_steering_controller/include/ackermann_steering_controller/visibility_control.h deleted file mode 100644 index c494ba3bb4..0000000000 --- a/steering_controllers/ackermann_steering_controller/include/ackermann_steering_controller/visibility_control.h +++ /dev/null @@ -1,52 +0,0 @@ -// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef ACKERMANN_STEERING_CONTROLLER__VISIBILITY_CONTROL_H_ -#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_CONTROL_H_ - -// This logic was borrowed (then namespaced) from the examples on the gcc wiki: -// https://gcc.gnu.org/wiki/Visibility - -#if defined _WIN32 || defined __CYGWIN__ -#ifdef __GNUC__ -#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_EXPORT __attribute__((dllexport)) -#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_IMPORT __attribute__((dllimport)) -#else -#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_EXPORT __declspec(dllexport) -#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_IMPORT __declspec(dllimport) -#endif -#ifdef ACKERMANN_STEERING_CONTROLLER__VISIBILITY_BUILDING_DLL -#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC \ - ACKERMANN_STEERING_CONTROLLER__VISIBILITY_EXPORT -#else -#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC \ - ACKERMANN_STEERING_CONTROLLER__VISIBILITY_IMPORT -#endif -#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC_TYPE \ - ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC -#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_LOCAL -#else -#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_EXPORT __attribute__((visibility("default"))) -#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_IMPORT -#if __GNUC__ >= 4 -#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC __attribute__((visibility("default"))) -#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_LOCAL __attribute__((visibility("hidden"))) -#else -#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC -#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_LOCAL -#endif -#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC_TYPE -#endif - -#endif // ACKERMANN_STEERING_CONTROLLER__VISIBILITY_CONTROL_H_ diff --git a/steering_controllers/ackermann_steering_controller/package.xml b/steering_controllers/ackermann_steering_controller/package.xml deleted file mode 100644 index 4f07afb940..0000000000 --- a/steering_controllers/ackermann_steering_controller/package.xml +++ /dev/null @@ -1,42 +0,0 @@ - - - - ackermann_steering_controller - 0.0.0 - DESCRIPTION - giridharvb - MIT - - ament_cmake - - generate_parameter_library - - control_msgs - controller_interface - geometry_msgs - hardware_interface - nav_msgs - pluginlib - rclcpp - rclcpp_lifecycle - realtime_tools - rcpputils - std_srvs - tf2 - tf2_msgs - tf2_geometry_msgs - ackermann_msgs - steering_odometry - custom_messages - steering_controllers - - ament_lint_auto - ament_cmake_gmock - controller_manager - hardware_interface - ros2_control_test_assets - - - ament_cmake - - diff --git a/steering_controllers/ackermann_steering_controller/src/ackermann_steering_controller.yaml b/steering_controllers/ackermann_steering_controller/src/ackermann_steering_controller.yaml deleted file mode 100644 index bd33eb00af..0000000000 --- a/steering_controllers/ackermann_steering_controller/src/ackermann_steering_controller.yaml +++ /dev/null @@ -1,164 +0,0 @@ -ackermann_steering_controller: - reference_timeout: { - type: double, - default_value: 1, - description: "Timeout for controller references after which they will be reset. This is especially useful for controllers that can cause unwanted and dangerous behaviour if reference is not reset, e.g., velocity controllers. If value is 0 the reference is reset after each run.", -# validation: { -# gt_eq<>: 0.0, -# } - } - - rear_wheel_names: { - type: string_array, - default_value: ["rear_wheel_joint"], - description: "Name of rear wheels.", - read_only: true, - - } - - front_steer_names: { - type: string_array, - default_value: ["front_steer_joint"], - description: "Names of front steer wheels.", - read_only: true, - - } - - open_loop: { - type: bool, - default_value: false, - description: "bool parameter decides if open oop or not (feedback).", - read_only: false, - - } - - wheel_separation_multiplier: { - type: double, - default_value: 1.0, - description: "Wheel separation height will be multiplied by value of the wheel_separation_h_multiplier.", - read_only: false, - - } - - wheel_separation: { - type: double, - default_value: 0.0, - description: "Wheel separation.", - read_only: false, - - } - - wheelbase_multiplier: { - type: double, - default_value: 1.0, - description: "Wheelbase will be multiplied by value of the wheelbase_multiplier.", - read_only: false, - - } - - wheelbase: { - type: double, - default_value: 0.0, - description: "Wheelbase length.", - read_only: false, - - } - - wheel_radius: { - type: double, - default_value: 0.0, - description: "Wheel radius.", - read_only: false, - - } - - wheel_radius_multiplier: { - type: double, - default_value: 1.0, - description: "Wheel radius will be multiplied by value of wheel_radius_multiplier.", - read_only: false, - - } - - steer_pos_multiplier: { - type: double, - default_value: 1.0, - description: "Steer pos will be multiplied by value of steer_pos_multiplier.", - read_only: false, - - } - - velocity_rolling_window_size: { - type: int, - default_value: 10, - description: "The number of velocity samples to average together to compute the odometry twist.linear.x and twist.angular.z velocities.", - read_only: false, - - } - - allow_multiple_cmd_vel_publishers: { - type: bool, - default_value: true, - description: "Allow multiple cmd_vel publishers is enabled or disabled?.", - read_only: false, - - } - - base_frame_id: { - type: string, - default_value: "base_link", - description: "Base frame_id set to value of base_frame_id.", - read_only: false, - - } - - odom_frame_id: { - type: string, - default_value: "odom", - description: "Odometry frame_id set to value of odom_frame_id.", - read_only: false, - - } - - - enable_odom_tf: { - type: bool, - default_value: true, - description: "Publishing to tf is enabled or disabled ?.", - read_only: false, - - } - - twist_covariance_diagonal: { - type: double_array, - default_value: [0, 7, 14, 21, 28, 35], - description: "diagonal values of twist covariance matrix.", - read_only: false, - - } - - pose_covariance_diagonal: { - type: double_array, - default_value: [0, 7, 14, 21, 28, 35], - description: "diagonal values of pose covariance matrix.", - read_only: false, - - } - - position_feedback: { - type: bool, - default_value: false, - description: "choice of feedbacktype, if position_feedback is false then HW_IF_VELOCITY is taken as interface type, if - position_feedback is true then HW_IF_POSITION is taken as interface type", - read_only: false, - - } - - use_stamped_vel: { - type: bool, - default_value: false, - description: "choice of vel type, if use_stamped_vel is false then geometry_msgs::msg::Twist is taken as vel msg type, if - use_stamped_vel is true then geometry_msgs::msg::TwistStamped is taken as vel msg type", - read_only: false, - - } diff --git a/steering_controllers/ackermann_steering_controller/test/ackermann_steering_controller_params.yaml b/steering_controllers/ackermann_steering_controller/test/ackermann_steering_controller_params.yaml deleted file mode 100644 index bc92244584..0000000000 --- a/steering_controllers/ackermann_steering_controller/test/ackermann_steering_controller_params.yaml +++ /dev/null @@ -1,36 +0,0 @@ -test_ackermann_steering_controller: - ros__parameters: - - reference_timeout: 0.2 - - rear_wheel_name: "rear_wheel_joint" - - front_steer_name: "front_steer_joint" - - open_loop: false - - wheel_separation_multiplier: 1.0 - - wheel_separation: 0.0 - - wheel_radius: 1.0 - - steer_pos_multiplier: 1.0 - - velocity_rolling_window_size: 10 - - allow_multiple_cmd_vel_publishers: true - - base_frame_id: "base_link" - - odom_frame_id: "odom" - - enable_odom_tf: true - - twist_covariance_diagonal: [0.0, 7.0, 14.0, 21.0, 28.0, 35.0] - - pose_covariance_diagonal: [0.0, 7.0, 14.0, 21.0, 28.0, 35.0] - - position_feedback: false - - use_stamped_vel: false diff --git a/steering_controllers/ackermann_steering_controller/test/ackermann_steering_controller_preceeding_params.yaml b/steering_controllers/ackermann_steering_controller/test/ackermann_steering_controller_preceeding_params.yaml deleted file mode 100644 index f19ef15914..0000000000 --- a/steering_controllers/ackermann_steering_controller/test/ackermann_steering_controller_preceeding_params.yaml +++ /dev/null @@ -1,11 +0,0 @@ -test_ackermann_steering_controller: - ros__parameters: - joints: - - joint1 - - interface_name: acceleration - - state_joints: - - joint1state - - reference_timeout: 0.1 diff --git a/steering_controllers/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp b/steering_controllers/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp deleted file mode 100644 index da93778310..0000000000 --- a/steering_controllers/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp +++ /dev/null @@ -1,566 +0,0 @@ -// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include -#include -#include -#include - -#include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "test_ackermann_steering_controller.hpp" - -using ackermann_steering_controller::NR_CMD_ITFS; -using ackermann_steering_controller::NR_REF_ITFS; -using ackermann_steering_controller::NR_STATE_ITFS; - -class AckermannSteeringControllerTest -: public AckermannSteeringControllerFixture -{ -}; - -TEST_F(AckermannSteeringControllerTest, 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_->params_.reference_timeout, 1); - - 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_); - ASSERT_EQ(controller_->params_.reference_timeout, 0.2); -} - -TEST_F(AckermannSteeringControllerTest, 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()); - EXPECT_EQ( - command_intefaces.names[0], - controller_->params_.rear_wheel_name + "/" + hardware_interface::HW_IF_VELOCITY); - EXPECT_EQ( - command_intefaces.names[1], - controller_->params_.front_steer_name + "/" + hardware_interface::HW_IF_POSITION); - - auto state_intefaces = controller_->state_interface_configuration(); - ASSERT_EQ(state_intefaces.names.size(), joint_state_values_.size()); - EXPECT_EQ( - state_intefaces.names[0], - controller_->params_.rear_wheel_name + "/" + hardware_interface::HW_IF_POSITION); - EXPECT_EQ( - state_intefaces.names[1], - controller_->params_.front_steer_name + "/" + hardware_interface::HW_IF_POSITION); - - // // check ref itfs - auto reference_interfaces = controller_->export_reference_interfaces(); - ASSERT_EQ(reference_interfaces.size(), NR_REF_ITFS); - - const std::string ref_itf_name_0 = std::string(controller_->get_node()->get_name()) + "/" + - "linear" + "/" + hardware_interface::HW_IF_VELOCITY; - EXPECT_EQ(reference_interfaces[0].get_name(), ref_itf_name_0); - EXPECT_EQ(reference_interfaces[0].get_prefix_name(), controller_->get_node()->get_name()); - EXPECT_EQ( - reference_interfaces[0].get_interface_name(), - std::string("linear") + "/" + hardware_interface::HW_IF_VELOCITY); - - const std::string ref_itf_name_1 = std::string(controller_->get_node()->get_name()) + "/" + - "angular" + "/" + hardware_interface::HW_IF_POSITION; - EXPECT_EQ(reference_interfaces[1].get_name(), ref_itf_name_1); - EXPECT_EQ(reference_interfaces[1].get_prefix_name(), controller_->get_node()->get_name()); - EXPECT_EQ( - reference_interfaces[1].get_interface_name(), - std::string("angular") + "/" + hardware_interface::HW_IF_POSITION); -} - -TEST_F(AckermannSteeringControllerTest, 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_TRUE(std::isnan((*msg)->twist.linear.x)); - - ASSERT_TRUE(std::isnan((*msg)->twist.angular.z)); - - EXPECT_EQ(controller_->reference_interfaces_.size(), NR_REF_ITFS); - for (const auto & interface : controller_->reference_interfaces_) - { - EXPECT_TRUE(std::isnan(interface)); - } -} - -TEST_F(AckermannSteeringControllerTest, 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_reference_from_subscribers(), controller_interface::return_type::OK); - ASSERT_EQ( - controller_->update_and_write_commands( - controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); -} - -TEST_F(AckermannSteeringControllerTest, 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(AckermannSteeringControllerTest, 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_[NR_CMD_ITFS - 2].get_value(), 101.101); - ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_TRUE(std::isnan(controller_->command_interfaces_[NR_CMD_ITFS - 2].get_value())); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_TRUE(std::isnan(controller_->command_interfaces_[NR_CMD_ITFS - 2].get_value())); - - ASSERT_EQ( - controller_->update_reference_from_subscribers(), controller_interface::return_type::OK); - ASSERT_EQ( - controller_->update_and_write_commands( - controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); -} - -TEST_F(AckermannSteeringControllerTest, publish_status_success) -{ - SetUpController(); - - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - controller_->reference_interfaces_[0] = 2.3; - controller_->reference_interfaces_[1] = 4.4; - ASSERT_EQ( - controller_->update_and_write_commands( - controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - - ControllerStateMsgOdom msg; - subscribe_and_get_messages(msg); - - ASSERT_EQ(msg.pose.pose.position.z, 0); -} - -TEST_F(AckermannSteeringControllerTest, 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); - controller_->reference_interfaces_[0] = 2.3; - controller_->reference_interfaces_[1] = 4.4; - ASSERT_EQ( - controller_->update_and_write_commands( - controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - ControllerStateMsgOdom msg; - subscribe_and_get_messages(msg); - - ASSERT_EQ(msg.pose.pose.position.z, 0); - - if (controller_->params_.use_stamped_vel) - { - publish_commands(controller_->get_node()->now()); - } - else - { - publish_commands_unstamped(); - } - ASSERT_TRUE(controller_->wait_for_commands(executor)); - - ASSERT_EQ( - controller_->update_reference_from_subscribers(), controller_interface::return_type::OK); - ASSERT_EQ( - controller_->update_and_write_commands( - controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - EXPECT_EQ(joint_command_values_[NR_CMD_ITFS - 2], 0.45); - - subscribe_and_get_messages(msg); - - ASSERT_EQ(msg.pose.pose.position.z, 0); -} - -TEST_F(AckermannSteeringControllerTest, test_sending_too_old_message) -{ - 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); - - // try to set command with time before timeout - command is not updated - auto reference = controller_->input_ref_.readFromNonRT(); - auto old_timestamp = (*reference)->header.stamp; - EXPECT_TRUE(std::isnan((*reference)->twist.linear.x)); - EXPECT_TRUE(std::isnan((*reference)->twist.angular.z)); - if (controller_->params_.use_stamped_vel) - { - publish_commands( - controller_->get_node()->now() - controller_->ref_timeout_ - - rclcpp::Duration::from_seconds(0.1)); - ASSERT_TRUE(controller_->wait_for_commands(executor)); - ASSERT_EQ(old_timestamp, (*(controller_->input_ref_.readFromNonRT()))->header.stamp); - EXPECT_TRUE(std::isnan((*reference)->twist.linear.x)); - EXPECT_TRUE(std::isnan((*reference)->twist.angular.z)); - } - else - { - publish_commands_unstamped(); - ASSERT_TRUE(controller_->wait_for_commands(executor)); - ASSERT_NE(old_timestamp, (*(controller_->input_ref_.readFromNonRT()))->header.stamp); - EXPECT_EQ((*reference)->twist.linear.x, 0.45); - EXPECT_EQ((*reference)->twist.angular.z, 0.0); - } - - // EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x, - // 0.45); - // EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z, - // 0.0); -} - -TEST_F(AckermannSteeringControllerTest, test_time_stamp_zero) -{ - 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); - - // try to set command with time before timeout - command is not updated - auto reference = controller_->input_ref_.readFromNonRT(); - auto old_timestamp = (*reference)->header.stamp; - EXPECT_TRUE(std::isnan((*reference)->twist.linear.x)); - EXPECT_TRUE(std::isnan((*reference)->twist.angular.z)); - if (controller_->params_.use_stamped_vel) - { - publish_commands(rclcpp::Time(0)); - } - else - { - publish_commands_unstamped(); - } - ASSERT_TRUE(controller_->wait_for_commands(executor)); - ASSERT_EQ(old_timestamp.sec, (*(controller_->input_ref_.readFromNonRT()))->header.stamp.sec); - EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x)); - EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z)); - EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x, 0.45); - EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z, 0.0); -} - -TEST_F(AckermannSteeringControllerTest, test_message_accepted) -{ - 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); - - // try to set command with time before timeout - command is not updated - auto reference = controller_->input_ref_.readFromNonRT(); - EXPECT_TRUE(std::isnan((*reference)->twist.linear.x)); - EXPECT_TRUE(std::isnan((*reference)->twist.angular.z)); - if (controller_->params_.use_stamped_vel) - { - publish_commands(controller_->get_node()->now()); - } - else - { - publish_commands_unstamped(); - } - ASSERT_TRUE(controller_->wait_for_commands(executor)); - EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x)); - EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z)); - EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x, 0.45); - EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z, 0.0); -} - -TEST_F(AckermannSteeringControllerTest, test_update_logic_chainable) -{ - // 1. age>ref_timeout 2. ageget_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_LINEAR_VELOCITY_X = 1.5; - static constexpr double TEST_ANGULAR_VELOCITY_Z = 1.1; - joint_command_values_[NR_CMD_ITFS - 2] = 111; - std::shared_ptr msg = std::make_shared(); - - msg->header.stamp = controller_->get_node()->now() - controller_->ref_timeout_ - - rclcpp::Duration::from_seconds(0.1); - msg->twist.linear.x = TEST_LINEAR_VELOCITY_X; - msg->twist.linear.y = std::numeric_limits::quiet_NaN(); - msg->twist.linear.z = std::numeric_limits::quiet_NaN(); - msg->twist.angular.x = std::numeric_limits::quiet_NaN(); - msg->twist.angular.y = std::numeric_limits::quiet_NaN(); - msg->twist.angular.z = TEST_ANGULAR_VELOCITY_Z; - controller_->input_ref_.writeFromNonRT(msg); - const auto age_of_last_command = - controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; - - // age_of_last_command > ref_timeout_ - ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_); - ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); - ASSERT_EQ( - controller_->update_reference_from_subscribers(), controller_interface::return_type::OK); - ASSERT_EQ( - controller_->update_and_write_commands( - controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - - EXPECT_EQ(joint_command_values_[NR_CMD_ITFS - 2], 0); - ASSERT_EQ(controller_->reference_interfaces_[0], 0); - for (const auto & interface : controller_->reference_interfaces_) - { - ASSERT_EQ(interface, 0); - } - - std::shared_ptr msg_2 = std::make_shared(); - msg_2->header.stamp = controller_->get_node()->now() - rclcpp::Duration::from_seconds(0.01); - msg_2->twist.linear.x = TEST_LINEAR_VELOCITY_X; - msg_2->twist.linear.y = std::numeric_limits::quiet_NaN(); - msg_2->twist.linear.z = std::numeric_limits::quiet_NaN(); - msg_2->twist.angular.x = std::numeric_limits::quiet_NaN(); - msg_2->twist.angular.y = std::numeric_limits::quiet_NaN(); - msg_2->twist.angular.z = TEST_ANGULAR_VELOCITY_Z; - controller_->input_ref_.writeFromNonRT(msg_2); - const auto age_of_last_command_2 = - controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; - - // age_of_last_command_2 < ref_timeout_ - ASSERT_TRUE(age_of_last_command_2 <= controller_->ref_timeout_); - ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); - ASSERT_EQ( - controller_->update_reference_from_subscribers(), controller_interface::return_type::OK); - ASSERT_EQ( - controller_->update_and_write_commands( - controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - - EXPECT_NE(joint_command_values_[NR_CMD_ITFS - 2], 111); - ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); - for (const auto & interface : controller_->reference_interfaces_) - { - EXPECT_FALSE(std::isnan(interface)); - } -} - -TEST_F(AckermannSteeringControllerTest, test_update_logic) -{ - 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); - - auto reference = controller_->input_ref_.readFromNonRT(); - - // set command statically - static constexpr double TEST_LINEAR_VELOCITY_X = 1.5; - static constexpr double TEST_ANGULAR_VELOCITY_Z = 1.1; - joint_command_values_[NR_CMD_ITFS - 2] = 111; - std::shared_ptr msg = std::make_shared(); - msg->header.stamp = controller_->get_node()->now() - controller_->ref_timeout_ - - rclcpp::Duration::from_seconds(0.1); - msg->twist.linear.x = TEST_LINEAR_VELOCITY_X; - msg->twist.linear.y = std::numeric_limits::quiet_NaN(); - msg->twist.linear.z = std::numeric_limits::quiet_NaN(); - msg->twist.angular.x = std::numeric_limits::quiet_NaN(); - msg->twist.angular.y = std::numeric_limits::quiet_NaN(); - msg->twist.angular.z = TEST_ANGULAR_VELOCITY_Z; - controller_->input_ref_.writeFromNonRT(msg); - const auto age_of_last_command = - controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; - - ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_); - ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); - ASSERT_EQ( - controller_->update_reference_from_subscribers(), controller_interface::return_type::OK); - ASSERT_EQ( - controller_->update_and_write_commands( - controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - - EXPECT_EQ(joint_command_values_[NR_CMD_ITFS - 2], 0); - ASSERT_EQ(controller_->reference_interfaces_[0], 0); - - std::shared_ptr msg_2 = std::make_shared(); - msg_2->header.stamp = controller_->get_node()->now(); - msg_2->twist.linear.x = TEST_LINEAR_VELOCITY_X; - msg_2->twist.linear.y = std::numeric_limits::quiet_NaN(); - msg_2->twist.linear.z = std::numeric_limits::quiet_NaN(); - msg_2->twist.angular.x = std::numeric_limits::quiet_NaN(); - msg_2->twist.angular.y = std::numeric_limits::quiet_NaN(); - msg_2->twist.angular.z = TEST_ANGULAR_VELOCITY_Z; - controller_->input_ref_.writeFromNonRT(msg_2); - const auto age_of_last_command_2 = - controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; - - ASSERT_TRUE(age_of_last_command_2 <= controller_->ref_timeout_); - ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); - ASSERT_EQ( - controller_->update_reference_from_subscribers(), controller_interface::return_type::OK); - ASSERT_EQ( - controller_->update_and_write_commands( - controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - - EXPECT_EQ(joint_command_values_[NR_CMD_ITFS - 2], TEST_LINEAR_VELOCITY_X); - EXPECT_NE(joint_command_values_[NR_CMD_ITFS - 2], 111); - ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); -} - -TEST_F(AckermannSteeringControllerTest, test_ref_timeout_zero_for_update) -{ - 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); - - auto reference = controller_->input_ref_.readFromNonRT(); - - // set command statically - static constexpr double TEST_LINEAR_VELOCITY_X = 1.5; - static constexpr double TEST_ANGULAR_VELOCITY_Z = 1.1; - - controller_->ref_timeout_ = rclcpp::Duration::from_seconds(0.0); - std::shared_ptr msg = std::make_shared(); - - msg->header.stamp = controller_->get_node()->now() - rclcpp::Duration::from_seconds(0.0); - msg->twist.linear.x = TEST_LINEAR_VELOCITY_X; - msg->twist.linear.y = std::numeric_limits::quiet_NaN(); - msg->twist.linear.z = std::numeric_limits::quiet_NaN(); - msg->twist.angular.x = std::numeric_limits::quiet_NaN(); - msg->twist.angular.y = std::numeric_limits::quiet_NaN(); - msg->twist.angular.z = TEST_ANGULAR_VELOCITY_Z; - controller_->input_ref_.writeFromNonRT(msg); - const auto age_of_last_command = - controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; - - ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); - ASSERT_EQ( - controller_->update_reference_from_subscribers(), controller_interface::return_type::OK); - ASSERT_EQ( - controller_->update_and_write_commands( - controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - - EXPECT_EQ(joint_command_values_[NR_STATE_ITFS - 2], TEST_LINEAR_VELOCITY_X); - ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); - ASSERT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x)); -} - -TEST_F(AckermannSteeringControllerTest, test_ref_timeout_zero_for_reference_callback) -{ - 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); - - EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x)); - EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z)); - controller_->ref_timeout_ = rclcpp::Duration::from_seconds(0.0); - if (controller_->params_.use_stamped_vel) - { - publish_commands(controller_->get_node()->now()); - } - else - { - publish_commands_unstamped(); - } - ASSERT_TRUE(controller_->wait_for_commands(executor)); - - EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x)); - EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z)); - EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x, 0.45); - EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z, 0.0); -} - -//test is expected to fail in case of param use_stamped_vel is true - -TEST_F(AckermannSteeringControllerTest, test_age_of_last_command_zero_reference_callback_unstamped) -{ - fprintf( - stderr, - "-------------------------Expect to fail when use_stamped_vel is true---------------- \n"); - 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); - - EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x)); - EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z)); - publish_commands_unstamped(); - ASSERT_TRUE(controller_->wait_for_commands(executor)); - - const auto age_of_last_command = - controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; - ASSERT_TRUE( - age_of_last_command >= rclcpp::Duration::from_seconds(0) && - age_of_last_command <= rclcpp::Duration::from_seconds(0.00001)); - - EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x)); - EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z)); - EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x, 0.45); - EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z, 0.0); -} - -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/steering_controllers/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp b/steering_controllers/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp deleted file mode 100644 index 13378e91a5..0000000000 --- a/steering_controllers/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp +++ /dev/null @@ -1,331 +0,0 @@ -// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) -// (template) -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef TEST_ACKERMANN_STEERING_CONTROLLER_HPP_ -#define TEST_ACKERMANN_STEERING_CONTROLLER_HPP_ - -#include -#include -#include -#include -#include -#include -#include - -#include "ackermann_steering_controller/ackermann_steering_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 "hardware_interface/types/hardware_interface_type_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 ControllerStateMsgOdom = - ackermann_steering_controller::AckermannSteeringController::ControllerStateMsgOdom; -using ControllerStateMsgTf = - ackermann_steering_controller::AckermannSteeringController::ControllerStateMsgTf; -using ControllerReferenceMsg = - ackermann_steering_controller::AckermannSteeringController::ControllerReferenceMsg; - -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 TestableAckermannSteeringController -: public ackermann_steering_controller::AckermannSteeringController -{ - FRIEND_TEST(AckermannSteeringControllerTest, all_parameters_set_configure_success); - FRIEND_TEST(AckermannSteeringControllerTest, check_exported_intefaces); - FRIEND_TEST(AckermannSteeringControllerTest, activate_success); - FRIEND_TEST(AckermannSteeringControllerTest, update_success); - FRIEND_TEST(AckermannSteeringControllerTest, deactivate_success); - FRIEND_TEST(AckermannSteeringControllerTest, reactivate_success); - FRIEND_TEST(AckermannSteeringControllerTest, publish_status_success); - FRIEND_TEST(AckermannSteeringControllerTest, receive_message_and_publish_updated_status); - FRIEND_TEST(AckermannSteeringControllerTest, test_sending_too_old_message); - FRIEND_TEST(AckermannSteeringControllerTest, test_time_stamp_zero); - FRIEND_TEST(AckermannSteeringControllerTest, test_message_accepted); - FRIEND_TEST(AckermannSteeringControllerTest, test_update_logic); - FRIEND_TEST(AckermannSteeringControllerTest, test_update_logic_chainable); - FRIEND_TEST(AckermannSteeringControllerTest, test_ref_timeout_zero_for_update); - FRIEND_TEST(AckermannSteeringControllerTest, test_ref_timeout_zero_for_reference_callback); - FRIEND_TEST( - AckermannSteeringControllerTest, test_age_of_last_command_zero_reference_callback_unstamped); - -public: - controller_interface::CallbackReturn on_configure( - const rclcpp_lifecycle::State & previous_state) override - { - auto ret = - ackermann_steering_controller::AckermannSteeringController::on_configure(previous_state); - // Only if on_configure is successful create subscription - if (ret == CallbackReturn::SUCCESS) - { - if (params_.use_stamped_vel) - { - ref_subscriber_wait_set_.add_subscription(ref_subscriber_); - } - else - { - ref_subscriber_wait_set_.add_subscription(ref_subscriber_unstamped_); - } - } - return ret; - } - - controller_interface::CallbackReturn on_activate( - const rclcpp_lifecycle::State & previous_state) override - { - auto ref_itfs = on_export_reference_interfaces(); - return ackermann_steering_controller::AckermannSteeringController::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 AckermannSteeringControllerFixture : 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_ackermann_steering_controller/reference", rclcpp::SystemDefaultsQoS()); - command_publisher_node_unstamped_ = - std::make_shared("command_publisher_unstamped"); - command_publisher_unstamped_ = - command_publisher_node_unstamped_->create_publisher( - "/test_ackermann_steering_controller/reference_unstamped", rclcpp::SystemDefaultsQoS()); - } - - static void TearDownTestCase() {} - - void TearDown() { controller_.reset(nullptr); } - -protected: - void SetUpController(const std::string controller_name = "test_ackermann_steering_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()); - - command_itfs_.emplace_back(hardware_interface::CommandInterface( - rear_wheel_name, hardware_interface::HW_IF_VELOCITY, &joint_command_values_[0])); - command_ifs.emplace_back(command_itfs_.back()); - - command_itfs_.emplace_back(hardware_interface::CommandInterface( - front_steer_name, hardware_interface::HW_IF_VELOCITY, &joint_command_values_[1])); - 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()); - - state_itfs_.emplace_back(hardware_interface::StateInterface( - rear_wheel_name, hardware_interface::HW_IF_POSITION, &joint_state_values_[0])); - state_ifs.emplace_back(state_itfs_.back()); - - state_itfs_.emplace_back(hardware_interface::StateInterface( - front_steer_name, hardware_interface::HW_IF_POSITION, &joint_state_values_[1])); - 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(ControllerStateMsgOdom & msg_odom) - { - // create a new subscriber - rclcpp::Node test_subscription_node("test_subscription_node"); - auto subs_callback_odom = [&](const ControllerStateMsgOdom::SharedPtr) {}; - auto subscription_odom = test_subscription_node.create_subscription( - "/test_ackermann_steering_controller/odometry", 10, subs_callback_odom); - - // call update to publish the test value - ASSERT_EQ( - controller_->update_reference_from_subscribers(), controller_interface::return_type::OK); - ASSERT_EQ( - controller_->update_and_write_commands( - controller_->get_node()->now(), 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_odom); - while (max_sub_check_loop_count--) - { - controller_->update_reference_from_subscribers(); - controller_->update_and_write_commands( - controller_->get_node()->now(), 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_odom_info; - ASSERT_TRUE(subscription_odom->take(msg_odom, msg_odom_info)); - } - - // TODO(anyone): add/remove arguments as it suites your command message type - void publish_commands( - const rclcpp::Time & stamp, const double & twist_linear_x = 0.45, - const double & twist_angular_z = 0.0) - { - 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.header.stamp = stamp; - msg.twist.linear.x = twist_linear_x; - msg.twist.linear.y = std::numeric_limits::quiet_NaN(); - msg.twist.linear.z = std::numeric_limits::quiet_NaN(); - msg.twist.angular.x = std::numeric_limits::quiet_NaN(); - msg.twist.angular.y = std::numeric_limits::quiet_NaN(); - msg.twist.angular.z = twist_angular_z; - - command_publisher_->publish(msg); - } - - void publish_commands_unstamped( - const double & twist_linear_x = 0.45, const double & twist_angular_z = 0.0) - { - auto wait_for_topic = [&](const auto topic_name) - { - size_t wait_count = 0; - while (command_publisher_node_unstamped_->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_unstamped_->get_topic_name()); - - geometry_msgs::msg::Twist msg_unstamped; - - msg_unstamped.linear.x = twist_linear_x; - msg_unstamped.linear.y = std::numeric_limits::quiet_NaN(); - msg_unstamped.linear.z = std::numeric_limits::quiet_NaN(); - msg_unstamped.angular.x = std::numeric_limits::quiet_NaN(); - msg_unstamped.angular.y = std::numeric_limits::quiet_NaN(); - msg_unstamped.angular.z = twist_angular_z; - - command_publisher_unstamped_->publish(msg_unstamped); - } - -protected: - // TODO(anyone): adjust the members as needed - std::string rear_wheel_name = "rear_wheel_joint"; - std::string front_steer_name = "front_steer_joint"; - - // Controller-related parameters - std::array joint_state_values_ = {1.1, 2.2}; - std::array joint_command_values_ = {101.101, 202.202}; - - std::vector state_itfs_; - std::vector command_itfs_; - - double ref_timeout_ = 0.2; - - // Test related parameters - std::unique_ptr controller_; - rclcpp::Node::SharedPtr command_publisher_node_; - rclcpp::Node::SharedPtr command_publisher_node_unstamped_; - rclcpp::Publisher::SharedPtr command_publisher_; - rclcpp::Publisher::SharedPtr command_publisher_unstamped_; -}; - -#endif // TEST_ACKERMANN_STEERING_CONTROLLER_HPP_ diff --git a/steering_controllers/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp b/steering_controllers/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp deleted file mode 100644 index 79bb1a418a..0000000000 --- a/steering_controllers/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp +++ /dev/null @@ -1,89 +0,0 @@ -// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "test_ackermann_steering_controller.hpp" - -#include -#include -#include -#include -#include - -using ackermann_steering_controller::CMD_MY_ITFS; -using ackermann_steering_controller::control_mode_type; -using ackermann_steering_controller::STATE_MY_ITFS; - -class AckermannSteeringControllerTest -: public AckermannSteeringControllerFixture -{ -}; - -TEST_F(AckermannSteeringControllerTest, 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(AckermannSteeringControllerTest, 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/steering_controllers/ackermann_steering_controller/test/test_load_ackermann_steering_controller.cpp b/steering_controllers/ackermann_steering_controller/test/test_load_ackermann_steering_controller.cpp deleted file mode 100644 index 27da4a4929..0000000000 --- a/steering_controllers/ackermann_steering_controller/test/test_load_ackermann_steering_controller.cpp +++ /dev/null @@ -1,42 +0,0 @@ -// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include - -#include "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(TestLoadAckermannSteeringController, 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_ackermann_steering_controller", - "ackermann_steering_controller/AckermannSteeringController")); - - rclcpp::shutdown(); -} diff --git a/steering_controllers/steering_controllers/CMakeLists.txt b/steering_controllers/steering_controllers/CMakeLists.txt index 9922830197..6f9cbf0591 100644 --- a/steering_controllers/steering_controllers/CMakeLists.txt +++ b/steering_controllers/steering_controllers/CMakeLists.txt @@ -34,6 +34,7 @@ add_library( ${PROJECT_NAME} SHARED src/steering_controllers.cpp + src/ackermann_steering_controller.cpp ) generate_parameter_library(steering_controllers_parameters src/steering_controllers.yaml @@ -47,8 +48,8 @@ target_link_libraries(${PROJECT_NAME} steering_controllers_parameters) ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS}) target_compile_definitions(${PROJECT_NAME} PRIVATE "STEERING_CONTROLLERS_BUILDING_DLL") -# pluginlib_export_plugin_description_file( -# controller_interface ackermann_steering_controller.xml) +pluginlib_export_plugin_description_file( + controller_interface steering_controllers.xml) install( TARGETS diff --git a/steering_controllers/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp b/steering_controllers/steering_controllers/include/steering_controllers/ackermann_steering_controller.hpp similarity index 100% rename from steering_controllers/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp rename to steering_controllers/steering_controllers/include/steering_controllers/ackermann_steering_controller.hpp diff --git a/steering_controllers/ackermann_steering_controller/src/ackermann_steering_controller.cpp b/steering_controllers/steering_controllers/src/ackermann_steering_controller.cpp similarity index 96% rename from steering_controllers/ackermann_steering_controller/src/ackermann_steering_controller.cpp rename to steering_controllers/steering_controllers/src/ackermann_steering_controller.cpp index 9ffb5959c8..c41042de2b 100644 --- a/steering_controllers/ackermann_steering_controller/src/ackermann_steering_controller.cpp +++ b/steering_controllers/steering_controllers/src/ackermann_steering_controller.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "ackermann_steering_controller/ackermann_steering_controller.hpp" +#include "steering_controllers/ackermann_steering_controller.hpp" namespace ackermann_steering_controller { diff --git a/steering_controllers/ackermann_steering_controller/ackermann_steering_controller.xml b/steering_controllers/steering_controllers/steering_controllers.xml similarity index 87% rename from steering_controllers/ackermann_steering_controller/ackermann_steering_controller.xml rename to steering_controllers/steering_controllers/steering_controllers.xml index de128d6911..2c0618261e 100644 --- a/steering_controllers/ackermann_steering_controller/ackermann_steering_controller.xml +++ b/steering_controllers/steering_controllers/steering_controllers.xml @@ -1,4 +1,4 @@ - + From 6695ad3b5be8b353c7e685fab5e742dfa51d3d7d Mon Sep 17 00:00:00 2001 From: petkovich Date: Sun, 18 Dec 2022 19:07:46 +0100 Subject: [PATCH 056/186] renamed ackermann to steering_controller_implementations --- steering_controllers/steering_controllers/CMakeLists.txt | 2 +- ...g_controller.hpp => steering_controller_implementations.hpp} | 0 ...g_controller.cpp => steering_controller_implementations.cpp} | 2 +- 3 files changed, 2 insertions(+), 2 deletions(-) rename steering_controllers/steering_controllers/include/steering_controllers/{ackermann_steering_controller.hpp => steering_controller_implementations.hpp} (100%) rename steering_controllers/steering_controllers/src/{ackermann_steering_controller.cpp => steering_controller_implementations.cpp} (96%) diff --git a/steering_controllers/steering_controllers/CMakeLists.txt b/steering_controllers/steering_controllers/CMakeLists.txt index 6f9cbf0591..ac6c7de959 100644 --- a/steering_controllers/steering_controllers/CMakeLists.txt +++ b/steering_controllers/steering_controllers/CMakeLists.txt @@ -34,7 +34,7 @@ add_library( ${PROJECT_NAME} SHARED src/steering_controllers.cpp - src/ackermann_steering_controller.cpp + src/steering_controller_implementations.cpp ) generate_parameter_library(steering_controllers_parameters src/steering_controllers.yaml diff --git a/steering_controllers/steering_controllers/include/steering_controllers/ackermann_steering_controller.hpp b/steering_controllers/steering_controllers/include/steering_controllers/steering_controller_implementations.hpp similarity index 100% rename from steering_controllers/steering_controllers/include/steering_controllers/ackermann_steering_controller.hpp rename to steering_controllers/steering_controllers/include/steering_controllers/steering_controller_implementations.hpp diff --git a/steering_controllers/steering_controllers/src/ackermann_steering_controller.cpp b/steering_controllers/steering_controllers/src/steering_controller_implementations.cpp similarity index 96% rename from steering_controllers/steering_controllers/src/ackermann_steering_controller.cpp rename to steering_controllers/steering_controllers/src/steering_controller_implementations.cpp index c41042de2b..8d2c6d1092 100644 --- a/steering_controllers/steering_controllers/src/ackermann_steering_controller.cpp +++ b/steering_controllers/steering_controllers/src/steering_controller_implementations.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "steering_controllers/ackermann_steering_controller.hpp" +#include "steering_controllers/steering_controller_implementations.hpp" namespace ackermann_steering_controller { From eed931014cb1434dee38a4c2e2667deb66769676 Mon Sep 17 00:00:00 2001 From: petkovich Date: Sun, 18 Dec 2022 19:14:14 +0100 Subject: [PATCH 057/186] added bicycle controller (the same as ackermann for now) --- .../steering_controller_implementations.hpp | 14 +++++++- .../steering_controller_implementations.cpp | 33 +++++++++++++++++-- .../steering_controllers.xml | 6 ++++ 3 files changed, 50 insertions(+), 3 deletions(-) diff --git a/steering_controllers/steering_controllers/include/steering_controllers/steering_controller_implementations.hpp b/steering_controllers/steering_controllers/include/steering_controllers/steering_controller_implementations.hpp index 54a2ee5bd8..048aff5e77 100644 --- a/steering_controllers/steering_controllers/include/steering_controllers/steering_controller_implementations.hpp +++ b/steering_controllers/steering_controllers/include/steering_controllers/steering_controller_implementations.hpp @@ -16,6 +16,7 @@ #define ACKERMANN_STEERING_CONTROLLER___HPP_ #include "steering_controllers/steering_controllers.hpp" + namespace ackermann_steering_controller { class AckermannSteeringController : public steering_controllers::SteeringControllers @@ -26,7 +27,18 @@ class AckermannSteeringController : public steering_controllers::SteeringControl STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::CallbackReturn configure_odometry() override; }; - } // namespace ackermann_steering_controller +namespace bicycle_steering_controller +{ +class BicycleSteeringController : public steering_controllers::SteeringControllers +{ +public: + BicycleSteeringController(); + + STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::CallbackReturn configure_odometry() + override; +}; +} // namespace bicycle_steering_controller + #endif // ACKERMANN_STEERING_CONTROLLER__HPP_ diff --git a/steering_controllers/steering_controllers/src/steering_controller_implementations.cpp b/steering_controllers/steering_controllers/src/steering_controller_implementations.cpp index 8d2c6d1092..fd570681be 100644 --- a/steering_controllers/steering_controllers/src/steering_controller_implementations.cpp +++ b/steering_controllers/steering_controllers/src/steering_controller_implementations.cpp @@ -16,7 +16,6 @@ namespace ackermann_steering_controller { -//using AckermannSteeringController = steering_controllers::SteeringControllers; AckermannSteeringController::AckermannSteeringController() : steering_controllers::SteeringControllers() { @@ -41,11 +40,41 @@ controller_interface::CallbackReturn AckermannSteeringController::configure_odom RCLCPP_INFO(get_node()->get_logger(), "odom configure successful INSIDE ACKERMANN"); return controller_interface::CallbackReturn::SUCCESS; } - } // namespace ackermann_steering_controller +namespace bicycle_steering_controller +{ +BicycleSteeringController::BicycleSteeringController() : steering_controllers::SteeringControllers() +{ +} + +controller_interface::CallbackReturn BicycleSteeringController::configure_odometry() +{ + params_ = param_listener_->get_params(); + + const double wheel_seperation = params_.wheel_separation_multiplier * params_.wheel_separation; + const double wheel_radius = params_.wheel_radius_multiplier * params_.wheel_radius; + const double wheelbase = params_.wheelbase_multiplier * params_.wheelbase; + odometry_.set_wheel_params(wheel_seperation, wheel_radius, wheelbase); + odometry_.set_velocity_rolling_window_size(params_.velocity_rolling_window_size); + + const size_t nr_state_itfs = 2; + const size_t nr_cmd_itfs = 2; + const size_t nr_ref_itfs = 2; + + set_interface_numbers(nr_state_itfs, nr_cmd_itfs, nr_ref_itfs); + + RCLCPP_INFO(get_node()->get_logger(), "odom configure successful INSIDE ACKERMANN"); + return controller_interface::CallbackReturn::SUCCESS; +} +} // namespace bicycle_steering_controller + #include "pluginlib/class_list_macros.hpp" PLUGINLIB_EXPORT_CLASS( ackermann_steering_controller::AckermannSteeringController, controller_interface::ChainableControllerInterface) + +PLUGINLIB_EXPORT_CLASS( + bicycle_steering_controller::BicycleSteeringController, + controller_interface::ChainableControllerInterface) diff --git a/steering_controllers/steering_controllers/steering_controllers.xml b/steering_controllers/steering_controllers/steering_controllers.xml index 2c0618261e..bd7b79b785 100644 --- a/steering_controllers/steering_controllers/steering_controllers.xml +++ b/steering_controllers/steering_controllers/steering_controllers.xml @@ -5,4 +5,10 @@ AckermannSteeringController ros2_control controller. + + + BicycleSteeringController ros2_control controller. + + From 33597a27d3bbcf14b95d838cd575ba0bbb02db4a Mon Sep 17 00:00:00 2001 From: petkovich Date: Sun, 18 Dec 2022 19:15:28 +0100 Subject: [PATCH 058/186] removed template from copyright --- .../steering_controller_implementations.hpp | 2 +- .../include/steering_controllers/steering_controllers.hpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/steering_controllers/steering_controllers/include/steering_controllers/steering_controller_implementations.hpp b/steering_controllers/steering_controllers/include/steering_controllers/steering_controller_implementations.hpp index 048aff5e77..8d45ad2158 100644 --- a/steering_controllers/steering_controllers/include/steering_controllers/steering_controller_implementations.hpp +++ b/steering_controllers/steering_controllers/include/steering_controllers/steering_controller_implementations.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/steering_controllers/steering_controllers/include/steering_controllers/steering_controllers.hpp b/steering_controllers/steering_controllers/include/steering_controllers/steering_controllers.hpp index 31847b6c44..26c8229dee 100644 --- a/steering_controllers/steering_controllers/include/steering_controllers/steering_controllers.hpp +++ b/steering_controllers/steering_controllers/include/steering_controllers/steering_controllers.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. From 16842f150f051f3475905f2cbdbe73e637c19890 Mon Sep 17 00:00:00 2001 From: petkovich Date: Sun, 18 Dec 2022 19:15:57 +0100 Subject: [PATCH 059/186] removed template from copyright --- .../include/steering_controllers/visibility_control.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/steering_controllers/steering_controllers/include/steering_controllers/visibility_control.h b/steering_controllers/steering_controllers/include/steering_controllers/visibility_control.h index e1ffb83e71..415a139440 100644 --- a/steering_controllers/steering_controllers/include/steering_controllers/visibility_control.h +++ b/steering_controllers/steering_controllers/include/steering_controllers/visibility_control.h @@ -1,4 +1,4 @@ -// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. From 127f834262a56d53751502186beb4bc59f89d6b2 Mon Sep 17 00:00:00 2001 From: petkovich Date: Sun, 18 Dec 2022 19:18:57 +0100 Subject: [PATCH 060/186] defines --- .../steering_controller_implementations.hpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/steering_controllers/steering_controllers/include/steering_controllers/steering_controller_implementations.hpp b/steering_controllers/steering_controllers/include/steering_controllers/steering_controller_implementations.hpp index 8d45ad2158..fb8d65c269 100644 --- a/steering_controllers/steering_controllers/include/steering_controllers/steering_controller_implementations.hpp +++ b/steering_controllers/steering_controllers/include/steering_controllers/steering_controller_implementations.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef ACKERMANN_STEERING_CONTROLLER__HPP_ -#define ACKERMANN_STEERING_CONTROLLER___HPP_ +#ifndef STEERING_CONTROLLERS_IMPLEMENTATIONS_HPP_ +#define STEERING_CONTROLLERS_IMPLEMENTATIONS_HPP_ #include "steering_controllers/steering_controllers.hpp" @@ -41,4 +41,4 @@ class BicycleSteeringController : public steering_controllers::SteeringControlle }; } // namespace bicycle_steering_controller -#endif // ACKERMANN_STEERING_CONTROLLER__HPP_ +#endif // STEERING_CONTROLLERS_IMPLEMENTATIONS_HPP_ From 8267fc8d826fcf8a9161fecc4bfb1c6d0ca1b03e Mon Sep 17 00:00:00 2001 From: petkovich Date: Sun, 18 Dec 2022 19:28:37 +0100 Subject: [PATCH 061/186] ported tricycle controller --- .../steering_controller_implementations.hpp | 12 ++++++ .../steering_controller_implementations.cpp | 43 +++++++++++++++++-- 2 files changed, 51 insertions(+), 4 deletions(-) diff --git a/steering_controllers/steering_controllers/include/steering_controllers/steering_controller_implementations.hpp b/steering_controllers/steering_controllers/include/steering_controllers/steering_controller_implementations.hpp index fb8d65c269..ed204a4047 100644 --- a/steering_controllers/steering_controllers/include/steering_controllers/steering_controller_implementations.hpp +++ b/steering_controllers/steering_controllers/include/steering_controllers/steering_controller_implementations.hpp @@ -41,4 +41,16 @@ class BicycleSteeringController : public steering_controllers::SteeringControlle }; } // namespace bicycle_steering_controller +namespace tricycle_steering_controller +{ +class TricycleSteeringController : public steering_controllers::SteeringControllers +{ +public: + TricycleSteeringController(); + + STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::CallbackReturn configure_odometry() + override; +}; +} // namespace tricycle_steering_controller + #endif // STEERING_CONTROLLERS_IMPLEMENTATIONS_HPP_ diff --git a/steering_controllers/steering_controllers/src/steering_controller_implementations.cpp b/steering_controllers/steering_controllers/src/steering_controller_implementations.cpp index fd570681be..569fc795ac 100644 --- a/steering_controllers/steering_controllers/src/steering_controller_implementations.cpp +++ b/steering_controllers/steering_controllers/src/steering_controller_implementations.cpp @@ -31,13 +31,14 @@ controller_interface::CallbackReturn AckermannSteeringController::configure_odom odometry_.set_wheel_params(wheel_seperation, wheel_radius, wheelbase); odometry_.set_velocity_rolling_window_size(params_.velocity_rolling_window_size); - const size_t nr_state_itfs = 2; - const size_t nr_cmd_itfs = 2; + // TODO: enable position/velocity configure + const size_t nr_state_itfs = 4; + const size_t nr_cmd_itfs = 4; const size_t nr_ref_itfs = 2; set_interface_numbers(nr_state_itfs, nr_cmd_itfs, nr_ref_itfs); - RCLCPP_INFO(get_node()->get_logger(), "odom configure successful INSIDE ACKERMANN"); + RCLCPP_INFO(get_node()->get_logger(), "ackermann odom configure successful"); return controller_interface::CallbackReturn::SUCCESS; } } // namespace ackermann_steering_controller @@ -58,17 +59,47 @@ controller_interface::CallbackReturn BicycleSteeringController::configure_odomet odometry_.set_wheel_params(wheel_seperation, wheel_radius, wheelbase); odometry_.set_velocity_rolling_window_size(params_.velocity_rolling_window_size); + // TODO: enable position/velocity configure const size_t nr_state_itfs = 2; const size_t nr_cmd_itfs = 2; const size_t nr_ref_itfs = 2; set_interface_numbers(nr_state_itfs, nr_cmd_itfs, nr_ref_itfs); - RCLCPP_INFO(get_node()->get_logger(), "odom configure successful INSIDE ACKERMANN"); + RCLCPP_INFO(get_node()->get_logger(), "bicycle odom configure successful"); return controller_interface::CallbackReturn::SUCCESS; } } // namespace bicycle_steering_controller +namespace tricycle_steering_controller +{ +TricycleSteeringController::TricycleSteeringController() +: steering_controllers::SteeringControllers() +{ +} + +controller_interface::CallbackReturn TricycleSteeringController::configure_odometry() +{ + params_ = param_listener_->get_params(); + + const double wheel_seperation = params_.wheel_separation_multiplier * params_.wheel_separation; + const double wheel_radius = params_.wheel_radius_multiplier * params_.wheel_radius; + const double wheelbase = params_.wheelbase_multiplier * params_.wheelbase; + odometry_.set_wheel_params(wheel_seperation, wheel_radius, wheelbase); + odometry_.set_velocity_rolling_window_size(params_.velocity_rolling_window_size); + + // TODO: enable position/velocity configure + const size_t nr_state_itfs = 3; + const size_t nr_cmd_itfs = 2; + const size_t nr_ref_itfs = 2; + + set_interface_numbers(nr_state_itfs, nr_cmd_itfs, nr_ref_itfs); + + RCLCPP_INFO(get_node()->get_logger(), "ackermann odom configure successful"); + return controller_interface::CallbackReturn::SUCCESS; +} +} // namespace tricycle_steering_controller + #include "pluginlib/class_list_macros.hpp" PLUGINLIB_EXPORT_CLASS( @@ -78,3 +109,7 @@ PLUGINLIB_EXPORT_CLASS( PLUGINLIB_EXPORT_CLASS( bicycle_steering_controller::BicycleSteeringController, controller_interface::ChainableControllerInterface) + +PLUGINLIB_EXPORT_CLASS( + tricycle_steering_controller::TricycleSteeringController, + controller_interface::ChainableControllerInterface) From da6198fe7bd47c3988e0a8af1b086f8bfc73dc4a Mon Sep 17 00:00:00 2001 From: petkovich Date: Sun, 18 Dec 2022 19:30:34 +0100 Subject: [PATCH 062/186] export tricycle controller --- .../steering_controllers/steering_controllers.xml | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/steering_controllers/steering_controllers/steering_controllers.xml b/steering_controllers/steering_controllers/steering_controllers.xml index bd7b79b785..01dca4c0ec 100644 --- a/steering_controllers/steering_controllers/steering_controllers.xml +++ b/steering_controllers/steering_controllers/steering_controllers.xml @@ -11,4 +11,10 @@ BicycleSteeringController ros2_control controller. + + + TricycleSteeringController ros2_control controller. + + From e38b79b5908a9ce6bbee9c61e46d85a5c3948543 Mon Sep 17 00:00:00 2001 From: petkovich Date: Mon, 19 Dec 2022 11:55:35 +0100 Subject: [PATCH 063/186] named variables for some odom parameters --- .../src/steering_controller_implementations.cpp | 11 +++++------ .../include/steering_odometry/steering_odometry.hpp | 5 +++-- .../steering_odometry/src/steering_odometry.cpp | 4 ++-- 3 files changed, 10 insertions(+), 10 deletions(-) diff --git a/steering_controllers/steering_controllers/src/steering_controller_implementations.cpp b/steering_controllers/steering_controllers/src/steering_controller_implementations.cpp index 569fc795ac..9e101eab64 100644 --- a/steering_controllers/steering_controllers/src/steering_controller_implementations.cpp +++ b/steering_controllers/steering_controllers/src/steering_controller_implementations.cpp @@ -25,10 +25,10 @@ controller_interface::CallbackReturn AckermannSteeringController::configure_odom { params_ = param_listener_->get_params(); - const double wheel_seperation = params_.wheel_separation_multiplier * params_.wheel_separation; const double wheel_radius = params_.wheel_radius_multiplier * params_.wheel_radius; + const double wheel_seperation = params_.wheel_separation_multiplier * params_.wheel_separation; const double wheelbase = params_.wheelbase_multiplier * params_.wheelbase; - odometry_.set_wheel_params(wheel_seperation, wheel_radius, wheelbase); + odometry_.set_wheel_params(wheel_radius, wheel_seperation, wheelbase); odometry_.set_velocity_rolling_window_size(params_.velocity_rolling_window_size); // TODO: enable position/velocity configure @@ -55,8 +55,7 @@ controller_interface::CallbackReturn BicycleSteeringController::configure_odomet const double wheel_seperation = params_.wheel_separation_multiplier * params_.wheel_separation; const double wheel_radius = params_.wheel_radius_multiplier * params_.wheel_radius; - const double wheelbase = params_.wheelbase_multiplier * params_.wheelbase; - odometry_.set_wheel_params(wheel_seperation, wheel_radius, wheelbase); + odometry_.set_wheel_params(wheel_radius, wheel_seperation); odometry_.set_velocity_rolling_window_size(params_.velocity_rolling_window_size); // TODO: enable position/velocity configure @@ -82,10 +81,10 @@ controller_interface::CallbackReturn TricycleSteeringController::configure_odome { params_ = param_listener_->get_params(); - const double wheel_seperation = params_.wheel_separation_multiplier * params_.wheel_separation; const double wheel_radius = params_.wheel_radius_multiplier * params_.wheel_radius; + const double wheel_seperation = params_.wheel_separation_multiplier * params_.wheel_separation; const double wheelbase = params_.wheelbase_multiplier * params_.wheelbase; - odometry_.set_wheel_params(wheel_seperation, wheel_radius, wheelbase); + odometry_.set_wheel_params(wheel_radius, wheel_seperation, wheelbase); odometry_.set_velocity_rolling_window_size(params_.velocity_rolling_window_size); // TODO: enable position/velocity configure diff --git a/steering_controllers/steering_odometry/include/steering_odometry/steering_odometry.hpp b/steering_controllers/steering_odometry/include/steering_odometry/steering_odometry.hpp index 2c0667d21c..212573f6dc 100644 --- a/steering_controllers/steering_odometry/include/steering_odometry/steering_odometry.hpp +++ b/steering_controllers/steering_odometry/include/steering_odometry/steering_odometry.hpp @@ -114,9 +114,10 @@ class SteeringOdometry double get_angular() const { return angular_; } /** - * \brief Sets the wheel parameters: radius and separation + * \brief Sets the wheel parameters: radius, separation and wheelbase */ - void set_wheel_params(double wheel_reparation_h, double wheel_radius, double wheelbase); + void set_wheel_params( + double wheel_radius, double wheel_separation_h = 0.0, double wheelbase = 0.0); /** * \brief Velocity rolling window size setter diff --git a/steering_controllers/steering_odometry/src/steering_odometry.cpp b/steering_controllers/steering_odometry/src/steering_odometry.cpp index 4aae3ee7da..2670deb048 100644 --- a/steering_controllers/steering_odometry/src/steering_odometry.cpp +++ b/steering_controllers/steering_odometry/src/steering_odometry.cpp @@ -134,10 +134,10 @@ void SteeringOdometry::update_open_loop(const double linear, const double angula } void SteeringOdometry::set_wheel_params( - double wheel_separation, double wheel_radius, double wheelbase) + double wheel_radius, double wheel_separation, double wheelbase) { - wheel_separation_ = wheel_separation; wheel_radius_ = wheel_radius; + wheel_separation_ = wheel_separation; wheelbase_ = wheelbase; } From 15a2eb653e15f5413ef1bc95b433e319558ea63e Mon Sep 17 00:00:00 2001 From: petkovich Date: Mon, 19 Dec 2022 12:07:45 +0100 Subject: [PATCH 064/186] integration function, trans rot vel to steering angle arguments --- .../steering_odometry/steering_odometry.hpp | 16 +++----------- .../src/steering_odometry.cpp | 22 ++++++++----------- 2 files changed, 12 insertions(+), 26 deletions(-) diff --git a/steering_controllers/steering_odometry/include/steering_odometry/steering_odometry.hpp b/steering_controllers/steering_odometry/include/steering_odometry/steering_odometry.hpp index 212573f6dc..f037646df9 100644 --- a/steering_controllers/steering_odometry/include/steering_odometry/steering_odometry.hpp +++ b/steering_controllers/steering_odometry/include/steering_odometry/steering_odometry.hpp @@ -36,9 +36,6 @@ namespace steering_odometry class SteeringOdometry { public: - /// Integration function, used to integrate the odometry: - typedef boost::function IntegrationFunction; - /** * \brief Constructor * Timestamp will get the current time value @@ -129,9 +126,8 @@ class SteeringOdometry * \brief TODO * \param Vx TODO * \param theta_dot TODO - * \param wheelbase TODO */ - double convert_trans_rot_vel_to_steering_angle(double Vx, double theta_dot, double wheelbase); + double convert_trans_rot_vel_to_steering_angle(double Vx, double theta_dot); /** * \brief TODO @@ -146,9 +142,6 @@ class SteeringOdometry void reset_odometry(); private: - /// Rolling mean accumulator and window: - typedef rcpputils::RollingMeanAccumulator RollingMeanAccumulator; - /** * \brief Integrates the velocities (linear and angular) using 2nd order Runge-Kutta * \param linear Linear velocity [m] (linear displacement, i.e. m/s * dt) computed by encoders @@ -189,10 +182,7 @@ class SteeringOdometry /// Rolling mean accumulators for the linar and angular velocities: size_t velocity_rolling_window_size_; - RollingMeanAccumulator linear_acc_; - RollingMeanAccumulator angular_acc_; - - /// Integration function, used to integrate the odometry: - IntegrationFunction integrate_fun_; + rcpputils::RollingMeanAccumulator linear_acc_; + rcpputils::RollingMeanAccumulator angular_acc_; }; } // namespace steering_odometry diff --git a/steering_controllers/steering_odometry/src/steering_odometry.cpp b/steering_controllers/steering_odometry/src/steering_odometry.cpp index 2670deb048..5c743bb2f6 100644 --- a/steering_controllers/steering_odometry/src/steering_odometry.cpp +++ b/steering_controllers/steering_odometry/src/steering_odometry.cpp @@ -39,10 +39,7 @@ SteeringOdometry::SteeringOdometry(size_t velocity_rolling_window_size) rear_wheel_old_pos_(0.0), velocity_rolling_window_size_(velocity_rolling_window_size), linear_acc_(velocity_rolling_window_size), - angular_acc_(velocity_rolling_window_size), - integrate_fun_(std::bind( - &SteeringOdometry::integrate_exact, this, std::placeholders::_1, std::placeholders::_2)) - + angular_acc_(velocity_rolling_window_size) { } @@ -77,7 +74,7 @@ bool SteeringOdometry::update_from_position( const double angular = tan(front_steer_pos) * linear_velocity / wheel_separation_; /// Integrate odometry: - integrate_fun_(linear_velocity * dt, angular); + SteeringOdometry::integrate_exact(linear_velocity * dt, angular); /// We cannot estimate the speed with very small time intervals: if (dt < 0.0001) @@ -105,7 +102,7 @@ bool SteeringOdometry::update_from_velocity( const double angular = tan(front_steer_pos) * linear_velocity / wheel_separation_; /// Integrate odometry: - integrate_fun_(linear_velocity * dt, angular * dt); + SteeringOdometry::integrate_exact(linear_velocity * dt, angular * dt); /// We cannot estimate the speed with very small time intervals: if (dt < 0.0001) @@ -130,7 +127,7 @@ void SteeringOdometry::update_open_loop(const double linear, const double angula angular_ = angular; /// Integrate odometry: - integrate_fun_(linear * dt, angular * dt); + SteeringOdometry::integrate_exact(linear * dt, angular * dt); } void SteeringOdometry::set_wheel_params( @@ -149,14 +146,13 @@ void SteeringOdometry::set_velocity_rolling_window_size(size_t velocity_rolling_ } //TODO: change functions depending on fwd kinematics model -double SteeringOdometry::convert_trans_rot_vel_to_steering_angle( - double Vx, double theta_dot, double wheelbase) +double SteeringOdometry::convert_trans_rot_vel_to_steering_angle(double Vx, double theta_dot) { if (theta_dot == 0 || Vx == 0) { return 0; } - return std::atan(theta_dot * wheelbase / Vx); + return std::atan(theta_dot * wheel_separation_ / Vx); } //TODO: change functions depending on fwd kinematics model @@ -172,7 +168,7 @@ std::tuple SteeringOdometry::twist_to_ackermann(double Vx, doubl return std::make_tuple(alpha, Ws); } - alpha = convert_trans_rot_vel_to_steering_angle(Vx, theta_dot, wheel_separation_); + alpha = SteeringOdometry::convert_trans_rot_vel_to_steering_angle(Vx, theta_dot); Ws = Vx / (wheel_radius_ * std::cos(alpha)); return std::make_tuple(alpha, Ws); } @@ -217,8 +213,8 @@ void SteeringOdometry::integrate_exact(double linear, double angular) void SteeringOdometry::reset_accumulators() { - linear_acc_ = RollingMeanAccumulator(velocity_rolling_window_size_); - angular_acc_ = RollingMeanAccumulator(velocity_rolling_window_size_); + linear_acc_ = rcpputils::RollingMeanAccumulator(velocity_rolling_window_size_); + angular_acc_ = rcpputils::RollingMeanAccumulator(velocity_rolling_window_size_); // TODO: angular rolling window size? } } // namespace steering_odometry From ef8fbc9d3e55b8b18d7a40e2ee0eccb948dee8fc Mon Sep 17 00:00:00 2001 From: petkovich Date: Mon, 19 Dec 2022 12:20:11 +0100 Subject: [PATCH 065/186] removed redundant classes and variables --- .../steering_controllers.hpp | 22 ------------------- 1 file changed, 22 deletions(-) diff --git a/steering_controllers/steering_controllers/include/steering_controllers/steering_controllers.hpp b/steering_controllers/steering_controllers/include/steering_controllers/steering_controllers.hpp index 26c8229dee..e368f759ed 100644 --- a/steering_controllers/steering_controllers/include/steering_controllers/steering_controllers.hpp +++ b/steering_controllers/steering_controllers/include/steering_controllers/steering_controllers.hpp @@ -107,30 +107,8 @@ class SteeringControllers : public controller_interface::ChainableControllerInte bool on_set_chained_mode(bool chained_mode) override; - struct WheelHandle - { - std::reference_wrapper feedback; - // std::reference_wrapper velocity; - }; - - std::vector registered_rear_wheel_handles_; - std::vector registered_front_wheel_handles_; - /// Odometry related: rclcpp::Duration publish_period_ = rclcpp::Duration::from_nanoseconds(0); - - bool open_loop_; - /// Velocity command related: - struct Commands - { - double lin; - double ang; - rclcpp::Time stamp; - - Commands() : lin(0.0), ang(0.0), stamp(0.0) {} - }; - - // Odometry related: steering_odometry::SteeringOdometry odometry_; using AckermanControllerState = control_msgs::msg::SteeringControllerStatus; From 6c73ecff4df9376d0ea9d3384fe1cd38c9ac6b13 Mon Sep 17 00:00:00 2001 From: petkovich Date: Mon, 19 Dec 2022 12:22:31 +0100 Subject: [PATCH 066/186] removed some more redundant classes and variables --- .../steering_controllers/steering_controllers.hpp | 12 +----------- 1 file changed, 1 insertion(+), 11 deletions(-) diff --git a/steering_controllers/steering_controllers/include/steering_controllers/steering_controllers.hpp b/steering_controllers/steering_controllers/include/steering_controllers/steering_controllers.hpp index e368f759ed..080420f347 100644 --- a/steering_controllers/steering_controllers/include/steering_controllers/steering_controllers.hpp +++ b/steering_controllers/steering_controllers/include/steering_controllers/steering_controllers.hpp @@ -107,8 +107,7 @@ class SteeringControllers : public controller_interface::ChainableControllerInte bool on_set_chained_mode(bool chained_mode) override; - /// Odometry related: - rclcpp::Duration publish_period_ = rclcpp::Duration::from_nanoseconds(0); + /// Odometry: steering_odometry::SteeringOdometry odometry_; using AckermanControllerState = control_msgs::msg::SteeringControllerStatus; @@ -131,15 +130,6 @@ class SteeringControllers : public controller_interface::ChainableControllerInte const std::shared_ptr msg); void reference_callback_unstamped(const std::shared_ptr msg); - /// Frame to use for the robot base: - std::string base_frame_id_; - - /// Frame to use for odometry and odom tf: - std::string odom_frame_id_; - - /// Whether to publish odometry to tf or not: - bool enable_odom_tf_; - // store last velocity double last_linear_velocity_ = 0.0; double last_angular_velocity_ = 0.0; From 61eaf2a7ca213a6d5cb04cf8ff6470de01ce8186 Mon Sep 17 00:00:00 2001 From: petkovich Date: Mon, 19 Dec 2022 12:24:56 +0100 Subject: [PATCH 067/186] configure_odometry pure virtual --- .../src/steering_controllers.cpp | 16 ---------------- 1 file changed, 16 deletions(-) diff --git a/steering_controllers/steering_controllers/src/steering_controllers.cpp b/steering_controllers/steering_controllers/src/steering_controllers.cpp index a4b108e193..5d1555c4f8 100644 --- a/steering_controllers/steering_controllers/src/steering_controllers.cpp +++ b/steering_controllers/steering_controllers/src/steering_controllers.cpp @@ -88,22 +88,6 @@ controller_interface::CallbackReturn SteeringControllers::set_interface_numbers( nr_ref_itfs_ = nr_ref_itfs; } -controller_interface::CallbackReturn SteeringControllers::configure_odometry() -{ - params_ = param_listener_->get_params(); - - const double wheel_seperation = params_.wheel_separation_multiplier * params_.wheel_separation; - const double wheel_radius = params_.wheel_radius_multiplier * params_.wheel_radius; - const double wheelbase = params_.wheelbase_multiplier * params_.wheelbase; - odometry_.set_wheel_params(wheel_seperation, wheel_radius, wheelbase); - odometry_.set_velocity_rolling_window_size(params_.velocity_rolling_window_size); - - set_interface_numbers(); - - RCLCPP_INFO(get_node()->get_logger(), "odom configure successful"); - return controller_interface::CallbackReturn::SUCCESS; -} - controller_interface::CallbackReturn SteeringControllers::on_configure( const rclcpp_lifecycle::State & /*previous_state*/) { From eaa0a3cf9d4f451110a6c3b90cdf7b9eb104b991 Mon Sep 17 00:00:00 2001 From: petkovich Date: Mon, 19 Dec 2022 12:35:50 +0100 Subject: [PATCH 068/186] removed tricycle package, integrated steering odometry with steering controller --- .../steering_controllers/CMakeLists.txt | 2 +- .../steering_controllers.hpp | 2 +- .../steering_odometry.hpp | 0 .../steering_controllers/package.xml | 1 - .../src/steering_odometry.cpp | 2 +- .../steering_odometry/CMakeLists.txt | 66 -- .../steering_odometry/package.xml | 22 - .../tricycle_controller/CHANGELOG.rst | 22 - .../tricycle_controller/CMakeLists.txt | 109 --- .../config/tricycle_drive_controller.yaml | 43 -- .../tricycle_controller/doc/userdoc.rst | 24 - .../tricycle_controller/steering_limiter.hpp | 94 --- .../tricycle_controller/traction_limiter.hpp | 102 --- .../tricycle_controller.hpp | 185 ----- .../tricycle_controller/visibility_control.h | 53 -- .../tricycle_controller/package.xml | 38 -- .../src/steering_limiter.cpp | 117 ---- .../src/traction_limiter.cpp | 142 ---- .../src/tricycle_controller.cpp | 644 ------------------ .../test/config/test_tricycle_controller.yaml | 20 - .../test/test_load_tricycle_controller.cpp | 43 -- .../test/test_tricycle_controller.cpp | 350 ---------- .../tricycle_controller.xml | 7 - 23 files changed, 3 insertions(+), 2085 deletions(-) rename steering_controllers/{steering_odometry/include/steering_odometry => steering_controllers/include/steering_controllers}/steering_odometry.hpp (100%) rename steering_controllers/{steering_odometry => steering_controllers}/src/steering_odometry.cpp (99%) delete mode 100644 steering_controllers/steering_odometry/CMakeLists.txt delete mode 100644 steering_controllers/steering_odometry/package.xml delete mode 100644 steering_controllers/tricycle_controller/CHANGELOG.rst delete mode 100644 steering_controllers/tricycle_controller/CMakeLists.txt delete mode 100644 steering_controllers/tricycle_controller/config/tricycle_drive_controller.yaml delete mode 100644 steering_controllers/tricycle_controller/doc/userdoc.rst delete mode 100644 steering_controllers/tricycle_controller/include/tricycle_controller/steering_limiter.hpp delete mode 100644 steering_controllers/tricycle_controller/include/tricycle_controller/traction_limiter.hpp delete mode 100644 steering_controllers/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp delete mode 100644 steering_controllers/tricycle_controller/include/tricycle_controller/visibility_control.h delete mode 100644 steering_controllers/tricycle_controller/package.xml delete mode 100644 steering_controllers/tricycle_controller/src/steering_limiter.cpp delete mode 100644 steering_controllers/tricycle_controller/src/traction_limiter.cpp delete mode 100644 steering_controllers/tricycle_controller/src/tricycle_controller.cpp delete mode 100644 steering_controllers/tricycle_controller/test/config/test_tricycle_controller.yaml delete mode 100644 steering_controllers/tricycle_controller/test/test_load_tricycle_controller.cpp delete mode 100644 steering_controllers/tricycle_controller/test/test_tricycle_controller.cpp delete mode 100644 steering_controllers/tricycle_controller/tricycle_controller.xml diff --git a/steering_controllers/steering_controllers/CMakeLists.txt b/steering_controllers/steering_controllers/CMakeLists.txt index ac6c7de959..f77a3a60e9 100644 --- a/steering_controllers/steering_controllers/CMakeLists.txt +++ b/steering_controllers/steering_controllers/CMakeLists.txt @@ -21,7 +21,6 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS tf2_msgs tf2_geometry_msgs ackermann_msgs - steering_odometry ) find_package(ament_cmake REQUIRED) @@ -34,6 +33,7 @@ add_library( ${PROJECT_NAME} SHARED src/steering_controllers.cpp + src/steering_odometry.cpp src/steering_controller_implementations.cpp ) generate_parameter_library(steering_controllers_parameters diff --git a/steering_controllers/steering_controllers/include/steering_controllers/steering_controllers.hpp b/steering_controllers/steering_controllers/include/steering_controllers/steering_controllers.hpp index 080420f347..39acab7314 100644 --- a/steering_controllers/steering_controllers/include/steering_controllers/steering_controllers.hpp +++ b/steering_controllers/steering_controllers/include/steering_controllers/steering_controllers.hpp @@ -30,9 +30,9 @@ #include "realtime_tools/realtime_buffer.h" #include "realtime_tools/realtime_publisher.h" #include "std_srvs/srv/set_bool.hpp" +#include "steering_controllers/steering_odometry.hpp" #include "steering_controllers/visibility_control.h" #include "steering_controllers_parameters.hpp" -#include "steering_odometry/steering_odometry.hpp" // TODO(anyone): Replace with controller specific messages #include "ackermann_msgs/msg/ackermann_drive.hpp" diff --git a/steering_controllers/steering_odometry/include/steering_odometry/steering_odometry.hpp b/steering_controllers/steering_controllers/include/steering_controllers/steering_odometry.hpp similarity index 100% rename from steering_controllers/steering_odometry/include/steering_odometry/steering_odometry.hpp rename to steering_controllers/steering_controllers/include/steering_controllers/steering_odometry.hpp diff --git a/steering_controllers/steering_controllers/package.xml b/steering_controllers/steering_controllers/package.xml index 4b65bc5b23..904774a81c 100644 --- a/steering_controllers/steering_controllers/package.xml +++ b/steering_controllers/steering_controllers/package.xml @@ -26,7 +26,6 @@ tf2_msgs tf2_geometry_msgs ackermann_msgs - steering_odometry ament_lint_auto ament_cmake_gmock diff --git a/steering_controllers/steering_odometry/src/steering_odometry.cpp b/steering_controllers/steering_controllers/src/steering_odometry.cpp similarity index 99% rename from steering_controllers/steering_odometry/src/steering_odometry.cpp rename to steering_controllers/steering_controllers/src/steering_odometry.cpp index 5c743bb2f6..af461fb735 100644 --- a/steering_controllers/steering_odometry/src/steering_odometry.cpp +++ b/steering_controllers/steering_controllers/src/steering_odometry.cpp @@ -19,7 +19,7 @@ * Author: Dr. Ing. Denis Stogl */ -#include "steering_odometry/steering_odometry.hpp" +#include "steering_controllers/steering_odometry.hpp" #include #include diff --git a/steering_controllers/steering_odometry/CMakeLists.txt b/steering_controllers/steering_odometry/CMakeLists.txt deleted file mode 100644 index f8b34c3b67..0000000000 --- a/steering_controllers/steering_odometry/CMakeLists.txt +++ /dev/null @@ -1,66 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(steering_odometry) - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -set(THIS_PACKAGE_INCLUDE_DEPENDS - rclcpp - realtime_tools -) -# find dependencies -find_package(ament_cmake REQUIRED) -# uncomment the following section in order to fill in -# further dependencies manually. -# find_package( REQUIRED) -foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) - find_package(${Dependency} REQUIRED) -endforeach() - -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() - -add_library( - ${PROJECT_NAME} - SHARED - src/steering_odometry.cpp -) -target_include_directories(${PROJECT_NAME} PRIVATE include) -ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS}) - -install( - TARGETS - ${PROJECT_NAME} - RUNTIME DESTINATION bin - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib -) - -install( - DIRECTORY include/ - DESTINATION include -) - - -ament_export_include_directories( - include -) - -ament_export_dependencies( - ${THIS_PACKAGE_INCLUDE_DEPENDS} -) -ament_export_libraries( - ${PROJECT_NAME} -) - -ament_package() diff --git a/steering_controllers/steering_odometry/package.xml b/steering_controllers/steering_odometry/package.xml deleted file mode 100644 index a128597f06..0000000000 --- a/steering_controllers/steering_odometry/package.xml +++ /dev/null @@ -1,22 +0,0 @@ - - - - steering_odometry - 0.0.0 - Package containing odometry implementation for steering robot configurations. - Apache License 2.0 - dr. sc. Tomislav Petkovic - Bence Magyar - Dr.-Ing. Denis Štogl - - ament_cmake - realtime_tools - rcpputils - - ament_lint_auto - ament_lint_common - - - ament_cmake - - diff --git a/steering_controllers/tricycle_controller/CHANGELOG.rst b/steering_controllers/tricycle_controller/CHANGELOG.rst deleted file mode 100644 index 65cc206bda..0000000000 --- a/steering_controllers/tricycle_controller/CHANGELOG.rst +++ /dev/null @@ -1,22 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package tricycle_controller -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -2.14.0 (2022-11-18) -------------------- -* Include to fix compilation error on macOS (`#467 `_) -* Contributors: light-tech - -2.13.0 (2022-10-05) -------------------- - -2.12.0 (2022-09-01) -------------------- -* Fix formatting CI job (`#418 `_) -* Fix formatting because pre-commit was not running on CI for some time. (`#409 `_) -* Contributors: Denis Štogl, Tyler Weaver - -2.11.0 (2022-08-04) -------------------- -* Tricycle controller (`#345 `_) -* Contributors: Bence Magyar, Tony Najjar diff --git a/steering_controllers/tricycle_controller/CMakeLists.txt b/steering_controllers/tricycle_controller/CMakeLists.txt deleted file mode 100644 index 77000e93be..0000000000 --- a/steering_controllers/tricycle_controller/CMakeLists.txt +++ /dev/null @@ -1,109 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(tricycle_controller) - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -# find dependencies -set(THIS_PACKAGE_INCLUDE_DEPENDS - controller_interface - hardware_interface - pluginlib - nav_msgs - std_srvs - geometry_msgs - rclcpp - rcpputils - rclcpp_lifecycle - realtime_tools - tf2 - tf2_msgs - ackermann_msgs - steering_odometry -) - -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_library( - ${PROJECT_NAME} - SHARED - src/tricycle_controller.cpp - src/traction_limiter.cpp - src/steering_limiter.cpp -) -target_include_directories( - ${PROJECT_NAME} - PRIVATE - include -) - -ament_target_dependencies( - ${PROJECT_NAME} - ${THIS_PACKAGE_INCLUDE_DEPENDS} -) - -pluginlib_export_plugin_description_file(controller_interface tricycle_controller.xml) - -install(TARGETS ${PROJECT_NAME} - RUNTIME DESTINATION bin - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib -) -install( - DIRECTORY - include/ - DESTINATION include -) - -if(BUILD_TESTING) - find_package(ament_cmake_gmock REQUIRED) - find_package(controller_manager REQUIRED) - find_package(ros2_control_test_assets REQUIRED) - - ament_add_gmock(test_tricycle_controller - test/test_tricycle_controller.cpp - ENV config_file=${CMAKE_CURRENT_SOURCE_DIR}/test/config/test_tricycle_controller.yaml) - target_include_directories(test_tricycle_controller PRIVATE include) - target_link_libraries(test_tricycle_controller - ${PROJECT_NAME} - ) - - ament_target_dependencies(test_tricycle_controller - geometry_msgs - hardware_interface - nav_msgs - rclcpp - rclcpp_lifecycle - realtime_tools - tf2 - tf2_msgs - ) - - ament_add_gmock( - test_load_tricycle_controller - test/test_load_tricycle_controller.cpp - ) - target_include_directories(test_load_tricycle_controller PRIVATE include) - ament_target_dependencies(test_load_tricycle_controller - controller_manager - ros2_control_test_assets - ) -endif() - -ament_export_include_directories( - include -) -ament_export_libraries( - ${PROJECT_NAME} -) -ament_export_dependencies( - ${THIS_PACKAGE_INCLUDE_DEPENDS} -) - -ament_package() diff --git a/steering_controllers/tricycle_controller/config/tricycle_drive_controller.yaml b/steering_controllers/tricycle_controller/config/tricycle_drive_controller.yaml deleted file mode 100644 index c1077b13c3..0000000000 --- a/steering_controllers/tricycle_controller/config/tricycle_drive_controller.yaml +++ /dev/null @@ -1,43 +0,0 @@ -tricycle_controller: - ros__parameters: - # Model - traction_joint_name: traction_joint # Name of traction joint in URDF - steering_joint_name: steering_joint # Name of steering joint in URDF - wheel_radius: 0.0 # Radius of front wheel - wheelbase: 0.0 # Distance between center of back wheels and front wheel - - # Odometry - odom_frame_id: odom - base_frame_id: base_link - publish_rate: 50.0 # publish rate of odom and tf - open_loop: false # if True, uses cmd_vel instead of hardware interface feedback to compute odometry - enable_odom_tf: true # If True, publishes odom<-base_link TF - odom_only_twist: false # If True, publishes on /odom only linear.x and angular.z; Useful for computing odometry in another node, e.g robot_localization's ekf - pose_covariance_diagonal: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # Need to be set if fusing odom with other localization source - twist_covariance_diagonal: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # Need to be set if fusing odom with other localization source - velocity_rolling_window_size: 10 # Rolling window size of rcppmath::RollingMeanAccumulator applied on linear and angular speeds published on odom - - # Rate Limiting - traction: # All values should be positive - # min_velocity: 0.0 - # max_velocity: 1000.0 - # min_acceleration: 0.0 - max_acceleration: 5.0 - # min_deceleration: 0.0 - max_deceleration: 8.0 - # min_jerk: 0.0 - # max_jerk: 1000.0 - steering: - min_position: -1.57 - max_position: 1.57 - # min_velocity: 0.0 - max_velocity: 1.0 - # min_acceleration: 0.0 - # max_acceleration: 1000.0 - - # cmd_vel input - cmd_vel_timeout: 500 # In milliseconds. Timeout to stop if no cmd_vel is received - use_stamped_vel: false # Set to True if using TwistStamped. - - # Debug - publish_ackermann_command: true # Publishes AckermannDrive. The speed does not comply to the msg definition, it the wheel angular speed in rad/s. diff --git a/steering_controllers/tricycle_controller/doc/userdoc.rst b/steering_controllers/tricycle_controller/doc/userdoc.rst deleted file mode 100644 index a248bbec96..0000000000 --- a/steering_controllers/tricycle_controller/doc/userdoc.rst +++ /dev/null @@ -1,24 +0,0 @@ -.. _tricycle_controller_userdoc: - -tricycle_controller -===================== - -Controller for mobile robots with tricycle drive. -Input for control are robot base_link twist commands which are translated to traction and steering -commands for the tricycle drive base. Odometry is computed from hardware feedback and published. - -Velocity commands ------------------ - -The controller works with a velocity twist from which it extracts -the x component of the linear velocity and the z component of the angular velocity. -Velocities on other components are ignored. - - -Other features --------------- - - Realtime-safe implementation. - Odometry publishing - Velocity, acceleration and jerk limits - Automatic stop after command timeout diff --git a/steering_controllers/tricycle_controller/include/tricycle_controller/steering_limiter.hpp b/steering_controllers/tricycle_controller/include/tricycle_controller/steering_limiter.hpp deleted file mode 100644 index a16f0de0f0..0000000000 --- a/steering_controllers/tricycle_controller/include/tricycle_controller/steering_limiter.hpp +++ /dev/null @@ -1,94 +0,0 @@ -// Copyright 2022 Pixel Robotics. -// -// 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. - -/* - * Author: Tony Najjar - */ - -#ifndef TRICYCLE_CONTROLLER__STEERING_LIMITER_HPP_ -#define TRICYCLE_CONTROLLER__STEERING_LIMITER_HPP_ - -#include - -namespace tricycle_controller -{ -class SteeringLimiter -{ -public: - /** - * \brief Constructor - * \param [in] min_position Minimum position [m] or [rad] - * \param [in] max_position Maximum position [m] or [rad] - * \param [in] min_velocity Minimum velocity [m/s] or [rad/s] - * \param [in] max_velocity Maximum velocity [m/s] or [rad/s] - * \param [in] min_acceleration Minimum acceleration [m/s^2] or [rad/s^2] - * \param [in] max_acceleration Maximum acceleration [m/s^2] or [rad/s^2] - */ - SteeringLimiter( - double min_position = NAN, double max_position = NAN, double min_velocity = NAN, - double max_velocity = NAN, double min_acceleration = NAN, double max_acceleration = NAN); - - /** - * \brief Limit the position, velocity and acceleration - * \param [in, out] p position [m] or [rad] - * \param [in] p0 Previous position to p [m] or [rad] - * \param [in] p1 Previous position to p0 [m] or [rad] - * \param [in] dt Time step [s] - * \return Limiting factor (1.0 if none) - */ - double limit(double & p, double p0, double p1, double dt); - - /** - * \brief Limit the jerk - * \param [in, out] p position [m] or [rad] - * \param [in] p0 Previous position to p [m] or [rad] - * \param [in] p1 Previous position to p0 [m] or [rad] - * \param [in] dt Time step [s] - * \return Limiting factor (1.0 if none) - */ - double limit_position(double & p); - - /** - * \brief Limit the velocity - * \param [in, out] p position [m] - * \return Limiting factor (1.0 if none) - */ - double limit_velocity(double & p, double p0, double dt); - - /** - * \brief Limit the acceleration - * \param [in, out] p Position [m] or [rad] - * \param [in] p0 Previous position [m] or [rad] - * \param [in] dt Time step [s] - * \return Limiting factor (1.0 if none) - */ - double limit_acceleration(double & p, double p0, double p1, double dt); - -private: - // Position limits: - double min_position_; - double max_position_; - - // Velocity limits: - double min_velocity_; - double max_velocity_; - - // Acceleration limits: - double min_acceleration_; - double max_acceleration_; -}; - -} // namespace tricycle_controller - -#endif // TRICYCLE_CONTROLLER__STEERING_LIMITER_HPP_ diff --git a/steering_controllers/tricycle_controller/include/tricycle_controller/traction_limiter.hpp b/steering_controllers/tricycle_controller/include/tricycle_controller/traction_limiter.hpp deleted file mode 100644 index ea0bb16025..0000000000 --- a/steering_controllers/tricycle_controller/include/tricycle_controller/traction_limiter.hpp +++ /dev/null @@ -1,102 +0,0 @@ -// Copyright 2022 Pixel Robotics. -// -// 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. - -/* - * Author: Tony Najjar - */ - -#ifndef TRICYCLE_CONTROLLER__TRACTION_LIMITER_HPP_ -#define TRICYCLE_CONTROLLER__TRACTION_LIMITER_HPP_ - -#include - -namespace tricycle_controller -{ -class TractionLimiter -{ -public: - /** - * \brief Constructor - * \param [in] min_velocity Minimum velocity [m/s] or [rad/s] - * \param [in] max_velocity Maximum velocity [m/s] or [rad/s] - * \param [in] min_acceleration Minimum acceleration [m/s^2] or [rad/s^2] - * \param [in] max_acceleration Maximum acceleration [m/s^2] or [rad/s^2] - * \param [in] min_deceleration Minimum deceleration [m/s^2] or [rad/s^2] - * \param [in] max_deceleration Maximum deceleration [m/s^2] or [rad/s^2] - * \param [in] min_jerk Minimum jerk [m/s^3], usually <= 0 - * \param [in] max_jerk Maximum jerk [m/s^3], usually >= 0 - */ - TractionLimiter( - double min_velocity = NAN, double max_velocity = NAN, double min_acceleration = NAN, - double max_acceleration = NAN, double min_deceleration = NAN, double max_deceleration = NAN, - double min_jerk = NAN, double max_jerk = NAN); - - /** - * \brief Limit the velocity and acceleration - * \param [in, out] v Velocity [m/s] or [rad/s] - * \param [in] v0 Previous velocity to v [m/s] or [rad/s] - * \param [in] v1 Previous velocity to v0 [m/s] or [rad/s] - * \param [in] dt Time step [s] - * \return Limiting factor (1.0 if none) - */ - double limit(double & v, double v0, double v1, double dt); - - /** - * \brief Limit the velocity - * \param [in, out] v Velocity [m/s] or [rad/s] - * \return Limiting factor (1.0 if none) - */ - double limit_velocity(double & v); - - /** - * \brief Limit the acceleration - * \param [in, out] v Velocity [m/s] or [rad/s] - * \param [in] v0 Previous velocity [m/s] or [rad/s] - * \param [in] dt Time step [s] - * \return Limiting factor (1.0 if none) - */ - double limit_acceleration(double & v, double v0, double dt); - - /** - * \brief Limit the jerk - * \param [in, out] v Velocity [m/s] or [rad/s] - * \param [in] v0 Previous velocity to v [m/s] or [rad/s] - * \param [in] v1 Previous velocity to v0 [m/s] or [rad/s] - * \param [in] dt Time step [s] - * \return Limiting factor (1.0 if none) - * \see http://en.wikipedia.org/wiki/Jerk_%28physics%29#Motion_control - */ - double limit_jerk(double & v, double v0, double v1, double dt); - -private: - // Velocity limits: - double min_velocity_; - double max_velocity_; - - // Acceleration limits: - double min_acceleration_; - double max_acceleration_; - - // Deceleration limits: - double min_deceleration_; - double max_deceleration_; - - // Jerk limits: - double min_jerk_; - double max_jerk_; -}; - -} // namespace tricycle_controller - -#endif // TRICYCLE_CONTROLLER__TRACTION_LIMITER_HPP_ diff --git a/steering_controllers/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp b/steering_controllers/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp deleted file mode 100644 index 356d9daf8a..0000000000 --- a/steering_controllers/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp +++ /dev/null @@ -1,185 +0,0 @@ -// Copyright 2022 Pixel Robotics. -// -// 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. - -/* - * Author: Tony Najjar - */ - -#ifndef TRICYCLE_CONTROLLER__TRICYCLE_CONTROLLER_HPP_ -#define TRICYCLE_CONTROLLER__TRICYCLE_CONTROLLER_HPP_ - -#include -#include -#include -#include -#include -#include -#include - -#include "ackermann_msgs/msg/ackermann_drive.hpp" -#include "controller_interface/controller_interface.hpp" -#include "geometry_msgs/msg/twist.hpp" -#include "geometry_msgs/msg/twist_stamped.hpp" -#include "hardware_interface/handle.hpp" -#include "nav_msgs/msg/odometry.hpp" -#include "rclcpp/rclcpp.hpp" -#include "rclcpp_lifecycle/state.hpp" -#include "realtime_tools/realtime_box.h" -#include "realtime_tools/realtime_buffer.h" -#include "realtime_tools/realtime_publisher.h" -#include "std_srvs/srv/empty.hpp" -#include "tf2_msgs/msg/tf_message.hpp" -//#include "tricycle_controller/odometry.hpp" -#include "steering_odometry/steering_odometry.hpp" -#include "tricycle_controller/steering_limiter.hpp" -#include "tricycle_controller/traction_limiter.hpp" -#include "tricycle_controller/visibility_control.h" - -namespace tricycle_controller -{ -using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; - -class TricycleController : public controller_interface::ControllerInterface -{ - using Twist = geometry_msgs::msg::Twist; - using TwistStamped = geometry_msgs::msg::TwistStamped; - using AckermannDrive = ackermann_msgs::msg::AckermannDrive; - -public: - TRICYCLE_CONTROLLER_PUBLIC - TricycleController(); - - TRICYCLE_CONTROLLER_PUBLIC - controller_interface::InterfaceConfiguration command_interface_configuration() const override; - - TRICYCLE_CONTROLLER_PUBLIC - controller_interface::InterfaceConfiguration state_interface_configuration() const override; - - TRICYCLE_CONTROLLER_PUBLIC - controller_interface::return_type update( - const rclcpp::Time & time, const rclcpp::Duration & period) override; - - TRICYCLE_CONTROLLER_PUBLIC - CallbackReturn on_init() override; - - TRICYCLE_CONTROLLER_PUBLIC - CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; - - TRICYCLE_CONTROLLER_PUBLIC - CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; - - TRICYCLE_CONTROLLER_PUBLIC - CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; - - TRICYCLE_CONTROLLER_PUBLIC - CallbackReturn on_cleanup(const rclcpp_lifecycle::State & previous_state) override; - - TRICYCLE_CONTROLLER_PUBLIC - CallbackReturn on_error(const rclcpp_lifecycle::State & previous_state) override; - - TRICYCLE_CONTROLLER_PUBLIC - CallbackReturn on_shutdown(const rclcpp_lifecycle::State & previous_state) override; - -protected: - struct TractionHandle - { - std::reference_wrapper velocity_state; - std::reference_wrapper velocity_command; - }; - struct SteeringHandle - { - std::reference_wrapper position_state; - std::reference_wrapper position_command; - }; - - CallbackReturn get_traction( - const std::string & traction_joint_name, std::vector & joint); - CallbackReturn get_steering( - const std::string & steering_joint_name, std::vector & joint); - - std::string traction_joint_name_; - std::string steering_joint_name_; - - // HACK: put into vector to avoid initializing structs because they have no default constructors - std::vector traction_joint_; - std::vector steering_joint_; - - struct WheelParams - { - double wheelbase = 0.0; - double radius = 0.0; - } wheel_params_; - - struct OdometryParams - { - bool open_loop = false; - bool enable_odom_tf = false; - bool odom_only_twist = false; // for doing the pose integration in separate node - std::string base_frame_id = "base_link"; - std::string odom_frame_id = "odom"; - std::array pose_covariance_diagonal; - std::array twist_covariance_diagonal; - } odom_params_; - - bool publish_ackermann_command_ = false; - std::shared_ptr> ackermann_command_publisher_ = nullptr; - std::shared_ptr> - realtime_ackermann_command_publisher_ = nullptr; - - steering_odometry::SteeringOdometry odometry_; - - std::shared_ptr> odometry_publisher_ = nullptr; - std::shared_ptr> - realtime_odometry_publisher_ = nullptr; - - std::shared_ptr> odometry_transform_publisher_ = - nullptr; - std::shared_ptr> - realtime_odometry_transform_publisher_ = nullptr; - - // Timeout to consider cmd_vel commands old - std::chrono::milliseconds cmd_vel_timeout_{500}; - - bool subscriber_is_active_ = false; - rclcpp::Subscription::SharedPtr velocity_command_subscriber_ = nullptr; - rclcpp::Subscription::SharedPtr - velocity_command_unstamped_subscriber_ = nullptr; - - realtime_tools::RealtimeBox> received_velocity_msg_ptr_{nullptr}; - - rclcpp::Service::SharedPtr reset_odom_service_; - - std::queue previous_commands_; // last two commands - - // speed limiters - TractionLimiter limiter_traction_; - SteeringLimiter limiter_steering_; - - // publish rate limiter - double publish_rate_ = 50.0; - rclcpp::Duration publish_period_ = rclcpp::Duration::from_nanoseconds(0); - rclcpp::Time previous_publish_timestamp_{0}; - - bool is_halted = false; - bool use_stamped_vel_ = true; - - void reset_odometry( - const std::shared_ptr request_header, - const std::shared_ptr req, - std::shared_ptr res); - bool reset(); - void halt(); -}; -} // namespace tricycle_controller -#endif // TRICYCLE_CONTROLLER__TRICYCLE_CONTROLLER_HPP_ diff --git a/steering_controllers/tricycle_controller/include/tricycle_controller/visibility_control.h b/steering_controllers/tricycle_controller/include/tricycle_controller/visibility_control.h deleted file mode 100644 index bc9b34898b..0000000000 --- a/steering_controllers/tricycle_controller/include/tricycle_controller/visibility_control.h +++ /dev/null @@ -1,53 +0,0 @@ -// Copyright 2022 Pixel Robotics. -// -// 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. - -/* - * Author: Tony Najjar - */ - -#ifndef TRICYCLE_CONTROLLER__VISIBILITY_CONTROL_H_ -#define TRICYCLE_CONTROLLER__VISIBILITY_CONTROL_H_ - -// This logic was borrowed (then namespaced) from the examples on the gcc wiki: -// https://gcc.gnu.org/wiki/Visibility - -#if defined _WIN32 || defined __CYGWIN__ -#ifdef __GNUC__ -#define TRICYCLE_CONTROLLER_EXPORT __attribute__((dllexport)) -#define TRICYCLE_CONTROLLER_IMPORT __attribute__((dllimport)) -#else -#define TRICYCLE_CONTROLLER_EXPORT __declspec(dllexport) -#define TRICYCLE_CONTROLLER_IMPORT __declspec(dllimport) -#endif -#ifdef TRICYCLE_CONTROLLER_BUILDING_DLL -#define TRICYCLE_CONTROLLER_PUBLIC TRICYCLE_CONTROLLER_EXPORT -#else -#define TRICYCLE_CONTROLLER_PUBLIC TRICYCLE_CONTROLLER_IMPORT -#endif -#define TRICYCLE_CONTROLLER_PUBLIC_TYPE TRICYCLE_CONTROLLER_PUBLIC -#define TRICYCLE_CONTROLLER_LOCAL -#else -#define TRICYCLE_CONTROLLER_EXPORT __attribute__((visibility("default"))) -#define TRICYCLE_CONTROLLER_IMPORT -#if __GNUC__ >= 4 -#define TRICYCLE_CONTROLLER_PUBLIC __attribute__((visibility("default"))) -#define TRICYCLE_CONTROLLER_LOCAL __attribute__((visibility("hidden"))) -#else -#define TRICYCLE_CONTROLLER_PUBLIC -#define TRICYCLE_CONTROLLER_LOCAL -#endif -#define TRICYCLE_CONTROLLER_PUBLIC_TYPE -#endif - -#endif // TRICYCLE_CONTROLLER__VISIBILITY_CONTROL_H_ diff --git a/steering_controllers/tricycle_controller/package.xml b/steering_controllers/tricycle_controller/package.xml deleted file mode 100644 index 6ca950c0c5..0000000000 --- a/steering_controllers/tricycle_controller/package.xml +++ /dev/null @@ -1,38 +0,0 @@ - - - - tricycle_controller - 2.14.0 - Controller for a tricycle drive mobile base - Bence Magyar - Tony Najjar - Apache License 2.0 - Tony Najjar - - ament_cmake - - controller_interface - geometry_msgs - hardware_interface - nav_msgs - std_srvs - rclcpp - rclcpp_lifecycle - rcpputils - realtime_tools - tf2 - tf2_msgs - ackermann_msgs - steering_controllers - stering_odometry - - pluginlib - - ament_cmake_gmock - controller_manager - ros2_control_test_assets - - - ament_cmake - - diff --git a/steering_controllers/tricycle_controller/src/steering_limiter.cpp b/steering_controllers/tricycle_controller/src/steering_limiter.cpp deleted file mode 100644 index b76cc0f602..0000000000 --- a/steering_controllers/tricycle_controller/src/steering_limiter.cpp +++ /dev/null @@ -1,117 +0,0 @@ -// Copyright 2022 Pixel Robotics. -// -// 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. - -/* - * Author: Tony Najjar - */ - -#include -#include -#include - -#include "rcppmath/clamp.hpp" -#include "tricycle_controller/steering_limiter.hpp" - -namespace tricycle_controller -{ -SteeringLimiter::SteeringLimiter( - double min_position, double max_position, double min_velocity, double max_velocity, - double min_acceleration, double max_acceleration) -: min_position_(min_position), - max_position_(max_position), - min_velocity_(min_velocity), - max_velocity_(max_velocity), - min_acceleration_(min_acceleration), - max_acceleration_(max_acceleration) -{ - if (!std::isnan(min_position_) && std::isnan(max_position_)) max_position_ = -min_position_; - if (!std::isnan(max_position_) && std::isnan(min_position_)) min_position_ = -max_position_; - - if (!std::isnan(min_velocity_) && std::isnan(max_velocity_)) - max_velocity_ = 1000.0; // Arbitrarily big number - if (!std::isnan(max_velocity_) && std::isnan(min_velocity_)) min_velocity_ = 0.0; - - if (!std::isnan(min_acceleration_) && std::isnan(max_acceleration_)) max_acceleration_ = 1000.0; - if (!std::isnan(max_acceleration_) && std::isnan(min_acceleration_)) min_acceleration_ = 0.0; - - const std::string error = - "The positive limit will be applied to both directions. Setting different limits for positive " - "and negative directions is not supported. Actuators are " - "assumed to have the same constraints in both directions"; - - if (min_velocity_ < 0 || max_velocity_ < 0) - { - throw std::invalid_argument("Velocity cannot be negative." + error); - } - - if (min_acceleration_ < 0 || max_acceleration_ < 0) - { - throw std::invalid_argument("Acceleration cannot be negative." + error); - } -} - -double SteeringLimiter::limit(double & p, double p0, double p1, double dt) -{ - const double tmp = p; - - if (!std::isnan(min_acceleration_) && !std::isnan(max_acceleration_)) - limit_acceleration(p, p0, p1, dt); - if (!std::isnan(min_velocity_) && !std::isnan(max_velocity_)) limit_velocity(p, p0, dt); - if (!std::isnan(min_position_) && !std::isnan(max_position_)) limit_position(p); - - return tmp != 0.0 ? p / tmp : 1.0; -} - -double SteeringLimiter::limit_position(double & p) -{ - const double tmp = p; - p = rcppmath::clamp(p, min_position_, max_position_); - - return tmp != 0.0 ? p / tmp : 1.0; -} - -double SteeringLimiter::limit_velocity(double & p, double p0, double dt) -{ - const double tmp = p; - - const double dv_min = min_velocity_ * dt; - const double dv_max = max_velocity_ * dt; - - double dv = rcppmath::clamp(std::fabs(p - p0), dv_min, dv_max); - dv *= (p - p0 >= 0 ? 1 : -1); - p = p0 + dv; - - return tmp != 0.0 ? p / tmp : 1.0; -} - -double SteeringLimiter::limit_acceleration(double & p, double p0, double p1, double dt) -{ - const double tmp = p; - - const double dv = p - p0; - const double dp0 = p0 - p1; - - const double dt2 = 2. * dt * dt; - - const double da_min = min_acceleration_ * dt2; - const double da_max = max_acceleration_ * dt2; - - double da = rcppmath::clamp(std::fabs(dv - dp0), da_min, da_max); - da *= (dv - dp0 >= 0 ? 1 : -1); - p = p0 + dp0 + da; - - return tmp != 0.0 ? p / tmp : 1.0; -} - -} // namespace tricycle_controller diff --git a/steering_controllers/tricycle_controller/src/traction_limiter.cpp b/steering_controllers/tricycle_controller/src/traction_limiter.cpp deleted file mode 100644 index 7865d4be71..0000000000 --- a/steering_controllers/tricycle_controller/src/traction_limiter.cpp +++ /dev/null @@ -1,142 +0,0 @@ -// Copyright 2022 Pixel Robotics. -// -// 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. - -/* - * Author: Tony Najjar - */ - -#include -#include -#include - -#include "rcppmath/clamp.hpp" -#include "tricycle_controller/traction_limiter.hpp" - -namespace tricycle_controller -{ -TractionLimiter::TractionLimiter( - double min_velocity, double max_velocity, double min_acceleration, double max_acceleration, - double min_deceleration, double max_deceleration, double min_jerk, double max_jerk) -: min_velocity_(min_velocity), - max_velocity_(max_velocity), - min_acceleration_(min_acceleration), - max_acceleration_(max_acceleration), - min_deceleration_(min_deceleration), - max_deceleration_(max_deceleration), - min_jerk_(min_jerk), - max_jerk_(max_jerk) -{ - if (!std::isnan(min_velocity_) && std::isnan(max_velocity_)) - max_velocity_ = 1000.0; // Arbitrarily big number - if (!std::isnan(max_velocity_) && std::isnan(min_velocity_)) min_velocity_ = 0.0; - - if (!std::isnan(min_acceleration_) && std::isnan(max_acceleration_)) max_acceleration_ = 1000.0; - if (!std::isnan(max_acceleration_) && std::isnan(min_acceleration_)) min_acceleration_ = 0.0; - - if (!std::isnan(min_deceleration_) && std::isnan(max_deceleration_)) max_deceleration_ = 1000.0; - if (!std::isnan(max_deceleration_) && std::isnan(min_acceleration_)) min_deceleration_ = 0.0; - - if (!std::isnan(min_jerk_) && std::isnan(max_jerk_)) max_jerk_ = 1000.0; - if (!std::isnan(max_jerk_) && std::isnan(min_jerk_)) min_jerk_ = 0.0; - - const std::string error = - "The positive limit will be applied to both directions. Setting different limits for positive " - "and negative directions is not supported. Actuators are " - "assumed to have the same constraints in both directions"; - if (min_velocity_ < 0 || max_velocity_ < 0) - { - throw std::invalid_argument("Velocity cannot be negative." + error); - } - - if (min_acceleration_ < 0 || max_acceleration_ < 0) - { - throw std::invalid_argument("Acceleration cannot be negative." + error); - } - - if (min_deceleration_ < 0 || max_deceleration_ < 0) - { - throw std::invalid_argument("Deceleration cannot be negative." + error); - } - - if (min_jerk_ < 0 || max_jerk_ < 0) - { - throw std::invalid_argument("Jerk cannot be negative." + error); - } -} - -double TractionLimiter::limit(double & v, double v0, double v1, double dt) -{ - const double tmp = v; - - if (!std::isnan(min_jerk_) && !std::isnan(max_jerk_)) limit_jerk(v, v0, v1, dt); - if (!std::isnan(min_acceleration_) && !std::isnan(max_acceleration_)) - limit_acceleration(v, v0, dt); - if (!std::isnan(min_velocity_) && !std::isnan(max_velocity_)) limit_velocity(v); - - return tmp != 0.0 ? v / tmp : 1.0; -} - -double TractionLimiter::limit_velocity(double & v) -{ - const double tmp = v; - - v = rcppmath::clamp(std::fabs(v), min_velocity_, max_velocity_); - - v *= tmp >= 0 ? 1 : -1; - return tmp != 0.0 ? v / tmp : 1.0; -} - -double TractionLimiter::limit_acceleration(double & v, double v0, double dt) -{ - const double tmp = v; - - double dv_min; - double dv_max; - if (abs(v) >= abs(v0)) - { - dv_min = min_acceleration_ * dt; - dv_max = max_acceleration_ * dt; - } - else - { - dv_min = min_deceleration_ * dt; - dv_max = max_deceleration_ * dt; - } - double dv = rcppmath::clamp(std::fabs(v - v0), dv_min, dv_max); - dv *= (v - v0 >= 0 ? 1 : -1); - v = v0 + dv; - - return tmp != 0.0 ? v / tmp : 1.0; -} - -double TractionLimiter::limit_jerk(double & v, double v0, double v1, double dt) -{ - const double tmp = v; - - const double dv = v - v0; - const double dv0 = v0 - v1; - - const double dt2 = 2. * dt * dt; - - const double da_min = min_jerk_ * dt2; - const double da_max = max_jerk_ * dt2; - - double da = rcppmath::clamp(std::fabs(dv - dv0), da_min, da_max); - da *= (dv - dv0 >= 0 ? 1 : -1); - v = v0 + dv0 + da; - - return tmp != 0.0 ? v / tmp : 1.0; -} - -} // namespace tricycle_controller diff --git a/steering_controllers/tricycle_controller/src/tricycle_controller.cpp b/steering_controllers/tricycle_controller/src/tricycle_controller.cpp deleted file mode 100644 index 42f09cfc94..0000000000 --- a/steering_controllers/tricycle_controller/src/tricycle_controller.cpp +++ /dev/null @@ -1,644 +0,0 @@ -// Copyright 2022 Pixel Robotics. -// -// 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. - -/* - * Author: Tony Najjar - */ - -#include -#include -#include -#include -#include - -#include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "lifecycle_msgs/msg/state.hpp" -#include "rclcpp/logging.hpp" -#include "tf2/LinearMath/Quaternion.h" -#include "tricycle_controller/tricycle_controller.hpp" - -namespace -{ -constexpr auto DEFAULT_COMMAND_TOPIC = "~/cmd_vel"; -constexpr auto DEFAULT_ACKERMANN_OUT_TOPIC = "~/cmd_ackermann"; -constexpr auto DEFAULT_ODOMETRY_TOPIC = "~/odom"; -constexpr auto DEFAULT_TRANSFORM_TOPIC = "/tf"; -constexpr auto DEFAULT_RESET_ODOM_SERVICE = "~/reset_odometry"; -} // namespace - -namespace tricycle_controller -{ -using namespace std::chrono_literals; -using controller_interface::interface_configuration_type; -using controller_interface::InterfaceConfiguration; -using hardware_interface::HW_IF_POSITION; -using hardware_interface::HW_IF_VELOCITY; -using lifecycle_msgs::msg::State; - -TricycleController::TricycleController() : controller_interface::ControllerInterface() {} - -CallbackReturn TricycleController::on_init() -{ - try - { - // with the lifecycle node being initialized, we can declare parameters - auto_declare("traction_joint_name", std::string()); - auto_declare("steering_joint_name", std::string()); - - auto_declare("wheelbase", wheel_params_.wheelbase); - auto_declare("wheel_radius", wheel_params_.radius); - - auto_declare("odom_frame_id", odom_params_.odom_frame_id); - auto_declare("base_frame_id", odom_params_.base_frame_id); - auto_declare>("pose_covariance_diagonal", std::vector()); - auto_declare>("twist_covariance_diagonal", std::vector()); - auto_declare("open_loop", odom_params_.open_loop); - auto_declare("enable_odom_tf", odom_params_.enable_odom_tf); - auto_declare("odom_only_twist", odom_params_.odom_only_twist); - - auto_declare("cmd_vel_timeout", cmd_vel_timeout_.count()); - auto_declare("publish_ackermann_command", publish_ackermann_command_); - auto_declare("velocity_rolling_window_size", 10); - auto_declare("use_stamped_vel", use_stamped_vel_); - auto_declare("publish_rate", publish_rate_); - - auto_declare("traction.max_velocity", NAN); - auto_declare("traction.min_velocity", NAN); - auto_declare("traction.max_acceleration", NAN); - auto_declare("traction.min_acceleration", NAN); - auto_declare("traction.max_deceleration", NAN); - auto_declare("traction.min_deceleration", NAN); - auto_declare("traction.max_jerk", NAN); - auto_declare("traction.min_jerk", NAN); - - auto_declare("steering.max_position", NAN); - auto_declare("steering.min_position", NAN); - auto_declare("steering.max_velocity", NAN); - auto_declare("steering.min_velocity", NAN); - auto_declare("steering.max_acceleration", NAN); - auto_declare("steering.min_acceleration", NAN); - } - catch (const std::exception & e) - { - fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); - return CallbackReturn::ERROR; - } - - return CallbackReturn::SUCCESS; -} - -InterfaceConfiguration TricycleController::command_interface_configuration() const -{ - InterfaceConfiguration command_interfaces_config; - command_interfaces_config.type = interface_configuration_type::INDIVIDUAL; - command_interfaces_config.names.push_back(traction_joint_name_ + "/" + HW_IF_VELOCITY); - command_interfaces_config.names.push_back(steering_joint_name_ + "/" + HW_IF_POSITION); - return command_interfaces_config; -} - -InterfaceConfiguration TricycleController::state_interface_configuration() const -{ - InterfaceConfiguration state_interfaces_config; - state_interfaces_config.type = interface_configuration_type::INDIVIDUAL; - state_interfaces_config.names.push_back(traction_joint_name_ + "/" + HW_IF_VELOCITY); - state_interfaces_config.names.push_back(steering_joint_name_ + "/" + HW_IF_POSITION); - return state_interfaces_config; -} - -controller_interface::return_type TricycleController::update( - const rclcpp::Time & time, const rclcpp::Duration & period) -{ - if (get_state().id() == State::PRIMARY_STATE_INACTIVE) - { - if (!is_halted) - { - halt(); - is_halted = true; - } - return controller_interface::return_type::OK; - } - std::shared_ptr last_command_msg; - received_velocity_msg_ptr_.get(last_command_msg); - if (last_command_msg == nullptr) - { - RCLCPP_WARN(get_node()->get_logger(), "Velocity message received was a nullptr."); - return controller_interface::return_type::ERROR; - } - - const auto age_of_last_command = time - last_command_msg->header.stamp; - // Brake if cmd_vel has timeout, override the stored command - if (age_of_last_command > cmd_vel_timeout_) - { - last_command_msg->twist.linear.x = 0.0; - last_command_msg->twist.angular.z = 0.0; - } - - // command may be limited further by Limiters, - // without affecting the stored twist command - TwistStamped command = *last_command_msg; - double & linear_command = command.twist.linear.x; - double & angular_command = command.twist.angular.z; - double Ws_read = traction_joint_[0].velocity_state.get().get_value(); // in radians/s - double alpha_read = steering_joint_[0].position_state.get().get_value(); // in radians - - if (odom_params_.open_loop) - { - odometry_.update_open_loop(linear_command, angular_command, period.seconds()); - } - else - { - if (std::isnan(Ws_read) || std::isnan(alpha_read)) - { - RCLCPP_ERROR(get_node()->get_logger(), "Could not read feedback value"); - return controller_interface::return_type::ERROR; - } - odometry_.update_from_velocity(Ws_read, alpha_read, period.seconds()); - } - - tf2::Quaternion orientation; - orientation.setRPY(0.0, 0.0, odometry_.get_heading()); - - if (previous_publish_timestamp_ + publish_period_ < time) - { - previous_publish_timestamp_ += publish_period_; - - if (realtime_odometry_publisher_->trylock()) - { - auto & odometry_message = realtime_odometry_publisher_->msg_; - odometry_message.header.stamp = time; - if (!odom_params_.odom_only_twist) - { - odometry_message.pose.pose.position.x = odometry_.get_x(); - odometry_message.pose.pose.position.y = odometry_.get_y(); - odometry_message.pose.pose.orientation.x = orientation.x(); - odometry_message.pose.pose.orientation.y = orientation.y(); - odometry_message.pose.pose.orientation.z = orientation.z(); - odometry_message.pose.pose.orientation.w = orientation.w(); - } - odometry_message.twist.twist.linear.x = odometry_.get_linear(); - odometry_message.twist.twist.angular.z = odometry_.get_angular(); - realtime_odometry_publisher_->unlockAndPublish(); - } - - if (odom_params_.enable_odom_tf && realtime_odometry_transform_publisher_->trylock()) - { - auto & transform = realtime_odometry_transform_publisher_->msg_.transforms.front(); - transform.header.stamp = time; - transform.transform.translation.x = odometry_.get_x(); - transform.transform.translation.y = odometry_.get_y(); - transform.transform.rotation.x = orientation.x(); - transform.transform.rotation.y = orientation.y(); - transform.transform.rotation.z = orientation.z(); - transform.transform.rotation.w = orientation.w(); - realtime_odometry_transform_publisher_->unlockAndPublish(); - } - } - - // Compute wheel velocity and angle - auto [alpha_write, Ws_write] = odometry_.twist_to_ackermann(linear_command, angular_command); - - // Reduce wheel speed until the target angle has been reached - double alpha_delta = abs(alpha_write - alpha_read); - double scale; - if (alpha_delta < M_PI / 6) - { - scale = 1; - } - else if (alpha_delta > M_PI_2) - { - scale = 0.01; - } - else - { - // TODO(anyone): find the best function, e.g convex power functions - scale = cos(alpha_delta); - } - Ws_write *= scale; - - auto & last_command = previous_commands_.back(); - auto & second_to_last_command = previous_commands_.front(); - - limiter_traction_.limit( - Ws_write, last_command.speed, second_to_last_command.speed, period.seconds()); - - limiter_steering_.limit( - alpha_write, last_command.steering_angle, second_to_last_command.steering_angle, - period.seconds()); - - previous_commands_.pop(); - AckermannDrive ackermann_command; - // speed in AckermannDrive is defined as desired forward speed (m/s) but it is used here as wheel - // speed (rad/s) - ackermann_command.speed = Ws_write; - ackermann_command.steering_angle = alpha_write; - previous_commands_.emplace(ackermann_command); - - // Publish ackermann command - if (publish_ackermann_command_ && realtime_ackermann_command_publisher_->trylock()) - { - auto & realtime_ackermann_command = realtime_ackermann_command_publisher_->msg_; - // speed in AckermannDrive is defined desired forward speed (m/s) but we use it here as wheel - // speed (rad/s) - realtime_ackermann_command.speed = Ws_write; - realtime_ackermann_command.steering_angle = alpha_write; - realtime_ackermann_command_publisher_->unlockAndPublish(); - } - - traction_joint_[0].velocity_command.get().set_value(Ws_write); - steering_joint_[0].position_command.get().set_value(alpha_write); - return controller_interface::return_type::OK; -} - -CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State & /*previous_state*/) -{ - auto logger = get_node()->get_logger(); - - // update parameters - traction_joint_name_ = get_node()->get_parameter("traction_joint_name").as_string(); - steering_joint_name_ = get_node()->get_parameter("steering_joint_name").as_string(); - if (traction_joint_name_.empty()) - { - RCLCPP_ERROR(logger, "'traction_joint_name' parameter was empty"); - return CallbackReturn::ERROR; - } - if (steering_joint_name_.empty()) - { - RCLCPP_ERROR(logger, "'steering_joint_name' parameter was empty"); - return CallbackReturn::ERROR; - } - - wheel_params_.wheelbase = get_node()->get_parameter("wheelbase").as_double(); - wheel_params_.radius = get_node()->get_parameter("wheel_radius").as_double(); - - odometry_.set_wheel_params(wheel_params_.wheelbase, wheel_params_.radius, 0.0); - odometry_.set_velocity_rolling_window_size( - get_node()->get_parameter("velocity_rolling_window_size").as_int()); - - odom_params_.odom_frame_id = get_node()->get_parameter("odom_frame_id").as_string(); - odom_params_.base_frame_id = get_node()->get_parameter("base_frame_id").as_string(); - - auto pose_diagonal = get_node()->get_parameter("pose_covariance_diagonal").as_double_array(); - std::copy( - pose_diagonal.begin(), pose_diagonal.end(), odom_params_.pose_covariance_diagonal.begin()); - - auto twist_diagonal = get_node()->get_parameter("twist_covariance_diagonal").as_double_array(); - std::copy( - twist_diagonal.begin(), twist_diagonal.end(), odom_params_.twist_covariance_diagonal.begin()); - - odom_params_.open_loop = get_node()->get_parameter("open_loop").as_bool(); - odom_params_.enable_odom_tf = get_node()->get_parameter("enable_odom_tf").as_bool(); - odom_params_.odom_only_twist = get_node()->get_parameter("odom_only_twist").as_bool(); - - cmd_vel_timeout_ = - std::chrono::milliseconds{get_node()->get_parameter("cmd_vel_timeout").as_int()}; - publish_ackermann_command_ = get_node()->get_parameter("publish_ackermann_command").as_bool(); - use_stamped_vel_ = get_node()->get_parameter("use_stamped_vel").as_bool(); - - publish_rate_ = get_node()->get_parameter("publish_rate").as_double(); - publish_period_ = rclcpp::Duration::from_seconds(1.0 / publish_rate_); - - try - { - limiter_traction_ = TractionLimiter( - get_node()->get_parameter("traction.min_velocity").as_double(), - get_node()->get_parameter("traction.max_velocity").as_double(), - get_node()->get_parameter("traction.min_acceleration").as_double(), - get_node()->get_parameter("traction.max_acceleration").as_double(), - get_node()->get_parameter("traction.min_deceleration").as_double(), - get_node()->get_parameter("traction.max_deceleration").as_double(), - get_node()->get_parameter("traction.min_jerk").as_double(), - get_node()->get_parameter("traction.max_jerk").as_double()); - } - catch (const std::invalid_argument & e) - { - RCLCPP_ERROR(get_node()->get_logger(), "Error configuring traction limiter: %s", e.what()); - return CallbackReturn::ERROR; - } - - try - { - limiter_steering_ = SteeringLimiter( - get_node()->get_parameter("steering.min_position").as_double(), - get_node()->get_parameter("steering.max_position").as_double(), - get_node()->get_parameter("steering.min_velocity").as_double(), - get_node()->get_parameter("steering.max_velocity").as_double(), - get_node()->get_parameter("steering.min_acceleration").as_double(), - get_node()->get_parameter("steering.max_acceleration").as_double()); - } - catch (const std::invalid_argument & e) - { - RCLCPP_ERROR(get_node()->get_logger(), "Error configuring steering limiter: %s", e.what()); - return CallbackReturn::ERROR; - } - - if (!reset()) - { - return CallbackReturn::ERROR; - } - - const TwistStamped empty_twist; - received_velocity_msg_ptr_.set(std::make_shared(empty_twist)); - - // Fill last two commands with default constructed commands - const AckermannDrive empty_ackermann_drive; - previous_commands_.emplace(empty_ackermann_drive); - previous_commands_.emplace(empty_ackermann_drive); - - // initialize ackermann command publisher - if (publish_ackermann_command_) - { - ackermann_command_publisher_ = get_node()->create_publisher( - DEFAULT_ACKERMANN_OUT_TOPIC, rclcpp::SystemDefaultsQoS()); - realtime_ackermann_command_publisher_ = - std::make_shared>( - ackermann_command_publisher_); - } - - // initialize command subscriber - if (use_stamped_vel_) - { - velocity_command_subscriber_ = get_node()->create_subscription( - DEFAULT_COMMAND_TOPIC, rclcpp::SystemDefaultsQoS(), - [this](const std::shared_ptr msg) -> void - { - if (!subscriber_is_active_) - { - RCLCPP_WARN( - get_node()->get_logger(), "Can't accept new commands. subscriber is inactive"); - return; - } - if ((msg->header.stamp.sec == 0) && (msg->header.stamp.nanosec == 0)) - { - RCLCPP_WARN_ONCE( - get_node()->get_logger(), - "Received TwistStamped with zero timestamp, setting it to current " - "time, this message will only be shown once"); - msg->header.stamp = get_node()->get_clock()->now(); - } - received_velocity_msg_ptr_.set(std::move(msg)); - }); - } - else - { - velocity_command_unstamped_subscriber_ = get_node()->create_subscription( - DEFAULT_COMMAND_TOPIC, rclcpp::SystemDefaultsQoS(), - [this](const std::shared_ptr msg) -> void - { - if (!subscriber_is_active_) - { - RCLCPP_WARN( - get_node()->get_logger(), "Can't accept new commands. subscriber is inactive"); - return; - } - - // Write fake header in the stored stamped command - std::shared_ptr twist_stamped; - received_velocity_msg_ptr_.get(twist_stamped); - twist_stamped->twist = *msg; - twist_stamped->header.stamp = get_node()->get_clock()->now(); - }); - } - - // initialize odometry publisher and messasge - odometry_publisher_ = get_node()->create_publisher( - DEFAULT_ODOMETRY_TOPIC, rclcpp::SystemDefaultsQoS()); - realtime_odometry_publisher_ = - std::make_shared>( - odometry_publisher_); - - auto & odometry_message = realtime_odometry_publisher_->msg_; - odometry_message.header.frame_id = odom_params_.odom_frame_id; - odometry_message.child_frame_id = odom_params_.base_frame_id; - - previous_publish_timestamp_ = get_node()->get_clock()->now(); - - // initialize odom values zeros - odometry_message.twist = - geometry_msgs::msg::TwistWithCovariance(rosidl_runtime_cpp::MessageInitialization::ALL); - - constexpr size_t NUM_DIMENSIONS = 6; - for (size_t index = 0; index < 6; ++index) - { - // 0, 7, 14, 21, 28, 35 - const size_t diagonal_index = NUM_DIMENSIONS * index + index; - odometry_message.pose.covariance[diagonal_index] = odom_params_.pose_covariance_diagonal[index]; - odometry_message.twist.covariance[diagonal_index] = - odom_params_.twist_covariance_diagonal[index]; - } - - // initialize transform publisher and message - if (odom_params_.enable_odom_tf) - { - odometry_transform_publisher_ = get_node()->create_publisher( - DEFAULT_TRANSFORM_TOPIC, rclcpp::SystemDefaultsQoS()); - realtime_odometry_transform_publisher_ = - std::make_shared>( - odometry_transform_publisher_); - - // keeping track of odom and base_link transforms only - auto & odometry_transform_message = realtime_odometry_transform_publisher_->msg_; - odometry_transform_message.transforms.resize(1); - odometry_transform_message.transforms.front().header.frame_id = odom_params_.odom_frame_id; - odometry_transform_message.transforms.front().child_frame_id = odom_params_.base_frame_id; - } - - // Create odom reset service - reset_odom_service_ = get_node()->create_service( - DEFAULT_RESET_ODOM_SERVICE, std::bind( - &TricycleController::reset_odometry, this, std::placeholders::_1, - std::placeholders::_2, std::placeholders::_3)); - - return CallbackReturn::SUCCESS; -} - -CallbackReturn TricycleController::on_activate(const rclcpp_lifecycle::State &) -{ - RCLCPP_INFO(get_node()->get_logger(), "On activate: Initialize Joints"); - - // Initialize the joints - const auto wheel_front_result = get_traction(traction_joint_name_, traction_joint_); - const auto steering_result = get_steering(steering_joint_name_, steering_joint_); - if (wheel_front_result == CallbackReturn::ERROR || steering_result == CallbackReturn::ERROR) - { - return CallbackReturn::ERROR; - } - if (traction_joint_.empty() || steering_joint_.empty()) - { - RCLCPP_ERROR( - get_node()->get_logger(), "Either steering or traction interfaces are non existent"); - return CallbackReturn::ERROR; - } - - is_halted = false; - subscriber_is_active_ = true; - - RCLCPP_DEBUG(get_node()->get_logger(), "Subscriber and publisher are now active."); - return CallbackReturn::SUCCESS; -} - -CallbackReturn TricycleController::on_deactivate(const rclcpp_lifecycle::State &) -{ - subscriber_is_active_ = false; - return CallbackReturn::SUCCESS; -} - -CallbackReturn TricycleController::on_cleanup(const rclcpp_lifecycle::State &) -{ - if (!reset()) - { - return CallbackReturn::ERROR; - } - - received_velocity_msg_ptr_.set(std::make_shared()); - return CallbackReturn::SUCCESS; -} - -CallbackReturn TricycleController::on_error(const rclcpp_lifecycle::State &) -{ - if (!reset()) - { - return CallbackReturn::ERROR; - } - return CallbackReturn::SUCCESS; -} - -void TricycleController::reset_odometry( - const std::shared_ptr /*request_header*/, - const std::shared_ptr /*req*/, - std::shared_ptr /*res*/) -{ - odometry_.reset_odometry(); - RCLCPP_INFO(get_node()->get_logger(), "Odometry successfully reset"); -} - -bool TricycleController::reset() -{ - odometry_.reset_odometry(); - - // release the old queue - std::queue empty_ackermann_drive; - std::swap(previous_commands_, empty_ackermann_drive); - - traction_joint_.clear(); - steering_joint_.clear(); - - subscriber_is_active_ = false; - velocity_command_subscriber_.reset(); - velocity_command_unstamped_subscriber_.reset(); - - received_velocity_msg_ptr_.set(nullptr); - is_halted = false; - return true; -} - -CallbackReturn TricycleController::on_shutdown(const rclcpp_lifecycle::State &) -{ - return CallbackReturn::SUCCESS; -} - -void TricycleController::halt() -{ - traction_joint_[0].velocity_command.get().set_value(0.0); - steering_joint_[0].position_command.get().set_value(0.0); -} - -CallbackReturn TricycleController::get_traction( - const std::string & traction_joint_name, std::vector & joint) -{ - RCLCPP_INFO(get_node()->get_logger(), "Get Wheel Joint Instance"); - - // Lookup the velocity state interface - const auto state_handle = std::find_if( - state_interfaces_.cbegin(), state_interfaces_.cend(), - [&traction_joint_name](const auto & interface) - { - return interface.get_prefix_name() == traction_joint_name && - interface.get_interface_name() == HW_IF_VELOCITY; - }); - if (state_handle == state_interfaces_.cend()) - { - RCLCPP_ERROR( - get_node()->get_logger(), "Unable to obtain joint state handle for %s", - traction_joint_name.c_str()); - return CallbackReturn::ERROR; - } - - // Lookup the velocity command interface - const auto command_handle = std::find_if( - command_interfaces_.begin(), command_interfaces_.end(), - [&traction_joint_name](const auto & interface) - { - return interface.get_prefix_name() == traction_joint_name && - interface.get_interface_name() == HW_IF_VELOCITY; - }); - if (command_handle == command_interfaces_.end()) - { - RCLCPP_ERROR( - get_node()->get_logger(), "Unable to obtain joint state handle for %s", - traction_joint_name.c_str()); - return CallbackReturn::ERROR; - } - - // Create the traction joint instance - joint.emplace_back(TractionHandle{std::ref(*state_handle), std::ref(*command_handle)}); - return CallbackReturn::SUCCESS; -} - -CallbackReturn TricycleController::get_steering( - const std::string & steering_joint_name, std::vector & joint) -{ - RCLCPP_INFO(get_node()->get_logger(), "Get Steering Joint Instance"); - - // Lookup the velocity state interface - const auto state_handle = std::find_if( - state_interfaces_.cbegin(), state_interfaces_.cend(), - [&steering_joint_name](const auto & interface) - { - return interface.get_prefix_name() == steering_joint_name && - interface.get_interface_name() == HW_IF_POSITION; - }); - if (state_handle == state_interfaces_.cend()) - { - RCLCPP_ERROR( - get_node()->get_logger(), "Unable to obtain joint state handle for %s", - steering_joint_name.c_str()); - return CallbackReturn::ERROR; - } - - // Lookup the velocity command interface - const auto command_handle = std::find_if( - command_interfaces_.begin(), command_interfaces_.end(), - [&steering_joint_name](const auto & interface) - { - return interface.get_prefix_name() == steering_joint_name && - interface.get_interface_name() == HW_IF_POSITION; - }); - if (command_handle == command_interfaces_.end()) - { - RCLCPP_ERROR( - get_node()->get_logger(), "Unable to obtain joint state handle for %s", - steering_joint_name.c_str()); - return CallbackReturn::ERROR; - } - - // Create the steering joint instance - joint.emplace_back(SteeringHandle{std::ref(*state_handle), std::ref(*command_handle)}); - return CallbackReturn::SUCCESS; -} - -} // namespace tricycle_controller - -#include -PLUGINLIB_EXPORT_CLASS( - tricycle_controller::TricycleController, controller_interface::ControllerInterface) diff --git a/steering_controllers/tricycle_controller/test/config/test_tricycle_controller.yaml b/steering_controllers/tricycle_controller/test/config/test_tricycle_controller.yaml deleted file mode 100644 index efecfe968f..0000000000 --- a/steering_controllers/tricycle_controller/test/config/test_tricycle_controller.yaml +++ /dev/null @@ -1,20 +0,0 @@ - -test_tricycle_controller: - ros__parameters: - traction_joint_name: traction_joint - steering_joint_name: steering_joint - wheel_radius: 0.125 - wheelbase: 1.252 - use_stamped_vel: false - enable_odom_tf: false - use_sim_time: false - pose_covariance_diagonal: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - twist_covariance_diagonal: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - odom_only_twist: true - publish_ackermann_command: true - traction: - max_acceleration: 5.0 - max_deceleration: 8.0 - steering: - max_position: 1.57 # pi/2 - max_velocity: 1.0 diff --git a/steering_controllers/tricycle_controller/test/test_load_tricycle_controller.cpp b/steering_controllers/tricycle_controller/test/test_load_tricycle_controller.cpp deleted file mode 100644 index d04b83ae27..0000000000 --- a/steering_controllers/tricycle_controller/test/test_load_tricycle_controller.cpp +++ /dev/null @@ -1,43 +0,0 @@ -// Copyright 2022 Pixel Robotics. -// -// 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. - -/* - * Author: Tony Najjar - */ - -#include -#include - -#include "controller_manager/controller_manager.hpp" -#include "hardware_interface/resource_manager.hpp" -#include "rclcpp/executors/single_threaded_executor.hpp" -#include "ros2_control_test_assets/descriptions.hpp" - -TEST(TestLoadTricycleController, 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_tricycle_controller", "tricycle_controller/TricycleController")); - - rclcpp::shutdown(); -} diff --git a/steering_controllers/tricycle_controller/test/test_tricycle_controller.cpp b/steering_controllers/tricycle_controller/test/test_tricycle_controller.cpp deleted file mode 100644 index a1f09ddaf8..0000000000 --- a/steering_controllers/tricycle_controller/test/test_tricycle_controller.cpp +++ /dev/null @@ -1,350 +0,0 @@ -// Copyright 2022 Pixel Robotics. -// -// 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. - -/* - * Author: Tony Najjar - */ - -#include - -#include -#include -#include -#include -#include -#include - -#include "hardware_interface/loaned_command_interface.hpp" -#include "hardware_interface/loaned_state_interface.hpp" -#include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "lifecycle_msgs/msg/state.hpp" -#include "rclcpp/rclcpp.hpp" -#include "tricycle_controller/tricycle_controller.hpp" - -using CallbackReturn = controller_interface::CallbackReturn; -using hardware_interface::HW_IF_POSITION; -using hardware_interface::HW_IF_VELOCITY; -using hardware_interface::LoanedCommandInterface; -using hardware_interface::LoanedStateInterface; -using lifecycle_msgs::msg::State; -using testing::SizeIs; - -class TestableTricycleController : public tricycle_controller::TricycleController -{ -public: - using TricycleController::TricycleController; - std::shared_ptr getLastReceivedTwist() - { - std::shared_ptr ret; - received_velocity_msg_ptr_.get(ret); - return ret; - } - - /** - * @brief wait_for_twist block until a new twist is received. - * Requires that the executor is not spinned elsewhere between the - * message publication and the call to this function - * - * @return true if new twist msg was received, false if timeout - */ - bool wait_for_twist( - rclcpp::Executor & executor, - const std::chrono::milliseconds & timeout = std::chrono::milliseconds(500)) - { - rclcpp::WaitSet wait_set; - wait_set.add_subscription(velocity_command_subscriber_); - - if (wait_set.wait(timeout).kind() == rclcpp::WaitResultKind::Ready) - { - executor.spin_some(); - return true; - } - return false; - } -}; - -class TestTricycleController : public ::testing::Test -{ -protected: - static void SetUpTestCase() { rclcpp::init(0, nullptr); } - - void SetUp() override - { - controller_ = std::make_unique(); - pub_node = std::make_shared("velocity_publisher"); - velocity_publisher = pub_node->create_publisher( - controller_name + "/cmd_vel", rclcpp::SystemDefaultsQoS()); - } - - static void TearDownTestCase() { rclcpp::shutdown(); } - - /// Publish velocity msgs - /** - * linear - magnitude of the linear command in the geometry_msgs::twist message - * angular - the magnitude of the angular command in geometry_msgs::twist message - */ - void publish(double linear, double angular) - { - int wait_count = 0; - auto topic = velocity_publisher->get_topic_name(); - while (pub_node->count_subscribers(topic) == 0) - { - if (wait_count >= 5) - { - auto error_msg = std::string("publishing to ") + topic + " but no node subscribes to it"; - throw std::runtime_error(error_msg); - } - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - ++wait_count; - } - - geometry_msgs::msg::TwistStamped velocity_message; - velocity_message.header.stamp = pub_node->get_clock()->now(); - velocity_message.twist.linear.x = linear; - velocity_message.twist.angular.z = angular; - velocity_publisher->publish(velocity_message); - } - - /// \brief wait for the subscriber and publisher to completely setup - void waitForSetup() - { - constexpr std::chrono::seconds TIMEOUT{2}; - auto clock = pub_node->get_clock(); - auto start = clock->now(); - while (velocity_publisher->get_subscription_count() <= 0) - { - if ((clock->now() - start) > TIMEOUT) - { - FAIL(); - } - rclcpp::spin_some(pub_node); - } - } - - void assignResources() - { - std::vector state_ifs; - state_ifs.emplace_back(steering_joint_pos_state_); - state_ifs.emplace_back(traction_joint_vel_state_); - - std::vector command_ifs; - command_ifs.emplace_back(steering_joint_pos_cmd_); - command_ifs.emplace_back(traction_joint_vel_cmd_); - - controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); - } - - const std::string controller_name = "test_tricycle_controller"; - std::unique_ptr controller_; - - const std::string traction_joint_name = "traction_joint"; - const std::string steering_joint_name = "steering_joint"; - double position_ = 0.1; - double velocity_ = 0.2; - - hardware_interface::StateInterface steering_joint_pos_state_{ - steering_joint_name, HW_IF_POSITION, &position_}; - - hardware_interface::StateInterface traction_joint_vel_state_{ - traction_joint_name, HW_IF_VELOCITY, &velocity_}; - - hardware_interface::CommandInterface steering_joint_pos_cmd_{ - steering_joint_name, HW_IF_POSITION, &position_}; - - hardware_interface::CommandInterface traction_joint_vel_cmd_{ - traction_joint_name, HW_IF_VELOCITY, &velocity_}; - - rclcpp::Node::SharedPtr pub_node; - rclcpp::Publisher::SharedPtr velocity_publisher; -}; - -TEST_F(TestTricycleController, configure_fails_without_parameters) -{ - const auto ret = controller_->init(controller_name); - ASSERT_EQ(ret, controller_interface::return_type::OK); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); -} - -TEST_F(TestTricycleController, configure_fails_if_only_traction_or_steering_side_defined) -{ - const auto ret = controller_->init(controller_name); - ASSERT_EQ(ret, controller_interface::return_type::OK); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("traction_joint_name", rclcpp::ParameterValue(traction_joint_name))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("steering_joint_name", rclcpp::ParameterValue(std::string()))); - - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("traction_joint_name", rclcpp::ParameterValue(std::string()))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("steering_joint_name", rclcpp::ParameterValue(steering_joint_name))); - - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); -} - -TEST_F(TestTricycleController, configure_succeeds_when_joints_are_specified) -{ - const auto ret = controller_->init(controller_name); - ASSERT_EQ(ret, controller_interface::return_type::OK); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("traction_joint_name", rclcpp::ParameterValue(traction_joint_name))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("steering_joint_name", rclcpp::ParameterValue(steering_joint_name))); - - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); -} - -TEST_F(TestTricycleController, activate_fails_without_resources_assigned) -{ - const auto ret = controller_->init(controller_name); - ASSERT_EQ(ret, controller_interface::return_type::OK); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("traction_joint_name", rclcpp::ParameterValue(traction_joint_name))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("steering_joint_name", rclcpp::ParameterValue(steering_joint_name))); - - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::ERROR); -} - -TEST_F(TestTricycleController, activate_succeeds_with_resources_assigned) -{ - const auto ret = controller_->init(controller_name); - ASSERT_EQ(ret, controller_interface::return_type::OK); - - // We implicitly test that by default position feedback is required - controller_->get_node()->set_parameter( - rclcpp::Parameter("traction_joint_name", rclcpp::ParameterValue(traction_joint_name))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("steering_joint_name", rclcpp::ParameterValue(steering_joint_name))); - - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - assignResources(); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); -} - -TEST_F(TestTricycleController, cleanup) -{ - const auto ret = controller_->init(controller_name); - ASSERT_EQ(ret, controller_interface::return_type::OK); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("traction_joint_name", rclcpp::ParameterValue(traction_joint_name))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("steering_joint_name", rclcpp::ParameterValue(steering_joint_name))); - controller_->get_node()->set_parameter(rclcpp::Parameter("wheelbase", 1.2)); - controller_->get_node()->set_parameter(rclcpp::Parameter("wheel_radius", 0.12)); - - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(controller_->get_node()->get_node_base_interface()); - auto state = controller_->get_node()->configure(); - ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); - assignResources(); - - state = controller_->get_node()->activate(); - ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); - - waitForSetup(); - - // send msg - const double linear = 1.0; - const double angular = 1.0; - publish(linear, angular); - controller_->wait_for_twist(executor); - - ASSERT_EQ( - controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - - state = controller_->get_node()->deactivate(); - ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); - ASSERT_EQ( - controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - - state = controller_->get_node()->cleanup(); - ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); - - // should be stopped - EXPECT_EQ(0.0, steering_joint_pos_cmd_.get_value()); - EXPECT_EQ(0.0, traction_joint_vel_cmd_.get_value()); - - executor.cancel(); -} - -TEST_F(TestTricycleController, correct_initialization_using_parameters) -{ - const auto ret = controller_->init(controller_name); - ASSERT_EQ(ret, controller_interface::return_type::OK); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("traction_joint_name", rclcpp::ParameterValue(traction_joint_name))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("steering_joint_name", rclcpp::ParameterValue(steering_joint_name))); - controller_->get_node()->set_parameter(rclcpp::Parameter("wheelbase", 0.4)); - controller_->get_node()->set_parameter(rclcpp::Parameter("wheel_radius", 1.0)); - - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(controller_->get_node()->get_node_base_interface()); - - auto state = controller_->get_node()->configure(); - assignResources(); - - ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); - EXPECT_EQ(position_, steering_joint_pos_cmd_.get_value()); - EXPECT_EQ(velocity_, traction_joint_vel_cmd_.get_value()); - - state = controller_->get_node()->activate(); - ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); - - // send msg - const double linear = 1.0; - const double angular = 0.0; - publish(linear, angular); - // wait for msg is be published to the system - ASSERT_TRUE(controller_->wait_for_twist(executor)); - - ASSERT_EQ( - controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - EXPECT_EQ(0.0, steering_joint_pos_cmd_.get_value()); - EXPECT_EQ(1.0, traction_joint_vel_cmd_.get_value()); - - // deactivated - // wait so controller process the second point when deactivated - std::this_thread::sleep_for(std::chrono::milliseconds(500)); - state = controller_->get_node()->deactivate(); - ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); - ASSERT_EQ( - controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - - EXPECT_EQ(0.0, steering_joint_pos_cmd_.get_value()) << "Wheels are halted on deactivate()"; - EXPECT_EQ(0.0, traction_joint_vel_cmd_.get_value()) << "Wheels are halted on deactivate()"; - - // cleanup - state = controller_->get_node()->cleanup(); - ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); - EXPECT_EQ(0.0, steering_joint_pos_cmd_.get_value()); - EXPECT_EQ(0.0, traction_joint_vel_cmd_.get_value()); - - state = controller_->get_node()->configure(); - ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); - executor.cancel(); -} diff --git a/steering_controllers/tricycle_controller/tricycle_controller.xml b/steering_controllers/tricycle_controller/tricycle_controller.xml deleted file mode 100644 index 568b13532b..0000000000 --- a/steering_controllers/tricycle_controller/tricycle_controller.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - - The tricycle controller transforms linear and angular velocity messages into signals for steering and traction joints for a tricycle drive robot. - - - From 6a874313f682347fc5e5233efdbf69bde1cd0a04 Mon Sep 17 00:00:00 2001 From: petkovich Date: Mon, 19 Dec 2022 12:36:20 +0100 Subject: [PATCH 069/186] removed subfolder --- steering_controllers/{steering_controllers => }/CMakeLists.txt | 0 .../steering_controllers/steering_controller_implementations.hpp | 0 .../include/steering_controllers/steering_controllers.hpp | 0 .../include/steering_controllers/steering_odometry.hpp | 0 .../include/steering_controllers/visibility_control.h | 0 steering_controllers/{steering_controllers => }/package.xml | 0 .../src/steering_controller_implementations.cpp | 0 .../{steering_controllers => }/src/steering_controllers.cpp | 0 .../{steering_controllers => }/src/steering_controllers.yaml | 0 .../{steering_controllers => }/src/steering_odometry.cpp | 0 .../{steering_controllers => }/steering_controllers.xml | 0 11 files changed, 0 insertions(+), 0 deletions(-) rename steering_controllers/{steering_controllers => }/CMakeLists.txt (100%) rename steering_controllers/{steering_controllers => }/include/steering_controllers/steering_controller_implementations.hpp (100%) rename steering_controllers/{steering_controllers => }/include/steering_controllers/steering_controllers.hpp (100%) rename steering_controllers/{steering_controllers => }/include/steering_controllers/steering_odometry.hpp (100%) rename steering_controllers/{steering_controllers => }/include/steering_controllers/visibility_control.h (100%) rename steering_controllers/{steering_controllers => }/package.xml (100%) rename steering_controllers/{steering_controllers => }/src/steering_controller_implementations.cpp (100%) rename steering_controllers/{steering_controllers => }/src/steering_controllers.cpp (100%) rename steering_controllers/{steering_controllers => }/src/steering_controllers.yaml (100%) rename steering_controllers/{steering_controllers => }/src/steering_odometry.cpp (100%) rename steering_controllers/{steering_controllers => }/steering_controllers.xml (100%) diff --git a/steering_controllers/steering_controllers/CMakeLists.txt b/steering_controllers/CMakeLists.txt similarity index 100% rename from steering_controllers/steering_controllers/CMakeLists.txt rename to steering_controllers/CMakeLists.txt diff --git a/steering_controllers/steering_controllers/include/steering_controllers/steering_controller_implementations.hpp b/steering_controllers/include/steering_controllers/steering_controller_implementations.hpp similarity index 100% rename from steering_controllers/steering_controllers/include/steering_controllers/steering_controller_implementations.hpp rename to steering_controllers/include/steering_controllers/steering_controller_implementations.hpp diff --git a/steering_controllers/steering_controllers/include/steering_controllers/steering_controllers.hpp b/steering_controllers/include/steering_controllers/steering_controllers.hpp similarity index 100% rename from steering_controllers/steering_controllers/include/steering_controllers/steering_controllers.hpp rename to steering_controllers/include/steering_controllers/steering_controllers.hpp diff --git a/steering_controllers/steering_controllers/include/steering_controllers/steering_odometry.hpp b/steering_controllers/include/steering_controllers/steering_odometry.hpp similarity index 100% rename from steering_controllers/steering_controllers/include/steering_controllers/steering_odometry.hpp rename to steering_controllers/include/steering_controllers/steering_odometry.hpp diff --git a/steering_controllers/steering_controllers/include/steering_controllers/visibility_control.h b/steering_controllers/include/steering_controllers/visibility_control.h similarity index 100% rename from steering_controllers/steering_controllers/include/steering_controllers/visibility_control.h rename to steering_controllers/include/steering_controllers/visibility_control.h diff --git a/steering_controllers/steering_controllers/package.xml b/steering_controllers/package.xml similarity index 100% rename from steering_controllers/steering_controllers/package.xml rename to steering_controllers/package.xml diff --git a/steering_controllers/steering_controllers/src/steering_controller_implementations.cpp b/steering_controllers/src/steering_controller_implementations.cpp similarity index 100% rename from steering_controllers/steering_controllers/src/steering_controller_implementations.cpp rename to steering_controllers/src/steering_controller_implementations.cpp diff --git a/steering_controllers/steering_controllers/src/steering_controllers.cpp b/steering_controllers/src/steering_controllers.cpp similarity index 100% rename from steering_controllers/steering_controllers/src/steering_controllers.cpp rename to steering_controllers/src/steering_controllers.cpp diff --git a/steering_controllers/steering_controllers/src/steering_controllers.yaml b/steering_controllers/src/steering_controllers.yaml similarity index 100% rename from steering_controllers/steering_controllers/src/steering_controllers.yaml rename to steering_controllers/src/steering_controllers.yaml diff --git a/steering_controllers/steering_controllers/src/steering_odometry.cpp b/steering_controllers/src/steering_odometry.cpp similarity index 100% rename from steering_controllers/steering_controllers/src/steering_odometry.cpp rename to steering_controllers/src/steering_odometry.cpp diff --git a/steering_controllers/steering_controllers/steering_controllers.xml b/steering_controllers/steering_controllers.xml similarity index 100% rename from steering_controllers/steering_controllers/steering_controllers.xml rename to steering_controllers/steering_controllers.xml From c95c7bbf8b12ca78a4bd4fee94f871c7eb1efa0b Mon Sep 17 00:00:00 2001 From: petkovich Date: Mon, 19 Dec 2022 12:55:52 +0100 Subject: [PATCH 070/186] returned configure odometry implementation that returns error, protected set intefrace number --- .../include/steering_controllers/steering_controllers.hpp | 6 +++--- .../include/steering_controllers/visibility_control.h | 1 + steering_controllers/src/steering_controllers.cpp | 6 ++++++ 3 files changed, 10 insertions(+), 3 deletions(-) diff --git a/steering_controllers/include/steering_controllers/steering_controllers.hpp b/steering_controllers/include/steering_controllers/steering_controllers.hpp index 39acab7314..9e7e41c0c3 100644 --- a/steering_controllers/include/steering_controllers/steering_controllers.hpp +++ b/steering_controllers/include/steering_controllers/steering_controllers.hpp @@ -57,9 +57,6 @@ class SteeringControllers : public controller_interface::ChainableControllerInte STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::InterfaceConfiguration state_interface_configuration() const override; - STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::CallbackReturn - set_interface_numbers(size_t nr_state_itfs, size_t nr_cmd_itfs, size_t nr_ref_itfs); - virtual STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::CallbackReturn configure_odometry(); @@ -124,6 +121,9 @@ class SteeringControllers : public controller_interface::ChainableControllerInte // name constants for reference interfaces size_t nr_ref_itfs_; + STEERING_CONTROLLERS__VISIBILITY_PROTECTED controller_interface::CallbackReturn + set_interface_numbers(size_t nr_state_itfs, size_t nr_cmd_itfs, size_t nr_ref_itfs); + private: // callback for topic interface STEERING_CONTROLLERS__VISIBILITY_LOCAL void reference_callback( diff --git a/steering_controllers/include/steering_controllers/visibility_control.h b/steering_controllers/include/steering_controllers/visibility_control.h index 415a139440..3601423cae 100644 --- a/steering_controllers/include/steering_controllers/visibility_control.h +++ b/steering_controllers/include/steering_controllers/visibility_control.h @@ -38,6 +38,7 @@ #define STEERING_CONTROLLERS__VISIBILITY_IMPORT #if __GNUC__ >= 4 #define STEERING_CONTROLLERS__VISIBILITY_PUBLIC __attribute__((visibility("default"))) +#define STEERING_CONTROLLERS__VISIBILITY_PROTECTED __attribute__((visibility("protected"))) #define STEERING_CONTROLLERS__VISIBILITY_LOCAL __attribute__((visibility("hidden"))) #else #define STEERING_CONTROLLERS__VISIBILITY_PUBLIC diff --git a/steering_controllers/src/steering_controllers.cpp b/steering_controllers/src/steering_controllers.cpp index 5d1555c4f8..713ab25431 100644 --- a/steering_controllers/src/steering_controllers.cpp +++ b/steering_controllers/src/steering_controllers.cpp @@ -88,6 +88,12 @@ controller_interface::CallbackReturn SteeringControllers::set_interface_numbers( nr_ref_itfs_ = nr_ref_itfs; } +controller_interface::CallbackReturn SteeringControllers::configure_odometry() +{ + RCLCPP_INFO(get_node()->get_logger(), "Configure odometry not implemented"); + return controller_interface::CallbackReturn::ERROR; +} + controller_interface::CallbackReturn SteeringControllers::on_configure( const rclcpp_lifecycle::State & /*previous_state*/) { From 8904ec2fdbdb73fa9ef17c1701ce928eeffbf809 Mon Sep 17 00:00:00 2001 From: petkovich Date: Mon, 19 Dec 2022 19:05:55 +0100 Subject: [PATCH 071/186] odometry update implementations for bicycle and tricycle, integration and accumulators in a new function --- .../steering_controller_implementations.hpp | 5 + .../steering_controllers.hpp | 11 ++- .../steering_odometry.hpp | 33 +++++-- .../steering_controller_implementations.cpp | 62 +++++++++++- .../src/steering_controllers.cpp | 30 ++---- .../src/steering_odometry.cpp | 97 ++++++++++++------- 6 files changed, 168 insertions(+), 70 deletions(-) diff --git a/steering_controllers/include/steering_controllers/steering_controller_implementations.hpp b/steering_controllers/include/steering_controllers/steering_controller_implementations.hpp index ed204a4047..617079b0e7 100644 --- a/steering_controllers/include/steering_controllers/steering_controller_implementations.hpp +++ b/steering_controllers/include/steering_controllers/steering_controller_implementations.hpp @@ -38,6 +38,9 @@ class BicycleSteeringController : public steering_controllers::SteeringControlle STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::CallbackReturn configure_odometry() override; + + STEERING_CONTROLLERS__VISIBILITY_PUBLIC bool update_odometry( + const rclcpp::Duration & period) override; }; } // namespace bicycle_steering_controller @@ -50,6 +53,8 @@ class TricycleSteeringController : public steering_controllers::SteeringControll STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::CallbackReturn configure_odometry() override; + STEERING_CONTROLLERS__VISIBILITY_PUBLIC bool update_odometry( + const rclcpp::Duration & period) override; }; } // namespace tricycle_steering_controller diff --git a/steering_controllers/include/steering_controllers/steering_controllers.hpp b/steering_controllers/include/steering_controllers/steering_controllers.hpp index 9e7e41c0c3..093c9aa6f3 100644 --- a/steering_controllers/include/steering_controllers/steering_controllers.hpp +++ b/steering_controllers/include/steering_controllers/steering_controllers.hpp @@ -60,6 +60,9 @@ class SteeringControllers : public controller_interface::ChainableControllerInte virtual STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::CallbackReturn configure_odometry(); + virtual STEERING_CONTROLLERS__VISIBILITY_PUBLIC bool update_odometry( + const rclcpp::Duration & period); + STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::CallbackReturn on_configure( const rclcpp_lifecycle::State & previous_state) override; @@ -124,15 +127,15 @@ class SteeringControllers : public controller_interface::ChainableControllerInte STEERING_CONTROLLERS__VISIBILITY_PROTECTED controller_interface::CallbackReturn set_interface_numbers(size_t nr_state_itfs, size_t nr_cmd_itfs, size_t nr_ref_itfs); + // store last velocity + double last_linear_velocity_ = 0.0; + double last_angular_velocity_ = 0.0; + private: // callback for topic interface STEERING_CONTROLLERS__VISIBILITY_LOCAL void reference_callback( const std::shared_ptr msg); void reference_callback_unstamped(const std::shared_ptr msg); - - // store last velocity - double last_linear_velocity_ = 0.0; - double last_angular_velocity_ = 0.0; }; } // namespace steering_controllers diff --git a/steering_controllers/include/steering_controllers/steering_odometry.hpp b/steering_controllers/include/steering_controllers/steering_odometry.hpp index f037646df9..2adb76fc6b 100644 --- a/steering_controllers/include/steering_controllers/steering_odometry.hpp +++ b/steering_controllers/include/steering_controllers/steering_odometry.hpp @@ -59,9 +59,14 @@ class SteeringOdometry * \param dt time difference to last call * \return true if the odometry is actually updated */ + bool update_from_position( const double rear_wheel_pos, const double front_steer_pos, const double dt); + bool update_from_position( + const double rear_right_wheel_pos, const double rear_left_wheel_pos, + const double front_steer_pos, const double dt); + /** * \brief Updates the odometry class with latest wheels position * \param rear_wheel_vel Rear wheel velocity [rad/s] @@ -72,6 +77,10 @@ class SteeringOdometry bool update_from_velocity( const double rear_wheel_vel, const double front_steer_pos, const double dt); + bool update_from_velocity( + const double rear_right_wheel_vel, const double rear_left_wheel_vel, + const double front_steer_pos, const double dt); + /** * \brief Updates the odometry class with latest velocity command * \param linear Linear velocity [m/s] @@ -122,13 +131,6 @@ class SteeringOdometry */ void set_velocity_rolling_window_size(size_t velocity_rolling_window_size); - /** - * \brief TODO - * \param Vx TODO - * \param theta_dot TODO - */ - double convert_trans_rot_vel_to_steering_angle(double Vx, double theta_dot); - /** * \brief TODO * \param Vx TODO @@ -142,6 +144,13 @@ class SteeringOdometry void reset_odometry(); private: + /** + * \brief Uses precomputed linear and angular velocities to compute dometry and update accumulators + * \param linear Linear velocity [m] (linear displacement, i.e. m/s * dt) computed by previous odometry method + * \param angular Angular velocity [rad] (angular displacement, i.e. m/s * dt) computed by previous odometry method + */ + bool update_odometry(const double linear_velocity, const double angular, const double dt); + /** * \brief Integrates the velocities (linear and angular) using 2nd order Runge-Kutta * \param linear Linear velocity [m] (linear displacement, i.e. m/s * dt) computed by encoders @@ -156,6 +165,13 @@ class SteeringOdometry */ void integrate_exact(double linear, double angular); + /** + * \brief TODO + * \param Vx TODO + * \param theta_dot TODO + */ + double convert_trans_rot_vel_to_steering_angle(double Vx, double theta_dot); + /** * \brief Reset linear and angular accumulators */ @@ -179,7 +195,8 @@ class SteeringOdometry /// Previous wheel position/state [rad]: double rear_wheel_old_pos_; - + double rear_right_wheel_old_pos_; + double rear_left_wheel_old_pos_; /// Rolling mean accumulators for the linar and angular velocities: size_t velocity_rolling_window_size_; rcpputils::RollingMeanAccumulator linear_acc_; diff --git a/steering_controllers/src/steering_controller_implementations.cpp b/steering_controllers/src/steering_controller_implementations.cpp index 9e101eab64..297f29b554 100644 --- a/steering_controllers/src/steering_controller_implementations.cpp +++ b/steering_controllers/src/steering_controller_implementations.cpp @@ -68,6 +68,33 @@ controller_interface::CallbackReturn BicycleSteeringController::configure_odomet RCLCPP_INFO(get_node()->get_logger(), "bicycle odom configure successful"); return controller_interface::CallbackReturn::SUCCESS; } + +bool BicycleSteeringController::update_odometry(const rclcpp::Duration & period) +{ + if (params_.open_loop) + { + odometry_.update_open_loop(last_linear_velocity_, last_angular_velocity_, period.seconds()); + } + else + { + const double rear_wheel_value = state_interfaces_[0].get_value(); + const double steer_position = state_interfaces_[1].get_value() * params_.steer_pos_multiplier; + if (!std::isnan(rear_wheel_value) && !std::isnan(steer_position)) + { + if (params_.position_feedback) + { + // Estimate linear and angular velocity using joint information + odometry_.update_from_position(rear_wheel_value, steer_position, period.seconds()); + } + else + { + // Estimate linear and angular velocity using joint information + odometry_.update_from_velocity(rear_wheel_value, steer_position, period.seconds()); + } + } + } + return true; +} } // namespace bicycle_steering_controller namespace tricycle_steering_controller @@ -89,7 +116,7 @@ controller_interface::CallbackReturn TricycleSteeringController::configure_odome // TODO: enable position/velocity configure const size_t nr_state_itfs = 3; - const size_t nr_cmd_itfs = 2; + const size_t nr_cmd_itfs = 3; const size_t nr_ref_itfs = 2; set_interface_numbers(nr_state_itfs, nr_cmd_itfs, nr_ref_itfs); @@ -97,6 +124,39 @@ controller_interface::CallbackReturn TricycleSteeringController::configure_odome RCLCPP_INFO(get_node()->get_logger(), "ackermann odom configure successful"); return controller_interface::CallbackReturn::SUCCESS; } + +bool TricycleSteeringController::update_odometry(const rclcpp::Duration & period) +{ + if (params_.open_loop) + { + odometry_.update_open_loop(last_linear_velocity_, last_angular_velocity_, period.seconds()); + } + else + { + const double rear_right_wheel_value = state_interfaces_[0].get_value(); + const double rear_left_wheel_value = state_interfaces_[1].get_value(); + const double steer_position = state_interfaces_[2].get_value() * params_.steer_pos_multiplier; + if ( + !std::isnan(rear_right_wheel_value) && !std::isnan(rear_left_wheel_value) && + !std::isnan(steer_position)) + { + if (params_.position_feedback) + { + // Estimate linear and angular velocity using joint information + odometry_.update_from_position( + rear_right_wheel_value, rear_left_wheel_value, steer_position, period.seconds()); + } + else + { + // Estimate linear and angular velocity using joint information + odometry_.update_from_velocity( + rear_right_wheel_value, rear_left_wheel_value, steer_position, period.seconds()); + } + } + } + return true; +} + } // namespace tricycle_steering_controller #include "pluginlib/class_list_macros.hpp" diff --git a/steering_controllers/src/steering_controllers.cpp b/steering_controllers/src/steering_controllers.cpp index 713ab25431..ff1dd8670d 100644 --- a/steering_controllers/src/steering_controllers.cpp +++ b/steering_controllers/src/steering_controllers.cpp @@ -388,32 +388,16 @@ controller_interface::return_type SteeringControllers::update_reference_from_sub return controller_interface::return_type::OK; } +bool SteeringControllers::update_odometry(const rclcpp::Duration & period) +{ + RCLCPP_INFO(get_node()->get_logger(), "Odometry update not implemented"); + return false; +} + controller_interface::return_type SteeringControllers::update_and_write_commands( const rclcpp::Time & time, const rclcpp::Duration & period) { - if (params_.open_loop) - { - odometry_.update_open_loop(last_linear_velocity_, last_angular_velocity_, period.seconds()); - } - else - { - const double rear_wheel_value = state_interfaces_[0].get_value(); - const double steer_position = state_interfaces_[1].get_value() * params_.steer_pos_multiplier; - - if (!std::isnan(rear_wheel_value) && !std::isnan(steer_position)) - { - if (params_.position_feedback) - { - // Estimate linear and angular velocity using joint information - odometry_.update_from_position(rear_wheel_value, steer_position, period.seconds()); - } - else - { - // Estimate linear and angular velocity using joint information - odometry_.update_from_velocity(rear_wheel_value, steer_position, period.seconds()); - } - } - } + update_odometry(period); // MOVE ROBOT diff --git a/steering_controllers/src/steering_odometry.cpp b/steering_controllers/src/steering_odometry.cpp index af461fb735..e10d32b4c6 100644 --- a/steering_controllers/src/steering_odometry.cpp +++ b/steering_controllers/src/steering_odometry.cpp @@ -50,72 +50,101 @@ void SteeringOdometry::init(const rclcpp::Time & time) timestamp_ = time; } +bool SteeringOdometry::update_odometry( + const double linear_velocity, const double angular, const double dt) +{ + /// Integrate odometry: + SteeringOdometry::integrate_exact(linear_velocity * dt, angular); + + /// We cannot estimate the speed with very small time intervals: + if (dt < 0.0001) + { + return false; // Interval too small to integrate with + } + + /// Estimate speeds using a rolling mean to filter them out: + linear_acc_.accumulate(linear_velocity); + angular_acc_.accumulate(angular / dt); + + linear_ = linear_acc_.getRollingMean(); + angular_ = angular_acc_.getRollingMean(); + + return true; +} + // TODO(destogl): enable also velocity interface to update using velocity from the rear wheel bool SteeringOdometry::update_from_position( const double rear_wheel_pos, const double front_steer_pos, const double dt) { /// Get current wheel joint positions: const double rear_wheel_cur_pos = rear_wheel_pos * wheel_radius_; - - /// Estimate velocity of wheels using old and current position: - //const double left_wheel_est_vel = left_wheel_cur_pos - left_wheel_old_pos_; - //const double right_wheel_est_vel = right_wheel_cur_pos - right_wheel_old_pos_; - const double rear_wheel_est_pos_diff = rear_wheel_cur_pos - rear_wheel_old_pos_; /// Update old position with current: rear_wheel_old_pos_ = rear_wheel_cur_pos; /// Compute linear and angular diff: - const double linear_velocity = - rear_wheel_est_pos_diff / dt; //(right_wheel_est_vel + left_wheel_est_vel) * 0.5; + const double linear_velocity = rear_wheel_est_pos_diff / dt; - //const double angular = (right_wheel_est_vel - left_wheel_est_vel) / wheel_separation_w_; const double angular = tan(front_steer_pos) * linear_velocity / wheel_separation_; - /// Integrate odometry: - SteeringOdometry::integrate_exact(linear_velocity * dt, angular); + update_odometry(linear_velocity, angular, dt); - /// We cannot estimate the speed with very small time intervals: - if (dt < 0.0001) - { - return false; // Interval too small to integrate with - } + return true; +} - /// Estimate speeds using a rolling mean to filter them out: - linear_acc_.accumulate(linear_velocity); - angular_acc_.accumulate(angular / dt); +bool SteeringOdometry::update_from_position( + const double rear_right_wheel_pos, const double rear_left_wheel_pos, const double front_steer_pos, + const double dt) +{ + /// Get current wheel joint positions: + const double rear_right_wheel_cur_pos = rear_right_wheel_pos * wheel_radius_; + const double rear_left_wheel_cur_pos = rear_left_wheel_cur_pos * wheel_radius_; - linear_ = linear_acc_.getRollingMean(); - angular_ = angular_acc_.getRollingMean(); + const double rear_right_wheel_est_pos_diff = rear_right_wheel_cur_pos - rear_right_wheel_old_pos_; + const double rear_left_wheel_est_pos_diff = rear_left_wheel_cur_pos - rear_left_wheel_old_pos_; + + /// Update old position with current: + rear_right_wheel_old_pos_ = rear_right_wheel_cur_pos; + rear_left_wheel_old_pos_ = rear_left_wheel_cur_pos; + + /// Compute linear and angular diff: + const double linear_velocity = + (rear_right_wheel_est_pos_diff + rear_left_wheel_est_pos_diff) * 0.5 / dt; + + //const double angular = (rear_right_wheel_est_pos_diff - rear_left_wheel_est_pos_diff) / wheel_separation_w_; + const double angular = tan(front_steer_pos) * linear_velocity / wheel_separation_; + + update_odometry(linear_velocity, angular, dt); return true; } bool SteeringOdometry::update_from_velocity( - const double rear_wheel_vel, const double front_steer_pos, const double dt) + const double rear_right_wheel_vel, const double rear_left_wheel_vel, const double front_steer_pos, + const double dt) { // (right_wheel_est_vel + left_wheel_est_vel) * 0.5; - double linear_velocity = rear_wheel_vel * wheel_radius_; + double linear_velocity = (rear_right_wheel_vel + rear_left_wheel_vel) * wheel_radius_ * 0.5; //const double angular = (right_wheel_est_vel - left_wheel_est_vel) / wheel_separation_w_; const double angular = tan(front_steer_pos) * linear_velocity / wheel_separation_; - /// Integrate odometry: - SteeringOdometry::integrate_exact(linear_velocity * dt, angular * dt); + update_odometry(linear_velocity, angular, dt); - /// We cannot estimate the speed with very small time intervals: - if (dt < 0.0001) - { - return false; // Interval too small to integrate with - } + return true; +} - /// Estimate speeds using a rolling mean to filter them out: - linear_acc_.accumulate(linear_velocity); - angular_acc_.accumulate(angular); +bool SteeringOdometry::update_from_velocity( + const double rear_wheel_vel, const double front_steer_pos, const double dt) +{ + // (right_wheel_est_vel + left_wheel_est_vel) * 0.5; + double linear_velocity = rear_wheel_vel * wheel_radius_; - linear_ = linear_acc_.getRollingMean(); - angular_ = angular_acc_.getRollingMean(); + //const double angular = (right_wheel_est_vel - left_wheel_est_vel) / wheel_separation_w_; + const double angular = tan(front_steer_pos) * linear_velocity / wheel_separation_; + + update_odometry(linear_velocity, angular, dt); return true; } From 267de9c98a84f597c8b82fd11519d4bf6fade715 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tomislav=20Petkovi=C4=87?= <51706321+petkovich@users.noreply.github.com> Date: Mon, 19 Dec 2022 22:43:52 +0100 Subject: [PATCH 072/186] Fix command_interface_configuration iterator --- steering_controllers/src/steering_controllers.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/steering_controllers/src/steering_controllers.cpp b/steering_controllers/src/steering_controllers.cpp index ff1dd8670d..60962a3d51 100644 --- a/steering_controllers/src/steering_controllers.cpp +++ b/steering_controllers/src/steering_controllers.cpp @@ -279,7 +279,7 @@ controller_interface::InterfaceConfiguration SteeringControllers::command_interf command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; command_interfaces_config.names.reserve(nr_cmd_itfs_); - for (size_t i = 0; i < params_.front_steer_names.size(); i++) + for (size_t i = 0; i < params_.rear_wheel_names.size(); i++) { command_interfaces_config.names.push_back( params_.rear_wheel_names[i] + "/" + hardware_interface::HW_IF_VELOCITY); From 9364caa4af91cc0d59c919008c789e69396306b9 Mon Sep 17 00:00:00 2001 From: petkovich Date: Tue, 20 Dec 2022 15:35:38 +0100 Subject: [PATCH 073/186] returned tricycle (in root folder) --- tricycle_controller/CHANGELOG.rst | 27 + tricycle_controller/CMakeLists.txt | 118 ++++ .../config/tricycle_drive_controller.yaml | 43 ++ tricycle_controller/doc/userdoc.rst | 24 + .../include/tricycle_controller/odometry.hpp | 75 ++ .../tricycle_controller/steering_limiter.hpp | 94 +++ .../tricycle_controller/traction_limiter.hpp | 102 +++ .../tricycle_controller.hpp | 181 +++++ .../tricycle_controller/visibility_control.h | 53 ++ tricycle_controller/package.xml | 36 + tricycle_controller/src/odometry.cpp | 121 ++++ tricycle_controller/src/steering_limiter.cpp | 117 ++++ tricycle_controller/src/traction_limiter.cpp | 142 ++++ .../src/tricycle_controller.cpp | 660 ++++++++++++++++++ .../test/config/test_tricycle_controller.yaml | 20 + .../test/test_load_tricycle_controller.cpp | 43 ++ .../test/test_tricycle_controller.cpp | 350 ++++++++++ tricycle_controller/tricycle_controller.xml | 7 + 18 files changed, 2213 insertions(+) create mode 100644 tricycle_controller/CHANGELOG.rst create mode 100644 tricycle_controller/CMakeLists.txt create mode 100644 tricycle_controller/config/tricycle_drive_controller.yaml create mode 100644 tricycle_controller/doc/userdoc.rst create mode 100644 tricycle_controller/include/tricycle_controller/odometry.hpp create mode 100644 tricycle_controller/include/tricycle_controller/steering_limiter.hpp create mode 100644 tricycle_controller/include/tricycle_controller/traction_limiter.hpp create mode 100644 tricycle_controller/include/tricycle_controller/tricycle_controller.hpp create mode 100644 tricycle_controller/include/tricycle_controller/visibility_control.h create mode 100644 tricycle_controller/package.xml create mode 100644 tricycle_controller/src/odometry.cpp create mode 100644 tricycle_controller/src/steering_limiter.cpp create mode 100644 tricycle_controller/src/traction_limiter.cpp create mode 100644 tricycle_controller/src/tricycle_controller.cpp create mode 100644 tricycle_controller/test/config/test_tricycle_controller.yaml create mode 100644 tricycle_controller/test/test_load_tricycle_controller.cpp create mode 100644 tricycle_controller/test/test_tricycle_controller.cpp create mode 100644 tricycle_controller/tricycle_controller.xml diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst new file mode 100644 index 0000000000..0739bf6446 --- /dev/null +++ b/tricycle_controller/CHANGELOG.rst @@ -0,0 +1,27 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package tricycle_controller +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +2.15.0 (2022-12-06) +------------------- +* [TricycleController] Removed “publish period” functionality ⏱ #abi-break #behavior-break (`#468 `_) +* Contributors: Robotgir, Denis Štogl + +2.14.0 (2022-11-18) +------------------- +* Include to fix compilation error on macOS (`#467 `_) +* Contributors: light-tech + +2.13.0 (2022-10-05) +------------------- + +2.12.0 (2022-09-01) +------------------- +* Fix formatting CI job (`#418 `_) +* Fix formatting because pre-commit was not running on CI for some time. (`#409 `_) +* Contributors: Denis Štogl, Tyler Weaver + +2.11.0 (2022-08-04) +------------------- +* Tricycle controller (`#345 `_) +* Contributors: Bence Magyar, Tony Najjar diff --git a/tricycle_controller/CMakeLists.txt b/tricycle_controller/CMakeLists.txt new file mode 100644 index 0000000000..c499bca983 --- /dev/null +++ b/tricycle_controller/CMakeLists.txt @@ -0,0 +1,118 @@ +cmake_minimum_required(VERSION 3.8) +project(tricycle_controller) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(controller_interface REQUIRED) +find_package(hardware_interface REQUIRED) +find_package(pluginlib REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(std_srvs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rcpputils REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(realtime_tools REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_msgs REQUIRED) +find_package(ackermann_msgs REQUIRED) + +add_library( + ${PROJECT_NAME} + SHARED + src/tricycle_controller.cpp + src/odometry.cpp + src/traction_limiter.cpp + src/steering_limiter.cpp +) +target_include_directories( + ${PROJECT_NAME} + PRIVATE + include +) + +ament_target_dependencies( + ${PROJECT_NAME} + ackermann_msgs + builtin_interfaces + controller_interface + geometry_msgs + hardware_interface + nav_msgs + std_srvs + pluginlib + rclcpp + rclcpp_lifecycle + rcpputils + realtime_tools + tf2 + tf2_msgs +) + +pluginlib_export_plugin_description_file(controller_interface tricycle_controller.xml) + +install(TARGETS ${PROJECT_NAME} + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib +) +install( + DIRECTORY + include/ + DESTINATION include +) + +if(BUILD_TESTING) + find_package(ament_cmake_gmock REQUIRED) + find_package(controller_manager REQUIRED) + find_package(ros2_control_test_assets REQUIRED) + + ament_add_gmock(test_tricycle_controller + test/test_tricycle_controller.cpp + ENV config_file=${CMAKE_CURRENT_SOURCE_DIR}/test/config/test_tricycle_controller.yaml) + target_include_directories(test_tricycle_controller PRIVATE include) + target_link_libraries(test_tricycle_controller + ${PROJECT_NAME} + ) + + ament_target_dependencies(test_tricycle_controller + geometry_msgs + hardware_interface + nav_msgs + rclcpp + rclcpp_lifecycle + realtime_tools + tf2 + tf2_msgs + ) + + ament_add_gmock( + test_load_tricycle_controller + test/test_load_tricycle_controller.cpp + ) + target_include_directories(test_load_tricycle_controller PRIVATE include) + ament_target_dependencies(test_load_tricycle_controller + controller_manager + ros2_control_test_assets + ) +endif() + +ament_export_include_directories( + include +) +ament_export_libraries( + ${PROJECT_NAME} +) +ament_export_dependencies( + controller_interface + geometry_msgs + hardware_interface + rclcpp + rclcpp_lifecycle +) + +ament_package() diff --git a/tricycle_controller/config/tricycle_drive_controller.yaml b/tricycle_controller/config/tricycle_drive_controller.yaml new file mode 100644 index 0000000000..c1077b13c3 --- /dev/null +++ b/tricycle_controller/config/tricycle_drive_controller.yaml @@ -0,0 +1,43 @@ +tricycle_controller: + ros__parameters: + # Model + traction_joint_name: traction_joint # Name of traction joint in URDF + steering_joint_name: steering_joint # Name of steering joint in URDF + wheel_radius: 0.0 # Radius of front wheel + wheelbase: 0.0 # Distance between center of back wheels and front wheel + + # Odometry + odom_frame_id: odom + base_frame_id: base_link + publish_rate: 50.0 # publish rate of odom and tf + open_loop: false # if True, uses cmd_vel instead of hardware interface feedback to compute odometry + enable_odom_tf: true # If True, publishes odom<-base_link TF + odom_only_twist: false # If True, publishes on /odom only linear.x and angular.z; Useful for computing odometry in another node, e.g robot_localization's ekf + pose_covariance_diagonal: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # Need to be set if fusing odom with other localization source + twist_covariance_diagonal: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # Need to be set if fusing odom with other localization source + velocity_rolling_window_size: 10 # Rolling window size of rcppmath::RollingMeanAccumulator applied on linear and angular speeds published on odom + + # Rate Limiting + traction: # All values should be positive + # min_velocity: 0.0 + # max_velocity: 1000.0 + # min_acceleration: 0.0 + max_acceleration: 5.0 + # min_deceleration: 0.0 + max_deceleration: 8.0 + # min_jerk: 0.0 + # max_jerk: 1000.0 + steering: + min_position: -1.57 + max_position: 1.57 + # min_velocity: 0.0 + max_velocity: 1.0 + # min_acceleration: 0.0 + # max_acceleration: 1000.0 + + # cmd_vel input + cmd_vel_timeout: 500 # In milliseconds. Timeout to stop if no cmd_vel is received + use_stamped_vel: false # Set to True if using TwistStamped. + + # Debug + publish_ackermann_command: true # Publishes AckermannDrive. The speed does not comply to the msg definition, it the wheel angular speed in rad/s. diff --git a/tricycle_controller/doc/userdoc.rst b/tricycle_controller/doc/userdoc.rst new file mode 100644 index 0000000000..a248bbec96 --- /dev/null +++ b/tricycle_controller/doc/userdoc.rst @@ -0,0 +1,24 @@ +.. _tricycle_controller_userdoc: + +tricycle_controller +===================== + +Controller for mobile robots with tricycle drive. +Input for control are robot base_link twist commands which are translated to traction and steering +commands for the tricycle drive base. Odometry is computed from hardware feedback and published. + +Velocity commands +----------------- + +The controller works with a velocity twist from which it extracts +the x component of the linear velocity and the z component of the angular velocity. +Velocities on other components are ignored. + + +Other features +-------------- + + Realtime-safe implementation. + Odometry publishing + Velocity, acceleration and jerk limits + Automatic stop after command timeout diff --git a/tricycle_controller/include/tricycle_controller/odometry.hpp b/tricycle_controller/include/tricycle_controller/odometry.hpp new file mode 100644 index 0000000000..62eb51d6cc --- /dev/null +++ b/tricycle_controller/include/tricycle_controller/odometry.hpp @@ -0,0 +1,75 @@ +// Copyright 2022 Pixel Robotics. +// +// 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. + +/* + * Author: Tony Najjar + */ + +#ifndef TRICYCLE_CONTROLLER__ODOMETRY_HPP_ +#define TRICYCLE_CONTROLLER__ODOMETRY_HPP_ + +#include + +#include "rclcpp/time.hpp" +#include "rcpputils/rolling_mean_accumulator.hpp" + +namespace tricycle_controller +{ +class Odometry +{ +public: + explicit Odometry(size_t velocity_rolling_window_size = 10); + + bool update(double left_vel, double right_vel, const rclcpp::Duration & dt); + void updateOpenLoop(double linear, double angular, const rclcpp::Duration & dt); + void resetOdometry(); + + double getX() const { return x_; } + double getY() const { return y_; } + double getHeading() const { return heading_; } + double getLinear() const { return linear_; } + double getAngular() const { return angular_; } + + void setWheelParams(double wheel_separation, double wheel_radius); + void setVelocityRollingWindowSize(size_t velocity_rolling_window_size); + +private: + using RollingMeanAccumulator = rcpputils::RollingMeanAccumulator; + + void integrateRungeKutta2(double linear, double angular); + void integrateExact(double linear, double angular); + void resetAccumulators(); + + // Current pose: + double x_; // [m] + double y_; // [m] + double heading_; // [rad] + + // Current velocity: + double linear_; // [m/s] + double angular_; // [rad/s] + + // Wheel kinematic parameters [m]: + double wheelbase_; + double wheel_radius_; + + // Rolling mean accumulators for the linear and angular velocities: + size_t velocity_rolling_window_size_; + RollingMeanAccumulator linear_accumulator_; + RollingMeanAccumulator angular_accumulator_; +}; + +} // namespace tricycle_controller + +#endif // TRICYCLE_CONTROLLER__ODOMETRY_HPP_ diff --git a/tricycle_controller/include/tricycle_controller/steering_limiter.hpp b/tricycle_controller/include/tricycle_controller/steering_limiter.hpp new file mode 100644 index 0000000000..a16f0de0f0 --- /dev/null +++ b/tricycle_controller/include/tricycle_controller/steering_limiter.hpp @@ -0,0 +1,94 @@ +// Copyright 2022 Pixel Robotics. +// +// 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. + +/* + * Author: Tony Najjar + */ + +#ifndef TRICYCLE_CONTROLLER__STEERING_LIMITER_HPP_ +#define TRICYCLE_CONTROLLER__STEERING_LIMITER_HPP_ + +#include + +namespace tricycle_controller +{ +class SteeringLimiter +{ +public: + /** + * \brief Constructor + * \param [in] min_position Minimum position [m] or [rad] + * \param [in] max_position Maximum position [m] or [rad] + * \param [in] min_velocity Minimum velocity [m/s] or [rad/s] + * \param [in] max_velocity Maximum velocity [m/s] or [rad/s] + * \param [in] min_acceleration Minimum acceleration [m/s^2] or [rad/s^2] + * \param [in] max_acceleration Maximum acceleration [m/s^2] or [rad/s^2] + */ + SteeringLimiter( + double min_position = NAN, double max_position = NAN, double min_velocity = NAN, + double max_velocity = NAN, double min_acceleration = NAN, double max_acceleration = NAN); + + /** + * \brief Limit the position, velocity and acceleration + * \param [in, out] p position [m] or [rad] + * \param [in] p0 Previous position to p [m] or [rad] + * \param [in] p1 Previous position to p0 [m] or [rad] + * \param [in] dt Time step [s] + * \return Limiting factor (1.0 if none) + */ + double limit(double & p, double p0, double p1, double dt); + + /** + * \brief Limit the jerk + * \param [in, out] p position [m] or [rad] + * \param [in] p0 Previous position to p [m] or [rad] + * \param [in] p1 Previous position to p0 [m] or [rad] + * \param [in] dt Time step [s] + * \return Limiting factor (1.0 if none) + */ + double limit_position(double & p); + + /** + * \brief Limit the velocity + * \param [in, out] p position [m] + * \return Limiting factor (1.0 if none) + */ + double limit_velocity(double & p, double p0, double dt); + + /** + * \brief Limit the acceleration + * \param [in, out] p Position [m] or [rad] + * \param [in] p0 Previous position [m] or [rad] + * \param [in] dt Time step [s] + * \return Limiting factor (1.0 if none) + */ + double limit_acceleration(double & p, double p0, double p1, double dt); + +private: + // Position limits: + double min_position_; + double max_position_; + + // Velocity limits: + double min_velocity_; + double max_velocity_; + + // Acceleration limits: + double min_acceleration_; + double max_acceleration_; +}; + +} // namespace tricycle_controller + +#endif // TRICYCLE_CONTROLLER__STEERING_LIMITER_HPP_ diff --git a/tricycle_controller/include/tricycle_controller/traction_limiter.hpp b/tricycle_controller/include/tricycle_controller/traction_limiter.hpp new file mode 100644 index 0000000000..ea0bb16025 --- /dev/null +++ b/tricycle_controller/include/tricycle_controller/traction_limiter.hpp @@ -0,0 +1,102 @@ +// Copyright 2022 Pixel Robotics. +// +// 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. + +/* + * Author: Tony Najjar + */ + +#ifndef TRICYCLE_CONTROLLER__TRACTION_LIMITER_HPP_ +#define TRICYCLE_CONTROLLER__TRACTION_LIMITER_HPP_ + +#include + +namespace tricycle_controller +{ +class TractionLimiter +{ +public: + /** + * \brief Constructor + * \param [in] min_velocity Minimum velocity [m/s] or [rad/s] + * \param [in] max_velocity Maximum velocity [m/s] or [rad/s] + * \param [in] min_acceleration Minimum acceleration [m/s^2] or [rad/s^2] + * \param [in] max_acceleration Maximum acceleration [m/s^2] or [rad/s^2] + * \param [in] min_deceleration Minimum deceleration [m/s^2] or [rad/s^2] + * \param [in] max_deceleration Maximum deceleration [m/s^2] or [rad/s^2] + * \param [in] min_jerk Minimum jerk [m/s^3], usually <= 0 + * \param [in] max_jerk Maximum jerk [m/s^3], usually >= 0 + */ + TractionLimiter( + double min_velocity = NAN, double max_velocity = NAN, double min_acceleration = NAN, + double max_acceleration = NAN, double min_deceleration = NAN, double max_deceleration = NAN, + double min_jerk = NAN, double max_jerk = NAN); + + /** + * \brief Limit the velocity and acceleration + * \param [in, out] v Velocity [m/s] or [rad/s] + * \param [in] v0 Previous velocity to v [m/s] or [rad/s] + * \param [in] v1 Previous velocity to v0 [m/s] or [rad/s] + * \param [in] dt Time step [s] + * \return Limiting factor (1.0 if none) + */ + double limit(double & v, double v0, double v1, double dt); + + /** + * \brief Limit the velocity + * \param [in, out] v Velocity [m/s] or [rad/s] + * \return Limiting factor (1.0 if none) + */ + double limit_velocity(double & v); + + /** + * \brief Limit the acceleration + * \param [in, out] v Velocity [m/s] or [rad/s] + * \param [in] v0 Previous velocity [m/s] or [rad/s] + * \param [in] dt Time step [s] + * \return Limiting factor (1.0 if none) + */ + double limit_acceleration(double & v, double v0, double dt); + + /** + * \brief Limit the jerk + * \param [in, out] v Velocity [m/s] or [rad/s] + * \param [in] v0 Previous velocity to v [m/s] or [rad/s] + * \param [in] v1 Previous velocity to v0 [m/s] or [rad/s] + * \param [in] dt Time step [s] + * \return Limiting factor (1.0 if none) + * \see http://en.wikipedia.org/wiki/Jerk_%28physics%29#Motion_control + */ + double limit_jerk(double & v, double v0, double v1, double dt); + +private: + // Velocity limits: + double min_velocity_; + double max_velocity_; + + // Acceleration limits: + double min_acceleration_; + double max_acceleration_; + + // Deceleration limits: + double min_deceleration_; + double max_deceleration_; + + // Jerk limits: + double min_jerk_; + double max_jerk_; +}; + +} // namespace tricycle_controller + +#endif // TRICYCLE_CONTROLLER__TRACTION_LIMITER_HPP_ diff --git a/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp b/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp new file mode 100644 index 0000000000..cef90d026a --- /dev/null +++ b/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp @@ -0,0 +1,181 @@ +// Copyright 2022 Pixel Robotics. +// +// 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. + +/* + * Author: Tony Najjar + */ + +#ifndef TRICYCLE_CONTROLLER__TRICYCLE_CONTROLLER_HPP_ +#define TRICYCLE_CONTROLLER__TRICYCLE_CONTROLLER_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "ackermann_msgs/msg/ackermann_drive.hpp" +#include "controller_interface/controller_interface.hpp" +#include "geometry_msgs/msg/twist.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "hardware_interface/handle.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include "realtime_tools/realtime_box.h" +#include "realtime_tools/realtime_buffer.h" +#include "realtime_tools/realtime_publisher.h" +#include "std_srvs/srv/empty.hpp" +#include "tf2_msgs/msg/tf_message.hpp" +#include "tricycle_controller/odometry.hpp" +#include "tricycle_controller/steering_limiter.hpp" +#include "tricycle_controller/traction_limiter.hpp" +#include "tricycle_controller/visibility_control.h" + +namespace tricycle_controller +{ +using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + +class TricycleController : public controller_interface::ControllerInterface +{ + using Twist = geometry_msgs::msg::Twist; + using TwistStamped = geometry_msgs::msg::TwistStamped; + using AckermannDrive = ackermann_msgs::msg::AckermannDrive; + +public: + TRICYCLE_CONTROLLER_PUBLIC + TricycleController(); + + TRICYCLE_CONTROLLER_PUBLIC + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + TRICYCLE_CONTROLLER_PUBLIC + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + TRICYCLE_CONTROLLER_PUBLIC + controller_interface::return_type update( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + TRICYCLE_CONTROLLER_PUBLIC + CallbackReturn on_init() override; + + TRICYCLE_CONTROLLER_PUBLIC + CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; + + TRICYCLE_CONTROLLER_PUBLIC + CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; + + TRICYCLE_CONTROLLER_PUBLIC + CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; + + TRICYCLE_CONTROLLER_PUBLIC + CallbackReturn on_cleanup(const rclcpp_lifecycle::State & previous_state) override; + + TRICYCLE_CONTROLLER_PUBLIC + CallbackReturn on_error(const rclcpp_lifecycle::State & previous_state) override; + + TRICYCLE_CONTROLLER_PUBLIC + CallbackReturn on_shutdown(const rclcpp_lifecycle::State & previous_state) override; + +protected: + struct TractionHandle + { + std::reference_wrapper velocity_state; + std::reference_wrapper velocity_command; + }; + struct SteeringHandle + { + std::reference_wrapper position_state; + std::reference_wrapper position_command; + }; + + CallbackReturn get_traction( + const std::string & traction_joint_name, std::vector & joint); + CallbackReturn get_steering( + const std::string & steering_joint_name, std::vector & joint); + double convert_trans_rot_vel_to_steering_angle(double v, double omega, double wheelbase); + std::tuple twist_to_ackermann(double linear_command, double angular_command); + + std::string traction_joint_name_; + std::string steering_joint_name_; + + // HACK: put into vector to avoid initializing structs because they have no default constructors + std::vector traction_joint_; + std::vector steering_joint_; + + struct WheelParams + { + double wheelbase = 0.0; + double radius = 0.0; + } wheel_params_; + + struct OdometryParams + { + bool open_loop = false; + bool enable_odom_tf = false; + bool odom_only_twist = false; // for doing the pose integration in separate node + std::string base_frame_id = "base_link"; + std::string odom_frame_id = "odom"; + std::array pose_covariance_diagonal; + std::array twist_covariance_diagonal; + } odom_params_; + + bool publish_ackermann_command_ = false; + std::shared_ptr> ackermann_command_publisher_ = nullptr; + std::shared_ptr> + realtime_ackermann_command_publisher_ = nullptr; + + Odometry odometry_; + + std::shared_ptr> odometry_publisher_ = nullptr; + std::shared_ptr> + realtime_odometry_publisher_ = nullptr; + + std::shared_ptr> odometry_transform_publisher_ = + nullptr; + std::shared_ptr> + realtime_odometry_transform_publisher_ = nullptr; + + // Timeout to consider cmd_vel commands old + std::chrono::milliseconds cmd_vel_timeout_{500}; + + bool subscriber_is_active_ = false; + rclcpp::Subscription::SharedPtr velocity_command_subscriber_ = nullptr; + rclcpp::Subscription::SharedPtr + velocity_command_unstamped_subscriber_ = nullptr; + + realtime_tools::RealtimeBox> received_velocity_msg_ptr_{nullptr}; + + rclcpp::Service::SharedPtr reset_odom_service_; + + std::queue previous_commands_; // last two commands + + // speed limiters + TractionLimiter limiter_traction_; + SteeringLimiter limiter_steering_; + + bool is_halted = false; + bool use_stamped_vel_ = true; + + void reset_odometry( + const std::shared_ptr request_header, + const std::shared_ptr req, + std::shared_ptr res); + bool reset(); + void halt(); +}; +} // namespace tricycle_controller +#endif // TRICYCLE_CONTROLLER__TRICYCLE_CONTROLLER_HPP_ diff --git a/tricycle_controller/include/tricycle_controller/visibility_control.h b/tricycle_controller/include/tricycle_controller/visibility_control.h new file mode 100644 index 0000000000..bc9b34898b --- /dev/null +++ b/tricycle_controller/include/tricycle_controller/visibility_control.h @@ -0,0 +1,53 @@ +// Copyright 2022 Pixel Robotics. +// +// 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. + +/* + * Author: Tony Najjar + */ + +#ifndef TRICYCLE_CONTROLLER__VISIBILITY_CONTROL_H_ +#define TRICYCLE_CONTROLLER__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define TRICYCLE_CONTROLLER_EXPORT __attribute__((dllexport)) +#define TRICYCLE_CONTROLLER_IMPORT __attribute__((dllimport)) +#else +#define TRICYCLE_CONTROLLER_EXPORT __declspec(dllexport) +#define TRICYCLE_CONTROLLER_IMPORT __declspec(dllimport) +#endif +#ifdef TRICYCLE_CONTROLLER_BUILDING_DLL +#define TRICYCLE_CONTROLLER_PUBLIC TRICYCLE_CONTROLLER_EXPORT +#else +#define TRICYCLE_CONTROLLER_PUBLIC TRICYCLE_CONTROLLER_IMPORT +#endif +#define TRICYCLE_CONTROLLER_PUBLIC_TYPE TRICYCLE_CONTROLLER_PUBLIC +#define TRICYCLE_CONTROLLER_LOCAL +#else +#define TRICYCLE_CONTROLLER_EXPORT __attribute__((visibility("default"))) +#define TRICYCLE_CONTROLLER_IMPORT +#if __GNUC__ >= 4 +#define TRICYCLE_CONTROLLER_PUBLIC __attribute__((visibility("default"))) +#define TRICYCLE_CONTROLLER_LOCAL __attribute__((visibility("hidden"))) +#else +#define TRICYCLE_CONTROLLER_PUBLIC +#define TRICYCLE_CONTROLLER_LOCAL +#endif +#define TRICYCLE_CONTROLLER_PUBLIC_TYPE +#endif + +#endif // TRICYCLE_CONTROLLER__VISIBILITY_CONTROL_H_ diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml new file mode 100644 index 0000000000..1681367c25 --- /dev/null +++ b/tricycle_controller/package.xml @@ -0,0 +1,36 @@ + + + + tricycle_controller + 2.15.0 + Controller for a tricycle drive mobile base + Bence Magyar + Tony Najjar + Apache License 2.0 + Tony Najjar + + ament_cmake + + controller_interface + geometry_msgs + hardware_interface + nav_msgs + std_srvs + rclcpp + rclcpp_lifecycle + rcpputils + realtime_tools + tf2 + tf2_msgs + ackermann_msgs + + pluginlib + + ament_cmake_gmock + controller_manager + ros2_control_test_assets + + + ament_cmake + + diff --git a/tricycle_controller/src/odometry.cpp b/tricycle_controller/src/odometry.cpp new file mode 100644 index 0000000000..e0e670480a --- /dev/null +++ b/tricycle_controller/src/odometry.cpp @@ -0,0 +1,121 @@ +// Copyright 2022 Pixel Robotics. +// +// 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. + +/* + * Author: Tony Najjar + */ + +#include "tricycle_controller/odometry.hpp" + +namespace tricycle_controller +{ +Odometry::Odometry(size_t velocity_rolling_window_size) +: x_(0.0), + y_(0.0), + heading_(0.0), + linear_(0.0), + angular_(0.0), + wheelbase_(0.0), + wheel_radius_(0.0), + velocity_rolling_window_size_(velocity_rolling_window_size), + linear_accumulator_(velocity_rolling_window_size), + angular_accumulator_(velocity_rolling_window_size) +{ +} + +bool Odometry::update(double Ws, double alpha, const rclcpp::Duration & dt) +{ + // using naming convention in http://users.isr.ist.utl.pt/~mir/cadeiras/robmovel/Kinematics.pdf + double Vs = Ws * wheel_radius_; + double Vx = Vs * std::cos(alpha); + double theta_dot = Vs * std::sin(alpha) / wheelbase_; + + // Integrate odometry: + integrateExact(Vx * dt.seconds(), theta_dot * dt.seconds()); + + // Estimate speeds using a rolling mean to filter them out: + linear_accumulator_.accumulate(Vx); + angular_accumulator_.accumulate(theta_dot); + + linear_ = linear_accumulator_.getRollingMean(); + angular_ = angular_accumulator_.getRollingMean(); + + return true; +} + +void Odometry::updateOpenLoop(double linear, double angular, const rclcpp::Duration & dt) +{ + /// Save last linear and angular velocity: + linear_ = linear; + angular_ = angular; + + /// Integrate odometry: + integrateExact(linear * dt.seconds(), angular * dt.seconds()); +} + +void Odometry::resetOdometry() +{ + x_ = 0.0; + y_ = 0.0; + heading_ = 0.0; + resetAccumulators(); +} + +void Odometry::setWheelParams(double wheelbase, double wheel_radius) +{ + wheelbase_ = wheelbase; + wheel_radius_ = wheel_radius; +} + +void Odometry::setVelocityRollingWindowSize(size_t velocity_rolling_window_size) +{ + velocity_rolling_window_size_ = velocity_rolling_window_size; + + resetAccumulators(); +} + +void Odometry::integrateRungeKutta2(double linear, double angular) +{ + const double direction = heading_ + angular * 0.5; + + /// Runge-Kutta 2nd order integration: + x_ += linear * cos(direction); + y_ += linear * sin(direction); + heading_ += angular; +} + +void Odometry::integrateExact(double linear, double angular) +{ + if (fabs(angular) < 1e-6) + { + integrateRungeKutta2(linear, angular); + } + else + { + /// Exact integration (should solve problems when angular is zero): + const double heading_old = heading_; + const double r = linear / angular; + heading_ += angular; + x_ += r * (sin(heading_) - sin(heading_old)); + y_ += -r * (cos(heading_) - cos(heading_old)); + } +} + +void Odometry::resetAccumulators() +{ + linear_accumulator_ = RollingMeanAccumulator(velocity_rolling_window_size_); + angular_accumulator_ = RollingMeanAccumulator(velocity_rolling_window_size_); +} + +} // namespace tricycle_controller diff --git a/tricycle_controller/src/steering_limiter.cpp b/tricycle_controller/src/steering_limiter.cpp new file mode 100644 index 0000000000..b76cc0f602 --- /dev/null +++ b/tricycle_controller/src/steering_limiter.cpp @@ -0,0 +1,117 @@ +// Copyright 2022 Pixel Robotics. +// +// 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. + +/* + * Author: Tony Najjar + */ + +#include +#include +#include + +#include "rcppmath/clamp.hpp" +#include "tricycle_controller/steering_limiter.hpp" + +namespace tricycle_controller +{ +SteeringLimiter::SteeringLimiter( + double min_position, double max_position, double min_velocity, double max_velocity, + double min_acceleration, double max_acceleration) +: min_position_(min_position), + max_position_(max_position), + min_velocity_(min_velocity), + max_velocity_(max_velocity), + min_acceleration_(min_acceleration), + max_acceleration_(max_acceleration) +{ + if (!std::isnan(min_position_) && std::isnan(max_position_)) max_position_ = -min_position_; + if (!std::isnan(max_position_) && std::isnan(min_position_)) min_position_ = -max_position_; + + if (!std::isnan(min_velocity_) && std::isnan(max_velocity_)) + max_velocity_ = 1000.0; // Arbitrarily big number + if (!std::isnan(max_velocity_) && std::isnan(min_velocity_)) min_velocity_ = 0.0; + + if (!std::isnan(min_acceleration_) && std::isnan(max_acceleration_)) max_acceleration_ = 1000.0; + if (!std::isnan(max_acceleration_) && std::isnan(min_acceleration_)) min_acceleration_ = 0.0; + + const std::string error = + "The positive limit will be applied to both directions. Setting different limits for positive " + "and negative directions is not supported. Actuators are " + "assumed to have the same constraints in both directions"; + + if (min_velocity_ < 0 || max_velocity_ < 0) + { + throw std::invalid_argument("Velocity cannot be negative." + error); + } + + if (min_acceleration_ < 0 || max_acceleration_ < 0) + { + throw std::invalid_argument("Acceleration cannot be negative." + error); + } +} + +double SteeringLimiter::limit(double & p, double p0, double p1, double dt) +{ + const double tmp = p; + + if (!std::isnan(min_acceleration_) && !std::isnan(max_acceleration_)) + limit_acceleration(p, p0, p1, dt); + if (!std::isnan(min_velocity_) && !std::isnan(max_velocity_)) limit_velocity(p, p0, dt); + if (!std::isnan(min_position_) && !std::isnan(max_position_)) limit_position(p); + + return tmp != 0.0 ? p / tmp : 1.0; +} + +double SteeringLimiter::limit_position(double & p) +{ + const double tmp = p; + p = rcppmath::clamp(p, min_position_, max_position_); + + return tmp != 0.0 ? p / tmp : 1.0; +} + +double SteeringLimiter::limit_velocity(double & p, double p0, double dt) +{ + const double tmp = p; + + const double dv_min = min_velocity_ * dt; + const double dv_max = max_velocity_ * dt; + + double dv = rcppmath::clamp(std::fabs(p - p0), dv_min, dv_max); + dv *= (p - p0 >= 0 ? 1 : -1); + p = p0 + dv; + + return tmp != 0.0 ? p / tmp : 1.0; +} + +double SteeringLimiter::limit_acceleration(double & p, double p0, double p1, double dt) +{ + const double tmp = p; + + const double dv = p - p0; + const double dp0 = p0 - p1; + + const double dt2 = 2. * dt * dt; + + const double da_min = min_acceleration_ * dt2; + const double da_max = max_acceleration_ * dt2; + + double da = rcppmath::clamp(std::fabs(dv - dp0), da_min, da_max); + da *= (dv - dp0 >= 0 ? 1 : -1); + p = p0 + dp0 + da; + + return tmp != 0.0 ? p / tmp : 1.0; +} + +} // namespace tricycle_controller diff --git a/tricycle_controller/src/traction_limiter.cpp b/tricycle_controller/src/traction_limiter.cpp new file mode 100644 index 0000000000..7865d4be71 --- /dev/null +++ b/tricycle_controller/src/traction_limiter.cpp @@ -0,0 +1,142 @@ +// Copyright 2022 Pixel Robotics. +// +// 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. + +/* + * Author: Tony Najjar + */ + +#include +#include +#include + +#include "rcppmath/clamp.hpp" +#include "tricycle_controller/traction_limiter.hpp" + +namespace tricycle_controller +{ +TractionLimiter::TractionLimiter( + double min_velocity, double max_velocity, double min_acceleration, double max_acceleration, + double min_deceleration, double max_deceleration, double min_jerk, double max_jerk) +: min_velocity_(min_velocity), + max_velocity_(max_velocity), + min_acceleration_(min_acceleration), + max_acceleration_(max_acceleration), + min_deceleration_(min_deceleration), + max_deceleration_(max_deceleration), + min_jerk_(min_jerk), + max_jerk_(max_jerk) +{ + if (!std::isnan(min_velocity_) && std::isnan(max_velocity_)) + max_velocity_ = 1000.0; // Arbitrarily big number + if (!std::isnan(max_velocity_) && std::isnan(min_velocity_)) min_velocity_ = 0.0; + + if (!std::isnan(min_acceleration_) && std::isnan(max_acceleration_)) max_acceleration_ = 1000.0; + if (!std::isnan(max_acceleration_) && std::isnan(min_acceleration_)) min_acceleration_ = 0.0; + + if (!std::isnan(min_deceleration_) && std::isnan(max_deceleration_)) max_deceleration_ = 1000.0; + if (!std::isnan(max_deceleration_) && std::isnan(min_acceleration_)) min_deceleration_ = 0.0; + + if (!std::isnan(min_jerk_) && std::isnan(max_jerk_)) max_jerk_ = 1000.0; + if (!std::isnan(max_jerk_) && std::isnan(min_jerk_)) min_jerk_ = 0.0; + + const std::string error = + "The positive limit will be applied to both directions. Setting different limits for positive " + "and negative directions is not supported. Actuators are " + "assumed to have the same constraints in both directions"; + if (min_velocity_ < 0 || max_velocity_ < 0) + { + throw std::invalid_argument("Velocity cannot be negative." + error); + } + + if (min_acceleration_ < 0 || max_acceleration_ < 0) + { + throw std::invalid_argument("Acceleration cannot be negative." + error); + } + + if (min_deceleration_ < 0 || max_deceleration_ < 0) + { + throw std::invalid_argument("Deceleration cannot be negative." + error); + } + + if (min_jerk_ < 0 || max_jerk_ < 0) + { + throw std::invalid_argument("Jerk cannot be negative." + error); + } +} + +double TractionLimiter::limit(double & v, double v0, double v1, double dt) +{ + const double tmp = v; + + if (!std::isnan(min_jerk_) && !std::isnan(max_jerk_)) limit_jerk(v, v0, v1, dt); + if (!std::isnan(min_acceleration_) && !std::isnan(max_acceleration_)) + limit_acceleration(v, v0, dt); + if (!std::isnan(min_velocity_) && !std::isnan(max_velocity_)) limit_velocity(v); + + return tmp != 0.0 ? v / tmp : 1.0; +} + +double TractionLimiter::limit_velocity(double & v) +{ + const double tmp = v; + + v = rcppmath::clamp(std::fabs(v), min_velocity_, max_velocity_); + + v *= tmp >= 0 ? 1 : -1; + return tmp != 0.0 ? v / tmp : 1.0; +} + +double TractionLimiter::limit_acceleration(double & v, double v0, double dt) +{ + const double tmp = v; + + double dv_min; + double dv_max; + if (abs(v) >= abs(v0)) + { + dv_min = min_acceleration_ * dt; + dv_max = max_acceleration_ * dt; + } + else + { + dv_min = min_deceleration_ * dt; + dv_max = max_deceleration_ * dt; + } + double dv = rcppmath::clamp(std::fabs(v - v0), dv_min, dv_max); + dv *= (v - v0 >= 0 ? 1 : -1); + v = v0 + dv; + + return tmp != 0.0 ? v / tmp : 1.0; +} + +double TractionLimiter::limit_jerk(double & v, double v0, double v1, double dt) +{ + const double tmp = v; + + const double dv = v - v0; + const double dv0 = v0 - v1; + + const double dt2 = 2. * dt * dt; + + const double da_min = min_jerk_ * dt2; + const double da_max = max_jerk_ * dt2; + + double da = rcppmath::clamp(std::fabs(dv - dv0), da_min, da_max); + da *= (dv - dv0 >= 0 ? 1 : -1); + v = v0 + dv0 + da; + + return tmp != 0.0 ? v / tmp : 1.0; +} + +} // namespace tricycle_controller diff --git a/tricycle_controller/src/tricycle_controller.cpp b/tricycle_controller/src/tricycle_controller.cpp new file mode 100644 index 0000000000..4f4e22ec9d --- /dev/null +++ b/tricycle_controller/src/tricycle_controller.cpp @@ -0,0 +1,660 @@ +// Copyright 2022 Pixel Robotics. +// +// 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. + +/* + * Author: Tony Najjar + */ + +#include +#include +#include +#include +#include + +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp/logging.hpp" +#include "tf2/LinearMath/Quaternion.h" +#include "tricycle_controller/tricycle_controller.hpp" + +namespace +{ +constexpr auto DEFAULT_COMMAND_TOPIC = "~/cmd_vel"; +constexpr auto DEFAULT_ACKERMANN_OUT_TOPIC = "~/cmd_ackermann"; +constexpr auto DEFAULT_ODOMETRY_TOPIC = "~/odom"; +constexpr auto DEFAULT_TRANSFORM_TOPIC = "/tf"; +constexpr auto DEFAULT_RESET_ODOM_SERVICE = "~/reset_odometry"; +} // namespace + +namespace tricycle_controller +{ +using namespace std::chrono_literals; +using controller_interface::interface_configuration_type; +using controller_interface::InterfaceConfiguration; +using hardware_interface::HW_IF_POSITION; +using hardware_interface::HW_IF_VELOCITY; +using lifecycle_msgs::msg::State; + +TricycleController::TricycleController() : controller_interface::ControllerInterface() {} + +CallbackReturn TricycleController::on_init() +{ + try + { + // with the lifecycle node being initialized, we can declare parameters + auto_declare("traction_joint_name", std::string()); + auto_declare("steering_joint_name", std::string()); + + auto_declare("wheelbase", wheel_params_.wheelbase); + auto_declare("wheel_radius", wheel_params_.radius); + + auto_declare("odom_frame_id", odom_params_.odom_frame_id); + auto_declare("base_frame_id", odom_params_.base_frame_id); + auto_declare>("pose_covariance_diagonal", std::vector()); + auto_declare>("twist_covariance_diagonal", std::vector()); + auto_declare("open_loop", odom_params_.open_loop); + auto_declare("enable_odom_tf", odom_params_.enable_odom_tf); + auto_declare("odom_only_twist", odom_params_.odom_only_twist); + + auto_declare("cmd_vel_timeout", cmd_vel_timeout_.count()); + auto_declare("publish_ackermann_command", publish_ackermann_command_); + auto_declare("velocity_rolling_window_size", 10); + auto_declare("use_stamped_vel", use_stamped_vel_); + + auto_declare("traction.max_velocity", NAN); + auto_declare("traction.min_velocity", NAN); + auto_declare("traction.max_acceleration", NAN); + auto_declare("traction.min_acceleration", NAN); + auto_declare("traction.max_deceleration", NAN); + auto_declare("traction.min_deceleration", NAN); + auto_declare("traction.max_jerk", NAN); + auto_declare("traction.min_jerk", NAN); + + auto_declare("steering.max_position", NAN); + auto_declare("steering.min_position", NAN); + auto_declare("steering.max_velocity", NAN); + auto_declare("steering.min_velocity", NAN); + auto_declare("steering.max_acceleration", NAN); + auto_declare("steering.min_acceleration", NAN); + } + catch (const std::exception & e) + { + fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); + return CallbackReturn::ERROR; + } + + return CallbackReturn::SUCCESS; +} + +InterfaceConfiguration TricycleController::command_interface_configuration() const +{ + InterfaceConfiguration command_interfaces_config; + command_interfaces_config.type = interface_configuration_type::INDIVIDUAL; + command_interfaces_config.names.push_back(traction_joint_name_ + "/" + HW_IF_VELOCITY); + command_interfaces_config.names.push_back(steering_joint_name_ + "/" + HW_IF_POSITION); + return command_interfaces_config; +} + +InterfaceConfiguration TricycleController::state_interface_configuration() const +{ + InterfaceConfiguration state_interfaces_config; + state_interfaces_config.type = interface_configuration_type::INDIVIDUAL; + state_interfaces_config.names.push_back(traction_joint_name_ + "/" + HW_IF_VELOCITY); + state_interfaces_config.names.push_back(steering_joint_name_ + "/" + HW_IF_POSITION); + return state_interfaces_config; +} + +controller_interface::return_type TricycleController::update( + const rclcpp::Time & time, const rclcpp::Duration & period) +{ + if (get_state().id() == State::PRIMARY_STATE_INACTIVE) + { + if (!is_halted) + { + halt(); + is_halted = true; + } + return controller_interface::return_type::OK; + } + std::shared_ptr last_command_msg; + received_velocity_msg_ptr_.get(last_command_msg); + if (last_command_msg == nullptr) + { + RCLCPP_WARN(get_node()->get_logger(), "Velocity message received was a nullptr."); + return controller_interface::return_type::ERROR; + } + + const auto age_of_last_command = time - last_command_msg->header.stamp; + // Brake if cmd_vel has timeout, override the stored command + if (age_of_last_command > cmd_vel_timeout_) + { + last_command_msg->twist.linear.x = 0.0; + last_command_msg->twist.angular.z = 0.0; + } + + // command may be limited further by Limiters, + // without affecting the stored twist command + TwistStamped command = *last_command_msg; + double & linear_command = command.twist.linear.x; + double & angular_command = command.twist.angular.z; + double Ws_read = traction_joint_[0].velocity_state.get().get_value(); // in radians/s + double alpha_read = steering_joint_[0].position_state.get().get_value(); // in radians + + if (odom_params_.open_loop) + { + odometry_.updateOpenLoop(linear_command, angular_command, period); + } + else + { + if (std::isnan(Ws_read) || std::isnan(alpha_read)) + { + RCLCPP_ERROR(get_node()->get_logger(), "Could not read feedback value"); + return controller_interface::return_type::ERROR; + } + odometry_.update(Ws_read, alpha_read, period); + } + + tf2::Quaternion orientation; + orientation.setRPY(0.0, 0.0, odometry_.getHeading()); + + if (realtime_odometry_publisher_->trylock()) + { + auto & odometry_message = realtime_odometry_publisher_->msg_; + odometry_message.header.stamp = time; + if (!odom_params_.odom_only_twist) + { + odometry_message.pose.pose.position.x = odometry_.getX(); + odometry_message.pose.pose.position.y = odometry_.getY(); + odometry_message.pose.pose.orientation.x = orientation.x(); + odometry_message.pose.pose.orientation.y = orientation.y(); + odometry_message.pose.pose.orientation.z = orientation.z(); + odometry_message.pose.pose.orientation.w = orientation.w(); + } + odometry_message.twist.twist.linear.x = odometry_.getLinear(); + odometry_message.twist.twist.angular.z = odometry_.getAngular(); + realtime_odometry_publisher_->unlockAndPublish(); + } + + if (odom_params_.enable_odom_tf && realtime_odometry_transform_publisher_->trylock()) + { + auto & transform = realtime_odometry_transform_publisher_->msg_.transforms.front(); + transform.header.stamp = time; + transform.transform.translation.x = odometry_.getX(); + transform.transform.translation.y = odometry_.getY(); + transform.transform.rotation.x = orientation.x(); + transform.transform.rotation.y = orientation.y(); + transform.transform.rotation.z = orientation.z(); + transform.transform.rotation.w = orientation.w(); + realtime_odometry_transform_publisher_->unlockAndPublish(); + } + + // Compute wheel velocity and angle + auto [alpha_write, Ws_write] = twist_to_ackermann(linear_command, angular_command); + + // Reduce wheel speed until the target angle has been reached + double alpha_delta = abs(alpha_write - alpha_read); + double scale; + if (alpha_delta < M_PI / 6) + { + scale = 1; + } + else if (alpha_delta > M_PI_2) + { + scale = 0.01; + } + else + { + // TODO(anyone): find the best function, e.g convex power functions + scale = cos(alpha_delta); + } + Ws_write *= scale; + + auto & last_command = previous_commands_.back(); + auto & second_to_last_command = previous_commands_.front(); + + limiter_traction_.limit( + Ws_write, last_command.speed, second_to_last_command.speed, period.seconds()); + + limiter_steering_.limit( + alpha_write, last_command.steering_angle, second_to_last_command.steering_angle, + period.seconds()); + + previous_commands_.pop(); + AckermannDrive ackermann_command; + // speed in AckermannDrive is defined as desired forward speed (m/s) but it is used here as wheel + // speed (rad/s) + ackermann_command.speed = Ws_write; + ackermann_command.steering_angle = alpha_write; + previous_commands_.emplace(ackermann_command); + + // Publish ackermann command + if (publish_ackermann_command_ && realtime_ackermann_command_publisher_->trylock()) + { + auto & realtime_ackermann_command = realtime_ackermann_command_publisher_->msg_; + // speed in AckermannDrive is defined desired forward speed (m/s) but we use it here as wheel + // speed (rad/s) + realtime_ackermann_command.speed = Ws_write; + realtime_ackermann_command.steering_angle = alpha_write; + realtime_ackermann_command_publisher_->unlockAndPublish(); + } + + traction_joint_[0].velocity_command.get().set_value(Ws_write); + steering_joint_[0].position_command.get().set_value(alpha_write); + return controller_interface::return_type::OK; +} + +CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State & /*previous_state*/) +{ + auto logger = get_node()->get_logger(); + + // update parameters + traction_joint_name_ = get_node()->get_parameter("traction_joint_name").as_string(); + steering_joint_name_ = get_node()->get_parameter("steering_joint_name").as_string(); + if (traction_joint_name_.empty()) + { + RCLCPP_ERROR(logger, "'traction_joint_name' parameter was empty"); + return CallbackReturn::ERROR; + } + if (steering_joint_name_.empty()) + { + RCLCPP_ERROR(logger, "'steering_joint_name' parameter was empty"); + return CallbackReturn::ERROR; + } + + wheel_params_.wheelbase = get_node()->get_parameter("wheelbase").as_double(); + wheel_params_.radius = get_node()->get_parameter("wheel_radius").as_double(); + + odometry_.setWheelParams(wheel_params_.wheelbase, wheel_params_.radius); + odometry_.setVelocityRollingWindowSize( + get_node()->get_parameter("velocity_rolling_window_size").as_int()); + + odom_params_.odom_frame_id = get_node()->get_parameter("odom_frame_id").as_string(); + odom_params_.base_frame_id = get_node()->get_parameter("base_frame_id").as_string(); + + auto pose_diagonal = get_node()->get_parameter("pose_covariance_diagonal").as_double_array(); + std::copy( + pose_diagonal.begin(), pose_diagonal.end(), odom_params_.pose_covariance_diagonal.begin()); + + auto twist_diagonal = get_node()->get_parameter("twist_covariance_diagonal").as_double_array(); + std::copy( + twist_diagonal.begin(), twist_diagonal.end(), odom_params_.twist_covariance_diagonal.begin()); + + odom_params_.open_loop = get_node()->get_parameter("open_loop").as_bool(); + odom_params_.enable_odom_tf = get_node()->get_parameter("enable_odom_tf").as_bool(); + odom_params_.odom_only_twist = get_node()->get_parameter("odom_only_twist").as_bool(); + + cmd_vel_timeout_ = + std::chrono::milliseconds{get_node()->get_parameter("cmd_vel_timeout").as_int()}; + publish_ackermann_command_ = get_node()->get_parameter("publish_ackermann_command").as_bool(); + use_stamped_vel_ = get_node()->get_parameter("use_stamped_vel").as_bool(); + + try + { + limiter_traction_ = TractionLimiter( + get_node()->get_parameter("traction.min_velocity").as_double(), + get_node()->get_parameter("traction.max_velocity").as_double(), + get_node()->get_parameter("traction.min_acceleration").as_double(), + get_node()->get_parameter("traction.max_acceleration").as_double(), + get_node()->get_parameter("traction.min_deceleration").as_double(), + get_node()->get_parameter("traction.max_deceleration").as_double(), + get_node()->get_parameter("traction.min_jerk").as_double(), + get_node()->get_parameter("traction.max_jerk").as_double()); + } + catch (const std::invalid_argument & e) + { + RCLCPP_ERROR(get_node()->get_logger(), "Error configuring traction limiter: %s", e.what()); + return CallbackReturn::ERROR; + } + + try + { + limiter_steering_ = SteeringLimiter( + get_node()->get_parameter("steering.min_position").as_double(), + get_node()->get_parameter("steering.max_position").as_double(), + get_node()->get_parameter("steering.min_velocity").as_double(), + get_node()->get_parameter("steering.max_velocity").as_double(), + get_node()->get_parameter("steering.min_acceleration").as_double(), + get_node()->get_parameter("steering.max_acceleration").as_double()); + } + catch (const std::invalid_argument & e) + { + RCLCPP_ERROR(get_node()->get_logger(), "Error configuring steering limiter: %s", e.what()); + return CallbackReturn::ERROR; + } + + if (!reset()) + { + return CallbackReturn::ERROR; + } + + const TwistStamped empty_twist; + received_velocity_msg_ptr_.set(std::make_shared(empty_twist)); + + // Fill last two commands with default constructed commands + const AckermannDrive empty_ackermann_drive; + previous_commands_.emplace(empty_ackermann_drive); + previous_commands_.emplace(empty_ackermann_drive); + + // initialize ackermann command publisher + if (publish_ackermann_command_) + { + ackermann_command_publisher_ = get_node()->create_publisher( + DEFAULT_ACKERMANN_OUT_TOPIC, rclcpp::SystemDefaultsQoS()); + realtime_ackermann_command_publisher_ = + std::make_shared>( + ackermann_command_publisher_); + } + + // initialize command subscriber + if (use_stamped_vel_) + { + velocity_command_subscriber_ = get_node()->create_subscription( + DEFAULT_COMMAND_TOPIC, rclcpp::SystemDefaultsQoS(), + [this](const std::shared_ptr msg) -> void + { + if (!subscriber_is_active_) + { + RCLCPP_WARN( + get_node()->get_logger(), "Can't accept new commands. subscriber is inactive"); + return; + } + if ((msg->header.stamp.sec == 0) && (msg->header.stamp.nanosec == 0)) + { + RCLCPP_WARN_ONCE( + get_node()->get_logger(), + "Received TwistStamped with zero timestamp, setting it to current " + "time, this message will only be shown once"); + msg->header.stamp = get_node()->get_clock()->now(); + } + received_velocity_msg_ptr_.set(std::move(msg)); + }); + } + else + { + velocity_command_unstamped_subscriber_ = get_node()->create_subscription( + DEFAULT_COMMAND_TOPIC, rclcpp::SystemDefaultsQoS(), + [this](const std::shared_ptr msg) -> void + { + if (!subscriber_is_active_) + { + RCLCPP_WARN( + get_node()->get_logger(), "Can't accept new commands. subscriber is inactive"); + return; + } + + // Write fake header in the stored stamped command + std::shared_ptr twist_stamped; + received_velocity_msg_ptr_.get(twist_stamped); + twist_stamped->twist = *msg; + twist_stamped->header.stamp = get_node()->get_clock()->now(); + }); + } + + // initialize odometry publisher and messasge + odometry_publisher_ = get_node()->create_publisher( + DEFAULT_ODOMETRY_TOPIC, rclcpp::SystemDefaultsQoS()); + realtime_odometry_publisher_ = + std::make_shared>( + odometry_publisher_); + + auto & odometry_message = realtime_odometry_publisher_->msg_; + odometry_message.header.frame_id = odom_params_.odom_frame_id; + odometry_message.child_frame_id = odom_params_.base_frame_id; + + // initialize odom values zeros + odometry_message.twist = + geometry_msgs::msg::TwistWithCovariance(rosidl_runtime_cpp::MessageInitialization::ALL); + + constexpr size_t NUM_DIMENSIONS = 6; + for (size_t index = 0; index < 6; ++index) + { + // 0, 7, 14, 21, 28, 35 + const size_t diagonal_index = NUM_DIMENSIONS * index + index; + odometry_message.pose.covariance[diagonal_index] = odom_params_.pose_covariance_diagonal[index]; + odometry_message.twist.covariance[diagonal_index] = + odom_params_.twist_covariance_diagonal[index]; + } + + // initialize transform publisher and message + if (odom_params_.enable_odom_tf) + { + odometry_transform_publisher_ = get_node()->create_publisher( + DEFAULT_TRANSFORM_TOPIC, rclcpp::SystemDefaultsQoS()); + realtime_odometry_transform_publisher_ = + std::make_shared>( + odometry_transform_publisher_); + + // keeping track of odom and base_link transforms only + auto & odometry_transform_message = realtime_odometry_transform_publisher_->msg_; + odometry_transform_message.transforms.resize(1); + odometry_transform_message.transforms.front().header.frame_id = odom_params_.odom_frame_id; + odometry_transform_message.transforms.front().child_frame_id = odom_params_.base_frame_id; + } + + // Create odom reset service + reset_odom_service_ = get_node()->create_service( + DEFAULT_RESET_ODOM_SERVICE, std::bind( + &TricycleController::reset_odometry, this, std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3)); + + return CallbackReturn::SUCCESS; +} + +CallbackReturn TricycleController::on_activate(const rclcpp_lifecycle::State &) +{ + RCLCPP_INFO(get_node()->get_logger(), "On activate: Initialize Joints"); + + // Initialize the joints + const auto wheel_front_result = get_traction(traction_joint_name_, traction_joint_); + const auto steering_result = get_steering(steering_joint_name_, steering_joint_); + if (wheel_front_result == CallbackReturn::ERROR || steering_result == CallbackReturn::ERROR) + { + return CallbackReturn::ERROR; + } + if (traction_joint_.empty() || steering_joint_.empty()) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Either steering or traction interfaces are non existent"); + return CallbackReturn::ERROR; + } + + is_halted = false; + subscriber_is_active_ = true; + + RCLCPP_DEBUG(get_node()->get_logger(), "Subscriber and publisher are now active."); + return CallbackReturn::SUCCESS; +} + +CallbackReturn TricycleController::on_deactivate(const rclcpp_lifecycle::State &) +{ + subscriber_is_active_ = false; + return CallbackReturn::SUCCESS; +} + +CallbackReturn TricycleController::on_cleanup(const rclcpp_lifecycle::State &) +{ + if (!reset()) + { + return CallbackReturn::ERROR; + } + + received_velocity_msg_ptr_.set(std::make_shared()); + return CallbackReturn::SUCCESS; +} + +CallbackReturn TricycleController::on_error(const rclcpp_lifecycle::State &) +{ + if (!reset()) + { + return CallbackReturn::ERROR; + } + return CallbackReturn::SUCCESS; +} + +void TricycleController::reset_odometry( + const std::shared_ptr /*request_header*/, + const std::shared_ptr /*req*/, + std::shared_ptr /*res*/) +{ + odometry_.resetOdometry(); + RCLCPP_INFO(get_node()->get_logger(), "Odometry successfully reset"); +} + +bool TricycleController::reset() +{ + odometry_.resetOdometry(); + + // release the old queue + std::queue empty_ackermann_drive; + std::swap(previous_commands_, empty_ackermann_drive); + + traction_joint_.clear(); + steering_joint_.clear(); + + subscriber_is_active_ = false; + velocity_command_subscriber_.reset(); + velocity_command_unstamped_subscriber_.reset(); + + received_velocity_msg_ptr_.set(nullptr); + is_halted = false; + return true; +} + +CallbackReturn TricycleController::on_shutdown(const rclcpp_lifecycle::State &) +{ + return CallbackReturn::SUCCESS; +} + +void TricycleController::halt() +{ + traction_joint_[0].velocity_command.get().set_value(0.0); + steering_joint_[0].position_command.get().set_value(0.0); +} + +CallbackReturn TricycleController::get_traction( + const std::string & traction_joint_name, std::vector & joint) +{ + RCLCPP_INFO(get_node()->get_logger(), "Get Wheel Joint Instance"); + + // Lookup the velocity state interface + const auto state_handle = std::find_if( + state_interfaces_.cbegin(), state_interfaces_.cend(), + [&traction_joint_name](const auto & interface) + { + return interface.get_prefix_name() == traction_joint_name && + interface.get_interface_name() == HW_IF_VELOCITY; + }); + if (state_handle == state_interfaces_.cend()) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Unable to obtain joint state handle for %s", + traction_joint_name.c_str()); + return CallbackReturn::ERROR; + } + + // Lookup the velocity command interface + const auto command_handle = std::find_if( + command_interfaces_.begin(), command_interfaces_.end(), + [&traction_joint_name](const auto & interface) + { + return interface.get_prefix_name() == traction_joint_name && + interface.get_interface_name() == HW_IF_VELOCITY; + }); + if (command_handle == command_interfaces_.end()) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Unable to obtain joint state handle for %s", + traction_joint_name.c_str()); + return CallbackReturn::ERROR; + } + + // Create the traction joint instance + joint.emplace_back(TractionHandle{std::ref(*state_handle), std::ref(*command_handle)}); + return CallbackReturn::SUCCESS; +} + +CallbackReturn TricycleController::get_steering( + const std::string & steering_joint_name, std::vector & joint) +{ + RCLCPP_INFO(get_node()->get_logger(), "Get Steering Joint Instance"); + + // Lookup the velocity state interface + const auto state_handle = std::find_if( + state_interfaces_.cbegin(), state_interfaces_.cend(), + [&steering_joint_name](const auto & interface) + { + return interface.get_prefix_name() == steering_joint_name && + interface.get_interface_name() == HW_IF_POSITION; + }); + if (state_handle == state_interfaces_.cend()) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Unable to obtain joint state handle for %s", + steering_joint_name.c_str()); + return CallbackReturn::ERROR; + } + + // Lookup the velocity command interface + const auto command_handle = std::find_if( + command_interfaces_.begin(), command_interfaces_.end(), + [&steering_joint_name](const auto & interface) + { + return interface.get_prefix_name() == steering_joint_name && + interface.get_interface_name() == HW_IF_POSITION; + }); + if (command_handle == command_interfaces_.end()) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Unable to obtain joint state handle for %s", + steering_joint_name.c_str()); + return CallbackReturn::ERROR; + } + + // Create the steering joint instance + joint.emplace_back(SteeringHandle{std::ref(*state_handle), std::ref(*command_handle)}); + return CallbackReturn::SUCCESS; +} + +double TricycleController::convert_trans_rot_vel_to_steering_angle( + double Vx, double theta_dot, double wheelbase) +{ + if (theta_dot == 0 || Vx == 0) + { + return 0; + } + return std::atan(theta_dot * wheelbase / Vx); +} + +std::tuple TricycleController::twist_to_ackermann(double Vx, double theta_dot) +{ + // using naming convention in http://users.isr.ist.utl.pt/~mir/cadeiras/robmovel/Kinematics.pdf + double alpha, Ws; + + if (Vx == 0 && theta_dot != 0) + { // is spin action + alpha = theta_dot > 0 ? M_PI_2 : -M_PI_2; + Ws = abs(theta_dot) * wheel_params_.wheelbase / wheel_params_.radius; + return std::make_tuple(alpha, Ws); + } + + alpha = convert_trans_rot_vel_to_steering_angle(Vx, theta_dot, wheel_params_.wheelbase); + Ws = Vx / (wheel_params_.radius * std::cos(alpha)); + return std::make_tuple(alpha, Ws); +} + +} // namespace tricycle_controller + +#include +PLUGINLIB_EXPORT_CLASS( + tricycle_controller::TricycleController, controller_interface::ControllerInterface) diff --git a/tricycle_controller/test/config/test_tricycle_controller.yaml b/tricycle_controller/test/config/test_tricycle_controller.yaml new file mode 100644 index 0000000000..efecfe968f --- /dev/null +++ b/tricycle_controller/test/config/test_tricycle_controller.yaml @@ -0,0 +1,20 @@ + +test_tricycle_controller: + ros__parameters: + traction_joint_name: traction_joint + steering_joint_name: steering_joint + wheel_radius: 0.125 + wheelbase: 1.252 + use_stamped_vel: false + enable_odom_tf: false + use_sim_time: false + pose_covariance_diagonal: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + twist_covariance_diagonal: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + odom_only_twist: true + publish_ackermann_command: true + traction: + max_acceleration: 5.0 + max_deceleration: 8.0 + steering: + max_position: 1.57 # pi/2 + max_velocity: 1.0 diff --git a/tricycle_controller/test/test_load_tricycle_controller.cpp b/tricycle_controller/test/test_load_tricycle_controller.cpp new file mode 100644 index 0000000000..d04b83ae27 --- /dev/null +++ b/tricycle_controller/test/test_load_tricycle_controller.cpp @@ -0,0 +1,43 @@ +// Copyright 2022 Pixel Robotics. +// +// 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. + +/* + * Author: Tony Najjar + */ + +#include +#include + +#include "controller_manager/controller_manager.hpp" +#include "hardware_interface/resource_manager.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "ros2_control_test_assets/descriptions.hpp" + +TEST(TestLoadTricycleController, 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_tricycle_controller", "tricycle_controller/TricycleController")); + + rclcpp::shutdown(); +} diff --git a/tricycle_controller/test/test_tricycle_controller.cpp b/tricycle_controller/test/test_tricycle_controller.cpp new file mode 100644 index 0000000000..a1f09ddaf8 --- /dev/null +++ b/tricycle_controller/test/test_tricycle_controller.cpp @@ -0,0 +1,350 @@ +// Copyright 2022 Pixel Robotics. +// +// 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. + +/* + * Author: Tony Najjar + */ + +#include + +#include +#include +#include +#include +#include +#include + +#include "hardware_interface/loaned_command_interface.hpp" +#include "hardware_interface/loaned_state_interface.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp/rclcpp.hpp" +#include "tricycle_controller/tricycle_controller.hpp" + +using CallbackReturn = controller_interface::CallbackReturn; +using hardware_interface::HW_IF_POSITION; +using hardware_interface::HW_IF_VELOCITY; +using hardware_interface::LoanedCommandInterface; +using hardware_interface::LoanedStateInterface; +using lifecycle_msgs::msg::State; +using testing::SizeIs; + +class TestableTricycleController : public tricycle_controller::TricycleController +{ +public: + using TricycleController::TricycleController; + std::shared_ptr getLastReceivedTwist() + { + std::shared_ptr ret; + received_velocity_msg_ptr_.get(ret); + return ret; + } + + /** + * @brief wait_for_twist block until a new twist is received. + * Requires that the executor is not spinned elsewhere between the + * message publication and the call to this function + * + * @return true if new twist msg was received, false if timeout + */ + bool wait_for_twist( + rclcpp::Executor & executor, + const std::chrono::milliseconds & timeout = std::chrono::milliseconds(500)) + { + rclcpp::WaitSet wait_set; + wait_set.add_subscription(velocity_command_subscriber_); + + if (wait_set.wait(timeout).kind() == rclcpp::WaitResultKind::Ready) + { + executor.spin_some(); + return true; + } + return false; + } +}; + +class TestTricycleController : public ::testing::Test +{ +protected: + static void SetUpTestCase() { rclcpp::init(0, nullptr); } + + void SetUp() override + { + controller_ = std::make_unique(); + pub_node = std::make_shared("velocity_publisher"); + velocity_publisher = pub_node->create_publisher( + controller_name + "/cmd_vel", rclcpp::SystemDefaultsQoS()); + } + + static void TearDownTestCase() { rclcpp::shutdown(); } + + /// Publish velocity msgs + /** + * linear - magnitude of the linear command in the geometry_msgs::twist message + * angular - the magnitude of the angular command in geometry_msgs::twist message + */ + void publish(double linear, double angular) + { + int wait_count = 0; + auto topic = velocity_publisher->get_topic_name(); + while (pub_node->count_subscribers(topic) == 0) + { + if (wait_count >= 5) + { + auto error_msg = std::string("publishing to ") + topic + " but no node subscribes to it"; + throw std::runtime_error(error_msg); + } + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + ++wait_count; + } + + geometry_msgs::msg::TwistStamped velocity_message; + velocity_message.header.stamp = pub_node->get_clock()->now(); + velocity_message.twist.linear.x = linear; + velocity_message.twist.angular.z = angular; + velocity_publisher->publish(velocity_message); + } + + /// \brief wait for the subscriber and publisher to completely setup + void waitForSetup() + { + constexpr std::chrono::seconds TIMEOUT{2}; + auto clock = pub_node->get_clock(); + auto start = clock->now(); + while (velocity_publisher->get_subscription_count() <= 0) + { + if ((clock->now() - start) > TIMEOUT) + { + FAIL(); + } + rclcpp::spin_some(pub_node); + } + } + + void assignResources() + { + std::vector state_ifs; + state_ifs.emplace_back(steering_joint_pos_state_); + state_ifs.emplace_back(traction_joint_vel_state_); + + std::vector command_ifs; + command_ifs.emplace_back(steering_joint_pos_cmd_); + command_ifs.emplace_back(traction_joint_vel_cmd_); + + controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); + } + + const std::string controller_name = "test_tricycle_controller"; + std::unique_ptr controller_; + + const std::string traction_joint_name = "traction_joint"; + const std::string steering_joint_name = "steering_joint"; + double position_ = 0.1; + double velocity_ = 0.2; + + hardware_interface::StateInterface steering_joint_pos_state_{ + steering_joint_name, HW_IF_POSITION, &position_}; + + hardware_interface::StateInterface traction_joint_vel_state_{ + traction_joint_name, HW_IF_VELOCITY, &velocity_}; + + hardware_interface::CommandInterface steering_joint_pos_cmd_{ + steering_joint_name, HW_IF_POSITION, &position_}; + + hardware_interface::CommandInterface traction_joint_vel_cmd_{ + traction_joint_name, HW_IF_VELOCITY, &velocity_}; + + rclcpp::Node::SharedPtr pub_node; + rclcpp::Publisher::SharedPtr velocity_publisher; +}; + +TEST_F(TestTricycleController, configure_fails_without_parameters) +{ + const auto ret = controller_->init(controller_name); + ASSERT_EQ(ret, controller_interface::return_type::OK); + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); +} + +TEST_F(TestTricycleController, configure_fails_if_only_traction_or_steering_side_defined) +{ + const auto ret = controller_->init(controller_name); + ASSERT_EQ(ret, controller_interface::return_type::OK); + + controller_->get_node()->set_parameter( + rclcpp::Parameter("traction_joint_name", rclcpp::ParameterValue(traction_joint_name))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("steering_joint_name", rclcpp::ParameterValue(std::string()))); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); + + controller_->get_node()->set_parameter( + rclcpp::Parameter("traction_joint_name", rclcpp::ParameterValue(std::string()))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("steering_joint_name", rclcpp::ParameterValue(steering_joint_name))); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); +} + +TEST_F(TestTricycleController, configure_succeeds_when_joints_are_specified) +{ + const auto ret = controller_->init(controller_name); + ASSERT_EQ(ret, controller_interface::return_type::OK); + + controller_->get_node()->set_parameter( + rclcpp::Parameter("traction_joint_name", rclcpp::ParameterValue(traction_joint_name))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("steering_joint_name", rclcpp::ParameterValue(steering_joint_name))); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); +} + +TEST_F(TestTricycleController, activate_fails_without_resources_assigned) +{ + const auto ret = controller_->init(controller_name); + ASSERT_EQ(ret, controller_interface::return_type::OK); + + controller_->get_node()->set_parameter( + rclcpp::Parameter("traction_joint_name", rclcpp::ParameterValue(traction_joint_name))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("steering_joint_name", rclcpp::ParameterValue(steering_joint_name))); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::ERROR); +} + +TEST_F(TestTricycleController, activate_succeeds_with_resources_assigned) +{ + const auto ret = controller_->init(controller_name); + ASSERT_EQ(ret, controller_interface::return_type::OK); + + // We implicitly test that by default position feedback is required + controller_->get_node()->set_parameter( + rclcpp::Parameter("traction_joint_name", rclcpp::ParameterValue(traction_joint_name))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("steering_joint_name", rclcpp::ParameterValue(steering_joint_name))); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + assignResources(); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); +} + +TEST_F(TestTricycleController, cleanup) +{ + const auto ret = controller_->init(controller_name); + ASSERT_EQ(ret, controller_interface::return_type::OK); + + controller_->get_node()->set_parameter( + rclcpp::Parameter("traction_joint_name", rclcpp::ParameterValue(traction_joint_name))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("steering_joint_name", rclcpp::ParameterValue(steering_joint_name))); + controller_->get_node()->set_parameter(rclcpp::Parameter("wheelbase", 1.2)); + controller_->get_node()->set_parameter(rclcpp::Parameter("wheel_radius", 0.12)); + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + auto state = controller_->get_node()->configure(); + ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); + assignResources(); + + state = controller_->get_node()->activate(); + ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); + + waitForSetup(); + + // send msg + const double linear = 1.0; + const double angular = 1.0; + publish(linear, angular); + controller_->wait_for_twist(executor); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + state = controller_->get_node()->deactivate(); + ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + state = controller_->get_node()->cleanup(); + ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); + + // should be stopped + EXPECT_EQ(0.0, steering_joint_pos_cmd_.get_value()); + EXPECT_EQ(0.0, traction_joint_vel_cmd_.get_value()); + + executor.cancel(); +} + +TEST_F(TestTricycleController, correct_initialization_using_parameters) +{ + const auto ret = controller_->init(controller_name); + ASSERT_EQ(ret, controller_interface::return_type::OK); + + controller_->get_node()->set_parameter( + rclcpp::Parameter("traction_joint_name", rclcpp::ParameterValue(traction_joint_name))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("steering_joint_name", rclcpp::ParameterValue(steering_joint_name))); + controller_->get_node()->set_parameter(rclcpp::Parameter("wheelbase", 0.4)); + controller_->get_node()->set_parameter(rclcpp::Parameter("wheel_radius", 1.0)); + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + auto state = controller_->get_node()->configure(); + assignResources(); + + ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); + EXPECT_EQ(position_, steering_joint_pos_cmd_.get_value()); + EXPECT_EQ(velocity_, traction_joint_vel_cmd_.get_value()); + + state = controller_->get_node()->activate(); + ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); + + // send msg + const double linear = 1.0; + const double angular = 0.0; + publish(linear, angular); + // wait for msg is be published to the system + ASSERT_TRUE(controller_->wait_for_twist(executor)); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + EXPECT_EQ(0.0, steering_joint_pos_cmd_.get_value()); + EXPECT_EQ(1.0, traction_joint_vel_cmd_.get_value()); + + // deactivated + // wait so controller process the second point when deactivated + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + state = controller_->get_node()->deactivate(); + ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_EQ(0.0, steering_joint_pos_cmd_.get_value()) << "Wheels are halted on deactivate()"; + EXPECT_EQ(0.0, traction_joint_vel_cmd_.get_value()) << "Wheels are halted on deactivate()"; + + // cleanup + state = controller_->get_node()->cleanup(); + ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(0.0, steering_joint_pos_cmd_.get_value()); + EXPECT_EQ(0.0, traction_joint_vel_cmd_.get_value()); + + state = controller_->get_node()->configure(); + ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); + executor.cancel(); +} diff --git a/tricycle_controller/tricycle_controller.xml b/tricycle_controller/tricycle_controller.xml new file mode 100644 index 0000000000..568b13532b --- /dev/null +++ b/tricycle_controller/tricycle_controller.xml @@ -0,0 +1,7 @@ + + + + The tricycle controller transforms linear and angular velocity messages into signals for steering and traction joints for a tricycle drive robot. + + + From 6fae035682637253b9c55cbfd23d5cd484d4cd7f Mon Sep 17 00:00:00 2001 From: petkovich Date: Tue, 20 Dec 2022 15:41:50 +0100 Subject: [PATCH 074/186] separated implementations in files --- steering_controllers/CMakeLists.txt | 9 +- .../ackermann_steering_controller.hpp | 32 ++++ .../bicycle_steering_controller.hpp | 35 ++++ ...s.hpp => tricycle_steering_controller.hpp} | 27 --- .../src/ackermann_steering_controller.cpp | 50 +++++ .../src/bicycle_steering_controller.cpp | 75 ++++++++ .../steering_controller_implementations.cpp | 174 ------------------ .../src/tricycle_steering_controller.cpp | 83 +++++++++ 8 files changed, 282 insertions(+), 203 deletions(-) create mode 100644 steering_controllers/include/steering_controllers/ackermann_steering_controller.hpp create mode 100644 steering_controllers/include/steering_controllers/bicycle_steering_controller.hpp rename steering_controllers/include/steering_controllers/{steering_controller_implementations.hpp => tricycle_steering_controller.hpp} (62%) create mode 100644 steering_controllers/src/ackermann_steering_controller.cpp create mode 100644 steering_controllers/src/bicycle_steering_controller.cpp delete mode 100644 steering_controllers/src/steering_controller_implementations.cpp create mode 100644 steering_controllers/src/tricycle_steering_controller.cpp diff --git a/steering_controllers/CMakeLists.txt b/steering_controllers/CMakeLists.txt index f77a3a60e9..c27002448b 100644 --- a/steering_controllers/CMakeLists.txt +++ b/steering_controllers/CMakeLists.txt @@ -25,6 +25,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS find_package(ament_cmake REQUIRED) find_package(generate_parameter_library REQUIRED) + foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() @@ -34,13 +35,15 @@ add_library( SHARED src/steering_controllers.cpp src/steering_odometry.cpp - src/steering_controller_implementations.cpp + src/ackermann_steering_controller.cpp + src/bicycle_steering_controller.cpp + src/tricycle_steering_controller.cpp ) generate_parameter_library(steering_controllers_parameters src/steering_controllers.yaml ) -#target_include_directories(${PROJECT_NAME} PRIVATE include) +# target_include_directories(${PROJECT_NAME} PRIVATE include) target_include_directories(${PROJECT_NAME} PUBLIC "$" "$") @@ -66,9 +69,11 @@ install( 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 diff --git a/steering_controllers/include/steering_controllers/ackermann_steering_controller.hpp b/steering_controllers/include/steering_controllers/ackermann_steering_controller.hpp new file mode 100644 index 0000000000..dada217d88 --- /dev/null +++ b/steering_controllers/include/steering_controllers/ackermann_steering_controller.hpp @@ -0,0 +1,32 @@ +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef STEERING_CONTROLLERS_IMPLEMENTATIONS_HPP_ +#define STEERING_CONTROLLERS_IMPLEMENTATIONS_HPP_ + +#include "steering_controllers/steering_controllers.hpp" + +namespace ackermann_steering_controller +{ +class AckermannSteeringController : public steering_controllers::SteeringControllers +{ +public: + AckermannSteeringController(); + + STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::CallbackReturn configure_odometry() + override; +}; +} // namespace ackermann_steering_controller + +#endif // STEERING_CONTROLLERS_IMPLEMENTATIONS_HPP_ diff --git a/steering_controllers/include/steering_controllers/bicycle_steering_controller.hpp b/steering_controllers/include/steering_controllers/bicycle_steering_controller.hpp new file mode 100644 index 0000000000..87da17ff70 --- /dev/null +++ b/steering_controllers/include/steering_controllers/bicycle_steering_controller.hpp @@ -0,0 +1,35 @@ +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef STEERING_CONTROLLERS_IMPLEMENTATIONS_HPP_ +#define STEERING_CONTROLLERS_IMPLEMENTATIONS_HPP_ + +#include "steering_controllers/steering_controllers.hpp" + +namespace bicycle_steering_controller +{ +class BicycleSteeringController : public steering_controllers::SteeringControllers +{ +public: + BicycleSteeringController(); + + STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::CallbackReturn configure_odometry() + override; + + STEERING_CONTROLLERS__VISIBILITY_PUBLIC bool update_odometry( + const rclcpp::Duration & period) override; +}; +} // namespace bicycle_steering_controller + +#endif // STEERING_CONTROLLERS_IMPLEMENTATIONS_HPP_ diff --git a/steering_controllers/include/steering_controllers/steering_controller_implementations.hpp b/steering_controllers/include/steering_controllers/tricycle_steering_controller.hpp similarity index 62% rename from steering_controllers/include/steering_controllers/steering_controller_implementations.hpp rename to steering_controllers/include/steering_controllers/tricycle_steering_controller.hpp index 617079b0e7..8beb4a1cb4 100644 --- a/steering_controllers/include/steering_controllers/steering_controller_implementations.hpp +++ b/steering_controllers/include/steering_controllers/tricycle_steering_controller.hpp @@ -17,33 +17,6 @@ #include "steering_controllers/steering_controllers.hpp" -namespace ackermann_steering_controller -{ -class AckermannSteeringController : public steering_controllers::SteeringControllers -{ -public: - AckermannSteeringController(); - - STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::CallbackReturn configure_odometry() - override; -}; -} // namespace ackermann_steering_controller - -namespace bicycle_steering_controller -{ -class BicycleSteeringController : public steering_controllers::SteeringControllers -{ -public: - BicycleSteeringController(); - - STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::CallbackReturn configure_odometry() - override; - - STEERING_CONTROLLERS__VISIBILITY_PUBLIC bool update_odometry( - const rclcpp::Duration & period) override; -}; -} // namespace bicycle_steering_controller - namespace tricycle_steering_controller { class TricycleSteeringController : public steering_controllers::SteeringControllers diff --git a/steering_controllers/src/ackermann_steering_controller.cpp b/steering_controllers/src/ackermann_steering_controller.cpp new file mode 100644 index 0000000000..0ee5f73833 --- /dev/null +++ b/steering_controllers/src/ackermann_steering_controller.cpp @@ -0,0 +1,50 @@ +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "steering_controllers/ackermann_steering_controller.hpp" + +namespace ackermann_steering_controller +{ +AckermannSteeringController::AckermannSteeringController() +: steering_controllers::SteeringControllers() +{ +} + +controller_interface::CallbackReturn AckermannSteeringController::configure_odometry() +{ + params_ = param_listener_->get_params(); + + const double wheel_radius = params_.wheel_radius_multiplier * params_.wheel_radius; + const double wheel_seperation = params_.wheel_separation_multiplier * params_.wheel_separation; + const double wheelbase = params_.wheelbase_multiplier * params_.wheelbase; + odometry_.set_wheel_params(wheel_radius, wheel_seperation, wheelbase); + odometry_.set_velocity_rolling_window_size(params_.velocity_rolling_window_size); + + // TODO: enable position/velocity configure + const size_t nr_state_itfs = 4; + const size_t nr_cmd_itfs = 4; + const size_t nr_ref_itfs = 2; + + set_interface_numbers(nr_state_itfs, nr_cmd_itfs, nr_ref_itfs); + + RCLCPP_INFO(get_node()->get_logger(), "ackermann odom configure successful"); + return controller_interface::CallbackReturn::SUCCESS; +} +} // namespace ackermann_steering_controller + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + ackermann_steering_controller::AckermannSteeringController, + controller_interface::ChainableControllerInterface) diff --git a/steering_controllers/src/bicycle_steering_controller.cpp b/steering_controllers/src/bicycle_steering_controller.cpp new file mode 100644 index 0000000000..5f0cce4b7f --- /dev/null +++ b/steering_controllers/src/bicycle_steering_controller.cpp @@ -0,0 +1,75 @@ +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "steering_controllers/bicycle_steering_controller.hpp" + +namespace bicycle_steering_controller +{ +BicycleSteeringController::BicycleSteeringController() : steering_controllers::SteeringControllers() +{ +} + +controller_interface::CallbackReturn BicycleSteeringController::configure_odometry() +{ + params_ = param_listener_->get_params(); + + const double wheel_seperation = params_.wheel_separation_multiplier * params_.wheel_separation; + const double wheel_radius = params_.wheel_radius_multiplier * params_.wheel_radius; + odometry_.set_wheel_params(wheel_radius, wheel_seperation); + odometry_.set_velocity_rolling_window_size(params_.velocity_rolling_window_size); + + // TODO: enable position/velocity configure + const size_t nr_state_itfs = 2; + const size_t nr_cmd_itfs = 2; + const size_t nr_ref_itfs = 2; + + set_interface_numbers(nr_state_itfs, nr_cmd_itfs, nr_ref_itfs); + + RCLCPP_INFO(get_node()->get_logger(), "bicycle odom configure successful"); + return controller_interface::CallbackReturn::SUCCESS; +} + +bool BicycleSteeringController::update_odometry(const rclcpp::Duration & period) +{ + if (params_.open_loop) + { + odometry_.update_open_loop(last_linear_velocity_, last_angular_velocity_, period.seconds()); + } + else + { + const double rear_wheel_value = state_interfaces_[0].get_value(); + const double steer_position = state_interfaces_[1].get_value() * params_.steer_pos_multiplier; + if (!std::isnan(rear_wheel_value) && !std::isnan(steer_position)) + { + if (params_.position_feedback) + { + // Estimate linear and angular velocity using joint information + odometry_.update_from_position(rear_wheel_value, steer_position, period.seconds()); + } + else + { + // Estimate linear and angular velocity using joint information + odometry_.update_from_velocity(rear_wheel_value, steer_position, period.seconds()); + } + } + } + return true; +} +} // namespace bicycle_steering_controller + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + bicycle_steering_controller::BicycleSteeringController, + controller_interface::ChainableControllerInterface) diff --git a/steering_controllers/src/steering_controller_implementations.cpp b/steering_controllers/src/steering_controller_implementations.cpp deleted file mode 100644 index 297f29b554..0000000000 --- a/steering_controllers/src/steering_controller_implementations.cpp +++ /dev/null @@ -1,174 +0,0 @@ -// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "steering_controllers/steering_controller_implementations.hpp" - -namespace ackermann_steering_controller -{ -AckermannSteeringController::AckermannSteeringController() -: steering_controllers::SteeringControllers() -{ -} - -controller_interface::CallbackReturn AckermannSteeringController::configure_odometry() -{ - params_ = param_listener_->get_params(); - - const double wheel_radius = params_.wheel_radius_multiplier * params_.wheel_radius; - const double wheel_seperation = params_.wheel_separation_multiplier * params_.wheel_separation; - const double wheelbase = params_.wheelbase_multiplier * params_.wheelbase; - odometry_.set_wheel_params(wheel_radius, wheel_seperation, wheelbase); - odometry_.set_velocity_rolling_window_size(params_.velocity_rolling_window_size); - - // TODO: enable position/velocity configure - const size_t nr_state_itfs = 4; - const size_t nr_cmd_itfs = 4; - const size_t nr_ref_itfs = 2; - - set_interface_numbers(nr_state_itfs, nr_cmd_itfs, nr_ref_itfs); - - RCLCPP_INFO(get_node()->get_logger(), "ackermann odom configure successful"); - return controller_interface::CallbackReturn::SUCCESS; -} -} // namespace ackermann_steering_controller - -namespace bicycle_steering_controller -{ -BicycleSteeringController::BicycleSteeringController() : steering_controllers::SteeringControllers() -{ -} - -controller_interface::CallbackReturn BicycleSteeringController::configure_odometry() -{ - params_ = param_listener_->get_params(); - - const double wheel_seperation = params_.wheel_separation_multiplier * params_.wheel_separation; - const double wheel_radius = params_.wheel_radius_multiplier * params_.wheel_radius; - odometry_.set_wheel_params(wheel_radius, wheel_seperation); - odometry_.set_velocity_rolling_window_size(params_.velocity_rolling_window_size); - - // TODO: enable position/velocity configure - const size_t nr_state_itfs = 2; - const size_t nr_cmd_itfs = 2; - const size_t nr_ref_itfs = 2; - - set_interface_numbers(nr_state_itfs, nr_cmd_itfs, nr_ref_itfs); - - RCLCPP_INFO(get_node()->get_logger(), "bicycle odom configure successful"); - return controller_interface::CallbackReturn::SUCCESS; -} - -bool BicycleSteeringController::update_odometry(const rclcpp::Duration & period) -{ - if (params_.open_loop) - { - odometry_.update_open_loop(last_linear_velocity_, last_angular_velocity_, period.seconds()); - } - else - { - const double rear_wheel_value = state_interfaces_[0].get_value(); - const double steer_position = state_interfaces_[1].get_value() * params_.steer_pos_multiplier; - if (!std::isnan(rear_wheel_value) && !std::isnan(steer_position)) - { - if (params_.position_feedback) - { - // Estimate linear and angular velocity using joint information - odometry_.update_from_position(rear_wheel_value, steer_position, period.seconds()); - } - else - { - // Estimate linear and angular velocity using joint information - odometry_.update_from_velocity(rear_wheel_value, steer_position, period.seconds()); - } - } - } - return true; -} -} // namespace bicycle_steering_controller - -namespace tricycle_steering_controller -{ -TricycleSteeringController::TricycleSteeringController() -: steering_controllers::SteeringControllers() -{ -} - -controller_interface::CallbackReturn TricycleSteeringController::configure_odometry() -{ - params_ = param_listener_->get_params(); - - const double wheel_radius = params_.wheel_radius_multiplier * params_.wheel_radius; - const double wheel_seperation = params_.wheel_separation_multiplier * params_.wheel_separation; - const double wheelbase = params_.wheelbase_multiplier * params_.wheelbase; - odometry_.set_wheel_params(wheel_radius, wheel_seperation, wheelbase); - odometry_.set_velocity_rolling_window_size(params_.velocity_rolling_window_size); - - // TODO: enable position/velocity configure - const size_t nr_state_itfs = 3; - const size_t nr_cmd_itfs = 3; - const size_t nr_ref_itfs = 2; - - set_interface_numbers(nr_state_itfs, nr_cmd_itfs, nr_ref_itfs); - - RCLCPP_INFO(get_node()->get_logger(), "ackermann odom configure successful"); - return controller_interface::CallbackReturn::SUCCESS; -} - -bool TricycleSteeringController::update_odometry(const rclcpp::Duration & period) -{ - if (params_.open_loop) - { - odometry_.update_open_loop(last_linear_velocity_, last_angular_velocity_, period.seconds()); - } - else - { - const double rear_right_wheel_value = state_interfaces_[0].get_value(); - const double rear_left_wheel_value = state_interfaces_[1].get_value(); - const double steer_position = state_interfaces_[2].get_value() * params_.steer_pos_multiplier; - if ( - !std::isnan(rear_right_wheel_value) && !std::isnan(rear_left_wheel_value) && - !std::isnan(steer_position)) - { - if (params_.position_feedback) - { - // Estimate linear and angular velocity using joint information - odometry_.update_from_position( - rear_right_wheel_value, rear_left_wheel_value, steer_position, period.seconds()); - } - else - { - // Estimate linear and angular velocity using joint information - odometry_.update_from_velocity( - rear_right_wheel_value, rear_left_wheel_value, steer_position, period.seconds()); - } - } - } - return true; -} - -} // namespace tricycle_steering_controller - -#include "pluginlib/class_list_macros.hpp" - -PLUGINLIB_EXPORT_CLASS( - ackermann_steering_controller::AckermannSteeringController, - controller_interface::ChainableControllerInterface) - -PLUGINLIB_EXPORT_CLASS( - bicycle_steering_controller::BicycleSteeringController, - controller_interface::ChainableControllerInterface) - -PLUGINLIB_EXPORT_CLASS( - tricycle_steering_controller::TricycleSteeringController, - controller_interface::ChainableControllerInterface) diff --git a/steering_controllers/src/tricycle_steering_controller.cpp b/steering_controllers/src/tricycle_steering_controller.cpp new file mode 100644 index 0000000000..9a3db36657 --- /dev/null +++ b/steering_controllers/src/tricycle_steering_controller.cpp @@ -0,0 +1,83 @@ +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "steering_controllers/tricycle_steering_controller.hpp" + +namespace tricycle_steering_controller +{ +TricycleSteeringController::TricycleSteeringController() +: steering_controllers::SteeringControllers() +{ +} + +controller_interface::CallbackReturn TricycleSteeringController::configure_odometry() +{ + params_ = param_listener_->get_params(); + + const double wheel_radius = params_.wheel_radius_multiplier * params_.wheel_radius; + const double wheel_seperation = params_.wheel_separation_multiplier * params_.wheel_separation; + const double wheelbase = params_.wheelbase_multiplier * params_.wheelbase; + odometry_.set_wheel_params(wheel_radius, wheel_seperation, wheelbase); + odometry_.set_velocity_rolling_window_size(params_.velocity_rolling_window_size); + + // TODO: enable position/velocity configure + const size_t nr_state_itfs = 3; + const size_t nr_cmd_itfs = 3; + const size_t nr_ref_itfs = 2; + + set_interface_numbers(nr_state_itfs, nr_cmd_itfs, nr_ref_itfs); + + RCLCPP_INFO(get_node()->get_logger(), "ackermann odom configure successful"); + return controller_interface::CallbackReturn::SUCCESS; +} + +bool TricycleSteeringController::update_odometry(const rclcpp::Duration & period) +{ + if (params_.open_loop) + { + odometry_.update_open_loop(last_linear_velocity_, last_angular_velocity_, period.seconds()); + } + else + { + const double rear_right_wheel_value = state_interfaces_[0].get_value(); + const double rear_left_wheel_value = state_interfaces_[1].get_value(); + const double steer_position = state_interfaces_[2].get_value() * params_.steer_pos_multiplier; + if ( + !std::isnan(rear_right_wheel_value) && !std::isnan(rear_left_wheel_value) && + !std::isnan(steer_position)) + { + if (params_.position_feedback) + { + // Estimate linear and angular velocity using joint information + odometry_.update_from_position( + rear_right_wheel_value, rear_left_wheel_value, steer_position, period.seconds()); + } + else + { + // Estimate linear and angular velocity using joint information + odometry_.update_from_velocity( + rear_right_wheel_value, rear_left_wheel_value, steer_position, period.seconds()); + } + } + } + return true; +} + +} // namespace tricycle_steering_controller + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + tricycle_steering_controller::TricycleSteeringController, + controller_interface::ChainableControllerInterface) From 2d86708336603069fe86f3305b1b5a1791f2085a Mon Sep 17 00:00:00 2001 From: petkovich Date: Tue, 20 Dec 2022 15:42:57 +0100 Subject: [PATCH 075/186] remove lint tests --- steering_controllers/CMakeLists.txt | 12 +----------- 1 file changed, 1 insertion(+), 11 deletions(-) diff --git a/steering_controllers/CMakeLists.txt b/steering_controllers/CMakeLists.txt index c27002448b..4cf697c33a 100644 --- a/steering_controllers/CMakeLists.txt +++ b/steering_controllers/CMakeLists.txt @@ -68,17 +68,7 @@ install( ) 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() + # TODO endif() ament_export_include_directories( From 4d14b5f9aee096e1332c2b5e7caa8ee440becc1d Mon Sep 17 00:00:00 2001 From: petkovich Date: Tue, 20 Dec 2022 16:06:12 +0100 Subject: [PATCH 076/186] ControllerTwistReferenceMsg, ControllerAckermannReferenceMsg --- .../steering_controllers/steering_controllers.hpp | 12 ++++++------ steering_controllers/src/steering_controllers.cpp | 12 +++++++----- 2 files changed, 13 insertions(+), 11 deletions(-) diff --git a/steering_controllers/include/steering_controllers/steering_controllers.hpp b/steering_controllers/include/steering_controllers/steering_controllers.hpp index 093c9aa6f3..9236f657ca 100644 --- a/steering_controllers/include/steering_controllers/steering_controllers.hpp +++ b/steering_controllers/include/steering_controllers/steering_controllers.hpp @@ -35,7 +35,7 @@ #include "steering_controllers_parameters.hpp" // TODO(anyone): Replace with controller specific messages -#include "ackermann_msgs/msg/ackermann_drive.hpp" +#include "ackermann_msgs/msg/ackermann_drive_stamped.hpp" #include "control_msgs/msg/steering_controller_status.hpp" #include "geometry_msgs/msg/twist.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" @@ -78,8 +78,8 @@ class SteeringControllers : public controller_interface::ChainableControllerInte STEERING_CONTROLLERS__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 = geometry_msgs::msg::TwistStamped; + using ControllerAckermannReferenceMsg = ackermann_msgs::msg::AckermannDriveStamped; + using ControllerTwistReferenceMsg = geometry_msgs::msg::TwistStamped; using ControllerStateMsgOdom = nav_msgs::msg::Odometry; using ControllerStateMsgTf = tf2_msgs::msg::TFMessage; @@ -88,9 +88,9 @@ class SteeringControllers : public controller_interface::ChainableControllerInte steering_controllers::Params params_; // Command subscribers and Controller State publisher - rclcpp::Subscription::SharedPtr ref_subscriber_ = nullptr; + rclcpp::Subscription::SharedPtr ref_subscriber_ = nullptr; rclcpp::Subscription::SharedPtr ref_subscriber_unstamped_ = nullptr; - realtime_tools::RealtimeBuffer> input_ref_; + realtime_tools::RealtimeBuffer> input_ref_; rclcpp::Duration ref_timeout_ = rclcpp::Duration::from_seconds(0.0); // 0ms using ControllerStatePublisherOdom = realtime_tools::RealtimePublisher; @@ -134,7 +134,7 @@ class SteeringControllers : public controller_interface::ChainableControllerInte private: // callback for topic interface STEERING_CONTROLLERS__VISIBILITY_LOCAL void reference_callback( - const std::shared_ptr msg); + const std::shared_ptr msg); void reference_callback_unstamped(const std::shared_ptr msg); }; diff --git a/steering_controllers/src/steering_controllers.cpp b/steering_controllers/src/steering_controllers.cpp index 60962a3d51..dad2232f41 100644 --- a/steering_controllers/src/steering_controllers.cpp +++ b/steering_controllers/src/steering_controllers.cpp @@ -43,11 +43,12 @@ static constexpr rmw_qos_profile_t rmw_qos_profile_services_hist_keep_all = { RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT, false}; -using ControllerReferenceMsg = steering_controllers::SteeringControllers::ControllerReferenceMsg; +using ControllerTwistReferenceMsg = + steering_controllers::SteeringControllers::ControllerTwistReferenceMsg; // called from RT control loop void reset_controller_reference_msg( - const std::shared_ptr & msg, + const std::shared_ptr & msg, const std::shared_ptr & node) { msg->header.stamp = node->now(); @@ -107,7 +108,7 @@ controller_interface::CallbackReturn SteeringControllers::on_configure( ref_timeout_ = rclcpp::Duration::from_seconds(params_.reference_timeout); if (params_.use_stamped_vel) { - ref_subscriber_ = get_node()->create_subscription( + ref_subscriber_ = get_node()->create_subscription( "~/reference", subscribers_qos, std::bind(&SteeringControllers::reference_callback, this, std::placeholders::_1)); } @@ -118,7 +119,8 @@ controller_interface::CallbackReturn SteeringControllers::on_configure( std::bind(&SteeringControllers::reference_callback_unstamped, this, std::placeholders::_1)); } - std::shared_ptr msg = std::make_shared(); + std::shared_ptr msg = + std::make_shared(); reset_controller_reference_msg(msg, get_node()); input_ref_.writeFromNonRT(msg); @@ -214,7 +216,7 @@ controller_interface::CallbackReturn SteeringControllers::on_configure( return controller_interface::CallbackReturn::SUCCESS; } -void SteeringControllers::reference_callback(const std::shared_ptr msg) +void SteeringControllers::reference_callback(const std::shared_ptr msg) { // if no timestamp provided use current time for command timestamp if (msg->header.stamp.sec == 0 && msg->header.stamp.nanosec == 0u) From 4a910cd0f44cb71e40e1eba6a86bd9da3a2dae37 Mon Sep 17 00:00:00 2001 From: petkovich Date: Tue, 20 Dec 2022 16:10:03 +0100 Subject: [PATCH 077/186] unstamped twist deprecation, ackermann subscriber --- .../include/steering_controllers/steering_controllers.hpp | 3 ++- steering_controllers/src/steering_controllers.cpp | 3 ++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/steering_controllers/include/steering_controllers/steering_controllers.hpp b/steering_controllers/include/steering_controllers/steering_controllers.hpp index 9236f657ca..75aaacb321 100644 --- a/steering_controllers/include/steering_controllers/steering_controllers.hpp +++ b/steering_controllers/include/steering_controllers/steering_controllers.hpp @@ -88,7 +88,8 @@ class SteeringControllers : public controller_interface::ChainableControllerInte steering_controllers::Params params_; // Command subscribers and Controller State publisher - rclcpp::Subscription::SharedPtr ref_subscriber_ = nullptr; + rclcpp::Subscription::SharedPtr ref_subscriber_twist_ = nullptr; + rclcpp::Subscription::SharedPtr ref_subscriber_ackermann_ = nullptr; rclcpp::Subscription::SharedPtr ref_subscriber_unstamped_ = nullptr; realtime_tools::RealtimeBuffer> input_ref_; rclcpp::Duration ref_timeout_ = rclcpp::Duration::from_seconds(0.0); // 0ms diff --git a/steering_controllers/src/steering_controllers.cpp b/steering_controllers/src/steering_controllers.cpp index dad2232f41..295a1d98cb 100644 --- a/steering_controllers/src/steering_controllers.cpp +++ b/steering_controllers/src/steering_controllers.cpp @@ -108,7 +108,7 @@ controller_interface::CallbackReturn SteeringControllers::on_configure( ref_timeout_ = rclcpp::Duration::from_seconds(params_.reference_timeout); if (params_.use_stamped_vel) { - ref_subscriber_ = get_node()->create_subscription( + ref_subscriber_twist_ = get_node()->create_subscription( "~/reference", subscribers_qos, std::bind(&SteeringControllers::reference_callback, this, std::placeholders::_1)); } @@ -246,6 +246,7 @@ void SteeringControllers::reference_callback(const std::shared_ptr msg) { + RCLCPP_WARN(get_node()->get_logger(), "Using unstamped messages is deprecated."); auto twist_stamped = *(input_ref_.readFromNonRT()); twist_stamped->header.stamp = get_node()->now(); // if no timestamp provided use current time for command timestamp From 8586362ed216e8375cb2d8335c933932cf01bc7d Mon Sep 17 00:00:00 2001 From: petkovich Date: Tue, 20 Dec 2022 16:13:26 +0100 Subject: [PATCH 078/186] authors, maintainers --- .../ackermann_steering_controller.hpp | 3 ++ .../bicycle_steering_controller.hpp | 3 ++ .../steering_odometry.hpp | 36 +++++++++---------- .../tricycle_steering_controller.hpp | 3 ++ .../steering_controllers/visibility_control.h | 3 ++ steering_controllers/package.xml | 7 +++- 6 files changed, 34 insertions(+), 21 deletions(-) diff --git a/steering_controllers/include/steering_controllers/ackermann_steering_controller.hpp b/steering_controllers/include/steering_controllers/ackermann_steering_controller.hpp index dada217d88..aac567123d 100644 --- a/steering_controllers/include/steering_controllers/ackermann_steering_controller.hpp +++ b/steering_controllers/include/steering_controllers/ackermann_steering_controller.hpp @@ -11,6 +11,9 @@ // 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. +// +// Authors: dr. sc. Tomislav Petkovic, Dr. Ing. Denis Štogl +// #ifndef STEERING_CONTROLLERS_IMPLEMENTATIONS_HPP_ #define STEERING_CONTROLLERS_IMPLEMENTATIONS_HPP_ diff --git a/steering_controllers/include/steering_controllers/bicycle_steering_controller.hpp b/steering_controllers/include/steering_controllers/bicycle_steering_controller.hpp index 87da17ff70..fd9e30f8a0 100644 --- a/steering_controllers/include/steering_controllers/bicycle_steering_controller.hpp +++ b/steering_controllers/include/steering_controllers/bicycle_steering_controller.hpp @@ -11,6 +11,9 @@ // 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. +// +// Authors: dr. sc. Tomislav Petkovic, Dr. Ing. Denis Štogl +// #ifndef STEERING_CONTROLLERS_IMPLEMENTATIONS_HPP_ #define STEERING_CONTROLLERS_IMPLEMENTATIONS_HPP_ diff --git a/steering_controllers/include/steering_controllers/steering_odometry.hpp b/steering_controllers/include/steering_controllers/steering_odometry.hpp index 2adb76fc6b..3b31348dff 100644 --- a/steering_controllers/include/steering_controllers/steering_odometry.hpp +++ b/steering_controllers/include/steering_controllers/steering_odometry.hpp @@ -1,23 +1,19 @@ -/********************************************************************* -* Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) -* -* Licensed under the Apache License, Version 2.0 (the "License"); -* you may not use this file except in compliance with the License. -* You may obtain a copy of the License at -* -* http://www.apache.org/licenses/LICENSE-2.0 -* -* Unless required by applicable law or agreed to in writing, software -* distributed under the License is distributed on an "AS IS" BASIS, -* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -* See the License for the specific language governing permissions and -* limitations under the License. - *********************************************************************/ - -/* - * Author: dr. sc. Tomislav Petkovic - * Author: Dr. Ing. Denis Stogl - */ +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Authors: dr. sc. Tomislav Petkovic, Dr. Ing. Denis Štogl +// #pragma once diff --git a/steering_controllers/include/steering_controllers/tricycle_steering_controller.hpp b/steering_controllers/include/steering_controllers/tricycle_steering_controller.hpp index 8beb4a1cb4..440d8d42c4 100644 --- a/steering_controllers/include/steering_controllers/tricycle_steering_controller.hpp +++ b/steering_controllers/include/steering_controllers/tricycle_steering_controller.hpp @@ -11,6 +11,9 @@ // 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. +// +// Authors: dr. sc. Tomislav Petkovic, Dr. Ing. Denis Štogl +// #ifndef STEERING_CONTROLLERS_IMPLEMENTATIONS_HPP_ #define STEERING_CONTROLLERS_IMPLEMENTATIONS_HPP_ diff --git a/steering_controllers/include/steering_controllers/visibility_control.h b/steering_controllers/include/steering_controllers/visibility_control.h index 3601423cae..9ee0e317fb 100644 --- a/steering_controllers/include/steering_controllers/visibility_control.h +++ b/steering_controllers/include/steering_controllers/visibility_control.h @@ -11,6 +11,9 @@ // 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. +// +// Authors: dr. sc. Tomislav Petkovic, Dr. Ing. Denis Štogl +// #ifndef STEERING_CONTROLLERS__VISIBILITY_CONTROL_H_ #define STEERING_CONTROLLERS__VISIBILITY_CONTROL_H_ diff --git a/steering_controllers/package.xml b/steering_controllers/package.xml index 904774a81c..b764fdd832 100644 --- a/steering_controllers/package.xml +++ b/steering_controllers/package.xml @@ -5,9 +5,14 @@ 0.0.0 Package for steering robot configurations including odometry and interfaces. Apache License 2.0 - dr. sc. Tomislav Petkovic Bence Magyar Dr.-Ing. Denis Štogl + dr. sc. Tomislav Petkovic + Tony Najjar + + Dr.-Ing. Denis Štogl + dr. sc. Tomislav Petkovic + Tony Najjar generate_parameter_library From f4a1880f1c0b82c956442766529dbaabf9676001 Mon Sep 17 00:00:00 2001 From: petkovich Date: Tue, 20 Dec 2022 16:15:32 +0100 Subject: [PATCH 079/186] removed rmw_qos_profile_services_hist_keep_all --- steering_controllers/src/steering_controllers.cpp | 13 ------------- 1 file changed, 13 deletions(-) diff --git a/steering_controllers/src/steering_controllers.cpp b/steering_controllers/src/steering_controllers.cpp index 295a1d98cb..6759d82e4f 100644 --- a/steering_controllers/src/steering_controllers.cpp +++ b/steering_controllers/src/steering_controllers.cpp @@ -30,19 +30,6 @@ 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 ControllerTwistReferenceMsg = steering_controllers::SteeringControllers::ControllerTwistReferenceMsg; From b1a62f4541ec24d513f936f9cd32586d6c90b58e Mon Sep 17 00:00:00 2001 From: petkovich Date: Tue, 20 Dec 2022 16:26:20 +0100 Subject: [PATCH 080/186] changed deprecation error --- steering_controllers/src/steering_controllers.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/steering_controllers/src/steering_controllers.cpp b/steering_controllers/src/steering_controllers.cpp index 6759d82e4f..6a5ef14d8a 100644 --- a/steering_controllers/src/steering_controllers.cpp +++ b/steering_controllers/src/steering_controllers.cpp @@ -233,7 +233,11 @@ void SteeringControllers::reference_callback(const std::shared_ptr msg) { - RCLCPP_WARN(get_node()->get_logger(), "Using unstamped messages is deprecated."); + RCLCPP_WARN( + get_node()->get_logger(), + "Use of Twist message without stamped is deprecated and it will be removed in ROS 2 J-Turtle " + "version. Use '~/reference' topic with 'geometry_msgs::msg::TwistStamped' message type in the " + "future."); auto twist_stamped = *(input_ref_.readFromNonRT()); twist_stamped->header.stamp = get_node()->now(); // if no timestamp provided use current time for command timestamp From d28e2a4f50d641720bb874a372f28103d28082f2 Mon Sep 17 00:00:00 2001 From: petkovich Date: Tue, 20 Dec 2022 16:35:00 +0100 Subject: [PATCH 081/186] parameters configured in base class, removed some comments --- .../src/ackermann_steering_controller.cpp | 2 -- .../src/bicycle_steering_controller.cpp | 2 -- steering_controllers/src/steering_controllers.cpp | 11 +++-------- .../src/tricycle_steering_controller.cpp | 2 -- 4 files changed, 3 insertions(+), 14 deletions(-) diff --git a/steering_controllers/src/ackermann_steering_controller.cpp b/steering_controllers/src/ackermann_steering_controller.cpp index 0ee5f73833..d23c3e8bc1 100644 --- a/steering_controllers/src/ackermann_steering_controller.cpp +++ b/steering_controllers/src/ackermann_steering_controller.cpp @@ -23,8 +23,6 @@ AckermannSteeringController::AckermannSteeringController() controller_interface::CallbackReturn AckermannSteeringController::configure_odometry() { - params_ = param_listener_->get_params(); - const double wheel_radius = params_.wheel_radius_multiplier * params_.wheel_radius; const double wheel_seperation = params_.wheel_separation_multiplier * params_.wheel_separation; const double wheelbase = params_.wheelbase_multiplier * params_.wheelbase; diff --git a/steering_controllers/src/bicycle_steering_controller.cpp b/steering_controllers/src/bicycle_steering_controller.cpp index 5f0cce4b7f..4a39d16860 100644 --- a/steering_controllers/src/bicycle_steering_controller.cpp +++ b/steering_controllers/src/bicycle_steering_controller.cpp @@ -22,8 +22,6 @@ BicycleSteeringController::BicycleSteeringController() : steering_controllers::S controller_interface::CallbackReturn BicycleSteeringController::configure_odometry() { - params_ = param_listener_->get_params(); - const double wheel_seperation = params_.wheel_separation_multiplier * params_.wheel_separation; const double wheel_radius = params_.wheel_radius_multiplier * params_.wheel_radius; odometry_.set_wheel_params(wheel_radius, wheel_seperation); diff --git a/steering_controllers/src/steering_controllers.cpp b/steering_controllers/src/steering_controllers.cpp index 6a5ef14d8a..080862c1a6 100644 --- a/steering_controllers/src/steering_controllers.cpp +++ b/steering_controllers/src/steering_controllers.cpp @@ -85,7 +85,8 @@ controller_interface::CallbackReturn SteeringControllers::configure_odometry() controller_interface::CallbackReturn SteeringControllers::on_configure( const rclcpp_lifecycle::State & /*previous_state*/) { - auto odometry_conf_msg = configure_odometry(); + params_ = param_listener_->get_params(); + configure_odometry(); // topics QoS auto subscribers_qos = rclcpp::SystemDefaultsQoS(); subscribers_qos.keep_last(1); @@ -296,7 +297,7 @@ controller_interface::InterfaceConfiguration SteeringControllers::state_interfac const auto rear_wheel_feedback = params_.position_feedback ? hardware_interface::HW_IF_POSITION : hardware_interface::HW_IF_VELOCITY; - for (size_t i = 0; i < params_.front_steer_names.size(); i++) + for (size_t i = 0; i < params_.rear_wheel_names.size(); i++) { state_interfaces_config.names.push_back( params_.rear_wheel_names[i] + "/" + rear_wheel_feedback); @@ -475,9 +476,3 @@ controller_interface::return_type SteeringControllers::update_and_write_commands } } // namespace steering_controllers - -// #include "pluginlib/class_list_macros.hpp" - -// PLUGINLIB_EXPORT_CLASS( -// steering_controllers::SteeringControllers, -// controller_interface::ChainableControllerInterface) diff --git a/steering_controllers/src/tricycle_steering_controller.cpp b/steering_controllers/src/tricycle_steering_controller.cpp index 9a3db36657..ecb581e075 100644 --- a/steering_controllers/src/tricycle_steering_controller.cpp +++ b/steering_controllers/src/tricycle_steering_controller.cpp @@ -23,8 +23,6 @@ TricycleSteeringController::TricycleSteeringController() controller_interface::CallbackReturn TricycleSteeringController::configure_odometry() { - params_ = param_listener_->get_params(); - const double wheel_radius = params_.wheel_radius_multiplier * params_.wheel_radius; const double wheel_seperation = params_.wheel_separation_multiplier * params_.wheel_separation; const double wheelbase = params_.wheelbase_multiplier * params_.wheelbase; From 9fe998a8577e8b6292a38602521182070927a0e7 Mon Sep 17 00:00:00 2001 From: petkovich Date: Wed, 21 Dec 2022 16:30:55 +0100 Subject: [PATCH 082/186] doc folder --- steering_controllers/doc/userdoc.rst | 23 +++++++++++++++++++++++ 1 file changed, 23 insertions(+) create mode 100644 steering_controllers/doc/userdoc.rst diff --git a/steering_controllers/doc/userdoc.rst b/steering_controllers/doc/userdoc.rst new file mode 100644 index 0000000000..612620aace --- /dev/null +++ b/steering_controllers/doc/userdoc.rst @@ -0,0 +1,23 @@ +.. _steering_controllers_userdoc: + +steering_controllers +===================== + +Controllers for mobile robots with steering drive. +Input for control are robot base_link twist stamped commands which are translated to traction and steering commands for the drive base. Odometry is computed from hardware feedback and published. + +Velocity commands +----------------- + +The controller works with a velocity twist from which it extracts +the x component of the linear velocity and the z component of the angular velocity. +Velocities on other components are ignored. + + +Other features +-------------- + + Realtime-safe implementation. + Odometry publishing + Velocity, acceleration and jerk limits + Automatic stop after command timeout From 453a534218172d7d4282d433442ead1dde3199c9 Mon Sep 17 00:00:00 2001 From: petkovich Date: Wed, 21 Dec 2022 16:55:33 +0100 Subject: [PATCH 083/186] virtual classes for odometry --- .../ackermann_steering_controller.hpp | 3 ++ .../steering_controllers.hpp | 4 +-- .../src/ackermann_steering_controller.cpp | 28 +++++++++++++++++++ .../src/steering_controllers.cpp | 12 -------- .../src/tricycle_steering_controller.cpp | 2 +- 5 files changed, 34 insertions(+), 15 deletions(-) diff --git a/steering_controllers/include/steering_controllers/ackermann_steering_controller.hpp b/steering_controllers/include/steering_controllers/ackermann_steering_controller.hpp index aac567123d..03bb11ffcb 100644 --- a/steering_controllers/include/steering_controllers/ackermann_steering_controller.hpp +++ b/steering_controllers/include/steering_controllers/ackermann_steering_controller.hpp @@ -29,6 +29,9 @@ class AckermannSteeringController : public steering_controllers::SteeringControl STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::CallbackReturn configure_odometry() override; + + STEERING_CONTROLLERS__VISIBILITY_PUBLIC bool update_odometry( + const rclcpp::Duration & period) override; }; } // namespace ackermann_steering_controller diff --git a/steering_controllers/include/steering_controllers/steering_controllers.hpp b/steering_controllers/include/steering_controllers/steering_controllers.hpp index 75aaacb321..c93ae5a2b7 100644 --- a/steering_controllers/include/steering_controllers/steering_controllers.hpp +++ b/steering_controllers/include/steering_controllers/steering_controllers.hpp @@ -58,10 +58,10 @@ class SteeringControllers : public controller_interface::ChainableControllerInte state_interface_configuration() const override; virtual STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::CallbackReturn - configure_odometry(); + configure_odometry() = 0; virtual STEERING_CONTROLLERS__VISIBILITY_PUBLIC bool update_odometry( - const rclcpp::Duration & period); + const rclcpp::Duration & period) = 0; STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::CallbackReturn on_configure( const rclcpp_lifecycle::State & previous_state) override; diff --git a/steering_controllers/src/ackermann_steering_controller.cpp b/steering_controllers/src/ackermann_steering_controller.cpp index d23c3e8bc1..24604f2130 100644 --- a/steering_controllers/src/ackermann_steering_controller.cpp +++ b/steering_controllers/src/ackermann_steering_controller.cpp @@ -39,6 +39,34 @@ controller_interface::CallbackReturn AckermannSteeringController::configure_odom RCLCPP_INFO(get_node()->get_logger(), "ackermann odom configure successful"); return controller_interface::CallbackReturn::SUCCESS; } + +// TODO: this is bicycle odometry ATM, implement ackermann +bool AckermannSteeringController::update_odometry(const rclcpp::Duration & period) +{ + if (params_.open_loop) + { + odometry_.update_open_loop(last_linear_velocity_, last_angular_velocity_, period.seconds()); + } + else + { + const double rear_wheel_value = state_interfaces_[0].get_value(); + const double steer_position = state_interfaces_[1].get_value() * params_.steer_pos_multiplier; + if (!std::isnan(rear_wheel_value) && !std::isnan(steer_position)) + { + if (params_.position_feedback) + { + // Estimate linear and angular velocity using joint information + odometry_.update_from_position(rear_wheel_value, steer_position, period.seconds()); + } + else + { + // Estimate linear and angular velocity using joint information + odometry_.update_from_velocity(rear_wheel_value, steer_position, period.seconds()); + } + } + } + return true; +} } // namespace ackermann_steering_controller #include "pluginlib/class_list_macros.hpp" diff --git a/steering_controllers/src/steering_controllers.cpp b/steering_controllers/src/steering_controllers.cpp index 080862c1a6..3a847ce4ff 100644 --- a/steering_controllers/src/steering_controllers.cpp +++ b/steering_controllers/src/steering_controllers.cpp @@ -76,12 +76,6 @@ controller_interface::CallbackReturn SteeringControllers::set_interface_numbers( nr_ref_itfs_ = nr_ref_itfs; } -controller_interface::CallbackReturn SteeringControllers::configure_odometry() -{ - RCLCPP_INFO(get_node()->get_logger(), "Configure odometry not implemented"); - return controller_interface::CallbackReturn::ERROR; -} - controller_interface::CallbackReturn SteeringControllers::on_configure( const rclcpp_lifecycle::State & /*previous_state*/) { @@ -383,12 +377,6 @@ controller_interface::return_type SteeringControllers::update_reference_from_sub return controller_interface::return_type::OK; } -bool SteeringControllers::update_odometry(const rclcpp::Duration & period) -{ - RCLCPP_INFO(get_node()->get_logger(), "Odometry update not implemented"); - return false; -} - controller_interface::return_type SteeringControllers::update_and_write_commands( const rclcpp::Time & time, const rclcpp::Duration & period) { diff --git a/steering_controllers/src/tricycle_steering_controller.cpp b/steering_controllers/src/tricycle_steering_controller.cpp index ecb581e075..b766462ca3 100644 --- a/steering_controllers/src/tricycle_steering_controller.cpp +++ b/steering_controllers/src/tricycle_steering_controller.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) +// Copyright 2022 Pixel Robotics. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. From e46bafac5cb6f671ff7f836ac577528bb8841253 Mon Sep 17 00:00:00 2001 From: petkovich Date: Wed, 21 Dec 2022 17:08:29 +0100 Subject: [PATCH 084/186] ackermann odometry implementation --- .../steering_odometry.hpp | 8 +++ .../src/ackermann_steering_controller.cpp | 21 +++++--- .../src/steering_controllers.cpp | 1 + .../src/steering_controllers.yaml | 8 --- .../src/steering_odometry.cpp | 49 ++++++++++++++++++- 5 files changed, 71 insertions(+), 16 deletions(-) diff --git a/steering_controllers/include/steering_controllers/steering_odometry.hpp b/steering_controllers/include/steering_controllers/steering_odometry.hpp index 3b31348dff..cc09918df2 100644 --- a/steering_controllers/include/steering_controllers/steering_odometry.hpp +++ b/steering_controllers/include/steering_controllers/steering_odometry.hpp @@ -63,6 +63,10 @@ class SteeringOdometry const double rear_right_wheel_pos, const double rear_left_wheel_pos, const double front_steer_pos, const double dt); + bool update_from_position( + const double rear_right_wheel_pos, const double rear_left_wheel_pos, + const double front_right_steer_pos, const double front_left_steer_pos, const double dt); + /** * \brief Updates the odometry class with latest wheels position * \param rear_wheel_vel Rear wheel velocity [rad/s] @@ -77,6 +81,10 @@ class SteeringOdometry const double rear_right_wheel_vel, const double rear_left_wheel_vel, const double front_steer_pos, const double dt); + bool update_from_velocity( + const double rear_right_wheel_vel, const double rear_left_wheel_vel, + const double front_right_steer_pos, const double front_left_steer_pos, const double dt); + /** * \brief Updates the odometry class with latest velocity command * \param linear Linear velocity [m/s] diff --git a/steering_controllers/src/ackermann_steering_controller.cpp b/steering_controllers/src/ackermann_steering_controller.cpp index 24604f2130..efd7460262 100644 --- a/steering_controllers/src/ackermann_steering_controller.cpp +++ b/steering_controllers/src/ackermann_steering_controller.cpp @@ -40,7 +40,6 @@ controller_interface::CallbackReturn AckermannSteeringController::configure_odom return controller_interface::CallbackReturn::SUCCESS; } -// TODO: this is bicycle odometry ATM, implement ackermann bool AckermannSteeringController::update_odometry(const rclcpp::Duration & period) { if (params_.open_loop) @@ -49,19 +48,29 @@ bool AckermannSteeringController::update_odometry(const rclcpp::Duration & perio } else { - const double rear_wheel_value = state_interfaces_[0].get_value(); - const double steer_position = state_interfaces_[1].get_value() * params_.steer_pos_multiplier; - if (!std::isnan(rear_wheel_value) && !std::isnan(steer_position)) + const double rear_right_wheel_value = state_interfaces_[0].get_value(); + const double rear_left_wheel_value = state_interfaces_[1].get_value(); + const double front_right_steer_position = + state_interfaces_[2].get_value() * params_.steer_pos_multiplier; + const double front_left_steer_position = + state_interfaces_[3].get_value() * params_.steer_pos_multiplier; + if ( + !std::isnan(rear_right_wheel_value) && !std::isnan(rear_left_wheel_value) && + !std::isnan(front_right_steer_position) && !std::isnan(front_left_steer_position)) { if (params_.position_feedback) { // Estimate linear and angular velocity using joint information - odometry_.update_from_position(rear_wheel_value, steer_position, period.seconds()); + odometry_.update_from_position( + rear_right_wheel_value, rear_left_wheel_value, front_right_steer_position, + front_left_steer_position, period.seconds()); } else { // Estimate linear and angular velocity using joint information - odometry_.update_from_velocity(rear_wheel_value, steer_position, period.seconds()); + odometry_.update_from_velocity( + rear_right_wheel_value, rear_left_wheel_value, front_right_steer_position, + front_left_steer_position, period.seconds()); } } } diff --git a/steering_controllers/src/steering_controllers.cpp b/steering_controllers/src/steering_controllers.cpp index 3a847ce4ff..7b1b2651af 100644 --- a/steering_controllers/src/steering_controllers.cpp +++ b/steering_controllers/src/steering_controllers.cpp @@ -74,6 +74,7 @@ controller_interface::CallbackReturn SteeringControllers::set_interface_numbers( nr_state_itfs_ = nr_state_itfs; nr_cmd_itfs_ = nr_cmd_itfs; nr_ref_itfs_ = nr_ref_itfs; + return controller_interface::CallbackReturn::SUCCESS; } controller_interface::CallbackReturn SteeringControllers::on_configure( diff --git a/steering_controllers/src/steering_controllers.yaml b/steering_controllers/src/steering_controllers.yaml index 5e4021b76b..71874e0fe8 100644 --- a/steering_controllers/src/steering_controllers.yaml +++ b/steering_controllers/src/steering_controllers.yaml @@ -96,14 +96,6 @@ steering_controllers: } - allow_multiple_cmd_vel_publishers: { - type: bool, - default_value: true, - description: "Allow multiple cmd_vel publishers is enabled or disabled?.", - read_only: false, - - } - base_frame_id: { type: string, default_value: "base_link", diff --git a/steering_controllers/src/steering_odometry.cpp b/steering_controllers/src/steering_odometry.cpp index e10d32b4c6..1d4e84bda9 100644 --- a/steering_controllers/src/steering_odometry.cpp +++ b/steering_controllers/src/steering_odometry.cpp @@ -120,6 +120,48 @@ bool SteeringOdometry::update_from_position( return true; } +bool SteeringOdometry::update_from_position( + const double rear_right_wheel_pos, const double rear_left_wheel_pos, + const double front_right_steer_pos, const double front_left_steer_pos, const double dt) +{ + /// Get current wheel joint positions: + const double rear_right_wheel_cur_pos = rear_right_wheel_pos * wheel_radius_; + const double rear_left_wheel_cur_pos = rear_left_wheel_cur_pos * wheel_radius_; + + const double rear_right_wheel_est_pos_diff = rear_right_wheel_cur_pos - rear_right_wheel_old_pos_; + const double rear_left_wheel_est_pos_diff = rear_left_wheel_cur_pos - rear_left_wheel_old_pos_; + + /// Update old position with current: + rear_right_wheel_old_pos_ = rear_right_wheel_cur_pos; + rear_left_wheel_old_pos_ = rear_left_wheel_cur_pos; + + /// Compute linear and angular diff: + const double linear_velocity = + (rear_right_wheel_est_pos_diff + rear_left_wheel_est_pos_diff) * 0.5 / dt; + + const double front_steer_pos = (front_right_steer_pos + front_left_steer_pos) * 0.5; + //const double angular = (rear_right_wheel_est_pos_diff - rear_left_wheel_est_pos_diff) / wheel_separation_w_; + const double angular = tan(front_steer_pos) * linear_velocity / wheel_separation_; + + update_odometry(linear_velocity, angular, dt); + + return true; +} + +bool SteeringOdometry::update_from_velocity( + const double rear_wheel_vel, const double front_steer_pos, const double dt) +{ + // (right_wheel_est_vel + left_wheel_est_vel) * 0.5; + double linear_velocity = rear_wheel_vel * wheel_radius_; + + //const double angular = (right_wheel_est_vel - left_wheel_est_vel) / wheel_separation_w_; + const double angular = tan(front_steer_pos) * linear_velocity / wheel_separation_; + + update_odometry(linear_velocity, angular, dt); + + return true; +} + bool SteeringOdometry::update_from_velocity( const double rear_right_wheel_vel, const double rear_left_wheel_vel, const double front_steer_pos, const double dt) @@ -136,12 +178,15 @@ bool SteeringOdometry::update_from_velocity( } bool SteeringOdometry::update_from_velocity( - const double rear_wheel_vel, const double front_steer_pos, const double dt) + const double rear_right_wheel_vel, const double rear_left_wheel_vel, + const double front_right_steer_pos, const double front_left_steer_pos, const double dt) { // (right_wheel_est_vel + left_wheel_est_vel) * 0.5; - double linear_velocity = rear_wheel_vel * wheel_radius_; + double linear_velocity = (rear_right_wheel_vel + rear_left_wheel_vel) * wheel_radius_ * 0.5; //const double angular = (right_wheel_est_vel - left_wheel_est_vel) / wheel_separation_w_; + const double front_steer_pos = (front_right_steer_pos + front_left_steer_pos) * 0.5; + const double angular = tan(front_steer_pos) * linear_velocity / wheel_separation_; update_odometry(linear_velocity, angular, dt); From 4e4d86a9ba4e4f255d43df2a5e24e954f2106cb0 Mon Sep 17 00:00:00 2001 From: petkovich Date: Wed, 21 Dec 2022 17:12:27 +0100 Subject: [PATCH 085/186] front_wheels_names, rear_wheels_names --- .../src/steering_controllers.cpp | 16 ++++++++-------- .../src/steering_controllers.yaml | 4 ++-- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/steering_controllers/src/steering_controllers.cpp b/steering_controllers/src/steering_controllers.cpp index 7b1b2651af..3906756fde 100644 --- a/steering_controllers/src/steering_controllers.cpp +++ b/steering_controllers/src/steering_controllers.cpp @@ -269,15 +269,15 @@ controller_interface::InterfaceConfiguration SteeringControllers::command_interf command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; command_interfaces_config.names.reserve(nr_cmd_itfs_); - for (size_t i = 0; i < params_.rear_wheel_names.size(); i++) + for (size_t i = 0; i < params_.rear_wheels_names.size(); i++) { command_interfaces_config.names.push_back( - params_.rear_wheel_names[i] + "/" + hardware_interface::HW_IF_VELOCITY); + params_.rear_wheels_names[i] + "/" + hardware_interface::HW_IF_VELOCITY); } - for (size_t i = 0; i < params_.front_steer_names.size(); i++) + for (size_t i = 0; i < params_.front_wheels_names.size(); i++) { command_interfaces_config.names.push_back( - params_.front_steer_names[i] + "/" + hardware_interface::HW_IF_POSITION); + params_.front_wheels_names[i] + "/" + hardware_interface::HW_IF_POSITION); } return command_interfaces_config; } @@ -292,16 +292,16 @@ controller_interface::InterfaceConfiguration SteeringControllers::state_interfac const auto rear_wheel_feedback = params_.position_feedback ? hardware_interface::HW_IF_POSITION : hardware_interface::HW_IF_VELOCITY; - for (size_t i = 0; i < params_.rear_wheel_names.size(); i++) + for (size_t i = 0; i < params_.rear_wheels_names.size(); i++) { state_interfaces_config.names.push_back( - params_.rear_wheel_names[i] + "/" + rear_wheel_feedback); + params_.rear_wheels_names[i] + "/" + rear_wheel_feedback); } - for (size_t i = 0; i < params_.front_steer_names.size(); i++) + for (size_t i = 0; i < params_.front_wheels_names.size(); i++) { state_interfaces_config.names.push_back( - params_.front_steer_names[i] + "/" + hardware_interface::HW_IF_POSITION); + params_.front_wheels_names[i] + "/" + hardware_interface::HW_IF_POSITION); } return state_interfaces_config; diff --git a/steering_controllers/src/steering_controllers.yaml b/steering_controllers/src/steering_controllers.yaml index 71874e0fe8..2a057ecac2 100644 --- a/steering_controllers/src/steering_controllers.yaml +++ b/steering_controllers/src/steering_controllers.yaml @@ -8,7 +8,7 @@ steering_controllers: # } } - rear_wheel_names: { + rear_wheels_names: { type: string_array, default_value: ["rear_wheel_joint"], description: "Name of rear wheels.", @@ -16,7 +16,7 @@ steering_controllers: } - front_steer_names: { + front_wheels_names: { type: string_array, default_value: ["front_steer_joint"], description: "Names of front steer wheels.", From 97ff28cd3f3e7adf51a185cab574200da7af17f3 Mon Sep 17 00:00:00 2001 From: petkovich Date: Wed, 21 Dec 2022 17:44:25 +0100 Subject: [PATCH 086/186] precommit and control_msgs repos --- .pre-commit-config.yaml | 90 +++++++++++++++++----------------- ros2_controllers.rolling.repos | 4 +- 2 files changed, 47 insertions(+), 47 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index e77e9e84dc..58fe46dd90 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -44,12 +44,12 @@ repos: - id: black args: ["--line-length=99"] - # # PEP 257 - # - repo: https://github.com/FalconSocial/pre-commit-mirrors-pep257 - # rev: v0.3.3 - # hooks: - # - id: pep257 - # args: ["--ignore=D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D404"] + # PEP 257 + - repo: https://github.com/FalconSocial/pre-commit-mirrors-pep257 + rev: v0.3.3 + hooks: + - id: pep257 + args: ["--ignore=D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D404"] - repo: https://github.com/pycqa/flake8 rev: 5.0.4 @@ -74,48 +74,48 @@ repos: #- id: cppcheck #args: ['--error-exitcode=1', '-f', '--inline-suppr', '-q', '-rp', '--suppress=internalAstError', '--suppress=unknownMacro', '--verbose'] - # - repo: local - # hooks: - # - id: ament_cppcheck - # name: ament_cppcheck - # description: Static code analysis of C/C++ files. - # stages: [commit] - # entry: ament_cppcheck - # language: system - # files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$ + - repo: local + hooks: + - id: ament_cppcheck + name: ament_cppcheck + description: Static code analysis of C/C++ files. + stages: [commit] + entry: ament_cppcheck + language: system + files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$ - # # Maybe use https://github.com/cpplint/cpplint instead - # - repo: local - # hooks: - # - id: ament_cpplint - # name: ament_cpplint - # description: Static code analysis of C/C++ files. - # stages: [commit] - # entry: ament_cpplint - # language: system - # files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$ - # args: ["--linelength=100", "--filter=-whitespace/newline"] + # Maybe use https://github.com/cpplint/cpplint instead + - repo: local + hooks: + - id: ament_cpplint + name: ament_cpplint + description: Static code analysis of C/C++ files. + stages: [commit] + entry: ament_cpplint + language: system + files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$ + args: ["--linelength=100", "--filter=-whitespace/newline"] - # # Cmake hooks - # - repo: local - # hooks: - # - id: ament_lint_cmake - # name: ament_lint_cmake - # description: Check format of CMakeLists.txt files. - # stages: [commit] - # entry: ament_lint_cmake - # language: system - # files: CMakeLists\.txt$ + # Cmake hooks + - repo: local + hooks: + - id: ament_lint_cmake + name: ament_lint_cmake + description: Check format of CMakeLists.txt files. + stages: [commit] + entry: ament_lint_cmake + language: system + files: CMakeLists\.txt$ - # # Copyright - # - repo: local - # hooks: - # - id: ament_copyright - # name: ament_copyright - # description: Check if copyright notice is available in all files. - # stages: [commit] - # entry: ament_copyright - # language: system + # Copyright + - repo: local + hooks: + - id: ament_copyright + name: ament_copyright + description: Check if copyright notice is available in all files. + stages: [commit] + entry: ament_copyright + language: system # Docs - RestructuredText hooks - repo: https://github.com/PyCQA/doc8 diff --git a/ros2_controllers.rolling.repos b/ros2_controllers.rolling.repos index dfed17059b..5f2446d43e 100644 --- a/ros2_controllers.rolling.repos +++ b/ros2_controllers.rolling.repos @@ -9,8 +9,8 @@ repositories: version: master control_msgs: type: git - url: https://github.com/ros-controls/control_msgs.git - version: humble + url: https://github.com/StoglRobotics-forks/control_msgs.git + version: add-steering-controller-status-msgs control_toolbox: type: git url: https://github.com/ros-controls/control_toolbox.git From 821bfd7c862887a9f85abb4f37a090969ed69cf3 Mon Sep 17 00:00:00 2001 From: petkovich Date: Sat, 24 Dec 2022 19:46:29 +0100 Subject: [PATCH 087/186] separated implementation-specific parameters to implemenation files --- steering_controllers/CMakeLists.txt | 18 ++++++- .../config/ackermann_steering_controller.yaml | 48 +++++++++++++++++++ .../config/bicycle_steering_controller.yaml | 32 +++++++++++++ .../{src => config}/steering_controllers.yaml | 48 ------------------- .../config/tricycle_steering_controller.yaml | 48 +++++++++++++++++++ .../ackermann_steering_controller.hpp | 15 ++++-- .../bicycle_steering_controller.hpp | 15 ++++-- .../steering_controllers.hpp | 5 +- .../tricycle_steering_controller.hpp | 16 +++++-- .../src/ackermann_steering_controller.cpp | 18 +++++-- .../src/bicycle_steering_controller.cpp | 16 +++++-- .../src/steering_controllers.cpp | 1 + .../src/tricycle_steering_controller.cpp | 17 +++++-- 13 files changed, 226 insertions(+), 71 deletions(-) create mode 100644 steering_controllers/config/ackermann_steering_controller.yaml create mode 100644 steering_controllers/config/bicycle_steering_controller.yaml rename steering_controllers/{src => config}/steering_controllers.yaml (74%) create mode 100644 steering_controllers/config/tricycle_steering_controller.yaml diff --git a/steering_controllers/CMakeLists.txt b/steering_controllers/CMakeLists.txt index 4cf697c33a..9689ad2e27 100644 --- a/steering_controllers/CMakeLists.txt +++ b/steering_controllers/CMakeLists.txt @@ -39,15 +39,29 @@ add_library( src/bicycle_steering_controller.cpp src/tricycle_steering_controller.cpp ) + generate_parameter_library(steering_controllers_parameters - src/steering_controllers.yaml + config/steering_controllers.yaml +) +generate_parameter_library(bicycle_controller_parameters + config/bicycle_steering_controller.yaml +) +generate_parameter_library(tricycle_controller_parameters + config/tricycle_steering_controller.yaml +) +generate_parameter_library(ackermann_controller_parameters + config/ackermann_steering_controller.yaml ) # target_include_directories(${PROJECT_NAME} PRIVATE include) target_include_directories(${PROJECT_NAME} PUBLIC "$" "$") -target_link_libraries(${PROJECT_NAME} steering_controllers_parameters) +target_link_libraries(${PROJECT_NAME} + steering_controllers_parameters + bicycle_controller_parameters + tricycle_controller_parameters + ackermann_controller_parameters) ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS}) target_compile_definitions(${PROJECT_NAME} PRIVATE "STEERING_CONTROLLERS_BUILDING_DLL") diff --git a/steering_controllers/config/ackermann_steering_controller.yaml b/steering_controllers/config/ackermann_steering_controller.yaml new file mode 100644 index 0000000000..cecbcf724c --- /dev/null +++ b/steering_controllers/config/ackermann_steering_controller.yaml @@ -0,0 +1,48 @@ +ackermann_steering_controller: + wheel_separation_multiplier: + { + type: double, + default_value: 1.0, + description: "Wheel separation height will be multiplied by value of the wheel_separation_h_multiplier.", + read_only: false, + } + + wheel_separation: + { + type: double, + default_value: 0.0, + description: "Wheel separation.", + read_only: false, + } + + wheelbase_multiplier: + { + type: double, + default_value: 1.0, + description: "Wheelbase will be multiplied by value of the wheelbase_multiplier.", + read_only: false, + } + + wheelbase: + { + type: double, + default_value: 0.0, + description: "Wheelbase length.", + read_only: false, + } + + wheel_radius: + { + type: double, + default_value: 0.0, + description: "Wheel radius.", + read_only: false, + } + + wheel_radius_multiplier: + { + type: double, + default_value: 1.0, + description: "Wheel radius will be multiplied by value of wheel_radius_multiplier.", + read_only: false, + } diff --git a/steering_controllers/config/bicycle_steering_controller.yaml b/steering_controllers/config/bicycle_steering_controller.yaml new file mode 100644 index 0000000000..c8d457f941 --- /dev/null +++ b/steering_controllers/config/bicycle_steering_controller.yaml @@ -0,0 +1,32 @@ +bicycle_steering_controller: + wheel_separation_multiplier: + { + type: double, + default_value: 1.0, + description: "Wheel separation height will be multiplied by value of the wheel_separation_h_multiplier.", + read_only: false, + } + + wheel_separation: + { + type: double, + default_value: 0.0, + description: "Wheel separation.", + read_only: false, + } + + wheel_radius: + { + type: double, + default_value: 0.0, + description: "Wheel radius.", + read_only: false, + } + + wheel_radius_multiplier: + { + type: double, + default_value: 1.0, + description: "Wheel radius will be multiplied by value of wheel_radius_multiplier.", + read_only: false, + } diff --git a/steering_controllers/src/steering_controllers.yaml b/steering_controllers/config/steering_controllers.yaml similarity index 74% rename from steering_controllers/src/steering_controllers.yaml rename to steering_controllers/config/steering_controllers.yaml index 2a057ecac2..395d2e70d8 100644 --- a/steering_controllers/src/steering_controllers.yaml +++ b/steering_controllers/config/steering_controllers.yaml @@ -32,54 +32,6 @@ steering_controllers: } - wheel_separation_multiplier: { - type: double, - default_value: 1.0, - description: "Wheel separation height will be multiplied by value of the wheel_separation_h_multiplier.", - read_only: false, - - } - - wheel_separation: { - type: double, - default_value: 0.0, - description: "Wheel separation.", - read_only: false, - - } - - wheelbase_multiplier: { - type: double, - default_value: 1.0, - description: "Wheelbase will be multiplied by value of the wheelbase_multiplier.", - read_only: false, - - } - - wheelbase: { - type: double, - default_value: 0.0, - description: "Wheelbase length.", - read_only: false, - - } - - wheel_radius: { - type: double, - default_value: 0.0, - description: "Wheel radius.", - read_only: false, - - } - - wheel_radius_multiplier: { - type: double, - default_value: 1.0, - description: "Wheel radius will be multiplied by value of wheel_radius_multiplier.", - read_only: false, - - } - steer_pos_multiplier: { type: double, default_value: 1.0, diff --git a/steering_controllers/config/tricycle_steering_controller.yaml b/steering_controllers/config/tricycle_steering_controller.yaml new file mode 100644 index 0000000000..b62601465c --- /dev/null +++ b/steering_controllers/config/tricycle_steering_controller.yaml @@ -0,0 +1,48 @@ +tricycle_steering_controller: + wheel_separation_multiplier: + { + type: double, + default_value: 1.0, + description: "Wheel separation height will be multiplied by value of the wheel_separation_h_multiplier.", + read_only: false, + } + + wheel_separation: + { + type: double, + default_value: 0.0, + description: "Wheel separation.", + read_only: false, + } + + wheelbase_multiplier: + { + type: double, + default_value: 1.0, + description: "Wheelbase will be multiplied by value of the wheelbase_multiplier.", + read_only: false, + } + + wheelbase: + { + type: double, + default_value: 0.0, + description: "Wheelbase length.", + read_only: false, + } + + wheel_radius: + { + type: double, + default_value: 0.0, + description: "Wheel radius.", + read_only: false, + } + + wheel_radius_multiplier: + { + type: double, + default_value: 1.0, + description: "Wheel radius will be multiplied by value of wheel_radius_multiplier.", + read_only: false, + } diff --git a/steering_controllers/include/steering_controllers/ackermann_steering_controller.hpp b/steering_controllers/include/steering_controllers/ackermann_steering_controller.hpp index 03bb11ffcb..db77fac8b4 100644 --- a/steering_controllers/include/steering_controllers/ackermann_steering_controller.hpp +++ b/steering_controllers/include/steering_controllers/ackermann_steering_controller.hpp @@ -15,9 +15,12 @@ // Authors: dr. sc. Tomislav Petkovic, Dr. Ing. Denis Štogl // -#ifndef STEERING_CONTROLLERS_IMPLEMENTATIONS_HPP_ -#define STEERING_CONTROLLERS_IMPLEMENTATIONS_HPP_ +#ifndef STEERING_CONTROLLERS__ACKERMANN_STEERING_CONTROLLER_HPP_ +#define STEERING_CONTROLLERS__ACKERMANN_STEERING_CONTROLLER_HPP_ +#include + +#include "ackermann_controller_parameters.hpp" #include "steering_controllers/steering_controllers.hpp" namespace ackermann_steering_controller @@ -32,7 +35,13 @@ class AckermannSteeringController : public steering_controllers::SteeringControl STEERING_CONTROLLERS__VISIBILITY_PUBLIC bool update_odometry( const rclcpp::Duration & period) override; + + STEERING_CONTROLLERS__VISIBILITY_PUBLIC void initialize_implementation_parameter_listener() + override; + +private: + std::shared_ptr ackermann_param_listener_; }; } // namespace ackermann_steering_controller -#endif // STEERING_CONTROLLERS_IMPLEMENTATIONS_HPP_ +#endif // STEERING_CONTROLLERS__ACKERMANN_STEERING_CONTROLLER_HPP_ diff --git a/steering_controllers/include/steering_controllers/bicycle_steering_controller.hpp b/steering_controllers/include/steering_controllers/bicycle_steering_controller.hpp index fd9e30f8a0..2c80561d34 100644 --- a/steering_controllers/include/steering_controllers/bicycle_steering_controller.hpp +++ b/steering_controllers/include/steering_controllers/bicycle_steering_controller.hpp @@ -15,9 +15,12 @@ // Authors: dr. sc. Tomislav Petkovic, Dr. Ing. Denis Štogl // -#ifndef STEERING_CONTROLLERS_IMPLEMENTATIONS_HPP_ -#define STEERING_CONTROLLERS_IMPLEMENTATIONS_HPP_ +#ifndef STEERING_CONTROLLERS__BICYCLE_STEERING_CONTROLLER_HPP_ +#define STEERING_CONTROLLERS__BICYCLE_STEERING_CONTROLLER_HPP_ +#include + +#include "bicycle_controller_parameters.hpp" #include "steering_controllers/steering_controllers.hpp" namespace bicycle_steering_controller @@ -32,7 +35,13 @@ class BicycleSteeringController : public steering_controllers::SteeringControlle STEERING_CONTROLLERS__VISIBILITY_PUBLIC bool update_odometry( const rclcpp::Duration & period) override; + + STEERING_CONTROLLERS__VISIBILITY_PUBLIC void initialize_implementation_parameter_listener() + override; + +private: + std::shared_ptr bicycle_param_listener_; }; } // namespace bicycle_steering_controller -#endif // STEERING_CONTROLLERS_IMPLEMENTATIONS_HPP_ +#endif // STEERING_CONTROLLERS__BICYCLE_STEERING_CONTROLLER_HPP_ diff --git a/steering_controllers/include/steering_controllers/steering_controllers.hpp b/steering_controllers/include/steering_controllers/steering_controllers.hpp index c93ae5a2b7..1d05f1c815 100644 --- a/steering_controllers/include/steering_controllers/steering_controllers.hpp +++ b/steering_controllers/include/steering_controllers/steering_controllers.hpp @@ -49,6 +49,9 @@ class SteeringControllers : public controller_interface::ChainableControllerInte public: STEERING_CONTROLLERS__VISIBILITY_PUBLIC SteeringControllers(); + virtual STEERING_CONTROLLERS__VISIBILITY_PUBLIC void + initialize_implementation_parameter_listener() = 0; + STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::CallbackReturn on_init() override; STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::InterfaceConfiguration @@ -141,4 +144,4 @@ class SteeringControllers : public controller_interface::ChainableControllerInte } // namespace steering_controllers -#endif // STEERING_CONTROLLERS__steering_controllers_HPP_ +#endif // STEERING_CONTROLLERS__STEERING_CONTROLLERS_HPP_ diff --git a/steering_controllers/include/steering_controllers/tricycle_steering_controller.hpp b/steering_controllers/include/steering_controllers/tricycle_steering_controller.hpp index 440d8d42c4..c7210c5c4a 100644 --- a/steering_controllers/include/steering_controllers/tricycle_steering_controller.hpp +++ b/steering_controllers/include/steering_controllers/tricycle_steering_controller.hpp @@ -15,10 +15,13 @@ // Authors: dr. sc. Tomislav Petkovic, Dr. Ing. Denis Štogl // -#ifndef STEERING_CONTROLLERS_IMPLEMENTATIONS_HPP_ -#define STEERING_CONTROLLERS_IMPLEMENTATIONS_HPP_ +#ifndef STEERING_CONTROLLERS__TRICYCLE_STEERING_CONTROLLER_HPP_ +#define STEERING_CONTROLLERS__TRICYCLE_STEERING_CONTROLLER_HPP_ + +#include #include "steering_controllers/steering_controllers.hpp" +#include "tricycle_controller_parameters.hpp" namespace tricycle_steering_controller { @@ -29,9 +32,16 @@ class TricycleSteeringController : public steering_controllers::SteeringControll STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::CallbackReturn configure_odometry() override; + STEERING_CONTROLLERS__VISIBILITY_PUBLIC bool update_odometry( const rclcpp::Duration & period) override; + + STEERING_CONTROLLERS__VISIBILITY_PUBLIC void initialize_implementation_parameter_listener() + override; + +private: + std::shared_ptr tricycle_param_listener_; }; } // namespace tricycle_steering_controller -#endif // STEERING_CONTROLLERS_IMPLEMENTATIONS_HPP_ +#endif // STEERING_CONTROLLERS__TRICYCLE_STEERING_CONTROLLER_HPP_ diff --git a/steering_controllers/src/ackermann_steering_controller.cpp b/steering_controllers/src/ackermann_steering_controller.cpp index efd7460262..f83907a573 100644 --- a/steering_controllers/src/ackermann_steering_controller.cpp +++ b/steering_controllers/src/ackermann_steering_controller.cpp @@ -21,15 +21,25 @@ AckermannSteeringController::AckermannSteeringController() { } +void AckermannSteeringController::initialize_implementation_parameter_listener() +{ + ackermann_param_listener_ = + std::make_shared(get_node()); +} + controller_interface::CallbackReturn AckermannSteeringController::configure_odometry() { - const double wheel_radius = params_.wheel_radius_multiplier * params_.wheel_radius; - const double wheel_seperation = params_.wheel_separation_multiplier * params_.wheel_separation; - const double wheelbase = params_.wheelbase_multiplier * params_.wheelbase; + ackermann_steering_controller::Params ackerman_params = ackermann_param_listener_->get_params(); + + const double wheel_radius = + ackerman_params.wheel_radius_multiplier * ackerman_params.wheel_radius; + const double wheel_seperation = + ackerman_params.wheel_separation_multiplier * ackerman_params.wheel_separation; + const double wheelbase = ackerman_params.wheelbase_multiplier * ackerman_params.wheelbase; odometry_.set_wheel_params(wheel_radius, wheel_seperation, wheelbase); odometry_.set_velocity_rolling_window_size(params_.velocity_rolling_window_size); - // TODO: enable position/velocity configure + // TODO(petkovich): enable position/velocity configure const size_t nr_state_itfs = 4; const size_t nr_cmd_itfs = 4; const size_t nr_ref_itfs = 2; diff --git a/steering_controllers/src/bicycle_steering_controller.cpp b/steering_controllers/src/bicycle_steering_controller.cpp index 4a39d16860..29a22af90b 100644 --- a/steering_controllers/src/bicycle_steering_controller.cpp +++ b/steering_controllers/src/bicycle_steering_controller.cpp @@ -20,19 +20,29 @@ BicycleSteeringController::BicycleSteeringController() : steering_controllers::S { } +void BicycleSteeringController::initialize_implementation_parameter_listener() +{ + bicycle_param_listener_ = + std::make_shared(get_node()); +} + controller_interface::CallbackReturn BicycleSteeringController::configure_odometry() { - const double wheel_seperation = params_.wheel_separation_multiplier * params_.wheel_separation; - const double wheel_radius = params_.wheel_radius_multiplier * params_.wheel_radius; + bicycle_steering_controller::Params bicycle_params = bicycle_param_listener_->get_params(); + + const double wheel_seperation = + bicycle_params.wheel_separation_multiplier * bicycle_params.wheel_separation; + const double wheel_radius = bicycle_params.wheel_radius_multiplier * bicycle_params.wheel_radius; odometry_.set_wheel_params(wheel_radius, wheel_seperation); odometry_.set_velocity_rolling_window_size(params_.velocity_rolling_window_size); - // TODO: enable position/velocity configure + // TODO(petkovich): enable position/velocity configure const size_t nr_state_itfs = 2; const size_t nr_cmd_itfs = 2; const size_t nr_ref_itfs = 2; set_interface_numbers(nr_state_itfs, nr_cmd_itfs, nr_ref_itfs); + RCLCPP_INFO(get_node()->get_logger(), "%f %f", wheel_seperation, wheel_radius); RCLCPP_INFO(get_node()->get_logger(), "bicycle odom configure successful"); return controller_interface::CallbackReturn::SUCCESS; diff --git a/steering_controllers/src/steering_controllers.cpp b/steering_controllers/src/steering_controllers.cpp index 3906756fde..232c0fd1d0 100644 --- a/steering_controllers/src/steering_controllers.cpp +++ b/steering_controllers/src/steering_controllers.cpp @@ -58,6 +58,7 @@ controller_interface::CallbackReturn SteeringControllers::on_init() try { param_listener_ = std::make_shared(get_node()); + initialize_implementation_parameter_listener(); } catch (const std::exception & e) { diff --git a/steering_controllers/src/tricycle_steering_controller.cpp b/steering_controllers/src/tricycle_steering_controller.cpp index b766462ca3..e9db124f41 100644 --- a/steering_controllers/src/tricycle_steering_controller.cpp +++ b/steering_controllers/src/tricycle_steering_controller.cpp @@ -20,16 +20,25 @@ TricycleSteeringController::TricycleSteeringController() : steering_controllers::SteeringControllers() { } +void TricycleSteeringController::initialize_implementation_parameter_listener() +{ + tricycle_param_listener_ = + std::make_shared(get_node()); +} controller_interface::CallbackReturn TricycleSteeringController::configure_odometry() { - const double wheel_radius = params_.wheel_radius_multiplier * params_.wheel_radius; - const double wheel_seperation = params_.wheel_separation_multiplier * params_.wheel_separation; - const double wheelbase = params_.wheelbase_multiplier * params_.wheelbase; + tricycle_steering_controller::Params tricycle_params = tricycle_param_listener_->get_params(); + + const double wheel_radius = + tricycle_params.wheel_radius_multiplier * tricycle_params.wheel_radius; + const double wheel_seperation = + tricycle_params.wheel_separation_multiplier * tricycle_params.wheel_separation; + const double wheelbase = tricycle_params.wheelbase_multiplier * tricycle_params.wheelbase; odometry_.set_wheel_params(wheel_radius, wheel_seperation, wheelbase); odometry_.set_velocity_rolling_window_size(params_.velocity_rolling_window_size); - // TODO: enable position/velocity configure + // TODO(petkovich): enable position/velocity configure const size_t nr_state_itfs = 3; const size_t nr_cmd_itfs = 3; const size_t nr_ref_itfs = 2; From d711a96222e1c47293cd8a3d3707670e4521c6de Mon Sep 17 00:00:00 2001 From: petkovich Date: Sat, 24 Dec 2022 20:32:29 +0100 Subject: [PATCH 088/186] removed comments in odometry --- .../src/steering_odometry.cpp | 28 ++++--------------- 1 file changed, 6 insertions(+), 22 deletions(-) diff --git a/steering_controllers/src/steering_odometry.cpp b/steering_controllers/src/steering_odometry.cpp index 1d4e84bda9..5b2d57feb0 100644 --- a/steering_controllers/src/steering_odometry.cpp +++ b/steering_controllers/src/steering_odometry.cpp @@ -72,7 +72,7 @@ bool SteeringOdometry::update_odometry( return true; } -// TODO(destogl): enable also velocity interface to update using velocity from the rear wheel +// TODO(petkovich): enable also velocity interface to update using velocity from the rear wheel bool SteeringOdometry::update_from_position( const double rear_wheel_pos, const double front_steer_pos, const double dt) { @@ -85,7 +85,6 @@ bool SteeringOdometry::update_from_position( /// Compute linear and angular diff: const double linear_velocity = rear_wheel_est_pos_diff / dt; - const double angular = tan(front_steer_pos) * linear_velocity / wheel_separation_; update_odometry(linear_velocity, angular, dt); @@ -99,7 +98,7 @@ bool SteeringOdometry::update_from_position( { /// Get current wheel joint positions: const double rear_right_wheel_cur_pos = rear_right_wheel_pos * wheel_radius_; - const double rear_left_wheel_cur_pos = rear_left_wheel_cur_pos * wheel_radius_; + const double rear_left_wheel_cur_pos = rear_left_wheel_pos * wheel_radius_; const double rear_right_wheel_est_pos_diff = rear_right_wheel_cur_pos - rear_right_wheel_old_pos_; const double rear_left_wheel_est_pos_diff = rear_left_wheel_cur_pos - rear_left_wheel_old_pos_; @@ -108,11 +107,8 @@ bool SteeringOdometry::update_from_position( rear_right_wheel_old_pos_ = rear_right_wheel_cur_pos; rear_left_wheel_old_pos_ = rear_left_wheel_cur_pos; - /// Compute linear and angular diff: const double linear_velocity = (rear_right_wheel_est_pos_diff + rear_left_wheel_est_pos_diff) * 0.5 / dt; - - //const double angular = (rear_right_wheel_est_pos_diff - rear_left_wheel_est_pos_diff) / wheel_separation_w_; const double angular = tan(front_steer_pos) * linear_velocity / wheel_separation_; update_odometry(linear_velocity, angular, dt); @@ -126,7 +122,7 @@ bool SteeringOdometry::update_from_position( { /// Get current wheel joint positions: const double rear_right_wheel_cur_pos = rear_right_wheel_pos * wheel_radius_; - const double rear_left_wheel_cur_pos = rear_left_wheel_cur_pos * wheel_radius_; + const double rear_left_wheel_cur_pos = rear_left_wheel_pos * wheel_radius_; const double rear_right_wheel_est_pos_diff = rear_right_wheel_cur_pos - rear_right_wheel_old_pos_; const double rear_left_wheel_est_pos_diff = rear_left_wheel_cur_pos - rear_left_wheel_old_pos_; @@ -138,9 +134,7 @@ bool SteeringOdometry::update_from_position( /// Compute linear and angular diff: const double linear_velocity = (rear_right_wheel_est_pos_diff + rear_left_wheel_est_pos_diff) * 0.5 / dt; - const double front_steer_pos = (front_right_steer_pos + front_left_steer_pos) * 0.5; - //const double angular = (rear_right_wheel_est_pos_diff - rear_left_wheel_est_pos_diff) / wheel_separation_w_; const double angular = tan(front_steer_pos) * linear_velocity / wheel_separation_; update_odometry(linear_velocity, angular, dt); @@ -151,10 +145,7 @@ bool SteeringOdometry::update_from_position( bool SteeringOdometry::update_from_velocity( const double rear_wheel_vel, const double front_steer_pos, const double dt) { - // (right_wheel_est_vel + left_wheel_est_vel) * 0.5; double linear_velocity = rear_wheel_vel * wheel_radius_; - - //const double angular = (right_wheel_est_vel - left_wheel_est_vel) / wheel_separation_w_; const double angular = tan(front_steer_pos) * linear_velocity / wheel_separation_; update_odometry(linear_velocity, angular, dt); @@ -166,10 +157,7 @@ bool SteeringOdometry::update_from_velocity( const double rear_right_wheel_vel, const double rear_left_wheel_vel, const double front_steer_pos, const double dt) { - // (right_wheel_est_vel + left_wheel_est_vel) * 0.5; double linear_velocity = (rear_right_wheel_vel + rear_left_wheel_vel) * wheel_radius_ * 0.5; - - //const double angular = (right_wheel_est_vel - left_wheel_est_vel) / wheel_separation_w_; const double angular = tan(front_steer_pos) * linear_velocity / wheel_separation_; update_odometry(linear_velocity, angular, dt); @@ -181,12 +169,8 @@ bool SteeringOdometry::update_from_velocity( const double rear_right_wheel_vel, const double rear_left_wheel_vel, const double front_right_steer_pos, const double front_left_steer_pos, const double dt) { - // (right_wheel_est_vel + left_wheel_est_vel) * 0.5; double linear_velocity = (rear_right_wheel_vel + rear_left_wheel_vel) * wheel_radius_ * 0.5; - - //const double angular = (right_wheel_est_vel - left_wheel_est_vel) / wheel_separation_w_; const double front_steer_pos = (front_right_steer_pos + front_left_steer_pos) * 0.5; - const double angular = tan(front_steer_pos) * linear_velocity / wheel_separation_; update_odometry(linear_velocity, angular, dt); @@ -219,7 +203,7 @@ void SteeringOdometry::set_velocity_rolling_window_size(size_t velocity_rolling_ reset_accumulators(); } -//TODO: change functions depending on fwd kinematics model +// TODO(petkovich): change functions depending on fwd kinematics model double SteeringOdometry::convert_trans_rot_vel_to_steering_angle(double Vx, double theta_dot) { if (theta_dot == 0 || Vx == 0) @@ -229,7 +213,7 @@ double SteeringOdometry::convert_trans_rot_vel_to_steering_angle(double Vx, doub return std::atan(theta_dot * wheel_separation_ / Vx); } -//TODO: change functions depending on fwd kinematics model +// TODO(petkovich): change functions depending on fwd kinematics model std::tuple SteeringOdometry::twist_to_ackermann(double Vx, double theta_dot) { // using naming convention in http://users.isr.ist.utl.pt/~mir/cadeiras/robmovel/Kinematics.pdf @@ -289,6 +273,6 @@ void SteeringOdometry::reset_accumulators() { linear_acc_ = rcpputils::RollingMeanAccumulator(velocity_rolling_window_size_); angular_acc_ = rcpputils::RollingMeanAccumulator(velocity_rolling_window_size_); - // TODO: angular rolling window size? + // TODO(petkovich): angular rolling window size? } } // namespace steering_odometry From d5b66a355e1cdd826bd891f0978b945f504b996a Mon Sep 17 00:00:00 2001 From: petkovich Date: Tue, 27 Dec 2022 19:46:11 +0100 Subject: [PATCH 089/186] removed multipliers --- .../config/ackermann_steering_controller.yaml | 23 ------------------- .../config/bicycle_steering_controller.yaml | 15 ------------ .../config/steering_controllers.yaml | 8 ------- .../config/tricycle_steering_controller.yaml | 23 ------------------- .../src/ackermann_steering_controller.cpp | 14 ++++------- .../src/bicycle_steering_controller.cpp | 7 +++--- .../src/steering_controllers.cpp | 3 +-- .../src/tricycle_steering_controller.cpp | 10 ++++---- 8 files changed, 13 insertions(+), 90 deletions(-) diff --git a/steering_controllers/config/ackermann_steering_controller.yaml b/steering_controllers/config/ackermann_steering_controller.yaml index cecbcf724c..c4b0de42b2 100644 --- a/steering_controllers/config/ackermann_steering_controller.yaml +++ b/steering_controllers/config/ackermann_steering_controller.yaml @@ -1,11 +1,4 @@ ackermann_steering_controller: - wheel_separation_multiplier: - { - type: double, - default_value: 1.0, - description: "Wheel separation height will be multiplied by value of the wheel_separation_h_multiplier.", - read_only: false, - } wheel_separation: { @@ -15,14 +8,6 @@ ackermann_steering_controller: read_only: false, } - wheelbase_multiplier: - { - type: double, - default_value: 1.0, - description: "Wheelbase will be multiplied by value of the wheelbase_multiplier.", - read_only: false, - } - wheelbase: { type: double, @@ -38,11 +23,3 @@ ackermann_steering_controller: description: "Wheel radius.", read_only: false, } - - wheel_radius_multiplier: - { - type: double, - default_value: 1.0, - description: "Wheel radius will be multiplied by value of wheel_radius_multiplier.", - read_only: false, - } diff --git a/steering_controllers/config/bicycle_steering_controller.yaml b/steering_controllers/config/bicycle_steering_controller.yaml index c8d457f941..80d8595ebb 100644 --- a/steering_controllers/config/bicycle_steering_controller.yaml +++ b/steering_controllers/config/bicycle_steering_controller.yaml @@ -1,11 +1,4 @@ bicycle_steering_controller: - wheel_separation_multiplier: - { - type: double, - default_value: 1.0, - description: "Wheel separation height will be multiplied by value of the wheel_separation_h_multiplier.", - read_only: false, - } wheel_separation: { @@ -22,11 +15,3 @@ bicycle_steering_controller: description: "Wheel radius.", read_only: false, } - - wheel_radius_multiplier: - { - type: double, - default_value: 1.0, - description: "Wheel radius will be multiplied by value of wheel_radius_multiplier.", - read_only: false, - } diff --git a/steering_controllers/config/steering_controllers.yaml b/steering_controllers/config/steering_controllers.yaml index 395d2e70d8..ce994cc8cd 100644 --- a/steering_controllers/config/steering_controllers.yaml +++ b/steering_controllers/config/steering_controllers.yaml @@ -32,14 +32,6 @@ steering_controllers: } - steer_pos_multiplier: { - type: double, - default_value: 1.0, - description: "Steer pos will be multiplied by value of steer_pos_multiplier.", - read_only: false, - - } - velocity_rolling_window_size: { type: int, default_value: 10, diff --git a/steering_controllers/config/tricycle_steering_controller.yaml b/steering_controllers/config/tricycle_steering_controller.yaml index b62601465c..049c65cdb5 100644 --- a/steering_controllers/config/tricycle_steering_controller.yaml +++ b/steering_controllers/config/tricycle_steering_controller.yaml @@ -1,11 +1,4 @@ tricycle_steering_controller: - wheel_separation_multiplier: - { - type: double, - default_value: 1.0, - description: "Wheel separation height will be multiplied by value of the wheel_separation_h_multiplier.", - read_only: false, - } wheel_separation: { @@ -15,14 +8,6 @@ tricycle_steering_controller: read_only: false, } - wheelbase_multiplier: - { - type: double, - default_value: 1.0, - description: "Wheelbase will be multiplied by value of the wheelbase_multiplier.", - read_only: false, - } - wheelbase: { type: double, @@ -38,11 +23,3 @@ tricycle_steering_controller: description: "Wheel radius.", read_only: false, } - - wheel_radius_multiplier: - { - type: double, - default_value: 1.0, - description: "Wheel radius will be multiplied by value of wheel_radius_multiplier.", - read_only: false, - } diff --git a/steering_controllers/src/ackermann_steering_controller.cpp b/steering_controllers/src/ackermann_steering_controller.cpp index f83907a573..1bfe5781f7 100644 --- a/steering_controllers/src/ackermann_steering_controller.cpp +++ b/steering_controllers/src/ackermann_steering_controller.cpp @@ -31,11 +31,9 @@ controller_interface::CallbackReturn AckermannSteeringController::configure_odom { ackermann_steering_controller::Params ackerman_params = ackermann_param_listener_->get_params(); - const double wheel_radius = - ackerman_params.wheel_radius_multiplier * ackerman_params.wheel_radius; - const double wheel_seperation = - ackerman_params.wheel_separation_multiplier * ackerman_params.wheel_separation; - const double wheelbase = ackerman_params.wheelbase_multiplier * ackerman_params.wheelbase; + const double wheel_radius = ackerman_params.wheel_radius; + const double wheel_seperation = ackerman_params.wheel_separation; + const double wheelbase = ackerman_params.wheelbase; odometry_.set_wheel_params(wheel_radius, wheel_seperation, wheelbase); odometry_.set_velocity_rolling_window_size(params_.velocity_rolling_window_size); @@ -60,10 +58,8 @@ bool AckermannSteeringController::update_odometry(const rclcpp::Duration & perio { const double rear_right_wheel_value = state_interfaces_[0].get_value(); const double rear_left_wheel_value = state_interfaces_[1].get_value(); - const double front_right_steer_position = - state_interfaces_[2].get_value() * params_.steer_pos_multiplier; - const double front_left_steer_position = - state_interfaces_[3].get_value() * params_.steer_pos_multiplier; + const double front_right_steer_position = state_interfaces_[2].get_value(); + const double front_left_steer_position = state_interfaces_[3].get_value(); if ( !std::isnan(rear_right_wheel_value) && !std::isnan(rear_left_wheel_value) && !std::isnan(front_right_steer_position) && !std::isnan(front_left_steer_position)) diff --git a/steering_controllers/src/bicycle_steering_controller.cpp b/steering_controllers/src/bicycle_steering_controller.cpp index 29a22af90b..70a478118a 100644 --- a/steering_controllers/src/bicycle_steering_controller.cpp +++ b/steering_controllers/src/bicycle_steering_controller.cpp @@ -30,9 +30,8 @@ controller_interface::CallbackReturn BicycleSteeringController::configure_odomet { bicycle_steering_controller::Params bicycle_params = bicycle_param_listener_->get_params(); - const double wheel_seperation = - bicycle_params.wheel_separation_multiplier * bicycle_params.wheel_separation; - const double wheel_radius = bicycle_params.wheel_radius_multiplier * bicycle_params.wheel_radius; + const double wheel_seperation = bicycle_params.wheel_separation; + const double wheel_radius = bicycle_params.wheel_radius; odometry_.set_wheel_params(wheel_radius, wheel_seperation); odometry_.set_velocity_rolling_window_size(params_.velocity_rolling_window_size); @@ -57,7 +56,7 @@ bool BicycleSteeringController::update_odometry(const rclcpp::Duration & period) else { const double rear_wheel_value = state_interfaces_[0].get_value(); - const double steer_position = state_interfaces_[1].get_value() * params_.steer_pos_multiplier; + const double steer_position = state_interfaces_[1].get_value(); if (!std::isnan(rear_wheel_value) && !std::isnan(steer_position)) { if (params_.position_feedback) diff --git a/steering_controllers/src/steering_controllers.cpp b/steering_controllers/src/steering_controllers.cpp index 232c0fd1d0..d5a05d5dae 100644 --- a/steering_controllers/src/steering_controllers.cpp +++ b/steering_controllers/src/steering_controllers.cpp @@ -454,8 +454,7 @@ controller_interface::return_type SteeringControllers::update_and_write_commands { controller_state_publisher_->msg_.rear_wheel_velocity = state_interfaces_[0].get_value(); } - controller_state_publisher_->msg_.steer_position = - state_interfaces_[1].get_value() * params_.steer_pos_multiplier; + controller_state_publisher_->msg_.steer_position = state_interfaces_[1].get_value(); controller_state_publisher_->msg_.linear_velocity_command = command_interfaces_[0].get_value(); controller_state_publisher_->msg_.steering_angle_command = command_interfaces_[1].get_value(); diff --git a/steering_controllers/src/tricycle_steering_controller.cpp b/steering_controllers/src/tricycle_steering_controller.cpp index e9db124f41..17c235ee95 100644 --- a/steering_controllers/src/tricycle_steering_controller.cpp +++ b/steering_controllers/src/tricycle_steering_controller.cpp @@ -30,11 +30,9 @@ controller_interface::CallbackReturn TricycleSteeringController::configure_odome { tricycle_steering_controller::Params tricycle_params = tricycle_param_listener_->get_params(); - const double wheel_radius = - tricycle_params.wheel_radius_multiplier * tricycle_params.wheel_radius; - const double wheel_seperation = - tricycle_params.wheel_separation_multiplier * tricycle_params.wheel_separation; - const double wheelbase = tricycle_params.wheelbase_multiplier * tricycle_params.wheelbase; + const double wheel_radius = tricycle_params.wheel_radius; + const double wheel_seperation = tricycle_params.wheel_separation; + const double wheelbase = tricycle_params.wheelbase; odometry_.set_wheel_params(wheel_radius, wheel_seperation, wheelbase); odometry_.set_velocity_rolling_window_size(params_.velocity_rolling_window_size); @@ -59,7 +57,7 @@ bool TricycleSteeringController::update_odometry(const rclcpp::Duration & period { const double rear_right_wheel_value = state_interfaces_[0].get_value(); const double rear_left_wheel_value = state_interfaces_[1].get_value(); - const double steer_position = state_interfaces_[2].get_value() * params_.steer_pos_multiplier; + const double steer_position = state_interfaces_[2].get_value(); if ( !std::isnan(rear_right_wheel_value) && !std::isnan(rear_left_wheel_value) && !std::isnan(steer_position)) From efbaf673746357363d0ff2c3928065a0cdf10577 Mon Sep 17 00:00:00 2001 From: petkovich Date: Tue, 27 Dec 2022 21:57:30 +0100 Subject: [PATCH 090/186] enable rear steering, different wheels radii --- .../config/ackermann_steering_controller.yaml | 23 +++++++-- .../config/bicycle_steering_controller.yaml | 15 ++++-- .../config/steering_controllers.yaml | 9 +++- .../config/tricycle_steering_controller.yaml | 17 +++++-- .../steering_odometry.hpp | 7 +-- .../src/ackermann_steering_controller.cpp | 18 +++++-- .../src/bicycle_steering_controller.cpp | 18 ++++--- .../src/steering_controllers.cpp | 50 +++++++++++++------ .../src/steering_odometry.cpp | 24 ++++----- .../src/tricycle_steering_controller.cpp | 19 ++++--- 10 files changed, 139 insertions(+), 61 deletions(-) diff --git a/steering_controllers/config/ackermann_steering_controller.yaml b/steering_controllers/config/ackermann_steering_controller.yaml index c4b0de42b2..a9f5d1b700 100644 --- a/steering_controllers/config/ackermann_steering_controller.yaml +++ b/steering_controllers/config/ackermann_steering_controller.yaml @@ -1,10 +1,17 @@ ackermann_steering_controller: + front_wheel_track: + { + type: double, + default_value: 0.0, + description: "Front wheel track length.", + read_only: false, + } - wheel_separation: + rear_wheel_track: { type: double, default_value: 0.0, - description: "Wheel separation.", + description: "Rear wheel track length.", read_only: false, } @@ -16,10 +23,18 @@ ackermann_steering_controller: read_only: false, } - wheel_radius: + front_wheels_radius: + { + type: double, + default_value: 0.0, + description: "Front wheels radius.", + read_only: false, + } + + rear_wheels_radius: { type: double, default_value: 0.0, - description: "Wheel radius.", + description: "Rear wheels radius.", read_only: false, } diff --git a/steering_controllers/config/bicycle_steering_controller.yaml b/steering_controllers/config/bicycle_steering_controller.yaml index 80d8595ebb..3f48c7d593 100644 --- a/steering_controllers/config/bicycle_steering_controller.yaml +++ b/steering_controllers/config/bicycle_steering_controller.yaml @@ -1,17 +1,24 @@ bicycle_steering_controller: + wheelbase: + { + type: double, + default_value: 0.0, + description: "Wheelbase length.", + read_only: false, + } - wheel_separation: + front_wheel_radius: { type: double, default_value: 0.0, - description: "Wheel separation.", + description: "Front wheel radius.", read_only: false, } - wheel_radius: + rear_wheel_radius: { type: double, default_value: 0.0, - description: "Wheel radius.", + description: "Rear wheel radius.", read_only: false, } diff --git a/steering_controllers/config/steering_controllers.yaml b/steering_controllers/config/steering_controllers.yaml index ce994cc8cd..d657bc08e5 100644 --- a/steering_controllers/config/steering_controllers.yaml +++ b/steering_controllers/config/steering_controllers.yaml @@ -8,6 +8,14 @@ steering_controllers: # } } + front_steering: { + type: bool, + default_value: true, + description: "Is the steering on the front of the robot?", + read_only: false, + + } + rear_wheels_names: { type: string_array, default_value: ["rear_wheel_joint"], @@ -56,7 +64,6 @@ steering_controllers: } - enable_odom_tf: { type: bool, default_value: true, diff --git a/steering_controllers/config/tricycle_steering_controller.yaml b/steering_controllers/config/tricycle_steering_controller.yaml index 049c65cdb5..9fc36b738d 100644 --- a/steering_controllers/config/tricycle_steering_controller.yaml +++ b/steering_controllers/config/tricycle_steering_controller.yaml @@ -1,10 +1,9 @@ tricycle_steering_controller: - - wheel_separation: + wheel_track: { type: double, default_value: 0.0, - description: "Wheel separation.", + description: "Wheel track length.", read_only: false, } @@ -16,10 +15,18 @@ tricycle_steering_controller: read_only: false, } - wheel_radius: + front_wheels_radius: + { + type: double, + default_value: 0.0, + description: "Front wheels radius.", + read_only: false, + } + + rear_wheels_radius: { type: double, default_value: 0.0, - description: "Wheel radius.", + description: "Rear wheels radius.", read_only: false, } diff --git a/steering_controllers/include/steering_controllers/steering_odometry.hpp b/steering_controllers/include/steering_controllers/steering_odometry.hpp index cc09918df2..8d037cf412 100644 --- a/steering_controllers/include/steering_controllers/steering_odometry.hpp +++ b/steering_controllers/include/steering_controllers/steering_odometry.hpp @@ -17,6 +17,8 @@ #pragma once +#include + #include "realtime_tools/realtime_buffer.h" #include "realtime_tools/realtime_publisher.h" @@ -126,8 +128,7 @@ class SteeringOdometry /** * \brief Sets the wheel parameters: radius, separation and wheelbase */ - void set_wheel_params( - double wheel_radius, double wheel_separation_h = 0.0, double wheelbase = 0.0); + void set_wheel_params(double wheel_radius, double wheelbase = 0.0, double wheel_track = 0.0); /** * \brief Velocity rolling window size setter @@ -193,7 +194,7 @@ class SteeringOdometry double linear_; // [m/s] double angular_; // [rad/s] - double wheel_separation_; + double wheel_track_; double wheelbase_; double wheel_radius_; diff --git a/steering_controllers/src/ackermann_steering_controller.cpp b/steering_controllers/src/ackermann_steering_controller.cpp index 1bfe5781f7..16362ca347 100644 --- a/steering_controllers/src/ackermann_steering_controller.cpp +++ b/steering_controllers/src/ackermann_steering_controller.cpp @@ -31,13 +31,21 @@ controller_interface::CallbackReturn AckermannSteeringController::configure_odom { ackermann_steering_controller::Params ackerman_params = ackermann_param_listener_->get_params(); - const double wheel_radius = ackerman_params.wheel_radius; - const double wheel_seperation = ackerman_params.wheel_separation; + const double front_wheels_radius = ackerman_params.front_wheels_radius; + const double rear_wheels_radius = ackerman_params.rear_wheels_radius; + const double front_wheel_track = ackerman_params.front_wheel_track; + const double rear_wheel_track = ackerman_params.rear_wheel_track; const double wheelbase = ackerman_params.wheelbase; - odometry_.set_wheel_params(wheel_radius, wheel_seperation, wheelbase); - odometry_.set_velocity_rolling_window_size(params_.velocity_rolling_window_size); - // TODO(petkovich): enable position/velocity configure + if (params_.front_steering) + { + odometry_.set_wheel_params(rear_wheels_radius, wheelbase, rear_wheel_track); + } + else + { + odometry_.set_wheel_params(front_wheels_radius, wheelbase, front_wheel_track); + } + const size_t nr_state_itfs = 4; const size_t nr_cmd_itfs = 4; const size_t nr_ref_itfs = 2; diff --git a/steering_controllers/src/bicycle_steering_controller.cpp b/steering_controllers/src/bicycle_steering_controller.cpp index 70a478118a..ab53c36482 100644 --- a/steering_controllers/src/bicycle_steering_controller.cpp +++ b/steering_controllers/src/bicycle_steering_controller.cpp @@ -30,18 +30,24 @@ controller_interface::CallbackReturn BicycleSteeringController::configure_odomet { bicycle_steering_controller::Params bicycle_params = bicycle_param_listener_->get_params(); - const double wheel_seperation = bicycle_params.wheel_separation; - const double wheel_radius = bicycle_params.wheel_radius; - odometry_.set_wheel_params(wheel_radius, wheel_seperation); - odometry_.set_velocity_rolling_window_size(params_.velocity_rolling_window_size); + const double wheelbase = bicycle_params.wheelbase; + const double front_wheel_radius = bicycle_params.front_wheel_radius; + const double rear_wheel_radius = bicycle_params.rear_wheel_radius; + + if (params_.front_steering) + { + odometry_.set_wheel_params(rear_wheel_radius, wheelbase); + } + else + { + odometry_.set_wheel_params(front_wheel_radius, wheelbase); + } - // TODO(petkovich): enable position/velocity configure const size_t nr_state_itfs = 2; const size_t nr_cmd_itfs = 2; const size_t nr_ref_itfs = 2; set_interface_numbers(nr_state_itfs, nr_cmd_itfs, nr_ref_itfs); - RCLCPP_INFO(get_node()->get_logger(), "%f %f", wheel_seperation, wheel_radius); RCLCPP_INFO(get_node()->get_logger(), "bicycle odom configure successful"); return controller_interface::CallbackReturn::SUCCESS; diff --git a/steering_controllers/src/steering_controllers.cpp b/steering_controllers/src/steering_controllers.cpp index d5a05d5dae..05c0154942 100644 --- a/steering_controllers/src/steering_controllers.cpp +++ b/steering_controllers/src/steering_controllers.cpp @@ -82,6 +82,8 @@ controller_interface::CallbackReturn SteeringControllers::on_configure( const rclcpp_lifecycle::State & /*previous_state*/) { params_ = param_listener_->get_params(); + odometry_.set_velocity_rolling_window_size(params_.velocity_rolling_window_size); + configure_odometry(); // topics QoS auto subscribers_qos = rclcpp::SystemDefaultsQoS(); @@ -269,16 +271,19 @@ controller_interface::InterfaceConfiguration SteeringControllers::command_interf controller_interface::InterfaceConfiguration command_interfaces_config; command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; command_interfaces_config.names.reserve(nr_cmd_itfs_); - + const auto front_wheels_command = params_.front_steering ? hardware_interface::HW_IF_POSITION + : hardware_interface::HW_IF_VELOCITY; + const auto rear_wheels_command = params_.front_steering ? hardware_interface::HW_IF_VELOCITY + : hardware_interface::HW_IF_POSITION; for (size_t i = 0; i < params_.rear_wheels_names.size(); i++) { command_interfaces_config.names.push_back( - params_.rear_wheels_names[i] + "/" + hardware_interface::HW_IF_VELOCITY); + params_.rear_wheels_names[i] + "/" + rear_wheels_command); } for (size_t i = 0; i < params_.front_wheels_names.size(); i++) { command_interfaces_config.names.push_back( - params_.front_wheels_names[i] + "/" + hardware_interface::HW_IF_POSITION); + params_.front_wheels_names[i] + "/" + front_wheels_command); } return command_interfaces_config; } @@ -290,19 +295,36 @@ controller_interface::InterfaceConfiguration SteeringControllers::state_interfac state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; state_interfaces_config.names.reserve(nr_state_itfs_); - const auto rear_wheel_feedback = params_.position_feedback ? hardware_interface::HW_IF_POSITION - : hardware_interface::HW_IF_VELOCITY; - - for (size_t i = 0; i < params_.rear_wheels_names.size(); i++) + const auto traction_wheels_feedback = params_.position_feedback + ? hardware_interface::HW_IF_POSITION + : hardware_interface::HW_IF_VELOCITY; + if (params_.front_steering) { - state_interfaces_config.names.push_back( - params_.rear_wheels_names[i] + "/" + rear_wheel_feedback); - } + for (size_t i = 0; i < params_.rear_wheels_names.size(); i++) + { + state_interfaces_config.names.push_back( + params_.rear_wheels_names[i] + "/" + traction_wheels_feedback); + } - for (size_t i = 0; i < params_.front_wheels_names.size(); i++) + for (size_t i = 0; i < params_.front_wheels_names.size(); i++) + { + state_interfaces_config.names.push_back( + params_.front_wheels_names[i] + "/" + hardware_interface::HW_IF_POSITION); + } + } + else { - state_interfaces_config.names.push_back( - params_.front_wheels_names[i] + "/" + hardware_interface::HW_IF_POSITION); + for (size_t i = 0; i < params_.front_wheels_names.size(); i++) + { + state_interfaces_config.names.push_back( + params_.front_wheels_names[i] + "/" + traction_wheels_feedback); + } + + for (size_t i = 0; i < params_.rear_wheels_names.size(); i++) + { + state_interfaces_config.names.push_back( + params_.rear_wheels_names[i] + "/" + hardware_interface::HW_IF_POSITION); + } } return state_interfaces_config; @@ -395,9 +417,7 @@ controller_interface::return_type SteeringControllers::update_and_write_commands const double linear_command = reference_interfaces_[0]; const double angular_command = reference_interfaces_[1]; auto [alpha_write, Ws_write] = odometry_.twist_to_ackermann(linear_command, angular_command); - // previous_publish_timestamp_ = time; - // omega = linear_vel / radius command_interfaces_[0].set_value(Ws_write); command_interfaces_[1].set_value(alpha_write); } diff --git a/steering_controllers/src/steering_odometry.cpp b/steering_controllers/src/steering_odometry.cpp index 5b2d57feb0..870cb9b10d 100644 --- a/steering_controllers/src/steering_odometry.cpp +++ b/steering_controllers/src/steering_odometry.cpp @@ -33,7 +33,7 @@ SteeringOdometry::SteeringOdometry(size_t velocity_rolling_window_size) heading_(0.0), linear_(0.0), angular_(0.0), - wheel_separation_(0.0), + wheel_track_(0.0), wheelbase_(0.0), wheel_radius_(0.0), rear_wheel_old_pos_(0.0), @@ -85,7 +85,7 @@ bool SteeringOdometry::update_from_position( /// Compute linear and angular diff: const double linear_velocity = rear_wheel_est_pos_diff / dt; - const double angular = tan(front_steer_pos) * linear_velocity / wheel_separation_; + const double angular = tan(front_steer_pos) * linear_velocity / wheelbase_; update_odometry(linear_velocity, angular, dt); @@ -109,7 +109,7 @@ bool SteeringOdometry::update_from_position( const double linear_velocity = (rear_right_wheel_est_pos_diff + rear_left_wheel_est_pos_diff) * 0.5 / dt; - const double angular = tan(front_steer_pos) * linear_velocity / wheel_separation_; + const double angular = tan(front_steer_pos) * linear_velocity / wheelbase_; update_odometry(linear_velocity, angular, dt); @@ -135,7 +135,7 @@ bool SteeringOdometry::update_from_position( const double linear_velocity = (rear_right_wheel_est_pos_diff + rear_left_wheel_est_pos_diff) * 0.5 / dt; const double front_steer_pos = (front_right_steer_pos + front_left_steer_pos) * 0.5; - const double angular = tan(front_steer_pos) * linear_velocity / wheel_separation_; + const double angular = tan(front_steer_pos) * linear_velocity / wheelbase_; update_odometry(linear_velocity, angular, dt); @@ -146,7 +146,7 @@ bool SteeringOdometry::update_from_velocity( const double rear_wheel_vel, const double front_steer_pos, const double dt) { double linear_velocity = rear_wheel_vel * wheel_radius_; - const double angular = tan(front_steer_pos) * linear_velocity / wheel_separation_; + const double angular = tan(front_steer_pos) * linear_velocity / wheelbase_; update_odometry(linear_velocity, angular, dt); @@ -158,7 +158,7 @@ bool SteeringOdometry::update_from_velocity( const double dt) { double linear_velocity = (rear_right_wheel_vel + rear_left_wheel_vel) * wheel_radius_ * 0.5; - const double angular = tan(front_steer_pos) * linear_velocity / wheel_separation_; + const double angular = tan(front_steer_pos) * linear_velocity / wheelbase_; update_odometry(linear_velocity, angular, dt); @@ -171,7 +171,8 @@ bool SteeringOdometry::update_from_velocity( { double linear_velocity = (rear_right_wheel_vel + rear_left_wheel_vel) * wheel_radius_ * 0.5; const double front_steer_pos = (front_right_steer_pos + front_left_steer_pos) * 0.5; - const double angular = tan(front_steer_pos) * linear_velocity / wheel_separation_; + const double angular = tan(front_steer_pos) * linear_velocity / wheelbase_; + // TODO(petkovich): Does wheel_track_ come into play if Wr and Wl are not the same? update_odometry(linear_velocity, angular, dt); @@ -188,12 +189,11 @@ void SteeringOdometry::update_open_loop(const double linear, const double angula SteeringOdometry::integrate_exact(linear * dt, angular * dt); } -void SteeringOdometry::set_wheel_params( - double wheel_radius, double wheel_separation, double wheelbase) +void SteeringOdometry::set_wheel_params(double wheel_radius, double wheelbase, double wheel_track) { wheel_radius_ = wheel_radius; - wheel_separation_ = wheel_separation; wheelbase_ = wheelbase; + wheel_track_ = wheel_track; } void SteeringOdometry::set_velocity_rolling_window_size(size_t velocity_rolling_window_size) @@ -210,7 +210,7 @@ double SteeringOdometry::convert_trans_rot_vel_to_steering_angle(double Vx, doub { return 0; } - return std::atan(theta_dot * wheel_separation_ / Vx); + return std::atan(theta_dot * wheelbase_ / Vx); } // TODO(petkovich): change functions depending on fwd kinematics model @@ -222,7 +222,7 @@ std::tuple SteeringOdometry::twist_to_ackermann(double Vx, doubl if (Vx == 0 && theta_dot != 0) { // is spin action alpha = theta_dot > 0 ? M_PI_2 : -M_PI_2; - Ws = abs(theta_dot) * wheel_separation_ / wheel_radius_; + Ws = abs(theta_dot) * wheelbase_ / wheel_radius_; return std::make_tuple(alpha, Ws); } diff --git a/steering_controllers/src/tricycle_steering_controller.cpp b/steering_controllers/src/tricycle_steering_controller.cpp index 17c235ee95..bcf0058062 100644 --- a/steering_controllers/src/tricycle_steering_controller.cpp +++ b/steering_controllers/src/tricycle_steering_controller.cpp @@ -30,20 +30,27 @@ controller_interface::CallbackReturn TricycleSteeringController::configure_odome { tricycle_steering_controller::Params tricycle_params = tricycle_param_listener_->get_params(); - const double wheel_radius = tricycle_params.wheel_radius; - const double wheel_seperation = tricycle_params.wheel_separation; + const double front_wheels_radius = tricycle_params.front_wheels_radius; + const double rear_wheels_radius = tricycle_params.rear_wheels_radius; + const double wheel_track = tricycle_params.wheel_track; const double wheelbase = tricycle_params.wheelbase; - odometry_.set_wheel_params(wheel_radius, wheel_seperation, wheelbase); - odometry_.set_velocity_rolling_window_size(params_.velocity_rolling_window_size); - // TODO(petkovich): enable position/velocity configure + if (params_.front_steering) + { + odometry_.set_wheel_params(rear_wheels_radius, wheelbase, wheel_track); + } + else + { + odometry_.set_wheel_params(front_wheels_radius, wheelbase, wheel_track); + } + const size_t nr_state_itfs = 3; const size_t nr_cmd_itfs = 3; const size_t nr_ref_itfs = 2; set_interface_numbers(nr_state_itfs, nr_cmd_itfs, nr_ref_itfs); - RCLCPP_INFO(get_node()->get_logger(), "ackermann odom configure successful"); + RCLCPP_INFO(get_node()->get_logger(), "tricycle odom configure successful"); return controller_interface::CallbackReturn::SUCCESS; } From f295112f677355bc1163cc2a6ade87c8b627d8f8 Mon Sep 17 00:00:00 2001 From: petkovich Date: Wed, 28 Dec 2022 17:39:58 +0100 Subject: [PATCH 091/186] changed SteeringControllerStatus --- steering_controllers/src/steering_controllers.cpp | 14 ++++++++++++-- 1 file changed, 12 insertions(+), 2 deletions(-) diff --git a/steering_controllers/src/steering_controllers.cpp b/steering_controllers/src/steering_controllers.cpp index 05c0154942..2f656965a0 100644 --- a/steering_controllers/src/steering_controllers.cpp +++ b/steering_controllers/src/steering_controllers.cpp @@ -468,11 +468,21 @@ controller_interface::return_type SteeringControllers::update_and_write_commands controller_state_publisher_->msg_.odom.twist.twist.angular.z = odometry_.get_angular(); if (params_.position_feedback) { - controller_state_publisher_->msg_.rear_wheel_position = state_interfaces_[0].get_value(); + controller_state_publisher_->msg_.traction_wheels_position.data.clear(); + for (size_t i = 0; i < state_interfaces_.size() - 1; i++) + { + controller_state_publisher_->msg_.traction_wheels_position.data.push_back( + state_interfaces_[0].get_value()); + } } else { - controller_state_publisher_->msg_.rear_wheel_velocity = state_interfaces_[0].get_value(); + controller_state_publisher_->msg_.traction_wheels_velocity.data.clear(); + for (size_t i = 0; i < state_interfaces_.size() - 1; i++) + { + controller_state_publisher_->msg_.traction_wheels_velocity.data.push_back( + state_interfaces_[0].get_value()); + } } controller_state_publisher_->msg_.steer_position = state_interfaces_[1].get_value(); controller_state_publisher_->msg_.linear_velocity_command = command_interfaces_[0].get_value(); From d2b87cad6f0b4f2a648300923cbeb58af1a27992 Mon Sep 17 00:00:00 2001 From: petkovich Date: Wed, 28 Dec 2022 18:34:42 +0100 Subject: [PATCH 092/186] command interfaces for multiple wheels --- .../src/steering_controllers.cpp | 30 ++++++++++++++++--- 1 file changed, 26 insertions(+), 4 deletions(-) diff --git a/steering_controllers/src/steering_controllers.cpp b/steering_controllers/src/steering_controllers.cpp index 2f656965a0..8132f36c31 100644 --- a/steering_controllers/src/steering_controllers.cpp +++ b/steering_controllers/src/steering_controllers.cpp @@ -417,9 +417,30 @@ controller_interface::return_type SteeringControllers::update_and_write_commands const double linear_command = reference_interfaces_[0]; const double angular_command = reference_interfaces_[1]; auto [alpha_write, Ws_write] = odometry_.twist_to_ackermann(linear_command, angular_command); - - command_interfaces_[0].set_value(Ws_write); - command_interfaces_[1].set_value(alpha_write); + if (params_.front_steering) + { + for (size_t i = 0; i < params_.rear_wheels_names.size(); i++) + { + command_interfaces_[i].set_value(Ws_write); + } + for (size_t i = 0; i < params_.front_wheels_names.size(); i++) + { + command_interfaces_[i + params_.rear_wheels_names.size()].set_value(alpha_write); + } + } + else + { + { + for (size_t i = 0; i < params_.front_wheels_names.size(); i++) + { + command_interfaces_[i].set_value(Ws_write); + } + for (size_t i = 0; i < params_.rear_wheels_names.size(); i++) + { + command_interfaces_[i + params_.front_wheels_names.size()].set_value(alpha_write); + } + } + } } if (ref_timeout_ == rclcpp::Duration::from_seconds(0) || is_in_chained_mode()) @@ -486,7 +507,8 @@ controller_interface::return_type SteeringControllers::update_and_write_commands } controller_state_publisher_->msg_.steer_position = state_interfaces_[1].get_value(); controller_state_publisher_->msg_.linear_velocity_command = command_interfaces_[0].get_value(); - controller_state_publisher_->msg_.steering_angle_command = command_interfaces_[1].get_value(); + controller_state_publisher_->msg_.steering_angle_command = + command_interfaces_[command_interfaces_.size() - 1].get_value(); controller_state_publisher_->unlockAndPublish(); } From 922099e66e853f36bf04559de14953a48ed447b9 Mon Sep 17 00:00:00 2001 From: petkovich Date: Thu, 29 Dec 2022 11:12:02 +0100 Subject: [PATCH 093/186] odometry types --- .../steering_odometry.hpp | 24 ++++++++++++++----- .../src/ackermann_steering_controller.cpp | 2 ++ .../src/bicycle_steering_controller.cpp | 2 ++ .../src/steering_odometry.cpp | 2 ++ .../src/tricycle_steering_controller.cpp | 2 ++ 5 files changed, 26 insertions(+), 6 deletions(-) diff --git a/steering_controllers/include/steering_controllers/steering_odometry.hpp b/steering_controllers/include/steering_controllers/steering_odometry.hpp index 8d037cf412..693c14bdf3 100644 --- a/steering_controllers/include/steering_controllers/steering_odometry.hpp +++ b/steering_controllers/include/steering_controllers/steering_odometry.hpp @@ -27,6 +27,9 @@ namespace steering_odometry { +const unsigned int BICYCLE_CONFIG = 0; +const unsigned int TRICYCLE_CONFIG = 1; +const unsigned int ACKERMANN_CONFIG = 2; /** * \brief The Odometry class handles odometry readings * (2D pose and velocity with related timestamp) @@ -95,6 +98,12 @@ class SteeringOdometry */ void update_open_loop(const double linear, const double angular, const double dt); + /** + * \brief Set odometry type + * \param type odometry type + */ + void set_odometry_type(const unsigned int type); + /** * \brief heading getter * \return heading [rad] @@ -137,9 +146,9 @@ class SteeringOdometry void set_velocity_rolling_window_size(size_t velocity_rolling_window_size); /** - * \brief TODO - * \param Vx TODO - * \param theta_dot TODO + * \brief Calculates inverse kinematics for the desired linear and angular velocities + * \param Vx Desired linear velocity [m/s] + * \param theta_dot Desired angular velocity [rad/s] */ std::tuple twist_to_ackermann(double Vx, double theta_dot); @@ -171,9 +180,9 @@ class SteeringOdometry void integrate_exact(double linear, double angular); /** - * \brief TODO - * \param Vx TODO - * \param theta_dot TODO + * \brief Calculates steering angle from the desired translational and rotational velocity + * \param Vx Linear velocity [m] + * \param theta_dot Angular velocity [rad] */ double convert_trans_rot_vel_to_steering_angle(double Vx, double theta_dot); @@ -198,6 +207,9 @@ class SteeringOdometry double wheelbase_; double wheel_radius_; + /// Configuration type used for thr forward kinematics + unsigned int config_type_; + /// Previous wheel position/state [rad]: double rear_wheel_old_pos_; double rear_right_wheel_old_pos_; diff --git a/steering_controllers/src/ackermann_steering_controller.cpp b/steering_controllers/src/ackermann_steering_controller.cpp index 16362ca347..f06d240dd0 100644 --- a/steering_controllers/src/ackermann_steering_controller.cpp +++ b/steering_controllers/src/ackermann_steering_controller.cpp @@ -46,6 +46,8 @@ controller_interface::CallbackReturn AckermannSteeringController::configure_odom odometry_.set_wheel_params(front_wheels_radius, wheelbase, front_wheel_track); } + odometry_.set_odometry_type(steering_odometry::TRICYCLE_CONFIG); + const size_t nr_state_itfs = 4; const size_t nr_cmd_itfs = 4; const size_t nr_ref_itfs = 2; diff --git a/steering_controllers/src/bicycle_steering_controller.cpp b/steering_controllers/src/bicycle_steering_controller.cpp index ab53c36482..f9ec4420b8 100644 --- a/steering_controllers/src/bicycle_steering_controller.cpp +++ b/steering_controllers/src/bicycle_steering_controller.cpp @@ -43,6 +43,8 @@ controller_interface::CallbackReturn BicycleSteeringController::configure_odomet odometry_.set_wheel_params(front_wheel_radius, wheelbase); } + odometry_.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); + const size_t nr_state_itfs = 2; const size_t nr_cmd_itfs = 2; const size_t nr_ref_itfs = 2; diff --git a/steering_controllers/src/steering_odometry.cpp b/steering_controllers/src/steering_odometry.cpp index 870cb9b10d..97922bc825 100644 --- a/steering_controllers/src/steering_odometry.cpp +++ b/steering_controllers/src/steering_odometry.cpp @@ -203,6 +203,8 @@ void SteeringOdometry::set_velocity_rolling_window_size(size_t velocity_rolling_ reset_accumulators(); } +void SteeringOdometry::set_odometry_type(const unsigned int type) { config_type_ = type; } + // TODO(petkovich): change functions depending on fwd kinematics model double SteeringOdometry::convert_trans_rot_vel_to_steering_angle(double Vx, double theta_dot) { diff --git a/steering_controllers/src/tricycle_steering_controller.cpp b/steering_controllers/src/tricycle_steering_controller.cpp index bcf0058062..f9988d7eef 100644 --- a/steering_controllers/src/tricycle_steering_controller.cpp +++ b/steering_controllers/src/tricycle_steering_controller.cpp @@ -44,6 +44,8 @@ controller_interface::CallbackReturn TricycleSteeringController::configure_odome odometry_.set_wheel_params(front_wheels_radius, wheelbase, wheel_track); } + odometry_.set_odometry_type(steering_odometry::TRICYCLE_CONFIG); + const size_t nr_state_itfs = 3; const size_t nr_cmd_itfs = 3; const size_t nr_ref_itfs = 2; From 599b223eddc310dbf7ee483dfa39e2671ac78f22 Mon Sep 17 00:00:00 2001 From: petkovich Date: Thu, 29 Dec 2022 11:50:04 +0100 Subject: [PATCH 094/186] forward kinematics model template --- .../steering_odometry.hpp | 5 ++- .../src/steering_controllers.cpp | 12 +++--- .../src/steering_odometry.cpp | 42 ++++++++++++++----- 3 files changed, 42 insertions(+), 17 deletions(-) diff --git a/steering_controllers/include/steering_controllers/steering_odometry.hpp b/steering_controllers/include/steering_controllers/steering_odometry.hpp index 693c14bdf3..138cb684a3 100644 --- a/steering_controllers/include/steering_controllers/steering_odometry.hpp +++ b/steering_controllers/include/steering_controllers/steering_odometry.hpp @@ -18,6 +18,7 @@ #pragma once #include +#include #include "realtime_tools/realtime_buffer.h" #include "realtime_tools/realtime_publisher.h" @@ -44,7 +45,6 @@ class SteeringOdometry * \param velocity_rolling_window_size Rolling window size used to compute the velocity mean * */ - // ackermann_steering_controller_ros2::Params params; explicit SteeringOdometry(size_t velocity_rolling_window_size = 10); /** @@ -149,8 +149,9 @@ class SteeringOdometry * \brief Calculates inverse kinematics for the desired linear and angular velocities * \param Vx Desired linear velocity [m/s] * \param theta_dot Desired angular velocity [rad/s] + * \return Tuple of velocity commands and steering commands */ - std::tuple twist_to_ackermann(double Vx, double theta_dot); + std::tuple, std::vector> get_commands(double Vx, double theta_dot); /** * \brief Reset poses, heading, and accumulators diff --git a/steering_controllers/src/steering_controllers.cpp b/steering_controllers/src/steering_controllers.cpp index 8132f36c31..5ec108e66a 100644 --- a/steering_controllers/src/steering_controllers.cpp +++ b/steering_controllers/src/steering_controllers.cpp @@ -416,16 +416,17 @@ controller_interface::return_type SteeringControllers::update_and_write_commands // store and set commands const double linear_command = reference_interfaces_[0]; const double angular_command = reference_interfaces_[1]; - auto [alpha_write, Ws_write] = odometry_.twist_to_ackermann(linear_command, angular_command); + auto [traction_commands, steering_commands] = + odometry_.get_commands(linear_command, angular_command); if (params_.front_steering) { for (size_t i = 0; i < params_.rear_wheels_names.size(); i++) { - command_interfaces_[i].set_value(Ws_write); + command_interfaces_[i].set_value(traction_commands[i]); } for (size_t i = 0; i < params_.front_wheels_names.size(); i++) { - command_interfaces_[i + params_.rear_wheels_names.size()].set_value(alpha_write); + command_interfaces_[i + params_.rear_wheels_names.size()].set_value(steering_commands[i]); } } else @@ -433,11 +434,12 @@ controller_interface::return_type SteeringControllers::update_and_write_commands { for (size_t i = 0; i < params_.front_wheels_names.size(); i++) { - command_interfaces_[i].set_value(Ws_write); + command_interfaces_[i].set_value(traction_commands[i]); } for (size_t i = 0; i < params_.rear_wheels_names.size(); i++) { - command_interfaces_[i + params_.front_wheels_names.size()].set_value(alpha_write); + command_interfaces_[i + params_.front_wheels_names.size()].set_value( + steering_commands[i]); } } } diff --git a/steering_controllers/src/steering_odometry.cpp b/steering_controllers/src/steering_odometry.cpp index 97922bc825..24d7852dc5 100644 --- a/steering_controllers/src/steering_odometry.cpp +++ b/steering_controllers/src/steering_odometry.cpp @@ -205,7 +205,6 @@ void SteeringOdometry::set_velocity_rolling_window_size(size_t velocity_rolling_ void SteeringOdometry::set_odometry_type(const unsigned int type) { config_type_ = type; } -// TODO(petkovich): change functions depending on fwd kinematics model double SteeringOdometry::convert_trans_rot_vel_to_steering_angle(double Vx, double theta_dot) { if (theta_dot == 0 || Vx == 0) @@ -215,22 +214,45 @@ double SteeringOdometry::convert_trans_rot_vel_to_steering_angle(double Vx, doub return std::atan(theta_dot * wheelbase_ / Vx); } -// TODO(petkovich): change functions depending on fwd kinematics model -std::tuple SteeringOdometry::twist_to_ackermann(double Vx, double theta_dot) +std::tuple, std::vector> SteeringOdometry::get_commands( + double Vx, double theta_dot) { - // using naming convention in http://users.isr.ist.utl.pt/~mir/cadeiras/robmovel/Kinematics.pdf - double alpha, Ws; + // desired velocty and steering angle of the midle of traction and steering axis + double Ws, alpha; if (Vx == 0 && theta_dot != 0) - { // is spin action + { alpha = theta_dot > 0 ? M_PI_2 : -M_PI_2; Ws = abs(theta_dot) * wheelbase_ / wheel_radius_; - return std::make_tuple(alpha, Ws); + } + else + { + alpha = SteeringOdometry::convert_trans_rot_vel_to_steering_angle(Vx, theta_dot); + Ws = Vx / (wheel_radius_ * std::cos(alpha)); } - alpha = SteeringOdometry::convert_trans_rot_vel_to_steering_angle(Vx, theta_dot); - Ws = Vx / (wheel_radius_ * std::cos(alpha)); - return std::make_tuple(alpha, Ws); + if (config_type_ == BICYCLE_CONFIG) + { + std::vector traction_commands = {Ws}; + std::vector steering_commands = {alpha}; + return std::make_tuple(traction_commands, steering_commands); + } + else if (config_type_ == TRICYCLE_CONFIG) + { + std::vector traction_commands = {Ws, Ws}; + std::vector steering_commands = {alpha}; + return std::make_tuple(traction_commands, steering_commands); + } + else if (config_type_ == ACKERMANN_CONFIG) + { + std::vector traction_commands = {Ws, Ws}; + std::vector steering_commands = {alpha, alpha}; + return std::make_tuple(traction_commands, steering_commands); + } + else + { + throw std::runtime_error("Config not implemented"); + } } void SteeringOdometry::reset_odometry() From da4b41c78a91664eac1f04aa61cf547ae1b9a1ff Mon Sep 17 00:00:00 2001 From: petkovich Date: Thu, 29 Dec 2022 12:01:11 +0100 Subject: [PATCH 095/186] command interface always has traction before steering --- .../src/steering_controllers.cpp | 35 +++++++++++++------ 1 file changed, 25 insertions(+), 10 deletions(-) diff --git a/steering_controllers/src/steering_controllers.cpp b/steering_controllers/src/steering_controllers.cpp index 5ec108e66a..70c3bdfcab 100644 --- a/steering_controllers/src/steering_controllers.cpp +++ b/steering_controllers/src/steering_controllers.cpp @@ -271,19 +271,34 @@ controller_interface::InterfaceConfiguration SteeringControllers::command_interf controller_interface::InterfaceConfiguration command_interfaces_config; command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; command_interfaces_config.names.reserve(nr_cmd_itfs_); - const auto front_wheels_command = params_.front_steering ? hardware_interface::HW_IF_POSITION - : hardware_interface::HW_IF_VELOCITY; - const auto rear_wheels_command = params_.front_steering ? hardware_interface::HW_IF_VELOCITY - : hardware_interface::HW_IF_POSITION; - for (size_t i = 0; i < params_.rear_wheels_names.size(); i++) + + if (params_.front_steering) { - command_interfaces_config.names.push_back( - params_.rear_wheels_names[i] + "/" + rear_wheels_command); + for (size_t i = 0; i < params_.rear_wheels_names.size(); i++) + { + command_interfaces_config.names.push_back( + params_.rear_wheels_names[i] + "/" + hardware_interface::HW_IF_VELOCITY); + } + + for (size_t i = 0; i < params_.front_wheels_names.size(); i++) + { + command_interfaces_config.names.push_back( + params_.front_wheels_names[i] + "/" + hardware_interface::HW_IF_POSITION); + } } - for (size_t i = 0; i < params_.front_wheels_names.size(); i++) + else { - command_interfaces_config.names.push_back( - params_.front_wheels_names[i] + "/" + front_wheels_command); + for (size_t i = 0; i < params_.front_wheels_names.size(); i++) + { + command_interfaces_config.names.push_back( + params_.front_wheels_names[i] + "/" + hardware_interface::HW_IF_VELOCITY); + } + + for (size_t i = 0; i < params_.rear_wheels_names.size(); i++) + { + command_interfaces_config.names.push_back( + params_.rear_wheels_names[i] + "/" + hardware_interface::HW_IF_POSITION); + } } return command_interfaces_config; } From 98eba40ad599ebd9c58bbe38ef9d4b050f2b74e1 Mon Sep 17 00:00:00 2001 From: petkovich Date: Thu, 29 Dec 2022 12:07:35 +0100 Subject: [PATCH 096/186] configs --- steering_controllers/src/ackermann_steering_controller.cpp | 2 +- steering_controllers/src/bicycle_steering_controller.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/steering_controllers/src/ackermann_steering_controller.cpp b/steering_controllers/src/ackermann_steering_controller.cpp index f06d240dd0..0afb8199fa 100644 --- a/steering_controllers/src/ackermann_steering_controller.cpp +++ b/steering_controllers/src/ackermann_steering_controller.cpp @@ -46,7 +46,7 @@ controller_interface::CallbackReturn AckermannSteeringController::configure_odom odometry_.set_wheel_params(front_wheels_radius, wheelbase, front_wheel_track); } - odometry_.set_odometry_type(steering_odometry::TRICYCLE_CONFIG); + odometry_.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); const size_t nr_state_itfs = 4; const size_t nr_cmd_itfs = 4; diff --git a/steering_controllers/src/bicycle_steering_controller.cpp b/steering_controllers/src/bicycle_steering_controller.cpp index f9ec4420b8..f296690ed0 100644 --- a/steering_controllers/src/bicycle_steering_controller.cpp +++ b/steering_controllers/src/bicycle_steering_controller.cpp @@ -43,7 +43,7 @@ controller_interface::CallbackReturn BicycleSteeringController::configure_odomet odometry_.set_wheel_params(front_wheel_radius, wheelbase); } - odometry_.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); + odometry_.set_odometry_type(steering_odometry::BICYCLE_CONFIG); const size_t nr_state_itfs = 2; const size_t nr_cmd_itfs = 2; From 0e3aa8f7a5a9e7befd81868c1926b0c63e8d1ed5 Mon Sep 17 00:00:00 2001 From: petkovich Date: Thu, 29 Dec 2022 13:01:10 +0100 Subject: [PATCH 097/186] fwd kinematics implementation, TODO ackermann --- .../src/steering_controllers.cpp | 61 +++++++++++++++---- .../src/steering_odometry.cpp | 36 +++++++++-- 2 files changed, 80 insertions(+), 17 deletions(-) diff --git a/steering_controllers/src/steering_controllers.cpp b/steering_controllers/src/steering_controllers.cpp index 70c3bdfcab..b5a76e0e4b 100644 --- a/steering_controllers/src/steering_controllers.cpp +++ b/steering_controllers/src/steering_controllers.cpp @@ -504,28 +504,63 @@ controller_interface::return_type SteeringControllers::update_and_write_commands controller_state_publisher_->msg_.odom.pose.pose.orientation = tf2::toMsg(orientation); controller_state_publisher_->msg_.odom.twist.twist.linear.x = odometry_.get_linear(); controller_state_publisher_->msg_.odom.twist.twist.angular.z = odometry_.get_angular(); - if (params_.position_feedback) + controller_state_publisher_->msg_.traction_wheels_position.data.clear(); + controller_state_publisher_->msg_.traction_wheels_velocity.data.clear(); + controller_state_publisher_->msg_.linear_velocity_command.data.clear(); + controller_state_publisher_->msg_.steer_positions.data.clear(); + controller_state_publisher_->msg_.steering_angle_command.data.clear(); + + if (params_.front_steering) { - controller_state_publisher_->msg_.traction_wheels_position.data.clear(); - for (size_t i = 0; i < state_interfaces_.size() - 1; i++) + for (size_t i = 0; i < params_.rear_wheels_names.size(); i++) + { + if (params_.position_feedback) + { + controller_state_publisher_->msg_.traction_wheels_position.data.push_back( + state_interfaces_[i].get_value()); + } + else + { + controller_state_publisher_->msg_.traction_wheels_velocity.data.push_back( + state_interfaces_[i].get_value()); + } + controller_state_publisher_->msg_.linear_velocity_command.data.push_back( + command_interfaces_[i].get_value()); + } + for (size_t i = 0; i < params_.front_wheels_names.size(); i++) { - controller_state_publisher_->msg_.traction_wheels_position.data.push_back( - state_interfaces_[0].get_value()); + controller_state_publisher_->msg_.steer_positions.data.push_back( + state_interfaces_[params_.rear_wheels_names.size() + i].get_value()); + controller_state_publisher_->msg_.steering_angle_command.data.push_back( + command_interfaces_[params_.rear_wheels_names.size() + i].get_value()); } } + else { - controller_state_publisher_->msg_.traction_wheels_velocity.data.clear(); - for (size_t i = 0; i < state_interfaces_.size() - 1; i++) + for (size_t i = 0; i < params_.front_wheels_names.size(); i++) + { + if (params_.position_feedback) + { + controller_state_publisher_->msg_.traction_wheels_position.data.push_back( + state_interfaces_[i].get_value()); + } + else + { + controller_state_publisher_->msg_.traction_wheels_velocity.data.push_back( + state_interfaces_[i].get_value()); + } + controller_state_publisher_->msg_.linear_velocity_command.data.push_back( + command_interfaces_[i].get_value()); + } + for (size_t i = 0; i < params_.rear_wheels_names.size(); i++) { - controller_state_publisher_->msg_.traction_wheels_velocity.data.push_back( - state_interfaces_[0].get_value()); + controller_state_publisher_->msg_.steer_positions.data.push_back( + state_interfaces_[params_.front_wheels_names.size() + i].get_value()); + controller_state_publisher_->msg_.steering_angle_command.data.push_back( + command_interfaces_[params_.front_wheels_names.size() + i].get_value()); } } - controller_state_publisher_->msg_.steer_position = state_interfaces_[1].get_value(); - controller_state_publisher_->msg_.linear_velocity_command = command_interfaces_[0].get_value(); - controller_state_publisher_->msg_.steering_angle_command = - command_interfaces_[command_interfaces_.size() - 1].get_value(); controller_state_publisher_->unlockAndPublish(); } diff --git a/steering_controllers/src/steering_odometry.cpp b/steering_controllers/src/steering_odometry.cpp index 24d7852dc5..41d18a8d29 100644 --- a/steering_controllers/src/steering_odometry.cpp +++ b/steering_controllers/src/steering_odometry.cpp @@ -239,14 +239,42 @@ std::tuple, std::vector> SteeringOdometry::get_comma } else if (config_type_ == TRICYCLE_CONFIG) { - std::vector traction_commands = {Ws, Ws}; - std::vector steering_commands = {alpha}; + std::vector traction_commands; + std::vector steering_commands; + if (fabs(heading_) < 1e-6) + { + traction_commands = {Ws, Ws}; + } + else + { + double turning_radius = wheelbase_ / std::tan(heading_); + double Wr = Ws * (turning_radius - wheel_track_ * 0.5) / turning_radius; + double Wl = Ws * (turning_radius + wheel_track_ * 0.5) / turning_radius; + traction_commands = {Wr, Wl}; + } + steering_commands = {alpha}; return std::make_tuple(traction_commands, steering_commands); } else if (config_type_ == ACKERMANN_CONFIG) { - std::vector traction_commands = {Ws, Ws}; - std::vector steering_commands = {alpha, alpha}; + std::vector traction_commands; + std::vector steering_commands; + if (fabs(heading_) < 1e-6) + { + traction_commands = {Ws, Ws}; + } + else + { + double turning_radius = wheelbase_ / std::tan(heading_); + double Wr = Ws * (turning_radius - wheel_track_ * 0.5) / turning_radius; + double Wl = Ws * (turning_radius + wheel_track_ * 0.5) / turning_radius; + traction_commands = {Wr, Wl}; + } + // TODO(petkovich): implement this + // double wanted_turning_radius = wheelbase_ / std::tan(alpha); + double alpha_r = alpha; + double alpha_l = alpha; + steering_commands = {alpha_r, alpha_l}; return std::make_tuple(traction_commands, steering_commands); } else From be9c0e17c56dbe56dfa28c4983ed0fe3feb814eb Mon Sep 17 00:00:00 2001 From: petkovich Date: Thu, 29 Dec 2022 15:04:56 +0100 Subject: [PATCH 098/186] steer_pos_ in odometry, adjusted names of arguments for update function --- .../steering_odometry.hpp | 76 ++++++++---- .../src/steering_odometry.cpp | 112 ++++++++++-------- 2 files changed, 115 insertions(+), 73 deletions(-) diff --git a/steering_controllers/include/steering_controllers/steering_odometry.hpp b/steering_controllers/include/steering_controllers/steering_odometry.hpp index 138cb684a3..61237a6845 100644 --- a/steering_controllers/include/steering_controllers/steering_odometry.hpp +++ b/steering_controllers/include/steering_controllers/steering_odometry.hpp @@ -55,40 +55,73 @@ class SteeringOdometry /** * \brief Updates the odometry class with latest wheels position - * \param rear_wheel_pos Rear wheel position [rad] - * \param front_steer_pos Front Steer position [rad] + * \param traction_wheel_pos traction wheel position [rad] + * \param steer_pos Front Steer position [rad] * \param dt time difference to last call * \return true if the odometry is actually updated */ - bool update_from_position( - const double rear_wheel_pos, const double front_steer_pos, const double dt); + const double traction_wheel_pos, const double steer_pos, const double dt); + /** + * \brief Updates the odometry class with latest wheels position + * \param right_traction_wheel_pos Right traction wheel velocity [rad] + * \param left_traction_wheel_pos Left traction wheel velocity [rad] + * \param front_steer_pos Steer wheel position [rad] + * \param dt time difference to last call + * \return true if the odometry is actually updated + */ bool update_from_position( - const double rear_right_wheel_pos, const double rear_left_wheel_pos, - const double front_steer_pos, const double dt); + const double right_traction_wheel_pos, const double left_traction_wheel_pos, + const double steer_pos, const double dt); + /** + * \brief Updates the odometry class with latest wheels position + * \param right_traction_wheel_pos Right traction wheel position [rad] + * \param left_traction_wheel_pos Left traction wheel position [rad] + * \param right_steer_pos Right steer wheel position [rad] + * \param left_steer_pos Left steer wheel position [rad] + * \param dt time difference to last call + * \return true if the odometry is actually updated + */ bool update_from_position( - const double rear_right_wheel_pos, const double rear_left_wheel_pos, - const double front_right_steer_pos, const double front_left_steer_pos, const double dt); + const double right_traction_wheel_pos, const double left_traction_wheel_pos, + const double right_steer_pos, const double left_steer_pos, const double dt); /** * \brief Updates the odometry class with latest wheels position - * \param rear_wheel_vel Rear wheel velocity [rad/s] - * \param front_steer_pos Front Steer position [rad] + * \param traction_wheel_vel Traction wheel velocity [rad/s] + * \param front_steer_pos Steer wheel position [rad] * \param dt time difference to last call * \return true if the odometry is actually updated */ bool update_from_velocity( - const double rear_wheel_vel, const double front_steer_pos, const double dt); + const double traction_wheel_vel, const double steer_pos, const double dt); + /** + * \brief Updates the odometry class with latest wheels position + * \param right_traction_wheel_vel Right traction wheel velocity [rad/s] + * \param left_traction_wheel_vel Left traction wheel velocity [rad/s] + * \param front_steer_pos Steer wheel position [rad] + * \param dt time difference to last call + * \return true if the odometry is actually updated + */ bool update_from_velocity( - const double rear_right_wheel_vel, const double rear_left_wheel_vel, - const double front_steer_pos, const double dt); + const double right_traction_wheel_vel, const double left_traction_wheel_vel, + const double steer_pos, const double dt); + /** + * \brief Updates the odometry class with latest wheels position + * \param right_traction_wheel_vel Right traction wheel velocity [rad/s] + * \param left_traction_wheel_vel Left traction wheel velocity [rad/s] + * \param right_steer_pos Right steer wheel position [rad] + * \param left_steer_pos Left steer wheel position [rad] + * \param dt time difference to last call + * \return true if the odometry is actually updated + */ bool update_from_velocity( - const double rear_right_wheel_vel, const double rear_left_wheel_vel, - const double front_right_steer_pos, const double front_left_steer_pos, const double dt); + const double right_traction_wheel_vel, const double left_traction_wheel_vel, + const double right_steer_pos, const double left_steer_pos, const double dt); /** * \brief Updates the odometry class with latest velocity command @@ -196,9 +229,10 @@ class SteeringOdometry rclcpp::Time timestamp_; /// Current pose: - double x_; // [m] - double y_; // [m] - double heading_; // [rad] + double x_; // [m] + double y_; // [m] + double steer_pos_; // [rad] + double heading_; // [rad] /// Current velocity: double linear_; // [m/s] @@ -212,9 +246,9 @@ class SteeringOdometry unsigned int config_type_; /// Previous wheel position/state [rad]: - double rear_wheel_old_pos_; - double rear_right_wheel_old_pos_; - double rear_left_wheel_old_pos_; + double traction_wheel_old_pos_; + double traction_right_wheel_old_pos_; + double traction_left_wheel_old_pos_; /// Rolling mean accumulators for the linar and angular velocities: size_t velocity_rolling_window_size_; rcpputils::RollingMeanAccumulator linear_acc_; diff --git a/steering_controllers/src/steering_odometry.cpp b/steering_controllers/src/steering_odometry.cpp index 41d18a8d29..416f289791 100644 --- a/steering_controllers/src/steering_odometry.cpp +++ b/steering_controllers/src/steering_odometry.cpp @@ -36,7 +36,7 @@ SteeringOdometry::SteeringOdometry(size_t velocity_rolling_window_size) wheel_track_(0.0), wheelbase_(0.0), wheel_radius_(0.0), - rear_wheel_old_pos_(0.0), + traction_wheel_old_pos_(0.0), velocity_rolling_window_size_(velocity_rolling_window_size), linear_acc_(velocity_rolling_window_size), angular_acc_(velocity_rolling_window_size) @@ -72,20 +72,21 @@ bool SteeringOdometry::update_odometry( return true; } -// TODO(petkovich): enable also velocity interface to update using velocity from the rear wheel +// TODO(petkovich): enable also velocity interface to update using velocity from the traction wheel bool SteeringOdometry::update_from_position( - const double rear_wheel_pos, const double front_steer_pos, const double dt) + const double traction_wheel_pos, const double steer_pos, const double dt) { /// Get current wheel joint positions: - const double rear_wheel_cur_pos = rear_wheel_pos * wheel_radius_; - const double rear_wheel_est_pos_diff = rear_wheel_cur_pos - rear_wheel_old_pos_; + const double traction_wheel_cur_pos = traction_wheel_pos * wheel_radius_; + const double traction_wheel_est_pos_diff = traction_wheel_cur_pos - traction_wheel_old_pos_; /// Update old position with current: - rear_wheel_old_pos_ = rear_wheel_cur_pos; + traction_wheel_old_pos_ = traction_wheel_cur_pos; /// Compute linear and angular diff: - const double linear_velocity = rear_wheel_est_pos_diff / dt; - const double angular = tan(front_steer_pos) * linear_velocity / wheelbase_; + const double linear_velocity = traction_wheel_est_pos_diff / dt; + steer_pos_ = steer_pos; + const double angular = tan(steer_pos) * linear_velocity / wheelbase_; update_odometry(linear_velocity, angular, dt); @@ -93,49 +94,53 @@ bool SteeringOdometry::update_from_position( } bool SteeringOdometry::update_from_position( - const double rear_right_wheel_pos, const double rear_left_wheel_pos, const double front_steer_pos, - const double dt) + const double traction_right_wheel_pos, const double traction_left_wheel_pos, + const double steer_pos, const double dt) { /// Get current wheel joint positions: - const double rear_right_wheel_cur_pos = rear_right_wheel_pos * wheel_radius_; - const double rear_left_wheel_cur_pos = rear_left_wheel_pos * wheel_radius_; + const double traction_right_wheel_cur_pos = traction_right_wheel_pos * wheel_radius_; + const double traction_left_wheel_cur_pos = traction_left_wheel_pos * wheel_radius_; - const double rear_right_wheel_est_pos_diff = rear_right_wheel_cur_pos - rear_right_wheel_old_pos_; - const double rear_left_wheel_est_pos_diff = rear_left_wheel_cur_pos - rear_left_wheel_old_pos_; + const double traction_right_wheel_est_pos_diff = + traction_right_wheel_cur_pos - traction_right_wheel_old_pos_; + const double traction_left_wheel_est_pos_diff = + traction_left_wheel_cur_pos - traction_left_wheel_old_pos_; /// Update old position with current: - rear_right_wheel_old_pos_ = rear_right_wheel_cur_pos; - rear_left_wheel_old_pos_ = rear_left_wheel_cur_pos; + traction_right_wheel_old_pos_ = traction_right_wheel_cur_pos; + traction_left_wheel_old_pos_ = traction_left_wheel_cur_pos; const double linear_velocity = - (rear_right_wheel_est_pos_diff + rear_left_wheel_est_pos_diff) * 0.5 / dt; - const double angular = tan(front_steer_pos) * linear_velocity / wheelbase_; - + (traction_right_wheel_est_pos_diff + traction_left_wheel_est_pos_diff) * 0.5 / dt; + steer_pos_ = steer_pos; + const double angular = tan(steer_pos_) * linear_velocity / wheelbase_; update_odometry(linear_velocity, angular, dt); return true; } bool SteeringOdometry::update_from_position( - const double rear_right_wheel_pos, const double rear_left_wheel_pos, - const double front_right_steer_pos, const double front_left_steer_pos, const double dt) + const double traction_right_wheel_pos, const double traction_left_wheel_pos, + const double right_steer_pos, const double left_steer_pos, const double dt) { /// Get current wheel joint positions: - const double rear_right_wheel_cur_pos = rear_right_wheel_pos * wheel_radius_; - const double rear_left_wheel_cur_pos = rear_left_wheel_pos * wheel_radius_; + const double traction_right_wheel_cur_pos = traction_right_wheel_pos * wheel_radius_; + const double traction_left_wheel_cur_pos = traction_left_wheel_pos * wheel_radius_; - const double rear_right_wheel_est_pos_diff = rear_right_wheel_cur_pos - rear_right_wheel_old_pos_; - const double rear_left_wheel_est_pos_diff = rear_left_wheel_cur_pos - rear_left_wheel_old_pos_; + const double traction_right_wheel_est_pos_diff = + traction_right_wheel_cur_pos - traction_right_wheel_old_pos_; + const double traction_left_wheel_est_pos_diff = + traction_left_wheel_cur_pos - traction_left_wheel_old_pos_; /// Update old position with current: - rear_right_wheel_old_pos_ = rear_right_wheel_cur_pos; - rear_left_wheel_old_pos_ = rear_left_wheel_cur_pos; + traction_right_wheel_old_pos_ = traction_right_wheel_cur_pos; + traction_left_wheel_old_pos_ = traction_left_wheel_cur_pos; /// Compute linear and angular diff: const double linear_velocity = - (rear_right_wheel_est_pos_diff + rear_left_wheel_est_pos_diff) * 0.5 / dt; - const double front_steer_pos = (front_right_steer_pos + front_left_steer_pos) * 0.5; - const double angular = tan(front_steer_pos) * linear_velocity / wheelbase_; + (traction_right_wheel_est_pos_diff + traction_left_wheel_est_pos_diff) * 0.5 / dt; + steer_pos_ = (right_steer_pos + left_steer_pos) * 0.5; + const double angular = tan(steer_pos_) * linear_velocity / wheelbase_; update_odometry(linear_velocity, angular, dt); @@ -143,10 +148,11 @@ bool SteeringOdometry::update_from_position( } bool SteeringOdometry::update_from_velocity( - const double rear_wheel_vel, const double front_steer_pos, const double dt) + const double traction_wheel_vel, const double steer_pos, const double dt) { - double linear_velocity = rear_wheel_vel * wheel_radius_; - const double angular = tan(front_steer_pos) * linear_velocity / wheelbase_; + steer_pos_ = steer_pos; + double linear_velocity = traction_wheel_vel * wheel_radius_; + const double angular = tan(steer_pos) * linear_velocity / wheelbase_; update_odometry(linear_velocity, angular, dt); @@ -154,25 +160,27 @@ bool SteeringOdometry::update_from_velocity( } bool SteeringOdometry::update_from_velocity( - const double rear_right_wheel_vel, const double rear_left_wheel_vel, const double front_steer_pos, - const double dt) + const double right_traction_wheel_vel, const double left_traction_wheel_vel, + const double steer_pos, const double dt) { - double linear_velocity = (rear_right_wheel_vel + rear_left_wheel_vel) * wheel_radius_ * 0.5; - const double angular = tan(front_steer_pos) * linear_velocity / wheelbase_; + double linear_velocity = + (right_traction_wheel_vel + left_traction_wheel_vel) * wheel_radius_ * 0.5; + steer_pos_ = steer_pos; + const double angular = tan(steer_pos_) * linear_velocity / wheelbase_; update_odometry(linear_velocity, angular, dt); return true; } bool SteeringOdometry::update_from_velocity( - const double rear_right_wheel_vel, const double rear_left_wheel_vel, - const double front_right_steer_pos, const double front_left_steer_pos, const double dt) + const double right_traction_wheel_vel, const double left_traction_wheel_vel, + const double right_steer_pos, const double left_steer_pos, const double dt) { - double linear_velocity = (rear_right_wheel_vel + rear_left_wheel_vel) * wheel_radius_ * 0.5; - const double front_steer_pos = (front_right_steer_pos + front_left_steer_pos) * 0.5; - const double angular = tan(front_steer_pos) * linear_velocity / wheelbase_; - // TODO(petkovich): Does wheel_track_ come into play if Wr and Wl are not the same? + steer_pos_ = (right_steer_pos + left_steer_pos) * 0.5; + double linear_velocity = + (right_traction_wheel_vel + left_traction_wheel_vel) * wheel_radius_ * 0.5; + const double angular = steer_pos_ * linear_velocity / wheelbase_; update_odometry(linear_velocity, angular, dt); @@ -228,7 +236,7 @@ std::tuple, std::vector> SteeringOdometry::get_comma else { alpha = SteeringOdometry::convert_trans_rot_vel_to_steering_angle(Vx, theta_dot); - Ws = Vx / (wheel_radius_ * std::cos(alpha)); + Ws = Vx / (wheel_radius_ * std::cos(steer_pos_)); } if (config_type_ == BICYCLE_CONFIG) @@ -241,15 +249,15 @@ std::tuple, std::vector> SteeringOdometry::get_comma { std::vector traction_commands; std::vector steering_commands; - if (fabs(heading_) < 1e-6) + if (fabs(steer_pos_) < 1e-6) { traction_commands = {Ws, Ws}; } else { - double turning_radius = wheelbase_ / std::tan(heading_); - double Wr = Ws * (turning_radius - wheel_track_ * 0.5) / turning_radius; - double Wl = Ws * (turning_radius + wheel_track_ * 0.5) / turning_radius; + double turning_radius = wheelbase_ / std::tan(steer_pos_); + double Wr = Ws * (turning_radius + wheel_track_ * 0.5) / turning_radius; + double Wl = Ws * (turning_radius - wheel_track_ * 0.5) / turning_radius; traction_commands = {Wr, Wl}; } steering_commands = {alpha}; @@ -259,15 +267,15 @@ std::tuple, std::vector> SteeringOdometry::get_comma { std::vector traction_commands; std::vector steering_commands; - if (fabs(heading_) < 1e-6) + if (fabs(steer_pos_) < 1e-6) { traction_commands = {Ws, Ws}; } else { - double turning_radius = wheelbase_ / std::tan(heading_); - double Wr = Ws * (turning_radius - wheel_track_ * 0.5) / turning_radius; - double Wl = Ws * (turning_radius + wheel_track_ * 0.5) / turning_radius; + double turning_radius = wheelbase_ / std::tan(steer_pos_); + double Wr = Ws * (turning_radius + wheel_track_ * 0.5) / turning_radius; + double Wl = Ws * (turning_radius - wheel_track_ * 0.5) / turning_radius; traction_commands = {Wr, Wl}; } // TODO(petkovich): implement this From e4ff91d376d4101bc79906186de5e7a1372eb8b8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Tue, 3 Jan 2023 17:00:26 +0100 Subject: [PATCH 099/186] Add new package for Bicycle controller. --- bicycle_steering_controller/CMakeLists.txt | 26 +++ bicycle_steering_controller/LICENSE | 202 +++++++++++++++++++++ bicycle_steering_controller/package.xml | 18 ++ ros2_controllers/package.xml | 1 + 4 files changed, 247 insertions(+) create mode 100644 bicycle_steering_controller/CMakeLists.txt create mode 100644 bicycle_steering_controller/LICENSE create mode 100644 bicycle_steering_controller/package.xml diff --git a/bicycle_steering_controller/CMakeLists.txt b/bicycle_steering_controller/CMakeLists.txt new file mode 100644 index 0000000000..3b1c3068d3 --- /dev/null +++ b/bicycle_steering_controller/CMakeLists.txt @@ -0,0 +1,26 @@ +cmake_minimum_required(VERSION 3.8) +project(bicycle_steering_controller) + +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/bicycle_steering_controller/LICENSE b/bicycle_steering_controller/LICENSE new file mode 100644 index 0000000000..d645695673 --- /dev/null +++ b/bicycle_steering_controller/LICENSE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + 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. diff --git a/bicycle_steering_controller/package.xml b/bicycle_steering_controller/package.xml new file mode 100644 index 0000000000..8623e04c43 --- /dev/null +++ b/bicycle_steering_controller/package.xml @@ -0,0 +1,18 @@ + + + + bicycle_steering_controller + 0.0.0 + Steering controller with bicycle kinematics. Rear fixed wheel is powering the wehicle and front wheel is steering. + Dr.-Ing. Denis Štogl + Apache-2.0 + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index e91679802c..db2730ed1c 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -11,6 +11,7 @@ ament_cmake admittance_controller + bicycle_steering_controller diff_drive_controller effort_controllers force_torque_sensor_broadcaster From 4074602de2efa33b835d31e1bebb6f993d38e829 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Tue, 3 Jan 2023 17:03:03 +0100 Subject: [PATCH 100/186] Add new package for Tricycle controller. --- ros2_controllers/package.xml | 1 + tricycle_steering_controller/CMakeLists.txt | 26 +++ tricycle_steering_controller/LICENSE | 202 ++++++++++++++++++++ tricycle_steering_controller/package.xml | 18 ++ 4 files changed, 247 insertions(+) create mode 100644 tricycle_steering_controller/CMakeLists.txt create mode 100644 tricycle_steering_controller/LICENSE create mode 100644 tricycle_steering_controller/package.xml diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index db2730ed1c..94e15973f1 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -21,6 +21,7 @@ joint_trajectory_controller position_controllers tricycle_controller + tricycle_steering_controller velocity_controllers diff --git a/tricycle_steering_controller/CMakeLists.txt b/tricycle_steering_controller/CMakeLists.txt new file mode 100644 index 0000000000..a0298ee275 --- /dev/null +++ b/tricycle_steering_controller/CMakeLists.txt @@ -0,0 +1,26 @@ +cmake_minimum_required(VERSION 3.8) +project(tricycle_steering_controller) + +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/tricycle_steering_controller/LICENSE b/tricycle_steering_controller/LICENSE new file mode 100644 index 0000000000..d645695673 --- /dev/null +++ b/tricycle_steering_controller/LICENSE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + 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. diff --git a/tricycle_steering_controller/package.xml b/tricycle_steering_controller/package.xml new file mode 100644 index 0000000000..3651a1085f --- /dev/null +++ b/tricycle_steering_controller/package.xml @@ -0,0 +1,18 @@ + + + + tricycle_steering_controller + 0.0.0 + Steering controller with tricycle kinematics. Rear fixed wheels are powering the vehicle and front wheel is steering. + Dr.-Ing. Denis Štogl + Apache-2.0 + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + From 5c5acb36999e82716f989876392b28744cfb492b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Tue, 3 Jan 2023 17:05:35 +0100 Subject: [PATCH 101/186] Add new package for Ackermann controller. --- ackermann_steering_controller/CMakeLists.txt | 26 +++ ackermann_steering_controller/LICENSE | 202 +++++++++++++++++++ ackermann_steering_controller/package.xml | 18 ++ ros2_controllers/package.xml | 1 + 4 files changed, 247 insertions(+) create mode 100644 ackermann_steering_controller/CMakeLists.txt create mode 100644 ackermann_steering_controller/LICENSE create mode 100644 ackermann_steering_controller/package.xml diff --git a/ackermann_steering_controller/CMakeLists.txt b/ackermann_steering_controller/CMakeLists.txt new file mode 100644 index 0000000000..3f187a7d63 --- /dev/null +++ b/ackermann_steering_controller/CMakeLists.txt @@ -0,0 +1,26 @@ +cmake_minimum_required(VERSION 3.8) +project(ackermann_steering_controller) + +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/ackermann_steering_controller/LICENSE b/ackermann_steering_controller/LICENSE new file mode 100644 index 0000000000..d645695673 --- /dev/null +++ b/ackermann_steering_controller/LICENSE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + 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. diff --git a/ackermann_steering_controller/package.xml b/ackermann_steering_controller/package.xml new file mode 100644 index 0000000000..2469696d16 --- /dev/null +++ b/ackermann_steering_controller/package.xml @@ -0,0 +1,18 @@ + + + + ackermann_steering_controller + 0.0.0 + Steering controller for Ackermann kinematics. Rear fixed wheels arepowering the wehicle and front wheels are steering it. + Dr.-Ing. Denis Štogl + Apache-2.0 + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 94e15973f1..c9b80d18f0 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -10,6 +10,7 @@ ament_cmake + ackermann_steering_controller admittance_controller bicycle_steering_controller diff_drive_controller From 0a8a2647548cedd375ea22d89664fbbe391a3d47 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Tue, 3 Jan 2023 17:08:59 +0100 Subject: [PATCH 102/186] Correct authors and descriptions. --- ackermann_steering_controller/LICENSE | 202 ---------------------- ackermann_steering_controller/package.xml | 11 +- bicycle_steering_controller/LICENSE | 202 ---------------------- bicycle_steering_controller/package.xml | 11 +- tricycle_steering_controller/LICENSE | 202 ---------------------- tricycle_steering_controller/package.xml | 11 +- 6 files changed, 25 insertions(+), 614 deletions(-) delete mode 100644 ackermann_steering_controller/LICENSE delete mode 100644 bicycle_steering_controller/LICENSE delete mode 100644 tricycle_steering_controller/LICENSE diff --git a/ackermann_steering_controller/LICENSE b/ackermann_steering_controller/LICENSE deleted file mode 100644 index d645695673..0000000000 --- a/ackermann_steering_controller/LICENSE +++ /dev/null @@ -1,202 +0,0 @@ - - Apache License - Version 2.0, January 2004 - http://www.apache.org/licenses/ - - TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION - - 1. Definitions. - - "License" shall mean the terms and conditions for use, reproduction, - and distribution as defined by Sections 1 through 9 of this document. - - "Licensor" shall mean the copyright owner or entity authorized by - the copyright owner that is granting the License. - - "Legal Entity" shall mean the union of the acting entity and all - other entities that control, are controlled by, or are under common - control with that entity. For the purposes of this definition, - "control" means (i) the power, direct or indirect, to cause the - direction or management of such entity, whether by contract or - otherwise, or (ii) ownership of fifty percent (50%) or more of the - outstanding shares, or (iii) beneficial ownership of such entity. - - "You" (or "Your") shall mean an individual or Legal Entity - exercising permissions granted by this License. - - "Source" form shall mean the preferred form for making modifications, - including but not limited to software source code, documentation - source, and configuration files. - - "Object" form shall mean any form resulting from mechanical - transformation or translation of a Source form, including but - not limited to compiled object code, generated documentation, - and conversions to other media types. - - "Work" shall mean the work of authorship, whether in Source or - Object form, made available under the License, as indicated by a - copyright notice that is included in or attached to the work - (an example is provided in the Appendix below). - - "Derivative Works" shall mean any work, whether in Source or Object - form, that is based on (or derived from) the Work and for which the - editorial revisions, annotations, elaborations, or other modifications - represent, as a whole, an original work of authorship. For the purposes - of this License, Derivative Works shall not include works that remain - separable from, or merely link (or bind by name) to the interfaces of, - the Work and Derivative Works thereof. - - "Contribution" shall mean any work of authorship, including - the original version of the Work and any modifications or additions - to that Work or Derivative Works thereof, that is intentionally - submitted to Licensor for inclusion in the Work by the copyright owner - or by an individual or Legal Entity authorized to submit on behalf of - the copyright owner. For the purposes of this definition, "submitted" - means any form of electronic, verbal, or written communication sent - to the Licensor or its representatives, including but not limited to - communication on electronic mailing lists, source code control systems, - and issue tracking systems that are managed by, or on behalf of, the - Licensor for the purpose of discussing and improving the Work, but - excluding communication that is conspicuously marked or otherwise - designated in writing by the copyright owner as "Not a Contribution." - - "Contributor" shall mean Licensor and any individual or Legal Entity - on behalf of whom a Contribution has been received by Licensor and - subsequently incorporated within the Work. - - 2. Grant of Copyright License. Subject to the terms and conditions of - this License, each Contributor hereby grants to You a perpetual, - worldwide, non-exclusive, no-charge, royalty-free, irrevocable - copyright license to reproduce, prepare Derivative Works of, - publicly display, publicly perform, sublicense, and distribute the - Work and such Derivative Works in Source or Object form. - - 3. Grant of Patent License. Subject to the terms and conditions of - this License, each Contributor hereby grants to You a perpetual, - worldwide, non-exclusive, no-charge, royalty-free, irrevocable - (except as stated in this section) patent license to make, have made, - use, offer to sell, sell, import, and otherwise transfer the Work, - where such license applies only to those patent claims licensable - by such Contributor that are necessarily infringed by their - Contribution(s) alone or by combination of their Contribution(s) - with the Work to which such Contribution(s) was submitted. If You - institute patent litigation against any entity (including a - cross-claim or counterclaim in a lawsuit) alleging that the Work - or a Contribution incorporated within the Work constitutes direct - or contributory patent infringement, then any patent licenses - granted to You under this License for that Work shall terminate - as of the date such litigation is filed. - - 4. Redistribution. You may reproduce and distribute copies of the - Work or Derivative Works thereof in any medium, with or without - modifications, and in Source or Object form, provided that You - meet the following conditions: - - (a) You must give any other recipients of the Work or - Derivative Works a copy of this License; and - - (b) You must cause any modified files to carry prominent notices - stating that You changed the files; and - - (c) You must retain, in the Source form of any Derivative Works - that You distribute, all copyright, patent, trademark, and - attribution notices from the Source form of the Work, - excluding those notices that do not pertain to any part of - the Derivative Works; and - - (d) If the Work includes a "NOTICE" text file as part of its - distribution, then any Derivative Works that You distribute must - include a readable copy of the attribution notices contained - within such NOTICE file, excluding those notices that do not - pertain to any part of the Derivative Works, in at least one - of the following places: within a NOTICE text file distributed - as part of the Derivative Works; within the Source form or - documentation, if provided along with the Derivative Works; or, - within a display generated by the Derivative Works, if and - wherever such third-party notices normally appear. The contents - of the NOTICE file are for informational purposes only and - do not modify the License. You may add Your own attribution - notices within Derivative Works that You distribute, alongside - or as an addendum to the NOTICE text from the Work, provided - that such additional attribution notices cannot be construed - as modifying the License. - - You may add Your own copyright statement to Your modifications and - may provide additional or different license terms and conditions - for use, reproduction, or distribution of Your modifications, or - for any such Derivative Works as a whole, provided Your use, - reproduction, and distribution of the Work otherwise complies with - the conditions stated in this License. - - 5. Submission of Contributions. Unless You explicitly state otherwise, - any Contribution intentionally submitted for inclusion in the Work - by You to the Licensor shall be under the terms and conditions of - this License, without any additional terms or conditions. - Notwithstanding the above, nothing herein shall supersede or modify - the terms of any separate license agreement you may have executed - with Licensor regarding such Contributions. - - 6. Trademarks. This License does not grant permission to use the trade - names, trademarks, service marks, or product names of the Licensor, - except as required for reasonable and customary use in describing the - origin of the Work and reproducing the content of the NOTICE file. - - 7. Disclaimer of Warranty. Unless required by applicable law or - agreed to in writing, Licensor provides the Work (and each - Contributor provides its Contributions) on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or - implied, including, without limitation, any warranties or conditions - of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A - PARTICULAR PURPOSE. You are solely responsible for determining the - appropriateness of using or redistributing the Work and assume any - risks associated with Your exercise of permissions under this License. - - 8. Limitation of Liability. In no event and under no legal theory, - whether in tort (including negligence), contract, or otherwise, - unless required by applicable law (such as deliberate and grossly - negligent acts) or agreed to in writing, shall any Contributor be - liable to You for damages, including any direct, indirect, special, - incidental, or consequential damages of any character arising as a - result of this License or out of the use or inability to use the - Work (including but not limited to damages for loss of goodwill, - work stoppage, computer failure or malfunction, or any and all - other commercial damages or losses), even if such Contributor - has been advised of the possibility of such damages. - - 9. Accepting Warranty or Additional Liability. While redistributing - the Work or Derivative Works thereof, You may choose to offer, - and charge a fee for, acceptance of support, warranty, indemnity, - or other liability obligations and/or rights consistent with this - License. However, in accepting such obligations, You may act only - on Your own behalf and on Your sole responsibility, not on behalf - of any other Contributor, and only if You agree to indemnify, - defend, and hold each Contributor harmless for any liability - incurred by, or claims asserted against, such Contributor by reason - of your accepting any such warranty or additional liability. - - END OF TERMS AND CONDITIONS - - APPENDIX: How to apply the Apache License to your work. - - To apply the Apache License to your work, attach the following - boilerplate notice, with the fields enclosed by brackets "[]" - replaced with your own identifying information. (Don't include - the brackets!) The text should be enclosed in the appropriate - comment syntax for the file format. We also recommend that a - file or class name and description of purpose be included on the - same "printed page" as the copyright notice for easier - identification within third-party archives. - - Copyright [yyyy] [name of copyright owner] - - 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. diff --git a/ackermann_steering_controller/package.xml b/ackermann_steering_controller/package.xml index 2469696d16..965fa2fb4b 100644 --- a/ackermann_steering_controller/package.xml +++ b/ackermann_steering_controller/package.xml @@ -3,9 +3,14 @@ ackermann_steering_controller 0.0.0 - Steering controller for Ackermann kinematics. Rear fixed wheels arepowering the wehicle and front wheels are steering it. - Dr.-Ing. Denis Štogl - Apache-2.0 + Steering controller for Ackermann kinematics. Rear fixed wheels are powering the vehicle and front wheels are steering it. + Apache License 2.0 + Bence Magyar + Dr.-Ing. Denis Štogl + dr. sc. Tomislav Petkovic + + Dr.-Ing. Denis Štogl + dr. sc. Tomislav Petkovic ament_cmake diff --git a/bicycle_steering_controller/LICENSE b/bicycle_steering_controller/LICENSE deleted file mode 100644 index d645695673..0000000000 --- a/bicycle_steering_controller/LICENSE +++ /dev/null @@ -1,202 +0,0 @@ - - Apache License - Version 2.0, January 2004 - http://www.apache.org/licenses/ - - TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION - - 1. Definitions. - - "License" shall mean the terms and conditions for use, reproduction, - and distribution as defined by Sections 1 through 9 of this document. - - "Licensor" shall mean the copyright owner or entity authorized by - the copyright owner that is granting the License. - - "Legal Entity" shall mean the union of the acting entity and all - other entities that control, are controlled by, or are under common - control with that entity. For the purposes of this definition, - "control" means (i) the power, direct or indirect, to cause the - direction or management of such entity, whether by contract or - otherwise, or (ii) ownership of fifty percent (50%) or more of the - outstanding shares, or (iii) beneficial ownership of such entity. - - "You" (or "Your") shall mean an individual or Legal Entity - exercising permissions granted by this License. - - "Source" form shall mean the preferred form for making modifications, - including but not limited to software source code, documentation - source, and configuration files. - - "Object" form shall mean any form resulting from mechanical - transformation or translation of a Source form, including but - not limited to compiled object code, generated documentation, - and conversions to other media types. - - "Work" shall mean the work of authorship, whether in Source or - Object form, made available under the License, as indicated by a - copyright notice that is included in or attached to the work - (an example is provided in the Appendix below). - - "Derivative Works" shall mean any work, whether in Source or Object - form, that is based on (or derived from) the Work and for which the - editorial revisions, annotations, elaborations, or other modifications - represent, as a whole, an original work of authorship. For the purposes - of this License, Derivative Works shall not include works that remain - separable from, or merely link (or bind by name) to the interfaces of, - the Work and Derivative Works thereof. - - "Contribution" shall mean any work of authorship, including - the original version of the Work and any modifications or additions - to that Work or Derivative Works thereof, that is intentionally - submitted to Licensor for inclusion in the Work by the copyright owner - or by an individual or Legal Entity authorized to submit on behalf of - the copyright owner. For the purposes of this definition, "submitted" - means any form of electronic, verbal, or written communication sent - to the Licensor or its representatives, including but not limited to - communication on electronic mailing lists, source code control systems, - and issue tracking systems that are managed by, or on behalf of, the - Licensor for the purpose of discussing and improving the Work, but - excluding communication that is conspicuously marked or otherwise - designated in writing by the copyright owner as "Not a Contribution." - - "Contributor" shall mean Licensor and any individual or Legal Entity - on behalf of whom a Contribution has been received by Licensor and - subsequently incorporated within the Work. - - 2. Grant of Copyright License. Subject to the terms and conditions of - this License, each Contributor hereby grants to You a perpetual, - worldwide, non-exclusive, no-charge, royalty-free, irrevocable - copyright license to reproduce, prepare Derivative Works of, - publicly display, publicly perform, sublicense, and distribute the - Work and such Derivative Works in Source or Object form. - - 3. Grant of Patent License. Subject to the terms and conditions of - this License, each Contributor hereby grants to You a perpetual, - worldwide, non-exclusive, no-charge, royalty-free, irrevocable - (except as stated in this section) patent license to make, have made, - use, offer to sell, sell, import, and otherwise transfer the Work, - where such license applies only to those patent claims licensable - by such Contributor that are necessarily infringed by their - Contribution(s) alone or by combination of their Contribution(s) - with the Work to which such Contribution(s) was submitted. If You - institute patent litigation against any entity (including a - cross-claim or counterclaim in a lawsuit) alleging that the Work - or a Contribution incorporated within the Work constitutes direct - or contributory patent infringement, then any patent licenses - granted to You under this License for that Work shall terminate - as of the date such litigation is filed. - - 4. Redistribution. You may reproduce and distribute copies of the - Work or Derivative Works thereof in any medium, with or without - modifications, and in Source or Object form, provided that You - meet the following conditions: - - (a) You must give any other recipients of the Work or - Derivative Works a copy of this License; and - - (b) You must cause any modified files to carry prominent notices - stating that You changed the files; and - - (c) You must retain, in the Source form of any Derivative Works - that You distribute, all copyright, patent, trademark, and - attribution notices from the Source form of the Work, - excluding those notices that do not pertain to any part of - the Derivative Works; and - - (d) If the Work includes a "NOTICE" text file as part of its - distribution, then any Derivative Works that You distribute must - include a readable copy of the attribution notices contained - within such NOTICE file, excluding those notices that do not - pertain to any part of the Derivative Works, in at least one - of the following places: within a NOTICE text file distributed - as part of the Derivative Works; within the Source form or - documentation, if provided along with the Derivative Works; or, - within a display generated by the Derivative Works, if and - wherever such third-party notices normally appear. The contents - of the NOTICE file are for informational purposes only and - do not modify the License. You may add Your own attribution - notices within Derivative Works that You distribute, alongside - or as an addendum to the NOTICE text from the Work, provided - that such additional attribution notices cannot be construed - as modifying the License. - - You may add Your own copyright statement to Your modifications and - may provide additional or different license terms and conditions - for use, reproduction, or distribution of Your modifications, or - for any such Derivative Works as a whole, provided Your use, - reproduction, and distribution of the Work otherwise complies with - the conditions stated in this License. - - 5. Submission of Contributions. Unless You explicitly state otherwise, - any Contribution intentionally submitted for inclusion in the Work - by You to the Licensor shall be under the terms and conditions of - this License, without any additional terms or conditions. - Notwithstanding the above, nothing herein shall supersede or modify - the terms of any separate license agreement you may have executed - with Licensor regarding such Contributions. - - 6. Trademarks. This License does not grant permission to use the trade - names, trademarks, service marks, or product names of the Licensor, - except as required for reasonable and customary use in describing the - origin of the Work and reproducing the content of the NOTICE file. - - 7. Disclaimer of Warranty. Unless required by applicable law or - agreed to in writing, Licensor provides the Work (and each - Contributor provides its Contributions) on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or - implied, including, without limitation, any warranties or conditions - of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A - PARTICULAR PURPOSE. You are solely responsible for determining the - appropriateness of using or redistributing the Work and assume any - risks associated with Your exercise of permissions under this License. - - 8. Limitation of Liability. In no event and under no legal theory, - whether in tort (including negligence), contract, or otherwise, - unless required by applicable law (such as deliberate and grossly - negligent acts) or agreed to in writing, shall any Contributor be - liable to You for damages, including any direct, indirect, special, - incidental, or consequential damages of any character arising as a - result of this License or out of the use or inability to use the - Work (including but not limited to damages for loss of goodwill, - work stoppage, computer failure or malfunction, or any and all - other commercial damages or losses), even if such Contributor - has been advised of the possibility of such damages. - - 9. Accepting Warranty or Additional Liability. While redistributing - the Work or Derivative Works thereof, You may choose to offer, - and charge a fee for, acceptance of support, warranty, indemnity, - or other liability obligations and/or rights consistent with this - License. However, in accepting such obligations, You may act only - on Your own behalf and on Your sole responsibility, not on behalf - of any other Contributor, and only if You agree to indemnify, - defend, and hold each Contributor harmless for any liability - incurred by, or claims asserted against, such Contributor by reason - of your accepting any such warranty or additional liability. - - END OF TERMS AND CONDITIONS - - APPENDIX: How to apply the Apache License to your work. - - To apply the Apache License to your work, attach the following - boilerplate notice, with the fields enclosed by brackets "[]" - replaced with your own identifying information. (Don't include - the brackets!) The text should be enclosed in the appropriate - comment syntax for the file format. We also recommend that a - file or class name and description of purpose be included on the - same "printed page" as the copyright notice for easier - identification within third-party archives. - - Copyright [yyyy] [name of copyright owner] - - 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. diff --git a/bicycle_steering_controller/package.xml b/bicycle_steering_controller/package.xml index 8623e04c43..f5f030d57b 100644 --- a/bicycle_steering_controller/package.xml +++ b/bicycle_steering_controller/package.xml @@ -3,9 +3,14 @@ bicycle_steering_controller 0.0.0 - Steering controller with bicycle kinematics. Rear fixed wheel is powering the wehicle and front wheel is steering. - Dr.-Ing. Denis Štogl - Apache-2.0 + Steering controller with bicycle kinematics. Rear fixed wheel is powering the vehicle and front wheel is steering. + Apache License 2.0 + Bence Magyar + Dr.-Ing. Denis Štogl + dr. sc. Tomislav Petkovic + + Dr.-Ing. Denis Štogl + dr. sc. Tomislav Petkovic ament_cmake diff --git a/tricycle_steering_controller/LICENSE b/tricycle_steering_controller/LICENSE deleted file mode 100644 index d645695673..0000000000 --- a/tricycle_steering_controller/LICENSE +++ /dev/null @@ -1,202 +0,0 @@ - - Apache License - Version 2.0, January 2004 - http://www.apache.org/licenses/ - - TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION - - 1. Definitions. - - "License" shall mean the terms and conditions for use, reproduction, - and distribution as defined by Sections 1 through 9 of this document. - - "Licensor" shall mean the copyright owner or entity authorized by - the copyright owner that is granting the License. - - "Legal Entity" shall mean the union of the acting entity and all - other entities that control, are controlled by, or are under common - control with that entity. For the purposes of this definition, - "control" means (i) the power, direct or indirect, to cause the - direction or management of such entity, whether by contract or - otherwise, or (ii) ownership of fifty percent (50%) or more of the - outstanding shares, or (iii) beneficial ownership of such entity. - - "You" (or "Your") shall mean an individual or Legal Entity - exercising permissions granted by this License. - - "Source" form shall mean the preferred form for making modifications, - including but not limited to software source code, documentation - source, and configuration files. - - "Object" form shall mean any form resulting from mechanical - transformation or translation of a Source form, including but - not limited to compiled object code, generated documentation, - and conversions to other media types. - - "Work" shall mean the work of authorship, whether in Source or - Object form, made available under the License, as indicated by a - copyright notice that is included in or attached to the work - (an example is provided in the Appendix below). - - "Derivative Works" shall mean any work, whether in Source or Object - form, that is based on (or derived from) the Work and for which the - editorial revisions, annotations, elaborations, or other modifications - represent, as a whole, an original work of authorship. For the purposes - of this License, Derivative Works shall not include works that remain - separable from, or merely link (or bind by name) to the interfaces of, - the Work and Derivative Works thereof. - - "Contribution" shall mean any work of authorship, including - the original version of the Work and any modifications or additions - to that Work or Derivative Works thereof, that is intentionally - submitted to Licensor for inclusion in the Work by the copyright owner - or by an individual or Legal Entity authorized to submit on behalf of - the copyright owner. For the purposes of this definition, "submitted" - means any form of electronic, verbal, or written communication sent - to the Licensor or its representatives, including but not limited to - communication on electronic mailing lists, source code control systems, - and issue tracking systems that are managed by, or on behalf of, the - Licensor for the purpose of discussing and improving the Work, but - excluding communication that is conspicuously marked or otherwise - designated in writing by the copyright owner as "Not a Contribution." - - "Contributor" shall mean Licensor and any individual or Legal Entity - on behalf of whom a Contribution has been received by Licensor and - subsequently incorporated within the Work. - - 2. Grant of Copyright License. Subject to the terms and conditions of - this License, each Contributor hereby grants to You a perpetual, - worldwide, non-exclusive, no-charge, royalty-free, irrevocable - copyright license to reproduce, prepare Derivative Works of, - publicly display, publicly perform, sublicense, and distribute the - Work and such Derivative Works in Source or Object form. - - 3. Grant of Patent License. Subject to the terms and conditions of - this License, each Contributor hereby grants to You a perpetual, - worldwide, non-exclusive, no-charge, royalty-free, irrevocable - (except as stated in this section) patent license to make, have made, - use, offer to sell, sell, import, and otherwise transfer the Work, - where such license applies only to those patent claims licensable - by such Contributor that are necessarily infringed by their - Contribution(s) alone or by combination of their Contribution(s) - with the Work to which such Contribution(s) was submitted. If You - institute patent litigation against any entity (including a - cross-claim or counterclaim in a lawsuit) alleging that the Work - or a Contribution incorporated within the Work constitutes direct - or contributory patent infringement, then any patent licenses - granted to You under this License for that Work shall terminate - as of the date such litigation is filed. - - 4. Redistribution. You may reproduce and distribute copies of the - Work or Derivative Works thereof in any medium, with or without - modifications, and in Source or Object form, provided that You - meet the following conditions: - - (a) You must give any other recipients of the Work or - Derivative Works a copy of this License; and - - (b) You must cause any modified files to carry prominent notices - stating that You changed the files; and - - (c) You must retain, in the Source form of any Derivative Works - that You distribute, all copyright, patent, trademark, and - attribution notices from the Source form of the Work, - excluding those notices that do not pertain to any part of - the Derivative Works; and - - (d) If the Work includes a "NOTICE" text file as part of its - distribution, then any Derivative Works that You distribute must - include a readable copy of the attribution notices contained - within such NOTICE file, excluding those notices that do not - pertain to any part of the Derivative Works, in at least one - of the following places: within a NOTICE text file distributed - as part of the Derivative Works; within the Source form or - documentation, if provided along with the Derivative Works; or, - within a display generated by the Derivative Works, if and - wherever such third-party notices normally appear. The contents - of the NOTICE file are for informational purposes only and - do not modify the License. You may add Your own attribution - notices within Derivative Works that You distribute, alongside - or as an addendum to the NOTICE text from the Work, provided - that such additional attribution notices cannot be construed - as modifying the License. - - You may add Your own copyright statement to Your modifications and - may provide additional or different license terms and conditions - for use, reproduction, or distribution of Your modifications, or - for any such Derivative Works as a whole, provided Your use, - reproduction, and distribution of the Work otherwise complies with - the conditions stated in this License. - - 5. Submission of Contributions. Unless You explicitly state otherwise, - any Contribution intentionally submitted for inclusion in the Work - by You to the Licensor shall be under the terms and conditions of - this License, without any additional terms or conditions. - Notwithstanding the above, nothing herein shall supersede or modify - the terms of any separate license agreement you may have executed - with Licensor regarding such Contributions. - - 6. Trademarks. This License does not grant permission to use the trade - names, trademarks, service marks, or product names of the Licensor, - except as required for reasonable and customary use in describing the - origin of the Work and reproducing the content of the NOTICE file. - - 7. Disclaimer of Warranty. Unless required by applicable law or - agreed to in writing, Licensor provides the Work (and each - Contributor provides its Contributions) on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or - implied, including, without limitation, any warranties or conditions - of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A - PARTICULAR PURPOSE. You are solely responsible for determining the - appropriateness of using or redistributing the Work and assume any - risks associated with Your exercise of permissions under this License. - - 8. Limitation of Liability. In no event and under no legal theory, - whether in tort (including negligence), contract, or otherwise, - unless required by applicable law (such as deliberate and grossly - negligent acts) or agreed to in writing, shall any Contributor be - liable to You for damages, including any direct, indirect, special, - incidental, or consequential damages of any character arising as a - result of this License or out of the use or inability to use the - Work (including but not limited to damages for loss of goodwill, - work stoppage, computer failure or malfunction, or any and all - other commercial damages or losses), even if such Contributor - has been advised of the possibility of such damages. - - 9. Accepting Warranty or Additional Liability. While redistributing - the Work or Derivative Works thereof, You may choose to offer, - and charge a fee for, acceptance of support, warranty, indemnity, - or other liability obligations and/or rights consistent with this - License. However, in accepting such obligations, You may act only - on Your own behalf and on Your sole responsibility, not on behalf - of any other Contributor, and only if You agree to indemnify, - defend, and hold each Contributor harmless for any liability - incurred by, or claims asserted against, such Contributor by reason - of your accepting any such warranty or additional liability. - - END OF TERMS AND CONDITIONS - - APPENDIX: How to apply the Apache License to your work. - - To apply the Apache License to your work, attach the following - boilerplate notice, with the fields enclosed by brackets "[]" - replaced with your own identifying information. (Don't include - the brackets!) The text should be enclosed in the appropriate - comment syntax for the file format. We also recommend that a - file or class name and description of purpose be included on the - same "printed page" as the copyright notice for easier - identification within third-party archives. - - Copyright [yyyy] [name of copyright owner] - - 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. diff --git a/tricycle_steering_controller/package.xml b/tricycle_steering_controller/package.xml index 3651a1085f..dada641359 100644 --- a/tricycle_steering_controller/package.xml +++ b/tricycle_steering_controller/package.xml @@ -4,8 +4,15 @@ tricycle_steering_controller 0.0.0 Steering controller with tricycle kinematics. Rear fixed wheels are powering the vehicle and front wheel is steering. - Dr.-Ing. Denis Štogl - Apache-2.0 + Apache License 2.0 + Bence Magyar + Dr.-Ing. Denis Štogl + dr. sc. Tomislav Petkovic + Tony Najjar + + Dr.-Ing. Denis Štogl + dr. sc. Tomislav Petkovic + Tony Najjar ament_cmake From cb3e8af663d4979613e060366b3a6896907d467e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Tue, 3 Jan 2023 18:01:38 +0100 Subject: [PATCH 103/186] Rename to steering_controllers_library. --- .../CMakeLists.txt | 44 +++++++--------- .../config/ackermann_steering_controller.yaml | 0 .../config/bicycle_steering_controller.yaml | 0 .../config/steering_controllers_library.yaml | 2 +- .../config/tricycle_steering_controller.yaml | 0 .../doc/userdoc.rst | 0 .../ackermann_steering_controller.hpp | 0 .../bicycle_steering_controller.hpp | 0 .../steering_controllers_library.hpp | 30 +++++------ .../steering_odometry.hpp | 0 .../tricycle_steering_controller.hpp | 0 .../visibility_control.h | 6 +-- .../package.xml | 2 +- .../src/ackermann_steering_controller.cpp | 0 .../src/bicycle_steering_controller.cpp | 0 .../src/steering_controllers_library.cpp | 51 ++++++++++--------- .../src/steering_odometry.cpp | 2 +- .../src/tricycle_steering_controller.cpp | 0 .../steering_controllers.xml | 0 19 files changed, 68 insertions(+), 69 deletions(-) rename {steering_controllers => steering_controllers_library}/CMakeLists.txt (60%) rename {steering_controllers => steering_controllers_library}/config/ackermann_steering_controller.yaml (100%) rename {steering_controllers => steering_controllers_library}/config/bicycle_steering_controller.yaml (100%) rename steering_controllers/config/steering_controllers.yaml => steering_controllers_library/config/steering_controllers_library.yaml (98%) rename {steering_controllers => steering_controllers_library}/config/tricycle_steering_controller.yaml (100%) rename {steering_controllers => steering_controllers_library}/doc/userdoc.rst (100%) rename {steering_controllers/include/steering_controllers => steering_controllers_library/include/steering_controllers_library}/ackermann_steering_controller.hpp (100%) rename {steering_controllers/include/steering_controllers => steering_controllers_library/include/steering_controllers_library}/bicycle_steering_controller.hpp (100%) rename steering_controllers/include/steering_controllers/steering_controllers.hpp => steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp (85%) rename {steering_controllers/include/steering_controllers => steering_controllers_library/include/steering_controllers_library}/steering_odometry.hpp (100%) rename {steering_controllers/include/steering_controllers => steering_controllers_library/include/steering_controllers_library}/tricycle_steering_controller.hpp (100%) rename {steering_controllers/include/steering_controllers => steering_controllers_library/include/steering_controllers_library}/visibility_control.h (92%) rename {steering_controllers => steering_controllers_library}/package.xml (97%) rename {steering_controllers => steering_controllers_library}/src/ackermann_steering_controller.cpp (100%) rename {steering_controllers => steering_controllers_library}/src/bicycle_steering_controller.cpp (100%) rename steering_controllers/src/steering_controllers.cpp => steering_controllers_library/src/steering_controllers_library.cpp (91%) rename {steering_controllers => steering_controllers_library}/src/steering_odometry.cpp (99%) rename {steering_controllers => steering_controllers_library}/src/tricycle_steering_controller.cpp (100%) rename {steering_controllers => steering_controllers_library}/steering_controllers.xml (100%) diff --git a/steering_controllers/CMakeLists.txt b/steering_controllers_library/CMakeLists.txt similarity index 60% rename from steering_controllers/CMakeLists.txt rename to steering_controllers_library/CMakeLists.txt index 9689ad2e27..8df39f7092 100644 --- a/steering_controllers/CMakeLists.txt +++ b/steering_controllers_library/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.8) -project(steering_controllers) +project(steering_controllers_library) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) @@ -33,41 +33,34 @@ endforeach() add_library( ${PROJECT_NAME} SHARED - src/steering_controllers.cpp + src/steering_controllers_library.cpp src/steering_odometry.cpp - src/ackermann_steering_controller.cpp - src/bicycle_steering_controller.cpp - src/tricycle_steering_controller.cpp + #src/ackermann_steering_controller.cpp + #src/bicycle_steering_controller.cpp + #src/tricycle_steering_controller.cpp ) -generate_parameter_library(steering_controllers_parameters - config/steering_controllers.yaml -) -generate_parameter_library(bicycle_controller_parameters - config/bicycle_steering_controller.yaml -) -generate_parameter_library(tricycle_controller_parameters - config/tricycle_steering_controller.yaml -) -generate_parameter_library(ackermann_controller_parameters - config/ackermann_steering_controller.yaml +generate_parameter_library(steering_controllers_library_parameters + config/steering_controllers_library.yaml ) +#generate_parameter_library(bicycle_controller_parameters + #config/bicycle_steering_controller.yaml +#) +#generate_parameter_library(tricycle_controller_parameters + #config/tricycle_steering_controller.yaml +#) +#generate_parameter_library(ackermann_controller_parameters + #config/ackermann_steering_controller.yaml +#) -# target_include_directories(${PROJECT_NAME} PRIVATE include) target_include_directories(${PROJECT_NAME} PUBLIC "$" "$") target_link_libraries(${PROJECT_NAME} - steering_controllers_parameters - bicycle_controller_parameters - tricycle_controller_parameters - ackermann_controller_parameters) + steering_controllers_library_parameters) ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS}) target_compile_definitions(${PROJECT_NAME} PRIVATE "STEERING_CONTROLLERS_BUILDING_DLL") -pluginlib_export_plugin_description_file( - controller_interface steering_controllers.xml) - install( TARGETS ${PROJECT_NAME} @@ -78,7 +71,8 @@ install( install( DIRECTORY include/ - DESTINATION include/${PROJECT_NAME} + #DESTINATION include/${PROJECT_NAME} # TODO: it should be this, but then it doesn't compile (?) + DESTINATION include ) if(BUILD_TESTING) diff --git a/steering_controllers/config/ackermann_steering_controller.yaml b/steering_controllers_library/config/ackermann_steering_controller.yaml similarity index 100% rename from steering_controllers/config/ackermann_steering_controller.yaml rename to steering_controllers_library/config/ackermann_steering_controller.yaml diff --git a/steering_controllers/config/bicycle_steering_controller.yaml b/steering_controllers_library/config/bicycle_steering_controller.yaml similarity index 100% rename from steering_controllers/config/bicycle_steering_controller.yaml rename to steering_controllers_library/config/bicycle_steering_controller.yaml diff --git a/steering_controllers/config/steering_controllers.yaml b/steering_controllers_library/config/steering_controllers_library.yaml similarity index 98% rename from steering_controllers/config/steering_controllers.yaml rename to steering_controllers_library/config/steering_controllers_library.yaml index d657bc08e5..1dd76a97ab 100644 --- a/steering_controllers/config/steering_controllers.yaml +++ b/steering_controllers_library/config/steering_controllers_library.yaml @@ -1,4 +1,4 @@ -steering_controllers: +steering_controllers_library: reference_timeout: { type: double, default_value: 1, diff --git a/steering_controllers/config/tricycle_steering_controller.yaml b/steering_controllers_library/config/tricycle_steering_controller.yaml similarity index 100% rename from steering_controllers/config/tricycle_steering_controller.yaml rename to steering_controllers_library/config/tricycle_steering_controller.yaml diff --git a/steering_controllers/doc/userdoc.rst b/steering_controllers_library/doc/userdoc.rst similarity index 100% rename from steering_controllers/doc/userdoc.rst rename to steering_controllers_library/doc/userdoc.rst diff --git a/steering_controllers/include/steering_controllers/ackermann_steering_controller.hpp b/steering_controllers_library/include/steering_controllers_library/ackermann_steering_controller.hpp similarity index 100% rename from steering_controllers/include/steering_controllers/ackermann_steering_controller.hpp rename to steering_controllers_library/include/steering_controllers_library/ackermann_steering_controller.hpp diff --git a/steering_controllers/include/steering_controllers/bicycle_steering_controller.hpp b/steering_controllers_library/include/steering_controllers_library/bicycle_steering_controller.hpp similarity index 100% rename from steering_controllers/include/steering_controllers/bicycle_steering_controller.hpp rename to steering_controllers_library/include/steering_controllers_library/bicycle_steering_controller.hpp diff --git a/steering_controllers/include/steering_controllers/steering_controllers.hpp b/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp similarity index 85% rename from steering_controllers/include/steering_controllers/steering_controllers.hpp rename to steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp index 1d05f1c815..040a9abcbd 100644 --- a/steering_controllers/include/steering_controllers/steering_controllers.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef STEERING_CONTROLLERS__STEERING_CONTROLLERS_HPP_ -#define STEERING_CONTROLLERS__STEERING_CONTROLLERS_HPP_ +#ifndef STEERING_CONTROLLERS_LIBRARY__STEERING_CONTROLLERS_LIBRARY_HPP_ +#define STEERING_CONTROLLERS_LIBRARY__STEERING_CONTROLLERS_LIBRARY_HPP_ #include #include @@ -30,9 +30,9 @@ #include "realtime_tools/realtime_buffer.h" #include "realtime_tools/realtime_publisher.h" #include "std_srvs/srv/set_bool.hpp" -#include "steering_controllers/steering_odometry.hpp" -#include "steering_controllers/visibility_control.h" -#include "steering_controllers_parameters.hpp" +#include "steering_controllers_library/steering_odometry.hpp" +#include "steering_controllers_library/visibility_control.h" +#include "steering_controllers_library_parameters.hpp" // TODO(anyone): Replace with controller specific messages #include "ackermann_msgs/msg/ackermann_drive_stamped.hpp" @@ -42,12 +42,12 @@ #include "nav_msgs/msg/odometry.hpp" #include "tf2_msgs/msg/tf_message.hpp" -namespace steering_controllers +namespace steering_controllers_library { -class SteeringControllers : public controller_interface::ChainableControllerInterface +class SteeringControllersLibrary : public controller_interface::ChainableControllerInterface { public: - STEERING_CONTROLLERS__VISIBILITY_PUBLIC SteeringControllers(); + STEERING_CONTROLLERS__VISIBILITY_PUBLIC SteeringControllersLibrary(); virtual STEERING_CONTROLLERS__VISIBILITY_PUBLIC void initialize_implementation_parameter_listener() = 0; @@ -87,8 +87,11 @@ class SteeringControllers : public controller_interface::ChainableControllerInte using ControllerStateMsgTf = tf2_msgs::msg::TFMessage; protected: - std::shared_ptr param_listener_; - steering_controllers::Params params_; + controller_interface::CallbackReturn set_interface_numbers( + size_t nr_state_itfs, size_t nr_cmd_itfs, size_t nr_ref_itfs); + + std::shared_ptr param_listener_; + steering_controllers_library::Params params_; // Command subscribers and Controller State publisher rclcpp::Subscription::SharedPtr ref_subscriber_twist_ = nullptr; @@ -128,9 +131,6 @@ class SteeringControllers : public controller_interface::ChainableControllerInte // name constants for reference interfaces size_t nr_ref_itfs_; - STEERING_CONTROLLERS__VISIBILITY_PROTECTED controller_interface::CallbackReturn - set_interface_numbers(size_t nr_state_itfs, size_t nr_cmd_itfs, size_t nr_ref_itfs); - // store last velocity double last_linear_velocity_ = 0.0; double last_angular_velocity_ = 0.0; @@ -142,6 +142,6 @@ class SteeringControllers : public controller_interface::ChainableControllerInte void reference_callback_unstamped(const std::shared_ptr msg); }; -} // namespace steering_controllers +} // namespace steering_controllers_library -#endif // STEERING_CONTROLLERS__STEERING_CONTROLLERS_HPP_ +#endif // STEERING_CONTROLLERS_LIBRARY__STEERING_CONTROLLERS_LIBRARY_HPP_ diff --git a/steering_controllers/include/steering_controllers/steering_odometry.hpp b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp similarity index 100% rename from steering_controllers/include/steering_controllers/steering_odometry.hpp rename to steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp diff --git a/steering_controllers/include/steering_controllers/tricycle_steering_controller.hpp b/steering_controllers_library/include/steering_controllers_library/tricycle_steering_controller.hpp similarity index 100% rename from steering_controllers/include/steering_controllers/tricycle_steering_controller.hpp rename to steering_controllers_library/include/steering_controllers_library/tricycle_steering_controller.hpp diff --git a/steering_controllers/include/steering_controllers/visibility_control.h b/steering_controllers_library/include/steering_controllers_library/visibility_control.h similarity index 92% rename from steering_controllers/include/steering_controllers/visibility_control.h rename to steering_controllers_library/include/steering_controllers_library/visibility_control.h index 9ee0e317fb..3ba04fce22 100644 --- a/steering_controllers/include/steering_controllers/visibility_control.h +++ b/steering_controllers_library/include/steering_controllers_library/visibility_control.h @@ -15,8 +15,8 @@ // Authors: dr. sc. Tomislav Petkovic, Dr. Ing. Denis Štogl // -#ifndef STEERING_CONTROLLERS__VISIBILITY_CONTROL_H_ -#define STEERING_CONTROLLERS__VISIBILITY_CONTROL_H_ +#ifndef STEERING_CONTROLLERS_LIBRARY__VISIBILITY_CONTROL_H_ +#define STEERING_CONTROLLERS_LIBRARY__VISIBILITY_CONTROL_H_ // This logic was borrowed (then namespaced) from the examples on the gcc wiki: // https://gcc.gnu.org/wiki/Visibility @@ -50,4 +50,4 @@ #define STEERING_CONTROLLERS__VISIBILITY_PUBLIC_TYPE #endif -#endif // STEERING_CONTROLLERS__VISIBILITY_CONTROL_H_ +#endif // STEERING_CONTROLLERS_LIBRARY__VISIBILITY_CONTROL_H_ diff --git a/steering_controllers/package.xml b/steering_controllers_library/package.xml similarity index 97% rename from steering_controllers/package.xml rename to steering_controllers_library/package.xml index b764fdd832..2e638f8ddc 100644 --- a/steering_controllers/package.xml +++ b/steering_controllers_library/package.xml @@ -1,7 +1,7 @@ - steering_controllers + steering_controllers_library 0.0.0 Package for steering robot configurations including odometry and interfaces. Apache License 2.0 diff --git a/steering_controllers/src/ackermann_steering_controller.cpp b/steering_controllers_library/src/ackermann_steering_controller.cpp similarity index 100% rename from steering_controllers/src/ackermann_steering_controller.cpp rename to steering_controllers_library/src/ackermann_steering_controller.cpp diff --git a/steering_controllers/src/bicycle_steering_controller.cpp b/steering_controllers_library/src/bicycle_steering_controller.cpp similarity index 100% rename from steering_controllers/src/bicycle_steering_controller.cpp rename to steering_controllers_library/src/bicycle_steering_controller.cpp diff --git a/steering_controllers/src/steering_controllers.cpp b/steering_controllers_library/src/steering_controllers_library.cpp similarity index 91% rename from steering_controllers/src/steering_controllers.cpp rename to steering_controllers_library/src/steering_controllers_library.cpp index b5a76e0e4b..ee75d08353 100644 --- a/steering_controllers/src/steering_controllers.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "steering_controllers/steering_controllers.hpp" +#include "steering_controllers_library/steering_controllers_library.hpp" #include #include @@ -31,7 +31,7 @@ namespace { // utility using ControllerTwistReferenceMsg = - steering_controllers::SteeringControllers::ControllerTwistReferenceMsg; + steering_controllers_library::SteeringControllersLibrary::ControllerTwistReferenceMsg; // called from RT control loop void reset_controller_reference_msg( @@ -49,15 +49,18 @@ void reset_controller_reference_msg( } // namespace -namespace steering_controllers +namespace steering_controllers_library { -SteeringControllers::SteeringControllers() : controller_interface::ChainableControllerInterface() {} +SteeringControllersLibrary::SteeringControllersLibrary() +: controller_interface::ChainableControllerInterface() +{ +} -controller_interface::CallbackReturn SteeringControllers::on_init() +controller_interface::CallbackReturn SteeringControllersLibrary::on_init() { try { - param_listener_ = std::make_shared(get_node()); + param_listener_ = std::make_shared(get_node()); initialize_implementation_parameter_listener(); } catch (const std::exception & e) @@ -69,7 +72,7 @@ controller_interface::CallbackReturn SteeringControllers::on_init() return controller_interface::CallbackReturn::SUCCESS; } -controller_interface::CallbackReturn SteeringControllers::set_interface_numbers( +controller_interface::CallbackReturn SteeringControllersLibrary::set_interface_numbers( size_t nr_state_itfs = 2, size_t nr_cmd_itfs = 2, size_t nr_ref_itfs = 2) { nr_state_itfs_ = nr_state_itfs; @@ -78,7 +81,7 @@ controller_interface::CallbackReturn SteeringControllers::set_interface_numbers( return controller_interface::CallbackReturn::SUCCESS; } -controller_interface::CallbackReturn SteeringControllers::on_configure( +controller_interface::CallbackReturn SteeringControllersLibrary::on_configure( const rclcpp_lifecycle::State & /*previous_state*/) { params_ = param_listener_->get_params(); @@ -96,13 +99,14 @@ controller_interface::CallbackReturn SteeringControllers::on_configure( { ref_subscriber_twist_ = get_node()->create_subscription( "~/reference", subscribers_qos, - std::bind(&SteeringControllers::reference_callback, this, std::placeholders::_1)); + std::bind(&SteeringControllersLibrary::reference_callback, this, std::placeholders::_1)); } else { ref_subscriber_unstamped_ = get_node()->create_subscription( "~/reference_unstamped", subscribers_qos, - std::bind(&SteeringControllers::reference_callback_unstamped, this, std::placeholders::_1)); + std::bind( + &SteeringControllersLibrary::reference_callback_unstamped, this, std::placeholders::_1)); } std::shared_ptr msg = @@ -202,7 +206,8 @@ controller_interface::CallbackReturn SteeringControllers::on_configure( return controller_interface::CallbackReturn::SUCCESS; } -void SteeringControllers::reference_callback(const std::shared_ptr msg) +void SteeringControllersLibrary::reference_callback( + const std::shared_ptr msg) { // if no timestamp provided use current time for command timestamp if (msg->header.stamp.sec == 0 && msg->header.stamp.nanosec == 0u) @@ -229,7 +234,7 @@ void SteeringControllers::reference_callback(const std::shared_ptr msg) { RCLCPP_WARN( @@ -265,8 +270,8 @@ void SteeringControllers::reference_callback_unstamped( } } -controller_interface::InterfaceConfiguration SteeringControllers::command_interface_configuration() - const +controller_interface::InterfaceConfiguration +SteeringControllersLibrary::command_interface_configuration() const { controller_interface::InterfaceConfiguration command_interfaces_config; command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; @@ -303,8 +308,8 @@ controller_interface::InterfaceConfiguration SteeringControllers::command_interf return command_interfaces_config; } -controller_interface::InterfaceConfiguration SteeringControllers::state_interface_configuration() - const +controller_interface::InterfaceConfiguration +SteeringControllersLibrary::state_interface_configuration() const { controller_interface::InterfaceConfiguration state_interfaces_config; state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; @@ -346,7 +351,7 @@ controller_interface::InterfaceConfiguration SteeringControllers::state_interfac } std::vector -SteeringControllers::on_export_reference_interfaces() +SteeringControllersLibrary::on_export_reference_interfaces() { reference_interfaces_.resize(nr_ref_itfs_, std::numeric_limits::quiet_NaN()); @@ -364,13 +369,13 @@ SteeringControllers::on_export_reference_interfaces() return reference_interfaces; } -bool SteeringControllers::on_set_chained_mode(bool chained_mode) +bool SteeringControllersLibrary::on_set_chained_mode(bool chained_mode) { // Always accept switch to/from chained mode return true || chained_mode; } -controller_interface::CallbackReturn SteeringControllers::on_activate( +controller_interface::CallbackReturn SteeringControllersLibrary::on_activate( const rclcpp_lifecycle::State & /*previous_state*/) { // Set default value in command @@ -379,7 +384,7 @@ controller_interface::CallbackReturn SteeringControllers::on_activate( return controller_interface::CallbackReturn::SUCCESS; } -controller_interface::CallbackReturn SteeringControllers::on_deactivate( +controller_interface::CallbackReturn SteeringControllersLibrary::on_deactivate( const rclcpp_lifecycle::State & /*previous_state*/) { // TODO(anyone): depending on number of interfaces, use definitions, e.g., `CMD_MY_ITFS`, @@ -391,7 +396,7 @@ controller_interface::CallbackReturn SteeringControllers::on_deactivate( return controller_interface::CallbackReturn::SUCCESS; } -controller_interface::return_type SteeringControllers::update_reference_from_subscribers() +controller_interface::return_type SteeringControllersLibrary::update_reference_from_subscribers() { auto current_ref = *(input_ref_.readFromRT()); const auto age_of_last_command = get_node()->now() - (current_ref)->header.stamp; @@ -416,7 +421,7 @@ controller_interface::return_type SteeringControllers::update_reference_from_sub return controller_interface::return_type::OK; } -controller_interface::return_type SteeringControllers::update_and_write_commands( +controller_interface::return_type SteeringControllersLibrary::update_and_write_commands( const rclcpp::Time & time, const rclcpp::Duration & period) { update_odometry(period); @@ -568,4 +573,4 @@ controller_interface::return_type SteeringControllers::update_and_write_commands return controller_interface::return_type::OK; } -} // namespace steering_controllers +} // namespace steering_controllers_library diff --git a/steering_controllers/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp similarity index 99% rename from steering_controllers/src/steering_odometry.cpp rename to steering_controllers_library/src/steering_odometry.cpp index 416f289791..0c1f6b1e0e 100644 --- a/steering_controllers/src/steering_odometry.cpp +++ b/steering_controllers_library/src/steering_odometry.cpp @@ -19,7 +19,7 @@ * Author: Dr. Ing. Denis Stogl */ -#include "steering_controllers/steering_odometry.hpp" +#include "steering_controllers_library/steering_odometry.hpp" #include #include diff --git a/steering_controllers/src/tricycle_steering_controller.cpp b/steering_controllers_library/src/tricycle_steering_controller.cpp similarity index 100% rename from steering_controllers/src/tricycle_steering_controller.cpp rename to steering_controllers_library/src/tricycle_steering_controller.cpp diff --git a/steering_controllers/steering_controllers.xml b/steering_controllers_library/steering_controllers.xml similarity index 100% rename from steering_controllers/steering_controllers.xml rename to steering_controllers_library/steering_controllers.xml From 47f6d2e25c7f9fc03843d9428b3e12a06ec7ae00 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Tue, 3 Jan 2023 18:03:07 +0100 Subject: [PATCH 104/186] Bicycle Controller moved to a separate package. --- bicycle_steering_controller/CMakeLists.txt | 106 ++++- .../bicycle_steering_controller.xml | 8 + .../bicycle_steering_controller.hpp | 48 +++ .../visibility_control.h | 53 +++ bicycle_steering_controller/package.xml | 17 +- .../src/bicycle_steering_controller.cpp | 91 ++++ .../src/bicycle_steering_controller.yaml | 24 ++ .../bicycle_steering_controller_params.yaml | 7 + ...steering_controller_preceeding_params.yaml | 9 + .../test/test_bicycle_steering_controller.cpp | 387 ++++++++++++++++++ .../test/test_bicycle_steering_controller.hpp | 280 +++++++++++++ ...bicycle_steering_controller_preceeding.cpp | 89 ++++ .../test_load_bicycle_steering_controller.cpp | 41 ++ ros2_controllers/package.xml | 1 + 14 files changed, 1147 insertions(+), 14 deletions(-) create mode 100644 bicycle_steering_controller/bicycle_steering_controller.xml create mode 100644 bicycle_steering_controller/include/bicycle_steering_controller/bicycle_steering_controller.hpp create mode 100644 bicycle_steering_controller/include/bicycle_steering_controller/visibility_control.h create mode 100644 bicycle_steering_controller/src/bicycle_steering_controller.cpp create mode 100644 bicycle_steering_controller/src/bicycle_steering_controller.yaml create mode 100644 bicycle_steering_controller/test/bicycle_steering_controller_params.yaml create mode 100644 bicycle_steering_controller/test/bicycle_steering_controller_preceeding_params.yaml create mode 100644 bicycle_steering_controller/test/test_bicycle_steering_controller.cpp create mode 100644 bicycle_steering_controller/test/test_bicycle_steering_controller.hpp create mode 100644 bicycle_steering_controller/test/test_bicycle_steering_controller_preceeding.cpp create mode 100644 bicycle_steering_controller/test/test_load_bicycle_steering_controller.cpp diff --git a/bicycle_steering_controller/CMakeLists.txt b/bicycle_steering_controller/CMakeLists.txt index 3b1c3068d3..98c5045f09 100644 --- a/bicycle_steering_controller/CMakeLists.txt +++ b/bicycle_steering_controller/CMakeLists.txt @@ -6,21 +6,103 @@ 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 + steering_controllers_library +) + 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 bicycle_steering_controller library related compile commands +generate_parameter_library(bicycle_steering_controller_parameters + src/bicycle_steering_controller.yaml +) + +add_library( + bicycle_steering_controller + SHARED + src/bicycle_steering_controller.cpp +) +target_include_directories(bicycle_steering_controller PUBLIC + "$" + "$") +target_link_libraries(bicycle_steering_controller bicycle_steering_controller_parameters) +ament_target_dependencies(bicycle_steering_controller ${THIS_PACKAGE_INCLUDE_DEPENDS}) +target_compile_definitions(bicycle_steering_controller PRIVATE "BICYCLE_STEERING_CONTROLLER_BUILDING_DLL") + +pluginlib_export_plugin_description_file( + controller_interface bicycle_steering_controller.xml) + +install( + TARGETS + bicycle_steering_controller + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib +) + +install( + DIRECTORY include/ + DESTINATION include/${PROJECT_NAME} +) if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # comment the line when a copyright and license is added to all source files - set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # comment the line when this package is in a git repo and when - # a copyright and license is added to all source files - set(ament_cmake_cpplint_FOUND TRUE) - ament_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_bicycle_steering_controller test/test_load_bicycle_steering_controller.cpp) + target_include_directories(test_load_bicycle_steering_controller PRIVATE include) + ament_target_dependencies( + test_load_bicycle_steering_controller + controller_manager + hardware_interface + ros2_control_test_assets + ) + + add_rostest_with_parameters_gmock( + test_bicycle_steering_controller test/test_bicycle_steering_controller.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/bicycle_steering_controller_params.yaml) + target_include_directories(test_bicycle_steering_controller PRIVATE include) + target_link_libraries(test_bicycle_steering_controller bicycle_steering_controller) + ament_target_dependencies( + test_bicycle_steering_controller + controller_interface + hardware_interface + ) + + add_rostest_with_parameters_gmock( + test_bicycle_steering_controller_preceeding test/test_bicycle_steering_controller_preceeding.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/bicycle_steering_controller_preceeding_params.yaml) + target_include_directories(test_bicycle_steering_controller_preceeding PRIVATE include) + target_link_libraries(test_bicycle_steering_controller_preceeding bicycle_steering_controller) + ament_target_dependencies( + test_bicycle_steering_controller_preceeding + controller_interface + hardware_interface + ) endif() +ament_export_include_directories( + include +) +ament_export_dependencies( + ${THIS_PACKAGE_INCLUDE_DEPENDS} +) +ament_export_libraries( + bicycle_steering_controller +) + ament_package() diff --git a/bicycle_steering_controller/bicycle_steering_controller.xml b/bicycle_steering_controller/bicycle_steering_controller.xml new file mode 100644 index 0000000000..4d40e4c7bd --- /dev/null +++ b/bicycle_steering_controller/bicycle_steering_controller.xml @@ -0,0 +1,8 @@ + + + + BicycleSteeringController ros2_control controller. + + + diff --git a/bicycle_steering_controller/include/bicycle_steering_controller/bicycle_steering_controller.hpp b/bicycle_steering_controller/include/bicycle_steering_controller/bicycle_steering_controller.hpp new file mode 100644 index 0000000000..03de9d9ab0 --- /dev/null +++ b/bicycle_steering_controller/include/bicycle_steering_controller/bicycle_steering_controller.hpp @@ -0,0 +1,48 @@ +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Authors: dr. sc. Tomislav Petkovic, Dr. Ing. Denis Štogl +// + +#ifndef BICYCLE_STEERING_CONTROLLER__BICYCLE_STEERING_CONTROLLER_HPP_ +#define BICYCLE_STEERING_CONTROLLER__BICYCLE_STEERING_CONTROLLER_HPP_ + +#include + +#include "bicycle_steering_controller/visibility_control.h" +#include "bicycle_steering_controller_parameters.hpp" +#include "steering_controllers_library/steering_controllers_library.hpp" + +namespace bicycle_steering_controller +{ +class BicycleSteeringController : public steering_controllers_library::SteeringControllersLibrary +{ +public: + BicycleSteeringController(); + + BICYCLE_STEERING_CONTROLLER__VISIBILITY_PUBLIC controller_interface::CallbackReturn + configure_odometry() override; + + BICYCLE_STEERING_CONTROLLER__VISIBILITY_PUBLIC bool update_odometry( + const rclcpp::Duration & period) override; + + BICYCLE_STEERING_CONTROLLER__VISIBILITY_PUBLIC void initialize_implementation_parameter_listener() + override; + +private: + std::shared_ptr bicycle_param_listener_; +}; +} // namespace bicycle_steering_controller + +#endif // BICYCLE_STEERING_CONTROLLER__BICYCLE_STEERING_CONTROLLER_HPP_ diff --git a/bicycle_steering_controller/include/bicycle_steering_controller/visibility_control.h b/bicycle_steering_controller/include/bicycle_steering_controller/visibility_control.h new file mode 100644 index 0000000000..9799355bb0 --- /dev/null +++ b/bicycle_steering_controller/include/bicycle_steering_controller/visibility_control.h @@ -0,0 +1,53 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// Copyright (c) 2023, 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 BICYCLE_STEERING_CONTROLLER__VISIBILITY_CONTROL_H_ +#define BICYCLE_STEERING_CONTROLLER__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define BICYCLE_STEERING_CONTROLLER__VISIBILITY_EXPORT __attribute__((dllexport)) +#define BICYCLE_STEERING_CONTROLLER__VISIBILITY_IMPORT __attribute__((dllimport)) +#else +#define BICYCLE_STEERING_CONTROLLER__VISIBILITY_EXPORT __declspec(dllexport) +#define BICYCLE_STEERING_CONTROLLER__VISIBILITY_IMPORT __declspec(dllimport) +#endif +#ifdef BICYCLE_STEERING_CONTROLLER__VISIBILITY_BUILDING_DLL +#define BICYCLE_STEERING_CONTROLLER__VISIBILITY_PUBLIC \ + BICYCLE_STEERING_CONTROLLER__VISIBILITY_EXPORT +#else +#define BICYCLE_STEERING_CONTROLLER__VISIBILITY_PUBLIC \ + BICYCLE_STEERING_CONTROLLER__VISIBILITY_IMPORT +#endif +#define BICYCLE_STEERING_CONTROLLER__VISIBILITY_PUBLIC_TYPE \ + BICYCLE_STEERING_CONTROLLER__VISIBILITY_PUBLIC +#define BICYCLE_STEERING_CONTROLLER__VISIBILITY_LOCAL +#else +#define BICYCLE_STEERING_CONTROLLER__VISIBILITY_EXPORT __attribute__((visibility("default"))) +#define BICYCLE_STEERING_CONTROLLER__VISIBILITY_IMPORT +#if __GNUC__ >= 4 +#define BICYCLE_STEERING_CONTROLLER__VISIBILITY_PUBLIC __attribute__((visibility("default"))) +#define BICYCLE_STEERING_CONTROLLER__VISIBILITY_LOCAL __attribute__((visibility("hidden"))) +#else +#define BICYCLE_STEERING_CONTROLLER__VISIBILITY_PUBLIC +#define BICYCLE_STEERING_CONTROLLER__VISIBILITY_LOCAL +#endif +#define BICYCLE_STEERING_CONTROLLER__VISIBILITY_PUBLIC_TYPE +#endif + +#endif // BICYCLE_STEERING_CONTROLLER__VISIBILITY_CONTROL_H_ diff --git a/bicycle_steering_controller/package.xml b/bicycle_steering_controller/package.xml index f5f030d57b..80932f7fda 100644 --- a/bicycle_steering_controller/package.xml +++ b/bicycle_steering_controller/package.xml @@ -14,8 +14,21 @@ ament_cmake - ament_lint_auto - ament_lint_common + generate_parameter_library + + control_msgs + controller_interface + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + std_srvs + steering_controllers_library + + ament_cmake_gmock + controller_manager + hardware_interface + ros2_control_test_assets ament_cmake diff --git a/bicycle_steering_controller/src/bicycle_steering_controller.cpp b/bicycle_steering_controller/src/bicycle_steering_controller.cpp new file mode 100644 index 0000000000..82b778c858 --- /dev/null +++ b/bicycle_steering_controller/src/bicycle_steering_controller.cpp @@ -0,0 +1,91 @@ +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "bicycle_steering_controller/bicycle_steering_controller.hpp" + +namespace bicycle_steering_controller +{ +BicycleSteeringController::BicycleSteeringController() +: steering_controllers_library::SteeringControllersLibrary() +{ +} + +void BicycleSteeringController::initialize_implementation_parameter_listener() +{ + bicycle_param_listener_ = + std::make_shared(get_node()); +} + +controller_interface::CallbackReturn BicycleSteeringController::configure_odometry() +{ + bicycle_steering_controller::Params bicycle_params = bicycle_param_listener_->get_params(); + + const double wheelbase = bicycle_params.wheelbase; + const double front_wheel_radius = bicycle_params.front_wheel_radius; + const double rear_wheel_radius = bicycle_params.rear_wheel_radius; + + if (params_.front_steering) + { + odometry_.set_wheel_params(rear_wheel_radius, wheelbase); + } + else + { + odometry_.set_wheel_params(front_wheel_radius, wheelbase); + } + + odometry_.set_odometry_type(steering_odometry::BICYCLE_CONFIG); + + const size_t nr_state_itfs = 2; + const size_t nr_cmd_itfs = 2; + const size_t nr_ref_itfs = 2; + + set_interface_numbers(nr_state_itfs, nr_cmd_itfs, nr_ref_itfs); + + RCLCPP_INFO(get_node()->get_logger(), "bicycle odometry configure successful"); + return controller_interface::CallbackReturn::SUCCESS; +} + +bool BicycleSteeringController::update_odometry(const rclcpp::Duration & period) +{ + if (params_.open_loop) + { + odometry_.update_open_loop(last_linear_velocity_, last_angular_velocity_, period.seconds()); + } + else + { + const double rear_wheel_value = state_interfaces_[0].get_value(); + const double steer_position = state_interfaces_[1].get_value(); + if (!std::isnan(rear_wheel_value) && !std::isnan(steer_position)) + { + if (params_.position_feedback) + { + // Estimate linear and angular velocity using joint information + odometry_.update_from_position(rear_wheel_value, steer_position, period.seconds()); + } + else + { + // Estimate linear and angular velocity using joint information + odometry_.update_from_velocity(rear_wheel_value, steer_position, period.seconds()); + } + } + } + return true; +} +} // namespace bicycle_steering_controller + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + bicycle_steering_controller::BicycleSteeringController, + controller_interface::ChainableControllerInterface) diff --git a/bicycle_steering_controller/src/bicycle_steering_controller.yaml b/bicycle_steering_controller/src/bicycle_steering_controller.yaml new file mode 100644 index 0000000000..3f48c7d593 --- /dev/null +++ b/bicycle_steering_controller/src/bicycle_steering_controller.yaml @@ -0,0 +1,24 @@ +bicycle_steering_controller: + wheelbase: + { + type: double, + default_value: 0.0, + description: "Wheelbase length.", + read_only: false, + } + + front_wheel_radius: + { + type: double, + default_value: 0.0, + description: "Front wheel radius.", + read_only: false, + } + + rear_wheel_radius: + { + type: double, + default_value: 0.0, + description: "Rear wheel radius.", + read_only: false, + } diff --git a/bicycle_steering_controller/test/bicycle_steering_controller_params.yaml b/bicycle_steering_controller/test/bicycle_steering_controller_params.yaml new file mode 100644 index 0000000000..c4f5e3e105 --- /dev/null +++ b/bicycle_steering_controller/test/bicycle_steering_controller_params.yaml @@ -0,0 +1,7 @@ +test_bicycle_steering_controller: + ros__parameters: + + joints: + - joint1 + + interface_name: acceleration diff --git a/bicycle_steering_controller/test/bicycle_steering_controller_preceeding_params.yaml b/bicycle_steering_controller/test/bicycle_steering_controller_preceeding_params.yaml new file mode 100644 index 0000000000..d7818fa21d --- /dev/null +++ b/bicycle_steering_controller/test/bicycle_steering_controller_preceeding_params.yaml @@ -0,0 +1,9 @@ +test_bicycle_steering_controller: + ros__parameters: + joints: + - joint1 + + interface_name: acceleration + + state_joints: + - joint1state diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp new file mode 100644 index 0000000000..28c306713f --- /dev/null +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp @@ -0,0 +1,387 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "test_bicycle_steering_controller.hpp" + +#include +#include +#include +#include +#include + +using bicycle_steering_controller::CMD_MY_ITFS; +using bicycle_steering_controller::control_mode_type; +using bicycle_steering_controller::STATE_MY_ITFS; + +class BicycleSteeringControllerTest +: public BicycleSteeringControllerFixture +{ +}; + +TEST_F(BicycleSteeringControllerTest, 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(BicycleSteeringControllerTest, 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(BicycleSteeringControllerTest, 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(BicycleSteeringControllerTest, 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(BicycleSteeringControllerTest, 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(BicycleSteeringControllerTest, 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(BicycleSteeringControllerTest, 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(BicycleSteeringControllerTest, 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(BicycleSteeringControllerTest, 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(BicycleSteeringControllerTest, 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(BicycleSteeringControllerTest, 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(BicycleSteeringControllerTest, 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(BicycleSteeringControllerTest, 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/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp new file mode 100644 index 0000000000..3ee49eb6cf --- /dev/null +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp @@ -0,0 +1,280 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TEST_BICYCLE_STEERING_CONTROLLER_HPP_ +#define TEST_BICYCLE_STEERING_CONTROLLER_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "bicycle_steering_controller/bicycle_steering_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 = + steering_controllers_library::SteeringControllersLibrary::ControllerStateMsg; +using ControllerReferenceMsg = + steering_controllers_library::SteeringControllersLibrary::ControllerReferenceMsg; +using ControllerModeSrvType = + steering_controllers_library::SteeringControllersLibrary::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 TestableBicycleSteeringController +: public bicycle_steering_controller::BicycleSteeringController +{ + FRIEND_TEST(BicycleSteeringControllerTest, all_parameters_set_configure_success); + FRIEND_TEST(BicycleSteeringControllerTest, activate_success); + FRIEND_TEST(BicycleSteeringControllerTest, reactivate_success); + FRIEND_TEST(BicycleSteeringControllerTest, test_setting_slow_mode_service); + FRIEND_TEST(BicycleSteeringControllerTest, test_update_logic_fast); + FRIEND_TEST(BicycleSteeringControllerTest, test_update_logic_slow); + FRIEND_TEST(BicycleSteeringControllerTest, test_update_logic_chainable_fast); + FRIEND_TEST(BicycleSteeringControllerTest, test_update_logic_chainable_slow); + +public: + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override + { + auto ret = bicycle_steering_controller::BicycleSteeringController::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 bicycle_steering_controller::BicycleSteeringController::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 BicycleSteeringControllerFixture : 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_bicycle_steering_controller/commands", rclcpp::SystemDefaultsQoS()); + + service_caller_node_ = std::make_shared("service_caller"); + slow_control_service_client_ = service_caller_node_->create_client( + "/test_bicycle_steering_controller/set_slow_control_mode"); + } + + static void TearDownTestCase() {} + + void TearDown() { controller_.reset(nullptr); } + +protected: + void SetUpController(const std::string controller_name = "test_bicycle_steering_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_bicycle_steering_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 // TEST_BICYCLE_STEERING_CONTROLLER_HPP_ diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller_preceeding.cpp b/bicycle_steering_controller/test/test_bicycle_steering_controller_preceeding.cpp new file mode 100644 index 0000000000..a353af6e76 --- /dev/null +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller_preceeding.cpp @@ -0,0 +1,89 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "test_bicycle_steering_controller.hpp" + +#include +#include +#include +#include +#include + +using bicycle_steering_controller::CMD_MY_ITFS; +using bicycle_steering_controller::control_mode_type; +using bicycle_steering_controller::STATE_MY_ITFS; + +class BicycleSteeringControllerTest +: public BicycleSteeringControllerFixture +{ +}; + +TEST_F(BicycleSteeringControllerTest, 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(BicycleSteeringControllerTest, 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/bicycle_steering_controller/test/test_load_bicycle_steering_controller.cpp b/bicycle_steering_controller/test/test_load_bicycle_steering_controller.cpp new file mode 100644 index 0000000000..f7861aa322 --- /dev/null +++ b/bicycle_steering_controller/test/test_load_bicycle_steering_controller.cpp @@ -0,0 +1,41 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#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(TestLoadBicycleSteeringController, 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_bicycle_steering_controller", "bicycle_steering_controller/BicycleSteeringController")); + + rclcpp::shutdown(); +} diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index c9b80d18f0..4ab4187813 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -21,6 +21,7 @@ joint_state_broadcaster joint_trajectory_controller position_controllers + steering_controllers_library tricycle_controller tricycle_steering_controller velocity_controllers From b9b2e7e7ecd895c905eb2e5014181a98b843a0fb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Tue, 3 Jan 2023 19:01:51 +0100 Subject: [PATCH 105/186] Cleanup some files. --- .../include/bicycle_steering_controller/visibility_control.h | 1 - .../include/steering_controllers_library/visibility_control.h | 3 --- 2 files changed, 4 deletions(-) diff --git a/bicycle_steering_controller/include/bicycle_steering_controller/visibility_control.h b/bicycle_steering_controller/include/bicycle_steering_controller/visibility_control.h index 9799355bb0..b076a00215 100644 --- a/bicycle_steering_controller/include/bicycle_steering_controller/visibility_control.h +++ b/bicycle_steering_controller/include/bicycle_steering_controller/visibility_control.h @@ -1,5 +1,4 @@ // Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) -// Copyright (c) 2023, 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. diff --git a/steering_controllers_library/include/steering_controllers_library/visibility_control.h b/steering_controllers_library/include/steering_controllers_library/visibility_control.h index 3ba04fce22..087cae1f90 100644 --- a/steering_controllers_library/include/steering_controllers_library/visibility_control.h +++ b/steering_controllers_library/include/steering_controllers_library/visibility_control.h @@ -11,9 +11,6 @@ // 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. -// -// Authors: dr. sc. Tomislav Petkovic, Dr. Ing. Denis Štogl -// #ifndef STEERING_CONTROLLERS_LIBRARY__VISIBILITY_CONTROL_H_ #define STEERING_CONTROLLERS_LIBRARY__VISIBILITY_CONTROL_H_ From 378d88b364d7cdb2e4a62c1089a9359f16ee4a16 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Tue, 3 Jan 2023 19:35:35 +0100 Subject: [PATCH 106/186] Move Ackermann steering controller to dedicated package. --- ackermann_steering_controller/CMakeLists.txt | 104 ++++- .../ackermann_steering_controller.xml | 8 + .../ackermann_steering_controller.hpp | 58 +++ .../visibility_control.h | 53 +++ ackermann_steering_controller/package.xml | 17 +- .../src/ackermann_steering_controller.cpp | 102 +++++ .../src/ackermann_steering_controller.yaml | 40 ++ .../ackermann_steering_controller_params.yaml | 7 + ...steering_controller_preceeding_params.yaml | 9 + .../test_ackermann_steering_controller.cpp | 388 ++++++++++++++++++ .../test_ackermann_steering_controller.hpp | 282 +++++++++++++ ...kermann_steering_controller_preceeding.cpp | 90 ++++ ...est_load_ackermann_steering_controller.cpp | 43 ++ 13 files changed, 1187 insertions(+), 14 deletions(-) create mode 100644 ackermann_steering_controller/ackermann_steering_controller.xml create mode 100644 ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp create mode 100644 ackermann_steering_controller/include/ackermann_steering_controller/visibility_control.h create mode 100644 ackermann_steering_controller/src/ackermann_steering_controller.cpp create mode 100644 ackermann_steering_controller/src/ackermann_steering_controller.yaml create mode 100644 ackermann_steering_controller/test/ackermann_steering_controller_params.yaml create mode 100644 ackermann_steering_controller/test/ackermann_steering_controller_preceeding_params.yaml create mode 100644 ackermann_steering_controller/test/test_ackermann_steering_controller.cpp create mode 100644 ackermann_steering_controller/test/test_ackermann_steering_controller.hpp create mode 100644 ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp create mode 100644 ackermann_steering_controller/test/test_load_ackermann_steering_controller.cpp diff --git a/ackermann_steering_controller/CMakeLists.txt b/ackermann_steering_controller/CMakeLists.txt index 3f187a7d63..15e56a0ad0 100644 --- a/ackermann_steering_controller/CMakeLists.txt +++ b/ackermann_steering_controller/CMakeLists.txt @@ -6,21 +6,101 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") endif() # find dependencies +set(THIS_PACKAGE_INCLUDE_DEPENDS + controller_interface + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + std_srvs + steering_controllers_library +) + 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 ackermann_steering_controller library related compile commands +generate_parameter_library(ackermann_steering_controller_parameters + src/ackermann_steering_controller.yaml +) +add_library( + ackermann_steering_controller + SHARED + src/ackermann_steering_controller.cpp +) +target_include_directories(ackermann_steering_controller PUBLIC + "$" + "$") +target_link_libraries(ackermann_steering_controller ackermann_steering_controller_parameters) +ament_target_dependencies(ackermann_steering_controller ${THIS_PACKAGE_INCLUDE_DEPENDS}) +target_compile_definitions(ackermann_steering_controller PRIVATE "ACKERMANN_STEERING_CONTROLLER_BUILDING_DLL") + +pluginlib_export_plugin_description_file( + controller_interface ackermann_steering_controller.xml) + +install( + TARGETS + ackermann_steering_controller + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib +) + +install( + DIRECTORY include/ + DESTINATION include/${PROJECT_NAME} +) if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # comment the line when a copyright and license is added to all source files - set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # comment the line when this package is in a git repo and when - # a copyright and license is added to all source files - set(ament_cmake_cpplint_FOUND TRUE) - ament_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_ackermann_steering_controller test/test_load_ackermann_steering_controller.cpp) + target_include_directories(test_load_ackermann_steering_controller PRIVATE include) + ament_target_dependencies( + test_load_ackermann_steering_controller + controller_manager + hardware_interface + ros2_control_test_assets + ) + + add_rostest_with_parameters_gmock( + test_ackermann_steering_controller test/test_ackermann_steering_controller.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/ackermann_steering_controller_params.yaml) + target_include_directories(test_ackermann_steering_controller PRIVATE include) + target_link_libraries(test_ackermann_steering_controller ackermann_steering_controller) + ament_target_dependencies( + test_ackermann_steering_controller + controller_interface + hardware_interface + ) + + add_rostest_with_parameters_gmock( + test_ackermann_steering_controller_preceeding test/test_ackermann_steering_controller_preceeding.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/ackermann_steering_controller_preceeding_params.yaml) + target_include_directories(test_ackermann_steering_controller_preceeding PRIVATE include) + target_link_libraries(test_ackermann_steering_controller_preceeding ackermann_steering_controller) + ament_target_dependencies( + test_ackermann_steering_controller_preceeding + controller_interface + hardware_interface + ) endif() +ament_export_include_directories( + include +) +ament_export_dependencies( + ${THIS_PACKAGE_INCLUDE_DEPENDS} +) +ament_export_libraries( + ackermann_steering_controller +) + ament_package() diff --git a/ackermann_steering_controller/ackermann_steering_controller.xml b/ackermann_steering_controller/ackermann_steering_controller.xml new file mode 100644 index 0000000000..de128d6911 --- /dev/null +++ b/ackermann_steering_controller/ackermann_steering_controller.xml @@ -0,0 +1,8 @@ + + + + AckermannSteeringController ros2_control controller. + + + diff --git a/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp b/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp new file mode 100644 index 0000000000..6483737092 --- /dev/null +++ b/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp @@ -0,0 +1,58 @@ +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Authors: dr. sc. Tomislav Petkovic, Dr. Ing. Denis Štogl +// + +#ifndef ACKERMANN_STEERING_CONTROLLER__ACKERMANN_STEERING_CONTROLLER_HPP_ +#define ACKERMANN_STEERING_CONTROLLER__ACKERMANN_STEERING_CONTROLLER_HPP_ + +#include + +#include "ackermann_steering_controller/visibility_control.h" +#include "ackermann_steering_controller_parameters.hpp" +#include "steering_controllers_library/steering_controllers_library.hpp" + +namespace ackermann_steering_controller +{ +// name constants for state interfaces +static constexpr size_t STATE_DRIVE_RIGHT_WHEEL = 0; +static constexpr size_t STATE_DRIVE_LEFT_WHEEL = 1; +static constexpr size_t STATE_STEER_RIGHT_WHEEL = 2; +static constexpr size_t STATE_STEER_LEFT_WHEEL = 4; + +// name constants for command interfaces +static constexpr size_t CMD_DRIVE_WHEEL = 0; +static constexpr size_t CMD_STEER = 1; + +class AckermannSteeringController : public steering_controllers_library::SteeringControllersLibrary +{ +public: + AckermannSteeringController(); + + ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC controller_interface::CallbackReturn + configure_odometry() override; + + ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC bool update_odometry( + const rclcpp::Duration & period) override; + + ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC void + initialize_implementation_parameter_listener() override; + +private: + std::shared_ptr ackermann_param_listener_; +}; +} // namespace ackermann_steering_controller + +#endif // ACKERMANN_STEERING_CONTROLLER__ACKERMANN_STEERING_CONTROLLER_HPP_ diff --git a/ackermann_steering_controller/include/ackermann_steering_controller/visibility_control.h b/ackermann_steering_controller/include/ackermann_steering_controller/visibility_control.h new file mode 100644 index 0000000000..f2e83bf985 --- /dev/null +++ b/ackermann_steering_controller/include/ackermann_steering_controller/visibility_control.h @@ -0,0 +1,53 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// Copyright (c) 2023, 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 ACKERMANN_STEERING_CONTROLLER__VISIBILITY_CONTROL_H_ +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_EXPORT __attribute__((dllexport)) +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_IMPORT __attribute__((dllimport)) +#else +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_EXPORT __declspec(dllexport) +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_IMPORT __declspec(dllimport) +#endif +#ifdef ACKERMANN_STEERING_CONTROLLER__VISIBILITY_BUILDING_DLL +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC \ + ACKERMANN_STEERING_CONTROLLER__VISIBILITY_EXPORT +#else +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC \ + ACKERMANN_STEERING_CONTROLLER__VISIBILITY_IMPORT +#endif +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC_TYPE \ + ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_LOCAL +#else +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_EXPORT __attribute__((visibility("default"))) +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_IMPORT +#if __GNUC__ >= 4 +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC __attribute__((visibility("default"))) +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_LOCAL __attribute__((visibility("hidden"))) +#else +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_LOCAL +#endif +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC_TYPE +#endif + +#endif // ACKERMANN_STEERING_CONTROLLER__VISIBILITY_CONTROL_H_ diff --git a/ackermann_steering_controller/package.xml b/ackermann_steering_controller/package.xml index 965fa2fb4b..24c3c8406b 100644 --- a/ackermann_steering_controller/package.xml +++ b/ackermann_steering_controller/package.xml @@ -14,8 +14,21 @@ ament_cmake - ament_lint_auto - ament_lint_common + generate_parameter_library + + control_msgs + controller_interface + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + std_srvs + steering_controllers_library + + ament_cmake_gmock + controller_manager + hardware_interface + ros2_control_test_assets ament_cmake diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.cpp b/ackermann_steering_controller/src/ackermann_steering_controller.cpp new file mode 100644 index 0000000000..9106f41ea1 --- /dev/null +++ b/ackermann_steering_controller/src/ackermann_steering_controller.cpp @@ -0,0 +1,102 @@ +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "ackermann_steering_controller/ackermann_steering_controller.hpp" + +namespace ackermann_steering_controller +{ +AckermannSteeringController::AckermannSteeringController() +: steering_controllers_library::SteeringControllersLibrary() +{ +} + +void AckermannSteeringController::initialize_implementation_parameter_listener() +{ + ackermann_param_listener_ = + std::make_shared(get_node()); +} + +controller_interface::CallbackReturn AckermannSteeringController::configure_odometry() +{ + ackermann_steering_controller::Params ackerman_params = ackermann_param_listener_->get_params(); + + const double front_wheels_radius = ackerman_params.front_wheels_radius; + const double rear_wheels_radius = ackerman_params.rear_wheels_radius; + const double front_wheel_track = ackerman_params.front_wheel_track; + const double rear_wheel_track = ackerman_params.rear_wheel_track; + const double wheelbase = ackerman_params.wheelbase; + + if (params_.front_steering) + { + odometry_.set_wheel_params(rear_wheels_radius, wheelbase, rear_wheel_track); + } + else + { + odometry_.set_wheel_params(front_wheels_radius, wheelbase, front_wheel_track); + } + + odometry_.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); + + const size_t nr_state_itfs = 4; + const size_t nr_cmd_itfs = 4; + const size_t nr_ref_itfs = 2; + + set_interface_numbers(nr_state_itfs, nr_cmd_itfs, nr_ref_itfs); + + RCLCPP_INFO(get_node()->get_logger(), "ackermann odom configure successful"); + return controller_interface::CallbackReturn::SUCCESS; +} + +bool AckermannSteeringController::update_odometry(const rclcpp::Duration & period) +{ + if (params_.open_loop) + { + odometry_.update_open_loop(last_linear_velocity_, last_angular_velocity_, period.seconds()); + } + else + { + const double rear_right_wheel_value = state_interfaces_[STATE_DRIVE_RIGHT_WHEEL].get_value(); + const double rear_left_wheel_value = state_interfaces_[STATE_DRIVE_LEFT_WHEEL].get_value(); + const double front_right_steer_position = + state_interfaces_[STATE_STEER_RIGHT_WHEEL].get_value(); + const double front_left_steer_position = state_interfaces_[STATE_STEER_LEFT_WHEEL].get_value(); + if ( + !std::isnan(rear_right_wheel_value) && !std::isnan(rear_left_wheel_value) && + !std::isnan(front_right_steer_position) && !std::isnan(front_left_steer_position)) + { + if (params_.position_feedback) + { + // Estimate linear and angular velocity using joint information + odometry_.update_from_position( + rear_right_wheel_value, rear_left_wheel_value, front_right_steer_position, + front_left_steer_position, period.seconds()); + } + else + { + // Estimate linear and angular velocity using joint information + odometry_.update_from_velocity( + rear_right_wheel_value, rear_left_wheel_value, front_right_steer_position, + front_left_steer_position, period.seconds()); + } + } + } + return true; +} +} // namespace ackermann_steering_controller + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + ackermann_steering_controller::AckermannSteeringController, + controller_interface::ChainableControllerInterface) diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.yaml b/ackermann_steering_controller/src/ackermann_steering_controller.yaml new file mode 100644 index 0000000000..a9f5d1b700 --- /dev/null +++ b/ackermann_steering_controller/src/ackermann_steering_controller.yaml @@ -0,0 +1,40 @@ +ackermann_steering_controller: + front_wheel_track: + { + type: double, + default_value: 0.0, + description: "Front wheel track length.", + read_only: false, + } + + rear_wheel_track: + { + type: double, + default_value: 0.0, + description: "Rear wheel track length.", + read_only: false, + } + + wheelbase: + { + type: double, + default_value: 0.0, + description: "Wheelbase length.", + read_only: false, + } + + front_wheels_radius: + { + type: double, + default_value: 0.0, + description: "Front wheels radius.", + read_only: false, + } + + rear_wheels_radius: + { + type: double, + default_value: 0.0, + description: "Rear wheels radius.", + read_only: false, + } diff --git a/ackermann_steering_controller/test/ackermann_steering_controller_params.yaml b/ackermann_steering_controller/test/ackermann_steering_controller_params.yaml new file mode 100644 index 0000000000..d09ee30a97 --- /dev/null +++ b/ackermann_steering_controller/test/ackermann_steering_controller_params.yaml @@ -0,0 +1,7 @@ +test_ackermann_steering_controller: + ros__parameters: + + joints: + - joint1 + + interface_name: acceleration diff --git a/ackermann_steering_controller/test/ackermann_steering_controller_preceeding_params.yaml b/ackermann_steering_controller/test/ackermann_steering_controller_preceeding_params.yaml new file mode 100644 index 0000000000..0fe973bb8d --- /dev/null +++ b/ackermann_steering_controller/test/ackermann_steering_controller_preceeding_params.yaml @@ -0,0 +1,9 @@ +test_ackermann_steering_controller: + ros__parameters: + joints: + - joint1 + + interface_name: acceleration + + state_joints: + - joint1state diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp new file mode 100644 index 0000000000..adc767e1eb --- /dev/null +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp @@ -0,0 +1,388 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// Copyright (c) 2023, 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_ackermann_steering_controller.hpp" + +#include +#include +#include +#include +#include + +using ackermann_steering_controller::CMD_DRIVE_WHEEL; +using ackermann_steering_controller::control_mode_type; +using ackermann_steering_controller::STATE_DRIVE_RIGHT_WHEEL; + +class AckermannSteeringControllerTest +: public AckermannSteeringControllerFixture +{ +}; + +TEST_F(AckermannSteeringControllerTest, 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(AckermannSteeringControllerTest, 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(AckermannSteeringControllerTest, 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(AckermannSteeringControllerTest, 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(AckermannSteeringControllerTest, 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(AckermannSteeringControllerTest, 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_DRIVE_WHEEL].get_value(), 101.101); + ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(std::isnan(controller_->command_interfaces_[CMD_DRIVE_WHEEL].get_value())); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(std::isnan(controller_->command_interfaces_[CMD_DRIVE_WHEEL].get_value())); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); +} + +TEST_F(AckermannSteeringControllerTest, 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(AckermannSteeringControllerTest, 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_DRIVE_RIGHT_WHEEL], 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(AckermannSteeringControllerTest, 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_DRIVE_RIGHT_WHEEL], 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(AckermannSteeringControllerTest, 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_DRIVE_RIGHT_WHEEL] = 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_DRIVE_RIGHT_WHEEL], 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(AckermannSteeringControllerTest, 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_DRIVE_RIGHT_WHEEL] = 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_DRIVE_RIGHT_WHEEL], 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(AckermannSteeringControllerTest, 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(AckermannSteeringControllerTest, 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_DRIVE_WHEEL], 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/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp new file mode 100644 index 0000000000..0ab7ad518c --- /dev/null +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp @@ -0,0 +1,282 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// Copyright (c) 2023, 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 TEST_ACKERMANN_STEERING_CONTROLLER_HPP_ +#define TEST_ACKERMANN_STEERING_CONTROLLER_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "ackermann_steering_controller/ackermann_steering_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 = + steering_controllers_library::SteeringControllersLibrary::ControllerStateMsg; +using ControllerReferenceMsg = + steering_controllers_library::SteeringControllersLibrary::ControllerReferenceMsg; +using ControllerModeSrvType = + steering_controllers_library::SteeringControllersLibrary::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 TestableAckermannSteeringController +: public ackermann_steering_controller::AckermannSteeringController +{ + FRIEND_TEST(AckermannSteeringControllerTest, all_parameters_set_configure_success); + FRIEND_TEST(AckermannSteeringControllerTest, activate_success); + FRIEND_TEST(AckermannSteeringControllerTest, reactivate_success); + FRIEND_TEST(AckermannSteeringControllerTest, test_setting_slow_mode_service); + FRIEND_TEST(AckermannSteeringControllerTest, test_update_logic_fast); + FRIEND_TEST(AckermannSteeringControllerTest, test_update_logic_slow); + FRIEND_TEST(AckermannSteeringControllerTest, test_update_logic_chainable_fast); + FRIEND_TEST(AckermannSteeringControllerTest, test_update_logic_chainable_slow); + +public: + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override + { + auto ret = + ackermann_steering_controller::AckermannSteeringController::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 ackermann_steering_controller::AckermannSteeringController::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 AckermannSteeringControllerFixture : 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_ackermann_steering_controller/commands", rclcpp::SystemDefaultsQoS()); + + service_caller_node_ = std::make_shared("service_caller"); + slow_control_service_client_ = service_caller_node_->create_client( + "/test_ackermann_steering_controller/set_slow_control_mode"); + } + + static void TearDownTestCase() {} + + void TearDown() { controller_.reset(nullptr); } + +protected: + void SetUpController(const std::string controller_name = "test_ackermann_steering_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_ackermann_steering_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 // TEST_ACKERMANN_STEERING_CONTROLLER_HPP_ diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp b/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp new file mode 100644 index 0000000000..b2df36f2d7 --- /dev/null +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp @@ -0,0 +1,90 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// Copyright (c) 2023, 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_ackermann_steering_controller.hpp" + +#include +#include +#include +#include +#include + +using ackermann_steering_controller::CMD_MY_ITFS; +using ackermann_steering_controller::control_mode_type; +using ackermann_steering_controller::STATE_MY_ITFS; + +class AckermannSteeringControllerTest +: public AckermannSteeringControllerFixture +{ +}; + +TEST_F(AckermannSteeringControllerTest, 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(AckermannSteeringControllerTest, 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/ackermann_steering_controller/test/test_load_ackermann_steering_controller.cpp b/ackermann_steering_controller/test/test_load_ackermann_steering_controller.cpp new file mode 100644 index 0000000000..7532cd2cba --- /dev/null +++ b/ackermann_steering_controller/test/test_load_ackermann_steering_controller.cpp @@ -0,0 +1,43 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// Copyright (c) 2023, 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(TestLoadAckermannSteeringController, 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_ackermann_steering_controller", + "ackermann_steering_controller/AckermannSteeringController")); + + rclcpp::shutdown(); +} From 952c7846c9dd684453d985b0aef8619b67e4470d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Tue, 3 Jan 2023 19:53:05 +0100 Subject: [PATCH 107/186] Moved Tricycle controller. --- .../config/ackermann_steering_controller.yaml | 40 -- .../config/bicycle_steering_controller.yaml | 24 -- .../ackermann_steering_controller.hpp | 47 --- .../bicycle_steering_controller.hpp | 47 --- .../src/ackermann_steering_controller.cpp | 101 ----- .../src/bicycle_steering_controller.cpp | 90 ---- tricycle_steering_controller/CMakeLists.txt | 97 ++++- .../tricycle_steering_controller.hpp | 22 +- .../visibility_control.h | 53 +++ tricycle_steering_controller/package.xml | 17 +- .../src/tricycle_steering_controller.cpp | 10 +- .../src}/tricycle_steering_controller.yaml | 0 ...test_load_tricycle_steering_controller.cpp | 43 ++ .../test_tricycle_steering_controller.cpp | 388 ++++++++++++++++++ .../test_tricycle_steering_controller.hpp | 282 +++++++++++++ ...ricycle_steering_controller_preceeding.cpp | 90 ++++ .../tricycle_steering_controller_params.yaml | 7 + ...steering_controller_preceeding_params.yaml | 9 + .../tricycle_steering_controller.xml | 8 + 19 files changed, 1009 insertions(+), 366 deletions(-) delete mode 100644 steering_controllers_library/config/ackermann_steering_controller.yaml delete mode 100644 steering_controllers_library/config/bicycle_steering_controller.yaml delete mode 100644 steering_controllers_library/include/steering_controllers_library/ackermann_steering_controller.hpp delete mode 100644 steering_controllers_library/include/steering_controllers_library/bicycle_steering_controller.hpp delete mode 100644 steering_controllers_library/src/ackermann_steering_controller.cpp delete mode 100644 steering_controllers_library/src/bicycle_steering_controller.cpp rename {steering_controllers_library/include/steering_controllers_library => tricycle_steering_controller/include/tricycle_steering_controller}/tricycle_steering_controller.hpp (60%) create mode 100644 tricycle_steering_controller/include/tricycle_steering_controller/visibility_control.h rename {steering_controllers_library => tricycle_steering_controller}/src/tricycle_steering_controller.cpp (88%) rename {steering_controllers_library/config => tricycle_steering_controller/src}/tricycle_steering_controller.yaml (100%) create mode 100644 tricycle_steering_controller/test/test_load_tricycle_steering_controller.cpp create mode 100644 tricycle_steering_controller/test/test_tricycle_steering_controller.cpp create mode 100644 tricycle_steering_controller/test/test_tricycle_steering_controller.hpp create mode 100644 tricycle_steering_controller/test/test_tricycle_steering_controller_preceeding.cpp create mode 100644 tricycle_steering_controller/test/tricycle_steering_controller_params.yaml create mode 100644 tricycle_steering_controller/test/tricycle_steering_controller_preceeding_params.yaml create mode 100644 tricycle_steering_controller/tricycle_steering_controller.xml diff --git a/steering_controllers_library/config/ackermann_steering_controller.yaml b/steering_controllers_library/config/ackermann_steering_controller.yaml deleted file mode 100644 index a9f5d1b700..0000000000 --- a/steering_controllers_library/config/ackermann_steering_controller.yaml +++ /dev/null @@ -1,40 +0,0 @@ -ackermann_steering_controller: - front_wheel_track: - { - type: double, - default_value: 0.0, - description: "Front wheel track length.", - read_only: false, - } - - rear_wheel_track: - { - type: double, - default_value: 0.0, - description: "Rear wheel track length.", - read_only: false, - } - - wheelbase: - { - type: double, - default_value: 0.0, - description: "Wheelbase length.", - read_only: false, - } - - front_wheels_radius: - { - type: double, - default_value: 0.0, - description: "Front wheels radius.", - read_only: false, - } - - rear_wheels_radius: - { - type: double, - default_value: 0.0, - description: "Rear wheels radius.", - read_only: false, - } diff --git a/steering_controllers_library/config/bicycle_steering_controller.yaml b/steering_controllers_library/config/bicycle_steering_controller.yaml deleted file mode 100644 index 3f48c7d593..0000000000 --- a/steering_controllers_library/config/bicycle_steering_controller.yaml +++ /dev/null @@ -1,24 +0,0 @@ -bicycle_steering_controller: - wheelbase: - { - type: double, - default_value: 0.0, - description: "Wheelbase length.", - read_only: false, - } - - front_wheel_radius: - { - type: double, - default_value: 0.0, - description: "Front wheel radius.", - read_only: false, - } - - rear_wheel_radius: - { - type: double, - default_value: 0.0, - description: "Rear wheel radius.", - read_only: false, - } diff --git a/steering_controllers_library/include/steering_controllers_library/ackermann_steering_controller.hpp b/steering_controllers_library/include/steering_controllers_library/ackermann_steering_controller.hpp deleted file mode 100644 index db77fac8b4..0000000000 --- a/steering_controllers_library/include/steering_controllers_library/ackermann_steering_controller.hpp +++ /dev/null @@ -1,47 +0,0 @@ -// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// Authors: dr. sc. Tomislav Petkovic, Dr. Ing. Denis Štogl -// - -#ifndef STEERING_CONTROLLERS__ACKERMANN_STEERING_CONTROLLER_HPP_ -#define STEERING_CONTROLLERS__ACKERMANN_STEERING_CONTROLLER_HPP_ - -#include - -#include "ackermann_controller_parameters.hpp" -#include "steering_controllers/steering_controllers.hpp" - -namespace ackermann_steering_controller -{ -class AckermannSteeringController : public steering_controllers::SteeringControllers -{ -public: - AckermannSteeringController(); - - STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::CallbackReturn configure_odometry() - override; - - STEERING_CONTROLLERS__VISIBILITY_PUBLIC bool update_odometry( - const rclcpp::Duration & period) override; - - STEERING_CONTROLLERS__VISIBILITY_PUBLIC void initialize_implementation_parameter_listener() - override; - -private: - std::shared_ptr ackermann_param_listener_; -}; -} // namespace ackermann_steering_controller - -#endif // STEERING_CONTROLLERS__ACKERMANN_STEERING_CONTROLLER_HPP_ diff --git a/steering_controllers_library/include/steering_controllers_library/bicycle_steering_controller.hpp b/steering_controllers_library/include/steering_controllers_library/bicycle_steering_controller.hpp deleted file mode 100644 index 2c80561d34..0000000000 --- a/steering_controllers_library/include/steering_controllers_library/bicycle_steering_controller.hpp +++ /dev/null @@ -1,47 +0,0 @@ -// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// Authors: dr. sc. Tomislav Petkovic, Dr. Ing. Denis Štogl -// - -#ifndef STEERING_CONTROLLERS__BICYCLE_STEERING_CONTROLLER_HPP_ -#define STEERING_CONTROLLERS__BICYCLE_STEERING_CONTROLLER_HPP_ - -#include - -#include "bicycle_controller_parameters.hpp" -#include "steering_controllers/steering_controllers.hpp" - -namespace bicycle_steering_controller -{ -class BicycleSteeringController : public steering_controllers::SteeringControllers -{ -public: - BicycleSteeringController(); - - STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::CallbackReturn configure_odometry() - override; - - STEERING_CONTROLLERS__VISIBILITY_PUBLIC bool update_odometry( - const rclcpp::Duration & period) override; - - STEERING_CONTROLLERS__VISIBILITY_PUBLIC void initialize_implementation_parameter_listener() - override; - -private: - std::shared_ptr bicycle_param_listener_; -}; -} // namespace bicycle_steering_controller - -#endif // STEERING_CONTROLLERS__BICYCLE_STEERING_CONTROLLER_HPP_ diff --git a/steering_controllers_library/src/ackermann_steering_controller.cpp b/steering_controllers_library/src/ackermann_steering_controller.cpp deleted file mode 100644 index 0afb8199fa..0000000000 --- a/steering_controllers_library/src/ackermann_steering_controller.cpp +++ /dev/null @@ -1,101 +0,0 @@ -// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "steering_controllers/ackermann_steering_controller.hpp" - -namespace ackermann_steering_controller -{ -AckermannSteeringController::AckermannSteeringController() -: steering_controllers::SteeringControllers() -{ -} - -void AckermannSteeringController::initialize_implementation_parameter_listener() -{ - ackermann_param_listener_ = - std::make_shared(get_node()); -} - -controller_interface::CallbackReturn AckermannSteeringController::configure_odometry() -{ - ackermann_steering_controller::Params ackerman_params = ackermann_param_listener_->get_params(); - - const double front_wheels_radius = ackerman_params.front_wheels_radius; - const double rear_wheels_radius = ackerman_params.rear_wheels_radius; - const double front_wheel_track = ackerman_params.front_wheel_track; - const double rear_wheel_track = ackerman_params.rear_wheel_track; - const double wheelbase = ackerman_params.wheelbase; - - if (params_.front_steering) - { - odometry_.set_wheel_params(rear_wheels_radius, wheelbase, rear_wheel_track); - } - else - { - odometry_.set_wheel_params(front_wheels_radius, wheelbase, front_wheel_track); - } - - odometry_.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); - - const size_t nr_state_itfs = 4; - const size_t nr_cmd_itfs = 4; - const size_t nr_ref_itfs = 2; - - set_interface_numbers(nr_state_itfs, nr_cmd_itfs, nr_ref_itfs); - - RCLCPP_INFO(get_node()->get_logger(), "ackermann odom configure successful"); - return controller_interface::CallbackReturn::SUCCESS; -} - -bool AckermannSteeringController::update_odometry(const rclcpp::Duration & period) -{ - if (params_.open_loop) - { - odometry_.update_open_loop(last_linear_velocity_, last_angular_velocity_, period.seconds()); - } - else - { - const double rear_right_wheel_value = state_interfaces_[0].get_value(); - const double rear_left_wheel_value = state_interfaces_[1].get_value(); - const double front_right_steer_position = state_interfaces_[2].get_value(); - const double front_left_steer_position = state_interfaces_[3].get_value(); - if ( - !std::isnan(rear_right_wheel_value) && !std::isnan(rear_left_wheel_value) && - !std::isnan(front_right_steer_position) && !std::isnan(front_left_steer_position)) - { - if (params_.position_feedback) - { - // Estimate linear and angular velocity using joint information - odometry_.update_from_position( - rear_right_wheel_value, rear_left_wheel_value, front_right_steer_position, - front_left_steer_position, period.seconds()); - } - else - { - // Estimate linear and angular velocity using joint information - odometry_.update_from_velocity( - rear_right_wheel_value, rear_left_wheel_value, front_right_steer_position, - front_left_steer_position, period.seconds()); - } - } - } - return true; -} -} // namespace ackermann_steering_controller - -#include "pluginlib/class_list_macros.hpp" - -PLUGINLIB_EXPORT_CLASS( - ackermann_steering_controller::AckermannSteeringController, - controller_interface::ChainableControllerInterface) diff --git a/steering_controllers_library/src/bicycle_steering_controller.cpp b/steering_controllers_library/src/bicycle_steering_controller.cpp deleted file mode 100644 index f296690ed0..0000000000 --- a/steering_controllers_library/src/bicycle_steering_controller.cpp +++ /dev/null @@ -1,90 +0,0 @@ -// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "steering_controllers/bicycle_steering_controller.hpp" - -namespace bicycle_steering_controller -{ -BicycleSteeringController::BicycleSteeringController() : steering_controllers::SteeringControllers() -{ -} - -void BicycleSteeringController::initialize_implementation_parameter_listener() -{ - bicycle_param_listener_ = - std::make_shared(get_node()); -} - -controller_interface::CallbackReturn BicycleSteeringController::configure_odometry() -{ - bicycle_steering_controller::Params bicycle_params = bicycle_param_listener_->get_params(); - - const double wheelbase = bicycle_params.wheelbase; - const double front_wheel_radius = bicycle_params.front_wheel_radius; - const double rear_wheel_radius = bicycle_params.rear_wheel_radius; - - if (params_.front_steering) - { - odometry_.set_wheel_params(rear_wheel_radius, wheelbase); - } - else - { - odometry_.set_wheel_params(front_wheel_radius, wheelbase); - } - - odometry_.set_odometry_type(steering_odometry::BICYCLE_CONFIG); - - const size_t nr_state_itfs = 2; - const size_t nr_cmd_itfs = 2; - const size_t nr_ref_itfs = 2; - - set_interface_numbers(nr_state_itfs, nr_cmd_itfs, nr_ref_itfs); - - RCLCPP_INFO(get_node()->get_logger(), "bicycle odom configure successful"); - return controller_interface::CallbackReturn::SUCCESS; -} - -bool BicycleSteeringController::update_odometry(const rclcpp::Duration & period) -{ - if (params_.open_loop) - { - odometry_.update_open_loop(last_linear_velocity_, last_angular_velocity_, period.seconds()); - } - else - { - const double rear_wheel_value = state_interfaces_[0].get_value(); - const double steer_position = state_interfaces_[1].get_value(); - if (!std::isnan(rear_wheel_value) && !std::isnan(steer_position)) - { - if (params_.position_feedback) - { - // Estimate linear and angular velocity using joint information - odometry_.update_from_position(rear_wheel_value, steer_position, period.seconds()); - } - else - { - // Estimate linear and angular velocity using joint information - odometry_.update_from_velocity(rear_wheel_value, steer_position, period.seconds()); - } - } - } - return true; -} -} // namespace bicycle_steering_controller - -#include "pluginlib/class_list_macros.hpp" - -PLUGINLIB_EXPORT_CLASS( - bicycle_steering_controller::BicycleSteeringController, - controller_interface::ChainableControllerInterface) diff --git a/tricycle_steering_controller/CMakeLists.txt b/tricycle_steering_controller/CMakeLists.txt index a0298ee275..9379e62d92 100644 --- a/tricycle_steering_controller/CMakeLists.txt +++ b/tricycle_steering_controller/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 + steering_controllers_library +) + 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 tricycle_steering_controller library related compile commands +generate_parameter_library(tricycle_steering_controller_parameters + src/tricycle_steering_controller.yaml +) +add_library( + tricycle_steering_controller + SHARED + src/tricycle_steering_controller.cpp +) +target_include_directories(tricycle_steering_controller PUBLIC + "$" + "$") +target_link_libraries(tricycle_steering_controller tricycle_steering_controller_parameters) +ament_target_dependencies(tricycle_steering_controller ${THIS_PACKAGE_INCLUDE_DEPENDS}) +target_compile_definitions(tricycle_steering_controller PRIVATE "TRICYCLE_STEERING_CONTROLLER_BUILDING_DLL") + +pluginlib_export_plugin_description_file( + controller_interface tricycle_steering_controller.xml) + +install( + TARGETS + tricycle_steering_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,51 @@ 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_tricycle_steering_controller test/test_load_tricycle_steering_controller.cpp) + target_include_directories(test_load_tricycle_steering_controller PRIVATE include) + ament_target_dependencies( + test_load_tricycle_steering_controller + controller_manager + hardware_interface + ros2_control_test_assets + ) + + add_rostest_with_parameters_gmock( + test_tricycle_steering_controller test/test_tricycle_steering_controller.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/tricycle_steering_controller_params.yaml) + target_include_directories(test_tricycle_steering_controller PRIVATE include) + target_link_libraries(test_tricycle_steering_controller tricycle_steering_controller) + ament_target_dependencies( + test_tricycle_steering_controller + controller_interface + hardware_interface + ) + + add_rostest_with_parameters_gmock( + test_tricycle_steering_controller_preceeding test/test_tricycle_steering_controller_preceeding.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/tricycle_steering_controller_preceeding_params.yaml) + target_include_directories(test_tricycle_steering_controller_preceeding PRIVATE include) + target_link_libraries(test_tricycle_steering_controller_preceeding tricycle_steering_controller) + ament_target_dependencies( + test_tricycle_steering_controller_preceeding + controller_interface + hardware_interface + ) endif() +ament_export_include_directories( + include +) +ament_export_dependencies( + ${THIS_PACKAGE_INCLUDE_DEPENDS} +) +ament_export_libraries( + tricycle_steering_controller +) + ament_package() diff --git a/steering_controllers_library/include/steering_controllers_library/tricycle_steering_controller.hpp b/tricycle_steering_controller/include/tricycle_steering_controller/tricycle_steering_controller.hpp similarity index 60% rename from steering_controllers_library/include/steering_controllers_library/tricycle_steering_controller.hpp rename to tricycle_steering_controller/include/tricycle_steering_controller/tricycle_steering_controller.hpp index c7210c5c4a..46cc718684 100644 --- a/steering_controllers_library/include/steering_controllers_library/tricycle_steering_controller.hpp +++ b/tricycle_steering_controller/include/tricycle_steering_controller/tricycle_steering_controller.hpp @@ -15,17 +15,27 @@ // Authors: dr. sc. Tomislav Petkovic, Dr. Ing. Denis Štogl // -#ifndef STEERING_CONTROLLERS__TRICYCLE_STEERING_CONTROLLER_HPP_ -#define STEERING_CONTROLLERS__TRICYCLE_STEERING_CONTROLLER_HPP_ +#ifndef TRICYCLE_STEERING_CONTROLLER__TRICYCLE_STEERING_CONTROLLER_HPP_ +#define TRICYCLE_STEERING_CONTROLLER__TRICYCLE_STEERING_CONTROLLER_HPP_ #include -#include "steering_controllers/steering_controllers.hpp" -#include "tricycle_controller_parameters.hpp" +#include "steering_controllers_library/steering_controllers_library.hpp" +#include "tricycle_steering_controller/visibility_control.h" +#include "tricycle_steering_controller_parameters.hpp" namespace tricycle_steering_controller { -class TricycleSteeringController : public steering_controllers::SteeringControllers +// name constants for state interfaces +static constexpr size_t STATE_DRIVE_RIGHT_WHEEL = 0; +static constexpr size_t STATE_DRIVE_LEFT_WHEEL = 1; +static constexpr size_t STATE_STEER_AXIS = 2; + +// name constants for command interfaces +static constexpr size_t CMD_DRIVE_WHEEL = 0; +static constexpr size_t CMD_STEER = 1; + +class TricycleSteeringController : public steering_controllers_library::SteeringControllersLibrary { public: TricycleSteeringController(); @@ -44,4 +54,4 @@ class TricycleSteeringController : public steering_controllers::SteeringControll }; } // namespace tricycle_steering_controller -#endif // STEERING_CONTROLLERS__TRICYCLE_STEERING_CONTROLLER_HPP_ +#endif // TRICYCLE_STEERING_CONTROLLER__TRICYCLE_STEERING_CONTROLLER_HPP_ diff --git a/tricycle_steering_controller/include/tricycle_steering_controller/visibility_control.h b/tricycle_steering_controller/include/tricycle_steering_controller/visibility_control.h new file mode 100644 index 0000000000..33fdfbbe38 --- /dev/null +++ b/tricycle_steering_controller/include/tricycle_steering_controller/visibility_control.h @@ -0,0 +1,53 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// Copyright (c) 2023, 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 TRICYCLE_STEERING_CONTROLLER__VISIBILITY_CONTROL_H_ +#define TRICYCLE_STEERING_CONTROLLER__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define TRICYCLE_STEERING_CONTROLLER__VISIBILITY_EXPORT __attribute__((dllexport)) +#define TRICYCLE_STEERING_CONTROLLER__VISIBILITY_IMPORT __attribute__((dllimport)) +#else +#define TRICYCLE_STEERING_CONTROLLER__VISIBILITY_EXPORT __declspec(dllexport) +#define TRICYCLE_STEERING_CONTROLLER__VISIBILITY_IMPORT __declspec(dllimport) +#endif +#ifdef TRICYCLE_STEERING_CONTROLLER__VISIBILITY_BUILDING_DLL +#define TRICYCLE_STEERING_CONTROLLER__VISIBILITY_PUBLIC \ + TRICYCLE_STEERING_CONTROLLER__VISIBILITY_EXPORT +#else +#define TRICYCLE_STEERING_CONTROLLER__VISIBILITY_PUBLIC \ + TRICYCLE_STEERING_CONTROLLER__VISIBILITY_IMPORT +#endif +#define TRICYCLE_STEERING_CONTROLLER__VISIBILITY_PUBLIC_TYPE \ + TRICYCLE_STEERING_CONTROLLER__VISIBILITY_PUBLIC +#define TRICYCLE_STEERING_CONTROLLER__VISIBILITY_LOCAL +#else +#define TRICYCLE_STEERING_CONTROLLER__VISIBILITY_EXPORT __attribute__((visibility("default"))) +#define TRICYCLE_STEERING_CONTROLLER__VISIBILITY_IMPORT +#if __GNUC__ >= 4 +#define TRICYCLE_STEERING_CONTROLLER__VISIBILITY_PUBLIC __attribute__((visibility("default"))) +#define TRICYCLE_STEERING_CONTROLLER__VISIBILITY_LOCAL __attribute__((visibility("hidden"))) +#else +#define TRICYCLE_STEERING_CONTROLLER__VISIBILITY_PUBLIC +#define TRICYCLE_STEERING_CONTROLLER__VISIBILITY_LOCAL +#endif +#define TRICYCLE_STEERING_CONTROLLER__VISIBILITY_PUBLIC_TYPE +#endif + +#endif // TRICYCLE_STEERING_CONTROLLER__VISIBILITY_CONTROL_H_ diff --git a/tricycle_steering_controller/package.xml b/tricycle_steering_controller/package.xml index dada641359..e911041cd1 100644 --- a/tricycle_steering_controller/package.xml +++ b/tricycle_steering_controller/package.xml @@ -16,8 +16,21 @@ ament_cmake - ament_lint_auto - ament_lint_common + generate_parameter_library + + control_msgs + controller_interface + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + std_srvs + steering_controllers_library + + ament_cmake_gmock + controller_manager + hardware_interface + ros2_control_test_assets ament_cmake diff --git a/steering_controllers_library/src/tricycle_steering_controller.cpp b/tricycle_steering_controller/src/tricycle_steering_controller.cpp similarity index 88% rename from steering_controllers_library/src/tricycle_steering_controller.cpp rename to tricycle_steering_controller/src/tricycle_steering_controller.cpp index f9988d7eef..15e7488cb0 100644 --- a/steering_controllers_library/src/tricycle_steering_controller.cpp +++ b/tricycle_steering_controller/src/tricycle_steering_controller.cpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "steering_controllers/tricycle_steering_controller.hpp" +#include "tricycle_steering_controller/tricycle_steering_controller.hpp" namespace tricycle_steering_controller { TricycleSteeringController::TricycleSteeringController() -: steering_controllers::SteeringControllers() +: steering_controllers_library::SteeringControllersLibrary() { } void TricycleSteeringController::initialize_implementation_parameter_listener() @@ -64,9 +64,9 @@ bool TricycleSteeringController::update_odometry(const rclcpp::Duration & period } else { - const double rear_right_wheel_value = state_interfaces_[0].get_value(); - const double rear_left_wheel_value = state_interfaces_[1].get_value(); - const double steer_position = state_interfaces_[2].get_value(); + const double rear_right_wheel_value = state_interfaces_[STATE_DRIVE_RIGHT_WHEEL].get_value(); + const double rear_left_wheel_value = state_interfaces_[STATE_DRIVE_LEFT_WHEEL].get_value(); + const double steer_position = state_interfaces_[STATE_STEER_AXIS].get_value(); if ( !std::isnan(rear_right_wheel_value) && !std::isnan(rear_left_wheel_value) && !std::isnan(steer_position)) diff --git a/steering_controllers_library/config/tricycle_steering_controller.yaml b/tricycle_steering_controller/src/tricycle_steering_controller.yaml similarity index 100% rename from steering_controllers_library/config/tricycle_steering_controller.yaml rename to tricycle_steering_controller/src/tricycle_steering_controller.yaml diff --git a/tricycle_steering_controller/test/test_load_tricycle_steering_controller.cpp b/tricycle_steering_controller/test/test_load_tricycle_steering_controller.cpp new file mode 100644 index 0000000000..d56a809f10 --- /dev/null +++ b/tricycle_steering_controller/test/test_load_tricycle_steering_controller.cpp @@ -0,0 +1,43 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// Copyright (c) 2023, 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(TestLoadTricycleSteeringController, 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_tricycle_steering_controller", + "tricycle_steering_controller/TricycleSteeringController")); + + rclcpp::shutdown(); +} diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp new file mode 100644 index 0000000000..91c8591bc1 --- /dev/null +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp @@ -0,0 +1,388 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// Copyright (c) 2023, 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_tricycle_steering_controller.hpp" + +#include +#include +#include +#include +#include + +using tricycle_steering_controller::CMD_STEER; +using tricycle_steering_controller::control_mode_type; +using tricycle_steering_controller::STATE_DRIVE_RIGHT_WHEEL; + +class TricycleSteeringControllerTest +: public TricycleSteeringControllerFixture +{ +}; + +TEST_F(TricycleSteeringControllerTest, 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(TricycleSteeringControllerTest, 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(TricycleSteeringControllerTest, 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(TricycleSteeringControllerTest, 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(TricycleSteeringControllerTest, 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(TricycleSteeringControllerTest, 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_STEER].get_value(), 101.101); + ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(std::isnan(controller_->command_interfaces_[CMD_STEER].get_value())); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(std::isnan(controller_->command_interfaces_[CMD_STEER].get_value())); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); +} + +TEST_F(TricycleSteeringControllerTest, 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(TricycleSteeringControllerTest, 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_DRIVE_RIGHT_WHEEL], 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(TricycleSteeringControllerTest, 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_DRIVE_RIGHT_WHEEL], 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(TricycleSteeringControllerTest, 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_DRIVE_RIGHT_WHEEL] = 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_DRIVE_RIGHT_WHEEL], 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(TricycleSteeringControllerTest, 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_DRIVE_RIGHT_WHEEL] = 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_DRIVE_RIGHT_WHEEL], 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(TricycleSteeringControllerTest, 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(TricycleSteeringControllerTest, 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_STEER], 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/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp new file mode 100644 index 0000000000..191dddcc14 --- /dev/null +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp @@ -0,0 +1,282 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// Copyright (c) 2023, 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 TEST_TRICYCLE_STEERING_CONTROLLER_HPP_ +#define TEST_TRICYCLE_STEERING_CONTROLLER_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#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" +#include "tricycle_steering_controller/tricycle_steering_controller.hpp" + +// TODO(anyone): replace the state and command message types +using ControllerStateMsg = + steering_controllers_library::SteeringControllersLibrary::ControllerStateMsg; +using ControllerReferenceMsg = + steering_controllers_library::SteeringControllersLibrary::ControllerReferenceMsg; +using ControllerModeSrvType = + steering_controllers_library::SteeringControllersLibrary::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 TestableTricycleSteeringController +: public tricycle_steering_controller::TricycleSteeringController +{ + FRIEND_TEST(TricycleSteeringControllerTest, all_parameters_set_configure_success); + FRIEND_TEST(TricycleSteeringControllerTest, activate_success); + FRIEND_TEST(TricycleSteeringControllerTest, reactivate_success); + FRIEND_TEST(TricycleSteeringControllerTest, test_setting_slow_mode_service); + FRIEND_TEST(TricycleSteeringControllerTest, test_update_logic_fast); + FRIEND_TEST(TricycleSteeringControllerTest, test_update_logic_slow); + FRIEND_TEST(TricycleSteeringControllerTest, test_update_logic_chainable_fast); + FRIEND_TEST(TricycleSteeringControllerTest, test_update_logic_chainable_slow); + +public: + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override + { + auto ret = + tricycle_steering_controller::TricycleSteeringController::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 tricycle_steering_controller::TricycleSteeringController::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 TricycleSteeringControllerFixture : 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_tricycle_steering_controller/commands", rclcpp::SystemDefaultsQoS()); + + service_caller_node_ = std::make_shared("service_caller"); + slow_control_service_client_ = service_caller_node_->create_client( + "/test_tricycle_steering_controller/set_slow_control_mode"); + } + + static void TearDownTestCase() {} + + void TearDown() { controller_.reset(nullptr); } + +protected: + void SetUpController(const std::string controller_name = "test_tricycle_steering_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_tricycle_steering_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 // TEST_TRICYCLE_STEERING_CONTROLLER_HPP_ diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller_preceeding.cpp b/tricycle_steering_controller/test/test_tricycle_steering_controller_preceeding.cpp new file mode 100644 index 0000000000..20f254fa2e --- /dev/null +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller_preceeding.cpp @@ -0,0 +1,90 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// Copyright (c) 2023, 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_tricycle_steering_controller.hpp" + +#include +#include +#include +#include +#include + +using tricycle_steering_controller::CMD_MY_ITFS; +using tricycle_steering_controller::control_mode_type; +using tricycle_steering_controller::STATE_MY_ITFS; + +class TricycleSteeringControllerTest +: public TricycleSteeringControllerFixture +{ +}; + +TEST_F(TricycleSteeringControllerTest, 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(TricycleSteeringControllerTest, 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/tricycle_steering_controller/test/tricycle_steering_controller_params.yaml b/tricycle_steering_controller/test/tricycle_steering_controller_params.yaml new file mode 100644 index 0000000000..b82a408311 --- /dev/null +++ b/tricycle_steering_controller/test/tricycle_steering_controller_params.yaml @@ -0,0 +1,7 @@ +test_tricycle_steering_controller: + ros__parameters: + + joints: + - joint1 + + interface_name: acceleration diff --git a/tricycle_steering_controller/test/tricycle_steering_controller_preceeding_params.yaml b/tricycle_steering_controller/test/tricycle_steering_controller_preceeding_params.yaml new file mode 100644 index 0000000000..37c7cac55e --- /dev/null +++ b/tricycle_steering_controller/test/tricycle_steering_controller_preceeding_params.yaml @@ -0,0 +1,9 @@ +test_tricycle_steering_controller: + ros__parameters: + joints: + - joint1 + + interface_name: acceleration + + state_joints: + - joint1state diff --git a/tricycle_steering_controller/tricycle_steering_controller.xml b/tricycle_steering_controller/tricycle_steering_controller.xml new file mode 100644 index 0000000000..8b8d9fe3cc --- /dev/null +++ b/tricycle_steering_controller/tricycle_steering_controller.xml @@ -0,0 +1,8 @@ + + + + TricycleSteeringController ros2_control controller. + + + From 82f096dbd68d984594b91a739145d868095abd1a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Tue, 3 Jan 2023 19:53:34 +0100 Subject: [PATCH 108/186] Updated ackermann controller stuff. --- .../ackermann_steering_controller.hpp | 2 +- .../include/ackermann_steering_controller/visibility_control.h | 1 - .../test/test_ackermann_steering_controller.cpp | 1 - .../test/test_ackermann_steering_controller.hpp | 1 - .../test/test_ackermann_steering_controller_preceeding.cpp | 1 - .../test/test_load_ackermann_steering_controller.cpp | 1 - 6 files changed, 1 insertion(+), 6 deletions(-) diff --git a/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp b/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp index 6483737092..19b097d48c 100644 --- a/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp +++ b/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp @@ -30,7 +30,7 @@ namespace ackermann_steering_controller static constexpr size_t STATE_DRIVE_RIGHT_WHEEL = 0; static constexpr size_t STATE_DRIVE_LEFT_WHEEL = 1; static constexpr size_t STATE_STEER_RIGHT_WHEEL = 2; -static constexpr size_t STATE_STEER_LEFT_WHEEL = 4; +static constexpr size_t STATE_STEER_LEFT_WHEEL = 3; // name constants for command interfaces static constexpr size_t CMD_DRIVE_WHEEL = 0; diff --git a/ackermann_steering_controller/include/ackermann_steering_controller/visibility_control.h b/ackermann_steering_controller/include/ackermann_steering_controller/visibility_control.h index f2e83bf985..177f0bf87c 100644 --- a/ackermann_steering_controller/include/ackermann_steering_controller/visibility_control.h +++ b/ackermann_steering_controller/include/ackermann_steering_controller/visibility_control.h @@ -1,5 +1,4 @@ // Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) -// Copyright (c) 2023, 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. diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp index adc767e1eb..916f9fca2f 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp @@ -1,5 +1,4 @@ // Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) -// Copyright (c) 2023, 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. diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp index 0ab7ad518c..fcd82a4d1b 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp @@ -1,5 +1,4 @@ // Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) -// Copyright (c) 2023, 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. diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp b/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp index b2df36f2d7..aaab85cf31 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp @@ -1,5 +1,4 @@ // Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) -// Copyright (c) 2023, 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. diff --git a/ackermann_steering_controller/test/test_load_ackermann_steering_controller.cpp b/ackermann_steering_controller/test/test_load_ackermann_steering_controller.cpp index 7532cd2cba..aee335cb4e 100644 --- a/ackermann_steering_controller/test/test_load_ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/test/test_load_ackermann_steering_controller.cpp @@ -1,5 +1,4 @@ // Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) -// Copyright (c) 2023, 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. From 662555166b958aba617ab9837db983c819ba76a0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Tue, 3 Jan 2023 19:53:52 +0100 Subject: [PATCH 109/186] Small fixes on bicycl eand library. --- bicycle_steering_controller/CMakeLists.txt | 1 - steering_controllers_library/CMakeLists.txt | 12 ------------ 2 files changed, 13 deletions(-) diff --git a/bicycle_steering_controller/CMakeLists.txt b/bicycle_steering_controller/CMakeLists.txt index 98c5045f09..81987bec30 100644 --- a/bicycle_steering_controller/CMakeLists.txt +++ b/bicycle_steering_controller/CMakeLists.txt @@ -7,7 +7,6 @@ endif() # find dependencies set(THIS_PACKAGE_INCLUDE_DEPENDS - control_msgs controller_interface hardware_interface pluginlib diff --git a/steering_controllers_library/CMakeLists.txt b/steering_controllers_library/CMakeLists.txt index 8df39f7092..50d61edbe1 100644 --- a/steering_controllers_library/CMakeLists.txt +++ b/steering_controllers_library/CMakeLists.txt @@ -35,23 +35,11 @@ add_library( SHARED src/steering_controllers_library.cpp src/steering_odometry.cpp - #src/ackermann_steering_controller.cpp - #src/bicycle_steering_controller.cpp - #src/tricycle_steering_controller.cpp ) generate_parameter_library(steering_controllers_library_parameters config/steering_controllers_library.yaml ) -#generate_parameter_library(bicycle_controller_parameters - #config/bicycle_steering_controller.yaml -#) -#generate_parameter_library(tricycle_controller_parameters - #config/tricycle_steering_controller.yaml -#) -#generate_parameter_library(ackermann_controller_parameters - #config/ackermann_steering_controller.yaml -#) target_include_directories(${PROJECT_NAME} PUBLIC "$" From 04a18d88bd425dd7cb9534dc8ed24b69bd2dfc20 Mon Sep 17 00:00:00 2001 From: petkovich Date: Wed, 4 Jan 2023 12:50:09 +0100 Subject: [PATCH 110/186] tests build --- bicycle_steering_controller/CMakeLists.txt | 20 +- .../test/test_bicycle_steering_controller.cpp | 685 +++++++++--------- .../test/test_bicycle_steering_controller.hpp | 190 ++--- .../steering_controllers_library.hpp | 2 +- 4 files changed, 448 insertions(+), 449 deletions(-) diff --git a/bicycle_steering_controller/CMakeLists.txt b/bicycle_steering_controller/CMakeLists.txt index 81987bec30..73232b25d7 100644 --- a/bicycle_steering_controller/CMakeLists.txt +++ b/bicycle_steering_controller/CMakeLists.txt @@ -82,16 +82,16 @@ if(BUILD_TESTING) hardware_interface ) - add_rostest_with_parameters_gmock( - test_bicycle_steering_controller_preceeding test/test_bicycle_steering_controller_preceeding.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/test/bicycle_steering_controller_preceeding_params.yaml) - target_include_directories(test_bicycle_steering_controller_preceeding PRIVATE include) - target_link_libraries(test_bicycle_steering_controller_preceeding bicycle_steering_controller) - ament_target_dependencies( - test_bicycle_steering_controller_preceeding - controller_interface - hardware_interface - ) + # add_rostest_with_parameters_gmock( + # test_bicycle_steering_controller_preceeding test/test_bicycle_steering_controller_preceeding.cpp + # ${CMAKE_CURRENT_SOURCE_DIR}/test/bicycle_steering_controller_preceeding_params.yaml) + # target_include_directories(test_bicycle_steering_controller_preceeding PRIVATE include) + # target_link_libraries(test_bicycle_steering_controller_preceeding bicycle_steering_controller) + # ament_target_dependencies( + # test_bicycle_steering_controller_preceeding + # controller_interface + # hardware_interface + # ) endif() ament_export_include_directories( diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp index 28c306713f..7a47487ad1 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp @@ -20,9 +20,9 @@ #include #include -using bicycle_steering_controller::CMD_MY_ITFS; -using bicycle_steering_controller::control_mode_type; -using bicycle_steering_controller::STATE_MY_ITFS; +// using bicycle_steering_controller::CMD_MY_ITFS; +// using bicycle_steering_controller::control_mode_type; +// using bicycle_steering_controller::STATE_MY_ITFS; class BicycleSteeringControllerTest : public BicycleSteeringControllerFixture @@ -32,350 +32,349 @@ class BicycleSteeringControllerTest TEST_F(BicycleSteeringControllerTest, 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_.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_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(BicycleSteeringControllerTest, 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(BicycleSteeringControllerTest, 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(BicycleSteeringControllerTest, 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(BicycleSteeringControllerTest, 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(BicycleSteeringControllerTest, 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(BicycleSteeringControllerTest, 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); + // 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(BicycleSteeringControllerTest, 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(BicycleSteeringControllerTest, 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(BicycleSteeringControllerTest, 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(BicycleSteeringControllerTest, 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(BicycleSteeringControllerTest, 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(BicycleSteeringControllerTest, 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); -} +// TEST_F(BicycleSteeringControllerTest, 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(BicycleSteeringControllerTest, 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(BicycleSteeringControllerTest, 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(BicycleSteeringControllerTest, 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(BicycleSteeringControllerTest, 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(BicycleSteeringControllerTest, 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(BicycleSteeringControllerTest, 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(BicycleSteeringControllerTest, 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(BicycleSteeringControllerTest, 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(BicycleSteeringControllerTest, 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(BicycleSteeringControllerTest, 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(BicycleSteeringControllerTest, 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) { diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp index 3ee49eb6cf..ab5bc762d1 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp @@ -35,11 +35,11 @@ // TODO(anyone): replace the state and command message types using ControllerStateMsg = - steering_controllers_library::SteeringControllersLibrary::ControllerStateMsg; + steering_controllers_library::SteeringControllersLibrary::AckermanControllerState; using ControllerReferenceMsg = - steering_controllers_library::SteeringControllersLibrary::ControllerReferenceMsg; -using ControllerModeSrvType = - steering_controllers_library::SteeringControllersLibrary::ControllerModeSrvType; + steering_controllers_library::SteeringControllersLibrary::ControllerTwistReferenceMsg; +// using ControllerModeSrvType = +// steering_controllers_library::SteeringControllersLibrary::ControllerModeSrvType; namespace { @@ -69,7 +69,7 @@ class TestableBicycleSteeringController // Only if on_configure is successful create subscription if (ret == CallbackReturn::SUCCESS) { - ref_subscriber_wait_set_.add_subscription(ref_subscriber_); + ref_subscriber_wait_set_.add_subscription(ref_subscriber_twist_); } return ret; } @@ -129,9 +129,9 @@ class BicycleSteeringControllerFixture : public ::testing::Test command_publisher_ = command_publisher_node_->create_publisher( "/test_bicycle_steering_controller/commands", rclcpp::SystemDefaultsQoS()); - service_caller_node_ = std::make_shared("service_caller"); - slow_control_service_client_ = service_caller_node_->create_client( - "/test_bicycle_steering_controller/set_slow_control_mode"); + // service_caller_node_ = std::make_shared("service_caller"); + // slow_control_service_client_ = service_caller_node_->create_client( + // "/test_bicycle_steering_controller/set_slow_control_mode"); } static void TearDownTestCase() {} @@ -170,91 +170,91 @@ class BicycleSteeringControllerFixture : public ::testing::Test 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_bicycle_steering_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(); - } + // 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_bicycle_steering_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 @@ -273,8 +273,8 @@ class BicycleSteeringControllerFixture : public ::testing::Test 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_; + // rclcpp::Node::SharedPtr service_caller_node_; + // rclcpp::Client::SharedPtr slow_control_service_client_; }; #endif // TEST_BICYCLE_STEERING_CONTROLLER_HPP_ diff --git a/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp b/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp index 040a9abcbd..e8485aec1a 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp @@ -85,6 +85,7 @@ class SteeringControllersLibrary : public controller_interface::ChainableControl using ControllerTwistReferenceMsg = geometry_msgs::msg::TwistStamped; using ControllerStateMsgOdom = nav_msgs::msg::Odometry; using ControllerStateMsgTf = tf2_msgs::msg::TFMessage; + using AckermanControllerState = control_msgs::msg::SteeringControllerStatus; protected: controller_interface::CallbackReturn set_interface_numbers( @@ -117,7 +118,6 @@ class SteeringControllersLibrary : public controller_interface::ChainableControl /// Odometry: steering_odometry::SteeringOdometry odometry_; - using AckermanControllerState = control_msgs::msg::SteeringControllerStatus; AckermanControllerState published_state_; using ControllerStatePublisher = realtime_tools::RealtimePublisher; From 3a81c9b14b9f7280ebe619dec3343ca4205f38c8 Mon Sep 17 00:00:00 2001 From: petkovich Date: Thu, 5 Jan 2023 16:33:28 +0100 Subject: [PATCH 111/186] on cofigure test --- .../test/bicycle_steering_controller_params.yaml | 14 +++++++++++--- .../test/test_bicycle_steering_controller.cpp | 5 +++-- 2 files changed, 14 insertions(+), 5 deletions(-) diff --git a/bicycle_steering_controller/test/bicycle_steering_controller_params.yaml b/bicycle_steering_controller/test/bicycle_steering_controller_params.yaml index c4f5e3e105..33ed41a319 100644 --- a/bicycle_steering_controller/test/bicycle_steering_controller_params.yaml +++ b/bicycle_steering_controller/test/bicycle_steering_controller_params.yaml @@ -1,7 +1,15 @@ test_bicycle_steering_controller: ros__parameters: - joints: - - joint1 + reference_timeout: 2.0 + front_steering: true + open_loop: false + velocity_rolling_window_size: 10 + position_feedback: false + use_stamped_vel: true + rear_wheels_names: [rear_right_wheel_joint, rear_left_wheel_joint] + front_wheels_names: [steering_axis_joint] - interface_name: acceleration + wheelbase: 3.24644 + front_wheel_radius: 0.45 + rear_wheels_radius: 0.45 diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp index 7a47487ad1..d987820a3f 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp @@ -32,11 +32,12 @@ class BicycleSteeringControllerTest TEST_F(BicycleSteeringControllerTest, all_parameters_set_configure_success) { SetUpController(); - // ASSERT_TRUE(controller_->params_.joints.empty()); + ASSERT_FALSE(controller_->params_.rear_wheels_names.empty()); + ASSERT_FALSE(controller_->params_.front_wheels_names.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_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()); From c96a83b6162b1ef51a28e9c17ec238b79b538dfb Mon Sep 17 00:00:00 2001 From: petkovich Date: Thu, 5 Jan 2023 18:06:17 +0100 Subject: [PATCH 112/186] first test finished --- .../bicycle_steering_controller_params.yaml | 2 +- .../test/test_bicycle_steering_controller.cpp | 16 +++-- .../test/test_bicycle_steering_controller.hpp | 68 ++++++++++++------- 3 files changed, 54 insertions(+), 32 deletions(-) diff --git a/bicycle_steering_controller/test/bicycle_steering_controller_params.yaml b/bicycle_steering_controller/test/bicycle_steering_controller_params.yaml index 33ed41a319..364626f6ee 100644 --- a/bicycle_steering_controller/test/bicycle_steering_controller_params.yaml +++ b/bicycle_steering_controller/test/bicycle_steering_controller_params.yaml @@ -7,7 +7,7 @@ test_bicycle_steering_controller: velocity_rolling_window_size: 10 position_feedback: false use_stamped_vel: true - rear_wheels_names: [rear_right_wheel_joint, rear_left_wheel_joint] + rear_wheels_names: [rear_wheel_joint] front_wheels_names: [steering_axis_joint] wheelbase: 3.24644 diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp index d987820a3f..b9def2b65d 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp @@ -32,17 +32,21 @@ class BicycleSteeringControllerTest TEST_F(BicycleSteeringControllerTest, all_parameters_set_configure_success) { SetUpController(); - ASSERT_FALSE(controller_->params_.rear_wheels_names.empty()); - ASSERT_FALSE(controller_->params_.front_wheels_names.empty()); + // ASSERT_FALSE(controller_->params_.rear_wheels_names.empty()); + // ASSERT_FALSE(controller_->params_.front_wheels_names.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_); + ASSERT_THAT( + controller_->params_.rear_wheels_names, testing::ElementsAreArray(rear_wheels_names_)); + ASSERT_THAT( + controller_->params_.front_wheels_names, testing::ElementsAreArray(front_wheels_names_)); + ASSERT_EQ(controller_->params_.front_steering, front_steering_); + ASSERT_EQ(controller_->params_.open_loop, open_loop_); + ASSERT_EQ(controller_->params_.velocity_rolling_window_size, velocity_rolling_window_size_); + ASSERT_EQ(controller_->params_.position_feedback, position_feedback_); } // TEST_F(BicycleSteeringControllerTest, check_exported_intefaces) diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp index ab5bc762d1..1383f53514 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp @@ -143,28 +143,39 @@ class BicycleSteeringControllerFixture : public ::testing::Test { ASSERT_EQ(controller_->init(controller_name), controller_interface::return_type::OK); + if (position_feedback_ == true) + { + traction_interface_name_ = "position"; + } + else + { + traction_interface_name_ = "velocity"; + } + 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()); - } + command_itfs_.emplace_back(hardware_interface::CommandInterface( + rear_wheels_names_[0], traction_interface_name_, &joint_command_values_[0])); + command_ifs.emplace_back(command_itfs_.back()); + + command_itfs_.emplace_back(hardware_interface::CommandInterface( + front_wheels_names_[0], steering_interface_name_, &joint_command_values_[1])); + 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()); - } + state_itfs_.emplace_back(hardware_interface::StateInterface( + rear_wheels_names_[0], traction_interface_name_, &joint_state_values_[0])); + state_ifs.emplace_back(state_itfs_.back()); + + state_itfs_.emplace_back(hardware_interface::StateInterface( + front_wheels_names_[0], steering_interface_name_, &joint_state_values_[1])); + 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)); @@ -175,13 +186,7 @@ class BicycleSteeringControllerFixture : public ::testing::Test // // 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_bicycle_steering_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); + // auto subscription = test_subscription_node.creinterface_name_ // // call update to publish the test value // // since update doesn't guarantee a published message, republish until received @@ -191,7 +196,7 @@ class BicycleSteeringControllerFixture : public ::testing::Test // while (max_sub_check_loop_count--) // { // controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); - // // check if message has been received + // // check if messageparams_ has been received // if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) // { // break; @@ -260,11 +265,24 @@ class BicycleSteeringControllerFixture : public ::testing::Test // 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}; + + double reference_timeout_ = 2.0; + bool front_steering_ = true; + bool open_loop_ = false; + unsigned int velocity_rolling_window_size_ = 10; + bool position_feedback_ = false; + bool use_stamped_vel_ = true; + std::vector rear_wheels_names_ = {"rear_wheel_joint"}; + std::vector front_wheels_names_ = {"steering_axis_joint"}; + double wheelbase_ = 3.24644; + double front_wheel_radius_ = 0.45; + double rear_wheels_radius_ = 0.45; + + std::array joint_state_values_ = {1.1, 2.0}; + std::array joint_command_values_ = {2.1, 101.101}; + std::string steering_interface_name_ = "position"; + + std::string traction_interface_name_ = ""; std::vector state_itfs_; std::vector command_itfs_; From d645557482593e1aec482c489143e79032ef17d9 Mon Sep 17 00:00:00 2001 From: petkovich Date: Sat, 7 Jan 2023 19:34:10 +0100 Subject: [PATCH 113/186] exported intefraces --- .../test/test_bicycle_steering_controller.cpp | 57 +++++++++---------- .../test/test_bicycle_steering_controller.hpp | 3 +- 2 files changed, 28 insertions(+), 32 deletions(-) diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp index b9def2b65d..7c73d6c511 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp @@ -49,39 +49,34 @@ TEST_F(BicycleSteeringControllerTest, all_parameters_set_configure_success) ASSERT_EQ(controller_->params_.position_feedback, position_feedback_); } -// TEST_F(BicycleSteeringControllerTest, 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_); -// } +TEST_F(BicycleSteeringControllerTest, check_exported_intefaces) +{ + SetUpController(); -// 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_); -// } + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); -// // 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_); -// } -// } + auto command_intefaces = controller_->command_interface_configuration(); + ASSERT_EQ(command_intefaces.names.size(), joint_command_values_.size()); + EXPECT_EQ(command_intefaces.names[0], rear_wheels_names_[0] + "/" + traction_interface_name_); + EXPECT_EQ(command_intefaces.names[1], front_wheels_names_[0] + "/" + steering_interface_name_); + + auto state_intefaces = controller_->state_interface_configuration(); + ASSERT_EQ(state_intefaces.names.size(), joint_state_values_.size()); + EXPECT_EQ(state_intefaces.names[0], rear_wheels_names_[0] + "/" + traction_interface_name_); + EXPECT_EQ(state_intefaces.names[1], front_wheels_names_[0] + "/" + steering_interface_name_); + + // check ref itfs + auto reference_interfaces = controller_->export_reference_interfaces(); + ASSERT_EQ(reference_interfaces.size(), joint_reference_interfaces_.size()); + for (size_t i = 0; i < joint_reference_interfaces_.size(); ++i) + { + const std::string ref_itf_name = + std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i]; + 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_reference_interfaces_[i]); + } +} // TEST_F(BicycleSteeringControllerTest, activate_success) // { diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp index 1383f53514..cb8e03539c 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp @@ -280,8 +280,9 @@ class BicycleSteeringControllerFixture : public ::testing::Test std::array joint_state_values_ = {1.1, 2.0}; std::array joint_command_values_ = {2.1, 101.101}; + std::array joint_reference_interfaces_ = {"linear/velocity", "angular/position"}; std::string steering_interface_name_ = "position"; - + // defined in setup std::string traction_interface_name_ = ""; std::vector state_itfs_; From 73edcceee36d62c1d59bbb6b92fde3793e3e17ef Mon Sep 17 00:00:00 2001 From: petkovich Date: Sat, 7 Jan 2023 19:43:25 +0100 Subject: [PATCH 114/186] on activate --- .../test/test_bicycle_steering_controller.cpp | 42 +++++++------------ .../test/test_bicycle_steering_controller.hpp | 2 + 2 files changed, 17 insertions(+), 27 deletions(-) diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp index 7c73d6c511..84c02eb084 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp @@ -78,34 +78,22 @@ TEST_F(BicycleSteeringControllerTest, check_exported_intefaces) } } -// TEST_F(BicycleSteeringControllerTest, activate_success) -// { -// SetUpController(); - -// ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); -// ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - -// // check that the message is reset -// auto msg = controller_->input_ref_.readFromNonRT(); -// EXPECT_EQ((*msg)->displacements.size(), joint_names_.size()); -// for (const auto & cmd : (*msg)->displacements) -// { -// EXPECT_TRUE(std::isnan(cmd)); -// } -// EXPECT_EQ((*msg)->velocities.size(), joint_names_.size()); -// for (const auto & cmd : (*msg)->velocities) -// { -// EXPECT_TRUE(std::isnan(cmd)); -// } - -// ASSERT_TRUE(std::isnan((*msg)->duration)); +TEST_F(BicycleSteeringControllerTest, activate_success) +{ + SetUpController(); -// EXPECT_EQ(controller_->reference_interfaces_.size(), joint_names_.size()); -// for (const auto & interface : controller_->reference_interfaces_) -// { -// EXPECT_TRUE(std::isnan(interface)); -// } -// } + 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_TRUE(std::isnan((*msg)->twist.linear.x)); + EXPECT_TRUE(std::isnan((*msg)->twist.linear.y)); + EXPECT_TRUE(std::isnan((*msg)->twist.linear.z)); + EXPECT_TRUE(std::isnan((*msg)->twist.angular.x)); + EXPECT_TRUE(std::isnan((*msg)->twist.angular.y)); + EXPECT_TRUE(std::isnan((*msg)->twist.angular.z)); +} // TEST_F(BicycleSteeringControllerTest, update_success) // { diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp index cb8e03539c..1bc6a95769 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp @@ -274,6 +274,8 @@ class BicycleSteeringControllerFixture : public ::testing::Test bool use_stamped_vel_ = true; std::vector rear_wheels_names_ = {"rear_wheel_joint"}; std::vector front_wheels_names_ = {"steering_axis_joint"}; + std::vector joint_names_ = {rear_wheels_names_[0], front_wheels_names_[0]}; + double wheelbase_ = 3.24644; double front_wheel_radius_ = 0.45; double rear_wheels_radius_ = 0.45; From b723724d0548cf3f67ddc16ebf89e39112e5ea45 Mon Sep 17 00:00:00 2001 From: petkovich Date: Sat, 7 Jan 2023 19:55:32 +0100 Subject: [PATCH 115/186] on activate --- .../test/test_bicycle_steering_controller.cpp | 122 +++++++++--------- .../test/test_bicycle_steering_controller.hpp | 4 +- 2 files changed, 59 insertions(+), 67 deletions(-) diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp index 84c02eb084..8c069db8fe 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp @@ -95,43 +95,42 @@ TEST_F(BicycleSteeringControllerTest, activate_success) EXPECT_TRUE(std::isnan((*msg)->twist.angular.z)); } -// TEST_F(BicycleSteeringControllerTest, update_success) -// { -// SetUpController(); +TEST_F(BicycleSteeringControllerTest, 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_->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); -// } + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); +} -// TEST_F(BicycleSteeringControllerTest, deactivate_success) -// { -// SetUpController(); +TEST_F(BicycleSteeringControllerTest, 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); -// } + 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(BicycleSteeringControllerTest, reactivate_success) -// { -// SetUpController(); +TEST_F(BicycleSteeringControllerTest, 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_->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); + ASSERT_TRUE(std::isnan(controller_->command_interfaces_[0].get_value())); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(std::isnan(controller_->command_interfaces_[0].get_value())); -// ASSERT_EQ( -// controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), -// controller_interface::return_type::OK); -// } + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); +} // TEST_F(BicycleSteeringControllerTest, test_setting_slow_mode_service) // { @@ -159,41 +158,36 @@ TEST_F(BicycleSteeringControllerTest, activate_success) // ASSERT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::FAST); // } -// TEST_F(BicycleSteeringControllerTest, 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); +TEST_F(BicycleSteeringControllerTest, test_update_logic) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); -// 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)); -// } -// } + 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_COMMAND = 2.1; + std::shared_ptr msg = std::make_shared(); + msg->twist.linear.x = 0.1; + msg->twist.angular.z = 0.0; + 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(joint_command_values_[0], TEST_COMMAND); + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x)); + EXPECT_EQ(controller_->reference_interfaces_.size(), joint_names_.size()); + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } +} // TEST_F(BicycleSteeringControllerTest, test_update_logic_slow) // { diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp index 1bc6a95769..0b2aee499d 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp @@ -56,7 +56,7 @@ class TestableBicycleSteeringController FRIEND_TEST(BicycleSteeringControllerTest, activate_success); FRIEND_TEST(BicycleSteeringControllerTest, reactivate_success); FRIEND_TEST(BicycleSteeringControllerTest, test_setting_slow_mode_service); - FRIEND_TEST(BicycleSteeringControllerTest, test_update_logic_fast); + FRIEND_TEST(BicycleSteeringControllerTest, test_update_logic); FRIEND_TEST(BicycleSteeringControllerTest, test_update_logic_slow); FRIEND_TEST(BicycleSteeringControllerTest, test_update_logic_chainable_fast); FRIEND_TEST(BicycleSteeringControllerTest, test_update_logic_chainable_slow); @@ -163,7 +163,6 @@ class BicycleSteeringControllerFixture : public ::testing::Test command_itfs_.emplace_back(hardware_interface::CommandInterface( front_wheels_names_[0], steering_interface_name_, &joint_command_values_[1])); 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()); @@ -176,7 +175,6 @@ class BicycleSteeringControllerFixture : public ::testing::Test state_itfs_.emplace_back(hardware_interface::StateInterface( front_wheels_names_[0], steering_interface_name_, &joint_state_values_[1])); 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)); } From 32db0c76e2ec330de90b2c99d3814a5a04e94411 Mon Sep 17 00:00:00 2001 From: petkovich Date: Sat, 7 Jan 2023 20:13:38 +0100 Subject: [PATCH 116/186] publish_commands --- .../test/test_bicycle_steering_controller.cpp | 24 ++++---- .../test/test_bicycle_steering_controller.hpp | 55 ++++++++++--------- 2 files changed, 39 insertions(+), 40 deletions(-) diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp index 8c069db8fe..bbb0a2a846 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp @@ -309,22 +309,20 @@ TEST_F(BicycleSteeringControllerTest, test_update_logic) // } // } -// TEST_F(BicycleSteeringControllerTest, publish_status_success) -// { -// SetUpController(); - -// ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); -// ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); +TEST_F(BicycleSteeringControllerTest, publish_status_success) +{ + SetUpController(); -// ASSERT_EQ( -// controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), -// controller_interface::return_type::OK); + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); -// ControllerStateMsg msg; -// subscribe_and_get_messages(msg); + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); -// ASSERT_EQ(msg.set_point, 101.101); -// } + ControllerStateMsg msg; + subscribe_and_get_messages(msg); +} // TEST_F(BicycleSteeringControllerTest, receive_message_and_publish_updated_status) // { diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp index 0b2aee499d..a3475dd7b0 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp @@ -179,34 +179,35 @@ class BicycleSteeringControllerFixture : public ::testing::Test 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.creinterface_name_ - - // // 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 messageparams_ 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"; + 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_bicycle_steering_controller/controller_state", 10, subs_callback); + + // 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 max_sub_check_loop_count + wait_set.add_subscription(subscription); + while (max_sub_check_loop_count--) + { + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + // check if messageparams_ 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)); - // } + // 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( From 467a4a58845fb42d065dec50fabf2f952247f26c Mon Sep 17 00:00:00 2001 From: petkovich Date: Mon, 9 Jan 2023 16:40:16 +0100 Subject: [PATCH 117/186] publish_commands --- .../test/test_bicycle_steering_controller.cpp | 53 +++++++++-------- .../test/test_bicycle_steering_controller.hpp | 59 +++++++++---------- 2 files changed, 55 insertions(+), 57 deletions(-) diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp index bbb0a2a846..97104405c3 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp @@ -170,17 +170,17 @@ TEST_F(BicycleSteeringControllerTest, test_update_logic) ASSERT_FALSE(controller_->is_in_chained_mode()); // set command statically - static constexpr double TEST_COMMAND = 2.1; std::shared_ptr msg = std::make_shared(); msg->twist.linear.x = 0.1; - msg->twist.angular.z = 0.0; + msg->twist.angular.z = 0.2; 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(joint_command_values_[0], TEST_COMMAND); + EXPECT_EQ(command_itfs_[0], 1.0); + EXPECT_EQ(command_itfs_[1], 1.0); EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x)); EXPECT_EQ(controller_->reference_interfaces_.size(), joint_names_.size()); for (const auto & interface : controller_->reference_interfaces_) @@ -324,37 +324,40 @@ TEST_F(BicycleSteeringControllerTest, publish_status_success) subscribe_and_get_messages(msg); } -// TEST_F(BicycleSteeringControllerTest, receive_message_and_publish_updated_status) -// { -// SetUpController(); -// rclcpp::executors::MultiThreadedExecutor executor; -// executor.add_node(controller_->get_node()->get_node_base_interface()); +TEST_F(BicycleSteeringControllerTest, 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_->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); + 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); + ControllerStateMsg msg; + subscribe_and_get_messages(msg); -// ASSERT_EQ(msg.set_point, 101.101); + EXPECT_EQ(msg.linear_velocity_command.data[0], 1.1); + EXPECT_EQ(msg.steering_angle_command.data[0], 2.2); -// publish_commands(); -// ASSERT_TRUE(controller_->wait_for_commands(executor)); + // 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); + 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); + EXPECT_EQ(command_itfs_[0], 1.0); + EXPECT_EQ(command_itfs_[1], 1.0); -// subscribe_and_get_messages(msg); + subscribe_and_get_messages(msg); -// ASSERT_EQ(msg.set_point, 0.45); -// } + EXPECT_EQ(msg.linear_velocity_command.data[0], 1.1); + EXPECT_EQ(msg.steering_angle_command.data[0], 2.2); +} int main(int argc, char ** argv) { diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp index a3475dd7b0..cced54915b 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp @@ -209,37 +209,32 @@ class BicycleSteeringControllerFixture : public ::testing::Test 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); - // } + void publish_commands(const double linear = 0.5, const double angular = 0.1) + { + 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.twist.linear.x = linear; + msg.twist.angular.z = angular; + + command_publisher_->publish(msg); + } // std::shared_ptr call_service( // const bool slow_control, rclcpp::Executor & executor) @@ -280,7 +275,7 @@ class BicycleSteeringControllerFixture : public ::testing::Test double rear_wheels_radius_ = 0.45; std::array joint_state_values_ = {1.1, 2.0}; - std::array joint_command_values_ = {2.1, 101.101}; + std::array joint_command_values_ = {1.1, 2.2}; std::array joint_reference_interfaces_ = {"linear/velocity", "angular/position"}; std::string steering_interface_name_ = "position"; // defined in setup From db15472ee603f38dfc860a7f60af8178e69a44a6 Mon Sep 17 00:00:00 2001 From: petkovich Date: Fri, 13 Jan 2023 11:32:37 +0100 Subject: [PATCH 118/186] cleared slow controllers, publishing test subscriber --- .../test/test_bicycle_steering_controller.cpp | 148 +----------------- .../test/test_bicycle_steering_controller.hpp | 10 +- 2 files changed, 2 insertions(+), 156 deletions(-) diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp index 97104405c3..fb773c7706 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp @@ -132,32 +132,6 @@ TEST_F(BicycleSteeringControllerTest, reactivate_success) controller_interface::return_type::OK); } -// TEST_F(BicycleSteeringControllerTest, 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(BicycleSteeringControllerTest, test_update_logic) { SetUpController(); @@ -189,126 +163,6 @@ TEST_F(BicycleSteeringControllerTest, test_update_logic) } } -// TEST_F(BicycleSteeringControllerTest, 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(BicycleSteeringControllerTest, 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(BicycleSteeringControllerTest, 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(BicycleSteeringControllerTest, publish_status_success) { SetUpController(); @@ -343,7 +197,7 @@ TEST_F(BicycleSteeringControllerTest, receive_message_and_publish_updated_status EXPECT_EQ(msg.linear_velocity_command.data[0], 1.1); EXPECT_EQ(msg.steering_angle_command.data[0], 2.2); - // publish_commands(); + publish_commands(); // ASSERT_TRUE(controller_->wait_for_commands(executor)); ASSERT_EQ( diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp index cced54915b..2e1206707c 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp @@ -55,11 +55,8 @@ class TestableBicycleSteeringController FRIEND_TEST(BicycleSteeringControllerTest, all_parameters_set_configure_success); FRIEND_TEST(BicycleSteeringControllerTest, activate_success); FRIEND_TEST(BicycleSteeringControllerTest, reactivate_success); - FRIEND_TEST(BicycleSteeringControllerTest, test_setting_slow_mode_service); FRIEND_TEST(BicycleSteeringControllerTest, test_update_logic); FRIEND_TEST(BicycleSteeringControllerTest, test_update_logic_slow); - FRIEND_TEST(BicycleSteeringControllerTest, test_update_logic_chainable_fast); - FRIEND_TEST(BicycleSteeringControllerTest, test_update_logic_chainable_slow); public: controller_interface::CallbackReturn on_configure( @@ -107,8 +104,6 @@ class TestableBicycleSteeringController 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_; }; @@ -130,8 +125,6 @@ class BicycleSteeringControllerFixture : public ::testing::Test "/test_bicycle_steering_controller/commands", rclcpp::SystemDefaultsQoS()); // service_caller_node_ = std::make_shared("service_caller"); - // slow_control_service_client_ = service_caller_node_->create_client( - // "/test_bicycle_steering_controller/set_slow_control_mode"); } static void TearDownTestCase() {} @@ -227,7 +220,7 @@ class BicycleSteeringControllerFixture : public ::testing::Test } }; - // wait_for_topic(command_publisher_->get_topic_name()); + wait_for_topic(command_publisher_->get_topic_name()); ControllerReferenceMsg msg; msg.twist.linear.x = linear; @@ -289,7 +282,6 @@ class BicycleSteeringControllerFixture : public ::testing::Test 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 // TEST_BICYCLE_STEERING_CONTROLLER_HPP_ From 28ffa17a5ebbddd83e0824a7f46e5acdd118a67e Mon Sep 17 00:00:00 2001 From: petkovich Date: Fri, 13 Jan 2023 11:51:06 +0100 Subject: [PATCH 119/186] command_interfaces_ don't update --- .../test/test_bicycle_steering_controller.cpp | 10 +++++----- .../test/test_bicycle_steering_controller.hpp | 7 +++++-- 2 files changed, 10 insertions(+), 7 deletions(-) diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp index fb773c7706..c08339066d 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp @@ -153,8 +153,8 @@ TEST_F(BicycleSteeringControllerTest, test_update_logic) controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - EXPECT_EQ(command_itfs_[0], 1.0); - EXPECT_EQ(command_itfs_[1], 1.0); + EXPECT_EQ(controller_->command_interfaces_[0].get_value(), 1.1); + EXPECT_EQ(controller_->command_interfaces_[1].get_value(), 2.2); EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x)); EXPECT_EQ(controller_->reference_interfaces_.size(), joint_names_.size()); for (const auto & interface : controller_->reference_interfaces_) @@ -197,15 +197,15 @@ TEST_F(BicycleSteeringControllerTest, receive_message_and_publish_updated_status EXPECT_EQ(msg.linear_velocity_command.data[0], 1.1); EXPECT_EQ(msg.steering_angle_command.data[0], 2.2); - publish_commands(); + // 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(command_itfs_[0], 1.0); - EXPECT_EQ(command_itfs_[1], 1.0); + EXPECT_EQ(controller_->command_interfaces_[0].get_value(), 1.1); + EXPECT_EQ(controller_->command_interfaces_[1].get_value(), 2.2); subscribe_and_get_messages(msg); diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp index 2e1206707c..e756eb3d79 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp @@ -54,9 +54,12 @@ class TestableBicycleSteeringController { FRIEND_TEST(BicycleSteeringControllerTest, all_parameters_set_configure_success); FRIEND_TEST(BicycleSteeringControllerTest, activate_success); + FRIEND_TEST(BicycleSteeringControllerTest, update_success); + FRIEND_TEST(BicycleSteeringControllerTest, deactivate_success); FRIEND_TEST(BicycleSteeringControllerTest, reactivate_success); FRIEND_TEST(BicycleSteeringControllerTest, test_update_logic); - FRIEND_TEST(BicycleSteeringControllerTest, test_update_logic_slow); + FRIEND_TEST(BicycleSteeringControllerTest, publish_status_success); + FRIEND_TEST(BicycleSteeringControllerTest, receive_message_and_publish_updated_status); public: controller_interface::CallbackReturn on_configure( @@ -220,7 +223,7 @@ class BicycleSteeringControllerFixture : public ::testing::Test } }; - wait_for_topic(command_publisher_->get_topic_name()); + // wait_for_topic(command_publisher_->get_topic_name()); ControllerReferenceMsg msg; msg.twist.linear.x = linear; From 067c41de9498473aeab0a6782f6fe90db1494fe3 Mon Sep 17 00:00:00 2001 From: petkovich Date: Fri, 13 Jan 2023 12:27:33 +0100 Subject: [PATCH 120/186] fixed reference update --- .../bicycle_steering_controller.hpp | 3 ++- .../src/bicycle_steering_controller.cpp | 8 +++--- .../bicycle_steering_controller_params.yaml | 2 +- .../test/test_bicycle_steering_controller.cpp | 25 ++++++++++++------- .../test/test_bicycle_steering_controller.hpp | 6 ++--- 5 files changed, 26 insertions(+), 18 deletions(-) diff --git a/bicycle_steering_controller/include/bicycle_steering_controller/bicycle_steering_controller.hpp b/bicycle_steering_controller/include/bicycle_steering_controller/bicycle_steering_controller.hpp index 03de9d9ab0..da86fbcbe2 100644 --- a/bicycle_steering_controller/include/bicycle_steering_controller/bicycle_steering_controller.hpp +++ b/bicycle_steering_controller/include/bicycle_steering_controller/bicycle_steering_controller.hpp @@ -40,8 +40,9 @@ class BicycleSteeringController : public steering_controllers_library::SteeringC BICYCLE_STEERING_CONTROLLER__VISIBILITY_PUBLIC void initialize_implementation_parameter_listener() override; -private: +protected: std::shared_ptr bicycle_param_listener_; + bicycle_steering_controller::Params bicycle_params_; }; } // namespace bicycle_steering_controller diff --git a/bicycle_steering_controller/src/bicycle_steering_controller.cpp b/bicycle_steering_controller/src/bicycle_steering_controller.cpp index 82b778c858..17fe7a0971 100644 --- a/bicycle_steering_controller/src/bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/src/bicycle_steering_controller.cpp @@ -29,11 +29,11 @@ void BicycleSteeringController::initialize_implementation_parameter_listener() controller_interface::CallbackReturn BicycleSteeringController::configure_odometry() { - bicycle_steering_controller::Params bicycle_params = bicycle_param_listener_->get_params(); + bicycle_params_ = bicycle_param_listener_->get_params(); - const double wheelbase = bicycle_params.wheelbase; - const double front_wheel_radius = bicycle_params.front_wheel_radius; - const double rear_wheel_radius = bicycle_params.rear_wheel_radius; + const double wheelbase = bicycle_params_.wheelbase; + const double front_wheel_radius = bicycle_params_.front_wheel_radius; + const double rear_wheel_radius = bicycle_params_.rear_wheel_radius; if (params_.front_steering) { diff --git a/bicycle_steering_controller/test/bicycle_steering_controller_params.yaml b/bicycle_steering_controller/test/bicycle_steering_controller_params.yaml index 364626f6ee..a2a6c6508b 100644 --- a/bicycle_steering_controller/test/bicycle_steering_controller_params.yaml +++ b/bicycle_steering_controller/test/bicycle_steering_controller_params.yaml @@ -12,4 +12,4 @@ test_bicycle_steering_controller: wheelbase: 3.24644 front_wheel_radius: 0.45 - rear_wheels_radius: 0.45 + rear_wheel_radius: 0.45 diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp index c08339066d..0dc5f9b260 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp @@ -47,6 +47,9 @@ TEST_F(BicycleSteeringControllerTest, all_parameters_set_configure_success) ASSERT_EQ(controller_->params_.open_loop, open_loop_); ASSERT_EQ(controller_->params_.velocity_rolling_window_size, velocity_rolling_window_size_); ASSERT_EQ(controller_->params_.position_feedback, position_feedback_); + ASSERT_EQ(controller_->bicycle_params_.wheelbase, wheelbase_); + ASSERT_EQ(controller_->bicycle_params_.front_wheel_radius, front_wheels_radius_); + ASSERT_EQ(controller_->bicycle_params_.rear_wheel_radius, rear_wheels_radius_); } TEST_F(BicycleSteeringControllerTest, check_exported_intefaces) @@ -103,7 +106,7 @@ TEST_F(BicycleSteeringControllerTest, update_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_->update_and_write_commands(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); } @@ -128,7 +131,7 @@ TEST_F(BicycleSteeringControllerTest, reactivate_success) ASSERT_TRUE(std::isnan(controller_->command_interfaces_[0].get_value())); ASSERT_EQ( - controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_->update_and_write_commands(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); } @@ -145,21 +148,25 @@ TEST_F(BicycleSteeringControllerTest, test_update_logic) // set command statically std::shared_ptr msg = std::make_shared(); + msg->header.stamp = controller_->get_node()->now(); msg->twist.linear.x = 0.1; msg->twist.angular.z = 0.2; controller_->input_ref_.writeFromNonRT(msg); ASSERT_EQ( - controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_->update_reference_from_subscribers(), controller_interface::return_type::OK); + + ASSERT_EQ( + controller_->update_and_write_commands(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - EXPECT_EQ(controller_->command_interfaces_[0].get_value(), 1.1); - EXPECT_EQ(controller_->command_interfaces_[1].get_value(), 2.2); + EXPECT_EQ(controller_->command_interfaces_[0].get_value(), 0.253221); + EXPECT_EQ(controller_->command_interfaces_[1].get_value(), 1.41798); EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x)); EXPECT_EQ(controller_->reference_interfaces_.size(), joint_names_.size()); for (const auto & interface : controller_->reference_interfaces_) { - EXPECT_TRUE(std::isnan(interface)); + EXPECT_FALSE(std::isnan(interface)); } } @@ -171,7 +178,7 @@ TEST_F(BicycleSteeringControllerTest, publish_status_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_->update_and_write_commands(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); ControllerStateMsg msg; @@ -188,7 +195,7 @@ TEST_F(BicycleSteeringControllerTest, receive_message_and_publish_updated_status ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ( - controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_->update_and_write_commands(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); ControllerStateMsg msg; @@ -201,7 +208,7 @@ TEST_F(BicycleSteeringControllerTest, receive_message_and_publish_updated_status // ASSERT_TRUE(controller_->wait_for_commands(executor)); ASSERT_EQ( - controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_->update_and_write_commands(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); EXPECT_EQ(controller_->command_interfaces_[0].get_value(), 1.1); diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp index e756eb3d79..1d61f7c60f 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp @@ -190,7 +190,7 @@ class BicycleSteeringControllerFixture : public ::testing::Test wait_set.add_subscription(subscription); while (max_sub_check_loop_count--) { - controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + controller_->update_and_write_commands(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); // check if messageparams_ has been received if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) { @@ -267,10 +267,10 @@ class BicycleSteeringControllerFixture : public ::testing::Test std::vector joint_names_ = {rear_wheels_names_[0], front_wheels_names_[0]}; double wheelbase_ = 3.24644; - double front_wheel_radius_ = 0.45; + double front_wheels_radius_ = 0.45; double rear_wheels_radius_ = 0.45; - std::array joint_state_values_ = {1.1, 2.0}; + std::array joint_state_values_ = {3.3, 0.5}; std::array joint_command_values_ = {1.1, 2.2}; std::array joint_reference_interfaces_ = {"linear/velocity", "angular/position"}; std::string steering_interface_name_ = "position"; From 872ff14b2fdc98b6ca22f9ec46ffe5c716af56b9 Mon Sep 17 00:00:00 2001 From: petkovich Date: Fri, 13 Jan 2023 12:39:59 +0100 Subject: [PATCH 121/186] publisher works --- .../test/test_bicycle_steering_controller.cpp | 15 +++++++++------ .../test/test_bicycle_steering_controller.hpp | 6 +++--- 2 files changed, 12 insertions(+), 9 deletions(-) diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp index 0dc5f9b260..7c1755d4dc 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp @@ -204,20 +204,23 @@ TEST_F(BicycleSteeringControllerTest, receive_message_and_publish_updated_status EXPECT_EQ(msg.linear_velocity_command.data[0], 1.1); EXPECT_EQ(msg.steering_angle_command.data[0], 2.2); - // publish_commands(); - // ASSERT_TRUE(controller_->wait_for_commands(executor)); + publish_commands(); + ASSERT_TRUE(controller_->wait_for_commands(executor)); + + ASSERT_EQ( + controller_->update_reference_from_subscribers(), controller_interface::return_type::OK); ASSERT_EQ( controller_->update_and_write_commands(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - EXPECT_EQ(controller_->command_interfaces_[0].get_value(), 1.1); - EXPECT_EQ(controller_->command_interfaces_[1].get_value(), 2.2); + EXPECT_EQ(controller_->command_interfaces_[0].get_value(), 0.253221); + EXPECT_EQ(controller_->command_interfaces_[1].get_value(), 1.41798); subscribe_and_get_messages(msg); - EXPECT_EQ(msg.linear_velocity_command.data[0], 1.1); - EXPECT_EQ(msg.steering_angle_command.data[0], 2.2); + EXPECT_EQ(msg.linear_velocity_command.data[0], 0.253221); + EXPECT_EQ(msg.steering_angle_command.data[0], 1.41798); } int main(int argc, char ** argv) diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp index 1d61f7c60f..e15788cb12 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp @@ -125,7 +125,7 @@ class BicycleSteeringControllerFixture : public ::testing::Test command_publisher_node_ = std::make_shared("command_publisher"); command_publisher_ = command_publisher_node_->create_publisher( - "/test_bicycle_steering_controller/commands", rclcpp::SystemDefaultsQoS()); + "/test_bicycle_steering_controller/reference", rclcpp::SystemDefaultsQoS()); // service_caller_node_ = std::make_shared("service_caller"); } @@ -205,7 +205,7 @@ class BicycleSteeringControllerFixture : public ::testing::Test ASSERT_TRUE(subscription->take(msg, msg_info)); } - void publish_commands(const double linear = 0.5, const double angular = 0.1) + void publish_commands(const double linear = 0.1, const double angular = 0.2) { auto wait_for_topic = [&](const auto topic_name) { @@ -223,7 +223,7 @@ class BicycleSteeringControllerFixture : public ::testing::Test } }; - // wait_for_topic(command_publisher_->get_topic_name()); + wait_for_topic(command_publisher_->get_topic_name()); ControllerReferenceMsg msg; msg.twist.linear.x = linear; From e0b06652732cb65efcfaaa446bdbb415325bc860 Mon Sep 17 00:00:00 2001 From: petkovich Date: Fri, 13 Jan 2023 14:58:59 +0100 Subject: [PATCH 122/186] bicycle finished --- .../test/test_bicycle_steering_controller.cpp | 33 +++++++++---------- .../test/test_bicycle_steering_controller.hpp | 2 ++ 2 files changed, 17 insertions(+), 18 deletions(-) diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp index 7c1755d4dc..6006fa1bb3 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp @@ -106,7 +106,7 @@ TEST_F(BicycleSteeringControllerTest, update_success) ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ( - controller_->update_and_write_commands(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); } @@ -131,7 +131,7 @@ TEST_F(BicycleSteeringControllerTest, reactivate_success) ASSERT_TRUE(std::isnan(controller_->command_interfaces_[0].get_value())); ASSERT_EQ( - controller_->update_and_write_commands(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); } @@ -154,14 +154,13 @@ TEST_F(BicycleSteeringControllerTest, test_update_logic) controller_->input_ref_.writeFromNonRT(msg); ASSERT_EQ( - controller_->update_reference_from_subscribers(), controller_interface::return_type::OK); - - ASSERT_EQ( - controller_->update_and_write_commands(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - EXPECT_EQ(controller_->command_interfaces_[0].get_value(), 0.253221); - EXPECT_EQ(controller_->command_interfaces_[1].get_value(), 1.41798); + EXPECT_NEAR(controller_->command_interfaces_[0].get_value(), 0.253221, COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[1].get_value(), 1.4179821977774734, COMMON_THRESHOLD); + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x)); EXPECT_EQ(controller_->reference_interfaces_.size(), joint_names_.size()); for (const auto & interface : controller_->reference_interfaces_) @@ -178,7 +177,7 @@ TEST_F(BicycleSteeringControllerTest, publish_status_success) ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ( - controller_->update_and_write_commands(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); ControllerStateMsg msg; @@ -195,7 +194,7 @@ TEST_F(BicycleSteeringControllerTest, receive_message_and_publish_updated_status ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ( - controller_->update_and_write_commands(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); ControllerStateMsg msg; @@ -208,19 +207,17 @@ TEST_F(BicycleSteeringControllerTest, receive_message_and_publish_updated_status ASSERT_TRUE(controller_->wait_for_commands(executor)); ASSERT_EQ( - controller_->update_reference_from_subscribers(), controller_interface::return_type::OK); - - ASSERT_EQ( - controller_->update_and_write_commands(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - EXPECT_EQ(controller_->command_interfaces_[0].get_value(), 0.253221); - EXPECT_EQ(controller_->command_interfaces_[1].get_value(), 1.41798); + EXPECT_NEAR(controller_->command_interfaces_[0].get_value(), 0.253221, COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[1].get_value(), 1.4179821977774734, COMMON_THRESHOLD); subscribe_and_get_messages(msg); - EXPECT_EQ(msg.linear_velocity_command.data[0], 0.253221); - EXPECT_EQ(msg.steering_angle_command.data[0], 1.41798); + EXPECT_NEAR(msg.linear_velocity_command.data[0], 0.253221, COMMON_THRESHOLD); + EXPECT_NEAR(msg.steering_angle_command.data[0], 1.4179821977774734, COMMON_THRESHOLD); } int main(int argc, char ** argv) diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp index e15788cb12..e022d09da6 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp @@ -45,6 +45,8 @@ namespace { constexpr auto NODE_SUCCESS = controller_interface::CallbackReturn::SUCCESS; constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR; +const double COMMON_THRESHOLD = 1e-6; // destogl: increased for 0.0001 for stable CI builds? + } // namespace // namespace From e0255e09bd78c9ad84964af7f6f75a2bc23624a0 Mon Sep 17 00:00:00 2001 From: petkovich Date: Fri, 13 Jan 2023 15:17:49 +0100 Subject: [PATCH 123/186] tricycle params# --- .../tricycle_steering_controller.hpp | 3 ++- .../src/tricycle_steering_controller.cpp | 10 +++++----- 2 files changed, 7 insertions(+), 6 deletions(-) diff --git a/tricycle_steering_controller/include/tricycle_steering_controller/tricycle_steering_controller.hpp b/tricycle_steering_controller/include/tricycle_steering_controller/tricycle_steering_controller.hpp index 46cc718684..346fdfa17f 100644 --- a/tricycle_steering_controller/include/tricycle_steering_controller/tricycle_steering_controller.hpp +++ b/tricycle_steering_controller/include/tricycle_steering_controller/tricycle_steering_controller.hpp @@ -49,8 +49,9 @@ class TricycleSteeringController : public steering_controllers_library::Steering STEERING_CONTROLLERS__VISIBILITY_PUBLIC void initialize_implementation_parameter_listener() override; -private: +protected: std::shared_ptr tricycle_param_listener_; + tricycle_steering_controller::Params tricycle_params_; }; } // namespace tricycle_steering_controller diff --git a/tricycle_steering_controller/src/tricycle_steering_controller.cpp b/tricycle_steering_controller/src/tricycle_steering_controller.cpp index 15e7488cb0..ac34fcca31 100644 --- a/tricycle_steering_controller/src/tricycle_steering_controller.cpp +++ b/tricycle_steering_controller/src/tricycle_steering_controller.cpp @@ -28,12 +28,12 @@ void TricycleSteeringController::initialize_implementation_parameter_listener() controller_interface::CallbackReturn TricycleSteeringController::configure_odometry() { - tricycle_steering_controller::Params tricycle_params = tricycle_param_listener_->get_params(); + tricycle_params_ = tricycle_param_listener_->get_params(); - const double front_wheels_radius = tricycle_params.front_wheels_radius; - const double rear_wheels_radius = tricycle_params.rear_wheels_radius; - const double wheel_track = tricycle_params.wheel_track; - const double wheelbase = tricycle_params.wheelbase; + const double front_wheels_radius = tricycle_params_.front_wheels_radius; + const double rear_wheels_radius = tricycle_params_.rear_wheels_radius; + const double wheel_track = tricycle_params_.wheel_track; + const double wheelbase = tricycle_params_.wheelbase; if (params_.front_steering) { From a9e249ac817f512dfdecba39f848778430aa07cd Mon Sep 17 00:00:00 2001 From: petkovich Date: Fri, 13 Jan 2023 15:48:09 +0100 Subject: [PATCH 124/186] tricycle tests compile, ready for implementations --- .../test/test_bicycle_steering_controller.hpp | 30 +- tricycle_steering_controller/CMakeLists.txt | 20 +- .../test_tricycle_steering_controller.cpp | 682 +++++++++--------- .../test_tricycle_steering_controller.hpp | 126 ++-- 4 files changed, 409 insertions(+), 449 deletions(-) diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp index e022d09da6..b4c5339f7d 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp @@ -33,19 +33,16 @@ #include "rclcpp/utilities.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" -// TODO(anyone): replace the state and command message types using ControllerStateMsg = steering_controllers_library::SteeringControllersLibrary::AckermanControllerState; using ControllerReferenceMsg = steering_controllers_library::SteeringControllersLibrary::ControllerTwistReferenceMsg; -// using ControllerModeSrvType = -// steering_controllers_library::SteeringControllersLibrary::ControllerModeSrvType; namespace { constexpr auto NODE_SUCCESS = controller_interface::CallbackReturn::SUCCESS; constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR; -const double COMMON_THRESHOLD = 1e-6; // destogl: increased for 0.0001 for stable CI builds? +const double COMMON_THRESHOLD = 1e-6; } // namespace // namespace @@ -128,8 +125,6 @@ class BicycleSteeringControllerFixture : public ::testing::Test command_publisher_node_ = std::make_shared("command_publisher"); command_publisher_ = command_publisher_node_->create_publisher( "/test_bicycle_steering_controller/reference", rclcpp::SystemDefaultsQoS()); - - // service_caller_node_ = std::make_shared("service_caller"); } static void TearDownTestCase() {} @@ -234,30 +229,8 @@ class BicycleSteeringControllerFixture : public ::testing::Test 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 - double reference_timeout_ = 2.0; bool front_steering_ = true; bool open_loop_ = false; @@ -286,7 +259,6 @@ class BicycleSteeringControllerFixture : public ::testing::Test std::unique_ptr controller_; rclcpp::Node::SharedPtr command_publisher_node_; rclcpp::Publisher::SharedPtr command_publisher_; - // rclcpp::Node::SharedPtr service_caller_node_; }; #endif // TEST_BICYCLE_STEERING_CONTROLLER_HPP_ diff --git a/tricycle_steering_controller/CMakeLists.txt b/tricycle_steering_controller/CMakeLists.txt index 9379e62d92..b5b5412f9e 100644 --- a/tricycle_steering_controller/CMakeLists.txt +++ b/tricycle_steering_controller/CMakeLists.txt @@ -90,16 +90,16 @@ if(BUILD_TESTING) hardware_interface ) - add_rostest_with_parameters_gmock( - test_tricycle_steering_controller_preceeding test/test_tricycle_steering_controller_preceeding.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/test/tricycle_steering_controller_preceeding_params.yaml) - target_include_directories(test_tricycle_steering_controller_preceeding PRIVATE include) - target_link_libraries(test_tricycle_steering_controller_preceeding tricycle_steering_controller) - ament_target_dependencies( - test_tricycle_steering_controller_preceeding - controller_interface - hardware_interface - ) + # add_rostest_with_parameters_gmock( + # test_tricycle_steering_controller_preceeding test/test_tricycle_steering_controller_preceeding.cpp + # ${CMAKE_CURRENT_SOURCE_DIR}/test/tricycle_steering_controller_preceeding_params.yaml) + # target_include_directories(test_tricycle_steering_controller_preceeding PRIVATE include) + # target_link_libraries(test_tricycle_steering_controller_preceeding tricycle_steering_controller) + # ament_target_dependencies( + # test_tricycle_steering_controller_preceeding + # controller_interface + # hardware_interface + # ) endif() ament_export_include_directories( diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp index 91c8591bc1..5257d0e0c5 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp @@ -21,10 +21,6 @@ #include #include -using tricycle_steering_controller::CMD_STEER; -using tricycle_steering_controller::control_mode_type; -using tricycle_steering_controller::STATE_DRIVE_RIGHT_WHEEL; - class TricycleSteeringControllerTest : public TricycleSteeringControllerFixture { @@ -34,349 +30,349 @@ TEST_F(TricycleSteeringControllerTest, 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(TricycleSteeringControllerTest, 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(TricycleSteeringControllerTest, 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(TricycleSteeringControllerTest, 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(TricycleSteeringControllerTest, 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(TricycleSteeringControllerTest, 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_STEER].get_value(), 101.101); - ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_TRUE(std::isnan(controller_->command_interfaces_[CMD_STEER].get_value())); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_TRUE(std::isnan(controller_->command_interfaces_[CMD_STEER].get_value())); - - ASSERT_EQ( - controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); -} - -TEST_F(TricycleSteeringControllerTest, 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); + // ASSERT_TRUE(controller_->params_.joints.empty()); + // ASSERT_TRUE(controller_->params_.state_joints.empty()); + // ASSERT_TRUE(controller_->params_.interface_name.empty()); - // set to true - ASSERT_NO_THROW(call_service(true, executor)); - ASSERT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::SLOW); + // ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - // set back to false - ASSERT_NO_THROW(call_service(false, executor)); - ASSERT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::FAST); + // 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(TricycleSteeringControllerTest, 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_DRIVE_RIGHT_WHEEL], 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(TricycleSteeringControllerTest, 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_DRIVE_RIGHT_WHEEL], 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(TricycleSteeringControllerTest, 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_DRIVE_RIGHT_WHEEL] = 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_DRIVE_RIGHT_WHEEL], 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(TricycleSteeringControllerTest, 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_DRIVE_RIGHT_WHEEL] = 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_DRIVE_RIGHT_WHEEL], 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(TricycleSteeringControllerTest, 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(TricycleSteeringControllerTest, 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_STEER], 0.45); - - subscribe_and_get_messages(msg); - - ASSERT_EQ(msg.set_point, 0.45); -} +// TEST_F(TricycleSteeringControllerTest, 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(TricycleSteeringControllerTest, 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(TricycleSteeringControllerTest, 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(TricycleSteeringControllerTest, 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(TricycleSteeringControllerTest, 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_STEER].get_value(), 101.101); +// ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); +// ASSERT_TRUE(std::isnan(controller_->command_interfaces_[CMD_STEER].get_value())); +// ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); +// ASSERT_TRUE(std::isnan(controller_->command_interfaces_[CMD_STEER].get_value())); + +// ASSERT_EQ( +// controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), +// controller_interface::return_type::OK); +// } + +// TEST_F(TricycleSteeringControllerTest, 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(TricycleSteeringControllerTest, 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_DRIVE_RIGHT_WHEEL], 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(TricycleSteeringControllerTest, 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_DRIVE_RIGHT_WHEEL], 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(TricycleSteeringControllerTest, 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_DRIVE_RIGHT_WHEEL] = 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_DRIVE_RIGHT_WHEEL], 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(TricycleSteeringControllerTest, 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_DRIVE_RIGHT_WHEEL] = 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_DRIVE_RIGHT_WHEEL], 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(TricycleSteeringControllerTest, 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(TricycleSteeringControllerTest, 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_STEER], 0.45); + +// subscribe_and_get_messages(msg); + +// ASSERT_EQ(msg.set_point, 0.45); +// } int main(int argc, char ** argv) { diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp index 191dddcc14..6a3f11563f 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp @@ -1,5 +1,4 @@ // Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) -// Copyright (c) 2023, 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. @@ -34,18 +33,17 @@ #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "tricycle_steering_controller/tricycle_steering_controller.hpp" -// TODO(anyone): replace the state and command message types using ControllerStateMsg = - steering_controllers_library::SteeringControllersLibrary::ControllerStateMsg; + steering_controllers_library::SteeringControllersLibrary::AckermanControllerState; using ControllerReferenceMsg = - steering_controllers_library::SteeringControllersLibrary::ControllerReferenceMsg; -using ControllerModeSrvType = - steering_controllers_library::SteeringControllersLibrary::ControllerModeSrvType; + steering_controllers_library::SteeringControllersLibrary::ControllerTwistReferenceMsg; namespace { constexpr auto NODE_SUCCESS = controller_interface::CallbackReturn::SUCCESS; constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR; +const double COMMON_THRESHOLD = 1e-6; + } // namespace // namespace @@ -55,12 +53,12 @@ class TestableTricycleSteeringController { FRIEND_TEST(TricycleSteeringControllerTest, all_parameters_set_configure_success); FRIEND_TEST(TricycleSteeringControllerTest, activate_success); + FRIEND_TEST(TricycleSteeringControllerTest, update_success); + FRIEND_TEST(TricycleSteeringControllerTest, deactivate_success); FRIEND_TEST(TricycleSteeringControllerTest, reactivate_success); - FRIEND_TEST(TricycleSteeringControllerTest, test_setting_slow_mode_service); - FRIEND_TEST(TricycleSteeringControllerTest, test_update_logic_fast); - FRIEND_TEST(TricycleSteeringControllerTest, test_update_logic_slow); - FRIEND_TEST(TricycleSteeringControllerTest, test_update_logic_chainable_fast); - FRIEND_TEST(TricycleSteeringControllerTest, test_update_logic_chainable_slow); + FRIEND_TEST(TricycleSteeringControllerTest, test_update_logic); + FRIEND_TEST(TricycleSteeringControllerTest, publish_status_success); + FRIEND_TEST(TricycleSteeringControllerTest, receive_message_and_publish_updated_status); public: controller_interface::CallbackReturn on_configure( @@ -71,7 +69,7 @@ class TestableTricycleSteeringController // Only if on_configure is successful create subscription if (ret == CallbackReturn::SUCCESS) { - ref_subscriber_wait_set_.add_subscription(ref_subscriber_); + ref_subscriber_wait_set_.add_subscription(ref_subscriber_twist_); } return ret; } @@ -129,11 +127,9 @@ class TricycleSteeringControllerFixture : public ::testing::Test command_publisher_node_ = std::make_shared("command_publisher"); command_publisher_ = command_publisher_node_->create_publisher( - "/test_tricycle_steering_controller/commands", rclcpp::SystemDefaultsQoS()); + "/test_tricycle_steering_controller/reference", rclcpp::SystemDefaultsQoS()); - service_caller_node_ = std::make_shared("service_caller"); - slow_control_service_client_ = service_caller_node_->create_client( - "/test_tricycle_steering_controller/set_slow_control_mode"); + // service_caller_node_ = std::make_shared("service_caller"); } static void TearDownTestCase() {} @@ -145,29 +141,38 @@ class TricycleSteeringControllerFixture : public ::testing::Test { ASSERT_EQ(controller_->init(controller_name), controller_interface::return_type::OK); + if (position_feedback_ == true) + { + traction_interface_name_ = "position"; + } + else + { + traction_interface_name_ = "velocity"; + } + 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 + command_itfs_.emplace_back(hardware_interface::CommandInterface( + rear_wheels_names_[0], traction_interface_name_, &joint_command_values_[0])); + command_ifs.emplace_back(command_itfs_.back()); + + command_itfs_.emplace_back(hardware_interface::CommandInterface( + front_wheels_names_[0], steering_interface_name_, &joint_command_values_[1])); + command_ifs.emplace_back(command_itfs_.back()); 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 + state_itfs_.emplace_back(hardware_interface::StateInterface( + rear_wheels_names_[0], traction_interface_name_, &joint_state_values_[0])); + state_ifs.emplace_back(state_itfs_.back()); + + state_itfs_.emplace_back(hardware_interface::StateInterface( + front_wheels_names_[0], steering_interface_name_, &joint_state_values_[1])); + state_ifs.emplace_back(state_itfs_.back()); controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); } @@ -178,7 +183,7 @@ class TricycleSteeringControllerFixture : public ::testing::Test rclcpp::Node test_subscription_node("test_subscription_node"); auto subs_callback = [&](const ControllerStateMsg::SharedPtr) {}; auto subscription = test_subscription_node.create_subscription( - "/test_tricycle_steering_controller/state", 10, subs_callback); + "/test_tricycle_steering_controller/controller_state", 10, subs_callback); // call update to publish the test value ASSERT_EQ( @@ -207,10 +212,7 @@ class TricycleSteeringControllerFixture : public ::testing::Test 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) + void publish_commands(const double linear = 0.1, const double angular = 0.2) { auto wait_for_topic = [&](const auto topic_name) { @@ -231,42 +233,34 @@ class TricycleSteeringControllerFixture : public ::testing::Test wait_for_topic(command_publisher_->get_topic_name()); ControllerReferenceMsg msg; - msg.joint_names = joint_names_; - msg.displacements = displacements; - msg.velocities = velocities; - msg.duration = duration; + msg.twist.linear.x = linear; + msg.twist.angular.z = angular; 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}; + double reference_timeout_ = 2.0; + bool front_steering_ = true; + bool open_loop_ = false; + unsigned int velocity_rolling_window_size_ = 10; + bool position_feedback_ = false; + bool use_stamped_vel_ = true; + std::vector rear_wheels_names_ = {"rear_wheel_joint"}; + std::vector front_wheels_names_ = {"steering_axis_joint"}; + std::vector joint_names_ = {rear_wheels_names_[0], front_wheels_names_[0]}; + + double wheelbase_ = 3.24644; + double front_wheels_radius_ = 0.45; + double rear_wheels_radius_ = 0.45; + + std::array joint_state_values_ = {3.3, 0.5}; + std::array joint_command_values_ = {1.1, 2.2}; + std::array joint_reference_interfaces_ = {"linear/velocity", "angular/position"}; + std::string steering_interface_name_ = "position"; + // defined in setup + std::string traction_interface_name_ = ""; std::vector state_itfs_; std::vector command_itfs_; @@ -275,8 +269,6 @@ class TricycleSteeringControllerFixture : public ::testing::Test 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 // TEST_TRICYCLE_STEERING_CONTROLLER_HPP_ From 8a1da69f97cbf6ed1d0a28e3740cb29bf92fb36a Mon Sep 17 00:00:00 2001 From: petkovich Date: Fri, 13 Jan 2023 16:18:17 +0100 Subject: [PATCH 125/186] bicycle uses CMD_DRIVE_WHEEL etc instead of hardcoded numbers --- .../bicycle_steering_controller.hpp | 7 ++++++ .../src/bicycle_steering_controller.cpp | 4 ++-- .../test/test_bicycle_steering_controller.cpp | 22 +++++++++---------- .../test/test_bicycle_steering_controller.hpp | 15 +++++++++---- .../test_tricycle_steering_controller.hpp | 2 +- .../tricycle_steering_controller_params.yaml | 16 ++++++++++---- 6 files changed, 43 insertions(+), 23 deletions(-) diff --git a/bicycle_steering_controller/include/bicycle_steering_controller/bicycle_steering_controller.hpp b/bicycle_steering_controller/include/bicycle_steering_controller/bicycle_steering_controller.hpp index da86fbcbe2..78987931dc 100644 --- a/bicycle_steering_controller/include/bicycle_steering_controller/bicycle_steering_controller.hpp +++ b/bicycle_steering_controller/include/bicycle_steering_controller/bicycle_steering_controller.hpp @@ -26,6 +26,13 @@ namespace bicycle_steering_controller { +// name constants for state interfaces +static constexpr size_t STATE_DRIVE_WHEEL = 0; +static constexpr size_t STATE_STEER_AXIS = 1; + +// name constants for command interfaces +static constexpr size_t CMD_DRIVE_WHEEL = 0; +static constexpr size_t CMD_STEER = 1; class BicycleSteeringController : public steering_controllers_library::SteeringControllersLibrary { public: diff --git a/bicycle_steering_controller/src/bicycle_steering_controller.cpp b/bicycle_steering_controller/src/bicycle_steering_controller.cpp index 17fe7a0971..91d0e5f198 100644 --- a/bicycle_steering_controller/src/bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/src/bicycle_steering_controller.cpp @@ -64,8 +64,8 @@ bool BicycleSteeringController::update_odometry(const rclcpp::Duration & period) } else { - const double rear_wheel_value = state_interfaces_[0].get_value(); - const double steer_position = state_interfaces_[1].get_value(); + const double rear_wheel_value = state_interfaces_[STATE_DRIVE_WHEEL].get_value(); + const double steer_position = state_interfaces_[STATE_STEER_AXIS].get_value(); if (!std::isnan(rear_wheel_value) && !std::isnan(steer_position)) { if (params_.position_feedback) diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp index 6006fa1bb3..4df726d4c6 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp @@ -20,10 +20,6 @@ #include #include -// using bicycle_steering_controller::CMD_MY_ITFS; -// using bicycle_steering_controller::control_mode_type; -// using bicycle_steering_controller::STATE_MY_ITFS; - class BicycleSteeringControllerTest : public BicycleSteeringControllerFixture { @@ -32,10 +28,6 @@ class BicycleSteeringControllerTest TEST_F(BicycleSteeringControllerTest, all_parameters_set_configure_success) { SetUpController(); - // ASSERT_FALSE(controller_->params_.rear_wheels_names.empty()); - // ASSERT_FALSE(controller_->params_.front_wheels_names.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); @@ -60,13 +52,19 @@ TEST_F(BicycleSteeringControllerTest, check_exported_intefaces) auto command_intefaces = controller_->command_interface_configuration(); ASSERT_EQ(command_intefaces.names.size(), joint_command_values_.size()); - EXPECT_EQ(command_intefaces.names[0], rear_wheels_names_[0] + "/" + traction_interface_name_); - EXPECT_EQ(command_intefaces.names[1], front_wheels_names_[0] + "/" + steering_interface_name_); + EXPECT_EQ( + command_intefaces.names[CMD_DRIVE_WHEEL], + rear_wheels_names_[0] + "/" + traction_interface_name_); + EXPECT_EQ( + command_intefaces.names[CMD_STEER], front_wheels_names_[0] + "/" + steering_interface_name_); auto state_intefaces = controller_->state_interface_configuration(); ASSERT_EQ(state_intefaces.names.size(), joint_state_values_.size()); - EXPECT_EQ(state_intefaces.names[0], rear_wheels_names_[0] + "/" + traction_interface_name_); - EXPECT_EQ(state_intefaces.names[1], front_wheels_names_[0] + "/" + steering_interface_name_); + EXPECT_EQ( + state_intefaces.names[STATE_DRIVE_WHEEL], + rear_wheels_names_[0] + "/" + traction_interface_name_); + EXPECT_EQ( + state_intefaces.names[CMD_STEER], front_wheels_names_[0] + "/" + steering_interface_name_); // check ref itfs auto reference_interfaces = controller_->export_reference_interfaces(); diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp index b4c5339f7d..05704f9517 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp @@ -38,6 +38,13 @@ using ControllerStateMsg = using ControllerReferenceMsg = steering_controllers_library::SteeringControllersLibrary::ControllerTwistReferenceMsg; +// name constants for state interfaces +using bicycle_steering_controller::STATE_DRIVE_WHEEL; +using bicycle_steering_controller::STATE_STEER_AXIS; + +using bicycle_steering_controller::CMD_DRIVE_WHEEL; +using bicycle_steering_controller::CMD_STEER; + namespace { constexpr auto NODE_SUCCESS = controller_interface::CallbackReturn::SUCCESS; @@ -150,11 +157,11 @@ class BicycleSteeringControllerFixture : public ::testing::Test command_ifs.reserve(joint_command_values_.size()); command_itfs_.emplace_back(hardware_interface::CommandInterface( - rear_wheels_names_[0], traction_interface_name_, &joint_command_values_[0])); + rear_wheels_names_[0], traction_interface_name_, &joint_command_values_[CMD_DRIVE_WHEEL])); command_ifs.emplace_back(command_itfs_.back()); command_itfs_.emplace_back(hardware_interface::CommandInterface( - front_wheels_names_[0], steering_interface_name_, &joint_command_values_[1])); + front_wheels_names_[0], steering_interface_name_, &joint_command_values_[CMD_STEER])); command_ifs.emplace_back(command_itfs_.back()); std::vector state_ifs; @@ -162,11 +169,11 @@ class BicycleSteeringControllerFixture : public ::testing::Test state_ifs.reserve(joint_state_values_.size()); state_itfs_.emplace_back(hardware_interface::StateInterface( - rear_wheels_names_[0], traction_interface_name_, &joint_state_values_[0])); + rear_wheels_names_[0], traction_interface_name_, &joint_state_values_[STATE_DRIVE_WHEEL])); state_ifs.emplace_back(state_itfs_.back()); state_itfs_.emplace_back(hardware_interface::StateInterface( - front_wheels_names_[0], steering_interface_name_, &joint_state_values_[1])); + front_wheels_names_[0], steering_interface_name_, &joint_state_values_[CMD_STEER])); state_ifs.emplace_back(state_itfs_.back()); controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp index 6a3f11563f..e8c4da8f0b 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp @@ -247,7 +247,7 @@ class TricycleSteeringControllerFixture : public ::testing::Test unsigned int velocity_rolling_window_size_ = 10; bool position_feedback_ = false; bool use_stamped_vel_ = true; - std::vector rear_wheels_names_ = {"rear_wheel_joint"}; + std::vector rear_wheels_names_ = {"rear_right_wheel_joint", "rear_left_wheel_joint"}; std::vector front_wheels_names_ = {"steering_axis_joint"}; std::vector joint_names_ = {rear_wheels_names_[0], front_wheels_names_[0]}; diff --git a/tricycle_steering_controller/test/tricycle_steering_controller_params.yaml b/tricycle_steering_controller/test/tricycle_steering_controller_params.yaml index b82a408311..18845bc629 100644 --- a/tricycle_steering_controller/test/tricycle_steering_controller_params.yaml +++ b/tricycle_steering_controller/test/tricycle_steering_controller_params.yaml @@ -1,7 +1,15 @@ -test_tricycle_steering_controller: +test_bicycle_steering_controller: ros__parameters: - joints: - - joint1 + reference_timeout: 2.0 + front_steering: true + open_loop: false + velocity_rolling_window_size: 10 + position_feedback: false + use_stamped_vel: true + rear_wheels_names: [rear_right_wheel_joint, rear_left_wheel_joint] + front_wheels_names: [steering_axis_joint] - interface_name: acceleration + wheelbase: 3.24644 + front_wheel_sradius: 0.45 + rear_wheels_radius: 0.45 From c291ea7d84b5c1c7e192c8f48dd5202e408657af Mon Sep 17 00:00:00 2001 From: petkovich Date: Fri, 13 Jan 2023 16:37:58 +0100 Subject: [PATCH 126/186] tricycle first test, add subscription fails --- .../test/test_bicycle_steering_controller.hpp | 1 + .../test_tricycle_steering_controller.cpp | 23 ++++++----- .../test_tricycle_steering_controller.hpp | 38 +++++++++++++------ 3 files changed, 40 insertions(+), 22 deletions(-) diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp index 05704f9517..3a377a2164 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp @@ -42,6 +42,7 @@ using ControllerReferenceMsg = using bicycle_steering_controller::STATE_DRIVE_WHEEL; using bicycle_steering_controller::STATE_STEER_AXIS; +// name constants for command interfaces using bicycle_steering_controller::CMD_DRIVE_WHEEL; using bicycle_steering_controller::CMD_STEER; diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp index 5257d0e0c5..169d4eac75 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp @@ -30,16 +30,19 @@ TEST_F(TricycleSteeringControllerTest, 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_); + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // ASSERT_THAT( + // controller_->params_.rear_wheels_names, testing::ElementsAreArray(rear_wheels_names_)); + // ASSERT_THAT( + // controller_->params_.front_wheels_names, testing::ElementsAreArray(front_wheels_names_)); + // ASSERT_EQ(controller_->params_.front_steering, front_steering_); + // ASSERT_EQ(controller_->params_.open_loop, open_loop_); + // ASSERT_EQ(controller_->params_.velocity_rolling_window_size, velocity_rolling_window_size_); + // ASSERT_EQ(controller_->params_.position_feedback, position_feedback_); + // ASSERT_EQ(controller_->tricycle_params_.wheelbase, wheelbase_); + // ASSERT_EQ(controller_->tricycle_params_.front_wheels_radius, front_wheels_radius_); + // ASSERT_EQ(controller_->tricycle_params_.rear_wheels_radius, rear_wheels_radius_); } // TEST_F(TricycleSteeringControllerTest, check_exported_intefaces) diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp index e8c4da8f0b..420c21226e 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp @@ -38,6 +38,15 @@ using ControllerStateMsg = using ControllerReferenceMsg = steering_controllers_library::SteeringControllersLibrary::ControllerTwistReferenceMsg; +// name constants for state interfaces +using tricycle_steering_controller::STATE_DRIVE_LEFT_WHEEL; +using tricycle_steering_controller::STATE_DRIVE_RIGHT_WHEEL; +using tricycle_steering_controller::STATE_STEER_AXIS; + +// name constants for command interfaces +using tricycle_steering_controller::CMD_DRIVE_WHEEL; +using tricycle_steering_controller::CMD_STEER; + namespace { constexpr auto NODE_SUCCESS = controller_interface::CallbackReturn::SUCCESS; @@ -67,10 +76,10 @@ class TestableTricycleSteeringController auto ret = tricycle_steering_controller::TricycleSteeringController::on_configure(previous_state); // Only if on_configure is successful create subscription - if (ret == CallbackReturn::SUCCESS) - { - ref_subscriber_wait_set_.add_subscription(ref_subscriber_twist_); - } + // if (ret == CallbackReturn::SUCCESS) + // { + // ref_subscriber_wait_set_.add_subscription(ref_subscriber_twist_); + // } return ret; } @@ -128,8 +137,6 @@ class TricycleSteeringControllerFixture : public ::testing::Test command_publisher_node_ = std::make_shared("command_publisher"); command_publisher_ = command_publisher_node_->create_publisher( "/test_tricycle_steering_controller/reference", rclcpp::SystemDefaultsQoS()); - - // service_caller_node_ = std::make_shared("service_caller"); } static void TearDownTestCase() {} @@ -155,11 +162,11 @@ class TricycleSteeringControllerFixture : public ::testing::Test command_ifs.reserve(joint_command_values_.size()); command_itfs_.emplace_back(hardware_interface::CommandInterface( - rear_wheels_names_[0], traction_interface_name_, &joint_command_values_[0])); + rear_wheels_names_[0], traction_interface_name_, &joint_command_values_[CMD_DRIVE_WHEEL])); command_ifs.emplace_back(command_itfs_.back()); command_itfs_.emplace_back(hardware_interface::CommandInterface( - front_wheels_names_[0], steering_interface_name_, &joint_command_values_[1])); + front_wheels_names_[0], steering_interface_name_, &joint_command_values_[CMD_STEER])); command_ifs.emplace_back(command_itfs_.back()); std::vector state_ifs; @@ -167,11 +174,17 @@ class TricycleSteeringControllerFixture : public ::testing::Test state_ifs.reserve(joint_state_values_.size()); state_itfs_.emplace_back(hardware_interface::StateInterface( - rear_wheels_names_[0], traction_interface_name_, &joint_state_values_[0])); + rear_wheels_names_[0], traction_interface_name_, + &joint_state_values_[STATE_DRIVE_RIGHT_WHEEL])); + state_ifs.emplace_back(state_itfs_.back()); + + state_itfs_.emplace_back(hardware_interface::StateInterface( + rear_wheels_names_[1], traction_interface_name_, + &joint_state_values_[STATE_DRIVE_LEFT_WHEEL])); state_ifs.emplace_back(state_itfs_.back()); state_itfs_.emplace_back(hardware_interface::StateInterface( - front_wheels_names_[0], steering_interface_name_, &joint_state_values_[1])); + front_wheels_names_[0], steering_interface_name_, &joint_state_values_[CMD_STEER])); state_ifs.emplace_back(state_itfs_.back()); controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); @@ -249,13 +262,14 @@ class TricycleSteeringControllerFixture : public ::testing::Test bool use_stamped_vel_ = true; std::vector rear_wheels_names_ = {"rear_right_wheel_joint", "rear_left_wheel_joint"}; std::vector front_wheels_names_ = {"steering_axis_joint"}; - std::vector joint_names_ = {rear_wheels_names_[0], front_wheels_names_[0]}; + std::vector joint_names_ = { + rear_wheels_names_[0], rear_wheels_names_[1], front_wheels_names_[0]}; double wheelbase_ = 3.24644; double front_wheels_radius_ = 0.45; double rear_wheels_radius_ = 0.45; - std::array joint_state_values_ = {3.3, 0.5}; + std::array joint_state_values_ = {3.3, 3.3, 0.5}; std::array joint_command_values_ = {1.1, 2.2}; std::array joint_reference_interfaces_ = {"linear/velocity", "angular/position"}; std::string steering_interface_name_ = "position"; From fafcd20b6f124c3d8c14ff198fa91bfb52298338 Mon Sep 17 00:00:00 2001 From: petkovich Date: Fri, 13 Jan 2023 16:42:45 +0100 Subject: [PATCH 127/186] first test passes --- .../test/test_tricycle_steering_controller.hpp | 8 ++++---- .../test/tricycle_steering_controller_params.yaml | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp index 420c21226e..d8410d56b8 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp @@ -76,10 +76,10 @@ class TestableTricycleSteeringController auto ret = tricycle_steering_controller::TricycleSteeringController::on_configure(previous_state); // Only if on_configure is successful create subscription - // if (ret == CallbackReturn::SUCCESS) - // { - // ref_subscriber_wait_set_.add_subscription(ref_subscriber_twist_); - // } + if (ret == CallbackReturn::SUCCESS) + { + ref_subscriber_wait_set_.add_subscription(ref_subscriber_twist_); + } return ret; } diff --git a/tricycle_steering_controller/test/tricycle_steering_controller_params.yaml b/tricycle_steering_controller/test/tricycle_steering_controller_params.yaml index 18845bc629..42081041f5 100644 --- a/tricycle_steering_controller/test/tricycle_steering_controller_params.yaml +++ b/tricycle_steering_controller/test/tricycle_steering_controller_params.yaml @@ -1,4 +1,4 @@ -test_bicycle_steering_controller: +test_tricycle_steering_controller: ros__parameters: reference_timeout: 2.0 From 1840708a699c0001d8ef6749c2b77300fbb647ec Mon Sep 17 00:00:00 2001 From: petkovich Date: Fri, 13 Jan 2023 16:44:20 +0100 Subject: [PATCH 128/186] try: first test passes --- .../test_tricycle_steering_controller.cpp | 22 +++++++++---------- .../tricycle_steering_controller_params.yaml | 2 +- 2 files changed, 12 insertions(+), 12 deletions(-) diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp index 169d4eac75..66a90dcddf 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp @@ -32,17 +32,17 @@ TEST_F(TricycleSteeringControllerTest, all_parameters_set_configure_success) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - // ASSERT_THAT( - // controller_->params_.rear_wheels_names, testing::ElementsAreArray(rear_wheels_names_)); - // ASSERT_THAT( - // controller_->params_.front_wheels_names, testing::ElementsAreArray(front_wheels_names_)); - // ASSERT_EQ(controller_->params_.front_steering, front_steering_); - // ASSERT_EQ(controller_->params_.open_loop, open_loop_); - // ASSERT_EQ(controller_->params_.velocity_rolling_window_size, velocity_rolling_window_size_); - // ASSERT_EQ(controller_->params_.position_feedback, position_feedback_); - // ASSERT_EQ(controller_->tricycle_params_.wheelbase, wheelbase_); - // ASSERT_EQ(controller_->tricycle_params_.front_wheels_radius, front_wheels_radius_); - // ASSERT_EQ(controller_->tricycle_params_.rear_wheels_radius, rear_wheels_radius_); + ASSERT_THAT( + controller_->params_.rear_wheels_names, testing::ElementsAreArray(rear_wheels_names_)); + ASSERT_THAT( + controller_->params_.front_wheels_names, testing::ElementsAreArray(front_wheels_names_)); + ASSERT_EQ(controller_->params_.front_steering, front_steering_); + ASSERT_EQ(controller_->params_.open_loop, open_loop_); + ASSERT_EQ(controller_->params_.velocity_rolling_window_size, velocity_rolling_window_size_); + ASSERT_EQ(controller_->params_.position_feedback, position_feedback_); + ASSERT_EQ(controller_->tricycle_params_.wheelbase, wheelbase_); + ASSERT_EQ(controller_->tricycle_params_.front_wheels_radius, front_wheels_radius_); + ASSERT_EQ(controller_->tricycle_params_.rear_wheels_radius, rear_wheels_radius_); } // TEST_F(TricycleSteeringControllerTest, check_exported_intefaces) diff --git a/tricycle_steering_controller/test/tricycle_steering_controller_params.yaml b/tricycle_steering_controller/test/tricycle_steering_controller_params.yaml index 42081041f5..743edabe5f 100644 --- a/tricycle_steering_controller/test/tricycle_steering_controller_params.yaml +++ b/tricycle_steering_controller/test/tricycle_steering_controller_params.yaml @@ -11,5 +11,5 @@ test_tricycle_steering_controller: front_wheels_names: [steering_axis_joint] wheelbase: 3.24644 - front_wheel_sradius: 0.45 + front_wheels_radius: 0.45 rear_wheels_radius: 0.45 From 38b7dcc6a376fd419827ea884a3e144341774921 Mon Sep 17 00:00:00 2001 From: petkovich Date: Fri, 13 Jan 2023 17:00:12 +0100 Subject: [PATCH 129/186] bi&tri: check_exported_intefaces --- .../test/test_bicycle_steering_controller.cpp | 8 ++- .../test/test_bicycle_steering_controller.hpp | 7 +- .../test_tricycle_steering_controller.cpp | 71 +++++++++++-------- .../test_tricycle_steering_controller.hpp | 14 +++- 4 files changed, 60 insertions(+), 40 deletions(-) diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp index 4df726d4c6..0a57885bd0 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp @@ -53,10 +53,11 @@ TEST_F(BicycleSteeringControllerTest, check_exported_intefaces) auto command_intefaces = controller_->command_interface_configuration(); ASSERT_EQ(command_intefaces.names.size(), joint_command_values_.size()); EXPECT_EQ( - command_intefaces.names[CMD_DRIVE_WHEEL], + command_intefaces.names[STATE_DRIVE_WHEEL], rear_wheels_names_[0] + "/" + traction_interface_name_); EXPECT_EQ( - command_intefaces.names[CMD_STEER], front_wheels_names_[0] + "/" + steering_interface_name_); + command_intefaces.names[STATE_STEER_AXIS], + front_wheels_names_[0] + "/" + steering_interface_name_); auto state_intefaces = controller_->state_interface_configuration(); ASSERT_EQ(state_intefaces.names.size(), joint_state_values_.size()); @@ -64,7 +65,8 @@ TEST_F(BicycleSteeringControllerTest, check_exported_intefaces) state_intefaces.names[STATE_DRIVE_WHEEL], rear_wheels_names_[0] + "/" + traction_interface_name_); EXPECT_EQ( - state_intefaces.names[CMD_STEER], front_wheels_names_[0] + "/" + steering_interface_name_); + state_intefaces.names[STATE_STEER_AXIS], + front_wheels_names_[0] + "/" + steering_interface_name_); // check ref itfs auto reference_interfaces = controller_->export_reference_interfaces(); diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp index 3a377a2164..e3e702dda5 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp @@ -60,6 +60,7 @@ class TestableBicycleSteeringController : public bicycle_steering_controller::BicycleSteeringController { FRIEND_TEST(BicycleSteeringControllerTest, all_parameters_set_configure_success); + FRIEND_TEST(BicycleSteeringControllerTest, check_exported_intefaces); FRIEND_TEST(BicycleSteeringControllerTest, activate_success); FRIEND_TEST(BicycleSteeringControllerTest, update_success); FRIEND_TEST(BicycleSteeringControllerTest, deactivate_success); @@ -158,11 +159,11 @@ class BicycleSteeringControllerFixture : public ::testing::Test command_ifs.reserve(joint_command_values_.size()); command_itfs_.emplace_back(hardware_interface::CommandInterface( - rear_wheels_names_[0], traction_interface_name_, &joint_command_values_[CMD_DRIVE_WHEEL])); + rear_wheels_names_[0], traction_interface_name_, &joint_command_values_[STATE_DRIVE_WHEEL])); command_ifs.emplace_back(command_itfs_.back()); command_itfs_.emplace_back(hardware_interface::CommandInterface( - front_wheels_names_[0], steering_interface_name_, &joint_command_values_[CMD_STEER])); + front_wheels_names_[0], steering_interface_name_, &joint_command_values_[STATE_STEER_AXIS])); command_ifs.emplace_back(command_itfs_.back()); std::vector state_ifs; @@ -174,7 +175,7 @@ class BicycleSteeringControllerFixture : public ::testing::Test state_ifs.emplace_back(state_itfs_.back()); state_itfs_.emplace_back(hardware_interface::StateInterface( - front_wheels_names_[0], steering_interface_name_, &joint_state_values_[CMD_STEER])); + front_wheels_names_[0], steering_interface_name_, &joint_state_values_[STATE_STEER_AXIS])); state_ifs.emplace_back(state_itfs_.back()); controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp index 66a90dcddf..2d7626c74c 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp @@ -45,39 +45,48 @@ TEST_F(TricycleSteeringControllerTest, all_parameters_set_configure_success) ASSERT_EQ(controller_->tricycle_params_.rear_wheels_radius, rear_wheels_radius_); } -// TEST_F(TricycleSteeringControllerTest, 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_); -// } +TEST_F(TricycleSteeringControllerTest, check_exported_intefaces) +{ + SetUpController(); -// 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_); -// } + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); -// // 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_); -// } -// } + auto command_intefaces = controller_->command_interface_configuration(); + ASSERT_EQ(command_intefaces.names.size(), joint_command_values_.size()); + EXPECT_EQ( + command_intefaces.names[STATE_DRIVE_RIGHT_WHEEL], + rear_wheels_names_[STATE_DRIVE_RIGHT_WHEEL] + "/" + traction_interface_name_); + EXPECT_EQ( + command_intefaces.names[STATE_DRIVE_LEFT_WHEEL], + rear_wheels_names_[STATE_DRIVE_LEFT_WHEEL] + "/" + traction_interface_name_); + EXPECT_EQ( + command_intefaces.names[STATE_STEER_AXIS], + front_wheels_names_[0] + "/" + steering_interface_name_); + + auto state_intefaces = controller_->state_interface_configuration(); + ASSERT_EQ(state_intefaces.names.size(), joint_state_values_.size()); + EXPECT_EQ( + state_intefaces.names[STATE_DRIVE_RIGHT_WHEEL], + rear_wheels_names_[STATE_DRIVE_RIGHT_WHEEL] + "/" + traction_interface_name_); + EXPECT_EQ( + state_intefaces.names[STATE_DRIVE_LEFT_WHEEL], + rear_wheels_names_[STATE_DRIVE_LEFT_WHEEL] + "/" + traction_interface_name_); + EXPECT_EQ( + state_intefaces.names[STATE_STEER_AXIS], + front_wheels_names_[0] + "/" + steering_interface_name_); + + // check ref itfs + auto reference_interfaces = controller_->export_reference_interfaces(); + ASSERT_EQ(reference_interfaces.size(), joint_reference_interfaces_.size()); + for (size_t i = 0; i < joint_reference_interfaces_.size(); ++i) + { + const std::string ref_itf_name = + std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i]; + 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_reference_interfaces_[i]); + } +} // TEST_F(TricycleSteeringControllerTest, activate_success) // { diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp index d8410d56b8..2ace631904 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp @@ -61,6 +61,7 @@ class TestableTricycleSteeringController : public tricycle_steering_controller::TricycleSteeringController { FRIEND_TEST(TricycleSteeringControllerTest, all_parameters_set_configure_success); + FRIEND_TEST(TricycleSteeringControllerTest, check_exported_intefaces); FRIEND_TEST(TricycleSteeringControllerTest, activate_success); FRIEND_TEST(TricycleSteeringControllerTest, update_success); FRIEND_TEST(TricycleSteeringControllerTest, deactivate_success); @@ -162,11 +163,18 @@ class TricycleSteeringControllerFixture : public ::testing::Test command_ifs.reserve(joint_command_values_.size()); command_itfs_.emplace_back(hardware_interface::CommandInterface( - rear_wheels_names_[0], traction_interface_name_, &joint_command_values_[CMD_DRIVE_WHEEL])); + rear_wheels_names_[0], traction_interface_name_, + &joint_command_values_[STATE_DRIVE_RIGHT_WHEEL])); + command_ifs.emplace_back(command_itfs_.back()); + + command_itfs_.emplace_back(hardware_interface::CommandInterface( + front_wheels_names_[0], steering_interface_name_, + &joint_command_values_[STATE_DRIVE_RIGHT_WHEEL])); command_ifs.emplace_back(command_itfs_.back()); command_itfs_.emplace_back(hardware_interface::CommandInterface( - front_wheels_names_[0], steering_interface_name_, &joint_command_values_[CMD_STEER])); + front_wheels_names_[0], steering_interface_name_, + &joint_command_values_[STATE_DRIVE_LEFT_WHEEL])); command_ifs.emplace_back(command_itfs_.back()); std::vector state_ifs; @@ -270,7 +278,7 @@ class TricycleSteeringControllerFixture : public ::testing::Test double rear_wheels_radius_ = 0.45; std::array joint_state_values_ = {3.3, 3.3, 0.5}; - std::array joint_command_values_ = {1.1, 2.2}; + std::array joint_command_values_ = {1.1, 3.3, 2.2}; std::array joint_reference_interfaces_ = {"linear/velocity", "angular/position"}; std::string steering_interface_name_ = "position"; // defined in setup From c6b51a04d66ae1aa9323fee69db0a95ee3ca27ca Mon Sep 17 00:00:00 2001 From: petkovich Date: Fri, 13 Jan 2023 17:12:17 +0100 Subject: [PATCH 130/186] tri: activate, update, deactivate, reactivate --- .../test_tricycle_steering_controller.cpp | 165 +++++------------- 1 file changed, 45 insertions(+), 120 deletions(-) diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp index 2d7626c74c..1463bbe12d 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp @@ -88,134 +88,59 @@ TEST_F(TricycleSteeringControllerTest, check_exported_intefaces) } } -// TEST_F(TricycleSteeringControllerTest, 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(TricycleSteeringControllerTest, 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(TricycleSteeringControllerTest, 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(TricycleSteeringControllerTest, 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_STEER].get_value(), 101.101); -// ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); -// ASSERT_TRUE(std::isnan(controller_->command_interfaces_[CMD_STEER].get_value())); -// ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); -// ASSERT_TRUE(std::isnan(controller_->command_interfaces_[CMD_STEER].get_value())); - -// ASSERT_EQ( -// controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), -// controller_interface::return_type::OK); -// } - -// TEST_F(TricycleSteeringControllerTest, 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); +TEST_F(TricycleSteeringControllerTest, activate_success) +{ + SetUpController(); -// // should stay 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); + + // check that the message is reset + auto msg = controller_->input_ref_.readFromNonRT(); + EXPECT_TRUE(std::isnan((*msg)->twist.linear.x)); + EXPECT_TRUE(std::isnan((*msg)->twist.linear.y)); + EXPECT_TRUE(std::isnan((*msg)->twist.linear.z)); + EXPECT_TRUE(std::isnan((*msg)->twist.angular.x)); + EXPECT_TRUE(std::isnan((*msg)->twist.angular.y)); + EXPECT_TRUE(std::isnan((*msg)->twist.angular.z)); +} -// // set to true -// ASSERT_NO_THROW(call_service(true, executor)); -// ASSERT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::SLOW); +TEST_F(TricycleSteeringControllerTest, update_success) +{ + SetUpController(); -// // set back to false -// ASSERT_NO_THROW(call_service(false, executor)); -// 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); -// TEST_F(TricycleSteeringControllerTest, 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_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); +} -// 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()); +TEST_F(TricycleSteeringControllerTest, deactivate_success) +{ + SetUpController(); -// // 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_->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); +} -// ASSERT_EQ( -// controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), -// controller_interface::return_type::OK); +TEST_F(TricycleSteeringControllerTest, reactivate_success) +{ + SetUpController(); -// EXPECT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::FAST); -// EXPECT_EQ(joint_command_values_[STATE_DRIVE_RIGHT_WHEEL], 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)); -// } -// } + 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); + ASSERT_TRUE(std::isnan(controller_->command_interfaces_[0].get_value())); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(std::isnan(controller_->command_interfaces_[0].get_value())); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); +} // TEST_F(TricycleSteeringControllerTest, test_update_logic_slow) // { From 607a774acff314d47f74df7fc9c3d47a42e71952 Mon Sep 17 00:00:00 2001 From: petkovich Date: Fri, 13 Jan 2023 17:22:52 +0100 Subject: [PATCH 131/186] update logic init tricycle --- .../test/test_bicycle_steering_controller.cpp | 2 +- .../test_tricycle_steering_controller.cpp | 150 ++++-------------- 2 files changed, 36 insertions(+), 116 deletions(-) diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp index 0a57885bd0..5508006b2e 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp @@ -162,7 +162,7 @@ TEST_F(BicycleSteeringControllerTest, test_update_logic) controller_->command_interfaces_[1].get_value(), 1.4179821977774734, COMMON_THRESHOLD); EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x)); - EXPECT_EQ(controller_->reference_interfaces_.size(), joint_names_.size()); + EXPECT_EQ(controller_->reference_interfaces_.size(), joint_reference_interfaces_.size()); for (const auto & interface : controller_->reference_interfaces_) { EXPECT_FALSE(std::isnan(interface)); diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp index 1463bbe12d..71c8064449 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp @@ -142,125 +142,45 @@ TEST_F(TricycleSteeringControllerTest, reactivate_success) controller_interface::return_type::OK); } -// TEST_F(TricycleSteeringControllerTest, 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_DRIVE_RIGHT_WHEEL], 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(TricycleSteeringControllerTest, 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_DRIVE_RIGHT_WHEEL] = TEST_DISPLACEMENT * 2; - -// ASSERT_EQ( -// controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), -// controller_interface::return_type::OK); +TEST_F(TricycleSteeringControllerTest, test_update_logic) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); -// EXPECT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::FAST); -// // reference_interfaces is directly applied -// EXPECT_EQ(joint_command_values_[STATE_DRIVE_RIGHT_WHEEL], 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)); -// } -// } + 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()); -// TEST_F(TricycleSteeringControllerTest, 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()); + // set command statically + std::shared_ptr msg = std::make_shared(); + msg->header.stamp = controller_->get_node()->now(); + msg->twist.linear.x = 0.1; + msg->twist.angular.z = 0.2; + controller_->input_ref_.writeFromNonRT(msg); -// 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_DRIVE_RIGHT_WHEEL] = 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); + 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_DRIVE_RIGHT_WHEEL], 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)); -// } -// } + EXPECT_NEAR( + controller_->command_interfaces_[STATE_DRIVE_RIGHT_WHEEL].get_value(), 0.253221, + COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[STATE_DRIVE_LEFT_WHEEL].get_value(), 0.253221, + COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[STATE_STEER_AXIS].get_value(), 1.4179821977774734, + COMMON_THRESHOLD); + + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x)); + EXPECT_EQ(controller_->reference_interfaces_.size(), joint_reference_interfaces_.size()); + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_FALSE(std::isnan(interface)); + } +} // TEST_F(TricycleSteeringControllerTest, publish_status_success) // { From 9c0c93fc8709fc7a84756cdfc0e1a2a566a1915b Mon Sep 17 00:00:00 2001 From: petkovich Date: Fri, 13 Jan 2023 17:34:17 +0100 Subject: [PATCH 132/186] update logic tricycle --- .../test/test_tricycle_steering_controller.cpp | 4 ++-- .../test/test_tricycle_steering_controller.hpp | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp index 71c8064449..9263312ba9 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp @@ -165,10 +165,10 @@ TEST_F(TricycleSteeringControllerTest, test_update_logic) controller_interface::return_type::OK); EXPECT_NEAR( - controller_->command_interfaces_[STATE_DRIVE_RIGHT_WHEEL].get_value(), 0.253221, + controller_->command_interfaces_[STATE_DRIVE_RIGHT_WHEEL].get_value(), 0.22222222222222224, COMMON_THRESHOLD); EXPECT_NEAR( - controller_->command_interfaces_[STATE_DRIVE_LEFT_WHEEL].get_value(), 0.253221, + controller_->command_interfaces_[STATE_DRIVE_LEFT_WHEEL].get_value(), 0.22222222222222224, COMMON_THRESHOLD); EXPECT_NEAR( controller_->command_interfaces_[STATE_STEER_AXIS].get_value(), 1.4179821977774734, diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp index 2ace631904..95a7f4f3a6 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp @@ -192,7 +192,7 @@ class TricycleSteeringControllerFixture : public ::testing::Test state_ifs.emplace_back(state_itfs_.back()); state_itfs_.emplace_back(hardware_interface::StateInterface( - front_wheels_names_[0], steering_interface_name_, &joint_state_values_[CMD_STEER])); + front_wheels_names_[0], steering_interface_name_, &joint_state_values_[STATE_STEER_AXIS])); state_ifs.emplace_back(state_itfs_.back()); controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); @@ -277,7 +277,7 @@ class TricycleSteeringControllerFixture : public ::testing::Test double front_wheels_radius_ = 0.45; double rear_wheels_radius_ = 0.45; - std::array joint_state_values_ = {3.3, 3.3, 0.5}; + std::array joint_state_values_ = {3.3, 3.3, 0.0}; std::array joint_command_values_ = {1.1, 3.3, 2.2}; std::array joint_reference_interfaces_ = {"linear/velocity", "angular/position"}; std::string steering_interface_name_ = "position"; From ec851addb4b2c8d5491bd493566ca38ddb00253c Mon Sep 17 00:00:00 2001 From: petkovich Date: Fri, 13 Jan 2023 17:52:30 +0100 Subject: [PATCH 133/186] tricycle done --- .../test/test_bicycle_steering_controller.cpp | 6 +- .../test/test_bicycle_steering_controller.hpp | 7 +- .../test_tricycle_steering_controller.cpp | 77 +++++++++---------- .../test_tricycle_steering_controller.hpp | 9 +-- 4 files changed, 50 insertions(+), 49 deletions(-) diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp index 5508006b2e..0a46844f69 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp @@ -210,9 +210,11 @@ TEST_F(BicycleSteeringControllerTest, receive_message_and_publish_updated_status controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - EXPECT_NEAR(controller_->command_interfaces_[0].get_value(), 0.253221, COMMON_THRESHOLD); EXPECT_NEAR( - controller_->command_interfaces_[1].get_value(), 1.4179821977774734, COMMON_THRESHOLD); + controller_->command_interfaces_[STATE_DRIVE_WHEEL].get_value(), 0.253221, COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[STATE_STEER_AXIS].get_value(), 1.4179821977774734, + COMMON_THRESHOLD); subscribe_and_get_messages(msg); diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp index e3e702dda5..755400ed21 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp @@ -190,14 +190,17 @@ class BicycleSteeringControllerFixture : public ::testing::Test "/test_bicycle_steering_controller/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); // 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 max_sub_check_loop_count wait_set.add_subscription(subscription); while (max_sub_check_loop_count--) { - controller_->update_and_write_commands(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); - // check if messageparams_ has been received + 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; diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp index 9263312ba9..4aade3d75c 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp @@ -182,54 +182,53 @@ TEST_F(TricycleSteeringControllerTest, test_update_logic) } } -// TEST_F(TricycleSteeringControllerTest, 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(TricycleSteeringControllerTest, receive_message_and_publish_updated_status) -// { -// SetUpController(); -// rclcpp::executors::MultiThreadedExecutor executor; -// executor.add_node(controller_->get_node()->get_node_base_interface()); +TEST_F(TricycleSteeringControllerTest, 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_->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); + 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); + ControllerStateMsg msg; + subscribe_and_get_messages(msg); -// ASSERT_EQ(msg.set_point, 101.101); + EXPECT_EQ(msg.linear_velocity_command.data[STATE_DRIVE_RIGHT_WHEEL], 1.1); + EXPECT_EQ(msg.linear_velocity_command.data[STATE_DRIVE_LEFT_WHEEL], 3.3); + EXPECT_EQ(msg.steering_angle_command.data[0], 2.2); -// publish_commands(); -// ASSERT_TRUE(controller_->wait_for_commands(executor)); + 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); + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); -// EXPECT_EQ(joint_command_values_[CMD_STEER], 0.45); + EXPECT_NEAR( + controller_->command_interfaces_[STATE_DRIVE_RIGHT_WHEEL].get_value(), 0.22222222222222224, + COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[STATE_DRIVE_LEFT_WHEEL].get_value(), 0.22222222222222224, + COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[STATE_STEER_AXIS].get_value(), 1.4179821977774734, + COMMON_THRESHOLD); -// subscribe_and_get_messages(msg); + subscribe_and_get_messages(msg); -// ASSERT_EQ(msg.set_point, 0.45); -// } + EXPECT_NEAR( + msg.linear_velocity_command.data[STATE_DRIVE_RIGHT_WHEEL], 0.22222222222222224, + COMMON_THRESHOLD); + EXPECT_NEAR( + msg.linear_velocity_command.data[STATE_DRIVE_LEFT_WHEEL], 0.22222222222222224, + COMMON_THRESHOLD); + EXPECT_NEAR(msg.steering_angle_command.data[0], 1.4179821977774734, COMMON_THRESHOLD); +} int main(int argc, char ** argv) { diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp index 95a7f4f3a6..6cae705257 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp @@ -169,12 +169,11 @@ class TricycleSteeringControllerFixture : public ::testing::Test command_itfs_.emplace_back(hardware_interface::CommandInterface( front_wheels_names_[0], steering_interface_name_, - &joint_command_values_[STATE_DRIVE_RIGHT_WHEEL])); + &joint_command_values_[STATE_DRIVE_LEFT_WHEEL])); command_ifs.emplace_back(command_itfs_.back()); command_itfs_.emplace_back(hardware_interface::CommandInterface( - front_wheels_names_[0], steering_interface_name_, - &joint_command_values_[STATE_DRIVE_LEFT_WHEEL])); + front_wheels_names_[0], steering_interface_name_, &joint_command_values_[STATE_STEER_AXIS])); command_ifs.emplace_back(command_itfs_.back()); std::vector state_ifs; @@ -210,8 +209,6 @@ class TricycleSteeringControllerFixture : public ::testing::Test 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 @@ -277,7 +274,7 @@ class TricycleSteeringControllerFixture : public ::testing::Test double front_wheels_radius_ = 0.45; double rear_wheels_radius_ = 0.45; - std::array joint_state_values_ = {3.3, 3.3, 0.0}; + std::array joint_state_values_ = {0.5, 0.5, 0.0}; std::array joint_command_values_ = {1.1, 3.3, 2.2}; std::array joint_reference_interfaces_ = {"linear/velocity", "angular/position"}; std::string steering_interface_name_ = "position"; From e63558fc7f38b46d055b109ef549b16c8b28406b Mon Sep 17 00:00:00 2001 From: petkovich Date: Fri, 13 Jan 2023 19:22:45 +0100 Subject: [PATCH 134/186] ackermann tests --- ackermann_steering_controller/CMakeLists.txt | 20 +- .../ackermann_steering_controller.hpp | 3 +- .../src/ackermann_steering_controller.cpp | 12 +- .../ackermann_steering_controller_params.yaml | 16 +- .../test_ackermann_steering_controller.cpp | 322 +++++------------- .../test_ackermann_steering_controller.hpp | 165 +++++---- .../test_tricycle_steering_controller.cpp | 1 + .../test_tricycle_steering_controller.hpp | 7 +- .../tricycle_steering_controller_params.yaml | 1 + 9 files changed, 226 insertions(+), 321 deletions(-) diff --git a/ackermann_steering_controller/CMakeLists.txt b/ackermann_steering_controller/CMakeLists.txt index 15e56a0ad0..8fbd0bd301 100644 --- a/ackermann_steering_controller/CMakeLists.txt +++ b/ackermann_steering_controller/CMakeLists.txt @@ -81,16 +81,16 @@ if(BUILD_TESTING) hardware_interface ) - add_rostest_with_parameters_gmock( - test_ackermann_steering_controller_preceeding test/test_ackermann_steering_controller_preceeding.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/test/ackermann_steering_controller_preceeding_params.yaml) - target_include_directories(test_ackermann_steering_controller_preceeding PRIVATE include) - target_link_libraries(test_ackermann_steering_controller_preceeding ackermann_steering_controller) - ament_target_dependencies( - test_ackermann_steering_controller_preceeding - controller_interface - hardware_interface - ) + # add_rostest_with_parameters_gmock( + # test_ackermann_steering_controller_preceeding test/test_ackermann_steering_controller_preceeding.cpp + # ${CMAKE_CURRENT_SOURCE_DIR}/test/ackermann_steering_controller_preceeding_params.yaml) + # target_include_directories(test_ackermann_steering_controller_preceeding PRIVATE include) + # target_link_libraries(test_ackermann_steering_controller_preceeding ackermann_steering_controller) + # ament_target_dependencies( + # test_ackermann_steering_controller_preceeding + # controller_interface + # hardware_interface + # ) endif() ament_export_include_directories( diff --git a/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp b/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp index 19b097d48c..61ee0305a7 100644 --- a/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp +++ b/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp @@ -50,8 +50,9 @@ class AckermannSteeringController : public steering_controllers_library::Steerin ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC void initialize_implementation_parameter_listener() override; -private: +protected: std::shared_ptr ackermann_param_listener_; + ackermann_steering_controller::Params ackermann_params_; }; } // namespace ackermann_steering_controller diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.cpp b/ackermann_steering_controller/src/ackermann_steering_controller.cpp index 9106f41ea1..0ac7a4c009 100644 --- a/ackermann_steering_controller/src/ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/src/ackermann_steering_controller.cpp @@ -29,13 +29,13 @@ void AckermannSteeringController::initialize_implementation_parameter_listener() controller_interface::CallbackReturn AckermannSteeringController::configure_odometry() { - ackermann_steering_controller::Params ackerman_params = ackermann_param_listener_->get_params(); + ackermann_params_ = ackermann_param_listener_->get_params(); - const double front_wheels_radius = ackerman_params.front_wheels_radius; - const double rear_wheels_radius = ackerman_params.rear_wheels_radius; - const double front_wheel_track = ackerman_params.front_wheel_track; - const double rear_wheel_track = ackerman_params.rear_wheel_track; - const double wheelbase = ackerman_params.wheelbase; + const double front_wheels_radius = ackermann_params_.front_wheels_radius; + const double rear_wheels_radius = ackermann_params_.rear_wheels_radius; + const double front_wheel_track = ackermann_params_.front_wheel_track; + const double rear_wheel_track = ackermann_params_.rear_wheel_track; + const double wheelbase = ackermann_params_.wheelbase; if (params_.front_steering) { diff --git a/ackermann_steering_controller/test/ackermann_steering_controller_params.yaml b/ackermann_steering_controller/test/ackermann_steering_controller_params.yaml index d09ee30a97..6b64f901c3 100644 --- a/ackermann_steering_controller/test/ackermann_steering_controller_params.yaml +++ b/ackermann_steering_controller/test/ackermann_steering_controller_params.yaml @@ -1,7 +1,17 @@ test_ackermann_steering_controller: ros__parameters: - joints: - - joint1 + reference_timeout: 2.0 + front_steering: true + open_loop: false + velocity_rolling_window_size: 10 + position_feedback: false + use_stamped_vel: true + rear_wheels_names: [rear_right_wheel_joint, rear_left_wheel_joint] + front_wheels_names: [front_right_steering_joint, front_left_steering_joint] - interface_name: acceleration + wheelbase: 3.24644 + front_wheel_track: 2.12321 + rear_wheel_track: 1.76868 + front_wheels_radius: 0.45 + rear_wheels_radius: 0.45 diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp index 916f9fca2f..2f69c29c06 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp @@ -20,10 +20,6 @@ #include #include -using ackermann_steering_controller::CMD_DRIVE_WHEEL; -using ackermann_steering_controller::control_mode_type; -using ackermann_steering_controller::STATE_DRIVE_RIGHT_WHEEL; - class AckermannSteeringControllerTest : public AckermannSteeringControllerFixture { @@ -33,16 +29,21 @@ TEST_F(AckermannSteeringControllerTest, 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_); + ASSERT_THAT( + controller_->params_.rear_wheels_names, testing::ElementsAreArray(rear_wheels_names_)); + ASSERT_THAT( + controller_->params_.front_wheels_names, testing::ElementsAreArray(front_wheels_names_)); + ASSERT_EQ(controller_->params_.front_steering, front_steering_); + ASSERT_EQ(controller_->params_.open_loop, open_loop_); + ASSERT_EQ(controller_->params_.velocity_rolling_window_size, velocity_rolling_window_size_); + ASSERT_EQ(controller_->params_.position_feedback, position_feedback_); + ASSERT_EQ(controller_->ackermann_params_.wheelbase, wheelbase_); + ASSERT_EQ(controller_->ackermann_params_.front_wheels_radius, front_wheels_radius_); + ASSERT_EQ(controller_->ackermann_params_.rear_wheels_radius, rear_wheels_radius_); + ASSERT_EQ(controller_->ackermann_params_.front_wheel_track, front_wheel_track_); + ASSERT_EQ(controller_->ackermann_params_.rear_wheel_track, rear_wheel_track_); } TEST_F(AckermannSteeringControllerTest, check_exported_intefaces) @@ -53,29 +54,44 @@ TEST_F(AckermannSteeringControllerTest, check_exported_intefaces) 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_); - } + EXPECT_EQ( + command_intefaces.names[STATE_DRIVE_RIGHT_WHEEL], + rear_wheels_names_[STATE_DRIVE_RIGHT_WHEEL] + "/" + traction_interface_name_); + EXPECT_EQ( + command_intefaces.names[STATE_DRIVE_LEFT_WHEEL], + rear_wheels_names_[STATE_DRIVE_LEFT_WHEEL] + "/" + traction_interface_name_); + EXPECT_EQ( + command_intefaces.names[STATE_STEER_RIGHT_WHEEL], + front_wheels_names_[0] + "/" + steering_interface_name_); + EXPECT_EQ( + command_intefaces.names[STATE_STEER_LEFT_WHEEL], + front_wheels_names_[1] + "/" + steering_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_); - } + EXPECT_EQ( + state_intefaces.names[STATE_DRIVE_RIGHT_WHEEL], + rear_wheels_names_[STATE_DRIVE_RIGHT_WHEEL] + "/" + traction_interface_name_); + EXPECT_EQ( + state_intefaces.names[STATE_DRIVE_LEFT_WHEEL], + rear_wheels_names_[STATE_DRIVE_LEFT_WHEEL] + "/" + traction_interface_name_); + EXPECT_EQ( + state_intefaces.names[STATE_STEER_RIGHT_WHEEL], + front_wheels_names_[0] + "/" + steering_interface_name_); + EXPECT_EQ( + state_intefaces.names[STATE_STEER_LEFT_WHEEL], + front_wheels_names_[1] + "/" + steering_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) + ASSERT_EQ(reference_interfaces.size(), joint_reference_interfaces_.size()); + for (size_t i = 0; i < joint_reference_interfaces_.size(); ++i) { - const std::string ref_itf_name = std::string(controller_->get_node()->get_name()) + "/" + - joint_names_[i] + "/" + interface_name_; + const std::string ref_itf_name = + std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i]; 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_); + EXPECT_EQ(reference_interfaces[i].get_interface_name(), joint_reference_interfaces_[i]); } } @@ -88,24 +104,12 @@ TEST_F(AckermannSteeringControllerTest, activate_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)); - } + EXPECT_TRUE(std::isnan((*msg)->twist.linear.x)); + EXPECT_TRUE(std::isnan((*msg)->twist.linear.y)); + EXPECT_TRUE(std::isnan((*msg)->twist.linear.z)); + EXPECT_TRUE(std::isnan((*msg)->twist.angular.x)); + EXPECT_TRUE(std::isnan((*msg)->twist.angular.y)); + EXPECT_TRUE(std::isnan((*msg)->twist.angular.z)); } TEST_F(AckermannSteeringControllerTest, update_success) @@ -135,85 +139,21 @@ TEST_F(AckermannSteeringControllerTest, reactivate_success) 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_DRIVE_WHEEL].get_value(), 101.101); ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_TRUE(std::isnan(controller_->command_interfaces_[CMD_DRIVE_WHEEL].get_value())); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_TRUE(std::isnan(controller_->command_interfaces_[CMD_DRIVE_WHEEL].get_value())); - - ASSERT_EQ( - controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); -} - -TEST_F(AckermannSteeringControllerTest, 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(AckermannSteeringControllerTest, 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_TRUE(std::isnan(controller_->command_interfaces_[0].get_value())); 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_TRUE(std::isnan(controller_->command_interfaces_[0].get_value())); 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_DRIVE_RIGHT_WHEEL], 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(AckermannSteeringControllerTest, test_update_logic_slow) +TEST_F(AckermannSteeringControllerTest, test_update_logic) { 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); @@ -221,130 +161,37 @@ TEST_F(AckermannSteeringControllerTest, test_update_logic_slow) 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(); + msg->header.stamp = controller_->get_node()->now(); + msg->twist.linear.x = 0.1; + msg->twist.angular.z = 0.2; 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_DRIVE_RIGHT_WHEEL], TEST_DISPLACEMENT / 2); - EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->displacements[0])); - EXPECT_EQ(controller_->reference_interfaces_.size(), joint_names_.size()); + EXPECT_NEAR( + controller_->command_interfaces_[STATE_DRIVE_RIGHT_WHEEL].get_value(), 0.22222222222222224, + COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[STATE_DRIVE_LEFT_WHEEL].get_value(), 0.22222222222222224, + COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[STATE_STEER_RIGHT_WHEEL].get_value(), 1.4179821977774734, + COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[STATE_STEER_LEFT_WHEEL].get_value(), 1.4179821977774734, + COMMON_THRESHOLD); + + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x)); + EXPECT_EQ(controller_->reference_interfaces_.size(), joint_reference_interfaces_.size()); for (const auto & interface : controller_->reference_interfaces_) { - EXPECT_TRUE(std::isnan(interface)); + EXPECT_FALSE(std::isnan(interface)); } } -TEST_F(AckermannSteeringControllerTest, 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_DRIVE_RIGHT_WHEEL] = 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_DRIVE_RIGHT_WHEEL], 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(AckermannSteeringControllerTest, 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_DRIVE_RIGHT_WHEEL] = 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_DRIVE_RIGHT_WHEEL], 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(AckermannSteeringControllerTest, 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(AckermannSteeringControllerTest, receive_message_and_publish_updated_status) { SetUpController(); @@ -361,7 +208,10 @@ TEST_F(AckermannSteeringControllerTest, receive_message_and_publish_updated_stat ControllerStateMsg msg; subscribe_and_get_messages(msg); - ASSERT_EQ(msg.set_point, 101.101); + EXPECT_EQ(msg.linear_velocity_command.data[STATE_DRIVE_RIGHT_WHEEL], 1.1); + EXPECT_EQ(msg.linear_velocity_command.data[STATE_DRIVE_LEFT_WHEEL], 3.3); + EXPECT_EQ(msg.steering_angle_command.data[0], 2.2); + EXPECT_EQ(msg.steering_angle_command.data[1], 4.4); publish_commands(); ASSERT_TRUE(controller_->wait_for_commands(executor)); @@ -370,13 +220,29 @@ TEST_F(AckermannSteeringControllerTest, receive_message_and_publish_updated_stat controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - EXPECT_EQ(joint_command_values_[CMD_DRIVE_WHEEL], 0.45); - + EXPECT_NEAR( + controller_->command_interfaces_[STATE_DRIVE_RIGHT_WHEEL].get_value(), 0.22222222222222224, + COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[STATE_DRIVE_LEFT_WHEEL].get_value(), 0.22222222222222224, + COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[STATE_STEER_RIGHT_WHEEL].get_value(), 1.4179821977774734, + COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[STATE_STEER_LEFT_WHEEL].get_value(), 1.4179821977774734, + COMMON_THRESHOLD); subscribe_and_get_messages(msg); - ASSERT_EQ(msg.set_point, 0.45); + EXPECT_NEAR( + msg.linear_velocity_command.data[STATE_DRIVE_RIGHT_WHEEL], 0.22222222222222224, + COMMON_THRESHOLD); + EXPECT_NEAR( + msg.linear_velocity_command.data[STATE_DRIVE_LEFT_WHEEL], 0.22222222222222224, + COMMON_THRESHOLD); + EXPECT_NEAR(msg.steering_angle_command.data[0], 1.4179821977774734, COMMON_THRESHOLD); + EXPECT_NEAR(msg.steering_angle_command.data[1], 1.4179821977774734, COMMON_THRESHOLD); } - int main(int argc, char ** argv) { ::testing::InitGoogleTest(&argc, argv); diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp index fcd82a4d1b..7f8075d624 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp @@ -33,18 +33,26 @@ #include "rclcpp/utilities.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" -// TODO(anyone): replace the state and command message types using ControllerStateMsg = - steering_controllers_library::SteeringControllersLibrary::ControllerStateMsg; + steering_controllers_library::SteeringControllersLibrary::AckermanControllerState; using ControllerReferenceMsg = - steering_controllers_library::SteeringControllersLibrary::ControllerReferenceMsg; -using ControllerModeSrvType = - steering_controllers_library::SteeringControllersLibrary::ControllerModeSrvType; + steering_controllers_library::SteeringControllersLibrary::ControllerTwistReferenceMsg; + +// name constants for state interfaces +using ackermann_steering_controller::STATE_DRIVE_LEFT_WHEEL; +using ackermann_steering_controller::STATE_DRIVE_RIGHT_WHEEL; +using ackermann_steering_controller::STATE_STEER_LEFT_WHEEL; +using ackermann_steering_controller::STATE_STEER_RIGHT_WHEEL; + +// name constants for command interfaces +using ackermann_steering_controller::CMD_DRIVE_WHEEL; +using ackermann_steering_controller::CMD_STEER; namespace { constexpr auto NODE_SUCCESS = controller_interface::CallbackReturn::SUCCESS; constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR; +const double COMMON_THRESHOLD = 1e-6; } // namespace // namespace @@ -53,13 +61,14 @@ class TestableAckermannSteeringController : public ackermann_steering_controller::AckermannSteeringController { FRIEND_TEST(AckermannSteeringControllerTest, all_parameters_set_configure_success); + FRIEND_TEST(AckermannSteeringControllerTest, check_exported_intefaces); FRIEND_TEST(AckermannSteeringControllerTest, activate_success); + FRIEND_TEST(AckermannSteeringControllerTest, update_success); + FRIEND_TEST(AckermannSteeringControllerTest, deactivate_success); FRIEND_TEST(AckermannSteeringControllerTest, reactivate_success); - FRIEND_TEST(AckermannSteeringControllerTest, test_setting_slow_mode_service); - FRIEND_TEST(AckermannSteeringControllerTest, test_update_logic_fast); - FRIEND_TEST(AckermannSteeringControllerTest, test_update_logic_slow); - FRIEND_TEST(AckermannSteeringControllerTest, test_update_logic_chainable_fast); - FRIEND_TEST(AckermannSteeringControllerTest, test_update_logic_chainable_slow); + FRIEND_TEST(AckermannSteeringControllerTest, test_update_logic); + FRIEND_TEST(AckermannSteeringControllerTest, publish_status_success); + FRIEND_TEST(AckermannSteeringControllerTest, receive_message_and_publish_updated_status); public: controller_interface::CallbackReturn on_configure( @@ -70,7 +79,7 @@ class TestableAckermannSteeringController // Only if on_configure is successful create subscription if (ret == CallbackReturn::SUCCESS) { - ref_subscriber_wait_set_.add_subscription(ref_subscriber_); + ref_subscriber_wait_set_.add_subscription(ref_subscriber_twist_); } return ret; } @@ -108,8 +117,6 @@ class TestableAckermannSteeringController 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_; }; @@ -128,11 +135,7 @@ class AckermannSteeringControllerFixture : public ::testing::Test command_publisher_node_ = std::make_shared("command_publisher"); command_publisher_ = command_publisher_node_->create_publisher( - "/test_ackermann_steering_controller/commands", rclcpp::SystemDefaultsQoS()); - - service_caller_node_ = std::make_shared("service_caller"); - slow_control_service_client_ = service_caller_node_->create_client( - "/test_ackermann_steering_controller/set_slow_control_mode"); + "/test_ackermann_steering_controller/reference", rclcpp::SystemDefaultsQoS()); } static void TearDownTestCase() {} @@ -144,29 +147,62 @@ class AckermannSteeringControllerFixture : public ::testing::Test { ASSERT_EQ(controller_->init(controller_name), controller_interface::return_type::OK); + if (position_feedback_ == true) + { + traction_interface_name_ = "position"; + } + else + { + traction_interface_name_ = "velocity"; + } + 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 + command_itfs_.emplace_back(hardware_interface::CommandInterface( + rear_wheels_names_[0], traction_interface_name_, + &joint_command_values_[STATE_DRIVE_RIGHT_WHEEL])); + command_ifs.emplace_back(command_itfs_.back()); + + command_itfs_.emplace_back(hardware_interface::CommandInterface( + rear_wheels_names_[1], steering_interface_name_, + &joint_command_values_[STATE_DRIVE_LEFT_WHEEL])); + command_ifs.emplace_back(command_itfs_.back()); + + command_itfs_.emplace_back(hardware_interface::CommandInterface( + front_wheels_names_[0], steering_interface_name_, + &joint_command_values_[STATE_STEER_RIGHT_WHEEL])); + command_ifs.emplace_back(command_itfs_.back()); + + command_itfs_.emplace_back(hardware_interface::CommandInterface( + front_wheels_names_[1], steering_interface_name_, + &joint_command_values_[STATE_STEER_LEFT_WHEEL])); + command_ifs.emplace_back(command_itfs_.back()); 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 + state_itfs_.emplace_back(hardware_interface::StateInterface( + rear_wheels_names_[0], traction_interface_name_, + &joint_state_values_[STATE_DRIVE_RIGHT_WHEEL])); + state_ifs.emplace_back(state_itfs_.back()); + + state_itfs_.emplace_back(hardware_interface::StateInterface( + rear_wheels_names_[1], traction_interface_name_, + &joint_state_values_[STATE_DRIVE_LEFT_WHEEL])); + state_ifs.emplace_back(state_itfs_.back()); + + state_itfs_.emplace_back(hardware_interface::StateInterface( + front_wheels_names_[0], steering_interface_name_, + &joint_state_values_[STATE_STEER_RIGHT_WHEEL])); + state_ifs.emplace_back(state_itfs_.back()); + + state_itfs_.emplace_back(hardware_interface::StateInterface( + front_wheels_names_[1], steering_interface_name_, + &joint_state_values_[STATE_STEER_LEFT_WHEEL])); + state_ifs.emplace_back(state_itfs_.back()); controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); } @@ -177,7 +213,7 @@ class AckermannSteeringControllerFixture : public ::testing::Test rclcpp::Node test_subscription_node("test_subscription_node"); auto subs_callback = [&](const ControllerStateMsg::SharedPtr) {}; auto subscription = test_subscription_node.create_subscription( - "/test_ackermann_steering_controller/state", 10, subs_callback); + "/test_ackermann_steering_controller/controller_state", 10, subs_callback); // call update to publish the test value ASSERT_EQ( @@ -206,10 +242,7 @@ class AckermannSteeringControllerFixture : public ::testing::Test 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) + void publish_commands(const double linear = 0.1, const double angular = 0.2) { auto wait_for_topic = [&](const auto topic_name) { @@ -230,42 +263,38 @@ class AckermannSteeringControllerFixture : public ::testing::Test wait_for_topic(command_publisher_->get_topic_name()); ControllerReferenceMsg msg; - msg.joint_names = joint_names_; - msg.displacements = displacements; - msg.velocities = velocities; - msg.duration = duration; + msg.twist.linear.x = linear; + msg.twist.angular.z = angular; 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}; + double reference_timeout_ = 2.0; + bool front_steering_ = true; + bool open_loop_ = false; + unsigned int velocity_rolling_window_size_ = 10; + bool position_feedback_ = false; + bool use_stamped_vel_ = true; + std::vector rear_wheels_names_ = {"rear_right_wheel_joint", "rear_left_wheel_joint"}; + std::vector front_wheels_names_ = { + "front_right_steering_joint", "front_left_steering_joint"}; + std::vector joint_names_ = { + rear_wheels_names_[0], rear_wheels_names_[1], front_wheels_names_[0], front_wheels_names_[1]}; + + double wheelbase_ = 3.24644; + double front_wheel_track_ = 2.12321; + double rear_wheel_track_ = 1.76868; + double front_wheels_radius_ = 0.45; + double rear_wheels_radius_ = 0.45; + + std::array joint_state_values_ = {0.5, 0.5, 0.0, 0.0}; + std::array joint_command_values_ = {1.1, 3.3, 2.2, 4.4}; + std::array joint_reference_interfaces_ = {"linear/velocity", "angular/position"}; + std::string steering_interface_name_ = "position"; + // defined in setup + std::string traction_interface_name_ = ""; std::vector state_itfs_; std::vector command_itfs_; @@ -274,8 +303,6 @@ class AckermannSteeringControllerFixture : public ::testing::Test 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 // TEST_ACKERMANN_STEERING_CONTROLLER_HPP_ diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp index 4aade3d75c..16a2195340 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp @@ -43,6 +43,7 @@ TEST_F(TricycleSteeringControllerTest, all_parameters_set_configure_success) ASSERT_EQ(controller_->tricycle_params_.wheelbase, wheelbase_); ASSERT_EQ(controller_->tricycle_params_.front_wheels_radius, front_wheels_radius_); ASSERT_EQ(controller_->tricycle_params_.rear_wheels_radius, rear_wheels_radius_); + ASSERT_EQ(controller_->tricycle_params_.wheel_track, wheel_track_); } TEST_F(TricycleSteeringControllerTest, check_exported_intefaces) diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp index 6cae705257..7ba172b8de 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp @@ -52,7 +52,6 @@ namespace constexpr auto NODE_SUCCESS = controller_interface::CallbackReturn::SUCCESS; constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR; const double COMMON_THRESHOLD = 1e-6; - } // namespace // namespace @@ -117,8 +116,6 @@ class TestableTricycleSteeringController 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_; }; @@ -168,7 +165,7 @@ class TricycleSteeringControllerFixture : public ::testing::Test command_ifs.emplace_back(command_itfs_.back()); command_itfs_.emplace_back(hardware_interface::CommandInterface( - front_wheels_names_[0], steering_interface_name_, + rear_wheels_names_[1], steering_interface_name_, &joint_command_values_[STATE_DRIVE_LEFT_WHEEL])); command_ifs.emplace_back(command_itfs_.back()); @@ -271,6 +268,8 @@ class TricycleSteeringControllerFixture : public ::testing::Test rear_wheels_names_[0], rear_wheels_names_[1], front_wheels_names_[0]}; double wheelbase_ = 3.24644; + double wheel_track_ = 1.212121; + double front_wheels_radius_ = 0.45; double rear_wheels_radius_ = 0.45; diff --git a/tricycle_steering_controller/test/tricycle_steering_controller_params.yaml b/tricycle_steering_controller/test/tricycle_steering_controller_params.yaml index 743edabe5f..6bfb87a892 100644 --- a/tricycle_steering_controller/test/tricycle_steering_controller_params.yaml +++ b/tricycle_steering_controller/test/tricycle_steering_controller_params.yaml @@ -11,5 +11,6 @@ test_tricycle_steering_controller: front_wheels_names: [steering_axis_joint] wheelbase: 3.24644 + wheel_track: 1.212121 front_wheels_radius: 0.45 rear_wheels_radius: 0.45 From b0adcaacbe769ba11114e285a517ad062046b9b0 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Fri, 13 Jan 2023 22:25:20 +0100 Subject: [PATCH 135/186] Delete steering_controllers.xml --- .../steering_controllers.xml | 20 ------------------- 1 file changed, 20 deletions(-) delete mode 100644 steering_controllers_library/steering_controllers.xml diff --git a/steering_controllers_library/steering_controllers.xml b/steering_controllers_library/steering_controllers.xml deleted file mode 100644 index 01dca4c0ec..0000000000 --- a/steering_controllers_library/steering_controllers.xml +++ /dev/null @@ -1,20 +0,0 @@ - - - - AckermannSteeringController ros2_control controller. - - - - - BicycleSteeringController ros2_control controller. - - - - - TricycleSteeringController ros2_control controller. - - - From c0d348d5eb40f77e331b3e8ce93916ccef35c302 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tomislav=20Petkovi=C4=87?= <51706321+petkovich@users.noreply.github.com> Date: Mon, 16 Jan 2023 10:12:44 +0100 Subject: [PATCH 136/186] Update ackermann_steering_controller/ackermann_steering_controller.xml Co-authored-by: Dr. Denis --- ackermann_steering_controller/ackermann_steering_controller.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ackermann_steering_controller/ackermann_steering_controller.xml b/ackermann_steering_controller/ackermann_steering_controller.xml index de128d6911..2ac2150dd1 100644 --- a/ackermann_steering_controller/ackermann_steering_controller.xml +++ b/ackermann_steering_controller/ackermann_steering_controller.xml @@ -2,7 +2,7 @@ - AckermannSteeringController ros2_control controller. + Steering controller for Ackermann (car-like) kinematics with two traction and two steering joints. From cf4b48a2f3a16bc91c3d6822147e32af08e7dfe8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tomislav=20Petkovi=C4=87?= <51706321+petkovich@users.noreply.github.com> Date: Mon, 16 Jan 2023 10:13:20 +0100 Subject: [PATCH 137/186] Update ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp Co-authored-by: Dr. Denis --- .../ackermann_steering_controller.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp b/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp index 61ee0305a7..75a44083b1 100644 --- a/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp +++ b/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp @@ -27,8 +27,8 @@ namespace ackermann_steering_controller { // name constants for state interfaces -static constexpr size_t STATE_DRIVE_RIGHT_WHEEL = 0; -static constexpr size_t STATE_DRIVE_LEFT_WHEEL = 1; +static constexpr size_t STATE_TRACTION_RIGHT_WHEEL = 0; +static constexpr size_t STATE_TRACTION_LEFT_WHEEL = 1; static constexpr size_t STATE_STEER_RIGHT_WHEEL = 2; static constexpr size_t STATE_STEER_LEFT_WHEEL = 3; From a4c37e29c8458a4832d4f4261ab21b666b58a3d3 Mon Sep 17 00:00:00 2001 From: petkovich Date: Mon, 16 Jan 2023 10:38:16 +0100 Subject: [PATCH 138/186] changed state_drive to state_traction --- .../ackermann_steering_controller.hpp | 4 +++ .../src/ackermann_steering_controller.cpp | 10 ++---- .../test_ackermann_steering_controller.cpp | 32 +++++++++---------- .../test_ackermann_steering_controller.hpp | 12 +++---- .../bicycle_steering_controller.hpp | 2 +- .../src/bicycle_steering_controller.cpp | 2 +- .../test/test_bicycle_steering_controller.cpp | 6 ++-- .../test/test_bicycle_steering_controller.hpp | 7 ++-- .../tricycle_steering_controller.hpp | 4 +-- .../src/tricycle_steering_controller.cpp | 4 +-- .../test_tricycle_steering_controller.cpp | 32 +++++++++---------- .../test_tricycle_steering_controller.hpp | 12 +++---- 12 files changed, 64 insertions(+), 63 deletions(-) diff --git a/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp b/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp index 75a44083b1..5a2320d369 100644 --- a/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp +++ b/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp @@ -36,6 +36,10 @@ static constexpr size_t STATE_STEER_LEFT_WHEEL = 3; static constexpr size_t CMD_DRIVE_WHEEL = 0; static constexpr size_t CMD_STEER = 1; +static constexpr size_t NR_STATE_ITFS = 4; +static constexpr size_t NR_CMD_ITFS = 4; +static constexpr size_t NR_REF_ITFS = 2; + class AckermannSteeringController : public steering_controllers_library::SteeringControllersLibrary { public: diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.cpp b/ackermann_steering_controller/src/ackermann_steering_controller.cpp index 0ac7a4c009..12ae731836 100644 --- a/ackermann_steering_controller/src/ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/src/ackermann_steering_controller.cpp @@ -48,11 +48,7 @@ controller_interface::CallbackReturn AckermannSteeringController::configure_odom odometry_.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); - const size_t nr_state_itfs = 4; - const size_t nr_cmd_itfs = 4; - const size_t nr_ref_itfs = 2; - - set_interface_numbers(nr_state_itfs, nr_cmd_itfs, nr_ref_itfs); + set_interface_numbers(NR_STATE_ITFS, NR_CMD_ITFS, NR_REF_ITFS); RCLCPP_INFO(get_node()->get_logger(), "ackermann odom configure successful"); return controller_interface::CallbackReturn::SUCCESS; @@ -66,8 +62,8 @@ bool AckermannSteeringController::update_odometry(const rclcpp::Duration & perio } else { - const double rear_right_wheel_value = state_interfaces_[STATE_DRIVE_RIGHT_WHEEL].get_value(); - const double rear_left_wheel_value = state_interfaces_[STATE_DRIVE_LEFT_WHEEL].get_value(); + const double rear_right_wheel_value = state_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_value(); + const double rear_left_wheel_value = state_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_value(); const double front_right_steer_position = state_interfaces_[STATE_STEER_RIGHT_WHEEL].get_value(); const double front_left_steer_position = state_interfaces_[STATE_STEER_LEFT_WHEEL].get_value(); diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp index 2f69c29c06..95291315b7 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp @@ -55,11 +55,11 @@ TEST_F(AckermannSteeringControllerTest, check_exported_intefaces) auto command_intefaces = controller_->command_interface_configuration(); ASSERT_EQ(command_intefaces.names.size(), joint_command_values_.size()); EXPECT_EQ( - command_intefaces.names[STATE_DRIVE_RIGHT_WHEEL], - rear_wheels_names_[STATE_DRIVE_RIGHT_WHEEL] + "/" + traction_interface_name_); + command_intefaces.names[STATE_TRACTION_RIGHT_WHEEL], + rear_wheels_names_[STATE_TRACTION_RIGHT_WHEEL] + "/" + traction_interface_name_); EXPECT_EQ( - command_intefaces.names[STATE_DRIVE_LEFT_WHEEL], - rear_wheels_names_[STATE_DRIVE_LEFT_WHEEL] + "/" + traction_interface_name_); + command_intefaces.names[STATE_TRACTION_LEFT_WHEEL], + rear_wheels_names_[STATE_TRACTION_LEFT_WHEEL] + "/" + traction_interface_name_); EXPECT_EQ( command_intefaces.names[STATE_STEER_RIGHT_WHEEL], front_wheels_names_[0] + "/" + steering_interface_name_); @@ -70,11 +70,11 @@ TEST_F(AckermannSteeringControllerTest, check_exported_intefaces) auto state_intefaces = controller_->state_interface_configuration(); ASSERT_EQ(state_intefaces.names.size(), joint_state_values_.size()); EXPECT_EQ( - state_intefaces.names[STATE_DRIVE_RIGHT_WHEEL], - rear_wheels_names_[STATE_DRIVE_RIGHT_WHEEL] + "/" + traction_interface_name_); + state_intefaces.names[STATE_TRACTION_RIGHT_WHEEL], + rear_wheels_names_[STATE_TRACTION_RIGHT_WHEEL] + "/" + traction_interface_name_); EXPECT_EQ( - state_intefaces.names[STATE_DRIVE_LEFT_WHEEL], - rear_wheels_names_[STATE_DRIVE_LEFT_WHEEL] + "/" + traction_interface_name_); + state_intefaces.names[STATE_TRACTION_LEFT_WHEEL], + rear_wheels_names_[STATE_TRACTION_LEFT_WHEEL] + "/" + traction_interface_name_); EXPECT_EQ( state_intefaces.names[STATE_STEER_RIGHT_WHEEL], front_wheels_names_[0] + "/" + steering_interface_name_); @@ -172,10 +172,10 @@ TEST_F(AckermannSteeringControllerTest, test_update_logic) controller_interface::return_type::OK); EXPECT_NEAR( - controller_->command_interfaces_[STATE_DRIVE_RIGHT_WHEEL].get_value(), 0.22222222222222224, + controller_->command_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_value(), 0.22222222222222224, COMMON_THRESHOLD); EXPECT_NEAR( - controller_->command_interfaces_[STATE_DRIVE_LEFT_WHEEL].get_value(), 0.22222222222222224, + controller_->command_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_value(), 0.22222222222222224, COMMON_THRESHOLD); EXPECT_NEAR( controller_->command_interfaces_[STATE_STEER_RIGHT_WHEEL].get_value(), 1.4179821977774734, @@ -208,8 +208,8 @@ TEST_F(AckermannSteeringControllerTest, receive_message_and_publish_updated_stat ControllerStateMsg msg; subscribe_and_get_messages(msg); - EXPECT_EQ(msg.linear_velocity_command.data[STATE_DRIVE_RIGHT_WHEEL], 1.1); - EXPECT_EQ(msg.linear_velocity_command.data[STATE_DRIVE_LEFT_WHEEL], 3.3); + EXPECT_EQ(msg.linear_velocity_command.data[STATE_TRACTION_RIGHT_WHEEL], 1.1); + EXPECT_EQ(msg.linear_velocity_command.data[STATE_TRACTION_LEFT_WHEEL], 3.3); EXPECT_EQ(msg.steering_angle_command.data[0], 2.2); EXPECT_EQ(msg.steering_angle_command.data[1], 4.4); @@ -221,10 +221,10 @@ TEST_F(AckermannSteeringControllerTest, receive_message_and_publish_updated_stat controller_interface::return_type::OK); EXPECT_NEAR( - controller_->command_interfaces_[STATE_DRIVE_RIGHT_WHEEL].get_value(), 0.22222222222222224, + controller_->command_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_value(), 0.22222222222222224, COMMON_THRESHOLD); EXPECT_NEAR( - controller_->command_interfaces_[STATE_DRIVE_LEFT_WHEEL].get_value(), 0.22222222222222224, + controller_->command_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_value(), 0.22222222222222224, COMMON_THRESHOLD); EXPECT_NEAR( controller_->command_interfaces_[STATE_STEER_RIGHT_WHEEL].get_value(), 1.4179821977774734, @@ -235,10 +235,10 @@ TEST_F(AckermannSteeringControllerTest, receive_message_and_publish_updated_stat subscribe_and_get_messages(msg); EXPECT_NEAR( - msg.linear_velocity_command.data[STATE_DRIVE_RIGHT_WHEEL], 0.22222222222222224, + msg.linear_velocity_command.data[STATE_TRACTION_RIGHT_WHEEL], 0.22222222222222224, COMMON_THRESHOLD); EXPECT_NEAR( - msg.linear_velocity_command.data[STATE_DRIVE_LEFT_WHEEL], 0.22222222222222224, + msg.linear_velocity_command.data[STATE_TRACTION_LEFT_WHEEL], 0.22222222222222224, COMMON_THRESHOLD); EXPECT_NEAR(msg.steering_angle_command.data[0], 1.4179821977774734, COMMON_THRESHOLD); EXPECT_NEAR(msg.steering_angle_command.data[1], 1.4179821977774734, COMMON_THRESHOLD); diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp index 7f8075d624..1198343920 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp @@ -39,10 +39,10 @@ using ControllerReferenceMsg = steering_controllers_library::SteeringControllersLibrary::ControllerTwistReferenceMsg; // name constants for state interfaces -using ackermann_steering_controller::STATE_DRIVE_LEFT_WHEEL; -using ackermann_steering_controller::STATE_DRIVE_RIGHT_WHEEL; using ackermann_steering_controller::STATE_STEER_LEFT_WHEEL; using ackermann_steering_controller::STATE_STEER_RIGHT_WHEEL; +using ackermann_steering_controller::STATE_TRACTION_LEFT_WHEEL; +using ackermann_steering_controller::STATE_TRACTION_RIGHT_WHEEL; // name constants for command interfaces using ackermann_steering_controller::CMD_DRIVE_WHEEL; @@ -162,12 +162,12 @@ class AckermannSteeringControllerFixture : public ::testing::Test command_itfs_.emplace_back(hardware_interface::CommandInterface( rear_wheels_names_[0], traction_interface_name_, - &joint_command_values_[STATE_DRIVE_RIGHT_WHEEL])); + &joint_command_values_[STATE_TRACTION_RIGHT_WHEEL])); command_ifs.emplace_back(command_itfs_.back()); command_itfs_.emplace_back(hardware_interface::CommandInterface( rear_wheels_names_[1], steering_interface_name_, - &joint_command_values_[STATE_DRIVE_LEFT_WHEEL])); + &joint_command_values_[STATE_TRACTION_LEFT_WHEEL])); command_ifs.emplace_back(command_itfs_.back()); command_itfs_.emplace_back(hardware_interface::CommandInterface( @@ -186,12 +186,12 @@ class AckermannSteeringControllerFixture : public ::testing::Test state_itfs_.emplace_back(hardware_interface::StateInterface( rear_wheels_names_[0], traction_interface_name_, - &joint_state_values_[STATE_DRIVE_RIGHT_WHEEL])); + &joint_state_values_[STATE_TRACTION_RIGHT_WHEEL])); state_ifs.emplace_back(state_itfs_.back()); state_itfs_.emplace_back(hardware_interface::StateInterface( rear_wheels_names_[1], traction_interface_name_, - &joint_state_values_[STATE_DRIVE_LEFT_WHEEL])); + &joint_state_values_[STATE_TRACTION_LEFT_WHEEL])); state_ifs.emplace_back(state_itfs_.back()); state_itfs_.emplace_back(hardware_interface::StateInterface( diff --git a/bicycle_steering_controller/include/bicycle_steering_controller/bicycle_steering_controller.hpp b/bicycle_steering_controller/include/bicycle_steering_controller/bicycle_steering_controller.hpp index 78987931dc..0bd7f75ae6 100644 --- a/bicycle_steering_controller/include/bicycle_steering_controller/bicycle_steering_controller.hpp +++ b/bicycle_steering_controller/include/bicycle_steering_controller/bicycle_steering_controller.hpp @@ -27,7 +27,7 @@ namespace bicycle_steering_controller { // name constants for state interfaces -static constexpr size_t STATE_DRIVE_WHEEL = 0; +static constexpr size_t STATE_TRACTION_WHEEL = 0; static constexpr size_t STATE_STEER_AXIS = 1; // name constants for command interfaces diff --git a/bicycle_steering_controller/src/bicycle_steering_controller.cpp b/bicycle_steering_controller/src/bicycle_steering_controller.cpp index 91d0e5f198..d7ff686c18 100644 --- a/bicycle_steering_controller/src/bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/src/bicycle_steering_controller.cpp @@ -64,7 +64,7 @@ bool BicycleSteeringController::update_odometry(const rclcpp::Duration & period) } else { - const double rear_wheel_value = state_interfaces_[STATE_DRIVE_WHEEL].get_value(); + const double rear_wheel_value = state_interfaces_[STATE_TRACTION_WHEEL].get_value(); const double steer_position = state_interfaces_[STATE_STEER_AXIS].get_value(); if (!std::isnan(rear_wheel_value) && !std::isnan(steer_position)) { diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp index 0a46844f69..42f87cb969 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp @@ -53,7 +53,7 @@ TEST_F(BicycleSteeringControllerTest, check_exported_intefaces) auto command_intefaces = controller_->command_interface_configuration(); ASSERT_EQ(command_intefaces.names.size(), joint_command_values_.size()); EXPECT_EQ( - command_intefaces.names[STATE_DRIVE_WHEEL], + command_intefaces.names[STATE_TRACTION_WHEEL], rear_wheels_names_[0] + "/" + traction_interface_name_); EXPECT_EQ( command_intefaces.names[STATE_STEER_AXIS], @@ -62,7 +62,7 @@ TEST_F(BicycleSteeringControllerTest, check_exported_intefaces) auto state_intefaces = controller_->state_interface_configuration(); ASSERT_EQ(state_intefaces.names.size(), joint_state_values_.size()); EXPECT_EQ( - state_intefaces.names[STATE_DRIVE_WHEEL], + state_intefaces.names[STATE_TRACTION_WHEEL], rear_wheels_names_[0] + "/" + traction_interface_name_); EXPECT_EQ( state_intefaces.names[STATE_STEER_AXIS], @@ -211,7 +211,7 @@ TEST_F(BicycleSteeringControllerTest, receive_message_and_publish_updated_status controller_interface::return_type::OK); EXPECT_NEAR( - controller_->command_interfaces_[STATE_DRIVE_WHEEL].get_value(), 0.253221, COMMON_THRESHOLD); + controller_->command_interfaces_[STATE_TRACTION_WHEEL].get_value(), 0.253221, COMMON_THRESHOLD); EXPECT_NEAR( controller_->command_interfaces_[STATE_STEER_AXIS].get_value(), 1.4179821977774734, COMMON_THRESHOLD); diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp index 755400ed21..bd976fef45 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp @@ -39,8 +39,8 @@ using ControllerReferenceMsg = steering_controllers_library::SteeringControllersLibrary::ControllerTwistReferenceMsg; // name constants for state interfaces -using bicycle_steering_controller::STATE_DRIVE_WHEEL; using bicycle_steering_controller::STATE_STEER_AXIS; +using bicycle_steering_controller::STATE_TRACTION_WHEEL; // name constants for command interfaces using bicycle_steering_controller::CMD_DRIVE_WHEEL; @@ -159,7 +159,8 @@ class BicycleSteeringControllerFixture : public ::testing::Test command_ifs.reserve(joint_command_values_.size()); command_itfs_.emplace_back(hardware_interface::CommandInterface( - rear_wheels_names_[0], traction_interface_name_, &joint_command_values_[STATE_DRIVE_WHEEL])); + rear_wheels_names_[0], traction_interface_name_, + &joint_command_values_[STATE_TRACTION_WHEEL])); command_ifs.emplace_back(command_itfs_.back()); command_itfs_.emplace_back(hardware_interface::CommandInterface( @@ -171,7 +172,7 @@ class BicycleSteeringControllerFixture : public ::testing::Test state_ifs.reserve(joint_state_values_.size()); state_itfs_.emplace_back(hardware_interface::StateInterface( - rear_wheels_names_[0], traction_interface_name_, &joint_state_values_[STATE_DRIVE_WHEEL])); + rear_wheels_names_[0], traction_interface_name_, &joint_state_values_[STATE_TRACTION_WHEEL])); state_ifs.emplace_back(state_itfs_.back()); state_itfs_.emplace_back(hardware_interface::StateInterface( diff --git a/tricycle_steering_controller/include/tricycle_steering_controller/tricycle_steering_controller.hpp b/tricycle_steering_controller/include/tricycle_steering_controller/tricycle_steering_controller.hpp index 346fdfa17f..2c966d5879 100644 --- a/tricycle_steering_controller/include/tricycle_steering_controller/tricycle_steering_controller.hpp +++ b/tricycle_steering_controller/include/tricycle_steering_controller/tricycle_steering_controller.hpp @@ -27,8 +27,8 @@ namespace tricycle_steering_controller { // name constants for state interfaces -static constexpr size_t STATE_DRIVE_RIGHT_WHEEL = 0; -static constexpr size_t STATE_DRIVE_LEFT_WHEEL = 1; +static constexpr size_t STATE_TRACTION_RIGHT_WHEEL = 0; +static constexpr size_t STATE_TRACTION_LEFT_WHEEL = 1; static constexpr size_t STATE_STEER_AXIS = 2; // name constants for command interfaces diff --git a/tricycle_steering_controller/src/tricycle_steering_controller.cpp b/tricycle_steering_controller/src/tricycle_steering_controller.cpp index ac34fcca31..afb73be33c 100644 --- a/tricycle_steering_controller/src/tricycle_steering_controller.cpp +++ b/tricycle_steering_controller/src/tricycle_steering_controller.cpp @@ -64,8 +64,8 @@ bool TricycleSteeringController::update_odometry(const rclcpp::Duration & period } else { - const double rear_right_wheel_value = state_interfaces_[STATE_DRIVE_RIGHT_WHEEL].get_value(); - const double rear_left_wheel_value = state_interfaces_[STATE_DRIVE_LEFT_WHEEL].get_value(); + const double rear_right_wheel_value = state_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_value(); + const double rear_left_wheel_value = state_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_value(); const double steer_position = state_interfaces_[STATE_STEER_AXIS].get_value(); if ( !std::isnan(rear_right_wheel_value) && !std::isnan(rear_left_wheel_value) && diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp index 16a2195340..f61eaa406a 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp @@ -55,11 +55,11 @@ TEST_F(TricycleSteeringControllerTest, check_exported_intefaces) auto command_intefaces = controller_->command_interface_configuration(); ASSERT_EQ(command_intefaces.names.size(), joint_command_values_.size()); EXPECT_EQ( - command_intefaces.names[STATE_DRIVE_RIGHT_WHEEL], - rear_wheels_names_[STATE_DRIVE_RIGHT_WHEEL] + "/" + traction_interface_name_); + command_intefaces.names[STATE_TRACTION_RIGHT_WHEEL], + rear_wheels_names_[STATE_TRACTION_RIGHT_WHEEL] + "/" + traction_interface_name_); EXPECT_EQ( - command_intefaces.names[STATE_DRIVE_LEFT_WHEEL], - rear_wheels_names_[STATE_DRIVE_LEFT_WHEEL] + "/" + traction_interface_name_); + command_intefaces.names[STATE_TRACTION_LEFT_WHEEL], + rear_wheels_names_[STATE_TRACTION_LEFT_WHEEL] + "/" + traction_interface_name_); EXPECT_EQ( command_intefaces.names[STATE_STEER_AXIS], front_wheels_names_[0] + "/" + steering_interface_name_); @@ -67,11 +67,11 @@ TEST_F(TricycleSteeringControllerTest, check_exported_intefaces) auto state_intefaces = controller_->state_interface_configuration(); ASSERT_EQ(state_intefaces.names.size(), joint_state_values_.size()); EXPECT_EQ( - state_intefaces.names[STATE_DRIVE_RIGHT_WHEEL], - rear_wheels_names_[STATE_DRIVE_RIGHT_WHEEL] + "/" + traction_interface_name_); + state_intefaces.names[STATE_TRACTION_RIGHT_WHEEL], + rear_wheels_names_[STATE_TRACTION_RIGHT_WHEEL] + "/" + traction_interface_name_); EXPECT_EQ( - state_intefaces.names[STATE_DRIVE_LEFT_WHEEL], - rear_wheels_names_[STATE_DRIVE_LEFT_WHEEL] + "/" + traction_interface_name_); + state_intefaces.names[STATE_TRACTION_LEFT_WHEEL], + rear_wheels_names_[STATE_TRACTION_LEFT_WHEEL] + "/" + traction_interface_name_); EXPECT_EQ( state_intefaces.names[STATE_STEER_AXIS], front_wheels_names_[0] + "/" + steering_interface_name_); @@ -166,10 +166,10 @@ TEST_F(TricycleSteeringControllerTest, test_update_logic) controller_interface::return_type::OK); EXPECT_NEAR( - controller_->command_interfaces_[STATE_DRIVE_RIGHT_WHEEL].get_value(), 0.22222222222222224, + controller_->command_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_value(), 0.22222222222222224, COMMON_THRESHOLD); EXPECT_NEAR( - controller_->command_interfaces_[STATE_DRIVE_LEFT_WHEEL].get_value(), 0.22222222222222224, + controller_->command_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_value(), 0.22222222222222224, COMMON_THRESHOLD); EXPECT_NEAR( controller_->command_interfaces_[STATE_STEER_AXIS].get_value(), 1.4179821977774734, @@ -199,8 +199,8 @@ TEST_F(TricycleSteeringControllerTest, receive_message_and_publish_updated_statu ControllerStateMsg msg; subscribe_and_get_messages(msg); - EXPECT_EQ(msg.linear_velocity_command.data[STATE_DRIVE_RIGHT_WHEEL], 1.1); - EXPECT_EQ(msg.linear_velocity_command.data[STATE_DRIVE_LEFT_WHEEL], 3.3); + EXPECT_EQ(msg.linear_velocity_command.data[STATE_TRACTION_RIGHT_WHEEL], 1.1); + EXPECT_EQ(msg.linear_velocity_command.data[STATE_TRACTION_LEFT_WHEEL], 3.3); EXPECT_EQ(msg.steering_angle_command.data[0], 2.2); publish_commands(); @@ -211,10 +211,10 @@ TEST_F(TricycleSteeringControllerTest, receive_message_and_publish_updated_statu controller_interface::return_type::OK); EXPECT_NEAR( - controller_->command_interfaces_[STATE_DRIVE_RIGHT_WHEEL].get_value(), 0.22222222222222224, + controller_->command_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_value(), 0.22222222222222224, COMMON_THRESHOLD); EXPECT_NEAR( - controller_->command_interfaces_[STATE_DRIVE_LEFT_WHEEL].get_value(), 0.22222222222222224, + controller_->command_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_value(), 0.22222222222222224, COMMON_THRESHOLD); EXPECT_NEAR( controller_->command_interfaces_[STATE_STEER_AXIS].get_value(), 1.4179821977774734, @@ -223,10 +223,10 @@ TEST_F(TricycleSteeringControllerTest, receive_message_and_publish_updated_statu subscribe_and_get_messages(msg); EXPECT_NEAR( - msg.linear_velocity_command.data[STATE_DRIVE_RIGHT_WHEEL], 0.22222222222222224, + msg.linear_velocity_command.data[STATE_TRACTION_RIGHT_WHEEL], 0.22222222222222224, COMMON_THRESHOLD); EXPECT_NEAR( - msg.linear_velocity_command.data[STATE_DRIVE_LEFT_WHEEL], 0.22222222222222224, + msg.linear_velocity_command.data[STATE_TRACTION_LEFT_WHEEL], 0.22222222222222224, COMMON_THRESHOLD); EXPECT_NEAR(msg.steering_angle_command.data[0], 1.4179821977774734, COMMON_THRESHOLD); } diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp index 7ba172b8de..c1c7b59150 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp @@ -39,9 +39,9 @@ using ControllerReferenceMsg = steering_controllers_library::SteeringControllersLibrary::ControllerTwistReferenceMsg; // name constants for state interfaces -using tricycle_steering_controller::STATE_DRIVE_LEFT_WHEEL; -using tricycle_steering_controller::STATE_DRIVE_RIGHT_WHEEL; using tricycle_steering_controller::STATE_STEER_AXIS; +using tricycle_steering_controller::STATE_TRACTION_LEFT_WHEEL; +using tricycle_steering_controller::STATE_TRACTION_RIGHT_WHEEL; // name constants for command interfaces using tricycle_steering_controller::CMD_DRIVE_WHEEL; @@ -161,12 +161,12 @@ class TricycleSteeringControllerFixture : public ::testing::Test command_itfs_.emplace_back(hardware_interface::CommandInterface( rear_wheels_names_[0], traction_interface_name_, - &joint_command_values_[STATE_DRIVE_RIGHT_WHEEL])); + &joint_command_values_[STATE_TRACTION_RIGHT_WHEEL])); command_ifs.emplace_back(command_itfs_.back()); command_itfs_.emplace_back(hardware_interface::CommandInterface( rear_wheels_names_[1], steering_interface_name_, - &joint_command_values_[STATE_DRIVE_LEFT_WHEEL])); + &joint_command_values_[STATE_TRACTION_LEFT_WHEEL])); command_ifs.emplace_back(command_itfs_.back()); command_itfs_.emplace_back(hardware_interface::CommandInterface( @@ -179,12 +179,12 @@ class TricycleSteeringControllerFixture : public ::testing::Test state_itfs_.emplace_back(hardware_interface::StateInterface( rear_wheels_names_[0], traction_interface_name_, - &joint_state_values_[STATE_DRIVE_RIGHT_WHEEL])); + &joint_state_values_[STATE_TRACTION_RIGHT_WHEEL])); state_ifs.emplace_back(state_itfs_.back()); state_itfs_.emplace_back(hardware_interface::StateInterface( rear_wheels_names_[1], traction_interface_name_, - &joint_state_values_[STATE_DRIVE_LEFT_WHEEL])); + &joint_state_values_[STATE_TRACTION_LEFT_WHEEL])); state_ifs.emplace_back(state_itfs_.back()); state_itfs_.emplace_back(hardware_interface::StateInterface( From 006b241235b66260672b70f87dabc7c13642cdeb Mon Sep 17 00:00:00 2001 From: petkovich Date: Mon, 16 Jan 2023 10:40:43 +0100 Subject: [PATCH 139/186] nr itfs in the header --- .../bicycle_steering_controller.hpp | 5 +++++ .../src/bicycle_steering_controller.cpp | 6 +----- .../tricycle_steering_controller.hpp | 4 ++++ .../src/tricycle_steering_controller.cpp | 6 +----- 4 files changed, 11 insertions(+), 10 deletions(-) diff --git a/bicycle_steering_controller/include/bicycle_steering_controller/bicycle_steering_controller.hpp b/bicycle_steering_controller/include/bicycle_steering_controller/bicycle_steering_controller.hpp index 0bd7f75ae6..c5f11cc68c 100644 --- a/bicycle_steering_controller/include/bicycle_steering_controller/bicycle_steering_controller.hpp +++ b/bicycle_steering_controller/include/bicycle_steering_controller/bicycle_steering_controller.hpp @@ -33,6 +33,11 @@ static constexpr size_t STATE_STEER_AXIS = 1; // name constants for command interfaces static constexpr size_t CMD_DRIVE_WHEEL = 0; static constexpr size_t CMD_STEER = 1; + +static constexpr size_t NR_STATE_ITFS = 2; +static constexpr size_t NR_CMD_ITFS = 2; +static constexpr size_t NR_REF_ITFS = 2; + class BicycleSteeringController : public steering_controllers_library::SteeringControllersLibrary { public: diff --git a/bicycle_steering_controller/src/bicycle_steering_controller.cpp b/bicycle_steering_controller/src/bicycle_steering_controller.cpp index d7ff686c18..9d35f3a52e 100644 --- a/bicycle_steering_controller/src/bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/src/bicycle_steering_controller.cpp @@ -46,11 +46,7 @@ controller_interface::CallbackReturn BicycleSteeringController::configure_odomet odometry_.set_odometry_type(steering_odometry::BICYCLE_CONFIG); - const size_t nr_state_itfs = 2; - const size_t nr_cmd_itfs = 2; - const size_t nr_ref_itfs = 2; - - set_interface_numbers(nr_state_itfs, nr_cmd_itfs, nr_ref_itfs); + set_interface_numbers(NR_STATE_ITFS, NR_CMD_ITFS, NR_REF_ITFS); RCLCPP_INFO(get_node()->get_logger(), "bicycle odometry configure successful"); return controller_interface::CallbackReturn::SUCCESS; diff --git a/tricycle_steering_controller/include/tricycle_steering_controller/tricycle_steering_controller.hpp b/tricycle_steering_controller/include/tricycle_steering_controller/tricycle_steering_controller.hpp index 2c966d5879..6c68b6cabd 100644 --- a/tricycle_steering_controller/include/tricycle_steering_controller/tricycle_steering_controller.hpp +++ b/tricycle_steering_controller/include/tricycle_steering_controller/tricycle_steering_controller.hpp @@ -35,6 +35,10 @@ static constexpr size_t STATE_STEER_AXIS = 2; static constexpr size_t CMD_DRIVE_WHEEL = 0; static constexpr size_t CMD_STEER = 1; +static constexpr size_t NR_STATE_ITFS = 3; +static constexpr size_t NR_CMD_ITFS = 3; +static constexpr size_t NR_REF_ITFS = 2; + class TricycleSteeringController : public steering_controllers_library::SteeringControllersLibrary { public: diff --git a/tricycle_steering_controller/src/tricycle_steering_controller.cpp b/tricycle_steering_controller/src/tricycle_steering_controller.cpp index afb73be33c..f89d78a52c 100644 --- a/tricycle_steering_controller/src/tricycle_steering_controller.cpp +++ b/tricycle_steering_controller/src/tricycle_steering_controller.cpp @@ -46,11 +46,7 @@ controller_interface::CallbackReturn TricycleSteeringController::configure_odome odometry_.set_odometry_type(steering_odometry::TRICYCLE_CONFIG); - const size_t nr_state_itfs = 3; - const size_t nr_cmd_itfs = 3; - const size_t nr_ref_itfs = 2; - - set_interface_numbers(nr_state_itfs, nr_cmd_itfs, nr_ref_itfs); + set_interface_numbers(NR_STATE_ITFS, NR_CMD_ITFS, NR_REF_ITFS); RCLCPP_INFO(get_node()->get_logger(), "tricycle odom configure successful"); return controller_interface::CallbackReturn::SUCCESS; From 7e31f935d1d2918ddb7bed4aa92fe93af56ab995 Mon Sep 17 00:00:00 2001 From: petkovich Date: Mon, 16 Jan 2023 10:55:52 +0100 Subject: [PATCH 140/186] ackermann tests cmd interfaces --- .../ackermann_steering_controller.hpp | 6 ++- .../test_ackermann_steering_controller.cpp | 38 ++++++++++--------- .../test_ackermann_steering_controller.hpp | 14 ++++--- 3 files changed, 32 insertions(+), 26 deletions(-) diff --git a/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp b/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp index 5a2320d369..61dc5d2ba7 100644 --- a/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp +++ b/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp @@ -33,8 +33,10 @@ static constexpr size_t STATE_STEER_RIGHT_WHEEL = 2; static constexpr size_t STATE_STEER_LEFT_WHEEL = 3; // name constants for command interfaces -static constexpr size_t CMD_DRIVE_WHEEL = 0; -static constexpr size_t CMD_STEER = 1; +static constexpr size_t CMD_TRACTION_RIGHT_WHEEL = 0; +static constexpr size_t CMD_TRACTION_LEFT_WHEEL = 1; +static constexpr size_t CMD_STEER_RIGHT_WHEEL = 2; +static constexpr size_t CMD_STEER_LEFT_WHEEL = 3; static constexpr size_t NR_STATE_ITFS = 4; static constexpr size_t NR_CMD_ITFS = 4; diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp index 95291315b7..dcc395ac9b 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp @@ -55,26 +55,26 @@ TEST_F(AckermannSteeringControllerTest, check_exported_intefaces) auto command_intefaces = controller_->command_interface_configuration(); ASSERT_EQ(command_intefaces.names.size(), joint_command_values_.size()); EXPECT_EQ( - command_intefaces.names[STATE_TRACTION_RIGHT_WHEEL], - rear_wheels_names_[STATE_TRACTION_RIGHT_WHEEL] + "/" + traction_interface_name_); + command_intefaces.names[CMD_TRACTION_RIGHT_WHEEL], + rear_wheels_names_[0] + "/" + traction_interface_name_); EXPECT_EQ( - command_intefaces.names[STATE_TRACTION_LEFT_WHEEL], - rear_wheels_names_[STATE_TRACTION_LEFT_WHEEL] + "/" + traction_interface_name_); + command_intefaces.names[CMD_TRACTION_LEFT_WHEEL], + rear_wheels_names_[1] + "/" + traction_interface_name_); EXPECT_EQ( - command_intefaces.names[STATE_STEER_RIGHT_WHEEL], + command_intefaces.names[CMD_STEER_RIGHT_WHEEL], front_wheels_names_[0] + "/" + steering_interface_name_); EXPECT_EQ( - command_intefaces.names[STATE_STEER_LEFT_WHEEL], + command_intefaces.names[CMD_STEER_LEFT_WHEEL], front_wheels_names_[1] + "/" + steering_interface_name_); auto state_intefaces = controller_->state_interface_configuration(); ASSERT_EQ(state_intefaces.names.size(), joint_state_values_.size()); EXPECT_EQ( state_intefaces.names[STATE_TRACTION_RIGHT_WHEEL], - rear_wheels_names_[STATE_TRACTION_RIGHT_WHEEL] + "/" + traction_interface_name_); + rear_wheels_names_[0] + "/" + traction_interface_name_); EXPECT_EQ( state_intefaces.names[STATE_TRACTION_LEFT_WHEEL], - rear_wheels_names_[STATE_TRACTION_LEFT_WHEEL] + "/" + traction_interface_name_); + rear_wheels_names_[1] + "/" + traction_interface_name_); EXPECT_EQ( state_intefaces.names[STATE_STEER_RIGHT_WHEEL], front_wheels_names_[0] + "/" + steering_interface_name_); @@ -172,16 +172,16 @@ TEST_F(AckermannSteeringControllerTest, test_update_logic) controller_interface::return_type::OK); EXPECT_NEAR( - controller_->command_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_value(), 0.22222222222222224, + controller_->command_interfaces_[CMD_TRACTION_RIGHT_WHEEL].get_value(), 0.22222222222222224, COMMON_THRESHOLD); EXPECT_NEAR( - controller_->command_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_value(), 0.22222222222222224, + controller_->command_interfaces_[CMD_TRACTION_LEFT_WHEEL].get_value(), 0.22222222222222224, COMMON_THRESHOLD); EXPECT_NEAR( - controller_->command_interfaces_[STATE_STEER_RIGHT_WHEEL].get_value(), 1.4179821977774734, + controller_->command_interfaces_[CMD_STEER_RIGHT_WHEEL].get_value(), 1.4179821977774734, COMMON_THRESHOLD); EXPECT_NEAR( - controller_->command_interfaces_[STATE_STEER_LEFT_WHEEL].get_value(), 1.4179821977774734, + controller_->command_interfaces_[CMD_STEER_LEFT_WHEEL].get_value(), 1.4179821977774734, COMMON_THRESHOLD); EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x)); @@ -221,28 +221,30 @@ TEST_F(AckermannSteeringControllerTest, receive_message_and_publish_updated_stat controller_interface::return_type::OK); EXPECT_NEAR( - controller_->command_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_value(), 0.22222222222222224, + controller_->command_interfaces_[CMD_TRACTION_RIGHT_WHEEL].get_value(), 0.22222222222222224, COMMON_THRESHOLD); EXPECT_NEAR( - controller_->command_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_value(), 0.22222222222222224, + controller_->command_interfaces_[CMD_TRACTION_LEFT_WHEEL].get_value(), 0.22222222222222224, COMMON_THRESHOLD); EXPECT_NEAR( - controller_->command_interfaces_[STATE_STEER_RIGHT_WHEEL].get_value(), 1.4179821977774734, + controller_->command_interfaces_[CMD_STEER_RIGHT_WHEEL].get_value(), 1.4179821977774734, COMMON_THRESHOLD); EXPECT_NEAR( - controller_->command_interfaces_[STATE_STEER_LEFT_WHEEL].get_value(), 1.4179821977774734, + controller_->command_interfaces_[CMD_STEER_LEFT_WHEEL].get_value(), 1.4179821977774734, COMMON_THRESHOLD); + subscribe_and_get_messages(msg); EXPECT_NEAR( - msg.linear_velocity_command.data[STATE_TRACTION_RIGHT_WHEEL], 0.22222222222222224, + msg.linear_velocity_command.data[CMD_TRACTION_RIGHT_WHEEL], 0.22222222222222224, COMMON_THRESHOLD); EXPECT_NEAR( - msg.linear_velocity_command.data[STATE_TRACTION_LEFT_WHEEL], 0.22222222222222224, + msg.linear_velocity_command.data[CMD_TRACTION_LEFT_WHEEL], 0.22222222222222224, COMMON_THRESHOLD); EXPECT_NEAR(msg.steering_angle_command.data[0], 1.4179821977774734, COMMON_THRESHOLD); EXPECT_NEAR(msg.steering_angle_command.data[1], 1.4179821977774734, COMMON_THRESHOLD); } + int main(int argc, char ** argv) { ::testing::InitGoogleTest(&argc, argv); diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp index 1198343920..e8dff4abde 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp @@ -45,8 +45,10 @@ using ackermann_steering_controller::STATE_TRACTION_LEFT_WHEEL; using ackermann_steering_controller::STATE_TRACTION_RIGHT_WHEEL; // name constants for command interfaces -using ackermann_steering_controller::CMD_DRIVE_WHEEL; -using ackermann_steering_controller::CMD_STEER; +using ackermann_steering_controller::CMD_STEER_LEFT_WHEEL; +using ackermann_steering_controller::CMD_STEER_RIGHT_WHEEL; +using ackermann_steering_controller::CMD_TRACTION_LEFT_WHEEL; +using ackermann_steering_controller::CMD_TRACTION_RIGHT_WHEEL; namespace { @@ -162,22 +164,22 @@ class AckermannSteeringControllerFixture : public ::testing::Test command_itfs_.emplace_back(hardware_interface::CommandInterface( rear_wheels_names_[0], traction_interface_name_, - &joint_command_values_[STATE_TRACTION_RIGHT_WHEEL])); + &joint_command_values_[CMD_TRACTION_RIGHT_WHEEL])); command_ifs.emplace_back(command_itfs_.back()); command_itfs_.emplace_back(hardware_interface::CommandInterface( rear_wheels_names_[1], steering_interface_name_, - &joint_command_values_[STATE_TRACTION_LEFT_WHEEL])); + &joint_command_values_[CMD_TRACTION_LEFT_WHEEL])); command_ifs.emplace_back(command_itfs_.back()); command_itfs_.emplace_back(hardware_interface::CommandInterface( front_wheels_names_[0], steering_interface_name_, - &joint_command_values_[STATE_STEER_RIGHT_WHEEL])); + &joint_command_values_[CMD_STEER_RIGHT_WHEEL])); command_ifs.emplace_back(command_itfs_.back()); command_itfs_.emplace_back(hardware_interface::CommandInterface( front_wheels_names_[1], steering_interface_name_, - &joint_command_values_[STATE_STEER_LEFT_WHEEL])); + &joint_command_values_[CMD_STEER_LEFT_WHEEL])); command_ifs.emplace_back(command_itfs_.back()); std::vector state_ifs; From c2849c93be2cdec190ee1a19a0d5848461253877 Mon Sep 17 00:00:00 2001 From: petkovich Date: Mon, 16 Jan 2023 11:38:47 +0100 Subject: [PATCH 141/186] control tricycle constants --- .../bicycle_steering_controller.hpp | 2 +- .../tricycle_steering_controller.hpp | 5 ++-- .../test_tricycle_steering_controller.cpp | 26 +++++++++---------- .../test_tricycle_steering_controller.hpp | 11 ++++---- 4 files changed, 23 insertions(+), 21 deletions(-) diff --git a/bicycle_steering_controller/include/bicycle_steering_controller/bicycle_steering_controller.hpp b/bicycle_steering_controller/include/bicycle_steering_controller/bicycle_steering_controller.hpp index c5f11cc68c..b69746d1a7 100644 --- a/bicycle_steering_controller/include/bicycle_steering_controller/bicycle_steering_controller.hpp +++ b/bicycle_steering_controller/include/bicycle_steering_controller/bicycle_steering_controller.hpp @@ -32,7 +32,7 @@ static constexpr size_t STATE_STEER_AXIS = 1; // name constants for command interfaces static constexpr size_t CMD_DRIVE_WHEEL = 0; -static constexpr size_t CMD_STEER = 1; +static constexpr size_t CMD_STEER_WHEEL = 1; static constexpr size_t NR_STATE_ITFS = 2; static constexpr size_t NR_CMD_ITFS = 2; diff --git a/tricycle_steering_controller/include/tricycle_steering_controller/tricycle_steering_controller.hpp b/tricycle_steering_controller/include/tricycle_steering_controller/tricycle_steering_controller.hpp index 6c68b6cabd..f80debb8a1 100644 --- a/tricycle_steering_controller/include/tricycle_steering_controller/tricycle_steering_controller.hpp +++ b/tricycle_steering_controller/include/tricycle_steering_controller/tricycle_steering_controller.hpp @@ -32,8 +32,9 @@ static constexpr size_t STATE_TRACTION_LEFT_WHEEL = 1; static constexpr size_t STATE_STEER_AXIS = 2; // name constants for command interfaces -static constexpr size_t CMD_DRIVE_WHEEL = 0; -static constexpr size_t CMD_STEER = 1; +static constexpr size_t CMD_TRACTION_RIGHT_WHEEL = 0; +static constexpr size_t CMD_TRACTION_LEFT_WHEEL = 1; +static constexpr size_t CMD_STEER_WHEEL = 2; static constexpr size_t NR_STATE_ITFS = 3; static constexpr size_t NR_CMD_ITFS = 3; diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp index f61eaa406a..f3ae849d52 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp @@ -55,23 +55,23 @@ TEST_F(TricycleSteeringControllerTest, check_exported_intefaces) auto command_intefaces = controller_->command_interface_configuration(); ASSERT_EQ(command_intefaces.names.size(), joint_command_values_.size()); EXPECT_EQ( - command_intefaces.names[STATE_TRACTION_RIGHT_WHEEL], - rear_wheels_names_[STATE_TRACTION_RIGHT_WHEEL] + "/" + traction_interface_name_); + command_intefaces.names[CMD_TRACTION_RIGHT_WHEEL], + rear_wheels_names_[0] + "/" + traction_interface_name_); EXPECT_EQ( - command_intefaces.names[STATE_TRACTION_LEFT_WHEEL], - rear_wheels_names_[STATE_TRACTION_LEFT_WHEEL] + "/" + traction_interface_name_); + command_intefaces.names[CMD_TRACTION_LEFT_WHEEL], + rear_wheels_names_[1] + "/" + traction_interface_name_); EXPECT_EQ( - command_intefaces.names[STATE_STEER_AXIS], + command_intefaces.names[CMD_STEER_WHEEL], front_wheels_names_[0] + "/" + steering_interface_name_); auto state_intefaces = controller_->state_interface_configuration(); ASSERT_EQ(state_intefaces.names.size(), joint_state_values_.size()); EXPECT_EQ( state_intefaces.names[STATE_TRACTION_RIGHT_WHEEL], - rear_wheels_names_[STATE_TRACTION_RIGHT_WHEEL] + "/" + traction_interface_name_); + rear_wheels_names_[0] + "/" + traction_interface_name_); EXPECT_EQ( state_intefaces.names[STATE_TRACTION_LEFT_WHEEL], - rear_wheels_names_[STATE_TRACTION_LEFT_WHEEL] + "/" + traction_interface_name_); + rear_wheels_names_[1] + "/" + traction_interface_name_); EXPECT_EQ( state_intefaces.names[STATE_STEER_AXIS], front_wheels_names_[0] + "/" + steering_interface_name_); @@ -166,13 +166,13 @@ TEST_F(TricycleSteeringControllerTest, test_update_logic) controller_interface::return_type::OK); EXPECT_NEAR( - controller_->command_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_value(), 0.22222222222222224, + controller_->command_interfaces_[CMD_TRACTION_RIGHT_WHEEL].get_value(), 0.22222222222222224, COMMON_THRESHOLD); EXPECT_NEAR( - controller_->command_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_value(), 0.22222222222222224, + controller_->command_interfaces_[CMD_TRACTION_LEFT_WHEEL].get_value(), 0.22222222222222224, COMMON_THRESHOLD); EXPECT_NEAR( - controller_->command_interfaces_[STATE_STEER_AXIS].get_value(), 1.4179821977774734, + controller_->command_interfaces_[CMD_STEER_WHEEL].get_value(), 1.4179821977774734, COMMON_THRESHOLD); EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x)); @@ -211,13 +211,13 @@ TEST_F(TricycleSteeringControllerTest, receive_message_and_publish_updated_statu controller_interface::return_type::OK); EXPECT_NEAR( - controller_->command_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_value(), 0.22222222222222224, + controller_->command_interfaces_[CMD_TRACTION_RIGHT_WHEEL].get_value(), 0.22222222222222224, COMMON_THRESHOLD); EXPECT_NEAR( - controller_->command_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_value(), 0.22222222222222224, + controller_->command_interfaces_[CMD_TRACTION_LEFT_WHEEL].get_value(), 0.22222222222222224, COMMON_THRESHOLD); EXPECT_NEAR( - controller_->command_interfaces_[STATE_STEER_AXIS].get_value(), 1.4179821977774734, + controller_->command_interfaces_[CMD_STEER_WHEEL].get_value(), 1.4179821977774734, COMMON_THRESHOLD); subscribe_and_get_messages(msg); diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp index c1c7b59150..24eb079fa1 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp @@ -44,8 +44,9 @@ using tricycle_steering_controller::STATE_TRACTION_LEFT_WHEEL; using tricycle_steering_controller::STATE_TRACTION_RIGHT_WHEEL; // name constants for command interfaces -using tricycle_steering_controller::CMD_DRIVE_WHEEL; -using tricycle_steering_controller::CMD_STEER; +using tricycle_steering_controller::CMD_STEER_WHEEL; +using tricycle_steering_controller::CMD_TRACTION_LEFT_WHEEL; +using tricycle_steering_controller::CMD_TRACTION_RIGHT_WHEEL; namespace { @@ -161,16 +162,16 @@ class TricycleSteeringControllerFixture : public ::testing::Test command_itfs_.emplace_back(hardware_interface::CommandInterface( rear_wheels_names_[0], traction_interface_name_, - &joint_command_values_[STATE_TRACTION_RIGHT_WHEEL])); + &joint_command_values_[CMD_TRACTION_RIGHT_WHEEL])); command_ifs.emplace_back(command_itfs_.back()); command_itfs_.emplace_back(hardware_interface::CommandInterface( rear_wheels_names_[1], steering_interface_name_, - &joint_command_values_[STATE_TRACTION_LEFT_WHEEL])); + &joint_command_values_[CMD_TRACTION_LEFT_WHEEL])); command_ifs.emplace_back(command_itfs_.back()); command_itfs_.emplace_back(hardware_interface::CommandInterface( - front_wheels_names_[0], steering_interface_name_, &joint_command_values_[STATE_STEER_AXIS])); + front_wheels_names_[0], steering_interface_name_, &joint_command_values_[CMD_STEER_WHEEL])); command_ifs.emplace_back(command_itfs_.back()); std::vector state_ifs; From 5198ddd4284335f5bfafa1efc51df666b48c669c Mon Sep 17 00:00:00 2001 From: petkovich Date: Mon, 16 Jan 2023 11:41:25 +0100 Subject: [PATCH 142/186] control bicycle constants --- .../test/test_bicycle_steering_controller.cpp | 14 ++++++++------ .../test/test_bicycle_steering_controller.hpp | 7 +++---- 2 files changed, 11 insertions(+), 10 deletions(-) diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp index 42f87cb969..95dabb655a 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp @@ -53,10 +53,10 @@ TEST_F(BicycleSteeringControllerTest, check_exported_intefaces) auto command_intefaces = controller_->command_interface_configuration(); ASSERT_EQ(command_intefaces.names.size(), joint_command_values_.size()); EXPECT_EQ( - command_intefaces.names[STATE_TRACTION_WHEEL], + command_intefaces.names[CMD_DRIVE_WHEEL], rear_wheels_names_[0] + "/" + traction_interface_name_); EXPECT_EQ( - command_intefaces.names[STATE_STEER_AXIS], + command_intefaces.names[CMD_STEER_WHEEL], front_wheels_names_[0] + "/" + steering_interface_name_); auto state_intefaces = controller_->state_interface_configuration(); @@ -157,9 +157,11 @@ TEST_F(BicycleSteeringControllerTest, test_update_logic) controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - EXPECT_NEAR(controller_->command_interfaces_[0].get_value(), 0.253221, COMMON_THRESHOLD); EXPECT_NEAR( - controller_->command_interfaces_[1].get_value(), 1.4179821977774734, COMMON_THRESHOLD); + controller_->command_interfaces_[CMD_DRIVE_WHEEL].get_value(), 0.253221, COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[CMD_STEER_WHEEL].get_value(), 1.4179821977774734, + COMMON_THRESHOLD); EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x)); EXPECT_EQ(controller_->reference_interfaces_.size(), joint_reference_interfaces_.size()); @@ -211,9 +213,9 @@ TEST_F(BicycleSteeringControllerTest, receive_message_and_publish_updated_status controller_interface::return_type::OK); EXPECT_NEAR( - controller_->command_interfaces_[STATE_TRACTION_WHEEL].get_value(), 0.253221, COMMON_THRESHOLD); + controller_->command_interfaces_[CMD_DRIVE_WHEEL].get_value(), 0.253221, COMMON_THRESHOLD); EXPECT_NEAR( - controller_->command_interfaces_[STATE_STEER_AXIS].get_value(), 1.4179821977774734, + controller_->command_interfaces_[CMD_STEER_WHEEL].get_value(), 1.4179821977774734, COMMON_THRESHOLD); subscribe_and_get_messages(msg); diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp index bd976fef45..303eb1367f 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp @@ -44,7 +44,7 @@ using bicycle_steering_controller::STATE_TRACTION_WHEEL; // name constants for command interfaces using bicycle_steering_controller::CMD_DRIVE_WHEEL; -using bicycle_steering_controller::CMD_STEER; +using bicycle_steering_controller::CMD_STEER_WHEEL; namespace { @@ -159,12 +159,11 @@ class BicycleSteeringControllerFixture : public ::testing::Test command_ifs.reserve(joint_command_values_.size()); command_itfs_.emplace_back(hardware_interface::CommandInterface( - rear_wheels_names_[0], traction_interface_name_, - &joint_command_values_[STATE_TRACTION_WHEEL])); + rear_wheels_names_[0], traction_interface_name_, &joint_command_values_[CMD_DRIVE_WHEEL])); command_ifs.emplace_back(command_itfs_.back()); command_itfs_.emplace_back(hardware_interface::CommandInterface( - front_wheels_names_[0], steering_interface_name_, &joint_command_values_[STATE_STEER_AXIS])); + front_wheels_names_[0], steering_interface_name_, &joint_command_values_[CMD_STEER_WHEEL])); command_ifs.emplace_back(command_itfs_.back()); std::vector state_ifs; From e92a4bd3f4dd62c669b66caa003541982fab823f Mon Sep 17 00:00:00 2001 From: petkovich Date: Mon, 16 Jan 2023 11:44:37 +0100 Subject: [PATCH 143/186] package descriptions, wheelbase param desc --- .../src/ackermann_steering_controller.yaml | 2 +- bicycle_steering_controller/bicycle_steering_controller.xml | 2 +- .../src/bicycle_steering_controller.yaml | 2 +- .../src/tricycle_steering_controller.yaml | 2 +- tricycle_steering_controller/tricycle_steering_controller.xml | 2 +- 5 files changed, 5 insertions(+), 5 deletions(-) diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.yaml b/ackermann_steering_controller/src/ackermann_steering_controller.yaml index a9f5d1b700..90ca93725e 100644 --- a/ackermann_steering_controller/src/ackermann_steering_controller.yaml +++ b/ackermann_steering_controller/src/ackermann_steering_controller.yaml @@ -19,7 +19,7 @@ ackermann_steering_controller: { type: double, default_value: 0.0, - description: "Wheelbase length.", + description: "Distance between front and rear wheels.", read_only: false, } diff --git a/bicycle_steering_controller/bicycle_steering_controller.xml b/bicycle_steering_controller/bicycle_steering_controller.xml index 4d40e4c7bd..644c8840fa 100644 --- a/bicycle_steering_controller/bicycle_steering_controller.xml +++ b/bicycle_steering_controller/bicycle_steering_controller.xml @@ -2,7 +2,7 @@ - BicycleSteeringController ros2_control controller. + Steering controller for Bicycle kinematics with one traction and one steering joint. diff --git a/bicycle_steering_controller/src/bicycle_steering_controller.yaml b/bicycle_steering_controller/src/bicycle_steering_controller.yaml index 3f48c7d593..45b4355a3d 100644 --- a/bicycle_steering_controller/src/bicycle_steering_controller.yaml +++ b/bicycle_steering_controller/src/bicycle_steering_controller.yaml @@ -3,7 +3,7 @@ bicycle_steering_controller: { type: double, default_value: 0.0, - description: "Wheelbase length.", + description: "Distance between front and rear wheel.", read_only: false, } diff --git a/tricycle_steering_controller/src/tricycle_steering_controller.yaml b/tricycle_steering_controller/src/tricycle_steering_controller.yaml index 9fc36b738d..a5975ee6b8 100644 --- a/tricycle_steering_controller/src/tricycle_steering_controller.yaml +++ b/tricycle_steering_controller/src/tricycle_steering_controller.yaml @@ -11,7 +11,7 @@ tricycle_steering_controller: { type: double, default_value: 0.0, - description: "Wheelbase length.", + description: "Distance between front and rear wheels.", read_only: false, } diff --git a/tricycle_steering_controller/tricycle_steering_controller.xml b/tricycle_steering_controller/tricycle_steering_controller.xml index 8b8d9fe3cc..f0d5d85467 100644 --- a/tricycle_steering_controller/tricycle_steering_controller.xml +++ b/tricycle_steering_controller/tricycle_steering_controller.xml @@ -2,7 +2,7 @@ - TricycleSteeringController ros2_control controller. + Steering controller for Tricycle kinematics with two traction and one steering joint. From 304b288102975186cfa3895972146dc6da821bb0 Mon Sep 17 00:00:00 2001 From: petkovich Date: Mon, 16 Jan 2023 12:41:48 +0100 Subject: [PATCH 144/186] preceeding ackermann --- ackermann_steering_controller/CMakeLists.txt | 20 +++--- ...steering_controller_preceeding_params.yaml | 22 ++++-- .../test_ackermann_steering_controller.hpp | 6 ++ ...kermann_steering_controller_preceeding.cpp | 70 ++++++++++++------- 4 files changed, 75 insertions(+), 43 deletions(-) diff --git a/ackermann_steering_controller/CMakeLists.txt b/ackermann_steering_controller/CMakeLists.txt index 8fbd0bd301..15e56a0ad0 100644 --- a/ackermann_steering_controller/CMakeLists.txt +++ b/ackermann_steering_controller/CMakeLists.txt @@ -81,16 +81,16 @@ if(BUILD_TESTING) hardware_interface ) - # add_rostest_with_parameters_gmock( - # test_ackermann_steering_controller_preceeding test/test_ackermann_steering_controller_preceeding.cpp - # ${CMAKE_CURRENT_SOURCE_DIR}/test/ackermann_steering_controller_preceeding_params.yaml) - # target_include_directories(test_ackermann_steering_controller_preceeding PRIVATE include) - # target_link_libraries(test_ackermann_steering_controller_preceeding ackermann_steering_controller) - # ament_target_dependencies( - # test_ackermann_steering_controller_preceeding - # controller_interface - # hardware_interface - # ) + add_rostest_with_parameters_gmock( + test_ackermann_steering_controller_preceeding test/test_ackermann_steering_controller_preceeding.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/ackermann_steering_controller_preceeding_params.yaml) + target_include_directories(test_ackermann_steering_controller_preceeding PRIVATE include) + target_link_libraries(test_ackermann_steering_controller_preceeding ackermann_steering_controller) + ament_target_dependencies( + test_ackermann_steering_controller_preceeding + controller_interface + hardware_interface + ) endif() ament_export_include_directories( diff --git a/ackermann_steering_controller/test/ackermann_steering_controller_preceeding_params.yaml b/ackermann_steering_controller/test/ackermann_steering_controller_preceeding_params.yaml index 0fe973bb8d..ecb1b071e3 100644 --- a/ackermann_steering_controller/test/ackermann_steering_controller_preceeding_params.yaml +++ b/ackermann_steering_controller/test/ackermann_steering_controller_preceeding_params.yaml @@ -1,9 +1,17 @@ test_ackermann_steering_controller: ros__parameters: - joints: - - joint1 - - interface_name: acceleration - - state_joints: - - joint1state + reference_timeout: 2.0 + front_steering: true + open_loop: false + velocity_rolling_window_size: 10 + position_feedback: false + use_stamped_vel: true + rear_wheels_names: [pid_controller/rear_right_wheel_joint, pid_controller/rear_left_wheel_joint] + front_wheels_names: [pid_controller/front_right_steering_joint, pid_controller/front_left_steering_joint] + rear_wheels_state_names: [rear_right_wheel_joint, rear_left_wheel_joint] + front_wheels_state_names: [front_right_steering_joint, front_left_steering_joint] + wheelbase: 3.24644 + front_wheel_track: 2.12321 + rear_wheel_track: 1.76868 + front_wheels_radius: 0.45 + rear_wheels_radius: 0.45 diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp index e8dff4abde..39e5ac3fc5 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp @@ -280,8 +280,13 @@ class AckermannSteeringControllerFixture : public ::testing::Test bool position_feedback_ = false; bool use_stamped_vel_ = true; std::vector rear_wheels_names_ = {"rear_right_wheel_joint", "rear_left_wheel_joint"}; + std::vector rear_wheels_preceeding_names_ = { + "pid_controller/rear_right_wheel_joint", "pid_controller/rear_left_wheel_joint"}; std::vector front_wheels_names_ = { "front_right_steering_joint", "front_left_steering_joint"}; + std::vector front_wheels_preceeding_names_ = { + "pid_controller/front_right_steering_joint", "pid_controller/front_left_steering_joint"}; + std::vector joint_names_ = { rear_wheels_names_[0], rear_wheels_names_[1], front_wheels_names_[0], front_wheels_names_[1]}; @@ -297,6 +302,7 @@ class AckermannSteeringControllerFixture : public ::testing::Test std::string steering_interface_name_ = "position"; // defined in setup std::string traction_interface_name_ = ""; + std::string preceeding_prefix_ = "pid_controller"; std::vector state_itfs_; std::vector command_itfs_; diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp b/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp index aaab85cf31..e90f190a62 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp @@ -20,10 +20,6 @@ #include #include -using ackermann_steering_controller::CMD_MY_ITFS; -using ackermann_steering_controller::control_mode_type; -using ackermann_steering_controller::STATE_MY_ITFS; - class AckermannSteeringControllerTest : public AckermannSteeringControllerFixture { @@ -33,16 +29,23 @@ TEST_F(AckermannSteeringControllerTest, 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_); + ASSERT_THAT( + controller_->params_.rear_wheels_names, + testing::ElementsAreArray(rear_wheels_preceeding_names_)); + ASSERT_THAT( + controller_->params_.front_wheels_names, + testing::ElementsAreArray(front_wheels_preceeding_names_)); + ASSERT_EQ(controller_->params_.front_steering, front_steering_); + ASSERT_EQ(controller_->params_.open_loop, open_loop_); + ASSERT_EQ(controller_->params_.velocity_rolling_window_size, velocity_rolling_window_size_); + ASSERT_EQ(controller_->params_.position_feedback, position_feedback_); + ASSERT_EQ(controller_->ackermann_params_.wheelbase, wheelbase_); + ASSERT_EQ(controller_->ackermann_params_.front_wheels_radius, front_wheels_radius_); + ASSERT_EQ(controller_->ackermann_params_.rear_wheels_radius, rear_wheels_radius_); + ASSERT_EQ(controller_->ackermann_params_.front_wheel_track, front_wheel_track_); + ASSERT_EQ(controller_->ackermann_params_.rear_wheel_track, rear_wheel_track_); } TEST_F(AckermannSteeringControllerTest, check_exported_intefaces) @@ -53,29 +56,44 @@ TEST_F(AckermannSteeringControllerTest, check_exported_intefaces) 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_); - } + EXPECT_EQ( + command_intefaces.names[CMD_TRACTION_RIGHT_WHEEL], + preceeding_prefix_ + "/" + rear_wheels_names_[0] + "/" + traction_interface_name_); + EXPECT_EQ( + command_intefaces.names[CMD_TRACTION_LEFT_WHEEL], + preceeding_prefix_ + "/" + rear_wheels_names_[1] + "/" + traction_interface_name_); + EXPECT_EQ( + command_intefaces.names[CMD_STEER_RIGHT_WHEEL], + preceeding_prefix_ + "/" + front_wheels_names_[0] + "/" + steering_interface_name_); + EXPECT_EQ( + command_intefaces.names[CMD_STEER_LEFT_WHEEL], + preceeding_prefix_ + "/" + front_wheels_names_[1] + "/" + steering_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_); - } + EXPECT_EQ( + state_intefaces.names[STATE_TRACTION_RIGHT_WHEEL], + preceeding_prefix_ + "/" + rear_wheels_names_[0] + "/" + traction_interface_name_); + EXPECT_EQ( + state_intefaces.names[STATE_TRACTION_LEFT_WHEEL], + preceeding_prefix_ + "/" + rear_wheels_names_[1] + "/" + traction_interface_name_); + EXPECT_EQ( + state_intefaces.names[STATE_STEER_RIGHT_WHEEL], + preceeding_prefix_ + "/" + front_wheels_names_[0] + "/" + steering_interface_name_); + EXPECT_EQ( + state_intefaces.names[STATE_STEER_LEFT_WHEEL], + preceeding_prefix_ + "/" + front_wheels_names_[1] + "/" + steering_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) + ASSERT_EQ(reference_interfaces.size(), joint_reference_interfaces_.size()); + for (size_t i = 0; i < joint_reference_interfaces_.size(); ++i) { - const std::string ref_itf_name = std::string(controller_->get_node()->get_name()) + "/" + - state_joint_names_[i] + "/" + interface_name_; + const std::string ref_itf_name = + std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i]; 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_); + EXPECT_EQ(reference_interfaces[i].get_interface_name(), joint_reference_interfaces_[i]); } } From cc72efc888585ffefb715bbf72601521b4287c9a Mon Sep 17 00:00:00 2001 From: petkovich Date: Mon, 16 Jan 2023 13:09:50 +0100 Subject: [PATCH 145/186] tricycle preceeding --- .../test_ackermann_steering_controller.hpp | 13 ++-- tricycle_steering_controller/CMakeLists.txt | 20 +++--- .../test_tricycle_steering_controller.hpp | 8 +++ ...ricycle_steering_controller_preceeding.cpp | 63 +++++++++++-------- ...steering_controller_preceeding_params.yaml | 21 ++++--- 5 files changed, 77 insertions(+), 48 deletions(-) diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp index 39e5ac3fc5..e1898bab64 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp @@ -280,16 +280,19 @@ class AckermannSteeringControllerFixture : public ::testing::Test bool position_feedback_ = false; bool use_stamped_vel_ = true; std::vector rear_wheels_names_ = {"rear_right_wheel_joint", "rear_left_wheel_joint"}; - std::vector rear_wheels_preceeding_names_ = { - "pid_controller/rear_right_wheel_joint", "pid_controller/rear_left_wheel_joint"}; std::vector front_wheels_names_ = { "front_right_steering_joint", "front_left_steering_joint"}; - std::vector front_wheels_preceeding_names_ = { - "pid_controller/front_right_steering_joint", "pid_controller/front_left_steering_joint"}; - std::vector joint_names_ = { rear_wheels_names_[0], rear_wheels_names_[1], front_wheels_names_[0], front_wheels_names_[1]}; + std::vector rear_wheels_preceeding_names_ = { + "pid_controller/rear_right_wheel_joint", "pid_controller/rear_left_wheel_joint"}; + std::vector front_wheels_preceeding_names_ = { + "pid_controller/front_right_steering_joint", "pid_controller/front_left_steering_joint"}; + std::vector preceeding_joint_names_ = { + rear_wheels_preceeding_names_[0], rear_wheels_preceeding_names_[1], + front_wheels_preceeding_names_[0], front_wheels_preceeding_names_[1]}; + double wheelbase_ = 3.24644; double front_wheel_track_ = 2.12321; double rear_wheel_track_ = 1.76868; diff --git a/tricycle_steering_controller/CMakeLists.txt b/tricycle_steering_controller/CMakeLists.txt index b5b5412f9e..9379e62d92 100644 --- a/tricycle_steering_controller/CMakeLists.txt +++ b/tricycle_steering_controller/CMakeLists.txt @@ -90,16 +90,16 @@ if(BUILD_TESTING) hardware_interface ) - # add_rostest_with_parameters_gmock( - # test_tricycle_steering_controller_preceeding test/test_tricycle_steering_controller_preceeding.cpp - # ${CMAKE_CURRENT_SOURCE_DIR}/test/tricycle_steering_controller_preceeding_params.yaml) - # target_include_directories(test_tricycle_steering_controller_preceeding PRIVATE include) - # target_link_libraries(test_tricycle_steering_controller_preceeding tricycle_steering_controller) - # ament_target_dependencies( - # test_tricycle_steering_controller_preceeding - # controller_interface - # hardware_interface - # ) + add_rostest_with_parameters_gmock( + test_tricycle_steering_controller_preceeding test/test_tricycle_steering_controller_preceeding.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/tricycle_steering_controller_preceeding_params.yaml) + target_include_directories(test_tricycle_steering_controller_preceeding PRIVATE include) + target_link_libraries(test_tricycle_steering_controller_preceeding tricycle_steering_controller) + ament_target_dependencies( + test_tricycle_steering_controller_preceeding + controller_interface + hardware_interface + ) endif() ament_export_include_directories( diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp index 24eb079fa1..1079e0c7a2 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp @@ -268,6 +268,13 @@ class TricycleSteeringControllerFixture : public ::testing::Test std::vector joint_names_ = { rear_wheels_names_[0], rear_wheels_names_[1], front_wheels_names_[0]}; + std::vector rear_wheels_preceeding_names_ = { + "pid_controller/rear_right_wheel_joint", "pid_controller/rear_left_wheel_joint"}; + std::vector front_wheels_preceeding_names_ = {"pid_controller/steering_axis_joint"}; + std::vector preceeding_joint_names_ = { + rear_wheels_preceeding_names_[0], rear_wheels_preceeding_names_[1], + front_wheels_preceeding_names_[0]}; + double wheelbase_ = 3.24644; double wheel_track_ = 1.212121; @@ -280,6 +287,7 @@ class TricycleSteeringControllerFixture : public ::testing::Test std::string steering_interface_name_ = "position"; // defined in setup std::string traction_interface_name_ = ""; + std::string preceeding_prefix_ = "pid_controller"; std::vector state_itfs_; std::vector command_itfs_; diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller_preceeding.cpp b/tricycle_steering_controller/test/test_tricycle_steering_controller_preceeding.cpp index 20f254fa2e..14a24f9f77 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller_preceeding.cpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller_preceeding.cpp @@ -21,10 +21,6 @@ #include #include -using tricycle_steering_controller::CMD_MY_ITFS; -using tricycle_steering_controller::control_mode_type; -using tricycle_steering_controller::STATE_MY_ITFS; - class TricycleSteeringControllerTest : public TricycleSteeringControllerFixture { @@ -34,16 +30,22 @@ TEST_F(TricycleSteeringControllerTest, 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_); + ASSERT_THAT( + controller_->params_.rear_wheels_names, + testing::ElementsAreArray(rear_wheels_preceeding_names_)); + ASSERT_THAT( + controller_->params_.front_wheels_names, + testing::ElementsAreArray(front_wheels_preceeding_names_)); + ASSERT_EQ(controller_->params_.front_steering, front_steering_); + ASSERT_EQ(controller_->params_.open_loop, open_loop_); + ASSERT_EQ(controller_->params_.velocity_rolling_window_size, velocity_rolling_window_size_); + ASSERT_EQ(controller_->params_.position_feedback, position_feedback_); + ASSERT_EQ(controller_->tricycle_params_.wheelbase, wheelbase_); + ASSERT_EQ(controller_->tricycle_params_.front_wheels_radius, front_wheels_radius_); + ASSERT_EQ(controller_->tricycle_params_.rear_wheels_radius, rear_wheels_radius_); + ASSERT_EQ(controller_->tricycle_params_.wheel_track, wheel_track_); } TEST_F(TricycleSteeringControllerTest, check_exported_intefaces) @@ -54,29 +56,38 @@ TEST_F(TricycleSteeringControllerTest, check_exported_intefaces) 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_); - } + EXPECT_EQ( + command_intefaces.names[CMD_TRACTION_RIGHT_WHEEL], + preceeding_prefix_ + "/" + rear_wheels_names_[0] + "/" + traction_interface_name_); + EXPECT_EQ( + command_intefaces.names[CMD_TRACTION_LEFT_WHEEL], + preceeding_prefix_ + "/" + rear_wheels_names_[1] + "/" + traction_interface_name_); + EXPECT_EQ( + command_intefaces.names[CMD_STEER_WHEEL], + preceeding_prefix_ + "/" + front_wheels_names_[0] + "/" + steering_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_); - } + EXPECT_EQ( + state_intefaces.names[STATE_TRACTION_RIGHT_WHEEL], + preceeding_prefix_ + "/" + rear_wheels_names_[0] + "/" + traction_interface_name_); + EXPECT_EQ( + state_intefaces.names[STATE_TRACTION_LEFT_WHEEL], + preceeding_prefix_ + "/" + rear_wheels_names_[1] + "/" + traction_interface_name_); + EXPECT_EQ( + state_intefaces.names[STATE_STEER_AXIS], + preceeding_prefix_ + "/" + front_wheels_names_[0] + "/" + steering_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) + ASSERT_EQ(reference_interfaces.size(), joint_reference_interfaces_.size()); + for (size_t i = 0; i < joint_reference_interfaces_.size(); ++i) { - const std::string ref_itf_name = std::string(controller_->get_node()->get_name()) + "/" + - state_joint_names_[i] + "/" + interface_name_; + const std::string ref_itf_name = + std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i]; 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_); + EXPECT_EQ(reference_interfaces[i].get_interface_name(), joint_reference_interfaces_[i]); } } diff --git a/tricycle_steering_controller/test/tricycle_steering_controller_preceeding_params.yaml b/tricycle_steering_controller/test/tricycle_steering_controller_preceeding_params.yaml index 37c7cac55e..ea8b88002e 100644 --- a/tricycle_steering_controller/test/tricycle_steering_controller_preceeding_params.yaml +++ b/tricycle_steering_controller/test/tricycle_steering_controller_preceeding_params.yaml @@ -1,9 +1,16 @@ test_tricycle_steering_controller: ros__parameters: - joints: - - joint1 - - interface_name: acceleration - - state_joints: - - joint1state + reference_timeout: 2.0 + front_steering: true + open_loop: false + velocity_rolling_window_size: 10 + position_feedback: false + use_stamped_vel: true + rear_wheels_names: [pid_controller/rear_right_wheel_joint, pid_controller/rear_left_wheel_joint] + front_wheels_names: [pid_controller/steering_axis_joint] + rear_wheels_state_names: [rear_right_wheel_joint, rear_left_wheel_joint] + front_wheels_state_names: [steering_axis_joint] + wheelbase: 3.24644 + wheel_track: 1.212121 + front_wheels_radius: 0.45 + rear_wheels_radius: 0.45 From 5dfdf1915924abfdad63ca39080c057006088a0e Mon Sep 17 00:00:00 2001 From: petkovich Date: Mon, 16 Jan 2023 13:28:34 +0100 Subject: [PATCH 146/186] bicycle preceeding --- bicycle_steering_controller/CMakeLists.txt | 20 +++---- .../bicycle_steering_controller.hpp | 2 +- ...steering_controller_preceeding_params.yaml | 19 ++++-- .../test/test_bicycle_steering_controller.cpp | 6 +- .../test/test_bicycle_steering_controller.hpp | 11 +++- ...bicycle_steering_controller_preceeding.cpp | 58 ++++++++++--------- 6 files changed, 68 insertions(+), 48 deletions(-) diff --git a/bicycle_steering_controller/CMakeLists.txt b/bicycle_steering_controller/CMakeLists.txt index 73232b25d7..81987bec30 100644 --- a/bicycle_steering_controller/CMakeLists.txt +++ b/bicycle_steering_controller/CMakeLists.txt @@ -82,16 +82,16 @@ if(BUILD_TESTING) hardware_interface ) - # add_rostest_with_parameters_gmock( - # test_bicycle_steering_controller_preceeding test/test_bicycle_steering_controller_preceeding.cpp - # ${CMAKE_CURRENT_SOURCE_DIR}/test/bicycle_steering_controller_preceeding_params.yaml) - # target_include_directories(test_bicycle_steering_controller_preceeding PRIVATE include) - # target_link_libraries(test_bicycle_steering_controller_preceeding bicycle_steering_controller) - # ament_target_dependencies( - # test_bicycle_steering_controller_preceeding - # controller_interface - # hardware_interface - # ) + add_rostest_with_parameters_gmock( + test_bicycle_steering_controller_preceeding test/test_bicycle_steering_controller_preceeding.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/bicycle_steering_controller_preceeding_params.yaml) + target_include_directories(test_bicycle_steering_controller_preceeding PRIVATE include) + target_link_libraries(test_bicycle_steering_controller_preceeding bicycle_steering_controller) + ament_target_dependencies( + test_bicycle_steering_controller_preceeding + controller_interface + hardware_interface + ) endif() ament_export_include_directories( diff --git a/bicycle_steering_controller/include/bicycle_steering_controller/bicycle_steering_controller.hpp b/bicycle_steering_controller/include/bicycle_steering_controller/bicycle_steering_controller.hpp index b69746d1a7..1bb80e4174 100644 --- a/bicycle_steering_controller/include/bicycle_steering_controller/bicycle_steering_controller.hpp +++ b/bicycle_steering_controller/include/bicycle_steering_controller/bicycle_steering_controller.hpp @@ -31,7 +31,7 @@ static constexpr size_t STATE_TRACTION_WHEEL = 0; static constexpr size_t STATE_STEER_AXIS = 1; // name constants for command interfaces -static constexpr size_t CMD_DRIVE_WHEEL = 0; +static constexpr size_t CMD_TRACTION_WHEEL = 0; static constexpr size_t CMD_STEER_WHEEL = 1; static constexpr size_t NR_STATE_ITFS = 2; diff --git a/bicycle_steering_controller/test/bicycle_steering_controller_preceeding_params.yaml b/bicycle_steering_controller/test/bicycle_steering_controller_preceeding_params.yaml index d7818fa21d..39ffeed878 100644 --- a/bicycle_steering_controller/test/bicycle_steering_controller_preceeding_params.yaml +++ b/bicycle_steering_controller/test/bicycle_steering_controller_preceeding_params.yaml @@ -1,9 +1,16 @@ test_bicycle_steering_controller: ros__parameters: - joints: - - joint1 + reference_timeout: 2.0 + front_steering: true + open_loop: false + velocity_rolling_window_size: 10 + position_feedback: false + use_stamped_vel: true + rear_wheels_names: [pid_controller/rear_wheel_joint] + front_wheels_names: [pid_controller/steering_axis_joint] + rear_wheels_state_names: [rear_wheel_joint] + front_wheels_state_names: [steering_axis_joint] - interface_name: acceleration - - state_joints: - - joint1state + wheelbase: 3.24644 + front_wheel_radius: 0.45 + rear_wheel_radius: 0.45 diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp index 95dabb655a..fb69c05b6a 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp @@ -53,7 +53,7 @@ TEST_F(BicycleSteeringControllerTest, check_exported_intefaces) auto command_intefaces = controller_->command_interface_configuration(); ASSERT_EQ(command_intefaces.names.size(), joint_command_values_.size()); EXPECT_EQ( - command_intefaces.names[CMD_DRIVE_WHEEL], + command_intefaces.names[CMD_TRACTION_WHEEL], rear_wheels_names_[0] + "/" + traction_interface_name_); EXPECT_EQ( command_intefaces.names[CMD_STEER_WHEEL], @@ -158,7 +158,7 @@ TEST_F(BicycleSteeringControllerTest, test_update_logic) controller_interface::return_type::OK); EXPECT_NEAR( - controller_->command_interfaces_[CMD_DRIVE_WHEEL].get_value(), 0.253221, COMMON_THRESHOLD); + controller_->command_interfaces_[CMD_TRACTION_WHEEL].get_value(), 0.253221, COMMON_THRESHOLD); EXPECT_NEAR( controller_->command_interfaces_[CMD_STEER_WHEEL].get_value(), 1.4179821977774734, COMMON_THRESHOLD); @@ -213,7 +213,7 @@ TEST_F(BicycleSteeringControllerTest, receive_message_and_publish_updated_status controller_interface::return_type::OK); EXPECT_NEAR( - controller_->command_interfaces_[CMD_DRIVE_WHEEL].get_value(), 0.253221, COMMON_THRESHOLD); + controller_->command_interfaces_[CMD_TRACTION_WHEEL].get_value(), 0.253221, COMMON_THRESHOLD); EXPECT_NEAR( controller_->command_interfaces_[CMD_STEER_WHEEL].get_value(), 1.4179821977774734, COMMON_THRESHOLD); diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp index 303eb1367f..38d1cf3a90 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp @@ -43,8 +43,8 @@ using bicycle_steering_controller::STATE_STEER_AXIS; using bicycle_steering_controller::STATE_TRACTION_WHEEL; // name constants for command interfaces -using bicycle_steering_controller::CMD_DRIVE_WHEEL; using bicycle_steering_controller::CMD_STEER_WHEEL; +using bicycle_steering_controller::CMD_TRACTION_WHEEL; namespace { @@ -159,7 +159,7 @@ class BicycleSteeringControllerFixture : public ::testing::Test command_ifs.reserve(joint_command_values_.size()); command_itfs_.emplace_back(hardware_interface::CommandInterface( - rear_wheels_names_[0], traction_interface_name_, &joint_command_values_[CMD_DRIVE_WHEEL])); + rear_wheels_names_[0], traction_interface_name_, &joint_command_values_[CMD_TRACTION_WHEEL])); command_ifs.emplace_back(command_itfs_.back()); command_itfs_.emplace_back(hardware_interface::CommandInterface( @@ -253,6 +253,11 @@ class BicycleSteeringControllerFixture : public ::testing::Test std::vector front_wheels_names_ = {"steering_axis_joint"}; std::vector joint_names_ = {rear_wheels_names_[0], front_wheels_names_[0]}; + std::vector rear_wheels_preceeding_names_ = {"pid_controller/rear_wheel_joint"}; + std::vector front_wheels_preceeding_names_ = {"pid_controller/steering_axis_joint"}; + std::vector preceeding_joint_names_ = { + rear_wheels_preceeding_names_[0], front_wheels_preceeding_names_[0]}; + double wheelbase_ = 3.24644; double front_wheels_radius_ = 0.45; double rear_wheels_radius_ = 0.45; @@ -261,8 +266,10 @@ class BicycleSteeringControllerFixture : public ::testing::Test std::array joint_command_values_ = {1.1, 2.2}; std::array joint_reference_interfaces_ = {"linear/velocity", "angular/position"}; std::string steering_interface_name_ = "position"; + // defined in setup std::string traction_interface_name_ = ""; + std::string preceeding_prefix_ = "pid_controller"; std::vector state_itfs_; std::vector command_itfs_; diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller_preceeding.cpp b/bicycle_steering_controller/test/test_bicycle_steering_controller_preceeding.cpp index a353af6e76..1a1bf35a9c 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller_preceeding.cpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller_preceeding.cpp @@ -20,10 +20,6 @@ #include #include -using bicycle_steering_controller::CMD_MY_ITFS; -using bicycle_steering_controller::control_mode_type; -using bicycle_steering_controller::STATE_MY_ITFS; - class BicycleSteeringControllerTest : public BicycleSteeringControllerFixture { @@ -33,16 +29,21 @@ TEST_F(BicycleSteeringControllerTest, 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_); + ASSERT_THAT( + controller_->params_.rear_wheels_names, + testing::ElementsAreArray(rear_wheels_preceeding_names_)); + ASSERT_THAT( + controller_->params_.front_wheels_names, + testing::ElementsAreArray(front_wheels_preceeding_names_)); + ASSERT_EQ(controller_->params_.front_steering, front_steering_); + ASSERT_EQ(controller_->params_.open_loop, open_loop_); + ASSERT_EQ(controller_->params_.velocity_rolling_window_size, velocity_rolling_window_size_); + ASSERT_EQ(controller_->params_.position_feedback, position_feedback_); + ASSERT_EQ(controller_->bicycle_params_.wheelbase, wheelbase_); + ASSERT_EQ(controller_->bicycle_params_.front_wheel_radius, front_wheels_radius_); + ASSERT_EQ(controller_->bicycle_params_.rear_wheel_radius, rear_wheels_radius_); } TEST_F(BicycleSteeringControllerTest, check_exported_intefaces) @@ -53,29 +54,34 @@ TEST_F(BicycleSteeringControllerTest, check_exported_intefaces) 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_); - } + EXPECT_EQ( + command_intefaces.names[CMD_TRACTION_WHEEL], + preceeding_prefix_ + "/" + rear_wheels_names_[0] + "/" + traction_interface_name_); + + EXPECT_EQ( + command_intefaces.names[CMD_STEER_WHEEL], + preceeding_prefix_ + "/" + front_wheels_names_[0] + "/" + steering_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_); - } + + EXPECT_EQ( + state_intefaces.names[STATE_TRACTION_WHEEL], + preceeding_prefix_ + "/" + rear_wheels_names_[0] + "/" + traction_interface_name_); + EXPECT_EQ( + state_intefaces.names[STATE_STEER_AXIS], + preceeding_prefix_ + "/" + front_wheels_names_[0] + "/" + steering_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) + ASSERT_EQ(reference_interfaces.size(), joint_reference_interfaces_.size()); + for (size_t i = 0; i < joint_reference_interfaces_.size(); ++i) { - const std::string ref_itf_name = std::string(controller_->get_node()->get_name()) + "/" + - state_joint_names_[i] + "/" + interface_name_; + const std::string ref_itf_name = + std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i]; 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_); + EXPECT_EQ(reference_interfaces[i].get_interface_name(), joint_reference_interfaces_[i]); } } From abf035fcffd9b5e5f5960b08af527174b7bf83b7 Mon Sep 17 00:00:00 2001 From: petkovich Date: Tue, 17 Jan 2023 11:49:20 +0100 Subject: [PATCH 147/186] update logic chained --- .../test_ackermann_steering_controller.cpp | 39 +++++++++++++++++++ .../test_ackermann_steering_controller.hpp | 1 + .../test/test_bicycle_steering_controller.cpp | 32 +++++++++++++++ .../test/test_bicycle_steering_controller.hpp | 1 + .../src/steering_controllers_library.cpp | 9 ++++- .../test_tricycle_steering_controller.cpp | 36 +++++++++++++++++ .../test_tricycle_steering_controller.hpp | 1 + 7 files changed, 117 insertions(+), 2 deletions(-) diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp index dcc395ac9b..0c2e7d3f80 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp @@ -192,6 +192,45 @@ TEST_F(AckermannSteeringControllerTest, test_update_logic) } } +TEST_F(AckermannSteeringControllerTest, test_update_logic_chained) +{ + 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); + controller_->set_chained_mode(true); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(controller_->is_in_chained_mode()); + + controller_->reference_interfaces_[0] = 0.1; + controller_->reference_interfaces_[1] = 0.2; + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + EXPECT_NEAR( + + controller_->command_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_value(), 0.22222222222222224, + COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_value(), 0.22222222222222224, + COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[STATE_STEER_RIGHT_WHEEL].get_value(), 1.4179821977774734, + COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[STATE_STEER_LEFT_WHEEL].get_value(), 1.4179821977774734, + COMMON_THRESHOLD); + + EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x)); + EXPECT_EQ(controller_->reference_interfaces_.size(), joint_reference_interfaces_.size()); + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } +} + TEST_F(AckermannSteeringControllerTest, receive_message_and_publish_updated_status) { SetUpController(); diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp index e1898bab64..d433d0b4c7 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp @@ -69,6 +69,7 @@ class TestableAckermannSteeringController FRIEND_TEST(AckermannSteeringControllerTest, deactivate_success); FRIEND_TEST(AckermannSteeringControllerTest, reactivate_success); FRIEND_TEST(AckermannSteeringControllerTest, test_update_logic); + FRIEND_TEST(AckermannSteeringControllerTest, test_update_logic_chained); FRIEND_TEST(AckermannSteeringControllerTest, publish_status_success); FRIEND_TEST(AckermannSteeringControllerTest, receive_message_and_publish_updated_status); diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp index fb69c05b6a..b805703abd 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp @@ -171,6 +171,38 @@ TEST_F(BicycleSteeringControllerTest, test_update_logic) } } +TEST_F(BicycleSteeringControllerTest, test_update_logic_chained) +{ + 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); + controller_->set_chained_mode(true); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(controller_->is_in_chained_mode()); + + controller_->reference_interfaces_[0] = 0.1; + controller_->reference_interfaces_[1] = 0.2; + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_NEAR( + controller_->command_interfaces_[CMD_TRACTION_WHEEL].get_value(), 0.253221, COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[CMD_STEER_WHEEL].get_value(), 1.4179821977774734, + COMMON_THRESHOLD); + + EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x)); + EXPECT_EQ(controller_->reference_interfaces_.size(), joint_reference_interfaces_.size()); + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } +} + TEST_F(BicycleSteeringControllerTest, publish_status_success) { SetUpController(); diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp index 38d1cf3a90..046c2e4df5 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp @@ -66,6 +66,7 @@ class TestableBicycleSteeringController FRIEND_TEST(BicycleSteeringControllerTest, deactivate_success); FRIEND_TEST(BicycleSteeringControllerTest, reactivate_success); FRIEND_TEST(BicycleSteeringControllerTest, test_update_logic); + FRIEND_TEST(BicycleSteeringControllerTest, test_update_logic_chained); FRIEND_TEST(BicycleSteeringControllerTest, publish_status_success); FRIEND_TEST(BicycleSteeringControllerTest, receive_message_and_publish_updated_status); diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index ee75d08353..3b8ebbb78b 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -414,8 +414,13 @@ controller_interface::return_type SteeringControllersLibrary::update_reference_f } else { - reference_interfaces_[0] = std::numeric_limits::quiet_NaN(); - reference_interfaces_[1] = std::numeric_limits::quiet_NaN(); + if (!std::isnan(current_ref->twist.linear.x) && !std::isnan(current_ref->twist.angular.z)) + { + reference_interfaces_[0] = 0.0; + reference_interfaces_[1] = 0.0; + current_ref->twist.linear.x = std::numeric_limits::quiet_NaN(); + current_ref->twist.angular.z = std::numeric_limits::quiet_NaN(); + } } return controller_interface::return_type::OK; diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp index f3ae849d52..eeb65ba747 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp @@ -183,6 +183,42 @@ TEST_F(TricycleSteeringControllerTest, test_update_logic) } } +TEST_F(TricycleSteeringControllerTest, test_update_logic_chained) +{ + 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); + controller_->set_chained_mode(true); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(controller_->is_in_chained_mode()); + + controller_->reference_interfaces_[0] = 0.1; + controller_->reference_interfaces_[1] = 0.2; + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_NEAR( + controller_->command_interfaces_[CMD_TRACTION_RIGHT_WHEEL].get_value(), 0.22222222222222224, + COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[CMD_TRACTION_LEFT_WHEEL].get_value(), 0.22222222222222224, + COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[CMD_STEER_WHEEL].get_value(), 1.4179821977774734, + COMMON_THRESHOLD); + + EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x)); + EXPECT_EQ(controller_->reference_interfaces_.size(), joint_reference_interfaces_.size()); + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } +} + TEST_F(TricycleSteeringControllerTest, receive_message_and_publish_updated_status) { SetUpController(); diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp index 1079e0c7a2..9eafe47785 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp @@ -67,6 +67,7 @@ class TestableTricycleSteeringController FRIEND_TEST(TricycleSteeringControllerTest, deactivate_success); FRIEND_TEST(TricycleSteeringControllerTest, reactivate_success); FRIEND_TEST(TricycleSteeringControllerTest, test_update_logic); + FRIEND_TEST(TricycleSteeringControllerTest, test_update_logic_chained); FRIEND_TEST(TricycleSteeringControllerTest, publish_status_success); FRIEND_TEST(TricycleSteeringControllerTest, receive_message_and_publish_updated_status); From f829c45c8ebc44a870d00feeaa047b2c2dd6f1cb Mon Sep 17 00:00:00 2001 From: petkovich Date: Tue, 17 Jan 2023 11:52:38 +0100 Subject: [PATCH 148/186] removed pragma once --- .../steering_controllers_library/steering_odometry.hpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp index 61237a6845..ed9ec74d41 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp @@ -15,7 +15,8 @@ // Authors: dr. sc. Tomislav Petkovic, Dr. Ing. Denis Štogl // -#pragma once +#ifndef STEERING_CONTROLLERS_LIBRARY__STEERING_ODOMETRY_HPP_ +#define STEERING_CONTROLLERS_LIBRARY__STEERING_ODOMETRY_HPP_ #include #include @@ -255,3 +256,5 @@ class SteeringOdometry rcpputils::RollingMeanAccumulator angular_acc_; }; } // namespace steering_odometry + +#endif // STEERING_CONTROLLERS_LIBRARY__STEERING_ODOMETRY_HPP_ From 6368ecab506d14f96826ef1fe77e141d22506617 Mon Sep 17 00:00:00 2001 From: petkovich Date: Tue, 17 Jan 2023 12:31:01 +0100 Subject: [PATCH 149/186] params and cosmetics --- .../config/steering_controllers_library.yaml | 19 ++----------------- steering_controllers_library/doc/userdoc.rst | 1 - steering_controllers_library/package.xml | 1 - .../src/steering_controllers_library.cpp | 1 - .../src/steering_odometry.cpp | 2 +- 5 files changed, 3 insertions(+), 21 deletions(-) diff --git a/steering_controllers_library/config/steering_controllers_library.yaml b/steering_controllers_library/config/steering_controllers_library.yaml index 1dd76a97ab..941ae75a6c 100644 --- a/steering_controllers_library/config/steering_controllers_library.yaml +++ b/steering_controllers_library/config/steering_controllers_library.yaml @@ -3,9 +3,6 @@ steering_controllers_library: type: double, default_value: 1, description: "Timeout for controller references after which they will be reset. This is especially useful for controllers that can cause unwanted and dangerous behaviour if reference is not reset, e.g., velocity controllers. If value is 0 the reference is reset after each run.", -# validation: { -# gt_eq<>: 0.0, -# } } front_steering: { @@ -13,7 +10,6 @@ steering_controllers_library: default_value: true, description: "Is the steering on the front of the robot?", read_only: false, - } rear_wheels_names: { @@ -21,7 +17,6 @@ steering_controllers_library: default_value: ["rear_wheel_joint"], description: "Name of rear wheels.", read_only: true, - } front_wheels_names: { @@ -29,7 +24,6 @@ steering_controllers_library: default_value: ["front_steer_joint"], description: "Names of front steer wheels.", read_only: true, - } open_loop: { @@ -37,7 +31,6 @@ steering_controllers_library: default_value: false, description: "bool parameter decides if open oop or not (feedback).", read_only: false, - } velocity_rolling_window_size: { @@ -45,7 +38,6 @@ steering_controllers_library: default_value: 10, description: "The number of velocity samples to average together to compute the odometry twist.linear.x and twist.angular.z velocities.", read_only: false, - } base_frame_id: { @@ -53,7 +45,6 @@ steering_controllers_library: default_value: "base_link", description: "Base frame_id set to value of base_frame_id.", read_only: false, - } odom_frame_id: { @@ -61,7 +52,6 @@ steering_controllers_library: default_value: "odom", description: "Odometry frame_id set to value of odom_frame_id.", read_only: false, - } enable_odom_tf: { @@ -69,7 +59,6 @@ steering_controllers_library: default_value: true, description: "Publishing to tf is enabled or disabled ?.", read_only: false, - } twist_covariance_diagonal: { @@ -77,7 +66,6 @@ steering_controllers_library: default_value: [0, 7, 14, 21, 28, 35], description: "diagonal values of twist covariance matrix.", read_only: false, - } pose_covariance_diagonal: { @@ -85,23 +73,20 @@ steering_controllers_library: default_value: [0, 7, 14, 21, 28, 35], description: "diagonal values of pose covariance matrix.", read_only: false, - } position_feedback: { type: bool, default_value: false, - description: "choice of feedbacktype, if position_feedback is false then HW_IF_VELOCITY is taken as interface type, if + description: "Choice of feedback type, if position_feedback is false then HW_IF_VELOCITY is taken as interface type, if position_feedback is true then HW_IF_POSITION is taken as interface type", read_only: false, - } use_stamped_vel: { type: bool, default_value: false, - description: "choice of vel type, if use_stamped_vel is false then geometry_msgs::msg::Twist is taken as vel msg type, if + description: "Choice of vel type, if use_stamped_vel is false then geometry_msgs::msg::Twist is taken as vel msg type, if use_stamped_vel is true then geometry_msgs::msg::TwistStamped is taken as vel msg type", read_only: false, - } diff --git a/steering_controllers_library/doc/userdoc.rst b/steering_controllers_library/doc/userdoc.rst index 612620aace..38d6cdbd36 100644 --- a/steering_controllers_library/doc/userdoc.rst +++ b/steering_controllers_library/doc/userdoc.rst @@ -19,5 +19,4 @@ Other features Realtime-safe implementation. Odometry publishing - Velocity, acceleration and jerk limits Automatic stop after command timeout diff --git a/steering_controllers_library/package.xml b/steering_controllers_library/package.xml index 2e638f8ddc..caba52e8e8 100644 --- a/steering_controllers_library/package.xml +++ b/steering_controllers_library/package.xml @@ -32,7 +32,6 @@ tf2_geometry_msgs ackermann_msgs - ament_lint_auto ament_cmake_gmock controller_manager hardware_interface diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index 3b8ebbb78b..d4834f5883 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -129,7 +129,6 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure( return controller_interface::CallbackReturn::ERROR; } - // TODO(anyone): Reserve memory in state publisher depending on the message type rt_odom_state_publisher_->lock(); rt_odom_state_publisher_->msg_.header.stamp = get_node()->now(); rt_odom_state_publisher_->msg_.header.frame_id = params_.odom_frame_id; diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp index 0c1f6b1e0e..abb9d5a741 100644 --- a/steering_controllers_library/src/steering_odometry.cpp +++ b/steering_controllers_library/src/steering_odometry.cpp @@ -333,6 +333,6 @@ void SteeringOdometry::reset_accumulators() { linear_acc_ = rcpputils::RollingMeanAccumulator(velocity_rolling_window_size_); angular_acc_ = rcpputils::RollingMeanAccumulator(velocity_rolling_window_size_); - // TODO(petkovich): angular rolling window size? } + } // namespace steering_odometry From bed538f8bcd32987d7584d299e48c842591aea23 Mon Sep 17 00:00:00 2001 From: petkovich Date: Tue, 17 Jan 2023 13:12:17 +0100 Subject: [PATCH 150/186] preceeding params and testing --- .../test_ackermann_steering_controller.cpp | 8 ++-- ...kermann_steering_controller_preceeding.cpp | 8 ++-- .../test/test_bicycle_steering_controller.cpp | 4 +- ...bicycle_steering_controller_preceeding.cpp | 4 +- .../config/steering_controllers_library.yaml | 40 ++++++++++++++++--- .../steering_controllers_library.hpp | 3 ++ .../src/steering_controllers_library.cpp | 36 ++++++++++++----- .../test_tricycle_steering_controller.cpp | 6 +-- ...ricycle_steering_controller_preceeding.cpp | 6 +-- 9 files changed, 83 insertions(+), 32 deletions(-) diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp index 0c2e7d3f80..57523bf0a0 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp @@ -71,16 +71,16 @@ TEST_F(AckermannSteeringControllerTest, check_exported_intefaces) ASSERT_EQ(state_intefaces.names.size(), joint_state_values_.size()); EXPECT_EQ( state_intefaces.names[STATE_TRACTION_RIGHT_WHEEL], - rear_wheels_names_[0] + "/" + traction_interface_name_); + controller_->rear_wheels_state_names_[0] + "/" + traction_interface_name_); EXPECT_EQ( state_intefaces.names[STATE_TRACTION_LEFT_WHEEL], - rear_wheels_names_[1] + "/" + traction_interface_name_); + controller_->rear_wheels_state_names_[1] + "/" + traction_interface_name_); EXPECT_EQ( state_intefaces.names[STATE_STEER_RIGHT_WHEEL], - front_wheels_names_[0] + "/" + steering_interface_name_); + controller_->front_wheels_state_names_[0] + "/" + steering_interface_name_); EXPECT_EQ( state_intefaces.names[STATE_STEER_LEFT_WHEEL], - front_wheels_names_[1] + "/" + steering_interface_name_); + controller_->front_wheels_state_names_[1] + "/" + steering_interface_name_); // check ref itfs auto reference_interfaces = controller_->export_reference_interfaces(); diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp b/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp index e90f190a62..2d951588c5 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp @@ -73,16 +73,16 @@ TEST_F(AckermannSteeringControllerTest, check_exported_intefaces) ASSERT_EQ(state_intefaces.names.size(), joint_state_values_.size()); EXPECT_EQ( state_intefaces.names[STATE_TRACTION_RIGHT_WHEEL], - preceeding_prefix_ + "/" + rear_wheels_names_[0] + "/" + traction_interface_name_); + controller_->rear_wheels_state_names_[0] + "/" + traction_interface_name_); EXPECT_EQ( state_intefaces.names[STATE_TRACTION_LEFT_WHEEL], - preceeding_prefix_ + "/" + rear_wheels_names_[1] + "/" + traction_interface_name_); + controller_->rear_wheels_state_names_[1] + "/" + traction_interface_name_); EXPECT_EQ( state_intefaces.names[STATE_STEER_RIGHT_WHEEL], - preceeding_prefix_ + "/" + front_wheels_names_[0] + "/" + steering_interface_name_); + controller_->front_wheels_state_names_[0] + "/" + steering_interface_name_); EXPECT_EQ( state_intefaces.names[STATE_STEER_LEFT_WHEEL], - preceeding_prefix_ + "/" + front_wheels_names_[1] + "/" + steering_interface_name_); + controller_->front_wheels_state_names_[1] + "/" + steering_interface_name_); // check ref itfs auto reference_interfaces = controller_->export_reference_interfaces(); diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp index b805703abd..c8e53e747c 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp @@ -63,10 +63,10 @@ TEST_F(BicycleSteeringControllerTest, check_exported_intefaces) ASSERT_EQ(state_intefaces.names.size(), joint_state_values_.size()); EXPECT_EQ( state_intefaces.names[STATE_TRACTION_WHEEL], - rear_wheels_names_[0] + "/" + traction_interface_name_); + controller_->rear_wheels_state_names_[0] + "/" + traction_interface_name_); EXPECT_EQ( state_intefaces.names[STATE_STEER_AXIS], - front_wheels_names_[0] + "/" + steering_interface_name_); + controller_->front_wheels_state_names_[0] + "/" + steering_interface_name_); // check ref itfs auto reference_interfaces = controller_->export_reference_interfaces(); diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller_preceeding.cpp b/bicycle_steering_controller/test/test_bicycle_steering_controller_preceeding.cpp index 1a1bf35a9c..875910ba23 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller_preceeding.cpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller_preceeding.cpp @@ -67,10 +67,10 @@ TEST_F(BicycleSteeringControllerTest, check_exported_intefaces) EXPECT_EQ( state_intefaces.names[STATE_TRACTION_WHEEL], - preceeding_prefix_ + "/" + rear_wheels_names_[0] + "/" + traction_interface_name_); + controller_->rear_wheels_state_names_[0] + "/" + traction_interface_name_); EXPECT_EQ( state_intefaces.names[STATE_STEER_AXIS], - preceeding_prefix_ + "/" + front_wheels_names_[0] + "/" + steering_interface_name_); + controller_->front_wheels_state_names_[0] + "/" + steering_interface_name_); // check ref itfs auto reference_interfaces = controller_->export_reference_interfaces(); diff --git a/steering_controllers_library/config/steering_controllers_library.yaml b/steering_controllers_library/config/steering_controllers_library.yaml index 941ae75a6c..86acb01dae 100644 --- a/steering_controllers_library/config/steering_controllers_library.yaml +++ b/steering_controllers_library/config/steering_controllers_library.yaml @@ -9,21 +9,51 @@ steering_controllers_library: type: bool, default_value: true, description: "Is the steering on the front of the robot?", - read_only: false, + read_only: true, } rear_wheels_names: { type: string_array, - default_value: ["rear_wheel_joint"], - description: "Name of rear wheels.", + description: "Names of rear wheel joints.", read_only: true, + validation: { + size_lt<>: [5], + unique<>: null, + not_empty<>: null, + } } front_wheels_names: { type: string_array, - default_value: ["front_steer_joint"], - description: "Names of front steer wheels.", + description: "Names of front wheel joints.", + read_only: true, + validation: { + size_lt<>: [5], + unique<>: null, + not_empty<>: null, + } + } + + rear_wheels_state_names: { + type: string_array, + description: "(Optional) Names of rear wheel joints to read states from. If not set joint names from 'rear_wheels_names' will be used.", + default_value: [], + read_only: true, + validation: { + size_lt<>: [5], + unique<>: null, + } + } + + front_wheels_state_names: { + type: string_array, + description: "(Optional) Names of front wheel joints to read states from. If not set joint names from 'front_wheels_names' will be used.", + default_value: [], read_only: true, + validation: { + size_lt<>: [5], + unique<>: null, + } } open_loop: { diff --git a/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp b/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp index e8485aec1a..2d86e8ce6f 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp @@ -135,6 +135,9 @@ class SteeringControllersLibrary : public controller_interface::ChainableControl double last_linear_velocity_ = 0.0; double last_angular_velocity_ = 0.0; + std::vector rear_wheels_state_names_; + std::vector front_wheels_state_names_; + private: // callback for topic interface STEERING_CONTROLLERS__VISIBILITY_LOCAL void reference_callback( diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index d4834f5883..a79b15c8aa 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -88,6 +88,25 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure( odometry_.set_velocity_rolling_window_size(params_.velocity_rolling_window_size); configure_odometry(); + + if (!params_.rear_wheels_state_names.empty()) + { + rear_wheels_state_names_ = params_.rear_wheels_state_names; + } + else + { + rear_wheels_state_names_ = params_.rear_wheels_names; + } + + if (!params_.front_wheels_state_names.empty()) + { + front_wheels_state_names_ = params_.front_wheels_state_names; + } + else + { + front_wheels_state_names_ = params_.front_wheels_names; + } + // topics QoS auto subscribers_qos = rclcpp::SystemDefaultsQoS(); subscribers_qos.keep_last(1); @@ -186,7 +205,6 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure( return controller_interface::CallbackReturn::ERROR; } - // TODO(anyone): Reserve memory in state publisher depending on the message type controller_state_publisher_->lock(); controller_state_publisher_->msg_.header.stamp = get_node()->now(); controller_state_publisher_->msg_.header.frame_id = params_.odom_frame_id; @@ -319,30 +337,30 @@ SteeringControllersLibrary::state_interface_configuration() const : hardware_interface::HW_IF_VELOCITY; if (params_.front_steering) { - for (size_t i = 0; i < params_.rear_wheels_names.size(); i++) + for (size_t i = 0; i < rear_wheels_state_names_.size(); i++) { state_interfaces_config.names.push_back( - params_.rear_wheels_names[i] + "/" + traction_wheels_feedback); + rear_wheels_state_names_[i] + "/" + traction_wheels_feedback); } - for (size_t i = 0; i < params_.front_wheels_names.size(); i++) + for (size_t i = 0; i < front_wheels_state_names_.size(); i++) { state_interfaces_config.names.push_back( - params_.front_wheels_names[i] + "/" + hardware_interface::HW_IF_POSITION); + front_wheels_state_names_[i] + "/" + hardware_interface::HW_IF_POSITION); } } else { - for (size_t i = 0; i < params_.front_wheels_names.size(); i++) + for (size_t i = 0; i < front_wheels_state_names_.size(); i++) { state_interfaces_config.names.push_back( - params_.front_wheels_names[i] + "/" + traction_wheels_feedback); + front_wheels_state_names_[i] + "/" + traction_wheels_feedback); } - for (size_t i = 0; i < params_.rear_wheels_names.size(); i++) + for (size_t i = 0; i < rear_wheels_state_names_.size(); i++) { state_interfaces_config.names.push_back( - params_.rear_wheels_names[i] + "/" + hardware_interface::HW_IF_POSITION); + rear_wheels_state_names_[i] + "/" + hardware_interface::HW_IF_POSITION); } } diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp index eeb65ba747..ad483f1c8f 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp @@ -68,13 +68,13 @@ TEST_F(TricycleSteeringControllerTest, check_exported_intefaces) ASSERT_EQ(state_intefaces.names.size(), joint_state_values_.size()); EXPECT_EQ( state_intefaces.names[STATE_TRACTION_RIGHT_WHEEL], - rear_wheels_names_[0] + "/" + traction_interface_name_); + controller_->rear_wheels_state_names_[0] + "/" + traction_interface_name_); EXPECT_EQ( state_intefaces.names[STATE_TRACTION_LEFT_WHEEL], - rear_wheels_names_[1] + "/" + traction_interface_name_); + controller_->rear_wheels_state_names_[1] + "/" + traction_interface_name_); EXPECT_EQ( state_intefaces.names[STATE_STEER_AXIS], - front_wheels_names_[0] + "/" + steering_interface_name_); + controller_->front_wheels_state_names_[0] + "/" + steering_interface_name_); // check ref itfs auto reference_interfaces = controller_->export_reference_interfaces(); diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller_preceeding.cpp b/tricycle_steering_controller/test/test_tricycle_steering_controller_preceeding.cpp index 14a24f9f77..55137c577d 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller_preceeding.cpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller_preceeding.cpp @@ -70,13 +70,13 @@ TEST_F(TricycleSteeringControllerTest, check_exported_intefaces) ASSERT_EQ(state_intefaces.names.size(), joint_state_values_.size()); EXPECT_EQ( state_intefaces.names[STATE_TRACTION_RIGHT_WHEEL], - preceeding_prefix_ + "/" + rear_wheels_names_[0] + "/" + traction_interface_name_); + controller_->rear_wheels_state_names_[0] + "/" + traction_interface_name_); EXPECT_EQ( state_intefaces.names[STATE_TRACTION_LEFT_WHEEL], - preceeding_prefix_ + "/" + rear_wheels_names_[1] + "/" + traction_interface_name_); + controller_->rear_wheels_state_names_[1] + "/" + traction_interface_name_); EXPECT_EQ( state_intefaces.names[STATE_STEER_AXIS], - preceeding_prefix_ + "/" + front_wheels_names_[0] + "/" + steering_interface_name_); + controller_->front_wheels_state_names_[0] + "/" + steering_interface_name_); // check ref itfs auto reference_interfaces = controller_->export_reference_interfaces(); From 9ccf5b20edf886d500ed787a24c2f6cb410e2817 Mon Sep 17 00:00:00 2001 From: petkovich Date: Tue, 17 Jan 2023 13:15:24 +0100 Subject: [PATCH 151/186] cosmetic changes --- .../src/steering_controllers_library.cpp | 5 ----- 1 file changed, 5 deletions(-) diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index a79b15c8aa..18fa0a20a8 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -404,8 +404,6 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_activate( controller_interface::CallbackReturn SteeringControllersLibrary::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 < nr_cmd_itfs_; ++i) { command_interfaces_[i].set_value(std::numeric_limits::quiet_NaN()); @@ -418,8 +416,6 @@ controller_interface::return_type SteeringControllersLibrary::update_reference_f auto current_ref = *(input_ref_.readFromRT()); const auto age_of_last_command = get_node()->now() - (current_ref)->header.stamp; - // TODO(anyone): depending on number of interfaces, use definitions, e.g., `CMD_MY_ITFS`, - // instead of a loop // send message only if there is no timeout if (age_of_last_command <= ref_timeout_ || ref_timeout_ == rclcpp::Duration::from_seconds(0)) { @@ -562,7 +558,6 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c command_interfaces_[params_.rear_wheels_names.size() + i].get_value()); } } - else { for (size_t i = 0; i < params_.front_wheels_names.size(); i++) From 5322b5675f257cddb863896b59ee349ffb4c3c65 Mon Sep 17 00:00:00 2001 From: petkovich Date: Tue, 17 Jan 2023 13:19:45 +0100 Subject: [PATCH 152/186] reset interfaces to nan --- .../test/test_ackermann_steering_controller.cpp | 2 +- .../test/test_bicycle_steering_controller.cpp | 2 +- .../src/steering_controllers_library.cpp | 9 +++------ .../test/test_tricycle_steering_controller.cpp | 2 +- 4 files changed, 6 insertions(+), 9 deletions(-) diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp index 57523bf0a0..60e65c55ae 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp @@ -188,7 +188,7 @@ TEST_F(AckermannSteeringControllerTest, test_update_logic) EXPECT_EQ(controller_->reference_interfaces_.size(), joint_reference_interfaces_.size()); for (const auto & interface : controller_->reference_interfaces_) { - EXPECT_FALSE(std::isnan(interface)); + EXPECT_TRUE(std::isnan(interface)); } } diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp index c8e53e747c..7b1587e623 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp @@ -167,7 +167,7 @@ TEST_F(BicycleSteeringControllerTest, test_update_logic) EXPECT_EQ(controller_->reference_interfaces_.size(), joint_reference_interfaces_.size()); for (const auto & interface : controller_->reference_interfaces_) { - EXPECT_FALSE(std::isnan(interface)); + EXPECT_TRUE(std::isnan(interface)); } } diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index 18fa0a20a8..2d54b2c1cb 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -483,12 +483,6 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c } } - if (ref_timeout_ == rclcpp::Duration::from_seconds(0) || is_in_chained_mode()) - { - reference_interfaces_[0] = std::numeric_limits::quiet_NaN(); - reference_interfaces_[1] = std::numeric_limits::quiet_NaN(); - } - // Publish odometry message // Compute and store orientation info tf2::Quaternion orientation; @@ -587,6 +581,9 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c controller_state_publisher_->unlockAndPublish(); } + reference_interfaces_[0] = std::numeric_limits::quiet_NaN(); + reference_interfaces_[1] = std::numeric_limits::quiet_NaN(); + return controller_interface::return_type::OK; } diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp index ad483f1c8f..02a1bf02b9 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp @@ -179,7 +179,7 @@ TEST_F(TricycleSteeringControllerTest, test_update_logic) EXPECT_EQ(controller_->reference_interfaces_.size(), joint_reference_interfaces_.size()); for (const auto & interface : controller_->reference_interfaces_) { - EXPECT_FALSE(std::isnan(interface)); + EXPECT_TRUE(std::isnan(interface)); } } From db764ab715aca6ec3dc0d78c48236e1ac7dccb02 Mon Sep 17 00:00:00 2001 From: petkovich Date: Tue, 17 Jan 2023 13:22:53 +0100 Subject: [PATCH 153/186] reduce code for controller state publisher --- .../src/steering_controllers_library.cpp | 66 +++++++------------ 1 file changed, 24 insertions(+), 42 deletions(-) diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index 2d54b2c1cb..7171f5b6c3 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -527,55 +527,37 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c controller_state_publisher_->msg_.steer_positions.data.clear(); controller_state_publisher_->msg_.steering_angle_command.data.clear(); - if (params_.front_steering) + auto number_of_traction_wheels = params_.rear_wheels_names.size(); + auto number_of_steering_wheels = params_.front_wheels_names.size(); + + if (!params_.front_steering) { - for (size_t i = 0; i < params_.rear_wheels_names.size(); i++) - { - if (params_.position_feedback) - { - controller_state_publisher_->msg_.traction_wheels_position.data.push_back( - state_interfaces_[i].get_value()); - } - else - { - controller_state_publisher_->msg_.traction_wheels_velocity.data.push_back( - state_interfaces_[i].get_value()); - } - controller_state_publisher_->msg_.linear_velocity_command.data.push_back( - command_interfaces_[i].get_value()); - } - for (size_t i = 0; i < params_.front_wheels_names.size(); i++) - { - controller_state_publisher_->msg_.steer_positions.data.push_back( - state_interfaces_[params_.rear_wheels_names.size() + i].get_value()); - controller_state_publisher_->msg_.steering_angle_command.data.push_back( - command_interfaces_[params_.rear_wheels_names.size() + i].get_value()); - } + number_of_traction_wheels = params_.front_wheels_names.size(); + number_of_steering_wheels = params_.rear_wheels_names.size(); } - else + + for (size_t i = 0; i < number_of_traction_wheels; ++i) { - for (size_t i = 0; i < params_.front_wheels_names.size(); i++) + if (params_.position_feedback) { - if (params_.position_feedback) - { - controller_state_publisher_->msg_.traction_wheels_position.data.push_back( - state_interfaces_[i].get_value()); - } - else - { - controller_state_publisher_->msg_.traction_wheels_velocity.data.push_back( - state_interfaces_[i].get_value()); - } - controller_state_publisher_->msg_.linear_velocity_command.data.push_back( - command_interfaces_[i].get_value()); + controller_state_publisher_->msg_.traction_wheels_position.data.push_back( + state_interfaces_[i].get_value()); } - for (size_t i = 0; i < params_.rear_wheels_names.size(); i++) + else { - controller_state_publisher_->msg_.steer_positions.data.push_back( - state_interfaces_[params_.front_wheels_names.size() + i].get_value()); - controller_state_publisher_->msg_.steering_angle_command.data.push_back( - command_interfaces_[params_.front_wheels_names.size() + i].get_value()); + controller_state_publisher_->msg_.traction_wheels_velocity.data.push_back( + state_interfaces_[i].get_value()); } + controller_state_publisher_->msg_.linear_velocity_command.data.push_back( + command_interfaces_[i].get_value()); + } + + for (size_t i = 0; i < number_of_steering_wheels; ++i) + { + controller_state_publisher_->msg_.steer_positions.data.push_back( + state_interfaces_[number_of_traction_wheels + i].get_value()); + controller_state_publisher_->msg_.steering_angle_command.data.push_back( + command_interfaces_[number_of_traction_wheels + i].get_value()); } controller_state_publisher_->unlockAndPublish(); From 29e9015be7287e25410496750bcd81322d1ab953 Mon Sep 17 00:00:00 2001 From: petkovich Date: Tue, 17 Jan 2023 13:42:28 +0100 Subject: [PATCH 154/186] steering commands for ackermann --- .../src/steering_odometry.cpp | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp index abb9d5a741..287e4f21e1 100644 --- a/steering_controllers_library/src/steering_odometry.cpp +++ b/steering_controllers_library/src/steering_odometry.cpp @@ -270,6 +270,7 @@ std::tuple, std::vector> SteeringOdometry::get_comma if (fabs(steer_pos_) < 1e-6) { traction_commands = {Ws, Ws}; + steering_commands = {alpha, alpha}; } else { @@ -277,12 +278,15 @@ std::tuple, std::vector> SteeringOdometry::get_comma double Wr = Ws * (turning_radius + wheel_track_ * 0.5) / turning_radius; double Wl = Ws * (turning_radius - wheel_track_ * 0.5) / turning_radius; traction_commands = {Wr, Wl}; + + double numerator = 2 * wheelbase_ * std::sin(alpha); + double denominator_first_member = 2 * wheelbase_ * std::cos(alpha); + double denominator_second_member = wheel_track_ * std::sin(alpha); + + double alpha_r = std::atan2(numerator, denominator_first_member - denominator_second_member); + double alpha_l = std::atan2(numerator, denominator_first_member + denominator_second_member); + steering_commands = {alpha_r, alpha_l}; } - // TODO(petkovich): implement this - // double wanted_turning_radius = wheelbase_ / std::tan(alpha); - double alpha_r = alpha; - double alpha_l = alpha; - steering_commands = {alpha_r, alpha_l}; return std::make_tuple(traction_commands, steering_commands); } else From e2b8eb3caa6adedfc0ea199a6794ec6eccfd2a39 Mon Sep 17 00:00:00 2001 From: GiridharBukka Date: Thu, 19 Jan 2023 02:26:19 +0530 Subject: [PATCH 155/186] adding ref_timeout update behavior --- .../steering_controllers_library.hpp | 2 +- .../src/steering_controllers_library.cpp | 90 +++++++++++++------ 2 files changed, 62 insertions(+), 30 deletions(-) diff --git a/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp b/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp index 2d86e8ce6f..d779e656d4 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp @@ -76,7 +76,7 @@ class SteeringControllersLibrary : public controller_interface::ChainableControl const rclcpp_lifecycle::State & previous_state) override; STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::return_type - update_reference_from_subscribers() override; + update_reference_from_subscribers(const rclcpp::Time & time, const rclcpp::Duration & period) override; STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::return_type update_and_write_commands(const rclcpp::Time & time, const rclcpp::Duration & period) override; diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index 7171f5b6c3..ba9031c5b4 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -411,10 +411,10 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_deactivate( return controller_interface::CallbackReturn::SUCCESS; } -controller_interface::return_type SteeringControllersLibrary::update_reference_from_subscribers() +controller_interface::return_type SteeringControllersLibrary::update_reference_from_subscribers(const rclcpp::Time & time, const rclcpp::Duration & period) { auto current_ref = *(input_ref_.readFromRT()); - const auto age_of_last_command = get_node()->now() - (current_ref)->header.stamp; + const auto age_of_last_command = time - (current_ref)->header.stamp; // send message only if there is no timeout if (age_of_last_command <= ref_timeout_ || ref_timeout_ == rclcpp::Duration::from_seconds(0)) @@ -429,10 +429,20 @@ controller_interface::return_type SteeringControllersLibrary::update_reference_f { if (!std::isnan(current_ref->twist.linear.x) && !std::isnan(current_ref->twist.angular.z)) { - reference_interfaces_[0] = 0.0; - reference_interfaces_[1] = 0.0; - current_ref->twist.linear.x = std::numeric_limits::quiet_NaN(); - current_ref->twist.angular.z = std::numeric_limits::quiet_NaN(); + if(params_.position_feedback == false) + { + reference_interfaces_[0] = 0.0; + reference_interfaces_[1] = 0.0; + current_ref->twist.linear.x = std::numeric_limits::quiet_NaN(); + current_ref->twist.angular.z = std::numeric_limits::quiet_NaN(); + } else + { + reference_interfaces_[0] = std::numeric_limits::quiet_NaN(); + reference_interfaces_[1] = std::numeric_limits::quiet_NaN(); + current_ref->twist.linear.x = std::numeric_limits::quiet_NaN(); + current_ref->twist.angular.z = std::numeric_limits::quiet_NaN(); + } + } } @@ -449,38 +459,60 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c // Limit velocities and accelerations: // TODO(destogl): add limiter for the velocities - if (!std::isnan(reference_interfaces_[0]) && !std::isnan(reference_interfaces_[1])) + auto current_ref = *(input_ref_.readFromRT()); + const auto age_of_last_command = time - current_ref->header.stamp; + if (age_of_last_command <= ref_timeout_ || ref_timeout_ == rclcpp::Duration::from_seconds(0)) { - // store and set commands - const double linear_command = reference_interfaces_[0]; - const double angular_command = reference_interfaces_[1]; - auto [traction_commands, steering_commands] = - odometry_.get_commands(linear_command, angular_command); - if (params_.front_steering) + if (!std::isnan(reference_interfaces_[0]) && !std::isnan(reference_interfaces_[1])) { - for (size_t i = 0; i < params_.rear_wheels_names.size(); i++) + // store and set commands + const double linear_command = reference_interfaces_[0]; + const double angular_command = reference_interfaces_[1]; + auto [traction_commands, steering_commands] = + odometry_.get_commands(linear_command, angular_command); + if (params_.front_steering) { - command_interfaces_[i].set_value(traction_commands[i]); - } - for (size_t i = 0; i < params_.front_wheels_names.size(); i++) - { - command_interfaces_[i + params_.rear_wheels_names.size()].set_value(steering_commands[i]); - } - } - else - { - { - for (size_t i = 0; i < params_.front_wheels_names.size(); i++) + for (size_t i = 0; i < params_.rear_wheels_names.size(); i++) { command_interfaces_[i].set_value(traction_commands[i]); } - for (size_t i = 0; i < params_.rear_wheels_names.size(); i++) + for (size_t i = 0; i < params_.front_wheels_names.size(); i++) { - command_interfaces_[i + params_.front_wheels_names.size()].set_value( - steering_commands[i]); + command_interfaces_[i + params_.rear_wheels_names.size()].set_value(steering_commands[i]); } } - } + else + { + { + for (size_t i = 0; i < params_.front_wheels_names.size(); i++) + { + command_interfaces_[i].set_value(traction_commands[i]); + } + for (size_t i = 0; i < params_.rear_wheels_names.size(); i++) + { + command_interfaces_[i + params_.front_wheels_names.size()].set_value( + steering_commands[i]); + } + } + } + } + } + else + { + reference_interfaces_[0] = std::numeric_limits::quiet_NaN(); + reference_interfaces_[1] = std::numeric_limits::quiet_NaN(); + + for (size_t i = 0; i < params_.rear_wheels_names.size(); i++) + { + command_interfaces_[i].set_value(0.0); + } + for (size_t i = 0; i < params_.front_wheels_names.size(); i++) + { + command_interfaces_[i + params_.rear_wheels_names.size()].set_value(0.0); + } + + current_ref->twist.linear.x = std::numeric_limits::quiet_NaN(); + current_ref->twist.angular.z = std::numeric_limits::quiet_NaN(); } // Publish odometry message From 5b74105b48dcdd3e6896f78f0e0068e49089a133 Mon Sep 17 00:00:00 2001 From: GiridharBukka Date: Thu, 19 Jan 2023 02:33:15 +0530 Subject: [PATCH 156/186] test header and param --- .../steering_controllers_library_params.yaml | 17 + .../test_steering_controllers_library.hpp | 316 ++++++++++++++++++ 2 files changed, 333 insertions(+) create mode 100644 steering_controllers_library/test/steering_controllers_library_params.yaml create mode 100644 steering_controllers_library/test/test_steering_controllers_library.hpp diff --git a/steering_controllers_library/test/steering_controllers_library_params.yaml b/steering_controllers_library/test/steering_controllers_library_params.yaml new file mode 100644 index 0000000000..d200d34961 --- /dev/null +++ b/steering_controllers_library/test/steering_controllers_library_params.yaml @@ -0,0 +1,17 @@ +test_steering_controllers_library: + ros__parameters: + + reference_timeout: 0.1 + front_steering: true + open_loop: false + velocity_rolling_window_size: 10 + position_feedback: false + use_stamped_vel: true + rear_wheels_names: [rear_right_wheel_joint, rear_left_wheel_joint] + front_wheels_names: [front_right_steering_joint, front_left_steering_joint] + + wheelbase: 3.24644 + front_wheel_track: 2.12321 + rear_wheel_track: 1.76868 + front_wheels_radius: 0.45 + rear_wheels_radius: 0.45 diff --git a/steering_controllers_library/test/test_steering_controllers_library.hpp b/steering_controllers_library/test/test_steering_controllers_library.hpp new file mode 100644 index 0000000000..509f8bec65 --- /dev/null +++ b/steering_controllers_library/test/test_steering_controllers_library.hpp @@ -0,0 +1,316 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TEST_STEERING_CONTROLLERS_LIBRARY_HPP_ +#define TEST_STEERING_CONTROLLERS_LIBRARY_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "steering_controllers_library/steering_controllers_library.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" + +using ControllerStateMsg = + steering_controllers_library::SteeringControllersLibrary::AckermanControllerState; +using ControllerReferenceMsg = + steering_controllers_library::SteeringControllersLibrary::ControllerTwistReferenceMsg; + +// name constants for state interfaces +using steering_controllers_library::STATE_STEER_LEFT_WHEEL; +using steering_controllers_library::STATE_STEER_RIGHT_WHEEL; +using steering_controllers_library::STATE_TRACTION_LEFT_WHEEL; +using steering_controllers_library::STATE_TRACTION_RIGHT_WHEEL; + +// name constants for command interfaces +using steering_controllers_library::CMD_STEER_LEFT_WHEEL; +using steering_controllers_library::CMD_STEER_RIGHT_WHEEL; +using steering_controllers_library::CMD_TRACTION_LEFT_WHEEL; +using steering_controllers_library::CMD_TRACTION_RIGHT_WHEEL; + +namespace +{ +constexpr auto NODE_SUCCESS = controller_interface::CallbackReturn::SUCCESS; +constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR; +const double COMMON_THRESHOLD = 1e-6; +} // namespace +// namespace + +// subclassing and friending so we can access member variables +class TestableSteeringControllersLibrary +: public steering_controllers_library::SteeringControllersLibrary +{ + FRIEND_TEST(SteeringControllersLibraryTest, test_update_logic_not_chainable); + + +public: + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override + { + auto ret = + steering_controllers_library::SteeringControllersLibrary::on_configure(previous_state); + // Only if on_configure is successful create subscription + if (ret == CallbackReturn::SUCCESS) + { + ref_subscriber_wait_set_.add_subscription(ref_subscriber_twist_); + } + return ret; + } + + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override + { + auto ref_itfs = on_export_reference_interfaces(); + return steering_controllers_library::SteeringControllersLibrary::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); + } + +private: + rclcpp::WaitSet ref_subscriber_wait_set_; +}; + +// We are using template class here for easier reuse of Fixture in specializations of controllers +template +class SteeringControllersLibraryFixture : 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_steering_controllers_library/reference", rclcpp::SystemDefaultsQoS()); + } + + static void TearDownTestCase() {} + + void TearDown() { controller_.reset(nullptr); } + +protected: + void SetUpController(const std::string controller_name = "test_steering_controllers_library") + { + ASSERT_EQ(controller_->init(controller_name), controller_interface::return_type::OK); + + if (position_feedback_ == true) + { + traction_interface_name_ = "position"; + } + else + { + traction_interface_name_ = "velocity"; + } + + std::vector command_ifs; + command_itfs_.reserve(joint_command_values_.size()); + command_ifs.reserve(joint_command_values_.size()); + + command_itfs_.emplace_back(hardware_interface::CommandInterface( + rear_wheels_names_[0], traction_interface_name_, + &joint_command_values_[CMD_TRACTION_RIGHT_WHEEL])); + command_ifs.emplace_back(command_itfs_.back()); + + command_itfs_.emplace_back(hardware_interface::CommandInterface( + rear_wheels_names_[1], steering_interface_name_, + &joint_command_values_[CMD_TRACTION_LEFT_WHEEL])); + command_ifs.emplace_back(command_itfs_.back()); + + command_itfs_.emplace_back(hardware_interface::CommandInterface( + front_wheels_names_[0], steering_interface_name_, + &joint_command_values_[CMD_STEER_RIGHT_WHEEL])); + command_ifs.emplace_back(command_itfs_.back()); + + command_itfs_.emplace_back(hardware_interface::CommandInterface( + front_wheels_names_[1], steering_interface_name_, + &joint_command_values_[CMD_STEER_LEFT_WHEEL])); + command_ifs.emplace_back(command_itfs_.back()); + + std::vector state_ifs; + state_itfs_.reserve(joint_state_values_.size()); + state_ifs.reserve(joint_state_values_.size()); + + state_itfs_.emplace_back(hardware_interface::StateInterface( + rear_wheels_names_[0], traction_interface_name_, + &joint_state_values_[STATE_TRACTION_RIGHT_WHEEL])); + state_ifs.emplace_back(state_itfs_.back()); + + state_itfs_.emplace_back(hardware_interface::StateInterface( + rear_wheels_names_[1], traction_interface_name_, + &joint_state_values_[STATE_TRACTION_LEFT_WHEEL])); + state_ifs.emplace_back(state_itfs_.back()); + + state_itfs_.emplace_back(hardware_interface::StateInterface( + front_wheels_names_[0], steering_interface_name_, + &joint_state_values_[STATE_STEER_RIGHT_WHEEL])); + state_ifs.emplace_back(state_itfs_.back()); + + state_itfs_.emplace_back(hardware_interface::StateInterface( + front_wheels_names_[1], steering_interface_name_, + &joint_state_values_[STATE_STEER_LEFT_WHEEL])); + state_ifs.emplace_back(state_itfs_.back()); + + 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_steering_controllers_library/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)); + } + + void publish_commands(const double linear = 0.1, const double angular = 0.2) + { + 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.twist.linear.x = linear; + msg.twist.angular.z = angular; + + command_publisher_->publish(msg); + } + +protected: + // Controller-related parameters + double reference_timeout_ = 2.0; + bool front_steering_ = true; + bool open_loop_ = false; + unsigned int velocity_rolling_window_size_ = 10; + bool position_feedback_ = false; + bool use_stamped_vel_ = true; + std::vector rear_wheels_names_ = {"rear_right_wheel_joint", "rear_left_wheel_joint"}; + std::vector front_wheels_names_ = { + "front_right_steering_joint", "front_left_steering_joint"}; + std::vector joint_names_ = { + rear_wheels_names_[0], rear_wheels_names_[1], front_wheels_names_[0], front_wheels_names_[1]}; + + std::vector rear_wheels_preceeding_names_ = { + "pid_controller/rear_right_wheel_joint", "pid_controller/rear_left_wheel_joint"}; + std::vector front_wheels_preceeding_names_ = { + "pid_controller/front_right_steering_joint", "pid_controller/front_left_steering_joint"}; + std::vector preceeding_joint_names_ = { + rear_wheels_preceeding_names_[0], rear_wheels_preceeding_names_[1], + front_wheels_preceeding_names_[0], front_wheels_preceeding_names_[1]}; + + double wheelbase_ = 3.24644; + double front_wheel_track_ = 2.12321; + double rear_wheel_track_ = 1.76868; + double front_wheels_radius_ = 0.45; + double rear_wheels_radius_ = 0.45; + + std::array joint_state_values_ = {0.5, 0.5, 0.0, 0.0}; + std::array joint_command_values_ = {1.1, 3.3, 2.2, 4.4}; + static constexpr double TEST_LINEAR_VELOCITY_X = 1.5; + static constexpr double TEST_LINEAR_VELOCITY_y = 0.0; + static constexpr double TEST_ANGULAR_VELOCITY_Z = 0.0; + double command_lin_x = 111; + std::array joint_reference_interfaces_ = {"linear/velocity", "angular/position"}; + std::string steering_interface_name_ = "position"; + // defined in setup + std::string traction_interface_name_ = ""; + std::string preceeding_prefix_ = "pid_controller"; + + 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_; +}; + +#endif // TEST_STEERING_CONTROLLERS_LIBRARY_HPP_ From 91e2d2c5d311171da99b9efc20efafbd94fb0c92 Mon Sep 17 00:00:00 2001 From: GiridharBukka Date: Thu, 19 Jan 2023 02:35:04 +0530 Subject: [PATCH 157/186] test cpp --- .../test_steering_controllers_library.cpp | 143 ++++++++++++++++++ 1 file changed, 143 insertions(+) create mode 100644 steering_controllers_library/test/test_steering_controllers_library.cpp diff --git a/steering_controllers_library/test/test_steering_controllers_library.cpp b/steering_controllers_library/test/test_steering_controllers_library.cpp new file mode 100644 index 0000000000..58ee8731d2 --- /dev/null +++ b/steering_controllers_library/test/test_steering_controllers_library.cpp @@ -0,0 +1,143 @@ +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "test_steering_controllers_library.hpp" + +#include +#include +#include +#include +#include + +#include "hardware_interface/types/hardware_interface_type_values.hpp" + +class SteeringControllersLibraryTest +: public SteeringControllersLibraryFixture +{ +}; + +// Tests controller behavior when too old msg is sent / age_of_last_command > ref_timeout case +TEST_F(SteeringControllersLibraryTest, test_update_logic_not_chainable) +{ + // 1. age>ref_timeout 2. ageget_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()); + + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } + + // set command statically + joint_command_values_[1] = command_lin_x; + + std::shared_ptr msg = std::make_shared(); + + msg->header.stamp = controller_->get_node()->now() - controller_->ref_timeout_ - + rclcpp::Duration::from_seconds(0.1); + msg->twist.linear.x = TEST_LINEAR_VELOCITY_X; + msg->twist.linear.y = TEST_LINEAR_VELOCITY_y; + msg->twist.linear.z = std::numeric_limits::quiet_NaN(); + msg->twist.angular.x = std::numeric_limits::quiet_NaN(); + msg->twist.angular.y = std::numeric_limits::quiet_NaN(); + msg->twist.angular.z = TEST_ANGULAR_VELOCITY_Z; + controller_->input_ref_.writeFromNonRT(msg); + const auto age_of_last_command = + controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; + + // age_of_last_command > ref_timeout_ + ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_); + ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); + ASSERT_EQ( + controller_->update_reference_from_subscribers( + controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_EQ(controller_->reference_interfaces_[1], 0); + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_FALSE(std::isnan(interface)); + } + + ASSERT_EQ( + controller_->update_and_write_commands( + controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[0])); + + EXPECT_EQ(joint_command_values_[1], 0); + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } + for (size_t i = 0; i < controller_->command_interfaces_.size(); ++i) + { + EXPECT_EQ(controller_->command_interfaces_[i].get_value(), 0.0); + } + + std::shared_ptr msg_2 = std::make_shared(); + msg_2->header.stamp = controller_->get_node()->now() - rclcpp::Duration::from_seconds(0.01); + msg_2->twist.linear.x = TEST_LINEAR_VELOCITY_X; + msg_2->twist.linear.y = TEST_LINEAR_VELOCITY_y; + msg_2->twist.linear.z = std::numeric_limits::quiet_NaN(); + msg_2->twist.angular.x = std::numeric_limits::quiet_NaN(); + msg_2->twist.angular.y = std::numeric_limits::quiet_NaN(); + msg_2->twist.angular.z = TEST_ANGULAR_VELOCITY_Z; + controller_->input_ref_.writeFromNonRT(msg_2); + const auto age_of_last_command_2 = + controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; + + // age_of_last_command_2 < ref_timeout_ + ASSERT_TRUE(age_of_last_command_2 <= controller_->ref_timeout_); + ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); + ASSERT_EQ( + controller_->update_reference_from_subscribers( + controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + ASSERT_EQ( + controller_->update_and_write_commands( + controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_NE(joint_command_values_[1], command_lin_x); + // w0_vel = 1.0 / params_.kinematics.wheels_radius * (body_velocity_center_frame_.linear_x - body_velocity_center_frame_.linear_y - params_.kinematics.wheels_k * body_velocity_center_frame_.angular_z); + // joint_command_values_[1] = 1.0 / 0.5 * (1.5 - 0.0 - 1 * 0.0) + EXPECT_EQ(joint_command_values_[1], 3.0); + ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_FALSE(std::isnan(interface)); + } + for (size_t i = 0; i < controller_->command_interfaces_.size(); ++i) + { + EXPECT_EQ(controller_->command_interfaces_[i].get_value(), 3.0); + } +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} From 83c882e29ff47dcdfe3fde511bacecd35994bf33 Mon Sep 17 00:00:00 2001 From: GiridharBukka Date: Thu, 19 Jan 2023 13:11:42 +0100 Subject: [PATCH 158/186] working tests for steering_controllers_library --- .pre-commit-config.yaml | 14 +-- steering_controllers_library/CMakeLists.txt | 16 ++- .../steering_controllers_library.hpp | 3 +- .../steering_odometry.hpp | 2 +- steering_controllers_library/package.xml | 2 + .../src/steering_controllers_library.cpp | 39 +++--- .../test_steering_controllers_library.cpp | 117 ++++++++++++++---- .../test_steering_controllers_library.hpp | 45 +++++-- 8 files changed, 171 insertions(+), 67 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 58fe46dd90..24d1c3b7d1 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -15,7 +15,7 @@ repos: # Standard hooks - repo: https://github.com/pre-commit/pre-commit-hooks - rev: v4.3.0 + rev: v4.4.0 hooks: - id: check-added-large-files - id: check-ast @@ -33,13 +33,13 @@ repos: # Python hooks - repo: https://github.com/asottile/pyupgrade - rev: v2.37.3 + rev: v3.3.1 hooks: - id: pyupgrade args: [--py36-plus] - repo: https://github.com/psf/black - rev: 22.6.0 + rev: 22.12.0 hooks: - id: black args: ["--line-length=99"] @@ -52,7 +52,7 @@ repos: args: ["--ignore=D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D404"] - repo: https://github.com/pycqa/flake8 - rev: 5.0.4 + rev: 6.0.0 hooks: - id: flake8 args: ["--ignore=E501"] @@ -119,14 +119,14 @@ repos: # Docs - RestructuredText hooks - repo: https://github.com/PyCQA/doc8 - rev: v1.0.0 + rev: v1.1.1 hooks: - id: doc8 args: ['--max-line-length=100', '--ignore=D001'] exclude: CHANGELOG\.rst$ - repo: https://github.com/pre-commit/pygrep-hooks - rev: v1.9.0 + rev: v1.10.0 hooks: - id: rst-backticks exclude: CHANGELOG\.rst$ @@ -136,7 +136,7 @@ repos: # Spellcheck in comments and docs # skipping of *.svg files is not working... - repo: https://github.com/codespell-project/codespell - rev: v2.1.0 + rev: v2.2.2 hooks: - id: codespell args: ['--write-changes', '--uri-ignore-words-list=ist'] diff --git a/steering_controllers_library/CMakeLists.txt b/steering_controllers_library/CMakeLists.txt index 50d61edbe1..9f919f0c87 100644 --- a/steering_controllers_library/CMakeLists.txt +++ b/steering_controllers_library/CMakeLists.txt @@ -64,7 +64,21 @@ install( ) if(BUILD_TESTING) - # TODO + find_package(ament_cmake_gmock REQUIRED) + find_package(controller_manager REQUIRED) + find_package(hardware_interface REQUIRED) + find_package(ros2_control_test_assets REQUIRED) + + add_rostest_with_parameters_gmock( + test_steering_controllers_library test/test_steering_controllers_library.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/steering_controllers_library_params.yaml) + target_include_directories(test_steering_controllers_library PRIVATE include) + target_link_libraries(test_steering_controllers_library steering_controllers_library) + ament_target_dependencies( + test_steering_controllers_library + controller_interface + hardware_interface + ) endif() ament_export_include_directories( diff --git a/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp b/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp index d779e656d4..81d8b5672a 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp @@ -76,7 +76,8 @@ class SteeringControllersLibrary : public controller_interface::ChainableControl const rclcpp_lifecycle::State & previous_state) override; STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::return_type - update_reference_from_subscribers(const rclcpp::Time & time, const rclcpp::Duration & period) override; + update_reference_from_subscribers( + const rclcpp::Time & time, const rclcpp::Duration & period) override; STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::return_type update_and_write_commands(const rclcpp::Time & time, const rclcpp::Duration & period) override; diff --git a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp index ed9ec74d41..54c1d7fd21 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp @@ -250,7 +250,7 @@ class SteeringOdometry double traction_wheel_old_pos_; double traction_right_wheel_old_pos_; double traction_left_wheel_old_pos_; - /// Rolling mean accumulators for the linar and angular velocities: + /// Rolling mean accumulators for the linear and angular velocities: size_t velocity_rolling_window_size_; rcpputils::RollingMeanAccumulator linear_acc_; rcpputils::RollingMeanAccumulator angular_acc_; diff --git a/steering_controllers_library/package.xml b/steering_controllers_library/package.xml index caba52e8e8..0d9aa9da39 100644 --- a/steering_controllers_library/package.xml +++ b/steering_controllers_library/package.xml @@ -14,6 +14,8 @@ dr. sc. Tomislav Petkovic Tony Najjar + ament_cmake + generate_parameter_library control_msgs diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index ba9031c5b4..abd15d4a76 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -411,7 +411,8 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_deactivate( return controller_interface::CallbackReturn::SUCCESS; } -controller_interface::return_type SteeringControllersLibrary::update_reference_from_subscribers(const rclcpp::Time & time, const rclcpp::Duration & period) +controller_interface::return_type SteeringControllersLibrary::update_reference_from_subscribers( + const rclcpp::Time & time, const rclcpp::Duration & period) { auto current_ref = *(input_ref_.readFromRT()); const auto age_of_last_command = time - (current_ref)->header.stamp; @@ -429,20 +430,20 @@ controller_interface::return_type SteeringControllersLibrary::update_reference_f { if (!std::isnan(current_ref->twist.linear.x) && !std::isnan(current_ref->twist.angular.z)) { - if(params_.position_feedback == false) + if (params_.position_feedback == false) { reference_interfaces_[0] = 0.0; reference_interfaces_[1] = 0.0; current_ref->twist.linear.x = std::numeric_limits::quiet_NaN(); - current_ref->twist.angular.z = std::numeric_limits::quiet_NaN(); - } else + current_ref->twist.angular.z = std::numeric_limits::quiet_NaN(); + } + else { reference_interfaces_[0] = std::numeric_limits::quiet_NaN(); reference_interfaces_[1] = std::numeric_limits::quiet_NaN(); current_ref->twist.linear.x = std::numeric_limits::quiet_NaN(); current_ref->twist.angular.z = std::numeric_limits::quiet_NaN(); - } - + } } } @@ -495,24 +496,24 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c } } } - } + } } else { - reference_interfaces_[0] = std::numeric_limits::quiet_NaN(); - reference_interfaces_[1] = std::numeric_limits::quiet_NaN(); + reference_interfaces_[0] = std::numeric_limits::quiet_NaN(); + reference_interfaces_[1] = std::numeric_limits::quiet_NaN(); - for (size_t i = 0; i < params_.rear_wheels_names.size(); i++) - { - command_interfaces_[i].set_value(0.0); - } - for (size_t i = 0; i < params_.front_wheels_names.size(); i++) - { - command_interfaces_[i + params_.rear_wheels_names.size()].set_value(0.0); - } + for (size_t i = 0; i < params_.rear_wheels_names.size(); i++) + { + command_interfaces_[i].set_value(0.0); + } + for (size_t i = 0; i < params_.front_wheels_names.size(); i++) + { + command_interfaces_[i + params_.rear_wheels_names.size()].set_value(0.0); + } - current_ref->twist.linear.x = std::numeric_limits::quiet_NaN(); - current_ref->twist.angular.z = std::numeric_limits::quiet_NaN(); + current_ref->twist.linear.x = std::numeric_limits::quiet_NaN(); + current_ref->twist.angular.z = std::numeric_limits::quiet_NaN(); } // Publish odometry message diff --git a/steering_controllers_library/test/test_steering_controllers_library.cpp b/steering_controllers_library/test/test_steering_controllers_library.cpp index 58ee8731d2..3a6a7e2a4c 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.cpp +++ b/steering_controllers_library/test/test_steering_controllers_library.cpp @@ -27,10 +27,60 @@ class SteeringControllersLibraryTest { }; -// Tests controller behavior when too old msg is sent / age_of_last_command > ref_timeout case -TEST_F(SteeringControllersLibraryTest, test_update_logic_not_chainable) +// checking if all interfaces, command, state and reference are exported as expected +TEST_F(SteeringControllersLibraryTest, 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()); + EXPECT_EQ( + command_intefaces.names[CMD_TRACTION_RIGHT_WHEEL], + rear_wheels_names_[0] + "/" + traction_interface_name_); + EXPECT_EQ( + command_intefaces.names[CMD_TRACTION_LEFT_WHEEL], + rear_wheels_names_[1] + "/" + traction_interface_name_); + EXPECT_EQ( + command_intefaces.names[CMD_STEER_RIGHT_WHEEL], + front_wheels_names_[0] + "/" + steering_interface_name_); + EXPECT_EQ( + command_intefaces.names[CMD_STEER_LEFT_WHEEL], + front_wheels_names_[1] + "/" + steering_interface_name_); + + auto state_intefaces = controller_->state_interface_configuration(); + ASSERT_EQ(state_intefaces.names.size(), joint_state_values_.size()); + EXPECT_EQ( + state_intefaces.names[STATE_TRACTION_RIGHT_WHEEL], + controller_->rear_wheels_state_names_[0] + "/" + traction_interface_name_); + EXPECT_EQ( + state_intefaces.names[STATE_TRACTION_LEFT_WHEEL], + controller_->rear_wheels_state_names_[1] + "/" + traction_interface_name_); + EXPECT_EQ( + state_intefaces.names[STATE_STEER_RIGHT_WHEEL], + controller_->front_wheels_state_names_[0] + "/" + steering_interface_name_); + EXPECT_EQ( + state_intefaces.names[STATE_STEER_LEFT_WHEEL], + controller_->front_wheels_state_names_[1] + "/" + steering_interface_name_); + + // check ref itfs + auto reference_interfaces = controller_->export_reference_interfaces(); + ASSERT_EQ(reference_interfaces.size(), joint_reference_interfaces_.size()); + for (size_t i = 0; i < joint_reference_interfaces_.size(); ++i) + { + const std::string ref_itf_name = + std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i]; + 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_reference_interfaces_[i]); + } +} + +// Tests controller update_reference_from_subscribers and its two cases for position_feedback true/false behavior +// when too old msg is sent i.e age_of_last_command > ref_timeout case +TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout) { - // 1. age>ref_timeout 2. age msg = std::make_shared(); + // adjusting to achieve age_of_last_command > ref_timeout msg->header.stamp = controller_->get_node()->now() - controller_->ref_timeout_ - rclcpp::Duration::from_seconds(0.1); msg->twist.linear.x = TEST_LINEAR_VELOCITY_X; @@ -60,9 +111,13 @@ TEST_F(SteeringControllersLibraryTest, test_update_logic_not_chainable) msg->twist.angular.y = std::numeric_limits::quiet_NaN(); msg->twist.angular.z = TEST_ANGULAR_VELOCITY_Z; controller_->input_ref_.writeFromNonRT(msg); + const auto age_of_last_command = controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; + // case 1 position_feedback = false + controller_->params_.position_feedback = false; + // age_of_last_command > ref_timeout_ ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_); ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); @@ -76,6 +131,8 @@ TEST_F(SteeringControllersLibraryTest, test_update_logic_not_chainable) { EXPECT_FALSE(std::isnan(interface)); } + EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x)); + EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z)); ASSERT_EQ( controller_->update_and_write_commands( @@ -83,53 +140,63 @@ TEST_F(SteeringControllersLibraryTest, test_update_logic_not_chainable) controller_interface::return_type::OK); EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[0])); - - EXPECT_EQ(joint_command_values_[1], 0); for (const auto & interface : controller_->reference_interfaces_) { EXPECT_TRUE(std::isnan(interface)); } + + EXPECT_EQ(joint_command_values_[1], 0); for (size_t i = 0; i < controller_->command_interfaces_.size(); ++i) { EXPECT_EQ(controller_->command_interfaces_[i].get_value(), 0.0); } - std::shared_ptr msg_2 = std::make_shared(); - msg_2->header.stamp = controller_->get_node()->now() - rclcpp::Duration::from_seconds(0.01); - msg_2->twist.linear.x = TEST_LINEAR_VELOCITY_X; - msg_2->twist.linear.y = TEST_LINEAR_VELOCITY_y; - msg_2->twist.linear.z = std::numeric_limits::quiet_NaN(); - msg_2->twist.angular.x = std::numeric_limits::quiet_NaN(); - msg_2->twist.angular.y = std::numeric_limits::quiet_NaN(); - msg_2->twist.angular.z = TEST_ANGULAR_VELOCITY_Z; - controller_->input_ref_.writeFromNonRT(msg_2); - const auto age_of_last_command_2 = - controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; + // case 2 position_feedback = true + controller_->params_.position_feedback = true; - // age_of_last_command_2 < ref_timeout_ - ASSERT_TRUE(age_of_last_command_2 <= controller_->ref_timeout_); + // adjusting to achieve age_of_last_command > ref_timeout + msg->header.stamp = controller_->get_node()->now() - controller_->ref_timeout_ - + rclcpp::Duration::from_seconds(0.1); + msg->twist.linear.x = TEST_LINEAR_VELOCITY_X; + msg->twist.linear.y = TEST_LINEAR_VELOCITY_y; + msg->twist.linear.z = std::numeric_limits::quiet_NaN(); + msg->twist.angular.x = std::numeric_limits::quiet_NaN(); + msg->twist.angular.y = std::numeric_limits::quiet_NaN(); + msg->twist.angular.z = TEST_ANGULAR_VELOCITY_Z; + controller_->input_ref_.writeFromNonRT(msg); + + // age_of_last_command > ref_timeout_ + ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_); ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); ASSERT_EQ( controller_->update_reference_from_subscribers( controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); + + EXPECT_NE(controller_->reference_interfaces_[1], 0); + EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[1])); + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } + EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x)); + EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z)); + ASSERT_EQ( controller_->update_and_write_commands( controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - EXPECT_NE(joint_command_values_[1], command_lin_x); - // w0_vel = 1.0 / params_.kinematics.wheels_radius * (body_velocity_center_frame_.linear_x - body_velocity_center_frame_.linear_y - params_.kinematics.wheels_k * body_velocity_center_frame_.angular_z); - // joint_command_values_[1] = 1.0 / 0.5 * (1.5 - 0.0 - 1 * 0.0) - EXPECT_EQ(joint_command_values_[1], 3.0); - ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); + EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[0])); for (const auto & interface : controller_->reference_interfaces_) { - EXPECT_FALSE(std::isnan(interface)); + EXPECT_TRUE(std::isnan(interface)); } + + EXPECT_EQ(joint_command_values_[1], 0); for (size_t i = 0; i < controller_->command_interfaces_.size(); ++i) { - EXPECT_EQ(controller_->command_interfaces_[i].get_value(), 3.0); + EXPECT_EQ(controller_->command_interfaces_[i].get_value(), 0.0); } } diff --git a/steering_controllers_library/test/test_steering_controllers_library.hpp b/steering_controllers_library/test/test_steering_controllers_library.hpp index 509f8bec65..1517b18800 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.hpp +++ b/steering_controllers_library/test/test_steering_controllers_library.hpp @@ -23,7 +23,6 @@ #include #include -#include "steering_controllers_library/steering_controllers_library.hpp" #include "gmock/gmock.h" #include "hardware_interface/loaned_command_interface.hpp" #include "hardware_interface/loaned_state_interface.hpp" @@ -32,29 +31,35 @@ #include "rclcpp/time.hpp" #include "rclcpp/utilities.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "steering_controllers_library/steering_controllers_library.hpp" using ControllerStateMsg = steering_controllers_library::SteeringControllersLibrary::AckermanControllerState; using ControllerReferenceMsg = steering_controllers_library::SteeringControllersLibrary::ControllerTwistReferenceMsg; +// NOTE: Testing steering_controllers_library for ackermann vehicle configuration only + // name constants for state interfaces -using steering_controllers_library::STATE_STEER_LEFT_WHEEL; -using steering_controllers_library::STATE_STEER_RIGHT_WHEEL; -using steering_controllers_library::STATE_TRACTION_LEFT_WHEEL; -using steering_controllers_library::STATE_TRACTION_RIGHT_WHEEL; +static constexpr size_t STATE_TRACTION_RIGHT_WHEEL = 0; +static constexpr size_t STATE_TRACTION_LEFT_WHEEL = 1; +static constexpr size_t STATE_STEER_RIGHT_WHEEL = 2; +static constexpr size_t STATE_STEER_LEFT_WHEEL = 3; // name constants for command interfaces -using steering_controllers_library::CMD_STEER_LEFT_WHEEL; -using steering_controllers_library::CMD_STEER_RIGHT_WHEEL; -using steering_controllers_library::CMD_TRACTION_LEFT_WHEEL; -using steering_controllers_library::CMD_TRACTION_RIGHT_WHEEL; +static constexpr size_t CMD_TRACTION_RIGHT_WHEEL = 0; +static constexpr size_t CMD_TRACTION_LEFT_WHEEL = 1; +static constexpr size_t CMD_STEER_RIGHT_WHEEL = 2; +static constexpr size_t CMD_STEER_LEFT_WHEEL = 3; + +static constexpr size_t NR_STATE_ITFS = 4; +static constexpr size_t NR_CMD_ITFS = 4; +static constexpr size_t NR_REF_ITFS = 2; namespace { constexpr auto NODE_SUCCESS = controller_interface::CallbackReturn::SUCCESS; constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR; -const double COMMON_THRESHOLD = 1e-6; } // namespace // namespace @@ -62,8 +67,8 @@ const double COMMON_THRESHOLD = 1e-6; class TestableSteeringControllersLibrary : public steering_controllers_library::SteeringControllersLibrary { - FRIEND_TEST(SteeringControllersLibraryTest, test_update_logic_not_chainable); - + FRIEND_TEST(SteeringControllersLibraryTest, check_exported_intefaces); + FRIEND_TEST(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout); public: controller_interface::CallbackReturn on_configure( @@ -112,6 +117,20 @@ class TestableSteeringControllersLibrary return wait_for_command(executor, ref_subscriber_wait_set_, timeout); } + // implementing methods which are declared virtual in the steering_controllers_library.hpp + void initialize_implementation_parameter_listener() + { + param_listener_ = std::make_shared(get_node()); + } + + controller_interface::CallbackReturn configure_odometry() + { + set_interface_numbers(NR_STATE_ITFS, NR_CMD_ITFS, NR_REF_ITFS); + return controller_interface::CallbackReturn::SUCCESS; + } + + bool update_odometry(const rclcpp::Duration & period) { return true; } + private: rclcpp::WaitSet ref_subscriber_wait_set_; }; @@ -308,7 +327,7 @@ class SteeringControllersLibraryFixture : public ::testing::Test std::vector command_itfs_; // Test related parameters - std::unique_ptr controller_; + std::unique_ptr controller_; rclcpp::Node::SharedPtr command_publisher_node_; rclcpp::Publisher::SharedPtr command_publisher_; }; From 9774c21f5985667184b68ea46b38f2d5f5b10f4f Mon Sep 17 00:00:00 2001 From: petkovich Date: Fri, 20 Jan 2023 18:24:57 +0100 Subject: [PATCH 159/186] reverted to old update_and_write_commands, steering tests do not pass ATM --- .../steering_controllers_library.hpp | 3 +- .../src/steering_controllers_library.cpp | 89 ++++++------------- .../test_steering_controllers_library.cpp | 8 +- 3 files changed, 31 insertions(+), 69 deletions(-) diff --git a/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp b/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp index 81d8b5672a..2d86e8ce6f 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp @@ -76,8 +76,7 @@ class SteeringControllersLibrary : public controller_interface::ChainableControl const rclcpp_lifecycle::State & previous_state) override; STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::return_type - update_reference_from_subscribers( - const rclcpp::Time & time, const rclcpp::Duration & period) override; + update_reference_from_subscribers() override; STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::return_type update_and_write_commands(const rclcpp::Time & time, const rclcpp::Duration & period) override; diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index abd15d4a76..7171f5b6c3 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -411,11 +411,10 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_deactivate( return controller_interface::CallbackReturn::SUCCESS; } -controller_interface::return_type SteeringControllersLibrary::update_reference_from_subscribers( - const rclcpp::Time & time, const rclcpp::Duration & period) +controller_interface::return_type SteeringControllersLibrary::update_reference_from_subscribers() { auto current_ref = *(input_ref_.readFromRT()); - const auto age_of_last_command = time - (current_ref)->header.stamp; + const auto age_of_last_command = get_node()->now() - (current_ref)->header.stamp; // send message only if there is no timeout if (age_of_last_command <= ref_timeout_ || ref_timeout_ == rclcpp::Duration::from_seconds(0)) @@ -430,20 +429,10 @@ controller_interface::return_type SteeringControllersLibrary::update_reference_f { if (!std::isnan(current_ref->twist.linear.x) && !std::isnan(current_ref->twist.angular.z)) { - if (params_.position_feedback == false) - { - reference_interfaces_[0] = 0.0; - reference_interfaces_[1] = 0.0; - current_ref->twist.linear.x = std::numeric_limits::quiet_NaN(); - current_ref->twist.angular.z = std::numeric_limits::quiet_NaN(); - } - else - { - reference_interfaces_[0] = std::numeric_limits::quiet_NaN(); - reference_interfaces_[1] = std::numeric_limits::quiet_NaN(); - current_ref->twist.linear.x = std::numeric_limits::quiet_NaN(); - current_ref->twist.angular.z = std::numeric_limits::quiet_NaN(); - } + reference_interfaces_[0] = 0.0; + reference_interfaces_[1] = 0.0; + current_ref->twist.linear.x = std::numeric_limits::quiet_NaN(); + current_ref->twist.angular.z = std::numeric_limits::quiet_NaN(); } } @@ -460,61 +449,39 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c // Limit velocities and accelerations: // TODO(destogl): add limiter for the velocities - auto current_ref = *(input_ref_.readFromRT()); - const auto age_of_last_command = time - current_ref->header.stamp; - if (age_of_last_command <= ref_timeout_ || ref_timeout_ == rclcpp::Duration::from_seconds(0)) + if (!std::isnan(reference_interfaces_[0]) && !std::isnan(reference_interfaces_[1])) { - if (!std::isnan(reference_interfaces_[0]) && !std::isnan(reference_interfaces_[1])) + // store and set commands + const double linear_command = reference_interfaces_[0]; + const double angular_command = reference_interfaces_[1]; + auto [traction_commands, steering_commands] = + odometry_.get_commands(linear_command, angular_command); + if (params_.front_steering) + { + for (size_t i = 0; i < params_.rear_wheels_names.size(); i++) + { + command_interfaces_[i].set_value(traction_commands[i]); + } + for (size_t i = 0; i < params_.front_wheels_names.size(); i++) + { + command_interfaces_[i + params_.rear_wheels_names.size()].set_value(steering_commands[i]); + } + } + else { - // store and set commands - const double linear_command = reference_interfaces_[0]; - const double angular_command = reference_interfaces_[1]; - auto [traction_commands, steering_commands] = - odometry_.get_commands(linear_command, angular_command); - if (params_.front_steering) { - for (size_t i = 0; i < params_.rear_wheels_names.size(); i++) - { - command_interfaces_[i].set_value(traction_commands[i]); - } for (size_t i = 0; i < params_.front_wheels_names.size(); i++) { - command_interfaces_[i + params_.rear_wheels_names.size()].set_value(steering_commands[i]); + command_interfaces_[i].set_value(traction_commands[i]); } - } - else - { + for (size_t i = 0; i < params_.rear_wheels_names.size(); i++) { - for (size_t i = 0; i < params_.front_wheels_names.size(); i++) - { - command_interfaces_[i].set_value(traction_commands[i]); - } - for (size_t i = 0; i < params_.rear_wheels_names.size(); i++) - { - command_interfaces_[i + params_.front_wheels_names.size()].set_value( - steering_commands[i]); - } + command_interfaces_[i + params_.front_wheels_names.size()].set_value( + steering_commands[i]); } } } } - else - { - reference_interfaces_[0] = std::numeric_limits::quiet_NaN(); - reference_interfaces_[1] = std::numeric_limits::quiet_NaN(); - - for (size_t i = 0; i < params_.rear_wheels_names.size(); i++) - { - command_interfaces_[i].set_value(0.0); - } - for (size_t i = 0; i < params_.front_wheels_names.size(); i++) - { - command_interfaces_[i + params_.rear_wheels_names.size()].set_value(0.0); - } - - current_ref->twist.linear.x = std::numeric_limits::quiet_NaN(); - current_ref->twist.angular.z = std::numeric_limits::quiet_NaN(); - } // Publish odometry message // Compute and store orientation info diff --git a/steering_controllers_library/test/test_steering_controllers_library.cpp b/steering_controllers_library/test/test_steering_controllers_library.cpp index 3a6a7e2a4c..87141b733f 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.cpp +++ b/steering_controllers_library/test/test_steering_controllers_library.cpp @@ -122,9 +122,7 @@ TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout) ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_); ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); ASSERT_EQ( - controller_->update_reference_from_subscribers( - controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); + controller_->update_reference_from_subscribers(), controller_interface::return_type::OK); EXPECT_EQ(controller_->reference_interfaces_[1], 0); for (const auto & interface : controller_->reference_interfaces_) @@ -169,9 +167,7 @@ TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout) ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_); ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); ASSERT_EQ( - controller_->update_reference_from_subscribers( - controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); + controller_->update_reference_from_subscribers(), controller_interface::return_type::OK); EXPECT_NE(controller_->reference_interfaces_[1], 0); EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[1])); From 26772b20ee29147bcd6bce17983b12d29c7d2e37 Mon Sep 17 00:00:00 2001 From: petkovich Date: Fri, 20 Jan 2023 22:09:37 +0100 Subject: [PATCH 160/186] odometry config, some variables renamed --- .../steering_odometry.hpp | 2 +- .../test_steering_controllers_library.cpp | 42 ++++++++----------- .../test_steering_controllers_library.hpp | 14 +++++-- 3 files changed, 29 insertions(+), 29 deletions(-) diff --git a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp index 54c1d7fd21..6cc8ffd4c2 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp @@ -244,7 +244,7 @@ class SteeringOdometry double wheel_radius_; /// Configuration type used for thr forward kinematics - unsigned int config_type_; + int config_type_ = -1; /// Previous wheel position/state [rad]: double traction_wheel_old_pos_; diff --git a/steering_controllers_library/test/test_steering_controllers_library.cpp b/steering_controllers_library/test/test_steering_controllers_library.cpp index 87141b733f..1b3678eb1a 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.cpp +++ b/steering_controllers_library/test/test_steering_controllers_library.cpp @@ -77,8 +77,9 @@ TEST_F(SteeringControllersLibraryTest, check_exported_intefaces) } } -// Tests controller update_reference_from_subscribers and its two cases for position_feedback true/false behavior -// when too old msg is sent i.e age_of_last_command > ref_timeout case +// Tests controller update_reference_from_subscribers and +// its two cases for position_feedback true/false behavior +// when too old msg is sent i.e age_of_last_command > ref_timeout case TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout) { SetUpController(); @@ -97,7 +98,9 @@ TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout) } // set command statically - joint_command_values_[1] = command_lin_x; + const double TEST_LINEAR_VELOCITY_X = 1.5; + const double TEST_LINEAR_VELOCITY_Y = 0.0; + const double TEST_ANGULAR_VELOCITY_Z = 0.3; std::shared_ptr msg = std::make_shared(); @@ -105,7 +108,7 @@ TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout) msg->header.stamp = controller_->get_node()->now() - controller_->ref_timeout_ - rclcpp::Duration::from_seconds(0.1); msg->twist.linear.x = TEST_LINEAR_VELOCITY_X; - msg->twist.linear.y = TEST_LINEAR_VELOCITY_y; + msg->twist.linear.y = TEST_LINEAR_VELOCITY_Y; msg->twist.linear.z = std::numeric_limits::quiet_NaN(); msg->twist.angular.x = std::numeric_limits::quiet_NaN(); msg->twist.angular.y = std::numeric_limits::quiet_NaN(); @@ -122,31 +125,27 @@ TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout) ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_); ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); ASSERT_EQ( - controller_->update_reference_from_subscribers(), controller_interface::return_type::OK); + controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); - EXPECT_EQ(controller_->reference_interfaces_[1], 0); + EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[0])); + EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[1])); for (const auto & interface : controller_->reference_interfaces_) { - EXPECT_FALSE(std::isnan(interface)); + EXPECT_TRUE(std::isnan(interface)); } EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x)); EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z)); - ASSERT_EQ( - controller_->update_and_write_commands( - controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[0])); for (const auto & interface : controller_->reference_interfaces_) { EXPECT_TRUE(std::isnan(interface)); } - EXPECT_EQ(joint_command_values_[1], 0); for (size_t i = 0; i < controller_->command_interfaces_.size(); ++i) { - EXPECT_EQ(controller_->command_interfaces_[i].get_value(), 0.0); + EXPECT_EQ(controller_->command_interfaces_[i].get_value(), 0); } // case 2 position_feedback = true @@ -156,7 +155,7 @@ TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout) msg->header.stamp = controller_->get_node()->now() - controller_->ref_timeout_ - rclcpp::Duration::from_seconds(0.1); msg->twist.linear.x = TEST_LINEAR_VELOCITY_X; - msg->twist.linear.y = TEST_LINEAR_VELOCITY_y; + msg->twist.linear.y = TEST_LINEAR_VELOCITY_Y; msg->twist.linear.z = std::numeric_limits::quiet_NaN(); msg->twist.angular.x = std::numeric_limits::quiet_NaN(); msg->twist.angular.y = std::numeric_limits::quiet_NaN(); @@ -167,9 +166,10 @@ TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout) ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_); ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); ASSERT_EQ( - controller_->update_reference_from_subscribers(), controller_interface::return_type::OK); + controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); - EXPECT_NE(controller_->reference_interfaces_[1], 0); + EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[0])); EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[1])); for (const auto & interface : controller_->reference_interfaces_) { @@ -178,21 +178,15 @@ TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout) EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x)); EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z)); - ASSERT_EQ( - controller_->update_and_write_commands( - controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[0])); for (const auto & interface : controller_->reference_interfaces_) { EXPECT_TRUE(std::isnan(interface)); } - EXPECT_EQ(joint_command_values_[1], 0); for (size_t i = 0; i < controller_->command_interfaces_.size(); ++i) { - EXPECT_EQ(controller_->command_interfaces_[i].get_value(), 0.0); + EXPECT_EQ(controller_->command_interfaces_[i].get_value(), 0); } } diff --git a/steering_controllers_library/test/test_steering_controllers_library.hpp b/steering_controllers_library/test/test_steering_controllers_library.hpp index 1517b18800..2a5123467c 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.hpp +++ b/steering_controllers_library/test/test_steering_controllers_library.hpp @@ -56,6 +56,12 @@ static constexpr size_t NR_STATE_ITFS = 4; static constexpr size_t NR_CMD_ITFS = 4; static constexpr size_t NR_REF_ITFS = 2; +static constexpr double WHEELBASE_ = 3.24644; +static constexpr double FRONT_WHEEL_TRACK_ = 2.12321; +static constexpr double REAR_WHEEL_TRACK_ = 1.76868; +static constexpr double FRONT_WHEELS_RADIUS_ = 0.45; +static constexpr double REAR_WHEELS_RADIUS_ = 0.45; + namespace { constexpr auto NODE_SUCCESS = controller_interface::CallbackReturn::SUCCESS; @@ -126,6 +132,9 @@ class TestableSteeringControllersLibrary controller_interface::CallbackReturn configure_odometry() { set_interface_numbers(NR_STATE_ITFS, NR_CMD_ITFS, NR_REF_ITFS); + odometry_.set_wheel_params(FRONT_WHEELS_RADIUS_, WHEELBASE_, REAR_WHEEL_TRACK_); + odometry_.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); + return controller_interface::CallbackReturn::SUCCESS; } @@ -313,10 +322,7 @@ class SteeringControllersLibraryFixture : public ::testing::Test std::array joint_state_values_ = {0.5, 0.5, 0.0, 0.0}; std::array joint_command_values_ = {1.1, 3.3, 2.2, 4.4}; - static constexpr double TEST_LINEAR_VELOCITY_X = 1.5; - static constexpr double TEST_LINEAR_VELOCITY_y = 0.0; - static constexpr double TEST_ANGULAR_VELOCITY_Z = 0.0; - double command_lin_x = 111; + std::array joint_reference_interfaces_ = {"linear/velocity", "angular/position"}; std::string steering_interface_name_ = "position"; // defined in setup From 797a0ceca588637fcb8687f7fafa4de5286b84a2 Mon Sep 17 00:00:00 2001 From: petkovich Date: Thu, 26 Jan 2023 18:44:08 +0100 Subject: [PATCH 161/186] cleaned todo --- steering_controllers_library/src/steering_odometry.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp index 287e4f21e1..2f168c70e2 100644 --- a/steering_controllers_library/src/steering_odometry.cpp +++ b/steering_controllers_library/src/steering_odometry.cpp @@ -72,7 +72,6 @@ bool SteeringOdometry::update_odometry( return true; } -// TODO(petkovich): enable also velocity interface to update using velocity from the traction wheel bool SteeringOdometry::update_from_position( const double traction_wheel_pos, const double steer_pos, const double dt) { From 38650d40e852f50915ea099d78927a90f3c09cd9 Mon Sep 17 00:00:00 2001 From: petkovich Date: Mon, 30 Jan 2023 12:14:52 +0100 Subject: [PATCH 162/186] fixed compile error, exported parameters to include --- steering_controllers_library/CMakeLists.txt | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/steering_controllers_library/CMakeLists.txt b/steering_controllers_library/CMakeLists.txt index 9f919f0c87..fd001cd0f3 100644 --- a/steering_controllers_library/CMakeLists.txt +++ b/steering_controllers_library/CMakeLists.txt @@ -1,5 +1,5 @@ -cmake_minimum_required(VERSION 3.8) -project(steering_controllers_library) +cmake_minimum_required(VERSION 3.16) +project(steering_controllers_library LANGUAGES CXX) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) @@ -59,7 +59,6 @@ install( install( DIRECTORY include/ - #DESTINATION include/${PROJECT_NAME} # TODO: it should be this, but then it doesn't compile (?) DESTINATION include ) @@ -83,6 +82,7 @@ endif() ament_export_include_directories( include + include/steering_controllers_library_parameters ) ament_export_dependencies( From 8d46a9a8a09e65e9bcd8606f4c2ab3fdff8b2c65 Mon Sep 17 00:00:00 2001 From: petkovich Date: Mon, 30 Jan 2023 12:28:52 +0100 Subject: [PATCH 163/186] DESTINATION include/ --- steering_controllers_library/CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/steering_controllers_library/CMakeLists.txt b/steering_controllers_library/CMakeLists.txt index fd001cd0f3..30cfecbecf 100644 --- a/steering_controllers_library/CMakeLists.txt +++ b/steering_controllers_library/CMakeLists.txt @@ -59,7 +59,7 @@ install( install( DIRECTORY include/ - DESTINATION include + DESTINATION include/${PROJECT_NAME} ) if(BUILD_TESTING) @@ -81,7 +81,7 @@ if(BUILD_TESTING) endif() ament_export_include_directories( - include + include/${PROJECT_NAME} include/steering_controllers_library_parameters ) From 7cd66832721a6ebb2cdc25f7a4ace751fac2c226 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Wed, 8 Feb 2023 16:56:59 +0100 Subject: [PATCH 164/186] Apply suggestions from code review --- .../src/ackermann_steering_controller.yaml | 6 +++--- steering_controllers_library/doc/userdoc.rst | 2 ++ .../tricycle_steering_controller/visibility_control.h | 1 - .../src/tricycle_steering_controller.yaml | 4 ++-- .../test/test_load_tricycle_steering_controller.cpp | 1 - .../test/test_tricycle_steering_controller.cpp | 1 - .../test/test_tricycle_steering_controller_preceeding.cpp | 1 - 7 files changed, 7 insertions(+), 9 deletions(-) diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.yaml b/ackermann_steering_controller/src/ackermann_steering_controller.yaml index 90ca93725e..3726146919 100644 --- a/ackermann_steering_controller/src/ackermann_steering_controller.yaml +++ b/ackermann_steering_controller/src/ackermann_steering_controller.yaml @@ -3,7 +3,7 @@ ackermann_steering_controller: { type: double, default_value: 0.0, - description: "Front wheel track length.", + description: "Front wheel track length. For details see: https://en.wikipedia.org/wiki/Wheelbase", read_only: false, } @@ -11,7 +11,7 @@ ackermann_steering_controller: { type: double, default_value: 0.0, - description: "Rear wheel track length.", + description: "Rear wheel track length. For details see: https://en.wikipedia.org/wiki/Wheelbase", read_only: false, } @@ -19,7 +19,7 @@ ackermann_steering_controller: { type: double, default_value: 0.0, - description: "Distance between front and rear wheels.", + description: "Distance between front and rear wheels. For details see: https://en.wikipedia.org/wiki/Wheelbase", read_only: false, } diff --git a/steering_controllers_library/doc/userdoc.rst b/steering_controllers_library/doc/userdoc.rst index 38d6cdbd36..7ca499ba47 100644 --- a/steering_controllers_library/doc/userdoc.rst +++ b/steering_controllers_library/doc/userdoc.rst @@ -6,6 +6,8 @@ steering_controllers Controllers for mobile robots with steering drive. Input for control are robot base_link twist stamped commands which are translated to traction and steering commands for the drive base. Odometry is computed from hardware feedback and published. +Nomenclature used for the controller is used from `wikipedia `_. + Velocity commands ----------------- diff --git a/tricycle_steering_controller/include/tricycle_steering_controller/visibility_control.h b/tricycle_steering_controller/include/tricycle_steering_controller/visibility_control.h index 33fdfbbe38..606b067ad8 100644 --- a/tricycle_steering_controller/include/tricycle_steering_controller/visibility_control.h +++ b/tricycle_steering_controller/include/tricycle_steering_controller/visibility_control.h @@ -1,5 +1,4 @@ // Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) -// Copyright (c) 2023, 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. diff --git a/tricycle_steering_controller/src/tricycle_steering_controller.yaml b/tricycle_steering_controller/src/tricycle_steering_controller.yaml index a5975ee6b8..1015865fd9 100644 --- a/tricycle_steering_controller/src/tricycle_steering_controller.yaml +++ b/tricycle_steering_controller/src/tricycle_steering_controller.yaml @@ -3,7 +3,7 @@ tricycle_steering_controller: { type: double, default_value: 0.0, - description: "Wheel track length.", + description: "Wheel track length. For details see: https://en.wikipedia.org/wiki/Wheelbase", read_only: false, } @@ -11,7 +11,7 @@ tricycle_steering_controller: { type: double, default_value: 0.0, - description: "Distance between front and rear wheels.", + description: "Distance between front and rear wheels. For details see: https://en.wikipedia.org/wiki/Wheelbase", read_only: false, } diff --git a/tricycle_steering_controller/test/test_load_tricycle_steering_controller.cpp b/tricycle_steering_controller/test/test_load_tricycle_steering_controller.cpp index d56a809f10..8421f4d3a0 100644 --- a/tricycle_steering_controller/test/test_load_tricycle_steering_controller.cpp +++ b/tricycle_steering_controller/test/test_load_tricycle_steering_controller.cpp @@ -1,5 +1,4 @@ // Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) -// Copyright (c) 2023, 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. diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp index 02a1bf02b9..1d2fa632b7 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp @@ -1,5 +1,4 @@ // Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) -// Copyright (c) 2023, 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. diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller_preceeding.cpp b/tricycle_steering_controller/test/test_tricycle_steering_controller_preceeding.cpp index 55137c577d..dd72332875 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller_preceeding.cpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller_preceeding.cpp @@ -1,5 +1,4 @@ // Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) -// Copyright (c) 2023, 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. From aaf5187d58d08c5662d4aca287fb8a3d923d96f1 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Wed, 8 Feb 2023 16:59:00 +0100 Subject: [PATCH 165/186] Update ackermann_steering_controller/test/test_ackermann_steering_controller.hpp --- .../test/test_ackermann_steering_controller.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp index d433d0b4c7..01e6eec9fa 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp @@ -56,7 +56,6 @@ constexpr auto NODE_SUCCESS = controller_interface::CallbackReturn::SUCCESS; constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR; const double COMMON_THRESHOLD = 1e-6; } // namespace -// namespace // subclassing and friending so we can access member variables class TestableAckermannSteeringController From 2f80658ed6b97b509ef96e80f591493f8c67b106 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Wed, 8 Feb 2023 16:59:14 +0100 Subject: [PATCH 166/186] Update bicycle_steering_controller/src/bicycle_steering_controller.yaml --- .../src/bicycle_steering_controller.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/bicycle_steering_controller/src/bicycle_steering_controller.yaml b/bicycle_steering_controller/src/bicycle_steering_controller.yaml index 45b4355a3d..c40e27ef96 100644 --- a/bicycle_steering_controller/src/bicycle_steering_controller.yaml +++ b/bicycle_steering_controller/src/bicycle_steering_controller.yaml @@ -3,7 +3,7 @@ bicycle_steering_controller: { type: double, default_value: 0.0, - description: "Distance between front and rear wheel.", + description: "Distance between front and rear wheel. For details see: https://en.wikipedia.org/wiki/Wheelbase", read_only: false, } From 8f527f1c7ac1bc861d0eb1054b37ac2331ef028e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Wed, 8 Feb 2023 21:36:03 +0100 Subject: [PATCH 167/186] Last updates of CMake files copyright year and docs. --- ackermann_steering_controller/CMakeLists.txt | 52 +++++----- ackermann_steering_controller/doc/userdoc.rst | 18 ++++ .../ackermann_steering_controller.hpp | 2 +- .../src/ackermann_steering_controller.cpp | 2 +- bicycle_steering_controller/CMakeLists.txt | 53 +++++------ bicycle_steering_controller/doc/userdoc.rst | 19 ++++ .../bicycle_steering_controller.hpp | 2 +- .../src/bicycle_steering_controller.cpp | 2 +- doc/controllers_index.rst | 10 ++ steering_controllers_library/CMakeLists.txt | 61 ++++++------ steering_controllers_library/doc/userdoc.rst | 94 ++++++++++++++++--- .../steering_controllers_library.hpp | 2 +- .../steering_odometry.hpp | 2 +- .../visibility_control.h | 2 +- .../src/steering_controllers_library.cpp | 2 +- .../steering_controllers_library.yaml | 0 .../src/steering_odometry.cpp | 2 +- .../test_steering_controllers_library.cpp | 2 +- tricycle_steering_controller/CMakeLists.txt | 63 +++++-------- tricycle_steering_controller/doc/userdocs.rst | 18 ++++ .../tricycle_steering_controller.hpp | 2 +- 21 files changed, 258 insertions(+), 152 deletions(-) create mode 100644 ackermann_steering_controller/doc/userdoc.rst create mode 100644 bicycle_steering_controller/doc/userdoc.rst rename steering_controllers_library/{config => src}/steering_controllers_library.yaml (100%) create mode 100644 tricycle_steering_controller/doc/userdocs.rst diff --git a/ackermann_steering_controller/CMakeLists.txt b/ackermann_steering_controller/CMakeLists.txt index 15e56a0ad0..d7d8d04269 100644 --- a/ackermann_steering_controller/CMakeLists.txt +++ b/ackermann_steering_controller/CMakeLists.txt @@ -1,7 +1,7 @@ -cmake_minimum_required(VERSION 3.8) -project(ackermann_steering_controller) +cmake_minimum_required(VERSION 3.16) +project(ackermann_steering_controller LANGUAGES CXX) -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Wpedantic) endif() @@ -9,6 +9,7 @@ endif() set(THIS_PACKAGE_INCLUDE_DEPENDS controller_interface hardware_interface + generate_parameter_library pluginlib rclcpp rclcpp_lifecycle @@ -18,43 +19,35 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS ) find_package(ament_cmake REQUIRED) -find_package(generate_parameter_library REQUIRED) +find_package(backward_ros REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() -# Add ackermann_steering_controller library related compile commands generate_parameter_library(ackermann_steering_controller_parameters src/ackermann_steering_controller.yaml ) + add_library( ackermann_steering_controller SHARED src/ackermann_steering_controller.cpp ) +target_compile_features(ackermann_steering_controller PUBLIC cxx_std_17) target_include_directories(ackermann_steering_controller PUBLIC "$" "$") -target_link_libraries(ackermann_steering_controller ackermann_steering_controller_parameters) -ament_target_dependencies(ackermann_steering_controller ${THIS_PACKAGE_INCLUDE_DEPENDS}) +target_link_libraries(ackermann_steering_controller PUBLIC + ackermann_steering_controller_parameters) +ament_target_dependencies(ackermann_steering_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) + +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. target_compile_definitions(ackermann_steering_controller PRIVATE "ACKERMANN_STEERING_CONTROLLER_BUILDING_DLL") pluginlib_export_plugin_description_file( controller_interface ackermann_steering_controller.xml) -install( - TARGETS - ackermann_steering_controller - RUNTIME DESTINATION bin - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib -) - -install( - DIRECTORY include/ - DESTINATION include/${PROJECT_NAME} -) - if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) find_package(controller_manager REQUIRED) @@ -93,14 +86,19 @@ if(BUILD_TESTING) ) endif() -ament_export_include_directories( - include -) -ament_export_dependencies( - ${THIS_PACKAGE_INCLUDE_DEPENDS} +install( + DIRECTORY include/ + DESTINATION include/ackermann_steering_controller ) -ament_export_libraries( - ackermann_steering_controller + +install( + TARGETS ackermann_steering_controller ackermann_steering_controller_parameters + EXPORT export_ackermann_steering_controller + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib ) +ament_export_targets(export_ackermann_steering_controller HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) ament_package() diff --git a/ackermann_steering_controller/doc/userdoc.rst b/ackermann_steering_controller/doc/userdoc.rst new file mode 100644 index 0000000000..12d1abc84d --- /dev/null +++ b/ackermann_steering_controller/doc/userdoc.rst @@ -0,0 +1,18 @@ +.. _ackermann_steering_controller_userdoc: + +ackermann_steering_controller +============================= + +This controller implements the kinamtics with two axes and four wheels, where wheels on one axis are fixed (traction/drive) wheels, and wheels on another axis are steerable. + +The controller expects to have two commanding joints for traction, one for each fixed wheel and two commanding joints for steering, one for each wheel. + +For more details on controller's execution and interfaces check the :ref:`Steering Controller Library `. + + +Parameters +,,,,,,,,,,, + +For list of parameters and their meaning YAML file in the ``src`` folder of the controller's package. + +For an exameplary parameterization see the ``test`` folder of the controller's package. diff --git a/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp b/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp index 61dc5d2ba7..0cb6bcd016 100644 --- a/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp +++ b/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.cpp b/ackermann_steering_controller/src/ackermann_steering_controller.cpp index 12ae731836..c3a7539c5a 100644 --- a/ackermann_steering_controller/src/ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/src/ackermann_steering_controller.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/bicycle_steering_controller/CMakeLists.txt b/bicycle_steering_controller/CMakeLists.txt index 81987bec30..857e7c9dbf 100644 --- a/bicycle_steering_controller/CMakeLists.txt +++ b/bicycle_steering_controller/CMakeLists.txt @@ -1,7 +1,7 @@ -cmake_minimum_required(VERSION 3.8) -project(bicycle_steering_controller) +cmake_minimum_required(VERSION 3.16) +project(bicycle_steering_controller LANGUAGES CXX) -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Wpedantic) endif() @@ -9,6 +9,7 @@ endif() set(THIS_PACKAGE_INCLUDE_DEPENDS controller_interface hardware_interface + generate_parameter_library pluginlib rclcpp rclcpp_lifecycle @@ -18,12 +19,11 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS ) find_package(ament_cmake REQUIRED) -find_package(generate_parameter_library REQUIRED) +find_package(backward_ros REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() -# Add bicycle_steering_controller library related compile commands generate_parameter_library(bicycle_steering_controller_parameters src/bicycle_steering_controller.yaml ) @@ -33,29 +33,21 @@ add_library( SHARED src/bicycle_steering_controller.cpp ) +target_compile_features(bicycle_steering_controller PUBLIC cxx_std_17) target_include_directories(bicycle_steering_controller PUBLIC "$" "$") -target_link_libraries(bicycle_steering_controller bicycle_steering_controller_parameters) -ament_target_dependencies(bicycle_steering_controller ${THIS_PACKAGE_INCLUDE_DEPENDS}) -target_compile_definitions(bicycle_steering_controller PRIVATE "BICYCLE_STEERING_CONTROLLER_BUILDING_DLL") +target_link_libraries(bicycle_steering_controller PUBLIC + bicycle_steering_controller_parameters) +ament_target_dependencies(bicycle_steering_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) + +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(bicycle_steering_controller PRIVATE "ACKERMANN_STEERING_CONTROLLER_BUILDING_DLL") pluginlib_export_plugin_description_file( controller_interface bicycle_steering_controller.xml) -install( - TARGETS - bicycle_steering_controller - RUNTIME DESTINATION bin - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib -) - -install( - DIRECTORY include/ - DESTINATION include/${PROJECT_NAME} -) - if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) find_package(controller_manager REQUIRED) @@ -94,14 +86,19 @@ if(BUILD_TESTING) ) endif() -ament_export_include_directories( - include -) -ament_export_dependencies( - ${THIS_PACKAGE_INCLUDE_DEPENDS} +install( + DIRECTORY include/ + DESTINATION include/bicycle_steering_controller ) -ament_export_libraries( - bicycle_steering_controller + +install( + TARGETS bicycle_steering_controller bicycle_steering_controller_parameters + EXPORT export_bicycle_steering_controller + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib ) +ament_export_targets(export_bicycle_steering_controller HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) ament_package() diff --git a/bicycle_steering_controller/doc/userdoc.rst b/bicycle_steering_controller/doc/userdoc.rst new file mode 100644 index 0000000000..52c95d6c4e --- /dev/null +++ b/bicycle_steering_controller/doc/userdoc.rst @@ -0,0 +1,19 @@ +.. _bicycle_steering_controller_userdoc: + +bicycle_steering_controller +============================= + +This controller implements the kinamtics with two axes and two wheels, where wheel on an axis is fixed (traction/drive) wheel, and wheel on another axis is steerable. + +The controller expects to have one commanding joint for traction, and one commanding joint for steering. +If your Ackermann steering vehicle uses differentials on axes, then you should probably use this controller since you can command only one traction velocity and steering angle for virtual wheels in the middle of the axes. + +For more details on controller's execution and interfaces check the :ref:`Steering Controller Library `. + + +Parameters +,,,,,,,,,,, + +For list of parameters and their meaning YAML file in the ``src`` folder of the controller's package. + +For an exameplary parameterization see the ``test`` folder of the controller's package. diff --git a/bicycle_steering_controller/include/bicycle_steering_controller/bicycle_steering_controller.hpp b/bicycle_steering_controller/include/bicycle_steering_controller/bicycle_steering_controller.hpp index 1bb80e4174..1b3e050a37 100644 --- a/bicycle_steering_controller/include/bicycle_steering_controller/bicycle_steering_controller.hpp +++ b/bicycle_steering_controller/include/bicycle_steering_controller/bicycle_steering_controller.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/bicycle_steering_controller/src/bicycle_steering_controller.cpp b/bicycle_steering_controller/src/bicycle_steering_controller.cpp index 9d35f3a52e..5f013d7d7a 100644 --- a/bicycle_steering_controller/src/bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/src/bicycle_steering_controller.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/doc/controllers_index.rst b/doc/controllers_index.rst index dee9d31abe..b73e2f2d2b 100644 --- a/doc/controllers_index.rst +++ b/doc/controllers_index.rst @@ -48,6 +48,7 @@ Available Controllers Effort Controllers <../effort_controllers/doc/userdoc.rst> + Available Broadcasters ********************** @@ -56,3 +57,12 @@ Available Broadcasters Joint State Broadcaster <../joint_state_broadcaster/doc/userdoc.rst> Imu Sensor Broadcaster <../imu_sensor_broadcaster/doc/userdoc.rst> + + +Available Controller Libraries +****************************** + +.. toctree:: + :titlesonly: + + Steering Controllers Library <../steering_controllers_library/doc/userdoc.rst> diff --git a/steering_controllers_library/CMakeLists.txt b/steering_controllers_library/CMakeLists.txt index 30cfecbecf..5fdd727188 100644 --- a/steering_controllers_library/CMakeLists.txt +++ b/steering_controllers_library/CMakeLists.txt @@ -1,7 +1,7 @@ cmake_minimum_required(VERSION 3.16) project(steering_controllers_library LANGUAGES CXX) -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Wpedantic) endif() @@ -9,6 +9,7 @@ endif() set(THIS_PACKAGE_INCLUDE_DEPENDS control_msgs controller_interface + generate_parameter_library geometry_msgs hardware_interface nav_msgs @@ -24,43 +25,32 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS ) find_package(ament_cmake REQUIRED) -find_package(generate_parameter_library REQUIRED) - +find_package(backward_ros REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() +generate_parameter_library(steering_controllers_library_parameters + src/steering_controllers_library.yaml +) + add_library( - ${PROJECT_NAME} + steering_controllers_library SHARED src/steering_controllers_library.cpp src/steering_odometry.cpp ) - -generate_parameter_library(steering_controllers_library_parameters - config/steering_controllers_library.yaml -) - -target_include_directories(${PROJECT_NAME} PUBLIC +target_compile_features(steering_controllers_library PUBLIC cxx_std_17) +target_include_directories(steering_controllers_library PUBLIC "$" - "$") -target_link_libraries(${PROJECT_NAME} + "$") +target_link_libraries(steering_controllers_library PUBLIC steering_controllers_library_parameters) -ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS}) -target_compile_definitions(${PROJECT_NAME} PRIVATE "STEERING_CONTROLLERS_BUILDING_DLL") - -install( - TARGETS - ${PROJECT_NAME} - RUNTIME DESTINATION bin - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib -) +ament_target_dependencies(steering_controllers_library PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) -install( - DIRECTORY include/ - DESTINATION include/${PROJECT_NAME} -) +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(steering_controllers_library PRIVATE "STEERING_CONTROLLERS_BUILDING_DLL") if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) @@ -80,16 +70,19 @@ if(BUILD_TESTING) ) endif() -ament_export_include_directories( - include/${PROJECT_NAME} - include/steering_controllers_library_parameters +install( + DIRECTORY include/ + DESTINATION include/steering_controllers_library ) -ament_export_dependencies( - ${THIS_PACKAGE_INCLUDE_DEPENDS} -) -ament_export_libraries( - ${PROJECT_NAME} +install( + TARGETS steering_controllers_library steering_controllers_library_parameters + EXPORT export_steering_controllers_library + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib ) +ament_export_targets(export_steering_controllers_library HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) ament_package() diff --git a/steering_controllers_library/doc/userdoc.rst b/steering_controllers_library/doc/userdoc.rst index 7ca499ba47..39c39e8a28 100644 --- a/steering_controllers_library/doc/userdoc.rst +++ b/steering_controllers_library/doc/userdoc.rst @@ -1,24 +1,88 @@ -.. _steering_controllers_userdoc: +.. _steering_controllers_libaray_userdoc: -steering_controllers -===================== +steering_controllers_library +============================= -Controllers for mobile robots with steering drive. -Input for control are robot base_link twist stamped commands which are translated to traction and steering commands for the drive base. Odometry is computed from hardware feedback and published. +Library with shared functionalities for mobile robots controllers with steering drive (2 degrees of freedom). +The library implements generic odometry and update methods and defines the main interfaces. Nomenclature used for the controller is used from `wikipedia `_. -Velocity commands ------------------ +Execution logic of the controller +---------------------------------- -The controller works with a velocity twist from which it extracts -the x component of the linear velocity and the z component of the angular velocity. -Velocities on other components are ignored. +The controller uses velocity input, i.e., stamped or unstamped Twist messages where linear ``x`` and angular ``z`` components are used. +Angular component is under +Values in other components are ignored. +In the chain mode the controller provides two reference interfaces, one for linear velocity and one for steering angle position. +Other relevant features are: + - support for front and rear steering configurations; + - odometry publishing as Odometry and TF message; + - input command timeout based on a parameter. -Other features --------------- +The command for the wheels are calculated using ``odometry`` library where based on concrete kinematics traction and steering commands are calculated. +Currently implemented kinematics in corresponding packages are: - Realtime-safe implementation. - Odometry publishing - Automatic stop after command timeout + - :ref:`Bicycle ` - with one steering and one drive joints; + - :ref:`Tricylce ` - with one steering and two drive joints; + - :ref:`Ackermann ` - with two seering and two drive joints. + + + +Description of controller's interfaces +-------------------------------------- + +References (from a preceding controller) +,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,, +- /linear/velocity [double] # in [m/s] +- /angular/position [double] # in [rad] + +Commands +,,,,,,,,, +``front_steering == true`` + +- /position [double] # in [rad] +- /velocity [double] # in [m/s] + +``front_steering == false`` + +- /velocity [double] # in [m/s] +- /position [double] # in [rad] + +States +,,,,,,, +``position_feedback == true`` --> ``TRACTION_FEEDBACK_TYPE = position`` +``position_feedback == false`` --> ``TRACTION_FEEDBACK_TYPE = velocity`` + +``front_steering == true`` + +- /position [double] # in [rad] +- / [double] # in [m] or [m/s] + +``front_steering == false`` + +- / [double] # [m] or [m/s] +- /position [double] # in [rad] + +Subscribers +,,,,,,,,,,,, +Used when controller is not in chained mode (``in_chained_mode == false``). + +- /reference [geometry_msgs/msg/TwistStamped] + **NOTE**: Parameter ``use_stamped_vel`` is ``true``. +- /reference_unstamped [geometry_msgs/msg/Twist] + **NOTE**: Parameter ``use_stamped_vel`` is ``true``. + +Publishers +,,,,,,,,,,, +- /odometry [nav_msgs/msg/Odometry] +- /tf_odometry [tf2_msgs/msg/TFMessage] +- /controller_state [control_msgs/msg/SteeringControllerStatus] + +Parameters +,,,,,,,,,,, + +For list of parameters and their meaning YAML file in the ``src`` folder of the controller's package. + +For an exameplary parameterization see the ``test`` folder of the controller's package. diff --git a/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp b/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp index 2d86e8ce6f..095db7674b 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp index 6cc8ffd4c2..0787810130 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/steering_controllers_library/include/steering_controllers_library/visibility_control.h b/steering_controllers_library/include/steering_controllers_library/visibility_control.h index 087cae1f90..123662031b 100644 --- a/steering_controllers_library/include/steering_controllers_library/visibility_control.h +++ b/steering_controllers_library/include/steering_controllers_library/visibility_control.h @@ -1,4 +1,4 @@ -// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index 7171f5b6c3..ccf924d527 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/steering_controllers_library/config/steering_controllers_library.yaml b/steering_controllers_library/src/steering_controllers_library.yaml similarity index 100% rename from steering_controllers_library/config/steering_controllers_library.yaml rename to steering_controllers_library/src/steering_controllers_library.yaml diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp index 2f168c70e2..509850a8e4 100644 --- a/steering_controllers_library/src/steering_odometry.cpp +++ b/steering_controllers_library/src/steering_odometry.cpp @@ -1,5 +1,5 @@ /********************************************************************* -* Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) +* Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. diff --git a/steering_controllers_library/test/test_steering_controllers_library.cpp b/steering_controllers_library/test/test_steering_controllers_library.cpp index 1b3678eb1a..34cb108c4f 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.cpp +++ b/steering_controllers_library/test/test_steering_controllers_library.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/tricycle_steering_controller/CMakeLists.txt b/tricycle_steering_controller/CMakeLists.txt index 9379e62d92..67aab9bccd 100644 --- a/tricycle_steering_controller/CMakeLists.txt +++ b/tricycle_steering_controller/CMakeLists.txt @@ -1,15 +1,15 @@ -cmake_minimum_required(VERSION 3.8) -project(tricycle_steering_controller) +cmake_minimum_required(VERSION 3.16) +project(tricycle_steering_controller LANGUAGES CXX) -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Wpedantic) endif() # find dependencies set(THIS_PACKAGE_INCLUDE_DEPENDS - control_msgs controller_interface hardware_interface + generate_parameter_library pluginlib rclcpp rclcpp_lifecycle @@ -19,52 +19,36 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS ) find_package(ament_cmake REQUIRED) -find_package(generate_parameter_library REQUIRED) +find_package(backward_ros REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() -# Add tricycle_steering_controller library related compile commands generate_parameter_library(tricycle_steering_controller_parameters src/tricycle_steering_controller.yaml ) + add_library( tricycle_steering_controller SHARED src/tricycle_steering_controller.cpp ) +target_compile_features(tricycle_steering_controller PUBLIC cxx_std_17) target_include_directories(tricycle_steering_controller PUBLIC "$" "$") -target_link_libraries(tricycle_steering_controller tricycle_steering_controller_parameters) -ament_target_dependencies(tricycle_steering_controller ${THIS_PACKAGE_INCLUDE_DEPENDS}) -target_compile_definitions(tricycle_steering_controller PRIVATE "TRICYCLE_STEERING_CONTROLLER_BUILDING_DLL") +target_link_libraries(tricycle_steering_controller PUBLIC + tricycle_steering_controller_parameters) +ament_target_dependencies(tricycle_steering_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) + +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(tricycle_steering_controller PRIVATE "ACKERMANN_STEERING_CONTROLLER_BUILDING_DLL") pluginlib_export_plugin_description_file( controller_interface tricycle_steering_controller.xml) -install( - TARGETS - tricycle_steering_controller - RUNTIME DESTINATION bin - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib -) - -install( - DIRECTORY include/ - DESTINATION include/${PROJECT_NAME} -) - if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # comment the line when a copyright and license is added to all source files - set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # comment the line when this package is in a git repo and when - # a copyright and license is added to all source files - set(ament_cmake_cpplint_FOUND TRUE) find_package(ament_cmake_gmock REQUIRED) find_package(controller_manager REQUIRED) find_package(hardware_interface REQUIRED) @@ -102,14 +86,19 @@ if(BUILD_TESTING) ) endif() -ament_export_include_directories( - include -) -ament_export_dependencies( - ${THIS_PACKAGE_INCLUDE_DEPENDS} +install( + DIRECTORY include/ + DESTINATION include/tricycle_steering_controller ) -ament_export_libraries( - tricycle_steering_controller + +install( + TARGETS tricycle_steering_controller tricycle_steering_controller_parameters + EXPORT export_tricycle_steering_controller + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib ) +ament_export_targets(export_tricycle_steering_controller HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) ament_package() diff --git a/tricycle_steering_controller/doc/userdocs.rst b/tricycle_steering_controller/doc/userdocs.rst new file mode 100644 index 0000000000..5ae48407eb --- /dev/null +++ b/tricycle_steering_controller/doc/userdocs.rst @@ -0,0 +1,18 @@ +.. _tricycle_steering_controller_userdoc: + +tricycle_steering_controller +============================= + +This controller implements the kinamtics with two axes and three wheels, where two wheels on an axis are fixed (traction/drive) wheels, and wheel on another axis is steerable. + +The controller expects to have two commanding joints for traction, one for each fixed wheel and one commanding joint for steering. + +For more details on controller's execution and interfaces check the :ref:`Steering Controller Library `. + + +Parameters +,,,,,,,,,,, + +For list of parameters and their meaning YAML file in the ``src`` folder of the controller's package. + +For an exameplary parameterization see the ``test`` folder of the controller's package. diff --git a/tricycle_steering_controller/include/tricycle_steering_controller/tricycle_steering_controller.hpp b/tricycle_steering_controller/include/tricycle_steering_controller/tricycle_steering_controller.hpp index f80debb8a1..607a684df5 100644 --- a/tricycle_steering_controller/include/tricycle_steering_controller/tricycle_steering_controller.hpp +++ b/tricycle_steering_controller/include/tricycle_steering_controller/tricycle_steering_controller.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. From 6021447337f1c98bab5ceed11f4e07d79c0607f1 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Wed, 8 Feb 2023 21:52:15 +0100 Subject: [PATCH 168/186] Update controllers_index.rst --- doc/controllers_index.rst | 3 +++ 1 file changed, 3 insertions(+) diff --git a/doc/controllers_index.rst b/doc/controllers_index.rst index b73e2f2d2b..858f286813 100644 --- a/doc/controllers_index.rst +++ b/doc/controllers_index.rst @@ -40,10 +40,13 @@ Available Controllers .. toctree:: :titlesonly: + Ackermann Steering Controller <../ackermann_steering_controller/doc/userdoc.rst> + Bicycle Steering Controller <../bicycle_steering_controller/doc/userdoc.rst> Differential Drive <../diff_drive_controller/doc/userdoc.rst> Forward Command <../forward_command_controller/doc/userdoc.rst> Joint Trajectory <../joint_trajectory_controller/doc/userdoc.rst> Position Controllers <../position_controllers/doc/userdoc.rst> + Tricycle Steering Controller <../tricycle_steering_controller/doc/userdoc.rst> Velocity Controllers <../velocity_controllers/doc/userdoc.rst> Effort Controllers <../effort_controllers/doc/userdoc.rst> From c1d7515f66ae30f4f1530f5388329d66c236b8ba Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Tue, 21 Feb 2023 18:05:56 +0000 Subject: [PATCH 169/186] fix typos MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Christoph Fröhlich --- tricycle_steering_controller/doc/userdocs.rst | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/tricycle_steering_controller/doc/userdocs.rst b/tricycle_steering_controller/doc/userdocs.rst index 5ae48407eb..9a6adfca37 100644 --- a/tricycle_steering_controller/doc/userdocs.rst +++ b/tricycle_steering_controller/doc/userdocs.rst @@ -3,11 +3,11 @@ tricycle_steering_controller ============================= -This controller implements the kinamtics with two axes and three wheels, where two wheels on an axis are fixed (traction/drive) wheels, and wheel on another axis is steerable. +This controller implements the kinematics with two axes and three wheels, where two wheels on an axis are fixed (traction/drive), and the wheel on the other axis is steerable. The controller expects to have two commanding joints for traction, one for each fixed wheel and one commanding joint for steering. -For more details on controller's execution and interfaces check the :ref:`Steering Controller Library `. +For more details on controller's execution and interfaces check the :ref:`Steering Controller Library `. Parameters @@ -15,4 +15,4 @@ Parameters For list of parameters and their meaning YAML file in the ``src`` folder of the controller's package. -For an exameplary parameterization see the ``test`` folder of the controller's package. +For an exemplary parameterization see the ``test`` folder of the controller's package. From 563f8a927d7e5692b64afd0e41bf5b51de5a5449 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Tue, 21 Feb 2023 18:08:54 +0000 Subject: [PATCH 170/186] Fix typos MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Christoph Fröhlich --- ackermann_steering_controller/doc/userdoc.rst | 6 +++--- bicycle_steering_controller/doc/userdoc.rst | 6 +++--- steering_controllers_library/doc/userdoc.rst | 6 +++--- 3 files changed, 9 insertions(+), 9 deletions(-) diff --git a/ackermann_steering_controller/doc/userdoc.rst b/ackermann_steering_controller/doc/userdoc.rst index 12d1abc84d..59cdb78108 100644 --- a/ackermann_steering_controller/doc/userdoc.rst +++ b/ackermann_steering_controller/doc/userdoc.rst @@ -3,11 +3,11 @@ ackermann_steering_controller ============================= -This controller implements the kinamtics with two axes and four wheels, where wheels on one axis are fixed (traction/drive) wheels, and wheels on another axis are steerable. +This controller implements the kinematics with two axes and four wheels, where the wheels on one axis are fixed (traction/drive) wheels, and the wheels on the other axis are steerable. The controller expects to have two commanding joints for traction, one for each fixed wheel and two commanding joints for steering, one for each wheel. -For more details on controller's execution and interfaces check the :ref:`Steering Controller Library `. +For more details on controller's execution and interfaces check the :ref:`Steering Controller Library `. Parameters @@ -15,4 +15,4 @@ Parameters For list of parameters and their meaning YAML file in the ``src`` folder of the controller's package. -For an exameplary parameterization see the ``test`` folder of the controller's package. +For an exemplary parameterization see the ``test`` folder of the controller's package. diff --git a/bicycle_steering_controller/doc/userdoc.rst b/bicycle_steering_controller/doc/userdoc.rst index 52c95d6c4e..6815dc6953 100644 --- a/bicycle_steering_controller/doc/userdoc.rst +++ b/bicycle_steering_controller/doc/userdoc.rst @@ -3,12 +3,12 @@ bicycle_steering_controller ============================= -This controller implements the kinamtics with two axes and two wheels, where wheel on an axis is fixed (traction/drive) wheel, and wheel on another axis is steerable. +This controller implements the kinematics with two axes and two wheels, where the wheel on one axis is fixed (traction/drive), and the wheel on the other axis is steerable. The controller expects to have one commanding joint for traction, and one commanding joint for steering. If your Ackermann steering vehicle uses differentials on axes, then you should probably use this controller since you can command only one traction velocity and steering angle for virtual wheels in the middle of the axes. -For more details on controller's execution and interfaces check the :ref:`Steering Controller Library `. +For more details on controller's execution and interfaces check the :ref:`Steering Controller Library `. Parameters @@ -16,4 +16,4 @@ Parameters For list of parameters and their meaning YAML file in the ``src`` folder of the controller's package. -For an exameplary parameterization see the ``test`` folder of the controller's package. +For an exemplary parameterization see the ``test`` folder of the controller's package. diff --git a/steering_controllers_library/doc/userdoc.rst b/steering_controllers_library/doc/userdoc.rst index 39c39e8a28..f02c2db4ef 100644 --- a/steering_controllers_library/doc/userdoc.rst +++ b/steering_controllers_library/doc/userdoc.rst @@ -12,7 +12,7 @@ Execution logic of the controller ---------------------------------- The controller uses velocity input, i.e., stamped or unstamped Twist messages where linear ``x`` and angular ``z`` components are used. -Angular component is under +Angular component under Values in other components are ignored. In the chain mode the controller provides two reference interfaces, one for linear velocity and one for steering angle position. Other relevant features are: @@ -35,7 +35,7 @@ Description of controller's interfaces References (from a preceding controller) ,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,, -- /linear/velocity [double] # in [m/s] +- /linear/velocity [double], in m/s - /angular/position [double] # in [rad] Commands @@ -85,4 +85,4 @@ Parameters For list of parameters and their meaning YAML file in the ``src`` folder of the controller's package. -For an exameplary parameterization see the ``test`` folder of the controller's package. +For an exemplary parameterization see the ``test`` folder of the controller's package. From 9b9cadb09eef061ac366679b93b9b6c01ade1560 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Wed, 22 Mar 2023 19:57:17 +0100 Subject: [PATCH 171/186] Update steering_controllers_library/src/steering_odometry.cpp MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Christoph Fröhlich --- steering_controllers_library/src/steering_odometry.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp index 509850a8e4..ee1c0b1286 100644 --- a/steering_controllers_library/src/steering_odometry.cpp +++ b/steering_controllers_library/src/steering_odometry.cpp @@ -224,7 +224,7 @@ double SteeringOdometry::convert_trans_rot_vel_to_steering_angle(double Vx, doub std::tuple, std::vector> SteeringOdometry::get_commands( double Vx, double theta_dot) { - // desired velocty and steering angle of the midle of traction and steering axis + // desired velocity and steering angle of the middle of traction and steering axis double Ws, alpha; if (Vx == 0 && theta_dot != 0) From c0144cf1d389bb649451eb09c3bdf6a7408b16f6 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Tue, 4 Apr 2023 18:48:02 +0100 Subject: [PATCH 172/186] Restore control_msgs repo version --- ros2_controllers.rolling.repos | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ros2_controllers.rolling.repos b/ros2_controllers.rolling.repos index 5f2446d43e..6bf7e7c404 100644 --- a/ros2_controllers.rolling.repos +++ b/ros2_controllers.rolling.repos @@ -9,7 +9,7 @@ repositories: version: master control_msgs: type: git - url: https://github.com/StoglRobotics-forks/control_msgs.git + url: https://github.com/ros-controls/control_msgs.git version: add-steering-controller-status-msgs control_toolbox: type: git From 2b416219e52284456a5bf773f43bcc53e17b1ed5 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Wed, 5 Apr 2023 14:44:14 +0100 Subject: [PATCH 173/186] fix format --- doc/controllers_index.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/doc/controllers_index.rst b/doc/controllers_index.rst index 4171199f42..23d37364ac 100644 --- a/doc/controllers_index.rst +++ b/doc/controllers_index.rst @@ -63,4 +63,4 @@ Available Broadcasters Joint State Broadcaster <../joint_state_broadcaster/doc/userdoc.rst> Imu Sensor Broadcaster <../imu_sensor_broadcaster/doc/userdoc.rst> Force Torque Sensor Broadcaster <../force_torque_sensor_broadcaster/doc/userdoc.rst> - \ No newline at end of file + From 469702cf1375fed30659b14b759e9abfcfc65275 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Wed, 5 Apr 2023 14:46:21 +0100 Subject: [PATCH 174/186] Fix typo in link --- steering_controllers_library/doc/userdoc.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/steering_controllers_library/doc/userdoc.rst b/steering_controllers_library/doc/userdoc.rst index f02c2db4ef..5c5ab5b4a8 100644 --- a/steering_controllers_library/doc/userdoc.rst +++ b/steering_controllers_library/doc/userdoc.rst @@ -1,4 +1,4 @@ -.. _steering_controllers_libaray_userdoc: +.. _steering_controllers_library_userdoc: steering_controllers_library ============================= From 63e4c2804419ae8a81ed7f0c59a7d71160acc6af Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Wed, 5 Apr 2023 14:47:13 +0100 Subject: [PATCH 175/186] Braces are good for you MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Márk Szitanics --- steering_controllers_library/src/steering_odometry.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp index ee1c0b1286..8022d558ef 100644 --- a/steering_controllers_library/src/steering_odometry.cpp +++ b/steering_controllers_library/src/steering_odometry.cpp @@ -320,7 +320,9 @@ void SteeringOdometry::integrate_runge_kutta_2(double linear, double angular) void SteeringOdometry::integrate_exact(double linear, double angular) { if (fabs(angular) < 1e-6) +{ integrate_runge_kutta_2(linear, angular); +} else { /// Exact integration (should solve problems when angular is zero): From 676239e75a8f8be3dd3026cd6223d3bd8cbbbbd3 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Wed, 5 Apr 2023 14:47:43 +0100 Subject: [PATCH 176/186] Fix typo --- steering_controllers_library/doc/userdoc.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/steering_controllers_library/doc/userdoc.rst b/steering_controllers_library/doc/userdoc.rst index 5c5ab5b4a8..ca84beedc8 100644 --- a/steering_controllers_library/doc/userdoc.rst +++ b/steering_controllers_library/doc/userdoc.rst @@ -3,7 +3,7 @@ steering_controllers_library ============================= -Library with shared functionalities for mobile robots controllers with steering drive (2 degrees of freedom). +Library with shared functionalities for mobile robot controllers with steering drive (2 degrees of freedom). The library implements generic odometry and update methods and defines the main interfaces. Nomenclature used for the controller is used from `wikipedia `_. From ce4f36a490f5582216fe27504f474d680dacbaaf Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Wed, 5 Apr 2023 14:48:31 +0100 Subject: [PATCH 177/186] Add units to parameters --- .../steering_controllers_library/steering_odometry.hpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp index 0787810130..9d62ae3efe 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp @@ -239,9 +239,10 @@ class SteeringOdometry double linear_; // [m/s] double angular_; // [rad/s] - double wheel_track_; - double wheelbase_; - double wheel_radius_; + /// Kinematic parameters + double wheel_track_; // [m] + double wheelbase_; // [m] + double wheel_radius_; // [m] /// Configuration type used for thr forward kinematics int config_type_ = -1; From c609707d1b234df1126fe05d61dbd39850f5c1b4 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Wed, 5 Apr 2023 14:49:00 +0100 Subject: [PATCH 178/186] Fix typo --- .../include/steering_controllers_library/steering_odometry.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp index 9d62ae3efe..002db32354 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp @@ -244,7 +244,7 @@ class SteeringOdometry double wheelbase_; // [m] double wheel_radius_; // [m] - /// Configuration type used for thr forward kinematics + /// Configuration type used for the forward kinematics int config_type_ = -1; /// Previous wheel position/state [rad]: From a3083e8e997f427d64d5f4717ff13077a5957859 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Wed, 5 Apr 2023 17:02:50 +0100 Subject: [PATCH 179/186] fix format again?! --- doc/controllers_index.rst | 1 - 1 file changed, 1 deletion(-) diff --git a/doc/controllers_index.rst b/doc/controllers_index.rst index 23d37364ac..3eabb3f160 100644 --- a/doc/controllers_index.rst +++ b/doc/controllers_index.rst @@ -63,4 +63,3 @@ Available Broadcasters Joint State Broadcaster <../joint_state_broadcaster/doc/userdoc.rst> Imu Sensor Broadcaster <../imu_sensor_broadcaster/doc/userdoc.rst> Force Torque Sensor Broadcaster <../force_torque_sensor_broadcaster/doc/userdoc.rst> - From c1431afff28a372f81b2701b65d3ea7873776100 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Wed, 5 Apr 2023 17:03:01 +0100 Subject: [PATCH 180/186] Return update_odometry() instead of return true --- .../src/steering_odometry.cpp | 26 ++++++------------- 1 file changed, 8 insertions(+), 18 deletions(-) diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp index 8022d558ef..28cd7fc80d 100644 --- a/steering_controllers_library/src/steering_odometry.cpp +++ b/steering_controllers_library/src/steering_odometry.cpp @@ -87,9 +87,7 @@ bool SteeringOdometry::update_from_position( steer_pos_ = steer_pos; const double angular = tan(steer_pos) * linear_velocity / wheelbase_; - update_odometry(linear_velocity, angular, dt); - - return true; + return update_odometry(linear_velocity, angular, dt); } bool SteeringOdometry::update_from_position( @@ -113,9 +111,8 @@ bool SteeringOdometry::update_from_position( (traction_right_wheel_est_pos_diff + traction_left_wheel_est_pos_diff) * 0.5 / dt; steer_pos_ = steer_pos; const double angular = tan(steer_pos_) * linear_velocity / wheelbase_; - update_odometry(linear_velocity, angular, dt); - return true; + return update_odometry(linear_velocity, angular, dt); } bool SteeringOdometry::update_from_position( @@ -141,9 +138,7 @@ bool SteeringOdometry::update_from_position( steer_pos_ = (right_steer_pos + left_steer_pos) * 0.5; const double angular = tan(steer_pos_) * linear_velocity / wheelbase_; - update_odometry(linear_velocity, angular, dt); - - return true; + return update_odometry(linear_velocity, angular, dt); } bool SteeringOdometry::update_from_velocity( @@ -153,9 +148,7 @@ bool SteeringOdometry::update_from_velocity( double linear_velocity = traction_wheel_vel * wheel_radius_; const double angular = tan(steer_pos) * linear_velocity / wheelbase_; - update_odometry(linear_velocity, angular, dt); - - return true; + return update_odometry(linear_velocity, angular, dt); } bool SteeringOdometry::update_from_velocity( @@ -167,9 +160,8 @@ bool SteeringOdometry::update_from_velocity( steer_pos_ = steer_pos; const double angular = tan(steer_pos_) * linear_velocity / wheelbase_; - update_odometry(linear_velocity, angular, dt); - return true; + return update_odometry(linear_velocity, angular, dt); } bool SteeringOdometry::update_from_velocity( @@ -181,9 +173,7 @@ bool SteeringOdometry::update_from_velocity( (right_traction_wheel_vel + left_traction_wheel_vel) * wheel_radius_ * 0.5; const double angular = steer_pos_ * linear_velocity / wheelbase_; - update_odometry(linear_velocity, angular, dt); - - return true; + return update_odometry(linear_velocity, angular, dt); } void SteeringOdometry::update_open_loop(const double linear, const double angular, const double dt) @@ -320,9 +310,9 @@ void SteeringOdometry::integrate_runge_kutta_2(double linear, double angular) void SteeringOdometry::integrate_exact(double linear, double angular) { if (fabs(angular) < 1e-6) -{ + { integrate_runge_kutta_2(linear, angular); -} + } else { /// Exact integration (should solve problems when angular is zero): From d78bb6a7c44435291a0b4bc05beb17780213ded3 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Wed, 5 Apr 2023 17:16:24 +0100 Subject: [PATCH 181/186] Update to new signature of update_reference_from_subscribers --- .../steering_controllers_library.hpp | 3 ++- .../src/steering_controllers_library.cpp | 5 +++-- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp b/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp index 095db7674b..b560e2a782 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp @@ -76,7 +76,8 @@ class SteeringControllersLibrary : public controller_interface::ChainableControl const rclcpp_lifecycle::State & previous_state) override; STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::return_type - update_reference_from_subscribers() override; + update_reference_from_subscribers( + const rclcpp::Time & time, const rclcpp::Duration & period) override; STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::return_type update_and_write_commands(const rclcpp::Time & time, const rclcpp::Duration & period) override; diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index ccf924d527..0eeadd276d 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -411,10 +411,11 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_deactivate( return controller_interface::CallbackReturn::SUCCESS; } -controller_interface::return_type SteeringControllersLibrary::update_reference_from_subscribers() +controller_interface::return_type SteeringControllersLibrary::update_reference_from_subscribers( + const rclcpp::Time & time, const rclcpp::Duration & period) { auto current_ref = *(input_ref_.readFromRT()); - const auto age_of_last_command = get_node()->now() - (current_ref)->header.stamp; + const auto age_of_last_command = time - (current_ref)->header.stamp; // send message only if there is no timeout if (age_of_last_command <= ref_timeout_ || ref_timeout_ == rclcpp::Duration::from_seconds(0)) From 092b459eb462611bee189b5c414f08c315ad50e0 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Wed, 5 Apr 2023 17:28:30 +0100 Subject: [PATCH 182/186] Remove odom member from state publishing (doesn't exist) Fix compilation --- .../src/steering_controllers_library.cpp | 37 ++++++------------- 1 file changed, 11 insertions(+), 26 deletions(-) diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index 0eeadd276d..af2736a8a3 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -208,17 +208,7 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure( controller_state_publisher_->lock(); controller_state_publisher_->msg_.header.stamp = get_node()->now(); controller_state_publisher_->msg_.header.frame_id = params_.odom_frame_id; - controller_state_publisher_->msg_.odom.pose.pose.position.z = 0; - controller_state_publisher_->msg_.odom.child_frame_id = params_.base_frame_id; controller_state_publisher_->unlock(); - auto & covariance_controller = controller_state_publisher_->msg_.odom.twist.covariance; - for (size_t index = 0; index < 6; ++index) - { - // 0, 7, 14, 21, 28, 35 - const size_t diagonal_index = NUM_DIMENSIONS * index + index; - covariance_controller[diagonal_index] = params_.pose_covariance_diagonal[index]; - covariance_controller[diagonal_index] = params_.twist_covariance_diagonal[index]; - } RCLCPP_INFO(get_node()->get_logger(), "configure successful"); return controller_interface::CallbackReturn::SUCCESS; } @@ -412,7 +402,7 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_deactivate( } controller_interface::return_type SteeringControllersLibrary::update_reference_from_subscribers( - const rclcpp::Time & time, const rclcpp::Duration & period) + const rclcpp::Time & time, const rclcpp::Duration & /*period*/) { auto current_ref = *(input_ref_.readFromRT()); const auto age_of_last_command = time - (current_ref)->header.stamp; @@ -517,16 +507,11 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c if (controller_state_publisher_->trylock()) { controller_state_publisher_->msg_.header.stamp = time; - controller_state_publisher_->msg_.odom.pose.pose.position.x = odometry_.get_x(); - controller_state_publisher_->msg_.odom.pose.pose.position.y = odometry_.get_y(); - controller_state_publisher_->msg_.odom.pose.pose.orientation = tf2::toMsg(orientation); - controller_state_publisher_->msg_.odom.twist.twist.linear.x = odometry_.get_linear(); - controller_state_publisher_->msg_.odom.twist.twist.angular.z = odometry_.get_angular(); - controller_state_publisher_->msg_.traction_wheels_position.data.clear(); - controller_state_publisher_->msg_.traction_wheels_velocity.data.clear(); - controller_state_publisher_->msg_.linear_velocity_command.data.clear(); - controller_state_publisher_->msg_.steer_positions.data.clear(); - controller_state_publisher_->msg_.steering_angle_command.data.clear(); + controller_state_publisher_->msg_.traction_wheels_position.clear(); + controller_state_publisher_->msg_.traction_wheels_velocity.clear(); + controller_state_publisher_->msg_.linear_velocity_command.clear(); + controller_state_publisher_->msg_.steer_positions.clear(); + controller_state_publisher_->msg_.steering_angle_command.clear(); auto number_of_traction_wheels = params_.rear_wheels_names.size(); auto number_of_steering_wheels = params_.front_wheels_names.size(); @@ -541,23 +526,23 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c { if (params_.position_feedback) { - controller_state_publisher_->msg_.traction_wheels_position.data.push_back( + controller_state_publisher_->msg_.traction_wheels_position.push_back( state_interfaces_[i].get_value()); } else { - controller_state_publisher_->msg_.traction_wheels_velocity.data.push_back( + controller_state_publisher_->msg_.traction_wheels_velocity.push_back( state_interfaces_[i].get_value()); } - controller_state_publisher_->msg_.linear_velocity_command.data.push_back( + controller_state_publisher_->msg_.linear_velocity_command.push_back( command_interfaces_[i].get_value()); } for (size_t i = 0; i < number_of_steering_wheels; ++i) { - controller_state_publisher_->msg_.steer_positions.data.push_back( + controller_state_publisher_->msg_.steer_positions.push_back( state_interfaces_[number_of_traction_wheels + i].get_value()); - controller_state_publisher_->msg_.steering_angle_command.data.push_back( + controller_state_publisher_->msg_.steering_angle_command.push_back( command_interfaces_[number_of_traction_wheels + i].get_value()); } From 55d158cbadbf6fa65e4ee2a8a1890ae845897889 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Wed, 12 Apr 2023 17:28:10 +0100 Subject: [PATCH 183/186] Fix test compilation --- .../test/test_ackermann_steering_controller.cpp | 16 ++++++++-------- .../test/test_bicycle_steering_controller.cpp | 8 ++++---- .../test/test_tricycle_steering_controller.cpp | 12 ++++++------ 3 files changed, 18 insertions(+), 18 deletions(-) diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp index 60e65c55ae..a29b62d7c4 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp @@ -247,10 +247,10 @@ TEST_F(AckermannSteeringControllerTest, receive_message_and_publish_updated_stat ControllerStateMsg msg; subscribe_and_get_messages(msg); - EXPECT_EQ(msg.linear_velocity_command.data[STATE_TRACTION_RIGHT_WHEEL], 1.1); - EXPECT_EQ(msg.linear_velocity_command.data[STATE_TRACTION_LEFT_WHEEL], 3.3); - EXPECT_EQ(msg.steering_angle_command.data[0], 2.2); - EXPECT_EQ(msg.steering_angle_command.data[1], 4.4); + EXPECT_EQ(msg.linear_velocity_command[STATE_TRACTION_RIGHT_WHEEL], 1.1); + EXPECT_EQ(msg.linear_velocity_command[STATE_TRACTION_LEFT_WHEEL], 3.3); + EXPECT_EQ(msg.steering_angle_command[0], 2.2); + EXPECT_EQ(msg.steering_angle_command[1], 4.4); publish_commands(); ASSERT_TRUE(controller_->wait_for_commands(executor)); @@ -275,13 +275,13 @@ TEST_F(AckermannSteeringControllerTest, receive_message_and_publish_updated_stat subscribe_and_get_messages(msg); EXPECT_NEAR( - msg.linear_velocity_command.data[CMD_TRACTION_RIGHT_WHEEL], 0.22222222222222224, + msg.linear_velocity_command[CMD_TRACTION_RIGHT_WHEEL], 0.22222222222222224, COMMON_THRESHOLD); EXPECT_NEAR( - msg.linear_velocity_command.data[CMD_TRACTION_LEFT_WHEEL], 0.22222222222222224, + msg.linear_velocity_command[CMD_TRACTION_LEFT_WHEEL], 0.22222222222222224, COMMON_THRESHOLD); - EXPECT_NEAR(msg.steering_angle_command.data[0], 1.4179821977774734, COMMON_THRESHOLD); - EXPECT_NEAR(msg.steering_angle_command.data[1], 1.4179821977774734, COMMON_THRESHOLD); + EXPECT_NEAR(msg.steering_angle_command[0], 1.4179821977774734, COMMON_THRESHOLD); + EXPECT_NEAR(msg.steering_angle_command[1], 1.4179821977774734, COMMON_THRESHOLD); } int main(int argc, char ** argv) diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp index 7b1587e623..4350138da4 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp @@ -234,8 +234,8 @@ TEST_F(BicycleSteeringControllerTest, receive_message_and_publish_updated_status ControllerStateMsg msg; subscribe_and_get_messages(msg); - EXPECT_EQ(msg.linear_velocity_command.data[0], 1.1); - EXPECT_EQ(msg.steering_angle_command.data[0], 2.2); + EXPECT_EQ(msg.linear_velocity_command[0], 1.1); + EXPECT_EQ(msg.steering_angle_command[0], 2.2); publish_commands(); ASSERT_TRUE(controller_->wait_for_commands(executor)); @@ -252,8 +252,8 @@ TEST_F(BicycleSteeringControllerTest, receive_message_and_publish_updated_status subscribe_and_get_messages(msg); - EXPECT_NEAR(msg.linear_velocity_command.data[0], 0.253221, COMMON_THRESHOLD); - EXPECT_NEAR(msg.steering_angle_command.data[0], 1.4179821977774734, COMMON_THRESHOLD); + EXPECT_NEAR(msg.linear_velocity_command[0], 0.253221, COMMON_THRESHOLD); + EXPECT_NEAR(msg.steering_angle_command[0], 1.4179821977774734, COMMON_THRESHOLD); } int main(int argc, char ** argv) diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp index 1d2fa632b7..e10b63de48 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp @@ -234,9 +234,9 @@ TEST_F(TricycleSteeringControllerTest, receive_message_and_publish_updated_statu ControllerStateMsg msg; subscribe_and_get_messages(msg); - EXPECT_EQ(msg.linear_velocity_command.data[STATE_TRACTION_RIGHT_WHEEL], 1.1); - EXPECT_EQ(msg.linear_velocity_command.data[STATE_TRACTION_LEFT_WHEEL], 3.3); - EXPECT_EQ(msg.steering_angle_command.data[0], 2.2); + EXPECT_EQ(msg.linear_velocity_command[STATE_TRACTION_RIGHT_WHEEL], 1.1); + EXPECT_EQ(msg.linear_velocity_command[STATE_TRACTION_LEFT_WHEEL], 3.3); + EXPECT_EQ(msg.steering_angle_command[0], 2.2); publish_commands(); ASSERT_TRUE(controller_->wait_for_commands(executor)); @@ -258,12 +258,12 @@ TEST_F(TricycleSteeringControllerTest, receive_message_and_publish_updated_statu subscribe_and_get_messages(msg); EXPECT_NEAR( - msg.linear_velocity_command.data[STATE_TRACTION_RIGHT_WHEEL], 0.22222222222222224, + msg.linear_velocity_command[STATE_TRACTION_RIGHT_WHEEL], 0.22222222222222224, COMMON_THRESHOLD); EXPECT_NEAR( - msg.linear_velocity_command.data[STATE_TRACTION_LEFT_WHEEL], 0.22222222222222224, + msg.linear_velocity_command[STATE_TRACTION_LEFT_WHEEL], 0.22222222222222224, COMMON_THRESHOLD); - EXPECT_NEAR(msg.steering_angle_command.data[0], 1.4179821977774734, COMMON_THRESHOLD); + EXPECT_NEAR(msg.steering_angle_command[0], 1.4179821977774734, COMMON_THRESHOLD); } int main(int argc, char ** argv) From ecdf2a55e01c6cbd44570f46613380903396573d Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Wed, 12 Apr 2023 17:48:55 +0100 Subject: [PATCH 184/186] fix format --- .../test/test_ackermann_steering_controller.cpp | 6 ++---- .../test/test_tricycle_steering_controller.cpp | 6 ++---- 2 files changed, 4 insertions(+), 8 deletions(-) diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp index a29b62d7c4..2f88190775 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp @@ -275,11 +275,9 @@ TEST_F(AckermannSteeringControllerTest, receive_message_and_publish_updated_stat subscribe_and_get_messages(msg); EXPECT_NEAR( - msg.linear_velocity_command[CMD_TRACTION_RIGHT_WHEEL], 0.22222222222222224, - COMMON_THRESHOLD); + msg.linear_velocity_command[CMD_TRACTION_RIGHT_WHEEL], 0.22222222222222224, COMMON_THRESHOLD); EXPECT_NEAR( - msg.linear_velocity_command[CMD_TRACTION_LEFT_WHEEL], 0.22222222222222224, - COMMON_THRESHOLD); + msg.linear_velocity_command[CMD_TRACTION_LEFT_WHEEL], 0.22222222222222224, COMMON_THRESHOLD); EXPECT_NEAR(msg.steering_angle_command[0], 1.4179821977774734, COMMON_THRESHOLD); EXPECT_NEAR(msg.steering_angle_command[1], 1.4179821977774734, COMMON_THRESHOLD); } diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp index e10b63de48..fd323869aa 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp @@ -258,11 +258,9 @@ TEST_F(TricycleSteeringControllerTest, receive_message_and_publish_updated_statu subscribe_and_get_messages(msg); EXPECT_NEAR( - msg.linear_velocity_command[STATE_TRACTION_RIGHT_WHEEL], 0.22222222222222224, - COMMON_THRESHOLD); + msg.linear_velocity_command[STATE_TRACTION_RIGHT_WHEEL], 0.22222222222222224, COMMON_THRESHOLD); EXPECT_NEAR( - msg.linear_velocity_command[STATE_TRACTION_LEFT_WHEEL], 0.22222222222222224, - COMMON_THRESHOLD); + msg.linear_velocity_command[STATE_TRACTION_LEFT_WHEEL], 0.22222222222222224, COMMON_THRESHOLD); EXPECT_NEAR(msg.steering_angle_command[0], 1.4179821977774734, COMMON_THRESHOLD); } From d81714496914284b3dcbb22432398942507d6844 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Wed, 12 Apr 2023 22:31:33 +0100 Subject: [PATCH 185/186] Renovate load controller tests --- .../test/test_load_ackermann_steering_controller.cpp | 8 +++++--- .../test/test_load_bicycle_steering_controller.cpp | 6 ++++-- .../test/test_load_tricycle_steering_controller.cpp | 8 +++++--- 3 files changed, 14 insertions(+), 8 deletions(-) diff --git a/ackermann_steering_controller/test/test_load_ackermann_steering_controller.cpp b/ackermann_steering_controller/test/test_load_ackermann_steering_controller.cpp index aee335cb4e..fd01762200 100644 --- a/ackermann_steering_controller/test/test_load_ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/test/test_load_ackermann_steering_controller.cpp @@ -34,9 +34,11 @@ TEST(TestLoadAckermannSteeringController, load_controller) ros2_control_test_assets::minimal_robot_urdf), executor, "test_controller_manager"); - ASSERT_NO_THROW(cm.load_controller( - "test_ackermann_steering_controller", - "ackermann_steering_controller/AckermannSteeringController")); + ASSERT_NE( + cm.load_controller( + "test_ackermann_steering_controller", + "ackermann_steering_controller/AckermannSteeringController"), + nullptr); rclcpp::shutdown(); } diff --git a/bicycle_steering_controller/test/test_load_bicycle_steering_controller.cpp b/bicycle_steering_controller/test/test_load_bicycle_steering_controller.cpp index f7861aa322..5bc92867bc 100644 --- a/bicycle_steering_controller/test/test_load_bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/test/test_load_bicycle_steering_controller.cpp @@ -34,8 +34,10 @@ TEST(TestLoadBicycleSteeringController, load_controller) ros2_control_test_assets::minimal_robot_urdf), executor, "test_controller_manager"); - ASSERT_NO_THROW(cm.load_controller( - "test_bicycle_steering_controller", "bicycle_steering_controller/BicycleSteeringController")); + ASSERT_NE(+ + cm.load_controller( + "test_bicycle_steering_controller", "bicycle_steering_controller/BicycleSteeringController"), + nullptr); rclcpp::shutdown(); } diff --git a/tricycle_steering_controller/test/test_load_tricycle_steering_controller.cpp b/tricycle_steering_controller/test/test_load_tricycle_steering_controller.cpp index 8421f4d3a0..94de84f137 100644 --- a/tricycle_steering_controller/test/test_load_tricycle_steering_controller.cpp +++ b/tricycle_steering_controller/test/test_load_tricycle_steering_controller.cpp @@ -34,9 +34,11 @@ TEST(TestLoadTricycleSteeringController, load_controller) ros2_control_test_assets::minimal_robot_urdf), executor, "test_controller_manager"); - ASSERT_NO_THROW(cm.load_controller( - "test_tricycle_steering_controller", - "tricycle_steering_controller/TricycleSteeringController")); + ASSERT_NE( + cm.load_controller( + "test_tricycle_steering_controller", + "tricycle_steering_controller/TricycleSteeringController"), + nullptr); rclcpp::shutdown(); } From 79f5102b715d544110770ffed5f965e141c578db Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Wed, 26 Apr 2023 18:26:22 +0100 Subject: [PATCH 186/186] remove typo --- .../test/test_load_bicycle_steering_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/bicycle_steering_controller/test/test_load_bicycle_steering_controller.cpp b/bicycle_steering_controller/test/test_load_bicycle_steering_controller.cpp index 5bc92867bc..16e738d5ec 100644 --- a/bicycle_steering_controller/test/test_load_bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/test/test_load_bicycle_steering_controller.cpp @@ -34,7 +34,7 @@ TEST(TestLoadBicycleSteeringController, load_controller) ros2_control_test_assets::minimal_robot_urdf), executor, "test_controller_manager"); - ASSERT_NE(+ + ASSERT_NE( cm.load_controller( "test_bicycle_steering_controller", "bicycle_steering_controller/BicycleSteeringController"), nullptr);