diff --git a/diff_drive_controller/CMakeLists.txt b/diff_drive_controller/CMakeLists.txt new file mode 100644 index 0000000000..54f5f006c5 --- /dev/null +++ b/diff_drive_controller/CMakeLists.txt @@ -0,0 +1,127 @@ +cmake_minimum_required(VERSION 3.5) +project(diff_drive_controller) + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra) +endif() + +find_package(ament_cmake REQUIRED) +find_package(controller_interface REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(hardware_interface REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(realtime_tools REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_msgs REQUIRED) + +set(Boost_USE_STATIC_LIBS OFF) +set(Boost_USE_MULTITHREADED ON) +set(Boost_USE_STATIC_RUNTIME OFF) +find_package(Boost REQUIRED) + +add_library(diff_drive_controller SHARED + src/diff_drive_controller.cpp + src/odometry.cpp + src/speed_limiter.cpp +) + +target_include_directories(diff_drive_controller PRIVATE include) +ament_target_dependencies(diff_drive_controller + builtin_interfaces + controller_interface + geometry_msgs + hardware_interface + nav_msgs + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + sensor_msgs + tf2 + tf2_msgs + Boost +) +# 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) + +install(DIRECTORY include/ + DESTINATION include +) + +install(TARGETS diff_drive_controller + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib +) + +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + find_package(ament_lint_auto REQUIRED) + find_package(controller_manager REQUIRED) + find_package(test_robot_hardware REQUIRED) + + ament_lint_auto_find_test_dependencies() + + ament_add_gtest(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_include_directories(test_diff_drive_controller PRIVATE include) + 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 + sensor_msgs + test_robot_hardware + tf2 + tf2_msgs + ) + + ament_add_gtest( + test_load_diff_drive_controller + test/test_load_diff_drive_controller.cpp + ) + + target_include_directories(test_load_diff_drive_controller PRIVATE include) + ament_target_dependencies(test_load_diff_drive_controller + controller_manager + test_robot_hardware + ) + +endif() + +ament_export_dependencies( + controller_interface + geometry_msgs + hardware_interface + rclcpp + rclcpp_lifecycle + sensor_msgs + tf2 + tf2_msgs +) +ament_export_include_directories( + include +) +ament_export_libraries( + diff_drive_controller +) +ament_package() diff --git a/diff_drive_controller/diff_drive_plugin.xml b/diff_drive_controller/diff_drive_plugin.xml new file mode 100644 index 0000000000..08d41cf69c --- /dev/null +++ b/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/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp b/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp new file mode 100644 index 0000000000..f00a5143c3 --- /dev/null +++ b/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp @@ -0,0 +1,178 @@ +// 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/controller_interface.hpp" +#include "diff_drive_controller/odometry.hpp" +#include "diff_drive_controller/speed_limiter.hpp" +#include "diff_drive_controller/visibility_control.h" +#include "geometry_msgs/msg/twist.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "hardware_interface/joint_command_handle.hpp" +#include "hardware_interface/operation_mode_handle.hpp" +#include "hardware_interface/robot_hardware.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "odometry.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include "realtime_tools/realtime_buffer.h" +#include "realtime_tools/realtime_publisher.h" +#include "sensor_msgs/msg/joint_state.hpp" +#include "tf2_msgs/msg/tf_message.hpp" + +namespace diff_drive_controller +{ +class DiffDriveController : public controller_interface::ControllerInterface +{ + using Twist = geometry_msgs::msg::TwistStamped; + +public: + DIFF_DRIVE_CONTROLLER_PUBLIC + DiffDriveController(); + + DIFF_DRIVE_CONTROLLER_PUBLIC + DiffDriveController( + std::vector left_wheel_names, + std::vector right_wheel_names, + std::vector operation_mode_names); + + DIFF_DRIVE_CONTROLLER_PUBLIC + controller_interface::controller_interface_ret_t + init( + std::weak_ptr robot_hardware, + const std::string & controller_name) override; + + DIFF_DRIVE_CONTROLLER_PUBLIC + controller_interface::controller_interface_ret_t update() override; + + DIFF_DRIVE_CONTROLLER_PUBLIC + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + on_configure(const rclcpp_lifecycle::State & previous_state) override; + + DIFF_DRIVE_CONTROLLER_PUBLIC + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + on_activate(const rclcpp_lifecycle::State & previous_state) override; + + DIFF_DRIVE_CONTROLLER_PUBLIC + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + on_deactivate(const rclcpp_lifecycle::State & previous_state) override; + + DIFF_DRIVE_CONTROLLER_PUBLIC + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + on_cleanup(const rclcpp_lifecycle::State & previous_state) override; + + DIFF_DRIVE_CONTROLLER_PUBLIC + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + on_error(const rclcpp_lifecycle::State & previous_state) override; + + DIFF_DRIVE_CONTROLLER_PUBLIC + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + on_shutdown(const rclcpp_lifecycle::State & previous_state) override; + +private: + struct WheelHandle + { + const hardware_interface::JointStateHandle * state = nullptr; + hardware_interface::JointCommandHandle * command = nullptr; + }; + + CallbackReturn configure_side( + const std::string & side, + const std::vector & wheel_names, + std::vector & registered_handles, + hardware_interface::RobotHardware & robot_hardware); + + std::vector left_wheel_names_; + std::vector right_wheel_names_; + + std::vector registered_left_wheel_handles_; + std::vector registered_right_wheel_handles_; + + struct WheelParams + { + size_t wheels_per_side = 0; + double separation = 0.0; // w.r.t. the midpoint of the wheel width + double radius = 0.0; // Assumed to be the same for both wheels + double separation_multiplier = 1.0; + double left_radius_multiplier = 1.0; + double right_radius_multiplier = 1.0; + } wheel_params_; + + struct OdometryParams + { + bool open_loop = false; + bool enable_odom_tf = true; + std::string base_frame_id = "base_link"; + std::string odom_frame_id = "odom"; + std::array pose_covariance_diagonal; + std::array twist_covariance_diagonal; + } odom_params_; + + Odometry odometry_; + + std::shared_ptr> odometry_publisher_ + = nullptr; + std::shared_ptr> + realtime_odometry_publisher_ = nullptr; + + std::shared_ptr> + odometry_transform_publisher_ = nullptr; + std::shared_ptr> + realtime_odometry_transform_publisher_ = nullptr; + + // Timeout to consider cmd_vel commands old + std::chrono::milliseconds cmd_vel_timeout_{500}; + + std::vector write_op_names_; + std::vector registered_operation_mode_handles_; + + bool subscriber_is_active_ = false; + rclcpp::Subscription::SharedPtr velocity_command_subscriber_ = nullptr; + + std::shared_ptr 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}; + + bool is_halted = false; + + bool reset(); + void set_op_mode(const hardware_interface::OperationMode & mode); + void halt(); +}; +} // namespace diff_drive_controller +#endif // DIFF_DRIVE_CONTROLLER__DIFF_DRIVE_CONTROLLER_HPP_ diff --git a/diff_drive_controller/include/diff_drive_controller/odometry.hpp b/diff_drive_controller/include/diff_drive_controller/odometry.hpp new file mode 100644 index 0000000000..abea3809c0 --- /dev/null +++ b/diff_drive_controller/include/diff_drive_controller/odometry.hpp @@ -0,0 +1,108 @@ +// 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 +#include +#include + +#include "rclcpp/time.hpp" + +namespace diff_drive_controller +{ +namespace bacc = boost::accumulators; + +class Odometry +{ +public: + 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); + 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 = + bacc::accumulator_set>; + using RollingWindow = bacc::tag::rolling_window; + + 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 diff_drive_controller + +#endif // DIFF_DRIVE_CONTROLLER__ODOMETRY_HPP_ diff --git a/diff_drive_controller/include/diff_drive_controller/speed_limiter.hpp b/diff_drive_controller/include/diff_drive_controller/speed_limiter.hpp new file mode 100644 index 0000000000..b7130e429f --- /dev/null +++ b/diff_drive_controller/include/diff_drive_controller/speed_limiter.hpp @@ -0,0 +1,103 @@ +// 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__OSPEED_LIMITER_HPP_ +#define DIFF_DRIVE_CONTROLLER__OSPEED_LIMITER_HPP_ + +namespace 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 = 0.0, double max_velocity = 0.0, double min_acceleration = 0.0, + double max_acceleration = 0.0, double min_jerk = 0.0, double max_jerk = 0.0); + + /** + * \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 diff_drive_controller + +#endif // DIFF_DRIVE_CONTROLLER__OSPEED_LIMITER_HPP_ diff --git a/diff_drive_controller/include/diff_drive_controller/visibility_control.h b/diff_drive_controller/include/diff_drive_controller/visibility_control.h new file mode 100644 index 0000000000..a6b3e5228b --- /dev/null +++ b/diff_drive_controller/include/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/diff_drive_controller/package.xml b/diff_drive_controller/package.xml new file mode 100644 index 0000000000..5216b45035 --- /dev/null +++ b/diff_drive_controller/package.xml @@ -0,0 +1,36 @@ + + + diff_drive_controller + 0.0.0 + Controller for a differential drive mobile base. + Bence Magyar + Jordan Palacios + Karsten Knese + + BSD + + ament_cmake + + controller_interface + geometry_msgs + hardware_interface + nav_msgs + rclcpp + rclcpp_lifecycle + realtime_tools + tf2 + tf2_msgs + + libboost-dev + pluginlib + + ament_cmake_gtest + ament_lint_auto + ament_lint_common + controller_manager + test_robot_hardware + + + ament_cmake + + diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp new file mode 100644 index 0000000000..4ed61da4f3 --- /dev/null +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -0,0 +1,585 @@ +// 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 "diff_drive_controller/diff_drive_controller.hpp" + +#include +#include +#include + +namespace +{ +constexpr auto DEFAULT_COMMAND_TOPIC = "~/cmd_vel"; +constexpr auto DEFAULT_COMMAND_OUT_TOPIC = "~/cmd_vel_out"; +constexpr auto DEFAULT_ODOMETRY_TOPIC = "/odom"; +constexpr auto DEFAULT_TRANSFORM_TOPIC = "/tf"; +} // namespace + +namespace diff_drive_controller +{ +using namespace std::chrono_literals; +using CallbackReturn = DiffDriveController::CallbackReturn; +using controller_interface::CONTROLLER_INTERFACE_RET_ERROR; +using controller_interface::CONTROLLER_INTERFACE_RET_SUCCESS; +using lifecycle_msgs::msg::State; + +DiffDriveController::DiffDriveController() +: controller_interface::ControllerInterface() {} + +DiffDriveController::DiffDriveController( + std::vector left_wheel_names, + std::vector right_wheel_names, + std::vector write_op_names) +: controller_interface::ControllerInterface(), + left_wheel_names_(std::move(left_wheel_names)), + right_wheel_names_(std::move(right_wheel_names)), + write_op_names_(std::move(write_op_names)) +{} + +controller_interface::controller_interface_ret_t +DiffDriveController::init( + std::weak_ptr robot_hardware, + const std::string & controller_name) +{ + // initialize lifecycle node + auto ret = ControllerInterface::init(robot_hardware, controller_name); + if (ret != CONTROLLER_INTERFACE_RET_SUCCESS) { + return ret; + } + + // with the lifecycle node being initialized, we can declare parameters + lifecycle_node_->declare_parameter>( + "left_wheel_names", + left_wheel_names_); + lifecycle_node_->declare_parameter>( + "right_wheel_names", + right_wheel_names_); + lifecycle_node_->declare_parameter>("write_op_modes", write_op_names_); + + lifecycle_node_->declare_parameter("wheel_separation", wheel_params_.separation); + lifecycle_node_->declare_parameter("wheels_per_side", wheel_params_.wheels_per_side); + lifecycle_node_->declare_parameter("wheel_radius", wheel_params_.radius); + lifecycle_node_->declare_parameter( + "wheel_separation_multiplier", + wheel_params_.separation_multiplier); + lifecycle_node_->declare_parameter( + "left_wheel_radius_multiplier", + wheel_params_.left_radius_multiplier); + lifecycle_node_->declare_parameter( + "right_wheel_radius_multiplier", + wheel_params_.right_radius_multiplier); + + lifecycle_node_->declare_parameter("odom_frame_id", odom_params_.odom_frame_id); + lifecycle_node_->declare_parameter("base_frame_id", odom_params_.base_frame_id); + lifecycle_node_->declare_parameter>("pose_covariance_diagonal", {}); // std::array + lifecycle_node_->declare_parameter>("twist_covariance_diagonal", {}); // std::array + lifecycle_node_->declare_parameter("open_loop", odom_params_.open_loop); + lifecycle_node_->declare_parameter("enable_odom_tf", odom_params_.enable_odom_tf); + + lifecycle_node_->declare_parameter("cmd_vel_timeout", cmd_vel_timeout_.count()); + lifecycle_node_->declare_parameter("publish_limited_velocity", publish_limited_velocity_); + lifecycle_node_->declare_parameter("velocity_rolling_window_size", 10); + + lifecycle_node_->declare_parameter("linear.x.has_velocity_limits", false); + lifecycle_node_->declare_parameter("linear.x.has_acceleration_limits", false); + lifecycle_node_->declare_parameter("linear.x.has_jerk_limits", false); + lifecycle_node_->declare_parameter("linear.x.max_velocity", 0.0); + lifecycle_node_->declare_parameter("linear.x.min_velocity", 0.0); + lifecycle_node_->declare_parameter("linear.x.max_acceleration", 0.0); + lifecycle_node_->declare_parameter("linear.x.min_acceleration", 0.0); + lifecycle_node_->declare_parameter("linear.x.max_jerk", 0.0); + lifecycle_node_->declare_parameter("linear.x.min_jerk", 0.0); + + lifecycle_node_->declare_parameter("angular.z.has_velocity_limits", false); + lifecycle_node_->declare_parameter("angular.z.has_acceleration_limits", false); + lifecycle_node_->declare_parameter("angular.z.has_jerk_limits", false); + lifecycle_node_->declare_parameter("angular.z.max_velocity", 0.0); + lifecycle_node_->declare_parameter("angular.z.min_velocity", 0.0); + lifecycle_node_->declare_parameter("angular.z.max_acceleration", 0.0); + lifecycle_node_->declare_parameter("angular.z.min_acceleration", 0.0); + lifecycle_node_->declare_parameter("angular.z.max_jerk", 0.0); + lifecycle_node_->declare_parameter("angular.z.min_jerk", 0.0); + + return CONTROLLER_INTERFACE_RET_SUCCESS; +} + +controller_interface::controller_interface_ret_t DiffDriveController::update() +{ + auto logger = lifecycle_node_->get_logger(); + if (lifecycle_node_->get_current_state().id() == State::PRIMARY_STATE_INACTIVE) { + if (!is_halted) { + halt(); + is_halted = true; + } + return CONTROLLER_INTERFACE_RET_SUCCESS; + } + + const auto current_time = lifecycle_node_->get_clock()->now(); + double & linear_command = received_velocity_msg_ptr_->twist.linear.x; + double & angular_command = received_velocity_msg_ptr_->twist.angular.z; + + // Apply (possibly new) multipliers: + const auto wheels = wheel_params_; + const double wheel_separation = wheels.separation_multiplier * wheels.separation; + const double left_wheel_radius = wheels.left_radius_multiplier * wheels.radius; + const double right_wheel_radius = wheels.right_radius_multiplier * wheels.radius; + + if (odom_params_.open_loop) { + odometry_.updateOpenLoop(linear_command, angular_command, current_time); + } else { + double left_position_mean = 0.0; + double right_position_mean = 0.0; + for (size_t index = 0; index < wheels.wheels_per_side; ++index) { + const double left_position = registered_left_wheel_handles_[index].state->get_position(); + const double right_position = registered_right_wheel_handles_[index].state->get_position(); + + if (std::isnan(left_position) or std::isnan(right_position)) { + RCLCPP_ERROR( + logger, "Either the left or right wheel position is invalid for index [%d]", + index); + return controller_interface::CONTROLLER_INTERFACE_RET_ERROR; + } + + left_position_mean += left_position; + right_position_mean += right_position; + } + left_position_mean /= wheels.wheels_per_side; + right_position_mean /= wheels.wheels_per_side; + + odometry_.update(left_position_mean, right_position_mean, current_time); + } + + tf2::Quaternion orientation; + orientation.setRPY(0.0, 0.0, odometry_.getHeading()); + + if (odometry_publisher_->is_activated() and realtime_odometry_publisher_->trylock()) { + auto & odometry_message = realtime_odometry_publisher_->msg_; + odometry_message.header.stamp = current_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 (odom_params_.enable_odom_tf and odometry_transform_publisher_->is_activated() and + realtime_odometry_transform_publisher_->trylock()) + { + auto & transform = realtime_odometry_transform_publisher_->msg_.transforms.front(); + transform.header.stamp = current_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(); + } + + const auto dt = current_time - received_velocity_msg_ptr_->header.stamp; + + // Brake if cmd_vel has timeout + if (dt > cmd_vel_timeout_) { + linear_command = 0.0; + angular_command = 0.0; + } + + const auto update_dt = current_time - previous_update_timestamp_; + previous_update_timestamp_ = current_time; + + 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, + update_dt.seconds()); + limiter_angular_.limit( + angular_command, last_command.angular.z, second_to_last_command.angular.z, update_dt.seconds()); + + previous_commands_.pop(); + previous_commands_.emplace(*received_velocity_msg_ptr_); + + // Publish limited velocity + if (publish_limited_velocity_ and limited_velocity_publisher_->is_activated() and + realtime_limited_velocity_publisher_->trylock()) + { + auto & limited_velocity_command = realtime_limited_velocity_publisher_->msg_; + limited_velocity_command.header.stamp = current_time; + limited_velocity_command.twist.linear.x = linear_command; + limited_velocity_command.twist.angular.z = angular_command; + realtime_limited_velocity_publisher_->unlockAndPublish(); + } + + if (received_velocity_msg_ptr_ == nullptr) { + RCLCPP_WARN(logger, "Velocity message received was a nullptr."); + return CONTROLLER_INTERFACE_RET_ERROR; + } + + // Compute wheels velocities: + const auto & current_command = *received_velocity_msg_ptr_; + const auto & linear = current_command.twist.linear.x; + const auto & angular = current_command.twist.angular.z; + + const double velocity_left = (linear - angular * wheel_separation / 2.0) / left_wheel_radius; + const double velocity_right = (linear + angular * wheel_separation / 2.0) / right_wheel_radius; + + // Set wheels velocities: + for (size_t index = 0; index < wheels.wheels_per_side; ++index) { + registered_left_wheel_handles_[index].command->set_cmd(velocity_left); + registered_right_wheel_handles_[index].command->set_cmd(velocity_right); + } + + set_op_mode(hardware_interface::OperationMode::ACTIVE); + return CONTROLLER_INTERFACE_RET_SUCCESS; +} + +CallbackReturn DiffDriveController::on_configure(const rclcpp_lifecycle::State &) +{ + auto logger = lifecycle_node_->get_logger(); + + // update parameters + left_wheel_names_ = lifecycle_node_->get_parameter("left_wheel_names").as_string_array(); + right_wheel_names_ = lifecycle_node_->get_parameter("right_wheel_names").as_string_array(); + write_op_names_ = lifecycle_node_->get_parameter("write_op_modes").as_string_array(); + + wheel_params_.separation = lifecycle_node_->get_parameter("wheel_separation").as_double(); + wheel_params_.wheels_per_side = + static_cast(lifecycle_node_->get_parameter("wheels_per_side").as_int()); + wheel_params_.radius = lifecycle_node_->get_parameter("wheel_radius").as_double(); + wheel_params_.separation_multiplier = + lifecycle_node_->get_parameter("wheel_separation_multiplier").as_double(); + wheel_params_.left_radius_multiplier = lifecycle_node_->get_parameter( + "left_wheel_radius_multiplier").as_double(); + wheel_params_.right_radius_multiplier = lifecycle_node_->get_parameter( + "right_wheel_radius_multiplier").as_double(); + + const auto wheels = wheel_params_; + + const double wheel_separation = wheels.separation_multiplier * wheels.separation; + const double left_wheel_radius = wheels.left_radius_multiplier * wheels.radius; + const double right_wheel_radius = wheels.right_radius_multiplier * wheels.radius; + + odometry_.setWheelParams(wheel_separation, left_wheel_radius, right_wheel_radius); + odometry_.setVelocityRollingWindowSize( + lifecycle_node_->get_parameter( + "velocity_rolling_window_size").as_int()); + + odom_params_.odom_frame_id = lifecycle_node_->get_parameter("odom_frame_id").as_string(); + odom_params_.base_frame_id = lifecycle_node_->get_parameter("base_frame_id").as_string(); + + auto pose_diagonal = lifecycle_node_->get_parameter("pose_covariance_diagonal").as_double_array(); + std::copy( + pose_diagonal.begin(), pose_diagonal.end(), + odom_params_.pose_covariance_diagonal.begin()); + + auto twist_diagonal = + lifecycle_node_->get_parameter("twist_covariance_diagonal").as_double_array(); + std::copy( + twist_diagonal.begin(), + twist_diagonal.end(), odom_params_.twist_covariance_diagonal.begin()); + + odom_params_.open_loop = lifecycle_node_->get_parameter("open_loop").as_bool(); + odom_params_.enable_odom_tf = lifecycle_node_->get_parameter("enable_odom_tf").as_bool(); + + cmd_vel_timeout_ = + std::chrono::milliseconds{lifecycle_node_->get_parameter("cmd_vel_timeout").as_int()}; + publish_limited_velocity_ = lifecycle_node_->get_parameter("publish_limited_velocity").as_bool(); + + limiter_linear_ = SpeedLimiter( + lifecycle_node_->get_parameter("linear.x.has_velocity_limits").as_bool(), + lifecycle_node_->get_parameter("linear.x.has_acceleration_limits").as_bool(), + lifecycle_node_->get_parameter("linear.x.has_jerk_limits").as_bool(), + lifecycle_node_->get_parameter("linear.x.min_velocity").as_double(), + lifecycle_node_->get_parameter("linear.x.max_velocity").as_double(), + lifecycle_node_->get_parameter("linear.x.min_acceleration").as_double(), + lifecycle_node_->get_parameter("linear.x.max_acceleration").as_double(), + lifecycle_node_->get_parameter("linear.x.min_jerk").as_double(), + lifecycle_node_->get_parameter("linear.x.max_jerk").as_double()); + + limiter_angular_ = SpeedLimiter( + lifecycle_node_->get_parameter("angular.z.has_velocity_limits").as_bool(), + lifecycle_node_->get_parameter("angular.z.has_acceleration_limits").as_bool(), + lifecycle_node_->get_parameter("angular.z.has_jerk_limits").as_bool(), + lifecycle_node_->get_parameter("angular.z.min_velocity").as_double(), + lifecycle_node_->get_parameter("angular.z.max_velocity").as_double(), + lifecycle_node_->get_parameter("angular.z.min_acceleration").as_double(), + lifecycle_node_->get_parameter("angular.z.max_acceleration").as_double(), + lifecycle_node_->get_parameter("angular.z.min_jerk").as_double(), + lifecycle_node_->get_parameter("angular.z.max_jerk").as_double()); + + if (left_wheel_names_.size() != right_wheel_names_.size()) { + RCLCPP_ERROR( + logger, + "The number of left wheels [%d] and the number of right wheels [%d] are different", + left_wheel_names_.size(), + right_wheel_names_.size()); + return CallbackReturn::ERROR; + } + + if (!reset()) { + return CallbackReturn::ERROR; + } + + if (auto robot_hardware = robot_hardware_.lock()) { + const auto left_result = + configure_side("left", left_wheel_names_, registered_left_wheel_handles_, *robot_hardware); + const auto right_result = + configure_side("right", right_wheel_names_, registered_right_wheel_handles_, *robot_hardware); + + if (left_result == CallbackReturn::FAILURE or right_result == CallbackReturn::FAILURE) { + return CallbackReturn::FAILURE; + } + + registered_operation_mode_handles_.resize(write_op_names_.size()); + for (size_t index = 0; index < write_op_names_.size(); ++index) { + const auto op_name = write_op_names_[index].c_str(); + auto & op_handle = registered_operation_mode_handles_[index]; + + auto result = robot_hardware->get_operation_mode_handle(op_name, &op_handle); + if (result != hardware_interface::HW_RET_OK) { + RCLCPP_WARN(logger, "unable to obtain operation mode handle for %s", op_name); + return CallbackReturn::FAILURE; + } + } + + } else { + return CallbackReturn::ERROR; + } + + if (registered_left_wheel_handles_.empty() or registered_right_wheel_handles_.empty() or + registered_operation_mode_handles_.empty()) + { + RCLCPP_ERROR( + logger, + "Either left wheel handles, right wheel handles, or operation modes are non existant"); + return CallbackReturn::ERROR; + } + + // left and right sides are both equal at this point + wheel_params_.wheels_per_side = registered_left_wheel_handles_.size(); + + if (publish_limited_velocity_) { + limited_velocity_publisher_ = + lifecycle_node_->create_publisher( + DEFAULT_COMMAND_OUT_TOPIC, + rclcpp::SystemDefaultsQoS()); + realtime_limited_velocity_publisher_ = + std::make_shared>(limited_velocity_publisher_); + } + + received_velocity_msg_ptr_ = std::make_shared(); + + // Fill last two commands with default constructed commands + previous_commands_.emplace(*received_velocity_msg_ptr_); + previous_commands_.emplace(*received_velocity_msg_ptr_); + + // initialize command subscriber + velocity_command_subscriber_ = lifecycle_node_->create_subscription( + DEFAULT_COMMAND_TOPIC, rclcpp::SystemDefaultsQoS(), [this]( + const std::shared_ptr msg) -> void { + if (!subscriber_is_active_) { + RCLCPP_WARN( + lifecycle_node_->get_logger(), + "Can't accept new commands. subscriber is inactive"); + return; + } + + received_velocity_msg_ptr_ = std::move(msg); + }); + + // initialize odometry publisher and messasge + odometry_publisher_ = + lifecycle_node_->create_publisher( + DEFAULT_ODOMETRY_TOPIC, + rclcpp::SystemDefaultsQoS()); + realtime_odometry_publisher_ = + std::make_shared>(odometry_publisher_); + + auto & odometry_message = realtime_odometry_publisher_->msg_; + odometry_message.header.frame_id = odom_params_.odom_frame_id; + odometry_message.child_frame_id = odom_params_.base_frame_id; + + // initialize odom values zeros + odometry_message.twist = geometry_msgs::msg::TwistWithCovariance( + rosidl_runtime_cpp::MessageInitialization::ALL); + + constexpr size_t NUM_DIMENSIONS = 6; + for (size_t index = 0; index < 6; ++index) { + // 0, 7, 14, 21, 28, 35 + const size_t diagonal_index = NUM_DIMENSIONS * index + index; + odometry_message.pose.covariance[diagonal_index] = odom_params_.pose_covariance_diagonal[index]; + odometry_message.twist.covariance[diagonal_index] = + odom_params_.twist_covariance_diagonal[index]; + } + + // initialize transform publisher and message + odometry_transform_publisher_ = + lifecycle_node_->create_publisher( + DEFAULT_TRANSFORM_TOPIC, + rclcpp::SystemDefaultsQoS()); + realtime_odometry_transform_publisher_ = + std::make_shared>( + odometry_transform_publisher_); + + auto & odometry_transform_message = realtime_odometry_transform_publisher_->msg_; + odometry_transform_message.transforms.resize(1); // keeping track of odom and base_link transforms only + odometry_transform_message.transforms.front().header.frame_id = odom_params_.odom_frame_id; + odometry_transform_message.transforms.front().child_frame_id = odom_params_.base_frame_id; + + previous_update_timestamp_ = lifecycle_node_->get_clock()->now(); + set_op_mode(hardware_interface::OperationMode::INACTIVE); + return CallbackReturn::SUCCESS; +} + +CallbackReturn DiffDriveController::on_activate(const rclcpp_lifecycle::State &) +{ + is_halted = false; + subscriber_is_active_ = true; + + odometry_transform_publisher_->on_activate(); + odometry_publisher_->on_activate(); + if (publish_limited_velocity_) { + limited_velocity_publisher_->on_activate(); + } + + RCLCPP_INFO( + lifecycle_node_->get_logger(), "Lifecycle subscriber and publisher are currently active."); + return CallbackReturn::SUCCESS; +} + +CallbackReturn DiffDriveController::on_deactivate(const rclcpp_lifecycle::State &) +{ + subscriber_is_active_ = false; + odometry_transform_publisher_->on_deactivate(); + odometry_publisher_->on_deactivate(); + if (publish_limited_velocity_) { + limited_velocity_publisher_->on_deactivate(); + } + return CallbackReturn::SUCCESS; +} + +CallbackReturn DiffDriveController::on_cleanup(const rclcpp_lifecycle::State &) +{ + if (!reset()) { + return CallbackReturn::ERROR; + } + + received_velocity_msg_ptr_ = std::make_shared(); + return CallbackReturn::SUCCESS; +} + +CallbackReturn DiffDriveController::on_error(const rclcpp_lifecycle::State &) +{ + if (!reset()) { + return CallbackReturn::ERROR; + } + return CallbackReturn::SUCCESS; +} + +bool DiffDriveController::reset() +{ + odometry_.resetOdometry(); + + while (!previous_commands_.empty()) { + previous_commands_.pop(); + } + + registered_left_wheel_handles_.clear(); + registered_right_wheel_handles_.clear(); + registered_operation_mode_handles_.clear(); + + subscriber_is_active_ = false; + velocity_command_subscriber_.reset(); + + received_velocity_msg_ptr_.reset(); + is_halted = false; + return true; +} + +CallbackReturn DiffDriveController::on_shutdown(const rclcpp_lifecycle::State &) +{ + return CallbackReturn::SUCCESS; +} + +void DiffDriveController::set_op_mode(const hardware_interface::OperationMode & mode) +{ + for (auto & op_mode_handle : registered_operation_mode_handles_) { + op_mode_handle->set_mode(mode); + } +} + +void DiffDriveController::halt() +{ + const auto halt_wheels = [](auto & wheel_handles) { + for (size_t index = 0; index < wheel_handles.size(); ++index) { + const auto wheel_handle = wheel_handles[index]; + wheel_handle.command->set_cmd(0); + } + }; + + halt_wheels(registered_left_wheel_handles_); + halt_wheels(registered_right_wheel_handles_); + set_op_mode(hardware_interface::OperationMode::ACTIVE); +} + +CallbackReturn DiffDriveController::configure_side( + const std::string & side, + const std::vector & wheel_names, + std::vector & registered_handles, + hardware_interface::RobotHardware & robot_hardware) +{ + auto logger = lifecycle_node_->get_logger(); + + if (wheel_names.empty()) { + std::stringstream ss; + ss << "No " << side << " wheel names specified."; + RCLCPP_ERROR(logger, ss.str().c_str()); + return CallbackReturn::ERROR; + } + + // register handles + registered_handles.resize(wheel_names.size()); + for (size_t index = 0; index < wheel_names.size(); ++index) { + const auto wheel_name = wheel_names[index].c_str(); + auto & wheel_handle = registered_handles[index]; + + auto result = robot_hardware.get_joint_state_handle(wheel_name, &wheel_handle.state); + if (result != hardware_interface::HW_RET_OK) { + RCLCPP_WARN(logger, "unable to obtain joint state handle for %s", wheel_name); + return CallbackReturn::FAILURE; + } + + auto ret = robot_hardware.get_joint_command_handle(wheel_name, &wheel_handle.command); + if (ret != hardware_interface::HW_RET_OK) { + RCLCPP_WARN(logger, "unable to obtain joint command handle for %s", wheel_name); + return CallbackReturn::FAILURE; + } + } + + return CallbackReturn::SUCCESS; +} +} // namespace diff_drive_controller + +#include "class_loader/register_macro.hpp" + +CLASS_LOADER_REGISTER_CLASS( + diff_drive_controller::DiffDriveController, + controller_interface::ControllerInterface) diff --git a/diff_drive_controller/src/odometry.cpp b/diff_drive_controller/src/odometry.cpp new file mode 100644 index 0000000000..bb177bec15 --- /dev/null +++ b/diff_drive_controller/src/odometry.cpp @@ -0,0 +1,155 @@ +// 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 "diff_drive_controller/odometry.hpp" + +namespace 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_(RollingWindow::window_size = velocity_rolling_window_size), + angular_accumulator_(RollingWindow::window_size = 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; + + // Compute linear and angular diff: + const double linear = (right_wheel_est_vel + left_wheel_est_vel) * 0.5; + // Now there is a bug about scout angular velocity + const double angular = (right_wheel_est_vel - left_wheel_est_vel) / wheel_separation_; + + // Integrate odometry: + integrateExact(linear, angular); + + timestamp_ = time; + + // Estimate speeds using a rolling mean to filter them out: + linear_accumulator_(linear / dt); + angular_accumulator_(angular / dt); + + linear_ = bacc::rolling_mean(linear_accumulator_); + angular_ = bacc::rolling_mean(angular_accumulator_); + + 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( + RollingWindow::window_size = velocity_rolling_window_size_); + angular_accumulator_ = RollingMeanAccumulator( + RollingWindow::window_size = velocity_rolling_window_size_); +} + +} // namespace diff_drive_controller diff --git a/diff_drive_controller/src/speed_limiter.cpp b/diff_drive_controller/src/speed_limiter.cpp new file mode 100644 index 0000000000..a73067feaa --- /dev/null +++ b/diff_drive_controller/src/speed_limiter.cpp @@ -0,0 +1,106 @@ +// 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 "diff_drive_controller/speed_limiter.hpp" + +template +T clamp(T x, T min, T max) +{ + return std::min(std::max(min, x), max); +} + +namespace 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) +{ +} + +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 = 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 = 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 = clamp(dv - dv0, da_min, da_max); + + v = v0 + dv0 + da; + } + + return tmp != 0.0 ? v / tmp : 1.0; +} + +} // namespace diff_drive_controller diff --git a/diff_drive_controller/test/config/test_diff_drive_controller.yaml b/diff_drive_controller/test/config/test_diff_drive_controller.yaml new file mode 100644 index 0000000000..30e57361bb --- /dev/null +++ b/diff_drive_controller/test/config/test_diff_drive_controller.yaml @@ -0,0 +1,44 @@ +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] + + 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/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp new file mode 100644 index 0000000000..e55a715373 --- /dev/null +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -0,0 +1,285 @@ +// 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 "diff_drive_controller/diff_drive_controller.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp/rclcpp.hpp" +#include "test_robot_hardware/test_robot_hardware.hpp" + +#include "gtest/gtest.h" +#include +#include +#include +#include +#include + +using lifecycle_msgs::msg::State; + +void spin(rclcpp::executors::MultiThreadedExecutor * exe) +{ + exe->spin(); +} + +class TestDiffDriveController : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + void SetUp() + { + test_robot = std::make_shared(); + test_robot->init(); + left_wheel_names = + {{test_robot->joint_name1, test_robot->joint_name2, test_robot->joint_name3}}; + right_wheel_names = + {{test_robot->joint_name1, test_robot->joint_name2, test_robot->joint_name3}}; + op_mode = {{test_robot->write_op_handle_name1}}; + + 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::Twist velocity_message; + velocity_message.linear.x = linear; + velocity_message.angular.z = angular; + velocity_publisher->publish(velocity_message); + } + + std::string controller_name = "test_diff_drive_controller"; + + std::shared_ptr test_robot; + std::vector left_wheel_names; + std::vector right_wheel_names; + std::vector op_mode; + + rclcpp::Node::SharedPtr pub_node; + rclcpp::Publisher::SharedPtr velocity_publisher; +}; + +TEST_F(TestDiffDriveController, wrong_initialization) +{ + auto uninitialized_robot = std::make_shared(); + auto diff_drive_controller = + std::make_shared( + left_wheel_names, + right_wheel_names, op_mode); + auto ret = diff_drive_controller->init(uninitialized_robot, controller_name); + ASSERT_EQ(ret, controller_interface::CONTROLLER_INTERFACE_RET_SUCCESS); + + auto unconfigured_state = diff_drive_controller->get_lifecycle_node()->configure(); + EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, unconfigured_state.id()); +} + +TEST_F(TestDiffDriveController, correct_initialization) +{ + auto initialized_robot = std::make_shared(); + initialized_robot->init(); + auto diff_drive_controller = + std::make_shared( + left_wheel_names, + right_wheel_names, op_mode); + auto ret = diff_drive_controller->init(initialized_robot, controller_name); + ASSERT_EQ(ret, controller_interface::CONTROLLER_INTERFACE_RET_SUCCESS); + + auto inactive_state = diff_drive_controller->get_lifecycle_node()->configure(); + EXPECT_EQ(State::PRIMARY_STATE_INACTIVE, inactive_state.id()); + EXPECT_EQ(1.1, initialized_robot->pos1); + EXPECT_EQ(2.2, initialized_robot->pos2); + EXPECT_EQ(3.3, initialized_robot->pos3); +} + +TEST_F(TestDiffDriveController, configuration) +{ + auto diff_drive_controller = + std::make_shared( + left_wheel_names, + right_wheel_names, op_mode); + auto ret = diff_drive_controller->init(test_robot, controller_name); + if (ret != controller_interface::CONTROLLER_INTERFACE_RET_SUCCESS) { + FAIL(); + } + + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(diff_drive_controller->get_lifecycle_node()->get_node_base_interface()); + auto future_handle_ = std::async(std::launch::async, spin, &executor); + + auto state = diff_drive_controller->get_lifecycle_node()->configure(); + ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); + + // add in check for linear vel command values + + executor.cancel(); +} + +TEST_F(TestDiffDriveController, cleanup) +{ + auto diff_drive_controller = + std::make_shared( + left_wheel_names, + right_wheel_names, op_mode); + auto ret = diff_drive_controller->init(test_robot, controller_name); + if (ret != controller_interface::CONTROLLER_INTERFACE_RET_SUCCESS) { + FAIL(); + } + + auto diff_drive_lifecycle_node = diff_drive_controller->get_lifecycle_node(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(diff_drive_lifecycle_node->get_node_base_interface()); + + auto state = diff_drive_lifecycle_node->configure(); + ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); + + state = diff_drive_lifecycle_node->activate(); + ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); + + // wait for the subscriber and publisher to completely setup + constexpr std::chrono::seconds TIMEOUT{2}; + auto clock = pub_node->get_clock(); + auto start = clock->now(); + auto timedout = true; + while (velocity_publisher->get_subscription_count() <= 0) { + if ((clock->now() - start) > TIMEOUT) { + timedout = false; + } + rclcpp::spin_some(pub_node); + } + ASSERT_TRUE(timedout); + + // send msg + const double linear = 1.0; + const double angular = 1.0; + publish(linear, angular); + + // wait for msg is be published to the system + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + executor.spin_once(); + + diff_drive_controller->update(); + test_robot->write(); + + state = diff_drive_lifecycle_node->deactivate(); + ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); + diff_drive_controller->update(); + test_robot->write(); + + state = diff_drive_lifecycle_node->cleanup(); + ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); + diff_drive_controller->update(); + test_robot->write(); + + // shouild be stopped + EXPECT_EQ(0, test_robot->cmd1); + EXPECT_EQ(0, test_robot->cmd2); + + executor.cancel(); +} + +TEST_F(TestDiffDriveController, correct_initialization_using_parameters) +{ + auto diff_drive_controller = + std::make_shared( + left_wheel_names, + right_wheel_names, op_mode); + auto ret = diff_drive_controller->init(test_robot, controller_name); + if (ret != controller_interface::CONTROLLER_INTERFACE_RET_SUCCESS) { + FAIL(); + } + + // This block is replacing the way parameters are set via launch + auto diff_drive_lifecycle_node = diff_drive_controller->get_lifecycle_node(); + rclcpp::Parameter joint_parameters("left_wheel_names", left_wheel_names); + diff_drive_lifecycle_node->set_parameter(joint_parameters); + + std::vector operation_mode_names = {"motor_controls"}; + rclcpp::Parameter operation_mode_parameters("write_op_modes", operation_mode_names); + diff_drive_lifecycle_node->set_parameter(operation_mode_parameters); + + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(diff_drive_lifecycle_node->get_node_base_interface()); + + auto future_handle = std::async(std::launch::async, [&executor]() -> void {executor.spin();}); + + auto state = diff_drive_lifecycle_node->configure(); + ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); + EXPECT_EQ(1.1, test_robot->cmd1); + EXPECT_EQ(2.2, test_robot->cmd2); + + state = diff_drive_lifecycle_node->activate(); + ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); + + // wait for the subscriber and publisher to completely setup + std::this_thread::sleep_for(std::chrono::seconds(2)); + + // send msg + const double linear = 1.0; + const double angular = 1.0; + publish(linear, angular); + // wait for msg is be published to the system + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + + diff_drive_controller->update(); + test_robot->write(); + + // deactivated + // wait so controller process the second point when deactivated + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + state = diff_drive_lifecycle_node->deactivate(); + ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); + diff_drive_controller->update(); + test_robot->write(); + + // no change in hw position + EXPECT_EQ(1.1, test_robot->cmd1); + EXPECT_EQ(2.2, test_robot->cmd2); + + // cleanup + state = diff_drive_lifecycle_node->cleanup(); + diff_drive_controller->update(); + test_robot->write(); + ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(0, test_robot->cmd1); + EXPECT_EQ(0, test_robot->cmd2); + + state = diff_drive_lifecycle_node->configure(); + ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); + executor.cancel(); +} diff --git a/diff_drive_controller/test/test_load_diff_drive_controller.cpp b/diff_drive_controller/test/test_load_diff_drive_controller.cpp new file mode 100644 index 0000000000..f121240c2b --- /dev/null +++ b/diff_drive_controller/test/test_load_diff_drive_controller.cpp @@ -0,0 +1,40 @@ +// 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 "test_robot_hardware/test_robot_hardware.hpp" +#include "rclcpp/utilities.hpp" + +TEST(TestLoadDiffDriveController, load_controller) +{ + rclcpp::init(0, nullptr); + + std::shared_ptr robot = + std::make_shared(); + + robot->init(); + + std::shared_ptr executor = + std::make_shared(); + + controller_manager::ControllerManager cm(robot, executor, "test_controller_manager"); + + ASSERT_NO_THROW( + cm.load_controller( + "test_diff_drive_controller", + "diff_drive_controller/DiffDriveController")); +}