diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index b927aeac78..5a7ac74d9b 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -126,7 +126,7 @@ repos: 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$ diff --git a/ackermann_steering_controller/CMakeLists.txt b/ackermann_steering_controller/CMakeLists.txt new file mode 100644 index 0000000000..ff76da8853 --- /dev/null +++ b/ackermann_steering_controller/CMakeLists.txt @@ -0,0 +1,107 @@ +cmake_minimum_required(VERSION 3.16) +project(ackermann_steering_controller LANGUAGES CXX) + +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +set(THIS_PACKAGE_INCLUDE_DEPENDS + controller_interface + hardware_interface + generate_parameter_library + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + std_srvs + steering_controllers_library +) + +find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +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 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) + +if(BUILD_TESTING) + 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_load_ackermann_steering_controller + test/test_load_ackermann_steering_controller.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/ackermann_steering_controller_params.yaml + ) + 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() + +install( + DIRECTORY include/ + DESTINATION include/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/ackermann_steering_controller.xml b/ackermann_steering_controller/ackermann_steering_controller.xml new file mode 100644 index 0000000000..2ac2150dd1 --- /dev/null +++ b/ackermann_steering_controller/ackermann_steering_controller.xml @@ -0,0 +1,8 @@ + + + + Steering controller for Ackermann (car-like) kinematics with two traction and two steering joints. + + + diff --git a/ackermann_steering_controller/doc/userdoc.rst b/ackermann_steering_controller/doc/userdoc.rst new file mode 100644 index 0000000000..59cdb78108 --- /dev/null +++ b/ackermann_steering_controller/doc/userdoc.rst @@ -0,0 +1,18 @@ +.. _ackermann_steering_controller_userdoc: + +ackermann_steering_controller +============================= + +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 `. + + +Parameters +,,,,,,,,,,, + +For list of parameters and their meaning YAML file in the ``src`` folder of the controller's package. + +For an exemplary 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 new file mode 100644 index 0000000000..0cb6bcd016 --- /dev/null +++ b/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp @@ -0,0 +1,65 @@ +// 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. +// +// 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_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 +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; + +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; + +protected: + std::shared_ptr ackermann_param_listener_; + ackermann_steering_controller::Params ackermann_params_; +}; +} // 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..177f0bf87c --- /dev/null +++ b/ackermann_steering_controller/include/ackermann_steering_controller/visibility_control.h @@ -0,0 +1,52 @@ +// 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 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 new file mode 100644 index 0000000000..24c3c8406b --- /dev/null +++ b/ackermann_steering_controller/package.xml @@ -0,0 +1,36 @@ + + + + ackermann_steering_controller + 0.0.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 + + 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..c3a7539c5a --- /dev/null +++ b/ackermann_steering_controller/src/ackermann_steering_controller.cpp @@ -0,0 +1,98 @@ +// 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 "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_params_ = ackermann_param_listener_->get_params(); + + 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) + { + 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); + + 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_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(); + 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..3726146919 --- /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. For details see: https://en.wikipedia.org/wiki/Wheelbase", + read_only: false, + } + + rear_wheel_track: + { + type: double, + default_value: 0.0, + description: "Rear wheel track length. For details see: https://en.wikipedia.org/wiki/Wheelbase", + read_only: false, + } + + wheelbase: + { + type: double, + default_value: 0.0, + description: "Distance between front and rear wheels. For details see: https://en.wikipedia.org/wiki/Wheelbase", + 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..6b64f901c3 --- /dev/null +++ b/ackermann_steering_controller/test/ackermann_steering_controller_params.yaml @@ -0,0 +1,17 @@ +test_ackermann_steering_controller: + ros__parameters: + + 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] + + 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/ackermann_steering_controller_preceeding_params.yaml b/ackermann_steering_controller/test/ackermann_steering_controller_preceeding_params.yaml new file mode 100644 index 0000000000..ecb1b071e3 --- /dev/null +++ b/ackermann_steering_controller/test/ackermann_steering_controller_preceeding_params.yaml @@ -0,0 +1,17 @@ +test_ackermann_steering_controller: + ros__parameters: + 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.cpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp new file mode 100644 index 0000000000..480e90e166 --- /dev/null +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp @@ -0,0 +1,292 @@ +// 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_ackermann_steering_controller.hpp" + +#include +#include +#include +#include +#include + +class AckermannSteeringControllerTest +: public AckermannSteeringControllerFixture +{ +}; + +TEST_F(AckermannSteeringControllerTest, all_parameters_set_configure_success) +{ + SetUpController(); + + 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_->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) +{ + 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 itfsTIME + 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(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)); + 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) +{ + 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, 0, RCL_ROS_TIME), 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_->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, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); +} + +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); + 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 + 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, 0, RCL_ROS_TIME), 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_RIGHT_WHEEL].get_value(), 1.4179821977774734, + COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[CMD_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)); + } +} + +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, 0, RCL_ROS_TIME), 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(); + 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, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + ControllerStateMsg msg; + subscribe_and_get_messages(msg); + + 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)); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), 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_RIGHT_WHEEL].get_value(), 1.4179821977774734, + COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[CMD_STEER_LEFT_WHEEL].get_value(), 1.4179821977774734, + COMMON_THRESHOLD); + + subscribe_and_get_messages(msg); + + EXPECT_NEAR( + 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); + 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) +{ + ::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..80258258c2 --- /dev/null +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp @@ -0,0 +1,319 @@ +// 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_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" + +using ControllerStateMsg = + steering_controllers_library::SteeringControllersLibrary::AckermanControllerState; +using ControllerReferenceMsg = + steering_controllers_library::SteeringControllersLibrary::ControllerTwistReferenceMsg; + +// name constants for state interfaces +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_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 +{ +constexpr auto NODE_SUCCESS = controller_interface::CallbackReturn::SUCCESS; +constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR; +const double COMMON_THRESHOLD = 1e-6; +} // 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, 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); + +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_twist_); + } + 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); + } + +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()); + } + + 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); + + 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_ackermann_steering_controller/controller_state", 10, subs_callback); + + // call update to publish the test value + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), 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, 0, RCL_ROS_TIME), 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}; + 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_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..2d951588c5 --- /dev/null +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp @@ -0,0 +1,107 @@ +// 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_ackermann_steering_controller.hpp" + +#include +#include +#include +#include +#include + +class AckermannSteeringControllerTest +: public AckermannSteeringControllerFixture +{ +}; + +TEST_F(AckermannSteeringControllerTest, all_parameters_set_configure_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + 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) +{ + 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], + 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()); + 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]); + } +} + +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..0a8cd7b80c --- /dev/null +++ b/ackermann_steering_controller/test/test_load_ackermann_steering_controller.cpp @@ -0,0 +1,49 @@ +// 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(TestLoadAckermannSteeringController, load_controller) +{ + 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_NE( + cm.load_controller( + "test_ackermann_steering_controller", + "ackermann_steering_controller/AckermannSteeringController"), + nullptr); +} + +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/CMakeLists.txt b/bicycle_steering_controller/CMakeLists.txt new file mode 100644 index 0000000000..8ac12fe0a0 --- /dev/null +++ b/bicycle_steering_controller/CMakeLists.txt @@ -0,0 +1,105 @@ +cmake_minimum_required(VERSION 3.16) +project(bicycle_steering_controller LANGUAGES CXX) + +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +set(THIS_PACKAGE_INCLUDE_DEPENDS + controller_interface + hardware_interface + generate_parameter_library + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + std_srvs + steering_controllers_library +) + +find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +generate_parameter_library(bicycle_steering_controller_parameters + src/bicycle_steering_controller.yaml +) + +add_library( + bicycle_steering_controller + 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 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) + +if(BUILD_TESTING) + 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_load_bicycle_steering_controller + test/test_load_bicycle_steering_controller.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/bicycle_steering_controller_params.yaml + ) + 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() + +install( + DIRECTORY include/ + DESTINATION include/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/bicycle_steering_controller.xml b/bicycle_steering_controller/bicycle_steering_controller.xml new file mode 100644 index 0000000000..644c8840fa --- /dev/null +++ b/bicycle_steering_controller/bicycle_steering_controller.xml @@ -0,0 +1,8 @@ + + + + Steering controller for Bicycle kinematics with one traction and one steering joint. + + + diff --git a/bicycle_steering_controller/doc/userdoc.rst b/bicycle_steering_controller/doc/userdoc.rst new file mode 100644 index 0000000000..6815dc6953 --- /dev/null +++ b/bicycle_steering_controller/doc/userdoc.rst @@ -0,0 +1,19 @@ +.. _bicycle_steering_controller_userdoc: + +bicycle_steering_controller +============================= + +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 `. + + +Parameters +,,,,,,,,,,, + +For list of parameters and their meaning YAML file in the ``src`` 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/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..1b3e050a37 --- /dev/null +++ b/bicycle_steering_controller/include/bicycle_steering_controller/bicycle_steering_controller.hpp @@ -0,0 +1,61 @@ +// 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. +// +// 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 +{ +// name constants for state interfaces +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_TRACTION_WHEEL = 0; +static constexpr size_t CMD_STEER_WHEEL = 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: + 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; + +protected: + std::shared_ptr bicycle_param_listener_; + bicycle_steering_controller::Params bicycle_params_; +}; +} // 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..b076a00215 --- /dev/null +++ b/bicycle_steering_controller/include/bicycle_steering_controller/visibility_control.h @@ -0,0 +1,52 @@ +// 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 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 new file mode 100644 index 0000000000..80932f7fda --- /dev/null +++ b/bicycle_steering_controller/package.xml @@ -0,0 +1,36 @@ + + + + bicycle_steering_controller + 0.0.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 + + 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..5f013d7d7a --- /dev/null +++ b/bicycle_steering_controller/src/bicycle_steering_controller.cpp @@ -0,0 +1,87 @@ +// 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 "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_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); + + 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_[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)) + { + 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..c40e27ef96 --- /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: "Distance between front and rear wheel. For details see: https://en.wikipedia.org/wiki/Wheelbase", + 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..a2a6c6508b --- /dev/null +++ b/bicycle_steering_controller/test/bicycle_steering_controller_params.yaml @@ -0,0 +1,15 @@ +test_bicycle_steering_controller: + ros__parameters: + + 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_wheel_joint] + front_wheels_names: [steering_axis_joint] + + wheelbase: 3.24644 + front_wheel_radius: 0.45 + rear_wheel_radius: 0.45 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..39ffeed878 --- /dev/null +++ b/bicycle_steering_controller/test/bicycle_steering_controller_preceeding_params.yaml @@ -0,0 +1,16 @@ +test_bicycle_steering_controller: + ros__parameters: + 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] + + 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 new file mode 100644 index 0000000000..06b0c7e846 --- /dev/null +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp @@ -0,0 +1,266 @@ +// 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 + +class BicycleSteeringControllerTest +: public BicycleSteeringControllerFixture +{ +}; + +TEST_F(BicycleSteeringControllerTest, all_parameters_set_configure_success) +{ + SetUpController(); + + 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_->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) +{ + 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_WHEEL], + rear_wheels_names_[0] + "/" + traction_interface_name_); + EXPECT_EQ( + 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_WHEEL], + controller_->rear_wheels_state_names_[0] + "/" + traction_interface_name_); + EXPECT_EQ( + state_intefaces.names[STATE_STEER_AXIS], + controller_->front_wheels_state_names_[0] + "/" + steering_interface_name_); + + // check ref itfsTIME + 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) +{ + 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)); + 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) +{ + 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, 0, RCL_ROS_TIME), 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_->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, 0, RCL_ROS_TIME), 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()); + + 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 + 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, 0, RCL_ROS_TIME), 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_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)); + } +} + +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, 0, RCL_ROS_TIME), 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(); + + 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, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + ControllerStateMsg msg; + 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()); + + 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, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + ControllerStateMsg msg; + subscribe_and_get_messages(msg); + + 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)); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), 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); + + subscribe_and_get_messages(msg); + + 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) +{ + ::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..065f9e1a0d --- /dev/null +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp @@ -0,0 +1,284 @@ +// 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" + +using ControllerStateMsg = + steering_controllers_library::SteeringControllersLibrary::AckermanControllerState; +using ControllerReferenceMsg = + steering_controllers_library::SteeringControllersLibrary::ControllerTwistReferenceMsg; + +// name constants for state interfaces +using bicycle_steering_controller::STATE_STEER_AXIS; +using bicycle_steering_controller::STATE_TRACTION_WHEEL; + +// name constants for command interfaces +using bicycle_steering_controller::CMD_STEER_WHEEL; +using bicycle_steering_controller::CMD_TRACTION_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 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); + 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); + +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_twist_); + } + 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); + } + +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/reference", rclcpp::SystemDefaultsQoS()); + } + + 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); + + 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_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_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_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_AXIS])); + 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_bicycle_steering_controller/controller_state", 10, subs_callback); + + // call update to publish the test value + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), 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(rclcpp::Time(0, 0, RCL_ROS_TIME), 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_wheel_joint"}; + 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; + + 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::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_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..875910ba23 --- /dev/null +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller_preceeding.cpp @@ -0,0 +1,95 @@ +// 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 + +class BicycleSteeringControllerTest +: public BicycleSteeringControllerFixture +{ +}; + +TEST_F(BicycleSteeringControllerTest, all_parameters_set_configure_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + 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) +{ + 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_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()); + + EXPECT_EQ( + state_intefaces.names[STATE_TRACTION_WHEEL], + controller_->rear_wheels_state_names_[0] + "/" + traction_interface_name_); + EXPECT_EQ( + state_intefaces.names[STATE_STEER_AXIS], + controller_->front_wheels_state_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]); + } +} + +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..955feb33c5 --- /dev/null +++ b/bicycle_steering_controller/test/test_load_bicycle_steering_controller.cpp @@ -0,0 +1,48 @@ +// 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) +{ + 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_NE( + cm.load_controller( + "test_bicycle_steering_controller", "bicycle_steering_controller/BicycleSteeringController"), + nullptr); +} + +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/doc/controllers_index.rst b/doc/controllers_index.rst index effe814325..cb5528bdb0 100644 --- a/doc/controllers_index.rst +++ b/doc/controllers_index.rst @@ -42,14 +42,19 @@ Available Controllers .. toctree:: :titlesonly: + Ackermann Steering Controller <../ackermann_steering_controller/doc/userdoc.rst> Admittance Controller <../admittance_controller/doc/userdoc.rst> + Bicycle Steering Controller <../bicycle_steering_controller/doc/userdoc.rst> Differential Drive Controller <../diff_drive_controller/doc/userdoc.rst> Effort Controllers <../effort_controllers/doc/userdoc.rst> Forward Command Controller <../forward_command_controller/doc/userdoc.rst> Joint Trajectory Controller <../joint_trajectory_controller/doc/userdoc.rst> Position Controllers <../position_controllers/doc/userdoc.rst> Tricycle Controller <../tricycle_controller/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> + Steering Controllers Library <../steering_controllers_library/doc/userdoc.rst> Available Broadcasters diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index b2150411d8..ffdcc89d1a 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -10,7 +10,9 @@ ament_cmake + ackermann_steering_controller admittance_controller + bicycle_steering_controller diff_drive_controller effort_controllers force_torque_sensor_broadcaster @@ -19,7 +21,9 @@ joint_state_broadcaster joint_trajectory_controller position_controllers + steering_controllers_library tricycle_controller + tricycle_steering_controller velocity_controllers diff --git a/steering_controllers_library/CMakeLists.txt b/steering_controllers_library/CMakeLists.txt new file mode 100644 index 0000000000..5fdd727188 --- /dev/null +++ b/steering_controllers_library/CMakeLists.txt @@ -0,0 +1,88 @@ +cmake_minimum_required(VERSION 3.16) +project(steering_controllers_library LANGUAGES CXX) + +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 + generate_parameter_library + geometry_msgs + hardware_interface + nav_msgs + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + std_srvs + tf2 + tf2_msgs + tf2_geometry_msgs + ackermann_msgs +) + +find_package(ament_cmake 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( + steering_controllers_library + SHARED + src/steering_controllers_library.cpp + src/steering_odometry.cpp +) +target_compile_features(steering_controllers_library PUBLIC cxx_std_17) +target_include_directories(steering_controllers_library PUBLIC + "$" + "$") +target_link_libraries(steering_controllers_library PUBLIC + steering_controllers_library_parameters) +ament_target_dependencies(steering_controllers_library 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(steering_controllers_library PRIVATE "STEERING_CONTROLLERS_BUILDING_DLL") + +if(BUILD_TESTING) + 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() + +install( + DIRECTORY include/ + DESTINATION include/steering_controllers_library +) + +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 new file mode 100644 index 0000000000..27133e2e77 --- /dev/null +++ b/steering_controllers_library/doc/userdoc.rst @@ -0,0 +1,90 @@ +.. _steering_controllers_library_userdoc: + +steering_controllers_library +============================= + +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 `_. + +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 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. + +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: + +* :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 +,,,,,,, +Depending on the ``position_feedback``, different feedback types are expected + +* ``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] + If parameter ``use_stamped_vel`` is ``true``. +- /reference_unstamped [geometry_msgs/msg/Twist] + If parameter ``use_stamped_vel`` is ``false``. + +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 exemplary 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 new file mode 100644 index 0000000000..b560e2a782 --- /dev/null +++ b/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp @@ -0,0 +1,151 @@ +// 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 STEERING_CONTROLLERS_LIBRARY__STEERING_CONTROLLERS_LIBRARY_HPP_ +#define STEERING_CONTROLLERS_LIBRARY__STEERING_CONTROLLERS_LIBRARY_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_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" +#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" +#include "tf2_msgs/msg/tf_message.hpp" + +namespace steering_controllers_library +{ +class SteeringControllersLibrary : public controller_interface::ChainableControllerInterface +{ +public: + STEERING_CONTROLLERS__VISIBILITY_PUBLIC SteeringControllersLibrary(); + + 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 + command_interface_configuration() const override; + + STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::InterfaceConfiguration + state_interface_configuration() const override; + + virtual STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::CallbackReturn + configure_odometry() = 0; + + virtual STEERING_CONTROLLERS__VISIBILITY_PUBLIC bool update_odometry( + const rclcpp::Duration & period) = 0; + + STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::CallbackReturn on_deactivate( + 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; + + STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::return_type + update_and_write_commands(const rclcpp::Time & time, const rclcpp::Duration & period) override; + + using ControllerAckermannReferenceMsg = ackermann_msgs::msg::AckermannDriveStamped; + 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( + 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; + 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 + + 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; + + /// Odometry: + steering_odometry::SteeringOdometry odometry_; + + AckermanControllerState published_state_; + + using ControllerStatePublisher = realtime_tools::RealtimePublisher; + 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_; + + // store last velocity + 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( + const std::shared_ptr msg); + void reference_callback_unstamped(const std::shared_ptr msg); +}; + +} // namespace steering_controllers_library + +#endif // STEERING_CONTROLLERS_LIBRARY__STEERING_CONTROLLERS_LIBRARY_HPP_ diff --git a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp new file mode 100644 index 0000000000..002db32354 --- /dev/null +++ b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp @@ -0,0 +1,261 @@ +// 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. +// +// Authors: dr. sc. Tomislav Petkovic, Dr. Ing. Denis Štogl +// + +#ifndef STEERING_CONTROLLERS_LIBRARY__STEERING_ODOMETRY_HPP_ +#define STEERING_CONTROLLERS_LIBRARY__STEERING_ODOMETRY_HPP_ + +#include +#include + +#include "realtime_tools/realtime_buffer.h" +#include "realtime_tools/realtime_publisher.h" + +#include +#include "rcpputils/rolling_mean_accumulator.hpp" + +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) + */ +class SteeringOdometry +{ +public: + /** + * \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 + * + */ + explicit SteeringOdometry(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 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 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 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 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 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 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 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 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 + * \param linear Linear velocity [m/s] + * \param angular Angular velocity [rad/s] + * \param time Current time + */ + 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] + */ + double get_heading() const { return heading_; } + + /** + * \brief x position getter + * \return x position [m] + */ + double get_x() const { return x_; } + + /** + * \brief y position getter + * \return y position [m] + */ + double get_y() const { return y_; } + + /** + * \brief linear velocity getter + * \return linear velocity [m/s] + */ + double get_linear() const { return linear_; } + + /** + * \brief angular velocity getter + * \return angular velocity [rad/s] + */ + double get_angular() const { return angular_; } + + /** + * \brief Sets the wheel parameters: radius, separation and wheelbase + */ + void set_wheel_params(double wheel_radius, double wheelbase = 0.0, double wheel_track = 0.0); + + /** + * \brief Velocity rolling window size setter + * \param velocity_rolling_window_size Velocity rolling window size + */ + void set_velocity_rolling_window_size(size_t velocity_rolling_window_size); + + /** + * \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, std::vector> get_commands(double Vx, double theta_dot); + + /** + * \brief Reset poses, heading, and accumulators + */ + 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 + * \param angular Angular velocity [rad] (angular displacement, i.e. m/s * dt) computed by encoders + */ + 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 integrate_exact(double linear, double angular); + + /** + * \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); + + /** + * \brief Reset linear and angular accumulators + */ + void reset_accumulators(); + + /// Current timestamp: + rclcpp::Time timestamp_; + + /// Current pose: + double x_; // [m] + double y_; // [m] + double steer_pos_; // [rad] + double heading_; // [rad] + + /// Current velocity: + double linear_; // [m/s] + double angular_; // [rad/s] + + /// Kinematic parameters + double wheel_track_; // [m] + double wheelbase_; // [m] + double wheel_radius_; // [m] + + /// Configuration type used for the forward kinematics + int config_type_ = -1; + + /// Previous wheel position/state [rad]: + double traction_wheel_old_pos_; + double traction_right_wheel_old_pos_; + double traction_left_wheel_old_pos_; + /// Rolling mean accumulators for the linear and angular velocities: + size_t velocity_rolling_window_size_; + rcpputils::RollingMeanAccumulator linear_acc_; + rcpputils::RollingMeanAccumulator angular_acc_; +}; +} // namespace steering_odometry + +#endif // STEERING_CONTROLLERS_LIBRARY__STEERING_ODOMETRY_HPP_ diff --git a/steering_controllers_library/include/steering_controllers_library/visibility_control.h b/steering_controllers_library/include/steering_controllers_library/visibility_control.h new file mode 100644 index 0000000000..123662031b --- /dev/null +++ b/steering_controllers_library/include/steering_controllers_library/visibility_control.h @@ -0,0 +1,50 @@ +// 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 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 + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define STEERING_CONTROLLERS__VISIBILITY_EXPORT __attribute__((dllexport)) +#define STEERING_CONTROLLERS__VISIBILITY_IMPORT __attribute__((dllimport)) +#else +#define STEERING_CONTROLLERS__VISIBILITY_EXPORT __declspec(dllexport) +#define STEERING_CONTROLLERS__VISIBILITY_IMPORT __declspec(dllimport) +#endif +#ifdef STEERING_CONTROLLERS__VISIBILITY_BUILDING_DLL +#define STEERING_CONTROLLERS__VISIBILITY_PUBLIC STEERING_CONTROLLERS__VISIBILITY_EXPORT +#else +#define STEERING_CONTROLLERS__VISIBILITY_PUBLIC STEERING_CONTROLLERS__VISIBILITY_IMPORT +#endif +#define STEERING_CONTROLLERS__VISIBILITY_PUBLIC_TYPE STEERING_CONTROLLERS__VISIBILITY_PUBLIC +#define STEERING_CONTROLLERS__VISIBILITY_LOCAL +#else +#define STEERING_CONTROLLERS__VISIBILITY_EXPORT __attribute__((visibility("default"))) +#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 +#define STEERING_CONTROLLERS__VISIBILITY_LOCAL +#endif +#define STEERING_CONTROLLERS__VISIBILITY_PUBLIC_TYPE +#endif + +#endif // STEERING_CONTROLLERS_LIBRARY__VISIBILITY_CONTROL_H_ diff --git a/steering_controllers_library/package.xml b/steering_controllers_library/package.xml new file mode 100644 index 0000000000..0d9aa9da39 --- /dev/null +++ b/steering_controllers_library/package.xml @@ -0,0 +1,45 @@ + + + + steering_controllers_library + 0.0.0 + Package for steering robot configurations including odometry and interfaces. + 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 + + 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 + + ament_cmake_gmock + controller_manager + hardware_interface + ros2_control_test_assets + + + ament_cmake + + diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp new file mode 100644 index 0000000000..af2736a8a3 --- /dev/null +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -0,0 +1,558 @@ +// 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 "steering_controllers_library/steering_controllers_library.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 + +using ControllerTwistReferenceMsg = + steering_controllers_library::SteeringControllersLibrary::ControllerTwistReferenceMsg; + +// 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 steering_controllers_library +{ +SteeringControllersLibrary::SteeringControllersLibrary() +: controller_interface::ChainableControllerInterface() +{ +} + +controller_interface::CallbackReturn SteeringControllersLibrary::on_init() +{ + try + { + param_listener_ = std::make_shared(get_node()); + initialize_implementation_parameter_listener(); + } + 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 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; + nr_cmd_itfs_ = nr_cmd_itfs; + nr_ref_itfs_ = nr_ref_itfs; + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn SteeringControllersLibrary::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(); + + 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); + subscribers_qos.best_effort(); + + // Reference Subscriber + ref_timeout_ = rclcpp::Duration::from_seconds(params_.reference_timeout); + if (params_.use_stamped_vel) + { + ref_subscriber_twist_ = get_node()->create_subscription( + "~/reference", subscribers_qos, + std::bind(&SteeringControllersLibrary::reference_callback, this, std::placeholders::_1)); + } + else + { + ref_subscriber_unstamped_ = get_node()->create_subscription( + "~/reference_unstamped", subscribers_qos, + std::bind( + &SteeringControllersLibrary::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; + } + + 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; + } + + 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_->unlock(); + RCLCPP_INFO(get_node()->get_logger(), "configure successful"); + return controller_interface::CallbackReturn::SUCCESS; +} + +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) + { + 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 SteeringControllersLibrary::reference_callback_unstamped( + const std::shared_ptr msg) +{ + 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 + 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 +SteeringControllersLibrary::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_); + + if (params_.front_steering) + { + 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); + } + } + else + { + 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; +} + +controller_interface::InterfaceConfiguration +SteeringControllersLibrary::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 traction_wheels_feedback = params_.position_feedback + ? hardware_interface::HW_IF_POSITION + : hardware_interface::HW_IF_VELOCITY; + if (params_.front_steering) + { + for (size_t i = 0; i < rear_wheels_state_names_.size(); i++) + { + state_interfaces_config.names.push_back( + rear_wheels_state_names_[i] + "/" + traction_wheels_feedback); + } + + for (size_t i = 0; i < front_wheels_state_names_.size(); i++) + { + state_interfaces_config.names.push_back( + front_wheels_state_names_[i] + "/" + hardware_interface::HW_IF_POSITION); + } + } + else + { + for (size_t i = 0; i < front_wheels_state_names_.size(); i++) + { + state_interfaces_config.names.push_back( + front_wheels_state_names_[i] + "/" + traction_wheels_feedback); + } + + for (size_t i = 0; i < rear_wheels_state_names_.size(); i++) + { + state_interfaces_config.names.push_back( + rear_wheels_state_names_[i] + "/" + hardware_interface::HW_IF_POSITION); + } + } + + return state_interfaces_config; +} + +std::vector +SteeringControllersLibrary::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 SteeringControllersLibrary::on_set_chained_mode(bool chained_mode) +{ + // Always accept switch to/from chained mode + return true || chained_mode; +} + +controller_interface::CallbackReturn SteeringControllersLibrary::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 SteeringControllersLibrary::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + 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 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; + + // 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 + { + 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; +} + +controller_interface::return_type SteeringControllersLibrary::update_and_write_commands( + const rclcpp::Time & time, const rclcpp::Duration & period) +{ + update_odometry(period); + + // 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 [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 + { + { + 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]); + } + } + } + } + + // 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_.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(); + + if (!params_.front_steering) + { + number_of_traction_wheels = params_.front_wheels_names.size(); + number_of_steering_wheels = params_.rear_wheels_names.size(); + } + + for (size_t i = 0; i < number_of_traction_wheels; ++i) + { + if (params_.position_feedback) + { + controller_state_publisher_->msg_.traction_wheels_position.push_back( + state_interfaces_[i].get_value()); + } + else + { + controller_state_publisher_->msg_.traction_wheels_velocity.push_back( + state_interfaces_[i].get_value()); + } + 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.push_back( + state_interfaces_[number_of_traction_wheels + i].get_value()); + controller_state_publisher_->msg_.steering_angle_command.push_back( + command_interfaces_[number_of_traction_wheels + i].get_value()); + } + + 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; +} + +} // namespace steering_controllers_library diff --git a/steering_controllers_library/src/steering_controllers_library.yaml b/steering_controllers_library/src/steering_controllers_library.yaml new file mode 100644 index 0000000000..86acb01dae --- /dev/null +++ b/steering_controllers_library/src/steering_controllers_library.yaml @@ -0,0 +1,122 @@ +steering_controllers_library: + 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.", + } + + front_steering: { + type: bool, + default_value: true, + description: "Is the steering on the front of the robot?", + read_only: true, + } + + rear_wheels_names: { + type: string_array, + description: "Names of rear wheel joints.", + read_only: true, + validation: { + size_lt<>: [5], + unique<>: null, + not_empty<>: null, + } + } + + front_wheels_names: { + type: string_array, + 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: { + type: bool, + default_value: false, + description: "bool parameter decides if open oop or not (feedback).", + 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, + } + + 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 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 + 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/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp new file mode 100644 index 0000000000..28cd7fc80d --- /dev/null +++ b/steering_controllers_library/src/steering_odometry.cpp @@ -0,0 +1,333 @@ +/********************************************************************* +* 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. + *********************************************************************/ + +/* + * Author: dr. sc. Tomislav Petkovic + * Author: Dr. Ing. Denis Stogl + */ + +#include "steering_controllers_library/steering_odometry.hpp" + +#include +#include + +namespace steering_odometry +{ +SteeringOdometry::SteeringOdometry(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_track_(0.0), + wheelbase_(0.0), + wheel_radius_(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) +{ +} + +void SteeringOdometry::init(const rclcpp::Time & time) +{ + // Reset accumulators and timestamp: + reset_accumulators(); + 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; +} + +bool SteeringOdometry::update_from_position( + const double traction_wheel_pos, const double steer_pos, const double dt) +{ + /// Get current wheel joint positions: + 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: + traction_wheel_old_pos_ = traction_wheel_cur_pos; + + /// Compute linear and angular diff: + const double linear_velocity = traction_wheel_est_pos_diff / dt; + steer_pos_ = steer_pos; + const double angular = tan(steer_pos) * linear_velocity / wheelbase_; + + return update_odometry(linear_velocity, angular, dt); +} + +bool SteeringOdometry::update_from_position( + 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 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 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: + traction_right_wheel_old_pos_ = traction_right_wheel_cur_pos; + traction_left_wheel_old_pos_ = traction_left_wheel_cur_pos; + + const double linear_velocity = + (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_; + + return update_odometry(linear_velocity, angular, dt); +} + +bool SteeringOdometry::update_from_position( + 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 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 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: + 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 = + (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_; + + return update_odometry(linear_velocity, angular, dt); +} + +bool SteeringOdometry::update_from_velocity( + const double traction_wheel_vel, const double steer_pos, const double dt) +{ + steer_pos_ = steer_pos; + double linear_velocity = traction_wheel_vel * wheel_radius_; + const double angular = tan(steer_pos) * linear_velocity / wheelbase_; + + return update_odometry(linear_velocity, angular, dt); +} + +bool SteeringOdometry::update_from_velocity( + const double right_traction_wheel_vel, const double left_traction_wheel_vel, + const double steer_pos, const double dt) +{ + 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_; + + return update_odometry(linear_velocity, angular, dt); +} + +bool SteeringOdometry::update_from_velocity( + 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) +{ + 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_; + + return update_odometry(linear_velocity, angular, dt); +} + +void SteeringOdometry::update_open_loop(const double linear, const double angular, const double dt) +{ + /// Save last linear and angular velocity: + linear_ = linear; + angular_ = angular; + + /// Integrate odometry: + SteeringOdometry::integrate_exact(linear * dt, angular * dt); +} + +void SteeringOdometry::set_wheel_params(double wheel_radius, double wheelbase, double wheel_track) +{ + wheel_radius_ = wheel_radius; + wheelbase_ = wheelbase; + wheel_track_ = wheel_track; +} + +void SteeringOdometry::set_velocity_rolling_window_size(size_t velocity_rolling_window_size) +{ + velocity_rolling_window_size_ = velocity_rolling_window_size; + + reset_accumulators(); +} + +void SteeringOdometry::set_odometry_type(const unsigned int type) { config_type_ = type; } + +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); +} + +std::tuple, std::vector> SteeringOdometry::get_commands( + double Vx, double theta_dot) +{ + // desired velocity and steering angle of the middle of traction and steering axis + double Ws, alpha; + + if (Vx == 0 && theta_dot != 0) + { + alpha = theta_dot > 0 ? M_PI_2 : -M_PI_2; + Ws = abs(theta_dot) * wheelbase_ / wheel_radius_; + } + else + { + alpha = SteeringOdometry::convert_trans_rot_vel_to_steering_angle(Vx, theta_dot); + Ws = Vx / (wheel_radius_ * std::cos(steer_pos_)); + } + + 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; + std::vector steering_commands; + if (fabs(steer_pos_) < 1e-6) + { + traction_commands = {Ws, Ws}; + } + else + { + 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}; + return std::make_tuple(traction_commands, steering_commands); + } + else if (config_type_ == ACKERMANN_CONFIG) + { + std::vector traction_commands; + std::vector steering_commands; + if (fabs(steer_pos_) < 1e-6) + { + traction_commands = {Ws, Ws}; + steering_commands = {alpha, alpha}; + } + else + { + 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}; + + 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}; + } + return std::make_tuple(traction_commands, steering_commands); + } + else + { + throw std::runtime_error("Config not implemented"); + } +} + +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; + + /// 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 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): + 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 SteeringOdometry::reset_accumulators() +{ + linear_acc_ = rcpputils::RollingMeanAccumulator(velocity_rolling_window_size_); + angular_acc_ = rcpputils::RollingMeanAccumulator(velocity_rolling_window_size_); +} + +} // namespace steering_odometry 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.cpp b/steering_controllers_library/test/test_steering_controllers_library.cpp new file mode 100644 index 0000000000..81075d1082 --- /dev/null +++ b/steering_controllers_library/test/test_steering_controllers_library.cpp @@ -0,0 +1,200 @@ +// 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_steering_controllers_library.hpp" + +#include +#include +#include +#include +#include + +#include "hardware_interface/types/hardware_interface_type_values.hpp" + +class SteeringControllersLibraryTest +: public SteeringControllersLibraryFixture +{ +}; + +// 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 itfsTIME + 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) +{ + 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(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 + 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(); + + // 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); + + 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); + ASSERT_EQ( + controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[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)); + + EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[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); + } + + // case 2 position_feedback = true + controller_->params_.position_feedback = true; + + // 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(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[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)); + + EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[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); + } +} + +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_library/test/test_steering_controllers_library.hpp b/steering_controllers_library/test/test_steering_controllers_library.hpp new file mode 100644 index 0000000000..23ab7b64f2 --- /dev/null +++ b/steering_controllers_library/test/test_steering_controllers_library.hpp @@ -0,0 +1,341 @@ +// 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 "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 "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 +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 +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; + +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; +constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR; +} // namespace +// namespace + +// subclassing and friending so we can access member variables +class TestableSteeringControllersLibrary +: public steering_controllers_library::SteeringControllersLibrary +{ + FRIEND_TEST(SteeringControllersLibraryTest, check_exported_intefaces); + FRIEND_TEST(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout); + +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); + } + + // 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); + odometry_.set_wheel_params(FRONT_WHEELS_RADIUS_, WHEELBASE_, REAR_WHEEL_TRACK_); + odometry_.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); + + return controller_interface::CallbackReturn::SUCCESS; + } + + bool update_odometry(const rclcpp::Duration & period) { return true; } + +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, 0, RCL_ROS_TIME), 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, 0, RCL_ROS_TIME), 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}; + + 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_ diff --git a/tricycle_steering_controller/CMakeLists.txt b/tricycle_steering_controller/CMakeLists.txt new file mode 100644 index 0000000000..c604b89452 --- /dev/null +++ b/tricycle_steering_controller/CMakeLists.txt @@ -0,0 +1,105 @@ +cmake_minimum_required(VERSION 3.16) +project(tricycle_steering_controller LANGUAGES CXX) + +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +set(THIS_PACKAGE_INCLUDE_DEPENDS + controller_interface + hardware_interface + generate_parameter_library + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + std_srvs + steering_controllers_library +) + +find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +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 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) + +if(BUILD_TESTING) + 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_load_tricycle_steering_controller + test/test_load_tricycle_steering_controller.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/tricycle_steering_controller_params.yaml + ) + 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() + +install( + DIRECTORY include/ + DESTINATION include/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..9a6adfca37 --- /dev/null +++ b/tricycle_steering_controller/doc/userdocs.rst @@ -0,0 +1,18 @@ +.. _tricycle_steering_controller_userdoc: + +tricycle_steering_controller +============================= + +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 `. + + +Parameters +,,,,,,,,,,, + +For list of parameters and their meaning YAML file in the ``src`` folder of the controller's package. + +For an exemplary 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 new file mode 100644 index 0000000000..607a684df5 --- /dev/null +++ b/tricycle_steering_controller/include/tricycle_steering_controller/tricycle_steering_controller.hpp @@ -0,0 +1,63 @@ +// 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. +// +// Authors: dr. sc. Tomislav Petkovic, Dr. Ing. Denis Štogl +// + +#ifndef TRICYCLE_STEERING_CONTROLLER__TRICYCLE_STEERING_CONTROLLER_HPP_ +#define TRICYCLE_STEERING_CONTROLLER__TRICYCLE_STEERING_CONTROLLER_HPP_ + +#include + +#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 +{ +// name constants for state interfaces +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 +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; +static constexpr size_t NR_REF_ITFS = 2; + +class TricycleSteeringController : public steering_controllers_library::SteeringControllersLibrary +{ +public: + TricycleSteeringController(); + + 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; + +protected: + std::shared_ptr tricycle_param_listener_; + tricycle_steering_controller::Params tricycle_params_; +}; +} // namespace tricycle_steering_controller + +#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..606b067ad8 --- /dev/null +++ b/tricycle_steering_controller/include/tricycle_steering_controller/visibility_control.h @@ -0,0 +1,52 @@ +// 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 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 new file mode 100644 index 0000000000..e911041cd1 --- /dev/null +++ b/tricycle_steering_controller/package.xml @@ -0,0 +1,38 @@ + + + + tricycle_steering_controller + 0.0.0 + Steering controller with tricycle kinematics. Rear fixed wheels are powering the vehicle and front wheel is steering. + 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 + + 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/tricycle_steering_controller/src/tricycle_steering_controller.cpp b/tricycle_steering_controller/src/tricycle_steering_controller.cpp new file mode 100644 index 0000000000..f89d78a52c --- /dev/null +++ b/tricycle_steering_controller/src/tricycle_steering_controller.cpp @@ -0,0 +1,93 @@ +// 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. + +#include "tricycle_steering_controller/tricycle_steering_controller.hpp" + +namespace tricycle_steering_controller +{ +TricycleSteeringController::TricycleSteeringController() +: steering_controllers_library::SteeringControllersLibrary() +{ +} +void TricycleSteeringController::initialize_implementation_parameter_listener() +{ + tricycle_param_listener_ = + std::make_shared(get_node()); +} + +controller_interface::CallbackReturn TricycleSteeringController::configure_odometry() +{ + 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; + + 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); + } + + odometry_.set_odometry_type(steering_odometry::TRICYCLE_CONFIG); + + 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; +} + +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_[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) && + !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) diff --git a/tricycle_steering_controller/src/tricycle_steering_controller.yaml b/tricycle_steering_controller/src/tricycle_steering_controller.yaml new file mode 100644 index 0000000000..1015865fd9 --- /dev/null +++ b/tricycle_steering_controller/src/tricycle_steering_controller.yaml @@ -0,0 +1,32 @@ +tricycle_steering_controller: + wheel_track: + { + type: double, + default_value: 0.0, + description: "Wheel track length. For details see: https://en.wikipedia.org/wiki/Wheelbase", + read_only: false, + } + + wheelbase: + { + type: double, + default_value: 0.0, + description: "Distance between front and rear wheels. For details see: https://en.wikipedia.org/wiki/Wheelbase", + 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/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..0d04afdf38 --- /dev/null +++ b/tricycle_steering_controller/test/test_load_tricycle_steering_controller.cpp @@ -0,0 +1,49 @@ +// 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(TestLoadTricycleSteeringController, load_controller) +{ + 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_NE( + cm.load_controller( + "test_tricycle_steering_controller", + "tricycle_steering_controller/TricycleSteeringController"), + nullptr); +} + +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.cpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp new file mode 100644 index 0000000000..82ba924305 --- /dev/null +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp @@ -0,0 +1,274 @@ +// 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_tricycle_steering_controller.hpp" + +#include +#include +#include +#include +#include + +class TricycleSteeringControllerTest +: public TricycleSteeringControllerFixture +{ +}; + +TEST_F(TricycleSteeringControllerTest, all_parameters_set_configure_success) +{ + SetUpController(); + + 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_EQ(controller_->tricycle_params_.wheel_track, wheel_track_); +} + +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()); + 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_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], + 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_AXIS], + controller_->front_wheels_state_names_[0] + "/" + steering_interface_name_); + + // check ref itfsTIME + 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) +{ + 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)); + 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(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, 0, RCL_ROS_TIME), 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_->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, 0, RCL_ROS_TIME), 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()); + + 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 + 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, 0, RCL_ROS_TIME), 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_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)); + } +} + +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, 0, RCL_ROS_TIME), 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(); + 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, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + ControllerStateMsg msg; + subscribe_and_get_messages(msg); + + 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)); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), 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); + + subscribe_and_get_messages(msg); + + EXPECT_NEAR( + 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); + EXPECT_NEAR(msg.steering_angle_command[0], 1.4179821977774734, COMMON_THRESHOLD); +} + +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..422f399ad4 --- /dev/null +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp @@ -0,0 +1,302 @@ +// 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_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" + +using ControllerStateMsg = + steering_controllers_library::SteeringControllersLibrary::AckermanControllerState; +using ControllerReferenceMsg = + steering_controllers_library::SteeringControllersLibrary::ControllerTwistReferenceMsg; + +// name constants for state interfaces +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_STEER_WHEEL; +using tricycle_steering_controller::CMD_TRACTION_LEFT_WHEEL; +using tricycle_steering_controller::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 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); + 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); + +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_twist_); + } + 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); + } + +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/reference", rclcpp::SystemDefaultsQoS()); + } + + 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); + + 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_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_AXIS])); + 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_tricycle_steering_controller/controller_state", 10, subs_callback); + + // call update to publish the test value + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), 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 message + wait_set.add_subscription(subscription); + while (max_sub_check_loop_count--) + { + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), 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_ = {"steering_axis_joint"}; + 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; + + double front_wheels_radius_ = 0.45; + double rear_wheels_radius_ = 0.45; + + 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"; + // 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_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..dd72332875 --- /dev/null +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller_preceeding.cpp @@ -0,0 +1,100 @@ +// 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_tricycle_steering_controller.hpp" + +#include +#include +#include +#include +#include + +class TricycleSteeringControllerTest +: public TricycleSteeringControllerFixture +{ +}; + +TEST_F(TricycleSteeringControllerTest, all_parameters_set_configure_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + 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) +{ + 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], + 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()); + 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_AXIS], + controller_->front_wheels_state_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]); + } +} + +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..6bfb87a892 --- /dev/null +++ b/tricycle_steering_controller/test/tricycle_steering_controller_params.yaml @@ -0,0 +1,16 @@ +test_tricycle_steering_controller: + ros__parameters: + + 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] + + wheelbase: 3.24644 + wheel_track: 1.212121 + front_wheels_radius: 0.45 + rear_wheels_radius: 0.45 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..ea8b88002e --- /dev/null +++ b/tricycle_steering_controller/test/tricycle_steering_controller_preceeding_params.yaml @@ -0,0 +1,16 @@ +test_tricycle_steering_controller: + ros__parameters: + 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 diff --git a/tricycle_steering_controller/tricycle_steering_controller.xml b/tricycle_steering_controller/tricycle_steering_controller.xml new file mode 100644 index 0000000000..f0d5d85467 --- /dev/null +++ b/tricycle_steering_controller/tricycle_steering_controller.xml @@ -0,0 +1,8 @@ + + + + Steering controller for Tricycle kinematics with two traction and one steering joint. + + +