From f480bad807077aac6886045eb443e185d99fb614 Mon Sep 17 00:00:00 2001 From: Brighten Lee Date: Wed, 12 Feb 2020 17:20:16 +0900 Subject: [PATCH] add differential drive controller --- diff_drive_controller/CMakeLists.txt | 89 +++ .../diff_drive_controller.hpp | 189 ++++++ .../diff_drive_controller/odometry.hpp | 116 ++++ .../diff_drive_controller/speed_limiter.hpp | 129 ++++ .../visibility_control.h | 56 ++ diff_drive_controller/package.xml | 29 + .../src/diff_drive_controller.cpp | 573 ++++++++++++++++++ diff_drive_controller/src/odometry.cpp | 172 ++++++ diff_drive_controller/src/speed_limiter.cpp | 128 ++++ 9 files changed, 1481 insertions(+) create mode 100644 diff_drive_controller/CMakeLists.txt create mode 100644 diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp create mode 100644 diff_drive_controller/include/diff_drive_controller/odometry.hpp create mode 100644 diff_drive_controller/include/diff_drive_controller/speed_limiter.hpp create mode 100644 diff_drive_controller/include/diff_drive_controller/visibility_control.h create mode 100644 diff_drive_controller/package.xml create mode 100644 diff_drive_controller/src/diff_drive_controller.cpp create mode 100644 diff_drive_controller/src/odometry.cpp create mode 100644 diff_drive_controller/src/speed_limiter.cpp diff --git a/diff_drive_controller/CMakeLists.txt b/diff_drive_controller/CMakeLists.txt new file mode 100644 index 0000000000..5e2b0c06e8 --- /dev/null +++ b/diff_drive_controller/CMakeLists.txt @@ -0,0 +1,89 @@ +cmake_minimum_required(VERSION 3.5) +project(diff_drive_controller) + +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(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(hardware_interface REQUIRED) +find_package(controller_interface REQUIRED) +find_package(controller_manager REQUIRED) +find_package(controller_parameter_server REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(realtime_tools 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" + "class_loader" + "rclcpp" + "rclcpp_lifecycle" + "rcutils" + "hardware_interface" + "controller_interface" + "controller_manager" + "controller_parameter_server" + "geometry_msgs" + "nav_msgs" + "sensor_msgs" + "tf2" + "tf2_ros" + "realtime_tools" +) + +target_compile_definitions(diff_drive_controller PRIVATE "ROS_CONTROLLERS_BUILDING_DLL") + +install( + DIRECTORY include/ + DESTINATION include +) + +install( + TARGETS + diff_drive_controller + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib +) + +ament_export_dependencies( + rclcpp + rclcpp_lifecycle + controller_interface + geometry_msgs + nav_msgs + sensor_msgs + tf2 + tf2_ros + realtime_tools +) + +ament_export_include_directories( + include +) + +ament_export_libraries( + diff_drive_controller +) + +ament_package() \ No newline at end of file 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..a8800b36bb --- /dev/null +++ b/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp @@ -0,0 +1,189 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, PAL Robotics, S.L. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the PAL Robotics nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* + * Author: Bence Magyar, Enrique Fernández + * Author: Brighten Lee + */ + +#ifndef DIFF_DRIVE_CONTROLLER__DIFF_DRIVE_CONTROLLER_HPP_ +#define DIFF_DRIVE_CONTROLLER__DIFF_DRIVE_CONTROLLER_HPP_ + +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/state.hpp" + +#include "hardware_interface/robot_hardware.hpp" +#include "hardware_interface/joint_command_handle.hpp" +#include "hardware_interface/operation_mode_handle.hpp" +#include "controller_interface/controller_interface.hpp" + +#include "tf2_msgs/msg/tf_message.hpp" +#include "realtime_tools/realtime_publisher.h" +#include "realtime_tools/realtime_buffer.h" +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "diff_drive_controller/visibility_control.h" +#include "diff_drive_controller/odometry.hpp" +#include "diff_drive_controller/speed_limiter.hpp" + +namespace diff_drive_controller +{ + +class DiffDriveController : public controller_interface::ControllerInterface +{ +public: + DIFF_DRIVE_CONTROLLER_PUBLIC + DiffDriveController(); + + DIFF_DRIVE_CONTROLLER_PUBLIC + DiffDriveController(const std::vector &left_joint_names, + const std::vector &right_joint_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 + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + on_configure(const rclcpp_lifecycle::State &previous_state) override; + + DIFF_DRIVE_CONTROLLER_PUBLIC + controller_interface::controller_interface_ret_t + update() 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; + + void cmdVelCallback(const geometry_msgs::msg::Twist::SharedPtr msg); + +private: + bool reset(); + + // Hardware handles + std::vector + registered_left_joint_state_handles_, registered_right_joint_state_handles_; + std::vector + registered_left_joint_cmd_handles_, registered_right_joint_cmd_handles_; + //std::vector registered_operation_mode_handles_; + + // Wheel parameters + std::vector left_joint_names_, right_joint_names_; + //std::vector write_op_names_; + size_t wheel_joints_size_; // Number of wheel joints + double wheel_separation_; // wrt the midpoint of the wheel width + double wheel_radius_; // assuming it's the same for the left and right wheels + double wheel_separation_multiplier_; // Wheel separation calibration multiplier + double left_wheel_radius_multiplier_; // Left wheel radius calibration multiplier + double right_wheel_radius_multiplier_; // Right wheel wheel radius calibration multiplier + + // Odometry + bool open_loop_; + bool enable_odom_tf_; // Whether to publish odometry to tf or not + std::string base_frame_id_; // Frame to use for the robot base + std::string odom_frame_id_; // Frame to use for odometry and odom tf + std::vector pose_covariance_diagonal_; + std::vector twist_covariance_diagonal_; + + std::shared_ptr odometry_ = nullptr; + std::shared_ptr> pub_odom_; + std::shared_ptr> pub_tf_odom_; + std::shared_ptr> rp_odom_; + std::shared_ptr> rp_tf_odom_; + + // Velocity command related + struct Commands + { + double lin; + double ang; + rclcpp::Time stamp; + + Commands() : lin(0.0), ang(0.0), stamp(0.0) {} + }; + Commands command_struct_; + bool subscriber_is_active_; + + // Whether to allow multiple publishers on cmd_vel topic or not + bool allow_multiple_cmd_vel_publishers_; + + int velocity_rolling_window_size_; + + // Timeout to consider cmd_vel commands old + double cmd_vel_timeout_; + + rclcpp::Subscription::SharedPtr sub_command_; + realtime_tools::RealtimeBuffer command_; + + std::shared_ptr> pub_cmd_vel_; + std::shared_ptr> rp_cmd_vel_; + + // Speed limiters + Commands last1_cmd_; + Commands last0_cmd_; + std::shared_ptr limiter_lin_ = nullptr; + std::shared_ptr limiter_ang_ = nullptr; + + // Publish limited velocity + bool publish_cmd_; + + // Previous time + rclcpp::Time previous_time_; +}; + +} // namespace diff_drive_controller + +#endif // DIFF_DRIVE_CONTROLLER__DIFF_DRIVE_CONTROLLER_HPP_ \ No newline at end of file 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..7948328bb5 --- /dev/null +++ b/diff_drive_controller/include/diff_drive_controller/odometry.hpp @@ -0,0 +1,116 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, PAL Robotics, S.L. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the PAL Robotics nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* + * Author: Enrique Fernández + */ + +#ifndef DIFF_DRIVE_CONTROLLER__ODOMETRY_HPP_ +#define DIFF_DRIVE_CONTROLLER__ODOMETRY_HPP_ + +#include +#include +#include +#include +#include +#include +#include "rclcpp/rclcpp.hpp" + +namespace diff_drive_controller +{ + +namespace bacc = boost::accumulators; + +class Odometry +{ +public: + typedef boost::function IntegrationFunction; + + 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: + typedef bacc::accumulator_set> RollingMeanAcc; + typedef bacc::tag::rolling_window RollingWindow; + + 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_; + + // Previou wheel position/state [rad]: + double left_wheel_old_pos_; + double right_wheel_old_pos_; + + // Rolling mean accumulators for the linar and angular velocities: + size_t velocity_rolling_window_size_; + RollingMeanAcc linear_acc_; + RollingMeanAcc angular_acc_; + + // Integration funcion, used to integrate the odometry: + IntegrationFunction integrate_fun_; +}; + +} // namespace diff_drive_controller + +#endif // DIFF_DRIVE_CONTROLLER__ODOMETRY_HPP_ \ No newline at end of file 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..ebb5b4848a --- /dev/null +++ b/diff_drive_controller/include/diff_drive_controller/speed_limiter.hpp @@ -0,0 +1,129 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, PAL Robotics, S.L. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the PAL Robotics nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* + * Author: 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); + +public: + // 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_ \ No newline at end of file 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..aab71a49b6 --- /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_ \ No newline at end of file diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml new file mode 100644 index 0000000000..b4bd503492 --- /dev/null +++ b/diff_drive_controller/package.xml @@ -0,0 +1,29 @@ + + + + diff_drive_controller + 0.0.0 + + Controller for a differential drive mobile base + + Brighten Lee + Apache License 2.0 + + ament_cmake + + rclcpp + rclcpp_lifecycle + hardware_interface + controller_interface + controller_manager + controller_parameter_server + geometry_msgs + nav_msgs + sensor_msgs + tf2 + tf2_ros + realtime_tools + + 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..727a969fc1 --- /dev/null +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -0,0 +1,573 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, PAL Robotics, S.L. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the PAL Robotics nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* + * Author: Bence Magyar, Enrique Fernández + * Author: Brighten Lee + */ + +#include "diff_drive_controller/diff_drive_controller.hpp" + +#include "rclcpp/time.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include "builtin_interfaces/msg/time.hpp" +#include "lifecycle_msgs/msg/transition.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "tf2/LinearMath/Quaternion.h" + +namespace diff_drive_controller +{ + +DiffDriveController::DiffDriveController() + : controller_interface::ControllerInterface(), + left_joint_names_({}), + right_joint_names_({}), + wheel_joints_size_(0), + wheel_separation_(0.0), + wheel_radius_(0.0), + wheel_separation_multiplier_(1.0), + left_wheel_radius_multiplier_(1.0), + right_wheel_radius_multiplier_(1.0), + open_loop_(false), + enable_odom_tf_(true), + base_frame_id_("base_link"), + odom_frame_id_("odom"), + pose_covariance_diagonal_({}), + twist_covariance_diagonal_({}), + command_struct_(), + subscriber_is_active_(false), + allow_multiple_cmd_vel_publishers_(true), + velocity_rolling_window_size_(10), + cmd_vel_timeout_(0.5), + last1_cmd_(), + last0_cmd_(), + publish_cmd_(false), + previous_time_(0.0) +{ +} + +DiffDriveController::DiffDriveController(const std::vector &left_joint_names, + const std::vector &right_joint_names) + : controller_interface::ControllerInterface(), + left_joint_names_(left_joint_names), + right_joint_names_(right_joint_names), + wheel_joints_size_(0), + wheel_separation_(0.0), + wheel_radius_(0.0), + wheel_separation_multiplier_(1.0), + left_wheel_radius_multiplier_(1.0), + right_wheel_radius_multiplier_(1.0), + open_loop_(false), + enable_odom_tf_(true), + base_frame_id_("base_link"), + odom_frame_id_("odom"), + pose_covariance_diagonal_({}), + twist_covariance_diagonal_({}), + command_struct_(), + subscriber_is_active_(false), + allow_multiple_cmd_vel_publishers_(true), + velocity_rolling_window_size_(10), + cmd_vel_timeout_(0.5), + last1_cmd_(), + last0_cmd_(), + publish_cmd_(false), + previous_time_(0.0) +{ +} + +controller_interface::controller_interface_ret_t +DiffDriveController::init(std::weak_ptr robot_hardware, + const std::string &controller_name) +{ + auto ret = ControllerInterface::init(robot_hardware, controller_name); + if (ret != controller_interface::CONTROLLER_INTERFACE_RET_SUCCESS) + return ret; + + lifecycle_node_->declare_parameter("left_joint_names"); + lifecycle_node_->declare_parameter("right_joint_names"); + lifecycle_node_->declare_parameter("pose_covariance_diagonal"); + lifecycle_node_->declare_parameter("twist_covariance_diagonal"); + lifecycle_node_->declare_parameter("wheel_separation"); + lifecycle_node_->declare_parameter("wheel_radius"); + lifecycle_node_->declare_parameter("wheel_separation_multiplier"); + lifecycle_node_->declare_parameter("wheel_radius_multiplier"); + lifecycle_node_->declare_parameter("open_loop"); + lifecycle_node_->declare_parameter("cmd_vel_timeout"); + lifecycle_node_->declare_parameter("allow_multiple_cmd_vel_publishers"); + lifecycle_node_->declare_parameter("base_frame_id"); + lifecycle_node_->declare_parameter("odom_frame_id"); + lifecycle_node_->declare_parameter("enable_odom_tf"); + lifecycle_node_->declare_parameter("publish_cmd"); + lifecycle_node_->declare_parameter("velocity_rolling_window_size"); + + lifecycle_node_->declare_parameter("linear.x.has_velocity_limits"); + lifecycle_node_->declare_parameter("linear.x.has_acceleration_limits"); + lifecycle_node_->declare_parameter("linear.x.has_jerk_limits"); + lifecycle_node_->declare_parameter("linear.x.max_velocity"); + lifecycle_node_->declare_parameter("linear.x.min_velocity"); + lifecycle_node_->declare_parameter("linear.x.max_acceleration"); + lifecycle_node_->declare_parameter("linear.x.min_acceleration"); + lifecycle_node_->declare_parameter("linear.x.max_jerk"); + lifecycle_node_->declare_parameter("linear.x.min_jerk"); + + lifecycle_node_->declare_parameter("angular.z.has_velocity_limits"); + lifecycle_node_->declare_parameter("angular.z.has_acceleration_limits"); + lifecycle_node_->declare_parameter("angular.z.has_jerk_limits"); + lifecycle_node_->declare_parameter("angular.z.max_velocity"); + lifecycle_node_->declare_parameter("angular.z.min_velocity"); + lifecycle_node_->declare_parameter("angular.z.max_acceleration"); + lifecycle_node_->declare_parameter("angular.z.min_acceleration"); + lifecycle_node_->declare_parameter("angular.z.max_jerk"); + lifecycle_node_->declare_parameter("angular.z.min_jerk"); + + odometry_ = std::make_shared(); + limiter_lin_ = std::make_shared(); + limiter_ang_ = std::make_shared(); + + return controller_interface::CONTROLLER_INTERFACE_RET_SUCCESS; +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +DiffDriveController::on_configure(const rclcpp_lifecycle::State &previous_state) +{ + (void)previous_state; + + auto logger = lifecycle_node_->get_logger(); + + if (!reset()) + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; + + lifecycle_node_->get_parameter>("left_joint_names", left_joint_names_); + lifecycle_node_->get_parameter>("right_joint_names", right_joint_names_); + lifecycle_node_->get_parameter>("pose_covariance_diagonal", pose_covariance_diagonal_); + lifecycle_node_->get_parameter>("twist_covariance_diagonal", twist_covariance_diagonal_); + lifecycle_node_->get_parameter("open_loop", open_loop_); + lifecycle_node_->get_parameter("wheel_separation", wheel_separation_); + lifecycle_node_->get_parameter("wheel_radius", wheel_radius_); + lifecycle_node_->get_parameter("wheel_separation_multiplier", wheel_separation_multiplier_); + lifecycle_node_->get_parameter("wheel_radius_multiplier", left_wheel_radius_multiplier_); + lifecycle_node_->get_parameter("wheel_radius_multiplier", right_wheel_radius_multiplier_); + lifecycle_node_->get_parameter("cmd_vel_timeout", cmd_vel_timeout_); + lifecycle_node_->get_parameter("allow_multiple_cmd_vel_publishers", allow_multiple_cmd_vel_publishers_); + lifecycle_node_->get_parameter("base_frame_id", base_frame_id_); + lifecycle_node_->get_parameter("odom_frame_id", odom_frame_id_); + lifecycle_node_->get_parameter("enable_odom_tf", enable_odom_tf_); + lifecycle_node_->get_parameter("publish_cmd", publish_cmd_); + lifecycle_node_->get_parameter("velocity_rolling_window_size", velocity_rolling_window_size_); + + lifecycle_node_->get_parameter("linear.x.has_velocity_limits", limiter_lin_->has_velocity_limits); + lifecycle_node_->get_parameter("linear.x.has_acceleration_limits", limiter_lin_->has_acceleration_limits); + lifecycle_node_->get_parameter("linear.x.has_jerk_limits", limiter_lin_->has_jerk_limits); + lifecycle_node_->get_parameter("linear.x.max_velocity", limiter_lin_->max_velocity); + lifecycle_node_->get_parameter("linear.x.min_velocity", limiter_lin_->min_velocity); + lifecycle_node_->get_parameter("linear.x.max_acceleration", limiter_lin_->max_acceleration); + lifecycle_node_->get_parameter("linear.x.min_acceleration", limiter_lin_->min_acceleration); + lifecycle_node_->get_parameter("linear.x.max_jerk", limiter_lin_->max_jerk); + lifecycle_node_->get_parameter("linear.x.min_jerk", limiter_lin_->min_jerk); + + lifecycle_node_->get_parameter("angular.z.has_velocity_limits", limiter_ang_->has_velocity_limits); + lifecycle_node_->get_parameter("angular.z.has_acceleration_limits", limiter_ang_->has_acceleration_limits); + lifecycle_node_->get_parameter("angular.z.has_jerk_limits", limiter_ang_->has_jerk_limits); + lifecycle_node_->get_parameter("angular.z.max_velocity", limiter_ang_->max_velocity); + lifecycle_node_->get_parameter("angular.z.min_velocity", limiter_ang_->min_velocity); + lifecycle_node_->get_parameter("angular.z.max_acceleration", limiter_ang_->max_acceleration); + lifecycle_node_->get_parameter("angular.z.min_acceleration", limiter_ang_->min_acceleration); + lifecycle_node_->get_parameter("angular.z.max_jerk", limiter_ang_->max_jerk); + lifecycle_node_->get_parameter("angular.z.min_jerk", limiter_ang_->min_jerk); + + odometry_->setVelocityRollingWindowSize(velocity_rolling_window_size_); + + if (left_joint_names_.empty() || right_joint_names_.empty()) + { + RCLCPP_ERROR(logger, "No joint names specified"); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; + } + if (left_joint_names_.size() != right_joint_names_.size()) + { + RCLCPP_ERROR(logger, "The number of left wheels[%d] and the number of right wheels[%d] are different", + left_joint_names_.size(), right_joint_names_.size()); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; + } + + wheel_joints_size_ = left_joint_names_.size(); + + RCLCPP_INFO(logger, "Wheel separation will be multiplied by %f", + wheel_separation_multiplier_); + RCLCPP_INFO(logger, "Left wheel radius will be multiplied by %f", + left_wheel_radius_multiplier_); + RCLCPP_INFO(logger, "Right wheel radius will be multiplied by %f", + right_wheel_radius_multiplier_); + RCLCPP_INFO(logger, "Velocity commands will be considered old if they are older than %f", + cmd_vel_timeout_); + RCLCPP_INFO(logger, "Allow mutiple cmd_vel publishers is %s", + (allow_multiple_cmd_vel_publishers_ ? "enabled" : "disabled")); + RCLCPP_INFO(logger, "Base frame id set to %s", + base_frame_id_.c_str()); + RCLCPP_INFO(logger, "Odometry frame id set to %s", + odom_frame_id_.c_str()); + RCLCPP_INFO(logger, "Publishing to tf is %s", + (enable_odom_tf_ ? "enabled" : "disabled")); + + if (auto robot_hardware = robot_hardware_.lock()) + { + registered_left_joint_state_handles_.resize(left_joint_names_.size()); + for (size_t index = 0; index < left_joint_names_.size(); ++index) + { + auto ret = robot_hardware->get_joint_state_handle( + left_joint_names_[index].c_str(), ®istered_left_joint_state_handles_[index]); + if (ret != hardware_interface::HW_RET_OK) + { + RCLCPP_WARN(logger, "Unable to obtain joint state handle for %s", + left_joint_names_[index].c_str()); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; + } + } + + registered_right_joint_state_handles_.resize(right_joint_names_.size()); + for (size_t index = 0; index < right_joint_names_.size(); ++index) + { + auto ret = robot_hardware->get_joint_state_handle( + right_joint_names_[index].c_str(), ®istered_right_joint_state_handles_[index]); + if (ret != hardware_interface::HW_RET_OK) + { + RCLCPP_WARN(logger, "Unable to obtain joint state handle for %s", + right_joint_names_[index].c_str()); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; + } + } + + registered_left_joint_cmd_handles_.resize(left_joint_names_.size()); + for (size_t index = 0; index < left_joint_names_.size(); ++index) + { + auto ret = robot_hardware->get_joint_command_handle( + left_joint_names_[index].c_str(), ®istered_left_joint_cmd_handles_[index]); + if (ret != hardware_interface::HW_RET_OK) + { + RCLCPP_WARN(logger, "Unable to obtain joint command handle for %s", + left_joint_names_[index].c_str()); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; + } + } + + registered_right_joint_cmd_handles_.resize(right_joint_names_.size()); + for (size_t index = 0; index < right_joint_names_.size(); ++index) + { + auto ret = robot_hardware->get_joint_command_handle( + right_joint_names_[index].c_str(), ®istered_right_joint_cmd_handles_[index]); + if (ret != hardware_interface::HW_RET_OK) + { + RCLCPP_WARN(logger, "Unable to obtain joint command handle for %s", + right_joint_names_[index].c_str()); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; + } + } + } + else + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; + + if (registered_left_joint_state_handles_.empty() || + registered_right_joint_state_handles_.empty() || + registered_left_joint_cmd_handles_.empty() || + registered_right_joint_cmd_handles_.empty()) + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; + + if (publish_cmd_) + { + pub_cmd_vel_ = lifecycle_node_->create_publisher( + "/cmd_vel_out", rclcpp::SystemDefaultsQoS()); + rp_cmd_vel_ = std::make_shared>(pub_cmd_vel_); + } + + sub_command_ = lifecycle_node_->create_subscription( + "~/cmd_vel", + rclcpp::SystemDefaultsQoS(), + std::bind(&DiffDriveController::cmdVelCallback, this, std::placeholders::_1)); + + pub_odom_ = lifecycle_node_->create_publisher( + "~/odom", rclcpp::SystemDefaultsQoS()); + rp_odom_ = std::make_shared>(pub_odom_); + + pub_tf_odom_ = lifecycle_node_->create_publisher( + "/tf", rclcpp::SystemDefaultsQoS()); + rp_tf_odom_ = std::make_shared>(pub_tf_odom_); + + const double ws = wheel_separation_multiplier_ * wheel_separation_; + const double lwr = left_wheel_radius_multiplier_ * wheel_radius_; + const double rwr = right_wheel_radius_multiplier_ * wheel_radius_; + odometry_->setWheelParams(ws, lwr, rwr); + RCLCPP_INFO(logger, "Odometry params : wheel separation %f, left wheel radius %f, right wheel radius %f", ws, lwr, rwr); + + rp_odom_->msg_.header.frame_id = odom_frame_id_; + rp_odom_->msg_.child_frame_id = base_frame_id_; + rp_odom_->msg_.pose.pose.position.z = 0.0; + rp_odom_->msg_.pose.covariance = { + pose_covariance_diagonal_[0], 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, pose_covariance_diagonal_[1], 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, pose_covariance_diagonal_[2], 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, pose_covariance_diagonal_[3], 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, pose_covariance_diagonal_[4], 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, pose_covariance_diagonal_[5]}; + + rp_odom_->msg_.twist.twist.linear.y = 0.0; + rp_odom_->msg_.twist.twist.linear.z = 0.0; + rp_odom_->msg_.twist.twist.angular.x = 0.0; + rp_odom_->msg_.twist.twist.angular.y = 0.0; + rp_odom_->msg_.twist.covariance = { + twist_covariance_diagonal_[0], 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, twist_covariance_diagonal_[1], 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, twist_covariance_diagonal_[2], 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, twist_covariance_diagonal_[3], 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, twist_covariance_diagonal_[4], 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, twist_covariance_diagonal_[5]}; + + rp_tf_odom_->msg_.transforms.resize(1); + rp_tf_odom_->msg_.transforms[0].transform.translation.z = 0.0; + rp_tf_odom_->msg_.transforms[0].child_frame_id = base_frame_id_; + rp_tf_odom_->msg_.transforms[0].header.frame_id = odom_frame_id_; + + previous_time_ = rclcpp::Clock().now(); + command_struct_.stamp = rclcpp::Clock().now(); + + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +controller_interface::controller_interface_ret_t +DiffDriveController::update() +{ + rclcpp::Time cur_time = rclcpp::Clock().now(); + + const double ws = wheel_separation_multiplier_ * wheel_separation_; + const double lwr = left_wheel_radius_multiplier_ * wheel_radius_; + const double rwr = right_wheel_radius_multiplier_ * wheel_radius_; + odometry_->setWheelParams(ws, lwr, rwr); + + if (open_loop_) + odometry_->updateOpenLoop(last0_cmd_.lin, last0_cmd_.ang, cur_time); + else + { + double left_pos = 0.0; + double right_pos = 0.0; + for (size_t i = 0; i < wheel_joints_size_; ++i) + { + const double lp = registered_left_joint_state_handles_[i]->get_position(); + const double rp = registered_right_joint_state_handles_[i]->get_position(); + if (std::isnan(lp) || std::isnan(rp)) + return controller_interface::CONTROLLER_INTERFACE_RET_ERROR; + + left_pos += lp; + right_pos += rp; + } + left_pos /= wheel_joints_size_; + right_pos /= wheel_joints_size_; + + odometry_->update(left_pos, right_pos, cur_time); + } + + tf2::Quaternion orientation; + orientation.setRPY(0.0, 0.0, odometry_->getHeading()); + + if (pub_odom_->is_activated() && rp_odom_->trylock()) + { + rp_odom_->msg_.header.stamp = cur_time; + rp_odom_->msg_.pose.pose.position.x = odometry_->getX(); + rp_odom_->msg_.pose.pose.position.y = odometry_->getY(); + rp_odom_->msg_.pose.pose.orientation.x = orientation.x(); + rp_odom_->msg_.pose.pose.orientation.y = orientation.y(); + rp_odom_->msg_.pose.pose.orientation.z = orientation.z(); + rp_odom_->msg_.pose.pose.orientation.w = orientation.w(); + rp_odom_->msg_.twist.twist.linear.x = odometry_->getLinear(); + rp_odom_->msg_.twist.twist.angular.z = odometry_->getAngular(); + rp_odom_->unlockAndPublish(); + } + + if (enable_odom_tf_ && pub_tf_odom_->is_activated() && rp_tf_odom_->trylock()) + { + rp_tf_odom_->msg_.transforms[0].header.stamp = cur_time; + rp_tf_odom_->msg_.transforms[0].transform.translation.x = odometry_->getX(); + rp_tf_odom_->msg_.transforms[0].transform.translation.y = odometry_->getY(); + rp_tf_odom_->msg_.transforms[0].transform.rotation.x = orientation.x(); + rp_tf_odom_->msg_.transforms[0].transform.rotation.y = orientation.y(); + rp_tf_odom_->msg_.transforms[0].transform.rotation.z = orientation.z(); + rp_tf_odom_->msg_.transforms[0].transform.rotation.w = orientation.w(); + rp_tf_odom_->unlockAndPublish(); + } + + // Retreive current velocity command and time step + Commands cur_cmd = *(command_.readFromRT()); + + const double dt = cur_time.seconds() - cur_cmd.stamp.seconds(); + + // Brake if cmd_vel has timeout + if (dt > cmd_vel_timeout_) + { + cur_cmd.lin = 0.0; + cur_cmd.ang = 0.0; + } + + const double cmd_dt = cur_time.seconds() - previous_time_.seconds(); + previous_time_ = cur_time; + + limiter_lin_->limit(cur_cmd.lin, last0_cmd_.lin, last1_cmd_.lin, cmd_dt); + limiter_ang_->limit(cur_cmd.ang, last0_cmd_.ang, last1_cmd_.ang, cmd_dt); + + last1_cmd_ = last0_cmd_; + last0_cmd_ = cur_cmd; + + // Publish limited velocity + if (publish_cmd_ && pub_cmd_vel_->is_activated() && rp_cmd_vel_->trylock()) + { + rp_cmd_vel_->msg_.header.stamp = cur_time; + rp_cmd_vel_->msg_.twist.linear.x = cur_cmd.lin; + rp_cmd_vel_->msg_.twist.angular.z = cur_cmd.ang; + rp_cmd_vel_->unlockAndPublish(); + } + + // Compute wheels velocities: + const double vel_left = (cur_cmd.lin - cur_cmd.ang * ws / 2.0) / lwr; + const double vel_right = (cur_cmd.lin + cur_cmd.ang * ws / 2.0) / rwr; + + // Set wheels velocities: + for (size_t i = 0; i < wheel_joints_size_; ++i) + { + registered_left_joint_cmd_handles_[i]->set_cmd(vel_left); + registered_right_joint_cmd_handles_[i]->set_cmd(vel_right); + } + + return controller_interface::CONTROLLER_INTERFACE_RET_SUCCESS; +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +DiffDriveController::on_activate(const rclcpp_lifecycle::State &previous_state) +{ + (void)previous_state; + + subscriber_is_active_ = true; + pub_odom_->on_activate(); + pub_tf_odom_->on_activate(); + if (publish_cmd_) + pub_cmd_vel_->on_activate(); + + RCLCPP_INFO(lifecycle_node_->get_logger(), + "Lifecycle subscriber and publisher are currently active."); + + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +DiffDriveController::on_deactivate(const rclcpp_lifecycle::State &previous_state) +{ + (void)previous_state; + + subscriber_is_active_ = false; + pub_odom_->on_deactivate(); + pub_tf_odom_->on_deactivate(); + if (publish_cmd_) + pub_cmd_vel_->on_deactivate(); + + RCLCPP_WARN(lifecycle_node_->get_logger(), + "Lifecycle subscriber and publisher are currently inactive. Can't subscribe and publish messages"); + + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +DiffDriveController::on_cleanup(const rclcpp_lifecycle::State &previous_state) +{ + (void)previous_state; + + odometry_->resetOdometry(); + + if (!reset()) + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; + + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +DiffDriveController::on_error(const rclcpp_lifecycle::State &previous_state) +{ + (void)previous_state; + + if (!reset()) + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; + + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +DiffDriveController::on_shutdown(const rclcpp_lifecycle::State &previous_state) +{ + (void)previous_state; + + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +void DiffDriveController::cmdVelCallback(const geometry_msgs::msg::Twist::SharedPtr msg) +{ + if (subscriber_is_active_) + { + if (!allow_multiple_cmd_vel_publishers_) + { + // TODO + } + + command_struct_.lin = msg->linear.x; + command_struct_.ang = msg->angular.z; + command_struct_.stamp = rclcpp::Clock().now(); + command_.writeFromNonRT(command_struct_); + } + else + RCLCPP_ERROR(lifecycle_node_->get_logger(), + "Can't accept new commands. subscriber is inactive"); +} + +bool DiffDriveController::reset() +{ + registered_left_joint_state_handles_.clear(); + registered_right_joint_state_handles_.clear(); + registered_left_joint_cmd_handles_.clear(); + registered_right_joint_cmd_handles_.clear(); + + subscriber_is_active_ = false; + sub_command_.reset(); + + return true; +} + +} // namespace diff_drive_controller + +#include "class_loader/register_macro.hpp" + +CLASS_LOADER_REGISTER_CLASS( + diff_drive_controller::DiffDriveController, controller_interface::ControllerInterface) \ No newline at end of file diff --git a/diff_drive_controller/src/odometry.cpp b/diff_drive_controller/src/odometry.cpp new file mode 100644 index 0000000000..ea365c831f --- /dev/null +++ b/diff_drive_controller/src/odometry.cpp @@ -0,0 +1,172 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, PAL Robotics, S.L. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the PAL Robotics nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* + * Author: 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_acc_(RollingWindow::window_size = velocity_rolling_window_size), + angular_acc_(RollingWindow::window_size = velocity_rolling_window_size), + integrate_fun_(boost::bind(&Odometry::integrateExact, this, _1, _2)) +{ +} + +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) +{ + // 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_ * 0.43396; + + // Integrate odometry: + integrate_fun_(linear, angular); + + // We cannot estimate the speed with very small time intervals: + const double dt = time.seconds() - timestamp_.seconds(); + if (dt < 0.0001) + return false; // Interval too small to integrate with + + timestamp_ = time; + + // Estimate speeds using a rolling mean to filter them out: + linear_acc_(linear / dt); + angular_acc_(angular / dt); + + linear_ = bacc::rolling_mean(linear_acc_); + angular_ = bacc::rolling_mean(angular_acc_); + + return true; +} + +void Odometry::updateOpenLoop(double linear, double angular, const rclcpp::Time &time) +{ + /// Save last linear and angular velocity: + linear_ = linear; + angular_ = angular; + + /// Integrate odometry: + const double dt = time.seconds() - timestamp_.seconds(); + timestamp_ = time; + integrate_fun_(linear * dt, angular * dt); +} + +void Odometry::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_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_); + angular_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_); +} + +} // namespace diff_drive_controller \ No newline at end of file diff --git a/diff_drive_controller/src/speed_limiter.cpp b/diff_drive_controller/src/speed_limiter.cpp new file mode 100644 index 0000000000..fc4c0f374e --- /dev/null +++ b/diff_drive_controller/src/speed_limiter.cpp @@ -0,0 +1,128 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, PAL Robotics, S.L. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the PAL Robotics nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* + * Author: 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 \ No newline at end of file