Add four wheels swerve drive to ROS controllers

swerve_controller/CMakeLists.txt

@@ -0,0 +1,56 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(swerve_controller)
+
+# Package dependencies
+find_package(catkin REQUIRED COMPONENTS 
+    controller_interface
+    joint_state_controller
+    nav_msgs
+    realtime_tools
+    robot_state_publisher
+    roslint
+    realtime_tools
+    tf
+    urdf_geometry_parser)
+
+# Create the package
+catkin_package(
+  INCLUDE_DIRS include
+  LIBRARIES ${PROJECT_NAME}
+  CATKIN_DEPENDS ${${PROJECT_NAME}_CATKIN_DEPS}
+)
+
+# Source and headers
+include_directories(include ${catkin_INCLUDE_DIRS})
+add_library(${PROJECT_NAME} src/odometry.cpp src/speed_limiter.cpp src/swerve_controller.cpp)
+target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
+
+# Paths for installation
+install(TARGETS ${PROJECT_NAME}
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+  )
+install(DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION})
+install(FILES swerve_controller_plugin.xml
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+
+# Tests for CI
+if(CATKIN_ENABLE_TESTING)
+  find_package(rostest REQUIRED)
+  find_package(std_srvs REQUIRED)
+  find_package(controller_manager REQUIRED)
+
+  include_directories(test/include)
+  include_directories(${catkin_INCLUDE_DIRS})
+
+  add_rostest_gtest(swerve_controller_test
+                    test/swervebot.test
+                    test/src/swervebot.cpp)
+
+  target_link_libraries(swerve_controller_test ${catkin_LIBRARIES})
+endif()
+
+# Syntax linter for CI
+roslint_cpp()

swerve_controller/include/swerve_controller/odometry.h

@@ -0,0 +1,203 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2020, Exobotic
+ *  Copyright (c) 2017, Irstea
+ *  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 Exobotic 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.
+ *********************************************************************/
+
+#ifndef SWERVE_CONTROLLER_ODOMETRY_H_
+#define SWERVE_CONTROLLER_ODOMETRY_H_
+
+#include <ros/time.h>
+#include <boost/function.hpp>
+
+namespace swerve_controller
+{
+
+/**
+* \brief The Odometry class handles odometry readings
+* (2D pose and velocity with related timestamp)
+*/
+class Odometry
+{
+    public:
+    /// Integration function, used to integrate the odometry:
+    typedef boost::function<void(double, double)> IntegrationFunction;
+
+    /**
+     * \brief Constructor
+     * Timestamp will get the current time value
+     * Value will be set to zero
+     * \param velocity_rolling_window_size Rolling window size used to compute the velocity mean
+     */
+    explicit Odometry(size_t velocity_rolling_window_size = 10);
+
+    /**
+     * \brief Initialize the odometry
+     * \param time Current time
+     */
+    void init(const ros::Time &time);
+
+    /**
+     * \brief Updates the odometry class with latest wheels and steerings position
+     * \param lf_speed front left wheel vehicle speed [rad/s]
+     * \param rf_speed front right wheel vehicle speed [rad/s]
+     * \param lh_speed rear left wheel vehicle speed [rad/s]
+     * \param rh_speed rear right wheel vehicle speed [rad/s]
+     * \param lf_steering front left steering position [rad]
+     * \param rf_steering front right steering position [rad]
+     * \param lh_steering rear left steering position [rad]
+     * \param rh_steering rear right steering position [rad]
+     * \param time Current time
+     * \return true if the odometry is actually updated
+     */
+    bool update(const double& lf_speed, const double& rf_speed, const double& lh_speed,
+                const double& rh_speed, const double& lf_steering, const double& rf_steering,
+                const double& lh_steering, const double& rh_steering, const ros::Time &time);
+
+    /**
+     * \brief heading getter
+     * \return heading [rad]
+     */
+    double getHeading() const
+    {
+      return heading_;
+    }
+
+    /**
+     * \brief x position getter
+     * \return x position [m]
+     */
+    double getX() const
+    {
+      return x_;
+    }
+
+    /**
+     * \brief y position getter
+     * \return y position [m]
+     */
+    double getY() const
+    {
+      return y_;
+    }
+
+    /**
+     * \brief linear velocity getter along X on the robot base link frame
+     * \return linear velocity [m/s]
+     */
+    double getLinearX() const
+    {
+      return linear_x_;
+    }
+
+    /**
+     * \brief linear velocity getter along Y on the robot base link frame
+     * \return linear velocity [m/s]
+     */
+    double getLinearY() const
+    {
+      return linear_y_;
+    }
+
+    /**
+     * \brief angular velocity getter
+     * \return angular velocity [rad/s]
+     */
+    double getAngular() const
+    {
+      return angular_;
+    }
+
+    /**
+     * \brief Sets the wheel parameters: radius and separation
+     * \param steering_track          Seperation between left and right steering joints [m]
+     * \param wheel_radius            Wheel radius [m]
+     * \param wheel_base              Wheel base [m]
+     */
+    void setWheelParams(double steering_track, double wheel_radius, double wheel_base);
+
+    /**
+     * \brief Velocity rolling window size setter
+     * \param velocity_rolling_window_size Velocity rolling window size
+     */
+    void setVelocityRollingWindowSize(size_t velocity_rolling_window_size);
+
+    private:
+    /**
+     * \brief Integrates the velocities (linear on x and y and angular)
+     * \param linear_x  Linear  velocity along x of the robot frame  [m] (linear  displacement, i.e. m/s * dt) computed by encoders
+     * \param linear_y  Linear  velocity along y of the robot frame   [m] (linear  displacement, i.e. m/s * dt) computed by encoders
+     * \param angular Angular velocity [rad] (angular displacement, i.e. m/s * dt) computed by encoders
+     */
+    void integrateXY(double linear_x, double linear_y, double angular);
+
+    /**
+     * \brief Integrates the velocities (linear and angular) using 2nd order Runge-Kutta
+     * \param linear  Linear  velocity   [m] (linear  displacement, i.e. m/s * dt) computed by encoders
+     * \param angular Angular velocity [rad] (angular displacement, i.e. m/s * dt) computed by encoders
+     */
+    void integrateRungeKutta2(double linear, double angular);
+
+    /**
+     * \brief Integrates the velocities (linear and angular) using exact method
+     * \param linear  Linear  velocity   [m] (linear  displacement, i.e. m/s * dt) computed by encoders
+     * \param angular Angular velocity [rad] (angular displacement, i.e. m/s * dt) computed by encoders
+     */
+    void integrateExact(double linear, double angular);
+
+    /**
+     *  \brief Reset linear and angular accumulators
+     */
+    void resetAccumulators();
+
+    // Current timestamp:
+    ros::Time last_update_timestamp_;
+
+    // Current pose:
+    double x_;        //   [m]
+    double y_;        //   [m]
+    double heading_;  // [rad]
+
+    // Current velocity:
+    double linear_x_, linear_y_;  // [m/s]
+    double angular_;  // [rad/s]
+
+    // Wheel kinematic parameters [m]:
+    double steering_track_;
+    double wheel_steering_y_offset_;
+    double wheel_radius_;
+    double wheel_base_;
+};
+
+}  // namespace swerve_controller
+
+#endif  // SWERVE_CONTROLLER_ODOMETRY_H_

swerve_controller/include/swerve_controller/speed_limiter.h

@@ -0,0 +1,134 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2020, Exobotic
+ *  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.
+ *********************************************************************/
+
+#ifndef SWERVE_CONTROLLER_SPEED_LIMITER_H
+#define SWERVE_CONTROLLER_SPEED_LIMITER_H
+
+#include <algorithm>
+
+namespace swerve_controller
+{
+
+template<typename T>
+T clamp(T x, T min, T max)
+{
+    return std::min(std::max(min, x), max);
+}
+
+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 swerve_controller
+
+#endif  // SWERVE_CONTROLLER_SPEED_LIMITER_H