From 7b04ea13cc80c54588863245313bb4dc1e07cb0d Mon Sep 17 00:00:00 2001 From: GiridharBukka Date: Fri, 6 Jan 2023 13:41:30 +0530 Subject: [PATCH 01/91] compiling without error --- custom_messages/CMakeLists.txt | 37 ++ .../msg/MecanumDriveControllerState.msg | 9 + custom_messages/package.xml | 24 + mecanum_drive_controller/CMakeLists.txt | 126 +++++ mecanum_drive_controller/LICENSE | 17 + .../mecanum_drive_controller.hpp | 133 ++++++ .../mecanum_drive_controller/odometry.hpp | 117 +++++ ...te_mecanum_drive_controller_parameters.hpp | 39 ++ .../visibility_control.h | 49 ++ .../mecanum_drive_controller.xml | 8 + mecanum_drive_controller/package.xml | 53 +++ .../src/mecanum_drive_controller.cpp | 438 +++++++++++++++++ .../src/mecanum_drive_controller.yaml | 100 ++++ mecanum_drive_controller/src/odometry.cpp | 90 ++++ .../test/mecanum_drive_controller_params.yaml | 30 ++ ...um_drive_controller_preceeding_params.yaml | 11 + .../test_load_mecanum_drive_controller.cpp | 41 ++ .../test/test_mecanum_drive_controller.cpp | 447 ++++++++++++++++++ .../test/test_mecanum_drive_controller.hpp | 283 +++++++++++ ...st_mecanum_drive_controller_preceeding.cpp | 85 ++++ 20 files changed, 2137 insertions(+) create mode 100644 custom_messages/CMakeLists.txt create mode 100644 custom_messages/msg/MecanumDriveControllerState.msg create mode 100644 custom_messages/package.xml create mode 100644 mecanum_drive_controller/CMakeLists.txt create mode 100644 mecanum_drive_controller/LICENSE create mode 100644 mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp create mode 100644 mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp create mode 100644 mecanum_drive_controller/include/mecanum_drive_controller/validate_mecanum_drive_controller_parameters.hpp create mode 100644 mecanum_drive_controller/include/mecanum_drive_controller/visibility_control.h create mode 100644 mecanum_drive_controller/mecanum_drive_controller.xml create mode 100644 mecanum_drive_controller/package.xml create mode 100644 mecanum_drive_controller/src/mecanum_drive_controller.cpp create mode 100644 mecanum_drive_controller/src/mecanum_drive_controller.yaml create mode 100644 mecanum_drive_controller/src/odometry.cpp create mode 100644 mecanum_drive_controller/test/mecanum_drive_controller_params.yaml create mode 100644 mecanum_drive_controller/test/mecanum_drive_controller_preceeding_params.yaml create mode 100644 mecanum_drive_controller/test/test_load_mecanum_drive_controller.cpp create mode 100644 mecanum_drive_controller/test/test_mecanum_drive_controller.cpp create mode 100644 mecanum_drive_controller/test/test_mecanum_drive_controller.hpp create mode 100644 mecanum_drive_controller/test/test_mecanum_drive_controller_preceeding.cpp diff --git a/custom_messages/CMakeLists.txt b/custom_messages/CMakeLists.txt new file mode 100644 index 0000000000..7b0dde3686 --- /dev/null +++ b/custom_messages/CMakeLists.txt @@ -0,0 +1,37 @@ +cmake_minimum_required(VERSION 3.8) +project(custom_messages) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(std_msgs REQUIRED) +find_package(nav_msgs REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/MecanumDriveControllerState.msg" + DEPENDENCIES + std_msgs + nav_msgs + ) + + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/custom_messages/msg/MecanumDriveControllerState.msg b/custom_messages/msg/MecanumDriveControllerState.msg new file mode 100644 index 0000000000..f01a67ecb0 --- /dev/null +++ b/custom_messages/msg/MecanumDriveControllerState.msg @@ -0,0 +1,9 @@ +std_msgs/Header header +nav_msgs/Odometry odom +float64 front_left_wheel_velocity +float64 back_left_wheel_velocity +float64 back_right_wheel_velocity +float64 front_right_wheel_velocity +float64 linear_x_velocity_command +float64 linear_y_velocity_command +float64 angular_z_command \ No newline at end of file diff --git a/custom_messages/package.xml b/custom_messages/package.xml new file mode 100644 index 0000000000..61b26ca04a --- /dev/null +++ b/custom_messages/package.xml @@ -0,0 +1,24 @@ + + + + custom_messages + 0.0.0 + TODO: Package description + giridhar + TODO: License declaration + + ament_cmake + + ament_lint_auto + ament_lint_common + rosidl_default_generators + std_msgs + nav_msgs + + rosidl_default_runtime + +rosidl_interface_packages + + ament_cmake + + diff --git a/mecanum_drive_controller/CMakeLists.txt b/mecanum_drive_controller/CMakeLists.txt new file mode 100644 index 0000000000..62da8f9088 --- /dev/null +++ b/mecanum_drive_controller/CMakeLists.txt @@ -0,0 +1,126 @@ +cmake_minimum_required(VERSION 3.8) +project(mecanum_drive_controller) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +set(THIS_PACKAGE_INCLUDE_DEPENDS + control_msgs + controller_interface + geometry_msgs + hardware_interface + nav_msgs + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + std_srvs + tf2 + tf2_msgs + tf2_geometry_msgs + custom_messages +) + +find_package(ament_cmake REQUIRED) +find_package(generate_parameter_library REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +cmake_minimum_required(VERSION 3.8) +project(mecanum_drive_controller) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) + +# Add mecanum_drive_controller library related compile commands +generate_parameter_library(mecanum_drive_controller_parameters + src/mecanum_drive_controller.yaml + include/mecanum_drive_controller/validate_mecanum_drive_controller_parameters.hpp +) +add_library( + mecanum_drive_controller + SHARED + src/mecanum_drive_controller.cpp + src/odometry.cpp +) +target_include_directories(mecanum_drive_controller PRIVATE include) +target_link_libraries(mecanum_drive_controller mecanum_drive_controller_parameters) +ament_target_dependencies(mecanum_drive_controller ${THIS_PACKAGE_INCLUDE_DEPENDS}) +target_compile_definitions(mecanum_drive_controller PRIVATE "MECANUM_DRIVE_CONTROLLER_BUILDING_DLL") + +pluginlib_export_plugin_description_file( + controller_interface mecanum_drive_controller.xml) + +install( + TARGETS + mecanum_drive_controller + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib +) + +install( + DIRECTORY include/ + DESTINATION include +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + find_package(ament_cmake_gmock REQUIRED) + find_package(controller_manager REQUIRED) + find_package(hardware_interface REQUIRED) + find_package(ros2_control_test_assets REQUIRED) + + ament_add_gmock(test_load_mecanum_drive_controller test/test_load_mecanum_drive_controller.cpp) + target_include_directories(test_load_mecanum_drive_controller PRIVATE include) + ament_target_dependencies( + test_load_mecanum_drive_controller + controller_manager + hardware_interface + ros2_control_test_assets + ) + + add_rostest_with_parameters_gmock(test_mecanum_drive_controller test/test_mecanum_drive_controller.cpp ${CMAKE_CURRENT_SOURCE_DIR}/test/mecanum_drive_controller_params.yaml) + target_include_directories(test_mecanum_drive_controller PRIVATE include) + target_link_libraries(test_mecanum_drive_controller mecanum_drive_controller) + ament_target_dependencies( + test_mecanum_drive_controller + controller_interface + hardware_interface + ) + + # add_rostest_with_parameters_gmock(test_mecanum_drive_controller_preceeding test/test_mecanum_drive_controller_preceeding.cpp ${CMAKE_CURRENT_SOURCE_DIR}/test/mecanum_drive_controller_preceeding_params.yaml) + # target_include_directories(test_mecanum_drive_controller_preceeding PRIVATE include) + # target_link_libraries(test_mecanum_drive_controller_preceeding mecanum_drive_controller) + # ament_target_dependencies( + # test_mecanum_drive_controller_preceeding + # controller_interface + # hardware_interface + # ) +endif() + +ament_export_include_directories( + include +) +ament_export_dependencies( + +) +ament_export_libraries( + mecanum_drive_controller +) + +ament_package() \ No newline at end of file diff --git a/mecanum_drive_controller/LICENSE b/mecanum_drive_controller/LICENSE new file mode 100644 index 0000000000..30e8e2ece5 --- /dev/null +++ b/mecanum_drive_controller/LICENSE @@ -0,0 +1,17 @@ +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp new file mode 100644 index 0000000000..8fb65940b7 --- /dev/null +++ b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp @@ -0,0 +1,133 @@ +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef MECANUM_DRIVE_CONTROLLER__MECANUM_DRIVE_CONTROLLER_HPP_ +#define MECANUM_DRIVE_CONTROLLER__MECANUM_DRIVE_CONTROLLER_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "mecanum_drive_controller/odometry.hpp" +#include "controller_interface/chainable_controller_interface.hpp" +#include "mecanum_drive_controller_parameters.hpp" +#include "mecanum_drive_controller/visibility_control.h" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include "realtime_tools/realtime_buffer.h" +#include "realtime_tools/realtime_publisher.h" +#include "std_srvs/srv/set_bool.hpp" + +// TODO(anyone): Replace with controller specific messages +#include "custom_messages/msg/mecanum_drive_controller_state.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "tf2_msgs/msg/tf_message.hpp" +namespace mecanum_drive_controller +{ +// name constants for state interfaces +static constexpr size_t NR_STATE_ITFS = 4; + +// name constants for command interfaces +static constexpr size_t NR_CMD_ITFS = 4; + +// name constants for reference interfaces +static constexpr size_t NR_REF_ITFS = 3; + + +class MecanumDriveController : public controller_interface::ChainableControllerInterface +{ +public: + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + MecanumDriveController(); + + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_init() override; + + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + controller_interface::return_type update_reference_from_subscribers( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + controller_interface::return_type update_and_write_commands( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + // TODO(anyone): replace the state and command message types + using ControllerReferenceMsg = geometry_msgs::msg::TwistStamped; + using OdomStateMsg = nav_msgs::msg::Odometry; + using TfStateMsg = tf2_msgs::msg::TFMessage; + using ControllerStateMsg = custom_messages::msg::MecanumDriveControllerState; + + +protected: + std::shared_ptr param_listener_; + mecanum_drive_controller::Params params_; + + // Command subscribers and Controller State publisher + rclcpp::Subscription::SharedPtr ref_subscriber_ = nullptr; + realtime_tools::RealtimeBuffer> input_ref_; + rclcpp::Duration ref_timeout_ = rclcpp::Duration::from_seconds(0.0); // 0ms + + using OdomStatePublisher = realtime_tools::RealtimePublisher; + rclcpp::Publisher::SharedPtr odom_s_publisher_; + std::unique_ptr rt_odom_state_publisher_; + + + using TfStatePublisher = realtime_tools::RealtimePublisher; + rclcpp::Publisher::SharedPtr tf_odom_s_publisher_; + std::unique_ptr rt_tf_odom_state_publisher_; + + using ControllerStatePublisher = realtime_tools::RealtimePublisher; + rclcpp::Publisher::SharedPtr controller_s_publisher_; + std::unique_ptr controller_state_publisher_; + + // override methods from ChainableControllerInterface + std::vector on_export_reference_interfaces() override; + + bool on_set_chained_mode(bool chained_mode) override; + + // Odometry related: + Odometry odometry_; + +private: + // callback for topic interface + TEMPLATES__ROS2_CONTROL__VISIBILITY_LOCAL + void reference_callback(const std::shared_ptr msg); +}; + +} // namespace mecanum_drive_controller + +#endif // MECANUM_DRIVE_CONTROLLER__MECANUM_DRIVE_CONTROLLER_HPP_ diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp b/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp new file mode 100644 index 0000000000..661988433c --- /dev/null +++ b/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp @@ -0,0 +1,117 @@ +#ifndef ODOMETRY_H_ +#define ODOMETRY_H_ + +// #include +#include "realtime_tools/realtime_buffer.h" +#include "realtime_tools/realtime_publisher.h" + +#include +#include +#include +#include + +#define PLANAR_POINT_DIM 3 + +namespace mecanum_drive_controller +{ +namespace bacc = boost::accumulators; + +/// \brief The Odometry class handles odometry readings +/// (2D pose and velocity with related timestamp) +class Odometry +{ +public: + /// Integration function, used to integrate the odometry: + typedef boost::function IntegrationFunction; + + /// \brief Constructor + /// Timestamp will get the current time value + /// Value will be set to zero + /// \param velocity_rolling_window_size Rolling window size used to compute the velocity mean + Odometry(size_t velocity_rolling_window_size = 10); + + /// \brief Initialize the odometry + /// \param time Current time + void init(const rclcpp::Time & time, double base_frame_offset[PLANAR_POINT_DIM]); + + /// \brief Updates the odometry class with latest wheels position + /// \param wheel0_vel Wheel velocity [rad/s] + /// \param wheel1_vel Wheel velocity [rad/s] + /// \param wheel2_vel Wheel velocity [rad/s] + /// \param wheel3_vel Wheel velocity [rad/s] + /// \param time Current time + /// \return true if the odometry is actually updated + bool update(double wheel0_vel, double wheel1_vel, double wheel2_vel, double wheel3_vel, const double dt); + + /// \return position (x component) [m] + double getX() const + { + return px_b_b0_; + } + /// \return position (y component) [m] + double getY() const + { + return py_b_b0_; + } + /// \return orientation (z component) [m] + double getRz() const + { + return rz_b_b0_; + } + /// \return body velocity of the base frame (linear x component) [m/s] + double getVx() const + { + return vx_Ob_b_b0_b_; + } + /// \return body velocity of the base frame (linear y component) [m/s] + double getVy() const + { + return vy_Ob_b_b0_b_; + } + /// \return body velocity of the base frame (angular z component) [m/s] + double getWz() const + { + return wz_b_b0_b_; + } + + /// \brief Sets the wheels parameters: mecanum geometric param and radius + /// \param wheels_k Wheels geometric param (used in mecanum wheels' ik) [m] + /// \param wheels_radius Wheels radius [m] + void setWheelsParams(double wheels_k, double wheels_radius); + +private: + /// Rolling mean accumulator and window: + typedef bacc::accumulator_set > RollingMeanAcc; + typedef bacc::tag::rolling_window RollingWindow; + + /// Current timestamp: + rclcpp::Time timestamp_; + + /// Reference frame (wrt to center frame). + double base_frame_offset_[PLANAR_POINT_DIM]; + + /// Current pose: + double px_b_b0_; // [m] + double py_b_b0_; // [m] + double rz_b_b0_; // [rad] + + /// Current velocity. + /// \note The indices meaning is the following: + /// b : base frame + /// c : center frame + /// Ob: origin of the base frame + /// Oc: origin of the center frame + /// b0: initial position if the base frame + /// c0: initial position of the center frame + double vx_Ob_b_b0_b_; // [m/s] + double vy_Ob_b_b0_b_; // [m/s] + double wz_b_b0_b_; // [rad/s] + + /// Wheels kinematic parameters [m]: + double wheels_k_; + double wheels_radius_; +}; + +} // namespace mecanum_drive_controller + +#endif /* ODOMETRY_H_ */ diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/validate_mecanum_drive_controller_parameters.hpp b/mecanum_drive_controller/include/mecanum_drive_controller/validate_mecanum_drive_controller_parameters.hpp new file mode 100644 index 0000000000..2f86a8507f --- /dev/null +++ b/mecanum_drive_controller/include/mecanum_drive_controller/validate_mecanum_drive_controller_parameters.hpp @@ -0,0 +1,39 @@ +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef MECANUM_DRIVE_CONTROLLER__VALIDATE_MECANUM_DRIVE_CONTROLLER_PARAMETERS_HPP_ +#define MECANUM_DRIVE_CONTROLLER__VALIDATE_MECANUM_DRIVE_CONTROLLER_PARAMETERS_HPP_ + +#include + +#include "parameter_traits/parameter_traits.hpp" +#include "rclcpp/parameter.hpp" +#include "tl_expected/expected.hpp" + +namespace parameter_traits +{ +tl::expected forbidden_interface_name_prefix(rclcpp::Parameter const & parameter) +{ + auto const & interface_name = parameter.as_string(); + + if (interface_name.rfind("blup_", 0) == 0) { + return tl::make_unexpected("'interface_name' parameter can not start with 'blup_'"); + } + + return {}; +} + +} // namespace parameter_traits + +#endif // MECANUM_DRIVE_CONTROLLER__VALIDATE_MECANUM_DRIVE_CONTROLLER_PARAMETERS_HPP_ diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/visibility_control.h b/mecanum_drive_controller/include/mecanum_drive_controller/visibility_control.h new file mode 100644 index 0000000000..cff3f4dff4 --- /dev/null +++ b/mecanum_drive_controller/include/mecanum_drive_controller/visibility_control.h @@ -0,0 +1,49 @@ +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef MECANUM_DRIVE_CONTROLLER__VISIBILITY_CONTROL_H_ +#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_EXPORT __attribute__((dllexport)) +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_IMPORT __attribute__((dllimport)) +#else +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_EXPORT __declspec(dllexport) +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_IMPORT __declspec(dllimport) +#endif +#ifdef TEMPLATES__ROS2_CONTROL__VISIBILITY_BUILDING_DLL +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC TEMPLATES__ROS2_CONTROL__VISIBILITY_EXPORT +#else +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC TEMPLATES__ROS2_CONTROL__VISIBILITY_IMPORT +#endif +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC_TYPE TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_LOCAL +#else +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_EXPORT __attribute__((visibility("default"))) +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_IMPORT +#if __GNUC__ >= 4 +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC __attribute__((visibility("default"))) +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_LOCAL __attribute__((visibility("hidden"))) +#else +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_LOCAL +#endif +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC_TYPE +#endif + +#endif // MECANUM_DRIVE_CONTROLLER__VISIBILITY_CONTROL_H_ diff --git a/mecanum_drive_controller/mecanum_drive_controller.xml b/mecanum_drive_controller/mecanum_drive_controller.xml new file mode 100644 index 0000000000..4754a6c1fa --- /dev/null +++ b/mecanum_drive_controller/mecanum_drive_controller.xml @@ -0,0 +1,8 @@ + + + + MecanumDriveController ros2_control controller. + + + diff --git a/mecanum_drive_controller/package.xml b/mecanum_drive_controller/package.xml new file mode 100644 index 0000000000..5abb337f43 --- /dev/null +++ b/mecanum_drive_controller/package.xml @@ -0,0 +1,53 @@ + + + + mecanum_drive_controller + 0.0.0 + DESCRIPTION + giridhar + MIT + + ament_cmake + + generate_parameter_library + + control_msgs + + controller_interface + + geometry_msgs + + hardware_interface + + nav_msgs + + pluginlib + + rclcpp + + rclcpp_lifecycle + + realtime_tools + + rcpputils + + tf2 + + tf2_msgs + + tf2_geometry_msgs + + std_srvs + + custom_messages + + ament_lint_auto + ament_cmake_gmock + controller_manager + hardware_interface + ros2_control_test_assets + + + ament_cmake + + diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp new file mode 100644 index 0000000000..0e6ae85ede --- /dev/null +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -0,0 +1,438 @@ +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) +// (template) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "mecanum_drive_controller/mecanum_drive_controller.hpp" + +#include +#include +#include +#include + +#include "controller_interface/helpers.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "tf2/transform_datatypes.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" + +namespace { // utility + +// TODO(destogl): remove this when merged upstream +// Changed services history QoS to keep all so we don't lose any client service +// calls +static constexpr rmw_qos_profile_t rmw_qos_profile_services_hist_keep_all = { + RMW_QOS_POLICY_HISTORY_KEEP_ALL, + 1, // message queue depth + RMW_QOS_POLICY_RELIABILITY_RELIABLE, + RMW_QOS_POLICY_DURABILITY_VOLATILE, + RMW_QOS_DEADLINE_DEFAULT, + RMW_QOS_LIFESPAN_DEFAULT, + RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT, + RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT, + false}; + +using ControllerReferenceMsg = + mecanum_drive_controller::MecanumDriveController::ControllerReferenceMsg; + +// called from RT control loop +void reset_controller_reference_msg( + const std::shared_ptr& msg, + const std::shared_ptr& node) { + + msg->header.stamp = node->now(); + msg->twist.linear.x = std::numeric_limits::quiet_NaN(); + msg->twist.linear.y = std::numeric_limits::quiet_NaN(); + msg->twist.linear.z = std::numeric_limits::quiet_NaN(); + msg->twist.angular.x = std::numeric_limits::quiet_NaN(); + msg->twist.angular.y = std::numeric_limits::quiet_NaN(); + msg->twist.angular.z = std::numeric_limits::quiet_NaN(); +} + +} // namespace + +namespace mecanum_drive_controller { +MecanumDriveController::MecanumDriveController() + : controller_interface::ChainableControllerInterface() {} + +controller_interface::CallbackReturn MecanumDriveController::on_init() { + try { + param_listener_ = + std::make_shared(get_node()); + } catch (const std::exception& e) { + fprintf(stderr, + "Exception thrown during controller's init with message: %s \n", + e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn MecanumDriveController::on_configure( + const rclcpp_lifecycle::State& /*previous_state*/) { + fprintf(stderr," here_ on_configure _1_199"); + params_ = param_listener_->get_params(); + + fprintf(stderr," here_ on_configure _1_2"); + // Set wheel params for the odometry computation + odometry_.setWheelsParams(params_.wheels_k, params_.wheels_radius); + + // 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); + ref_subscriber_ = get_node()->create_subscription( + "~/reference", subscribers_qos, + std::bind(&MecanumDriveController::reference_callback, this, + std::placeholders::_1)); + + std::shared_ptr msg = + std::make_shared(); + reset_controller_reference_msg(msg,get_node()); + input_ref_.writeFromNonRT(msg); + + try + { + // Odom state publisher + odom_s_publisher_ = get_node()->create_publisher( + "~/odometry", rclcpp::SystemDefaultsQoS()); + rt_odom_state_publisher_ = std::make_unique(odom_s_publisher_); + } + catch (const std::exception & e) + { + fprintf( + stderr, "Exception thrown during publisher creation at configure stage with message : %s \n", + e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + // TODO(anyone): Reserve memory in state publisher depending on the message type + rt_odom_state_publisher_->lock(); + rt_odom_state_publisher_->msg_.header.stamp = get_node()->now(); + rt_odom_state_publisher_->msg_.header.frame_id = params_.odom_frame_id; + rt_odom_state_publisher_->msg_.child_frame_id = params_.base_frame_id; + rt_odom_state_publisher_->msg_.pose.pose.position.z = 0; + + auto & covariance = rt_odom_state_publisher_->msg_.twist.covariance; + constexpr size_t NUM_DIMENSIONS = 6; + for (size_t index = 0; index < 6; ++index) + { + // 0, 7, 14, 21, 28, 35 + const size_t diagonal_index = NUM_DIMENSIONS * index + index; + covariance[diagonal_index] = params_.pose_covariance_diagonal[index]; + covariance[diagonal_index] = params_.twist_covariance_diagonal[index]; + } + rt_odom_state_publisher_->unlock(); + + try + { + // Tf State publisher + tf_odom_s_publisher_ = get_node()->create_publisher( + "~/tf_odometry", rclcpp::SystemDefaultsQoS()); + rt_tf_odom_state_publisher_ = + std::make_unique(tf_odom_s_publisher_); + } + catch (const std::exception & e) + { + fprintf( + stderr, "Exception thrown during publisher creation at configure stage with message : %s \n", + e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + rt_tf_odom_state_publisher_->lock(); + rt_tf_odom_state_publisher_->msg_.transforms.resize(1); + rt_tf_odom_state_publisher_->msg_.transforms[0].header.stamp = get_node()->now(); + rt_tf_odom_state_publisher_->msg_.transforms[0].header.frame_id = params_.odom_frame_id; + rt_tf_odom_state_publisher_->msg_.transforms[0].child_frame_id = params_.base_frame_id; + rt_tf_odom_state_publisher_->msg_.transforms[0].transform.translation.z = 0.0; + rt_tf_odom_state_publisher_->unlock(); + + + try { + // controller State publisher + controller_s_publisher_ = get_node()->create_publisher( + "~/controller_state", rclcpp::SystemDefaultsQoS()); + controller_state_publisher_ = std::make_unique(controller_s_publisher_); + } catch (const std::exception& e) { + fprintf(stderr, + "Exception thrown during publisher creation at configure stage " + "with message : %s \n", + e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + // // TODO(anyone): Reserve memory in state publisher depending on the message + // // type + + controller_state_publisher_->lock(); + controller_state_publisher_->msg_.header.stamp = get_node()->now(); + controller_state_publisher_->msg_.header.frame_id = params_.odom_frame_id; + controller_state_publisher_->msg_.odom.pose.pose.position.z = 0; + controller_state_publisher_->msg_.odom.child_frame_id = params_.base_frame_id; + controller_state_publisher_->unlock(); + auto & covariance_controller = controller_state_publisher_->msg_.odom.twist.covariance; + for (size_t index = 0; index < 6; ++index) + { + // 0, 7, 14, 21, 28, 35 + const size_t diagonal_index = NUM_DIMENSIONS * index + index; + covariance_controller[diagonal_index] = params_.pose_covariance_diagonal[index]; + covariance_controller[diagonal_index] = params_.twist_covariance_diagonal[index]; + } + + RCLCPP_INFO(get_node()->get_logger(), "configure successful"); + return controller_interface::CallbackReturn::SUCCESS; +} + +void MecanumDriveController::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()); + } +} + +controller_interface::InterfaceConfiguration +MecanumDriveController::command_interface_configuration() const { + + controller_interface::InterfaceConfiguration command_interfaces_config; + command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + command_interfaces_config.names.reserve(NR_CMD_ITFS); + command_interfaces_config.names.push_back( + params_.wheel0_name + "/" + hardware_interface::HW_IF_VELOCITY); + command_interfaces_config.names.push_back( + params_.wheel1_name + "/" + hardware_interface::HW_IF_VELOCITY); + command_interfaces_config.names.push_back( + params_.wheel2_name + "/" + hardware_interface::HW_IF_VELOCITY); + command_interfaces_config.names.push_back( + params_.wheel3_name + "/" + hardware_interface::HW_IF_VELOCITY); + + + return command_interfaces_config; +} + +controller_interface::InterfaceConfiguration +MecanumDriveController::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); + state_interfaces_config.names.push_back(params_.wheel0_name + "/" + hardware_interface::HW_IF_VELOCITY); + state_interfaces_config.names.push_back(params_.wheel1_name + "/" + hardware_interface::HW_IF_VELOCITY); + state_interfaces_config.names.push_back(params_.wheel2_name + "/" + hardware_interface::HW_IF_VELOCITY); + state_interfaces_config.names.push_back(params_.wheel3_name + "/" + hardware_interface::HW_IF_VELOCITY); + + return state_interfaces_config; +} + +std::vector +MecanumDriveController::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_x/") + hardware_interface::HW_IF_VELOCITY, + &reference_interfaces_[0])); + + reference_interfaces.push_back(hardware_interface::CommandInterface( + get_node()->get_name(), std::string("linear_y/") + hardware_interface::HW_IF_VELOCITY, + &reference_interfaces_[1])); + + reference_interfaces.push_back(hardware_interface::CommandInterface( + get_node()->get_name(), std::string("angular_z/") + hardware_interface::HW_IF_VELOCITY, + &reference_interfaces_[2])); + + return reference_interfaces; +} + +bool MecanumDriveController::on_set_chained_mode(bool chained_mode) { + // Always accept switch to/from chained mode + return true || chained_mode; +} + +controller_interface::CallbackReturn MecanumDriveController::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 MecanumDriveController::on_deactivate( + const rclcpp_lifecycle::State& /*previous_state*/) { + // TODO(anyone): depending on number of interfaces, use definitions, e.g., + // `CMD_MY_ITFS`, instead of a loop + for (size_t i = 0; i < NR_CMD_ITFS; ++i) { + command_interfaces_[i].set_value(std::numeric_limits::quiet_NaN()); + } + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::return_type +MecanumDriveController::update_reference_from_subscribers( + const rclcpp::Time& time, const rclcpp::Duration& /*period*/) { + auto current_ref = *(input_ref_.readFromRT()); + const auto age_of_last_command = get_node()->now() - (current_ref)->header.stamp; + + // TODO(anyone): depending on number of interfaces, use definitions, e.g., `CMD_MY_ITFS`, + // instead of a loop + // send message only if there is no timeout + if (age_of_last_command <= ref_timeout_ || ref_timeout_ == rclcpp::Duration::from_seconds(0)) + { + if (!std::isnan(current_ref->twist.linear.x) && !std::isnan(current_ref->twist.angular.z)) + { + reference_interfaces_[0] = current_ref->twist.linear.x; + reference_interfaces_[1] = current_ref->twist.linear.y; + reference_interfaces_[2] = current_ref->twist.angular.z; + } + } + else + { + reference_interfaces_[0] = std::numeric_limits::quiet_NaN(); + reference_interfaces_[1] = std::numeric_limits::quiet_NaN(); + reference_interfaces_[2] = std::numeric_limits::quiet_NaN(); + } + + return controller_interface::return_type::OK; +} + +controller_interface::return_type MecanumDriveController::update_and_write_commands( + const rclcpp::Time& time, const rclcpp::Duration& period) { + + // FORWARD KINEMATICS (odometry). + double wheel0_vel = state_interfaces_[0].get_value(); + double wheel1_vel = state_interfaces_[1].get_value(); + double wheel2_vel = state_interfaces_[2].get_value(); + double wheel3_vel = state_interfaces_[3].get_value(); + + if (!std::isnan(wheel0_vel) && !std::isnan(wheel1_vel) && !std::isnan(wheel2_vel) && !std::isnan(wheel3_vel)){ + + // Estimate twist (using joint information) and integrate + odometry_.update(wheel0_vel, wheel1_vel, wheel2_vel, wheel3_vel, period.seconds()); + } + + + + // Publish odometry message + // Compute and store orientation info + tf2::Quaternion orientation; + orientation.setRPY(0.0, 0.0, odometry_.getRz()); + + // Populate odom message and publish + if (rt_odom_state_publisher_->trylock()) + { + rt_odom_state_publisher_->msg_.header.stamp = time; + rt_odom_state_publisher_->msg_.pose.pose.position.x = odometry_.getX(); + rt_odom_state_publisher_->msg_.pose.pose.position.y = odometry_.getY(); + rt_odom_state_publisher_->msg_.pose.pose.orientation = tf2::toMsg(orientation); + rt_odom_state_publisher_->msg_.twist.twist.linear.x = odometry_.getVx(); + rt_odom_state_publisher_->msg_.twist.twist.linear.y = odometry_.getVy(); + rt_odom_state_publisher_->msg_.twist.twist.angular.z = odometry_.getWz(); + rt_odom_state_publisher_->unlockAndPublish(); + } + + // Publish tf /odom frame + if (params_.enable_odom_tf && rt_tf_odom_state_publisher_->trylock()) + { + rt_tf_odom_state_publisher_->msg_.transforms.front().header.stamp = time; + rt_tf_odom_state_publisher_->msg_.transforms.front().transform.translation.x = odometry_.getX(); + rt_tf_odom_state_publisher_->msg_.transforms.front().transform.translation.y = odometry_.getY(); + rt_tf_odom_state_publisher_->msg_.transforms.front().transform.rotation = + tf2::toMsg(orientation); + rt_tf_odom_state_publisher_->unlockAndPublish(); + } + + if (controller_state_publisher_->trylock()) + { + controller_state_publisher_->msg_.header.stamp = get_node()->now(); + controller_state_publisher_->msg_.odom.pose.pose.position.x = odometry_.getX(); + controller_state_publisher_->msg_.odom.pose.pose.position.y = odometry_.getY(); + controller_state_publisher_->msg_.odom.pose.pose.orientation = tf2::toMsg(orientation); + controller_state_publisher_->msg_.odom.twist.twist.linear.x = odometry_.getVx(); + controller_state_publisher_->msg_.odom.twist.twist.linear.y = odometry_.getVy(); + controller_state_publisher_->msg_.odom.twist.twist.angular.z = odometry_.getWz(); + controller_state_publisher_->msg_.front_left_wheel_velocity = state_interfaces_[0].get_value(); + controller_state_publisher_->msg_.back_left_wheel_velocity = state_interfaces_[1].get_value(); + controller_state_publisher_->msg_.back_right_wheel_velocity = state_interfaces_[2].get_value(); + controller_state_publisher_->msg_.front_right_wheel_velocity = state_interfaces_[3].get_value(); + controller_state_publisher_->msg_.linear_x_velocity_command = reference_interfaces_[0]; + controller_state_publisher_->msg_.linear_y_velocity_command = reference_interfaces_[1]; + controller_state_publisher_->msg_.angular_z_command = reference_interfaces_[2]; + + controller_state_publisher_->unlockAndPublish(); + } + + // INVERSE KINEMATICS (move robot). + // Compute wheels velocities (this is the actual ik): + // NOTE: the input desired twist (from topic /cmd_vel) is a body twist. + + tf2::Quaternion orientation_2; + orientation_2.setRPY(0.0, 0.0, params_.base_frame_offset[2]); + + tf2::Matrix3x3 R_b_c = tf2::Matrix3x3((orientation_2)); + tf2::Vector3 v_Ob_b_b0_c = R_b_c * tf2::Vector3(reference_interfaces_[0], reference_interfaces_[1], 0.0); + tf2::Vector3 Ob_c = tf2::Vector3(params_.base_frame_offset[0], params_.base_frame_offset[1], 0.0); + + double vx_Oc_c_c0_c_ = v_Ob_b_b0_c.x() + Ob_c.y() * reference_interfaces_[2]; + double vy_Oc_c_c0_c_ = v_Ob_b_b0_c.y() - Ob_c.x() * reference_interfaces_[2]; + double wz_c_c0_c_ = reference_interfaces_[2]; + + double w0_vel = 1.0 / params_.wheels_radius * (vx_Oc_c_c0_c_ - vy_Oc_c_c0_c_ - params_.wheels_k * wz_c_c0_c_); + double w1_vel = 1.0 / params_.wheels_radius * (vx_Oc_c_c0_c_ + vy_Oc_c_c0_c_ - params_.wheels_k * wz_c_c0_c_); + double w2_vel = 1.0 / params_.wheels_radius * (vx_Oc_c_c0_c_ - vy_Oc_c_c0_c_ + params_.wheels_k * wz_c_c0_c_); + double w3_vel = 1.0 / params_.wheels_radius * (vx_Oc_c_c0_c_ + vy_Oc_c_c0_c_ + params_.wheels_k * wz_c_c0_c_); + + // Set wheels velocities: + + command_interfaces_[0].set_value(w0_vel); + command_interfaces_[1].set_value(w1_vel); + command_interfaces_[2].set_value(w2_vel); + command_interfaces_[3].set_value(w3_vel); + + return controller_interface::return_type::OK; + +} + +} // namespace mecanum_drive_controller + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS(mecanum_drive_controller::MecanumDriveController, + controller_interface::ChainableControllerInterface) diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.yaml b/mecanum_drive_controller/src/mecanum_drive_controller.yaml new file mode 100644 index 0000000000..9b10f0da8b --- /dev/null +++ b/mecanum_drive_controller/src/mecanum_drive_controller.yaml @@ -0,0 +1,100 @@ +mecanum_drive_controller: + reference_timeout: { + type: double, + default_value: 0.0, + description: "Timeout for controller references after which they will be reset. This is especially useful for controllers that can cause unwanted and dangerous behaviour if reference is not reset, e.g., velocity controllers. If value is 0 the reference is reset after each run.", + + } + wheel0_name: { + type: string, + default_value: "front_left_wheel_joint", + description: "Name of the front left wheel.", + read_only: true, + + } + wheel1_name: { + type: string, + default_value: "back_left_wheel_joint", + description: "Name of the back left wheel.", + read_only: true, + + } + wheel2_name: { + type: string, + default_value: "back_right_wheel_joint", + description: "Name of the back right wheel.", + read_only: true, + + } + wheel3_name: { + type: string, + default_value: "front_right_wheel_joint", + description: "Name of the front right wheel.", + read_only: true, + + } + 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, + + } + base_frame_offset: { + type: double_array, + default_value: [0.0, 0.0, 0.0], + description: "base frame offset values, [base_frame_offset/x base_frame_offset/y base_frame_offset/theta].", + read_only: false, + + } + wheels_radius: { + type: double, + default_value: 0.0, + description: "Wheel's radius.", + read_only: false, + + } + wheels_k: { + type: double, + default_value: 0.0, + description: "wheels geometric param used in mecanum wheels' ik.", + read_only: false, + + } + use_realigned_roller_joints: { + type: bool, + default_value: false, + description: "This mode is used when the mecanum wheels are simulated and we use realigned rollers to mimic mecanum wheels..", + 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, + + } \ No newline at end of file diff --git a/mecanum_drive_controller/src/odometry.cpp b/mecanum_drive_controller/src/odometry.cpp new file mode 100644 index 0000000000..5339ca34aa --- /dev/null +++ b/mecanum_drive_controller/src/odometry.cpp @@ -0,0 +1,90 @@ +#include "mecanum_drive_controller/odometry.hpp" + +#include +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" + + +#include + +namespace mecanum_drive_controller +{ +namespace bacc = boost::accumulators; + +Odometry::Odometry(size_t velocity_rolling_window_size) + : timestamp_(0.0) + , px_b_b0_(0.0) + , py_b_b0_(0.0) + , rz_b_b0_(0.0) + , vx_Ob_b_b0_b_(0.0) + , vy_Ob_b_b0_b_(0.0) + , wz_b_b0_b_(0.0) + , wheels_k_(0.0) + , wheels_radius_(0.0) +{ +} + +void Odometry::init(const rclcpp::Time& time, double base_frame_offset[PLANAR_POINT_DIM]) +{ + // Reset timestamp: + timestamp_ = time; + + // Base frame offset (wrt to center frame). + base_frame_offset_[0] = base_frame_offset[0]; + base_frame_offset_[1] = base_frame_offset[1]; + base_frame_offset_[2] = base_frame_offset[2]; +} + +bool Odometry::update(double wheel0_vel, double wheel1_vel, double wheel2_vel, double wheel3_vel, const double dt) +{ + /// We cannot estimate the speed with very small time intervals: + // const double dt = (time - timestamp_).toSec(); + if (dt < 0.0001) + return false; // Interval too small to integrate with + + // timestamp_ = time; + + /// Compute FK (i.e. compute mobile robot's body twist out of its wheels velocities): + /// NOTE: we use the IK of the mecanum wheels which we invert using a pseudo-inverse. + /// NOTE: the mecanum IK gives the body speed at the center frame, we then offset this velocity at the base frame. + /// NOTE: in the diff drive the velocity is filtered out, but we prefer to return it raw and let the user perform + /// post-processing at will. We prefer this way of doing as filtering introduces delay (which makes it + /// difficult to interpret and compare behavior curves). + double vx_Oc_c_c0_c = 0.25 * wheels_radius_ * (wheel0_vel + wheel1_vel + wheel2_vel + wheel3_vel); + double vy_Oc_c_c0_c = 0.25 * wheels_radius_ * (-wheel0_vel + wheel1_vel - wheel2_vel + wheel3_vel); + double wz_c_c0_c = 0.25 * wheels_radius_ / wheels_k_ * (-wheel0_vel - wheel1_vel + wheel2_vel + wheel3_vel); + + tf2::Quaternion orientation_R_c_b; + orientation_R_c_b.setRPY(0.0, 0.0,-base_frame_offset_[2]); + + tf2::Matrix3x3 R_c_b = tf2::Matrix3x3((orientation_R_c_b)); + tf2::Vector3 v_Oc_c_c0_b = R_c_b * tf2::Vector3(vx_Oc_c_c0_c, vy_Oc_c_c0_c, 0.0); + tf2::Vector3 Oc_b = R_c_b * tf2::Vector3(-base_frame_offset_[0], -base_frame_offset_[1], 0.0); + + vx_Ob_b_b0_b_ = v_Oc_c_c0_b.x() + Oc_b.y() * wz_c_c0_c; + vy_Ob_b_b0_b_ = v_Oc_c_c0_b.y() - Oc_b.x() * wz_c_c0_c; + wz_b_b0_b_ = wz_c_c0_c; + + /// Integration. + /// NOTE: the position is expressed in the odometry frame (frame b0), unlike the twist which is expressed in the body + /// frame (frame b). + rz_b_b0_ += wz_b_b0_b_ * dt; + + tf2::Quaternion orientation_R_b_b0; + orientation_R_b_b0.setRPY(0.0, 0.0,-base_frame_offset_[2]); + + tf2::Matrix3x3 R_b_b0 = tf2::Matrix3x3((orientation_R_b_b0)); + tf2::Vector3 vx_Ob_b_b0_b0 = R_b_b0 * tf2::Vector3(vx_Ob_b_b0_b_, vy_Ob_b_b0_b_, 0.0); + + px_b_b0_ += vx_Ob_b_b0_b0.x() * dt; + py_b_b0_ += vx_Ob_b_b0_b0.y() * dt; + + return true; +} + +void Odometry::setWheelsParams(double wheels_k, double wheels_radius) +{ + wheels_k_ = wheels_k; + wheels_radius_ = wheels_radius; +} + +} // namespace mecanum_drive_controller diff --git a/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml b/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml new file mode 100644 index 0000000000..97a1a1afc8 --- /dev/null +++ b/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml @@ -0,0 +1,30 @@ +test_mecanum_drive_controller: + ros__parameters: + + reference_timeout: 0.1 + + wheel0_name: "front_left_wheel_joint" + + wheel1_name: "back_left_wheel_joint" + + wheel2_name: "back_right_wheel_joint" + + wheel3_name: "front_right_wheel_joint" + + base_frame_id: "base_link" + + odom_frame_id: "odom" + + enable_odom_tf: true + + base_frame_offset: [0.0, 0.0, 0.0] + + wheels_radius: 0.0 + + wheels_k: 0.0 + + use_realigned_roller_joints: false + + twist_covariance_diagonal: [0.0, 7.0, 14.0, 21.0, 28.0, 35.0] + + pose_covariance_diagonal: [0.0, 7.0, 14.0, 21.0, 28.0, 35.0] \ No newline at end of file diff --git a/mecanum_drive_controller/test/mecanum_drive_controller_preceeding_params.yaml b/mecanum_drive_controller/test/mecanum_drive_controller_preceeding_params.yaml new file mode 100644 index 0000000000..6f57fa39bb --- /dev/null +++ b/mecanum_drive_controller/test/mecanum_drive_controller_preceeding_params.yaml @@ -0,0 +1,11 @@ +test_mecanum_drive_controller: + ros__parameters: + joints: + - joint1 + + interface_name: acceleration + + state_joints: + - joint1state + + reference_timeout: 0.1 diff --git a/mecanum_drive_controller/test/test_load_mecanum_drive_controller.cpp b/mecanum_drive_controller/test/test_load_mecanum_drive_controller.cpp new file mode 100644 index 0000000000..681d7659f9 --- /dev/null +++ b/mecanum_drive_controller/test/test_load_mecanum_drive_controller.cpp @@ -0,0 +1,41 @@ +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "controller_manager/controller_manager.hpp" +#include "hardware_interface/resource_manager.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/utilities.hpp" +#include "ros2_control_test_assets/descriptions.hpp" + +TEST(TestLoadMecanumDriveController, load_controller) +{ + rclcpp::init(0, nullptr); + + std::shared_ptr executor = + std::make_shared(); + + controller_manager::ControllerManager cm( + std::make_unique( + ros2_control_test_assets::minimal_robot_urdf), + executor, "test_controller_manager"); + + ASSERT_NO_THROW( + cm.load_controller("test_mecanum_drive_controller", "mecanum_drive_controller/MecanumDriveController")); + + rclcpp::shutdown(); +} diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp new file mode 100644 index 0000000000..961495077f --- /dev/null +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp @@ -0,0 +1,447 @@ +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "test_mecanum_drive_controller.hpp" + +#include +#include +#include +#include +#include + +using mecanum_drive_controller::NR_CMD_ITFS; +using mecanum_drive_controller::NR_STATE_ITFS; +using mecanum_drive_controller::NR_REF_ITFS; + +class MecanumDriveControllerTest : public MecanumDriveControllerFixture +{ +}; + +TEST_F(MecanumDriveControllerTest, all_parameters_set_configure_success) +{ + fprintf(stderr," here_1"); + ASSERT_EQ(controller_->params_.reference_timeout, 0.0); + fprintf(stderr," here_2"); + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_EQ(controller_->params_.reference_timeout, 0.1); + fprintf(stderr," here_3"); +} + +// TEST_F(MecanumDriveControllerTest, check_exported_intefaces) +// { +// SetUpController(); + +// ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + +// auto command_intefaces = controller_->command_interface_configuration(); +// ASSERT_EQ(command_intefaces.names.size(), joint_command_values_.size()); +// for (size_t i = 0; i < command_intefaces.names.size(); ++i) { +// EXPECT_EQ(command_intefaces.names[i], joint_names_[i] + "/" + interface_name_); +// } + +// auto state_intefaces = controller_->state_interface_configuration(); +// ASSERT_EQ(state_intefaces.names.size(), joint_state_values_.size()); +// for (size_t i = 0; i < state_intefaces.names.size(); ++i) { +// EXPECT_EQ(state_intefaces.names[i], joint_names_[i] + "/" + interface_name_); +// } + +// // check ref itfs +// auto reference_interfaces = controller_->export_reference_interfaces(); +// ASSERT_EQ(reference_interfaces.size(), NR_REF_ITFS); + +// const std::string ref_itf_name_0 = std::string(controller_->get_node()->get_name()) + "/" + +// "linear_x" + "/" + hardware_interface::HW_IF_VELOCITY; +// EXPECT_EQ(reference_interfaces[0].get_name(), ref_itf_name_0); +// EXPECT_EQ(reference_interfaces[0].get_prefix_name(), controller_->get_node()->get_name()); +// EXPECT_EQ( +// reference_interfaces[0].get_interface_name(), +// std::string("linear_x") + "/" + hardware_interface::HW_IF_VELOCITY); + +// const std::string ref_itf_name_1 = std::string(controller_->get_node()->get_name()) + "/" + +// "linear_y" + "/" + hardware_interface::HW_IF_VELOCITY; +// EXPECT_EQ(reference_interfaces[1].get_name(), ref_itf_name_1); +// EXPECT_EQ(reference_interfaces[1].get_prefix_name(), controller_->get_node()->get_name()); +// EXPECT_EQ( +// reference_interfaces[1].get_interface_name(), +// std::string("linear_y") + "/" + hardware_interface::HW_IF_VELOCITY); + +// const std::string ref_itf_name_2 = std::string(controller_->get_node()->get_name()) + "/" + +// "angular_z" + "/" + hardware_interface::HW_IF_VELOCITY; +// EXPECT_EQ(reference_interfaces[2].get_name(), ref_itf_name_2); +// EXPECT_EQ(reference_interfaces[2].get_prefix_name(), controller_->get_node()->get_name()); +// EXPECT_EQ( +// reference_interfaces[2].get_interface_name(), +// std::string("angular_z") + "/" + hardware_interface::HW_IF_VELOCITY); + +// } + +// TEST_F(MecanumDriveControllerTest, activate_success) +// { +// SetUpController(); + +// ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); +// ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + +// // check that the message is reset +// auto msg = controller_->input_ref_.readFromNonRT(); +// EXPECT_TRUE(std::isnan((*msg)->twist.linear.x)); + +// ASSERT_TRUE(std::isnan((*msg)->twist.angular.z)); + +// EXPECT_EQ(controller_->reference_interfaces_.size(), NR_REF_ITFS); +// for (const auto & interface : controller_->reference_interfaces_) +// { +// EXPECT_TRUE(std::isnan(interface)); +// } +// } + +// TEST_F(MecanumDriveControllerTest, update_success) +// { +// SetUpController(); + +// ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); +// ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + +// ASSERT_EQ( +// controller_->update_reference_from_subscribers( +// controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), +// controller_interface::return_type::OK); +// ASSERT_EQ( +// controller_->update_and_write_commands( +// controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), +// controller_interface::return_type::OK); +// } + +// TEST_F(MecanumDriveControllerTest, 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(MecanumDriveControllerTest, reactivate_success) +// { +// SetUpController(); + +// ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); +// ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); +// ASSERT_EQ(controller_->command_interfaces_[NR_CMD_ITFS].get_value(), 101.101); +// ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); +// ASSERT_TRUE(std::isnan(controller_->command_interfaces_[NR_CMD_ITFS].get_value())); +// ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); +// ASSERT_TRUE(std::isnan(controller_->command_interfaces_[NR_CMD_ITFS].get_value())); + +// ASSERT_EQ( +// controller_->update_reference_from_subscribers( +// controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), +// controller_interface::return_type::OK); +// ASSERT_EQ( +// controller_->update_and_write_commands( +// controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), +// controller_interface::return_type::OK); +// } + +// TEST_F(MecanumDriveControllerTest, publish_status_success) +// { fprintf(stderr," here_publish_1_1"); +// SetUpController(); +// fprintf(stderr," here_publish_1_2"); +// ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); +// ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + +// ASSERT_EQ( +// controller_->update_reference_from_subscribers( +// controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), +// controller_interface::return_type::OK); +// ASSERT_EQ( +// controller_->update_and_write_commands( +// controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), +// controller_interface::return_type::OK); + +// ControllerStateMsg msg; +// subscribe_and_get_messages(msg); +// fprintf(stderr," here_publish_1_3"); +// ASSERT_EQ(msg.odom.pose.pose.position.x, 0); +// } + +// TEST_F(MecanumDriveControllerTest, 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_reference_from_subscribers( +// controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), +// controller_interface::return_type::OK); +// ASSERT_EQ( +// controller_->update_and_write_commands( +// controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), +// controller_interface::return_type::OK); +// //here +// ControllerStateMsg msg; +// subscribe_and_get_messages(msg); + +// ASSERT_EQ(msg.set_point, 101.101); + +// publish_commands(controller_->get_node()->now()); +// ASSERT_TRUE(controller_->wait_for_commands(executor)); + +// ASSERT_EQ( +// controller_->update_reference_from_subscribers( +// controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), +// controller_interface::return_type::OK); +// ASSERT_EQ( +// controller_->update_and_write_commands( +// controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), +// controller_interface::return_type::OK); +// //here +// EXPECT_EQ(joint_command_values_[NR_CMD_ITFS], 0.45); + +// subscribe_and_get_messages(msg); + +// ASSERT_EQ(msg.set_point, 0.45); +// } + +// TEST_F(MecanumDriveControllerTest, test_message_timeout) +// { +// SetUpController(); +// rclcpp::executors::MultiThreadedExecutor executor; +// executor.add_node(controller_->get_node()->get_node_base_interface()); + +// ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); +// ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + +// // try to set command with time before timeout - command is not updated +// auto reference = controller_->input_ref_.readFromNonRT(); +// auto old_timestamp = (*reference)->header.stamp; +// EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->joint_names.size(), joint_names_.size()); +// EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->joint_names[0], joint_names_[0]); +// EXPECT_TRUE(std::isnan((*reference)->displacements[0])); +// EXPECT_TRUE(std::isnan((*reference)->velocities[0])); +// EXPECT_TRUE(std::isnan((*reference)->duration)); +// publish_commands(controller_->get_node()->now() - controller_->ref_timeout_ - rclcpp::Duration::from_seconds(0.1)); +// ASSERT_TRUE(controller_->wait_for_commands(executor)); +// ASSERT_EQ(old_timestamp, (*(controller_->input_ref_.readFromNonRT()))->header.stamp); +// EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->joint_names.size(), joint_names_.size()); +// EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->joint_names[0], joint_names_[0]); +// EXPECT_TRUE(std::isnan((*reference)->displacements[0])); +// EXPECT_TRUE(std::isnan((*reference)->velocities[0])); +// EXPECT_TRUE(std::isnan((*reference)->duration)); +// } + + + +// TEST_F(MecanumDriveControllerTest, test_message_wrong_num_joints) +// { +// SetUpController(); +// rclcpp::executors::MultiThreadedExecutor executor; +// executor.add_node(controller_->get_node()->get_node_base_interface()); + +// ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); +// ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + +// // try to set command with time before timeout - command is not updated +// auto reference = controller_->input_ref_.readFromNonRT(); +// auto old_timestamp = (*reference)->header.stamp; +// EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->joint_names.size(), joint_names_.size()); +// EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->joint_names[0], joint_names_[0]); +// EXPECT_TRUE(std::isnan((*reference)->displacements[0])); +// EXPECT_TRUE(std::isnan((*reference)->velocities[0])); +// EXPECT_TRUE(std::isnan((*reference)->duration)); +// publish_commands(controller_->get_node()->now(), {"joint1","joint2"}); +// ASSERT_TRUE(controller_->wait_for_commands(executor)); +// EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->joint_names.size(), joint_names_.size()); +// EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->joint_names[0], joint_names_[0]); +// EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->displacements[0])); +// EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->velocities[0])); +// EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->duration)); +// } + +// TEST_F(MecanumDriveControllerTest, test_message_accepted) +// { +// SetUpController(); +// rclcpp::executors::MultiThreadedExecutor executor; +// executor.add_node(controller_->get_node()->get_node_base_interface()); + +// ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); +// ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + +// // try to set command with time before timeout - command is not updated +// auto reference = controller_->input_ref_.readFromNonRT(); +// EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->joint_names.size(), joint_names_.size()); +// EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->joint_names[0], joint_names_[0]); +// EXPECT_TRUE(std::isnan((*reference)->displacements[0])); +// EXPECT_TRUE(std::isnan((*reference)->velocities[0])); +// EXPECT_TRUE(std::isnan((*reference)->duration)); +// publish_commands(controller_->get_node()->now()); +// ASSERT_TRUE(controller_->wait_for_commands(executor)); +// EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->joint_names.size(), joint_names_.size()); +// EXPECT_NE((*(controller_->input_ref_.readFromNonRT()))->joint_names[0], joint_names_[0]); +// EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->displacements[0])); +// EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->velocities[0])); +// EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->duration)); +// EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->displacements[0], 0.45); +// EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->velocities[0], 0.0); +// EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->duration, 1.25); + +// } + +// TEST_F(MecanumDriveControllerTest, test_update_logic) +// { +// SetUpController(); +// rclcpp::executors::MultiThreadedExecutor executor; +// executor.add_node(controller_->get_node()->get_node_base_interface()); +// executor.add_node(service_caller_node_->get_node_base_interface()); + +// ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); +// ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + +// auto reference = controller_->input_ref_.readFromNonRT(); + +// // set command statically +// static constexpr double TEST_DISPLACEMENT = 23.24; +// joint_command_values_[NR_STATE_ITFS] = 111; +// std::shared_ptr msg = std::make_shared(); +// msg->header.stamp = controller_->get_node()->now() - controller_->ref_timeout_ - rclcpp::Duration::from_seconds(0.1); +// msg->joint_names = joint_names_; +// msg->displacements.resize(joint_names_.size(), TEST_DISPLACEMENT); +// msg->velocities.resize(joint_names_.size(), std::numeric_limits::quiet_NaN()); +// msg->duration = std::numeric_limits::quiet_NaN(); +// controller_->input_ref_.writeFromNonRT(msg); +// const auto age_of_last_command = controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; + +// ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_); +// ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT); +// ASSERT_EQ( +// controller_->update_reference_from_subscribers( +// controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), +// controller_interface::return_type::OK); +// ASSERT_EQ( +// controller_->update_and_write_commands( +// controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), +// controller_interface::return_type::OK); + +// EXPECT_EQ(joint_command_values_[NR_STATE_ITFS], 111); +// ASSERT_TRUE(std::isnan(controller_->reference_interfaces_[0])); + +// std::shared_ptr msg_2 = std::make_shared(); +// msg_2->header.stamp = controller_->get_node()->now(); +// msg_2->joint_names = joint_names_; +// msg_2->displacements.resize(joint_names_.size(), TEST_DISPLACEMENT); +// msg_2->velocities.resize(joint_names_.size(), std::numeric_limits::quiet_NaN()); +// msg_2->duration = std::numeric_limits::quiet_NaN(); +// controller_->input_ref_.writeFromNonRT(msg_2); +// const auto age_of_last_command_2 = controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; + +// ASSERT_TRUE(age_of_last_command_2 <= controller_->ref_timeout_); +// ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT); +// ASSERT_EQ( +// controller_->update_reference_from_subscribers( +// controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), +// controller_interface::return_type::OK); +// ASSERT_EQ( +// controller_->update_and_write_commands( +// controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), +// controller_interface::return_type::OK); + +// EXPECT_EQ(joint_command_values_[NR_STATE_ITFS], TEST_DISPLACEMENT);//exact value +// EXPECT_NE(joint_command_values_[NR_STATE_ITFS], 111); +// ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT); +// } + + +// TEST_F(MecanumDriveControllerTest, test_ref_timeout_zero_for_update) +// { +// SetUpController(); +// rclcpp::executors::MultiThreadedExecutor executor; +// executor.add_node(controller_->get_node()->get_node_base_interface()); +// executor.add_node(service_caller_node_->get_node_base_interface()); + +// ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); +// ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + +// auto reference = controller_->input_ref_.readFromNonRT(); + +// // set command statically +// static constexpr double TEST_DISPLACEMENT = 23.24; +// controller_->ref_timeout_= rclcpp::Duration::from_seconds(0.0); +// std::shared_ptr msg = std::make_shared(); +// msg->header.stamp = controller_->get_node()->now() - rclcpp::Duration::from_seconds(0.0); +// msg->joint_names = joint_names_; +// msg->displacements.resize(joint_names_.size(), TEST_DISPLACEMENT); +// msg->velocities.resize(joint_names_.size(), std::numeric_limits::quiet_NaN()); +// msg->duration = std::numeric_limits::quiet_NaN(); +// controller_->input_ref_.writeFromNonRT(msg); +// const auto age_of_last_command = controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; + +// ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT); +// ASSERT_EQ( +// controller_->update_reference_from_subscribers( +// controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), +// controller_interface::return_type::OK); +// ASSERT_EQ( +// controller_->update_and_write_commands( +// controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), +// controller_interface::return_type::OK); + +// EXPECT_EQ(joint_command_values_[NR_STATE_ITFS], TEST_DISPLACEMENT); +// ASSERT_NE((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT); +// ASSERT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->displacements[0])); +// } + +// TEST_F(MecanumDriveControllerTest, test_ref_timeout_zero_for_reference_callback) +// { +// SetUpController(); +// rclcpp::executors::MultiThreadedExecutor executor; +// executor.add_node(controller_->get_node()->get_node_base_interface()); +// executor.add_node(service_caller_node_->get_node_base_interface()); + +// ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); +// ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + +// EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->joint_names.size(), joint_names_.size()); +// EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->joint_names[0], joint_names_[0]); +// EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->displacements[0])); +// EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->velocities[0])); +// EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->duration)); +// controller_->ref_timeout_= rclcpp::Duration::from_seconds(0.0); +// //reference_callback() is called implicitly when publish_commands() is called. +// publish_commands(controller_->get_node()->now()); +// ASSERT_TRUE(controller_->wait_for_commands(executor)); +// EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->joint_names.size(), joint_names_.size()); +// EXPECT_NE((*(controller_->input_ref_.readFromNonRT()))->joint_names[0], joint_names_[0]); +// EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->displacements[0])); +// EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->velocities[0])); +// EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->duration)); +// EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->displacements[0], 0.45); +// EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->velocities[0], 0.0); +// EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->duration, 1.25); +// } + + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp new file mode 100644 index 0000000000..64a989498f --- /dev/null +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp @@ -0,0 +1,283 @@ +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TEMPLATES__ROS2_CONTROL__CONTROLLER__TEST_MECANUM_DRIVE_CONTROLLER_HPP_ +#define TEMPLATES__ROS2_CONTROL__CONTROLLER__TEST_MECANUM_DRIVE_CONTROLLER_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "mecanum_drive_controller/mecanum_drive_controller.hpp" +#include "gmock/gmock.h" +#include "hardware_interface/loaned_command_interface.hpp" +#include "hardware_interface/loaned_state_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "rclcpp/parameter_value.hpp" +#include "rclcpp/time.hpp" +#include "rclcpp/utilities.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" + +// TODO(anyone): replace the state and command message types +using ControllerStateMsg = mecanum_drive_controller::MecanumDriveController::ControllerStateMsg; +using ControllerReferenceMsg = mecanum_drive_controller::MecanumDriveController::ControllerReferenceMsg; +using TfStateMsg = mecanum_drive_controller::MecanumDriveController::TfStateMsg; +using OdomStateMsg = mecanum_drive_controller::MecanumDriveController::OdomStateMsg; + +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 TestableMecanumDriveController : public mecanum_drive_controller::MecanumDriveController +{ + FRIEND_TEST(MecanumDriveControllerTest, all_parameters_set_configure_success); + // FRIEND_TEST(MecanumDriveControllerTest, check_exported_intefaces); + // FRIEND_TEST(MecanumDriveControllerTest, activate_success); + // FRIEND_TEST(MecanumDriveControllerTest, update_success); + // FRIEND_TEST(MecanumDriveControllerTest, deactivate_success); + // FRIEND_TEST(MecanumDriveControllerTest, reactivate_success); + // FRIEND_TEST(MecanumDriveControllerTest, publish_status_success); + // FRIEND_TEST(MecanumDriveControllerTest, receive_message_and_publish_updated_status); + // FRIEND_TEST(MecanumDriveControllerTest, test_message_timeout); + // FRIEND_TEST(MecanumDriveControllerTest, test_message_wrong_num_joints); + // FRIEND_TEST(MecanumDriveControllerTest, test_message_accepted); + // FRIEND_TEST(MecanumDriveControllerTest, test_update_logic); + // FRIEND_TEST(MecanumDriveControllerTest, test_ref_timeout_zero_for_update); + // FRIEND_TEST(MecanumDriveControllerTest, test_ref_timeout_zero_for_reference_callback); + +public: + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override + { fprintf(stderr," here_ on_configure _1"); + auto ret = mecanum_drive_controller::MecanumDriveController::on_configure(previous_state); + // Only if on_configure is successful create subscription + fprintf(stderr," here_ on_configure _2"); + if (ret == CallbackReturn::SUCCESS) { + ref_subscriber_wait_set_.add_subscription(ref_subscriber_); + } + return ret; + } + + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override + { + auto ref_itfs = on_export_reference_interfaces(); + return mecanum_drive_controller::MecanumDriveController::on_activate(previous_state); + } + + /** + * @brief wait_for_command blocks until a new ControllerReferenceMsg is received. + * Requires that the executor is not spinned elsewhere between the + * message publication and the call to this function. + * + * @return true if new ControllerReferenceMsg msg was received, false if timeout. + */ + bool wait_for_command( + rclcpp::Executor & executor, rclcpp::WaitSet & subscriber_wait_set, + const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) + { + bool success = subscriber_wait_set.wait(timeout).kind() == rclcpp::WaitResultKind::Ready; + if (success) { + executor.spin_some(); + } + return success; + } + + bool wait_for_commands( + rclcpp::Executor & executor, + const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) + { + return wait_for_command(executor, ref_subscriber_wait_set_, timeout); + } + + // TODO(anyone): add implementation of any methods of your controller is needed + +private: + rclcpp::WaitSet ref_subscriber_wait_set_; +}; + +// We are using template class here for easier reuse of Fixture in specializations of controllers +template +class MecanumDriveControllerFixture : 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_mecanum_drive_controller/commands", rclcpp::SystemDefaultsQoS()); + + odom_s_publisher_node_ = std::make_shared("odom_s_publisher"); + odom_s_publisher_ = odom_s_publisher_node_->create_publisher( + "/test_mecanum_drive_controller/odometry", rclcpp::SystemDefaultsQoS()); + + + tf_odom_s_publisher_node_ = std::make_shared("tf_odom_s_publisher"); + tf_odom_s_publisher_ = tf_odom_s_publisher_node_->create_publisher( + "/test_mecanum_drive_controller/tf_odometry", rclcpp::SystemDefaultsQoS()); + + } + + static void TearDownTestCase() {} + + void TearDown() { controller_.reset(nullptr); } + +protected: + void SetUpController(const std::string controller_name = "test_mecanum_drive_controller") + { + ASSERT_EQ(controller_->init(controller_name), controller_interface::return_type::OK); + + std::vector command_ifs; + command_itfs_.reserve(joint_command_values_.size()); + command_ifs.reserve(joint_command_values_.size()); + + for (size_t i = 0; i < joint_command_values_.size(); ++i) { + command_itfs_.emplace_back(hardware_interface::CommandInterface( + joint_names_[i], interface_name_, &joint_command_values_[i])); + command_ifs.emplace_back(command_itfs_.back()); + } + // TODO(anyone): Add other command interfaces, if any + + std::vector state_ifs; + state_itfs_.reserve(joint_state_values_.size()); + state_ifs.reserve(joint_state_values_.size()); + + for (size_t i = 0; i < joint_state_values_.size(); ++i) { + state_itfs_.emplace_back(hardware_interface::StateInterface( + joint_names_[i], interface_name_, &joint_state_values_[i])); + state_ifs.emplace_back(state_itfs_.back()); + } + // TODO(anyone): Add other state interfaces, if any + + controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); + } + + void subscribe_and_get_messages(ControllerStateMsg & msg) + { fprintf(stderr," here_publish_1"); + // 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_mecanum_drive_controller/controller_state", 10, subs_callback); + fprintf(stderr," here_publish_2"); + // call update to publish the test value + ASSERT_EQ( + controller_->update_reference_from_subscribers( + controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + ASSERT_EQ( + controller_->update_and_write_commands( + controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + fprintf(stderr," here_publish_3"); + // 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); + fprintf(stderr," here_publish_4"); + while (max_sub_check_loop_count--) { + controller_->update_reference_from_subscribers( + controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)); + controller_->update_and_write_commands( + controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)); + // check if message has been received + if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) { + break; + } + } + fprintf(stderr," here_publish_5"); + 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)); + fprintf(stderr," here_publish_6"); + } + + // TODO(anyone): add/remove arguments as it suites your command message type + void publish_commands( + const rclcpp::Time & stamp, const double & twist_linear_x = 0.45, const double & twist_linear_y = 0.45, + const double & twist_angular_z = 0.0) + { + auto wait_for_topic = [&](const auto topic_name) { + size_t wait_count = 0; + while (command_publisher_node_->count_subscribers(topic_name) == 0) { + if (wait_count >= 5) { + auto error_msg = + std::string("publishing to ") + topic_name + " but no node subscribes to it"; + throw std::runtime_error(error_msg); + } + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + ++wait_count; + } + }; + + wait_for_topic(command_publisher_->get_topic_name()); + + ControllerReferenceMsg msg; + msg.header.stamp = stamp; + msg.twist.linear.x = twist_linear_x; + msg.twist.linear.y = twist_linear_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 = twist_angular_z; + + command_publisher_->publish(msg); + } + + +protected: + // TODO(anyone): adjust the members as needed + + std::vector joint_names_ = {"front_left_wheel_joint", "back_left_wheel_joint", "back_right_wheel_joint", "front_right_wheel_joint"}; + std::vector state_joint_names_ = {"state_front_left_wheel_joint", "state_back_left_wheel_joint", "state_back_right_wheel_joint", "state_front_right_wheel_joint"}; + std::string interface_name_ = "velocity"; + + // Controller-related parameters + + std::array joint_state_values_ = {1.1, 2.1, 3.1, 4.1}; + std::array joint_command_values_ = {101.101, 201.201, 301.301, 401.401}; + + std::vector state_itfs_; + std::vector command_itfs_; + + double ref_timeout_ = 0.2; + + // Test related parameters + std::unique_ptr controller_; + rclcpp::Node::SharedPtr command_publisher_node_; + rclcpp::Publisher::SharedPtr command_publisher_; + rclcpp::Node::SharedPtr odom_s_publisher_node_; + rclcpp::Publisher::SharedPtr odom_s_publisher_; + rclcpp::Node::SharedPtr tf_odom_s_publisher_node_; + rclcpp::Publisher::SharedPtr tf_odom_s_publisher_; +}; + +#endif // TEMPLATES__ROS2_CONTROL__CONTROLLER__TEST_MECANUM_DRIVE_CONTROLLER_HPP_ diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller_preceeding.cpp b/mecanum_drive_controller/test/test_mecanum_drive_controller_preceeding.cpp new file mode 100644 index 0000000000..98a2c07dec --- /dev/null +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller_preceeding.cpp @@ -0,0 +1,85 @@ +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "test_mecanum_drive_controller.hpp" + +#include +#include +#include +#include +#include + +using mecanum_drive_controller::CMD_MY_ITFS; +using mecanum_drive_controller::control_mode_type; +using mecanum_drive_controller::STATE_MY_ITFS; + +class MecanumDriveControllerTest : public MecanumDriveControllerFixture +{ +}; + +TEST_F(MecanumDriveControllerTest, all_parameters_set_configure_success) +{ + SetUpController(); + + ASSERT_TRUE(controller_->params_.joints.empty()); + ASSERT_TRUE(controller_->params_.state_joints.empty()); + ASSERT_TRUE(controller_->params_.interface_name.empty()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_THAT(controller_->params_.joints, testing::ElementsAreArray(joint_names_)); + ASSERT_THAT(controller_->params_.state_joints, testing::ElementsAreArray(state_joint_names_)); + ASSERT_THAT(controller_->state_joints_, testing::ElementsAreArray(state_joint_names_)); + ASSERT_EQ(controller_->params_.interface_name, interface_name_); +} + +TEST_F(MecanumDriveControllerTest, check_exported_intefaces) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto command_intefaces = controller_->command_interface_configuration(); + ASSERT_EQ(command_intefaces.names.size(), joint_command_values_.size()); + for (size_t i = 0; i < command_intefaces.names.size(); ++i) { + EXPECT_EQ(command_intefaces.names[i], joint_names_[i] + "/" + interface_name_); + } + + auto state_intefaces = controller_->state_interface_configuration(); + ASSERT_EQ(state_intefaces.names.size(), joint_state_values_.size()); + for (size_t i = 0; i < state_intefaces.names.size(); ++i) { + EXPECT_EQ(state_intefaces.names[i], state_joint_names_[i] + "/" + interface_name_); + } + + // check ref itfs + auto reference_interfaces = controller_->export_reference_interfaces(); + ASSERT_EQ(reference_interfaces.size(), joint_names_.size()); + for (size_t i = 0; i < joint_names_.size(); ++i) { + const std::string ref_itf_name = std::string(controller_->get_node()->get_name()) + "/" + + state_joint_names_[i] + "/" + interface_name_; + EXPECT_EQ(reference_interfaces[i].get_name(), ref_itf_name); + EXPECT_EQ(reference_interfaces[i].get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ( + reference_interfaces[i].get_interface_name(), state_joint_names_[i] + "/" + interface_name_); + } +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} From f03dd0568d61546289c66e5885ddcb7d836778d7 Mon Sep 17 00:00:00 2001 From: GiridharBukka Date: Fri, 6 Jan 2023 21:10:16 +0530 Subject: [PATCH 02/91] all tests working --- .../src/mecanum_drive_controller.cpp | 66 +- .../test/mecanum_drive_controller_params.yaml | 2 +- .../test/test_mecanum_drive_controller.cpp | 901 ++++++++++-------- .../test/test_mecanum_drive_controller.hpp | 39 +- 4 files changed, 561 insertions(+), 447 deletions(-) diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index 0e6ae85ede..597e63a689 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -81,10 +81,8 @@ controller_interface::CallbackReturn MecanumDriveController::on_init() { controller_interface::CallbackReturn MecanumDriveController::on_configure( const rclcpp_lifecycle::State& /*previous_state*/) { - fprintf(stderr," here_ on_configure _1_199"); params_ = param_listener_->get_params(); - fprintf(stderr," here_ on_configure _1_2"); // Set wheel params for the odometry computation odometry_.setWheelsParams(params_.wheels_k, params_.wheels_radius); @@ -182,7 +180,7 @@ controller_interface::CallbackReturn MecanumDriveController::on_configure( controller_state_publisher_->lock(); controller_state_publisher_->msg_.header.stamp = get_node()->now(); controller_state_publisher_->msg_.header.frame_id = params_.odom_frame_id; - controller_state_publisher_->msg_.odom.pose.pose.position.z = 0; + controller_state_publisher_->msg_.odom.pose.pose.position.x = 0; controller_state_publisher_->msg_.odom.child_frame_id = params_.base_frame_id; controller_state_publisher_->unlock(); auto & covariance_controller = controller_state_publisher_->msg_.odom.twist.covariance; @@ -402,29 +400,45 @@ controller_interface::return_type MecanumDriveController::update_and_write_comma // INVERSE KINEMATICS (move robot). // Compute wheels velocities (this is the actual ik): // NOTE: the input desired twist (from topic /cmd_vel) is a body twist. - - tf2::Quaternion orientation_2; - orientation_2.setRPY(0.0, 0.0, params_.base_frame_offset[2]); - - tf2::Matrix3x3 R_b_c = tf2::Matrix3x3((orientation_2)); - tf2::Vector3 v_Ob_b_b0_c = R_b_c * tf2::Vector3(reference_interfaces_[0], reference_interfaces_[1], 0.0); - tf2::Vector3 Ob_c = tf2::Vector3(params_.base_frame_offset[0], params_.base_frame_offset[1], 0.0); - - double vx_Oc_c_c0_c_ = v_Ob_b_b0_c.x() + Ob_c.y() * reference_interfaces_[2]; - double vy_Oc_c_c0_c_ = v_Ob_b_b0_c.y() - Ob_c.x() * reference_interfaces_[2]; - double wz_c_c0_c_ = reference_interfaces_[2]; - - double w0_vel = 1.0 / params_.wheels_radius * (vx_Oc_c_c0_c_ - vy_Oc_c_c0_c_ - params_.wheels_k * wz_c_c0_c_); - double w1_vel = 1.0 / params_.wheels_radius * (vx_Oc_c_c0_c_ + vy_Oc_c_c0_c_ - params_.wheels_k * wz_c_c0_c_); - double w2_vel = 1.0 / params_.wheels_radius * (vx_Oc_c_c0_c_ - vy_Oc_c_c0_c_ + params_.wheels_k * wz_c_c0_c_); - double w3_vel = 1.0 / params_.wheels_radius * (vx_Oc_c_c0_c_ + vy_Oc_c_c0_c_ + params_.wheels_k * wz_c_c0_c_); - - // Set wheels velocities: - - command_interfaces_[0].set_value(w0_vel); - command_interfaces_[1].set_value(w1_vel); - command_interfaces_[2].set_value(w2_vel); - command_interfaces_[3].set_value(w3_vel); + auto current_ref = input_ref_.readFromRT(); + const auto age_of_last_command = time - (*current_ref)->header.stamp; + if (age_of_last_command <= ref_timeout_ || + ref_timeout_ == rclcpp::Duration::from_seconds(0)) + { + tf2::Quaternion orientation_2; + orientation_2.setRPY(0.0, 0.0, params_.base_frame_offset[2]); + + tf2::Matrix3x3 R_b_c = tf2::Matrix3x3((orientation_2)); + tf2::Vector3 v_Ob_b_b0_c = R_b_c * tf2::Vector3(reference_interfaces_[0], reference_interfaces_[1], 0.0); + tf2::Vector3 Ob_c = tf2::Vector3(params_.base_frame_offset[0], params_.base_frame_offset[1], 0.0); + + double vx_Oc_c_c0_c_ = v_Ob_b_b0_c.x() + Ob_c.y() * reference_interfaces_[2]; + double vy_Oc_c_c0_c_ = v_Ob_b_b0_c.y() - Ob_c.x() * reference_interfaces_[2]; + double wz_c_c0_c_ = reference_interfaces_[2]; + + double w0_vel = 1.0 / params_.wheels_radius * (vx_Oc_c_c0_c_ - vy_Oc_c_c0_c_ - params_.wheels_k * wz_c_c0_c_); + double w1_vel = 1.0 / params_.wheels_radius * (vx_Oc_c_c0_c_ + vy_Oc_c_c0_c_ - params_.wheels_k * wz_c_c0_c_); + double w2_vel = 1.0 / params_.wheels_radius * (vx_Oc_c_c0_c_ - vy_Oc_c_c0_c_ + params_.wheels_k * wz_c_c0_c_); + double w3_vel = 1.0 / params_.wheels_radius * (vx_Oc_c_c0_c_ + vy_Oc_c_c0_c_ + params_.wheels_k * wz_c_c0_c_); + + // Set wheels velocities: + + command_interfaces_[0].set_value(w0_vel); + // command_interfaces_[1].set_value(w1_vel); + command_interfaces_[1].set_value(99); + command_interfaces_[2].set_value(w2_vel); + command_interfaces_[3].set_value(w3_vel); + + } else { + reference_interfaces_[0] = std::numeric_limits::quiet_NaN(); + reference_interfaces_[1] = std::numeric_limits::quiet_NaN(); + reference_interfaces_[2] = std::numeric_limits::quiet_NaN(); + command_interfaces_[0].set_value(0.0); + // command_interfaces_[1].set_value(w1_vel); + command_interfaces_[1].set_value(0.0); + command_interfaces_[2].set_value(0.0); + command_interfaces_[3].set_value(0.0); + } return controller_interface::return_type::OK; diff --git a/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml b/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml index 97a1a1afc8..d6e36800cd 100644 --- a/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml +++ b/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml @@ -19,7 +19,7 @@ test_mecanum_drive_controller: base_frame_offset: [0.0, 0.0, 0.0] - wheels_radius: 0.0 + wheels_radius: 0.5 wheels_k: 0.0 diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp index 961495077f..5b8f7f9fd9 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp @@ -30,411 +30,516 @@ class MecanumDriveControllerTest : public MecanumDriveControllerFixtureparams_.reference_timeout, 0.0); - fprintf(stderr," here_2"); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_EQ(controller_->params_.reference_timeout, 0.1); - fprintf(stderr," here_3"); } -// TEST_F(MecanumDriveControllerTest, check_exported_intefaces) -// { -// SetUpController(); - -// ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - -// auto command_intefaces = controller_->command_interface_configuration(); -// ASSERT_EQ(command_intefaces.names.size(), joint_command_values_.size()); -// for (size_t i = 0; i < command_intefaces.names.size(); ++i) { -// EXPECT_EQ(command_intefaces.names[i], joint_names_[i] + "/" + interface_name_); -// } - -// auto state_intefaces = controller_->state_interface_configuration(); -// ASSERT_EQ(state_intefaces.names.size(), joint_state_values_.size()); -// for (size_t i = 0; i < state_intefaces.names.size(); ++i) { -// EXPECT_EQ(state_intefaces.names[i], joint_names_[i] + "/" + interface_name_); -// } - -// // check ref itfs -// auto reference_interfaces = controller_->export_reference_interfaces(); -// ASSERT_EQ(reference_interfaces.size(), NR_REF_ITFS); - -// const std::string ref_itf_name_0 = std::string(controller_->get_node()->get_name()) + "/" + -// "linear_x" + "/" + hardware_interface::HW_IF_VELOCITY; -// EXPECT_EQ(reference_interfaces[0].get_name(), ref_itf_name_0); -// EXPECT_EQ(reference_interfaces[0].get_prefix_name(), controller_->get_node()->get_name()); -// EXPECT_EQ( -// reference_interfaces[0].get_interface_name(), -// std::string("linear_x") + "/" + hardware_interface::HW_IF_VELOCITY); - -// const std::string ref_itf_name_1 = std::string(controller_->get_node()->get_name()) + "/" + -// "linear_y" + "/" + hardware_interface::HW_IF_VELOCITY; -// EXPECT_EQ(reference_interfaces[1].get_name(), ref_itf_name_1); -// EXPECT_EQ(reference_interfaces[1].get_prefix_name(), controller_->get_node()->get_name()); -// EXPECT_EQ( -// reference_interfaces[1].get_interface_name(), -// std::string("linear_y") + "/" + hardware_interface::HW_IF_VELOCITY); - -// const std::string ref_itf_name_2 = std::string(controller_->get_node()->get_name()) + "/" + -// "angular_z" + "/" + hardware_interface::HW_IF_VELOCITY; -// EXPECT_EQ(reference_interfaces[2].get_name(), ref_itf_name_2); -// EXPECT_EQ(reference_interfaces[2].get_prefix_name(), controller_->get_node()->get_name()); -// EXPECT_EQ( -// reference_interfaces[2].get_interface_name(), -// std::string("angular_z") + "/" + hardware_interface::HW_IF_VELOCITY); - -// } - -// TEST_F(MecanumDriveControllerTest, activate_success) -// { -// SetUpController(); - -// ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); -// ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - -// // check that the message is reset -// auto msg = controller_->input_ref_.readFromNonRT(); -// EXPECT_TRUE(std::isnan((*msg)->twist.linear.x)); - -// ASSERT_TRUE(std::isnan((*msg)->twist.angular.z)); - -// EXPECT_EQ(controller_->reference_interfaces_.size(), NR_REF_ITFS); -// for (const auto & interface : controller_->reference_interfaces_) -// { -// EXPECT_TRUE(std::isnan(interface)); -// } -// } - -// TEST_F(MecanumDriveControllerTest, update_success) -// { -// SetUpController(); - -// ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); -// ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - -// ASSERT_EQ( -// controller_->update_reference_from_subscribers( -// controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), -// controller_interface::return_type::OK); -// ASSERT_EQ( -// controller_->update_and_write_commands( -// controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), -// controller_interface::return_type::OK); -// } - -// TEST_F(MecanumDriveControllerTest, 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(MecanumDriveControllerTest, reactivate_success) -// { -// SetUpController(); - -// ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); -// ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); -// ASSERT_EQ(controller_->command_interfaces_[NR_CMD_ITFS].get_value(), 101.101); -// ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); -// ASSERT_TRUE(std::isnan(controller_->command_interfaces_[NR_CMD_ITFS].get_value())); -// ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); -// ASSERT_TRUE(std::isnan(controller_->command_interfaces_[NR_CMD_ITFS].get_value())); - -// ASSERT_EQ( -// controller_->update_reference_from_subscribers( -// controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), -// controller_interface::return_type::OK); -// ASSERT_EQ( -// controller_->update_and_write_commands( -// controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), -// controller_interface::return_type::OK); -// } - -// TEST_F(MecanumDriveControllerTest, publish_status_success) -// { fprintf(stderr," here_publish_1_1"); -// SetUpController(); -// fprintf(stderr," here_publish_1_2"); -// ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); -// ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - -// ASSERT_EQ( -// controller_->update_reference_from_subscribers( -// controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), -// controller_interface::return_type::OK); -// ASSERT_EQ( -// controller_->update_and_write_commands( -// controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), -// controller_interface::return_type::OK); - -// ControllerStateMsg msg; -// subscribe_and_get_messages(msg); -// fprintf(stderr," here_publish_1_3"); -// ASSERT_EQ(msg.odom.pose.pose.position.x, 0); -// } - -// TEST_F(MecanumDriveControllerTest, 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_reference_from_subscribers( -// controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), -// controller_interface::return_type::OK); -// ASSERT_EQ( -// controller_->update_and_write_commands( -// controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), -// controller_interface::return_type::OK); -// //here -// ControllerStateMsg msg; -// subscribe_and_get_messages(msg); - -// ASSERT_EQ(msg.set_point, 101.101); - -// publish_commands(controller_->get_node()->now()); -// ASSERT_TRUE(controller_->wait_for_commands(executor)); - -// ASSERT_EQ( -// controller_->update_reference_from_subscribers( -// controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), -// controller_interface::return_type::OK); -// ASSERT_EQ( -// controller_->update_and_write_commands( -// controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), -// controller_interface::return_type::OK); -// //here -// EXPECT_EQ(joint_command_values_[NR_CMD_ITFS], 0.45); - -// subscribe_and_get_messages(msg); - -// ASSERT_EQ(msg.set_point, 0.45); -// } - -// TEST_F(MecanumDriveControllerTest, test_message_timeout) -// { -// SetUpController(); -// rclcpp::executors::MultiThreadedExecutor executor; -// executor.add_node(controller_->get_node()->get_node_base_interface()); - -// ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); -// ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - -// // try to set command with time before timeout - command is not updated -// auto reference = controller_->input_ref_.readFromNonRT(); -// auto old_timestamp = (*reference)->header.stamp; -// EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->joint_names.size(), joint_names_.size()); -// EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->joint_names[0], joint_names_[0]); -// EXPECT_TRUE(std::isnan((*reference)->displacements[0])); -// EXPECT_TRUE(std::isnan((*reference)->velocities[0])); -// EXPECT_TRUE(std::isnan((*reference)->duration)); -// publish_commands(controller_->get_node()->now() - controller_->ref_timeout_ - rclcpp::Duration::from_seconds(0.1)); -// ASSERT_TRUE(controller_->wait_for_commands(executor)); -// ASSERT_EQ(old_timestamp, (*(controller_->input_ref_.readFromNonRT()))->header.stamp); -// EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->joint_names.size(), joint_names_.size()); -// EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->joint_names[0], joint_names_[0]); -// EXPECT_TRUE(std::isnan((*reference)->displacements[0])); -// EXPECT_TRUE(std::isnan((*reference)->velocities[0])); -// EXPECT_TRUE(std::isnan((*reference)->duration)); -// } - - - -// TEST_F(MecanumDriveControllerTest, test_message_wrong_num_joints) -// { -// SetUpController(); -// rclcpp::executors::MultiThreadedExecutor executor; -// executor.add_node(controller_->get_node()->get_node_base_interface()); - -// ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); -// ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - -// // try to set command with time before timeout - command is not updated -// auto reference = controller_->input_ref_.readFromNonRT(); -// auto old_timestamp = (*reference)->header.stamp; -// EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->joint_names.size(), joint_names_.size()); -// EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->joint_names[0], joint_names_[0]); -// EXPECT_TRUE(std::isnan((*reference)->displacements[0])); -// EXPECT_TRUE(std::isnan((*reference)->velocities[0])); -// EXPECT_TRUE(std::isnan((*reference)->duration)); -// publish_commands(controller_->get_node()->now(), {"joint1","joint2"}); -// ASSERT_TRUE(controller_->wait_for_commands(executor)); -// EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->joint_names.size(), joint_names_.size()); -// EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->joint_names[0], joint_names_[0]); -// EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->displacements[0])); -// EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->velocities[0])); -// EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->duration)); -// } - -// TEST_F(MecanumDriveControllerTest, test_message_accepted) -// { -// SetUpController(); -// rclcpp::executors::MultiThreadedExecutor executor; -// executor.add_node(controller_->get_node()->get_node_base_interface()); - -// ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); -// ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - -// // try to set command with time before timeout - command is not updated -// auto reference = controller_->input_ref_.readFromNonRT(); -// EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->joint_names.size(), joint_names_.size()); -// EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->joint_names[0], joint_names_[0]); -// EXPECT_TRUE(std::isnan((*reference)->displacements[0])); -// EXPECT_TRUE(std::isnan((*reference)->velocities[0])); -// EXPECT_TRUE(std::isnan((*reference)->duration)); -// publish_commands(controller_->get_node()->now()); -// ASSERT_TRUE(controller_->wait_for_commands(executor)); -// EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->joint_names.size(), joint_names_.size()); -// EXPECT_NE((*(controller_->input_ref_.readFromNonRT()))->joint_names[0], joint_names_[0]); -// EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->displacements[0])); -// EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->velocities[0])); -// EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->duration)); -// EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->displacements[0], 0.45); -// EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->velocities[0], 0.0); -// EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->duration, 1.25); - -// } - -// TEST_F(MecanumDriveControllerTest, test_update_logic) -// { -// SetUpController(); -// rclcpp::executors::MultiThreadedExecutor executor; -// executor.add_node(controller_->get_node()->get_node_base_interface()); -// executor.add_node(service_caller_node_->get_node_base_interface()); - -// ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); -// ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - -// auto reference = controller_->input_ref_.readFromNonRT(); - -// // set command statically -// static constexpr double TEST_DISPLACEMENT = 23.24; -// joint_command_values_[NR_STATE_ITFS] = 111; -// std::shared_ptr msg = std::make_shared(); -// msg->header.stamp = controller_->get_node()->now() - controller_->ref_timeout_ - rclcpp::Duration::from_seconds(0.1); -// msg->joint_names = joint_names_; -// msg->displacements.resize(joint_names_.size(), TEST_DISPLACEMENT); -// msg->velocities.resize(joint_names_.size(), std::numeric_limits::quiet_NaN()); -// msg->duration = std::numeric_limits::quiet_NaN(); -// controller_->input_ref_.writeFromNonRT(msg); -// const auto age_of_last_command = controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; - -// ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_); -// ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT); -// ASSERT_EQ( -// controller_->update_reference_from_subscribers( -// controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), -// controller_interface::return_type::OK); -// ASSERT_EQ( -// controller_->update_and_write_commands( -// controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), -// controller_interface::return_type::OK); - -// EXPECT_EQ(joint_command_values_[NR_STATE_ITFS], 111); -// ASSERT_TRUE(std::isnan(controller_->reference_interfaces_[0])); - -// std::shared_ptr msg_2 = std::make_shared(); -// msg_2->header.stamp = controller_->get_node()->now(); -// msg_2->joint_names = joint_names_; -// msg_2->displacements.resize(joint_names_.size(), TEST_DISPLACEMENT); -// msg_2->velocities.resize(joint_names_.size(), std::numeric_limits::quiet_NaN()); -// msg_2->duration = std::numeric_limits::quiet_NaN(); -// controller_->input_ref_.writeFromNonRT(msg_2); -// const auto age_of_last_command_2 = controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; +TEST_F(MecanumDriveControllerTest, check_exported_intefaces) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto command_intefaces = controller_->command_interface_configuration(); + ASSERT_EQ(command_intefaces.names.size(), joint_command_values_.size()); + for (size_t i = 0; i < command_intefaces.names.size(); ++i) { + EXPECT_EQ(command_intefaces.names[i], joint_names_[i] + "/" + interface_name_); + } + + auto state_intefaces = controller_->state_interface_configuration(); + ASSERT_EQ(state_intefaces.names.size(), joint_state_values_.size()); + for (size_t i = 0; i < state_intefaces.names.size(); ++i) { + EXPECT_EQ(state_intefaces.names[i], joint_names_[i] + "/" + interface_name_); + } + + // check ref itfs + auto reference_interfaces = controller_->export_reference_interfaces(); + ASSERT_EQ(reference_interfaces.size(), NR_REF_ITFS); + + const std::string ref_itf_name_0 = std::string(controller_->get_node()->get_name()) + "/" + + "linear_x" + "/" + hardware_interface::HW_IF_VELOCITY; + EXPECT_EQ(reference_interfaces[0].get_name(), ref_itf_name_0); + EXPECT_EQ(reference_interfaces[0].get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ( + reference_interfaces[0].get_interface_name(), + std::string("linear_x") + "/" + hardware_interface::HW_IF_VELOCITY); + + const std::string ref_itf_name_1 = std::string(controller_->get_node()->get_name()) + "/" + + "linear_y" + "/" + hardware_interface::HW_IF_VELOCITY; + EXPECT_EQ(reference_interfaces[1].get_name(), ref_itf_name_1); + EXPECT_EQ(reference_interfaces[1].get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ( + reference_interfaces[1].get_interface_name(), + std::string("linear_y") + "/" + hardware_interface::HW_IF_VELOCITY); + + const std::string ref_itf_name_2 = std::string(controller_->get_node()->get_name()) + "/" + + "angular_z" + "/" + hardware_interface::HW_IF_VELOCITY; + EXPECT_EQ(reference_interfaces[2].get_name(), ref_itf_name_2); + EXPECT_EQ(reference_interfaces[2].get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ( + reference_interfaces[2].get_interface_name(), + std::string("angular_z") + "/" + hardware_interface::HW_IF_VELOCITY); -// ASSERT_TRUE(age_of_last_command_2 <= controller_->ref_timeout_); -// ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT); -// ASSERT_EQ( -// controller_->update_reference_from_subscribers( -// controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), -// controller_interface::return_type::OK); -// ASSERT_EQ( -// controller_->update_and_write_commands( -// controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), -// controller_interface::return_type::OK); - -// EXPECT_EQ(joint_command_values_[NR_STATE_ITFS], TEST_DISPLACEMENT);//exact value -// EXPECT_NE(joint_command_values_[NR_STATE_ITFS], 111); -// ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT); -// } - - -// TEST_F(MecanumDriveControllerTest, test_ref_timeout_zero_for_update) -// { -// SetUpController(); -// rclcpp::executors::MultiThreadedExecutor executor; -// executor.add_node(controller_->get_node()->get_node_base_interface()); -// executor.add_node(service_caller_node_->get_node_base_interface()); - -// ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); -// ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - -// auto reference = controller_->input_ref_.readFromNonRT(); - -// // set command statically -// static constexpr double TEST_DISPLACEMENT = 23.24; -// controller_->ref_timeout_= rclcpp::Duration::from_seconds(0.0); -// std::shared_ptr msg = std::make_shared(); -// msg->header.stamp = controller_->get_node()->now() - rclcpp::Duration::from_seconds(0.0); -// msg->joint_names = joint_names_; -// msg->displacements.resize(joint_names_.size(), TEST_DISPLACEMENT); -// msg->velocities.resize(joint_names_.size(), std::numeric_limits::quiet_NaN()); -// msg->duration = std::numeric_limits::quiet_NaN(); -// controller_->input_ref_.writeFromNonRT(msg); -// const auto age_of_last_command = controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; +} + +TEST_F(MecanumDriveControllerTest, activate_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // check that the message is reset + auto msg = controller_->input_ref_.readFromNonRT(); + EXPECT_TRUE(std::isnan((*msg)->twist.linear.x)); + + ASSERT_TRUE(std::isnan((*msg)->twist.angular.z)); + + EXPECT_EQ(controller_->reference_interfaces_.size(), NR_REF_ITFS); + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } +} + +TEST_F(MecanumDriveControllerTest, update_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_EQ( + controller_->update_reference_from_subscribers( + controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + ASSERT_EQ( + controller_->update_and_write_commands( + controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); +} + +TEST_F(MecanumDriveControllerTest, 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(MecanumDriveControllerTest, reactivate_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->command_interfaces_[NR_CMD_ITFS-4].get_value(), 101.101); + ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(std::isnan(controller_->command_interfaces_[NR_CMD_ITFS-4].get_value())); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(std::isnan(controller_->command_interfaces_[NR_CMD_ITFS-4].get_value())); + + ASSERT_EQ( + controller_->update_reference_from_subscribers( + controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + ASSERT_EQ( + controller_->update_and_write_commands( + controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + fprintf(stderr," reactivate_success _1"); + +} + +TEST_F(MecanumDriveControllerTest, publish_status_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_EQ( + controller_->update_reference_from_subscribers( + controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + ASSERT_EQ( + controller_->update_and_write_commands( + controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + ControllerStateMsg msg; + subscribe_and_get_messages(msg); + + EXPECT_TRUE(std::isnan(msg.odom.pose.pose.position.x)); +} + +TEST_F(MecanumDriveControllerTest, 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_reference_from_subscribers( + controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + ASSERT_EQ( + controller_->update_and_write_commands( + controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); +//here + ControllerStateMsg msg; + subscribe_and_get_messages(msg); + joint_command_values_[1] = 111; + EXPECT_TRUE(std::isnan(msg.linear_x_velocity_command)); + + publish_commands(controller_->get_node()->now()); + ASSERT_TRUE(controller_->wait_for_commands(executor)); + + ASSERT_EQ( + controller_->update_reference_from_subscribers( + controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + ASSERT_EQ( + controller_->update_and_write_commands( + controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); +//here + EXPECT_EQ(joint_command_values_[1], 99); + EXPECT_FALSE(std::isnan(joint_command_values_[1])); + fprintf(stderr," joint_command_values_[1]= %f", joint_command_values_[1]); + + subscribe_and_get_messages(msg); + + ASSERT_EQ(msg.linear_x_velocity_command, 0.45); +} + +TEST_F(MecanumDriveControllerTest, test_sending_too_old_message) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // try to set command with time before timeout - command is not updated + auto reference = controller_->input_ref_.readFromNonRT(); + auto old_timestamp = (*reference)->header.stamp; + EXPECT_TRUE(std::isnan((*reference)->twist.linear.x)); + EXPECT_TRUE(std::isnan((*reference)->twist.linear.y)); + EXPECT_TRUE(std::isnan((*reference)->twist.angular.z)); + + publish_commands( + controller_->get_node()->now() - controller_->ref_timeout_ - + rclcpp::Duration::from_seconds(0.1)); + ASSERT_TRUE(controller_->wait_for_commands(executor)); + ASSERT_EQ(old_timestamp, (*(controller_->input_ref_.readFromNonRT()))->header.stamp); + // ASSERT_EQ((*reference)->twist.linear.x, 0.45); + // ASSERT_EQ((*reference)->twist.linear.y, 0.45); + // ASSERT_EQ((*reference)->twist.angular.z, 0.0); + EXPECT_TRUE(std::isnan((*reference)->twist.linear.x)); + EXPECT_TRUE(std::isnan((*reference)->twist.linear.y)); + EXPECT_TRUE(std::isnan((*reference)->twist.angular.z)); -// ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT); -// ASSERT_EQ( -// controller_->update_reference_from_subscribers( -// controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), -// controller_interface::return_type::OK); -// ASSERT_EQ( -// controller_->update_and_write_commands( -// controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), -// controller_interface::return_type::OK); - -// EXPECT_EQ(joint_command_values_[NR_STATE_ITFS], TEST_DISPLACEMENT); -// ASSERT_NE((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT); -// ASSERT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->displacements[0])); -// } - -// TEST_F(MecanumDriveControllerTest, test_ref_timeout_zero_for_reference_callback) -// { -// SetUpController(); -// rclcpp::executors::MultiThreadedExecutor executor; -// executor.add_node(controller_->get_node()->get_node_base_interface()); -// executor.add_node(service_caller_node_->get_node_base_interface()); - -// ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); -// ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - -// EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->joint_names.size(), joint_names_.size()); -// EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->joint_names[0], joint_names_[0]); -// EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->displacements[0])); -// EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->velocities[0])); -// EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->duration)); -// controller_->ref_timeout_= rclcpp::Duration::from_seconds(0.0); -// //reference_callback() is called implicitly when publish_commands() is called. -// publish_commands(controller_->get_node()->now()); -// ASSERT_TRUE(controller_->wait_for_commands(executor)); -// EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->joint_names.size(), joint_names_.size()); -// EXPECT_NE((*(controller_->input_ref_.readFromNonRT()))->joint_names[0], joint_names_[0]); -// EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->displacements[0])); -// EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->velocities[0])); -// EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->duration)); -// EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->displacements[0], 0.45); -// EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->velocities[0], 0.0); -// EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->duration, 1.25); -// } +} + + + +TEST_F(MecanumDriveControllerTest, test_time_stamp_zero) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // try to set command with time before timeout - command is not updated + auto reference = controller_->input_ref_.readFromNonRT(); + auto old_timestamp = (*reference)->header.stamp; + EXPECT_TRUE(std::isnan((*reference)->twist.linear.x)); + EXPECT_TRUE(std::isnan((*reference)->twist.linear.y)); + EXPECT_TRUE(std::isnan((*reference)->twist.angular.z)); + + publish_commands(rclcpp::Time(0)); + + ASSERT_TRUE(controller_->wait_for_commands(executor)); + ASSERT_EQ(old_timestamp.sec, (*(controller_->input_ref_.readFromNonRT()))->header.stamp.sec); + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x)); + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z)); + EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x, 0.45); + EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.linear.y, 0.45); + EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z, 0.0); +} + + +TEST_F(MecanumDriveControllerTest, test_message_accepted) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // try to set command with time before timeout - command is not updated + auto reference = controller_->input_ref_.readFromNonRT(); + EXPECT_TRUE(std::isnan((*reference)->twist.linear.x)); + EXPECT_TRUE(std::isnan((*reference)->twist.angular.z)); + + publish_commands(controller_->get_node()->now()); + + ASSERT_TRUE(controller_->wait_for_commands(executor)); + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x)); + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z)); + EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x, 0.45); + EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.linear.y, 0.45); + EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z, 0.0); +} + +TEST_F(MecanumDriveControllerTest, test_update_logic_chainable) +{ + // 1. age>ref_timeout 2. ageget_node()->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + controller_->set_chained_mode(false); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_FALSE(controller_->is_in_chained_mode()); + + // set command statically + static constexpr double TEST_LINEAR_VELOCITY_X = 1.5; + static constexpr double TEST_LINEAR_VELOCITY_y = 1.4; + static constexpr double TEST_ANGULAR_VELOCITY_Z = 1.1; + joint_command_values_[1] = 111; + std::shared_ptr msg = std::make_shared(); + + msg->header.stamp = controller_->get_node()->now() - controller_->ref_timeout_ - + rclcpp::Duration::from_seconds(0.1); + msg->twist.linear.x = TEST_LINEAR_VELOCITY_X; + msg->twist.linear.y = TEST_LINEAR_VELOCITY_y; + msg->twist.linear.z = std::numeric_limits::quiet_NaN(); + msg->twist.angular.x = std::numeric_limits::quiet_NaN(); + msg->twist.angular.y = std::numeric_limits::quiet_NaN(); + msg->twist.angular.z = TEST_ANGULAR_VELOCITY_Z; + controller_->input_ref_.writeFromNonRT(msg); + const auto age_of_last_command = + controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; + + // age_of_last_command > ref_timeout_ + ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_); + ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); + ASSERT_EQ( + controller_->update_reference_from_subscribers( + controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + ASSERT_EQ( + controller_->update_and_write_commands( + controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + // EXPECT_TRUE(std::isnan(joint_command_values_[NR_CMD_ITFS - 2])); + EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[0])); + + EXPECT_EQ(joint_command_values_[1], 0); + // ASSERT_EQ(controller_->reference_interfaces_[0], 0); + for (const auto & interface : controller_->reference_interfaces_) + { + // ASSERT_EQ(interface, 0); + EXPECT_TRUE(std::isnan(interface)); + } + + std::shared_ptr msg_2 = std::make_shared(); + msg_2->header.stamp = controller_->get_node()->now() - rclcpp::Duration::from_seconds(0.01); + msg_2->twist.linear.x = TEST_LINEAR_VELOCITY_X; + msg_2->twist.linear.y = TEST_LINEAR_VELOCITY_y; + msg_2->twist.linear.z = std::numeric_limits::quiet_NaN(); + msg_2->twist.angular.x = std::numeric_limits::quiet_NaN(); + msg_2->twist.angular.y = std::numeric_limits::quiet_NaN(); + msg_2->twist.angular.z = TEST_ANGULAR_VELOCITY_Z; + controller_->input_ref_.writeFromNonRT(msg_2); + const auto age_of_last_command_2 = + controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; + + // age_of_last_command_2 < ref_timeout_ + ASSERT_TRUE(age_of_last_command_2 <= controller_->ref_timeout_); + ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); + ASSERT_EQ( + controller_->update_reference_from_subscribers( + controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + ASSERT_EQ( + controller_->update_and_write_commands( + controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_NE(joint_command_values_[1], 111); + EXPECT_EQ(joint_command_values_[1], 99); + ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_FALSE(std::isnan(interface)); + } +} + +TEST_F(MecanumDriveControllerTest, test_update_logic) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto reference = controller_->input_ref_.readFromNonRT(); + + // set command statically + static constexpr double TEST_LINEAR_VELOCITY_X = 1.5; + static constexpr double TEST_LINEAR_VELOCITY_y = 1.4; + static constexpr double TEST_ANGULAR_VELOCITY_Z = 1.1; + joint_command_values_[1] = 111; + std::shared_ptr msg = std::make_shared(); + msg->header.stamp = controller_->get_node()->now() - controller_->ref_timeout_ - + rclcpp::Duration::from_seconds(0.1); + msg->twist.linear.x = TEST_LINEAR_VELOCITY_X; + msg->twist.linear.y = 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; + + ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_); + ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); + ASSERT_EQ( + controller_->update_reference_from_subscribers( + controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + ASSERT_EQ( + controller_->update_and_write_commands( + controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_EQ(joint_command_values_[1], 0); + EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[0])); + + std::shared_ptr msg_2 = std::make_shared(); + msg_2->header.stamp = controller_->get_node()->now(); + msg_2->twist.linear.x = TEST_LINEAR_VELOCITY_X; + msg_2->twist.linear.y = TEST_LINEAR_VELOCITY_y; + msg_2->twist.linear.z = std::numeric_limits::quiet_NaN(); + msg_2->twist.angular.x = std::numeric_limits::quiet_NaN(); + msg_2->twist.angular.y = std::numeric_limits::quiet_NaN(); + msg_2->twist.angular.z = TEST_ANGULAR_VELOCITY_Z; + controller_->input_ref_.writeFromNonRT(msg_2); + const auto age_of_last_command_2 = + controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; + + ASSERT_TRUE(age_of_last_command_2 <= controller_->ref_timeout_); + ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); + ASSERT_EQ( + controller_->update_reference_from_subscribers( + controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + ASSERT_EQ( + controller_->update_and_write_commands( + controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + // EXPECT_EQ(joint_command_values_[NR_CMD_ITFS - 2], TEST_LINEAR_VELOCITY_X); + EXPECT_FALSE(std::isnan(joint_command_values_[1])); + EXPECT_NE(joint_command_values_[1], 111); + EXPECT_EQ(joint_command_values_[1], 99); + + ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); +} + +TEST_F(MecanumDriveControllerTest, test_ref_timeout_zero_for_update) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto reference = controller_->input_ref_.readFromNonRT(); + + // set command statically + static constexpr double TEST_LINEAR_VELOCITY_X = 1.5; + static constexpr double TEST_LINEAR_VELOCITY_y = 1.4; + static constexpr double TEST_ANGULAR_VELOCITY_Z = 1.1; + joint_command_values_[1] = 111; + + controller_->ref_timeout_ = rclcpp::Duration::from_seconds(0.0); + std::shared_ptr msg = std::make_shared(); + + msg->header.stamp = controller_->get_node()->now() - rclcpp::Duration::from_seconds(0.0); + msg->twist.linear.x = TEST_LINEAR_VELOCITY_X; + msg->twist.linear.y = 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; + + ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); + ASSERT_EQ( + controller_->update_reference_from_subscribers( + controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + ASSERT_EQ( + controller_->update_and_write_commands( + controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + // EXPECT_EQ(joint_command_values_[NR_STATE_ITFS - 2], TEST_LINEAR_VELOCITY_X); + EXPECT_FALSE(std::isnan(joint_command_values_[1])); + EXPECT_NE(joint_command_values_[1], 111); + EXPECT_EQ(joint_command_values_[1], 99); + ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); + ASSERT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x)); +} + +TEST_F(MecanumDriveControllerTest, test_ref_timeout_zero_for_reference_callback) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x)); + EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.y)); + EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z)); + controller_->ref_timeout_ = rclcpp::Duration::from_seconds(0.0); + + publish_commands(controller_->get_node()->now()); + + ASSERT_TRUE(controller_->wait_for_commands(executor)); + + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x)); + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.y)); + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z)); + EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x, 0.45); + EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.linear.y, 0.45); + EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z, 0.0); +} int main(int argc, char ** argv) diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp index 64a989498f..d05ff08c54 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp @@ -50,27 +50,27 @@ constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR; class TestableMecanumDriveController : public mecanum_drive_controller::MecanumDriveController { FRIEND_TEST(MecanumDriveControllerTest, all_parameters_set_configure_success); - // FRIEND_TEST(MecanumDriveControllerTest, check_exported_intefaces); - // FRIEND_TEST(MecanumDriveControllerTest, activate_success); - // FRIEND_TEST(MecanumDriveControllerTest, update_success); - // FRIEND_TEST(MecanumDriveControllerTest, deactivate_success); - // FRIEND_TEST(MecanumDriveControllerTest, reactivate_success); - // FRIEND_TEST(MecanumDriveControllerTest, publish_status_success); - // FRIEND_TEST(MecanumDriveControllerTest, receive_message_and_publish_updated_status); - // FRIEND_TEST(MecanumDriveControllerTest, test_message_timeout); - // FRIEND_TEST(MecanumDriveControllerTest, test_message_wrong_num_joints); - // FRIEND_TEST(MecanumDriveControllerTest, test_message_accepted); - // FRIEND_TEST(MecanumDriveControllerTest, test_update_logic); - // FRIEND_TEST(MecanumDriveControllerTest, test_ref_timeout_zero_for_update); - // FRIEND_TEST(MecanumDriveControllerTest, test_ref_timeout_zero_for_reference_callback); + FRIEND_TEST(MecanumDriveControllerTest, check_exported_intefaces); + FRIEND_TEST(MecanumDriveControllerTest, activate_success); + FRIEND_TEST(MecanumDriveControllerTest, update_success); + FRIEND_TEST(MecanumDriveControllerTest, deactivate_success); + FRIEND_TEST(MecanumDriveControllerTest, reactivate_success); + FRIEND_TEST(MecanumDriveControllerTest, publish_status_success); + FRIEND_TEST(MecanumDriveControllerTest, receive_message_and_publish_updated_status); + FRIEND_TEST(MecanumDriveControllerTest, test_sending_too_old_message); + FRIEND_TEST(MecanumDriveControllerTest, test_time_stamp_zero); + FRIEND_TEST(MecanumDriveControllerTest, test_message_accepted); + FRIEND_TEST(MecanumDriveControllerTest, test_update_logic_chainable); + FRIEND_TEST(MecanumDriveControllerTest, test_update_logic); + FRIEND_TEST(MecanumDriveControllerTest, test_ref_timeout_zero_for_update); + FRIEND_TEST(MecanumDriveControllerTest, test_ref_timeout_zero_for_reference_callback); public: controller_interface::CallbackReturn on_configure( const rclcpp_lifecycle::State & previous_state) override - { fprintf(stderr," here_ on_configure _1"); + { auto ret = mecanum_drive_controller::MecanumDriveController::on_configure(previous_state); // Only if on_configure is successful create subscription - fprintf(stderr," here_ on_configure _2"); if (ret == CallbackReturn::SUCCESS) { ref_subscriber_wait_set_.add_subscription(ref_subscriber_); } @@ -129,7 +129,7 @@ class MecanumDriveControllerFixture : public ::testing::Test command_publisher_node_ = std::make_shared("command_publisher"); command_publisher_ = command_publisher_node_->create_publisher( - "/test_mecanum_drive_controller/commands", rclcpp::SystemDefaultsQoS()); + "/test_mecanum_drive_controller/reference", rclcpp::SystemDefaultsQoS()); odom_s_publisher_node_ = std::make_shared("odom_s_publisher"); odom_s_publisher_ = odom_s_publisher_node_->create_publisher( @@ -177,13 +177,12 @@ class MecanumDriveControllerFixture : public ::testing::Test } void subscribe_and_get_messages(ControllerStateMsg & msg) - { fprintf(stderr," here_publish_1"); + { // 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_mecanum_drive_controller/controller_state", 10, subs_callback); - fprintf(stderr," here_publish_2"); // call update to publish the test value ASSERT_EQ( controller_->update_reference_from_subscribers( @@ -193,13 +192,11 @@ class MecanumDriveControllerFixture : public ::testing::Test controller_->update_and_write_commands( controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - fprintf(stderr," here_publish_3"); // 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); - fprintf(stderr," here_publish_4"); while (max_sub_check_loop_count--) { controller_->update_reference_from_subscribers( controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)); @@ -210,14 +207,12 @@ class MecanumDriveControllerFixture : public ::testing::Test break; } } - fprintf(stderr," here_publish_5"); 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)); - fprintf(stderr," here_publish_6"); } // TODO(anyone): add/remove arguments as it suites your command message type From c5e26dbeeb455489f4433392fb518a8a3e2ccf47 Mon Sep 17 00:00:00 2001 From: GiridharBukka Date: Sat, 7 Jan 2023 10:33:41 +0530 Subject: [PATCH 03/91] reorganized publishers added code to print values for debug --- .../src/mecanum_drive_controller.cpp | 87 +++++++++---------- mecanum_drive_controller/src/odometry.cpp | 20 ++++- .../test/mecanum_drive_controller_params.yaml | 2 +- 3 files changed, 63 insertions(+), 46 deletions(-) diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index 597e63a689..c6c130ba23 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -346,8 +346,50 @@ controller_interface::return_type MecanumDriveController::update_and_write_comma odometry_.update(wheel0_vel, wheel1_vel, wheel2_vel, wheel3_vel, period.seconds()); } - + // INVERSE KINEMATICS (move robot). + // Compute wheels velocities (this is the actual ik): + // NOTE: the input desired twist (from topic /cmd_vel) is a body twist. + auto current_ref = input_ref_.readFromRT(); + const auto age_of_last_command = time - (*current_ref)->header.stamp; + if (age_of_last_command <= ref_timeout_ || + ref_timeout_ == rclcpp::Duration::from_seconds(0)) + { + tf2::Quaternion orientation_2; + orientation_2.setRPY(0.0, 0.0, params_.base_frame_offset[2]); + + tf2::Matrix3x3 R_b_c = tf2::Matrix3x3((orientation_2)); + tf2::Vector3 v_Ob_b_b0_c = R_b_c * tf2::Vector3(reference_interfaces_[0], reference_interfaces_[1], 0.0); + tf2::Vector3 Ob_c = tf2::Vector3(params_.base_frame_offset[0], params_.base_frame_offset[1], 0.0); + + double vx_Oc_c_c0_c_ = v_Ob_b_b0_c.x() + Ob_c.y() * reference_interfaces_[2]; + double vy_Oc_c_c0_c_ = v_Ob_b_b0_c.y() - Ob_c.x() * reference_interfaces_[2]; + double wz_c_c0_c_ = reference_interfaces_[2]; + + double w0_vel = 1.0 / params_.wheels_radius * (vx_Oc_c_c0_c_ - vy_Oc_c_c0_c_ - params_.wheels_k * wz_c_c0_c_); + double w1_vel = 1.0 / params_.wheels_radius * (vx_Oc_c_c0_c_ + vy_Oc_c_c0_c_ - params_.wheels_k * wz_c_c0_c_); + double w2_vel = 1.0 / params_.wheels_radius * (vx_Oc_c_c0_c_ - vy_Oc_c_c0_c_ + params_.wheels_k * wz_c_c0_c_); + double w3_vel = 1.0 / params_.wheels_radius * (vx_Oc_c_c0_c_ + vy_Oc_c_c0_c_ + params_.wheels_k * wz_c_c0_c_); + + // Set wheels velocities: + + command_interfaces_[0].set_value(w0_vel); + // command_interfaces_[1].set_value(w1_vel); + command_interfaces_[1].set_value(99); + command_interfaces_[2].set_value(w2_vel); + command_interfaces_[3].set_value(w3_vel); + + } else { + reference_interfaces_[0] = std::numeric_limits::quiet_NaN(); + reference_interfaces_[1] = std::numeric_limits::quiet_NaN(); + reference_interfaces_[2] = std::numeric_limits::quiet_NaN(); + command_interfaces_[0].set_value(0.0); + // command_interfaces_[1].set_value(w1_vel); + command_interfaces_[1].set_value(0.0); + command_interfaces_[2].set_value(0.0); + command_interfaces_[3].set_value(0.0); + } + // Publish odometry message // Compute and store orientation info tf2::Quaternion orientation; @@ -397,49 +439,6 @@ controller_interface::return_type MecanumDriveController::update_and_write_comma controller_state_publisher_->unlockAndPublish(); } - // INVERSE KINEMATICS (move robot). - // Compute wheels velocities (this is the actual ik): - // NOTE: the input desired twist (from topic /cmd_vel) is a body twist. - auto current_ref = input_ref_.readFromRT(); - const auto age_of_last_command = time - (*current_ref)->header.stamp; - if (age_of_last_command <= ref_timeout_ || - ref_timeout_ == rclcpp::Duration::from_seconds(0)) - { - tf2::Quaternion orientation_2; - orientation_2.setRPY(0.0, 0.0, params_.base_frame_offset[2]); - - tf2::Matrix3x3 R_b_c = tf2::Matrix3x3((orientation_2)); - tf2::Vector3 v_Ob_b_b0_c = R_b_c * tf2::Vector3(reference_interfaces_[0], reference_interfaces_[1], 0.0); - tf2::Vector3 Ob_c = tf2::Vector3(params_.base_frame_offset[0], params_.base_frame_offset[1], 0.0); - - double vx_Oc_c_c0_c_ = v_Ob_b_b0_c.x() + Ob_c.y() * reference_interfaces_[2]; - double vy_Oc_c_c0_c_ = v_Ob_b_b0_c.y() - Ob_c.x() * reference_interfaces_[2]; - double wz_c_c0_c_ = reference_interfaces_[2]; - - double w0_vel = 1.0 / params_.wheels_radius * (vx_Oc_c_c0_c_ - vy_Oc_c_c0_c_ - params_.wheels_k * wz_c_c0_c_); - double w1_vel = 1.0 / params_.wheels_radius * (vx_Oc_c_c0_c_ + vy_Oc_c_c0_c_ - params_.wheels_k * wz_c_c0_c_); - double w2_vel = 1.0 / params_.wheels_radius * (vx_Oc_c_c0_c_ - vy_Oc_c_c0_c_ + params_.wheels_k * wz_c_c0_c_); - double w3_vel = 1.0 / params_.wheels_radius * (vx_Oc_c_c0_c_ + vy_Oc_c_c0_c_ + params_.wheels_k * wz_c_c0_c_); - - // Set wheels velocities: - - command_interfaces_[0].set_value(w0_vel); - // command_interfaces_[1].set_value(w1_vel); - command_interfaces_[1].set_value(99); - command_interfaces_[2].set_value(w2_vel); - command_interfaces_[3].set_value(w3_vel); - - } else { - reference_interfaces_[0] = std::numeric_limits::quiet_NaN(); - reference_interfaces_[1] = std::numeric_limits::quiet_NaN(); - reference_interfaces_[2] = std::numeric_limits::quiet_NaN(); - command_interfaces_[0].set_value(0.0); - // command_interfaces_[1].set_value(w1_vel); - command_interfaces_[1].set_value(0.0); - command_interfaces_[2].set_value(0.0); - command_interfaces_[3].set_value(0.0); - } - return controller_interface::return_type::OK; } diff --git a/mecanum_drive_controller/src/odometry.cpp b/mecanum_drive_controller/src/odometry.cpp index 5339ca34aa..edda0b55bb 100644 --- a/mecanum_drive_controller/src/odometry.cpp +++ b/mecanum_drive_controller/src/odometry.cpp @@ -5,7 +5,7 @@ #include - +using namespace std; namespace mecanum_drive_controller { namespace bacc = boost::accumulators; @@ -60,6 +60,22 @@ bool Odometry::update(double wheel0_vel, double wheel1_vel, double wheel2_vel, d tf2::Vector3 v_Oc_c_c0_b = R_c_b * tf2::Vector3(vx_Oc_c_c0_c, vy_Oc_c_c0_c, 0.0); tf2::Vector3 Oc_b = R_c_b * tf2::Vector3(-base_frame_offset_[0], -base_frame_offset_[1], 0.0); + // fprintf(stderr," R_c_b = %f", R_c_b[2][2]); + + // for(int x=0;x<3;x++) // loop 3 times for three lines + // { + // for(int y=0;y<3;y++) // loop for the three elements on the line + // { + // cout< Date: Sun, 8 Jan 2023 09:25:58 +0530 Subject: [PATCH 04/91] added meaning full param values --- .../src/mecanum_drive_controller.cpp | 9 +++-- mecanum_drive_controller/src/odometry.cpp | 4 +- .../test/test_mecanum_drive_controller.cpp | 37 ++++++++++--------- .../test/test_mecanum_drive_controller.hpp | 6 +-- 4 files changed, 30 insertions(+), 26 deletions(-) diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index c6c130ba23..5d0a4814a7 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -180,7 +180,7 @@ controller_interface::CallbackReturn MecanumDriveController::on_configure( controller_state_publisher_->lock(); controller_state_publisher_->msg_.header.stamp = get_node()->now(); controller_state_publisher_->msg_.header.frame_id = params_.odom_frame_id; - controller_state_publisher_->msg_.odom.pose.pose.position.x = 0; + controller_state_publisher_->msg_.odom.pose.pose.position.x = 0.0; controller_state_publisher_->msg_.odom.child_frame_id = params_.base_frame_id; controller_state_publisher_->unlock(); auto & covariance_controller = controller_state_publisher_->msg_.odom.twist.covariance; @@ -373,10 +373,13 @@ controller_interface::return_type MecanumDriveController::update_and_write_comma // Set wheels velocities: command_interfaces_[0].set_value(w0_vel); - // command_interfaces_[1].set_value(w1_vel); - command_interfaces_[1].set_value(99); + fprintf(stderr," command_interfaces_[0] = %f", w0_vel); + command_interfaces_[1].set_value(w1_vel); + fprintf(stderr," command_interfaces_[1] = %f", w1_vel); command_interfaces_[2].set_value(w2_vel); + fprintf(stderr," command_interfaces_[2] = %f", w2_vel); command_interfaces_[3].set_value(w3_vel); + fprintf(stderr," command_interfaces_[3] = %f", w3_vel); } else { reference_interfaces_[0] = std::numeric_limits::quiet_NaN(); diff --git a/mecanum_drive_controller/src/odometry.cpp b/mecanum_drive_controller/src/odometry.cpp index edda0b55bb..796bfc4a68 100644 --- a/mecanum_drive_controller/src/odometry.cpp +++ b/mecanum_drive_controller/src/odometry.cpp @@ -93,8 +93,8 @@ bool Odometry::update(double wheel0_vel, double wheel1_vel, double wheel2_vel, d px_b_b0_ += vx_Ob_b_b0_b0.x() * dt; py_b_b0_ += vx_Ob_b_b0_b0.y() * dt; - // fprintf(stderr," px_b_b0_ = %f", px_b_b0_); - // fprintf(stderr," py_b_b0_ = %f", py_b_b0_); + fprintf(stderr," px_b_b0_ = %f", px_b_b0_); + fprintf(stderr," py_b_b0_ = %f", py_b_b0_); return true; } diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp index 5b8f7f9fd9..9a5c67e6f4 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp @@ -175,7 +175,8 @@ TEST_F(MecanumDriveControllerTest, publish_status_success) ControllerStateMsg msg; subscribe_and_get_messages(msg); - EXPECT_TRUE(std::isnan(msg.odom.pose.pose.position.x)); + EXPECT_FALSE(std::isnan(msg.odom.pose.pose.position.x)); + // EXPECT_EQ(msg.odom.pose.pose.position.x, 0); } TEST_F(MecanumDriveControllerTest, receive_message_and_publish_updated_status) @@ -214,13 +215,13 @@ TEST_F(MecanumDriveControllerTest, receive_message_and_publish_updated_status) controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); //here - EXPECT_EQ(joint_command_values_[1], 99); + EXPECT_EQ(joint_command_values_[1], 3.0); EXPECT_FALSE(std::isnan(joint_command_values_[1])); fprintf(stderr," joint_command_values_[1]= %f", joint_command_values_[1]); subscribe_and_get_messages(msg); - ASSERT_EQ(msg.linear_x_velocity_command, 0.45); + ASSERT_EQ(msg.linear_x_velocity_command, 1.5); } TEST_F(MecanumDriveControllerTest, test_sending_too_old_message) @@ -277,8 +278,8 @@ TEST_F(MecanumDriveControllerTest, test_time_stamp_zero) ASSERT_EQ(old_timestamp.sec, (*(controller_->input_ref_.readFromNonRT()))->header.stamp.sec); EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x)); EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z)); - EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x, 0.45); - EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.linear.y, 0.45); + EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x, 1.5); + EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.linear.y, 0.0); EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z, 0.0); } @@ -302,8 +303,8 @@ TEST_F(MecanumDriveControllerTest, test_message_accepted) ASSERT_TRUE(controller_->wait_for_commands(executor)); EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x)); EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z)); - EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x, 0.45); - EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.linear.y, 0.45); + EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x, 1.5); + EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.linear.y, 0.0); EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z, 0.0); } @@ -321,8 +322,8 @@ TEST_F(MecanumDriveControllerTest, test_update_logic_chainable) // set command statically static constexpr double TEST_LINEAR_VELOCITY_X = 1.5; - static constexpr double TEST_LINEAR_VELOCITY_y = 1.4; - static constexpr double TEST_ANGULAR_VELOCITY_Z = 1.1; + static constexpr double TEST_LINEAR_VELOCITY_y = 0.0; + static constexpr double TEST_ANGULAR_VELOCITY_Z = 0.0; joint_command_values_[1] = 111; std::shared_ptr msg = std::make_shared(); @@ -386,7 +387,7 @@ TEST_F(MecanumDriveControllerTest, test_update_logic_chainable) controller_interface::return_type::OK); EXPECT_NE(joint_command_values_[1], 111); - EXPECT_EQ(joint_command_values_[1], 99); + EXPECT_EQ(joint_command_values_[1], 3.0); ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); for (const auto & interface : controller_->reference_interfaces_) { @@ -407,8 +408,8 @@ TEST_F(MecanumDriveControllerTest, test_update_logic) // set command statically static constexpr double TEST_LINEAR_VELOCITY_X = 1.5; - static constexpr double TEST_LINEAR_VELOCITY_y = 1.4; - static constexpr double TEST_ANGULAR_VELOCITY_Z = 1.1; + static constexpr double TEST_LINEAR_VELOCITY_y = 0.0; + static constexpr double TEST_ANGULAR_VELOCITY_Z = 0.0; joint_command_values_[1] = 111; std::shared_ptr msg = std::make_shared(); msg->header.stamp = controller_->get_node()->now() - controller_->ref_timeout_ - @@ -463,7 +464,7 @@ TEST_F(MecanumDriveControllerTest, test_update_logic) // EXPECT_EQ(joint_command_values_[NR_CMD_ITFS - 2], TEST_LINEAR_VELOCITY_X); EXPECT_FALSE(std::isnan(joint_command_values_[1])); EXPECT_NE(joint_command_values_[1], 111); - EXPECT_EQ(joint_command_values_[1], 99); + EXPECT_EQ(joint_command_values_[1], 3.0); ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); } @@ -480,8 +481,8 @@ TEST_F(MecanumDriveControllerTest, test_ref_timeout_zero_for_update) // set command statically static constexpr double TEST_LINEAR_VELOCITY_X = 1.5; - static constexpr double TEST_LINEAR_VELOCITY_y = 1.4; - static constexpr double TEST_ANGULAR_VELOCITY_Z = 1.1; + static constexpr double TEST_LINEAR_VELOCITY_y = 0.0; + static constexpr double TEST_ANGULAR_VELOCITY_Z = 0.0; joint_command_values_[1] = 111; controller_->ref_timeout_ = rclcpp::Duration::from_seconds(0.0); @@ -511,7 +512,7 @@ TEST_F(MecanumDriveControllerTest, test_ref_timeout_zero_for_update) // EXPECT_EQ(joint_command_values_[NR_STATE_ITFS - 2], TEST_LINEAR_VELOCITY_X); EXPECT_FALSE(std::isnan(joint_command_values_[1])); EXPECT_NE(joint_command_values_[1], 111); - EXPECT_EQ(joint_command_values_[1], 99); + EXPECT_EQ(joint_command_values_[1], 3.0); ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); ASSERT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x)); } @@ -536,8 +537,8 @@ TEST_F(MecanumDriveControllerTest, test_ref_timeout_zero_for_reference_callback) EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x)); EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.y)); EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z)); - EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x, 0.45); - EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.linear.y, 0.45); + EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x, 1.5); + EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.linear.y, 0.0); EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z, 0.0); } diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp index d05ff08c54..df69ae487b 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp @@ -217,7 +217,7 @@ class MecanumDriveControllerFixture : public ::testing::Test // TODO(anyone): add/remove arguments as it suites your command message type void publish_commands( - const rclcpp::Time & stamp, const double & twist_linear_x = 0.45, const double & twist_linear_y = 0.45, + const rclcpp::Time & stamp, const double & twist_linear_x = 1.5, const double & twist_linear_y = 0.0, const double & twist_angular_z = 0.0) { auto wait_for_topic = [&](const auto topic_name) { @@ -257,8 +257,8 @@ class MecanumDriveControllerFixture : public ::testing::Test // Controller-related parameters - std::array joint_state_values_ = {1.1, 2.1, 3.1, 4.1}; - std::array joint_command_values_ = {101.101, 201.201, 301.301, 401.401}; + std::array joint_state_values_ = {0.1, 0.1, 0.1, 0.1}; + std::array joint_command_values_ = {101.101, 101.101, 101.101, 101.101}; std::vector state_itfs_; std::vector command_itfs_; From fd91be73526005bb12b7ef373633507b6b46fa2d Mon Sep 17 00:00:00 2001 From: GiridharBukka Date: Sun, 8 Jan 2023 09:34:26 +0530 Subject: [PATCH 05/91] next line in comment --- mecanum_drive_controller/src/mecanum_drive_controller.cpp | 8 ++++---- mecanum_drive_controller/src/odometry.cpp | 8 ++++---- .../test/test_mecanum_drive_controller.cpp | 3 +-- 3 files changed, 9 insertions(+), 10 deletions(-) diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index 5d0a4814a7..2103917479 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -373,13 +373,13 @@ controller_interface::return_type MecanumDriveController::update_and_write_comma // Set wheels velocities: command_interfaces_[0].set_value(w0_vel); - fprintf(stderr," command_interfaces_[0] = %f", w0_vel); + fprintf(stderr," command_interfaces_[0] = %f \n", w0_vel); command_interfaces_[1].set_value(w1_vel); - fprintf(stderr," command_interfaces_[1] = %f", w1_vel); + fprintf(stderr," command_interfaces_[1] = %f \n", w1_vel); command_interfaces_[2].set_value(w2_vel); - fprintf(stderr," command_interfaces_[2] = %f", w2_vel); + fprintf(stderr," command_interfaces_[2] = %f \n", w2_vel); command_interfaces_[3].set_value(w3_vel); - fprintf(stderr," command_interfaces_[3] = %f", w3_vel); + fprintf(stderr," command_interfaces_[3] = %f \n", w3_vel); } else { reference_interfaces_[0] = std::numeric_limits::quiet_NaN(); diff --git a/mecanum_drive_controller/src/odometry.cpp b/mecanum_drive_controller/src/odometry.cpp index 796bfc4a68..faf4cbbc44 100644 --- a/mecanum_drive_controller/src/odometry.cpp +++ b/mecanum_drive_controller/src/odometry.cpp @@ -66,14 +66,14 @@ bool Odometry::update(double wheel0_vel, double wheel1_vel, double wheel2_vel, d // { // for(int y=0;y<3;y++) // loop for the three elements on the line // { - // cout<update_and_write_commands( controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - fprintf(stderr," reactivate_success _1"); } @@ -217,7 +216,7 @@ TEST_F(MecanumDriveControllerTest, receive_message_and_publish_updated_status) //here EXPECT_EQ(joint_command_values_[1], 3.0); EXPECT_FALSE(std::isnan(joint_command_values_[1])); - fprintf(stderr," joint_command_values_[1]= %f", joint_command_values_[1]); + fprintf(stderr," joint_command_values_[1]= %f \n", joint_command_values_[1]); subscribe_and_get_messages(msg); From 99413af945b28b65e09e178e066c63b1b4f729e2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Mon, 9 Jan 2023 17:33:33 +0100 Subject: [PATCH 06/91] Automated formatting files. --- .../msg/MecanumDriveControllerState.msg | 2 +- mecanum_drive_controller/CMakeLists.txt | 4 +- .../mecanum_drive_controller.hpp | 15 +- .../mecanum_drive_controller/odometry.hpp | 35 +-- ...te_mecanum_drive_controller_parameters.hpp | 3 +- mecanum_drive_controller/package.xml | 2 +- .../src/mecanum_drive_controller.cpp | 235 ++++++++++-------- .../src/mecanum_drive_controller.yaml | 2 +- mecanum_drive_controller/src/odometry.cpp | 47 ++-- .../test/mecanum_drive_controller_params.yaml | 2 +- .../test_load_mecanum_drive_controller.cpp | 4 +- .../test/test_mecanum_drive_controller.cpp | 40 ++- .../test/test_mecanum_drive_controller.hpp | 61 +++-- ...st_mecanum_drive_controller_preceeding.cpp | 12 +- 14 files changed, 240 insertions(+), 224 deletions(-) diff --git a/custom_messages/msg/MecanumDriveControllerState.msg b/custom_messages/msg/MecanumDriveControllerState.msg index f01a67ecb0..a4a6bd0a19 100644 --- a/custom_messages/msg/MecanumDriveControllerState.msg +++ b/custom_messages/msg/MecanumDriveControllerState.msg @@ -6,4 +6,4 @@ float64 back_right_wheel_velocity float64 front_right_wheel_velocity float64 linear_x_velocity_command float64 linear_y_velocity_command -float64 angular_z_command \ No newline at end of file +float64 angular_z_command diff --git a/mecanum_drive_controller/CMakeLists.txt b/mecanum_drive_controller/CMakeLists.txt index 62da8f9088..8d2b5d1982 100644 --- a/mecanum_drive_controller/CMakeLists.txt +++ b/mecanum_drive_controller/CMakeLists.txt @@ -117,10 +117,10 @@ ament_export_include_directories( include ) ament_export_dependencies( - + ) ament_export_libraries( mecanum_drive_controller ) -ament_package() \ No newline at end of file +ament_package() diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp index 8fb65940b7..e6e96c65df 100644 --- a/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp +++ b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp @@ -16,17 +16,17 @@ #define MECANUM_DRIVE_CONTROLLER__MECANUM_DRIVE_CONTROLLER_HPP_ #include -#include -#include -#include #include +#include #include +#include #include +#include -#include "mecanum_drive_controller/odometry.hpp" #include "controller_interface/chainable_controller_interface.hpp" -#include "mecanum_drive_controller_parameters.hpp" +#include "mecanum_drive_controller/odometry.hpp" #include "mecanum_drive_controller/visibility_control.h" +#include "mecanum_drive_controller_parameters.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/realtime_buffer.h" @@ -49,7 +49,6 @@ static constexpr size_t NR_CMD_ITFS = 4; // name constants for reference interfaces static constexpr size_t NR_REF_ITFS = 3; - class MecanumDriveController : public controller_interface::ChainableControllerInterface { public: @@ -91,7 +90,6 @@ class MecanumDriveController : public controller_interface::ChainableControllerI using TfStateMsg = tf2_msgs::msg::TFMessage; using ControllerStateMsg = custom_messages::msg::MecanumDriveControllerState; - protected: std::shared_ptr param_listener_; mecanum_drive_controller::Params params_; @@ -105,7 +103,6 @@ class MecanumDriveController : public controller_interface::ChainableControllerI rclcpp::Publisher::SharedPtr odom_s_publisher_; std::unique_ptr rt_odom_state_publisher_; - using TfStatePublisher = realtime_tools::RealtimePublisher; rclcpp::Publisher::SharedPtr tf_odom_s_publisher_; std::unique_ptr rt_tf_odom_state_publisher_; @@ -121,7 +118,7 @@ class MecanumDriveController : public controller_interface::ChainableControllerI // Odometry related: Odometry odometry_; - + private: // callback for topic interface TEMPLATES__ROS2_CONTROL__VISIBILITY_LOCAL diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp b/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp index 661988433c..a7e44666b8 100644 --- a/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp +++ b/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp @@ -6,8 +6,8 @@ #include "realtime_tools/realtime_publisher.h" #include -#include #include +#include #include #define PLANAR_POINT_DIM 3 @@ -41,38 +41,21 @@ class Odometry /// \param wheel3_vel Wheel velocity [rad/s] /// \param time Current time /// \return true if the odometry is actually updated - bool update(double wheel0_vel, double wheel1_vel, double wheel2_vel, double wheel3_vel, const double dt); + bool update( + double wheel0_vel, double wheel1_vel, double wheel2_vel, double wheel3_vel, const double dt); /// \return position (x component) [m] - double getX() const - { - return px_b_b0_; - } + double getX() const { return px_b_b0_; } /// \return position (y component) [m] - double getY() const - { - return py_b_b0_; - } + double getY() const { return py_b_b0_; } /// \return orientation (z component) [m] - double getRz() const - { - return rz_b_b0_; - } + double getRz() const { return rz_b_b0_; } /// \return body velocity of the base frame (linear x component) [m/s] - double getVx() const - { - return vx_Ob_b_b0_b_; - } + double getVx() const { return vx_Ob_b_b0_b_; } /// \return body velocity of the base frame (linear y component) [m/s] - double getVy() const - { - return vy_Ob_b_b0_b_; - } + double getVy() const { return vy_Ob_b_b0_b_; } /// \return body velocity of the base frame (angular z component) [m/s] - double getWz() const - { - return wz_b_b0_b_; - } + double getWz() const { return wz_b_b0_b_; } /// \brief Sets the wheels parameters: mecanum geometric param and radius /// \param wheels_k Wheels geometric param (used in mecanum wheels' ik) [m] diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/validate_mecanum_drive_controller_parameters.hpp b/mecanum_drive_controller/include/mecanum_drive_controller/validate_mecanum_drive_controller_parameters.hpp index 2f86a8507f..adcc7eea88 100644 --- a/mecanum_drive_controller/include/mecanum_drive_controller/validate_mecanum_drive_controller_parameters.hpp +++ b/mecanum_drive_controller/include/mecanum_drive_controller/validate_mecanum_drive_controller_parameters.hpp @@ -27,7 +27,8 @@ tl::expected forbidden_interface_name_prefix(rclcpp::Paramete { auto const & interface_name = parameter.as_string(); - if (interface_name.rfind("blup_", 0) == 0) { + if (interface_name.rfind("blup_", 0) == 0) + { return tl::make_unexpected("'interface_name' parameter can not start with 'blup_'"); } diff --git a/mecanum_drive_controller/package.xml b/mecanum_drive_controller/package.xml index 5abb337f43..7bd95282af 100644 --- a/mecanum_drive_controller/package.xml +++ b/mecanum_drive_controller/package.xml @@ -14,7 +14,7 @@ control_msgs controller_interface - + geometry_msgs hardware_interface diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index 2103917479..6f6b10d361 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -26,30 +26,31 @@ #include "tf2/transform_datatypes.h" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" -namespace { // utility +namespace +{ // utility // TODO(destogl): remove this when merged upstream // Changed services history QoS to keep all so we don't lose any client service // calls static constexpr rmw_qos_profile_t rmw_qos_profile_services_hist_keep_all = { - RMW_QOS_POLICY_HISTORY_KEEP_ALL, - 1, // message queue depth - RMW_QOS_POLICY_RELIABILITY_RELIABLE, - RMW_QOS_POLICY_DURABILITY_VOLATILE, - RMW_QOS_DEADLINE_DEFAULT, - RMW_QOS_LIFESPAN_DEFAULT, - RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT, - RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT, - false}; + RMW_QOS_POLICY_HISTORY_KEEP_ALL, + 1, // message queue depth + RMW_QOS_POLICY_RELIABILITY_RELIABLE, + RMW_QOS_POLICY_DURABILITY_VOLATILE, + RMW_QOS_DEADLINE_DEFAULT, + RMW_QOS_LIFESPAN_DEFAULT, + RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT, + RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT, + false}; using ControllerReferenceMsg = - mecanum_drive_controller::MecanumDriveController::ControllerReferenceMsg; + mecanum_drive_controller::MecanumDriveController::ControllerReferenceMsg; // called from RT control loop void reset_controller_reference_msg( - const std::shared_ptr& msg, - const std::shared_ptr& node) { - + 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(); @@ -61,18 +62,22 @@ void reset_controller_reference_msg( } // namespace -namespace mecanum_drive_controller { +namespace mecanum_drive_controller +{ MecanumDriveController::MecanumDriveController() - : controller_interface::ChainableControllerInterface() {} - -controller_interface::CallbackReturn MecanumDriveController::on_init() { - try { - param_listener_ = - std::make_shared(get_node()); - } catch (const std::exception& e) { - fprintf(stderr, - "Exception thrown during controller's init with message: %s \n", - e.what()); +: controller_interface::ChainableControllerInterface() +{ +} + +controller_interface::CallbackReturn MecanumDriveController::on_init() +{ + try + { + param_listener_ = std::make_shared(get_node()); + } + catch (const std::exception & e) + { + fprintf(stderr, "Exception thrown during controller's init with message: %s \n", e.what()); return controller_interface::CallbackReturn::ERROR; } @@ -80,11 +85,12 @@ controller_interface::CallbackReturn MecanumDriveController::on_init() { } controller_interface::CallbackReturn MecanumDriveController::on_configure( - const rclcpp_lifecycle::State& /*previous_state*/) { + const rclcpp_lifecycle::State & /*previous_state*/) +{ params_ = param_listener_->get_params(); // Set wheel params for the odometry computation - odometry_.setWheelsParams(params_.wheels_k, params_.wheels_radius); + odometry_.setWheelsParams(params_.wheels_k, params_.wheels_radius); // topics QoS auto subscribers_qos = rclcpp::SystemDefaultsQoS(); @@ -94,20 +100,18 @@ controller_interface::CallbackReturn MecanumDriveController::on_configure( // Reference Subscriber ref_timeout_ = rclcpp::Duration::from_seconds(params_.reference_timeout); ref_subscriber_ = get_node()->create_subscription( - "~/reference", subscribers_qos, - std::bind(&MecanumDriveController::reference_callback, this, - std::placeholders::_1)); + "~/reference", subscribers_qos, + std::bind(&MecanumDriveController::reference_callback, this, std::placeholders::_1)); - std::shared_ptr msg = - std::make_shared(); - reset_controller_reference_msg(msg,get_node()); + 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()); + 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) @@ -139,10 +143,9 @@ controller_interface::CallbackReturn MecanumDriveController::on_configure( 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_); + 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) { @@ -160,17 +163,21 @@ controller_interface::CallbackReturn MecanumDriveController::on_configure( rt_tf_odom_state_publisher_->msg_.transforms[0].transform.translation.z = 0.0; rt_tf_odom_state_publisher_->unlock(); - - try { + try + { // controller 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()); + "~/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; } @@ -196,13 +203,15 @@ controller_interface::CallbackReturn MecanumDriveController::on_configure( return controller_interface::CallbackReturn::SUCCESS; } -void MecanumDriveController::reference_callback( - const std::shared_ptr msg) { +void MecanumDriveController::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."); + 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; @@ -223,8 +232,8 @@ void MecanumDriveController::reference_callback( } controller_interface::InterfaceConfiguration -MecanumDriveController::command_interface_configuration() const { - +MecanumDriveController::command_interface_configuration() const +{ controller_interface::InterfaceConfiguration command_interfaces_config; command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; @@ -238,27 +247,31 @@ MecanumDriveController::command_interface_configuration() const { command_interfaces_config.names.push_back( params_.wheel3_name + "/" + hardware_interface::HW_IF_VELOCITY); - return command_interfaces_config; } -controller_interface::InterfaceConfiguration -MecanumDriveController::state_interface_configuration() const { - +controller_interface::InterfaceConfiguration MecanumDriveController::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); - state_interfaces_config.names.push_back(params_.wheel0_name + "/" + hardware_interface::HW_IF_VELOCITY); - state_interfaces_config.names.push_back(params_.wheel1_name + "/" + hardware_interface::HW_IF_VELOCITY); - state_interfaces_config.names.push_back(params_.wheel2_name + "/" + hardware_interface::HW_IF_VELOCITY); - state_interfaces_config.names.push_back(params_.wheel3_name + "/" + hardware_interface::HW_IF_VELOCITY); + state_interfaces_config.names.push_back( + params_.wheel0_name + "/" + hardware_interface::HW_IF_VELOCITY); + state_interfaces_config.names.push_back( + params_.wheel1_name + "/" + hardware_interface::HW_IF_VELOCITY); + state_interfaces_config.names.push_back( + params_.wheel2_name + "/" + hardware_interface::HW_IF_VELOCITY); + state_interfaces_config.names.push_back( + params_.wheel3_name + "/" + hardware_interface::HW_IF_VELOCITY); return state_interfaces_config; } std::vector -MecanumDriveController::on_export_reference_interfaces() { +MecanumDriveController::on_export_reference_interfaces() +{ reference_interfaces_.resize(NR_REF_ITFS, std::numeric_limits::quiet_NaN()); std::vector reference_interfaces; @@ -279,33 +292,36 @@ MecanumDriveController::on_export_reference_interfaces() { return reference_interfaces; } -bool MecanumDriveController::on_set_chained_mode(bool chained_mode) { +bool MecanumDriveController::on_set_chained_mode(bool chained_mode) +{ // Always accept switch to/from chained mode return true || chained_mode; } controller_interface::CallbackReturn MecanumDriveController::on_activate( - const rclcpp_lifecycle::State& /*previous_state*/) { + const rclcpp_lifecycle::State & /*previous_state*/) +{ // Set default value in command - reset_controller_reference_msg(*(input_ref_.readFromRT()), - get_node()); + reset_controller_reference_msg(*(input_ref_.readFromRT()), get_node()); return controller_interface::CallbackReturn::SUCCESS; } controller_interface::CallbackReturn MecanumDriveController::on_deactivate( - const rclcpp_lifecycle::State& /*previous_state*/) { + const rclcpp_lifecycle::State & /*previous_state*/) +{ // TODO(anyone): depending on number of interfaces, use definitions, e.g., // `CMD_MY_ITFS`, instead of a loop - for (size_t i = 0; i < NR_CMD_ITFS; ++i) { + 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 -MecanumDriveController::update_reference_from_subscribers( - const rclcpp::Time& time, const rclcpp::Duration& /*period*/) { +controller_interface::return_type MecanumDriveController::update_reference_from_subscribers( + const rclcpp::Time & time, const rclcpp::Duration & /*period*/) +{ auto current_ref = *(input_ref_.readFromRT()); const auto age_of_last_command = get_node()->now() - (current_ref)->header.stamp; @@ -332,16 +348,18 @@ MecanumDriveController::update_reference_from_subscribers( } controller_interface::return_type MecanumDriveController::update_and_write_commands( - const rclcpp::Time& time, const rclcpp::Duration& period) { - + const rclcpp::Time & time, const rclcpp::Duration & period) +{ // FORWARD KINEMATICS (odometry). double wheel0_vel = state_interfaces_[0].get_value(); double wheel1_vel = state_interfaces_[1].get_value(); double wheel2_vel = state_interfaces_[2].get_value(); double wheel3_vel = state_interfaces_[3].get_value(); - if (!std::isnan(wheel0_vel) && !std::isnan(wheel1_vel) && !std::isnan(wheel2_vel) && !std::isnan(wheel3_vel)){ - + if ( + !std::isnan(wheel0_vel) && !std::isnan(wheel1_vel) && !std::isnan(wheel2_vel) && + !std::isnan(wheel3_vel)) + { // Estimate twist (using joint information) and integrate odometry_.update(wheel0_vel, wheel1_vel, wheel2_vel, wheel3_vel, period.seconds()); } @@ -351,48 +369,53 @@ controller_interface::return_type MecanumDriveController::update_and_write_comma // NOTE: the input desired twist (from topic /cmd_vel) is a body twist. auto current_ref = input_ref_.readFromRT(); const auto age_of_last_command = time - (*current_ref)->header.stamp; - if (age_of_last_command <= ref_timeout_ || - ref_timeout_ == rclcpp::Duration::from_seconds(0)) + if (age_of_last_command <= ref_timeout_ || ref_timeout_ == rclcpp::Duration::from_seconds(0)) { tf2::Quaternion orientation_2; orientation_2.setRPY(0.0, 0.0, params_.base_frame_offset[2]); tf2::Matrix3x3 R_b_c = tf2::Matrix3x3((orientation_2)); - tf2::Vector3 v_Ob_b_b0_c = R_b_c * tf2::Vector3(reference_interfaces_[0], reference_interfaces_[1], 0.0); - tf2::Vector3 Ob_c = tf2::Vector3(params_.base_frame_offset[0], params_.base_frame_offset[1], 0.0); + tf2::Vector3 v_Ob_b_b0_c = + R_b_c * tf2::Vector3(reference_interfaces_[0], reference_interfaces_[1], 0.0); + tf2::Vector3 Ob_c = + tf2::Vector3(params_.base_frame_offset[0], params_.base_frame_offset[1], 0.0); double vx_Oc_c_c0_c_ = v_Ob_b_b0_c.x() + Ob_c.y() * reference_interfaces_[2]; double vy_Oc_c_c0_c_ = v_Ob_b_b0_c.y() - Ob_c.x() * reference_interfaces_[2]; double wz_c_c0_c_ = reference_interfaces_[2]; - double w0_vel = 1.0 / params_.wheels_radius * (vx_Oc_c_c0_c_ - vy_Oc_c_c0_c_ - params_.wheels_k * wz_c_c0_c_); - double w1_vel = 1.0 / params_.wheels_radius * (vx_Oc_c_c0_c_ + vy_Oc_c_c0_c_ - params_.wheels_k * wz_c_c0_c_); - double w2_vel = 1.0 / params_.wheels_radius * (vx_Oc_c_c0_c_ - vy_Oc_c_c0_c_ + params_.wheels_k * wz_c_c0_c_); - double w3_vel = 1.0 / params_.wheels_radius * (vx_Oc_c_c0_c_ + vy_Oc_c_c0_c_ + params_.wheels_k * wz_c_c0_c_); + double w0_vel = + 1.0 / params_.wheels_radius * (vx_Oc_c_c0_c_ - vy_Oc_c_c0_c_ - params_.wheels_k * wz_c_c0_c_); + double w1_vel = + 1.0 / params_.wheels_radius * (vx_Oc_c_c0_c_ + vy_Oc_c_c0_c_ - params_.wheels_k * wz_c_c0_c_); + double w2_vel = + 1.0 / params_.wheels_radius * (vx_Oc_c_c0_c_ - vy_Oc_c_c0_c_ + params_.wheels_k * wz_c_c0_c_); + double w3_vel = + 1.0 / params_.wheels_radius * (vx_Oc_c_c0_c_ + vy_Oc_c_c0_c_ + params_.wheels_k * wz_c_c0_c_); // Set wheels velocities: command_interfaces_[0].set_value(w0_vel); - fprintf(stderr," command_interfaces_[0] = %f \n", w0_vel); + fprintf(stderr, " command_interfaces_[0] = %f \n", w0_vel); command_interfaces_[1].set_value(w1_vel); - fprintf(stderr," command_interfaces_[1] = %f \n", w1_vel); + fprintf(stderr, " command_interfaces_[1] = %f \n", w1_vel); command_interfaces_[2].set_value(w2_vel); - fprintf(stderr," command_interfaces_[2] = %f \n", w2_vel); + fprintf(stderr, " command_interfaces_[2] = %f \n", w2_vel); command_interfaces_[3].set_value(w3_vel); - fprintf(stderr," command_interfaces_[3] = %f \n", w3_vel); - - } else { - reference_interfaces_[0] = std::numeric_limits::quiet_NaN(); - reference_interfaces_[1] = std::numeric_limits::quiet_NaN(); - reference_interfaces_[2] = std::numeric_limits::quiet_NaN(); - command_interfaces_[0].set_value(0.0); - // command_interfaces_[1].set_value(w1_vel); - command_interfaces_[1].set_value(0.0); - command_interfaces_[2].set_value(0.0); - command_interfaces_[3].set_value(0.0); - } + fprintf(stderr, " command_interfaces_[3] = %f \n", w3_vel); + } + else + { + reference_interfaces_[0] = std::numeric_limits::quiet_NaN(); + reference_interfaces_[1] = std::numeric_limits::quiet_NaN(); + reference_interfaces_[2] = std::numeric_limits::quiet_NaN(); + command_interfaces_[0].set_value(0.0); + // command_interfaces_[1].set_value(w1_vel); + command_interfaces_[1].set_value(0.0); + command_interfaces_[2].set_value(0.0); + command_interfaces_[3].set_value(0.0); + } - // Publish odometry message // Compute and store orientation info tf2::Quaternion orientation; @@ -425,7 +448,7 @@ controller_interface::return_type MecanumDriveController::update_and_write_comma if (controller_state_publisher_->trylock()) { controller_state_publisher_->msg_.header.stamp = get_node()->now(); - controller_state_publisher_->msg_.odom.pose.pose.position.x = odometry_.getX(); + controller_state_publisher_->msg_.odom.pose.pose.position.x = odometry_.getX(); controller_state_publisher_->msg_.odom.pose.pose.position.y = odometry_.getY(); controller_state_publisher_->msg_.odom.pose.pose.orientation = tf2::toMsg(orientation); controller_state_publisher_->msg_.odom.twist.twist.linear.x = odometry_.getVx(); @@ -443,12 +466,12 @@ controller_interface::return_type MecanumDriveController::update_and_write_comma } return controller_interface::return_type::OK; - } } // namespace mecanum_drive_controller #include "pluginlib/class_list_macros.hpp" -PLUGINLIB_EXPORT_CLASS(mecanum_drive_controller::MecanumDriveController, - controller_interface::ChainableControllerInterface) +PLUGINLIB_EXPORT_CLASS( + mecanum_drive_controller::MecanumDriveController, + controller_interface::ChainableControllerInterface) diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.yaml b/mecanum_drive_controller/src/mecanum_drive_controller.yaml index 9b10f0da8b..07c5264200 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.yaml +++ b/mecanum_drive_controller/src/mecanum_drive_controller.yaml @@ -97,4 +97,4 @@ mecanum_drive_controller: description: "diagonal values of pose covariance matrix.", read_only: false, - } \ No newline at end of file + } diff --git a/mecanum_drive_controller/src/odometry.cpp b/mecanum_drive_controller/src/odometry.cpp index faf4cbbc44..39661316df 100644 --- a/mecanum_drive_controller/src/odometry.cpp +++ b/mecanum_drive_controller/src/odometry.cpp @@ -3,7 +3,6 @@ #include #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" - #include using namespace std; namespace mecanum_drive_controller @@ -11,19 +10,19 @@ namespace mecanum_drive_controller namespace bacc = boost::accumulators; Odometry::Odometry(size_t velocity_rolling_window_size) - : timestamp_(0.0) - , px_b_b0_(0.0) - , py_b_b0_(0.0) - , rz_b_b0_(0.0) - , vx_Ob_b_b0_b_(0.0) - , vy_Ob_b_b0_b_(0.0) - , wz_b_b0_b_(0.0) - , wheels_k_(0.0) - , wheels_radius_(0.0) +: timestamp_(0.0), + px_b_b0_(0.0), + py_b_b0_(0.0), + rz_b_b0_(0.0), + vx_Ob_b_b0_b_(0.0), + vy_Ob_b_b0_b_(0.0), + wz_b_b0_b_(0.0), + wheels_k_(0.0), + wheels_radius_(0.0) { } -void Odometry::init(const rclcpp::Time& time, double base_frame_offset[PLANAR_POINT_DIM]) +void Odometry::init(const rclcpp::Time & time, double base_frame_offset[PLANAR_POINT_DIM]) { // Reset timestamp: timestamp_ = time; @@ -34,12 +33,12 @@ void Odometry::init(const rclcpp::Time& time, double base_frame_offset[PLANAR_PO base_frame_offset_[2] = base_frame_offset[2]; } -bool Odometry::update(double wheel0_vel, double wheel1_vel, double wheel2_vel, double wheel3_vel, const double dt) +bool Odometry::update( + double wheel0_vel, double wheel1_vel, double wheel2_vel, double wheel3_vel, const double dt) { /// We cannot estimate the speed with very small time intervals: // const double dt = (time - timestamp_).toSec(); - if (dt < 0.0001) - return false; // Interval too small to integrate with + if (dt < 0.0001) return false; // Interval too small to integrate with // timestamp_ = time; @@ -50,12 +49,14 @@ bool Odometry::update(double wheel0_vel, double wheel1_vel, double wheel2_vel, d /// post-processing at will. We prefer this way of doing as filtering introduces delay (which makes it /// difficult to interpret and compare behavior curves). double vx_Oc_c_c0_c = 0.25 * wheels_radius_ * (wheel0_vel + wheel1_vel + wheel2_vel + wheel3_vel); - double vy_Oc_c_c0_c = 0.25 * wheels_radius_ * (-wheel0_vel + wheel1_vel - wheel2_vel + wheel3_vel); - double wz_c_c0_c = 0.25 * wheels_radius_ / wheels_k_ * (-wheel0_vel - wheel1_vel + wheel2_vel + wheel3_vel); + double vy_Oc_c_c0_c = + 0.25 * wheels_radius_ * (-wheel0_vel + wheel1_vel - wheel2_vel + wheel3_vel); + double wz_c_c0_c = + 0.25 * wheels_radius_ / wheels_k_ * (-wheel0_vel - wheel1_vel + wheel2_vel + wheel3_vel); tf2::Quaternion orientation_R_c_b; - orientation_R_c_b.setRPY(0.0, 0.0,-base_frame_offset_[2]); - + orientation_R_c_b.setRPY(0.0, 0.0, -base_frame_offset_[2]); + tf2::Matrix3x3 R_c_b = tf2::Matrix3x3((orientation_R_c_b)); tf2::Vector3 v_Oc_c_c0_b = R_c_b * tf2::Vector3(vx_Oc_c_c0_c, vy_Oc_c_c0_c, 0.0); tf2::Vector3 Oc_b = R_c_b * tf2::Vector3(-base_frame_offset_[0], -base_frame_offset_[1], 0.0); @@ -65,14 +66,14 @@ bool Odometry::update(double wheel0_vel, double wheel1_vel, double wheel2_vel, d // for(int x=0;x<3;x++) // loop 3 times for three lines // { // for(int y=0;y<3;y++) // loop for the three elements on the line - // { + // { // cout< #include @@ -22,15 +22,16 @@ #include using mecanum_drive_controller::NR_CMD_ITFS; -using mecanum_drive_controller::NR_STATE_ITFS; using mecanum_drive_controller::NR_REF_ITFS; +using mecanum_drive_controller::NR_STATE_ITFS; -class MecanumDriveControllerTest : public MecanumDriveControllerFixture +class MecanumDriveControllerTest +: public MecanumDriveControllerFixture { }; TEST_F(MecanumDriveControllerTest, all_parameters_set_configure_success) -{ +{ SetUpController(); ASSERT_EQ(controller_->params_.reference_timeout, 0.0); @@ -46,13 +47,15 @@ TEST_F(MecanumDriveControllerTest, check_exported_intefaces) auto command_intefaces = controller_->command_interface_configuration(); ASSERT_EQ(command_intefaces.names.size(), joint_command_values_.size()); - for (size_t i = 0; i < command_intefaces.names.size(); ++i) { + for (size_t i = 0; i < command_intefaces.names.size(); ++i) + { EXPECT_EQ(command_intefaces.names[i], joint_names_[i] + "/" + interface_name_); } auto state_intefaces = controller_->state_interface_configuration(); ASSERT_EQ(state_intefaces.names.size(), joint_state_values_.size()); - for (size_t i = 0; i < state_intefaces.names.size(); ++i) { + for (size_t i = 0; i < state_intefaces.names.size(); ++i) + { EXPECT_EQ(state_intefaces.names[i], joint_names_[i] + "/" + interface_name_); } @@ -83,7 +86,6 @@ TEST_F(MecanumDriveControllerTest, check_exported_intefaces) EXPECT_EQ( reference_interfaces[2].get_interface_name(), std::string("angular_z") + "/" + hardware_interface::HW_IF_VELOCITY); - } TEST_F(MecanumDriveControllerTest, activate_success) @@ -138,11 +140,11 @@ TEST_F(MecanumDriveControllerTest, reactivate_success) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_EQ(controller_->command_interfaces_[NR_CMD_ITFS-4].get_value(), 101.101); + ASSERT_EQ(controller_->command_interfaces_[NR_CMD_ITFS - 4].get_value(), 101.101); ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_TRUE(std::isnan(controller_->command_interfaces_[NR_CMD_ITFS-4].get_value())); + ASSERT_TRUE(std::isnan(controller_->command_interfaces_[NR_CMD_ITFS - 4].get_value())); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_TRUE(std::isnan(controller_->command_interfaces_[NR_CMD_ITFS-4].get_value())); + ASSERT_TRUE(std::isnan(controller_->command_interfaces_[NR_CMD_ITFS - 4].get_value())); ASSERT_EQ( controller_->update_reference_from_subscribers( @@ -152,11 +154,10 @@ TEST_F(MecanumDriveControllerTest, reactivate_success) controller_->update_and_write_commands( controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - } TEST_F(MecanumDriveControllerTest, publish_status_success) -{ +{ SetUpController(); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); @@ -196,7 +197,7 @@ TEST_F(MecanumDriveControllerTest, receive_message_and_publish_updated_status) controller_->update_and_write_commands( controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); -//here + //here ControllerStateMsg msg; subscribe_and_get_messages(msg); joint_command_values_[1] = 111; @@ -213,10 +214,10 @@ TEST_F(MecanumDriveControllerTest, receive_message_and_publish_updated_status) controller_->update_and_write_commands( controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); -//here + //here EXPECT_EQ(joint_command_values_[1], 3.0); EXPECT_FALSE(std::isnan(joint_command_values_[1])); - fprintf(stderr," joint_command_values_[1]= %f \n", joint_command_values_[1]); + fprintf(stderr, " joint_command_values_[1]= %f \n", joint_command_values_[1]); subscribe_and_get_messages(msg); @@ -243,18 +244,15 @@ TEST_F(MecanumDriveControllerTest, test_sending_too_old_message) controller_->get_node()->now() - controller_->ref_timeout_ - rclcpp::Duration::from_seconds(0.1)); ASSERT_TRUE(controller_->wait_for_commands(executor)); - ASSERT_EQ(old_timestamp, (*(controller_->input_ref_.readFromNonRT()))->header.stamp); + ASSERT_EQ(old_timestamp, (*(controller_->input_ref_.readFromNonRT()))->header.stamp); // ASSERT_EQ((*reference)->twist.linear.x, 0.45); // ASSERT_EQ((*reference)->twist.linear.y, 0.45); // ASSERT_EQ((*reference)->twist.angular.z, 0.0); EXPECT_TRUE(std::isnan((*reference)->twist.linear.x)); EXPECT_TRUE(std::isnan((*reference)->twist.linear.y)); EXPECT_TRUE(std::isnan((*reference)->twist.angular.z)); - } - - TEST_F(MecanumDriveControllerTest, test_time_stamp_zero) { SetUpController(); @@ -272,7 +270,7 @@ TEST_F(MecanumDriveControllerTest, test_time_stamp_zero) EXPECT_TRUE(std::isnan((*reference)->twist.angular.z)); publish_commands(rclcpp::Time(0)); - + ASSERT_TRUE(controller_->wait_for_commands(executor)); ASSERT_EQ(old_timestamp.sec, (*(controller_->input_ref_.readFromNonRT()))->header.stamp.sec); EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x)); @@ -282,7 +280,6 @@ TEST_F(MecanumDriveControllerTest, test_time_stamp_zero) EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z, 0.0); } - TEST_F(MecanumDriveControllerTest, test_message_accepted) { SetUpController(); @@ -541,7 +538,6 @@ TEST_F(MecanumDriveControllerTest, test_ref_timeout_zero_for_reference_callback) EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z, 0.0); } - int main(int argc, char ** argv) { ::testing::InitGoogleTest(&argc, argv); diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp index df69ae487b..72b597f1b0 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp @@ -23,11 +23,11 @@ #include #include -#include "mecanum_drive_controller/mecanum_drive_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 "mecanum_drive_controller/mecanum_drive_controller.hpp" #include "rclcpp/parameter_value.hpp" #include "rclcpp/time.hpp" #include "rclcpp/utilities.hpp" @@ -35,7 +35,8 @@ // TODO(anyone): replace the state and command message types using ControllerStateMsg = mecanum_drive_controller::MecanumDriveController::ControllerStateMsg; -using ControllerReferenceMsg = mecanum_drive_controller::MecanumDriveController::ControllerReferenceMsg; +using ControllerReferenceMsg = + mecanum_drive_controller::MecanumDriveController::ControllerReferenceMsg; using TfStateMsg = mecanum_drive_controller::MecanumDriveController::TfStateMsg; using OdomStateMsg = mecanum_drive_controller::MecanumDriveController::OdomStateMsg; @@ -71,7 +72,8 @@ class TestableMecanumDriveController : public mecanum_drive_controller::MecanumD { auto ret = mecanum_drive_controller::MecanumDriveController::on_configure(previous_state); // Only if on_configure is successful create subscription - if (ret == CallbackReturn::SUCCESS) { + if (ret == CallbackReturn::SUCCESS) + { ref_subscriber_wait_set_.add_subscription(ref_subscriber_); } return ret; @@ -96,7 +98,8 @@ class TestableMecanumDriveController : public mecanum_drive_controller::MecanumD const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) { bool success = subscriber_wait_set.wait(timeout).kind() == rclcpp::WaitResultKind::Ready; - if (success) { + if (success) + { executor.spin_some(); } return success; @@ -135,11 +138,9 @@ class MecanumDriveControllerFixture : public ::testing::Test odom_s_publisher_ = odom_s_publisher_node_->create_publisher( "/test_mecanum_drive_controller/odometry", rclcpp::SystemDefaultsQoS()); - tf_odom_s_publisher_node_ = std::make_shared("tf_odom_s_publisher"); tf_odom_s_publisher_ = tf_odom_s_publisher_node_->create_publisher( "/test_mecanum_drive_controller/tf_odometry", rclcpp::SystemDefaultsQoS()); - } static void TearDownTestCase() {} @@ -154,8 +155,9 @@ class MecanumDriveControllerFixture : public ::testing::Test std::vector command_ifs; command_itfs_.reserve(joint_command_values_.size()); command_ifs.reserve(joint_command_values_.size()); - - for (size_t i = 0; i < joint_command_values_.size(); ++i) { + + for (size_t i = 0; i < joint_command_values_.size(); ++i) + { command_itfs_.emplace_back(hardware_interface::CommandInterface( joint_names_[i], interface_name_, &joint_command_values_[i])); command_ifs.emplace_back(command_itfs_.back()); @@ -166,7 +168,8 @@ class MecanumDriveControllerFixture : public ::testing::Test state_itfs_.reserve(joint_state_values_.size()); state_ifs.reserve(joint_state_values_.size()); - for (size_t i = 0; i < joint_state_values_.size(); ++i) { + for (size_t i = 0; i < joint_state_values_.size(); ++i) + { state_itfs_.emplace_back(hardware_interface::StateInterface( joint_names_[i], interface_name_, &joint_state_values_[i])); state_ifs.emplace_back(state_itfs_.back()); @@ -177,7 +180,7 @@ class MecanumDriveControllerFixture : public ::testing::Test } 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) {}; @@ -185,25 +188,27 @@ class MecanumDriveControllerFixture : public ::testing::Test "/test_mecanum_drive_controller/controller_state", 10, subs_callback); // call update to publish the test value ASSERT_EQ( - controller_->update_reference_from_subscribers( - controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); + controller_->update_reference_from_subscribers( + controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); ASSERT_EQ( - controller_->update_and_write_commands( - controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_->update_and_write_commands( + controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); // call update to publish the test value // 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--) { + while (max_sub_check_loop_count--) + { controller_->update_reference_from_subscribers( controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)); controller_->update_and_write_commands( controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)); // check if message has been received - if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) { + if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) + { break; } } @@ -217,13 +222,16 @@ class MecanumDriveControllerFixture : public ::testing::Test // TODO(anyone): add/remove arguments as it suites your command message type void publish_commands( - const rclcpp::Time & stamp, const double & twist_linear_x = 1.5, const double & twist_linear_y = 0.0, - const double & twist_angular_z = 0.0) + const rclcpp::Time & stamp, const double & twist_linear_x = 1.5, + const double & twist_linear_y = 0.0, const double & twist_angular_z = 0.0) { - auto wait_for_topic = [&](const auto topic_name) { + auto wait_for_topic = [&](const auto topic_name) + { size_t wait_count = 0; - while (command_publisher_node_->count_subscribers(topic_name) == 0) { - if (wait_count >= 5) { + while (command_publisher_node_->count_subscribers(topic_name) == 0) + { + if (wait_count >= 5) + { auto error_msg = std::string("publishing to ") + topic_name + " but no node subscribes to it"; throw std::runtime_error(error_msg); @@ -247,12 +255,15 @@ class MecanumDriveControllerFixture : public ::testing::Test command_publisher_->publish(msg); } - protected: // TODO(anyone): adjust the members as needed - std::vector joint_names_ = {"front_left_wheel_joint", "back_left_wheel_joint", "back_right_wheel_joint", "front_right_wheel_joint"}; - std::vector state_joint_names_ = {"state_front_left_wheel_joint", "state_back_left_wheel_joint", "state_back_right_wheel_joint", "state_front_right_wheel_joint"}; + std::vector joint_names_ = { + "front_left_wheel_joint", "back_left_wheel_joint", "back_right_wheel_joint", + "front_right_wheel_joint"}; + std::vector state_joint_names_ = { + "state_front_left_wheel_joint", "state_back_left_wheel_joint", "state_back_right_wheel_joint", + "state_front_right_wheel_joint"}; std::string interface_name_ = "velocity"; // Controller-related parameters diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller_preceeding.cpp b/mecanum_drive_controller/test/test_mecanum_drive_controller_preceeding.cpp index 98a2c07dec..cbd8fb4a9d 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller_preceeding.cpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller_preceeding.cpp @@ -24,7 +24,8 @@ using mecanum_drive_controller::CMD_MY_ITFS; using mecanum_drive_controller::control_mode_type; using mecanum_drive_controller::STATE_MY_ITFS; -class MecanumDriveControllerTest : public MecanumDriveControllerFixture +class MecanumDriveControllerTest +: public MecanumDriveControllerFixture { }; @@ -52,20 +53,23 @@ TEST_F(MecanumDriveControllerTest, check_exported_intefaces) auto command_intefaces = controller_->command_interface_configuration(); ASSERT_EQ(command_intefaces.names.size(), joint_command_values_.size()); - for (size_t i = 0; i < command_intefaces.names.size(); ++i) { + for (size_t i = 0; i < command_intefaces.names.size(); ++i) + { EXPECT_EQ(command_intefaces.names[i], joint_names_[i] + "/" + interface_name_); } auto state_intefaces = controller_->state_interface_configuration(); ASSERT_EQ(state_intefaces.names.size(), joint_state_values_.size()); - for (size_t i = 0; i < state_intefaces.names.size(); ++i) { + for (size_t i = 0; i < state_intefaces.names.size(); ++i) + { EXPECT_EQ(state_intefaces.names[i], state_joint_names_[i] + "/" + interface_name_); } // check ref itfs auto reference_interfaces = controller_->export_reference_interfaces(); ASSERT_EQ(reference_interfaces.size(), joint_names_.size()); - for (size_t i = 0; i < joint_names_.size(); ++i) { + for (size_t i = 0; i < joint_names_.size(); ++i) + { const std::string ref_itf_name = std::string(controller_->get_node()->get_name()) + "/" + state_joint_names_[i] + "/" + interface_name_; EXPECT_EQ(reference_interfaces[i].get_name(), ref_itf_name); From 149c39abe1512c436eccffb7923f5dd2da36444c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Mon, 9 Jan 2023 17:44:57 +0100 Subject: [PATCH 07/91] Manual changes of formatting. --- mecanum_drive_controller/CMakeLists.txt | 9 +++-- .../mecanum_drive_controller.hpp | 20 +++++----- .../mecanum_drive_controller/odometry.hpp | 21 ++++++++-- ...te_mecanum_drive_controller_parameters.hpp | 40 ------------------- .../visibility_control.h | 32 +++++++-------- mecanum_drive_controller/src/odometry.cpp | 31 ++++++++++---- .../test/test_mecanum_drive_controller.cpp | 5 +-- .../test/test_mecanum_drive_controller.hpp | 6 +-- 8 files changed, 79 insertions(+), 85 deletions(-) delete mode 100644 mecanum_drive_controller/include/mecanum_drive_controller/validate_mecanum_drive_controller_parameters.hpp diff --git a/mecanum_drive_controller/CMakeLists.txt b/mecanum_drive_controller/CMakeLists.txt index 8d2b5d1982..980e74a3af 100644 --- a/mecanum_drive_controller/CMakeLists.txt +++ b/mecanum_drive_controller/CMakeLists.txt @@ -42,7 +42,6 @@ find_package(ament_cmake REQUIRED) # Add mecanum_drive_controller library related compile commands generate_parameter_library(mecanum_drive_controller_parameters src/mecanum_drive_controller.yaml - include/mecanum_drive_controller/validate_mecanum_drive_controller_parameters.hpp ) add_library( mecanum_drive_controller @@ -94,7 +93,9 @@ if(BUILD_TESTING) ros2_control_test_assets ) - add_rostest_with_parameters_gmock(test_mecanum_drive_controller test/test_mecanum_drive_controller.cpp ${CMAKE_CURRENT_SOURCE_DIR}/test/mecanum_drive_controller_params.yaml) + add_rostest_with_parameters_gmock( + test_mecanum_drive_controller test/test_mecanum_drive_controller.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/mecanum_drive_controller_params.yaml) target_include_directories(test_mecanum_drive_controller PRIVATE include) target_link_libraries(test_mecanum_drive_controller mecanum_drive_controller) ament_target_dependencies( @@ -103,7 +104,9 @@ if(BUILD_TESTING) hardware_interface ) - # add_rostest_with_parameters_gmock(test_mecanum_drive_controller_preceeding test/test_mecanum_drive_controller_preceeding.cpp ${CMAKE_CURRENT_SOURCE_DIR}/test/mecanum_drive_controller_preceeding_params.yaml) + # add_rostest_with_parameters_gmock( + # test_mecanum_drive_controller_preceeding test/test_mecanum_drive_controller_preceeding.cpp + # ${CMAKE_CURRENT_SOURCE_DIR}/test/mecanum_drive_controller_preceeding_params.yaml) # target_include_directories(test_mecanum_drive_controller_preceeding PRIVATE include) # target_link_libraries(test_mecanum_drive_controller_preceeding mecanum_drive_controller) # ament_target_dependencies( diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp index e6e96c65df..21bcd5d90f 100644 --- a/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp +++ b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp @@ -52,35 +52,35 @@ static constexpr size_t NR_REF_ITFS = 3; class MecanumDriveController : public controller_interface::ChainableControllerInterface { public: - TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC MecanumDriveController(); - TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC controller_interface::CallbackReturn on_init() override; - TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC controller_interface::InterfaceConfiguration command_interface_configuration() const override; - TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC controller_interface::InterfaceConfiguration state_interface_configuration() const override; - TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC controller_interface::CallbackReturn on_configure( const rclcpp_lifecycle::State & previous_state) override; - TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC controller_interface::CallbackReturn on_activate( const rclcpp_lifecycle::State & previous_state) override; - TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC controller_interface::CallbackReturn on_deactivate( const rclcpp_lifecycle::State & previous_state) override; - TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC controller_interface::return_type update_reference_from_subscribers( const rclcpp::Time & time, const rclcpp::Duration & period) override; - TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC controller_interface::return_type update_and_write_commands( const rclcpp::Time & time, const rclcpp::Duration & period) override; @@ -121,7 +121,7 @@ class MecanumDriveController : public controller_interface::ChainableControllerI private: // callback for topic interface - TEMPLATES__ROS2_CONTROL__VISIBILITY_LOCAL + MECANUM_DRIVE_CONTROLLER__VISIBILITY_LOCAL void reference_callback(const std::shared_ptr msg); }; diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp b/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp index a7e44666b8..ea0b032f54 100644 --- a/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp +++ b/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp @@ -1,5 +1,20 @@ -#ifndef ODOMETRY_H_ -#define ODOMETRY_H_ +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) +// (template) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef MECANUM_DRIVE_CONTROLLER__ODOMETRY_HPP_ +#define MECANUM_DRIVE_CONTROLLER__ODOMETRY_HPP_ // #include #include "realtime_tools/realtime_buffer.h" @@ -97,4 +112,4 @@ class Odometry } // namespace mecanum_drive_controller -#endif /* ODOMETRY_H_ */ +#endif /* MECANUM_DRIVE_CONTROLLER__ODOMETRY_HPP_ */ diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/validate_mecanum_drive_controller_parameters.hpp b/mecanum_drive_controller/include/mecanum_drive_controller/validate_mecanum_drive_controller_parameters.hpp deleted file mode 100644 index adcc7eea88..0000000000 --- a/mecanum_drive_controller/include/mecanum_drive_controller/validate_mecanum_drive_controller_parameters.hpp +++ /dev/null @@ -1,40 +0,0 @@ -// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef MECANUM_DRIVE_CONTROLLER__VALIDATE_MECANUM_DRIVE_CONTROLLER_PARAMETERS_HPP_ -#define MECANUM_DRIVE_CONTROLLER__VALIDATE_MECANUM_DRIVE_CONTROLLER_PARAMETERS_HPP_ - -#include - -#include "parameter_traits/parameter_traits.hpp" -#include "rclcpp/parameter.hpp" -#include "tl_expected/expected.hpp" - -namespace parameter_traits -{ -tl::expected forbidden_interface_name_prefix(rclcpp::Parameter const & parameter) -{ - auto const & interface_name = parameter.as_string(); - - if (interface_name.rfind("blup_", 0) == 0) - { - return tl::make_unexpected("'interface_name' parameter can not start with 'blup_'"); - } - - return {}; -} - -} // namespace parameter_traits - -#endif // MECANUM_DRIVE_CONTROLLER__VALIDATE_MECANUM_DRIVE_CONTROLLER_PARAMETERS_HPP_ diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/visibility_control.h b/mecanum_drive_controller/include/mecanum_drive_controller/visibility_control.h index cff3f4dff4..e31b02cf1e 100644 --- a/mecanum_drive_controller/include/mecanum_drive_controller/visibility_control.h +++ b/mecanum_drive_controller/include/mecanum_drive_controller/visibility_control.h @@ -20,30 +20,30 @@ #if defined _WIN32 || defined __CYGWIN__ #ifdef __GNUC__ -#define TEMPLATES__ROS2_CONTROL__VISIBILITY_EXPORT __attribute__((dllexport)) -#define TEMPLATES__ROS2_CONTROL__VISIBILITY_IMPORT __attribute__((dllimport)) +#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_EXPORT __attribute__((dllexport)) +#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_IMPORT __attribute__((dllimport)) #else -#define TEMPLATES__ROS2_CONTROL__VISIBILITY_EXPORT __declspec(dllexport) -#define TEMPLATES__ROS2_CONTROL__VISIBILITY_IMPORT __declspec(dllimport) +#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_EXPORT __declspec(dllexport) +#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_IMPORT __declspec(dllimport) #endif -#ifdef TEMPLATES__ROS2_CONTROL__VISIBILITY_BUILDING_DLL -#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC TEMPLATES__ROS2_CONTROL__VISIBILITY_EXPORT +#ifdef MECANUM_DRIVE_CONTROLLER__VISIBILITY_BUILDING_DLL +#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC MECANUM_DRIVE_CONTROLLER__VISIBILITY_EXPORT #else -#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC TEMPLATES__ROS2_CONTROL__VISIBILITY_IMPORT +#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC MECANUM_DRIVE_CONTROLLER__VISIBILITY_IMPORT #endif -#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC_TYPE TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC -#define TEMPLATES__ROS2_CONTROL__VISIBILITY_LOCAL +#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC_TYPE MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC +#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_LOCAL #else -#define TEMPLATES__ROS2_CONTROL__VISIBILITY_EXPORT __attribute__((visibility("default"))) -#define TEMPLATES__ROS2_CONTROL__VISIBILITY_IMPORT +#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_EXPORT __attribute__((visibility("default"))) +#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_IMPORT #if __GNUC__ >= 4 -#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC __attribute__((visibility("default"))) -#define TEMPLATES__ROS2_CONTROL__VISIBILITY_LOCAL __attribute__((visibility("hidden"))) +#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC __attribute__((visibility("default"))) +#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_LOCAL __attribute__((visibility("hidden"))) #else -#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC -#define TEMPLATES__ROS2_CONTROL__VISIBILITY_LOCAL +#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC +#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_LOCAL #endif -#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC_TYPE +#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC_TYPE #endif #endif // MECANUM_DRIVE_CONTROLLER__VISIBILITY_CONTROL_H_ diff --git a/mecanum_drive_controller/src/odometry.cpp b/mecanum_drive_controller/src/odometry.cpp index 39661316df..97c0b1ba24 100644 --- a/mecanum_drive_controller/src/odometry.cpp +++ b/mecanum_drive_controller/src/odometry.cpp @@ -1,6 +1,21 @@ +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) +// (template) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #include "mecanum_drive_controller/odometry.hpp" -#include +#include "tf2/transform_datatypes.h" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include @@ -44,10 +59,12 @@ bool Odometry::update( /// Compute FK (i.e. compute mobile robot's body twist out of its wheels velocities): /// NOTE: we use the IK of the mecanum wheels which we invert using a pseudo-inverse. - /// NOTE: the mecanum IK gives the body speed at the center frame, we then offset this velocity at the base frame. - /// NOTE: in the diff drive the velocity is filtered out, but we prefer to return it raw and let the user perform - /// post-processing at will. We prefer this way of doing as filtering introduces delay (which makes it - /// difficult to interpret and compare behavior curves). + /// NOTE: the mecanum IK gives the body speed at the center frame, we then offset this velocity + /// at the base frame. + /// NOTE: in the diff drive the velocity is filtered out, but we prefer to return it raw and + /// let the user perform post-processing at will. + /// We prefer this way of doing as filtering introduces delay (which makes it difficult + /// to interpret and compare behavior curves). double vx_Oc_c_c0_c = 0.25 * wheels_radius_ * (wheel0_vel + wheel1_vel + wheel2_vel + wheel3_vel); double vy_Oc_c_c0_c = 0.25 * wheels_radius_ * (-wheel0_vel + wheel1_vel - wheel2_vel + wheel3_vel); @@ -82,8 +99,8 @@ bool Odometry::update( wz_b_b0_b_ = wz_c_c0_c; /// Integration. - /// NOTE: the position is expressed in the odometry frame (frame b0), unlike the twist which is expressed in the body - /// frame (frame b). + /// NOTE: the position is expressed in the odometry frame (frame b0), unlike the twist which is + /// expressed in the body frame (frame b). rz_b_b0_ += wz_b_b0_b_ * dt; tf2::Quaternion orientation_R_b_b0; diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp index 5051505018..ffdad132cc 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp @@ -13,7 +13,6 @@ // limitations under the License. #include "test_mecanum_drive_controller.hpp" -#include "hardware_interface/types/hardware_interface_type_values.hpp" #include #include @@ -21,6 +20,8 @@ #include #include +#include "hardware_interface/types/hardware_interface_type_values.hpp" + using mecanum_drive_controller::NR_CMD_ITFS; using mecanum_drive_controller::NR_REF_ITFS; using mecanum_drive_controller::NR_STATE_ITFS; @@ -197,7 +198,6 @@ TEST_F(MecanumDriveControllerTest, receive_message_and_publish_updated_status) controller_->update_and_write_commands( controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - //here ControllerStateMsg msg; subscribe_and_get_messages(msg); joint_command_values_[1] = 111; @@ -214,7 +214,6 @@ TEST_F(MecanumDriveControllerTest, receive_message_and_publish_updated_status) controller_->update_and_write_commands( controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - //here EXPECT_EQ(joint_command_values_[1], 3.0); EXPECT_FALSE(std::isnan(joint_command_values_[1])); fprintf(stderr, " joint_command_values_[1]= %f \n", joint_command_values_[1]); diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp index 72b597f1b0..a38a877413 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TEMPLATES__ROS2_CONTROL__CONTROLLER__TEST_MECANUM_DRIVE_CONTROLLER_HPP_ -#define TEMPLATES__ROS2_CONTROL__CONTROLLER__TEST_MECANUM_DRIVE_CONTROLLER_HPP_ +#ifndef TEST_MECANUM_DRIVE_CONTROLLER_HPP_ +#define TEST_MECANUM_DRIVE_CONTROLLER_HPP_ #include #include @@ -286,4 +286,4 @@ class MecanumDriveControllerFixture : public ::testing::Test rclcpp::Publisher::SharedPtr tf_odom_s_publisher_; }; -#endif // TEMPLATES__ROS2_CONTROL__CONTROLLER__TEST_MECANUM_DRIVE_CONTROLLER_HPP_ +#endif // TEST_MECANUM_DRIVE_CONTROLLER_HPP_ From c55911b05a61052ebcfe3bfa5743cdae77df59da Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Mon, 9 Jan 2023 17:49:42 +0100 Subject: [PATCH 08/91] Delete LICENSE --- mecanum_drive_controller/LICENSE | 17 ----------------- 1 file changed, 17 deletions(-) delete mode 100644 mecanum_drive_controller/LICENSE diff --git a/mecanum_drive_controller/LICENSE b/mecanum_drive_controller/LICENSE deleted file mode 100644 index 30e8e2ece5..0000000000 --- a/mecanum_drive_controller/LICENSE +++ /dev/null @@ -1,17 +0,0 @@ -Permission is hereby granted, free of charge, to any person obtaining a copy -of this software and associated documentation files (the "Software"), to deal -in the Software without restriction, including without limitation the rights -to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -copies of the Software, and to permit persons to whom the Software is -furnished to do so, subject to the following conditions: - -The above copyright notice and this permission notice shall be included in -all copies or substantial portions of the Software. - -THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -THE SOFTWARE. From 2a87584e0a3f2c29d1571f19294e5e8812244a22 Mon Sep 17 00:00:00 2001 From: Robotgir <47585672+Robotgir@users.noreply.github.com> Date: Tue, 10 Jan 2023 20:36:25 +0530 Subject: [PATCH 09/91] Apply suggestions from code review MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit general changes not any fundamental changes Co-authored-by: Denis Štogl --- mecanum_drive_controller/CMakeLists.txt | 11 +----- .../mecanum_drive_controller.hpp | 7 ++-- .../mecanum_drive_controller/odometry.hpp | 5 --- .../visibility_control.h | 2 +- mecanum_drive_controller/package.xml | 17 +-------- .../src/mecanum_drive_controller.cpp | 35 ++++--------------- .../src/mecanum_drive_controller.yaml | 6 ++-- mecanum_drive_controller/src/odometry.cpp | 17 --------- .../test_load_mecanum_drive_controller.cpp | 2 +- .../test/test_mecanum_drive_controller.cpp | 10 ++---- .../test/test_mecanum_drive_controller.hpp | 10 +----- ...st_mecanum_drive_controller_preceeding.cpp | 2 +- 12 files changed, 20 insertions(+), 104 deletions(-) diff --git a/mecanum_drive_controller/CMakeLists.txt b/mecanum_drive_controller/CMakeLists.txt index 980e74a3af..3a1c0b99b5 100644 --- a/mecanum_drive_controller/CMakeLists.txt +++ b/mecanum_drive_controller/CMakeLists.txt @@ -20,7 +20,6 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS tf2 tf2_msgs tf2_geometry_msgs - custom_messages ) find_package(ament_cmake REQUIRED) @@ -71,14 +70,6 @@ install( ) if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # comment the line when a copyright and license is added to all source files - set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # comment the line when this package is in a git repo and when - # a copyright and license is added to all source files - set(ament_cmake_cpplint_FOUND TRUE) find_package(ament_cmake_gmock REQUIRED) find_package(controller_manager REQUIRED) find_package(hardware_interface REQUIRED) @@ -120,7 +111,7 @@ ament_export_include_directories( include ) ament_export_dependencies( - + ${THIS_PACKAGE_INCLUDE_DEPENDS} ) ament_export_libraries( mecanum_drive_controller diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp index 21bcd5d90f..3e40ef9ef4 100644 --- a/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp +++ b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -33,7 +33,6 @@ #include "realtime_tools/realtime_publisher.h" #include "std_srvs/srv/set_bool.hpp" -// TODO(anyone): Replace with controller specific messages #include "custom_messages/msg/mecanum_drive_controller_state.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" @@ -84,7 +83,6 @@ class MecanumDriveController : public controller_interface::ChainableControllerI controller_interface::return_type update_and_write_commands( const rclcpp::Time & time, const rclcpp::Duration & period) override; - // TODO(anyone): replace the state and command message types using ControllerReferenceMsg = geometry_msgs::msg::TwistStamped; using OdomStateMsg = nav_msgs::msg::Odometry; using TfStateMsg = tf2_msgs::msg::TFMessage; @@ -97,7 +95,7 @@ class MecanumDriveController : public controller_interface::ChainableControllerI // Command subscribers and Controller State publisher rclcpp::Subscription::SharedPtr ref_subscriber_ = nullptr; realtime_tools::RealtimeBuffer> input_ref_; - rclcpp::Duration ref_timeout_ = rclcpp::Duration::from_seconds(0.0); // 0ms + rclcpp::Duration ref_timeout_ = rclcpp::Duration::from_seconds(0.0); using OdomStatePublisher = realtime_tools::RealtimePublisher; rclcpp::Publisher::SharedPtr odom_s_publisher_; @@ -116,7 +114,6 @@ class MecanumDriveController : public controller_interface::ChainableControllerI bool on_set_chained_mode(bool chained_mode) override; - // Odometry related: Odometry odometry_; private: diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp b/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp index ea0b032f54..303282c876 100644 --- a/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp +++ b/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp @@ -16,14 +16,9 @@ #ifndef MECANUM_DRIVE_CONTROLLER__ODOMETRY_HPP_ #define MECANUM_DRIVE_CONTROLLER__ODOMETRY_HPP_ -// #include #include "realtime_tools/realtime_buffer.h" #include "realtime_tools/realtime_publisher.h" -#include -#include -#include -#include #define PLANAR_POINT_DIM 3 diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/visibility_control.h b/mecanum_drive_controller/include/mecanum_drive_controller/visibility_control.h index e31b02cf1e..b9a5062612 100644 --- a/mecanum_drive_controller/include/mecanum_drive_controller/visibility_control.h +++ b/mecanum_drive_controller/include/mecanum_drive_controller/visibility_control.h @@ -1,4 +1,4 @@ -// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/mecanum_drive_controller/package.xml b/mecanum_drive_controller/package.xml index 7bd95282af..a0d38babe1 100644 --- a/mecanum_drive_controller/package.xml +++ b/mecanum_drive_controller/package.xml @@ -12,36 +12,21 @@ 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 - std_srvs - - custom_messages - ament_lint_auto ament_cmake_gmock controller_manager hardware_interface diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index 6f6b10d361..7ca15b756d 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -1,5 +1,4 @@ // Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) -// (template) // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -29,19 +28,6 @@ namespace { // utility -// TODO(destogl): remove this when merged upstream -// Changed services history QoS to keep all so we don't lose any client service -// calls -static constexpr rmw_qos_profile_t rmw_qos_profile_services_hist_keep_all = { - RMW_QOS_POLICY_HISTORY_KEEP_ALL, - 1, // message queue depth - RMW_QOS_POLICY_RELIABILITY_RELIABLE, - RMW_QOS_POLICY_DURABILITY_VOLATILE, - RMW_QOS_DEADLINE_DEFAULT, - RMW_QOS_LIFESPAN_DEFAULT, - RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT, - RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT, - false}; using ControllerReferenceMsg = mecanum_drive_controller::MecanumDriveController::ControllerReferenceMsg; @@ -133,7 +119,6 @@ controller_interface::CallbackReturn MecanumDriveController::on_configure( 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]; @@ -181,14 +166,11 @@ controller_interface::CallbackReturn MecanumDriveController::on_configure( return controller_interface::CallbackReturn::ERROR; } - // // TODO(anyone): Reserve memory in state publisher depending on the message - // // type - controller_state_publisher_->lock(); controller_state_publisher_->msg_.header.stamp = get_node()->now(); controller_state_publisher_->msg_.header.frame_id = params_.odom_frame_id; - controller_state_publisher_->msg_.odom.pose.pose.position.x = 0.0; controller_state_publisher_->msg_.odom.child_frame_id = params_.base_frame_id; + controller_state_publisher_->msg_.odom.pose.pose.position.x = 0.0; controller_state_publisher_->unlock(); auto & covariance_controller = controller_state_publisher_->msg_.odom.twist.covariance; for (size_t index = 0; index < 6; ++index) @@ -310,8 +292,6 @@ controller_interface::CallbackReturn MecanumDriveController::on_activate( controller_interface::CallbackReturn MecanumDriveController::on_deactivate( const rclcpp_lifecycle::State & /*previous_state*/) { - // TODO(anyone): depending on number of interfaces, use definitions, e.g., - // `CMD_MY_ITFS`, instead of a loop for (size_t i = 0; i < NR_CMD_ITFS; ++i) { command_interfaces_[i].set_value(std::numeric_limits::quiet_NaN()); @@ -323,10 +303,8 @@ controller_interface::return_type MecanumDriveController::update_reference_from_ const rclcpp::Time & time, const rclcpp::Duration & /*period*/) { auto current_ref = *(input_ref_.readFromRT()); - const auto age_of_last_command = get_node()->now() - (current_ref)->header.stamp; + const auto age_of_last_command = time - (current_ref)->header.stamp; - // TODO(anyone): depending on number of interfaces, use definitions, e.g., `CMD_MY_ITFS`, - // instead of a loop // send message only if there is no timeout if (age_of_last_command <= ref_timeout_ || ref_timeout_ == rclcpp::Duration::from_seconds(0)) { @@ -366,15 +344,15 @@ controller_interface::return_type MecanumDriveController::update_and_write_comma // INVERSE KINEMATICS (move robot). // Compute wheels velocities (this is the actual ik): - // NOTE: the input desired twist (from topic /cmd_vel) is a body twist. + // NOTE: the input desired twist (from topic `~/reference`) is a body twist. auto current_ref = input_ref_.readFromRT(); const auto age_of_last_command = time - (*current_ref)->header.stamp; if (age_of_last_command <= ref_timeout_ || ref_timeout_ == rclcpp::Duration::from_seconds(0)) { - tf2::Quaternion orientation_2; - orientation_2.setRPY(0.0, 0.0, params_.base_frame_offset[2]); + tf2::Quaternion quaternion; + quaternion.setRPY(0.0, 0.0, params_.base_frame_offset[2]); - tf2::Matrix3x3 R_b_c = tf2::Matrix3x3((orientation_2)); + tf2::Matrix3x3 R_b_c = tf2::Matrix3x3((quaternion)); tf2::Vector3 v_Ob_b_b0_c = R_b_c * tf2::Vector3(reference_interfaces_[0], reference_interfaces_[1], 0.0); tf2::Vector3 Ob_c = @@ -410,7 +388,6 @@ controller_interface::return_type MecanumDriveController::update_and_write_comma reference_interfaces_[1] = std::numeric_limits::quiet_NaN(); reference_interfaces_[2] = std::numeric_limits::quiet_NaN(); command_interfaces_[0].set_value(0.0); - // command_interfaces_[1].set_value(w1_vel); command_interfaces_[1].set_value(0.0); command_interfaces_[2].set_value(0.0); command_interfaces_[3].set_value(0.0); diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.yaml b/mecanum_drive_controller/src/mecanum_drive_controller.yaml index 07c5264200..ad350a0fdd 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.yaml +++ b/mecanum_drive_controller/src/mecanum_drive_controller.yaml @@ -57,7 +57,7 @@ mecanum_drive_controller: base_frame_offset: { type: double_array, default_value: [0.0, 0.0, 0.0], - description: "base frame offset values, [base_frame_offset/x base_frame_offset/y base_frame_offset/theta].", + description: "Base frame offset values, [base_frame_offset/x base_frame_offset/y base_frame_offset/theta].", read_only: false, } @@ -71,14 +71,14 @@ mecanum_drive_controller: wheels_k: { type: double, default_value: 0.0, - description: "wheels geometric param used in mecanum wheels' ik.", + description: "Wheels geometric param used in mecanum wheels' IK.", read_only: false, } use_realigned_roller_joints: { type: bool, default_value: false, - description: "This mode is used when the mecanum wheels are simulated and we use realigned rollers to mimic mecanum wheels..", + description: "This mode is used when the mecanum wheels are simulated and we use realigned rollers to mimic mecanum wheels.", read_only: false, } diff --git a/mecanum_drive_controller/src/odometry.cpp b/mecanum_drive_controller/src/odometry.cpp index 97c0b1ba24..ed82d5038d 100644 --- a/mecanum_drive_controller/src/odometry.cpp +++ b/mecanum_drive_controller/src/odometry.cpp @@ -1,5 +1,4 @@ // Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) -// (template) // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -78,22 +77,6 @@ bool Odometry::update( tf2::Vector3 v_Oc_c_c0_b = R_c_b * tf2::Vector3(vx_Oc_c_c0_c, vy_Oc_c_c0_c, 0.0); tf2::Vector3 Oc_b = R_c_b * tf2::Vector3(-base_frame_offset_[0], -base_frame_offset_[1], 0.0); - // fprintf(stderr," R_c_b = %f", R_c_b[2][2]); - - // for(int x=0;x<3;x++) // loop 3 times for three lines - // { - // for(int y=0;y<3;y++) // loop for the three elements on the line - // { - // cout<on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); // try to set command with time before timeout - command is not updated - auto reference = controller_->input_ref_.readFromNonRT(); - auto old_timestamp = (*reference)->header.stamp; + auto reference = *(controller_->input_ref_.readFromNonRT()); + auto old_timestamp = reference->header.stamp; EXPECT_TRUE(std::isnan((*reference)->twist.linear.x)); EXPECT_TRUE(std::isnan((*reference)->twist.linear.y)); EXPECT_TRUE(std::isnan((*reference)->twist.angular.z)); @@ -244,9 +243,6 @@ TEST_F(MecanumDriveControllerTest, test_sending_too_old_message) rclcpp::Duration::from_seconds(0.1)); ASSERT_TRUE(controller_->wait_for_commands(executor)); ASSERT_EQ(old_timestamp, (*(controller_->input_ref_.readFromNonRT()))->header.stamp); - // ASSERT_EQ((*reference)->twist.linear.x, 0.45); - // ASSERT_EQ((*reference)->twist.linear.y, 0.45); - // ASSERT_EQ((*reference)->twist.angular.z, 0.0); EXPECT_TRUE(std::isnan((*reference)->twist.linear.x)); EXPECT_TRUE(std::isnan((*reference)->twist.linear.y)); EXPECT_TRUE(std::isnan((*reference)->twist.angular.z)); diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp index a38a877413..e38d169007 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -33,7 +33,6 @@ #include "rclcpp/utilities.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" -// TODO(anyone): replace the state and command message types using ControllerStateMsg = mecanum_drive_controller::MecanumDriveController::ControllerStateMsg; using ControllerReferenceMsg = mecanum_drive_controller::MecanumDriveController::ControllerReferenceMsg; @@ -112,8 +111,6 @@ class TestableMecanumDriveController : public mecanum_drive_controller::MecanumD return wait_for_command(executor, ref_subscriber_wait_set_, timeout); } - // TODO(anyone): add implementation of any methods of your controller is needed - private: rclcpp::WaitSet ref_subscriber_wait_set_; }; @@ -162,7 +159,6 @@ class MecanumDriveControllerFixture : public ::testing::Test joint_names_[i], interface_name_, &joint_command_values_[i])); command_ifs.emplace_back(command_itfs_.back()); } - // TODO(anyone): Add other command interfaces, if any std::vector state_ifs; state_itfs_.reserve(joint_state_values_.size()); @@ -174,7 +170,6 @@ class MecanumDriveControllerFixture : public ::testing::Test joint_names_[i], interface_name_, &joint_state_values_[i])); state_ifs.emplace_back(state_itfs_.back()); } - // TODO(anyone): Add other state interfaces, if any controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); } @@ -220,7 +215,6 @@ class MecanumDriveControllerFixture : public ::testing::Test ASSERT_TRUE(subscription->take(msg, msg_info)); } - // TODO(anyone): add/remove arguments as it suites your command message type void publish_commands( const rclcpp::Time & stamp, const double & twist_linear_x = 1.5, const double & twist_linear_y = 0.0, const double & twist_angular_z = 0.0) @@ -256,8 +250,6 @@ class MecanumDriveControllerFixture : public ::testing::Test } protected: - // TODO(anyone): adjust the members as needed - std::vector joint_names_ = { "front_left_wheel_joint", "back_left_wheel_joint", "back_right_wheel_joint", "front_right_wheel_joint"}; diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller_preceeding.cpp b/mecanum_drive_controller/test/test_mecanum_drive_controller_preceeding.cpp index cbd8fb4a9d..81ac0cc6e4 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller_preceeding.cpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller_preceeding.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. From 13e1c7afd6898f64055c0d382c76980ae731462e Mon Sep 17 00:00:00 2001 From: Robotgir <47585672+Robotgir@users.noreply.github.com> Date: Tue, 10 Jan 2023 21:46:51 +0530 Subject: [PATCH 10/91] Apply suggestions from code review MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit using std instead of boost Co-authored-by: Denis Štogl --- .../include/mecanum_drive_controller/odometry.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp b/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp index 303282c876..f4834fa30e 100644 --- a/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp +++ b/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp @@ -32,7 +32,7 @@ class Odometry { public: /// Integration function, used to integrate the odometry: - typedef boost::function IntegrationFunction; + typedef std::function IntegrationFunction; /// \brief Constructor /// Timestamp will get the current time value From 9dc99d3ee0d9fc194715c4b82fbfbd317b62b7e4 Mon Sep 17 00:00:00 2001 From: GiridharBukka Date: Tue, 10 Jan 2023 21:16:34 +0530 Subject: [PATCH 11/91] used the suggested structure --- mecanum_drive_controller/CMakeLists.txt | 11 +---------- 1 file changed, 1 insertion(+), 10 deletions(-) diff --git a/mecanum_drive_controller/CMakeLists.txt b/mecanum_drive_controller/CMakeLists.txt index 3a1c0b99b5..7c363c7db2 100644 --- a/mecanum_drive_controller/CMakeLists.txt +++ b/mecanum_drive_controller/CMakeLists.txt @@ -20,6 +20,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS tf2 tf2_msgs tf2_geometry_msgs + custom_messages ) find_package(ament_cmake REQUIRED) @@ -28,16 +29,6 @@ foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() -cmake_minimum_required(VERSION 3.8) -project(mecanum_drive_controller) - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -# find dependencies -find_package(ament_cmake REQUIRED) - # Add mecanum_drive_controller library related compile commands generate_parameter_library(mecanum_drive_controller_parameters src/mecanum_drive_controller.yaml From 02b08cff6df86742116ecd04db131b122b22fb52 Mon Sep 17 00:00:00 2001 From: GiridharBukka Date: Tue, 10 Jan 2023 21:42:33 +0530 Subject: [PATCH 12/91] general changes --- .../include/mecanum_drive_controller/odometry.hpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp b/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp index f4834fa30e..f3d0059e09 100644 --- a/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp +++ b/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp @@ -24,7 +24,6 @@ namespace mecanum_drive_controller { -namespace bacc = boost::accumulators; /// \brief The Odometry class handles odometry readings /// (2D pose and velocity with related timestamp) @@ -81,7 +80,7 @@ class Odometry rclcpp::Time timestamp_; /// Reference frame (wrt to center frame). - double base_frame_offset_[PLANAR_POINT_DIM]; + std::array base_frame_offset_; /// Current pose: double px_b_b0_; // [m] From e7247bd1e9323832c65d6d1651df7c0c5f1c00a6 Mon Sep 17 00:00:00 2001 From: GiridharBukka Date: Tue, 10 Jan 2023 21:44:18 +0530 Subject: [PATCH 13/91] removed bacc accumulators --- .../include/mecanum_drive_controller/odometry.hpp | 3 --- mecanum_drive_controller/src/odometry.cpp | 3 +-- 2 files changed, 1 insertion(+), 5 deletions(-) diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp b/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp index f3d0059e09..cd2fb6d864 100644 --- a/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp +++ b/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp @@ -72,9 +72,6 @@ class Odometry void setWheelsParams(double wheels_k, double wheels_radius); private: - /// Rolling mean accumulator and window: - typedef bacc::accumulator_set > RollingMeanAcc; - typedef bacc::tag::rolling_window RollingWindow; /// Current timestamp: rclcpp::Time timestamp_; diff --git a/mecanum_drive_controller/src/odometry.cpp b/mecanum_drive_controller/src/odometry.cpp index ed82d5038d..91887063b4 100644 --- a/mecanum_drive_controller/src/odometry.cpp +++ b/mecanum_drive_controller/src/odometry.cpp @@ -21,8 +21,7 @@ using namespace std; namespace mecanum_drive_controller { -namespace bacc = boost::accumulators; - + Odometry::Odometry(size_t velocity_rolling_window_size) : timestamp_(0.0), px_b_b0_(0.0), From 1268588bc24e12f2f493db8e3084a27b38c05b23 Mon Sep 17 00:00:00 2001 From: GiridharBukka Date: Wed, 11 Jan 2023 09:19:40 +0530 Subject: [PATCH 14/91] renaming to more meaningful and readable variable names --- .../mecanum_drive_controller.hpp | 9 ++++ .../mecanum_drive_controller/odometry.hpp | 39 +++++++++----- .../src/mecanum_drive_controller.cpp | 27 +++++----- mecanum_drive_controller/src/odometry.cpp | 52 ++++++++++--------- 4 files changed, 77 insertions(+), 50 deletions(-) diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp index 3e40ef9ef4..a02200ec27 100644 --- a/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp +++ b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp @@ -120,6 +120,15 @@ class MecanumDriveController : public controller_interface::ChainableControllerI // callback for topic interface MECANUM_DRIVE_CONTROLLER__VISIBILITY_LOCAL void reference_callback(const std::shared_ptr msg); + struct body_velocity{ + double linear_x; // [m/s] + double linear_y; // [m/s] + double angular_z; // [rad/s] + + body_velocity() : linear_x(0.0), linear_y(0.0), angular_z(0.0) {} + }; + body_velocity body_velocity_center_frame_; + }; } // namespace mecanum_drive_controller diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp b/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp index cd2fb6d864..3b77bb4604 100644 --- a/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp +++ b/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp @@ -18,6 +18,7 @@ #include "realtime_tools/realtime_buffer.h" #include "realtime_tools/realtime_publisher.h" +#include "geometry_msgs/msg/twist.hpp" #define PLANAR_POINT_DIM 3 @@ -54,17 +55,17 @@ class Odometry double wheel0_vel, double wheel1_vel, double wheel2_vel, double wheel3_vel, const double dt); /// \return position (x component) [m] - double getX() const { return px_b_b0_; } + double getX() const { return position_x_base_frame_; } /// \return position (y component) [m] - double getY() const { return py_b_b0_; } + double getY() const { return position_y_base_frame_; } /// \return orientation (z component) [m] - double getRz() const { return rz_b_b0_; } + double getRz() const { return orientation_z_base_frame_; } /// \return body velocity of the base frame (linear x component) [m/s] - double getVx() const { return vx_Ob_b_b0_b_; } + double getVx() const { return body_velocity_base_frame_.linear_x; } /// \return body velocity of the base frame (linear y component) [m/s] - double getVy() const { return vy_Ob_b_b0_b_; } + double getVy() const { return body_velocity_base_frame_.linear_y; } /// \return body velocity of the base frame (angular z component) [m/s] - double getWz() const { return wz_b_b0_b_; } + double getWz() const { return body_velocity_base_frame_.angular_z; } /// \brief Sets the wheels parameters: mecanum geometric param and radius /// \param wheels_k Wheels geometric param (used in mecanum wheels' ik) [m] @@ -80,21 +81,33 @@ class Odometry std::array base_frame_offset_; /// Current pose: - double px_b_b0_; // [m] - double py_b_b0_; // [m] - double rz_b_b0_; // [rad] + double position_x_base_frame_; // [m] + double position_y_base_frame_; // [m] + double orientation_z_base_frame_; // [rad] /// Current velocity. /// \note The indices meaning is the following: /// b : base frame /// c : center frame - /// Ob: origin of the base frame + /// body_velocity_base_frame_: body_velocity w.r.t base frame + /// body_velocity_center_frame_: body_velocity w.r.t center frame /// Oc: origin of the center frame /// b0: initial position if the base frame /// c0: initial position of the center frame - double vx_Ob_b_b0_b_; // [m/s] - double vy_Ob_b_b0_b_; // [m/s] - double wz_b_b0_b_; // [rad/s] + + struct body_velocity{ + double linear_x; // [m/s] + double linear_y; // [m/s] + double angular_z; // [rad/s] + + body_velocity() : linear_x(0.0), linear_y(0.0), angular_z(0.0) {} + }; + + body_velocity body_velocity_base_frame_; + body_velocity body_velocity_center_frame_; + // // body_velocity_base_frame_.linear_x; // [m/s] body_velocity_base_frame_.linear_x + // // double body_velocity_base_frame_.linear_y; // [m/s] + // // double body_velocity_base_frame_.angular_z; // [rad/s] /// Wheels kinematic parameters [m]: double wheels_k_; diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index 7ca15b756d..813d6b3e99 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -351,25 +351,28 @@ controller_interface::return_type MecanumDriveController::update_and_write_comma { tf2::Quaternion quaternion; quaternion.setRPY(0.0, 0.0, params_.base_frame_offset[2]); - - tf2::Matrix3x3 R_b_c = tf2::Matrix3x3((quaternion)); - tf2::Vector3 v_Ob_b_b0_c = - R_b_c * tf2::Vector3(reference_interfaces_[0], reference_interfaces_[1], 0.0); - tf2::Vector3 Ob_c = + /// \note The variables meaning: + /// angular_transformation_from_base_2_center: Rotation transformation matrix, to transform from base frame to center frame + /// linear_transformation_from_base_2_center: offset/linear transformation matrix, to transform from base frame to center frame + + tf2::Matrix3x3 angular_transformation_from_base_2_center = tf2::Matrix3x3((quaternion)); + tf2::Vector3 body_velocity_base_frame_w_r_t_center_frame_ = + angular_transformation_from_base_2_center * tf2::Vector3(reference_interfaces_[0], reference_interfaces_[1], 0.0); + tf2::Vector3 linear_transformation_from_base_2_center = tf2::Vector3(params_.base_frame_offset[0], params_.base_frame_offset[1], 0.0); - double vx_Oc_c_c0_c_ = v_Ob_b_b0_c.x() + Ob_c.y() * reference_interfaces_[2]; - double vy_Oc_c_c0_c_ = v_Ob_b_b0_c.y() - Ob_c.x() * reference_interfaces_[2]; - double wz_c_c0_c_ = reference_interfaces_[2]; + body_velocity_center_frame_.linear_x = body_velocity_base_frame_w_r_t_center_frame_.x() + linear_transformation_from_base_2_center.y() * reference_interfaces_[2]; + body_velocity_center_frame_.linear_y = body_velocity_base_frame_w_r_t_center_frame_.y() - linear_transformation_from_base_2_center.x() * reference_interfaces_[2]; + body_velocity_center_frame_.angular_z = reference_interfaces_[2]; double w0_vel = - 1.0 / params_.wheels_radius * (vx_Oc_c_c0_c_ - vy_Oc_c_c0_c_ - params_.wheels_k * wz_c_c0_c_); + 1.0 / params_.wheels_radius * (body_velocity_center_frame_.linear_x - body_velocity_center_frame_.linear_y - params_.wheels_k * body_velocity_center_frame_.angular_z); double w1_vel = - 1.0 / params_.wheels_radius * (vx_Oc_c_c0_c_ + vy_Oc_c_c0_c_ - params_.wheels_k * wz_c_c0_c_); + 1.0 / params_.wheels_radius * (body_velocity_center_frame_.linear_x + body_velocity_center_frame_.linear_y - params_.wheels_k * body_velocity_center_frame_.angular_z); double w2_vel = - 1.0 / params_.wheels_radius * (vx_Oc_c_c0_c_ - vy_Oc_c_c0_c_ + params_.wheels_k * wz_c_c0_c_); + 1.0 / params_.wheels_radius * (body_velocity_center_frame_.linear_x - body_velocity_center_frame_.linear_y + params_.wheels_k * body_velocity_center_frame_.angular_z); double w3_vel = - 1.0 / params_.wheels_radius * (vx_Oc_c_c0_c_ + vy_Oc_c_c0_c_ + params_.wheels_k * wz_c_c0_c_); + 1.0 / params_.wheels_radius * (body_velocity_center_frame_.linear_x + body_velocity_center_frame_.linear_y + params_.wheels_k * body_velocity_center_frame_.angular_z); // Set wheels velocities: diff --git a/mecanum_drive_controller/src/odometry.cpp b/mecanum_drive_controller/src/odometry.cpp index 91887063b4..81a24777d5 100644 --- a/mecanum_drive_controller/src/odometry.cpp +++ b/mecanum_drive_controller/src/odometry.cpp @@ -21,15 +21,12 @@ using namespace std; namespace mecanum_drive_controller { - + Odometry::Odometry(size_t velocity_rolling_window_size) : timestamp_(0.0), - px_b_b0_(0.0), - py_b_b0_(0.0), - rz_b_b0_(0.0), - vx_Ob_b_b0_b_(0.0), - vy_Ob_b_b0_b_(0.0), - wz_b_b0_b_(0.0), + position_x_base_frame_(0.0), + position_y_base_frame_(0.0), + orientation_z_base_frame_(0.0), wheels_k_(0.0), wheels_radius_(0.0) { @@ -63,38 +60,43 @@ bool Odometry::update( /// let the user perform post-processing at will. /// We prefer this way of doing as filtering introduces delay (which makes it difficult /// to interpret and compare behavior curves). - double vx_Oc_c_c0_c = 0.25 * wheels_radius_ * (wheel0_vel + wheel1_vel + wheel2_vel + wheel3_vel); - double vy_Oc_c_c0_c = + + /// \note The variables meaning: + /// angular_transformation_from_center_2_base: Rotation transformation matrix, to transform from center frame to base frame + /// linear_transformation_from_center_2_base: offset/linear transformation matrix, to transform from center frame to base frame + + body_velocity_center_frame_.linear_x = 0.25 * wheels_radius_ * (wheel0_vel + wheel1_vel + wheel2_vel + wheel3_vel); + body_velocity_center_frame_.linear_y = 0.25 * wheels_radius_ * (-wheel0_vel + wheel1_vel - wheel2_vel + wheel3_vel); - double wz_c_c0_c = + body_velocity_center_frame_.angular_z = 0.25 * wheels_radius_ / wheels_k_ * (-wheel0_vel - wheel1_vel + wheel2_vel + wheel3_vel); tf2::Quaternion orientation_R_c_b; orientation_R_c_b.setRPY(0.0, 0.0, -base_frame_offset_[2]); - tf2::Matrix3x3 R_c_b = tf2::Matrix3x3((orientation_R_c_b)); - tf2::Vector3 v_Oc_c_c0_b = R_c_b * tf2::Vector3(vx_Oc_c_c0_c, vy_Oc_c_c0_c, 0.0); - tf2::Vector3 Oc_b = R_c_b * tf2::Vector3(-base_frame_offset_[0], -base_frame_offset_[1], 0.0); + tf2::Matrix3x3 angular_transformation_from_center_2_base = tf2::Matrix3x3((orientation_R_c_b)); + tf2::Vector3 body_velocity_center_frame_w_r_t_base_frame_ =angular_transformation_from_center_2_base * tf2::Vector3(body_velocity_center_frame_.linear_x, body_velocity_center_frame_.linear_y, 0.0); + tf2::Vector3 linear_transformation_from_center_2_base =angular_transformation_from_center_2_base * tf2::Vector3(-base_frame_offset_[0], -base_frame_offset_[1], 0.0); - vx_Ob_b_b0_b_ = v_Oc_c_c0_b.x() + Oc_b.y() * wz_c_c0_c; - vy_Ob_b_b0_b_ = v_Oc_c_c0_b.y() - Oc_b.x() * wz_c_c0_c; - wz_b_b0_b_ = wz_c_c0_c; + body_velocity_base_frame_.linear_x = body_velocity_center_frame_w_r_t_base_frame_.x() + linear_transformation_from_center_2_base.y() * body_velocity_center_frame_.angular_z; + body_velocity_base_frame_.linear_y = body_velocity_center_frame_w_r_t_base_frame_.y() - linear_transformation_from_center_2_base.x() * body_velocity_center_frame_.angular_z; + body_velocity_base_frame_.angular_z = body_velocity_center_frame_.angular_z; /// Integration. /// NOTE: the position is expressed in the odometry frame (frame b0), unlike the twist which is /// expressed in the body frame (frame b). - rz_b_b0_ += wz_b_b0_b_ * dt; + orientation_z_base_frame_ += body_velocity_base_frame_.angular_z * dt; - tf2::Quaternion orientation_R_b_b0; - orientation_R_b_b0.setRPY(0.0, 0.0, -base_frame_offset_[2]); + tf2::Quaternion orientation_R_b_odom; + orientation_R_b_odom.setRPY(0.0, 0.0, -base_frame_offset_[2]); - tf2::Matrix3x3 R_b_b0 = tf2::Matrix3x3((orientation_R_b_b0)); - tf2::Vector3 vx_Ob_b_b0_b0 = R_b_b0 * tf2::Vector3(vx_Ob_b_b0_b_, vy_Ob_b_b0_b_, 0.0); + tf2::Matrix3x3 angular_transformation_from_base_2_odom = tf2::Matrix3x3((orientation_R_b_odom)); + tf2::Vector3 body_velocity_base_frame_w_r_t_odom_frame_ = angular_transformation_from_base_2_odom * tf2::Vector3(body_velocity_base_frame_.linear_x, body_velocity_base_frame_.linear_y, 0.0); - px_b_b0_ += vx_Ob_b_b0_b0.x() * dt; - py_b_b0_ += vx_Ob_b_b0_b0.y() * dt; - fprintf(stderr, " px_b_b0_ = %f \n", px_b_b0_); - fprintf(stderr, " py_b_b0_ = %f \n", py_b_b0_); + position_x_base_frame_ += body_velocity_base_frame_w_r_t_odom_frame_.x() * dt; + position_y_base_frame_ += body_velocity_base_frame_w_r_t_odom_frame_.y() * dt; + fprintf(stderr, " position_x_base_frame_ = %f \n", position_x_base_frame_); + fprintf(stderr, " position_y_base_frame_ = %f \n", position_y_base_frame_); return true; } From 92a755d499e21ae42712b10df503ad6dd5a6596b Mon Sep 17 00:00:00 2001 From: GiridharBukka Date: Wed, 11 Jan 2023 10:07:15 +0530 Subject: [PATCH 15/91] code review suggestions implementing --- .../test/test_mecanum_drive_controller.cpp | 46 ++++++++++--------- .../test/test_mecanum_drive_controller.hpp | 5 ++ 2 files changed, 29 insertions(+), 22 deletions(-) diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp index 407109f3e5..ce87fca358 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp @@ -199,7 +199,8 @@ TEST_F(MecanumDriveControllerTest, receive_message_and_publish_updated_status) controller_interface::return_type::OK); ControllerStateMsg msg; subscribe_and_get_messages(msg); - joint_command_values_[1] = 111; + joint_command_values_[1] = command_lin_x; + EXPECT_TRUE(std::isnan(msg.linear_x_velocity_command)); publish_commands(controller_->get_node()->now()); @@ -213,6 +214,8 @@ TEST_F(MecanumDriveControllerTest, receive_message_and_publish_updated_status) controller_->update_and_write_commands( controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); +// w0_vel = 1.0 / params_.wheels_radius * (body_velocity_center_frame_.linear_x - body_velocity_center_frame_.linear_y - params_.wheels_k * body_velocity_center_frame_.angular_z); +// joint_command_values_[1] = 1.0 / 0.5 * (1.5 - 0.0 - 1 * 0.0) EXPECT_EQ(joint_command_values_[1], 3.0); EXPECT_FALSE(std::isnan(joint_command_values_[1])); fprintf(stderr, " joint_command_values_[1]= %f \n", joint_command_values_[1]); @@ -234,18 +237,18 @@ TEST_F(MecanumDriveControllerTest, test_sending_too_old_message) // try to set command with time before timeout - command is not updated auto reference = *(controller_->input_ref_.readFromNonRT()); auto old_timestamp = reference->header.stamp; - EXPECT_TRUE(std::isnan((*reference)->twist.linear.x)); - EXPECT_TRUE(std::isnan((*reference)->twist.linear.y)); - EXPECT_TRUE(std::isnan((*reference)->twist.angular.z)); + EXPECT_TRUE(std::isnan((reference)->twist.linear.x)); + EXPECT_TRUE(std::isnan((reference)->twist.linear.y)); + EXPECT_TRUE(std::isnan((reference)->twist.angular.z)); publish_commands( controller_->get_node()->now() - controller_->ref_timeout_ - rclcpp::Duration::from_seconds(0.1)); ASSERT_TRUE(controller_->wait_for_commands(executor)); ASSERT_EQ(old_timestamp, (*(controller_->input_ref_.readFromNonRT()))->header.stamp); - EXPECT_TRUE(std::isnan((*reference)->twist.linear.x)); - EXPECT_TRUE(std::isnan((*reference)->twist.linear.y)); - EXPECT_TRUE(std::isnan((*reference)->twist.angular.z)); + EXPECT_TRUE(std::isnan((reference)->twist.linear.x)); + EXPECT_TRUE(std::isnan((reference)->twist.linear.y)); + EXPECT_TRUE(std::isnan((reference)->twist.angular.z)); } TEST_F(MecanumDriveControllerTest, test_time_stamp_zero) @@ -312,10 +315,8 @@ TEST_F(MecanumDriveControllerTest, test_update_logic_chainable) ASSERT_FALSE(controller_->is_in_chained_mode()); // set command statically - static constexpr double TEST_LINEAR_VELOCITY_X = 1.5; - static constexpr double TEST_LINEAR_VELOCITY_y = 0.0; - static constexpr double TEST_ANGULAR_VELOCITY_Z = 0.0; - joint_command_values_[1] = 111; + joint_command_values_[1] = command_lin_x; + std::shared_ptr msg = std::make_shared(); msg->header.stamp = controller_->get_node()->now() - controller_->ref_timeout_ - @@ -377,7 +378,9 @@ TEST_F(MecanumDriveControllerTest, test_update_logic_chainable) controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - EXPECT_NE(joint_command_values_[1], 111); + EXPECT_NE(joint_command_values_[1], command_lin_x); +// w0_vel = 1.0 / params_.wheels_radius * (body_velocity_center_frame_.linear_x - body_velocity_center_frame_.linear_y - params_.wheels_k * body_velocity_center_frame_.angular_z); +// joint_command_values_[1] = 1.0 / 0.5 * (1.5 - 0.0 - 1 * 0.0) EXPECT_EQ(joint_command_values_[1], 3.0); ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); for (const auto & interface : controller_->reference_interfaces_) @@ -398,10 +401,8 @@ TEST_F(MecanumDriveControllerTest, test_update_logic) auto reference = controller_->input_ref_.readFromNonRT(); // set command statically - static constexpr double TEST_LINEAR_VELOCITY_X = 1.5; - static constexpr double TEST_LINEAR_VELOCITY_y = 0.0; - static constexpr double TEST_ANGULAR_VELOCITY_Z = 0.0; - joint_command_values_[1] = 111; + joint_command_values_[1] = command_lin_x; + std::shared_ptr msg = std::make_shared(); msg->header.stamp = controller_->get_node()->now() - controller_->ref_timeout_ - rclcpp::Duration::from_seconds(0.1); @@ -454,7 +455,9 @@ TEST_F(MecanumDriveControllerTest, test_update_logic) // EXPECT_EQ(joint_command_values_[NR_CMD_ITFS - 2], TEST_LINEAR_VELOCITY_X); EXPECT_FALSE(std::isnan(joint_command_values_[1])); - EXPECT_NE(joint_command_values_[1], 111); + EXPECT_NE(joint_command_values_[1], command_lin_x); +// w0_vel = 1.0 / params_.wheels_radius * (body_velocity_center_frame_.linear_x - body_velocity_center_frame_.linear_y - params_.wheels_k * body_velocity_center_frame_.angular_z); +// joint_command_values_[1] = 1.0 / 0.5 * (1.5 - 0.0 - 1 * 0.0) EXPECT_EQ(joint_command_values_[1], 3.0); ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); @@ -471,10 +474,7 @@ TEST_F(MecanumDriveControllerTest, test_ref_timeout_zero_for_update) auto reference = controller_->input_ref_.readFromNonRT(); // set command statically - static constexpr double TEST_LINEAR_VELOCITY_X = 1.5; - static constexpr double TEST_LINEAR_VELOCITY_y = 0.0; - static constexpr double TEST_ANGULAR_VELOCITY_Z = 0.0; - joint_command_values_[1] = 111; + joint_command_values_[1] = command_lin_x; controller_->ref_timeout_ = rclcpp::Duration::from_seconds(0.0); std::shared_ptr msg = std::make_shared(); @@ -502,7 +502,9 @@ TEST_F(MecanumDriveControllerTest, test_ref_timeout_zero_for_update) // EXPECT_EQ(joint_command_values_[NR_STATE_ITFS - 2], TEST_LINEAR_VELOCITY_X); EXPECT_FALSE(std::isnan(joint_command_values_[1])); - EXPECT_NE(joint_command_values_[1], 111); + EXPECT_NE(joint_command_values_[1], command_lin_x); +// w0_vel = 1.0 / params_.wheels_radius * (body_velocity_center_frame_.linear_x - body_velocity_center_frame_.linear_y - params_.wheels_k * body_velocity_center_frame_.angular_z); +// joint_command_values_[1] = 1.0 / 0.5 * (1.5 - 0.0 - 1 * 0.0) EXPECT_EQ(joint_command_values_[1], 3.0); ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); ASSERT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x)); diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp index e38d169007..0bd4014d36 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp @@ -263,6 +263,11 @@ class MecanumDriveControllerFixture : public ::testing::Test std::array joint_state_values_ = {0.1, 0.1, 0.1, 0.1}; std::array joint_command_values_ = {101.101, 101.101, 101.101, 101.101}; + static constexpr double TEST_LINEAR_VELOCITY_X = 1.5; + static constexpr double TEST_LINEAR_VELOCITY_y = 0.0; + static constexpr double TEST_ANGULAR_VELOCITY_Z = 0.0; + double command_lin_x = 111; + std::vector state_itfs_; std::vector command_itfs_; From 62663d83e3eee2b2d9d2a08654f380f0feb8c7cb Mon Sep 17 00:00:00 2001 From: Robotgir <47585672+Robotgir@users.noreply.github.com> Date: Wed, 11 Jan 2023 14:11:27 +0530 Subject: [PATCH 16/91] Apply suggestions from code review MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit re-positioned controller_state_publisher_->unlock(); Co-authored-by: Denis Štogl --- mecanum_drive_controller/src/mecanum_drive_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index 813d6b3e99..3698d5c089 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -171,7 +171,6 @@ controller_interface::CallbackReturn MecanumDriveController::on_configure( controller_state_publisher_->msg_.header.frame_id = params_.odom_frame_id; controller_state_publisher_->msg_.odom.child_frame_id = params_.base_frame_id; controller_state_publisher_->msg_.odom.pose.pose.position.x = 0.0; - controller_state_publisher_->unlock(); auto & covariance_controller = controller_state_publisher_->msg_.odom.twist.covariance; for (size_t index = 0; index < 6; ++index) { @@ -180,6 +179,7 @@ controller_interface::CallbackReturn MecanumDriveController::on_configure( covariance_controller[diagonal_index] = params_.pose_covariance_diagonal[index]; covariance_controller[diagonal_index] = params_.twist_covariance_diagonal[index]; } + controller_state_publisher_->unlock(); RCLCPP_INFO(get_node()->get_logger(), "configure successful"); return controller_interface::CallbackReturn::SUCCESS; From e130765e432bc4f83e5f4bf48d14b6092bb5cb6a Mon Sep 17 00:00:00 2001 From: GiridharBukka Date: Wed, 11 Jan 2023 13:59:35 +0530 Subject: [PATCH 17/91] added description --- .../include/mecanum_drive_controller/odometry.hpp | 2 ++ .../mecanum_drive_controller.xml | 2 +- mecanum_drive_controller/package.xml | 15 ++++++++++++--- 3 files changed, 15 insertions(+), 4 deletions(-) diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp b/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp index 3b77bb4604..fcb957f5ab 100644 --- a/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp +++ b/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp @@ -110,6 +110,8 @@ class Odometry // // double body_velocity_base_frame_.angular_z; // [rad/s] /// Wheels kinematic parameters [m]: + /// lx and ly represent the distance from the robot's center to the wheels projected on + /// the x and y axis with origin at robots center respectively, wheels_k_ = lx+ly double wheels_k_; double wheels_radius_; }; diff --git a/mecanum_drive_controller/mecanum_drive_controller.xml b/mecanum_drive_controller/mecanum_drive_controller.xml index 4754a6c1fa..af1bfb1f3a 100644 --- a/mecanum_drive_controller/mecanum_drive_controller.xml +++ b/mecanum_drive_controller/mecanum_drive_controller.xml @@ -2,7 +2,7 @@ - MecanumDriveController ros2_control controller. + The MecanumDriveController tracks velocity commands. It 4 joints should have hardware interface type of hardware_interface::HW_IF_VELOCITY. diff --git a/mecanum_drive_controller/package.xml b/mecanum_drive_controller/package.xml index a0d38babe1..26a4f8ddd5 100644 --- a/mecanum_drive_controller/package.xml +++ b/mecanum_drive_controller/package.xml @@ -3,9 +3,18 @@ mecanum_drive_controller 0.0.0 - DESCRIPTION - giridhar - MIT + Implementation of mecanum drive controller for 4 wheel drive. + + Denis Štogl + Bence Magyar + Giridhar Bukka + + + Dr.-Ing. Denis Štogl + Bence Magyar + Giridhar Bukka + + Apache License 2.0 ament_cmake From 179a84e2fe893ddc9da659af4020499f12e3a106 Mon Sep 17 00:00:00 2001 From: GiridharBukka Date: Wed, 11 Jan 2023 19:37:32 +0530 Subject: [PATCH 18/91] checked more values against the behavior of controller methods --- .../src/mecanum_drive_controller.cpp | 10 ++-- .../src/mecanum_drive_controller.yaml | 4 +- .../test/test_mecanum_drive_controller.cpp | 58 +++++++++++++++++-- .../test/test_mecanum_drive_controller.hpp | 2 +- 4 files changed, 61 insertions(+), 13 deletions(-) diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index 3698d5c089..94f1194a51 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -210,6 +210,8 @@ void MecanumDriveController::reference_callback(const std::shared_ptrheader.stamp).seconds(), age_of_last_command.seconds(), ref_timeout_.seconds()); + reset_controller_reference_msg(msg, get_node()); + } } @@ -308,7 +310,7 @@ controller_interface::return_type MecanumDriveController::update_reference_from_ // 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)) + if (!std::isnan(current_ref->twist.linear.x) && !std::isnan(current_ref->twist.linear.y) && !std::isnan(current_ref->twist.angular.z)) { reference_interfaces_[0] = current_ref->twist.linear.x; reference_interfaces_[1] = current_ref->twist.linear.y; @@ -351,9 +353,9 @@ controller_interface::return_type MecanumDriveController::update_and_write_comma { tf2::Quaternion quaternion; quaternion.setRPY(0.0, 0.0, params_.base_frame_offset[2]); - /// \note The variables meaning: - /// angular_transformation_from_base_2_center: Rotation transformation matrix, to transform from base frame to center frame - /// linear_transformation_from_base_2_center: offset/linear transformation matrix, to transform from base frame to center frame + /// \note The variables meaning: + /// angular_transformation_from_base_2_center: Rotation transformation matrix, to transform from base frame to center frame + /// linear_transformation_from_base_2_center: offset/linear transformation matrix, to transform from base frame to center frame tf2::Matrix3x3 angular_transformation_from_base_2_center = tf2::Matrix3x3((quaternion)); tf2::Vector3 body_velocity_base_frame_w_r_t_center_frame_ = diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.yaml b/mecanum_drive_controller/src/mecanum_drive_controller.yaml index ad350a0fdd..0bf29ac523 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.yaml +++ b/mecanum_drive_controller/src/mecanum_drive_controller.yaml @@ -85,7 +85,7 @@ mecanum_drive_controller: twist_covariance_diagonal: { type: double_array, - default_value: [0, 7, 14, 21, 28, 35], + default_value: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1], description: "diagonal values of twist covariance matrix.", read_only: false, @@ -93,7 +93,7 @@ mecanum_drive_controller: pose_covariance_diagonal: { type: double_array, - default_value: [0, 7, 14, 21, 28, 35], + default_value: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1], description: "diagonal values of pose covariance matrix.", read_only: false, diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp index ce87fca358..411e391923 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp @@ -36,8 +36,20 @@ TEST_F(MecanumDriveControllerTest, all_parameters_set_configure_success) SetUpController(); ASSERT_EQ(controller_->params_.reference_timeout, 0.0); + ASSERT_EQ(controller_->params_.wheels_radius, 0.0); + ASSERT_EQ(controller_->params_.wheels_k, 0.0); + + for(size_t i = 0; iparams_.base_frame_offset.size(); i++){ + ASSERT_EQ(controller_->params_.base_frame_offset[i], 0.0); + } ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(controller_->params_.reference_timeout, 0.1); + ASSERT_EQ(controller_->params_.wheels_radius, 0.5); + ASSERT_EQ(controller_->params_.wheels_k, 1.0); + + for(size_t i = 0; iparams_.base_frame_offset.size(); i++){ + ASSERT_EQ(controller_->params_.base_frame_offset[i], 0.0); + } } TEST_F(MecanumDriveControllerTest, check_exported_intefaces) @@ -164,6 +176,12 @@ TEST_F(MecanumDriveControllerTest, publish_status_success) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ControllerStateMsg msg; + subscribe_and_get_messages(msg); + + EXPECT_TRUE(msg.odom.pose.pose.position.x >= 0.0 && msg.odom.pose.pose.position.x <= 0.0005); + EXPECT_TRUE(msg.odom.pose.pose.position.y >= 0.0 && msg.odom.pose.pose.position.y <= 0.0005); + ASSERT_EQ( controller_->update_reference_from_subscribers( controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), @@ -173,10 +191,12 @@ TEST_F(MecanumDriveControllerTest, publish_status_success) controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - ControllerStateMsg msg; subscribe_and_get_messages(msg); + fprintf(stderr, " msg.odom.pose.pose.position.y= %f \n", msg.odom.pose.pose.position.y); + + EXPECT_FALSE(msg.odom.pose.pose.position.x >= 0.0 && msg.odom.pose.pose.position.x <= 0.0005); + EXPECT_TRUE(msg.odom.pose.pose.position.y >= 0.0 && msg.odom.pose.pose.position.y <= 0.0005); - EXPECT_FALSE(std::isnan(msg.odom.pose.pose.position.x)); } TEST_F(MecanumDriveControllerTest, receive_message_and_publish_updated_status) @@ -260,7 +280,6 @@ TEST_F(MecanumDriveControllerTest, test_time_stamp_zero) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - // try to set command with time before timeout - command is not updated auto reference = controller_->input_ref_.readFromNonRT(); auto old_timestamp = (*reference)->header.stamp; EXPECT_TRUE(std::isnan((*reference)->twist.linear.x)); @@ -276,11 +295,13 @@ TEST_F(MecanumDriveControllerTest, test_time_stamp_zero) EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x, 1.5); EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.linear.y, 0.0); EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z, 0.0); + EXPECT_NE((*(controller_->input_ref_.readFromNonRT()))->header.stamp.sec, 0.0); } TEST_F(MecanumDriveControllerTest, test_message_accepted) { SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; executor.add_node(controller_->get_node()->get_node_base_interface()); @@ -302,10 +323,11 @@ TEST_F(MecanumDriveControllerTest, test_message_accepted) EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z, 0.0); } -TEST_F(MecanumDriveControllerTest, test_update_logic_chainable) +TEST_F(MecanumDriveControllerTest, test_update_logic_not_chainable) { // 1. age>ref_timeout 2. ageget_node()->get_node_base_interface()); @@ -347,12 +369,15 @@ TEST_F(MecanumDriveControllerTest, test_update_logic_chainable) EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[0])); EXPECT_EQ(joint_command_values_[1], 0); - // ASSERT_EQ(controller_->reference_interfaces_[0], 0); for (const auto & interface : controller_->reference_interfaces_) { - // ASSERT_EQ(interface, 0); EXPECT_TRUE(std::isnan(interface)); } + for (size_t i = 0; i < controller_->command_interfaces_.size(); ++i) + { + // EXPECT_TRUE(std::isnan(controller_->command_interfaces_[i].get_value())); + EXPECT_EQ(controller_->command_interfaces_[i].get_value(), 0.0); + } std::shared_ptr msg_2 = std::make_shared(); msg_2->header.stamp = controller_->get_node()->now() - rclcpp::Duration::from_seconds(0.01); @@ -387,6 +412,10 @@ TEST_F(MecanumDriveControllerTest, test_update_logic_chainable) { EXPECT_FALSE(std::isnan(interface)); } + for (size_t i = 0; i < controller_->command_interfaces_.size(); ++i) + { + EXPECT_EQ(controller_->command_interfaces_[i].get_value(), 3.0); + } } TEST_F(MecanumDriveControllerTest, test_update_logic) @@ -429,6 +458,15 @@ TEST_F(MecanumDriveControllerTest, test_update_logic) EXPECT_EQ(joint_command_values_[1], 0); 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_TRUE(std::isnan(controller_->command_interfaces_[i].get_value())); + EXPECT_EQ(controller_->command_interfaces_[i].get_value(), 0.0); + } std::shared_ptr msg_2 = std::make_shared(); msg_2->header.stamp = controller_->get_node()->now(); @@ -461,6 +499,14 @@ TEST_F(MecanumDriveControllerTest, test_update_logic) EXPECT_EQ(joint_command_values_[1], 3.0); ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_FALSE(std::isnan(interface)); + } + for (size_t i = 0; i < controller_->command_interfaces_.size(); ++i) + { + EXPECT_EQ(controller_->command_interfaces_[i].get_value(), 3.0); + } } TEST_F(MecanumDriveControllerTest, test_ref_timeout_zero_for_update) diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp index 0bd4014d36..61bd721380 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp @@ -60,7 +60,7 @@ class TestableMecanumDriveController : public mecanum_drive_controller::MecanumD FRIEND_TEST(MecanumDriveControllerTest, test_sending_too_old_message); FRIEND_TEST(MecanumDriveControllerTest, test_time_stamp_zero); FRIEND_TEST(MecanumDriveControllerTest, test_message_accepted); - FRIEND_TEST(MecanumDriveControllerTest, test_update_logic_chainable); + FRIEND_TEST(MecanumDriveControllerTest, test_update_logic_not_chainable); FRIEND_TEST(MecanumDriveControllerTest, test_update_logic); FRIEND_TEST(MecanumDriveControllerTest, test_ref_timeout_zero_for_update); FRIEND_TEST(MecanumDriveControllerTest, test_ref_timeout_zero_for_reference_callback); From 826f8ae9d0814dbb62a2a4702aa6965d6590ce3b Mon Sep 17 00:00:00 2001 From: GiridharBukka Date: Wed, 11 Jan 2023 19:42:09 +0530 Subject: [PATCH 19/91] changed controller type to CtrlType --- mecanum_drive_controller/test/test_mecanum_drive_controller.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp index 61bd721380..7cfddc1306 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp @@ -274,7 +274,7 @@ class MecanumDriveControllerFixture : public ::testing::Test double ref_timeout_ = 0.2; // Test related parameters - std::unique_ptr controller_; + std::unique_ptr controller_; rclcpp::Node::SharedPtr command_publisher_node_; rclcpp::Publisher::SharedPtr command_publisher_; rclcpp::Node::SharedPtr odom_s_publisher_node_; From 14bcf33c2062809f8e4743d37e015aff04cf534a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Wed, 11 Jan 2023 18:41:12 +0100 Subject: [PATCH 20/91] Update mecanum_drive_controller/test/test_mecanum_drive_controller.cpp --- mecanum_drive_controller/test/test_mecanum_drive_controller.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp index 411e391923..07b0be5dbe 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp @@ -238,7 +238,6 @@ TEST_F(MecanumDriveControllerTest, receive_message_and_publish_updated_status) // joint_command_values_[1] = 1.0 / 0.5 * (1.5 - 0.0 - 1 * 0.0) EXPECT_EQ(joint_command_values_[1], 3.0); EXPECT_FALSE(std::isnan(joint_command_values_[1])); - fprintf(stderr, " joint_command_values_[1]= %f \n", joint_command_values_[1]); subscribe_and_get_messages(msg); From a637de4bcb0fe1b35156608c600e1fc22d5c95a5 Mon Sep 17 00:00:00 2001 From: GiridharBukka Date: Thu, 12 Jan 2023 13:54:02 +0530 Subject: [PATCH 21/91] updated cmakelists structure --- mecanum_drive_controller/CMakeLists.txt | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/mecanum_drive_controller/CMakeLists.txt b/mecanum_drive_controller/CMakeLists.txt index 7c363c7db2..4b60354ee2 100644 --- a/mecanum_drive_controller/CMakeLists.txt +++ b/mecanum_drive_controller/CMakeLists.txt @@ -39,7 +39,9 @@ add_library( src/mecanum_drive_controller.cpp src/odometry.cpp ) -target_include_directories(mecanum_drive_controller PRIVATE include) +target_include_directories(mecanum_drive_controller PUBLIC +"$" +"$") target_link_libraries(mecanum_drive_controller mecanum_drive_controller_parameters) ament_target_dependencies(mecanum_drive_controller ${THIS_PACKAGE_INCLUDE_DEPENDS}) target_compile_definitions(mecanum_drive_controller PRIVATE "MECANUM_DRIVE_CONTROLLER_BUILDING_DLL") From 9ca0a501ccc0c9c51e425d6c6f0af8939a819997 Mon Sep 17 00:00:00 2001 From: GiridharBukka Date: Thu, 12 Jan 2023 16:01:58 +0530 Subject: [PATCH 22/91] renaming variables --- .../mecanum_drive_controller.hpp | 12 ++--- .../mecanum_drive_controller/odometry.hpp | 48 ++++++------------- .../src/mecanum_drive_controller.cpp | 18 +++---- mecanum_drive_controller/src/odometry.cpp | 37 +++++++------- 4 files changed, 48 insertions(+), 67 deletions(-) diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp index a02200ec27..8d24eb383e 100644 --- a/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp +++ b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp @@ -120,14 +120,10 @@ class MecanumDriveController : public controller_interface::ChainableControllerI // callback for topic interface MECANUM_DRIVE_CONTROLLER__VISIBILITY_LOCAL void reference_callback(const std::shared_ptr msg); - struct body_velocity{ - double linear_x; // [m/s] - double linear_y; // [m/s] - double angular_z; // [rad/s] - - body_velocity() : linear_x(0.0), linear_y(0.0), angular_z(0.0) {} - }; - body_velocity body_velocity_center_frame_; + + double velocity_in_center_frame_linear_x; // [m/s] + double velocity_in_center_frame_linear_y; // [m/s] + double velocity_in_center_frame_angular_z; // [rad/s] }; diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp b/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp index fcb957f5ab..71b516eaf1 100644 --- a/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp +++ b/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp @@ -42,7 +42,7 @@ class Odometry /// \brief Initialize the odometry /// \param time Current time - void init(const rclcpp::Time & time, double base_frame_offset[PLANAR_POINT_DIM]); + void init(const rclcpp::Time & time, std::array base_frame_offset); /// \brief Updates the odometry class with latest wheels position /// \param wheel0_vel Wheel velocity [rad/s] @@ -55,17 +55,17 @@ class Odometry double wheel0_vel, double wheel1_vel, double wheel2_vel, double wheel3_vel, const double dt); /// \return position (x component) [m] - double getX() const { return position_x_base_frame_; } + double getX() const { return position_x_in_base_frame_; } /// \return position (y component) [m] - double getY() const { return position_y_base_frame_; } + double getY() const { return position_y_in_base_frame_; } /// \return orientation (z component) [m] - double getRz() const { return orientation_z_base_frame_; } + double getRz() const { return orientation_z_in_base_frame_; } /// \return body velocity of the base frame (linear x component) [m/s] - double getVx() const { return body_velocity_base_frame_.linear_x; } + double getVx() const { return velocity_in_base_frame_linear_x; } /// \return body velocity of the base frame (linear y component) [m/s] - double getVy() const { return body_velocity_base_frame_.linear_y; } + double getVy() const { return velocity_in_base_frame_linear_y; } /// \return body velocity of the base frame (angular z component) [m/s] - double getWz() const { return body_velocity_base_frame_.angular_z; } + double getWz() const { return velocity_in_base_frame_angular_z;; } /// \brief Sets the wheels parameters: mecanum geometric param and radius /// \param wheels_k Wheels geometric param (used in mecanum wheels' ik) [m] @@ -81,33 +81,13 @@ class Odometry std::array base_frame_offset_; /// Current pose: - double position_x_base_frame_; // [m] - double position_y_base_frame_; // [m] - double orientation_z_base_frame_; // [rad] - - /// Current velocity. - /// \note The indices meaning is the following: - /// b : base frame - /// c : center frame - /// body_velocity_base_frame_: body_velocity w.r.t base frame - /// body_velocity_center_frame_: body_velocity w.r.t center frame - /// Oc: origin of the center frame - /// b0: initial position if the base frame - /// c0: initial position of the center frame - - struct body_velocity{ - double linear_x; // [m/s] - double linear_y; // [m/s] - double angular_z; // [rad/s] - - body_velocity() : linear_x(0.0), linear_y(0.0), angular_z(0.0) {} - }; - - body_velocity body_velocity_base_frame_; - body_velocity body_velocity_center_frame_; - // // body_velocity_base_frame_.linear_x; // [m/s] body_velocity_base_frame_.linear_x - // // double body_velocity_base_frame_.linear_y; // [m/s] - // // double body_velocity_base_frame_.angular_z; // [rad/s] + double position_x_in_base_frame_; // [m] + double position_y_in_base_frame_; // [m] + double orientation_z_in_base_frame_; // [rad] + + double velocity_in_base_frame_linear_x; // [m/s] + double velocity_in_base_frame_linear_y; // [m/s] + double velocity_in_base_frame_angular_z; // [rad/s] /// Wheels kinematic parameters [m]: /// lx and ly represent the distance from the robot's center to the wheels projected on diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index 94f1194a51..8e5a04bcd4 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -357,24 +357,26 @@ controller_interface::return_type MecanumDriveController::update_and_write_comma /// angular_transformation_from_base_2_center: Rotation transformation matrix, to transform from base frame to center frame /// linear_transformation_from_base_2_center: offset/linear transformation matrix, to transform from base frame to center frame + + tf2::Matrix3x3 angular_transformation_from_base_2_center = tf2::Matrix3x3((quaternion)); - tf2::Vector3 body_velocity_base_frame_w_r_t_center_frame_ = + tf2::Vector3 velocity_in_base_frame_w_r_t_center_frame_ = angular_transformation_from_base_2_center * tf2::Vector3(reference_interfaces_[0], reference_interfaces_[1], 0.0); tf2::Vector3 linear_transformation_from_base_2_center = tf2::Vector3(params_.base_frame_offset[0], params_.base_frame_offset[1], 0.0); - body_velocity_center_frame_.linear_x = body_velocity_base_frame_w_r_t_center_frame_.x() + linear_transformation_from_base_2_center.y() * reference_interfaces_[2]; - body_velocity_center_frame_.linear_y = body_velocity_base_frame_w_r_t_center_frame_.y() - linear_transformation_from_base_2_center.x() * reference_interfaces_[2]; - body_velocity_center_frame_.angular_z = reference_interfaces_[2]; + velocity_in_center_frame_linear_x = velocity_in_base_frame_w_r_t_center_frame_.x() + linear_transformation_from_base_2_center.y() * reference_interfaces_[2]; + velocity_in_center_frame_linear_y = velocity_in_base_frame_w_r_t_center_frame_.y() - linear_transformation_from_base_2_center.x() * reference_interfaces_[2]; + velocity_in_center_frame_angular_z = reference_interfaces_[2]; double w0_vel = - 1.0 / params_.wheels_radius * (body_velocity_center_frame_.linear_x - body_velocity_center_frame_.linear_y - params_.wheels_k * body_velocity_center_frame_.angular_z); + 1.0 / params_.wheels_radius * (velocity_in_center_frame_linear_x - velocity_in_center_frame_linear_y - params_.wheels_k * velocity_in_center_frame_angular_z); double w1_vel = - 1.0 / params_.wheels_radius * (body_velocity_center_frame_.linear_x + body_velocity_center_frame_.linear_y - params_.wheels_k * body_velocity_center_frame_.angular_z); + 1.0 / params_.wheels_radius * (velocity_in_center_frame_linear_x + velocity_in_center_frame_linear_y - params_.wheels_k * velocity_in_center_frame_angular_z); double w2_vel = - 1.0 / params_.wheels_radius * (body_velocity_center_frame_.linear_x - body_velocity_center_frame_.linear_y + params_.wheels_k * body_velocity_center_frame_.angular_z); + 1.0 / params_.wheels_radius * (velocity_in_center_frame_linear_x - velocity_in_center_frame_linear_y + params_.wheels_k * velocity_in_center_frame_angular_z); double w3_vel = - 1.0 / params_.wheels_radius * (body_velocity_center_frame_.linear_x + body_velocity_center_frame_.linear_y + params_.wheels_k * body_velocity_center_frame_.angular_z); + 1.0 / params_.wheels_radius * (velocity_in_center_frame_linear_x + velocity_in_center_frame_linear_y + params_.wheels_k * velocity_in_center_frame_angular_z); // Set wheels velocities: diff --git a/mecanum_drive_controller/src/odometry.cpp b/mecanum_drive_controller/src/odometry.cpp index 81a24777d5..aad2b4c170 100644 --- a/mecanum_drive_controller/src/odometry.cpp +++ b/mecanum_drive_controller/src/odometry.cpp @@ -24,15 +24,18 @@ namespace mecanum_drive_controller Odometry::Odometry(size_t velocity_rolling_window_size) : timestamp_(0.0), - position_x_base_frame_(0.0), - position_y_base_frame_(0.0), - orientation_z_base_frame_(0.0), + position_x_in_base_frame_(0.0), + position_y_in_base_frame_(0.0), + orientation_z_in_base_frame_(0.0), + velocity_in_base_frame_linear_x(0.0), + velocity_in_base_frame_linear_y(0.0), + velocity_in_base_frame_angular_z(0.0), wheels_k_(0.0), wheels_radius_(0.0) { } -void Odometry::init(const rclcpp::Time & time, double base_frame_offset[PLANAR_POINT_DIM]) +void Odometry::init(const rclcpp::Time & time, std::array base_frame_offset) { // Reset timestamp: timestamp_ = time; @@ -65,38 +68,38 @@ bool Odometry::update( /// angular_transformation_from_center_2_base: Rotation transformation matrix, to transform from center frame to base frame /// linear_transformation_from_center_2_base: offset/linear transformation matrix, to transform from center frame to base frame - body_velocity_center_frame_.linear_x = 0.25 * wheels_radius_ * (wheel0_vel + wheel1_vel + wheel2_vel + wheel3_vel); - body_velocity_center_frame_.linear_y = + double velocity_in_center_frame_linear_x = 0.25 * wheels_radius_ * (wheel0_vel + wheel1_vel + wheel2_vel + wheel3_vel); + double velocity_in_center_frame_linear_y = 0.25 * wheels_radius_ * (-wheel0_vel + wheel1_vel - wheel2_vel + wheel3_vel); - body_velocity_center_frame_.angular_z = + double velocity_in_center_frame_angular_z = 0.25 * wheels_radius_ / wheels_k_ * (-wheel0_vel - wheel1_vel + wheel2_vel + wheel3_vel); tf2::Quaternion orientation_R_c_b; orientation_R_c_b.setRPY(0.0, 0.0, -base_frame_offset_[2]); tf2::Matrix3x3 angular_transformation_from_center_2_base = tf2::Matrix3x3((orientation_R_c_b)); - tf2::Vector3 body_velocity_center_frame_w_r_t_base_frame_ =angular_transformation_from_center_2_base * tf2::Vector3(body_velocity_center_frame_.linear_x, body_velocity_center_frame_.linear_y, 0.0); + tf2::Vector3 velocity_in_center_frame_w_r_t_base_frame_ =angular_transformation_from_center_2_base * tf2::Vector3(velocity_in_center_frame_linear_x, velocity_in_center_frame_linear_y, 0.0); tf2::Vector3 linear_transformation_from_center_2_base =angular_transformation_from_center_2_base * tf2::Vector3(-base_frame_offset_[0], -base_frame_offset_[1], 0.0); - body_velocity_base_frame_.linear_x = body_velocity_center_frame_w_r_t_base_frame_.x() + linear_transformation_from_center_2_base.y() * body_velocity_center_frame_.angular_z; - body_velocity_base_frame_.linear_y = body_velocity_center_frame_w_r_t_base_frame_.y() - linear_transformation_from_center_2_base.x() * body_velocity_center_frame_.angular_z; - body_velocity_base_frame_.angular_z = body_velocity_center_frame_.angular_z; + velocity_in_base_frame_linear_x = velocity_in_center_frame_w_r_t_base_frame_.x() + linear_transformation_from_center_2_base.y() * velocity_in_center_frame_angular_z; + velocity_in_base_frame_linear_y = velocity_in_center_frame_w_r_t_base_frame_.y() - linear_transformation_from_center_2_base.x() * velocity_in_center_frame_angular_z; + velocity_in_base_frame_angular_z = velocity_in_center_frame_angular_z; /// Integration. /// NOTE: the position is expressed in the odometry frame (frame b0), unlike the twist which is /// expressed in the body frame (frame b). - orientation_z_base_frame_ += body_velocity_base_frame_.angular_z * dt; + orientation_z_in_base_frame_ += velocity_in_base_frame_angular_z * dt; tf2::Quaternion orientation_R_b_odom; orientation_R_b_odom.setRPY(0.0, 0.0, -base_frame_offset_[2]); tf2::Matrix3x3 angular_transformation_from_base_2_odom = tf2::Matrix3x3((orientation_R_b_odom)); - tf2::Vector3 body_velocity_base_frame_w_r_t_odom_frame_ = angular_transformation_from_base_2_odom * tf2::Vector3(body_velocity_base_frame_.linear_x, body_velocity_base_frame_.linear_y, 0.0); + tf2::Vector3 velocity_in_base_frame_w_r_t_odom_frame_ = angular_transformation_from_base_2_odom * tf2::Vector3(velocity_in_base_frame_linear_x, velocity_in_base_frame_linear_y, 0.0); - position_x_base_frame_ += body_velocity_base_frame_w_r_t_odom_frame_.x() * dt; - position_y_base_frame_ += body_velocity_base_frame_w_r_t_odom_frame_.y() * dt; - fprintf(stderr, " position_x_base_frame_ = %f \n", position_x_base_frame_); - fprintf(stderr, " position_y_base_frame_ = %f \n", position_y_base_frame_); + position_x_in_base_frame_ += velocity_in_base_frame_w_r_t_odom_frame_.x() * dt; + position_y_in_base_frame_ += velocity_in_base_frame_w_r_t_odom_frame_.y() * dt; + fprintf(stderr, " position_x_in_base_frame_ = %f \n", position_x_in_base_frame_); + fprintf(stderr, " position_y_in_base_frame_ = %f \n", position_y_in_base_frame_); return true; } From 56e5817e382c35cb7b5d80c1c90253538d12ce77 Mon Sep 17 00:00:00 2001 From: GiridharBukka Date: Thu, 12 Jan 2023 16:10:25 +0530 Subject: [PATCH 23/91] changed description --- mecanum_drive_controller/mecanum_drive_controller.xml | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/mecanum_drive_controller/mecanum_drive_controller.xml b/mecanum_drive_controller/mecanum_drive_controller.xml index af1bfb1f3a..5bfbad4e32 100644 --- a/mecanum_drive_controller/mecanum_drive_controller.xml +++ b/mecanum_drive_controller/mecanum_drive_controller.xml @@ -2,7 +2,6 @@ - The MecanumDriveController tracks velocity commands. It 4 joints should have hardware interface type of hardware_interface::HW_IF_VELOCITY. - + The mecanum drive controller transforms linear and angular velocity messages into signals for each wheel(s) for a 4 mecanum wheeled robot. From 4962c0a8435bf5d749da5617e9723df5c40c7399 Mon Sep 17 00:00:00 2001 From: GiridharBukka Date: Fri, 13 Jan 2023 16:37:33 +0530 Subject: [PATCH 24/91] resetting msg if msg is valid after receiving a to old msg --- .../src/mecanum_drive_controller.cpp | 26 +++++++++++++++++-- 1 file changed, 24 insertions(+), 2 deletions(-) diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index 8e5a04bcd4..263c5b5b78 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -315,6 +315,14 @@ controller_interface::return_type MecanumDriveController::update_reference_from_ reference_interfaces_[0] = current_ref->twist.linear.x; reference_interfaces_[1] = current_ref->twist.linear.y; reference_interfaces_[2] = current_ref->twist.angular.z; + + if (ref_timeout_ == rclcpp::Duration::from_seconds(0)) + { + current_ref->twist.linear.x = std::numeric_limits::quiet_NaN(); + current_ref->twist.linear.y = std::numeric_limits::quiet_NaN(); + current_ref->twist.angular.z = std::numeric_limits::quiet_NaN(); + } + } } else @@ -322,6 +330,13 @@ controller_interface::return_type MecanumDriveController::update_reference_from_ reference_interfaces_[0] = std::numeric_limits::quiet_NaN(); reference_interfaces_[1] = std::numeric_limits::quiet_NaN(); reference_interfaces_[2] = std::numeric_limits::quiet_NaN(); + + if (!std::isnan(current_ref->twist.linear.x) && !std::isnan(current_ref->twist.linear.y) && !std::isnan(current_ref->twist.angular.z)) + { + current_ref->twist.linear.x = std::numeric_limits::quiet_NaN(); + current_ref->twist.linear.y = std::numeric_limits::quiet_NaN(); + current_ref->twist.angular.z = std::numeric_limits::quiet_NaN(); + } } return controller_interface::return_type::OK; @@ -347,8 +362,8 @@ controller_interface::return_type MecanumDriveController::update_and_write_comma // INVERSE KINEMATICS (move robot). // Compute wheels velocities (this is the actual ik): // NOTE: the input desired twist (from topic `~/reference`) is a body twist. - auto current_ref = input_ref_.readFromRT(); - const auto age_of_last_command = time - (*current_ref)->header.stamp; + auto current_ref = *(input_ref_.readFromRT()); + const auto age_of_last_command = time - current_ref->header.stamp; if (age_of_last_command <= ref_timeout_ || ref_timeout_ == rclcpp::Duration::from_seconds(0)) { tf2::Quaternion quaternion; @@ -388,6 +403,8 @@ controller_interface::return_type MecanumDriveController::update_and_write_comma fprintf(stderr, " command_interfaces_[2] = %f \n", w2_vel); command_interfaces_[3].set_value(w3_vel); fprintf(stderr, " command_interfaces_[3] = %f \n", w3_vel); + + } else { @@ -395,9 +412,14 @@ controller_interface::return_type MecanumDriveController::update_and_write_comma reference_interfaces_[1] = std::numeric_limits::quiet_NaN(); reference_interfaces_[2] = std::numeric_limits::quiet_NaN(); command_interfaces_[0].set_value(0.0); + fprintf(stderr, " command_interfaces_[0] = %f \n", command_interfaces_[0].get_value()); command_interfaces_[1].set_value(0.0); command_interfaces_[2].set_value(0.0); command_interfaces_[3].set_value(0.0); + + current_ref->twist.linear.x = std::numeric_limits::quiet_NaN(); + current_ref->twist.linear.y = std::numeric_limits::quiet_NaN(); + current_ref->twist.angular.z = std::numeric_limits::quiet_NaN(); } // Publish odometry message From 0c9eb020e5f8e0da70da13e5a0fd9ef6ca6893e8 Mon Sep 17 00:00:00 2001 From: GiridharBukka Date: Fri, 13 Jan 2023 19:24:52 +0530 Subject: [PATCH 25/91] estructuring parameters in yaml --- .../src/mecanum_drive_controller.cpp | 30 +++---- .../src/mecanum_drive_controller.yaml | 87 +++++++++---------- .../test/mecanum_drive_controller_params.yaml | 16 ++-- 3 files changed, 65 insertions(+), 68 deletions(-) diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index 263c5b5b78..76566daf03 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -76,7 +76,7 @@ controller_interface::CallbackReturn MecanumDriveController::on_configure( params_ = param_listener_->get_params(); // Set wheel params for the odometry computation - odometry_.setWheelsParams(params_.wheels_k, params_.wheels_radius); + odometry_.setWheelsParams(params_.kinematics.wheels_k, params_.kinematics.wheels_radius); // topics QoS auto subscribers_qos = rclcpp::SystemDefaultsQoS(); @@ -223,13 +223,13 @@ MecanumDriveController::command_interface_configuration() const command_interfaces_config.names.reserve(NR_CMD_ITFS); command_interfaces_config.names.push_back( - params_.wheel0_name + "/" + hardware_interface::HW_IF_VELOCITY); + params_.wheel_names[0] + "/" + hardware_interface::HW_IF_VELOCITY); command_interfaces_config.names.push_back( - params_.wheel1_name + "/" + hardware_interface::HW_IF_VELOCITY); + params_.wheel_names[1] + "/" + hardware_interface::HW_IF_VELOCITY); command_interfaces_config.names.push_back( - params_.wheel2_name + "/" + hardware_interface::HW_IF_VELOCITY); + params_.wheel_names[2] + "/" + hardware_interface::HW_IF_VELOCITY); command_interfaces_config.names.push_back( - params_.wheel3_name + "/" + hardware_interface::HW_IF_VELOCITY); + params_.wheel_names[3] + "/" + hardware_interface::HW_IF_VELOCITY); return command_interfaces_config; } @@ -242,13 +242,13 @@ controller_interface::InterfaceConfiguration MecanumDriveController::state_inter state_interfaces_config.names.reserve(NR_STATE_ITFS); state_interfaces_config.names.push_back( - params_.wheel0_name + "/" + hardware_interface::HW_IF_VELOCITY); + params_.wheel_names[0] + "/" + hardware_interface::HW_IF_VELOCITY); state_interfaces_config.names.push_back( - params_.wheel1_name + "/" + hardware_interface::HW_IF_VELOCITY); + params_.wheel_names[1] + "/" + hardware_interface::HW_IF_VELOCITY); state_interfaces_config.names.push_back( - params_.wheel2_name + "/" + hardware_interface::HW_IF_VELOCITY); + params_.wheel_names[2] + "/" + hardware_interface::HW_IF_VELOCITY); state_interfaces_config.names.push_back( - params_.wheel3_name + "/" + hardware_interface::HW_IF_VELOCITY); + params_.wheel_names[3] + "/" + hardware_interface::HW_IF_VELOCITY); return state_interfaces_config; } @@ -367,7 +367,7 @@ controller_interface::return_type MecanumDriveController::update_and_write_comma if (age_of_last_command <= ref_timeout_ || ref_timeout_ == rclcpp::Duration::from_seconds(0)) { tf2::Quaternion quaternion; - quaternion.setRPY(0.0, 0.0, params_.base_frame_offset[2]); + quaternion.setRPY(0.0, 0.0, params_.kinematics.base_frame_offset.theta); /// \note The variables meaning: /// angular_transformation_from_base_2_center: Rotation transformation matrix, to transform from base frame to center frame /// linear_transformation_from_base_2_center: offset/linear transformation matrix, to transform from base frame to center frame @@ -378,20 +378,20 @@ controller_interface::return_type MecanumDriveController::update_and_write_comma tf2::Vector3 velocity_in_base_frame_w_r_t_center_frame_ = angular_transformation_from_base_2_center * tf2::Vector3(reference_interfaces_[0], reference_interfaces_[1], 0.0); tf2::Vector3 linear_transformation_from_base_2_center = - tf2::Vector3(params_.base_frame_offset[0], params_.base_frame_offset[1], 0.0); + tf2::Vector3(params_.kinematics.base_frame_offset.x, params_.kinematics.base_frame_offset.y, 0.0); velocity_in_center_frame_linear_x = velocity_in_base_frame_w_r_t_center_frame_.x() + linear_transformation_from_base_2_center.y() * reference_interfaces_[2]; velocity_in_center_frame_linear_y = velocity_in_base_frame_w_r_t_center_frame_.y() - linear_transformation_from_base_2_center.x() * reference_interfaces_[2]; velocity_in_center_frame_angular_z = reference_interfaces_[2]; double w0_vel = - 1.0 / params_.wheels_radius * (velocity_in_center_frame_linear_x - velocity_in_center_frame_linear_y - params_.wheels_k * velocity_in_center_frame_angular_z); + 1.0 / params_.kinematics.wheels_radius * (velocity_in_center_frame_linear_x - velocity_in_center_frame_linear_y - params_.kinematics.wheels_k * velocity_in_center_frame_angular_z); double w1_vel = - 1.0 / params_.wheels_radius * (velocity_in_center_frame_linear_x + velocity_in_center_frame_linear_y - params_.wheels_k * velocity_in_center_frame_angular_z); + 1.0 / params_.kinematics.wheels_radius * (velocity_in_center_frame_linear_x + velocity_in_center_frame_linear_y - params_.kinematics.wheels_k * velocity_in_center_frame_angular_z); double w2_vel = - 1.0 / params_.wheels_radius * (velocity_in_center_frame_linear_x - velocity_in_center_frame_linear_y + params_.wheels_k * velocity_in_center_frame_angular_z); + 1.0 / params_.kinematics.wheels_radius * (velocity_in_center_frame_linear_x - velocity_in_center_frame_linear_y + params_.kinematics.wheels_k * velocity_in_center_frame_angular_z); double w3_vel = - 1.0 / params_.wheels_radius * (velocity_in_center_frame_linear_x + velocity_in_center_frame_linear_y + params_.wheels_k * velocity_in_center_frame_angular_z); + 1.0 / params_.kinematics.wheels_radius * (velocity_in_center_frame_linear_x + velocity_in_center_frame_linear_y + params_.kinematics.wheels_k * velocity_in_center_frame_angular_z); // Set wheels velocities: diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.yaml b/mecanum_drive_controller/src/mecanum_drive_controller.yaml index 0bf29ac523..c7502b63e3 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.yaml +++ b/mecanum_drive_controller/src/mecanum_drive_controller.yaml @@ -5,32 +5,49 @@ mecanum_drive_controller: 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.", } - wheel0_name: { - type: string, - default_value: "front_left_wheel_joint", - description: "Name of the front left wheel.", - read_only: true, - - } - wheel1_name: { - type: string, - default_value: "back_left_wheel_joint", - description: "Name of the back left wheel.", - read_only: true, - - } - wheel2_name: { - type: string, - default_value: "back_right_wheel_joint", - description: "Name of the back right wheel.", - read_only: true, - } - wheel3_name: { - type: string, - default_value: "front_right_wheel_joint", - description: "Name of the front right wheel.", - read_only: true, + kinematics: + base_frame_offset: + x: { + type: double, + default_value: 0.0, + description: "Base frame offset along X axis of base_frame (base_link frame)." , + read_only: true, + } + y: { + type: double, + default_value: 0.0, + description: "Base frame offset along Y axis of base_frame (base_link frame)." , + read_only: true, + } + theta: { + type: double, + default_value: 0.0, + description: "Base frame offset along Theta axis of base_frame (base_link frame).", + read_only: true, + } + + wheels_radius: { + type: double, + default_value: 0.0, + description: "Wheel's radius.", + read_only: false, + + } + + wheels_k: { + type: double, + default_value: 0.0, + description: "Wheels geometric param used in mecanum wheels' IK.", + read_only: false, + + } + + wheel_names: { + type: string_array, + default_value: ["", "", "", ""], + description: "Name of the wheels.", + read_only: false, } base_frame_id: { @@ -54,27 +71,7 @@ mecanum_drive_controller: read_only: false, } - base_frame_offset: { - type: double_array, - default_value: [0.0, 0.0, 0.0], - description: "Base frame offset values, [base_frame_offset/x base_frame_offset/y base_frame_offset/theta].", - read_only: false, - } - wheels_radius: { - type: double, - default_value: 0.0, - description: "Wheel's radius.", - read_only: false, - - } - wheels_k: { - type: double, - default_value: 0.0, - description: "Wheels geometric param used in mecanum wheels' IK.", - read_only: false, - - } use_realigned_roller_joints: { type: bool, default_value: false, diff --git a/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml b/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml index ed792bb0b3..42e7b96bd2 100644 --- a/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml +++ b/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml @@ -3,13 +3,17 @@ test_mecanum_drive_controller: reference_timeout: 0.1 - wheel0_name: "front_left_wheel_joint" + kinematics: + base_frame_offset: + x: 0.0 + y: 0.0 + theta: 0.0 - wheel1_name: "back_left_wheel_joint" + wheels_radius: 0.5 - wheel2_name: "back_right_wheel_joint" + wheels_k: 1.0 - wheel3_name: "front_right_wheel_joint" + wheel_names: ["front_left_wheel_joint", "back_left_wheel_joint", "back_right_wheel_joint", "front_right_wheel_joint"] base_frame_id: "base_link" @@ -19,10 +23,6 @@ test_mecanum_drive_controller: base_frame_offset: [0.0, 0.0, 0.0] - wheels_radius: 0.5 - - wheels_k: 1.0 - use_realigned_roller_joints: false twist_covariance_diagonal: [0.0, 7.0, 14.0, 21.0, 28.0, 35.0] From 56e8e2f10747939fdc0c0ae4dd9aaec8ea84687b Mon Sep 17 00:00:00 2001 From: GiridharBukka Date: Fri, 13 Jan 2023 19:25:20 +0530 Subject: [PATCH 26/91] comparing float values --- .../test/test_mecanum_drive_controller.cpp | 56 ++++++++++++------- 1 file changed, 35 insertions(+), 21 deletions(-) diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp index 07b0be5dbe..c730000c86 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp @@ -31,25 +31,32 @@ class MecanumDriveControllerTest { }; +namespace +{ +// Floating-point value comparison threshold +const double EPS = 1e-3; +} // namespace + TEST_F(MecanumDriveControllerTest, all_parameters_set_configure_success) { SetUpController(); ASSERT_EQ(controller_->params_.reference_timeout, 0.0); - ASSERT_EQ(controller_->params_.wheels_radius, 0.0); - ASSERT_EQ(controller_->params_.wheels_k, 0.0); + ASSERT_EQ(controller_->params_.kinematics.wheels_radius, 0.0); + ASSERT_EQ(controller_->params_.kinematics.wheels_k, 0.0); + + ASSERT_EQ(controller_->params_.kinematics.base_frame_offset.x, 0.0); + ASSERT_EQ(controller_->params_.kinematics.base_frame_offset.y, 0.0); + ASSERT_EQ(controller_->params_.kinematics.base_frame_offset.theta, 0.0); - for(size_t i = 0; iparams_.base_frame_offset.size(); i++){ - ASSERT_EQ(controller_->params_.base_frame_offset[i], 0.0); - } ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(controller_->params_.reference_timeout, 0.1); - ASSERT_EQ(controller_->params_.wheels_radius, 0.5); - ASSERT_EQ(controller_->params_.wheels_k, 1.0); + ASSERT_EQ(controller_->params_.kinematics.wheels_radius, 0.5); + ASSERT_EQ(controller_->params_.kinematics.wheels_k, 1.0); - for(size_t i = 0; iparams_.base_frame_offset.size(); i++){ - ASSERT_EQ(controller_->params_.base_frame_offset[i], 0.0); - } + ASSERT_EQ(controller_->params_.kinematics.base_frame_offset.x, 0.0); + ASSERT_EQ(controller_->params_.kinematics.base_frame_offset.y, 0.0); + ASSERT_EQ(controller_->params_.kinematics.base_frame_offset.theta, 0.0); } TEST_F(MecanumDriveControllerTest, check_exported_intefaces) @@ -179,8 +186,11 @@ TEST_F(MecanumDriveControllerTest, publish_status_success) ControllerStateMsg msg; subscribe_and_get_messages(msg); - EXPECT_TRUE(msg.odom.pose.pose.position.x >= 0.0 && msg.odom.pose.pose.position.x <= 0.0005); - EXPECT_TRUE(msg.odom.pose.pose.position.y >= 0.0 && msg.odom.pose.pose.position.y <= 0.0005); + // EXPECT_NEAR(msg.odom.pose.pose.position.x, msg.odom.pose.pose.position.Y, EPS); + EXPECT_NEAR(msg.odom.pose.pose.position.x, 0.0, EPS); + EXPECT_NEAR(msg.odom.pose.pose.position.y, 0.0, EPS); + + ASSERT_EQ( controller_->update_reference_from_subscribers( @@ -192,10 +202,10 @@ TEST_F(MecanumDriveControllerTest, publish_status_success) controller_interface::return_type::OK); subscribe_and_get_messages(msg); - fprintf(stderr, " msg.odom.pose.pose.position.y= %f \n", msg.odom.pose.pose.position.y); + fprintf(stderr, " msg.odom.pose.pose.position.x= %f \n", msg.odom.pose.pose.position.x); - EXPECT_FALSE(msg.odom.pose.pose.position.x >= 0.0 && msg.odom.pose.pose.position.x <= 0.0005); - EXPECT_TRUE(msg.odom.pose.pose.position.y >= 0.0 && msg.odom.pose.pose.position.y <= 0.0005); + EXPECT_TRUE(msg.odom.pose.pose.position.x >= 0.0); + EXPECT_NEAR(msg.odom.pose.pose.position.y, 0.0, EPS); } @@ -234,7 +244,7 @@ TEST_F(MecanumDriveControllerTest, receive_message_and_publish_updated_status) controller_->update_and_write_commands( controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); -// w0_vel = 1.0 / params_.wheels_radius * (body_velocity_center_frame_.linear_x - body_velocity_center_frame_.linear_y - params_.wheels_k * body_velocity_center_frame_.angular_z); +// w0_vel = 1.0 / params_.kinematics.wheels_radius * (body_velocity_center_frame_.linear_x - body_velocity_center_frame_.linear_y - params_.kinematics.wheels_k * body_velocity_center_frame_.angular_z); // joint_command_values_[1] = 1.0 / 0.5 * (1.5 - 0.0 - 1 * 0.0) EXPECT_EQ(joint_command_values_[1], 3.0); EXPECT_FALSE(std::isnan(joint_command_values_[1])); @@ -403,7 +413,7 @@ TEST_F(MecanumDriveControllerTest, test_update_logic_not_chainable) controller_interface::return_type::OK); EXPECT_NE(joint_command_values_[1], command_lin_x); -// w0_vel = 1.0 / params_.wheels_radius * (body_velocity_center_frame_.linear_x - body_velocity_center_frame_.linear_y - params_.wheels_k * body_velocity_center_frame_.angular_z); +// w0_vel = 1.0 / params_.kinematics.wheels_radius * (body_velocity_center_frame_.linear_x - body_velocity_center_frame_.linear_y - params_.kinematics.wheels_k * body_velocity_center_frame_.angular_z); // joint_command_values_[1] = 1.0 / 0.5 * (1.5 - 0.0 - 1 * 0.0) EXPECT_EQ(joint_command_values_[1], 3.0); ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); @@ -493,7 +503,7 @@ TEST_F(MecanumDriveControllerTest, test_update_logic) // EXPECT_EQ(joint_command_values_[NR_CMD_ITFS - 2], TEST_LINEAR_VELOCITY_X); EXPECT_FALSE(std::isnan(joint_command_values_[1])); EXPECT_NE(joint_command_values_[1], command_lin_x); -// w0_vel = 1.0 / params_.wheels_radius * (body_velocity_center_frame_.linear_x - body_velocity_center_frame_.linear_y - params_.wheels_k * body_velocity_center_frame_.angular_z); +// w0_vel = 1.0 / params_.kinematics.wheels_radius * (body_velocity_center_frame_.linear_x - body_velocity_center_frame_.linear_y - params_.kinematics.wheels_k * body_velocity_center_frame_.angular_z); // joint_command_values_[1] = 1.0 / 0.5 * (1.5 - 0.0 - 1 * 0.0) EXPECT_EQ(joint_command_values_[1], 3.0); @@ -535,6 +545,10 @@ TEST_F(MecanumDriveControllerTest, test_ref_timeout_zero_for_update) const auto age_of_last_command = controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; + fprintf(stderr, " age_of_last_command= %f \n", age_of_last_command); + fprintf(stderr, " controller_->ref_timeout_= %f \n", controller_->ref_timeout_); + + ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_); ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); ASSERT_EQ( controller_->update_reference_from_subscribers( @@ -548,11 +562,11 @@ TEST_F(MecanumDriveControllerTest, test_ref_timeout_zero_for_update) // EXPECT_EQ(joint_command_values_[NR_STATE_ITFS - 2], TEST_LINEAR_VELOCITY_X); EXPECT_FALSE(std::isnan(joint_command_values_[1])); EXPECT_NE(joint_command_values_[1], command_lin_x); -// w0_vel = 1.0 / params_.wheels_radius * (body_velocity_center_frame_.linear_x - body_velocity_center_frame_.linear_y - params_.wheels_k * body_velocity_center_frame_.angular_z); +// w0_vel = 1.0 / params_.kinematics.wheels_radius * (body_velocity_center_frame_.linear_x - body_velocity_center_frame_.linear_y - params_.kinematics.wheels_k * body_velocity_center_frame_.angular_z); // joint_command_values_[1] = 1.0 / 0.5 * (1.5 - 0.0 - 1 * 0.0) EXPECT_EQ(joint_command_values_[1], 3.0); - ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); - ASSERT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x)); + // ASSERT_TRUE((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); + ASSERT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x)); } TEST_F(MecanumDriveControllerTest, test_ref_timeout_zero_for_reference_callback) From 3a59aef1080f76d5b3a77a7b6a0f7d3b941f077e Mon Sep 17 00:00:00 2001 From: Robotgir <47585672+Robotgir@users.noreply.github.com> Date: Fri, 13 Jan 2023 19:41:09 +0530 Subject: [PATCH 27/91] Apply suggestions from code review Co-authored-by: Dr. Denis --- .../include/mecanum_drive_controller/odometry.hpp | 2 -- mecanum_drive_controller/package.xml | 4 +--- mecanum_drive_controller/src/mecanum_drive_controller.cpp | 1 - .../test/test_mecanum_drive_controller.cpp | 7 +++---- 4 files changed, 4 insertions(+), 10 deletions(-) diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp b/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp index 71b516eaf1..1ea1677fc6 100644 --- a/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp +++ b/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp @@ -1,5 +1,4 @@ // Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) -// (template) // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -20,7 +19,6 @@ #include "realtime_tools/realtime_publisher.h" #include "geometry_msgs/msg/twist.hpp" - #define PLANAR_POINT_DIM 3 namespace mecanum_drive_controller diff --git a/mecanum_drive_controller/package.xml b/mecanum_drive_controller/package.xml index 26a4f8ddd5..bde04b5610 100644 --- a/mecanum_drive_controller/package.xml +++ b/mecanum_drive_controller/package.xml @@ -5,13 +5,11 @@ 0.0.0 Implementation of mecanum drive controller for 4 wheel drive. - Denis Štogl + Dr.-Ing. Denis Štogl Bence Magyar Giridhar Bukka - Dr.-Ing. Denis Štogl - Bence Magyar Giridhar Bukka Apache License 2.0 diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index 76566daf03..0547e53127 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -108,7 +108,6 @@ controller_interface::CallbackReturn MecanumDriveController::on_configure( return controller_interface::CallbackReturn::ERROR; } - // TODO(anyone): Reserve memory in state publisher depending on the message type rt_odom_state_publisher_->lock(); rt_odom_state_publisher_->msg_.header.stamp = get_node()->now(); rt_odom_state_publisher_->msg_.header.frame_id = params_.odom_frame_id; diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp index c730000c86..4002e0e0c4 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp @@ -266,9 +266,9 @@ TEST_F(MecanumDriveControllerTest, test_sending_too_old_message) // try to set command with time before timeout - command is not updated auto reference = *(controller_->input_ref_.readFromNonRT()); auto old_timestamp = reference->header.stamp; - EXPECT_TRUE(std::isnan((reference)->twist.linear.x)); - EXPECT_TRUE(std::isnan((reference)->twist.linear.y)); - EXPECT_TRUE(std::isnan((reference)->twist.angular.z)); + EXPECT_TRUE(std::isnan(reference->twist.linear.x)); + EXPECT_TRUE(std::isnan(reference->twist.linear.y)); + EXPECT_TRUE(std::isnan(reference->twist.angular.z)); publish_commands( controller_->get_node()->now() - controller_->ref_timeout_ - @@ -374,7 +374,6 @@ TEST_F(MecanumDriveControllerTest, test_update_logic_not_chainable) controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - // EXPECT_TRUE(std::isnan(joint_command_values_[NR_CMD_ITFS - 2])); EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[0])); EXPECT_EQ(joint_command_values_[1], 0); From b6d1f51d93950f3fbfa7b226ab7689dd945e3b1e Mon Sep 17 00:00:00 2001 From: GiridharBukka Date: Fri, 13 Jan 2023 21:40:02 +0530 Subject: [PATCH 28/91] added description for each test as comment --- .../test/test_mecanum_drive_controller.cpp | 129 +++--------------- .../test/test_mecanum_drive_controller.hpp | 1 - 2 files changed, 22 insertions(+), 108 deletions(-) diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp index 4002e0e0c4..704baa2304 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp @@ -37,6 +37,7 @@ namespace const double EPS = 1e-3; } // namespace +// checking if all parameters are initialized and set as expected TEST_F(MecanumDriveControllerTest, all_parameters_set_configure_success) { SetUpController(); @@ -59,12 +60,14 @@ TEST_F(MecanumDriveControllerTest, all_parameters_set_configure_success) ASSERT_EQ(controller_->params_.kinematics.base_frame_offset.theta, 0.0); } +// checking if all interfaces, command, state and reference are exported as expected TEST_F(MecanumDriveControllerTest, check_exported_intefaces) { SetUpController(); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + // check command itfs configuration auto command_intefaces = controller_->command_interface_configuration(); ASSERT_EQ(command_intefaces.names.size(), joint_command_values_.size()); for (size_t i = 0; i < command_intefaces.names.size(); ++i) @@ -72,6 +75,7 @@ TEST_F(MecanumDriveControllerTest, check_exported_intefaces) EXPECT_EQ(command_intefaces.names[i], joint_names_[i] + "/" + interface_name_); } + // check state itfs configuration auto state_intefaces = controller_->state_interface_configuration(); ASSERT_EQ(state_intefaces.names.size(), joint_state_values_.size()); for (size_t i = 0; i < state_intefaces.names.size(); ++i) @@ -79,7 +83,7 @@ TEST_F(MecanumDriveControllerTest, check_exported_intefaces) EXPECT_EQ(state_intefaces.names[i], joint_names_[i] + "/" + interface_name_); } - // check ref itfs + // check ref itfs configuration auto reference_interfaces = controller_->export_reference_interfaces(); ASSERT_EQ(reference_interfaces.size(), NR_REF_ITFS); @@ -108,6 +112,7 @@ TEST_F(MecanumDriveControllerTest, check_exported_intefaces) std::string("angular_z") + "/" + hardware_interface::HW_IF_VELOCITY); } +// checking if calling activate() resets the controller reference msg TEST_F(MecanumDriveControllerTest, activate_success) { SetUpController(); @@ -128,6 +133,7 @@ TEST_F(MecanumDriveControllerTest, activate_success) } } +// checks if return type of update methods are a success TEST_F(MecanumDriveControllerTest, update_success) { SetUpController(); @@ -145,6 +151,7 @@ TEST_F(MecanumDriveControllerTest, update_success) controller_interface::return_type::OK); } +// checks if return type of controller lifecycle methods is a success TEST_F(MecanumDriveControllerTest, deactivate_success) { SetUpController(); @@ -154,6 +161,7 @@ TEST_F(MecanumDriveControllerTest, deactivate_success) ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); } +//checks the functionality of on_activate and on_deactivate methods from the controller TEST_F(MecanumDriveControllerTest, reactivate_success) { SetUpController(); @@ -176,6 +184,7 @@ TEST_F(MecanumDriveControllerTest, reactivate_success) controller_interface::return_type::OK); } +//test to check the status of controller state publisher TEST_F(MecanumDriveControllerTest, publish_status_success) { SetUpController(); @@ -183,14 +192,7 @@ TEST_F(MecanumDriveControllerTest, publish_status_success) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - ControllerStateMsg msg; - subscribe_and_get_messages(msg); - - // EXPECT_NEAR(msg.odom.pose.pose.position.x, msg.odom.pose.pose.position.Y, EPS); - EXPECT_NEAR(msg.odom.pose.pose.position.x, 0.0, EPS); - EXPECT_NEAR(msg.odom.pose.pose.position.y, 0.0, EPS); - - + controller_->reference_interfaces_[0] = 1.5; ASSERT_EQ( controller_->update_reference_from_subscribers( @@ -201,14 +203,16 @@ TEST_F(MecanumDriveControllerTest, publish_status_success) controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); + ControllerStateMsg msg; subscribe_and_get_messages(msg); - fprintf(stderr, " msg.odom.pose.pose.position.x= %f \n", msg.odom.pose.pose.position.x); - EXPECT_TRUE(msg.odom.pose.pose.position.x >= 0.0); EXPECT_NEAR(msg.odom.pose.pose.position.y, 0.0, EPS); + EXPECT_EQ(msg.linear_x_velocity_command, 1.5); + } +// Tests the msg subscriber and publisher TEST_F(MecanumDriveControllerTest, receive_message_and_publish_updated_status) { SetUpController(); @@ -247,13 +251,13 @@ TEST_F(MecanumDriveControllerTest, receive_message_and_publish_updated_status) // w0_vel = 1.0 / params_.kinematics.wheels_radius * (body_velocity_center_frame_.linear_x - body_velocity_center_frame_.linear_y - params_.kinematics.wheels_k * body_velocity_center_frame_.angular_z); // joint_command_values_[1] = 1.0 / 0.5 * (1.5 - 0.0 - 1 * 0.0) EXPECT_EQ(joint_command_values_[1], 3.0); - EXPECT_FALSE(std::isnan(joint_command_values_[1])); subscribe_and_get_messages(msg); ASSERT_EQ(msg.linear_x_velocity_command, 1.5); } +// Tests controller behavior when too old msg is sent / age_of_last_command > ref_timeout case TEST_F(MecanumDriveControllerTest, test_sending_too_old_message) { SetUpController(); @@ -263,7 +267,6 @@ TEST_F(MecanumDriveControllerTest, test_sending_too_old_message) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - // try to set command with time before timeout - command is not updated auto reference = *(controller_->input_ref_.readFromNonRT()); auto old_timestamp = reference->header.stamp; EXPECT_TRUE(std::isnan(reference->twist.linear.x)); @@ -280,6 +283,7 @@ TEST_F(MecanumDriveControllerTest, test_sending_too_old_message) EXPECT_TRUE(std::isnan((reference)->twist.angular.z)); } +//Tests the case when msg time stamp is zero TEST_F(MecanumDriveControllerTest, test_time_stamp_zero) { SetUpController(); @@ -307,6 +311,7 @@ TEST_F(MecanumDriveControllerTest, test_time_stamp_zero) EXPECT_NE((*(controller_->input_ref_.readFromNonRT()))->header.stamp.sec, 0.0); } +//Tests the condition for msg to be accepted, i.e, if age_of_last_command < ref_timeout TEST_F(MecanumDriveControllerTest, test_message_accepted) { SetUpController(); @@ -332,6 +337,7 @@ TEST_F(MecanumDriveControllerTest, test_message_accepted) EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z, 0.0); } +//Test that checks the status of chainable mode and update methods logic TEST_F(MecanumDriveControllerTest, test_update_logic_not_chainable) { // 1. age>ref_timeout 2. agecommand_interfaces_.size(); ++i) { - // EXPECT_TRUE(std::isnan(controller_->command_interfaces_[i].get_value())); EXPECT_EQ(controller_->command_interfaces_[i].get_value(), 0.0); } @@ -426,97 +431,7 @@ TEST_F(MecanumDriveControllerTest, test_update_logic_not_chainable) } } -TEST_F(MecanumDriveControllerTest, test_update_logic) -{ - SetUpController(); - rclcpp::executors::MultiThreadedExecutor executor; - executor.add_node(controller_->get_node()->get_node_base_interface()); - - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - - auto reference = controller_->input_ref_.readFromNonRT(); - - // set command statically - joint_command_values_[1] = command_lin_x; - - std::shared_ptr msg = std::make_shared(); - msg->header.stamp = controller_->get_node()->now() - controller_->ref_timeout_ - - rclcpp::Duration::from_seconds(0.1); - msg->twist.linear.x = TEST_LINEAR_VELOCITY_X; - msg->twist.linear.y = TEST_LINEAR_VELOCITY_y; - msg->twist.linear.z = std::numeric_limits::quiet_NaN(); - msg->twist.angular.x = std::numeric_limits::quiet_NaN(); - msg->twist.angular.y = std::numeric_limits::quiet_NaN(); - msg->twist.angular.z = TEST_ANGULAR_VELOCITY_Z; - controller_->input_ref_.writeFromNonRT(msg); - const auto age_of_last_command = - controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; - - ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_); - ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); - ASSERT_EQ( - controller_->update_reference_from_subscribers( - controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - ASSERT_EQ( - controller_->update_and_write_commands( - controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - - EXPECT_EQ(joint_command_values_[1], 0); - 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_TRUE(std::isnan(controller_->command_interfaces_[i].get_value())); - EXPECT_EQ(controller_->command_interfaces_[i].get_value(), 0.0); - } - - std::shared_ptr msg_2 = std::make_shared(); - msg_2->header.stamp = controller_->get_node()->now(); - msg_2->twist.linear.x = TEST_LINEAR_VELOCITY_X; - msg_2->twist.linear.y = TEST_LINEAR_VELOCITY_y; - msg_2->twist.linear.z = std::numeric_limits::quiet_NaN(); - msg_2->twist.angular.x = std::numeric_limits::quiet_NaN(); - msg_2->twist.angular.y = std::numeric_limits::quiet_NaN(); - msg_2->twist.angular.z = TEST_ANGULAR_VELOCITY_Z; - controller_->input_ref_.writeFromNonRT(msg_2); - const auto age_of_last_command_2 = - controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; - - ASSERT_TRUE(age_of_last_command_2 <= controller_->ref_timeout_); - ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); - ASSERT_EQ( - controller_->update_reference_from_subscribers( - controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - ASSERT_EQ( - controller_->update_and_write_commands( - controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - - // EXPECT_EQ(joint_command_values_[NR_CMD_ITFS - 2], TEST_LINEAR_VELOCITY_X); - EXPECT_FALSE(std::isnan(joint_command_values_[1])); - EXPECT_NE(joint_command_values_[1], command_lin_x); -// w0_vel = 1.0 / params_.kinematics.wheels_radius * (body_velocity_center_frame_.linear_x - body_velocity_center_frame_.linear_y - params_.kinematics.wheels_k * body_velocity_center_frame_.angular_z); -// joint_command_values_[1] = 1.0 / 0.5 * (1.5 - 0.0 - 1 * 0.0) - EXPECT_EQ(joint_command_values_[1], 3.0); - - ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); - for (const auto & interface : controller_->reference_interfaces_) - { - EXPECT_FALSE(std::isnan(interface)); - } - for (size_t i = 0; i < controller_->command_interfaces_.size(); ++i) - { - EXPECT_EQ(controller_->command_interfaces_[i].get_value(), 3.0); - } -} - +// Tests behavior of update methods if ref_timeout = 0 TEST_F(MecanumDriveControllerTest, test_ref_timeout_zero_for_update) { SetUpController(); @@ -558,16 +473,15 @@ TEST_F(MecanumDriveControllerTest, test_ref_timeout_zero_for_update) controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - // EXPECT_EQ(joint_command_values_[NR_STATE_ITFS - 2], TEST_LINEAR_VELOCITY_X); EXPECT_FALSE(std::isnan(joint_command_values_[1])); EXPECT_NE(joint_command_values_[1], command_lin_x); // w0_vel = 1.0 / params_.kinematics.wheels_radius * (body_velocity_center_frame_.linear_x - body_velocity_center_frame_.linear_y - params_.kinematics.wheels_k * body_velocity_center_frame_.angular_z); // joint_command_values_[1] = 1.0 / 0.5 * (1.5 - 0.0 - 1 * 0.0) EXPECT_EQ(joint_command_values_[1], 3.0); - // ASSERT_TRUE((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); ASSERT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x)); } +// Tests behavior of reference_callback() if ref_timeout = 0 TEST_F(MecanumDriveControllerTest, test_ref_timeout_zero_for_reference_callback) { SetUpController(); @@ -581,6 +495,7 @@ TEST_F(MecanumDriveControllerTest, test_ref_timeout_zero_for_reference_callback) EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z)); controller_->ref_timeout_ = rclcpp::Duration::from_seconds(0.0); + //reference_callback() is called implicitly when publish_commands() is called. publish_commands(controller_->get_node()->now()); ASSERT_TRUE(controller_->wait_for_commands(executor)); diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp index 7cfddc1306..dc1dd3e042 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp @@ -61,7 +61,6 @@ class TestableMecanumDriveController : public mecanum_drive_controller::MecanumD FRIEND_TEST(MecanumDriveControllerTest, test_time_stamp_zero); FRIEND_TEST(MecanumDriveControllerTest, test_message_accepted); FRIEND_TEST(MecanumDriveControllerTest, test_update_logic_not_chainable); - FRIEND_TEST(MecanumDriveControllerTest, test_update_logic); FRIEND_TEST(MecanumDriveControllerTest, test_ref_timeout_zero_for_update); FRIEND_TEST(MecanumDriveControllerTest, test_ref_timeout_zero_for_reference_callback); From 8624f72bec5aa9652e70744d0eabb3fddaa3c2ca Mon Sep 17 00:00:00 2001 From: GiridharBukka Date: Sat, 14 Jan 2023 12:09:00 +0530 Subject: [PATCH 29/91] added preceding controller tests --- mecanum_drive_controller/CMakeLists.txt | 20 +++++------ .../mecanum_drive_controller.hpp | 2 ++ .../src/mecanum_drive_controller.cpp | 30 ++++++++++++----- .../src/mecanum_drive_controller.yaml | 19 +++++++++-- .../test/mecanum_drive_controller_params.yaml | 6 ++-- ...um_drive_controller_preceeding_params.yaml | 33 +++++++++++++++---- ...st_mecanum_drive_controller_preceeding.cpp | 31 +++++++---------- 7 files changed, 92 insertions(+), 49 deletions(-) diff --git a/mecanum_drive_controller/CMakeLists.txt b/mecanum_drive_controller/CMakeLists.txt index 4b60354ee2..8ef6aefe5c 100644 --- a/mecanum_drive_controller/CMakeLists.txt +++ b/mecanum_drive_controller/CMakeLists.txt @@ -88,16 +88,16 @@ if(BUILD_TESTING) hardware_interface ) - # add_rostest_with_parameters_gmock( - # test_mecanum_drive_controller_preceeding test/test_mecanum_drive_controller_preceeding.cpp - # ${CMAKE_CURRENT_SOURCE_DIR}/test/mecanum_drive_controller_preceeding_params.yaml) - # target_include_directories(test_mecanum_drive_controller_preceeding PRIVATE include) - # target_link_libraries(test_mecanum_drive_controller_preceeding mecanum_drive_controller) - # ament_target_dependencies( - # test_mecanum_drive_controller_preceeding - # controller_interface - # hardware_interface - # ) + add_rostest_with_parameters_gmock( + test_mecanum_drive_controller_preceeding test/test_mecanum_drive_controller_preceeding.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/mecanum_drive_controller_preceeding_params.yaml) + target_include_directories(test_mecanum_drive_controller_preceeding PRIVATE include) + target_link_libraries(test_mecanum_drive_controller_preceeding mecanum_drive_controller) + ament_target_dependencies( + test_mecanum_drive_controller_preceeding + controller_interface + hardware_interface + ) endif() ament_export_include_directories( diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp index 8d24eb383e..2fe70df290 100644 --- a/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp +++ b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp @@ -92,6 +92,8 @@ class MecanumDriveController : public controller_interface::ChainableControllerI std::shared_ptr param_listener_; mecanum_drive_controller::Params params_; + std::vector state_joint_names_; + // Command subscribers and Controller State publisher rclcpp::Subscription::SharedPtr ref_subscriber_ = nullptr; realtime_tools::RealtimeBuffer> input_ref_; diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index 0547e53127..14946ff1ec 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -75,6 +75,20 @@ controller_interface::CallbackReturn MecanumDriveController::on_configure( { params_ = param_listener_->get_params(); + if (!params_.state_joint_names.empty()) { + state_joint_names_ = params_.state_joint_names; + } else { + state_joint_names_ = params_.joint_names; + } + + if (params_.joint_names.size() != state_joint_names_.size()) { + RCLCPP_FATAL(get_node()->get_logger(), + "Size of 'joints' (%d) and 'state_joint_names' (%d) parameters has " + "to be the same!", + params_.joint_names.size(), state_joint_names_.size()); + return CallbackReturn::FAILURE; + } + // Set wheel params for the odometry computation odometry_.setWheelsParams(params_.kinematics.wheels_k, params_.kinematics.wheels_radius); @@ -222,13 +236,13 @@ MecanumDriveController::command_interface_configuration() const command_interfaces_config.names.reserve(NR_CMD_ITFS); command_interfaces_config.names.push_back( - params_.wheel_names[0] + "/" + hardware_interface::HW_IF_VELOCITY); + params_.joint_names[0] + "/" + hardware_interface::HW_IF_VELOCITY); command_interfaces_config.names.push_back( - params_.wheel_names[1] + "/" + hardware_interface::HW_IF_VELOCITY); + params_.joint_names[1] + "/" + hardware_interface::HW_IF_VELOCITY); command_interfaces_config.names.push_back( - params_.wheel_names[2] + "/" + hardware_interface::HW_IF_VELOCITY); + params_.joint_names[2] + "/" + hardware_interface::HW_IF_VELOCITY); command_interfaces_config.names.push_back( - params_.wheel_names[3] + "/" + hardware_interface::HW_IF_VELOCITY); + params_.joint_names[3] + "/" + hardware_interface::HW_IF_VELOCITY); return command_interfaces_config; } @@ -241,13 +255,13 @@ controller_interface::InterfaceConfiguration MecanumDriveController::state_inter state_interfaces_config.names.reserve(NR_STATE_ITFS); state_interfaces_config.names.push_back( - params_.wheel_names[0] + "/" + hardware_interface::HW_IF_VELOCITY); + params_.state_joint_names[0] + "/" + hardware_interface::HW_IF_VELOCITY); state_interfaces_config.names.push_back( - params_.wheel_names[1] + "/" + hardware_interface::HW_IF_VELOCITY); + params_.state_joint_names[1] + "/" + hardware_interface::HW_IF_VELOCITY); state_interfaces_config.names.push_back( - params_.wheel_names[2] + "/" + hardware_interface::HW_IF_VELOCITY); + params_.state_joint_names[2] + "/" + hardware_interface::HW_IF_VELOCITY); state_interfaces_config.names.push_back( - params_.wheel_names[3] + "/" + hardware_interface::HW_IF_VELOCITY); + params_.state_joint_names[3] + "/" + hardware_interface::HW_IF_VELOCITY); return state_interfaces_config; } diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.yaml b/mecanum_drive_controller/src/mecanum_drive_controller.yaml index c7502b63e3..0b43c1ac92 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.yaml +++ b/mecanum_drive_controller/src/mecanum_drive_controller.yaml @@ -6,6 +6,21 @@ mecanum_drive_controller: } + state_joint_names: { + type: string_array, + default_value: [], + description: "(optional) Specifies joints for reading states. This parameter is only relevant when state joints are different then command joint, i.e., when a following controller is used.", + read_only: true, + + } + + interface_name: { + type: string, + default_value: "", + description: "Name of the interface used by the controller on joints and command_joints.", + read_only: true, + } + kinematics: base_frame_offset: x: { @@ -43,9 +58,9 @@ mecanum_drive_controller: } - wheel_names: { + joint_names: { type: string_array, - default_value: ["", "", "", ""], + default_value: [], description: "Name of the wheels.", read_only: false, diff --git a/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml b/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml index 42e7b96bd2..7670ef438f 100644 --- a/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml +++ b/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml @@ -3,6 +3,8 @@ test_mecanum_drive_controller: reference_timeout: 0.1 + interface_name: velocity + kinematics: base_frame_offset: x: 0.0 @@ -13,7 +15,7 @@ test_mecanum_drive_controller: wheels_k: 1.0 - wheel_names: ["front_left_wheel_joint", "back_left_wheel_joint", "back_right_wheel_joint", "front_right_wheel_joint"] + joint_names: ["front_left_wheel_joint", "back_left_wheel_joint", "back_right_wheel_joint", "front_right_wheel_joint"] base_frame_id: "base_link" @@ -21,8 +23,6 @@ test_mecanum_drive_controller: enable_odom_tf: true - base_frame_offset: [0.0, 0.0, 0.0] - use_realigned_roller_joints: false twist_covariance_diagonal: [0.0, 7.0, 14.0, 21.0, 28.0, 35.0] diff --git a/mecanum_drive_controller/test/mecanum_drive_controller_preceeding_params.yaml b/mecanum_drive_controller/test/mecanum_drive_controller_preceeding_params.yaml index 6f57fa39bb..94e86d0d91 100644 --- a/mecanum_drive_controller/test/mecanum_drive_controller_preceeding_params.yaml +++ b/mecanum_drive_controller/test/mecanum_drive_controller_preceeding_params.yaml @@ -1,11 +1,32 @@ test_mecanum_drive_controller: ros__parameters: - joints: - - joint1 - interface_name: acceleration + reference_timeout: 0.1 - state_joints: - - joint1state + state_joint_names: ["state_front_left_wheel_joint", "state_back_left_wheel_joint", "state_back_right_wheel_joint", "state_front_right_wheel_joint"] - reference_timeout: 0.1 + interface_name: velocity + + kinematics: + base_frame_offset: + x: 0.0 + y: 0.0 + theta: 0.0 + + wheels_radius: 0.5 + + wheels_k: 1.0 + + joint_names: ["front_left_wheel_joint", "back_left_wheel_joint", "back_right_wheel_joint", "front_right_wheel_joint"] + + base_frame_id: "base_link" + + odom_frame_id: "odom" + + enable_odom_tf: true + + use_realigned_roller_joints: false + + twist_covariance_diagonal: [0.0, 7.0, 14.0, 21.0, 28.0, 35.0] + + pose_covariance_diagonal: [0.0, 7.0, 14.0, 21.0, 28.0, 35.0] diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller_preceeding.cpp b/mecanum_drive_controller/test/test_mecanum_drive_controller_preceeding.cpp index 81ac0cc6e4..67ec4b9434 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller_preceeding.cpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller_preceeding.cpp @@ -20,9 +20,9 @@ #include #include -using mecanum_drive_controller::CMD_MY_ITFS; -using mecanum_drive_controller::control_mode_type; -using mecanum_drive_controller::STATE_MY_ITFS; +using mecanum_drive_controller::NR_CMD_ITFS; +using mecanum_drive_controller::NR_REF_ITFS; +using mecanum_drive_controller::NR_STATE_ITFS; class MecanumDriveControllerTest : public MecanumDriveControllerFixture @@ -33,16 +33,19 @@ TEST_F(MecanumDriveControllerTest, all_parameters_set_configure_success) { SetUpController(); - ASSERT_TRUE(controller_->params_.joints.empty()); - ASSERT_TRUE(controller_->params_.state_joints.empty()); + ASSERT_EQ(controller_->params_.reference_timeout, 0.0); + ASSERT_TRUE(controller_->params_.joint_names.empty()); + ASSERT_TRUE(controller_->params_.state_joint_names.empty()); ASSERT_TRUE(controller_->params_.interface_name.empty()); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_THAT(controller_->params_.joints, testing::ElementsAreArray(joint_names_)); - ASSERT_THAT(controller_->params_.state_joints, testing::ElementsAreArray(state_joint_names_)); - ASSERT_THAT(controller_->state_joints_, testing::ElementsAreArray(state_joint_names_)); + ASSERT_EQ(controller_->params_.reference_timeout, 0.1); + ASSERT_THAT(controller_->params_.joint_names, testing::ElementsAreArray(joint_names_)); + ASSERT_THAT(controller_->params_.state_joint_names, testing::ElementsAreArray(state_joint_names_)); + ASSERT_THAT(controller_->state_joint_names_, testing::ElementsAreArray(state_joint_names_)); ASSERT_EQ(controller_->params_.interface_name, interface_name_); + } TEST_F(MecanumDriveControllerTest, check_exported_intefaces) @@ -65,18 +68,6 @@ TEST_F(MecanumDriveControllerTest, check_exported_intefaces) EXPECT_EQ(state_intefaces.names[i], state_joint_names_[i] + "/" + interface_name_); } - // check ref itfs - auto reference_interfaces = controller_->export_reference_interfaces(); - ASSERT_EQ(reference_interfaces.size(), joint_names_.size()); - for (size_t i = 0; i < joint_names_.size(); ++i) - { - const std::string ref_itf_name = std::string(controller_->get_node()->get_name()) + "/" + - state_joint_names_[i] + "/" + interface_name_; - EXPECT_EQ(reference_interfaces[i].get_name(), ref_itf_name); - EXPECT_EQ(reference_interfaces[i].get_prefix_name(), controller_->get_node()->get_name()); - EXPECT_EQ( - reference_interfaces[i].get_interface_name(), state_joint_names_[i] + "/" + interface_name_); - } } int main(int argc, char ** argv) From 38a35002223513412f40d6014435d6ce90eb9a76 Mon Sep 17 00:00:00 2001 From: GiridharBukka Date: Sat, 14 Jan 2023 12:10:01 +0530 Subject: [PATCH 30/91] reorganized reference_interfaces configuration method --- .../test/test_mecanum_drive_controller.cpp | 35 ++++++------------- .../test/test_mecanum_drive_controller.hpp | 4 ++- 2 files changed, 13 insertions(+), 26 deletions(-) diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp index 704baa2304..b13524538b 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp @@ -83,33 +83,18 @@ TEST_F(MecanumDriveControllerTest, check_exported_intefaces) EXPECT_EQ(state_intefaces.names[i], joint_names_[i] + "/" + interface_name_); } - // check ref itfs configuration + // check ref itfs configuration reference_names_ + auto reference_interfaces = controller_->export_reference_interfaces(); ASSERT_EQ(reference_interfaces.size(), NR_REF_ITFS); - - const std::string ref_itf_name_0 = std::string(controller_->get_node()->get_name()) + "/" + - "linear_x" + "/" + hardware_interface::HW_IF_VELOCITY; - EXPECT_EQ(reference_interfaces[0].get_name(), ref_itf_name_0); - EXPECT_EQ(reference_interfaces[0].get_prefix_name(), controller_->get_node()->get_name()); - EXPECT_EQ( - reference_interfaces[0].get_interface_name(), - std::string("linear_x") + "/" + hardware_interface::HW_IF_VELOCITY); - - const std::string ref_itf_name_1 = std::string(controller_->get_node()->get_name()) + "/" + - "linear_y" + "/" + hardware_interface::HW_IF_VELOCITY; - EXPECT_EQ(reference_interfaces[1].get_name(), ref_itf_name_1); - EXPECT_EQ(reference_interfaces[1].get_prefix_name(), controller_->get_node()->get_name()); - EXPECT_EQ( - reference_interfaces[1].get_interface_name(), - std::string("linear_y") + "/" + hardware_interface::HW_IF_VELOCITY); - - const std::string ref_itf_name_2 = std::string(controller_->get_node()->get_name()) + "/" + - "angular_z" + "/" + hardware_interface::HW_IF_VELOCITY; - EXPECT_EQ(reference_interfaces[2].get_name(), ref_itf_name_2); - EXPECT_EQ(reference_interfaces[2].get_prefix_name(), controller_->get_node()->get_name()); - EXPECT_EQ( - reference_interfaces[2].get_interface_name(), - std::string("angular_z") + "/" + hardware_interface::HW_IF_VELOCITY); + for (size_t i = 0; i < reference_names_.size(); ++i) { + const std::string ref_itf_name = std::string(controller_->get_node()->get_name()) + "/" + + reference_names_[i] + "/" + interface_name_; + EXPECT_EQ(reference_interfaces[i].get_name(), ref_itf_name); + EXPECT_EQ(reference_interfaces[i].get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ( + reference_interfaces[i].get_interface_name(), reference_names_[i] + "/" + interface_name_); + } } // checking if calling activate() resets the controller reference msg diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp index dc1dd3e042..200750f795 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp @@ -61,7 +61,7 @@ class TestableMecanumDriveController : public mecanum_drive_controller::MecanumD FRIEND_TEST(MecanumDriveControllerTest, test_time_stamp_zero); FRIEND_TEST(MecanumDriveControllerTest, test_message_accepted); FRIEND_TEST(MecanumDriveControllerTest, test_update_logic_not_chainable); - FRIEND_TEST(MecanumDriveControllerTest, test_ref_timeout_zero_for_update); + FRIEND_TEST(MecanumDriveControllerTest, test_ref_timeout_zero_for_update); FRIEND_TEST(MecanumDriveControllerTest, test_ref_timeout_zero_for_reference_callback); public: @@ -249,9 +249,11 @@ class MecanumDriveControllerFixture : public ::testing::Test } protected: + std::vector reference_names_ = {"linear_x", "linear_y", "angular_z"}; std::vector joint_names_ = { "front_left_wheel_joint", "back_left_wheel_joint", "back_right_wheel_joint", "front_right_wheel_joint"}; + std::vector state_joint_names_ = { "state_front_left_wheel_joint", "state_back_left_wheel_joint", "state_back_right_wheel_joint", "state_front_right_wheel_joint"}; From 08f2f2de4c67e3fc17d84fb7b483cb5dd1b4e479 Mon Sep 17 00:00:00 2001 From: GiridharBukka Date: Sat, 14 Jan 2023 12:47:24 +0530 Subject: [PATCH 31/91] configuration of interfaces in loop --- .../mecanum_drive_controller.hpp | 2 + .../src/mecanum_drive_controller.cpp | 48 ++++++++----------- .../src/mecanum_drive_controller.yaml | 7 +++ .../test/mecanum_drive_controller_params.yaml | 2 + 4 files changed, 30 insertions(+), 29 deletions(-) diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp index 2fe70df290..f3cd49880e 100644 --- a/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp +++ b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp @@ -93,6 +93,8 @@ class MecanumDriveController : public controller_interface::ChainableControllerI mecanum_drive_controller::Params params_; std::vector state_joint_names_; + std::vector reference_names_; + // Command subscribers and Controller State publisher rclcpp::Subscription::SharedPtr ref_subscriber_ = nullptr; diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index 14946ff1ec..321b40da75 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -89,6 +89,8 @@ controller_interface::CallbackReturn MecanumDriveController::on_configure( return CallbackReturn::FAILURE; } + reference_names_ = params_.reference_names; + // Set wheel params for the odometry computation odometry_.setWheelsParams(params_.kinematics.wheels_k, params_.kinematics.wheels_radius); @@ -234,15 +236,11 @@ MecanumDriveController::command_interface_configuration() const controller_interface::InterfaceConfiguration command_interfaces_config; command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; - command_interfaces_config.names.reserve(NR_CMD_ITFS); - command_interfaces_config.names.push_back( - params_.joint_names[0] + "/" + hardware_interface::HW_IF_VELOCITY); - command_interfaces_config.names.push_back( - params_.joint_names[1] + "/" + hardware_interface::HW_IF_VELOCITY); - command_interfaces_config.names.push_back( - params_.joint_names[2] + "/" + hardware_interface::HW_IF_VELOCITY); - command_interfaces_config.names.push_back( - params_.joint_names[3] + "/" + hardware_interface::HW_IF_VELOCITY); + command_interfaces_config.names.reserve(params_.joint_names.size()); + for (const auto& joint : params_.joint_names) { + command_interfaces_config.names.push_back(joint + "/" + + params_.interface_name); + } return command_interfaces_config; } @@ -253,15 +251,12 @@ controller_interface::InterfaceConfiguration MecanumDriveController::state_inter controller_interface::InterfaceConfiguration state_interfaces_config; state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; - state_interfaces_config.names.reserve(NR_STATE_ITFS); - state_interfaces_config.names.push_back( - params_.state_joint_names[0] + "/" + hardware_interface::HW_IF_VELOCITY); - state_interfaces_config.names.push_back( - params_.state_joint_names[1] + "/" + hardware_interface::HW_IF_VELOCITY); - state_interfaces_config.names.push_back( - params_.state_joint_names[2] + "/" + hardware_interface::HW_IF_VELOCITY); - state_interfaces_config.names.push_back( - params_.state_joint_names[3] + "/" + hardware_interface::HW_IF_VELOCITY); + state_interfaces_config.names.reserve(state_joint_names_.size()); + + for (const auto& joint : state_joint_names_) { + state_interfaces_config.names.push_back(joint + "/" + + params_.interface_name); + } return state_interfaces_config; } @@ -272,19 +267,14 @@ MecanumDriveController::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_x/") + hardware_interface::HW_IF_VELOCITY, - &reference_interfaces_[0])); - reference_interfaces.push_back(hardware_interface::CommandInterface( - get_node()->get_name(), std::string("linear_y/") + hardware_interface::HW_IF_VELOCITY, - &reference_interfaces_[1])); + reference_interfaces.reserve(reference_interfaces_.size()); - reference_interfaces.push_back(hardware_interface::CommandInterface( - get_node()->get_name(), std::string("angular_z/") + hardware_interface::HW_IF_VELOCITY, - &reference_interfaces_[2])); + for (size_t i = 0; i < reference_interfaces_.size(); ++i) { + reference_interfaces.push_back(hardware_interface::CommandInterface( + get_node()->get_name(), reference_names_[i] + "/" + params_.interface_name, + &reference_interfaces_[i])); + } return reference_interfaces; } diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.yaml b/mecanum_drive_controller/src/mecanum_drive_controller.yaml index 0b43c1ac92..2239be6de0 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.yaml +++ b/mecanum_drive_controller/src/mecanum_drive_controller.yaml @@ -14,6 +14,13 @@ mecanum_drive_controller: } + reference_names: { + type: string_array, + default_value: [], + description: " Names of the references, ex: high level vel commands from MoveIt, Nav2, etc.", + read_only: true, + } + interface_name: { type: string, default_value: "", diff --git a/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml b/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml index 7670ef438f..b5d556bbaf 100644 --- a/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml +++ b/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml @@ -3,6 +3,8 @@ test_mecanum_drive_controller: reference_timeout: 0.1 + reference_names: ["linear_x", "linear_y", "angular_z"] + interface_name: velocity kinematics: From 2e52fd3fd4cbb24495df2c213635021da0921c9a Mon Sep 17 00:00:00 2001 From: GiridharBukka Date: Sat, 14 Jan 2023 13:40:38 +0530 Subject: [PATCH 32/91] final fixups --- .../mecanum_drive_controller.hpp | 5 ++++- .../mecanum_drive_controller/odometry.hpp | 4 ++-- .../src/mecanum_drive_controller.yaml | 18 ++++++++++-------- mecanum_drive_controller/src/odometry.cpp | 7 +++---- .../test/mecanum_drive_controller_params.yaml | 4 ++-- ...num_drive_controller_preceeding_params.yaml | 4 ++-- .../test/test_mecanum_drive_controller.cpp | 12 +++++++++++- .../test/test_mecanum_drive_controller.hpp | 4 ++-- 8 files changed, 36 insertions(+), 22 deletions(-) diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp index f3cd49880e..211d77c3a9 100644 --- a/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp +++ b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp @@ -92,11 +92,14 @@ class MecanumDriveController : public controller_interface::ChainableControllerI std::shared_ptr param_listener_; mecanum_drive_controller::Params params_; + // used for chained controller std::vector state_joint_names_; + + //Names of the references, ex: high level vel commands from MoveIt, Nav2, etc. used for preceding controller std::vector reference_names_; - // Command subscribers and Controller State publisher + // Command subscribers and Controller State, odom state, tf state publishers rclcpp::Subscription::SharedPtr ref_subscriber_ = nullptr; realtime_tools::RealtimeBuffer> input_ref_; rclcpp::Duration ref_timeout_ = rclcpp::Duration::from_seconds(0.0); diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp b/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp index 1ea1677fc6..d05f58381b 100644 --- a/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp +++ b/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp @@ -75,7 +75,7 @@ class Odometry /// Current timestamp: rclcpp::Time timestamp_; - /// Reference frame (wrt to center frame). + /// Reference frame (wrt to center frame). [x, y, theta] std::array base_frame_offset_; /// Current pose: @@ -91,7 +91,7 @@ class Odometry /// lx and ly represent the distance from the robot's center to the wheels projected on /// the x and y axis with origin at robots center respectively, wheels_k_ = lx+ly double wheels_k_; - double wheels_radius_; + double wheels_radius_; // [m] }; } // namespace mecanum_drive_controller diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.yaml b/mecanum_drive_controller/src/mecanum_drive_controller.yaml index 2239be6de0..3ae082db8e 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.yaml +++ b/mecanum_drive_controller/src/mecanum_drive_controller.yaml @@ -5,6 +5,14 @@ mecanum_drive_controller: 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.", } + + joint_names: { + type: string_array, + default_value: [], + description: "Name of the wheels joints.", + read_only: false, + + } state_joint_names: { type: string_array, @@ -60,18 +68,12 @@ mecanum_drive_controller: wheels_k: { type: double, default_value: 0.0, - description: "Wheels geometric param used in mecanum wheels' IK.", + description: "Wheels geometric param used in mecanum wheels' IK. lx and ly represent the distance from the robot's center to the wheels projected on + the x and y axis with origin at robots center respectively, wheels_k = lx+ly", read_only: false, } - joint_names: { - type: string_array, - default_value: [], - description: "Name of the wheels.", - read_only: false, - - } base_frame_id: { type: string, default_value: "base_link", diff --git a/mecanum_drive_controller/src/odometry.cpp b/mecanum_drive_controller/src/odometry.cpp index aad2b4c170..6c0affb561 100644 --- a/mecanum_drive_controller/src/odometry.cpp +++ b/mecanum_drive_controller/src/odometry.cpp @@ -53,8 +53,6 @@ bool Odometry::update( // const double dt = (time - timestamp_).toSec(); if (dt < 0.0001) return false; // Interval too small to integrate with - // timestamp_ = time; - /// Compute FK (i.e. compute mobile robot's body twist out of its wheels velocities): /// NOTE: we use the IK of the mecanum wheels which we invert using a pseudo-inverse. /// NOTE: the mecanum IK gives the body speed at the center frame, we then offset this velocity @@ -98,8 +96,9 @@ bool Odometry::update( position_x_in_base_frame_ += velocity_in_base_frame_w_r_t_odom_frame_.x() * dt; position_y_in_base_frame_ += velocity_in_base_frame_w_r_t_odom_frame_.y() * dt; - fprintf(stderr, " position_x_in_base_frame_ = %f \n", position_x_in_base_frame_); - fprintf(stderr, " position_y_in_base_frame_ = %f \n", position_y_in_base_frame_); + // leaving the below comments intentionally + // fprintf(stderr, " position_x_in_base_frame_ = %f \n", position_x_in_base_frame_); + // fprintf(stderr, " position_y_in_base_frame_ = %f \n", position_y_in_base_frame_); return true; } diff --git a/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml b/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml index b5d556bbaf..cc8ad75f56 100644 --- a/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml +++ b/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml @@ -3,6 +3,8 @@ test_mecanum_drive_controller: reference_timeout: 0.1 + joint_names: ["front_left_wheel_joint", "back_left_wheel_joint", "back_right_wheel_joint", "front_right_wheel_joint"] + reference_names: ["linear_x", "linear_y", "angular_z"] interface_name: velocity @@ -17,8 +19,6 @@ test_mecanum_drive_controller: wheels_k: 1.0 - joint_names: ["front_left_wheel_joint", "back_left_wheel_joint", "back_right_wheel_joint", "front_right_wheel_joint"] - base_frame_id: "base_link" odom_frame_id: "odom" diff --git a/mecanum_drive_controller/test/mecanum_drive_controller_preceeding_params.yaml b/mecanum_drive_controller/test/mecanum_drive_controller_preceeding_params.yaml index 94e86d0d91..7517c29441 100644 --- a/mecanum_drive_controller/test/mecanum_drive_controller_preceeding_params.yaml +++ b/mecanum_drive_controller/test/mecanum_drive_controller_preceeding_params.yaml @@ -3,6 +3,8 @@ test_mecanum_drive_controller: reference_timeout: 0.1 + joint_names: ["front_left_wheel_joint", "back_left_wheel_joint", "back_right_wheel_joint", "front_right_wheel_joint"] + state_joint_names: ["state_front_left_wheel_joint", "state_back_left_wheel_joint", "state_back_right_wheel_joint", "state_front_right_wheel_joint"] interface_name: velocity @@ -17,8 +19,6 @@ test_mecanum_drive_controller: wheels_k: 1.0 - joint_names: ["front_left_wheel_joint", "back_left_wheel_joint", "back_right_wheel_joint", "front_right_wheel_joint"] - base_frame_id: "base_link" odom_frame_id: "odom" diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp index b13524538b..edce311a64 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp @@ -50,7 +50,12 @@ TEST_F(MecanumDriveControllerTest, all_parameters_set_configure_success) ASSERT_EQ(controller_->params_.kinematics.base_frame_offset.y, 0.0); ASSERT_EQ(controller_->params_.kinematics.base_frame_offset.theta, 0.0); + ASSERT_TRUE(controller_->params_.joint_names.empty()); + ASSERT_TRUE(controller_->params_.state_joint_names.empty()); + ASSERT_TRUE(controller_->params_.interface_name.empty()); + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->params_.reference_timeout, 0.1); ASSERT_EQ(controller_->params_.kinematics.wheels_radius, 0.5); ASSERT_EQ(controller_->params_.kinematics.wheels_k, 1.0); @@ -58,6 +63,11 @@ TEST_F(MecanumDriveControllerTest, all_parameters_set_configure_success) ASSERT_EQ(controller_->params_.kinematics.base_frame_offset.x, 0.0); ASSERT_EQ(controller_->params_.kinematics.base_frame_offset.y, 0.0); ASSERT_EQ(controller_->params_.kinematics.base_frame_offset.theta, 0.0); + + ASSERT_THAT(controller_->params_.joint_names, testing::ElementsAreArray(joint_names_)); + ASSERT_TRUE(controller_->params_.state_joint_names.empty()); + ASSERT_EQ(controller_->params_.interface_name, interface_name_); + } // checking if all interfaces, command, state and reference are exported as expected @@ -83,7 +93,7 @@ TEST_F(MecanumDriveControllerTest, check_exported_intefaces) EXPECT_EQ(state_intefaces.names[i], joint_names_[i] + "/" + interface_name_); } - // check ref itfs configuration reference_names_ + // check ref itfs configuration, reference_names_ auto reference_interfaces = controller_->export_reference_interfaces(); ASSERT_EQ(reference_interfaces.size(), NR_REF_ITFS); diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp index 200750f795..cfd99e1570 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp @@ -61,7 +61,7 @@ class TestableMecanumDriveController : public mecanum_drive_controller::MecanumD FRIEND_TEST(MecanumDriveControllerTest, test_time_stamp_zero); FRIEND_TEST(MecanumDriveControllerTest, test_message_accepted); FRIEND_TEST(MecanumDriveControllerTest, test_update_logic_not_chainable); - FRIEND_TEST(MecanumDriveControllerTest, test_ref_timeout_zero_for_update); + FRIEND_TEST(MecanumDriveControllerTest, test_ref_timeout_zero_for_update); FRIEND_TEST(MecanumDriveControllerTest, test_ref_timeout_zero_for_reference_callback); public: @@ -272,7 +272,7 @@ class MecanumDriveControllerFixture : public ::testing::Test std::vector state_itfs_; std::vector command_itfs_; - double ref_timeout_ = 0.2; + double ref_timeout_ = 0.1; // Test related parameters std::unique_ptr controller_; From 3d437bff3ce16e0bfc32d6b944a2dc144f94d705 Mon Sep 17 00:00:00 2001 From: GiridharBukka Date: Mon, 16 Jan 2023 10:59:12 +0530 Subject: [PATCH 33/91] using control_msgs::msg::MecanumDriveControllerState and removing custom_msgs folder --- mecanum_drive_controller/CMakeLists.txt | 1 - .../mecanum_drive_controller/mecanum_drive_controller.hpp | 4 ++-- mecanum_drive_controller/src/mecanum_drive_controller.cpp | 6 +++--- .../test/test_mecanum_drive_controller.cpp | 6 +++--- 4 files changed, 8 insertions(+), 9 deletions(-) diff --git a/mecanum_drive_controller/CMakeLists.txt b/mecanum_drive_controller/CMakeLists.txt index 8ef6aefe5c..eea8cc2b1a 100644 --- a/mecanum_drive_controller/CMakeLists.txt +++ b/mecanum_drive_controller/CMakeLists.txt @@ -20,7 +20,6 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS tf2 tf2_msgs tf2_geometry_msgs - custom_messages ) find_package(ament_cmake REQUIRED) diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp index 211d77c3a9..9a1aaebfde 100644 --- a/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp +++ b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp @@ -33,7 +33,7 @@ #include "realtime_tools/realtime_publisher.h" #include "std_srvs/srv/set_bool.hpp" -#include "custom_messages/msg/mecanum_drive_controller_state.hpp" +#include "control_msgs/msg/mecanum_drive_controller_state.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" #include "tf2_msgs/msg/tf_message.hpp" @@ -86,7 +86,7 @@ class MecanumDriveController : public controller_interface::ChainableControllerI using ControllerReferenceMsg = geometry_msgs::msg::TwistStamped; using OdomStateMsg = nav_msgs::msg::Odometry; using TfStateMsg = tf2_msgs::msg::TFMessage; - using ControllerStateMsg = custom_messages::msg::MecanumDriveControllerState; + using ControllerStateMsg = control_msgs::msg::MecanumDriveControllerState; protected: std::shared_ptr param_listener_; diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index 321b40da75..7af3c4e604 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -467,9 +467,9 @@ controller_interface::return_type MecanumDriveController::update_and_write_comma controller_state_publisher_->msg_.back_left_wheel_velocity = state_interfaces_[1].get_value(); controller_state_publisher_->msg_.back_right_wheel_velocity = state_interfaces_[2].get_value(); controller_state_publisher_->msg_.front_right_wheel_velocity = state_interfaces_[3].get_value(); - controller_state_publisher_->msg_.linear_x_velocity_command = reference_interfaces_[0]; - controller_state_publisher_->msg_.linear_y_velocity_command = reference_interfaces_[1]; - controller_state_publisher_->msg_.angular_z_command = reference_interfaces_[2]; + controller_state_publisher_->msg_.reference_vel.linear.x = reference_interfaces_[0]; + controller_state_publisher_->msg_.reference_vel.linear.y = reference_interfaces_[1]; + controller_state_publisher_->msg_.reference_vel.angular.z = reference_interfaces_[2]; controller_state_publisher_->unlockAndPublish(); } diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp index edce311a64..da2f08d529 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp @@ -202,7 +202,7 @@ TEST_F(MecanumDriveControllerTest, publish_status_success) subscribe_and_get_messages(msg); EXPECT_NEAR(msg.odom.pose.pose.position.y, 0.0, EPS); - EXPECT_EQ(msg.linear_x_velocity_command, 1.5); + EXPECT_EQ(msg.reference_vel.linear.x, 1.5); } @@ -230,7 +230,7 @@ TEST_F(MecanumDriveControllerTest, receive_message_and_publish_updated_status) subscribe_and_get_messages(msg); joint_command_values_[1] = command_lin_x; - EXPECT_TRUE(std::isnan(msg.linear_x_velocity_command)); + EXPECT_TRUE(std::isnan(msg.reference_vel.linear.x)); publish_commands(controller_->get_node()->now()); ASSERT_TRUE(controller_->wait_for_commands(executor)); @@ -249,7 +249,7 @@ TEST_F(MecanumDriveControllerTest, receive_message_and_publish_updated_status) subscribe_and_get_messages(msg); - ASSERT_EQ(msg.linear_x_velocity_command, 1.5); + ASSERT_EQ(msg.reference_vel.linear.x, 1.5); } // Tests controller behavior when too old msg is sent / age_of_last_command > ref_timeout case From 1090a3aed049d3100c20464ea39d281e3ea7d020 Mon Sep 17 00:00:00 2001 From: GiridharBukka Date: Mon, 16 Jan 2023 11:03:37 +0530 Subject: [PATCH 34/91] using control_msgs::msg::MecanumDriveControllerState and removing custom_msgs folder --- custom_messages/CMakeLists.txt | 37 ------------------- .../msg/MecanumDriveControllerState.msg | 9 ----- custom_messages/package.xml | 24 ------------ 3 files changed, 70 deletions(-) delete mode 100644 custom_messages/CMakeLists.txt delete mode 100644 custom_messages/msg/MecanumDriveControllerState.msg delete mode 100644 custom_messages/package.xml diff --git a/custom_messages/CMakeLists.txt b/custom_messages/CMakeLists.txt deleted file mode 100644 index 7b0dde3686..0000000000 --- a/custom_messages/CMakeLists.txt +++ /dev/null @@ -1,37 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(custom_messages) - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -# find dependencies -find_package(ament_cmake REQUIRED) -# uncomment the following section in order to fill in -# further dependencies manually. -# find_package( REQUIRED) -find_package(rosidl_default_generators REQUIRED) -find_package(std_msgs REQUIRED) -find_package(nav_msgs REQUIRED) - -rosidl_generate_interfaces(${PROJECT_NAME} - "msg/MecanumDriveControllerState.msg" - DEPENDENCIES - std_msgs - nav_msgs - ) - - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # comment the line when a copyright and license is added to all source files - set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # comment the line when this package is in a git repo and when - # a copyright and license is added to all source files - set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() -endif() - -ament_package() diff --git a/custom_messages/msg/MecanumDriveControllerState.msg b/custom_messages/msg/MecanumDriveControllerState.msg deleted file mode 100644 index a4a6bd0a19..0000000000 --- a/custom_messages/msg/MecanumDriveControllerState.msg +++ /dev/null @@ -1,9 +0,0 @@ -std_msgs/Header header -nav_msgs/Odometry odom -float64 front_left_wheel_velocity -float64 back_left_wheel_velocity -float64 back_right_wheel_velocity -float64 front_right_wheel_velocity -float64 linear_x_velocity_command -float64 linear_y_velocity_command -float64 angular_z_command diff --git a/custom_messages/package.xml b/custom_messages/package.xml deleted file mode 100644 index 61b26ca04a..0000000000 --- a/custom_messages/package.xml +++ /dev/null @@ -1,24 +0,0 @@ - - - - custom_messages - 0.0.0 - TODO: Package description - giridhar - TODO: License declaration - - ament_cmake - - ament_lint_auto - ament_lint_common - rosidl_default_generators - std_msgs - nav_msgs - - rosidl_default_runtime - -rosidl_interface_packages - - ament_cmake - - From 0f3aef464d5910c51379cdc08c25a06b894db8ec Mon Sep 17 00:00:00 2001 From: GiridharBukka Date: Mon, 16 Jan 2023 17:14:55 +0530 Subject: [PATCH 35/91] added chained mode test for update logic --- .../test/test_mecanum_drive_controller.cpp | 55 +++++++++++++++++++ .../test/test_mecanum_drive_controller.hpp | 1 + 2 files changed, 56 insertions(+) diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp index da2f08d529..90f4c2db8d 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp @@ -346,6 +346,11 @@ TEST_F(MecanumDriveControllerTest, test_update_logic_not_chainable) ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_FALSE(controller_->is_in_chained_mode()); + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } + // set command statically joint_command_values_[1] = command_lin_x; @@ -426,6 +431,56 @@ TEST_F(MecanumDriveControllerTest, test_update_logic_not_chainable) } } +TEST_F(MecanumDriveControllerTest, test_update_logic_chainable) +{ + // 1. age>ref_timeout 2. ageget_node()->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + controller_->set_chained_mode(true); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(controller_->is_in_chained_mode()); + + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } + + // set command statically + joint_command_values_[1] = command_lin_x; + + controller_->reference_interfaces_[0] = 1.5; + controller_->reference_interfaces_[1] = 0.0; + controller_->reference_interfaces_[2] = 0.0; + + const auto age_of_last_command = + controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; + + // age_of_last_command < ref_timeout_ + ASSERT_TRUE(age_of_last_command <= controller_->ref_timeout_); + + ASSERT_EQ( + controller_->update_and_write_commands( + controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_NE(joint_command_values_[1], command_lin_x); +// w0_vel = 1.0 / params_.kinematics.wheels_radius * (body_velocity_center_frame_.linear_x - body_velocity_center_frame_.linear_y - params_.kinematics.wheels_k * body_velocity_center_frame_.angular_z); +// joint_command_values_[1] = 1.0 / 0.5 * (1.5 - 0.0 - 1 * 0.0) + EXPECT_EQ(joint_command_values_[1], 3.0); + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_FALSE(std::isnan(interface)); + } + for (size_t i = 0; i < controller_->command_interfaces_.size(); ++i) + { + EXPECT_EQ(controller_->command_interfaces_[i].get_value(), 3.0); + } +} + // Tests behavior of update methods if ref_timeout = 0 TEST_F(MecanumDriveControllerTest, test_ref_timeout_zero_for_update) { diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp index cfd99e1570..d5b1eb6855 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp @@ -61,6 +61,7 @@ class TestableMecanumDriveController : public mecanum_drive_controller::MecanumD FRIEND_TEST(MecanumDriveControllerTest, test_time_stamp_zero); FRIEND_TEST(MecanumDriveControllerTest, test_message_accepted); FRIEND_TEST(MecanumDriveControllerTest, test_update_logic_not_chainable); + FRIEND_TEST(MecanumDriveControllerTest, test_update_logic_chainable); FRIEND_TEST(MecanumDriveControllerTest, test_ref_timeout_zero_for_update); FRIEND_TEST(MecanumDriveControllerTest, test_ref_timeout_zero_for_reference_callback); From 72a3d72d13bc676c4cf4309d3089ef6320689659 Mon Sep 17 00:00:00 2001 From: GiridharBukka Date: Mon, 16 Jan 2023 17:21:25 +0530 Subject: [PATCH 36/91] changed wheel vl names to more descriptive --- .../src/mecanum_drive_controller.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index 7af3c4e604..c52c7ac755 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -349,17 +349,17 @@ controller_interface::return_type MecanumDriveController::update_and_write_comma const rclcpp::Time & time, const rclcpp::Duration & period) { // FORWARD KINEMATICS (odometry). - double wheel0_vel = state_interfaces_[0].get_value(); - double wheel1_vel = state_interfaces_[1].get_value(); - double wheel2_vel = state_interfaces_[2].get_value(); - double wheel3_vel = state_interfaces_[3].get_value(); + double wheel_front_left_vel = state_interfaces_[0].get_value(); + double wheel_back_left_vel = state_interfaces_[1].get_value(); + double wheel_back_right_vel = state_interfaces_[2].get_value(); + double wheel_front_right_vel = state_interfaces_[3].get_value(); if ( - !std::isnan(wheel0_vel) && !std::isnan(wheel1_vel) && !std::isnan(wheel2_vel) && - !std::isnan(wheel3_vel)) + !std::isnan(wheel_front_left_vel) && !std::isnan(wheel_back_left_vel) && !std::isnan(wheel_back_right_vel) && + !std::isnan(wheel_front_right_vel)) { // Estimate twist (using joint information) and integrate - odometry_.update(wheel0_vel, wheel1_vel, wheel2_vel, wheel3_vel, period.seconds()); + odometry_.update(wheel_front_left_vel, wheel_back_left_vel, wheel_back_right_vel, wheel_front_right_vel, period.seconds()); } // INVERSE KINEMATICS (move robot). From 1d0562f3c96c114c904899c54d5b29dbd994cb58 Mon Sep 17 00:00:00 2001 From: GiridharBukka Date: Mon, 16 Jan 2023 17:50:42 +0530 Subject: [PATCH 37/91] renamed rot and traslation matrix variable names --- .../src/mecanum_drive_controller.cpp | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index c52c7ac755..c73f33c115 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -372,19 +372,19 @@ controller_interface::return_type MecanumDriveController::update_and_write_comma tf2::Quaternion quaternion; quaternion.setRPY(0.0, 0.0, params_.kinematics.base_frame_offset.theta); /// \note The variables meaning: - /// angular_transformation_from_base_2_center: Rotation transformation matrix, to transform from base frame to center frame - /// linear_transformation_from_base_2_center: offset/linear transformation matrix, to transform from base frame to center frame + /// rotation_from_base_to_center: Rotation transformation matrix, to transform from base frame to center frame + /// linear_trans_from_base_to_center: offset/linear transformation matrix, to transform from base frame to center frame - tf2::Matrix3x3 angular_transformation_from_base_2_center = tf2::Matrix3x3((quaternion)); + tf2::Matrix3x3 rotation_from_base_to_center = tf2::Matrix3x3((quaternion)); tf2::Vector3 velocity_in_base_frame_w_r_t_center_frame_ = - angular_transformation_from_base_2_center * tf2::Vector3(reference_interfaces_[0], reference_interfaces_[1], 0.0); - tf2::Vector3 linear_transformation_from_base_2_center = + rotation_from_base_to_center * tf2::Vector3(reference_interfaces_[0], reference_interfaces_[1], 0.0); + tf2::Vector3 linear_trans_from_base_to_center = tf2::Vector3(params_.kinematics.base_frame_offset.x, params_.kinematics.base_frame_offset.y, 0.0); - velocity_in_center_frame_linear_x = velocity_in_base_frame_w_r_t_center_frame_.x() + linear_transformation_from_base_2_center.y() * reference_interfaces_[2]; - velocity_in_center_frame_linear_y = velocity_in_base_frame_w_r_t_center_frame_.y() - linear_transformation_from_base_2_center.x() * reference_interfaces_[2]; + velocity_in_center_frame_linear_x = velocity_in_base_frame_w_r_t_center_frame_.x() + linear_trans_from_base_to_center.y() * reference_interfaces_[2]; + velocity_in_center_frame_linear_y = velocity_in_base_frame_w_r_t_center_frame_.y() - linear_trans_from_base_to_center.x() * reference_interfaces_[2]; velocity_in_center_frame_angular_z = reference_interfaces_[2]; double w0_vel = @@ -414,6 +414,7 @@ controller_interface::return_type MecanumDriveController::update_and_write_comma reference_interfaces_[0] = std::numeric_limits::quiet_NaN(); reference_interfaces_[1] = std::numeric_limits::quiet_NaN(); reference_interfaces_[2] = std::numeric_limits::quiet_NaN(); + command_interfaces_[0].set_value(0.0); fprintf(stderr, " command_interfaces_[0] = %f \n", command_interfaces_[0].get_value()); command_interfaces_[1].set_value(0.0); From 16b086fc6d61dd50fc41feab254e13b5661af884 Mon Sep 17 00:00:00 2001 From: GiridharBukka Date: Mon, 16 Jan 2023 17:57:40 +0530 Subject: [PATCH 38/91] renamed vl variable to more descriptive --- .../src/mecanum_drive_controller.cpp | 24 +++++++++---------- 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index c73f33c115..b1c2894179 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -387,25 +387,25 @@ controller_interface::return_type MecanumDriveController::update_and_write_comma velocity_in_center_frame_linear_y = velocity_in_base_frame_w_r_t_center_frame_.y() - linear_trans_from_base_to_center.x() * reference_interfaces_[2]; velocity_in_center_frame_angular_z = reference_interfaces_[2]; - double w0_vel = + double w_front_left_vel = 1.0 / params_.kinematics.wheels_radius * (velocity_in_center_frame_linear_x - velocity_in_center_frame_linear_y - params_.kinematics.wheels_k * velocity_in_center_frame_angular_z); - double w1_vel = + double w_back_left_vel = 1.0 / params_.kinematics.wheels_radius * (velocity_in_center_frame_linear_x + velocity_in_center_frame_linear_y - params_.kinematics.wheels_k * velocity_in_center_frame_angular_z); - double w2_vel = + double w_back_right_vel = 1.0 / params_.kinematics.wheels_radius * (velocity_in_center_frame_linear_x - velocity_in_center_frame_linear_y + params_.kinematics.wheels_k * velocity_in_center_frame_angular_z); - double w3_vel = + double w_front_right_vel = 1.0 / params_.kinematics.wheels_radius * (velocity_in_center_frame_linear_x + velocity_in_center_frame_linear_y + params_.kinematics.wheels_k * velocity_in_center_frame_angular_z); // Set wheels velocities: - command_interfaces_[0].set_value(w0_vel); - fprintf(stderr, " command_interfaces_[0] = %f \n", w0_vel); - command_interfaces_[1].set_value(w1_vel); - fprintf(stderr, " command_interfaces_[1] = %f \n", w1_vel); - command_interfaces_[2].set_value(w2_vel); - fprintf(stderr, " command_interfaces_[2] = %f \n", w2_vel); - command_interfaces_[3].set_value(w3_vel); - fprintf(stderr, " command_interfaces_[3] = %f \n", w3_vel); + command_interfaces_[0].set_value(w_front_left_vel); + fprintf(stderr, " command_interfaces_[0] = %f \n", w_front_left_vel); + command_interfaces_[1].set_value(w_back_left_vel); + fprintf(stderr, " command_interfaces_[1] = %f \n", w_back_left_vel); + command_interfaces_[2].set_value(w_back_right_vel); + fprintf(stderr, " command_interfaces_[2] = %f \n", w_back_right_vel); + command_interfaces_[3].set_value(w_front_right_vel); + fprintf(stderr, " command_interfaces_[3] = %f \n", w_front_right_vel); } From 5650ad9711d308d28296adc67ff7c9b3defaf0b3 Mon Sep 17 00:00:00 2001 From: GiridharBukka Date: Mon, 16 Jan 2023 18:10:16 +0530 Subject: [PATCH 39/91] ran pre-commit formatter --- mecanum_drive_controller/CMakeLists.txt | 2 +- .../mecanum_drive_controller.hpp | 8 +- .../mecanum_drive_controller/odometry.hpp | 22 +++-- .../src/mecanum_drive_controller.cpp | 98 +++++++++++-------- .../src/mecanum_drive_controller.yaml | 4 +- mecanum_drive_controller/src/odometry.cpp | 29 ++++-- .../test/mecanum_drive_controller_params.yaml | 2 +- .../test/test_mecanum_drive_controller.cpp | 26 +++-- ...st_mecanum_drive_controller_preceeding.cpp | 5 +- 9 files changed, 111 insertions(+), 85 deletions(-) diff --git a/mecanum_drive_controller/CMakeLists.txt b/mecanum_drive_controller/CMakeLists.txt index eea8cc2b1a..925183f260 100644 --- a/mecanum_drive_controller/CMakeLists.txt +++ b/mecanum_drive_controller/CMakeLists.txt @@ -38,7 +38,7 @@ add_library( src/mecanum_drive_controller.cpp src/odometry.cpp ) -target_include_directories(mecanum_drive_controller PUBLIC +target_include_directories(mecanum_drive_controller PUBLIC "$" "$") target_link_libraries(mecanum_drive_controller mecanum_drive_controller_parameters) diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp index 9a1aaebfde..47dd1afe97 100644 --- a/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp +++ b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp @@ -98,7 +98,6 @@ class MecanumDriveController : public controller_interface::ChainableControllerI //Names of the references, ex: high level vel commands from MoveIt, Nav2, etc. used for preceding controller std::vector reference_names_; - // Command subscribers and Controller State, odom state, tf state publishers rclcpp::Subscription::SharedPtr ref_subscriber_ = nullptr; realtime_tools::RealtimeBuffer> input_ref_; @@ -128,10 +127,9 @@ class MecanumDriveController : public controller_interface::ChainableControllerI MECANUM_DRIVE_CONTROLLER__VISIBILITY_LOCAL void reference_callback(const std::shared_ptr msg); - double velocity_in_center_frame_linear_x; // [m/s] - double velocity_in_center_frame_linear_y; // [m/s] - double velocity_in_center_frame_angular_z; // [rad/s] - + double velocity_in_center_frame_linear_x; // [m/s] + double velocity_in_center_frame_linear_y; // [m/s] + double velocity_in_center_frame_angular_z; // [rad/s] }; } // namespace mecanum_drive_controller diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp b/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp index d05f58381b..3ac054768a 100644 --- a/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp +++ b/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp @@ -15,15 +15,14 @@ #ifndef MECANUM_DRIVE_CONTROLLER__ODOMETRY_HPP_ #define MECANUM_DRIVE_CONTROLLER__ODOMETRY_HPP_ +#include "geometry_msgs/msg/twist.hpp" #include "realtime_tools/realtime_buffer.h" #include "realtime_tools/realtime_publisher.h" -#include "geometry_msgs/msg/twist.hpp" #define PLANAR_POINT_DIM 3 namespace mecanum_drive_controller { - /// \brief The Odometry class handles odometry readings /// (2D pose and velocity with related timestamp) class Odometry @@ -63,7 +62,11 @@ class Odometry /// \return body velocity of the base frame (linear y component) [m/s] double getVy() const { return velocity_in_base_frame_linear_y; } /// \return body velocity of the base frame (angular z component) [m/s] - double getWz() const { return velocity_in_base_frame_angular_z;; } + double getWz() const + { + return velocity_in_base_frame_angular_z; + ; + } /// \brief Sets the wheels parameters: mecanum geometric param and radius /// \param wheels_k Wheels geometric param (used in mecanum wheels' ik) [m] @@ -71,7 +74,6 @@ class Odometry void setWheelsParams(double wheels_k, double wheels_radius); private: - /// Current timestamp: rclcpp::Time timestamp_; @@ -79,16 +81,16 @@ class Odometry std::array base_frame_offset_; /// Current pose: - double position_x_in_base_frame_; // [m] - double position_y_in_base_frame_; // [m] + double position_x_in_base_frame_; // [m] + double position_y_in_base_frame_; // [m] double orientation_z_in_base_frame_; // [rad] - double velocity_in_base_frame_linear_x; // [m/s] - double velocity_in_base_frame_linear_y; // [m/s] - double velocity_in_base_frame_angular_z; // [rad/s] + double velocity_in_base_frame_linear_x; // [m/s] + double velocity_in_base_frame_linear_y; // [m/s] + double velocity_in_base_frame_angular_z; // [rad/s] /// Wheels kinematic parameters [m]: - /// lx and ly represent the distance from the robot's center to the wheels projected on + /// lx and ly represent the distance from the robot's center to the wheels projected on /// the x and y axis with origin at robots center respectively, wheels_k_ = lx+ly double wheels_k_; double wheels_radius_; // [m] diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index b1c2894179..51213d997a 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -28,7 +28,6 @@ namespace { // utility - using ControllerReferenceMsg = mecanum_drive_controller::MecanumDriveController::ControllerReferenceMsg; @@ -75,17 +74,22 @@ controller_interface::CallbackReturn MecanumDriveController::on_configure( { params_ = param_listener_->get_params(); - if (!params_.state_joint_names.empty()) { + if (!params_.state_joint_names.empty()) + { state_joint_names_ = params_.state_joint_names; - } else { + } + else + { state_joint_names_ = params_.joint_names; } - if (params_.joint_names.size() != state_joint_names_.size()) { - RCLCPP_FATAL(get_node()->get_logger(), - "Size of 'joints' (%d) and 'state_joint_names' (%d) parameters has " - "to be the same!", - params_.joint_names.size(), state_joint_names_.size()); + if (params_.joint_names.size() != state_joint_names_.size()) + { + RCLCPP_FATAL( + get_node()->get_logger(), + "Size of 'joints' (%d) and 'state_joint_names' (%d) parameters has " + "to be the same!", + params_.joint_names.size(), state_joint_names_.size()); return CallbackReturn::FAILURE; } @@ -225,8 +229,7 @@ void MecanumDriveController::reference_callback(const std::shared_ptrheader.stamp).seconds(), age_of_last_command.seconds(), ref_timeout_.seconds()); - reset_controller_reference_msg(msg, get_node()); - + reset_controller_reference_msg(msg, get_node()); } } @@ -237,9 +240,9 @@ MecanumDriveController::command_interface_configuration() const command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; command_interfaces_config.names.reserve(params_.joint_names.size()); - for (const auto& joint : params_.joint_names) { - command_interfaces_config.names.push_back(joint + "/" + - params_.interface_name); + for (const auto & joint : params_.joint_names) + { + command_interfaces_config.names.push_back(joint + "/" + params_.interface_name); } return command_interfaces_config; @@ -253,9 +256,9 @@ controller_interface::InterfaceConfiguration MecanumDriveController::state_inter state_interfaces_config.names.reserve(state_joint_names_.size()); - for (const auto& joint : state_joint_names_) { - state_interfaces_config.names.push_back(joint + "/" + - params_.interface_name); + for (const auto & joint : state_joint_names_) + { + state_interfaces_config.names.push_back(joint + "/" + params_.interface_name); } return state_interfaces_config; @@ -270,10 +273,11 @@ MecanumDriveController::on_export_reference_interfaces() reference_interfaces.reserve(reference_interfaces_.size()); - for (size_t i = 0; i < reference_interfaces_.size(); ++i) { + for (size_t i = 0; i < reference_interfaces_.size(); ++i) + { reference_interfaces.push_back(hardware_interface::CommandInterface( - get_node()->get_name(), reference_names_[i] + "/" + params_.interface_name, - &reference_interfaces_[i])); + get_node()->get_name(), reference_names_[i] + "/" + params_.interface_name, + &reference_interfaces_[i])); } return reference_interfaces; @@ -313,7 +317,9 @@ controller_interface::return_type MecanumDriveController::update_reference_from_ // 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.linear.y) && !std::isnan(current_ref->twist.angular.z)) + if ( + !std::isnan(current_ref->twist.linear.x) && !std::isnan(current_ref->twist.linear.y) && + !std::isnan(current_ref->twist.angular.z)) { reference_interfaces_[0] = current_ref->twist.linear.x; reference_interfaces_[1] = current_ref->twist.linear.y; @@ -325,7 +331,6 @@ controller_interface::return_type MecanumDriveController::update_reference_from_ current_ref->twist.linear.y = std::numeric_limits::quiet_NaN(); current_ref->twist.angular.z = std::numeric_limits::quiet_NaN(); } - } } else @@ -334,7 +339,9 @@ controller_interface::return_type MecanumDriveController::update_reference_from_ reference_interfaces_[1] = std::numeric_limits::quiet_NaN(); reference_interfaces_[2] = std::numeric_limits::quiet_NaN(); - if (!std::isnan(current_ref->twist.linear.x) && !std::isnan(current_ref->twist.linear.y) && !std::isnan(current_ref->twist.angular.z)) + if ( + !std::isnan(current_ref->twist.linear.x) && !std::isnan(current_ref->twist.linear.y) && + !std::isnan(current_ref->twist.angular.z)) { current_ref->twist.linear.x = std::numeric_limits::quiet_NaN(); current_ref->twist.linear.y = std::numeric_limits::quiet_NaN(); @@ -355,11 +362,13 @@ controller_interface::return_type MecanumDriveController::update_and_write_comma double wheel_front_right_vel = state_interfaces_[3].get_value(); if ( - !std::isnan(wheel_front_left_vel) && !std::isnan(wheel_back_left_vel) && !std::isnan(wheel_back_right_vel) && - !std::isnan(wheel_front_right_vel)) + !std::isnan(wheel_front_left_vel) && !std::isnan(wheel_back_left_vel) && + !std::isnan(wheel_back_right_vel) && !std::isnan(wheel_front_right_vel)) { // Estimate twist (using joint information) and integrate - odometry_.update(wheel_front_left_vel, wheel_back_left_vel, wheel_back_right_vel, wheel_front_right_vel, period.seconds()); + odometry_.update( + wheel_front_left_vel, wheel_back_left_vel, wheel_back_right_vel, wheel_front_right_vel, + period.seconds()); } // INVERSE KINEMATICS (move robot). @@ -375,26 +384,37 @@ controller_interface::return_type MecanumDriveController::update_and_write_comma /// rotation_from_base_to_center: Rotation transformation matrix, to transform from base frame to center frame /// linear_trans_from_base_to_center: offset/linear transformation matrix, to transform from base frame to center frame - - tf2::Matrix3x3 rotation_from_base_to_center = tf2::Matrix3x3((quaternion)); tf2::Vector3 velocity_in_base_frame_w_r_t_center_frame_ = - rotation_from_base_to_center * tf2::Vector3(reference_interfaces_[0], reference_interfaces_[1], 0.0); - tf2::Vector3 linear_trans_from_base_to_center = - tf2::Vector3(params_.kinematics.base_frame_offset.x, params_.kinematics.base_frame_offset.y, 0.0); - - velocity_in_center_frame_linear_x = velocity_in_base_frame_w_r_t_center_frame_.x() + linear_trans_from_base_to_center.y() * reference_interfaces_[2]; - velocity_in_center_frame_linear_y = velocity_in_base_frame_w_r_t_center_frame_.y() - linear_trans_from_base_to_center.x() * reference_interfaces_[2]; + rotation_from_base_to_center * + tf2::Vector3(reference_interfaces_[0], reference_interfaces_[1], 0.0); + tf2::Vector3 linear_trans_from_base_to_center = tf2::Vector3( + params_.kinematics.base_frame_offset.x, params_.kinematics.base_frame_offset.y, 0.0); + + velocity_in_center_frame_linear_x = + velocity_in_base_frame_w_r_t_center_frame_.x() + + linear_trans_from_base_to_center.y() * reference_interfaces_[2]; + velocity_in_center_frame_linear_y = + velocity_in_base_frame_w_r_t_center_frame_.y() - + linear_trans_from_base_to_center.x() * reference_interfaces_[2]; velocity_in_center_frame_angular_z = reference_interfaces_[2]; double w_front_left_vel = - 1.0 / params_.kinematics.wheels_radius * (velocity_in_center_frame_linear_x - velocity_in_center_frame_linear_y - params_.kinematics.wheels_k * velocity_in_center_frame_angular_z); + 1.0 / params_.kinematics.wheels_radius * + (velocity_in_center_frame_linear_x - velocity_in_center_frame_linear_y - + params_.kinematics.wheels_k * velocity_in_center_frame_angular_z); double w_back_left_vel = - 1.0 / params_.kinematics.wheels_radius * (velocity_in_center_frame_linear_x + velocity_in_center_frame_linear_y - params_.kinematics.wheels_k * velocity_in_center_frame_angular_z); + 1.0 / params_.kinematics.wheels_radius * + (velocity_in_center_frame_linear_x + velocity_in_center_frame_linear_y - + params_.kinematics.wheels_k * velocity_in_center_frame_angular_z); double w_back_right_vel = - 1.0 / params_.kinematics.wheels_radius * (velocity_in_center_frame_linear_x - velocity_in_center_frame_linear_y + params_.kinematics.wheels_k * velocity_in_center_frame_angular_z); + 1.0 / params_.kinematics.wheels_radius * + (velocity_in_center_frame_linear_x - velocity_in_center_frame_linear_y + + params_.kinematics.wheels_k * velocity_in_center_frame_angular_z); double w_front_right_vel = - 1.0 / params_.kinematics.wheels_radius * (velocity_in_center_frame_linear_x + velocity_in_center_frame_linear_y + params_.kinematics.wheels_k * velocity_in_center_frame_angular_z); + 1.0 / params_.kinematics.wheels_radius * + (velocity_in_center_frame_linear_x + velocity_in_center_frame_linear_y + + params_.kinematics.wheels_k * velocity_in_center_frame_angular_z); // Set wheels velocities: @@ -406,15 +426,13 @@ controller_interface::return_type MecanumDriveController::update_and_write_comma fprintf(stderr, " command_interfaces_[2] = %f \n", w_back_right_vel); command_interfaces_[3].set_value(w_front_right_vel); fprintf(stderr, " command_interfaces_[3] = %f \n", w_front_right_vel); - - } else { reference_interfaces_[0] = std::numeric_limits::quiet_NaN(); reference_interfaces_[1] = std::numeric_limits::quiet_NaN(); reference_interfaces_[2] = std::numeric_limits::quiet_NaN(); - + command_interfaces_[0].set_value(0.0); fprintf(stderr, " command_interfaces_[0] = %f \n", command_interfaces_[0].get_value()); command_interfaces_[1].set_value(0.0); diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.yaml b/mecanum_drive_controller/src/mecanum_drive_controller.yaml index 3ae082db8e..d7fabc4ab0 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.yaml +++ b/mecanum_drive_controller/src/mecanum_drive_controller.yaml @@ -5,7 +5,7 @@ mecanum_drive_controller: 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.", } - + joint_names: { type: string_array, default_value: [], @@ -68,7 +68,7 @@ mecanum_drive_controller: wheels_k: { type: double, default_value: 0.0, - description: "Wheels geometric param used in mecanum wheels' IK. lx and ly represent the distance from the robot's center to the wheels projected on + description: "Wheels geometric param used in mecanum wheels' IK. lx and ly represent the distance from the robot's center to the wheels projected on the x and y axis with origin at robots center respectively, wheels_k = lx+ly", read_only: false, diff --git a/mecanum_drive_controller/src/odometry.cpp b/mecanum_drive_controller/src/odometry.cpp index 6c0affb561..5064e70285 100644 --- a/mecanum_drive_controller/src/odometry.cpp +++ b/mecanum_drive_controller/src/odometry.cpp @@ -21,7 +21,6 @@ using namespace std; namespace mecanum_drive_controller { - Odometry::Odometry(size_t velocity_rolling_window_size) : timestamp_(0.0), position_x_in_base_frame_(0.0), @@ -35,7 +34,8 @@ Odometry::Odometry(size_t velocity_rolling_window_size) { } -void Odometry::init(const rclcpp::Time & time, std::array base_frame_offset) +void Odometry::init( + const rclcpp::Time & time, std::array base_frame_offset) { // Reset timestamp: timestamp_ = time; @@ -66,7 +66,8 @@ bool Odometry::update( /// angular_transformation_from_center_2_base: Rotation transformation matrix, to transform from center frame to base frame /// linear_transformation_from_center_2_base: offset/linear transformation matrix, to transform from center frame to base frame - double velocity_in_center_frame_linear_x = 0.25 * wheels_radius_ * (wheel0_vel + wheel1_vel + wheel2_vel + wheel3_vel); + double velocity_in_center_frame_linear_x = + 0.25 * wheels_radius_ * (wheel0_vel + wheel1_vel + wheel2_vel + wheel3_vel); double velocity_in_center_frame_linear_y = 0.25 * wheels_radius_ * (-wheel0_vel + wheel1_vel - wheel2_vel + wheel3_vel); double velocity_in_center_frame_angular_z = @@ -76,11 +77,19 @@ bool Odometry::update( orientation_R_c_b.setRPY(0.0, 0.0, -base_frame_offset_[2]); tf2::Matrix3x3 angular_transformation_from_center_2_base = tf2::Matrix3x3((orientation_R_c_b)); - tf2::Vector3 velocity_in_center_frame_w_r_t_base_frame_ =angular_transformation_from_center_2_base * tf2::Vector3(velocity_in_center_frame_linear_x, velocity_in_center_frame_linear_y, 0.0); - tf2::Vector3 linear_transformation_from_center_2_base =angular_transformation_from_center_2_base * tf2::Vector3(-base_frame_offset_[0], -base_frame_offset_[1], 0.0); - - velocity_in_base_frame_linear_x = velocity_in_center_frame_w_r_t_base_frame_.x() + linear_transformation_from_center_2_base.y() * velocity_in_center_frame_angular_z; - velocity_in_base_frame_linear_y = velocity_in_center_frame_w_r_t_base_frame_.y() - linear_transformation_from_center_2_base.x() * velocity_in_center_frame_angular_z; + tf2::Vector3 velocity_in_center_frame_w_r_t_base_frame_ = + angular_transformation_from_center_2_base * + tf2::Vector3(velocity_in_center_frame_linear_x, velocity_in_center_frame_linear_y, 0.0); + tf2::Vector3 linear_transformation_from_center_2_base = + angular_transformation_from_center_2_base * + tf2::Vector3(-base_frame_offset_[0], -base_frame_offset_[1], 0.0); + + velocity_in_base_frame_linear_x = + velocity_in_center_frame_w_r_t_base_frame_.x() + + linear_transformation_from_center_2_base.y() * velocity_in_center_frame_angular_z; + velocity_in_base_frame_linear_y = + velocity_in_center_frame_w_r_t_base_frame_.y() - + linear_transformation_from_center_2_base.x() * velocity_in_center_frame_angular_z; velocity_in_base_frame_angular_z = velocity_in_center_frame_angular_z; /// Integration. @@ -92,7 +101,9 @@ bool Odometry::update( orientation_R_b_odom.setRPY(0.0, 0.0, -base_frame_offset_[2]); tf2::Matrix3x3 angular_transformation_from_base_2_odom = tf2::Matrix3x3((orientation_R_b_odom)); - tf2::Vector3 velocity_in_base_frame_w_r_t_odom_frame_ = angular_transformation_from_base_2_odom * tf2::Vector3(velocity_in_base_frame_linear_x, velocity_in_base_frame_linear_y, 0.0); + tf2::Vector3 velocity_in_base_frame_w_r_t_odom_frame_ = + angular_transformation_from_base_2_odom * + tf2::Vector3(velocity_in_base_frame_linear_x, velocity_in_base_frame_linear_y, 0.0); position_x_in_base_frame_ += velocity_in_base_frame_w_r_t_odom_frame_.x() * dt; position_y_in_base_frame_ += velocity_in_base_frame_w_r_t_odom_frame_.y() * dt; diff --git a/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml b/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml index cc8ad75f56..ae99da685f 100644 --- a/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml +++ b/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml @@ -7,7 +7,7 @@ test_mecanum_drive_controller: reference_names: ["linear_x", "linear_y", "angular_z"] - interface_name: velocity + interface_name: velocity kinematics: base_frame_offset: diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp index 90f4c2db8d..16b2ebee89 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp @@ -67,7 +67,6 @@ TEST_F(MecanumDriveControllerTest, all_parameters_set_configure_success) ASSERT_THAT(controller_->params_.joint_names, testing::ElementsAreArray(joint_names_)); ASSERT_TRUE(controller_->params_.state_joint_names.empty()); ASSERT_EQ(controller_->params_.interface_name, interface_name_); - } // checking if all interfaces, command, state and reference are exported as expected @@ -97,7 +96,8 @@ TEST_F(MecanumDriveControllerTest, check_exported_intefaces) auto reference_interfaces = controller_->export_reference_interfaces(); ASSERT_EQ(reference_interfaces.size(), NR_REF_ITFS); - for (size_t i = 0; i < reference_names_.size(); ++i) { + for (size_t i = 0; i < reference_names_.size(); ++i) + { const std::string ref_itf_name = std::string(controller_->get_node()->get_name()) + "/" + reference_names_[i] + "/" + interface_name_; EXPECT_EQ(reference_interfaces[i].get_name(), ref_itf_name); @@ -203,11 +203,9 @@ TEST_F(MecanumDriveControllerTest, publish_status_success) EXPECT_NEAR(msg.odom.pose.pose.position.y, 0.0, EPS); EXPECT_EQ(msg.reference_vel.linear.x, 1.5); - - } -// Tests the msg subscriber and publisher +// Tests the msg subscriber and publisher TEST_F(MecanumDriveControllerTest, receive_message_and_publish_updated_status) { SetUpController(); @@ -243,8 +241,8 @@ TEST_F(MecanumDriveControllerTest, receive_message_and_publish_updated_status) controller_->update_and_write_commands( controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); -// w0_vel = 1.0 / params_.kinematics.wheels_radius * (body_velocity_center_frame_.linear_x - body_velocity_center_frame_.linear_y - params_.kinematics.wheels_k * body_velocity_center_frame_.angular_z); -// joint_command_values_[1] = 1.0 / 0.5 * (1.5 - 0.0 - 1 * 0.0) + // w0_vel = 1.0 / params_.kinematics.wheels_radius * (body_velocity_center_frame_.linear_x - body_velocity_center_frame_.linear_y - params_.kinematics.wheels_k * body_velocity_center_frame_.angular_z); + // joint_command_values_[1] = 1.0 / 0.5 * (1.5 - 0.0 - 1 * 0.0) EXPECT_EQ(joint_command_values_[1], 3.0); subscribe_and_get_messages(msg); @@ -310,7 +308,7 @@ TEST_F(MecanumDriveControllerTest, test_time_stamp_zero) TEST_F(MecanumDriveControllerTest, test_message_accepted) { SetUpController(); - + rclcpp::executors::MultiThreadedExecutor executor; executor.add_node(controller_->get_node()->get_node_base_interface()); @@ -417,8 +415,8 @@ TEST_F(MecanumDriveControllerTest, test_update_logic_not_chainable) controller_interface::return_type::OK); EXPECT_NE(joint_command_values_[1], command_lin_x); -// w0_vel = 1.0 / params_.kinematics.wheels_radius * (body_velocity_center_frame_.linear_x - body_velocity_center_frame_.linear_y - params_.kinematics.wheels_k * body_velocity_center_frame_.angular_z); -// joint_command_values_[1] = 1.0 / 0.5 * (1.5 - 0.0 - 1 * 0.0) + // w0_vel = 1.0 / params_.kinematics.wheels_radius * (body_velocity_center_frame_.linear_x - body_velocity_center_frame_.linear_y - params_.kinematics.wheels_k * body_velocity_center_frame_.angular_z); + // joint_command_values_[1] = 1.0 / 0.5 * (1.5 - 0.0 - 1 * 0.0) EXPECT_EQ(joint_command_values_[1], 3.0); ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); for (const auto & interface : controller_->reference_interfaces_) @@ -468,8 +466,8 @@ TEST_F(MecanumDriveControllerTest, test_update_logic_chainable) controller_interface::return_type::OK); EXPECT_NE(joint_command_values_[1], command_lin_x); -// w0_vel = 1.0 / params_.kinematics.wheels_radius * (body_velocity_center_frame_.linear_x - body_velocity_center_frame_.linear_y - params_.kinematics.wheels_k * body_velocity_center_frame_.angular_z); -// joint_command_values_[1] = 1.0 / 0.5 * (1.5 - 0.0 - 1 * 0.0) + // w0_vel = 1.0 / params_.kinematics.wheels_radius * (body_velocity_center_frame_.linear_x - body_velocity_center_frame_.linear_y - params_.kinematics.wheels_k * body_velocity_center_frame_.angular_z); + // joint_command_values_[1] = 1.0 / 0.5 * (1.5 - 0.0 - 1 * 0.0) EXPECT_EQ(joint_command_values_[1], 3.0); for (const auto & interface : controller_->reference_interfaces_) { @@ -525,8 +523,8 @@ TEST_F(MecanumDriveControllerTest, test_ref_timeout_zero_for_update) EXPECT_FALSE(std::isnan(joint_command_values_[1])); EXPECT_NE(joint_command_values_[1], command_lin_x); -// w0_vel = 1.0 / params_.kinematics.wheels_radius * (body_velocity_center_frame_.linear_x - body_velocity_center_frame_.linear_y - params_.kinematics.wheels_k * body_velocity_center_frame_.angular_z); -// joint_command_values_[1] = 1.0 / 0.5 * (1.5 - 0.0 - 1 * 0.0) + // w0_vel = 1.0 / params_.kinematics.wheels_radius * (body_velocity_center_frame_.linear_x - body_velocity_center_frame_.linear_y - params_.kinematics.wheels_k * body_velocity_center_frame_.angular_z); + // joint_command_values_[1] = 1.0 / 0.5 * (1.5 - 0.0 - 1 * 0.0) EXPECT_EQ(joint_command_values_[1], 3.0); ASSERT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x)); } diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller_preceeding.cpp b/mecanum_drive_controller/test/test_mecanum_drive_controller_preceeding.cpp index 67ec4b9434..ae261004f2 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller_preceeding.cpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller_preceeding.cpp @@ -42,10 +42,10 @@ TEST_F(MecanumDriveControllerTest, all_parameters_set_configure_success) ASSERT_EQ(controller_->params_.reference_timeout, 0.1); ASSERT_THAT(controller_->params_.joint_names, testing::ElementsAreArray(joint_names_)); - ASSERT_THAT(controller_->params_.state_joint_names, testing::ElementsAreArray(state_joint_names_)); + ASSERT_THAT( + controller_->params_.state_joint_names, testing::ElementsAreArray(state_joint_names_)); ASSERT_THAT(controller_->state_joint_names_, testing::ElementsAreArray(state_joint_names_)); ASSERT_EQ(controller_->params_.interface_name, interface_name_); - } TEST_F(MecanumDriveControllerTest, check_exported_intefaces) @@ -67,7 +67,6 @@ TEST_F(MecanumDriveControllerTest, check_exported_intefaces) { EXPECT_EQ(state_intefaces.names[i], state_joint_names_[i] + "/" + interface_name_); } - } int main(int argc, char ** argv) From 6cae0d2c81c8b0120387b3a1e0407bb5d768c26a Mon Sep 17 00:00:00 2001 From: GiridharBukka Date: Tue, 17 Jan 2023 01:08:31 +0530 Subject: [PATCH 40/91] set ref_itfces to 0 to stop vehicle in case of timeout --- mecanum_drive_controller/src/mecanum_drive_controller.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index 51213d997a..a0f4416a11 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -335,14 +335,14 @@ controller_interface::return_type MecanumDriveController::update_reference_from_ } else { - reference_interfaces_[0] = std::numeric_limits::quiet_NaN(); - reference_interfaces_[1] = std::numeric_limits::quiet_NaN(); - reference_interfaces_[2] = std::numeric_limits::quiet_NaN(); - if ( !std::isnan(current_ref->twist.linear.x) && !std::isnan(current_ref->twist.linear.y) && !std::isnan(current_ref->twist.angular.z)) { + reference_interfaces_[0] = 0.0; + reference_interfaces_[1] = 0.0; + reference_interfaces_[2] = 0.0; + current_ref->twist.linear.x = std::numeric_limits::quiet_NaN(); current_ref->twist.linear.y = std::numeric_limits::quiet_NaN(); current_ref->twist.angular.z = std::numeric_limits::quiet_NaN(); From 37f9d10df7b376f69cea598eb5227ef4a5aecec1 Mon Sep 17 00:00:00 2001 From: GiridharBukka Date: Wed, 18 Jan 2023 17:23:38 +0530 Subject: [PATCH 41/91] added description --- .../test/test_mecanum_drive_controller.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp index 16b2ebee89..2ad73c2abb 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp @@ -330,7 +330,7 @@ TEST_F(MecanumDriveControllerTest, test_message_accepted) EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z, 0.0); } -//Test that checks the status of chainable mode and update methods logic +//Test that checks the status of chainable mode and update methods logic accordingly, when ref_timeout and within ref_timeout TEST_F(MecanumDriveControllerTest, test_update_logic_not_chainable) { // 1. age>ref_timeout 2. ageref_timeout 2. age Date: Thu, 19 Jan 2023 13:42:21 +0100 Subject: [PATCH 42/91] check nan --- .../test/test_mecanum_drive_controller.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp index 2ad73c2abb..9132c03f0b 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp @@ -373,6 +373,13 @@ TEST_F(MecanumDriveControllerTest, test_update_logic_not_chainable) controller_->update_reference_from_subscribers( controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); + + EXPECT_EQ(controller_->reference_interfaces_[1], 0); + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_FALSE(std::isnan(interface)); + } + ASSERT_EQ( controller_->update_and_write_commands( controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), From 7dc9a60e5affd5ea3555124766a190a8f87644b0 Mon Sep 17 00:00:00 2001 From: GiridharBukka Date: Tue, 24 Jan 2023 11:35:42 +0100 Subject: [PATCH 43/91] odom vel naming descriptive --- .../include/mecanum_drive_controller/odometry.hpp | 10 +++++----- mecanum_drive_controller/src/odometry.cpp | 8 ++++---- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp b/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp index 3ac054768a..56157f9f69 100644 --- a/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp +++ b/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp @@ -42,14 +42,14 @@ class Odometry void init(const rclcpp::Time & time, std::array base_frame_offset); /// \brief Updates the odometry class with latest wheels position - /// \param wheel0_vel Wheel velocity [rad/s] - /// \param wheel1_vel Wheel velocity [rad/s] - /// \param wheel2_vel Wheel velocity [rad/s] - /// \param wheel3_vel Wheel velocity [rad/s] + /// \param wheel_front_left_vel Wheel velocity [rad/s] + /// \param wheel_back_left_vel Wheel velocity [rad/s] + /// \param wheel_back_right_vel Wheel velocity [rad/s] + /// \param wheel_front_right_vel Wheel velocity [rad/s] /// \param time Current time /// \return true if the odometry is actually updated bool update( - double wheel0_vel, double wheel1_vel, double wheel2_vel, double wheel3_vel, const double dt); + double wheel_front_left_vel, double wheel_back_left_vel, double wheel_back_right_vel, double wheel_front_right_vel, const double dt); /// \return position (x component) [m] double getX() const { return position_x_in_base_frame_; } diff --git a/mecanum_drive_controller/src/odometry.cpp b/mecanum_drive_controller/src/odometry.cpp index 5064e70285..32226b9165 100644 --- a/mecanum_drive_controller/src/odometry.cpp +++ b/mecanum_drive_controller/src/odometry.cpp @@ -47,7 +47,7 @@ void Odometry::init( } bool Odometry::update( - double wheel0_vel, double wheel1_vel, double wheel2_vel, double wheel3_vel, const double dt) + double wheel_front_left_vel, double wheel_back_left_vel, double wheel_back_right_vel, double wheel_front_right_vel, const double dt) { /// We cannot estimate the speed with very small time intervals: // const double dt = (time - timestamp_).toSec(); @@ -67,11 +67,11 @@ bool Odometry::update( /// linear_transformation_from_center_2_base: offset/linear transformation matrix, to transform from center frame to base frame double velocity_in_center_frame_linear_x = - 0.25 * wheels_radius_ * (wheel0_vel + wheel1_vel + wheel2_vel + wheel3_vel); + 0.25 * wheels_radius_ * (wheel_front_left_vel + wheel_back_left_vel + wheel_back_right_vel + wheel_front_right_vel); double velocity_in_center_frame_linear_y = - 0.25 * wheels_radius_ * (-wheel0_vel + wheel1_vel - wheel2_vel + wheel3_vel); + 0.25 * wheels_radius_ * (-wheel_front_left_vel + wheel_back_left_vel - wheel_back_right_vel + wheel_front_right_vel); double velocity_in_center_frame_angular_z = - 0.25 * wheels_radius_ / wheels_k_ * (-wheel0_vel - wheel1_vel + wheel2_vel + wheel3_vel); + 0.25 * wheels_radius_ / wheels_k_ * (-wheel_front_left_vel - wheel_back_left_vel + wheel_back_right_vel + wheel_front_right_vel); tf2::Quaternion orientation_R_c_b; orientation_R_c_b.setRPY(0.0, 0.0, -base_frame_offset_[2]); From 0405b9b94df2d82ba9dcf679fdaf5d88ab23a5a6 Mon Sep 17 00:00:00 2001 From: GiridharBukka Date: Tue, 24 Jan 2023 12:15:49 +0100 Subject: [PATCH 44/91] removed outdated nomenclature --- mecanum_drive_controller/src/mecanum_drive_controller.cpp | 3 ++- mecanum_drive_controller/src/mecanum_drive_controller.yaml | 2 +- mecanum_drive_controller/src/odometry.cpp | 4 ++-- .../test/test_mecanum_drive_controller.cpp | 4 ++-- .../test/test_mecanum_drive_controller.hpp | 2 +- 5 files changed, 8 insertions(+), 7 deletions(-) diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index a0f4416a11..5c7f8f064c 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -434,7 +434,8 @@ controller_interface::return_type MecanumDriveController::update_and_write_comma reference_interfaces_[2] = std::numeric_limits::quiet_NaN(); command_interfaces_[0].set_value(0.0); - fprintf(stderr, " command_interfaces_[0] = %f \n", command_interfaces_[0].get_value()); + // left intentionally for debug + // fprintf(stderr, " command_interfaces_[0] = %f \n", command_interfaces_[0].get_value()); command_interfaces_[1].set_value(0.0); command_interfaces_[2].set_value(0.0); command_interfaces_[3].set_value(0.0); diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.yaml b/mecanum_drive_controller/src/mecanum_drive_controller.yaml index d7fabc4ab0..9dd6f55f2f 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.yaml +++ b/mecanum_drive_controller/src/mecanum_drive_controller.yaml @@ -2,7 +2,7 @@ mecanum_drive_controller: reference_timeout: { type: double, default_value: 0.0, - description: "Timeout for controller references after which they will be reset. This is especially useful for controllers that can cause unwanted and dangerous behaviour if reference is not reset, e.g., velocity controllers. If value is 0 the reference is reset after each run.", + description: "Timeout for controller references after which they will be reset. This is especially useful for controllers that can cause unwanted and dangerous behavior if reference is not reset, e.g., velocity controllers. If value is 0 the reference is reset after each run.", } diff --git a/mecanum_drive_controller/src/odometry.cpp b/mecanum_drive_controller/src/odometry.cpp index 32226b9165..cfd80668ae 100644 --- a/mecanum_drive_controller/src/odometry.cpp +++ b/mecanum_drive_controller/src/odometry.cpp @@ -93,8 +93,8 @@ bool Odometry::update( velocity_in_base_frame_angular_z = velocity_in_center_frame_angular_z; /// Integration. - /// NOTE: the position is expressed in the odometry frame (frame b0), unlike the twist which is - /// expressed in the body frame (frame b). + /// NOTE: the position is expressed in the odometry frame , unlike the twist which is + /// expressed in the body frame. orientation_z_in_base_frame_ += velocity_in_base_frame_angular_z * dt; tf2::Quaternion orientation_R_b_odom; diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp index 9132c03f0b..a4e45989be 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp @@ -70,7 +70,7 @@ TEST_F(MecanumDriveControllerTest, all_parameters_set_configure_success) } // checking if all interfaces, command, state and reference are exported as expected -TEST_F(MecanumDriveControllerTest, check_exported_intefaces) +TEST_F(MecanumDriveControllerTest, check_exported_interfaces) { SetUpController(); @@ -436,7 +436,7 @@ TEST_F(MecanumDriveControllerTest, test_update_logic_not_chainable) } } -//Test that checks the status of chainable mode and update methods logic accordingly, when ref_timeout and within ref_timeout +//Test that checks the status of chainable mode and update_and_write_commands() method logic accordingly, when ref_timeout and within ref_timeout TEST_F(MecanumDriveControllerTest, test_update_logic_chainable) { // 1. age>ref_timeout 2. age Date: Tue, 24 Jan 2023 12:19:32 +0100 Subject: [PATCH 45/91] add comment --- .../test/test_mecanum_drive_controller_preceeding.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller_preceeding.cpp b/mecanum_drive_controller/test/test_mecanum_drive_controller_preceeding.cpp index ae261004f2..5fca426813 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller_preceeding.cpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller_preceeding.cpp @@ -29,6 +29,7 @@ class MecanumDriveControllerTest { }; +// checking if all parameters are initialized and set as expected TEST_F(MecanumDriveControllerTest, all_parameters_set_configure_success) { SetUpController(); @@ -48,6 +49,7 @@ TEST_F(MecanumDriveControllerTest, all_parameters_set_configure_success) ASSERT_EQ(controller_->params_.interface_name, interface_name_); } +// checking if all interfaces, command and state interfaces are exported as expected TEST_F(MecanumDriveControllerTest, check_exported_intefaces) { SetUpController(); From c6c9c973ad0f8d73e9fb18299447d0fcd72ae9db Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Fri, 27 Jan 2023 23:52:04 +0100 Subject: [PATCH 46/91] Apply suggestions from code review --- .../mecanum_drive_controller.hpp | 8 +++---- .../mecanum_drive_controller/odometry.hpp | 2 +- .../visibility_control.h | 2 +- mecanum_drive_controller/package.xml | 1 - .../src/mecanum_drive_controller.cpp | 23 ++++--------------- .../src/mecanum_drive_controller.yaml | 2 +- mecanum_drive_controller/src/odometry.cpp | 5 +--- .../test/mecanum_drive_controller_params.yaml | 5 +--- ...um_drive_controller_preceeding_params.yaml | 5 +--- .../test_load_mecanum_drive_controller.cpp | 2 +- .../test/test_mecanum_drive_controller.cpp | 2 +- .../test/test_mecanum_drive_controller.hpp | 2 +- ...st_mecanum_drive_controller_preceeding.cpp | 2 +- 13 files changed, 18 insertions(+), 43 deletions(-) diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp index 47dd1afe97..9bd260ee44 100644 --- a/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp +++ b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -127,9 +127,9 @@ class MecanumDriveController : public controller_interface::ChainableControllerI MECANUM_DRIVE_CONTROLLER__VISIBILITY_LOCAL void reference_callback(const std::shared_ptr msg); - double velocity_in_center_frame_linear_x; // [m/s] - double velocity_in_center_frame_linear_y; // [m/s] - double velocity_in_center_frame_angular_z; // [rad/s] + double velocity_in_center_frame_linear_x_; // [m/s] + double velocity_in_center_frame_linear_y_; // [m/s] + double velocity_in_center_frame_angular_z_; // [rad/s] }; } // namespace mecanum_drive_controller diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp b/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp index 56157f9f69..fc04bef50e 100644 --- a/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp +++ b/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/visibility_control.h b/mecanum_drive_controller/include/mecanum_drive_controller/visibility_control.h index b9a5062612..3222b2fa52 100644 --- a/mecanum_drive_controller/include/mecanum_drive_controller/visibility_control.h +++ b/mecanum_drive_controller/include/mecanum_drive_controller/visibility_control.h @@ -1,4 +1,4 @@ -// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/mecanum_drive_controller/package.xml b/mecanum_drive_controller/package.xml index bde04b5610..c989b9266f 100644 --- a/mecanum_drive_controller/package.xml +++ b/mecanum_drive_controller/package.xml @@ -36,7 +36,6 @@ ament_cmake_gmock controller_manager - hardware_interface ros2_control_test_assets diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index 5c7f8f064c..5db3f794c7 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -374,9 +374,9 @@ controller_interface::return_type MecanumDriveController::update_and_write_comma // INVERSE KINEMATICS (move robot). // Compute wheels velocities (this is the actual ik): // NOTE: the input desired twist (from topic `~/reference`) is a body twist. - auto current_ref = *(input_ref_.readFromRT()); - const auto age_of_last_command = time - current_ref->header.stamp; - if (age_of_last_command <= ref_timeout_ || ref_timeout_ == rclcpp::Duration::from_seconds(0)) + if ( + !std::isnan(reference_interfaces_[0]) && !std::isnan(reference_interfaces_[1]) && + !std::isnan(reference_interfaces_[2])) { tf2::Quaternion quaternion; quaternion.setRPY(0.0, 0.0, params_.kinematics.base_frame_offset.theta); @@ -417,32 +417,17 @@ controller_interface::return_type MecanumDriveController::update_and_write_comma params_.kinematics.wheels_k * velocity_in_center_frame_angular_z); // Set wheels velocities: - command_interfaces_[0].set_value(w_front_left_vel); - fprintf(stderr, " command_interfaces_[0] = %f \n", w_front_left_vel); command_interfaces_[1].set_value(w_back_left_vel); - fprintf(stderr, " command_interfaces_[1] = %f \n", w_back_left_vel); command_interfaces_[2].set_value(w_back_right_vel); - fprintf(stderr, " command_interfaces_[2] = %f \n", w_back_right_vel); command_interfaces_[3].set_value(w_front_right_vel); - fprintf(stderr, " command_interfaces_[3] = %f \n", w_front_right_vel); } else { - reference_interfaces_[0] = std::numeric_limits::quiet_NaN(); - reference_interfaces_[1] = std::numeric_limits::quiet_NaN(); - reference_interfaces_[2] = std::numeric_limits::quiet_NaN(); - command_interfaces_[0].set_value(0.0); - // left intentionally for debug - // fprintf(stderr, " command_interfaces_[0] = %f \n", command_interfaces_[0].get_value()); command_interfaces_[1].set_value(0.0); command_interfaces_[2].set_value(0.0); command_interfaces_[3].set_value(0.0); - - current_ref->twist.linear.x = std::numeric_limits::quiet_NaN(); - current_ref->twist.linear.y = std::numeric_limits::quiet_NaN(); - current_ref->twist.angular.z = std::numeric_limits::quiet_NaN(); } // Publish odometry message diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.yaml b/mecanum_drive_controller/src/mecanum_drive_controller.yaml index 9dd6f55f2f..ad4e7860e6 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.yaml +++ b/mecanum_drive_controller/src/mecanum_drive_controller.yaml @@ -32,7 +32,7 @@ mecanum_drive_controller: interface_name: { type: string, default_value: "", - description: "Name of the interface used by the controller on joints and command_joints.", + description: "Name of the interface used by the controller for sending commands, reading states and getting references.", read_only: true, } diff --git a/mecanum_drive_controller/src/odometry.cpp b/mecanum_drive_controller/src/odometry.cpp index cfd80668ae..e1b0c4778c 100644 --- a/mecanum_drive_controller/src/odometry.cpp +++ b/mecanum_drive_controller/src/odometry.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -107,9 +107,6 @@ bool Odometry::update( position_x_in_base_frame_ += velocity_in_base_frame_w_r_t_odom_frame_.x() * dt; position_y_in_base_frame_ += velocity_in_base_frame_w_r_t_odom_frame_.y() * dt; - // leaving the below comments intentionally - // fprintf(stderr, " position_x_in_base_frame_ = %f \n", position_x_in_base_frame_); - // fprintf(stderr, " position_y_in_base_frame_ = %f \n", position_y_in_base_frame_); return true; } diff --git a/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml b/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml index ae99da685f..b8a1307234 100644 --- a/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml +++ b/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml @@ -10,10 +10,7 @@ test_mecanum_drive_controller: interface_name: velocity kinematics: - base_frame_offset: - x: 0.0 - y: 0.0 - theta: 0.0 + base_frame_offset: { x: 0.0, y: 0.0, theta: 0.0 } wheels_radius: 0.5 diff --git a/mecanum_drive_controller/test/mecanum_drive_controller_preceeding_params.yaml b/mecanum_drive_controller/test/mecanum_drive_controller_preceeding_params.yaml index 7517c29441..ee1c7ffca0 100644 --- a/mecanum_drive_controller/test/mecanum_drive_controller_preceeding_params.yaml +++ b/mecanum_drive_controller/test/mecanum_drive_controller_preceeding_params.yaml @@ -10,10 +10,7 @@ test_mecanum_drive_controller: interface_name: velocity kinematics: - base_frame_offset: - x: 0.0 - y: 0.0 - theta: 0.0 + base_frame_offset: { x: 0.0, y: 0.0, theta: 0.0 } wheels_radius: 0.5 diff --git a/mecanum_drive_controller/test/test_load_mecanum_drive_controller.cpp b/mecanum_drive_controller/test/test_load_mecanum_drive_controller.cpp index ab454ba236..a6de9504a8 100644 --- a/mecanum_drive_controller/test/test_load_mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/test/test_load_mecanum_drive_controller.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp index a4e45989be..76078fbc17 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp index f0ebea668b..f13a3bd72c 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller_preceeding.cpp b/mecanum_drive_controller/test/test_mecanum_drive_controller_preceeding.cpp index 5fca426813..a16ae4db16 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller_preceeding.cpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller_preceeding.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. From 0e4879abeece19f79878d5b90661e44ac828d0ce Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Fri, 27 Jan 2023 23:59:39 +0100 Subject: [PATCH 47/91] Update mecanum_drive_controller/src/mecanum_drive_controller.cpp --- mecanum_drive_controller/src/mecanum_drive_controller.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index 5db3f794c7..53df6d393f 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -475,7 +475,6 @@ controller_interface::return_type MecanumDriveController::update_and_write_comma controller_state_publisher_->msg_.reference_vel.linear.x = reference_interfaces_[0]; controller_state_publisher_->msg_.reference_vel.linear.y = reference_interfaces_[1]; controller_state_publisher_->msg_.reference_vel.angular.z = reference_interfaces_[2]; - controller_state_publisher_->unlockAndPublish(); } From 619118ce463168dbc7740214b524d5ee290bc695 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Fri, 27 Jan 2023 23:59:56 +0100 Subject: [PATCH 48/91] Update mecanum_drive_controller/src/mecanum_drive_controller.cpp --- mecanum_drive_controller/src/mecanum_drive_controller.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index 53df6d393f..734007abbe 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -478,6 +478,10 @@ controller_interface::return_type MecanumDriveController::update_and_write_comma controller_state_publisher_->unlockAndPublish(); } + reference_interfaces_[0] = std::numeric_limits::quiet_NaN(); + reference_interfaces_[1] = std::numeric_limits::quiet_NaN(); + reference_interfaces_[2] = std::numeric_limits::quiet_NaN(); + return controller_interface::return_type::OK; } From c26cb64f1ec8a4018b1132232e851898768743d1 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Sat, 28 Jan 2023 00:02:37 +0100 Subject: [PATCH 49/91] Update .pre-commit-config.yaml --- .pre-commit-config.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index c75acbd76d..bea2ea0374 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -116,7 +116,7 @@ repos: exclude: CHANGELOG\.rst$ - repo: https://github.com/pre-commit/pygrep-hooks - rev: v1.10.0 + rev: v1.9.0 hooks: - id: rst-backticks exclude: CHANGELOG\.rst$ From 28036028a1d6e207e74b7283301ab5168a838bf4 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Sat, 28 Jan 2023 00:03:08 +0100 Subject: [PATCH 50/91] Update package.xml --- ros2_controllers/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 278ecb0b1a..4a014c610b 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -22,6 +22,7 @@ imu_sensor_broadcaster joint_state_broadcaster joint_trajectory_controller + mecanum_drive_controller pid_controller position_controllers range_sensor_broadcaster From 6897de8911d98d0dafbb0f9977276236652bf7f8 Mon Sep 17 00:00:00 2001 From: GiridharBukka Date: Mon, 30 Jan 2023 13:31:40 +0100 Subject: [PATCH 51/91] removing boost --- mecanum_drive_controller/src/odometry.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/mecanum_drive_controller/src/odometry.cpp b/mecanum_drive_controller/src/odometry.cpp index e1b0c4778c..49b97db319 100644 --- a/mecanum_drive_controller/src/odometry.cpp +++ b/mecanum_drive_controller/src/odometry.cpp @@ -17,7 +17,6 @@ #include "tf2/transform_datatypes.h" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" -#include using namespace std; namespace mecanum_drive_controller { From ef205a2909a4fea40b6ced5750d4e34dcb5e5dfa Mon Sep 17 00:00:00 2001 From: GiridharBukka Date: Mon, 30 Jan 2023 13:32:38 +0100 Subject: [PATCH 52/91] removing unused variable --- .../include/mecanum_drive_controller/odometry.hpp | 3 +-- mecanum_drive_controller/src/odometry.cpp | 2 +- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp b/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp index fc04bef50e..c953ee095e 100644 --- a/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp +++ b/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp @@ -34,8 +34,7 @@ class Odometry /// \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 - Odometry(size_t velocity_rolling_window_size = 10); + Odometry(); /// \brief Initialize the odometry /// \param time Current time diff --git a/mecanum_drive_controller/src/odometry.cpp b/mecanum_drive_controller/src/odometry.cpp index 49b97db319..6f4d24fdde 100644 --- a/mecanum_drive_controller/src/odometry.cpp +++ b/mecanum_drive_controller/src/odometry.cpp @@ -20,7 +20,7 @@ using namespace std; namespace mecanum_drive_controller { -Odometry::Odometry(size_t velocity_rolling_window_size) +Odometry::Odometry() : timestamp_(0.0), position_x_in_base_frame_(0.0), position_y_in_base_frame_(0.0), From 9cd3d11a391c14b3dc0dcd640da110f64e0583fd Mon Sep 17 00:00:00 2001 From: GiridharBukka Date: Mon, 30 Jan 2023 13:34:31 +0100 Subject: [PATCH 53/91] addressing warnings --- .../src/mecanum_drive_controller.cpp | 24 +++++++++---------- 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index 734007abbe..fec1386c31 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -87,7 +87,7 @@ controller_interface::CallbackReturn MecanumDriveController::on_configure( { RCLCPP_FATAL( get_node()->get_logger(), - "Size of 'joints' (%d) and 'state_joint_names' (%d) parameters has " + "Size of 'joints' (%ld) and 'state_joint_names' (%ld) parameters has " "to be the same!", params_.joint_names.size(), state_joint_names_.size()); return CallbackReturn::FAILURE; @@ -391,30 +391,30 @@ controller_interface::return_type MecanumDriveController::update_and_write_comma tf2::Vector3 linear_trans_from_base_to_center = tf2::Vector3( params_.kinematics.base_frame_offset.x, params_.kinematics.base_frame_offset.y, 0.0); - velocity_in_center_frame_linear_x = + velocity_in_center_frame_linear_x_ = velocity_in_base_frame_w_r_t_center_frame_.x() + linear_trans_from_base_to_center.y() * reference_interfaces_[2]; - velocity_in_center_frame_linear_y = + velocity_in_center_frame_linear_y_ = velocity_in_base_frame_w_r_t_center_frame_.y() - linear_trans_from_base_to_center.x() * reference_interfaces_[2]; - velocity_in_center_frame_angular_z = reference_interfaces_[2]; + velocity_in_center_frame_angular_z_ = reference_interfaces_[2]; double w_front_left_vel = 1.0 / params_.kinematics.wheels_radius * - (velocity_in_center_frame_linear_x - velocity_in_center_frame_linear_y - - params_.kinematics.wheels_k * velocity_in_center_frame_angular_z); + (velocity_in_center_frame_linear_x_ - velocity_in_center_frame_linear_y_ - + params_.kinematics.wheels_k * velocity_in_center_frame_angular_z_); double w_back_left_vel = 1.0 / params_.kinematics.wheels_radius * - (velocity_in_center_frame_linear_x + velocity_in_center_frame_linear_y - - params_.kinematics.wheels_k * velocity_in_center_frame_angular_z); + (velocity_in_center_frame_linear_x_ + velocity_in_center_frame_linear_y_ - + params_.kinematics.wheels_k * velocity_in_center_frame_angular_z_); double w_back_right_vel = 1.0 / params_.kinematics.wheels_radius * - (velocity_in_center_frame_linear_x - velocity_in_center_frame_linear_y + - params_.kinematics.wheels_k * velocity_in_center_frame_angular_z); + (velocity_in_center_frame_linear_x_ - velocity_in_center_frame_linear_y_ + + params_.kinematics.wheels_k * velocity_in_center_frame_angular_z_); double w_front_right_vel = 1.0 / params_.kinematics.wheels_radius * - (velocity_in_center_frame_linear_x + velocity_in_center_frame_linear_y + - params_.kinematics.wheels_k * velocity_in_center_frame_angular_z); + (velocity_in_center_frame_linear_x_ + velocity_in_center_frame_linear_y_ + + params_.kinematics.wheels_k * velocity_in_center_frame_angular_z_); // Set wheels velocities: command_interfaces_[0].set_value(w_front_left_vel); From f9fdf305ae19f97a768aa540d092bfd64007ab0a Mon Sep 17 00:00:00 2001 From: GiridharBukka Date: Mon, 30 Jan 2023 13:52:31 +0100 Subject: [PATCH 54/91] update cmake version --- mecanum_drive_controller/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mecanum_drive_controller/CMakeLists.txt b/mecanum_drive_controller/CMakeLists.txt index 925183f260..d167d9c673 100644 --- a/mecanum_drive_controller/CMakeLists.txt +++ b/mecanum_drive_controller/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.8) +cmake_minimum_required(VERSION 3.16) project(mecanum_drive_controller) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") From b73cf7bafd31c6836ac882c5758da061ae2e2804 Mon Sep 17 00:00:00 2001 From: GiridharBukka Date: Mon, 30 Jan 2023 14:03:16 +0100 Subject: [PATCH 55/91] renaming wheels_k to more descriptive naming sum_of_robot_center_projection_on_X_Y_axis --- .../include/mecanum_drive_controller/odometry.hpp | 8 ++++---- .../src/mecanum_drive_controller.cpp | 10 +++++----- .../src/mecanum_drive_controller.yaml | 4 ++-- mecanum_drive_controller/src/odometry.cpp | 8 ++++---- .../test/mecanum_drive_controller_params.yaml | 2 +- .../mecanum_drive_controller_preceeding_params.yaml | 2 +- .../test/test_mecanum_drive_controller.cpp | 12 ++++++------ 7 files changed, 23 insertions(+), 23 deletions(-) diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp b/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp index c953ee095e..3ddc8c67f1 100644 --- a/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp +++ b/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp @@ -68,9 +68,9 @@ class Odometry } /// \brief Sets the wheels parameters: mecanum geometric param and radius - /// \param wheels_k Wheels geometric param (used in mecanum wheels' ik) [m] + /// \param sum_of_robot_center_projection_on_X_Y_axis Wheels geometric param (used in mecanum wheels' ik) [m] /// \param wheels_radius Wheels radius [m] - void setWheelsParams(double wheels_k, double wheels_radius); + void setWheelsParams(double sum_of_robot_center_projection_on_X_Y_axis, double wheels_radius); private: /// Current timestamp: @@ -90,8 +90,8 @@ class Odometry /// Wheels kinematic parameters [m]: /// lx and ly represent the distance from the robot's center to the wheels projected on - /// the x and y axis with origin at robots center respectively, wheels_k_ = lx+ly - double wheels_k_; + /// the x and y axis with origin at robots center respectively, sum_of_robot_center_projection_on_X_Y_axis_ = lx+ly + double sum_of_robot_center_projection_on_X_Y_axis_; double wheels_radius_; // [m] }; diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index fec1386c31..31072a0d98 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -96,7 +96,7 @@ controller_interface::CallbackReturn MecanumDriveController::on_configure( reference_names_ = params_.reference_names; // Set wheel params for the odometry computation - odometry_.setWheelsParams(params_.kinematics.wheels_k, params_.kinematics.wheels_radius); + odometry_.setWheelsParams(params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis, params_.kinematics.wheels_radius); // topics QoS auto subscribers_qos = rclcpp::SystemDefaultsQoS(); @@ -402,19 +402,19 @@ controller_interface::return_type MecanumDriveController::update_and_write_comma double w_front_left_vel = 1.0 / params_.kinematics.wheels_radius * (velocity_in_center_frame_linear_x_ - velocity_in_center_frame_linear_y_ - - params_.kinematics.wheels_k * velocity_in_center_frame_angular_z_); + params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis * velocity_in_center_frame_angular_z_); double w_back_left_vel = 1.0 / params_.kinematics.wheels_radius * (velocity_in_center_frame_linear_x_ + velocity_in_center_frame_linear_y_ - - params_.kinematics.wheels_k * velocity_in_center_frame_angular_z_); + params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis * velocity_in_center_frame_angular_z_); double w_back_right_vel = 1.0 / params_.kinematics.wheels_radius * (velocity_in_center_frame_linear_x_ - velocity_in_center_frame_linear_y_ + - params_.kinematics.wheels_k * velocity_in_center_frame_angular_z_); + params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis * velocity_in_center_frame_angular_z_); double w_front_right_vel = 1.0 / params_.kinematics.wheels_radius * (velocity_in_center_frame_linear_x_ + velocity_in_center_frame_linear_y_ + - params_.kinematics.wheels_k * velocity_in_center_frame_angular_z_); + params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis * velocity_in_center_frame_angular_z_); // Set wheels velocities: command_interfaces_[0].set_value(w_front_left_vel); diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.yaml b/mecanum_drive_controller/src/mecanum_drive_controller.yaml index ad4e7860e6..3d606a7616 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.yaml +++ b/mecanum_drive_controller/src/mecanum_drive_controller.yaml @@ -65,11 +65,11 @@ mecanum_drive_controller: } - wheels_k: { + sum_of_robot_center_projection_on_X_Y_axis: { type: double, default_value: 0.0, description: "Wheels geometric param used in mecanum wheels' IK. lx and ly represent the distance from the robot's center to the wheels projected on - the x and y axis with origin at robots center respectively, wheels_k = lx+ly", + the x and y axis with origin at robots center respectively, sum_of_robot_center_projection_on_X_Y_axis = lx+ly", read_only: false, } diff --git a/mecanum_drive_controller/src/odometry.cpp b/mecanum_drive_controller/src/odometry.cpp index 6f4d24fdde..86fbf2705e 100644 --- a/mecanum_drive_controller/src/odometry.cpp +++ b/mecanum_drive_controller/src/odometry.cpp @@ -28,7 +28,7 @@ Odometry::Odometry() velocity_in_base_frame_linear_x(0.0), velocity_in_base_frame_linear_y(0.0), velocity_in_base_frame_angular_z(0.0), - wheels_k_(0.0), + sum_of_robot_center_projection_on_X_Y_axis_(0.0), wheels_radius_(0.0) { } @@ -70,7 +70,7 @@ bool Odometry::update( double velocity_in_center_frame_linear_y = 0.25 * wheels_radius_ * (-wheel_front_left_vel + wheel_back_left_vel - wheel_back_right_vel + wheel_front_right_vel); double velocity_in_center_frame_angular_z = - 0.25 * wheels_radius_ / wheels_k_ * (-wheel_front_left_vel - wheel_back_left_vel + wheel_back_right_vel + wheel_front_right_vel); + 0.25 * wheels_radius_ / sum_of_robot_center_projection_on_X_Y_axis_ * (-wheel_front_left_vel - wheel_back_left_vel + wheel_back_right_vel + wheel_front_right_vel); tf2::Quaternion orientation_R_c_b; orientation_R_c_b.setRPY(0.0, 0.0, -base_frame_offset_[2]); @@ -110,9 +110,9 @@ bool Odometry::update( return true; } -void Odometry::setWheelsParams(double wheels_k, double wheels_radius) +void Odometry::setWheelsParams(double sum_of_robot_center_projection_on_X_Y_axis, double wheels_radius) { - wheels_k_ = wheels_k; + sum_of_robot_center_projection_on_X_Y_axis_ = sum_of_robot_center_projection_on_X_Y_axis; wheels_radius_ = wheels_radius; } diff --git a/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml b/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml index b8a1307234..23f5e4dea1 100644 --- a/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml +++ b/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml @@ -14,7 +14,7 @@ test_mecanum_drive_controller: wheels_radius: 0.5 - wheels_k: 1.0 + sum_of_robot_center_projection_on_X_Y_axis: 1.0 base_frame_id: "base_link" diff --git a/mecanum_drive_controller/test/mecanum_drive_controller_preceeding_params.yaml b/mecanum_drive_controller/test/mecanum_drive_controller_preceeding_params.yaml index ee1c7ffca0..08af04a090 100644 --- a/mecanum_drive_controller/test/mecanum_drive_controller_preceeding_params.yaml +++ b/mecanum_drive_controller/test/mecanum_drive_controller_preceeding_params.yaml @@ -14,7 +14,7 @@ test_mecanum_drive_controller: wheels_radius: 0.5 - wheels_k: 1.0 + sum_of_robot_center_projection_on_X_Y_axis: 1.0 base_frame_id: "base_link" diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp index 76078fbc17..9eaabfc5e7 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp @@ -44,7 +44,7 @@ TEST_F(MecanumDriveControllerTest, all_parameters_set_configure_success) ASSERT_EQ(controller_->params_.reference_timeout, 0.0); ASSERT_EQ(controller_->params_.kinematics.wheels_radius, 0.0); - ASSERT_EQ(controller_->params_.kinematics.wheels_k, 0.0); + ASSERT_EQ(controller_->params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis, 0.0); ASSERT_EQ(controller_->params_.kinematics.base_frame_offset.x, 0.0); ASSERT_EQ(controller_->params_.kinematics.base_frame_offset.y, 0.0); @@ -58,7 +58,7 @@ TEST_F(MecanumDriveControllerTest, all_parameters_set_configure_success) ASSERT_EQ(controller_->params_.reference_timeout, 0.1); ASSERT_EQ(controller_->params_.kinematics.wheels_radius, 0.5); - ASSERT_EQ(controller_->params_.kinematics.wheels_k, 1.0); + ASSERT_EQ(controller_->params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis, 1.0); ASSERT_EQ(controller_->params_.kinematics.base_frame_offset.x, 0.0); ASSERT_EQ(controller_->params_.kinematics.base_frame_offset.y, 0.0); @@ -241,7 +241,7 @@ TEST_F(MecanumDriveControllerTest, receive_message_and_publish_updated_status) controller_->update_and_write_commands( controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - // w0_vel = 1.0 / params_.kinematics.wheels_radius * (body_velocity_center_frame_.linear_x - body_velocity_center_frame_.linear_y - params_.kinematics.wheels_k * body_velocity_center_frame_.angular_z); + // w0_vel = 1.0 / params_.kinematics.wheels_radius * (body_velocity_center_frame_.linear_x - body_velocity_center_frame_.linear_y - params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis * body_velocity_center_frame_.angular_z); // joint_command_values_[1] = 1.0 / 0.5 * (1.5 - 0.0 - 1 * 0.0) EXPECT_EQ(joint_command_values_[1], 3.0); @@ -422,7 +422,7 @@ TEST_F(MecanumDriveControllerTest, test_update_logic_not_chainable) controller_interface::return_type::OK); EXPECT_NE(joint_command_values_[1], command_lin_x); - // w0_vel = 1.0 / params_.kinematics.wheels_radius * (body_velocity_center_frame_.linear_x - body_velocity_center_frame_.linear_y - params_.kinematics.wheels_k * body_velocity_center_frame_.angular_z); + // w0_vel = 1.0 / params_.kinematics.wheels_radius * (body_velocity_center_frame_.linear_x - body_velocity_center_frame_.linear_y - params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis * body_velocity_center_frame_.angular_z); // joint_command_values_[1] = 1.0 / 0.5 * (1.5 - 0.0 - 1 * 0.0) EXPECT_EQ(joint_command_values_[1], 3.0); ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); @@ -474,7 +474,7 @@ TEST_F(MecanumDriveControllerTest, test_update_logic_chainable) controller_interface::return_type::OK); EXPECT_NE(joint_command_values_[1], command_lin_x); - // w0_vel = 1.0 / params_.kinematics.wheels_radius * (body_velocity_center_frame_.linear_x - body_velocity_center_frame_.linear_y - params_.kinematics.wheels_k * body_velocity_center_frame_.angular_z); + // w0_vel = 1.0 / params_.kinematics.wheels_radius * (body_velocity_center_frame_.linear_x - body_velocity_center_frame_.linear_y - params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis * body_velocity_center_frame_.angular_z); // joint_command_values_[1] = 1.0 / 0.5 * (1.5 - 0.0 - 1 * 0.0) EXPECT_EQ(joint_command_values_[1], 3.0); for (const auto & interface : controller_->reference_interfaces_) @@ -531,7 +531,7 @@ TEST_F(MecanumDriveControllerTest, test_ref_timeout_zero_for_update) EXPECT_FALSE(std::isnan(joint_command_values_[1])); EXPECT_NE(joint_command_values_[1], command_lin_x); - // w0_vel = 1.0 / params_.kinematics.wheels_radius * (body_velocity_center_frame_.linear_x - body_velocity_center_frame_.linear_y - params_.kinematics.wheels_k * body_velocity_center_frame_.angular_z); + // w0_vel = 1.0 / params_.kinematics.wheels_radius * (body_velocity_center_frame_.linear_x - body_velocity_center_frame_.linear_y - params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis * body_velocity_center_frame_.angular_z); // joint_command_values_[1] = 1.0 / 0.5 * (1.5 - 0.0 - 1 * 0.0) EXPECT_EQ(joint_command_values_[1], 3.0); ASSERT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x)); From 8b92a479560cf0be9111f24c476fcdef3eab813b Mon Sep 17 00:00:00 2001 From: GiridharBukka Date: Mon, 30 Jan 2023 14:16:08 +0100 Subject: [PATCH 56/91] renames joint_names to command_joint_names --- .../src/mecanum_drive_controller.cpp | 10 +++++----- .../src/mecanum_drive_controller.yaml | 2 +- .../test/mecanum_drive_controller_params.yaml | 2 +- .../mecanum_drive_controller_preceeding_params.yaml | 2 +- .../test/test_mecanum_drive_controller.cpp | 8 ++++---- .../test/test_mecanum_drive_controller.hpp | 6 +++--- .../test/test_mecanum_drive_controller_preceeding.cpp | 6 +++--- 7 files changed, 18 insertions(+), 18 deletions(-) diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index 31072a0d98..ecb7b6602f 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -80,16 +80,16 @@ controller_interface::CallbackReturn MecanumDriveController::on_configure( } else { - state_joint_names_ = params_.joint_names; + state_joint_names_ = params_.command_joint_names; } - if (params_.joint_names.size() != state_joint_names_.size()) + if (params_.command_joint_names.size() != state_joint_names_.size()) { RCLCPP_FATAL( get_node()->get_logger(), "Size of 'joints' (%ld) and 'state_joint_names' (%ld) parameters has " "to be the same!", - params_.joint_names.size(), state_joint_names_.size()); + params_.command_joint_names.size(), state_joint_names_.size()); return CallbackReturn::FAILURE; } @@ -239,8 +239,8 @@ MecanumDriveController::command_interface_configuration() const controller_interface::InterfaceConfiguration command_interfaces_config; command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; - command_interfaces_config.names.reserve(params_.joint_names.size()); - for (const auto & joint : params_.joint_names) + command_interfaces_config.names.reserve(params_.command_joint_names.size()); + for (const auto & joint : params_.command_joint_names) { command_interfaces_config.names.push_back(joint + "/" + params_.interface_name); } diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.yaml b/mecanum_drive_controller/src/mecanum_drive_controller.yaml index 3d606a7616..f4e61edef8 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.yaml +++ b/mecanum_drive_controller/src/mecanum_drive_controller.yaml @@ -6,7 +6,7 @@ mecanum_drive_controller: } - joint_names: { + command_joint_names: { type: string_array, default_value: [], description: "Name of the wheels joints.", diff --git a/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml b/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml index 23f5e4dea1..9dcc156b84 100644 --- a/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml +++ b/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml @@ -3,7 +3,7 @@ test_mecanum_drive_controller: reference_timeout: 0.1 - joint_names: ["front_left_wheel_joint", "back_left_wheel_joint", "back_right_wheel_joint", "front_right_wheel_joint"] + command_joint_names: ["front_left_wheel_joint", "back_left_wheel_joint", "back_right_wheel_joint", "front_right_wheel_joint"] reference_names: ["linear_x", "linear_y", "angular_z"] diff --git a/mecanum_drive_controller/test/mecanum_drive_controller_preceeding_params.yaml b/mecanum_drive_controller/test/mecanum_drive_controller_preceeding_params.yaml index 08af04a090..d4dc2d9ca2 100644 --- a/mecanum_drive_controller/test/mecanum_drive_controller_preceeding_params.yaml +++ b/mecanum_drive_controller/test/mecanum_drive_controller_preceeding_params.yaml @@ -3,7 +3,7 @@ test_mecanum_drive_controller: reference_timeout: 0.1 - joint_names: ["front_left_wheel_joint", "back_left_wheel_joint", "back_right_wheel_joint", "front_right_wheel_joint"] + command_joint_names: ["front_left_wheel_joint", "back_left_wheel_joint", "back_right_wheel_joint", "front_right_wheel_joint"] state_joint_names: ["state_front_left_wheel_joint", "state_back_left_wheel_joint", "state_back_right_wheel_joint", "state_front_right_wheel_joint"] diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp index 9eaabfc5e7..8ac1f46dd2 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp @@ -50,7 +50,7 @@ TEST_F(MecanumDriveControllerTest, all_parameters_set_configure_success) ASSERT_EQ(controller_->params_.kinematics.base_frame_offset.y, 0.0); ASSERT_EQ(controller_->params_.kinematics.base_frame_offset.theta, 0.0); - ASSERT_TRUE(controller_->params_.joint_names.empty()); + ASSERT_TRUE(controller_->params_.command_joint_names.empty()); ASSERT_TRUE(controller_->params_.state_joint_names.empty()); ASSERT_TRUE(controller_->params_.interface_name.empty()); @@ -64,7 +64,7 @@ TEST_F(MecanumDriveControllerTest, all_parameters_set_configure_success) ASSERT_EQ(controller_->params_.kinematics.base_frame_offset.y, 0.0); ASSERT_EQ(controller_->params_.kinematics.base_frame_offset.theta, 0.0); - ASSERT_THAT(controller_->params_.joint_names, testing::ElementsAreArray(joint_names_)); + ASSERT_THAT(controller_->params_.command_joint_names, testing::ElementsAreArray(command_joint_names_)); ASSERT_TRUE(controller_->params_.state_joint_names.empty()); ASSERT_EQ(controller_->params_.interface_name, interface_name_); } @@ -81,7 +81,7 @@ TEST_F(MecanumDriveControllerTest, check_exported_interfaces) ASSERT_EQ(command_intefaces.names.size(), joint_command_values_.size()); for (size_t i = 0; i < command_intefaces.names.size(); ++i) { - EXPECT_EQ(command_intefaces.names[i], joint_names_[i] + "/" + interface_name_); + EXPECT_EQ(command_intefaces.names[i], command_joint_names_[i] + "/" + interface_name_); } // check state itfs configuration @@ -89,7 +89,7 @@ TEST_F(MecanumDriveControllerTest, check_exported_interfaces) ASSERT_EQ(state_intefaces.names.size(), joint_state_values_.size()); for (size_t i = 0; i < state_intefaces.names.size(); ++i) { - EXPECT_EQ(state_intefaces.names[i], joint_names_[i] + "/" + interface_name_); + EXPECT_EQ(state_intefaces.names[i], command_joint_names_[i] + "/" + interface_name_); } // check ref itfs configuration, reference_names_ diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp index f13a3bd72c..fdb3169508 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp @@ -156,7 +156,7 @@ class MecanumDriveControllerFixture : public ::testing::Test for (size_t i = 0; i < joint_command_values_.size(); ++i) { command_itfs_.emplace_back(hardware_interface::CommandInterface( - joint_names_[i], interface_name_, &joint_command_values_[i])); + command_joint_names_[i], interface_name_, &joint_command_values_[i])); command_ifs.emplace_back(command_itfs_.back()); } @@ -167,7 +167,7 @@ class MecanumDriveControllerFixture : public ::testing::Test for (size_t i = 0; i < joint_state_values_.size(); ++i) { state_itfs_.emplace_back(hardware_interface::StateInterface( - joint_names_[i], interface_name_, &joint_state_values_[i])); + command_joint_names_[i], interface_name_, &joint_state_values_[i])); state_ifs.emplace_back(state_itfs_.back()); } @@ -251,7 +251,7 @@ class MecanumDriveControllerFixture : public ::testing::Test protected: std::vector reference_names_ = {"linear_x", "linear_y", "angular_z"}; - std::vector joint_names_ = { + std::vector command_joint_names_ = { "front_left_wheel_joint", "back_left_wheel_joint", "back_right_wheel_joint", "front_right_wheel_joint"}; diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller_preceeding.cpp b/mecanum_drive_controller/test/test_mecanum_drive_controller_preceeding.cpp index a16ae4db16..c32d6ebbe9 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller_preceeding.cpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller_preceeding.cpp @@ -35,14 +35,14 @@ TEST_F(MecanumDriveControllerTest, all_parameters_set_configure_success) SetUpController(); ASSERT_EQ(controller_->params_.reference_timeout, 0.0); - ASSERT_TRUE(controller_->params_.joint_names.empty()); + ASSERT_TRUE(controller_->params_.command_joint_names.empty()); ASSERT_TRUE(controller_->params_.state_joint_names.empty()); ASSERT_TRUE(controller_->params_.interface_name.empty()); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(controller_->params_.reference_timeout, 0.1); - ASSERT_THAT(controller_->params_.joint_names, testing::ElementsAreArray(joint_names_)); + ASSERT_THAT(controller_->params_.command_joint_names, testing::ElementsAreArray(command_joint_names_)); ASSERT_THAT( controller_->params_.state_joint_names, testing::ElementsAreArray(state_joint_names_)); ASSERT_THAT(controller_->state_joint_names_, testing::ElementsAreArray(state_joint_names_)); @@ -60,7 +60,7 @@ TEST_F(MecanumDriveControllerTest, check_exported_intefaces) ASSERT_EQ(command_intefaces.names.size(), joint_command_values_.size()); for (size_t i = 0; i < command_intefaces.names.size(); ++i) { - EXPECT_EQ(command_intefaces.names[i], joint_names_[i] + "/" + interface_name_); + EXPECT_EQ(command_intefaces.names[i],command_joint_names_[i] + "/" + interface_name_); } auto state_intefaces = controller_->state_interface_configuration(); From 0d363ea0dddfaba8efb1cf3e4c32fc771b4aeebb Mon Sep 17 00:00:00 2001 From: GiridharBukka Date: Mon, 30 Jan 2023 14:17:49 +0100 Subject: [PATCH 57/91] removing unused variable --- .../src/mecanum_drive_controller.yaml | 8 -------- .../test/mecanum_drive_controller_params.yaml | 2 -- .../test/mecanum_drive_controller_preceeding_params.yaml | 2 -- 3 files changed, 12 deletions(-) diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.yaml b/mecanum_drive_controller/src/mecanum_drive_controller.yaml index f4e61edef8..47dbae58f6 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.yaml +++ b/mecanum_drive_controller/src/mecanum_drive_controller.yaml @@ -96,14 +96,6 @@ mecanum_drive_controller: } - use_realigned_roller_joints: { - type: bool, - default_value: false, - description: "This mode is used when the mecanum wheels are simulated and we use realigned rollers to mimic mecanum wheels.", - read_only: false, - - } - twist_covariance_diagonal: { type: double_array, default_value: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1], diff --git a/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml b/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml index 9dcc156b84..88bc9ddb86 100644 --- a/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml +++ b/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml @@ -22,8 +22,6 @@ test_mecanum_drive_controller: enable_odom_tf: true - use_realigned_roller_joints: false - twist_covariance_diagonal: [0.0, 7.0, 14.0, 21.0, 28.0, 35.0] pose_covariance_diagonal: [0.0, 7.0, 14.0, 21.0, 28.0, 35.0] diff --git a/mecanum_drive_controller/test/mecanum_drive_controller_preceeding_params.yaml b/mecanum_drive_controller/test/mecanum_drive_controller_preceeding_params.yaml index d4dc2d9ca2..459a515eb1 100644 --- a/mecanum_drive_controller/test/mecanum_drive_controller_preceeding_params.yaml +++ b/mecanum_drive_controller/test/mecanum_drive_controller_preceeding_params.yaml @@ -22,8 +22,6 @@ test_mecanum_drive_controller: enable_odom_tf: true - use_realigned_roller_joints: false - twist_covariance_diagonal: [0.0, 7.0, 14.0, 21.0, 28.0, 35.0] pose_covariance_diagonal: [0.0, 7.0, 14.0, 21.0, 28.0, 35.0] From 20c91bb6603906082c796086f570c324934aaa5a Mon Sep 17 00:00:00 2001 From: GiridharBukka Date: Mon, 30 Jan 2023 14:32:31 +0100 Subject: [PATCH 58/91] pre-commit clang format 14 --- .../mecanum_drive_controller/odometry.hpp | 3 ++- .../src/mecanum_drive_controller.cpp | 16 +++++++++++----- mecanum_drive_controller/src/odometry.cpp | 15 ++++++++++----- .../test/test_mecanum_drive_controller.cpp | 7 ++++--- .../test_mecanum_drive_controller_preceeding.cpp | 5 +++-- 5 files changed, 30 insertions(+), 16 deletions(-) diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp b/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp index 3ddc8c67f1..c151f399e5 100644 --- a/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp +++ b/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp @@ -48,7 +48,8 @@ class Odometry /// \param time Current time /// \return true if the odometry is actually updated bool update( - double wheel_front_left_vel, double wheel_back_left_vel, double wheel_back_right_vel, double wheel_front_right_vel, const double dt); + double wheel_front_left_vel, double wheel_back_left_vel, double wheel_back_right_vel, + double wheel_front_right_vel, const double dt); /// \return position (x component) [m] double getX() const { return position_x_in_base_frame_; } diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index ecb7b6602f..1e2e5495a3 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -96,7 +96,9 @@ controller_interface::CallbackReturn MecanumDriveController::on_configure( reference_names_ = params_.reference_names; // Set wheel params for the odometry computation - odometry_.setWheelsParams(params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis, params_.kinematics.wheels_radius); + odometry_.setWheelsParams( + params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis, + params_.kinematics.wheels_radius); // topics QoS auto subscribers_qos = rclcpp::SystemDefaultsQoS(); @@ -402,19 +404,23 @@ controller_interface::return_type MecanumDriveController::update_and_write_comma double w_front_left_vel = 1.0 / params_.kinematics.wheels_radius * (velocity_in_center_frame_linear_x_ - velocity_in_center_frame_linear_y_ - - params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis * velocity_in_center_frame_angular_z_); + params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis * + velocity_in_center_frame_angular_z_); double w_back_left_vel = 1.0 / params_.kinematics.wheels_radius * (velocity_in_center_frame_linear_x_ + velocity_in_center_frame_linear_y_ - - params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis * velocity_in_center_frame_angular_z_); + params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis * + velocity_in_center_frame_angular_z_); double w_back_right_vel = 1.0 / params_.kinematics.wheels_radius * (velocity_in_center_frame_linear_x_ - velocity_in_center_frame_linear_y_ + - params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis * velocity_in_center_frame_angular_z_); + params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis * + velocity_in_center_frame_angular_z_); double w_front_right_vel = 1.0 / params_.kinematics.wheels_radius * (velocity_in_center_frame_linear_x_ + velocity_in_center_frame_linear_y_ + - params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis * velocity_in_center_frame_angular_z_); + params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis * + velocity_in_center_frame_angular_z_); // Set wheels velocities: command_interfaces_[0].set_value(w_front_left_vel); diff --git a/mecanum_drive_controller/src/odometry.cpp b/mecanum_drive_controller/src/odometry.cpp index 86fbf2705e..0aaadc4d3d 100644 --- a/mecanum_drive_controller/src/odometry.cpp +++ b/mecanum_drive_controller/src/odometry.cpp @@ -46,7 +46,8 @@ void Odometry::init( } bool Odometry::update( - double wheel_front_left_vel, double wheel_back_left_vel, double wheel_back_right_vel, double wheel_front_right_vel, const double dt) + double wheel_front_left_vel, double wheel_back_left_vel, double wheel_back_right_vel, + double wheel_front_right_vel, const double dt) { /// We cannot estimate the speed with very small time intervals: // const double dt = (time - timestamp_).toSec(); @@ -66,11 +67,14 @@ bool Odometry::update( /// linear_transformation_from_center_2_base: offset/linear transformation matrix, to transform from center frame to base frame double velocity_in_center_frame_linear_x = - 0.25 * wheels_radius_ * (wheel_front_left_vel + wheel_back_left_vel + wheel_back_right_vel + wheel_front_right_vel); + 0.25 * wheels_radius_ * + (wheel_front_left_vel + wheel_back_left_vel + wheel_back_right_vel + wheel_front_right_vel); double velocity_in_center_frame_linear_y = - 0.25 * wheels_radius_ * (-wheel_front_left_vel + wheel_back_left_vel - wheel_back_right_vel + wheel_front_right_vel); + 0.25 * wheels_radius_ * + (-wheel_front_left_vel + wheel_back_left_vel - wheel_back_right_vel + wheel_front_right_vel); double velocity_in_center_frame_angular_z = - 0.25 * wheels_radius_ / sum_of_robot_center_projection_on_X_Y_axis_ * (-wheel_front_left_vel - wheel_back_left_vel + wheel_back_right_vel + wheel_front_right_vel); + 0.25 * wheels_radius_ / sum_of_robot_center_projection_on_X_Y_axis_ * + (-wheel_front_left_vel - wheel_back_left_vel + wheel_back_right_vel + wheel_front_right_vel); tf2::Quaternion orientation_R_c_b; orientation_R_c_b.setRPY(0.0, 0.0, -base_frame_offset_[2]); @@ -110,7 +114,8 @@ bool Odometry::update( return true; } -void Odometry::setWheelsParams(double sum_of_robot_center_projection_on_X_Y_axis, double wheels_radius) +void Odometry::setWheelsParams( + double sum_of_robot_center_projection_on_X_Y_axis, double wheels_radius) { sum_of_robot_center_projection_on_X_Y_axis_ = sum_of_robot_center_projection_on_X_Y_axis; wheels_radius_ = wheels_radius; diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp index 8ac1f46dd2..259f58bb97 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp @@ -64,7 +64,8 @@ TEST_F(MecanumDriveControllerTest, all_parameters_set_configure_success) ASSERT_EQ(controller_->params_.kinematics.base_frame_offset.y, 0.0); ASSERT_EQ(controller_->params_.kinematics.base_frame_offset.theta, 0.0); - ASSERT_THAT(controller_->params_.command_joint_names, testing::ElementsAreArray(command_joint_names_)); + ASSERT_THAT( + controller_->params_.command_joint_names, testing::ElementsAreArray(command_joint_names_)); ASSERT_TRUE(controller_->params_.state_joint_names.empty()); ASSERT_EQ(controller_->params_.interface_name, interface_name_); } @@ -330,7 +331,7 @@ TEST_F(MecanumDriveControllerTest, test_message_accepted) EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z, 0.0); } -//Test that checks the status of chainable mode and update methods logic accordingly, when ref_timeout and within ref_timeout +//Test that checks the status of chainable mode and update methods logic accordingly, when ref_timeout and within ref_timeout TEST_F(MecanumDriveControllerTest, test_update_logic_not_chainable) { // 1. age>ref_timeout 2. ageref_timeout 2. ageon_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(controller_->params_.reference_timeout, 0.1); - ASSERT_THAT(controller_->params_.command_joint_names, testing::ElementsAreArray(command_joint_names_)); + ASSERT_THAT( + controller_->params_.command_joint_names, testing::ElementsAreArray(command_joint_names_)); ASSERT_THAT( controller_->params_.state_joint_names, testing::ElementsAreArray(state_joint_names_)); ASSERT_THAT(controller_->state_joint_names_, testing::ElementsAreArray(state_joint_names_)); @@ -60,7 +61,7 @@ TEST_F(MecanumDriveControllerTest, check_exported_intefaces) ASSERT_EQ(command_intefaces.names.size(), joint_command_values_.size()); for (size_t i = 0; i < command_intefaces.names.size(); ++i) { - EXPECT_EQ(command_intefaces.names[i],command_joint_names_[i] + "/" + interface_name_); + EXPECT_EQ(command_intefaces.names[i], command_joint_names_[i] + "/" + interface_name_); } auto state_intefaces = controller_->state_interface_configuration(); From 0a6a204339acce984d74639e14b9e2abe225a9f7 Mon Sep 17 00:00:00 2001 From: GiridharBukka Date: Mon, 30 Jan 2023 15:41:48 +0100 Subject: [PATCH 59/91] updating test logic w.r.t reset ref_interfaces --- .../test/test_mecanum_drive_controller.cpp | 14 +++----------- 1 file changed, 3 insertions(+), 11 deletions(-) diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp index 259f58bb97..76f164ea5d 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp @@ -190,15 +190,6 @@ TEST_F(MecanumDriveControllerTest, publish_status_success) controller_->reference_interfaces_[0] = 1.5; - ASSERT_EQ( - controller_->update_reference_from_subscribers( - controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - ASSERT_EQ( - controller_->update_and_write_commands( - controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - ControllerStateMsg msg; subscribe_and_get_messages(msg); @@ -429,8 +420,9 @@ TEST_F(MecanumDriveControllerTest, test_update_logic_not_chainable) ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); for (const auto & interface : controller_->reference_interfaces_) { - EXPECT_FALSE(std::isnan(interface)); + EXPECT_TRUE(std::isnan(interface)); } + for (size_t i = 0; i < controller_->command_interfaces_.size(); ++i) { EXPECT_EQ(controller_->command_interfaces_[i].get_value(), 3.0); @@ -480,7 +472,7 @@ TEST_F(MecanumDriveControllerTest, test_update_logic_chainable) EXPECT_EQ(joint_command_values_[1], 3.0); for (const auto & interface : controller_->reference_interfaces_) { - EXPECT_FALSE(std::isnan(interface)); + EXPECT_TRUE(std::isnan(interface)); } for (size_t i = 0; i < controller_->command_interfaces_.size(); ++i) { From 77642917fcedf010345aeb40f57ddc66f9a29ac6 Mon Sep 17 00:00:00 2001 From: GiridharBukka Date: Mon, 30 Jan 2023 15:55:05 +0100 Subject: [PATCH 60/91] added changes w.r.t control_msgs update --- .../src/mecanum_drive_controller.cpp | 24 +++++++++---------- .../test/test_mecanum_drive_controller.cpp | 8 +++---- 2 files changed, 16 insertions(+), 16 deletions(-) diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index 1e2e5495a3..34b653917a 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -190,9 +190,9 @@ controller_interface::CallbackReturn MecanumDriveController::on_configure( controller_state_publisher_->lock(); controller_state_publisher_->msg_.header.stamp = get_node()->now(); controller_state_publisher_->msg_.header.frame_id = params_.odom_frame_id; - controller_state_publisher_->msg_.odom.child_frame_id = params_.base_frame_id; - controller_state_publisher_->msg_.odom.pose.pose.position.x = 0.0; - auto & covariance_controller = controller_state_publisher_->msg_.odom.twist.covariance; + controller_state_publisher_->msg_.odometry.child_frame_id = params_.base_frame_id; + controller_state_publisher_->msg_.odometry.pose.pose.position.x = 0.0; + auto & covariance_controller = controller_state_publisher_->msg_.odometry.twist.covariance; for (size_t index = 0; index < 6; ++index) { // 0, 7, 14, 21, 28, 35 @@ -468,19 +468,19 @@ controller_interface::return_type MecanumDriveController::update_and_write_comma if (controller_state_publisher_->trylock()) { controller_state_publisher_->msg_.header.stamp = get_node()->now(); - controller_state_publisher_->msg_.odom.pose.pose.position.x = odometry_.getX(); - controller_state_publisher_->msg_.odom.pose.pose.position.y = odometry_.getY(); - controller_state_publisher_->msg_.odom.pose.pose.orientation = tf2::toMsg(orientation); - controller_state_publisher_->msg_.odom.twist.twist.linear.x = odometry_.getVx(); - controller_state_publisher_->msg_.odom.twist.twist.linear.y = odometry_.getVy(); - controller_state_publisher_->msg_.odom.twist.twist.angular.z = odometry_.getWz(); + controller_state_publisher_->msg_.odometry.pose.pose.position.x = odometry_.getX(); + controller_state_publisher_->msg_.odometry.pose.pose.position.y = odometry_.getY(); + controller_state_publisher_->msg_.odometry.pose.pose.orientation = tf2::toMsg(orientation); + controller_state_publisher_->msg_.odometry.twist.twist.linear.x = odometry_.getVx(); + controller_state_publisher_->msg_.odometry.twist.twist.linear.y = odometry_.getVy(); + controller_state_publisher_->msg_.odometry.twist.twist.angular.z = odometry_.getWz(); controller_state_publisher_->msg_.front_left_wheel_velocity = state_interfaces_[0].get_value(); controller_state_publisher_->msg_.back_left_wheel_velocity = state_interfaces_[1].get_value(); controller_state_publisher_->msg_.back_right_wheel_velocity = state_interfaces_[2].get_value(); controller_state_publisher_->msg_.front_right_wheel_velocity = state_interfaces_[3].get_value(); - controller_state_publisher_->msg_.reference_vel.linear.x = reference_interfaces_[0]; - controller_state_publisher_->msg_.reference_vel.linear.y = reference_interfaces_[1]; - controller_state_publisher_->msg_.reference_vel.angular.z = reference_interfaces_[2]; + controller_state_publisher_->msg_.reference_velocity.linear.x = reference_interfaces_[0]; + controller_state_publisher_->msg_.reference_velocity.linear.y = reference_interfaces_[1]; + controller_state_publisher_->msg_.reference_velocity.angular.z = reference_interfaces_[2]; controller_state_publisher_->unlockAndPublish(); } diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp index 76f164ea5d..37646dc495 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp @@ -193,8 +193,8 @@ TEST_F(MecanumDriveControllerTest, publish_status_success) ControllerStateMsg msg; subscribe_and_get_messages(msg); - EXPECT_NEAR(msg.odom.pose.pose.position.y, 0.0, EPS); - EXPECT_EQ(msg.reference_vel.linear.x, 1.5); + EXPECT_NEAR(msg.odometry.pose.pose.position.y, 0.0, EPS); + EXPECT_EQ(msg.reference_velocity.linear.x, 1.5); } // Tests the msg subscriber and publisher @@ -220,7 +220,7 @@ TEST_F(MecanumDriveControllerTest, receive_message_and_publish_updated_status) subscribe_and_get_messages(msg); joint_command_values_[1] = command_lin_x; - EXPECT_TRUE(std::isnan(msg.reference_vel.linear.x)); + EXPECT_TRUE(std::isnan(msg.reference_velocity.linear.x)); publish_commands(controller_->get_node()->now()); ASSERT_TRUE(controller_->wait_for_commands(executor)); @@ -239,7 +239,7 @@ TEST_F(MecanumDriveControllerTest, receive_message_and_publish_updated_status) subscribe_and_get_messages(msg); - ASSERT_EQ(msg.reference_vel.linear.x, 1.5); + ASSERT_EQ(msg.reference_velocity.linear.x, 1.5); } // Tests controller behavior when too old msg is sent / age_of_last_command > ref_timeout case From a1a288dce6195cb1c0d9212745948497f19f03d3 Mon Sep 17 00:00:00 2001 From: GiridharBukka Date: Mon, 30 Jan 2023 17:02:33 +0100 Subject: [PATCH 61/91] cpp_lint check for pre-comit --- .../mecanum_drive_controller.hpp | 3 +- .../mecanum_drive_controller/odometry.hpp | 10 +++-- .../src/mecanum_drive_controller.cpp | 6 ++- .../test/test_mecanum_drive_controller.cpp | 38 +++++++++++++------ 4 files changed, 38 insertions(+), 19 deletions(-) diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp index 9bd260ee44..144b295610 100644 --- a/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp +++ b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp @@ -95,7 +95,8 @@ class MecanumDriveController : public controller_interface::ChainableControllerI // used for chained controller std::vector state_joint_names_; - //Names of the references, ex: high level vel commands from MoveIt, Nav2, etc. used for preceding controller + // Names of the references, ex: high level vel commands from MoveIt, Nav2, etc. + // used for preceding controller std::vector reference_names_; // Command subscribers and Controller State, odom state, tf state publishers diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp b/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp index c151f399e5..8167759301 100644 --- a/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp +++ b/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp @@ -69,7 +69,8 @@ class Odometry } /// \brief Sets the wheels parameters: mecanum geometric param and radius - /// \param sum_of_robot_center_projection_on_X_Y_axis Wheels geometric param (used in mecanum wheels' ik) [m] + /// \param sum_of_robot_center_projection_on_X_Y_axis Wheels geometric param + /// (used in mecanum wheels' ik) [m] /// \param wheels_radius Wheels radius [m] void setWheelsParams(double sum_of_robot_center_projection_on_X_Y_axis, double wheels_radius); @@ -90,12 +91,13 @@ class Odometry double velocity_in_base_frame_angular_z; // [rad/s] /// Wheels kinematic parameters [m]: - /// lx and ly represent the distance from the robot's center to the wheels projected on - /// the x and y axis with origin at robots center respectively, sum_of_robot_center_projection_on_X_Y_axis_ = lx+ly + /// lx and ly represent the distance from the robot's center to the wheels + /// projected on the x and y axis with origin at robots center respectively, + /// sum_of_robot_center_projection_on_X_Y_axis_ = lx+ly double sum_of_robot_center_projection_on_X_Y_axis_; double wheels_radius_; // [m] }; } // namespace mecanum_drive_controller -#endif /* MECANUM_DRIVE_CONTROLLER__ODOMETRY_HPP_ */ +#endif // MECANUM_DRIVE_CONTROLLER__ODOMETRY_HPP_ */ diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index 34b653917a..977138e17b 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -383,8 +383,10 @@ controller_interface::return_type MecanumDriveController::update_and_write_comma tf2::Quaternion quaternion; quaternion.setRPY(0.0, 0.0, params_.kinematics.base_frame_offset.theta); /// \note The variables meaning: - /// rotation_from_base_to_center: Rotation transformation matrix, to transform from base frame to center frame - /// linear_trans_from_base_to_center: offset/linear transformation matrix, to transform from base frame to center frame + /// rotation_from_base_to_center: Rotation transformation matrix, to transform from + /// base frame to center frame + /// linear_trans_from_base_to_center: offset/linear transformation matrix, to + /// transform from base frame to center frame tf2::Matrix3x3 rotation_from_base_to_center = tf2::Matrix3x3((quaternion)); tf2::Vector3 velocity_in_base_frame_w_r_t_center_frame_ = diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp index 37646dc495..f7449166bb 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp @@ -157,7 +157,7 @@ TEST_F(MecanumDriveControllerTest, deactivate_success) ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); } -//checks the functionality of on_activate and on_deactivate methods from the controller +// checks the functionality of on_activate and on_deactivate methods from the controller TEST_F(MecanumDriveControllerTest, reactivate_success) { SetUpController(); @@ -180,7 +180,7 @@ TEST_F(MecanumDriveControllerTest, reactivate_success) controller_interface::return_type::OK); } -//test to check the status of controller state publisher +// test to check the status of controller state publisher TEST_F(MecanumDriveControllerTest, publish_status_success) { SetUpController(); @@ -233,7 +233,10 @@ TEST_F(MecanumDriveControllerTest, receive_message_and_publish_updated_status) controller_->update_and_write_commands( controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - // w0_vel = 1.0 / params_.kinematics.wheels_radius * (body_velocity_center_frame_.linear_x - body_velocity_center_frame_.linear_y - params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis * body_velocity_center_frame_.angular_z); + // w0_vel = 1.0 / params_.kinematics.wheels_radius * + // (body_velocity_center_frame_.linear_x - body_velocity_center_frame_.linear_y + // - params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis + // * body_velocity_center_frame_.angular_z); // joint_command_values_[1] = 1.0 / 0.5 * (1.5 - 0.0 - 1 * 0.0) EXPECT_EQ(joint_command_values_[1], 3.0); @@ -268,7 +271,7 @@ TEST_F(MecanumDriveControllerTest, test_sending_too_old_message) EXPECT_TRUE(std::isnan((reference)->twist.angular.z)); } -//Tests the case when msg time stamp is zero +// Tests the case when msg time stamp is zero TEST_F(MecanumDriveControllerTest, test_time_stamp_zero) { SetUpController(); @@ -296,7 +299,7 @@ TEST_F(MecanumDriveControllerTest, test_time_stamp_zero) EXPECT_NE((*(controller_->input_ref_.readFromNonRT()))->header.stamp.sec, 0.0); } -//Tests the condition for msg to be accepted, i.e, if age_of_last_command < ref_timeout +// Tests the condition for msg to be accepted, i.e, if age_of_last_command < ref_timeout TEST_F(MecanumDriveControllerTest, test_message_accepted) { SetUpController(); @@ -322,7 +325,8 @@ TEST_F(MecanumDriveControllerTest, test_message_accepted) EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z, 0.0); } -//Test that checks the status of chainable mode and update methods logic accordingly, when ref_timeout and within ref_timeout +// Test that checks the status of chainable mode and update methods logic accordingly, +// when ref_timeout and within ref_timeout TEST_F(MecanumDriveControllerTest, test_update_logic_not_chainable) { // 1. age>ref_timeout 2. ageinput_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); @@ -422,14 +429,15 @@ TEST_F(MecanumDriveControllerTest, test_update_logic_not_chainable) { EXPECT_TRUE(std::isnan(interface)); } - + for (size_t i = 0; i < controller_->command_interfaces_.size(); ++i) { EXPECT_EQ(controller_->command_interfaces_[i].get_value(), 3.0); } } -//Test that checks the status of chainable mode and update_and_write_commands() method logic accordingly, when ref_timeout and within ref_timeout +// Test that checks the status of chainable mode and update_and_write_commands() +// method logic accordingly, when ref_timeout and within ref_timeout TEST_F(MecanumDriveControllerTest, test_update_logic_chainable) { // 1. age>ref_timeout 2. agereference_interfaces_) @@ -524,7 +535,10 @@ TEST_F(MecanumDriveControllerTest, test_ref_timeout_zero_for_update) EXPECT_FALSE(std::isnan(joint_command_values_[1])); EXPECT_NE(joint_command_values_[1], command_lin_x); - // w0_vel = 1.0 / params_.kinematics.wheels_radius * (body_velocity_center_frame_.linear_x - body_velocity_center_frame_.linear_y - params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis * body_velocity_center_frame_.angular_z); + // w0_vel = 1.0 / params_.kinematics.wheels_radius + // * (body_velocity_center_frame_.linear_x - body_velocity_center_frame_.linear_y + // - params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis + // * body_velocity_center_frame_.angular_z); // joint_command_values_[1] = 1.0 / 0.5 * (1.5 - 0.0 - 1 * 0.0) EXPECT_EQ(joint_command_values_[1], 3.0); ASSERT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x)); @@ -544,7 +558,7 @@ TEST_F(MecanumDriveControllerTest, test_ref_timeout_zero_for_reference_callback) EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z)); controller_->ref_timeout_ = rclcpp::Duration::from_seconds(0.0); - //reference_callback() is called implicitly when publish_commands() is called. + // reference_callback() is called implicitly when publish_commands() is called. publish_commands(controller_->get_node()->now()); ASSERT_TRUE(controller_->wait_for_commands(executor)); From 8df9f78ad9333661f7f1872ec0d86b2f1127494d Mon Sep 17 00:00:00 2001 From: GiridharBukka Date: Mon, 30 Jan 2023 17:46:50 +0100 Subject: [PATCH 62/91] cpp_lint check update --- mecanum_drive_controller/src/odometry.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/mecanum_drive_controller/src/odometry.cpp b/mecanum_drive_controller/src/odometry.cpp index 0aaadc4d3d..eb2e584371 100644 --- a/mecanum_drive_controller/src/odometry.cpp +++ b/mecanum_drive_controller/src/odometry.cpp @@ -17,7 +17,6 @@ #include "tf2/transform_datatypes.h" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" -using namespace std; namespace mecanum_drive_controller { Odometry::Odometry() @@ -63,8 +62,10 @@ bool Odometry::update( /// to interpret and compare behavior curves). /// \note The variables meaning: - /// angular_transformation_from_center_2_base: Rotation transformation matrix, to transform from center frame to base frame - /// linear_transformation_from_center_2_base: offset/linear transformation matrix, to transform from center frame to base frame + /// angular_transformation_from_center_2_base: Rotation transformation matrix, to transform + /// from center frame to base frame + /// linear_transformation_from_center_2_base: offset/linear transformation matrix, + /// to transform from center frame to base frame double velocity_in_center_frame_linear_x = 0.25 * wheels_radius_ * From 7fc528479740402e18ca05d60031fb5062e4cb0b Mon Sep 17 00:00:00 2001 From: GiridharBukka Date: Mon, 30 Jan 2023 21:04:18 +0100 Subject: [PATCH 63/91] handling ref_timeout==0 in update_and_write method --- mecanum_drive_controller/src/mecanum_drive_controller.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index 977138e17b..7acba2ca60 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -429,6 +429,13 @@ controller_interface::return_type MecanumDriveController::update_and_write_comma command_interfaces_[1].set_value(w_back_left_vel); command_interfaces_[2].set_value(w_back_right_vel); command_interfaces_[3].set_value(w_front_right_vel); + + if (ref_timeout_ == rclcpp::Duration::from_seconds(0)) + { + reference_interfaces_[0] = std::numeric_limits::quiet_NaN(); + reference_interfaces_[1] = std::numeric_limits::quiet_NaN(); + reference_interfaces_[2] = std::numeric_limits::quiet_NaN(); + } } else { From 970bd1b730ea9e1cd872e30d5cc411d03004502e Mon Sep 17 00:00:00 2001 From: GiridharBukka Date: Tue, 31 Jan 2023 00:02:20 +0100 Subject: [PATCH 64/91] updated test comments with when xx then yy scheme --- .../test/test_mecanum_drive_controller.cpp | 48 +++++++++++-------- 1 file changed, 29 insertions(+), 19 deletions(-) diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp index f7449166bb..4465443ce3 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp @@ -37,7 +37,7 @@ namespace const double EPS = 1e-3; } // namespace -// checking if all parameters are initialized and set as expected +// when_all_parameters_are_set_expect_them_in_storage" TEST_F(MecanumDriveControllerTest, all_parameters_set_configure_success) { SetUpController(); @@ -70,7 +70,7 @@ TEST_F(MecanumDriveControllerTest, all_parameters_set_configure_success) ASSERT_EQ(controller_->params_.interface_name, interface_name_); } -// checking if all interfaces, command, state and reference are exported as expected +// when all command, state and reference interfaces are exported then expect them in storage TEST_F(MecanumDriveControllerTest, check_exported_interfaces) { SetUpController(); @@ -108,7 +108,7 @@ TEST_F(MecanumDriveControllerTest, check_exported_interfaces) } } -// checking if calling activate() resets the controller reference msg +// when calling activate() expect resetting of the controller reference msg TEST_F(MecanumDriveControllerTest, activate_success) { SetUpController(); @@ -129,7 +129,7 @@ TEST_F(MecanumDriveControllerTest, activate_success) } } -// checks if return type of update methods are a success +// when calling update methods expect return type are a success TEST_F(MecanumDriveControllerTest, update_success) { SetUpController(); @@ -147,7 +147,7 @@ TEST_F(MecanumDriveControllerTest, update_success) controller_interface::return_type::OK); } -// checks if return type of controller lifecycle methods is a success +// when controller lifecycle methods expect return type is a success TEST_F(MecanumDriveControllerTest, deactivate_success) { SetUpController(); @@ -157,7 +157,8 @@ TEST_F(MecanumDriveControllerTest, deactivate_success) ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); } -// checks the functionality of on_activate and on_deactivate methods from the controller +// when calling on_activate and on_deactivate methods from the controller expect +// resetting of reference msg and nan values in command_interfaces respectively TEST_F(MecanumDriveControllerTest, reactivate_success) { SetUpController(); @@ -180,7 +181,7 @@ TEST_F(MecanumDriveControllerTest, reactivate_success) controller_interface::return_type::OK); } -// test to check the status of controller state publisher +// when controller state published expect state value in storage TEST_F(MecanumDriveControllerTest, publish_status_success) { SetUpController(); @@ -197,7 +198,7 @@ TEST_F(MecanumDriveControllerTest, publish_status_success) EXPECT_EQ(msg.reference_velocity.linear.x, 1.5); } -// Tests the msg subscriber and publisher +// when msg subscribed and published expect value in storage TEST_F(MecanumDriveControllerTest, receive_message_and_publish_updated_status) { SetUpController(); @@ -222,6 +223,7 @@ TEST_F(MecanumDriveControllerTest, receive_message_and_publish_updated_status) EXPECT_TRUE(std::isnan(msg.reference_velocity.linear.x)); + // reference_callback() is implicitly called when publish_commands() is called publish_commands(controller_->get_node()->now()); ASSERT_TRUE(controller_->wait_for_commands(executor)); @@ -245,7 +247,7 @@ TEST_F(MecanumDriveControllerTest, receive_message_and_publish_updated_status) ASSERT_EQ(msg.reference_velocity.linear.x, 1.5); } -// Tests controller behavior when too old msg is sent / age_of_last_command > ref_timeout case +// when too old msg is sent expect nan values in reference msg TEST_F(MecanumDriveControllerTest, test_sending_too_old_message) { SetUpController(); @@ -261,6 +263,7 @@ TEST_F(MecanumDriveControllerTest, test_sending_too_old_message) EXPECT_TRUE(std::isnan(reference->twist.linear.y)); EXPECT_TRUE(std::isnan(reference->twist.angular.z)); + // reference_callback() is implicitly called when publish_commands() is called publish_commands( controller_->get_node()->now() - controller_->ref_timeout_ - rclcpp::Duration::from_seconds(0.1)); @@ -271,7 +274,7 @@ TEST_F(MecanumDriveControllerTest, test_sending_too_old_message) EXPECT_TRUE(std::isnan((reference)->twist.angular.z)); } -// Tests the case when msg time stamp is zero +// when time stamp is zero expect that time stamp is set to current time stamp TEST_F(MecanumDriveControllerTest, test_time_stamp_zero) { SetUpController(); @@ -287,6 +290,7 @@ TEST_F(MecanumDriveControllerTest, test_time_stamp_zero) EXPECT_TRUE(std::isnan((*reference)->twist.linear.y)); EXPECT_TRUE(std::isnan((*reference)->twist.angular.z)); + // reference_callback() is implicitly called when publish_commands() is called publish_commands(rclcpp::Time(0)); ASSERT_TRUE(controller_->wait_for_commands(executor)); @@ -299,7 +303,7 @@ TEST_F(MecanumDriveControllerTest, test_time_stamp_zero) EXPECT_NE((*(controller_->input_ref_.readFromNonRT()))->header.stamp.sec, 0.0); } -// Tests the condition for msg to be accepted, i.e, if age_of_last_command < ref_timeout +// when age_of_last_command < ref_timeout expect reference msg is accepted and is in rt buffer TEST_F(MecanumDriveControllerTest, test_message_accepted) { SetUpController(); @@ -310,11 +314,11 @@ TEST_F(MecanumDriveControllerTest, test_message_accepted) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - // try to set command with time before timeout - command is not updated auto reference = controller_->input_ref_.readFromNonRT(); EXPECT_TRUE(std::isnan((*reference)->twist.linear.x)); EXPECT_TRUE(std::isnan((*reference)->twist.angular.z)); + // reference_callback() is implicitly called when publish_commands() is called publish_commands(controller_->get_node()->now()); ASSERT_TRUE(controller_->wait_for_commands(executor)); @@ -325,8 +329,12 @@ TEST_F(MecanumDriveControllerTest, test_message_accepted) EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z, 0.0); } -// Test that checks the status of chainable mode and update methods logic accordingly, -// when ref_timeout and within ref_timeout +// when not in chainable mode and age_of_last_command > reference_timeout expect +// command_interfaces are set to 0.0 and reference_interfaces set to nan +// followed by +// when not in chainable mode and age_of_last_command < reference_timeout expect +// command_interfaces are calculated to non-nan and reference_interfaces set to nan + TEST_F(MecanumDriveControllerTest, test_update_logic_not_chainable) { // 1. age>ref_timeout 2. ageref_timeout 2. ageinput_ref_.readFromRT()))->twist.linear.x)); } -// Tests behavior of reference_callback() if ref_timeout = 0 +// when ref_timeout = 0 expect reference_callback() writes reference_msg to rt buffer +// from nonrt thread TEST_F(MecanumDriveControllerTest, test_ref_timeout_zero_for_reference_callback) { SetUpController(); From 7fc7b907cdbe97f2ec8bd0e27b4262e942b9bee6 Mon Sep 17 00:00:00 2001 From: GiridharBukka Date: Tue, 31 Jan 2023 00:03:55 +0100 Subject: [PATCH 65/91] Revert "handling ref_timeout==0 in update_and_write method" This reverts commit 823253a387aa3e23d29e9a6fad5094b219fc3994. --- mecanum_drive_controller/src/mecanum_drive_controller.cpp | 7 ------- 1 file changed, 7 deletions(-) diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index 7acba2ca60..977138e17b 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -429,13 +429,6 @@ controller_interface::return_type MecanumDriveController::update_and_write_comma command_interfaces_[1].set_value(w_back_left_vel); command_interfaces_[2].set_value(w_back_right_vel); command_interfaces_[3].set_value(w_front_right_vel); - - if (ref_timeout_ == rclcpp::Duration::from_seconds(0)) - { - reference_interfaces_[0] = std::numeric_limits::quiet_NaN(); - reference_interfaces_[1] = std::numeric_limits::quiet_NaN(); - reference_interfaces_[2] = std::numeric_limits::quiet_NaN(); - } } else { From 40855bb6ac1e7372606d949bbd8fe5f4ee748d7f Mon Sep 17 00:00:00 2001 From: GiridharBukka Date: Tue, 31 Jan 2023 02:40:37 +0100 Subject: [PATCH 66/91] renaming test names to whenn xx expect yy scheme --- .../test/test_mecanum_drive_controller.cpp | 35 ++++++++++--------- .../test/test_mecanum_drive_controller.hpp | 28 +++++++-------- 2 files changed, 32 insertions(+), 31 deletions(-) diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp index 4465443ce3..280cd7f1e1 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp @@ -37,8 +37,8 @@ namespace const double EPS = 1e-3; } // namespace -// when_all_parameters_are_set_expect_them_in_storage" -TEST_F(MecanumDriveControllerTest, all_parameters_set_configure_success) +// when_all_parameters_are_set_expect_them_in_storage +TEST_F(MecanumDriveControllerTest, when_all_parameters_are_set_expect_them_in_storage) { SetUpController(); @@ -71,7 +71,7 @@ TEST_F(MecanumDriveControllerTest, all_parameters_set_configure_success) } // when all command, state and reference interfaces are exported then expect them in storage -TEST_F(MecanumDriveControllerTest, check_exported_interfaces) +TEST_F(MecanumDriveControllerTest, when_exported_all_interfaces_expect_them_in_storage) { SetUpController(); @@ -109,7 +109,7 @@ TEST_F(MecanumDriveControllerTest, check_exported_interfaces) } // when calling activate() expect resetting of the controller reference msg -TEST_F(MecanumDriveControllerTest, activate_success) +TEST_F(MecanumDriveControllerTest, when_activated_expect_reference_msg_reset) { SetUpController(); @@ -130,7 +130,7 @@ TEST_F(MecanumDriveControllerTest, activate_success) } // when calling update methods expect return type are a success -TEST_F(MecanumDriveControllerTest, update_success) +TEST_F(MecanumDriveControllerTest, when_update_successful_expect_return_type_success) { SetUpController(); @@ -148,7 +148,7 @@ TEST_F(MecanumDriveControllerTest, update_success) } // when controller lifecycle methods expect return type is a success -TEST_F(MecanumDriveControllerTest, deactivate_success) +TEST_F(MecanumDriveControllerTest, when_deactivated_expect_return_type_success) { SetUpController(); @@ -157,9 +157,10 @@ TEST_F(MecanumDriveControllerTest, deactivate_success) ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); } -// when calling on_activate and on_deactivate methods from the controller expect -// resetting of reference msg and nan values in command_interfaces respectively -TEST_F(MecanumDriveControllerTest, reactivate_success) +// when calling on_activate, on_deactivate and on_activate methods consecutively +// expect resetting of reference msg, nan values in command_interfaces and +// resetting of reference msg respectively +TEST_F(MecanumDriveControllerTest, when_reactivated_expect_reference_msg_reset) { SetUpController(); @@ -182,7 +183,7 @@ TEST_F(MecanumDriveControllerTest, reactivate_success) } // when controller state published expect state value in storage -TEST_F(MecanumDriveControllerTest, publish_status_success) +TEST_F(MecanumDriveControllerTest, when_published_success_expect_in_storage) { SetUpController(); @@ -199,7 +200,7 @@ TEST_F(MecanumDriveControllerTest, publish_status_success) } // when msg subscribed and published expect value in storage -TEST_F(MecanumDriveControllerTest, receive_message_and_publish_updated_status) +TEST_F(MecanumDriveControllerTest, when_subscribed_msg_received_publish_succeeded_expect_value_in_storage) { SetUpController(); @@ -248,7 +249,7 @@ TEST_F(MecanumDriveControllerTest, receive_message_and_publish_updated_status) } // when too old msg is sent expect nan values in reference msg -TEST_F(MecanumDriveControllerTest, test_sending_too_old_message) +TEST_F(MecanumDriveControllerTest, when_sending_too_old_message_expect_nan_in_reference_msg) { SetUpController(); rclcpp::executors::MultiThreadedExecutor executor; @@ -275,7 +276,7 @@ TEST_F(MecanumDriveControllerTest, test_sending_too_old_message) } // when time stamp is zero expect that time stamp is set to current time stamp -TEST_F(MecanumDriveControllerTest, test_time_stamp_zero) +TEST_F(MecanumDriveControllerTest, when_time_stamp_zero_expect_setting_to_current) { SetUpController(); rclcpp::executors::MultiThreadedExecutor executor; @@ -304,7 +305,7 @@ TEST_F(MecanumDriveControllerTest, test_time_stamp_zero) } // when age_of_last_command < ref_timeout expect reference msg is accepted and is in rt buffer -TEST_F(MecanumDriveControllerTest, test_message_accepted) +TEST_F(MecanumDriveControllerTest, when_message_accepted_expect_reference_msg_in_rt_buffer) { SetUpController(); @@ -335,7 +336,7 @@ TEST_F(MecanumDriveControllerTest, test_message_accepted) // when not in chainable mode and age_of_last_command < reference_timeout expect // command_interfaces are calculated to non-nan and reference_interfaces set to nan -TEST_F(MecanumDriveControllerTest, test_update_logic_not_chainable) +TEST_F(MecanumDriveControllerTest, test_update_logic_not_chainable_mode) { // 1. age>ref_timeout 2. age Date: Tue, 31 Jan 2023 16:55:53 +0100 Subject: [PATCH 67/91] Update tests --- .../test_load_mecanum_drive_controller.cpp | 2 +- .../test/test_mecanum_drive_controller.cpp | 63 +++++++------------ .../test/test_mecanum_drive_controller.hpp | 14 +---- 3 files changed, 25 insertions(+), 54 deletions(-) diff --git a/mecanum_drive_controller/test/test_load_mecanum_drive_controller.cpp b/mecanum_drive_controller/test/test_load_mecanum_drive_controller.cpp index a6de9504a8..34bada08dd 100644 --- a/mecanum_drive_controller/test/test_load_mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/test/test_load_mecanum_drive_controller.cpp @@ -22,7 +22,7 @@ #include "rclcpp/utilities.hpp" #include "ros2_control_test_assets/descriptions.hpp" -TEST(TestLoadMecanumDriveController, load_controller) +TEST(TestLoadMecanumDriveController, when_loading_controller_expect_no_exception) { rclcpp::init(0, nullptr); diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp index 280cd7f1e1..490c71413e 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp @@ -37,8 +37,7 @@ namespace const double EPS = 1e-3; } // namespace -// when_all_parameters_are_set_expect_them_in_storage -TEST_F(MecanumDriveControllerTest, when_all_parameters_are_set_expect_them_in_storage) +TEST_F(MecanumDriveControllerTest, when_controller_is_configured_expect_all_parameters_set) { SetUpController(); @@ -71,7 +70,7 @@ TEST_F(MecanumDriveControllerTest, when_all_parameters_are_set_expect_them_in_st } // when all command, state and reference interfaces are exported then expect them in storage -TEST_F(MecanumDriveControllerTest, when_exported_all_interfaces_expect_them_in_storage) +TEST_F(MecanumDriveControllerTest, when_controller_configured_expect_properly_exported_interfaces) { SetUpController(); @@ -108,8 +107,7 @@ TEST_F(MecanumDriveControllerTest, when_exported_all_interfaces_expect_them_in_s } } -// when calling activate() expect resetting of the controller reference msg -TEST_F(MecanumDriveControllerTest, when_activated_expect_reference_msg_reset) +TEST_F(MecanumDriveControllerTest, when_controller_is_activated_expect_reference_reset) { SetUpController(); @@ -130,7 +128,7 @@ TEST_F(MecanumDriveControllerTest, when_activated_expect_reference_msg_reset) } // when calling update methods expect return type are a success -TEST_F(MecanumDriveControllerTest, when_update_successful_expect_return_type_success) +TEST_F(MecanumDriveControllerTest, when_controller_active_and_update_called_expect_success) { SetUpController(); @@ -138,17 +136,12 @@ TEST_F(MecanumDriveControllerTest, when_update_successful_expect_return_type_suc ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ( - controller_->update_reference_from_subscribers( - controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - ASSERT_EQ( - controller_->update_and_write_commands( - controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); } // when controller lifecycle methods expect return type is a success -TEST_F(MecanumDriveControllerTest, when_deactivated_expect_return_type_success) +TEST_F(MecanumDriveControllerTest, when_active_controller_is_deactivated_expect_success) { SetUpController(); @@ -160,7 +153,7 @@ TEST_F(MecanumDriveControllerTest, when_deactivated_expect_return_type_success) // when calling on_activate, on_deactivate and on_activate methods consecutively // expect resetting of reference msg, nan values in command_interfaces and // resetting of reference msg respectively -TEST_F(MecanumDriveControllerTest, when_reactivated_expect_reference_msg_reset) +TEST_F(MecanumDriveControllerTest, when_controller_is_reactivated_expect_cmd_itfs_not_set_and_update_success) { SetUpController(); @@ -173,17 +166,12 @@ TEST_F(MecanumDriveControllerTest, when_reactivated_expect_reference_msg_reset) ASSERT_TRUE(std::isnan(controller_->command_interfaces_[NR_CMD_ITFS - 4].get_value())); ASSERT_EQ( - controller_->update_reference_from_subscribers( - controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - ASSERT_EQ( - controller_->update_and_write_commands( - controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); } // when controller state published expect state value in storage -TEST_F(MecanumDriveControllerTest, when_published_success_expect_in_storage) +TEST_F(MecanumDriveControllerTest, when_update_is_called_expect_status_message) { SetUpController(); @@ -193,14 +181,14 @@ TEST_F(MecanumDriveControllerTest, when_published_success_expect_in_storage) controller_->reference_interfaces_[0] = 1.5; ControllerStateMsg msg; - subscribe_and_get_messages(msg); + subscribe_to_controller_status_execute_update_and_get_messages(msg); EXPECT_NEAR(msg.odometry.pose.pose.position.y, 0.0, EPS); EXPECT_EQ(msg.reference_velocity.linear.x, 1.5); } // when msg subscribed and published expect value in storage -TEST_F(MecanumDriveControllerTest, when_subscribed_msg_received_publish_succeeded_expect_value_in_storage) +TEST_F(MecanumDriveControllerTest, when_reference_msg_received_expect_updated_commands_and_status_message) { SetUpController(); @@ -211,12 +199,7 @@ TEST_F(MecanumDriveControllerTest, when_subscribed_msg_received_publish_succeede ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ( - controller_->update_reference_from_subscribers( - controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - ASSERT_EQ( - controller_->update_and_write_commands( - controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); ControllerStateMsg msg; subscribe_and_get_messages(msg); @@ -229,13 +212,9 @@ TEST_F(MecanumDriveControllerTest, when_subscribed_msg_received_publish_succeede ASSERT_TRUE(controller_->wait_for_commands(executor)); ASSERT_EQ( - controller_->update_reference_from_subscribers( - controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - ASSERT_EQ( - controller_->update_and_write_commands( - controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); + // w0_vel = 1.0 / params_.kinematics.wheels_radius * // (body_velocity_center_frame_.linear_x - body_velocity_center_frame_.linear_y // - params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis @@ -246,10 +225,11 @@ TEST_F(MecanumDriveControllerTest, when_subscribed_msg_received_publish_succeede subscribe_and_get_messages(msg); ASSERT_EQ(msg.reference_velocity.linear.x, 1.5); + ASSERT_EQ(msg.back_left_wheel_velocity, 3.0); } // when too old msg is sent expect nan values in reference msg -TEST_F(MecanumDriveControllerTest, when_sending_too_old_message_expect_nan_in_reference_msg) +TEST_F(MecanumDriveControllerTest, when_reference_msg_is_too_old_expect_unset_reference) { SetUpController(); rclcpp::executors::MultiThreadedExecutor executor; @@ -276,7 +256,7 @@ TEST_F(MecanumDriveControllerTest, when_sending_too_old_message_expect_nan_in_re } // when time stamp is zero expect that time stamp is set to current time stamp -TEST_F(MecanumDriveControllerTest, when_time_stamp_zero_expect_setting_to_current) +TEST_F(MecanumDriveControllerTest, when_reference_msg_has_timestamp_zero_expect_reference_set_and_timestamp_set_to_current_time) { SetUpController(); rclcpp::executors::MultiThreadedExecutor executor; @@ -305,7 +285,7 @@ TEST_F(MecanumDriveControllerTest, when_time_stamp_zero_expect_setting_to_curren } // when age_of_last_command < ref_timeout expect reference msg is accepted and is in rt buffer -TEST_F(MecanumDriveControllerTest, when_message_accepted_expect_reference_msg_in_rt_buffer) +TEST_F(MecanumDriveControllerTest, when_message_has_valid_timestamp_expect_reference_set) { SetUpController(); @@ -335,8 +315,7 @@ TEST_F(MecanumDriveControllerTest, when_message_accepted_expect_reference_msg_in // followed by // when not in chainable mode and age_of_last_command < reference_timeout expect // command_interfaces are calculated to non-nan and reference_interfaces set to nan - -TEST_F(MecanumDriveControllerTest, test_update_logic_not_chainable_mode) +TEST_F(MecanumDriveControllerTest, when_reference_message_times_out_expect_commands_are_zeroed) { // 1. age>ref_timeout 2. ageassign_interfaces(std::move(command_ifs), std::move(state_ifs)); } - void subscribe_and_get_messages(ControllerStateMsg & msg) + void subscribe_to_controller_status_execute_update_and_get_messages(ControllerStateMsg & msg) { // create a new subscriber rclcpp::Node test_subscription_node("test_subscription_node"); @@ -183,12 +183,7 @@ class MecanumDriveControllerFixture : public ::testing::Test "/test_mecanum_drive_controller/controller_state", 10, subs_callback); // call update to publish the test value ASSERT_EQ( - controller_->update_reference_from_subscribers( - controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - ASSERT_EQ( - controller_->update_and_write_commands( - controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); // call update to publish the test value // since update doesn't guarantee a published message, republish until received @@ -197,10 +192,7 @@ class MecanumDriveControllerFixture : public ::testing::Test wait_set.add_subscription(subscription); while (max_sub_check_loop_count--) { - controller_->update_reference_from_subscribers( - controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)); - controller_->update_and_write_commands( - controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)); + controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)); // check if message has been received if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) { From ec6424a9550d9fec598d0000f1470a0bd81489a9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Wed, 8 Feb 2023 21:53:32 +0100 Subject: [PATCH 68/91] Restructure CMakeLists.txt and add docs. --- doc/controllers_index.rst | 1 + mecanum_drive_controller/CMakeLists.txt | 62 +++++++++----------- mecanum_drive_controller/doc/userdoc.rst | 55 +++++++++++++++++ mecanum_drive_controller/package.xml | 2 +- steering_controllers_library/doc/userdoc.rst | 2 +- 5 files changed, 86 insertions(+), 36 deletions(-) create mode 100644 mecanum_drive_controller/doc/userdoc.rst diff --git a/doc/controllers_index.rst b/doc/controllers_index.rst index 2cb55150ce..7379d1afeb 100644 --- a/doc/controllers_index.rst +++ b/doc/controllers_index.rst @@ -28,6 +28,7 @@ Controllers for Wheeled Mobile Robots :titlesonly: Differential Drive Controller <../diff_drive_controller/doc/userdoc.rst> + Mecanum Drive Controllers <../mecanum_drive_controller/doc/userdoc.rst> Steering Controllers Library <../steering_controllers_library/doc/userdoc.rst> Tricycle Controller <../tricycle_controller/doc/userdoc.rst> diff --git a/mecanum_drive_controller/CMakeLists.txt b/mecanum_drive_controller/CMakeLists.txt index d167d9c673..ce5d349d5d 100644 --- a/mecanum_drive_controller/CMakeLists.txt +++ b/mecanum_drive_controller/CMakeLists.txt @@ -1,66 +1,55 @@ cmake_minimum_required(VERSION 3.16) -project(mecanum_drive_controller) +project(mecanum_drive_controller LANGUAGES CXX) -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Wpedantic) endif() # find dependencies set(THIS_PACKAGE_INCLUDE_DEPENDS - control_msgs controller_interface - geometry_msgs hardware_interface - nav_msgs + generate_parameter_library pluginlib rclcpp rclcpp_lifecycle realtime_tools std_srvs tf2 - tf2_msgs tf2_geometry_msgs + tf2_msgs ) find_package(ament_cmake REQUIRED) -find_package(generate_parameter_library REQUIRED) +find_package(backward_ros REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() -# Add mecanum_drive_controller library related compile commands generate_parameter_library(mecanum_drive_controller_parameters src/mecanum_drive_controller.yaml ) + add_library( mecanum_drive_controller SHARED src/mecanum_drive_controller.cpp - src/odometry.cpp ) +target_compile_features(mecanum_drive_controller PUBLIC cxx_std_17) target_include_directories(mecanum_drive_controller PUBLIC -"$" -"$") -target_link_libraries(mecanum_drive_controller mecanum_drive_controller_parameters) -ament_target_dependencies(mecanum_drive_controller ${THIS_PACKAGE_INCLUDE_DEPENDS}) -target_compile_definitions(mecanum_drive_controller PRIVATE "MECANUM_DRIVE_CONTROLLER_BUILDING_DLL") + "$" + "$") +target_link_libraries(mecanum_drive_controller PUBLIC + mecanum_drive_controller_parameters) +ament_target_dependencies(mecanum_drive_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(mecanum_drive_controller PRIVATE "ACKERMANN_STEERING_CONTROLLER_BUILDING_DLL") pluginlib_export_plugin_description_file( controller_interface mecanum_drive_controller.xml) -install( - TARGETS - mecanum_drive_controller - RUNTIME DESTINATION bin - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib -) - -install( - DIRECTORY include/ - DESTINATION include -) - if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) find_package(controller_manager REQUIRED) @@ -99,14 +88,19 @@ if(BUILD_TESTING) ) endif() -ament_export_include_directories( - include -) -ament_export_dependencies( - ${THIS_PACKAGE_INCLUDE_DEPENDS} +install( + DIRECTORY include/ + DESTINATION include/mecanum_drive_controller ) -ament_export_libraries( - mecanum_drive_controller + +install( + TARGETS mecanum_drive_controller mecanum_drive_controller_parameters + EXPORT export_mecanum_drive_controller + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib ) +ament_export_targets(export_mecanum_drive_controller HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) ament_package() diff --git a/mecanum_drive_controller/doc/userdoc.rst b/mecanum_drive_controller/doc/userdoc.rst new file mode 100644 index 0000000000..f056cf2a8c --- /dev/null +++ b/mecanum_drive_controller/doc/userdoc.rst @@ -0,0 +1,55 @@ +.. _mecanum_drive_controller_userdoc: + +mecanum_drive_controller +========================= + +Library with shared functionalities for mobile robots controllers with mecanum drive (four wheels). +The library implements generic odometry and update methods and defines the main interfaces. + +Execution logic of the controller +---------------------------------- + +The controller uses velocity input, i.e., stamped or unstamped Twist messages where linear ``x``, ``y``, and angular ``z`` components are used. +Values in other components are ignored. +In the chain mode the controller provides three reference interfaces, one for linear velocity and one for steering angle position. +Other relevant features are: + + - odometry publishing as Odometry and TF message; + - input command timeout based on a parameter. + + +Description of controller's interfaces +-------------------------------------- + +References (from a preceding controller) +,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,, +- / [double] # in [rad] or [rad/s] + +Commands +,,,,,,,,, +- / [double] # in [rad] or [rad/s] + +States +,,,,,,, +- / [double] # in [rad] or [rad/s] + **NOTE**: ``joint_name`` can be ``state_joint_names`` parameter of if empty ``command_joint_names``. + + +Subscribers +,,,,,,,,,,,, +Used when controller is not in chained mode (``in_chained_mode == false``). + +- /reference [geometry_msgs/msg/TwistStamped] + +Publishers +,,,,,,,,,,, +- /odometry [nav_msgs/msg/Odometry] +- /tf_odometry [tf2_msgs/msg/TFMessage] +- /controller_state [control_msgs/msg/MecanumDriveControllerState] + +Parameters +,,,,,,,,,,, + +For list of parameters and their meaning YAML file in the ``src`` folder of the controller's package. + +For an exameplary parameterization see the ``test`` folder of the controller's package. diff --git a/mecanum_drive_controller/package.xml b/mecanum_drive_controller/package.xml index c989b9266f..a12951faae 100644 --- a/mecanum_drive_controller/package.xml +++ b/mecanum_drive_controller/package.xml @@ -30,8 +30,8 @@ rcpputils std_srvs tf2 - tf2_msgs tf2_geometry_msgs + tf2_msgs ament_cmake_gmock diff --git a/steering_controllers_library/doc/userdoc.rst b/steering_controllers_library/doc/userdoc.rst index a52e34c120..d9e7fef4ee 100644 --- a/steering_controllers_library/doc/userdoc.rst +++ b/steering_controllers_library/doc/userdoc.rst @@ -10,7 +10,7 @@ steering_controllers_library .. _twist_msg: https://github.com/ros2/common_interfaces/blob/{DISTRO}/geometry_msgs/msg/TwistStamped.msg .. _tf_msg: https://github.com/ros2/geometry2/blob/{DISTRO}/tf2_msgs/msg/TFMessage.msg -Library with shared functionalities for mobile robot controllers with steering drives, with so-called non-holonomic constraints. +Library with shared functionalities for mobile robot controllers with steering drives (2 degrees of freedom), with so-called non-holonomic constraints. The library implements generic odometry and update methods and defines the main interfaces. From 447a504a27570004935c3eef4f05828498caeab8 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Mon, 13 Feb 2023 15:43:55 +0100 Subject: [PATCH 69/91] Update mecanum_drive_controller/mecanum_drive_controller.xml --- mecanum_drive_controller/mecanum_drive_controller.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mecanum_drive_controller/mecanum_drive_controller.xml b/mecanum_drive_controller/mecanum_drive_controller.xml index 5bfbad4e32..c012ad53ca 100644 --- a/mecanum_drive_controller/mecanum_drive_controller.xml +++ b/mecanum_drive_controller/mecanum_drive_controller.xml @@ -2,6 +2,6 @@ - The mecanum drive controller transforms linear and angular velocity messages into signals for each wheel(s) for a 4 mecanum wheeled robot. + The mecanum drive controller transforms linear and angular velocity messages into signals for each wheel(s) for a 4 mecanum wheeled robot. From 37b27d1681d7e22895717acb0f331455e476e511 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Mon, 13 Feb 2023 15:44:31 +0100 Subject: [PATCH 70/91] Update mecanum_drive_controller/package.xml --- mecanum_drive_controller/package.xml | 1 - 1 file changed, 1 deletion(-) diff --git a/mecanum_drive_controller/package.xml b/mecanum_drive_controller/package.xml index a12951faae..aa718b57f0 100644 --- a/mecanum_drive_controller/package.xml +++ b/mecanum_drive_controller/package.xml @@ -33,7 +33,6 @@ tf2_geometry_msgs tf2_msgs - ament_cmake_gmock controller_manager ros2_control_test_assets From ade56b686f38113b18555b914e1c67473841b3cb Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Mon, 13 Feb 2023 15:46:05 +0100 Subject: [PATCH 71/91] Update mecanum_drive_controller.yaml --- .../src/mecanum_drive_controller.yaml | 9 --------- 1 file changed, 9 deletions(-) diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.yaml b/mecanum_drive_controller/src/mecanum_drive_controller.yaml index 47dbae58f6..283d806481 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.yaml +++ b/mecanum_drive_controller/src/mecanum_drive_controller.yaml @@ -3,7 +3,6 @@ mecanum_drive_controller: type: double, default_value: 0.0, description: "Timeout for controller references after which they will be reset. This is especially useful for controllers that can cause unwanted and dangerous behavior if reference is not reset, e.g., velocity controllers. If value is 0 the reference is reset after each run.", - } command_joint_names: { @@ -11,7 +10,6 @@ mecanum_drive_controller: default_value: [], description: "Name of the wheels joints.", read_only: false, - } state_joint_names: { @@ -19,7 +17,6 @@ mecanum_drive_controller: default_value: [], description: "(optional) Specifies joints for reading states. This parameter is only relevant when state joints are different then command joint, i.e., when a following controller is used.", read_only: true, - } reference_names: { @@ -62,7 +59,6 @@ mecanum_drive_controller: default_value: 0.0, description: "Wheel's radius.", read_only: false, - } sum_of_robot_center_projection_on_X_Y_axis: { @@ -71,7 +67,6 @@ mecanum_drive_controller: description: "Wheels geometric param used in mecanum wheels' IK. lx and ly represent the distance from the robot's center to the wheels projected on the x and y axis with origin at robots center respectively, sum_of_robot_center_projection_on_X_Y_axis = lx+ly", read_only: false, - } base_frame_id: { @@ -86,14 +81,12 @@ mecanum_drive_controller: 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: { @@ -101,7 +94,6 @@ mecanum_drive_controller: default_value: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1], description: "diagonal values of twist covariance matrix.", read_only: false, - } pose_covariance_diagonal: { @@ -109,5 +101,4 @@ mecanum_drive_controller: default_value: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1], description: "diagonal values of pose covariance matrix.", read_only: false, - } From ea9b21fe609ffbbde16b7b98d714f27f547a2947 Mon Sep 17 00:00:00 2001 From: GiridharBukka Date: Mon, 13 Feb 2023 17:14:38 +0100 Subject: [PATCH 72/91] implemneted suggested changes compiling and ll tests pass --- mecanum_drive_controller/CMakeLists.txt | 1 + .../src/mecanum_drive_controller.cpp | 6 +- .../src/mecanum_drive_controller.yaml | 7 - .../test/mecanum_drive_controller_params.yaml | 2 - .../test/test_mecanum_drive_controller.cpp | 162 +++++++----------- .../test/test_mecanum_drive_controller.hpp | 33 ++-- ...st_mecanum_drive_controller_preceeding.cpp | 5 +- 7 files changed, 87 insertions(+), 129 deletions(-) diff --git a/mecanum_drive_controller/CMakeLists.txt b/mecanum_drive_controller/CMakeLists.txt index ce5d349d5d..5168fac7ae 100644 --- a/mecanum_drive_controller/CMakeLists.txt +++ b/mecanum_drive_controller/CMakeLists.txt @@ -34,6 +34,7 @@ add_library( mecanum_drive_controller SHARED src/mecanum_drive_controller.cpp + src/odometry.cpp ) target_compile_features(mecanum_drive_controller PUBLIC cxx_std_17) target_include_directories(mecanum_drive_controller PUBLIC diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index 977138e17b..8183d3756e 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -93,8 +93,6 @@ controller_interface::CallbackReturn MecanumDriveController::on_configure( return CallbackReturn::FAILURE; } - reference_names_ = params_.reference_names; - // Set wheel params for the odometry computation odometry_.setWheelsParams( params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis, @@ -275,10 +273,12 @@ MecanumDriveController::on_export_reference_interfaces() reference_interfaces.reserve(reference_interfaces_.size()); + std::vector reference_interface_names = {"linear/x/velocity", "linear/y/velocity", "angular/z/velocity"}; + for (size_t i = 0; i < reference_interfaces_.size(); ++i) { reference_interfaces.push_back(hardware_interface::CommandInterface( - get_node()->get_name(), reference_names_[i] + "/" + params_.interface_name, + get_node()->get_name(), reference_interface_names[i], &reference_interfaces_[i])); } diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.yaml b/mecanum_drive_controller/src/mecanum_drive_controller.yaml index 283d806481..da0c1ffb99 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.yaml +++ b/mecanum_drive_controller/src/mecanum_drive_controller.yaml @@ -19,13 +19,6 @@ mecanum_drive_controller: read_only: true, } - reference_names: { - type: string_array, - default_value: [], - description: " Names of the references, ex: high level vel commands from MoveIt, Nav2, etc.", - read_only: true, - } - interface_name: { type: string, default_value: "", diff --git a/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml b/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml index 88bc9ddb86..9eec559dd1 100644 --- a/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml +++ b/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml @@ -5,8 +5,6 @@ test_mecanum_drive_controller: command_joint_names: ["front_left_wheel_joint", "back_left_wheel_joint", "back_right_wheel_joint", "front_right_wheel_joint"] - reference_names: ["linear_x", "linear_y", "angular_z"] - interface_name: velocity kinematics: diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp index 490c71413e..739bb9b407 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp @@ -69,7 +69,6 @@ TEST_F(MecanumDriveControllerTest, when_controller_is_configured_expect_all_para ASSERT_EQ(controller_->params_.interface_name, interface_name_); } -// when all command, state and reference interfaces are exported then expect them in storage TEST_F(MecanumDriveControllerTest, when_controller_configured_expect_properly_exported_interfaces) { SetUpController(); @@ -92,18 +91,18 @@ TEST_F(MecanumDriveControllerTest, when_controller_configured_expect_properly_ex EXPECT_EQ(state_intefaces.names[i], command_joint_names_[i] + "/" + interface_name_); } - // check ref itfs configuration, reference_names_ + // check ref itfs configuration, auto reference_interfaces = controller_->export_reference_interfaces(); ASSERT_EQ(reference_interfaces.size(), NR_REF_ITFS); - for (size_t i = 0; i < reference_names_.size(); ++i) + + for (size_t i = 0; i < reference_interface_names.size(); ++i) { - const std::string ref_itf_name = std::string(controller_->get_node()->get_name()) + "/" + - reference_names_[i] + "/" + interface_name_; + const std::string ref_itf_name = std::string(controller_->get_node()->get_name()) + std::string("/") +reference_interface_names[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(), reference_names_[i] + "/" + interface_name_); + reference_interfaces[i].get_interface_name(), reference_interface_names[i]); } } @@ -119,15 +118,8 @@ TEST_F(MecanumDriveControllerTest, when_controller_is_activated_expect_reference EXPECT_TRUE(std::isnan((*msg)->twist.linear.x)); ASSERT_TRUE(std::isnan((*msg)->twist.angular.z)); - - EXPECT_EQ(controller_->reference_interfaces_.size(), NR_REF_ITFS); - for (const auto & interface : controller_->reference_interfaces_) - { - EXPECT_TRUE(std::isnan(interface)); - } } -// when calling update methods expect return type are a success TEST_F(MecanumDriveControllerTest, when_controller_active_and_update_called_expect_success) { SetUpController(); @@ -140,7 +132,6 @@ TEST_F(MecanumDriveControllerTest, when_controller_active_and_update_called_expe controller_interface::return_type::OK); } -// when controller lifecycle methods expect return type is a success TEST_F(MecanumDriveControllerTest, when_active_controller_is_deactivated_expect_success) { SetUpController(); @@ -150,9 +141,6 @@ TEST_F(MecanumDriveControllerTest, when_active_controller_is_deactivated_expect_ ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); } -// when calling on_activate, on_deactivate and on_activate methods consecutively -// expect resetting of reference msg, nan values in command_interfaces and -// resetting of reference msg respectively TEST_F(MecanumDriveControllerTest, when_controller_is_reactivated_expect_cmd_itfs_not_set_and_update_success) { SetUpController(); @@ -170,7 +158,7 @@ TEST_F(MecanumDriveControllerTest, when_controller_is_reactivated_expect_cmd_itf controller_interface::return_type::OK); } -// when controller state published expect state value in storage +// when update is called expect the previously set reference before calling update, inside the controller state message TEST_F(MecanumDriveControllerTest, when_update_is_called_expect_status_message) { SetUpController(); @@ -187,7 +175,8 @@ TEST_F(MecanumDriveControllerTest, when_update_is_called_expect_status_message) EXPECT_EQ(msg.reference_velocity.linear.x, 1.5); } -// when msg subscribed and published expect value in storage +// reference_interfaces and command_interfaces values depend on the reference_msg, the below test shows two cases +// when reference_msg is not received and when it is received. TEST_F(MecanumDriveControllerTest, when_reference_msg_received_expect_updated_commands_and_status_message) { SetUpController(); @@ -197,17 +186,18 @@ TEST_F(MecanumDriveControllerTest, when_reference_msg_received_expect_updated_co ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - + // no reference_msg sent ASSERT_EQ( controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); ControllerStateMsg msg; - subscribe_and_get_messages(msg); + subscribe_to_controller_status_execute_update_and_get_messages(msg); joint_command_values_[1] = command_lin_x; EXPECT_TRUE(std::isnan(msg.reference_velocity.linear.x)); // reference_callback() is implicitly called when publish_commands() is called + // reference_msg is published with provided time stamp when publish_commands( time_stamp) is called publish_commands(controller_->get_node()->now()); ASSERT_TRUE(controller_->wait_for_commands(executor)); @@ -215,20 +205,22 @@ TEST_F(MecanumDriveControllerTest, when_reference_msg_received_expect_updated_co controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - // w0_vel = 1.0 / params_.kinematics.wheels_radius * - // (body_velocity_center_frame_.linear_x - body_velocity_center_frame_.linear_y - // - params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis - // * body_velocity_center_frame_.angular_z); + // w_back_left_vel = + // 1.0 / params_.kinematics.wheels_radius * + // (velocity_in_center_frame_linear_x_ + velocity_in_center_frame_linear_y_ - + // params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis * + // velocity_in_center_frame_angular_z_); // joint_command_values_[1] = 1.0 / 0.5 * (1.5 - 0.0 - 1 * 0.0) EXPECT_EQ(joint_command_values_[1], 3.0); - subscribe_and_get_messages(msg); + subscribe_to_controller_status_execute_update_and_get_messages(msg); ASSERT_EQ(msg.reference_velocity.linear.x, 1.5); - ASSERT_EQ(msg.back_left_wheel_velocity, 3.0); + ASSERT_EQ(msg.back_left_wheel_velocity, 0.1); + ASSERT_EQ(msg.back_right_wheel_velocity, 0.1); } -// when too old msg is sent expect nan values in reference msg +// when too old msg is sent expect reference msg reset TEST_F(MecanumDriveControllerTest, when_reference_msg_is_too_old_expect_unset_reference) { SetUpController(); @@ -245,6 +237,7 @@ TEST_F(MecanumDriveControllerTest, when_reference_msg_is_too_old_expect_unset_re EXPECT_TRUE(std::isnan(reference->twist.angular.z)); // reference_callback() is implicitly called when publish_commands() is called + // reference_msg is published with provided time stamp when publish_commands( time_stamp) is called publish_commands( controller_->get_node()->now() - controller_->ref_timeout_ - rclcpp::Duration::from_seconds(0.1)); @@ -272,6 +265,7 @@ TEST_F(MecanumDriveControllerTest, when_reference_msg_has_timestamp_zero_expect_ EXPECT_TRUE(std::isnan((*reference)->twist.angular.z)); // reference_callback() is implicitly called when publish_commands() is called + // reference_msg is published with provided time stamp when publish_commands( time_stamp) is called publish_commands(rclcpp::Time(0)); ASSERT_TRUE(controller_->wait_for_commands(executor)); @@ -284,7 +278,8 @@ TEST_F(MecanumDriveControllerTest, when_reference_msg_has_timestamp_zero_expect_ EXPECT_NE((*(controller_->input_ref_.readFromNonRT()))->header.stamp.sec, 0.0); } -// when age_of_last_command < ref_timeout expect reference msg is accepted and is in rt buffer +// when the reference_msg has valid timestamp then the timeout check in reference_callback() +// shall pass and reference_msg is not reset TEST_F(MecanumDriveControllerTest, when_message_has_valid_timestamp_expect_reference_set) { SetUpController(); @@ -300,6 +295,7 @@ TEST_F(MecanumDriveControllerTest, when_message_has_valid_timestamp_expect_refer EXPECT_TRUE(std::isnan((*reference)->twist.angular.z)); // reference_callback() is implicitly called when publish_commands() is called + // reference_msg is published with provided time stamp when publish_commands( time_stamp) is called publish_commands(controller_->get_node()->now()); ASSERT_TRUE(controller_->wait_for_commands(executor)); @@ -310,12 +306,10 @@ TEST_F(MecanumDriveControllerTest, when_message_has_valid_timestamp_expect_refer EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z, 0.0); } -// when not in chainable mode and age_of_last_command > reference_timeout expect -// command_interfaces are set to 0.0 and reference_interfaces set to nan -// followed by -// when not in chainable mode and age_of_last_command < reference_timeout expect -// command_interfaces are calculated to non-nan and reference_interfaces set to nan -TEST_F(MecanumDriveControllerTest, when_reference_message_times_out_expect_commands_are_zeroed) +// when not in chainable mode and ref_msg_timedout expect +// command_interfaces are set to 0.0 and when ref_msg is not timedout expect command_interfaces are set to +// valid command values +TEST_F(MecanumDriveControllerTest, when_ref_msg_old_expect_cmnd_itfs_set_to_zero_otherwise_to_valid_cmnds) { // 1. age>ref_timeout 2. ageref_timeout_); ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); ASSERT_EQ( - controller_->update_reference_from_subscribers( - controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - - EXPECT_EQ(controller_->reference_interfaces_[1], 0); - for (const auto & interface : controller_->reference_interfaces_) - { - EXPECT_FALSE(std::isnan(interface)); - } - - ASSERT_EQ( - controller_->update_and_write_commands( - controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_->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_EQ(joint_command_values_[1], 0); @@ -397,19 +378,15 @@ TEST_F(MecanumDriveControllerTest, when_reference_message_times_out_expect_comma ASSERT_TRUE(age_of_last_command_2 <= controller_->ref_timeout_); ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); ASSERT_EQ( - controller_->update_reference_from_subscribers( - controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - ASSERT_EQ( - controller_->update_and_write_commands( - controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); EXPECT_NE(joint_command_values_[1], command_lin_x); - // w0_vel = 1.0 / params_.kinematics.wheels_radius - // * (body_velocity_center_frame_.linear_x - body_velocity_center_frame_.linear_y - // - params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis - // * body_velocity_center_frame_.angular_z); + // w_back_left_vel = + // 1.0 / params_.kinematics.wheels_radius * + // (velocity_in_center_frame_linear_x_ + velocity_in_center_frame_linear_y_ - + // params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis * + // velocity_in_center_frame_angular_z_); // joint_command_values_[1] = 1.0 / 0.5 * (1.5 - 0.0 - 1 * 0.0) EXPECT_EQ(joint_command_values_[1], 3.0); ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); @@ -424,9 +401,8 @@ TEST_F(MecanumDriveControllerTest, when_reference_message_times_out_expect_comma } } -// when in chainable mode and age_of_last_command < reference_timeout expect -// reference_interfaces set by preceding controller and command_interfaces -// are calculated to non-nan values and reference_interfaces are set to nan +// when in chained mode the reference_interfaces of chained controller and command_interfaces of preceding controller point to same +// memory location, hence reference_interfaces are not exclusively set by the update method of chained controller TEST_F(MecanumDriveControllerTest, when_controller_in_chainable_mode_expect_receiving_commands_from_reference_interfaces_directly) { SetUpController(); @@ -446,41 +422,41 @@ TEST_F(MecanumDriveControllerTest, when_controller_in_chainable_mode_expect_rece // set command statically joint_command_values_[1] = command_lin_x; - - controller_->reference_interfaces_[0] = 1.5; + // imitating preceding controllers command_interfaces setting reference_interfaces of chained controller. + controller_->reference_interfaces_[0] = 3.0; controller_->reference_interfaces_[1] = 0.0; controller_->reference_interfaces_[2] = 0.0; - const auto age_of_last_command = - controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; - - // age_of_last_command < ref_timeout_ - ASSERT_TRUE(age_of_last_command <= controller_->ref_timeout_); + // reference_callback() is implicitly called when publish_commands() is called + // reference_msg is published with provided time stamp when publish_commands( time_stamp) is called + publish_commands(controller_->get_node()->now()); ASSERT_EQ( - controller_->update_and_write_commands( - controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); EXPECT_NE(joint_command_values_[1], command_lin_x); - // w0_vel = 1.0 / params_.kinematics.wheels_radius - // * (body_velocity_center_frame_.linear_x - body_velocity_center_frame_.linear_y - // - params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis - // * body_velocity_center_frame_.angular_z); - // joint_command_values_[1] = 1.0 / 0.5 * (1.5 - 0.0 - 1 * 0.0) - EXPECT_EQ(joint_command_values_[1], 3.0); + + // w_back_left_vel = + // 1.0 / params_.kinematics.wheels_radius * + // (velocity_in_center_frame_linear_x_ + velocity_in_center_frame_linear_y_ - + // params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis * + // velocity_in_center_frame_angular_z_); + + // joint_command_values_[1] = 1.0 / 0.5 * (3.0 - 0.0 - 1 * 0.0) + EXPECT_EQ(joint_command_values_[1], 6.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(), 3.0); + EXPECT_EQ(controller_->command_interfaces_[i].get_value(), 6.0); } } -// when ref_timeout = 0 expect reference_msg is accepted and command_interfaces -// are calculated to non-nan values and reference_interfaces are set to nan +// when ref_timeout = 0 expect reference_msg is accepted only once and command_interfaces +// are calculated to valid values and reference_interfaces are unset TEST_F(MecanumDriveControllerTest, when_reference_timeout_is_zero_expect_reference_msg_being_used_only_once) { SetUpController(); @@ -489,8 +465,6 @@ TEST_F(MecanumDriveControllerTest, when_reference_timeout_is_zero_expect_referen ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - auto reference = controller_->input_ref_.readFromNonRT(); - // set command statically joint_command_values_[1] = command_lin_x; @@ -508,34 +482,25 @@ TEST_F(MecanumDriveControllerTest, when_reference_timeout_is_zero_expect_referen const auto age_of_last_command = controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; - fprintf(stderr, " age_of_last_command= %f \n", age_of_last_command); - fprintf(stderr, " controller_->ref_timeout_= %f \n", controller_->ref_timeout_); - ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_); ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); ASSERT_EQ( - controller_->update_reference_from_subscribers( - controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - ASSERT_EQ( - controller_->update_and_write_commands( - controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); EXPECT_FALSE(std::isnan(joint_command_values_[1])); EXPECT_NE(joint_command_values_[1], command_lin_x); - // w0_vel = 1.0 / params_.kinematics.wheels_radius - // * (body_velocity_center_frame_.linear_x - body_velocity_center_frame_.linear_y - // - params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis - // * body_velocity_center_frame_.angular_z); + // w_back_left_vel = + // 1.0 / params_.kinematics.wheels_radius * + // (velocity_in_center_frame_linear_x_ + velocity_in_center_frame_linear_y_ - + // params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis * + // velocity_in_center_frame_angular_z_); // joint_command_values_[1] = 1.0 / 0.5 * (1.5 - 0.0 - 1 * 0.0) EXPECT_EQ(joint_command_values_[1], 3.0); ASSERT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x)); } -// when ref_timeout = 0 expect reference_callback() writes reference_msg to rt buffer -// from nonrt thread -TEST_F(MecanumDriveControllerTest, when_ref_timeout_zero_for_reference_callback_expect_reference_msg_in_rt_buffer) +TEST_F(MecanumDriveControllerTest, when_ref_timeout_zero_for_reference_callback_expect_reference_msg_being_used_only_once) { SetUpController(); rclcpp::executors::MultiThreadedExecutor executor; @@ -549,6 +514,7 @@ TEST_F(MecanumDriveControllerTest, when_ref_timeout_zero_for_reference_callback_ controller_->ref_timeout_ = rclcpp::Duration::from_seconds(0.0); // reference_callback() is called implicitly when publish_commands() is called. + // reference_msg is published with provided time stamp when publish_commands( time_stamp) is called publish_commands(controller_->get_node()->now()); ASSERT_TRUE(controller_->wait_for_commands(executor)); diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp index e22686ae76..be662377fa 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp @@ -49,21 +49,22 @@ constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR; // subclassing and friending so we can access member variables class TestableMecanumDriveController : public mecanum_drive_controller::MecanumDriveController { - FRIEND_TEST(MecanumDriveControllerTest, when_all_parameters_are_set_expect_them_in_storage); - FRIEND_TEST(MecanumDriveControllerTest, when_exported_all_interfaces_expect_them_in_storage); - FRIEND_TEST(MecanumDriveControllerTest, when_activated_expect_reference_msg_reset); - FRIEND_TEST(MecanumDriveControllerTest, when_update_successful_expect_return_type_success); - FRIEND_TEST(MecanumDriveControllerTest, when_deactivated_expect_return_type_success); - FRIEND_TEST(MecanumDriveControllerTest, when_reactivated_expect_reference_msg_reset); - FRIEND_TEST(MecanumDriveControllerTest, when_published_success_expect_in_storage); - FRIEND_TEST(MecanumDriveControllerTest, when_subscribed_msg_received_publish_succeeded_expect_value_in_storage); - FRIEND_TEST(MecanumDriveControllerTest, when_sending_too_old_message_expect_nan_in_reference_msg); - FRIEND_TEST(MecanumDriveControllerTest, when_time_stamp_zero_expect_setting_to_current); - FRIEND_TEST(MecanumDriveControllerTest, when_message_accepted_expect_reference_msg_in_rt_buffer); - FRIEND_TEST(MecanumDriveControllerTest, test_update_logic_not_chainable_mode); - FRIEND_TEST(MecanumDriveControllerTest, test_update_logic_chainable_mode); - FRIEND_TEST(MecanumDriveControllerTest, test_ref_timeout_zero_for_update); - FRIEND_TEST(MecanumDriveControllerTest, when_ref_timeout_zero_for_reference_callback_expect_reference_msg_in_rt_buffer); + FRIEND_TEST(MecanumDriveControllerTest, when_controller_is_configured_expect_all_parameters_set); + FRIEND_TEST(MecanumDriveControllerTest, when_controller_configured_expect_properly_exported_interfaces); + FRIEND_TEST(MecanumDriveControllerTest, when_controller_is_activated_expect_reference_reset); + FRIEND_TEST(MecanumDriveControllerTest, when_controller_active_and_update_called_expect_success); + FRIEND_TEST(MecanumDriveControllerTest, when_active_controller_is_deactivated_expect_success); + FRIEND_TEST(MecanumDriveControllerTest, when_controller_is_reactivated_expect_cmd_itfs_not_set_and_update_success); + FRIEND_TEST(MecanumDriveControllerTest, when_update_is_called_expect_status_message); + FRIEND_TEST(MecanumDriveControllerTest, when_reference_msg_received_expect_updated_commands_and_status_message); + FRIEND_TEST(MecanumDriveControllerTest, when_reference_msg_is_too_old_expect_unset_reference); + FRIEND_TEST(MecanumDriveControllerTest, when_reference_msg_has_timestamp_zero_expect_reference_set_and_timestamp_set_to_current_time); + FRIEND_TEST(MecanumDriveControllerTest, when_message_has_valid_timestamp_expect_reference_set); + FRIEND_TEST(MecanumDriveControllerTest, when_ref_msg_old_expect_cmnd_itfs_set_to_zero_otherwise_to_valid_cmnds); + FRIEND_TEST(MecanumDriveControllerTest, when_controller_in_chainable_mode_expect_receiving_commands_from_reference_interfaces_directly); + FRIEND_TEST(MecanumDriveControllerTest, when_reference_timeout_is_zero_expect_reference_msg_being_used_only_once); + FRIEND_TEST(MecanumDriveControllerTest, when_ref_timeout_zero_for_reference_callback_expect_reference_msg_being_used_only_once); + public: controller_interface::CallbackReturn on_configure( @@ -242,7 +243,7 @@ class MecanumDriveControllerFixture : public ::testing::Test } protected: - std::vector reference_names_ = {"linear_x", "linear_y", "angular_z"}; + std::vector reference_interface_names = {"linear/x/velocity", "linear/y/velocity", "angular/z/velocity"}; std::vector command_joint_names_ = { "front_left_wheel_joint", "back_left_wheel_joint", "back_right_wheel_joint", "front_right_wheel_joint"}; diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller_preceeding.cpp b/mecanum_drive_controller/test/test_mecanum_drive_controller_preceeding.cpp index dbb8160f5c..5e8dcfefa1 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller_preceeding.cpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller_preceeding.cpp @@ -29,8 +29,7 @@ class MecanumDriveControllerTest { }; -// checking if all parameters are initialized and set as expected -TEST_F(MecanumDriveControllerTest, all_parameters_set_configure_success) +TEST_F(MecanumDriveControllerTest, when_controller_is_configured_expect_all_parameters_set) { SetUpController(); @@ -51,7 +50,7 @@ TEST_F(MecanumDriveControllerTest, all_parameters_set_configure_success) } // checking if all interfaces, command and state interfaces are exported as expected -TEST_F(MecanumDriveControllerTest, check_exported_intefaces) +TEST_F(MecanumDriveControllerTest, when_controller_configured_expect_properly_exported_interfaces) { SetUpController(); From 6077003661c11793c1a4c87416ac0818ff6a6e26 Mon Sep 17 00:00:00 2001 From: GiridharBukka Date: Mon, 13 Feb 2023 17:23:51 +0100 Subject: [PATCH 73/91] ran precommit --- .../src/mecanum_drive_controller.cpp | 8 +- .../test/test_mecanum_drive_controller.cpp | 73 ++++++++++++------- .../test/test_mecanum_drive_controller.hpp | 35 ++++++--- 3 files changed, 77 insertions(+), 39 deletions(-) diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index 8183d3756e..fbc3a1ccc8 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -273,13 +273,13 @@ MecanumDriveController::on_export_reference_interfaces() reference_interfaces.reserve(reference_interfaces_.size()); - std::vector reference_interface_names = {"linear/x/velocity", "linear/y/velocity", "angular/z/velocity"}; - + std::vector reference_interface_names = { + "linear/x/velocity", "linear/y/velocity", "angular/z/velocity"}; + for (size_t i = 0; i < reference_interfaces_.size(); ++i) { reference_interfaces.push_back(hardware_interface::CommandInterface( - get_node()->get_name(), reference_interface_names[i], - &reference_interfaces_[i])); + get_node()->get_name(), reference_interface_names[i], &reference_interfaces_[i])); } return reference_interfaces; diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp index 739bb9b407..296c716b2a 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp @@ -91,18 +91,18 @@ TEST_F(MecanumDriveControllerTest, when_controller_configured_expect_properly_ex EXPECT_EQ(state_intefaces.names[i], command_joint_names_[i] + "/" + interface_name_); } - // check ref itfs configuration, + // check ref itfs configuration, auto reference_interfaces = controller_->export_reference_interfaces(); ASSERT_EQ(reference_interfaces.size(), NR_REF_ITFS); for (size_t i = 0; i < reference_interface_names.size(); ++i) { - const std::string ref_itf_name = std::string(controller_->get_node()->get_name()) + std::string("/") +reference_interface_names[i]; + const std::string ref_itf_name = std::string(controller_->get_node()->get_name()) + + std::string("/") + reference_interface_names[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(), reference_interface_names[i]); + EXPECT_EQ(reference_interfaces[i].get_interface_name(), reference_interface_names[i]); } } @@ -141,7 +141,9 @@ TEST_F(MecanumDriveControllerTest, when_active_controller_is_deactivated_expect_ ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); } -TEST_F(MecanumDriveControllerTest, when_controller_is_reactivated_expect_cmd_itfs_not_set_and_update_success) +TEST_F( + MecanumDriveControllerTest, + when_controller_is_reactivated_expect_cmd_itfs_not_set_and_update_success) { SetUpController(); @@ -158,7 +160,8 @@ TEST_F(MecanumDriveControllerTest, when_controller_is_reactivated_expect_cmd_itf controller_interface::return_type::OK); } -// when update is called expect the previously set reference before calling update, inside the controller state message +// when update is called expect the previously set reference before calling update, +// inside the controller state message TEST_F(MecanumDriveControllerTest, when_update_is_called_expect_status_message) { SetUpController(); @@ -175,9 +178,11 @@ TEST_F(MecanumDriveControllerTest, when_update_is_called_expect_status_message) EXPECT_EQ(msg.reference_velocity.linear.x, 1.5); } -// reference_interfaces and command_interfaces values depend on the reference_msg, the below test shows two cases -// when reference_msg is not received and when it is received. -TEST_F(MecanumDriveControllerTest, when_reference_msg_received_expect_updated_commands_and_status_message) +// reference_interfaces and command_interfaces values depend on the reference_msg, +// the below test shows two cases when reference_msg is not received and when it is received. +TEST_F( + MecanumDriveControllerTest, + when_reference_msg_received_expect_updated_commands_and_status_message) { SetUpController(); @@ -197,7 +202,8 @@ TEST_F(MecanumDriveControllerTest, when_reference_msg_received_expect_updated_co EXPECT_TRUE(std::isnan(msg.reference_velocity.linear.x)); // reference_callback() is implicitly called when publish_commands() is called - // reference_msg is published with provided time stamp when publish_commands( time_stamp) is called + // reference_msg is published with provided time stamp when publish_commands( time_stamp) + // is called publish_commands(controller_->get_node()->now()); ASSERT_TRUE(controller_->wait_for_commands(executor)); @@ -237,7 +243,8 @@ TEST_F(MecanumDriveControllerTest, when_reference_msg_is_too_old_expect_unset_re EXPECT_TRUE(std::isnan(reference->twist.angular.z)); // reference_callback() is implicitly called when publish_commands() is called - // reference_msg is published with provided time stamp when publish_commands( time_stamp) is called + // reference_msg is published with provided time stamp when publish_commands( time_stamp) + // is called publish_commands( controller_->get_node()->now() - controller_->ref_timeout_ - rclcpp::Duration::from_seconds(0.1)); @@ -249,7 +256,9 @@ TEST_F(MecanumDriveControllerTest, when_reference_msg_is_too_old_expect_unset_re } // when time stamp is zero expect that time stamp is set to current time stamp -TEST_F(MecanumDriveControllerTest, when_reference_msg_has_timestamp_zero_expect_reference_set_and_timestamp_set_to_current_time) +TEST_F( + MecanumDriveControllerTest, + when_reference_msg_has_timestamp_zero_expect_reference_set_and_timestamp_set_to_current_time) { SetUpController(); rclcpp::executors::MultiThreadedExecutor executor; @@ -265,7 +274,8 @@ TEST_F(MecanumDriveControllerTest, when_reference_msg_has_timestamp_zero_expect_ EXPECT_TRUE(std::isnan((*reference)->twist.angular.z)); // reference_callback() is implicitly called when publish_commands() is called - // reference_msg is published with provided time stamp when publish_commands( time_stamp) is called + // reference_msg is published with provided time stamp when publish_commands( time_stamp) + // is called publish_commands(rclcpp::Time(0)); ASSERT_TRUE(controller_->wait_for_commands(executor)); @@ -295,7 +305,8 @@ TEST_F(MecanumDriveControllerTest, when_message_has_valid_timestamp_expect_refer EXPECT_TRUE(std::isnan((*reference)->twist.angular.z)); // reference_callback() is implicitly called when publish_commands() is called - // reference_msg is published with provided time stamp when publish_commands( time_stamp) is called + // reference_msg is published with provided time stamp when publish_commands( time_stamp) + // is called publish_commands(controller_->get_node()->now()); ASSERT_TRUE(controller_->wait_for_commands(executor)); @@ -307,9 +318,11 @@ TEST_F(MecanumDriveControllerTest, when_message_has_valid_timestamp_expect_refer } // when not in chainable mode and ref_msg_timedout expect -// command_interfaces are set to 0.0 and when ref_msg is not timedout expect command_interfaces are set to -// valid command values -TEST_F(MecanumDriveControllerTest, when_ref_msg_old_expect_cmnd_itfs_set_to_zero_otherwise_to_valid_cmnds) +// command_interfaces are set to 0.0 and when ref_msg is not timedout expect +// command_interfaces are set to valid command values +TEST_F( + MecanumDriveControllerTest, + when_ref_msg_old_expect_cmnd_itfs_set_to_zero_otherwise_to_valid_cmnds) { // 1. age>ref_timeout 2. agereference_interfaces_[0] = 3.0; controller_->reference_interfaces_[1] = 0.0; controller_->reference_interfaces_[2] = 0.0; // reference_callback() is implicitly called when publish_commands() is called - // reference_msg is published with provided time stamp when publish_commands( time_stamp) is called + // reference_msg is published with provided time stamp when publish_commands( time_stamp) + // is called publish_commands(controller_->get_node()->now()); ASSERT_EQ( @@ -457,7 +475,9 @@ TEST_F(MecanumDriveControllerTest, when_controller_in_chainable_mode_expect_rece // when ref_timeout = 0 expect reference_msg is accepted only once and command_interfaces // are calculated to valid values and reference_interfaces are unset -TEST_F(MecanumDriveControllerTest, when_reference_timeout_is_zero_expect_reference_msg_being_used_only_once) +TEST_F( + MecanumDriveControllerTest, + when_reference_timeout_is_zero_expect_reference_msg_being_used_only_once) { SetUpController(); rclcpp::executors::MultiThreadedExecutor executor; @@ -500,7 +520,9 @@ TEST_F(MecanumDriveControllerTest, when_reference_timeout_is_zero_expect_referen ASSERT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x)); } -TEST_F(MecanumDriveControllerTest, when_ref_timeout_zero_for_reference_callback_expect_reference_msg_being_used_only_once) +TEST_F( + MecanumDriveControllerTest, + when_ref_timeout_zero_for_reference_callback_expect_reference_msg_being_used_only_once) { SetUpController(); rclcpp::executors::MultiThreadedExecutor executor; @@ -514,7 +536,8 @@ TEST_F(MecanumDriveControllerTest, when_ref_timeout_zero_for_reference_callback_ controller_->ref_timeout_ = rclcpp::Duration::from_seconds(0.0); // reference_callback() is called implicitly when publish_commands() is called. - // reference_msg is published with provided time stamp when publish_commands( time_stamp) is called + // reference_msg is published with provided time stamp when publish_commands( time_stamp) + // is called publish_commands(controller_->get_node()->now()); ASSERT_TRUE(controller_->wait_for_commands(executor)); diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp index be662377fa..dd9a7b358c 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp @@ -50,21 +50,35 @@ constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR; class TestableMecanumDriveController : public mecanum_drive_controller::MecanumDriveController { FRIEND_TEST(MecanumDriveControllerTest, when_controller_is_configured_expect_all_parameters_set); - FRIEND_TEST(MecanumDriveControllerTest, when_controller_configured_expect_properly_exported_interfaces); + FRIEND_TEST( + MecanumDriveControllerTest, when_controller_configured_expect_properly_exported_interfaces); FRIEND_TEST(MecanumDriveControllerTest, when_controller_is_activated_expect_reference_reset); FRIEND_TEST(MecanumDriveControllerTest, when_controller_active_and_update_called_expect_success); FRIEND_TEST(MecanumDriveControllerTest, when_active_controller_is_deactivated_expect_success); - FRIEND_TEST(MecanumDriveControllerTest, when_controller_is_reactivated_expect_cmd_itfs_not_set_and_update_success); + FRIEND_TEST( + MecanumDriveControllerTest, + when_controller_is_reactivated_expect_cmd_itfs_not_set_and_update_success); FRIEND_TEST(MecanumDriveControllerTest, when_update_is_called_expect_status_message); - FRIEND_TEST(MecanumDriveControllerTest, when_reference_msg_received_expect_updated_commands_and_status_message); + FRIEND_TEST( + MecanumDriveControllerTest, + when_reference_msg_received_expect_updated_commands_and_status_message); FRIEND_TEST(MecanumDriveControllerTest, when_reference_msg_is_too_old_expect_unset_reference); - FRIEND_TEST(MecanumDriveControllerTest, when_reference_msg_has_timestamp_zero_expect_reference_set_and_timestamp_set_to_current_time); + FRIEND_TEST( + MecanumDriveControllerTest, + when_reference_msg_has_timestamp_zero_expect_reference_set_and_timestamp_set_to_current_time); FRIEND_TEST(MecanumDriveControllerTest, when_message_has_valid_timestamp_expect_reference_set); - FRIEND_TEST(MecanumDriveControllerTest, when_ref_msg_old_expect_cmnd_itfs_set_to_zero_otherwise_to_valid_cmnds); - FRIEND_TEST(MecanumDriveControllerTest, when_controller_in_chainable_mode_expect_receiving_commands_from_reference_interfaces_directly); - FRIEND_TEST(MecanumDriveControllerTest, when_reference_timeout_is_zero_expect_reference_msg_being_used_only_once); - FRIEND_TEST(MecanumDriveControllerTest, when_ref_timeout_zero_for_reference_callback_expect_reference_msg_being_used_only_once); - + FRIEND_TEST( + MecanumDriveControllerTest, + when_ref_msg_old_expect_cmnd_itfs_set_to_zero_otherwise_to_valid_cmnds); + FRIEND_TEST( + MecanumDriveControllerTest, + when_controller_in_chainable_mode_expect_receiving_commands_from_reference_interfaces_directly); + FRIEND_TEST( + MecanumDriveControllerTest, + when_reference_timeout_is_zero_expect_reference_msg_being_used_only_once); + FRIEND_TEST( + MecanumDriveControllerTest, + when_ref_timeout_zero_for_reference_callback_expect_reference_msg_being_used_only_once); public: controller_interface::CallbackReturn on_configure( @@ -243,7 +257,8 @@ class MecanumDriveControllerFixture : public ::testing::Test } protected: - std::vector reference_interface_names = {"linear/x/velocity", "linear/y/velocity", "angular/z/velocity"}; + std::vector reference_interface_names = { + "linear/x/velocity", "linear/y/velocity", "angular/z/velocity"}; std::vector command_joint_names_ = { "front_left_wheel_joint", "back_left_wheel_joint", "back_right_wheel_joint", "front_right_wheel_joint"}; From f00cd5e0517e9a933dbab4cb23dec0a99e2df982 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Tue, 11 Apr 2023 18:39:06 +0200 Subject: [PATCH 74/91] Update mecanum_drive_controller/test/test_mecanum_drive_controller.cpp --- mecanum_drive_controller/test/test_mecanum_drive_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp index 296c716b2a..4a92319a34 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp @@ -439,7 +439,7 @@ TEST_F( // set command statically joint_command_values_[1] = command_lin_x; // imitating preceding controllers command_interfaces setting reference_interfaces of chained - //controller. + // controller. controller_->reference_interfaces_[0] = 3.0; controller_->reference_interfaces_[1] = 0.0; controller_->reference_interfaces_[2] = 0.0; From 8e853c766cded8b1cd27b18539ba9e5ab9af6b50 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Tue, 11 Apr 2023 19:41:55 +0200 Subject: [PATCH 75/91] Add missing nav_msgs dependecdy --- mecanum_drive_controller/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/mecanum_drive_controller/CMakeLists.txt b/mecanum_drive_controller/CMakeLists.txt index 5168fac7ae..4e930ccb72 100644 --- a/mecanum_drive_controller/CMakeLists.txt +++ b/mecanum_drive_controller/CMakeLists.txt @@ -10,6 +10,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS controller_interface hardware_interface generate_parameter_library + nav_msgs pluginlib rclcpp rclcpp_lifecycle From 3815bcfb02fa44fcc068303a93748df5d15038ce Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Tue, 11 Apr 2023 19:55:46 +0200 Subject: [PATCH 76/91] Remove odometry field from status message --- .../src/mecanum_drive_controller.cpp | 16 ---------------- 1 file changed, 16 deletions(-) diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index fbc3a1ccc8..89b2b6f289 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -188,16 +188,6 @@ controller_interface::CallbackReturn MecanumDriveController::on_configure( controller_state_publisher_->lock(); controller_state_publisher_->msg_.header.stamp = get_node()->now(); controller_state_publisher_->msg_.header.frame_id = params_.odom_frame_id; - controller_state_publisher_->msg_.odometry.child_frame_id = params_.base_frame_id; - controller_state_publisher_->msg_.odometry.pose.pose.position.x = 0.0; - auto & covariance_controller = controller_state_publisher_->msg_.odometry.twist.covariance; - for (size_t index = 0; index < 6; ++index) - { - // 0, 7, 14, 21, 28, 35 - const size_t diagonal_index = NUM_DIMENSIONS * index + index; - covariance_controller[diagonal_index] = params_.pose_covariance_diagonal[index]; - covariance_controller[diagonal_index] = params_.twist_covariance_diagonal[index]; - } controller_state_publisher_->unlock(); RCLCPP_INFO(get_node()->get_logger(), "configure successful"); @@ -470,12 +460,6 @@ controller_interface::return_type MecanumDriveController::update_and_write_comma if (controller_state_publisher_->trylock()) { controller_state_publisher_->msg_.header.stamp = get_node()->now(); - controller_state_publisher_->msg_.odometry.pose.pose.position.x = odometry_.getX(); - controller_state_publisher_->msg_.odometry.pose.pose.position.y = odometry_.getY(); - controller_state_publisher_->msg_.odometry.pose.pose.orientation = tf2::toMsg(orientation); - controller_state_publisher_->msg_.odometry.twist.twist.linear.x = odometry_.getVx(); - controller_state_publisher_->msg_.odometry.twist.twist.linear.y = odometry_.getVy(); - controller_state_publisher_->msg_.odometry.twist.twist.angular.z = odometry_.getWz(); controller_state_publisher_->msg_.front_left_wheel_velocity = state_interfaces_[0].get_value(); controller_state_publisher_->msg_.back_left_wheel_velocity = state_interfaces_[1].get_value(); controller_state_publisher_->msg_.back_right_wheel_velocity = state_interfaces_[2].get_value(); From 5033c9dd6a03699533e71075a6a567f7114c3874 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Tue, 11 Apr 2023 20:15:33 +0200 Subject: [PATCH 77/91] Apply suggestions from code review --- mecanum_drive_controller/test/test_mecanum_drive_controller.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp index 4a92319a34..34009f2d7f 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp @@ -174,7 +174,6 @@ TEST_F(MecanumDriveControllerTest, when_update_is_called_expect_status_message) ControllerStateMsg msg; subscribe_to_controller_status_execute_update_and_get_messages(msg); - EXPECT_NEAR(msg.odometry.pose.pose.position.y, 0.0, EPS); EXPECT_EQ(msg.reference_velocity.linear.x, 1.5); } From bd3bfc0d0dd8affb11301d1acc48a2832a9bcbab Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Sun, 14 May 2023 20:05:19 +0100 Subject: [PATCH 78/91] Fix a few typos and rephrase MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Christoph Fröhlich --- mecanum_drive_controller/doc/userdoc.rst | 8 +++++--- .../src/mecanum_drive_controller.yaml | 4 ++-- 2 files changed, 7 insertions(+), 5 deletions(-) diff --git a/mecanum_drive_controller/doc/userdoc.rst b/mecanum_drive_controller/doc/userdoc.rst index f056cf2a8c..c244155b72 100644 --- a/mecanum_drive_controller/doc/userdoc.rst +++ b/mecanum_drive_controller/doc/userdoc.rst @@ -32,7 +32,9 @@ Commands States ,,,,,,, - / [double] # in [rad] or [rad/s] - **NOTE**: ``joint_name`` can be ``state_joint_names`` parameter of if empty ``command_joint_names``. + ..note :: + + ``joint_names[i]`` can be of ``state_joint_names`` parameter (if used), ``command_joint_names`` otherwise. Subscribers @@ -50,6 +52,6 @@ Publishers Parameters ,,,,,,,,,,, -For list of parameters and their meaning YAML file in the ``src`` folder of the controller's package. +For list of parameters and their meaning, see the YAML file in the ``src`` folder of the controller's package. -For an exameplary parameterization see the ``test`` folder of the controller's package. +For an exemplary parameterization see the ``test`` folder of the controller's package. diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.yaml b/mecanum_drive_controller/src/mecanum_drive_controller.yaml index da0c1ffb99..577240e5be 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.yaml +++ b/mecanum_drive_controller/src/mecanum_drive_controller.yaml @@ -85,13 +85,13 @@ mecanum_drive_controller: twist_covariance_diagonal: { type: double_array, default_value: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1], - description: "diagonal values of twist covariance matrix.", + description: "Diagonal values of twist covariance matrix.", read_only: false, } pose_covariance_diagonal: { type: double_array, default_value: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1], - description: "diagonal values of pose covariance matrix.", + description: "Diagonal values of pose covariance matrix.", read_only: false, } From f060d177ba6513d8dd72a2be27aec9fd5bc1dc51 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Mon, 28 Aug 2023 18:51:19 +0200 Subject: [PATCH 79/91] Correct controller loading test --- .../test/test_load_mecanum_drive_controller.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/mecanum_drive_controller/test/test_load_mecanum_drive_controller.cpp b/mecanum_drive_controller/test/test_load_mecanum_drive_controller.cpp index 34bada08dd..cfab634376 100644 --- a/mecanum_drive_controller/test/test_load_mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/test/test_load_mecanum_drive_controller.cpp @@ -34,8 +34,11 @@ TEST(TestLoadMecanumDriveController, when_loading_controller_expect_no_exception ros2_control_test_assets::minimal_robot_urdf), executor, "test_controller_manager"); - ASSERT_NO_THROW(cm.load_controller( - "test_mecanum_drive_controller", "mecanum_drive_controller/MecanumDriveController")); + ASSERT_NE( + cm.load_controller( + "test_mecanum_drive_controller", + "mecanum_drive_controller/MecanumDriveController"), + nullptr); rclcpp::shutdown(); } From 0a5e56356e921d34b32993f5f5dea01476aea357 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Mon, 28 Aug 2023 18:54:23 +0200 Subject: [PATCH 80/91] Update mecanum_drive_controller/src/odometry.cpp --- mecanum_drive_controller/src/odometry.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/mecanum_drive_controller/src/odometry.cpp b/mecanum_drive_controller/src/odometry.cpp index eb2e584371..40650cc359 100644 --- a/mecanum_drive_controller/src/odometry.cpp +++ b/mecanum_drive_controller/src/odometry.cpp @@ -53,7 +53,6 @@ bool Odometry::update( if (dt < 0.0001) return false; // Interval too small to integrate with /// Compute FK (i.e. compute mobile robot's body twist out of its wheels velocities): - /// NOTE: we use the IK of the mecanum wheels which we invert using a pseudo-inverse. /// NOTE: the mecanum IK gives the body speed at the center frame, we then offset this velocity /// at the base frame. /// NOTE: in the diff drive the velocity is filtered out, but we prefer to return it raw and From bc87e31b328c92c1eb9bdba8d39087721ebd6861 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Mon, 28 Aug 2023 18:57:05 +0200 Subject: [PATCH 81/91] Update docs with note about odometry calculation. --- mecanum_drive_controller/doc/userdoc.rst | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/mecanum_drive_controller/doc/userdoc.rst b/mecanum_drive_controller/doc/userdoc.rst index c244155b72..3aca53a859 100644 --- a/mecanum_drive_controller/doc/userdoc.rst +++ b/mecanum_drive_controller/doc/userdoc.rst @@ -3,7 +3,7 @@ mecanum_drive_controller ========================= -Library with shared functionalities for mobile robots controllers with mecanum drive (four wheels). +Library with shared functionalities for mobile robot controllers with mecanum drive (four wheels). The library implements generic odometry and update methods and defines the main interfaces. Execution logic of the controller @@ -11,12 +11,16 @@ Execution logic of the controller The controller uses velocity input, i.e., stamped or unstamped Twist messages where linear ``x``, ``y``, and angular ``z`` components are used. Values in other components are ignored. -In the chain mode the controller provides three reference interfaces, one for linear velocity and one for steering angle position. +In the chain mode, the controller provides three reference interfaces, one for linear velocity and one for steering angle position. Other relevant features are: - odometry publishing as Odometry and TF message; - input command timeout based on a parameter. +Note about odometry calculation: +In the DiffDRiveController, the velocity is filtered out, but we prefer to return it raw and let the user perform post-processing at will. +We prefer this way of doing so as filtering introduces delay (which makes it difficult to interpret and compare behavior curves). + Description of controller's interfaces -------------------------------------- @@ -39,7 +43,7 @@ States Subscribers ,,,,,,,,,,,, -Used when controller is not in chained mode (``in_chained_mode == false``). +Used when the controller is not in chained mode (``in_chained_mode == false``). - /reference [geometry_msgs/msg/TwistStamped] @@ -52,6 +56,6 @@ Publishers Parameters ,,,,,,,,,,, -For list of parameters and their meaning, see the YAML file in the ``src`` folder of the controller's package. +For a list of parameters and their meaning, see the YAML file in the ``src`` folder of the controller's package. -For an exemplary parameterization see the ``test`` folder of the controller's package. +For an exemplary parameterization, see the ``test`` folder of the controller's package. From 93d1bf19f7879e7716662c7ab7da44319b798d85 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Mon, 28 Aug 2023 18:58:01 +0200 Subject: [PATCH 82/91] Update mecanum_drive_controller/src/mecanum_drive_controller.yaml --- mecanum_drive_controller/src/mecanum_drive_controller.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.yaml b/mecanum_drive_controller/src/mecanum_drive_controller.yaml index 577240e5be..225f62414f 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.yaml +++ b/mecanum_drive_controller/src/mecanum_drive_controller.yaml @@ -78,7 +78,7 @@ mecanum_drive_controller: enable_odom_tf: { type: bool, default_value: true, - description: "Publishing to tf is enabled or disabled ?.", + description: "Publishing to tf is enabled or disabled?", read_only: false, } From b2605a128148268e2f698fd2a98b3318461c5c82 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= Date: Mon, 17 Jun 2024 13:02:46 +0200 Subject: [PATCH 83/91] Fix mixing of joints and fix to velocity only interface. --- .../mecanum_drive_controller.hpp | 18 ++++- .../src/mecanum_drive_controller.cpp | 56 +++++++------- .../src/mecanum_drive_controller.yaml | 74 +++++++++++++++---- .../test/mecanum_drive_controller_params.yaml | 14 +--- ...um_drive_controller_preceeding_params.yaml | 19 ++--- .../test/test_mecanum_drive_controller.cpp | 27 +++++-- .../test/test_mecanum_drive_controller.hpp | 20 +++-- ...st_mecanum_drive_controller_preceeding.cpp | 28 +++++-- 8 files changed, 174 insertions(+), 82 deletions(-) diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp index 144b295610..847483cfc9 100644 --- a/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp +++ b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp @@ -92,7 +92,23 @@ class MecanumDriveController : public controller_interface::ChainableControllerI std::shared_ptr param_listener_; mecanum_drive_controller::Params params_; - // used for chained controller + /// Internal lists with joint names. + /** + * The list is sorted in the following order: + * - front left wheel + * - front right wheel + * - back right wheel + * - back left wheel + */ + std::vector command_joint_names_; + /// Internal lists with joint names. + /** + * The list is sorted in the following order: + * - front left wheel + * - front right wheel + * - back right wheel + * - back left wheel + */ std::vector state_joint_names_; // Names of the references, ex: high level vel commands from MoveIt, Nav2, etc. diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index 89b2b6f289..afaad73291 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -74,24 +74,24 @@ controller_interface::CallbackReturn MecanumDriveController::on_configure( { params_ = param_listener_->get_params(); - if (!params_.state_joint_names.empty()) + auto prepare_lists_with_joint_names = [&command_joints = this->command_joint_names_, &state_joints = this->state_joint_names_](const std::string & command_joint_name, const std::string & state_joint_name) { - state_joint_names_ = params_.state_joint_names; - } - else - { - state_joint_names_ = params_.command_joint_names; - } + command_joints.push_back(command_joint_name); + if (state_joint_name.empty()) + { + state_joints.push_back(command_joint_name); + } + else + { + state_joints.push_back(state_joint_name); + } + }; - if (params_.command_joint_names.size() != state_joint_names_.size()) - { - RCLCPP_FATAL( - get_node()->get_logger(), - "Size of 'joints' (%ld) and 'state_joint_names' (%ld) parameters has " - "to be the same!", - params_.command_joint_names.size(), state_joint_names_.size()); - return CallbackReturn::FAILURE; - } + // The joint names are sorted accoring to the order documented in the header file! + prepare_lists_with_joint_names(params_.front_left_wheel_command_joint_name, params_.front_left_wheel_state_joint_name); + prepare_lists_with_joint_names(params_.front_right_wheel_command_joint_name, params_.front_right_wheel_state_joint_name); + prepare_lists_with_joint_names(params_.rear_right_wheel_command_joint_name, params_.rear_right_wheel_state_joint_name); + prepare_lists_with_joint_names(params_.rear_left_wheel_command_joint_name, params_.rear_left_wheel_state_joint_name); // Set wheel params for the odometry computation odometry_.setWheelsParams( @@ -229,10 +229,10 @@ MecanumDriveController::command_interface_configuration() const controller_interface::InterfaceConfiguration command_interfaces_config; command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; - command_interfaces_config.names.reserve(params_.command_joint_names.size()); - for (const auto & joint : params_.command_joint_names) + command_interfaces_config.names.reserve(command_joint_names_.size()); + for (const auto & joint : command_joint_names_) { - command_interfaces_config.names.push_back(joint + "/" + params_.interface_name); + command_interfaces_config.names.push_back(joint + "/" + hardware_interface::HW_IF_VELOCITY); } return command_interfaces_config; @@ -248,7 +248,7 @@ controller_interface::InterfaceConfiguration MecanumDriveController::state_inter for (const auto & joint : state_joint_names_) { - state_interfaces_config.names.push_back(joint + "/" + params_.interface_name); + state_interfaces_config.names.push_back(joint + "/" + hardware_interface::HW_IF_VELOCITY); } return state_interfaces_config; @@ -393,32 +393,32 @@ controller_interface::return_type MecanumDriveController::update_and_write_comma linear_trans_from_base_to_center.x() * reference_interfaces_[2]; velocity_in_center_frame_angular_z_ = reference_interfaces_[2]; - double w_front_left_vel = + const double w_front_left_vel = 1.0 / params_.kinematics.wheels_radius * (velocity_in_center_frame_linear_x_ - velocity_in_center_frame_linear_y_ - params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis * velocity_in_center_frame_angular_z_); - double w_back_left_vel = + const double w_front_right_vel = 1.0 / params_.kinematics.wheels_radius * - (velocity_in_center_frame_linear_x_ + velocity_in_center_frame_linear_y_ - + (velocity_in_center_frame_linear_x_ + velocity_in_center_frame_linear_y_ + params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis * velocity_in_center_frame_angular_z_); - double w_back_right_vel = + const double w_back_right_vel = 1.0 / params_.kinematics.wheels_radius * (velocity_in_center_frame_linear_x_ - velocity_in_center_frame_linear_y_ + params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis * velocity_in_center_frame_angular_z_); - double w_front_right_vel = + const double w_back_left_vel = 1.0 / params_.kinematics.wheels_radius * - (velocity_in_center_frame_linear_x_ + velocity_in_center_frame_linear_y_ + + (velocity_in_center_frame_linear_x_ + velocity_in_center_frame_linear_y_ - params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis * velocity_in_center_frame_angular_z_); // Set wheels velocities: command_interfaces_[0].set_value(w_front_left_vel); - command_interfaces_[1].set_value(w_back_left_vel); + command_interfaces_[1].set_value(w_front_right_vel); command_interfaces_[2].set_value(w_back_right_vel); - command_interfaces_[3].set_value(w_front_right_vel); + command_interfaces_[3].set_value(w_back_left_vel); } else { diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.yaml b/mecanum_drive_controller/src/mecanum_drive_controller.yaml index 225f62414f..88627c0f60 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.yaml +++ b/mecanum_drive_controller/src/mecanum_drive_controller.yaml @@ -5,24 +5,73 @@ mecanum_drive_controller: description: "Timeout for controller references after which they will be reset. This is especially useful for controllers that can cause unwanted and dangerous behavior if reference is not reset, e.g., velocity controllers. If value is 0 the reference is reset after each run.", } - command_joint_names: { - type: string_array, - default_value: [], - description: "Name of the wheels joints.", - read_only: false, + # Command joint names + front_left_wheel_command_joint_name: { + type: string, + default_value: "", + description: "Name of the joints for commanding front left wheel.", + read_only: true, + validation: { + not_empty<>: null, + } + } + + front_right_wheel_command_joint_name: { + type: string, + default_value: "", + description: "Name of the joints for commanding front right wheel.", + read_only: true, + validation: { + not_empty<>: null, + } } - state_joint_names: { - type: string_array, - default_value: [], - description: "(optional) Specifies joints for reading states. This parameter is only relevant when state joints are different then command joint, i.e., when a following controller is used.", + rear_right_wheel_command_joint_name: { + type: string, + default_value: "", + description: "Name of the joints for commanding rear right wheel.", + read_only: true, + validation: { + not_empty<>: null, + } + } + + rear_left_wheel_command_joint_name: { + type: string, + default_value: "", + description: "Name of the joints for commanding rear left wheel.", + read_only: true, + validation: { + not_empty<>: null, + } + } + + # State joint names + front_left_wheel_state_joint_name: { + type: string, + default_value: "", + description: "(optional) Specifies a joint name for reading states of front left wheel. This parameter is only relevant when state joints are different then command joint, i.e., when a following controller is used.", + read_only: true, + } + + front_right_wheel_state_joint_name: { + type: string, + default_value: "", + description: "(optional) Specifies a joint name for reading states of front right wheel. This parameter is only relevant when state joints are different then command joint, i.e., when a following controller is used.", + read_only: true, + } + + rear_right_wheel_state_joint_name: { + type: string, + default_value: "", + description: "(optional) Specifies a joint name for reading states of rear right wheel. This parameter is only relevant when state joints are different then command joint, i.e., when a following controller is used.", read_only: true, } - interface_name: { + rear_left_wheel_state_joint_name: { type: string, default_value: "", - description: "Name of the interface used by the controller for sending commands, reading states and getting references.", + description: "(optional) Specifies a joint name for reading states of rear left wheel. This parameter is only relevant when state joints are different then command joint, i.e., when a following controller is used.", read_only: true, } @@ -57,8 +106,7 @@ mecanum_drive_controller: sum_of_robot_center_projection_on_X_Y_axis: { type: double, default_value: 0.0, - description: "Wheels geometric param used in mecanum wheels' IK. lx and ly represent the distance from the robot's center to the wheels projected on - the x and y axis with origin at robots center respectively, sum_of_robot_center_projection_on_X_Y_axis = lx+ly", + description: "Wheels geometric param used in mecanum wheels' IK. lx and ly represent the distance from the robot's center to the wheels projected on the x and y axis with origin at robots center respectively, sum_of_robot_center_projection_on_X_Y_axis = lx+ly", read_only: false, } diff --git a/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml b/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml index 9eec559dd1..bc97335dd5 100644 --- a/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml +++ b/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml @@ -1,25 +1,19 @@ test_mecanum_drive_controller: ros__parameters: - reference_timeout: 0.1 - command_joint_names: ["front_left_wheel_joint", "back_left_wheel_joint", "back_right_wheel_joint", "front_right_wheel_joint"] - - interface_name: velocity + front_left_wheel_command_joint_name: "front_left_wheel_joint" + front_right_wheel_command_joint_name: "front_right_wheel_joint" + rear_right_wheel_command_joint_name: "back_right_wheel_joint" + rear_left_wheel_command_joint_name: "back_left_wheel_joint" kinematics: base_frame_offset: { x: 0.0, y: 0.0, theta: 0.0 } - wheels_radius: 0.5 - sum_of_robot_center_projection_on_X_Y_axis: 1.0 base_frame_id: "base_link" - odom_frame_id: "odom" - enable_odom_tf: true - twist_covariance_diagonal: [0.0, 7.0, 14.0, 21.0, 28.0, 35.0] - pose_covariance_diagonal: [0.0, 7.0, 14.0, 21.0, 28.0, 35.0] diff --git a/mecanum_drive_controller/test/mecanum_drive_controller_preceeding_params.yaml b/mecanum_drive_controller/test/mecanum_drive_controller_preceeding_params.yaml index 459a515eb1..c4e5bb391a 100644 --- a/mecanum_drive_controller/test/mecanum_drive_controller_preceeding_params.yaml +++ b/mecanum_drive_controller/test/mecanum_drive_controller_preceeding_params.yaml @@ -1,27 +1,24 @@ test_mecanum_drive_controller: ros__parameters: - reference_timeout: 0.1 - command_joint_names: ["front_left_wheel_joint", "back_left_wheel_joint", "back_right_wheel_joint", "front_right_wheel_joint"] - - state_joint_names: ["state_front_left_wheel_joint", "state_back_left_wheel_joint", "state_back_right_wheel_joint", "state_front_right_wheel_joint"] + front_left_wheel_command_joint_name: "front_left_wheel_joint" + front_right_wheel_command_joint_name: "front_right_wheel_joint" + rear_right_wheel_command_joint_name: "back_right_wheel_joint" + rear_left_wheel_command_joint_name: "back_left_wheel_joint" - interface_name: velocity + front_left_wheel_state_joint_name: "state_front_left_wheel_joint" + front_right_wheel_state_joint_name: "state_front_right_wheel_joint" + rear_right_wheel_state_joint_name: "state_back_right_wheel_joint" + rear_left_wheel_state_joint_name: "state_back_left_wheel_joint" kinematics: base_frame_offset: { x: 0.0, y: 0.0, theta: 0.0 } - wheels_radius: 0.5 - sum_of_robot_center_projection_on_X_Y_axis: 1.0 base_frame_id: "base_link" - odom_frame_id: "odom" - enable_odom_tf: true - twist_covariance_diagonal: [0.0, 7.0, 14.0, 21.0, 28.0, 35.0] - pose_covariance_diagonal: [0.0, 7.0, 14.0, 21.0, 28.0, 35.0] diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp index 34009f2d7f..83ec710741 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp @@ -49,9 +49,14 @@ TEST_F(MecanumDriveControllerTest, when_controller_is_configured_expect_all_para ASSERT_EQ(controller_->params_.kinematics.base_frame_offset.y, 0.0); ASSERT_EQ(controller_->params_.kinematics.base_frame_offset.theta, 0.0); - ASSERT_TRUE(controller_->params_.command_joint_names.empty()); - ASSERT_TRUE(controller_->params_.state_joint_names.empty()); - ASSERT_TRUE(controller_->params_.interface_name.empty()); + ASSERT_TRUE(controller_->params_.front_left_wheel_command_joint_name.empty()); + ASSERT_TRUE(controller_->params_.front_right_wheel_command_joint_name.empty()); + ASSERT_TRUE(controller_->params_.rear_right_wheel_command_joint_name.empty()); + ASSERT_TRUE(controller_->params_.rear_left_wheel_command_joint_name.empty()); + ASSERT_TRUE(controller_->params_.front_left_wheel_state_joint_name.empty()); + ASSERT_TRUE(controller_->params_.front_right_wheel_state_joint_name.empty()); + ASSERT_TRUE(controller_->params_.rear_right_wheel_state_joint_name.empty()); + ASSERT_TRUE(controller_->params_.rear_left_wheel_state_joint_name.empty()); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); @@ -63,10 +68,18 @@ TEST_F(MecanumDriveControllerTest, when_controller_is_configured_expect_all_para ASSERT_EQ(controller_->params_.kinematics.base_frame_offset.y, 0.0); ASSERT_EQ(controller_->params_.kinematics.base_frame_offset.theta, 0.0); - ASSERT_THAT( - controller_->params_.command_joint_names, testing::ElementsAreArray(command_joint_names_)); - ASSERT_TRUE(controller_->params_.state_joint_names.empty()); - ASSERT_EQ(controller_->params_.interface_name, interface_name_); + ASSERT_EQ(controller_->params_.front_left_wheel_command_joint_name, TEST_FRONT_LEFT_CMD_JOINT_NAME); + ASSERT_EQ(controller_->params_.front_right_wheel_command_joint_name, TEST_FRONT_RIGHT_CMD_JOINT_NAME); + ASSERT_EQ(controller_->params_.rear_right_wheel_command_joint_name, TEST_REAR_RIGHT_CMD_JOINT_NAME); + ASSERT_EQ(controller_->params_.rear_left_wheel_command_joint_name, TEST_REAR_LEFT_CMD_JOINT_NAME); + ASSERT_THAT(controller_->command_joint_names_, testing::ElementsAreArray(command_joint_names_)); + + // When state joint names are empty they are the same as the command joint names + ASSERT_TRUE(controller_->params_.front_left_wheel_state_joint_name.empty()); + ASSERT_TRUE(controller_->params_.front_right_wheel_state_joint_name.empty()); + ASSERT_TRUE(controller_->params_.rear_right_wheel_state_joint_name.empty()); + ASSERT_TRUE(controller_->params_.rear_left_wheel_state_joint_name.empty()); + ASSERT_THAT(controller_->state_joint_names_, testing::ElementsAreArray(command_joint_names_)); } TEST_F(MecanumDriveControllerTest, when_controller_configured_expect_properly_exported_interfaces) diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp index dd9a7b358c..d2c0a79b0f 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp @@ -27,6 +27,7 @@ #include "hardware_interface/loaned_command_interface.hpp" #include "hardware_interface/loaned_state_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" #include "mecanum_drive_controller/mecanum_drive_controller.hpp" #include "rclcpp/parameter_value.hpp" #include "rclcpp/time.hpp" @@ -259,14 +260,23 @@ class MecanumDriveControllerFixture : public ::testing::Test protected: std::vector reference_interface_names = { "linear/x/velocity", "linear/y/velocity", "angular/z/velocity"}; + + static constexpr char TEST_FRONT_LEFT_CMD_JOINT_NAME[] = "front_left_wheel_joint"; + static constexpr char TEST_FRONT_RIGHT_CMD_JOINT_NAME[] = "front_right_wheel_joint"; + static constexpr char TEST_REAR_RIGHT_CMD_JOINT_NAME[] = "back_right_wheel_joint"; + static constexpr char TEST_REAR_LEFT_CMD_JOINT_NAME[] = "back_left_wheel_joint"; std::vector command_joint_names_ = { - "front_left_wheel_joint", "back_left_wheel_joint", "back_right_wheel_joint", - "front_right_wheel_joint"}; + TEST_FRONT_LEFT_CMD_JOINT_NAME, TEST_FRONT_RIGHT_CMD_JOINT_NAME, TEST_REAR_RIGHT_CMD_JOINT_NAME, + TEST_REAR_LEFT_CMD_JOINT_NAME}; + static constexpr char TEST_FRONT_LEFT_STATE_JOINT_NAME[] = "state_front_left_wheel_joint"; + static constexpr char TEST_FRONT_RIGHT_STATE_JOINT_NAME[] = "state_front_right_wheel_joint"; + static constexpr char TEST_REAR_RIGHT_STATE_JOINT_NAME[] = "state_back_right_wheel_joint"; + static constexpr char TEST_REAR_LEFT_STATE_JOINT_NAME[] = "state_back_left_wheel_joint"; std::vector state_joint_names_ = { - "state_front_left_wheel_joint", "state_back_left_wheel_joint", "state_back_right_wheel_joint", - "state_front_right_wheel_joint"}; - std::string interface_name_ = "velocity"; + TEST_FRONT_LEFT_STATE_JOINT_NAME, TEST_FRONT_RIGHT_STATE_JOINT_NAME, + TEST_REAR_RIGHT_STATE_JOINT_NAME, TEST_REAR_LEFT_STATE_JOINT_NAME}; + std::string interface_name_ = hardware_interface::HW_IF_VELOCITY; // Controller-related parameters diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller_preceeding.cpp b/mecanum_drive_controller/test/test_mecanum_drive_controller_preceeding.cpp index 5e8dcfefa1..232e902429 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller_preceeding.cpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller_preceeding.cpp @@ -34,19 +34,33 @@ TEST_F(MecanumDriveControllerTest, when_controller_is_configured_expect_all_para SetUpController(); ASSERT_EQ(controller_->params_.reference_timeout, 0.0); - ASSERT_TRUE(controller_->params_.command_joint_names.empty()); - ASSERT_TRUE(controller_->params_.state_joint_names.empty()); - ASSERT_TRUE(controller_->params_.interface_name.empty()); + + ASSERT_TRUE(controller_->params_.front_left_wheel_command_joint_name.empty()); + ASSERT_TRUE(controller_->params_.front_right_wheel_command_joint_name.empty()); + ASSERT_TRUE(controller_->params_.rear_right_wheel_command_joint_name.empty()); + ASSERT_TRUE(controller_->params_.rear_left_wheel_command_joint_name.empty()); + ASSERT_TRUE(controller_->params_.front_left_wheel_state_joint_name.empty()); + ASSERT_TRUE(controller_->params_.front_right_wheel_state_joint_name.empty()); + ASSERT_TRUE(controller_->params_.rear_right_wheel_state_joint_name.empty()); + ASSERT_TRUE(controller_->params_.rear_left_wheel_state_joint_name.empty()); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(controller_->params_.reference_timeout, 0.1); + + ASSERT_EQ(controller_->params_.front_left_wheel_command_joint_name, TEST_FRONT_LEFT_CMD_JOINT_NAME); + ASSERT_EQ(controller_->params_.front_right_wheel_command_joint_name, TEST_FRONT_RIGHT_CMD_JOINT_NAME); + ASSERT_EQ(controller_->params_.rear_right_wheel_command_joint_name, TEST_REAR_RIGHT_CMD_JOINT_NAME); + ASSERT_EQ(controller_->params_.rear_left_wheel_command_joint_name, TEST_REAR_LEFT_CMD_JOINT_NAME); ASSERT_THAT( - controller_->params_.command_joint_names, testing::ElementsAreArray(command_joint_names_)); + controller_->command_joint_names_, testing::ElementsAreArray(command_joint_names_)); + + ASSERT_EQ(controller_->params_.front_left_wheel_state_joint_name, TEST_FRONT_LEFT_STATE_JOINT_NAME); + ASSERT_EQ(controller_->params_.front_right_wheel_state_joint_name, TEST_FRONT_RIGHT_STATE_JOINT_NAME); + ASSERT_EQ(controller_->params_.rear_right_wheel_state_joint_name, TEST_REAR_RIGHT_STATE_JOINT_NAME); + ASSERT_EQ(controller_->params_.rear_left_wheel_state_joint_name, TEST_REAR_LEFT_STATE_JOINT_NAME); ASSERT_THAT( - controller_->params_.state_joint_names, testing::ElementsAreArray(state_joint_names_)); - ASSERT_THAT(controller_->state_joint_names_, testing::ElementsAreArray(state_joint_names_)); - ASSERT_EQ(controller_->params_.interface_name, interface_name_); + controller_->state_joint_names_, testing::ElementsAreArray(state_joint_names_)); } // checking if all interfaces, command and state interfaces are exported as expected From 50cd77c1de645ab8e21d02acf1db41a14fe2bb4b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= Date: Mon, 17 Jun 2024 13:04:49 +0200 Subject: [PATCH 84/91] Fix formatting and typos. --- mecanum_drive_controller/doc/userdoc.rst | 2 +- .../src/mecanum_drive_controller.cpp | 18 ++++++++----- .../test_load_mecanum_drive_controller.cpp | 3 +-- .../test/test_mecanum_drive_controller.cpp | 9 ++++--- ...st_mecanum_drive_controller_preceeding.cpp | 26 +++++++++++-------- 5 files changed, 35 insertions(+), 23 deletions(-) diff --git a/mecanum_drive_controller/doc/userdoc.rst b/mecanum_drive_controller/doc/userdoc.rst index 3aca53a859..e58ba3c2b0 100644 --- a/mecanum_drive_controller/doc/userdoc.rst +++ b/mecanum_drive_controller/doc/userdoc.rst @@ -37,7 +37,7 @@ States ,,,,,,, - / [double] # in [rad] or [rad/s] ..note :: - + ``joint_names[i]`` can be of ``state_joint_names`` parameter (if used), ``command_joint_names`` otherwise. diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index afaad73291..025c45bb2a 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -74,7 +74,9 @@ controller_interface::CallbackReturn MecanumDriveController::on_configure( { params_ = param_listener_->get_params(); - auto prepare_lists_with_joint_names = [&command_joints = this->command_joint_names_, &state_joints = this->state_joint_names_](const std::string & command_joint_name, const std::string & state_joint_name) + auto prepare_lists_with_joint_names = + [&command_joints = this->command_joint_names_, &state_joints = this->state_joint_names_]( + const std::string & command_joint_name, const std::string & state_joint_name) { command_joints.push_back(command_joint_name); if (state_joint_name.empty()) @@ -87,11 +89,15 @@ controller_interface::CallbackReturn MecanumDriveController::on_configure( } }; - // The joint names are sorted accoring to the order documented in the header file! - prepare_lists_with_joint_names(params_.front_left_wheel_command_joint_name, params_.front_left_wheel_state_joint_name); - prepare_lists_with_joint_names(params_.front_right_wheel_command_joint_name, params_.front_right_wheel_state_joint_name); - prepare_lists_with_joint_names(params_.rear_right_wheel_command_joint_name, params_.rear_right_wheel_state_joint_name); - prepare_lists_with_joint_names(params_.rear_left_wheel_command_joint_name, params_.rear_left_wheel_state_joint_name); + // The joint names are sorted according to the order documented in the header file! + prepare_lists_with_joint_names( + params_.front_left_wheel_command_joint_name, params_.front_left_wheel_state_joint_name); + prepare_lists_with_joint_names( + params_.front_right_wheel_command_joint_name, params_.front_right_wheel_state_joint_name); + prepare_lists_with_joint_names( + params_.rear_right_wheel_command_joint_name, params_.rear_right_wheel_state_joint_name); + prepare_lists_with_joint_names( + params_.rear_left_wheel_command_joint_name, params_.rear_left_wheel_state_joint_name); // Set wheel params for the odometry computation odometry_.setWheelsParams( diff --git a/mecanum_drive_controller/test/test_load_mecanum_drive_controller.cpp b/mecanum_drive_controller/test/test_load_mecanum_drive_controller.cpp index cfab634376..36d0b679f2 100644 --- a/mecanum_drive_controller/test/test_load_mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/test/test_load_mecanum_drive_controller.cpp @@ -36,8 +36,7 @@ TEST(TestLoadMecanumDriveController, when_loading_controller_expect_no_exception ASSERT_NE( cm.load_controller( - "test_mecanum_drive_controller", - "mecanum_drive_controller/MecanumDriveController"), + "test_mecanum_drive_controller", "mecanum_drive_controller/MecanumDriveController"), nullptr); rclcpp::shutdown(); diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp index 83ec710741..9d49e344dd 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp @@ -68,9 +68,12 @@ TEST_F(MecanumDriveControllerTest, when_controller_is_configured_expect_all_para ASSERT_EQ(controller_->params_.kinematics.base_frame_offset.y, 0.0); ASSERT_EQ(controller_->params_.kinematics.base_frame_offset.theta, 0.0); - ASSERT_EQ(controller_->params_.front_left_wheel_command_joint_name, TEST_FRONT_LEFT_CMD_JOINT_NAME); - ASSERT_EQ(controller_->params_.front_right_wheel_command_joint_name, TEST_FRONT_RIGHT_CMD_JOINT_NAME); - ASSERT_EQ(controller_->params_.rear_right_wheel_command_joint_name, TEST_REAR_RIGHT_CMD_JOINT_NAME); + ASSERT_EQ( + controller_->params_.front_left_wheel_command_joint_name, TEST_FRONT_LEFT_CMD_JOINT_NAME); + ASSERT_EQ( + controller_->params_.front_right_wheel_command_joint_name, TEST_FRONT_RIGHT_CMD_JOINT_NAME); + ASSERT_EQ( + controller_->params_.rear_right_wheel_command_joint_name, TEST_REAR_RIGHT_CMD_JOINT_NAME); ASSERT_EQ(controller_->params_.rear_left_wheel_command_joint_name, TEST_REAR_LEFT_CMD_JOINT_NAME); ASSERT_THAT(controller_->command_joint_names_, testing::ElementsAreArray(command_joint_names_)); diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller_preceeding.cpp b/mecanum_drive_controller/test/test_mecanum_drive_controller_preceeding.cpp index 232e902429..edbee8a702 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller_preceeding.cpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller_preceeding.cpp @@ -48,19 +48,23 @@ TEST_F(MecanumDriveControllerTest, when_controller_is_configured_expect_all_para ASSERT_EQ(controller_->params_.reference_timeout, 0.1); - ASSERT_EQ(controller_->params_.front_left_wheel_command_joint_name, TEST_FRONT_LEFT_CMD_JOINT_NAME); - ASSERT_EQ(controller_->params_.front_right_wheel_command_joint_name, TEST_FRONT_RIGHT_CMD_JOINT_NAME); - ASSERT_EQ(controller_->params_.rear_right_wheel_command_joint_name, TEST_REAR_RIGHT_CMD_JOINT_NAME); + ASSERT_EQ( + controller_->params_.front_left_wheel_command_joint_name, TEST_FRONT_LEFT_CMD_JOINT_NAME); + ASSERT_EQ( + controller_->params_.front_right_wheel_command_joint_name, TEST_FRONT_RIGHT_CMD_JOINT_NAME); + ASSERT_EQ( + controller_->params_.rear_right_wheel_command_joint_name, TEST_REAR_RIGHT_CMD_JOINT_NAME); ASSERT_EQ(controller_->params_.rear_left_wheel_command_joint_name, TEST_REAR_LEFT_CMD_JOINT_NAME); - ASSERT_THAT( - controller_->command_joint_names_, testing::ElementsAreArray(command_joint_names_)); - - ASSERT_EQ(controller_->params_.front_left_wheel_state_joint_name, TEST_FRONT_LEFT_STATE_JOINT_NAME); - ASSERT_EQ(controller_->params_.front_right_wheel_state_joint_name, TEST_FRONT_RIGHT_STATE_JOINT_NAME); - ASSERT_EQ(controller_->params_.rear_right_wheel_state_joint_name, TEST_REAR_RIGHT_STATE_JOINT_NAME); + ASSERT_THAT(controller_->command_joint_names_, testing::ElementsAreArray(command_joint_names_)); + + ASSERT_EQ( + controller_->params_.front_left_wheel_state_joint_name, TEST_FRONT_LEFT_STATE_JOINT_NAME); + ASSERT_EQ( + controller_->params_.front_right_wheel_state_joint_name, TEST_FRONT_RIGHT_STATE_JOINT_NAME); + ASSERT_EQ( + controller_->params_.rear_right_wheel_state_joint_name, TEST_REAR_RIGHT_STATE_JOINT_NAME); ASSERT_EQ(controller_->params_.rear_left_wheel_state_joint_name, TEST_REAR_LEFT_STATE_JOINT_NAME); - ASSERT_THAT( - controller_->state_joint_names_, testing::ElementsAreArray(state_joint_names_)); + ASSERT_THAT(controller_->state_joint_names_, testing::ElementsAreArray(state_joint_names_)); } // checking if all interfaces, command and state interfaces are exported as expected From 98b1e59738d25d59795ad0470bc9930ec32dd98e Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Wed, 19 Jun 2024 21:25:11 +0200 Subject: [PATCH 85/91] Update mecanum_drive_controller/src/mecanum_drive_controller.yaml MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Christoph Fröhlich --- mecanum_drive_controller/src/mecanum_drive_controller.yaml | 3 +++ 1 file changed, 3 insertions(+) diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.yaml b/mecanum_drive_controller/src/mecanum_drive_controller.yaml index 88627c0f60..896b731953 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.yaml +++ b/mecanum_drive_controller/src/mecanum_drive_controller.yaml @@ -101,6 +101,9 @@ mecanum_drive_controller: default_value: 0.0, description: "Wheel's radius.", read_only: false, + validation: { + gt<>: [0.0] + } } sum_of_robot_center_projection_on_X_Y_axis: { From a34c665b68051dfbdc613fbfdfb721bb1897ee30 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= Date: Wed, 19 Jun 2024 22:43:29 +0200 Subject: [PATCH 86/91] Update the use explict parameters for the wheels. --- mecanum_drive_controller/CMakeLists.txt | 10 +++++----- .../test/test_load_mecanum_drive_controller.cpp | 13 ++++++++++--- .../test/test_mecanum_drive_controller.hpp | 6 +++++- 3 files changed, 20 insertions(+), 9 deletions(-) diff --git a/mecanum_drive_controller/CMakeLists.txt b/mecanum_drive_controller/CMakeLists.txt index 4e930ccb72..6ff338e75e 100644 --- a/mecanum_drive_controller/CMakeLists.txt +++ b/mecanum_drive_controller/CMakeLists.txt @@ -58,12 +58,12 @@ if(BUILD_TESTING) find_package(hardware_interface REQUIRED) find_package(ros2_control_test_assets REQUIRED) - ament_add_gmock(test_load_mecanum_drive_controller test/test_load_mecanum_drive_controller.cpp) - target_include_directories(test_load_mecanum_drive_controller PRIVATE include) - ament_target_dependencies( - test_load_mecanum_drive_controller + add_rostest_with_parameters_gmock(test_load_mecanum_drive_controller + test/test_load_mecanum_drive_controller.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/mecanum_drive_controller_params.yaml + ) + ament_target_dependencies(test_load_mecanum_drive_controller controller_manager - hardware_interface ros2_control_test_assets ) diff --git a/mecanum_drive_controller/test/test_load_mecanum_drive_controller.cpp b/mecanum_drive_controller/test/test_load_mecanum_drive_controller.cpp index 36d0b679f2..52ec07ded0 100644 --- a/mecanum_drive_controller/test/test_load_mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/test/test_load_mecanum_drive_controller.cpp @@ -22,10 +22,8 @@ #include "rclcpp/utilities.hpp" #include "ros2_control_test_assets/descriptions.hpp" -TEST(TestLoadMecanumDriveController, when_loading_controller_expect_no_exception) +TEST(TestLoadMecanumDriveController, load_controller) { - rclcpp::init(0, nullptr); - std::shared_ptr executor = std::make_shared(); @@ -41,3 +39,12 @@ TEST(TestLoadMecanumDriveController, when_loading_controller_expect_no_exception rclcpp::shutdown(); } + +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/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp index d2c0a79b0f..8c6fdfc44d 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp @@ -163,7 +163,11 @@ class MecanumDriveControllerFixture : public ::testing::Test protected: void SetUpController(const std::string controller_name = "test_mecanum_drive_controller") { - ASSERT_EQ(controller_->init(controller_name), controller_interface::return_type::OK); + const auto urdf = ""; + const auto ns = ""; + ASSERT_EQ( + controller_->init(controller_name, urdf, 0, ns, controller_->define_custom_node_options()), + controller_interface::return_type::OK); std::vector command_ifs; command_itfs_.reserve(joint_command_values_.size()); From 3da18a85a643fd866c30987573947f7ef9061324 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Thu, 27 Jun 2024 15:46:09 +0200 Subject: [PATCH 87/91] Update .pre-commit-config.yaml --- .pre-commit-config.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index bea2ea0374..c75acbd76d 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -116,7 +116,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$ From 1f58f8daf170952f83e920e285433576ff71bd2a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= Date: Tue, 2 Jul 2024 18:58:03 +0200 Subject: [PATCH 88/91] Fix ordering of wheels to be correct. --- .../mecanum_drive_controller.hpp | 22 ++++-- .../mecanum_drive_controller/odometry.hpp | 10 +-- .../src/mecanum_drive_controller.cpp | 73 +++++++++---------- mecanum_drive_controller/src/odometry.cpp | 12 +-- 4 files changed, 62 insertions(+), 55 deletions(-) diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp index 847483cfc9..9f588fb2bf 100644 --- a/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp +++ b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp @@ -92,7 +92,6 @@ class MecanumDriveController : public controller_interface::ChainableControllerI std::shared_ptr param_listener_; mecanum_drive_controller::Params params_; - /// Internal lists with joint names. /** * The list is sorted in the following order: * - front left wheel @@ -100,14 +99,25 @@ class MecanumDriveController : public controller_interface::ChainableControllerI * - back right wheel * - back left wheel */ + enum WheelIndex : std::size_t + { + FRONT_LEFT = 0, + FRONT_RIGHT = 1, + REAR_RIGHT = 2, + REAR_LEFT = 3 + }; + + /// Internal lists with joint names. + /** + * Internal lists with joint names sorted as in `WheelIndex` enum. + */ std::vector command_joint_names_; + /// Internal lists with joint names. /** - * The list is sorted in the following order: - * - front left wheel - * - front right wheel - * - back right wheel - * - back left wheel + * Internal lists with joint names sorted as in `WheelIndex` enum. + * If parameters for state joint names are *not* defined, this list is the same as + * `command_joint_names_`. */ std::vector state_joint_names_; diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp b/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp index 8167759301..d1b380b353 100644 --- a/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp +++ b/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp @@ -42,14 +42,14 @@ class Odometry /// \brief Updates the odometry class with latest wheels position /// \param wheel_front_left_vel Wheel velocity [rad/s] - /// \param wheel_back_left_vel Wheel velocity [rad/s] - /// \param wheel_back_right_vel Wheel velocity [rad/s] + /// \param wheel_rear_left_vel Wheel velocity [rad/s] + /// \param wheel_rear_right_vel Wheel velocity [rad/s] /// \param wheel_front_right_vel Wheel velocity [rad/s] /// \param time Current time /// \return true if the odometry is actually updated bool update( - double wheel_front_left_vel, double wheel_back_left_vel, double wheel_back_right_vel, - double wheel_front_right_vel, const double dt); + const double wheel_front_left_vel, const double wheel_rear_left_vel, const double wheel_rear_right_vel, + const double wheel_front_right_vel, const double dt); /// \return position (x component) [m] double getX() const { return position_x_in_base_frame_; } @@ -72,7 +72,7 @@ class Odometry /// \param sum_of_robot_center_projection_on_X_Y_axis Wheels geometric param /// (used in mecanum wheels' ik) [m] /// \param wheels_radius Wheels radius [m] - void setWheelsParams(double sum_of_robot_center_projection_on_X_Y_axis, double wheels_radius); + void setWheelsParams(const double sum_of_robot_center_projection_on_X_Y_axis, const double wheels_radius); private: /// Current timestamp: diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index 025c45bb2a..3197dae2ab 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -74,30 +74,27 @@ controller_interface::CallbackReturn MecanumDriveController::on_configure( { params_ = param_listener_->get_params(); - auto prepare_lists_with_joint_names = - [&command_joints = this->command_joint_names_, &state_joints = this->state_joint_names_]( - const std::string & command_joint_name, const std::string & state_joint_name) + auto prepare_lists_with_joint_names = [&command_joints = this->command_joint_names_, &state_joints = this->state_joint_names_](const std::size_t index, const std::string & command_joint_name, const std::string & state_joint_name) { - command_joints.push_back(command_joint_name); + command_joints[index] = command_joint_name; if (state_joint_name.empty()) { - state_joints.push_back(command_joint_name); + state_joints[index] = command_joint_name; } else { - state_joints.push_back(state_joint_name); + state_joints[index] = state_joint_name; } }; + command_joint_names_.resize(4); + state_joint_names_.resize(4); + // The joint names are sorted according to the order documented in the header file! - prepare_lists_with_joint_names( - params_.front_left_wheel_command_joint_name, params_.front_left_wheel_state_joint_name); - prepare_lists_with_joint_names( - params_.front_right_wheel_command_joint_name, params_.front_right_wheel_state_joint_name); - prepare_lists_with_joint_names( - params_.rear_right_wheel_command_joint_name, params_.rear_right_wheel_state_joint_name); - prepare_lists_with_joint_names( - params_.rear_left_wheel_command_joint_name, params_.rear_left_wheel_state_joint_name); + prepare_lists_with_joint_names(FRONT_LEFT, params_.front_left_wheel_command_joint_name, params_.front_left_wheel_state_joint_name); + prepare_lists_with_joint_names(FRONT_RIGHT, params_.front_right_wheel_command_joint_name, params_.front_right_wheel_state_joint_name); + prepare_lists_with_joint_names(REAR_RIGHT, params_.rear_right_wheel_command_joint_name, params_.rear_right_wheel_state_joint_name); + prepare_lists_with_joint_names(REAR_LEFT, params_.rear_left_wheel_command_joint_name, params_.rear_left_wheel_state_joint_name); // Set wheel params for the odometry computation odometry_.setWheelsParams( @@ -354,18 +351,18 @@ controller_interface::return_type MecanumDriveController::update_and_write_comma const rclcpp::Time & time, const rclcpp::Duration & period) { // FORWARD KINEMATICS (odometry). - double wheel_front_left_vel = state_interfaces_[0].get_value(); - double wheel_back_left_vel = state_interfaces_[1].get_value(); - double wheel_back_right_vel = state_interfaces_[2].get_value(); - double wheel_front_right_vel = state_interfaces_[3].get_value(); + const double wheel_front_left_state_vel = state_interfaces_[FRONT_LEFT].get_value(); + const double wheel_front_right_state_vel = state_interfaces_[FRONT_RIGHT].get_value(); + const double wheel_rear_right_state_vel = state_interfaces_[REAR_RIGHT].get_value(); + const double wheel_rear_left_state_vel = state_interfaces_[REAR_LEFT].get_value(); if ( - !std::isnan(wheel_front_left_vel) && !std::isnan(wheel_back_left_vel) && - !std::isnan(wheel_back_right_vel) && !std::isnan(wheel_front_right_vel)) + !std::isnan(wheel_front_left_state_vel) && !std::isnan(wheel_rear_left_state_vel) && + !std::isnan(wheel_rear_right_state_vel) && !std::isnan(wheel_front_right_state_vel)) { // Estimate twist (using joint information) and integrate odometry_.update( - wheel_front_left_vel, wheel_back_left_vel, wheel_back_right_vel, wheel_front_right_vel, + wheel_front_left_state_vel, wheel_rear_left_state_vel, wheel_rear_right_state_vel, wheel_front_right_state_vel, period.seconds()); } @@ -399,39 +396,39 @@ controller_interface::return_type MecanumDriveController::update_and_write_comma linear_trans_from_base_to_center.x() * reference_interfaces_[2]; velocity_in_center_frame_angular_z_ = reference_interfaces_[2]; - const double w_front_left_vel = + const double wheel_front_left_vel = 1.0 / params_.kinematics.wheels_radius * (velocity_in_center_frame_linear_x_ - velocity_in_center_frame_linear_y_ - params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis * velocity_in_center_frame_angular_z_); - const double w_front_right_vel = + const double wheel_front_right_vel = 1.0 / params_.kinematics.wheels_radius * (velocity_in_center_frame_linear_x_ + velocity_in_center_frame_linear_y_ + params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis * velocity_in_center_frame_angular_z_); - const double w_back_right_vel = + const double wheel_rear_right_vel = 1.0 / params_.kinematics.wheels_radius * (velocity_in_center_frame_linear_x_ - velocity_in_center_frame_linear_y_ + params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis * velocity_in_center_frame_angular_z_); - const double w_back_left_vel = + const double wheel_rear_left_vel = 1.0 / params_.kinematics.wheels_radius * (velocity_in_center_frame_linear_x_ + velocity_in_center_frame_linear_y_ - params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis * velocity_in_center_frame_angular_z_); - // Set wheels velocities: - command_interfaces_[0].set_value(w_front_left_vel); - command_interfaces_[1].set_value(w_front_right_vel); - command_interfaces_[2].set_value(w_back_right_vel); - command_interfaces_[3].set_value(w_back_left_vel); + // Set wheels velocities - The joint names are sorted accoring to the order documented in the header file! + command_interfaces_[FRONT_LEFT].set_value(wheel_front_left_vel); + command_interfaces_[FRONT_RIGHT].set_value(wheel_front_right_vel); + command_interfaces_[REAR_RIGHT].set_value(wheel_rear_right_vel); + command_interfaces_[REAR_LEFT].set_value(wheel_rear_left_vel); } else { - command_interfaces_[0].set_value(0.0); - command_interfaces_[1].set_value(0.0); - command_interfaces_[2].set_value(0.0); - command_interfaces_[3].set_value(0.0); + command_interfaces_[FRONT_LEFT].set_value(0.0); + command_interfaces_[FRONT_RIGHT].set_value(0.0); + command_interfaces_[REAR_RIGHT].set_value(0.0); + command_interfaces_[REAR_LEFT].set_value(0.0); } // Publish odometry message @@ -466,10 +463,10 @@ controller_interface::return_type MecanumDriveController::update_and_write_comma if (controller_state_publisher_->trylock()) { controller_state_publisher_->msg_.header.stamp = get_node()->now(); - controller_state_publisher_->msg_.front_left_wheel_velocity = state_interfaces_[0].get_value(); - controller_state_publisher_->msg_.back_left_wheel_velocity = state_interfaces_[1].get_value(); - controller_state_publisher_->msg_.back_right_wheel_velocity = state_interfaces_[2].get_value(); - controller_state_publisher_->msg_.front_right_wheel_velocity = state_interfaces_[3].get_value(); + controller_state_publisher_->msg_.front_left_wheel_velocity = state_interfaces_[FRONT_LEFT].get_value(); + controller_state_publisher_->msg_.front_right_wheel_velocity = state_interfaces_[FRONT_RIGHT].get_value(); + controller_state_publisher_->msg_.back_right_wheel_velocity = state_interfaces_[REAR_RIGHT].get_value(); + controller_state_publisher_->msg_.back_left_wheel_velocity = state_interfaces_[REAR_LEFT].get_value(); controller_state_publisher_->msg_.reference_velocity.linear.x = reference_interfaces_[0]; controller_state_publisher_->msg_.reference_velocity.linear.y = reference_interfaces_[1]; controller_state_publisher_->msg_.reference_velocity.angular.z = reference_interfaces_[2]; diff --git a/mecanum_drive_controller/src/odometry.cpp b/mecanum_drive_controller/src/odometry.cpp index 40650cc359..32b3bb0dbd 100644 --- a/mecanum_drive_controller/src/odometry.cpp +++ b/mecanum_drive_controller/src/odometry.cpp @@ -45,8 +45,8 @@ void Odometry::init( } bool Odometry::update( - double wheel_front_left_vel, double wheel_back_left_vel, double wheel_back_right_vel, - double wheel_front_right_vel, const double dt) + const double wheel_front_left_vel, const double wheel_rear_left_vel, const double wheel_rear_right_vel, + const double wheel_front_right_vel, const double dt) { /// We cannot estimate the speed with very small time intervals: // const double dt = (time - timestamp_).toSec(); @@ -68,13 +68,13 @@ bool Odometry::update( double velocity_in_center_frame_linear_x = 0.25 * wheels_radius_ * - (wheel_front_left_vel + wheel_back_left_vel + wheel_back_right_vel + wheel_front_right_vel); + (wheel_front_left_vel + wheel_rear_left_vel + wheel_rear_right_vel + wheel_front_right_vel); double velocity_in_center_frame_linear_y = 0.25 * wheels_radius_ * - (-wheel_front_left_vel + wheel_back_left_vel - wheel_back_right_vel + wheel_front_right_vel); + (-wheel_front_left_vel + wheel_rear_left_vel - wheel_rear_right_vel + wheel_front_right_vel); double velocity_in_center_frame_angular_z = 0.25 * wheels_radius_ / sum_of_robot_center_projection_on_X_Y_axis_ * - (-wheel_front_left_vel - wheel_back_left_vel + wheel_back_right_vel + wheel_front_right_vel); + (-wheel_front_left_vel - wheel_rear_left_vel + wheel_rear_right_vel + wheel_front_right_vel); tf2::Quaternion orientation_R_c_b; orientation_R_c_b.setRPY(0.0, 0.0, -base_frame_offset_[2]); @@ -115,7 +115,7 @@ bool Odometry::update( } void Odometry::setWheelsParams( - double sum_of_robot_center_projection_on_X_Y_axis, double wheels_radius) + const double sum_of_robot_center_projection_on_X_Y_axis, const double wheels_radius) { sum_of_robot_center_projection_on_X_Y_axis_ = sum_of_robot_center_projection_on_X_Y_axis; wheels_radius_ = wheels_radius; From 4852699337cebd0fcb8f600d7a4df27913960f30 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Wed, 3 Jul 2024 19:46:31 +0200 Subject: [PATCH 89/91] Update mecanum_drive_controller/doc/userdoc.rst --- mecanum_drive_controller/doc/userdoc.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mecanum_drive_controller/doc/userdoc.rst b/mecanum_drive_controller/doc/userdoc.rst index e58ba3c2b0..37c8d7d0e7 100644 --- a/mecanum_drive_controller/doc/userdoc.rst +++ b/mecanum_drive_controller/doc/userdoc.rst @@ -9,7 +9,7 @@ The library implements generic odometry and update methods and defines the main Execution logic of the controller ---------------------------------- -The controller uses velocity input, i.e., stamped or unstamped Twist messages where linear ``x``, ``y``, and angular ``z`` components are used. +The controller uses velocity input, i.e., stamped Twist messages where linear ``x``, ``y``, and angular ``z`` components are used. Values in other components are ignored. In the chain mode, the controller provides three reference interfaces, one for linear velocity and one for steering angle position. Other relevant features are: From 57fac95f381d28d367d3463d575960e5fb176342 Mon Sep 17 00:00:00 2001 From: luis-camero <88782189+luis-camero@users.noreply.github.com> Date: Wed, 31 Jul 2024 12:50:07 -0400 Subject: [PATCH 90/91] Odometry Position Frame (#21) * Add hardware interface testing dependency * Use orientation w.r.t. odom to set position --- mecanum_drive_controller/package.xml | 1 + mecanum_drive_controller/src/odometry.cpp | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/mecanum_drive_controller/package.xml b/mecanum_drive_controller/package.xml index aa718b57f0..fc6c03ad8b 100644 --- a/mecanum_drive_controller/package.xml +++ b/mecanum_drive_controller/package.xml @@ -35,6 +35,7 @@ ament_cmake_gmock controller_manager + hardware_interface_testing ros2_control_test_assets diff --git a/mecanum_drive_controller/src/odometry.cpp b/mecanum_drive_controller/src/odometry.cpp index 32b3bb0dbd..e5760f18e6 100644 --- a/mecanum_drive_controller/src/odometry.cpp +++ b/mecanum_drive_controller/src/odometry.cpp @@ -101,7 +101,7 @@ bool Odometry::update( orientation_z_in_base_frame_ += velocity_in_base_frame_angular_z * dt; tf2::Quaternion orientation_R_b_odom; - orientation_R_b_odom.setRPY(0.0, 0.0, -base_frame_offset_[2]); + orientation_R_b_odom.setRPY(0.0, 0.0, orientation_z_in_base_frame_); tf2::Matrix3x3 angular_transformation_from_base_2_odom = tf2::Matrix3x3((orientation_R_b_odom)); tf2::Vector3 velocity_in_base_frame_w_r_t_odom_frame_ = From 355fae3c99aaeaf1ec69b52c009040e2c084e698 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Wed, 21 Aug 2024 17:14:12 +0200 Subject: [PATCH 91/91] Fix mecanum (#23) * Fix tests * Fix format --- .../mecanum_drive_controller/odometry.hpp | 7 +-- .../src/mecanum_drive_controller.cpp | 48 +++++++++++------ mecanum_drive_controller/src/odometry.cpp | 4 +- .../test_load_mecanum_drive_controller.cpp | 4 +- .../test/test_mecanum_drive_controller.cpp | 10 ++-- .../test/test_mecanum_drive_controller.hpp | 51 +++++++++---------- 6 files changed, 69 insertions(+), 55 deletions(-) diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp b/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp index d1b380b353..ac1ad060dd 100644 --- a/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp +++ b/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp @@ -48,8 +48,8 @@ class Odometry /// \param time Current time /// \return true if the odometry is actually updated bool update( - const double wheel_front_left_vel, const double wheel_rear_left_vel, const double wheel_rear_right_vel, - const double wheel_front_right_vel, const double dt); + const double wheel_front_left_vel, const double wheel_rear_left_vel, + const double wheel_rear_right_vel, const double wheel_front_right_vel, const double dt); /// \return position (x component) [m] double getX() const { return position_x_in_base_frame_; } @@ -72,7 +72,8 @@ class Odometry /// \param sum_of_robot_center_projection_on_X_Y_axis Wheels geometric param /// (used in mecanum wheels' ik) [m] /// \param wheels_radius Wheels radius [m] - void setWheelsParams(const double sum_of_robot_center_projection_on_X_Y_axis, const double wheels_radius); + void setWheelsParams( + const double sum_of_robot_center_projection_on_X_Y_axis, const double wheels_radius); private: /// Current timestamp: diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index 3197dae2ab..7c77cd9eb4 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -74,7 +74,10 @@ controller_interface::CallbackReturn MecanumDriveController::on_configure( { params_ = param_listener_->get_params(); - auto prepare_lists_with_joint_names = [&command_joints = this->command_joint_names_, &state_joints = this->state_joint_names_](const std::size_t index, const std::string & command_joint_name, const std::string & state_joint_name) + auto prepare_lists_with_joint_names = + [&command_joints = this->command_joint_names_, &state_joints = this->state_joint_names_]( + const std::size_t index, const std::string & command_joint_name, + const std::string & state_joint_name) { command_joints[index] = command_joint_name; if (state_joint_name.empty()) @@ -91,10 +94,18 @@ controller_interface::CallbackReturn MecanumDriveController::on_configure( state_joint_names_.resize(4); // The joint names are sorted according to the order documented in the header file! - prepare_lists_with_joint_names(FRONT_LEFT, params_.front_left_wheel_command_joint_name, params_.front_left_wheel_state_joint_name); - prepare_lists_with_joint_names(FRONT_RIGHT, params_.front_right_wheel_command_joint_name, params_.front_right_wheel_state_joint_name); - prepare_lists_with_joint_names(REAR_RIGHT, params_.rear_right_wheel_command_joint_name, params_.rear_right_wheel_state_joint_name); - prepare_lists_with_joint_names(REAR_LEFT, params_.rear_left_wheel_command_joint_name, params_.rear_left_wheel_state_joint_name); + prepare_lists_with_joint_names( + FRONT_LEFT, params_.front_left_wheel_command_joint_name, + params_.front_left_wheel_state_joint_name); + prepare_lists_with_joint_names( + FRONT_RIGHT, params_.front_right_wheel_command_joint_name, + params_.front_right_wheel_state_joint_name); + prepare_lists_with_joint_names( + REAR_RIGHT, params_.rear_right_wheel_command_joint_name, + params_.rear_right_wheel_state_joint_name); + prepare_lists_with_joint_names( + REAR_LEFT, params_.rear_left_wheel_command_joint_name, + params_.rear_left_wheel_state_joint_name); // Set wheel params for the odometry computation odometry_.setWheelsParams( @@ -362,8 +373,8 @@ controller_interface::return_type MecanumDriveController::update_and_write_comma { // Estimate twist (using joint information) and integrate odometry_.update( - wheel_front_left_state_vel, wheel_rear_left_state_vel, wheel_rear_right_state_vel, wheel_front_right_state_vel, - period.seconds()); + wheel_front_left_state_vel, wheel_rear_left_state_vel, wheel_rear_right_state_vel, + wheel_front_right_state_vel, period.seconds()); } // INVERSE KINEMATICS (move robot). @@ -417,11 +428,12 @@ controller_interface::return_type MecanumDriveController::update_and_write_comma params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis * velocity_in_center_frame_angular_z_); - // Set wheels velocities - The joint names are sorted accoring to the order documented in the header file! - command_interfaces_[FRONT_LEFT].set_value(wheel_front_left_vel); - command_interfaces_[FRONT_RIGHT].set_value(wheel_front_right_vel); - command_interfaces_[REAR_RIGHT].set_value(wheel_rear_right_vel); - command_interfaces_[REAR_LEFT].set_value(wheel_rear_left_vel); + // Set wheels velocities - The joint names are sorted according to the order documented in the + // header file! + command_interfaces_[FRONT_LEFT].set_value(wheel_front_left_vel); + command_interfaces_[FRONT_RIGHT].set_value(wheel_front_right_vel); + command_interfaces_[REAR_RIGHT].set_value(wheel_rear_right_vel); + command_interfaces_[REAR_LEFT].set_value(wheel_rear_left_vel); } else { @@ -463,10 +475,14 @@ controller_interface::return_type MecanumDriveController::update_and_write_comma if (controller_state_publisher_->trylock()) { controller_state_publisher_->msg_.header.stamp = get_node()->now(); - controller_state_publisher_->msg_.front_left_wheel_velocity = state_interfaces_[FRONT_LEFT].get_value(); - controller_state_publisher_->msg_.front_right_wheel_velocity = state_interfaces_[FRONT_RIGHT].get_value(); - controller_state_publisher_->msg_.back_right_wheel_velocity = state_interfaces_[REAR_RIGHT].get_value(); - controller_state_publisher_->msg_.back_left_wheel_velocity = state_interfaces_[REAR_LEFT].get_value(); + controller_state_publisher_->msg_.front_left_wheel_velocity = + state_interfaces_[FRONT_LEFT].get_value(); + controller_state_publisher_->msg_.front_right_wheel_velocity = + state_interfaces_[FRONT_RIGHT].get_value(); + controller_state_publisher_->msg_.back_right_wheel_velocity = + state_interfaces_[REAR_RIGHT].get_value(); + controller_state_publisher_->msg_.back_left_wheel_velocity = + state_interfaces_[REAR_LEFT].get_value(); controller_state_publisher_->msg_.reference_velocity.linear.x = reference_interfaces_[0]; controller_state_publisher_->msg_.reference_velocity.linear.y = reference_interfaces_[1]; controller_state_publisher_->msg_.reference_velocity.angular.z = reference_interfaces_[2]; diff --git a/mecanum_drive_controller/src/odometry.cpp b/mecanum_drive_controller/src/odometry.cpp index e5760f18e6..bb873fcfdb 100644 --- a/mecanum_drive_controller/src/odometry.cpp +++ b/mecanum_drive_controller/src/odometry.cpp @@ -45,8 +45,8 @@ void Odometry::init( } bool Odometry::update( - const double wheel_front_left_vel, const double wheel_rear_left_vel, const double wheel_rear_right_vel, - const double wheel_front_right_vel, const double dt) + const double wheel_front_left_vel, const double wheel_rear_left_vel, + const double wheel_rear_right_vel, const double wheel_front_right_vel, const double dt) { /// We cannot estimate the speed with very small time intervals: // const double dt = (time - timestamp_).toSec(); diff --git a/mecanum_drive_controller/test/test_load_mecanum_drive_controller.cpp b/mecanum_drive_controller/test/test_load_mecanum_drive_controller.cpp index 52ec07ded0..fa44e19b80 100644 --- a/mecanum_drive_controller/test/test_load_mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/test/test_load_mecanum_drive_controller.cpp @@ -28,9 +28,7 @@ TEST(TestLoadMecanumDriveController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - std::make_unique( - ros2_control_test_assets::minimal_robot_urdf), - executor, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); ASSERT_NE( cm.load_controller( diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp index 9d49e344dd..c0fb9892b0 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp @@ -220,7 +220,7 @@ TEST_F( // reference_msg is published with provided time stamp when publish_commands( time_stamp) // is called publish_commands(controller_->get_node()->now()); - ASSERT_TRUE(controller_->wait_for_commands(executor)); + controller_->wait_for_commands(executor); ASSERT_EQ( controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), @@ -263,7 +263,7 @@ TEST_F(MecanumDriveControllerTest, when_reference_msg_is_too_old_expect_unset_re publish_commands( controller_->get_node()->now() - controller_->ref_timeout_ - rclcpp::Duration::from_seconds(0.1)); - ASSERT_TRUE(controller_->wait_for_commands(executor)); + controller_->wait_for_commands(executor); ASSERT_EQ(old_timestamp, (*(controller_->input_ref_.readFromNonRT()))->header.stamp); EXPECT_TRUE(std::isnan((reference)->twist.linear.x)); EXPECT_TRUE(std::isnan((reference)->twist.linear.y)); @@ -293,7 +293,7 @@ TEST_F( // is called publish_commands(rclcpp::Time(0)); - ASSERT_TRUE(controller_->wait_for_commands(executor)); + controller_->wait_for_commands(executor); ASSERT_EQ(old_timestamp.sec, (*(controller_->input_ref_.readFromNonRT()))->header.stamp.sec); EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x)); EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z)); @@ -324,7 +324,7 @@ TEST_F(MecanumDriveControllerTest, when_message_has_valid_timestamp_expect_refer // is called publish_commands(controller_->get_node()->now()); - ASSERT_TRUE(controller_->wait_for_commands(executor)); + controller_->wait_for_commands(executor); EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x)); EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z)); EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x, 1.5); @@ -555,7 +555,7 @@ TEST_F( // is called publish_commands(controller_->get_node()->now()); - ASSERT_TRUE(controller_->wait_for_commands(executor)); + controller_->wait_for_commands(executor); EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x)); EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.y)); diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp index 8c6fdfc44d..8ed2335b88 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp @@ -29,6 +29,8 @@ #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "mecanum_drive_controller/mecanum_drive_controller.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors.hpp" #include "rclcpp/parameter_value.hpp" #include "rclcpp/time.hpp" #include "rclcpp/utilities.hpp" @@ -85,13 +87,7 @@ class TestableMecanumDriveController : public mecanum_drive_controller::MecanumD controller_interface::CallbackReturn on_configure( const rclcpp_lifecycle::State & previous_state) override { - auto ret = mecanum_drive_controller::MecanumDriveController::on_configure(previous_state); - // Only if on_configure is successful create subscription - if (ret == CallbackReturn::SUCCESS) - { - ref_subscriber_wait_set_.add_subscription(ref_subscriber_); - } - return ret; + return mecanum_drive_controller::MecanumDriveController::on_configure(previous_state); } controller_interface::CallbackReturn on_activate( @@ -105,30 +101,25 @@ class TestableMecanumDriveController : public mecanum_drive_controller::MecanumD * @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, + void wait_for_command( + rclcpp::Executor & executor, const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) { - bool success = subscriber_wait_set.wait(timeout).kind() == rclcpp::WaitResultKind::Ready; - if (success) + auto until = get_node()->get_clock()->now() + timeout; + while (get_node()->get_clock()->now() < until) { executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); } - return success; } - bool wait_for_commands( + void 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); + wait_for_command(executor, timeout); } - -private: - rclcpp::WaitSet ref_subscriber_wait_set_; }; // We are using template class here for easier reuse of Fixture in specializations of controllers @@ -197,10 +188,13 @@ class MecanumDriveControllerFixture : public ::testing::Test void subscribe_to_controller_status_execute_update_and_get_messages(ControllerStateMsg & msg) { // create a new subscriber + ControllerStateMsg::SharedPtr received_msg; rclcpp::Node test_subscription_node("test_subscription_node"); - auto subs_callback = [&](const ControllerStateMsg::SharedPtr) {}; + auto subs_callback = [&](const ControllerStateMsg::SharedPtr cb_msg) { received_msg = cb_msg; }; auto subscription = test_subscription_node.create_subscription( "/test_mecanum_drive_controller/controller_state", 10, subs_callback); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(test_subscription_node.get_node_base_interface()); // call update to publish the test value ASSERT_EQ( controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), @@ -208,23 +202,28 @@ class MecanumDriveControllerFixture : public ::testing::Test // 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(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)); + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)); + const auto timeout = std::chrono::milliseconds{5}; + const auto until = test_subscription_node.get_clock()->now() + timeout; + while (!received_msg && test_subscription_node.get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } // check if message has been received - if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) + if (received_msg.get()) { break; } } ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " "controller/broadcaster update loop"; + ASSERT_TRUE(received_msg); // take message from subscription - rclcpp::MessageInfo msg_info; - ASSERT_TRUE(subscription->take(msg, msg_info)); + msg = *received_msg; } void publish_commands(