Tricycle controller

ros2_controllers/package.xml

@@ -19,6 +19,7 @@
   <exec_depend>joint_state_broadcaster</exec_depend>
   <exec_depend>joint_trajectory_controller</exec_depend>
   <exec_depend>position_controllers</exec_depend>
+  <exec_depend>tricycle_controller</exec_depend>
   <exec_depend>velocity_controllers</exec_depend>
 
   <export>

tricycle_controller/CMakeLists.txt

@@ -0,0 +1,101 @@
+cmake_minimum_required(VERSION 3.8)
+project(tricycle_controller)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(controller_interface REQUIRED)
+find_package(hardware_interface REQUIRED)
+find_package(pluginlib REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rcpputils REQUIRED)
+find_package(rclcpp_lifecycle REQUIRED)
+find_package(realtime_tools REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_msgs REQUIRED)
+find_package(ackermann_msgs REQUIRED)
+
+add_library(
+  ${PROJECT_NAME}
+  SHARED
+  src/tricycle_controller.cpp
+  src/odometry.cpp
+  src/speed_limiter.cpp
+)
+target_include_directories(
+  ${PROJECT_NAME}
+  PRIVATE
+  include
+)
+
+
+ament_target_dependencies(
+  ${PROJECT_NAME}
+  ackermann_msgs
+  builtin_interfaces
+  controller_interface
+  geometry_msgs
+  hardware_interface
+  nav_msgs
+  pluginlib
+  rclcpp
+  rclcpp_lifecycle
+  rcpputils
+  realtime_tools
+  tf2
+  tf2_msgs
+)
+
+pluginlib_export_plugin_description_file(controller_interface tricycle_controller.xml)
+
+install(TARGETS  ${PROJECT_NAME}
+
+RUNTIME DESTINATION bin
+ARCHIVE DESTINATION lib
+LIBRARY DESTINATION lib
+)
+install(
+  DIRECTORY
+
+  include/
+  DESTINATION include
+)
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_gmock REQUIRED)
+  find_package(controller_manager REQUIRED)
+  find_package(ros2_control_test_assets REQUIRED)
+
+  ament_add_gmock(
+    test_load_tricycle_controller
+    test/test_load_tricycle_controller.cpp
+  )
+  target_include_directories(test_load_tricycle_controller PRIVATE include)
+  ament_target_dependencies(test_load_tricycle_controller
+    controller_manager
+    ros2_control_test_assets
+  )
+
+endif()
+
+
+ament_export_include_directories(
+  include
+)
+ament_export_libraries(
+  ${PROJECT_NAME}
+)
+ament_export_dependencies(
+  controller_interface
+  geometry_msgs
+  hardware_interface
+  rclcpp
+  rclcpp_lifecycle
+)
+
+ament_package()

tricycle_controller/doc/userdoc.rst

@@ -0,0 +1,26 @@
+.. _tricycle_controller_userdoc:
+
+tricycle_controller
+=====================
+
+Controller for mobile robots with tricycle drive.
+Input for control are robot body velocity commands which are translated to wheel commands for the tricycle drive base.
+Odometry is computed from hardware feedback and published.
+
+Velocity commands
+-----------------
+
+The controller works with a velocity twist from which it extracts the x component of the linear velocity and the z component of the angular velocity. Velocities on other components are ignored.
+
+Hardware interface type
+-----------------------
+
+The controller works with wheel joints through a velocity interface.
+
+Other features
+--------------
+
+    Realtime-safe implementation.
+    Odometry publishing
+    Task-space velocity, acceleration and jerk limits
+    Automatic stop after command time-out

tricycle_controller/include/tricycle_controller/odometry.hpp

@@ -0,0 +1,80 @@
+// Copyright 2022 Pixel Robotics.
+//
+// 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: Tony Najjar
+ * Author: Sara Al Arab
+ */
+
+#ifndef TRICYCLE_CONTROLLER__ODOMETRY_HPP_
+#define TRICYCLE_CONTROLLER__ODOMETRY_HPP_
+
+#include <cmath>
+
+#include "rclcpp/time.hpp"
+#include "rcppmath/rolling_mean_accumulator.hpp"
+
+namespace tricycle_controller
+{
+class Odometry
+{
+public:
+  explicit Odometry(size_t velocity_rolling_window_size = 10);
+
+  void init(const rclcpp::Time & time);
+  bool updateFromVelocity(double left_vel, double right_vel, const rclcpp::Time & time);
+  void updateOpenLoop(double linear, double angular, const rclcpp::Time & time);
+  void resetOdometry();
+
+  double getX() const { return x_; }
+  double getY() const { return y_; }
+  double getHeading() const { return heading_; }
+  double getLinear() const { return linear_; }
+  double getAngular() const { return angular_; }
+
+  void setWheelParams(double wheel_separation, double wheel_radius);
+  void setVelocityRollingWindowSize(size_t velocity_rolling_window_size);
+
+private:
+  using RollingMeanAccumulator = rcppmath::RollingMeanAccumulator<double>;
+
+  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 wheelbase_;
+  double wheel_radius_;
+
+  // Rolling mean accumulators for the linear and angular velocities:
+  size_t velocity_rolling_window_size_;
+  RollingMeanAccumulator linear_accumulator_;
+  RollingMeanAccumulator angular_accumulator_;
+};
+
+}  // namespace tricycle_controller
+
+#endif  // TRICYCLE_CONTROLLER__ODOMETRY_HPP_

