Steering odometry library and controllers

.pre-commit-config.yaml

@@ -126,7 +126,7 @@ repos:
         exclude: CHANGELOG\.rst$
 
   - repo: https://github.com/pre-commit/pygrep-hooks
-    rev: v1.9.0
+    rev: v1.10.0
     hooks:
       - id: rst-backticks
         exclude: CHANGELOG\.rst$

ackermann_steering_controller/CMakeLists.txt

@@ -0,0 +1,104 @@
+cmake_minimum_required(VERSION 3.16)
+project(ackermann_steering_controller LANGUAGES CXX)
+
+if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+set(THIS_PACKAGE_INCLUDE_DEPENDS
+  controller_interface
+  hardware_interface
+  generate_parameter_library
+  pluginlib
+  rclcpp
+  rclcpp_lifecycle
+  realtime_tools
+  std_srvs
+  steering_controllers_library
+)
+
+find_package(ament_cmake REQUIRED)
+find_package(backward_ros REQUIRED)
+foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
+  find_package(${Dependency} REQUIRED)
+endforeach()
+
+generate_parameter_library(ackermann_steering_controller_parameters
+  src/ackermann_steering_controller.yaml
+)
+
+add_library(
+  ackermann_steering_controller
+  SHARED
+  src/ackermann_steering_controller.cpp
+)
+target_compile_features(ackermann_steering_controller PUBLIC cxx_std_17)
+target_include_directories(ackermann_steering_controller PUBLIC
+  "$<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>"
+  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
+target_link_libraries(ackermann_steering_controller PUBLIC
+  ackermann_steering_controller_parameters)
+ament_target_dependencies(ackermann_steering_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS})
+
+# Causes the visibility macros to use dllexport rather than dllimport,
+# which is appropriate when building the dll but not consuming it.
+target_compile_definitions(ackermann_steering_controller PRIVATE "ACKERMANN_STEERING_CONTROLLER_BUILDING_DLL")
+
+pluginlib_export_plugin_description_file(
+  controller_interface ackermann_steering_controller.xml)
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_gmock REQUIRED)
+  find_package(controller_manager REQUIRED)
+  find_package(hardware_interface REQUIRED)
+  find_package(ros2_control_test_assets REQUIRED)
+
+  ament_add_gmock(test_load_ackermann_steering_controller test/test_load_ackermann_steering_controller.cpp)
+  target_include_directories(test_load_ackermann_steering_controller PRIVATE include)
+  ament_target_dependencies(
+    test_load_ackermann_steering_controller
+    controller_manager
+    hardware_interface
+    ros2_control_test_assets
+  )
+
+  add_rostest_with_parameters_gmock(
+    test_ackermann_steering_controller test/test_ackermann_steering_controller.cpp
+    ${CMAKE_CURRENT_SOURCE_DIR}/test/ackermann_steering_controller_params.yaml)
+  target_include_directories(test_ackermann_steering_controller PRIVATE include)
+  target_link_libraries(test_ackermann_steering_controller ackermann_steering_controller)
+  ament_target_dependencies(
+    test_ackermann_steering_controller
+    controller_interface
+    hardware_interface
+  )
+
+  add_rostest_with_parameters_gmock(
+    test_ackermann_steering_controller_preceeding test/test_ackermann_steering_controller_preceeding.cpp
+    ${CMAKE_CURRENT_SOURCE_DIR}/test/ackermann_steering_controller_preceeding_params.yaml)
+  target_include_directories(test_ackermann_steering_controller_preceeding PRIVATE include)
+  target_link_libraries(test_ackermann_steering_controller_preceeding ackermann_steering_controller)
+  ament_target_dependencies(
+    test_ackermann_steering_controller_preceeding
+    controller_interface
+    hardware_interface
+  )
+endif()
+
+install(
+  DIRECTORY include/
+  DESTINATION include/ackermann_steering_controller
+)
+
+install(
+  TARGETS ackermann_steering_controller ackermann_steering_controller_parameters
+  EXPORT export_ackermann_steering_controller
+  RUNTIME DESTINATION bin
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+)
+
+ament_export_targets(export_ackermann_steering_controller HAS_LIBRARY_TARGET)
+ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
+ament_package()

ackermann_steering_controller/ackermann_steering_controller.xml

@@ -0,0 +1,8 @@
+<library path="ackermann_steering_controller">
+  <class name="ackermann_steering_controller/AckermannSteeringController"
+         type="ackermann_steering_controller::AckermannSteeringController" base_class_type="controller_interface::ChainableControllerInterface">
+  <description>
+    Steering controller for Ackermann (car-like) kinematics with two traction and two steering joints.
+  </description>
+  </class>
+</library>

ackermann_steering_controller/doc/userdoc.rst

