four_wheel_steering controller

four_wheel_steering_controller/CMakeLists.txt

@@ -0,0 +1,107 @@
+cmake_minimum_required(VERSION 3.16)
+project(four_wheel_steering_controller LANGUAGES CXX)
+
+if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)")
+  add_compile_options(-Wall -Wextra -Wpedantic -Wconversion)
+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(four_wheel_steering_controller_parameters
+  src/four_wheel_steering_controller.yaml
+)
+
+add_library(
+  four_wheel_steering_controller
+  SHARED
+  src/four_wheel_steering_controller.cpp
+)
+target_compile_features(four_wheel_steering_controller PUBLIC cxx_std_17)
+target_include_directories(four_wheel_steering_controller PUBLIC
+  $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include/${PROJECT_NAME}>)
+target_link_libraries(four_wheel_steering_controller PUBLIC
+  four_wheel_steering_controller_parameters)
+ament_target_dependencies(four_wheel_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(four_wheel_steering_controller PRIVATE "FOUR_WHEEL_STEERING_CONTROLLER_BUILDING_DLL")
+
+pluginlib_export_plugin_description_file(
+  controller_interface four_wheel_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)
+
+
+#   add_rostest_with_parameters_gmock(test_load_four_wheel_steering_controller
+#     test/test_load_four_wheel_steering_controller.cpp
+#     ${CMAKE_CURRENT_SOURCE_DIR}/test/four_wheel_steering_controller_params.yaml
+#   )
+#   ament_target_dependencies(test_load_four_wheel_steering_controller
+#     controller_manager
+#     hardware_interface
+#     ros2_control_test_assets
+#   )
+
+#   add_rostest_with_parameters_gmock(test_four_wheel_steering_controller
+#     test/test_four_wheel_steering_controller.cpp
+#     ${CMAKE_CURRENT_SOURCE_DIR}/test/four_wheel_steering_controller_params.yaml
+#   )
+#   target_include_directories(test_four_wheel_steering_controller PRIVATE include)
+#   target_link_libraries(test_four_wheel_steering_controller four_wheel_steering_controller)
+#   ament_target_dependencies(
+#     test_four_wheel_steering_controller
+#     controller_interface
+#     hardware_interface
+#   )
+
+#   add_rostest_with_parameters_gmock(
+#     test_four_wheel_steering_controller_preceeding test/test_four_wheel_steering_controller_preceeding.cpp
+#     ${CMAKE_CURRENT_SOURCE_DIR}/test/four_wheel_steering_controller_preceeding_params.yaml)
+#   target_include_directories(test_four_wheel_steering_controller_preceeding PRIVATE include)
+#   target_link_libraries(test_four_wheel_steering_controller_preceeding four_wheel_steering_controller)
+#   ament_target_dependencies(
+#     test_four_wheel_steering_controller_preceeding
+#     controller_interface
+#     hardware_interface
+#   )
+# endif()
+
+install(
+  DIRECTORY include/
+  DESTINATION include/four_wheel_steering_controller
+)
+
+install(
+  TARGETS four_wheel_steering_controller four_wheel_steering_controller_parameters
+  EXPORT export_four_wheel_steering_controller
+  RUNTIME DESTINATION bin
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+)
+
+ament_export_targets(export_four_wheel_steering_controller HAS_LIBRARY_TARGET)
+ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
+ament_package()

four_wheel_steering_controller/four_wheel_steering_controller.xml

@@ -0,0 +1,8 @@
+<library path="four_wheel_steering_controller">
+  <class name="four_wheel_steering_controller/FourWheelSteeringController"
+         type="four_wheel_steering_controller::FourWheelSteeringController" base_class_type="controller_interface::ChainableControllerInterface">
+  <description>
+    Steering controller for 4 WheelSteering robotic platform with four traction and four steering joints.
+  </description>
+  </class>
+</library>

four_wheel_steering_controller/include/four_wheel_steering_controller/four_wheel_steering_controller.hpp

@@ -0,0 +1,58 @@
+#ifndef FOUR_WHEEL_STEERING_CONTROLLER__FOUR_WHEEL_STEERING_CONTROLLER_HPP_
+#define FOUR_WHEEL_STEERING_CONTROLLER__FOUR_WHEEL_STEERING_CONTROLLER_HPP_
+
+#include <memory>
+
+#include "four_wheel_steering_controller/visibility_control.h"
+#include "four_wheel_steering_controller_parameters.hpp"
+#include "steering_controllers_library/steering_controllers_library.hpp"
+
+namespace four_wheel_steering_controller
+{
+// name constants for state interfaces
+static constexpr size_t STATE_TRACTION_FRONT_RIGHT_WHEEL = 0;
+static constexpr size_t STATE_TRACTION_FRONT_LEFT_WHEEL = 1;
+static constexpr size_t STATE_TRACTION_REAR_RIGHT_WHEEL = 2;
+static constexpr size_t STATE_TRACTION_REAR_LEFT_WHEEL = 3;
+
+static constexpr size_t STATE_STEER_FRONT_RIGHT_WHEEL = 4;
+static constexpr size_t STATE_STEER_FRONT_LEFT_WHEEL = 5;
+static constexpr size_t STATE_STEER_REAR_RIGHT_WHEEL = 6;
+static constexpr size_t STATE_STEER_REAR_LEFT_WHEEL = 7;
+
+// name constants for command interfaces
+static constexpr size_t CMD_TRACTION_FRONT_RIGHT_WHEEL = 0;
+static constexpr size_t CMD_TRACTION_FRONT_LEFT_WHEEL = 1;
+static constexpr size_t CMD_TRACTION_REAR_RIGHT_WHEEL = 2;
+static constexpr size_t CMD_TRACTION_REAR_LEFT_WHEEL = 3;
+
+static constexpr size_t CMD_STEER_FRONT_RIGHT_WHEEL = 4;
+static constexpr size_t CMD_STEER_FRONT_LEFT_WHEEL = 5;
+static constexpr size_t CMD_STEER_REAR_RIGHT_WHEEL = 6;
+static constexpr size_t CMD_STEER_REAR_LEFT_WHEEL = 7;
+
+static constexpr size_t NR_STATE_ITFS = 8;
+static constexpr size_t NR_CMD_ITFS = 8;
+static constexpr size_t NR_REF_ITFS = 2;
+
+class FourWheelSteeringController : public steering_controllers_library::SteeringControllersLibrary
+{
+public:
+  FourWheelSteeringController();
+
+  FOUR_WHEEL_STEERING_CONTROLLER__VISIBILITY_PUBLIC controller_interface::CallbackReturn
+  configure_odometry() override;
+
+  FOUR_WHEEL_STEERING_CONTROLLER__VISIBILITY_PUBLIC bool update_odometry(
+    const rclcpp::Duration & period) override;
+
+  FOUR_WHEEL_STEERING_CONTROLLER__VISIBILITY_PUBLIC void
+  initialize_implementation_parameter_listener() override;
+
+protected:
+  std::shared_ptr<four_wheel_steering_controller::ParamListener> four_wheel_steering_param_listener_;
+  four_wheel_steering_controller::Params four_wheel_steering_param_;
+};
+}  // namespace four_wheel_steering_controller
+
+#endif  // FOUR_WHEEL_STEERING_CONTROLLER__FOUR_WHEEL_STEERING_CONTROLLER_HPP_

four_wheel_steering_controller/include/four_wheel_steering_controller/visibility_control.h

@@ -0,0 +1,35 @@
+#ifndef FOUR_WHEEL_STEERING_CONTROLLER__VISIBILITY_CONTROL_H_
+#define FOUR_WHEEL_STEERING_CONTROLLER__VISIBILITY_CONTROL_H_
+
+#if defined _WIN32 || defined __CYGWIN__
+#ifdef __GNUC__
+#define FOUR_WHEEL_STEERING_CONTROLLER__VISIBILITY_EXPORT __attribute__((dllexport))
+#define FOUR_WHEEL_STEERING_CONTROLLER__VISIBILITY_IMPORT __attribute__((dllimport))
+#else
+#define FOUR_WHEEL_STEERING_CONTROLLER__VISIBILITY_EXPORT __declspec(dllexport)
+#define FOUR_WHEEL_STEERING_CONTROLLER__VISIBILITY_IMPORT __declspec(dllimport)
+#endif
+#ifdef FOUR_WHEEL_STEERING_CONTROLLER__VISIBILITY_BUILDING_DLL
+#define FOUR_WHEEL_STEERING_CONTROLLER__VISIBILITY_PUBLIC \
+  FOUR_WHEEL_STEERING_CONTROLLER__VISIBILITY_EXPORT
+#else
+#define FOUR_WHEEL_STEERING_CONTROLLER__VISIBILITY_PUBLIC \
+  FOUR_WHEEL_STEERING_CONTROLLER__VISIBILITY_IMPORT
+#endif
+#define FOUR_WHEEL_STEERING_CONTROLLER__VISIBILITY_PUBLIC_TYPE \
+  FOUR_WHEEL_STEERING_CONTROLLER__VISIBILITY_PUBLIC
+#define FOUR_WHEEL_STEERING_CONTROLLER__VISIBILITY_LOCAL
+#else
+#define FOUR_WHEEL_STEERING_CONTROLLER__VISIBILITY_EXPORT __attribute__((visibility("default")))
+#define FOUR_WHEEL_STEERING_CONTROLLER__VISIBILITY_IMPORT
+#if __GNUC__ >= 4
+#define FOUR_WHEEL_STEERING_CONTROLLER__VISIBILITY_PUBLIC __attribute__((visibility("default")))
+#define FOUR_WHEEL_STEERING_CONTROLLER__VISIBILITY_LOCAL __attribute__((visibility("hidden")))
+#else
+#define FOUR_WHEEL_STEERING_CONTROLLER__VISIBILITY_PUBLIC
+#define FOUR_WHEEL_STEERING_CONTROLLER__VISIBILITY_LOCAL
+#endif
+#define FOUR_WHEEL_STEERING_CONTROLLER__VISIBILITY_PUBLIC_TYPE
+#endif
+
+#endif  // FOUR_WHEEL_STEERING_CONTROLLER__VISIBILITY_CONTROL_H_

four_wheel_steering_controller/package.xml

@@ -0,0 +1,31 @@
+<?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>four_wheel_steering_controller</name>
+  <version>3.10.1</version>
+  <description>Steering controller for Four-Wheels kinematics. All wheels can steer and drive the robotic platform.</description>
+  <license>Apache License 2.0</license>
+  <maintainer email="[email protected]">Davide Graziato</maintainer>
+
+  <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>

four_wheel_steering_controller/src/four_wheel_steering_controller.cpp

@@ -0,0 +1,102 @@
+// 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 "four_wheel_steering_controller/four_wheel_steering_controller.hpp"
+
+namespace four_wheel_steering_controller
+{
+FourWheelSteeringController::FourWheelSteeringController()
+: steering_controllers_library::SteeringControllersLibrary()
+{
+}
+
+void FourWheelSteeringController::initialize_implementation_parameter_listener()
+{
+  four_wheel_steering_param_listener_ =
+    std::make_shared<four_wheel_steering_controller::ParamListener>(get_node());
+}
+
+controller_interface::CallbackReturn FourWheelSteeringController::configure_odometry()
+{
+  four_wheel_steering_param_ = four_wheel_steering_param_listener_->get_params();
+
+  const double wheel_radius = four_wheel_steering_param_.wheel_radius;
+  const double wheel_track = four_wheel_steering_param_.wheel_track;
+  const double wheelbase = four_wheel_steering_param_.wheelbase;
+  const double y_steering_offset = four_wheel_steering_param_.y_steering_offset;
+
+  odometry_.set_wheel_params(wheel_radius, wheelbase, wheel_track, y_steering_offset);
+
+  odometry_.set_odometry_type(steering_odometry::FOUR_STEERING_CONFIG);
+
+  set_interface_numbers(NR_STATE_ITFS, NR_CMD_ITFS, NR_REF_ITFS);
+
+  RCLCPP_INFO(get_node()->get_logger(), "four_wheel odom configure successful");
+  return controller_interface::CallbackReturn::SUCCESS;
+}
+
+bool FourWheelSteeringController::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 front_right_wheel_value = state_interfaces_[STATE_TRACTION_FRONT_RIGHT_WHEEL].get_value();
+    const double front_left_wheel_value = state_interfaces_[STATE_TRACTION_FRONT_LEFT_WHEEL].get_value();
+    const double rear_right_wheel_value = state_interfaces_[STATE_TRACTION_REAR_RIGHT_WHEEL].get_value();
+    const double rear_left_wheel_value = state_interfaces_[STATE_TRACTION_REAR_LEFT_WHEEL].get_value();
+
+    const double front_right_steer_position = state_interfaces_[STATE_STEER_FRONT_RIGHT_WHEEL].get_value();
+    const double front_left_steer_position = state_interfaces_[STATE_STEER_FRONT_LEFT_WHEEL].get_value();
+    const double rear_right_steer_position = state_interfaces_[STATE_STEER_REAR_RIGHT_WHEEL].get_value();
+    const double rear_left_steer_position = state_interfaces_[STATE_STEER_REAR_LEFT_WHEEL].get_value();
+
+    if (
+      !std::isnan(front_right_wheel_value) && !std::isnan(front_left_wheel_value) &&
+      !std::isnan(rear_right_wheel_value) && !std::isnan(rear_left_wheel_value) &&
+      !std::isnan(front_right_steer_position) && !std::isnan(front_left_steer_position) &&
+      !std::isnan(rear_right_steer_position) && !std::isnan(rear_left_steer_position))
+    {
+      if (params_.position_feedback)
+      {
+        double front_steer_position = 0.0;
+        if(fabs(front_right_steer_position) > 0.001 || fabs(front_left_steer_position) > 0.001)
+        {
+          front_steer_position = atan(2 * tan(front_right_steer_position) * tan(front_left_steer_position) /
+                                          (tan(front_right_steer_position) + tan(front_left_steer_position)));
+        }
+        double rear_steer_position = 0.0;
+        if(fabs(rear_right_steer_position) > 0.001 || fabs(rear_left_steer_position) > 0.001)
+        {
+          rear_steer_position = atan(2*tan(rear_right_steer_position)*tan(rear_left_steer_position)/
+                                        (tan(rear_right_steer_position) + tan(rear_left_steer_position)));
+        }
+        // Estimate linear and angular velocity using joint information
+        odometry_.update_four_steering(
+          front_right_wheel_value, front_left_wheel_value, rear_right_wheel_value,
+          rear_left_wheel_value, front_steer_position, rear_steer_position, period.seconds());
+      }
+    }
+    return true;
+  }
+}
+}  // namespace four_wheel_steering_controller
+
+#include "pluginlib/class_list_macros.hpp"
+
+PLUGINLIB_EXPORT_CLASS(
+  four_wheel_steering_controller::FourWheelSteeringController,
+  controller_interface::ChainableControllerInterface)