tricycle_controller/include/tricycle_controller/speed_limiter.hpp

@@ -0,0 +1,105 @@
+// Copyright 2020 PAL Robotics S.L.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+/*
+ * Author: Enrique Fernández
+ */
+
+#ifndef TRICYCLE_CONTROLLER__SPEED_LIMITER_HPP_
+#define TRICYCLE_CONTROLLER__SPEED_LIMITER_HPP_
+
+#include <cmath>
+
+namespace tricycle_controller
+{
+class SpeedLimiter
+{
+public:
+  /**
+   * \brief Constructor
+   * \param [in] has_velocity_limits     if true, applies velocity limits
+   * \param [in] has_acceleration_limits if true, applies acceleration limits
+   * \param [in] has_jerk_limits         if true, applies jerk limits
+   * \param [in] min_velocity Minimum velocity [m/s], usually <= 0
+   * \param [in] max_velocity Maximum velocity [m/s], usually >= 0
+   * \param [in] min_acceleration Minimum acceleration [m/s^2], usually <= 0
+   * \param [in] max_acceleration Maximum acceleration [m/s^2], usually >= 0
+   * \param [in] min_jerk Minimum jerk [m/s^3], usually <= 0
+   * \param [in] max_jerk Maximum jerk [m/s^3], usually >= 0
+   */
+  SpeedLimiter(
+    bool has_velocity_limits = false, bool has_acceleration_limits = false,
+    bool has_jerk_limits = false, double min_velocity = NAN, double max_velocity = NAN,
+    double min_acceleration = NAN, double max_acceleration = NAN, double min_jerk = NAN,
+    double max_jerk = NAN);
+
+  /**
+   * \brief Limit the velocity and acceleration
+   * \param [in, out] v  Velocity [m/s]
+   * \param [in]      v0 Previous velocity to v  [m/s]
+   * \param [in]      v1 Previous velocity to v0 [m/s]
+   * \param [in]      dt Time step [s]
+   * \return Limiting factor (1.0 if none)
+   */
+  double limit(double & v, double v0, double v1, double dt);
+
+  /**
+   * \brief Limit the velocity
+   * \param [in, out] v Velocity [m/s]
+   * \return Limiting factor (1.0 if none)
+   */
+  double limit_velocity(double & v);
+
+  /**
+   * \brief Limit the acceleration
+   * \param [in, out] v  Velocity [m/s]
+   * \param [in]      v0 Previous velocity [m/s]
+   * \param [in]      dt Time step [s]
+   * \return Limiting factor (1.0 if none)
+   */
+  double limit_acceleration(double & v, double v0, double dt);
+
+  /**
+   * \brief Limit the jerk
+   * \param [in, out] v  Velocity [m/s]
+   * \param [in]      v0 Previous velocity to v  [m/s]
+   * \param [in]      v1 Previous velocity to v0 [m/s]
+   * \param [in]      dt Time step [s]
+   * \return Limiting factor (1.0 if none)
+   * \see http://en.wikipedia.org/wiki/Jerk_%28physics%29#Motion_control
+   */
+  double limit_jerk(double & v, double v0, double v1, double dt);
+
+private:
+  // Enable/Disable velocity/acceleration/jerk limits:
+  bool has_velocity_limits_;
+  bool has_acceleration_limits_;
+  bool has_jerk_limits_;
+
+  // Velocity limits:
+  double min_velocity_;
+  double max_velocity_;
+
+  // Acceleration limits:
+  double min_acceleration_;
+  double max_acceleration_;
+
+  // Jerk limits:
+  double min_jerk_;
+  double max_jerk_;
+};
+
+}  // namespace tricycle_controller
+
+#endif  // TRICYCLE_CONTROLLER__SPEED_LIMITER_HPP_