@@ -0,0 +1,18 @@
+.. _ackermann_steering_controller_userdoc:
+
+ackermann_steering_controller
+=============================
+
+This controller implements the kinematics with two axes and four wheels, where the wheels on one axis are fixed (traction/drive) wheels, and the wheels on the other axis are steerable.
+
+The controller expects to have two commanding joints for traction, one for each fixed wheel and two commanding joints for steering, one for each wheel.
+
+For more details on controller's execution and interfaces check the :ref:`Steering Controller Library <steering_controllers_library>`.
+
+
+Parameters
+,,,,,,,,,,,
+
+For list of parameters and their meaning YAML file in the ``src`` folder of the controller's package.
+
+For an exemplary parameterization see the ``test`` folder of the controller's package.

ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp

@@ -0,0 +1,65 @@
+// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt)
+//
+// 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.
+//
+// Authors: dr. sc. Tomislav Petkovic, Dr. Ing. Denis Štogl
+//
+
+#ifndef ACKERMANN_STEERING_CONTROLLER__ACKERMANN_STEERING_CONTROLLER_HPP_
+#define ACKERMANN_STEERING_CONTROLLER__ACKERMANN_STEERING_CONTROLLER_HPP_
+
+#include <memory>
+
+#include "ackermann_steering_controller/visibility_control.h"
+#include "ackermann_steering_controller_parameters.hpp"
+#include "steering_controllers_library/steering_controllers_library.hpp"
+
+namespace ackermann_steering_controller
+{
+// name constants for state interfaces
+static constexpr size_t STATE_TRACTION_RIGHT_WHEEL = 0;
+static constexpr size_t STATE_TRACTION_LEFT_WHEEL = 1;
+static constexpr size_t STATE_STEER_RIGHT_WHEEL = 2;
+static constexpr size_t STATE_STEER_LEFT_WHEEL = 3;
+
+// name constants for command interfaces
+static constexpr size_t CMD_TRACTION_RIGHT_WHEEL = 0;
+static constexpr size_t CMD_TRACTION_LEFT_WHEEL = 1;
+static constexpr size_t CMD_STEER_RIGHT_WHEEL = 2;
+static constexpr size_t CMD_STEER_LEFT_WHEEL = 3;
+
+static constexpr size_t NR_STATE_ITFS = 4;
+static constexpr size_t NR_CMD_ITFS = 4;
+static constexpr size_t NR_REF_ITFS = 2;
+
+class AckermannSteeringController : public steering_controllers_library::SteeringControllersLibrary
+{
+public:
+  AckermannSteeringController();
+
+  ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC controller_interface::CallbackReturn
+  configure_odometry() override;
+
+  ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC bool update_odometry(
+    const rclcpp::Duration & period) override;
+
+  ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC void
+  initialize_implementation_parameter_listener() override;
+
+protected:
+  std::shared_ptr<ackermann_steering_controller::ParamListener> ackermann_param_listener_;
+  ackermann_steering_controller::Params ackermann_params_;
+};
+}  // namespace ackermann_steering_controller
+
+#endif  // ACKERMANN_STEERING_CONTROLLER__ACKERMANN_STEERING_CONTROLLER_HPP_

ackermann_steering_controller/include/ackermann_steering_controller/visibility_control.h

@@ -0,0 +1,52 @@
+// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt)
+//
+// 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.
+
+#ifndef ACKERMANN_STEERING_CONTROLLER__VISIBILITY_CONTROL_H_
+#define ACKERMANN_STEERING_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 ACKERMANN_STEERING_CONTROLLER__VISIBILITY_EXPORT __attribute__((dllexport))
+#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_IMPORT __attribute__((dllimport))
+#else
+#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_EXPORT __declspec(dllexport)
+#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_IMPORT __declspec(dllimport)
+#endif
+#ifdef ACKERMANN_STEERING_CONTROLLER__VISIBILITY_BUILDING_DLL
+#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC \
+  ACKERMANN_STEERING_CONTROLLER__VISIBILITY_EXPORT
+#else
+#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC \
+  ACKERMANN_STEERING_CONTROLLER__VISIBILITY_IMPORT
+#endif
+#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC_TYPE \
+  ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC
+#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_LOCAL
+#else
+#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_EXPORT __attribute__((visibility("default")))
+#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_IMPORT
+#if __GNUC__ >= 4
+#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC __attribute__((visibility("default")))
+#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_LOCAL __attribute__((visibility("hidden")))
+#else
+#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC
+#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_LOCAL
+#endif
+#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC_TYPE
+#endif
+
+#endif  // ACKERMANN_STEERING_CONTROLLER__VISIBILITY_CONTROL_H_

ackermann_steering_controller/package.xml