four_wheel_steering_controller/src/four_wheel_steering_controller.yaml

@@ -0,0 +1,29 @@
+four_wheel_steering_controller:
+  wheel_radius:
+    {
+      type: double,
+      default_value: 0.0,
+      description: "Wheel radius.",
+      read_only: false,
+    }
+  wheel_track:
+    {
+      type: double,
+      default_value: 0.0,
+      description: "Wheels track length.",
+      read_only: false,
+    }
+  wheelbase:
+    {
+      type: double,
+      default_value: 0.0,
+      description: "Distance between front and rear wheels.",
+      read_only: false,
+    }
+  y_steering_offset:
+    {
+      type: double,
+      default_value: 0.0,
+      description: "Offest between the steering and wheel joints.",
+      read_only: false,
+    }

steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp

@@ -36,6 +36,7 @@
 
 // TODO(anyone): Replace with controller specific messages
 #include "ackermann_msgs/msg/ackermann_drive_stamped.hpp"
+//#include "four_wheel_steering_msgs/msg/four_wheel_steering.hpp"
 #include "control_msgs/msg/steering_controller_status.hpp"
 #include "geometry_msgs/msg/twist.hpp"
 #include "geometry_msgs/msg/twist_stamped.hpp"
@@ -138,6 +139,8 @@ class SteeringControllersLibrary : public controller_interface::ChainableControl
 
   std::vector<std::string> rear_wheels_state_names_;
   std::vector<std::string> front_wheels_state_names_;
+  std::vector<std::string> wheels_names_;
+  std::vector<std::string> steers_names_;
 
 private:
   // callback for topic interface