diff --git a/README.md b/README.md index bd26d28..8310fca 100644 --- a/README.md +++ b/README.md @@ -3,9 +3,16 @@ Package defining custom controllers that are generic and we want to contribute them into ros2_controllers. The controllers have been prefixed with `picknik_` so that when they eventually land upstream the prefix can be removed and the versions here will become deprecated. +- `picknik_diff_drive_controller::DiffDriveController` - `picknik_twist_controller::PicknikTwistController` - `picknik_reset_fault_controller::PicknikResetFaultController` +## DiffDriveController +Generic ROS2 controller with chaining support for JTC path following. +The implementation is currently specific to MoveIt (x/y/theta joints), we might eventually break out the velocity tracking into linear and rotational components to bring this upstream to ros2_controllers. + +### Usage +See the [Stretch ROS](https://github.com/PickNikRobotics/stretch_ros) configuration for an example setup, running a JointTrajectoryController and the chained DiffDriveController for computing wheel commands and mocked odometry. ## PicknikTwistController Generic ROS2 controller to forward cartesian twist commands. diff --git a/picknik_diff_drive_controller/CHANGELOG.rst b/picknik_diff_drive_controller/CHANGELOG.rst new file mode 100644 index 0000000..782b28e --- /dev/null +++ b/picknik_diff_drive_controller/CHANGELOG.rst @@ -0,0 +1,278 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package diff_drive_controller +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +2.33.0 (2024-02-12) +------------------- +* Add test_depend on `hardware_interface_testing` (backport `#1018 `_) (`#1019 `_) +* Add tests for `interface_configuration_type` consistently (`#899 `_) (`#1011 `_) +* Contributors: mergify[bot] + +2.32.0 (2024-01-20) +------------------- + +2.31.0 (2024-01-11) +------------------- + +2.30.0 (2023-12-20) +------------------- +* [DiffDriveController] Optional tf namespace prefixes instead of using node namespace (backport `#533 `_) (`#726 `_) +* Contributors: mergify[bot] + +2.29.0 (2023-12-05) +------------------- + +2.28.0 (2023-11-30) +------------------- + +2.27.0 (2023-11-14) +------------------- +* [diff_drive_controller] Fixed typos in diff_drive_controller_parameter.yaml. (`#822 `_) (`#823 `_) +* Contributors: Tony Baltovski + +2.26.0 (2023-10-03) +------------------- + +2.25.0 (2023-09-15) +------------------- +* removed duplicated previous_publish_timestamp\_ increment by publish_period\_ in diff_drive_controller.cpp (`#644 `_) (`#777 `_) +* Update docs for diff drive controller (`#751 `_) (`#753 `_) +* Contributors: Christoph Fröhlich, Jules CARPENTIER + +2.24.0 (2023-08-07) +------------------- + +2.23.0 (2023-06-23) +------------------- +* Renovate load controller tests (`#569 `_) (`#677 `_) +* Contributors: Bence Magyar + +2.22.0 (2023-06-14) +------------------- +* Use generate_parameter_library for all params (`#601 `_) (`#627 `_) +* Docs: Use branch name substitution for all links (backport `#618 `_) (`#633 `_) +* [Formatting] enable ReflowComments to also use ColumnLimit on comments (`#628 `_) +* Contributors: Sai Kishor Kothakota, Christoph Fröhlich + +2.21.0 (2023-05-28) +------------------- +* Fix compilation warnings (`#621 `_) (`#623 `_) +* Remove compile warnings. (`#519 `_) (`#620 `_) +* Fix github links on control.ros.org (`#604 `_) (`#617 `_) +* Fix overriding of install (`#510 `_) (`#605 `_) +* Contributors: Felix Exner (fexner), Christoph Fröhlich, Mathias Lüdtke, Noel Jiménez García + +2.20.0 (2023-05-14) +------------------- +* Clear registered handles of DiffDriveController on deactivate (`#596 `_) (`#606 `_) +* Contributors: Noel Jiménez García + +2.19.0 (2023-05-02) +------------------- +* Fix wrong publish timestamp initialization (`#585 `_) (`#593 `_) +* Contributors: mergify[bot] + +2.18.0 (2023-04-29) +------------------- +* adjusted open_loop param description in diff_drive_controller_parameter.yaml (`#570 `_) (`#576 `_) +* Contributors: muritane + +2.17.3 (2023-04-14) +------------------- + +2.17.2 (2023-03-07) +------------------- + +2.17.1 (2023-02-20) +------------------- +* [DiffDriveController] Fix prefixing of frame id with controller's namespace (`#522 `_) +* Contributors: Tim Verbelen + +2.17.0 (2023-02-13) +------------------- + +2.16.1 (2023-01-31) +------------------- + +2.16.0 (2023-01-19) +------------------- +* diff_drive base_frame_id param (`#495 `_) (`#498 `_) +* Add backward_ros to all controllers (`#489 `_) (`#493 `_) +* Contributors: Bence Magyar, Jakub Delicat + +2.15.0 (2022-12-06) +------------------- +* [DiffDriveController] Use generate parameter library (`#386 `_) +* [DiffDriveController] Change units of velocity feedback (`#452 `_) +* Contributors: Maciej Stępień, Paul Gesel, Denis Štogl, Bence Magyar + +2.14.0 (2022-11-18) +------------------- +* Odom Topic & Frame Namespaces (`#461 `_) +* Write detailed Diff-Drive-Controller documentation to make all the interfaces understandable. (`#371 `_) +* Contributors: Denis Štogl, sp-sophia-labs + +2.13.0 (2022-10-05) +------------------- + +2.12.0 (2022-09-01) +------------------- +* Fix formatting CI job (`#418 `_) +* Contributors: Tyler Weaver + +2.11.0 (2022-08-04) +------------------- + +2.10.0 (2022-08-01) +------------------- +* Formatting changes from pre-commit (`#400 `_) +* Parameter loading fixup in diff_drive and gripper controllers (`#385 `_) +* Contributors: Andy Zelenak, Tyler Weaver + +2.9.0 (2022-07-14) +------------------ + +2.8.0 (2022-07-09) +------------------ + +2.7.0 (2022-07-03) +------------------ +* Update controllers with new get_name hardware interfaces (`#369 `_) +* Contributors: Lucas Schulze + +2.6.0 (2022-06-18) +------------------ +* Disable failing workflows (`#363 `_) +* CMakeLists cleanup (`#362 `_) +* Fix exception about parameter already been declared & Change default c++ version to 17 (`#360 `_) + * Default C++ version to 17 + * Replace explicit use of declare_paremeter with auto_declare +* Contributors: Andy Zelenak, Jafar Abdi + +2.5.0 (2022-05-13) +------------------ +* [diff_drive_controller] Made odom topic name relative as it was in ROS1. (`#343 `_) +* Fix wrong integration of velocity feedback in odometry in diff_drive_controller (`#331 `_) +* Contributors: Patrick Roncagliolo, Tony Baltovski + +2.4.0 (2022-04-29) +------------------ +* updated to use node getter functions (`#329 `_) +* Contributors: Bence Magyar, Denis Štogl, Jack Center + +2.3.0 (2022-04-21) +------------------ +* Use CallbackReturn from controller_interface namespace (`#333 `_) +* Contributors: Bence Magyar, Denis Štogl + +2.2.0 (2022-03-25) +------------------ +* Use lifecycle node as base for controllers (`#244 `_) +* Contributors: Denis Štogl, Vatan Aksoy Tezer, Bence Magyar + +2.1.0 (2022-02-23) +------------------ +* use rolling mean from rcppmath (`#211 `_) +* Contributors: Karsten Knese, Bence Magyar + +2.0.1 (2022-02-01) +------------------ + +2.0.0 (2022-01-28) +------------------ + +1.3.0 (2022-01-11) +------------------ +* Add publish_rate option for the diff_drive_controller (`#278 `_) +* Fix angular velocity direction of diff_drive_controller odometry (`#281 `_) +* Contributors: Benjamin Hug, Paul Verhoeckx + +1.2.0 (2021-12-29) +------------------ +* Add velocity feedback option for diff_drive_controller (`#260 `_) +* Contributors: Patrick Roncagliolo + +1.1.0 (2021-10-25) +------------------ +* Use common test URDF from descriptions.hpp (`#258 `_) +* Fix header include on Fedora `_ (`#256 `_) +* Fix diff_drive accel limit (`#242 `_) (`#252 `_) +* Contributors: Denis Štogl, Josh Newans, Noeël Moeskops, bailaC + +1.0.0 (2021-09-29) +------------------ +* Add time and period to update function (`#241 `_) +* Unify style of controllers. (`#236 `_) +* ros2_controllers code changes to support ros2_controls issue `#489 `_ (`#233 `_) +* Removing Boost from controllers. (`#235 `_) +* refactor get_current_state to get_state (`#232 `_) +* Contributors: Bence Magyar, Denis Štogl, Márk Szitanics, bailaC + +0.5.0 (2021-08-30) +------------------ +* Add auto declaration of parameters. (`#224 `_) +* Bring precommit config up to speed with ros2_control (`#227 `_) +* Add initial pre-commit setup. (`#220 `_) +* Reduce docs warnings and correct adding guidelines (`#219 `_) +* Contributors: Bence Magyar, Denis Štogl, Lovro Ivanov + +0.4.1 (2021-07-08) +------------------ + +0.4.0 (2021-06-28) +------------------ +* Force torque sensor broadcaster (`#152 `_) + * Add rclcpp::shutdown(); to all standalone test functions +* Fixes for Windows (`#205 `_) + * Fix MSVC build for diff_drive_controller test +* Fix parameter initialisation for galactic (`#199 `_) +* Contributors: Akash, Denis Štogl, Tim Clephas + +0.3.1 (2021-05-23) +------------------ + +0.3.0 (2021-05-21) +------------------ + +0.2.1 (2021-05-03) +------------------ +* Migrate from deprecated controller_interface::return_type::SUCCESS -> OK (`#167 `_) +* Add basic user docs pages for each package (`#156 `_) +* [diff_drive_controller] Change header math.h in cmath for better C++ compliance (`#148 `_) + and isnan inclusion. +* Contributors: Bence Magyar, Olivier Stasse + +0.2.0 (2021-02-06) +------------------ +* Fix diff drive twist concurrency issues (`#146 `_) + * Fix diff drive twist concurrency issues + Before this fix, a twist message could be received and stored one + thread, in the middle of the update() of the controller. + This would be fixed by making a copy of the shared pointer at the + beginning of the update() function, added realtime box to ensure safe + concurrent access to the pointer. + * Don't store limited command as last command + Before these changes, the limited command overwrote the original + command, which mean that it too much more time to reach the commanded + speed. + We only want this behavior when the command is too old and we replace it + with 0 speed. +* Diff drive parameter fixes (`#145 `_) + * Recover old speed limiter behavior, if unspecified min defaults to -max + * Change cmd_vel_timeout to seconds (double) as ROS1 instead of ms(int) +* Unstamped cmd_vel subscriber rebased (`#143 `_) +* Contributors: Anas Abou Allaban, Victor Lopez + +0.1.2 (2021-01-07) +------------------ +* Remove unused sensor_msgs dependency (was non-declared in package.xml) (`#139 `_) +* Contributors: Bence Magyar + +0.1.1 (2021-01-06) +------------------ +* avoid warnings (`#137 `_) +* Migrate diff drive controller to resourcemanager (`#128 `_) +* Contributors: Bence Magyar, Karsten Knese + +0.1.0 (2020-12-23) +------------------ diff --git a/picknik_diff_drive_controller/CMakeLists.txt b/picknik_diff_drive_controller/CMakeLists.txt new file mode 100644 index 0000000..add4aff --- /dev/null +++ b/picknik_diff_drive_controller/CMakeLists.txt @@ -0,0 +1,94 @@ +cmake_minimum_required(VERSION 3.16) +project(picknik_diff_drive_controller LANGUAGES CXX) + +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra) +endif() + +set(THIS_PACKAGE_INCLUDE_DEPENDS + controller_interface + generate_parameter_library + geometry_msgs + hardware_interface + nav_msgs + pluginlib + rclcpp + rclcpp_lifecycle + rcpputils + realtime_tools + tf2 + tf2_msgs +) + +find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +generate_parameter_library(diff_drive_controller_parameters + src/diff_drive_controller_parameter.yaml +) + +add_library(diff_drive_controller SHARED + src/diff_drive_controller.cpp + src/odometry.cpp + src/speed_limiter.cpp +) +target_compile_features(diff_drive_controller PUBLIC cxx_std_17) +target_include_directories(diff_drive_controller PUBLIC + $ + $ +) +target_link_libraries(diff_drive_controller PUBLIC diff_drive_controller_parameters) +ament_target_dependencies(diff_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(diff_drive_controller PRIVATE "DIFF_DRIVE_CONTROLLER_BUILDING_DLL") +pluginlib_export_plugin_description_file(controller_interface diff_drive_plugin.xml) + +if(BUILD_TESTING) + find_package(ament_cmake_gmock REQUIRED) + find_package(controller_manager REQUIRED) + find_package(ros2_control_test_assets REQUIRED) + + ament_add_gmock(test_diff_drive_controller + test/test_diff_drive_controller.cpp + ENV config_file=${CMAKE_CURRENT_SOURCE_DIR}/test/config/test_diff_drive_controller.yaml) + target_link_libraries(test_diff_drive_controller + diff_drive_controller + ) + ament_target_dependencies(test_diff_drive_controller + geometry_msgs + hardware_interface + nav_msgs + rclcpp + rclcpp_lifecycle + realtime_tools + tf2 + tf2_msgs + ) + + ament_add_gmock(test_load_diff_drive_controller + test/test_load_diff_drive_controller.cpp + ) + ament_target_dependencies(test_load_diff_drive_controller + controller_manager + ros2_control_test_assets + ) +endif() + +install( + DIRECTORY include/ + DESTINATION include/picknik_diff_drive_controller +) +install(TARGETS diff_drive_controller diff_drive_controller_parameters + EXPORT export_diff_drive_controller + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib +) + +ament_export_targets(export_diff_drive_controller HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/picknik_diff_drive_controller/diff_drive_plugin.xml b/picknik_diff_drive_controller/diff_drive_plugin.xml new file mode 100644 index 0000000..a4c43bc --- /dev/null +++ b/picknik_diff_drive_controller/diff_drive_plugin.xml @@ -0,0 +1,7 @@ + + + + The differential drive controller transforms linear and angular velocity messages into signals for each wheel(s) for a differential drive robot. + + + diff --git a/picknik_diff_drive_controller/doc/parameters_context.yaml b/picknik_diff_drive_controller/doc/parameters_context.yaml new file mode 100644 index 0000000..81e9280 --- /dev/null +++ b/picknik_diff_drive_controller/doc/parameters_context.yaml @@ -0,0 +1,9 @@ +linear.x: | + Joint limits structure for the linear ``x``-axis. + The limiter ignores position limits. + For details see ``joint_limits`` package from ros2_control repository. + +angular.z: | + Joint limits structure for the rotation about ``z``-axis. + The limiter ignores position limits. + For details see ``joint_limits`` package from ros2_control repository. diff --git a/picknik_diff_drive_controller/doc/userdoc.rst b/picknik_diff_drive_controller/doc/userdoc.rst new file mode 100644 index 0000000..36391c1 --- /dev/null +++ b/picknik_diff_drive_controller/doc/userdoc.rst @@ -0,0 +1,79 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/diff_drive_controller/doc/userdoc.rst + +.. _diff_drive_controller_userdoc: + +diff_drive_controller +===================== + +Controller for mobile robots with differential drive. + +As input it takes velocity commands for the robot body, which are translated to wheel commands for the differential drive base. +In addition, this controller exports 3-DOF velocity command interfaces (``position/x``, ``position/y``, ``position/theta``) for chaining to another controller, for example a JointTrajectoryController. This design directly supports MoveIt's internal representation of the mobile base joint. + +Odometry is computed from hardware feedback and published. + +Other features +-------------- + + + Realtime-safe implementation. + + Odometry publishing + + Task-space velocity, acceleration and jerk limits + + Automatic stop after command time-out + + +Description of controller's interfaces +------------------------------------------------ + +References +,,,,,,,,,,,,,,,,,, + +(the controller is not yet implemented as chainable controller) + +Feedback +,,,,,,,,,,,,,, + +As feedback interface type the joints' position (``hardware_interface::HW_IF_POSITION``) or velocity (``hardware_interface::HW_IF_VELOCITY``,if parameter ``position_feedback=false``) are used. + +Output +,,,,,,,,, + +Joints' velocity (``hardware_interface::HW_IF_VELOCITY``) are used. + + +ROS 2 Interfaces +------------------------ + +Subscribers +,,,,,,,,,,,, + +~/cmd_vel [geometry_msgs/msg/TwistStamped] + Velocity command for the controller, if ``use_stamped_vel=true``. The controller extracts the x component of the linear velocity and the z component of the angular velocity. Velocities on other components are ignored. + +~/cmd_vel_unstamped [geometry_msgs::msg::Twist] + Velocity command for the controller, if ``use_stamped_vel=false``. The controller extracts the x component of the linear velocity and the z component of the angular velocity. Velocities on other components are ignored. + + +Publishers +,,,,,,,,,,, +~/odom [nav_msgs::msg::Odometry] + This represents an estimate of the robot's position and velocity in free space. + +/tf [tf2_msgs::msg::TFMessage] + tf tree. Published only if ``enable_odom_tf=true`` + +~/cmd_vel_out [geometry_msgs/msg/TwistStamped] + Velocity command for the controller, where limits were applied. Published only if ``publish_limited_velocity=true`` + + +Parameters +,,,,,,,,,,,, + +This controller uses the `generate_parameter_library `_ to handle its parameters. The parameter `definition file located in the src folder `_ contains descriptions for all the parameters used by the controller. + +.. generate_parameter_library_details:: ../src/diff_drive_controller_parameter.yaml + parameters_context.yaml + +An example parameter file for this controller can be found in `the test directory `_: + +.. literalinclude:: ../test/config/test_diff_drive_controller.yaml + :language: yaml diff --git a/picknik_diff_drive_controller/include/picknik_diff_drive_controller/diff_drive_controller.hpp b/picknik_diff_drive_controller/include/picknik_diff_drive_controller/diff_drive_controller.hpp new file mode 100644 index 0000000..106f3b8 --- /dev/null +++ b/picknik_diff_drive_controller/include/picknik_diff_drive_controller/diff_drive_controller.hpp @@ -0,0 +1,172 @@ +// Copyright 2020 PAL Robotics S.L. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/* + * Author: Bence Magyar, Enrique Fernández, Manuel Meraz + */ + +#ifndef DIFF_DRIVE_CONTROLLER__DIFF_DRIVE_CONTROLLER_HPP_ +#define DIFF_DRIVE_CONTROLLER__DIFF_DRIVE_CONTROLLER_HPP_ + +#include +#include +#include +#include +#include +#include + +#include "controller_interface/chainable_controller_interface.hpp" +#include "geometry_msgs/msg/twist.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "hardware_interface/handle.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "odometry.hpp" +#include "picknik_diff_drive_controller/odometry.hpp" +#include "picknik_diff_drive_controller/speed_limiter.hpp" +#include "picknik_diff_drive_controller/visibility_control.h" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include "realtime_tools/realtime_buffer.h" +#include "realtime_tools/realtime_publisher.h" +#include "tf2_msgs/msg/tf_message.hpp" + +#include "diff_drive_controller_parameters.hpp" + +namespace picknik_diff_drive_controller +{ +class DiffDriveController : public controller_interface::ChainableControllerInterface +{ + using Twist = geometry_msgs::msg::TwistStamped; + +public: + DIFF_DRIVE_CONTROLLER_PUBLIC + DiffDriveController(); + + DIFF_DRIVE_CONTROLLER_PUBLIC + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + DIFF_DRIVE_CONTROLLER_PUBLIC + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + DIFF_DRIVE_CONTROLLER_PUBLIC controller_interface::return_type update_reference_from_subscribers() + override; + + DIFF_DRIVE_CONTROLLER_PUBLIC controller_interface::return_type update_and_write_commands( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + DIFF_DRIVE_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_init() override; + + DIFF_DRIVE_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + DIFF_DRIVE_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + DIFF_DRIVE_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + + DIFF_DRIVE_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_cleanup( + const rclcpp_lifecycle::State & previous_state) override; + + DIFF_DRIVE_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_error( + const rclcpp_lifecycle::State & previous_state) override; + + DIFF_DRIVE_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_shutdown( + const rclcpp_lifecycle::State & previous_state) override; + + using ControllerTwistReferenceMsg = geometry_msgs::msg::TwistStamped; + +protected: + bool on_set_chained_mode(bool chained_mode) override; + + std::vector on_export_reference_interfaces() override; + + struct WheelHandle + { + std::reference_wrapper feedback; + std::reference_wrapper velocity; + }; + + const char * feedback_type() const; + controller_interface::CallbackReturn configure_side( + const std::string & side, const std::vector & wheel_names, + std::vector & registered_handles); + + std::vector registered_left_wheel_handles_; + std::vector registered_right_wheel_handles_; + + // Parameters from ROS for diff_drive_controller + std::shared_ptr param_listener_; + Params params_; + + Odometry odometry_; + + // Timeout to consider cmd_vel commands old + rclcpp::Duration ref_timeout_ = rclcpp::Duration::from_seconds(0.5); + + std::shared_ptr> odometry_publisher_ = nullptr; + std::shared_ptr> + realtime_odometry_publisher_ = nullptr; + + std::shared_ptr> odometry_transform_publisher_ = + nullptr; + std::shared_ptr> + realtime_odometry_transform_publisher_ = nullptr; + + bool subscriber_is_active_ = false; + rclcpp::Subscription::SharedPtr velocity_command_subscriber_ = nullptr; + rclcpp::Subscription::SharedPtr + velocity_command_unstamped_subscriber_ = nullptr; + + realtime_tools::RealtimeBuffer> + received_velocity_msg_ptr_{nullptr}; + + std::queue previous_commands_; // last two commands + + // speed limiters + SpeedLimiter limiter_linear_; + SpeedLimiter limiter_angular_; + + bool publish_limited_velocity_ = false; + std::shared_ptr> limited_velocity_publisher_ = nullptr; + std::shared_ptr> realtime_limited_velocity_publisher_ = + nullptr; + + rclcpp::Time previous_update_timestamp_{0}; + + // publish rate limiter + double publish_rate_ = 50.0; + rclcpp::Duration publish_period_ = rclcpp::Duration::from_nanoseconds(0); + rclcpp::Time previous_publish_timestamp_{0, 0, RCL_CLOCK_UNINITIALIZED}; + + bool is_halted = false; + bool use_stamped_vel_ = true; + + bool reset(); + void halt(); + +private: + // callback for topic interface + void reference_callback(const std::shared_ptr msg); + void reference_callback_unstamped(const std::shared_ptr msg); +}; +} // namespace picknik_diff_drive_controller +#endif // DIFF_DRIVE_CONTROLLER__DIFF_DRIVE_CONTROLLER_HPP_ diff --git a/picknik_diff_drive_controller/include/picknik_diff_drive_controller/odometry.hpp b/picknik_diff_drive_controller/include/picknik_diff_drive_controller/odometry.hpp new file mode 100644 index 0000000..3493eba --- /dev/null +++ b/picknik_diff_drive_controller/include/picknik_diff_drive_controller/odometry.hpp @@ -0,0 +1,88 @@ +// Copyright 2020 PAL Robotics S.L. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/* + * Author: Luca Marchionni + * Author: Bence Magyar + * Author: Enrique Fernández + * Author: Paul Mathieu + */ + +#ifndef DIFF_DRIVE_CONTROLLER__ODOMETRY_HPP_ +#define DIFF_DRIVE_CONTROLLER__ODOMETRY_HPP_ + +#include + +#include "rclcpp/time.hpp" +#include "rcppmath/rolling_mean_accumulator.hpp" + +namespace picknik_diff_drive_controller +{ +class Odometry +{ +public: + explicit Odometry(size_t velocity_rolling_window_size = 10); + + void init(const rclcpp::Time & time); + bool update(double left_pos, double right_pos, const rclcpp::Time & time); + bool updateFromVelocity(double left_vel, double right_vel, const rclcpp::Time & time); + void updateOpenLoop(double linear, double angular, const rclcpp::Time & time); + void resetOdometry(); + + double getX() const { return x_; } + double getY() const { return y_; } + double getHeading() const { return heading_; } + double getLinear() const { return linear_; } + double getAngular() const { return angular_; } + + void setWheelParams(double wheel_separation, double left_wheel_radius, double right_wheel_radius); + void setVelocityRollingWindowSize(size_t velocity_rolling_window_size); + +private: + using RollingMeanAccumulator = rcppmath::RollingMeanAccumulator; + + void integrateRungeKutta2(double linear, double angular); + void integrateExact(double linear, double angular); + void resetAccumulators(); + + // Current timestamp: + rclcpp::Time timestamp_; + + // Current pose: + double x_; // [m] + double y_; // [m] + double heading_; // [rad] + + // Current velocity: + double linear_; // [m/s] + double angular_; // [rad/s] + + // Wheel kinematic parameters [m]: + double wheel_separation_; + double left_wheel_radius_; + double right_wheel_radius_; + + // Previous wheel position/state [rad]: + double left_wheel_old_pos_; + double right_wheel_old_pos_; + + // Rolling mean accumulators for the linear and angular velocities: + size_t velocity_rolling_window_size_; + RollingMeanAccumulator linear_accumulator_; + RollingMeanAccumulator angular_accumulator_; +}; + +} // namespace picknik_diff_drive_controller + +#endif // DIFF_DRIVE_CONTROLLER__ODOMETRY_HPP_ diff --git a/picknik_diff_drive_controller/include/picknik_diff_drive_controller/speed_limiter.hpp b/picknik_diff_drive_controller/include/picknik_diff_drive_controller/speed_limiter.hpp new file mode 100644 index 0000000..135c18d --- /dev/null +++ b/picknik_diff_drive_controller/include/picknik_diff_drive_controller/speed_limiter.hpp @@ -0,0 +1,105 @@ +// Copyright 2020 PAL Robotics S.L. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/* + * Author: Enrique Fernández + */ + +#ifndef DIFF_DRIVE_CONTROLLER__SPEED_LIMITER_HPP_ +#define DIFF_DRIVE_CONTROLLER__SPEED_LIMITER_HPP_ + +#include + +namespace picknik_diff_drive_controller +{ +class SpeedLimiter +{ +public: + /** + * \brief Constructor + * \param [in] has_velocity_limits if true, applies velocity limits + * \param [in] has_acceleration_limits if true, applies acceleration limits + * \param [in] has_jerk_limits if true, applies jerk limits + * \param [in] min_velocity Minimum velocity [m/s], usually <= 0 + * \param [in] max_velocity Maximum velocity [m/s], usually >= 0 + * \param [in] min_acceleration Minimum acceleration [m/s^2], usually <= 0 + * \param [in] max_acceleration Maximum acceleration [m/s^2], usually >= 0 + * \param [in] min_jerk Minimum jerk [m/s^3], usually <= 0 + * \param [in] max_jerk Maximum jerk [m/s^3], usually >= 0 + */ + SpeedLimiter( + bool has_velocity_limits = false, bool has_acceleration_limits = false, + bool has_jerk_limits = false, double min_velocity = NAN, double max_velocity = NAN, + double min_acceleration = NAN, double max_acceleration = NAN, double min_jerk = NAN, + double max_jerk = NAN); + + /** + * \brief Limit the velocity and acceleration + * \param [in, out] v Velocity [m/s] + * \param [in] v0 Previous velocity to v [m/s] + * \param [in] v1 Previous velocity to v0 [m/s] + * \param [in] dt Time step [s] + * \return Limiting factor (1.0 if none) + */ + double limit(double & v, double v0, double v1, double dt); + + /** + * \brief Limit the velocity + * \param [in, out] v Velocity [m/s] + * \return Limiting factor (1.0 if none) + */ + double limit_velocity(double & v); + + /** + * \brief Limit the acceleration + * \param [in, out] v Velocity [m/s] + * \param [in] v0 Previous velocity [m/s] + * \param [in] dt Time step [s] + * \return Limiting factor (1.0 if none) + */ + double limit_acceleration(double & v, double v0, double dt); + + /** + * \brief Limit the jerk + * \param [in, out] v Velocity [m/s] + * \param [in] v0 Previous velocity to v [m/s] + * \param [in] v1 Previous velocity to v0 [m/s] + * \param [in] dt Time step [s] + * \return Limiting factor (1.0 if none) + * \see http://en.wikipedia.org/wiki/Jerk_%28physics%29#Motion_control + */ + double limit_jerk(double & v, double v0, double v1, double dt); + +private: + // Enable/Disable velocity/acceleration/jerk limits: + bool has_velocity_limits_; + bool has_acceleration_limits_; + bool has_jerk_limits_; + + // Velocity limits: + double min_velocity_; + double max_velocity_; + + // Acceleration limits: + double min_acceleration_; + double max_acceleration_; + + // Jerk limits: + double min_jerk_; + double max_jerk_; +}; + +} // namespace picknik_diff_drive_controller + +#endif // DIFF_DRIVE_CONTROLLER__SPEED_LIMITER_HPP_ diff --git a/picknik_diff_drive_controller/include/picknik_diff_drive_controller/visibility_control.h b/picknik_diff_drive_controller/include/picknik_diff_drive_controller/visibility_control.h new file mode 100644 index 0000000..94d7815 --- /dev/null +++ b/picknik_diff_drive_controller/include/picknik_diff_drive_controller/visibility_control.h @@ -0,0 +1,56 @@ +// Copyright 2017 Open Source Robotics Foundation, Inc. +// +// 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. + +/* This header must be included by all rclcpp headers which declare symbols + * which are defined in the rclcpp library. When not building the rclcpp + * library, i.e. when using the headers in other package's code, the contents + * of this header change the visibility of certain symbols which the rclcpp + * library cannot have, but the consuming code must have inorder to link. + */ + +#ifndef DIFF_DRIVE_CONTROLLER__VISIBILITY_CONTROL_H_ +#define DIFF_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 DIFF_DRIVE_CONTROLLER_EXPORT __attribute__((dllexport)) +#define DIFF_DRIVE_CONTROLLER_IMPORT __attribute__((dllimport)) +#else +#define DIFF_DRIVE_CONTROLLER_EXPORT __declspec(dllexport) +#define DIFF_DRIVE_CONTROLLER_IMPORT __declspec(dllimport) +#endif +#ifdef DIFF_DRIVE_CONTROLLER_BUILDING_DLL +#define DIFF_DRIVE_CONTROLLER_PUBLIC DIFF_DRIVE_CONTROLLER_EXPORT +#else +#define DIFF_DRIVE_CONTROLLER_PUBLIC DIFF_DRIVE_CONTROLLER_IMPORT +#endif +#define DIFF_DRIVE_CONTROLLER_PUBLIC_TYPE DIFF_DRIVE_CONTROLLER_PUBLIC +#define DIFF_DRIVE_CONTROLLER_LOCAL +#else +#define DIFF_DRIVE_CONTROLLER_EXPORT __attribute__((visibility("default"))) +#define DIFF_DRIVE_CONTROLLER_IMPORT +#if __GNUC__ >= 4 +#define DIFF_DRIVE_CONTROLLER_PUBLIC __attribute__((visibility("default"))) +#define DIFF_DRIVE_CONTROLLER_LOCAL __attribute__((visibility("hidden"))) +#else +#define DIFF_DRIVE_CONTROLLER_PUBLIC +#define DIFF_DRIVE_CONTROLLER_LOCAL +#endif +#define DIFF_DRIVE_CONTROLLER_PUBLIC_TYPE +#endif + +#endif // DIFF_DRIVE_CONTROLLER__VISIBILITY_CONTROL_H_ diff --git a/picknik_diff_drive_controller/package.xml b/picknik_diff_drive_controller/package.xml new file mode 100644 index 0000000..89f1662 --- /dev/null +++ b/picknik_diff_drive_controller/package.xml @@ -0,0 +1,35 @@ + + + picknik_diff_drive_controller + 2.33.0 + Controller for a differential drive mobile base. + Bence Magyar + Jordan Palacios + + Apache License 2.0 + + ament_cmake + generate_parameter_library + + backward_ros + controller_interface + geometry_msgs + hardware_interface + nav_msgs + pluginlib + rclcpp + rclcpp_lifecycle + rcpputils + realtime_tools + tf2 + tf2_msgs + + ament_cmake_gmock + controller_manager + hardware_interface_testing + ros2_control_test_assets + + + ament_cmake + + diff --git a/picknik_diff_drive_controller/src/diff_drive_controller.cpp b/picknik_diff_drive_controller/src/diff_drive_controller.cpp new file mode 100644 index 0000000..a52c6a6 --- /dev/null +++ b/picknik_diff_drive_controller/src/diff_drive_controller.cpp @@ -0,0 +1,709 @@ +// Copyright 2020 PAL Robotics S.L. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/* + * Author: Bence Magyar, Enrique Fernández, Manuel Meraz + */ + +#include +#include +#include +#include +#include + +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "picknik_diff_drive_controller/diff_drive_controller.hpp" +#include "rclcpp/logging.hpp" +#include "tf2/LinearMath/Quaternion.h" + +namespace +{ +constexpr auto DEFAULT_COMMAND_TOPIC = "~/cmd_vel"; +constexpr auto DEFAULT_COMMAND_UNSTAMPED_TOPIC = "~/cmd_vel_unstamped"; +constexpr auto DEFAULT_COMMAND_OUT_TOPIC = "~/cmd_vel_out"; +constexpr auto DEFAULT_ODOMETRY_TOPIC = "~/odom"; +constexpr auto DEFAULT_TRANSFORM_TOPIC = "/tf"; + +using ControllerTwistReferenceMsg = geometry_msgs::msg::TwistStamped; + +} // namespace + +namespace +{ // utility + +// 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 picknik_diff_drive_controller +{ +using namespace std::chrono_literals; +using controller_interface::interface_configuration_type; +using controller_interface::InterfaceConfiguration; +using hardware_interface::HW_IF_POSITION; +using hardware_interface::HW_IF_VELOCITY; +using lifecycle_msgs::msg::State; + +DiffDriveController::DiffDriveController() : controller_interface::ChainableControllerInterface() {} + +const char * DiffDriveController::feedback_type() const +{ + return params_.position_feedback ? HW_IF_POSITION : HW_IF_VELOCITY; +} + +controller_interface::CallbackReturn DiffDriveController::on_init() +{ + try + { + // Create the parameter listener and get the parameters + param_listener_ = std::make_shared(get_node()); + params_ = param_listener_->get_params(); + } + catch (const std::exception & e) + { + fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + return controller_interface::CallbackReturn::SUCCESS; +} + +InterfaceConfiguration DiffDriveController::command_interface_configuration() const +{ + std::vector conf_names; + for (const auto & joint_name : params_.left_wheel_names) + { + conf_names.push_back(joint_name + "/" + HW_IF_VELOCITY); + } + for (const auto & joint_name : params_.right_wheel_names) + { + conf_names.push_back(joint_name + "/" + HW_IF_VELOCITY); + } + return {interface_configuration_type::INDIVIDUAL, conf_names}; +} + +InterfaceConfiguration DiffDriveController::state_interface_configuration() const +{ + std::vector conf_names; + for (const auto & joint_name : params_.left_wheel_names) + { + conf_names.push_back(joint_name + "/" + feedback_type()); + } + for (const auto & joint_name : params_.right_wheel_names) + { + conf_names.push_back(joint_name + "/" + feedback_type()); + } + return {interface_configuration_type::INDIVIDUAL, conf_names}; +} + +std::vector +DiffDriveController::on_export_reference_interfaces() +{ + // Contrary to the original design of the chainable DiffDriveController, we are not depending on + // two interfaces for "linear" and "rotational" velocities, but instead we take global x/y/theta + // velocities into account. The reason is that we want to support a position tracking approach + // using JTC in combination with MoveIt's 3DOF planar joint. In the future, we might consider + // using an intermediary controller that fuses the x/y components into a single linear velocity. + const int nr_ref_itfs = 3; + reference_interfaces_.resize(nr_ref_itfs, 0.0); + std::vector reference_interfaces; + reference_interfaces.reserve(nr_ref_itfs); + + reference_interfaces.push_back(hardware_interface::CommandInterface( + get_node()->get_name(), std::string("x/") + hardware_interface::HW_IF_VELOCITY, + &reference_interfaces_[0])); + + reference_interfaces.push_back(hardware_interface::CommandInterface( + get_node()->get_name(), std::string("y/") + hardware_interface::HW_IF_VELOCITY, + &reference_interfaces_[1])); + + reference_interfaces.push_back(hardware_interface::CommandInterface( + get_node()->get_name(), std::string("theta/") + hardware_interface::HW_IF_VELOCITY, + &reference_interfaces_[2])); + + return reference_interfaces; +} + +void DiffDriveController::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_) + { + received_velocity_msg_ptr_.writeFromNonRT(msg); + } + else + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Received message has timestamp %.10f older for %.10f which is more then allowed timeout " + "(%.4f).", + rclcpp::Time(msg->header.stamp).seconds(), age_of_last_command.seconds(), + ref_timeout_.seconds()); + } +} + +void DiffDriveController::reference_callback_unstamped( + const std::shared_ptr msg) +{ + auto twist_stamped = *(received_velocity_msg_ptr_.readFromNonRT()); + twist_stamped->header.stamp = get_node()->now(); + // if no timestamp provided use current time for command timestamp + if (twist_stamped->header.stamp.sec == 0 && twist_stamped->header.stamp.nanosec == 0u) + { + RCLCPP_WARN( + get_node()->get_logger(), + "Timestamp in header is missing, using current time as command timestamp."); + twist_stamped->header.stamp = get_node()->now(); + } + + const auto age_of_last_command = get_node()->now() - twist_stamped->header.stamp; + + if (ref_timeout_ == rclcpp::Duration::from_seconds(0) || age_of_last_command <= ref_timeout_) + { + twist_stamped->twist = *msg; + } + else + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Received message has timestamp %.10f older for %.10f which is more then allowed timeout " + "(%.4f).", + rclcpp::Time(twist_stamped->header.stamp).seconds(), age_of_last_command.seconds(), + ref_timeout_.seconds()); + } +} + +controller_interface::return_type DiffDriveController::update_reference_from_subscribers() +{ + auto current_ref = *(received_velocity_msg_ptr_.readFromRT()); + + if (!std::isnan(current_ref->twist.linear.x) && !std::isnan(current_ref->twist.angular.z)) + { + // The linear velocity is decomposed into x/y components since we depend on that in the control + // loop + reference_interfaces_[0] = std::cos(odometry_.getHeading()) * current_ref->twist.linear.x; + reference_interfaces_[1] = std::sin(odometry_.getHeading()) * current_ref->twist.linear.x; + reference_interfaces_[2] = current_ref->twist.angular.z; + } + + return controller_interface::return_type::OK; +} + +controller_interface::return_type DiffDriveController::update_and_write_commands( + const rclcpp::Time & time, const rclcpp::Duration & period) +{ + // Compute and set the linear velocity and direction, copy the angular velocity command. + // The velocity angle can converge significantly from the current direction if theta is non-zero. + // The backward flag allows for M_PI_2 tolerance between these angles. + const double vel_angle = std::atan2(reference_interfaces_[1], reference_interfaces_[0]); + const bool backward = std::abs(std::abs(odometry_.getHeading() - vel_angle) - M_PI) < M_PI_2; + const double linear_command = + std::hypot(reference_interfaces_[0], reference_interfaces_[1]) * (backward ? -1 : 1); + const double angular_command = reference_interfaces_[2]; + + previous_update_timestamp_ = time; + + // Apply (possibly new) multipliers: + const double wheel_separation = params_.wheel_separation_multiplier * params_.wheel_separation; + const double left_wheel_radius = params_.left_wheel_radius_multiplier * params_.wheel_radius; + const double right_wheel_radius = params_.right_wheel_radius_multiplier * params_.wheel_radius; + + if (params_.open_loop) + { + odometry_.updateOpenLoop(linear_command, angular_command, time); + } + else + { + double left_feedback_mean = 0.0; + double right_feedback_mean = 0.0; + for (size_t index = 0; index < static_cast(params_.wheels_per_side); ++index) + { + const double left_feedback = registered_left_wheel_handles_[index].feedback.get().get_value(); + const double right_feedback = + registered_right_wheel_handles_[index].feedback.get().get_value(); + + if (std::isnan(left_feedback) || std::isnan(right_feedback)) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Either the left or right wheel %s is invalid for index [%zu]", + feedback_type(), index); + return controller_interface::return_type::ERROR; + } + + left_feedback_mean += left_feedback; + right_feedback_mean += right_feedback; + } + left_feedback_mean /= params_.wheels_per_side; + right_feedback_mean /= params_.wheels_per_side; + + if (params_.position_feedback) + { + odometry_.update(left_feedback_mean, right_feedback_mean, time); + } + else + { + odometry_.updateFromVelocity( + left_feedback_mean * left_wheel_radius * period.seconds(), + right_feedback_mean * right_wheel_radius * period.seconds(), time); + } + } + + tf2::Quaternion orientation; + orientation.setRPY(0.0, 0.0, odometry_.getHeading()); + + bool should_publish = false; + try + { + if (previous_publish_timestamp_ + publish_period_ < time) + { + previous_publish_timestamp_ += publish_period_; + should_publish = true; + } + } + catch (const std::runtime_error &) + { + // Handle exceptions when the time source changes and initialize publish timestamp + previous_publish_timestamp_ = time; + should_publish = true; + } + + if (should_publish) + { + if (realtime_odometry_publisher_->trylock()) + { + auto & odometry_message = realtime_odometry_publisher_->msg_; + odometry_message.header.stamp = time; + odometry_message.pose.pose.position.x = odometry_.getX(); + odometry_message.pose.pose.position.y = odometry_.getY(); + odometry_message.pose.pose.orientation.x = orientation.x(); + odometry_message.pose.pose.orientation.y = orientation.y(); + odometry_message.pose.pose.orientation.z = orientation.z(); + odometry_message.pose.pose.orientation.w = orientation.w(); + odometry_message.twist.twist.linear.x = odometry_.getLinear(); + odometry_message.twist.twist.angular.z = odometry_.getAngular(); + realtime_odometry_publisher_->unlockAndPublish(); + } + + if (params_.enable_odom_tf && realtime_odometry_transform_publisher_->trylock()) + { + auto & transform = realtime_odometry_transform_publisher_->msg_.transforms.front(); + transform.header.stamp = time; + transform.transform.translation.x = odometry_.getX(); + transform.transform.translation.y = odometry_.getY(); + transform.transform.rotation.x = orientation.x(); + transform.transform.rotation.y = orientation.y(); + transform.transform.rotation.z = orientation.z(); + transform.transform.rotation.w = orientation.w(); + realtime_odometry_transform_publisher_->unlockAndPublish(); + } + } + + // auto & last_command = previous_commands_.back().twist; + // auto & second_to_last_command = previous_commands_.front().twist; + // limiter_linear_.limit( + // linear_command, last_command.linear.x, second_to_last_command.linear.x, period.seconds()); + // limiter_angular_.limit( + // angular_command, last_command.angular.z, second_to_last_command.angular.z, period.seconds()); + + // previous_commands_.pop(); + // previous_commands_.emplace(command); + + // // Publish limited velocity + // if (publish_limited_velocity_ && realtime_limited_velocity_publisher_->trylock()) + // { + // auto & limited_velocity_command = realtime_limited_velocity_publisher_->msg_; + // limited_velocity_command.header.stamp = time; + // limited_velocity_command.twist = command.twist; + // realtime_limited_velocity_publisher_->unlockAndPublish(); + // } + + // Compute wheels velocities: + const double velocity_left = + (linear_command - angular_command * wheel_separation / 2.0) / left_wheel_radius; + const double velocity_right = + (linear_command + angular_command * wheel_separation / 2.0) / right_wheel_radius; + + // Set wheels velocities: + for (size_t index = 0; index < static_cast(params_.wheels_per_side); ++index) + { + registered_left_wheel_handles_[index].velocity.get().set_value(velocity_left); + registered_right_wheel_handles_[index].velocity.get().set_value(velocity_right); + } + + return controller_interface::return_type::OK; +} + +controller_interface::CallbackReturn DiffDriveController::on_configure( + const rclcpp_lifecycle::State &) +{ + auto logger = get_node()->get_logger(); + + // update parameters if they have changed + if (param_listener_->is_old(params_)) + { + params_ = param_listener_->get_params(); + RCLCPP_INFO(logger, "Parameters were updated"); + } + + if (params_.left_wheel_names.size() != params_.right_wheel_names.size()) + { + RCLCPP_ERROR( + logger, "The number of left wheels [%zu] and the number of right wheels [%zu] are different", + params_.left_wheel_names.size(), params_.right_wheel_names.size()); + return controller_interface::CallbackReturn::ERROR; + } + + if (params_.left_wheel_names.empty()) + { + RCLCPP_ERROR(logger, "Wheel names parameters are empty!"); + return controller_interface::CallbackReturn::ERROR; + } + + const double wheel_separation = params_.wheel_separation_multiplier * params_.wheel_separation; + const double left_wheel_radius = params_.left_wheel_radius_multiplier * params_.wheel_radius; + const double right_wheel_radius = params_.right_wheel_radius_multiplier * params_.wheel_radius; + + odometry_.setWheelParams(wheel_separation, left_wheel_radius, right_wheel_radius); + odometry_.setVelocityRollingWindowSize(params_.velocity_rolling_window_size); + + ref_timeout_ = rclcpp::Duration::from_seconds(params_.cmd_vel_timeout); + publish_limited_velocity_ = params_.publish_limited_velocity; + + limiter_linear_ = SpeedLimiter( + params_.linear.x.has_velocity_limits, params_.linear.x.has_acceleration_limits, + params_.linear.x.has_jerk_limits, params_.linear.x.min_velocity, params_.linear.x.max_velocity, + params_.linear.x.min_acceleration, params_.linear.x.max_acceleration, params_.linear.x.min_jerk, + params_.linear.x.max_jerk); + + limiter_angular_ = SpeedLimiter( + params_.angular.z.has_velocity_limits, params_.angular.z.has_acceleration_limits, + params_.angular.z.has_jerk_limits, params_.angular.z.min_velocity, + params_.angular.z.max_velocity, params_.angular.z.min_acceleration, + params_.angular.z.max_acceleration, params_.angular.z.min_jerk, params_.angular.z.max_jerk); + + if (!reset()) + { + return controller_interface::CallbackReturn::ERROR; + } + + // left and right sides are both equal at this point + params_.wheels_per_side = params_.left_wheel_names.size(); + + if (publish_limited_velocity_) + { + limited_velocity_publisher_ = get_node()->create_publisher( + DEFAULT_COMMAND_OUT_TOPIC, rclcpp::SystemDefaultsQoS()); + realtime_limited_velocity_publisher_ = + std::make_shared>( + limited_velocity_publisher_); + } + + received_velocity_msg_ptr_.reset(); + + // Fill last two commands with default constructed commands + previous_commands_.emplace(ControllerTwistReferenceMsg()); + previous_commands_.emplace(ControllerTwistReferenceMsg()); + + // 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_.cmd_vel_timeout); + if (params_.use_stamped_vel) + { + velocity_command_subscriber_ = get_node()->create_subscription( + DEFAULT_COMMAND_TOPIC, subscribers_qos, + std::bind(&DiffDriveController::reference_callback, this, std::placeholders::_1)); + } + else + { + velocity_command_unstamped_subscriber_ = + get_node()->create_subscription( + DEFAULT_COMMAND_UNSTAMPED_TOPIC, subscribers_qos, + std::bind(&DiffDriveController::reference_callback_unstamped, this, std::placeholders::_1)); + } + + std::shared_ptr msg = + std::make_shared(); + reset_controller_reference_msg(msg, get_node()); + received_velocity_msg_ptr_.writeFromNonRT(msg); + + // initialize odometry publisher and messasge + odometry_publisher_ = get_node()->create_publisher( + DEFAULT_ODOMETRY_TOPIC, rclcpp::SystemDefaultsQoS()); + realtime_odometry_publisher_ = + std::make_shared>( + odometry_publisher_); + + // Append the tf prefix if there is one + std::string tf_prefix = ""; + if (params_.tf_frame_prefix_enable) + { + if (params_.tf_frame_prefix != "") + { + tf_prefix = params_.tf_frame_prefix; + } + else + { + tf_prefix = std::string(get_node()->get_namespace()); + } + + if (tf_prefix == "/") + { + tf_prefix = ""; + } + else + { + tf_prefix = tf_prefix + "/"; + } + } + + const auto odom_frame_id = tf_prefix + params_.odom_frame_id; + const auto base_frame_id = tf_prefix + params_.base_frame_id; + + auto & odometry_message = realtime_odometry_publisher_->msg_; + odometry_message.header.frame_id = odom_frame_id; + odometry_message.child_frame_id = base_frame_id; + + // limit the publication on the topics /odom and /tf + publish_rate_ = params_.publish_rate; + publish_period_ = rclcpp::Duration::from_seconds(1.0 / publish_rate_); + + // initialize odom values zeros + odometry_message.twist = + geometry_msgs::msg::TwistWithCovariance(rosidl_runtime_cpp::MessageInitialization::ALL); + + constexpr size_t NUM_DIMENSIONS = 6; + for (size_t index = 0; index < 6; ++index) + { + // 0, 7, 14, 21, 28, 35 + const size_t diagonal_index = NUM_DIMENSIONS * index + index; + odometry_message.pose.covariance[diagonal_index] = params_.pose_covariance_diagonal[index]; + odometry_message.twist.covariance[diagonal_index] = params_.twist_covariance_diagonal[index]; + } + + // initialize transform publisher and message + odometry_transform_publisher_ = get_node()->create_publisher( + DEFAULT_TRANSFORM_TOPIC, rclcpp::SystemDefaultsQoS()); + realtime_odometry_transform_publisher_ = + std::make_shared>( + odometry_transform_publisher_); + + // keeping track of odom and base_link transforms only + auto & odometry_transform_message = realtime_odometry_transform_publisher_->msg_; + odometry_transform_message.transforms.resize(1); + odometry_transform_message.transforms.front().header.frame_id = odom_frame_id; + odometry_transform_message.transforms.front().child_frame_id = base_frame_id; + + previous_update_timestamp_ = get_node()->get_clock()->now(); + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn DiffDriveController::on_activate( + const rclcpp_lifecycle::State &) +{ + // Set default value in command + reset_controller_reference_msg(*(received_velocity_msg_ptr_.readFromRT()), get_node()); + + const auto left_result = + configure_side("left", params_.left_wheel_names, registered_left_wheel_handles_); + const auto right_result = + configure_side("right", params_.right_wheel_names, registered_right_wheel_handles_); + + if ( + left_result == controller_interface::CallbackReturn::ERROR || + right_result == controller_interface::CallbackReturn::ERROR) + { + return controller_interface::CallbackReturn::ERROR; + } + + if (registered_left_wheel_handles_.empty() || registered_right_wheel_handles_.empty()) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Either left wheel interfaces, right wheel interfaces are non existent"); + return controller_interface::CallbackReturn::ERROR; + } + + is_halted = false; + subscriber_is_active_ = true; + + RCLCPP_DEBUG(get_node()->get_logger(), "Subscriber and publisher are now active."); + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn DiffDriveController::on_deactivate( + const rclcpp_lifecycle::State &) +{ + subscriber_is_active_ = false; + if (!is_halted) + { + halt(); + is_halted = true; + } + registered_left_wheel_handles_.clear(); + registered_right_wheel_handles_.clear(); + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn DiffDriveController::on_cleanup( + const rclcpp_lifecycle::State &) +{ + if (!reset()) + { + return controller_interface::CallbackReturn::ERROR; + } + + received_velocity_msg_ptr_.reset(); + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn DiffDriveController::on_error(const rclcpp_lifecycle::State &) +{ + if (!reset()) + { + return controller_interface::CallbackReturn::ERROR; + } + return controller_interface::CallbackReturn::SUCCESS; +} + +bool DiffDriveController::reset() +{ + odometry_.resetOdometry(); + + // release the old queue + std::queue empty; + std::swap(previous_commands_, empty); + + registered_left_wheel_handles_.clear(); + registered_right_wheel_handles_.clear(); + + subscriber_is_active_ = false; + velocity_command_subscriber_.reset(); + velocity_command_unstamped_subscriber_.reset(); + + received_velocity_msg_ptr_.reset(); + is_halted = false; + return true; +} + +controller_interface::CallbackReturn DiffDriveController::on_shutdown( + const rclcpp_lifecycle::State &) +{ + return controller_interface::CallbackReturn::SUCCESS; +} + +void DiffDriveController::halt() +{ + const auto halt_wheels = [](auto & wheel_handles) + { + for (const auto & wheel_handle : wheel_handles) + { + wheel_handle.velocity.get().set_value(0.0); + } + }; + + halt_wheels(registered_left_wheel_handles_); + halt_wheels(registered_right_wheel_handles_); +} + +controller_interface::CallbackReturn DiffDriveController::configure_side( + const std::string & side, const std::vector & wheel_names, + std::vector & registered_handles) +{ + auto logger = get_node()->get_logger(); + + if (wheel_names.empty()) + { + RCLCPP_ERROR(logger, "No '%s' wheel names specified", side.c_str()); + return controller_interface::CallbackReturn::ERROR; + } + + // register handles + registered_handles.reserve(wheel_names.size()); + for (const auto & wheel_name : wheel_names) + { + const auto interface_name = feedback_type(); + const auto state_handle = std::find_if( + state_interfaces_.cbegin(), state_interfaces_.cend(), + [&wheel_name, &interface_name](const auto & interface) + { + return interface.get_prefix_name() == wheel_name && + interface.get_interface_name() == interface_name; + }); + + if (state_handle == state_interfaces_.cend()) + { + RCLCPP_ERROR(logger, "Unable to obtain joint state handle for %s", wheel_name.c_str()); + return controller_interface::CallbackReturn::ERROR; + } + + const auto command_handle = std::find_if( + command_interfaces_.begin(), command_interfaces_.end(), + [&wheel_name](const auto & interface) + { + return interface.get_prefix_name() == wheel_name && + interface.get_interface_name() == HW_IF_VELOCITY; + }); + + if (command_handle == command_interfaces_.end()) + { + RCLCPP_ERROR(logger, "Unable to obtain joint command handle for %s", wheel_name.c_str()); + return controller_interface::CallbackReturn::ERROR; + } + + registered_handles.emplace_back( + WheelHandle{std::ref(*state_handle), std::ref(*command_handle)}); + } + + return controller_interface::CallbackReturn::SUCCESS; +} + +bool DiffDriveController::on_set_chained_mode(bool chained_mode) +{ + // Always accept switch to/from chained mode + return true || chained_mode; +} + +} // namespace picknik_diff_drive_controller + +#include "class_loader/register_macro.hpp" + +CLASS_LOADER_REGISTER_CLASS( + picknik_diff_drive_controller::DiffDriveController, + controller_interface::ChainableControllerInterface) diff --git a/picknik_diff_drive_controller/src/diff_drive_controller_parameter.yaml b/picknik_diff_drive_controller/src/diff_drive_controller_parameter.yaml new file mode 100644 index 0000000..bfafe7b --- /dev/null +++ b/picknik_diff_drive_controller/src/diff_drive_controller_parameter.yaml @@ -0,0 +1,187 @@ +picknik_diff_drive_controller: + left_wheel_names: { + type: string_array, + default_value: [], + description: "Link names of the left side wheels", + } + right_wheel_names: { + type: string_array, + default_value: [], + description: "Link names of the right side wheels", + } + wheel_separation: { + type: double, + default_value: 0.0, + description: "Shortest distance between the left and right wheels. If this parameter is wrong, the robot will not behave correctly in curves.", + } + wheels_per_side: { + type: int, + default_value: 0, + description: "Number of wheels on each side of the robot. This is important to take the wheels slip into account when multiple wheels on each side are present. If there are more wheels then control signals for each side, you should enter number for control signals. For example, Husky has two wheels on each side, but they use one control signal, in this case '1' is the correct value of the parameter.", + } + wheel_radius: { + type: double, + default_value: 0.0, + description: "Radius of a wheel, i.e., wheels size, used for transformation of linear velocity into wheel rotations. If this parameter is wrong the robot will move faster or slower then expected.", + } + wheel_separation_multiplier: { + type: double, + default_value: 1.0, + description: "Correction factor for wheel separation (TODO(destogl): Please help me describe this correctly)", + } + left_wheel_radius_multiplier: { + type: double, + default_value: 1.0, + description: "Correction factor when radius of left wheels differs from the nominal value in ``wheel_radius`` parameter.", + } + right_wheel_radius_multiplier: { + type: double, + default_value: 1.0, + description: "Correction factor when radius of right wheels differs from the nominal value in ``wheel_radius`` parameter.", + } + tf_frame_prefix_enable: { + type: bool, + default_value: true, + description: "Enables or disables appending tf_prefix to tf frame id's.", + } + tf_frame_prefix: { + type: string, + default_value: "", + description: "(optional) Prefix to be appended to the tf frames, will be added to odom_id and base_frame_id before publishing. If the parameter is empty, controller's namespace will be used.", + } + odom_frame_id: { + type: string, + default_value: "odom", + description: "Name of the frame for odometry. This frame is parent of ``base_frame_id`` when controller publishes odometry.", + } + base_frame_id: { + type: string, + default_value: "base_link", + description: "Name of the robot's base frame that is child of the odometry frame.", + } + pose_covariance_diagonal: { + type: double_array, + default_value: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + description: "Odometry covariance for the encoder output of the robot for the pose. These values should be tuned to your robot's sample odometry data, but these values are a good place to start: ``[0.001, 0.001, 0.001, 0.001, 0.001, 0.01]``.", + } + twist_covariance_diagonal: { + type: double_array, + default_value: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + description: "Odometry covariance for the encoder output of the robot for the speed. These values should be tuned to your robot's sample odometry data, but these values are a good place to start: ``[0.001, 0.001, 0.001, 0.001, 0.001, 0.01]``.", + } + open_loop: { + type: bool, + default_value: false, + description: "If set to true the odometry of the robot will be calculated from the commanded values and not from feedback.", + } + position_feedback: { + type: bool, + default_value: true, + description: "Is there position feedback from hardware.", + } + enable_odom_tf: { + type: bool, + default_value: true, + description: "Publish transformation between ``odom_frame_id`` and ``base_frame_id``.", + } + cmd_vel_timeout: { + type: double, + default_value: 0.5, # seconds + description: "Timeout in seconds, after which input command on ``cmd_vel`` topic is considered staled.", + } + publish_limited_velocity: { + type: bool, + default_value: false, + description: "Publish limited velocity value.", + } + velocity_rolling_window_size: { + type: int, + default_value: 10, + description: "Size of the rolling window for calculation of mean velocity use in odometry.", + } + use_stamped_vel: { + type: bool, + default_value: true, + description: "Use stamp from input velocity message to calculate how old the command actually is.", + } + publish_rate: { + type: double, + default_value: 50.0, # Hz + description: "Publishing rate (Hz) of the odometry and TF messages.", + } + linear: + x: + has_velocity_limits: { + type: bool, + default_value: false, + } + has_acceleration_limits: { + type: bool, + default_value: false, + } + has_jerk_limits: { + type: bool, + default_value: false, + } + max_velocity: { + type: double, + default_value: .NAN, + } + min_velocity: { + type: double, + default_value: .NAN, + } + max_acceleration: { + type: double, + default_value: .NAN, + } + min_acceleration: { + type: double, + default_value: .NAN, + } + max_jerk: { + type: double, + default_value: .NAN, + } + min_jerk: { + type: double, + default_value: .NAN, + } + angular: + z: + has_velocity_limits: { + type: bool, + default_value: false, + } + has_acceleration_limits: { + type: bool, + default_value: false, + } + has_jerk_limits: { + type: bool, + default_value: false, + } + max_velocity: { + type: double, + default_value: .NAN, + } + min_velocity: { + type: double, + default_value: .NAN, + } + max_acceleration: { + type: double, + default_value: .NAN, + } + min_acceleration: { + type: double, + default_value: .NAN, + } + max_jerk: { + type: double, + default_value: .NAN, + } + min_jerk: { + type: double, + default_value: .NAN, + } diff --git a/picknik_diff_drive_controller/src/odometry.cpp b/picknik_diff_drive_controller/src/odometry.cpp new file mode 100644 index 0000000..b816f80 --- /dev/null +++ b/picknik_diff_drive_controller/src/odometry.cpp @@ -0,0 +1,165 @@ +// Copyright 2020 PAL Robotics S.L. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/* + * Author: Enrique Fernández + */ + +#include "picknik_diff_drive_controller/odometry.hpp" + +namespace picknik_diff_drive_controller +{ +Odometry::Odometry(size_t velocity_rolling_window_size) +: timestamp_(0.0), + x_(0.0), + y_(0.0), + heading_(0.0), + linear_(0.0), + angular_(0.0), + wheel_separation_(0.0), + left_wheel_radius_(0.0), + right_wheel_radius_(0.0), + left_wheel_old_pos_(0.0), + right_wheel_old_pos_(0.0), + velocity_rolling_window_size_(velocity_rolling_window_size), + linear_accumulator_(velocity_rolling_window_size), + angular_accumulator_(velocity_rolling_window_size) +{ +} + +void Odometry::init(const rclcpp::Time & time) +{ + // Reset accumulators and timestamp: + resetAccumulators(); + timestamp_ = time; +} + +bool Odometry::update(double left_pos, double right_pos, const rclcpp::Time & time) +{ + // We cannot estimate the speed with very small time intervals: + const double dt = time.seconds() - timestamp_.seconds(); + if (dt < 0.0001) + { + return false; // Interval too small to integrate with + } + + // Get current wheel joint positions: + const double left_wheel_cur_pos = left_pos * left_wheel_radius_; + const double right_wheel_cur_pos = right_pos * right_wheel_radius_; + + // Estimate velocity of wheels using old and current position: + const double left_wheel_est_vel = left_wheel_cur_pos - left_wheel_old_pos_; + const double right_wheel_est_vel = right_wheel_cur_pos - right_wheel_old_pos_; + + // Update old position with current: + left_wheel_old_pos_ = left_wheel_cur_pos; + right_wheel_old_pos_ = right_wheel_cur_pos; + + updateFromVelocity(left_wheel_est_vel, right_wheel_est_vel, time); + + return true; +} + +bool Odometry::updateFromVelocity(double left_vel, double right_vel, const rclcpp::Time & time) +{ + const double dt = time.seconds() - timestamp_.seconds(); + + // Compute linear and angular diff: + const double linear = (left_vel + right_vel) * 0.5; + // Now there is a bug about scout angular velocity + const double angular = (right_vel - left_vel) / wheel_separation_; + + // Integrate odometry: + integrateExact(linear, angular); + + timestamp_ = time; + + // Estimate speeds using a rolling mean to filter them out: + linear_accumulator_.accumulate(linear / dt); + angular_accumulator_.accumulate(angular / dt); + + linear_ = linear_accumulator_.getRollingMean(); + angular_ = angular_accumulator_.getRollingMean(); + + return true; +} + +void Odometry::updateOpenLoop(double linear, double angular, const rclcpp::Time & time) +{ + /// Save last linear and angular velocity: + linear_ = linear; + angular_ = angular; + + /// Integrate odometry: + const double dt = time.seconds() - timestamp_.seconds(); + timestamp_ = time; + integrateExact(linear * dt, angular * dt); +} + +void Odometry::resetOdometry() +{ + x_ = 0.0; + y_ = 0.0; + heading_ = 0.0; +} + +void Odometry::setWheelParams( + double wheel_separation, double left_wheel_radius, double right_wheel_radius) +{ + wheel_separation_ = wheel_separation; + left_wheel_radius_ = left_wheel_radius; + right_wheel_radius_ = right_wheel_radius; +} + +void Odometry::setVelocityRollingWindowSize(size_t velocity_rolling_window_size) +{ + velocity_rolling_window_size_ = velocity_rolling_window_size; + + resetAccumulators(); +} + +void Odometry::integrateRungeKutta2(double linear, double angular) +{ + const double direction = heading_ + angular * 0.5; + + /// Runge-Kutta 2nd order integration: + x_ += linear * cos(direction); + y_ += linear * sin(direction); + heading_ += angular; +} + +void Odometry::integrateExact(double linear, double angular) +{ + if (fabs(angular) < 1e-6) + { + integrateRungeKutta2(linear, angular); + } + else + { + /// Exact integration (should solve problems when angular is zero): + const double heading_old = heading_; + const double r = linear / angular; + heading_ += angular; + x_ += r * (sin(heading_) - sin(heading_old)); + y_ += -r * (cos(heading_) - cos(heading_old)); + } +} + +void Odometry::resetAccumulators() +{ + linear_accumulator_ = RollingMeanAccumulator(velocity_rolling_window_size_); + angular_accumulator_ = RollingMeanAccumulator(velocity_rolling_window_size_); +} + +} // namespace picknik_diff_drive_controller diff --git a/picknik_diff_drive_controller/src/speed_limiter.cpp b/picknik_diff_drive_controller/src/speed_limiter.cpp new file mode 100644 index 0000000..d0ca632 --- /dev/null +++ b/picknik_diff_drive_controller/src/speed_limiter.cpp @@ -0,0 +1,140 @@ +// Copyright 2020 PAL Robotics S.L. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/* + * Author: Enrique Fernández + */ + +#include +#include + +#include "picknik_diff_drive_controller/speed_limiter.hpp" +#include "rcppmath/clamp.hpp" + +namespace picknik_diff_drive_controller +{ +SpeedLimiter::SpeedLimiter( + bool has_velocity_limits, bool has_acceleration_limits, bool has_jerk_limits, double min_velocity, + double max_velocity, double min_acceleration, double max_acceleration, double min_jerk, + double max_jerk) +: has_velocity_limits_(has_velocity_limits), + has_acceleration_limits_(has_acceleration_limits), + has_jerk_limits_(has_jerk_limits), + min_velocity_(min_velocity), + max_velocity_(max_velocity), + min_acceleration_(min_acceleration), + max_acceleration_(max_acceleration), + min_jerk_(min_jerk), + max_jerk_(max_jerk) +{ + // Check if limits are valid, max must be specified, min defaults to -max if unspecified + if (has_velocity_limits_) + { + if (std::isnan(max_velocity_)) + { + throw std::runtime_error("Cannot apply velocity limits if max_velocity is not specified"); + } + if (std::isnan(min_velocity_)) + { + min_velocity_ = -max_velocity_; + } + } + if (has_acceleration_limits_) + { + if (std::isnan(max_acceleration_)) + { + throw std::runtime_error( + "Cannot apply acceleration limits if max_acceleration is not specified"); + } + if (std::isnan(min_acceleration_)) + { + min_acceleration_ = -max_acceleration_; + } + } + if (has_jerk_limits_) + { + if (std::isnan(max_jerk_)) + { + throw std::runtime_error("Cannot apply jerk limits if max_jerk is not specified"); + } + if (std::isnan(min_jerk_)) + { + min_jerk_ = -max_jerk_; + } + } +} + +double SpeedLimiter::limit(double & v, double v0, double v1, double dt) +{ + const double tmp = v; + + limit_jerk(v, v0, v1, dt); + limit_acceleration(v, v0, dt); + limit_velocity(v); + + return tmp != 0.0 ? v / tmp : 1.0; +} + +double SpeedLimiter::limit_velocity(double & v) +{ + const double tmp = v; + + if (has_velocity_limits_) + { + v = rcppmath::clamp(v, min_velocity_, max_velocity_); + } + + return tmp != 0.0 ? v / tmp : 1.0; +} + +double SpeedLimiter::limit_acceleration(double & v, double v0, double dt) +{ + const double tmp = v; + + if (has_acceleration_limits_) + { + const double dv_min = min_acceleration_ * dt; + const double dv_max = max_acceleration_ * dt; + + const double dv = rcppmath::clamp(v - v0, dv_min, dv_max); + + v = v0 + dv; + } + + return tmp != 0.0 ? v / tmp : 1.0; +} + +double SpeedLimiter::limit_jerk(double & v, double v0, double v1, double dt) +{ + const double tmp = v; + + if (has_jerk_limits_) + { + const double dv = v - v0; + const double dv0 = v0 - v1; + + const double dt2 = 2. * dt * dt; + + const double da_min = min_jerk_ * dt2; + const double da_max = max_jerk_ * dt2; + + const double da = rcppmath::clamp(dv - dv0, da_min, da_max); + + v = v0 + dv0 + da; + } + + return tmp != 0.0 ? v / tmp : 1.0; +} + +} // namespace picknik_diff_drive_controller diff --git a/picknik_diff_drive_controller/test/config/test_diff_drive_controller.yaml b/picknik_diff_drive_controller/test/config/test_diff_drive_controller.yaml new file mode 100644 index 0000000..a2149eb --- /dev/null +++ b/picknik_diff_drive_controller/test/config/test_diff_drive_controller.yaml @@ -0,0 +1,45 @@ +test_diff_drive_controller: + ros__parameters: + left_wheel_names: ["left_wheels"] + right_wheel_names: ["right_wheels"] + write_op_modes: ["motor_controller"] + + wheel_separation: 0.40 + wheels_per_side: 1 # actually 2, but both are controlled by 1 signal + wheel_radius: 0.02 + + wheel_separation_multiplier: 1.0 + left_wheel_radius_multiplier: 1.0 + right_wheel_radius_multiplier: 1.0 + + odom_frame_id: odom + base_frame_id: base_link + pose_covariance_diagonal: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + twist_covariance_diagonal: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + + position_feedback: false + open_loop: true + enable_odom_tf: true + + cmd_vel_timeout: 500 # milliseconds + publish_limited_velocity: true + velocity_rolling_window_size: 10 + + linear.x.has_velocity_limits: false + linear.x.has_acceleration_limits: false + linear.x.has_jerk_limits: false + linear.x.max_velocity: 0.0 + linear.x.min_velocity: 0.0 + linear.x.max_acceleration: 0.0 + linear.x.max_jerk: 0.0 + linear.x.min_jerk: 0.0 + + angular.z.has_velocity_limits: false + angular.z.has_acceleration_limits: false + angular.z.has_jerk_limits: false + angular.z.max_velocity: 0.0 + angular.z.min_velocity: 0.0 + angular.z.max_acceleration: 0.0 + angular.z.min_acceleration: 0.0 + angular.z.max_jerk: 0.0 + angular.z.min_jerk: 0.0 diff --git a/picknik_diff_drive_controller/test/test_diff_drive_controller.cpp b/picknik_diff_drive_controller/test/test_diff_drive_controller.cpp new file mode 100644 index 0000000..4c94396 --- /dev/null +++ b/picknik_diff_drive_controller/test/test_diff_drive_controller.cpp @@ -0,0 +1,655 @@ +// Copyright 2020 PAL Robotics SL. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include +#include +#include + +#include "hardware_interface/loaned_command_interface.hpp" +#include "hardware_interface/loaned_state_interface.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "picknik_diff_drive_controller/diff_drive_controller.hpp" +#include "rclcpp/rclcpp.hpp" + +using CallbackReturn = controller_interface::CallbackReturn; +using hardware_interface::HW_IF_POSITION; +using hardware_interface::HW_IF_VELOCITY; +using hardware_interface::LoanedCommandInterface; +using hardware_interface::LoanedStateInterface; +using lifecycle_msgs::msg::State; +using testing::SizeIs; + +class TestableDiffDriveController : public picknik_diff_drive_controller::DiffDriveController +{ +public: + using DiffDriveController::DiffDriveController; + std::shared_ptr getLastReceivedTwist() + { + return *(received_velocity_msg_ptr_.readFromNonRT()); + } + + /** + * @brief wait_for_twist block until a new twist is received. + * Requires that the executor is not spinned elsewhere between the + * message publication and the call to this function + * + * @return true if new twist msg was received, false if timeout + */ + bool wait_for_twist( + rclcpp::Executor & executor, + const std::chrono::milliseconds & timeout = std::chrono::milliseconds(500)) + { + rclcpp::WaitSet wait_set; + wait_set.add_subscription(velocity_command_subscriber_); + + if (wait_set.wait(timeout).kind() == rclcpp::WaitResultKind::Ready) + { + executor.spin_some(); + return true; + } + return false; + } + + /** + * @brief Used to get the real_time_odometry_publisher to verify its contents + * + * @return Copy of realtime_odometry_publisher_ object + */ + std::shared_ptr> + get_rt_odom_publisher() + { + return realtime_odometry_publisher_; + } +}; + +class TestDiffDriveController : public ::testing::Test +{ +protected: + static void SetUpTestCase() { rclcpp::init(0, nullptr); } + + void SetUp() override + { + controller_ = std::make_unique(); + + pub_node = std::make_shared("velocity_publisher"); + velocity_publisher = pub_node->create_publisher( + controller_name + "/cmd_vel", rclcpp::SystemDefaultsQoS()); + } + + static void TearDownTestCase() { rclcpp::shutdown(); } + + /// Publish velocity msgs + /** + * linear - magnitude of the linear command in the geometry_msgs::twist message + * angular - the magnitude of the angular command in geometry_msgs::twist message + */ + void publish(double linear, double angular) + { + int wait_count = 0; + auto topic = velocity_publisher->get_topic_name(); + while (pub_node->count_subscribers(topic) == 0) + { + if (wait_count >= 5) + { + auto error_msg = std::string("publishing to ") + topic + " but no node subscribes to it"; + throw std::runtime_error(error_msg); + } + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + ++wait_count; + } + + geometry_msgs::msg::TwistStamped velocity_message; + velocity_message.header.stamp = pub_node->get_clock()->now(); + velocity_message.twist.linear.x = linear; + velocity_message.twist.angular.z = angular; + velocity_publisher->publish(velocity_message); + } + + /// \brief wait for the subscriber and publisher to completely setup + void waitForSetup() + { + constexpr std::chrono::seconds TIMEOUT{2}; + auto clock = pub_node->get_clock(); + auto start = clock->now(); + while (velocity_publisher->get_subscription_count() <= 0) + { + if ((clock->now() - start) > TIMEOUT) + { + FAIL(); + } + rclcpp::spin_some(pub_node); + } + } + + void assignResourcesPosFeedback() + { + std::vector state_ifs; + state_ifs.emplace_back(left_wheel_pos_state_); + state_ifs.emplace_back(right_wheel_pos_state_); + + std::vector command_ifs; + command_ifs.emplace_back(left_wheel_vel_cmd_); + command_ifs.emplace_back(right_wheel_vel_cmd_); + + controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); + } + + void assignResourcesVelFeedback() + { + std::vector state_ifs; + state_ifs.emplace_back(left_wheel_vel_state_); + state_ifs.emplace_back(right_wheel_vel_state_); + + std::vector command_ifs; + command_ifs.emplace_back(left_wheel_vel_cmd_); + command_ifs.emplace_back(right_wheel_vel_cmd_); + + controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); + } + + const std::string controller_name = "test_diff_drive_controller"; + std::unique_ptr controller_; + + const std::vector left_wheel_names = {"left_wheel_joint"}; + const std::vector right_wheel_names = {"right_wheel_joint"}; + std::vector position_values_ = {0.1, 0.2}; + std::vector velocity_values_ = {0.01, 0.02}; + + hardware_interface::StateInterface left_wheel_pos_state_{ + left_wheel_names[0], HW_IF_POSITION, &position_values_[0]}; + hardware_interface::StateInterface right_wheel_pos_state_{ + right_wheel_names[0], HW_IF_POSITION, &position_values_[1]}; + hardware_interface::StateInterface left_wheel_vel_state_{ + left_wheel_names[0], HW_IF_VELOCITY, &velocity_values_[0]}; + hardware_interface::StateInterface right_wheel_vel_state_{ + right_wheel_names[0], HW_IF_VELOCITY, &velocity_values_[1]}; + hardware_interface::CommandInterface left_wheel_vel_cmd_{ + left_wheel_names[0], HW_IF_VELOCITY, &velocity_values_[0]}; + hardware_interface::CommandInterface right_wheel_vel_cmd_{ + right_wheel_names[0], HW_IF_VELOCITY, &velocity_values_[1]}; + + rclcpp::Node::SharedPtr pub_node; + rclcpp::Publisher::SharedPtr velocity_publisher; +}; + +TEST_F(TestDiffDriveController, configure_fails_without_parameters) +{ + const auto ret = controller_->init(controller_name); + ASSERT_EQ(ret, controller_interface::return_type::OK); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); +} + +TEST_F(TestDiffDriveController, configure_fails_with_only_left_or_only_right_side_defined) +{ + const auto ret = controller_->init(controller_name); + ASSERT_EQ(ret, controller_interface::return_type::OK); + + controller_->get_node()->set_parameter( + rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(std::vector()))); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); + + controller_->get_node()->set_parameter( + rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(std::vector()))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); +} + +TEST_F(TestDiffDriveController, configure_fails_with_mismatching_wheel_side_size) +{ + const auto ret = controller_->init(controller_name); + ASSERT_EQ(ret, controller_interface::return_type::OK); + + controller_->get_node()->set_parameter( + rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); + + auto extended_right_wheel_names = right_wheel_names; + extended_right_wheel_names.push_back("extra_wheel"); + controller_->get_node()->set_parameter( + rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(extended_right_wheel_names))); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); +} + +TEST_F(TestDiffDriveController, configure_succeeds_when_wheels_are_specified) +{ + const auto ret = controller_->init(controller_name); + ASSERT_EQ(ret, controller_interface::return_type::OK); + + controller_->get_node()->set_parameter( + rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + + auto state_if_conf = controller_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, SizeIs(left_wheel_names.size() + right_wheel_names.size())); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + auto cmd_if_conf = controller_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, SizeIs(left_wheel_names.size() + right_wheel_names.size())); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); +} + +TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_false_no_namespace) +{ + const auto ret = controller_->init(controller_name); + ASSERT_EQ(ret, controller_interface::return_type::OK); + + std::string odom_id = "odom"; + std::string base_link_id = "base_link"; + std::string frame_prefix = "test_prefix"; + + controller_->get_node()->set_parameter( + rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); + + controller_->get_node()->set_parameter( + rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(false))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + + auto odometry_message = controller_->get_rt_odom_publisher()->msg_; + std::string test_odom_frame_id = odometry_message.header.frame_id; + std::string test_base_frame_id = odometry_message.child_frame_id; + /* tf_frame_prefix_enable is false so no modifications to the frame id's */ + ASSERT_EQ(test_odom_frame_id, odom_id); + ASSERT_EQ(test_base_frame_id, base_link_id); +} + +TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_true_no_namespace) +{ + const auto ret = controller_->init(controller_name); + ASSERT_EQ(ret, controller_interface::return_type::OK); + + std::string odom_id = "odom"; + std::string base_link_id = "base_link"; + std::string frame_prefix = "test_prefix"; + + controller_->get_node()->set_parameter( + rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); + + controller_->get_node()->set_parameter( + rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + + auto odometry_message = controller_->get_rt_odom_publisher()->msg_; + std::string test_odom_frame_id = odometry_message.header.frame_id; + std::string test_base_frame_id = odometry_message.child_frame_id; + + /* tf_frame_prefix_enable is true and frame_prefix is not blank so should be appended to the frame + * id's */ + ASSERT_EQ(test_odom_frame_id, frame_prefix + "/" + odom_id); + ASSERT_EQ(test_base_frame_id, frame_prefix + "/" + base_link_id); +} + +TEST_F(TestDiffDriveController, configure_succeeds_tf_blank_prefix_true_no_namespace) +{ + const auto ret = controller_->init(controller_name); + ASSERT_EQ(ret, controller_interface::return_type::OK); + + std::string odom_id = "odom"; + std::string base_link_id = "base_link"; + std::string frame_prefix = ""; + + controller_->get_node()->set_parameter( + rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); + + controller_->get_node()->set_parameter( + rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + + auto odometry_message = controller_->get_rt_odom_publisher()->msg_; + std::string test_odom_frame_id = odometry_message.header.frame_id; + std::string test_base_frame_id = odometry_message.child_frame_id; + /* tf_frame_prefix_enable is true but frame_prefix is blank so should not be appended to the frame + * id's */ + ASSERT_EQ(test_odom_frame_id, odom_id); + ASSERT_EQ(test_base_frame_id, base_link_id); +} + +TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_false_set_namespace) +{ + std::string test_namespace = "/test_namespace"; + + const auto ret = controller_->init(controller_name, test_namespace); + ASSERT_EQ(ret, controller_interface::return_type::OK); + + std::string odom_id = "odom"; + std::string base_link_id = "base_link"; + std::string frame_prefix = "test_prefix"; + + controller_->get_node()->set_parameter( + rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); + + controller_->get_node()->set_parameter( + rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(false))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + + auto odometry_message = controller_->get_rt_odom_publisher()->msg_; + std::string test_odom_frame_id = odometry_message.header.frame_id; + std::string test_base_frame_id = odometry_message.child_frame_id; + /* tf_frame_prefix_enable is false so no modifications to the frame id's */ + ASSERT_EQ(test_odom_frame_id, odom_id); + ASSERT_EQ(test_base_frame_id, base_link_id); +} + +TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_true_set_namespace) +{ + std::string test_namespace = "/test_namespace"; + + const auto ret = controller_->init(controller_name, test_namespace); + ASSERT_EQ(ret, controller_interface::return_type::OK); + + std::string odom_id = "odom"; + std::string base_link_id = "base_link"; + std::string frame_prefix = "test_prefix"; + + controller_->get_node()->set_parameter( + rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); + + controller_->get_node()->set_parameter( + rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + + auto odometry_message = controller_->get_rt_odom_publisher()->msg_; + std::string test_odom_frame_id = odometry_message.header.frame_id; + std::string test_base_frame_id = odometry_message.child_frame_id; + + /* tf_frame_prefix_enable is true and frame_prefix is not blank so should be appended to the frame + * id's instead of the namespace*/ + ASSERT_EQ(test_odom_frame_id, frame_prefix + "/" + odom_id); + ASSERT_EQ(test_base_frame_id, frame_prefix + "/" + base_link_id); +} + +TEST_F(TestDiffDriveController, configure_succeeds_tf_blank_prefix_true_set_namespace) +{ + std::string test_namespace = "/test_namespace"; + + const auto ret = controller_->init(controller_name, test_namespace); + ASSERT_EQ(ret, controller_interface::return_type::OK); + + std::string odom_id = "odom"; + std::string base_link_id = "base_link"; + std::string frame_prefix = ""; + + controller_->get_node()->set_parameter( + rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); + + controller_->get_node()->set_parameter( + rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + + auto odometry_message = controller_->get_rt_odom_publisher()->msg_; + std::string test_odom_frame_id = odometry_message.header.frame_id; + std::string test_base_frame_id = odometry_message.child_frame_id; + /* tf_frame_prefix_enable is true but frame_prefix is blank so namespace should be appended to the + * frame id's */ + ASSERT_EQ(test_odom_frame_id, test_namespace + "/" + odom_id); + ASSERT_EQ(test_base_frame_id, test_namespace + "/" + base_link_id); +} + +TEST_F(TestDiffDriveController, activate_fails_without_resources_assigned) +{ + const auto ret = controller_->init(controller_name); + ASSERT_EQ(ret, controller_interface::return_type::OK); + + controller_->get_node()->set_parameter( + rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::ERROR); +} + +TEST_F(TestDiffDriveController, activate_succeeds_with_pos_resources_assigned) +{ + const auto ret = controller_->init(controller_name); + ASSERT_EQ(ret, controller_interface::return_type::OK); + + // We implicitly test that by default position feedback is required + controller_->get_node()->set_parameter( + rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + assignResourcesPosFeedback(); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); +} + +TEST_F(TestDiffDriveController, activate_succeeds_with_vel_resources_assigned) +{ + const auto ret = controller_->init(controller_name); + ASSERT_EQ(ret, controller_interface::return_type::OK); + + controller_->get_node()->set_parameter( + rclcpp::Parameter("position_feedback", rclcpp::ParameterValue(false))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + assignResourcesVelFeedback(); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); +} + +TEST_F(TestDiffDriveController, activate_fails_with_wrong_resources_assigned_1) +{ + const auto ret = controller_->init(controller_name); + ASSERT_EQ(ret, controller_interface::return_type::OK); + + controller_->get_node()->set_parameter( + rclcpp::Parameter("position_feedback", rclcpp::ParameterValue(false))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + assignResourcesPosFeedback(); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::ERROR); +} + +TEST_F(TestDiffDriveController, activate_fails_with_wrong_resources_assigned_2) +{ + const auto ret = controller_->init(controller_name); + ASSERT_EQ(ret, controller_interface::return_type::OK); + + controller_->get_node()->set_parameter( + rclcpp::Parameter("position_feedback", rclcpp::ParameterValue(true))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + assignResourcesVelFeedback(); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::ERROR); +} + +TEST_F(TestDiffDriveController, cleanup) +{ + const auto ret = controller_->init(controller_name); + ASSERT_EQ(ret, controller_interface::return_type::OK); + + controller_->get_node()->set_parameter( + rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); + controller_->get_node()->set_parameter(rclcpp::Parameter("wheel_separation", 0.4)); + controller_->get_node()->set_parameter(rclcpp::Parameter("wheel_radius", 0.1)); + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + auto state = controller_->get_node()->configure(); + ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); + assignResourcesPosFeedback(); + + state = controller_->get_node()->activate(); + ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); + + waitForSetup(); + + // send msg + const double linear = 1.0; + const double angular = 1.0; + publish(linear, angular); + controller_->wait_for_twist(executor); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + state = controller_->get_node()->deactivate(); + ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + state = controller_->get_node()->cleanup(); + ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); + + // should be stopped + EXPECT_EQ(0.0, left_wheel_vel_cmd_.get_value()); + EXPECT_EQ(0.0, right_wheel_vel_cmd_.get_value()); + + executor.cancel(); +} + +TEST_F(TestDiffDriveController, correct_initialization_using_parameters) +{ + const auto ret = controller_->init(controller_name); + ASSERT_EQ(ret, controller_interface::return_type::OK); + + controller_->get_node()->set_parameter( + rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); + controller_->get_node()->set_parameter(rclcpp::Parameter("wheel_separation", 0.4)); + controller_->get_node()->set_parameter(rclcpp::Parameter("wheel_radius", 1.0)); + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + auto state = controller_->get_node()->configure(); + assignResourcesPosFeedback(); + + ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); + EXPECT_EQ(0.01, left_wheel_vel_cmd_.get_value()); + EXPECT_EQ(0.02, right_wheel_vel_cmd_.get_value()); + + state = controller_->get_node()->activate(); + ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); + + // send msg + const double linear = 1.0; + const double angular = 0.0; + publish(linear, angular); + // wait for msg is be published to the system + ASSERT_TRUE(controller_->wait_for_twist(executor)); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + EXPECT_EQ(1.0, left_wheel_vel_cmd_.get_value()); + EXPECT_EQ(1.0, right_wheel_vel_cmd_.get_value()); + + // deactivated + // wait so controller process the second point when deactivated + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + state = controller_->get_node()->deactivate(); + ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_EQ(0.0, left_wheel_vel_cmd_.get_value()) << "Wheels are halted on deactivate()"; + EXPECT_EQ(0.0, right_wheel_vel_cmd_.get_value()) << "Wheels are halted on deactivate()"; + + // cleanup + state = controller_->get_node()->cleanup(); + ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(0.0, left_wheel_vel_cmd_.get_value()); + EXPECT_EQ(0.0, right_wheel_vel_cmd_.get_value()); + + state = controller_->get_node()->configure(); + ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); + executor.cancel(); +} diff --git a/picknik_diff_drive_controller/test/test_load_diff_drive_controller.cpp b/picknik_diff_drive_controller/test/test_load_diff_drive_controller.cpp new file mode 100644 index 0000000..ab957f2 --- /dev/null +++ b/picknik_diff_drive_controller/test/test_load_diff_drive_controller.cpp @@ -0,0 +1,42 @@ +// Copyright 2020 PAL Robotics SL. +// +// 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(TestLoadDiffDriveController, load_controller) +{ + rclcpp::init(0, nullptr); + + std::shared_ptr executor = + std::make_shared(); + + controller_manager::ControllerManager cm( + std::make_unique(ros2_control_test_assets::diffbot_urdf), + executor, "test_controller_manager"); + + ASSERT_NE( + cm.load_controller( + "test_diff_drive_controller", "picknik_diff_drive_controller/DiffDriveController"), + nullptr); + + rclcpp::shutdown(); +}