@@ -0,0 +1,36 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>ackermann_steering_controller</name>
+  <version>0.0.0</version>
+  <description>Steering controller for Ackermann kinematics. Rear fixed wheels are powering the vehicle and front wheels are steering it.</description>
+  <license>Apache License 2.0</license>
+  <maintainer email="bence.magyar.robotics@gmail.com">Bence Magyar</maintainer>
+  <maintainer email="denis.stogl@stoglrobotics.de">Dr.-Ing. Denis Štogl</maintainer>
+  <maintainer email="tomislav.petkovic.fer@gmail.com">dr. sc. Tomislav Petkovic</maintainer>
+
+  <author email="denis.stogl@stoglrobotics.de">Dr.-Ing. Denis Štogl</author>
+  <author email="tomislav.petkovic.fer@gmail.com">dr. sc. Tomislav Petkovic</author>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <build_depend>generate_parameter_library</build_depend>
+
+  <depend>control_msgs</depend>
+  <depend>controller_interface</depend>
+  <depend>hardware_interface</depend>
+  <depend>pluginlib</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_lifecycle</depend>
+  <depend>std_srvs</depend>
+  <depend>steering_controllers_library</depend>
+
+  <test_depend>ament_cmake_gmock</test_depend>
+  <test_depend>controller_manager</test_depend>
+  <test_depend>hardware_interface</test_depend>
+  <test_depend>ros2_control_test_assets</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

ackermann_steering_controller/src/ackermann_steering_controller.cpp

@@ -0,0 +1,98 @@
+// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt)
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "ackermann_steering_controller/ackermann_steering_controller.hpp"
+
+namespace ackermann_steering_controller
+{
+AckermannSteeringController::AckermannSteeringController()
+: steering_controllers_library::SteeringControllersLibrary()
+{
+}
+
+void AckermannSteeringController::initialize_implementation_parameter_listener()
+{
+  ackermann_param_listener_ =
+    std::make_shared<ackermann_steering_controller::ParamListener>(get_node());
+}
+
+controller_interface::CallbackReturn AckermannSteeringController::configure_odometry()
+{
+  ackermann_params_ = ackermann_param_listener_->get_params();
+
+  const double front_wheels_radius = ackermann_params_.front_wheels_radius;
+  const double rear_wheels_radius = ackermann_params_.rear_wheels_radius;
+  const double front_wheel_track = ackermann_params_.front_wheel_track;
+  const double rear_wheel_track = ackermann_params_.rear_wheel_track;
+  const double wheelbase = ackermann_params_.wheelbase;
+
+  if (params_.front_steering)
+  {
+    odometry_.set_wheel_params(rear_wheels_radius, wheelbase, rear_wheel_track);
+  }
+  else
+  {
+    odometry_.set_wheel_params(front_wheels_radius, wheelbase, front_wheel_track);
+  }
+
+  odometry_.set_odometry_type(steering_odometry::ACKERMANN_CONFIG);
+
+  set_interface_numbers(NR_STATE_ITFS, NR_CMD_ITFS, NR_REF_ITFS);
+
+  RCLCPP_INFO(get_node()->get_logger(), "ackermann odom configure successful");
+  return controller_interface::CallbackReturn::SUCCESS;
+}
+
+bool AckermannSteeringController::update_odometry(const rclcpp::Duration & period)
+{
+  if (params_.open_loop)
+  {
+    odometry_.update_open_loop(last_linear_velocity_, last_angular_velocity_, period.seconds());
+  }
+  else
+  {
+    const double rear_right_wheel_value = state_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_value();
+    const double rear_left_wheel_value = state_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_value();
+    const double front_right_steer_position =
+      state_interfaces_[STATE_STEER_RIGHT_WHEEL].get_value();
+    const double front_left_steer_position = state_interfaces_[STATE_STEER_LEFT_WHEEL].get_value();
+    if (
+      !std::isnan(rear_right_wheel_value) && !std::isnan(rear_left_wheel_value) &&
+      !std::isnan(front_right_steer_position) && !std::isnan(front_left_steer_position))
+    {
+      if (params_.position_feedback)
+      {
+        // Estimate linear and angular velocity using joint information
+        odometry_.update_from_position(
+          rear_right_wheel_value, rear_left_wheel_value, front_right_steer_position,
+          front_left_steer_position, period.seconds());
+      }
+      else
+      {
+        // Estimate linear and angular velocity using joint information
+        odometry_.update_from_velocity(
+          rear_right_wheel_value, rear_left_wheel_value, front_right_steer_position,
+          front_left_steer_position, period.seconds());
+      }
+    }
+  }
+  return true;
+}
+}  // namespace ackermann_steering_controller
+
+#include "pluginlib/class_list_macros.hpp"
+
+PLUGINLIB_EXPORT_CLASS(
+  ackermann_steering_controller::AckermannSteeringController,
+  controller_interface::ChainableControllerInterface)