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,118 @@
+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(std_srvs 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/traction_limiter.cpp
+  src/steering_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
+  std_srvs
+  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_tricycle_controller
+    test/test_tricycle_controller.cpp
+    ENV config_file=${CMAKE_CURRENT_SOURCE_DIR}/test/config/test_tricycle_controller.yaml)
+  target_include_directories(test_tricycle_controller PRIVATE include)
+  target_link_libraries(test_tricycle_controller
+    ${PROJECT_NAME}
+  )
+
+  ament_target_dependencies(test_tricycle_controller
+    geometry_msgs
+    hardware_interface
+    nav_msgs
+    rclcpp
+    rclcpp_lifecycle
+    realtime_tools
+    tf2
+    tf2_msgs
+  )
+
+  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/config/tricycle_drive_controller.yaml

@@ -0,0 +1,43 @@
+tricycle_controller: 
+  ros__parameters:
+    # Model
+    traction_joint_name: traction_joint # Name of traction joint in URDF
+    steering_joint_name: steering_joint # Name of steering joint in URDF
+    wheel_radius: 0.0 # Radius of front wheel
+    wheelbase: 0.0 # Distance between center of back wheels and front wheel
+
+    # Odometry
+    odom_frame_id: odom
+    base_frame_id: base_link
+    publish_rate: 50.0 # publish rate of odom and tf
+    open_loop: false # if True, uses cmd_vel instead of hardware interface feedback to compute odometry
+    enable_odom_tf: true # If True, publishes odom<-base_link TF
+    odom_only_twist: false # If True, publishes on /odom only linear.x and angular.z; Useful for computing odometry in another node, e.g robot_localization's ekf
+    pose_covariance_diagonal: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # Need to be set if fusing odom with other localization source
+    twist_covariance_diagonal: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # Need to be set if fusing odom with other localization source
+    velocity_rolling_window_size: 10 # Rolling window size of rcppmath::RollingMeanAccumulator applied on linear and angular speeds published on odom
+
+    # Rate Limiting
+    traction: # All values should be positive
+      # min_velocity: 0.0
+      # max_velocity: 1000.0
+      # min_acceleration: 0.0
+      max_acceleration: 5.0
+      # min_deceleration: 0.0
+      max_deceleration: 8.0
+      # min_jerk: 0.0
+      # max_jerk: 1000.0
+    steering:
+      min_position: -1.57
+      max_position: 1.57
+      # min_velocity: 0.0
+      max_velocity: 1.0
+      # min_acceleration: 0.0
+      # max_acceleration: 1000.0
+
+    # cmd_vel input
+    cmd_vel_timeout: 500 # In milliseconds. Timeout to stop if no cmd_vel is received
+    use_stamped_vel: false # Set to True if using TwistStamped.
+
+    # Debug
+    publish_ackermann_command: true # Publishes AckermannDrive. The speed does not comply to the msg definition, it the wheel angular speed in rad/s.

tricycle_controller/doc/userdoc.rst

@@ -0,0 +1,24 @@
+.. _tricycle_controller_userdoc:
+
+tricycle_controller
+=====================
+
+Controller for mobile robots with tricycle drive.
+Input for control are robot base_link twist commands which are translated to traction and steering
+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.
+
+
+Other features
+--------------
+
+    Realtime-safe implementation.
+    Odometry publishing
+    Velocity, acceleration and jerk limits
+    Automatic stop after command timeout

tricycle_controller/include/tricycle_controller/odometry.hpp

@@ -0,0 +1,75 @@
+// 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
+ */
+
+#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);
+
+  bool update(double left_vel, double right_vel, const rclcpp::Duration & dt);
+  void updateOpenLoop(double linear, double angular, const rclcpp::Duration & dt);
+  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 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/steering_limiter.hpp

@@ -0,0 +1,98 @@
+// 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
+ */
+
+#ifndef TRICYCLE_CONTROLLER__STEERING_LIMITER_HPP_
+#define TRICYCLE_CONTROLLER__STEERING_LIMITER_HPP_
+
+#include <cmath>
+
+namespace tricycle_controller
+{
+
+class SteeringLimiter
+{
+public:
+  /**
+   * \brief Constructor
+   * \param [in] min_position Minimum position [m] or [rad]
+   * \param [in] max_position Maximum position [m] or [rad]
+   * \param [in] min_velocity Minimum velocity [m/s] or [rad/s]
+   * \param [in] max_velocity Maximum velocity [m/s] or [rad/s]
+   * \param [in] min_acceleration Minimum acceleration [m/s^2] or [rad/s^2]
+   * \param [in] max_acceleration Maximum acceleration [m/s^2] or [rad/s^2]
+   */
+  SteeringLimiter(
+    double min_position = NAN, double max_position = NAN,
+    double min_velocity = NAN, double max_velocity = NAN,
+    double min_acceleration = NAN, double max_acceleration = NAN);
+
+  /**
+   * \brief Limit the position, velocity and acceleration
+   * \param [in, out] p  position [m] or [rad]
+   * \param [in]      p0 Previous position to p  [m] or [rad]
+   * \param [in]      p1 Previous position to p0 [m] or [rad]
+   * \param [in]      dt Time step [s]
+   * \return Limiting factor (1.0 if none)
+   */
+  double limit(double & p, double p0, double p1, double dt);
+
+  /**
+   * \brief Limit the jerk
+   * \param [in, out] p  position [m] or [rad]
+   * \param [in]      p0 Previous position to p  [m] or [rad]
+   * \param [in]      p1 Previous position to p0 [m] or [rad]
+   * \param [in]      dt Time step [s]
+   * \return Limiting factor (1.0 if none)
+   */
+  double limit_position(double & p);
+
+  /**
+   * \brief Limit the velocity
+   * \param [in, out] p position [m]
+   * \return Limiting factor (1.0 if none)
+   */
+  double limit_velocity(double & p, double p0, double dt);
+
+  /**
+   * \brief Limit the acceleration
+   * \param [in, out] p  Position [m] or [rad]
+   * \param [in]      p0 Previous position [m] or [rad]
+   * \param [in]      dt Time step [s]
+   * \return Limiting factor (1.0 if none)
+   */
+  double limit_acceleration(double & p, double p0, double p1, double dt);
+
+private:
+
+  // Position limits:
+  double min_position_;
+  double max_position_;
+
+  // Velocity limits:
+  double min_velocity_;
+  double max_velocity_;
+
+  // Acceleration limits:
+  double min_acceleration_;
+  double max_acceleration_;
+
+};
+
+}  // namespace tricycle_controller
+
+#endif  // TRICYCLE_CONTROLLER__STEERING_LIMITER_HPP_