diff --git a/doc/migration.rst b/doc/migration.rst index d96f0f6110..83ce653d22 100644 --- a/doc/migration.rst +++ b/doc/migration.rst @@ -41,3 +41,36 @@ omni_wheel_drive_controller ***************************** * Instead of using ``tf_frame_prefix_enable:=false``, set an empty ``tf_frame_prefix:=""`` parameter instead. (`#2073 `_). * For using node namespace as tf prefix: Set ``tf_frame_prefix:="~"``, where the ("~") character is substituted with node namespace. (`#2073 `_). + +tricycle_steering_controller +***************************** +* Use tricycle_steering_controller instead of tricycle_controller for single-drive tricycle steering (`#1695 `_). + Necessary changes can be found `here `_ and are summarized in the following: + + * ROS parameters + + .. code:: yaml + + # parameters to be changed from tricycle_controller + traction_joint_name: traction_joint + steering_joint_name: steering_joint + wheel_radius: 0.3 + cmd_vel_timeout: 500 + + # to tricycle_steering_controller + traction_joints_names: ["traction_joint"] + steering_joints_names: ["steering_joint"] + traction_wheels_radius: 0.3 + reference_timeout: 0.5 # In seconds. + reduce_wheel_speed_until_steering_reached: true # is default false + + # remove parameters + odom_only_twist: false + publish_ackermann_command: true + + * ROS command topic changed from ``/tricycle_controller/cmd_vel`` to ``/tricycle_controller/reference``. + * Limiters: While tricycle_controller had limiters of the command interfaces does tricycle_steering_controller instead have limiters for the reference velocity. Update the URDF to enforce command limits from the ressource_manager, and add ``limits.angular.z`` and ``limits.linear.x`` if necessary. + +tricycle_controller +***************************** +See tricycle_steering_controller above. diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 8a6174108e..996eb81b40 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -57,7 +57,16 @@ steering_controllers_library ***************************** * Parameter ``tf_frame_prefix`` added with the similar functionality to other controllers. (`#2080 `_). * Set odometry service added to be used at runtime. (`#2244 `_). +* Kinematics for single-drive tricycle added. (`#1695 `_). magnetometer_broadcaster ************************ New package to broadcast ``sensor_msgs/msg/MagneticField`` from state interfaces defined by the ``semantic_components::MagneticFieldSensor``. + +tricycle_controller +***************************** +* Deprecated in favor of the new tricycle_steering_controller. (`#1695 `_). + +tricycle_steering_controller +***************************** +* Added drop-in replacement for tricycle_controller (single-drive tricycle). (`#1695 `_). diff --git a/steering_controllers_library/doc/userdoc.rst b/steering_controllers_library/doc/userdoc.rst index 1c9f6c0cb1..503e1f42ea 100644 --- a/steering_controllers_library/doc/userdoc.rst +++ b/steering_controllers_library/doc/userdoc.rst @@ -38,7 +38,7 @@ Currently implemented kinematics -------------------------------------------------------------- * Bicycle - with one steering and one drive joints; -* Tricycle - with one steering and two drive joints; +* Tricycle - with one steering and one or two drive joints; * Ackermann - with two steering and two drive joints. .. toctree:: diff --git a/steering_controllers_library/include/steering_controllers_library/steering_kinematics.hpp b/steering_controllers_library/include/steering_controllers_library/steering_kinematics.hpp index 983effae85..4bdba977be 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_kinematics.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_kinematics.hpp @@ -98,6 +98,9 @@ class SteeringKinematics /** * \brief Updates the SteeringKinematics class with latest wheels position + * + * for bicycle or tricycle config with single traction + * * \param traction_wheel_vel Traction wheel velocity [rad/s] * \param steer_pos Steer wheel position [rad] * \param dt time difference to last call @@ -108,6 +111,9 @@ class SteeringKinematics /** * \brief Updates the SteeringKinematics class with latest wheels position + * + * for tricycle config with dual traction + * * \param right_traction_wheel_vel Right traction wheel velocity [rad/s] * \param left_traction_wheel_vel Left traction wheel velocity [rad/s] * \param steer_pos Steer wheel position [rad] @@ -120,6 +126,9 @@ class SteeringKinematics /** * \brief Updates the SteeringKinematics class with latest wheels position + * + * for ackermann config + * * \param right_traction_wheel_vel Right traction wheel velocity [rad/s] * \param left_traction_wheel_vel Left traction wheel velocity [rad/s] * \param right_steer_pos Right steer wheel position [rad] @@ -220,6 +229,20 @@ class SteeringKinematics * \param heading heading [rad] */ void set_odometry(const double & x, const double & y, const double & heading); + /** + * \brief Set tricycle config + * \param nr_traction_wheels number of traction wheels + */ + void set_tricycle_nr_traction_wheels(const size_t nr_traction_wheels) + { + tricycle_nr_traction_wheels_ = nr_traction_wheels; + } + + /** + * \brief Get tricycle config + * \return number of traction wheels + */ + size_t get_tricycle_nr_traction_wheels() const { return tricycle_nr_traction_wheels_; } private: /** @@ -296,6 +319,7 @@ class SteeringKinematics /// Configuration type used for the forward kinematics int config_type_ = -1; + size_t tricycle_nr_traction_wheels_ = 1; /// Previous wheel position/state [rad]: double traction_wheel_old_pos_; diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index 32997db6c1..b374613074 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -99,13 +99,23 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure( } else if (odometry_.get_odometry_type() == steering_kinematics::TRICYCLE_CONFIG) { - if (params_.traction_joints_names.size() != 2) + switch (params_.traction_joints_names.size()) { - RCLCPP_ERROR( - get_node()->get_logger(), - "Tricycle configuration requires exactly two traction joints, but %zu were provided", - params_.traction_joints_names.size()); - return controller_interface::CallbackReturn::ERROR; + case 1: + odometry_.set_tricycle_nr_traction_wheels(1); + set_interface_numbers(2, 2); + break; + case 2: + odometry_.set_tricycle_nr_traction_wheels(2); + set_interface_numbers(3, 3); + break; + default: + RCLCPP_ERROR( + get_node()->get_logger(), + "Tricycle configuration requires one (for single-drive) or two (for dual-drive) traction " + "joints, but %zu were provided", + params_.traction_joints_names.size()); + return controller_interface::CallbackReturn::ERROR; } } else if (odometry_.get_odometry_type() == steering_kinematics::ACKERMANN_CONFIG) @@ -173,13 +183,14 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure( } else if (odometry_.get_odometry_type() == steering_kinematics::TRICYCLE_CONFIG) { - if (params_.traction_joints_state_names.size() != 2) + if (params_.traction_joints_state_names.size() != odometry_.get_tricycle_nr_traction_wheels()) { RCLCPP_ERROR( get_node()->get_logger(), - "Tricycle configuration requires exactly two traction joints, but %zu state interface " + "Current tricycle configuration requires exactly %zu traction joints, but %zu state " + "interface " "names were provided", - params_.traction_joints_state_names.size()); + odometry_.get_tricycle_nr_traction_wheels(), params_.traction_joints_state_names.size()); return controller_interface::CallbackReturn::ERROR; } } diff --git a/steering_controllers_library/src/steering_kinematics.cpp b/steering_controllers_library/src/steering_kinematics.cpp index d8a160035e..e6ce8147f5 100644 --- a/steering_controllers_library/src/steering_kinematics.cpp +++ b/steering_controllers_library/src/steering_kinematics.cpp @@ -126,6 +126,10 @@ bool SteeringKinematics::update_from_velocity( { steer_pos_ = steer_pos; double linear_velocity = traction_wheel_vel * wheel_radius_; + if (config_type_ == TRICYCLE_CONFIG && get_tricycle_nr_traction_wheels() == 1) + { + linear_velocity *= std::cos(steer_pos); + } const double angular_velocity = std::tan(steer_pos) * linear_velocity / wheel_base_; return update_odometry(linear_velocity, angular_velocity, dt); @@ -239,21 +243,18 @@ std::tuple, std::vector> SteeringKinematics::get_com // desired wheel speed and steering angle of the middle of traction and steering axis double Ws, phi, phi_IK = steer_pos_; -#if 0 - if (v_bx == 0 && omega_bz != 0) + // calculate steering angle + if ( + config_type_ == TRICYCLE_CONFIG && get_tricycle_nr_traction_wheels() == 1 && + is_close_to_zero(v_bx) && !is_close_to_zero(omega_bz)) { - // TODO(anyone) this would be only possible if traction is on the steering axis + // turning on the spot phi = omega_bz > 0 ? M_PI_2 : -M_PI_2; - Ws = abs(omega_bz) * wheelbase_ / wheel_radius_; } else { - // TODO(anyone) this would be valid only if traction is on the steering axis - Ws = v_bx / (wheel_radius_ * std::cos(phi_IK)); // using the measured steering angle + phi = SteeringKinematics::convert_twist_to_steering_angle(v_bx, omega_bz); } -#endif - // steering angle - phi = SteeringKinematics::convert_twist_to_steering_angle(v_bx, omega_bz); if (open_loop) { phi_IK = phi; @@ -290,18 +291,40 @@ std::tuple, std::vector> SteeringKinematics::get_com { std::vector traction_commands; std::vector steering_commands; - // double-traction axle - if (is_close_to_zero(phi_IK)) + + if (get_tricycle_nr_traction_wheels() == 1) { - // avoid division by zero - traction_commands = {Ws, Ws}; + // if traction is on the steering axis + if (is_close_to_zero(v_bx) && !is_close_to_zero(omega_bz)) + { + Ws = abs(omega_bz) * wheel_base_ / wheel_radius_; + } + else + { + Ws /= std::cos(phi_IK); + } + traction_commands = {Ws}; } else { - const double turning_radius = wheel_base_ / std::tan(phi_IK); - const double Wr = Ws * (turning_radius + wheel_track_traction_ * 0.5) / turning_radius; - const double Wl = Ws * (turning_radius - wheel_track_traction_ * 0.5) / turning_radius; - traction_commands = {Wr, Wl}; + // double-traction axle + if (is_close_to_zero(wheel_track_traction_)) + { + throw std::runtime_error( + "Tricycle configuration with double-traction axle requires non-zero wheel track"); + } + if (is_close_to_zero(phi_IK)) + { + // avoid division by zero + traction_commands = {Ws, Ws}; + } + else + { + const double turning_radius = wheel_base_ / std::tan(phi_IK); + const double Wr = Ws * (turning_radius + wheel_track_traction_ * 0.5) / turning_radius; + const double Wl = Ws * (turning_radius - wheel_track_traction_ * 0.5) / turning_radius; + traction_commands = {Wr, Wl}; + } } // simple steering steering_commands = {phi}; diff --git a/steering_controllers_library/test/test_steering_controllers_library.cpp b/steering_controllers_library/test/test_steering_controllers_library.cpp index 75e1087dcd..9b02c28172 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.cpp +++ b/steering_controllers_library/test/test_steering_controllers_library.cpp @@ -21,7 +21,7 @@ #include "test_steering_controllers_library.hpp" class SteeringControllersLibraryTest -: public SteeringControllersLibraryFixture +: public SteeringControllersLibraryFixture { }; diff --git a/steering_controllers_library/test/test_steering_controllers_library.hpp b/steering_controllers_library/test/test_steering_controllers_library.hpp index 982dceeab2..2c683982b1 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.hpp +++ b/steering_controllers_library/test/test_steering_controllers_library.hpp @@ -63,7 +63,7 @@ static constexpr double WHEELS_TRACK_ = 2.12321; static constexpr double WHEELS_RADIUS_ = 0.45; // subclassing and friending so we can access member variables -class TestableSteeringControllersLibrary +class TestableAckermannSteeringController : public steering_controllers_library::SteeringControllersLibrary { FRIEND_TEST(SteeringControllersLibraryTest, check_exported_interfaces); diff --git a/steering_controllers_library/test/test_steering_odometry.cpp b/steering_controllers_library/test/test_steering_odometry.cpp index fafff62387..895e2d75b6 100644 --- a/steering_controllers_library/test/test_steering_odometry.cpp +++ b/steering_controllers_library/test/test_steering_odometry.cpp @@ -283,80 +283,147 @@ TEST(TestSteeringOdometry, bicycle_odometry) // ----------------- tricycle ----------------- -TEST(TestSteeringOdometry, tricycle_IK_linear) +TEST(TestSteeringOdometry, tricycle_dual_IK_linear) { steering_kinematics::SteeringKinematics odom(1); odom.set_wheel_params(1., 2., 1.); odom.set_odometry_type(steering_kinematics::TRICYCLE_CONFIG); + odom.set_tricycle_nr_traction_wheels(2); odom.update_open_loop(1., 0., 1.); auto cmd = odom.get_commands(1., 0., true); auto cmd0 = std::get<0>(cmd); // vel - EXPECT_EQ(cmd0[0], cmd0[1]); // linear + ASSERT_THAT(cmd0.size(), 2); + EXPECT_EQ(cmd0[0], cmd0[1]); // linear + EXPECT_GT(cmd0[0], 0.0); + auto cmd1 = std::get<1>(cmd); // steer + ASSERT_THAT(cmd1.size(), 1); + EXPECT_EQ(cmd1[0], 0.0); // no steering +} + +TEST(TestSteeringOdometry, tricycle_single_IK_linear) +{ + steering_kinematics::SteeringKinematics odom(1); + odom.set_wheel_params(1., 2., 1.); + odom.set_odometry_type(steering_kinematics::TRICYCLE_CONFIG); + odom.set_tricycle_nr_traction_wheels(1); + + odom.update_open_loop(1., 0., 1.); + auto cmd = odom.get_commands(1., 0., true); + auto cmd0 = std::get<0>(cmd); // vel + ASSERT_THAT(cmd0.size(), 1); + EXPECT_GT(cmd0[0], 0.0); // linear + auto cmd1 = std::get<1>(cmd); // steer + ASSERT_THAT(cmd1.size(), 1); + EXPECT_EQ(cmd1[0], 0.0); // no steering +} + +TEST(TestSteeringOdometry, tricycle_dual_IK_left) +{ + steering_kinematics::SteeringKinematics odom(1); + odom.set_wheel_params(1., 2., 1.); + odom.set_odometry_type(steering_kinematics::TRICYCLE_CONFIG); + odom.set_tricycle_nr_traction_wheels(2); + odom.update_from_position(0., 0.2, 1.); // assume already turn + auto cmd = odom.get_commands(1., 0.1, false); + auto cmd0 = std::get<0>(cmd); // vel + ASSERT_THAT(cmd0.size(), 2); + EXPECT_GT(cmd0[0], cmd0[1]); // right (outer) > left (inner) + EXPECT_GT(cmd0[0], 0); + auto cmd1 = std::get<1>(cmd); // steer + ASSERT_THAT(cmd1.size(), 1); + EXPECT_GT(cmd1[0], 0); // left steering +} + +TEST(TestSteeringOdometry, tricycle_dual_IK_right) +{ + steering_kinematics::SteeringKinematics odom(1); + odom.set_wheel_params(1., 2., 1.); + odom.set_odometry_type(steering_kinematics::TRICYCLE_CONFIG); + odom.set_tricycle_nr_traction_wheels(2); + odom.update_from_position(0., -0.2, 1.); // assume already turn + auto cmd = odom.get_commands(1., -0.1, false); + auto cmd0 = std::get<0>(cmd); // vel + ASSERT_THAT(cmd0.size(), 2); + EXPECT_LT(cmd0[0], cmd0[1]); // right (inner) < left (outer) EXPECT_GT(cmd0[0], 0); auto cmd1 = std::get<1>(cmd); // steer - EXPECT_EQ(cmd1[0], 0); // no steering + ASSERT_THAT(cmd1.size(), 1); + EXPECT_LT(cmd1[0], 0); // right steering } -TEST(TestSteeringOdometry, tricycle_IK_left) +TEST(TestSteeringOdometry, tricycle_single_IK_left) { steering_kinematics::SteeringKinematics odom(1); odom.set_wheel_params(1., 2., 1.); odom.set_odometry_type(steering_kinematics::TRICYCLE_CONFIG); + odom.set_tricycle_nr_traction_wheels(1); odom.update_from_position(0., 0.2, 1.); // assume already turn auto cmd = odom.get_commands(1., 0.1, false); auto cmd0 = std::get<0>(cmd); // vel - EXPECT_GT(cmd0[0], cmd0[1]); // right (outer) > left (inner) + ASSERT_THAT(cmd0.size(), 1); EXPECT_GT(cmd0[0], 0); auto cmd1 = std::get<1>(cmd); // steer - EXPECT_GT(cmd1[0], 0); // left steering + ASSERT_THAT(cmd1.size(), 1); + EXPECT_GT(cmd1[0], 0); // left steering } -TEST(TestSteeringOdometry, tricycle_IK_right) +TEST(TestSteeringOdometry, tricycle_single_IK_right) { steering_kinematics::SteeringKinematics odom(1); odom.set_wheel_params(1., 2., 1.); odom.set_odometry_type(steering_kinematics::TRICYCLE_CONFIG); + odom.set_tricycle_nr_traction_wheels(1); odom.update_from_position(0., -0.2, 1.); // assume already turn auto cmd = odom.get_commands(1., -0.1, false); auto cmd0 = std::get<0>(cmd); // vel - EXPECT_LT(cmd0[0], cmd0[1]); // right (inner) < left (outer) + ASSERT_THAT(cmd0.size(), 1); EXPECT_GT(cmd0[0], 0); auto cmd1 = std::get<1>(cmd); // steer - EXPECT_LT(cmd1[0], 0); // right steering + ASSERT_THAT(cmd1.size(), 1); + EXPECT_LT(cmd1[0], 0); // right steering } -TEST(TestSteeringOdometry, tricycle_IK_right_steering_limited) +TEST(TestSteeringOdometry, tricycle_dual_IK_right_steering_limited) { steering_kinematics::SteeringKinematics odom(1); odom.set_wheel_params(1., 2., 1.); odom.set_odometry_type(steering_kinematics::TRICYCLE_CONFIG); + odom.set_tricycle_nr_traction_wheels(2); + // already steered, reduce_wheel_speed_until_steering_reached = true { odom.update_from_position(0., -0.785, 1.); // already steered auto cmd = odom.get_commands(1., -0.5, false, true); - auto vel_cmd_steered = std::get<0>(cmd); // vel + auto vel_cmd_steered = std::get<0>(cmd); // vel + ASSERT_THAT(vel_cmd_steered.size(), 2); EXPECT_LT(vel_cmd_steered[0], vel_cmd_steered[1]); // right (inner) < left (outer) EXPECT_GT(vel_cmd_steered[0], 0); auto cmd1 = std::get<1>(cmd); // steer + ASSERT_THAT(cmd1.size(), 1); EXPECT_LT(cmd1[0], 0); } + // not fully steered, reduce_wheel_speed_until_steering_reached = false std::vector vel_cmd_not_steered; { odom.update_from_position(0., -0.1, 1.); // not fully steered auto cmd = odom.get_commands(1., -0.5, false, false); - vel_cmd_not_steered = std::get<0>(cmd); // vel + vel_cmd_not_steered = std::get<0>(cmd); // vel + ASSERT_THAT(vel_cmd_not_steered.size(), 2); EXPECT_LT(vel_cmd_not_steered[0], vel_cmd_not_steered[1]); // right (inner) < left (outer) EXPECT_GT(vel_cmd_not_steered[0], 0); auto cmd1 = std::get<1>(cmd); // steer + ASSERT_THAT(cmd1.size(), 1); EXPECT_LT(cmd1[0], 0); } + // not fully steered, reduce_wheel_speed_until_steering_reached = true { odom.update_from_position(0., -0.1, 1.); // not fully steered auto cmd = odom.get_commands(1., -0.5, false, true); auto cmd0 = std::get<0>(cmd); // vel - EXPECT_LT(cmd0[0], cmd0[1]); // right (inner) < left (outer) + ASSERT_THAT(cmd0.size(), 2); + EXPECT_LT(cmd0[0], cmd0[1]); // right (inner) < left (outer) EXPECT_GT(cmd0[0], 0); // vel should be less than vel_cmd_not_steered now for (size_t i = 0; i < cmd0.size(); ++i) @@ -364,18 +431,80 @@ TEST(TestSteeringOdometry, tricycle_IK_right_steering_limited) EXPECT_LT(cmd0[i], vel_cmd_not_steered[i]); } auto cmd1 = std::get<1>(cmd); // steer + ASSERT_THAT(cmd1.size(), 1); EXPECT_LT(cmd1[0], 0); } } -TEST(TestSteeringOdometry, tricycle_odometry) +TEST(TestSteeringOdometry, tricycle_single_IK_right_steering_limited) +{ + steering_kinematics::SteeringKinematics odom(1); + odom.set_wheel_params(1., 2., 1.); + odom.set_odometry_type(steering_kinematics::TRICYCLE_CONFIG); + odom.set_tricycle_nr_traction_wheels(1); + + // already steered, reduce_wheel_speed_until_steering_reached = true + { + odom.update_from_position(0., -0.785, 1.); // already steered + auto cmd = odom.get_commands(1., -0.5, false, true); + auto vel_cmd_steered = std::get<0>(cmd); // vel + ASSERT_THAT(vel_cmd_steered.size(), 1); + EXPECT_GT(vel_cmd_steered[0], 0); + auto cmd1 = std::get<1>(cmd); // steer + ASSERT_THAT(cmd1.size(), 1); + EXPECT_LT(cmd1[0], 0); + } + + // not fully steered, reduce_wheel_speed_until_steering_reached = false + std::vector vel_cmd_not_steered; + { + odom.update_from_position(0., -0.1, 1.); // not fully steered + auto cmd = odom.get_commands(1., -0.5, false, false); + vel_cmd_not_steered = std::get<0>(cmd); // vel + ASSERT_THAT(vel_cmd_not_steered.size(), 1); + EXPECT_GT(vel_cmd_not_steered[0], 0); + auto cmd1 = std::get<1>(cmd); // steer + ASSERT_THAT(cmd1.size(), 1); + EXPECT_LT(cmd1[0], 0); + } + + // not fully steered, reduce_wheel_speed_until_steering_reached = true + { + odom.update_from_position(0., -0.1, 1.); // not fully steered + auto cmd = odom.get_commands(1., -0.5, false, true); + auto cmd0 = std::get<0>(cmd); // vel + ASSERT_THAT(cmd0.size(), 1); + EXPECT_GT(cmd0[0], 0); + // vel should be less than vel_cmd_not_steered now + EXPECT_LT(cmd0[0], vel_cmd_not_steered[0]); + auto cmd1 = std::get<1>(cmd); // steer + ASSERT_THAT(cmd1.size(), 1); + EXPECT_LT(cmd1[0], 0); + } +} + +TEST(TestSteeringOdometry, tricycle_dual_odometry) { steering_kinematics::SteeringKinematics odom(1); odom.set_wheel_params(1., 1., 1.); odom.set_odometry_type(steering_kinematics::TRICYCLE_CONFIG); + odom.set_tricycle_nr_traction_wheels(2); ASSERT_TRUE(odom.update_from_velocity(1., 1., .1, .1)); EXPECT_NEAR(odom.get_linear(), 1.002, 1e-3); EXPECT_NEAR(odom.get_angular(), .1, 1e-3); EXPECT_NEAR(odom.get_x(), .1, 1e-3); EXPECT_NEAR(odom.get_heading(), .01, 1e-3); } + +TEST(TestSteeringOdometry, tricycle_single_odometry) +{ + steering_kinematics::SteeringKinematics odom(1); + odom.set_wheel_params(1., 1., 1.); + odom.set_odometry_type(steering_kinematics::TRICYCLE_CONFIG); + odom.set_tricycle_nr_traction_wheels(1); + ASSERT_TRUE(odom.update_from_velocity(1., .1, .1)); + EXPECT_NEAR(odom.get_linear(), 0.995, 1e-3); + EXPECT_NEAR(odom.get_angular(), .1, 1e-3); + EXPECT_NEAR(odom.get_x(), .1, 1e-3); + EXPECT_NEAR(odom.get_heading(), .01, 1e-3); +} diff --git a/tricycle_controller/doc/userdoc.rst b/tricycle_controller/doc/userdoc.rst index 2ede52e1b6..9f50b6ee75 100644 --- a/tricycle_controller/doc/userdoc.rst +++ b/tricycle_controller/doc/userdoc.rst @@ -5,6 +5,10 @@ tricycle_controller ===================== +.. note:: + + This controller is deprecated and will be removed in a future release. Please use the :ref:`tricycle_steering_controller_userdoc` instead. + Controller for mobile robots with a single double-actuated wheel, including traction and steering. An example is a tricycle robot with the actuated wheel in the front and two trailing wheels on the rear axle. Input for control are robot base_link twist commands which are translated to traction and steering diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index 3455453f56..35ca6a34f3 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -46,5 +46,9 @@ ament_cmake + + This package will be removed in ROS 2 Lyrical Luth, use tricycle_steering_controller instead. + For migration guides see https://control.ros.org/lyrical/doc/ros2_controllers/doc/migration.html + diff --git a/tricycle_controller/src/tricycle_controller.cpp b/tricycle_controller/src/tricycle_controller.cpp index c61d781919..618b40f03e 100644 --- a/tricycle_controller/src/tricycle_controller.cpp +++ b/tricycle_controller/src/tricycle_controller.cpp @@ -53,6 +53,9 @@ TricycleController::TricycleController() : controller_interface::ControllerInter CallbackReturn TricycleController::on_init() { + RCLCPP_WARN( + get_node()->get_logger(), + "[Deprecated]: the `TricycleController` is replaced by 'TricycleSteeringController'"); try { // Create the parameter listener and get the parameters diff --git a/tricycle_steering_controller/CMakeLists.txt b/tricycle_steering_controller/CMakeLists.txt index 65cec48ed6..ce47e67abb 100644 --- a/tricycle_steering_controller/CMakeLists.txt +++ b/tricycle_steering_controller/CMakeLists.txt @@ -67,10 +67,18 @@ if(BUILD_TESTING) ) add_rostest_with_parameters_gmock( - test_tricycle_steering_controller test/test_tricycle_steering_controller.cpp + test_tricycle_steering_controller_dual_traction + test/test_tricycle_steering_controller_dual_traction.cpp ${CMAKE_CURRENT_SOURCE_DIR}/test/tricycle_steering_controller_params.yaml) - target_include_directories(test_tricycle_steering_controller PRIVATE include) - target_link_libraries(test_tricycle_steering_controller tricycle_steering_controller) + target_include_directories(test_tricycle_steering_controller_dual_traction PRIVATE include) + target_link_libraries(test_tricycle_steering_controller_dual_traction tricycle_steering_controller) + + add_rostest_with_parameters_gmock( + test_tricycle_steering_controller_single_traction + test/test_tricycle_steering_controller_single_traction.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/tricycle_steering_controller_params.yaml) + target_include_directories(test_tricycle_steering_controller_single_traction PRIVATE include) + target_link_libraries(test_tricycle_steering_controller_single_traction tricycle_steering_controller) add_rostest_with_parameters_gmock( test_tricycle_steering_controller_preceding test/test_tricycle_steering_controller_preceding.cpp diff --git a/tricycle_steering_controller/doc/userdoc.rst b/tricycle_steering_controller/doc/userdoc.rst index 618d7f04a7..2023d51945 100644 --- a/tricycle_steering_controller/doc/userdoc.rst +++ b/tricycle_steering_controller/doc/userdoc.rst @@ -5,9 +5,12 @@ tricycle_steering_controller ============================= -This controller implements the kinematics with two axes and three wheels, where two wheels on an axis are fixed (traction/drive), and the wheel on the other axis is steerable. +This controller implements the kinematics with two axes and three wheels. + +Two possible configurations are supported: +1. Dual-Traction: Two wheels on an axis are fixed (traction/drive), and the wheel on the other axis is steerable. The controller expects to have two commanding joints for traction, one for each fixed wheel and one commanding joint for steering. +2. Single-Traction: A single traction wheel, which is also steerable. The controller expects a single command joint for traction and one commanding joint for steering. -The controller expects to have two commanding joints for traction, one for each fixed wheel and one commanding joint for steering. For more details on controller's execution and interfaces check the :ref:`Steering Controller Library `. diff --git a/tricycle_steering_controller/include/tricycle_steering_controller/tricycle_steering_controller.hpp b/tricycle_steering_controller/include/tricycle_steering_controller/tricycle_steering_controller.hpp index acc23034d2..275551467d 100644 --- a/tricycle_steering_controller/include/tricycle_steering_controller/tricycle_steering_controller.hpp +++ b/tricycle_steering_controller/include/tricycle_steering_controller/tricycle_steering_controller.hpp @@ -28,16 +28,20 @@ namespace tricycle_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_AXIS = 2; +static constexpr size_t STATE_DUAL_TRACTION_STEER_AXIS = 2; // 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_WHEEL = 2; +static constexpr size_t CMD_DUAL_TRACTION_STEER_WHEEL = 2; -static constexpr size_t NR_STATE_ITFS = 3; -static constexpr size_t NR_CMD_ITFS = 3; -static constexpr size_t NR_REF_ITFS = 2; +// name constants for state interfaces +static constexpr size_t STATE_TRACTION_SINGLE_WHEEL = 0; +static constexpr size_t STATE_SINGLE_TRACTION_STEER_AXIS = 1; + +// name constants for command interfaces +static constexpr size_t CMD_TRACTION_SINGLE_WHEEL = 0; +static constexpr size_t CMD_SINGLE_TRACTION_STEER_WHEEL = 1; class TricycleSteeringController : public steering_controllers_library::SteeringControllersLibrary { diff --git a/tricycle_steering_controller/src/tricycle_steering_controller.cpp b/tricycle_steering_controller/src/tricycle_steering_controller.cpp index d799ac4e5e..f27b9c6b35 100644 --- a/tricycle_steering_controller/src/tricycle_steering_controller.cpp +++ b/tricycle_steering_controller/src/tricycle_steering_controller.cpp @@ -37,8 +37,6 @@ controller_interface::CallbackReturn TricycleSteeringController::configure_odome odometry_.set_wheel_params(traction_wheels_radius, wheelbase, traction_track_width); odometry_.set_odometry_type(steering_kinematics::TRICYCLE_CONFIG); - set_interface_numbers(NR_STATE_ITFS, NR_CMD_ITFS, NR_REF_ITFS); - RCLCPP_INFO(get_node()->get_logger(), "tricycle odom configure successful"); return controller_interface::CallbackReturn::SUCCESS; } @@ -51,41 +49,75 @@ bool TricycleSteeringController::update_odometry(const rclcpp::Duration & period } else { - const auto traction_right_wheel_value_op = - state_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_optional(); - const auto traction_left_wheel_value_op = - state_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_optional(); - const auto steering_position_op = state_interfaces_[STATE_STEER_AXIS].get_optional(); - - if ( - !traction_right_wheel_value_op.has_value() || !traction_left_wheel_value_op.has_value() || - !steering_position_op.has_value()) + if (odometry_.get_tricycle_nr_traction_wheels() == 2) { - RCLCPP_WARN( - get_node()->get_logger(), - "Unable to retrieve the data from right wheel or left wheel or steering position"); - return true; + const auto traction_right_wheel_value_op = + state_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_optional(); + const auto traction_left_wheel_value_op = + state_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_optional(); + const auto steering_position_op = + state_interfaces_[STATE_DUAL_TRACTION_STEER_AXIS].get_optional(); + + if ( + !traction_right_wheel_value_op.has_value() || !traction_left_wheel_value_op.has_value() || + !steering_position_op.has_value()) + { + RCLCPP_WARN( + get_node()->get_logger(), + "Unable to retrieve the data from right wheel or left wheel or steering position"); + return true; + } + const double traction_right_wheel_value = traction_right_wheel_value_op.value(); + const double traction_left_wheel_value = traction_left_wheel_value_op.value(); + const double steering_position = steering_position_op.value(); + if ( + std::isfinite(traction_right_wheel_value) && std::isfinite(traction_left_wheel_value) && + std::isfinite(steering_position)) + { + if (params_.position_feedback) + { + // Estimate linear and angular velocity using joint information + odometry_.update_from_position( + traction_right_wheel_value, traction_left_wheel_value, steering_position, + period.seconds()); + } + else + { + // Estimate linear and angular velocity using joint information + odometry_.update_from_velocity( + traction_right_wheel_value, traction_left_wheel_value, steering_position, + period.seconds()); + } + } } - const double traction_right_wheel_value = traction_right_wheel_value_op.value(); - const double traction_left_wheel_value = traction_left_wheel_value_op.value(); - const double steering_position = steering_position_op.value(); - if ( - std::isfinite(traction_right_wheel_value) && std::isfinite(traction_left_wheel_value) && - std::isfinite(steering_position)) + else if (odometry_.get_tricycle_nr_traction_wheels() == 1) { - if (params_.position_feedback) + const auto traction_wheel_value_op = + state_interfaces_[STATE_TRACTION_SINGLE_WHEEL].get_optional(); + const auto steering_position_op = + state_interfaces_[STATE_SINGLE_TRACTION_STEER_AXIS].get_optional(); + + if (!traction_wheel_value_op.has_value() || !steering_position_op.has_value()) { - // Estimate linear and angular velocity using joint information - odometry_.update_from_position( - traction_right_wheel_value, traction_left_wheel_value, steering_position, - period.seconds()); + RCLCPP_WARN( + get_node()->get_logger(), + "Unable to retrieve the data from traction wheel or steering position"); + return true; } - else + const double traction_wheel_value = traction_wheel_value_op.value(); + const double steering_position = steering_position_op.value(); + if (std::isfinite(traction_wheel_value) && std::isfinite(steering_position)) { - // Estimate linear and angular velocity using joint information - odometry_.update_from_velocity( - traction_right_wheel_value, traction_left_wheel_value, steering_position, - period.seconds()); + if (params_.position_feedback) + { + // Estimate linear and angular velocity using joint information + odometry_.update_from_position(traction_wheel_value, steering_position, period.seconds()); + } + else + { + // Estimate linear and angular velocity using joint information + odometry_.update_from_velocity(traction_wheel_value, steering_position, period.seconds()); + } } } } diff --git a/tricycle_steering_controller/src/tricycle_steering_controller.yaml b/tricycle_steering_controller/src/tricycle_steering_controller.yaml index 90d2687262..f6bbaa2aaa 100644 --- a/tricycle_steering_controller/src/tricycle_steering_controller.yaml +++ b/tricycle_steering_controller/src/tricycle_steering_controller.yaml @@ -3,11 +3,8 @@ tricycle_steering_controller: { type: double, default_value: 0.0, - description: "Axle track", + description: "Axle track, only for dual-traction configuration. Distance between the two traction wheels.", read_only: false, - validation: { - gt<>: [0.0] - } } wheelbase: diff --git a/tricycle_steering_controller/test/test_load_tricycle_steering_controller.cpp b/tricycle_steering_controller/test/test_load_tricycle_steering_controller.cpp index d3579c902f..dc81692888 100644 --- a/tricycle_steering_controller/test/test_load_tricycle_steering_controller.cpp +++ b/tricycle_steering_controller/test/test_load_tricycle_steering_controller.cpp @@ -32,12 +32,12 @@ TEST(TestLoadTricycleSteeringController, load_controller) const std::string test_file_path = std::string(TEST_FILES_DIRECTORY) + "/tricycle_steering_controller_params.yaml"; - cm.set_parameter({"test_tricycle_steering_controller.params_file", test_file_path}); + cm.set_parameter({"test_tricycle_steering_controller_dual_traction.params_file", test_file_path}); cm.set_parameter( - {"test_tricycle_steering_controller.type", + {"test_tricycle_steering_controller_dual_traction.type", "tricycle_steering_controller/TricycleSteeringController"}); - ASSERT_NE(cm.load_controller("test_tricycle_steering_controller"), nullptr); + ASSERT_NE(cm.load_controller("test_tricycle_steering_controller_dual_traction"), nullptr); } int main(int argc, char ** argv) diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp b/tricycle_steering_controller/test/test_tricycle_steering_controller_dual_traction.cpp similarity index 86% rename from tricycle_steering_controller/test/test_tricycle_steering_controller.cpp rename to tricycle_steering_controller/test/test_tricycle_steering_controller_dual_traction.cpp index 5b3e8b00e5..d009616a62 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller_dual_traction.cpp @@ -17,14 +17,15 @@ #include #include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "test_tricycle_steering_controller.hpp" +#include "test_tricycle_steering_controller_dual_traction.hpp" -class TricycleSteeringControllerTest -: public TricycleSteeringControllerFixture +class TricycleSteeringControllerDualTractionTest +: public TricycleSteeringControllerDualTractionFixture< + TestableTricycleSteeringControllerDualTraction> { }; -TEST_F(TricycleSteeringControllerTest, all_parameters_set_configure_success) +TEST_F(TricycleSteeringControllerDualTractionTest, all_parameters_set_configure_success) { SetUpController(); @@ -42,7 +43,7 @@ TEST_F(TricycleSteeringControllerTest, all_parameters_set_configure_success) ASSERT_EQ(controller_->tricycle_params_.traction_track_width, traction_track_width_); } -TEST_F(TricycleSteeringControllerTest, check_exported_interfaces) +TEST_F(TricycleSteeringControllerDualTractionTest, check_exported_interfaces) { SetUpController(); @@ -57,7 +58,8 @@ TEST_F(TricycleSteeringControllerTest, check_exported_interfaces) cmd_if_conf.names[CMD_TRACTION_LEFT_WHEEL], traction_joints_names_[1] + "/" + traction_interface_name_); EXPECT_EQ( - cmd_if_conf.names[CMD_STEER_WHEEL], steering_joints_names_[0] + "/" + steering_interface_name_); + cmd_if_conf.names[CMD_DUAL_TRACTION_STEER_WHEEL], + steering_joints_names_[0] + "/" + steering_interface_name_); EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); auto state_if_conf = controller_->state_interface_configuration(); @@ -69,7 +71,7 @@ TEST_F(TricycleSteeringControllerTest, check_exported_interfaces) state_if_conf.names[STATE_TRACTION_LEFT_WHEEL], controller_->traction_joints_state_names_[1] + "/" + traction_interface_name_); EXPECT_EQ( - state_if_conf.names[STATE_STEER_AXIS], + state_if_conf.names[STATE_DUAL_TRACTION_STEER_AXIS], controller_->steering_joints_state_names_[0] + "/" + steering_interface_name_); EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); @@ -88,7 +90,7 @@ TEST_F(TricycleSteeringControllerTest, check_exported_interfaces) } } -TEST_F(TricycleSteeringControllerTest, activate_success) +TEST_F(TricycleSteeringControllerDualTractionTest, activate_success) { SetUpController(); @@ -105,7 +107,7 @@ TEST_F(TricycleSteeringControllerTest, activate_success) EXPECT_TRUE(std::isnan(msg.twist.angular.z)); } -TEST_F(TricycleSteeringControllerTest, update_success) +TEST_F(TricycleSteeringControllerDualTractionTest, update_success) { SetUpController(); @@ -117,7 +119,7 @@ TEST_F(TricycleSteeringControllerTest, update_success) controller_interface::return_type::OK); } -TEST_F(TricycleSteeringControllerTest, deactivate_success) +TEST_F(TricycleSteeringControllerDualTractionTest, deactivate_success) { SetUpController(); @@ -126,7 +128,7 @@ TEST_F(TricycleSteeringControllerTest, deactivate_success) ASSERT_TRUE(deactivate_succeeds(controller_)); } -TEST_F(TricycleSteeringControllerTest, reactivate_success) +TEST_F(TricycleSteeringControllerDualTractionTest, reactivate_success) { SetUpController(); @@ -142,7 +144,7 @@ TEST_F(TricycleSteeringControllerTest, reactivate_success) controller_interface::return_type::OK); } -TEST_F(TricycleSteeringControllerTest, test_update_logic) +TEST_F(TricycleSteeringControllerDualTractionTest, test_update_logic) { SetUpController(); rclcpp::executors::MultiThreadedExecutor executor; @@ -171,8 +173,8 @@ TEST_F(TricycleSteeringControllerTest, test_update_logic) controller_->command_interfaces_[CMD_TRACTION_LEFT_WHEEL].get_optional().value(), 0.22222222222222224, COMMON_THRESHOLD); EXPECT_NEAR( - controller_->command_interfaces_[CMD_STEER_WHEEL].get_optional().value(), 1.4179821977774734, - COMMON_THRESHOLD); + controller_->command_interfaces_[CMD_DUAL_TRACTION_STEER_WHEEL].get_optional().value(), + 1.4179821977774734, COMMON_THRESHOLD); EXPECT_FALSE(std::isnan(controller_->input_ref_.get().twist.linear.x)); EXPECT_EQ(controller_->reference_interfaces_.size(), reference_interface_names_.size()); @@ -182,7 +184,7 @@ TEST_F(TricycleSteeringControllerTest, test_update_logic) } } -TEST_F(TricycleSteeringControllerTest, test_update_logic_chained) +TEST_F(TricycleSteeringControllerDualTractionTest, test_update_logic_chained) { SetUpController(); rclcpp::executors::MultiThreadedExecutor executor; @@ -208,8 +210,8 @@ TEST_F(TricycleSteeringControllerTest, test_update_logic_chained) controller_->command_interfaces_[CMD_TRACTION_LEFT_WHEEL].get_optional().value(), 0.22222222222222224, COMMON_THRESHOLD); EXPECT_NEAR( - controller_->command_interfaces_[CMD_STEER_WHEEL].get_optional().value(), 1.4179821977774734, - COMMON_THRESHOLD); + controller_->command_interfaces_[CMD_DUAL_TRACTION_STEER_WHEEL].get_optional().value(), + 1.4179821977774734, COMMON_THRESHOLD); EXPECT_TRUE(std::isnan(controller_->input_ref_.get().twist.linear.x)); EXPECT_EQ(controller_->reference_interfaces_.size(), reference_interface_names_.size()); @@ -219,7 +221,7 @@ TEST_F(TricycleSteeringControllerTest, test_update_logic_chained) } } -TEST_F(TricycleSteeringControllerTest, receive_message_and_publish_updated_status) +TEST_F(TricycleSteeringControllerDualTractionTest, receive_message_and_publish_updated_status) { SetUpController(); rclcpp::executors::MultiThreadedExecutor executor; @@ -255,8 +257,8 @@ TEST_F(TricycleSteeringControllerTest, receive_message_and_publish_updated_statu controller_->command_interfaces_[CMD_TRACTION_LEFT_WHEEL].get_optional().value(), 0.22222222222222224, COMMON_THRESHOLD); EXPECT_NEAR( - controller_->command_interfaces_[CMD_STEER_WHEEL].get_optional().value(), 1.4179821977774734, - COMMON_THRESHOLD); + controller_->command_interfaces_[CMD_DUAL_TRACTION_STEER_WHEEL].get_optional().value(), + 1.4179821977774734, COMMON_THRESHOLD); subscribe_and_get_messages(msg); diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp b/tricycle_steering_controller/test/test_tricycle_steering_controller_dual_traction.hpp similarity index 85% rename from tricycle_steering_controller/test/test_tricycle_steering_controller.hpp rename to tricycle_steering_controller/test/test_tricycle_steering_controller_dual_traction.hpp index 32d3658d5d..e3fdd4447b 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller_dual_traction.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TEST_TRICYCLE_STEERING_CONTROLLER_HPP_ -#define TEST_TRICYCLE_STEERING_CONTROLLER_HPP_ +#ifndef TEST_TRICYCLE_STEERING_CONTROLLER_DUAL_TRACTION_HPP_ +#define TEST_TRICYCLE_STEERING_CONTROLLER_DUAL_TRACTION_HPP_ #include @@ -38,12 +38,12 @@ using ControllerReferenceMsg = steering_controllers_library::SteeringControllersLibrary::ControllerTwistReferenceMsg; // name constants for state interfaces -using tricycle_steering_controller::STATE_STEER_AXIS; +using tricycle_steering_controller::STATE_DUAL_TRACTION_STEER_AXIS; using tricycle_steering_controller::STATE_TRACTION_LEFT_WHEEL; using tricycle_steering_controller::STATE_TRACTION_RIGHT_WHEEL; // name constants for command interfaces -using tricycle_steering_controller::CMD_STEER_WHEEL; +using tricycle_steering_controller::CMD_DUAL_TRACTION_STEER_WHEEL; using tricycle_steering_controller::CMD_TRACTION_LEFT_WHEEL; using tricycle_steering_controller::CMD_TRACTION_RIGHT_WHEEL; @@ -58,19 +58,20 @@ const double COMMON_THRESHOLD = 1e-6; } // namespace // subclassing and friending so we can access member variables -class TestableTricycleSteeringController +class TestableTricycleSteeringControllerDualTraction : public tricycle_steering_controller::TricycleSteeringController { - FRIEND_TEST(TricycleSteeringControllerTest, all_parameters_set_configure_success); - FRIEND_TEST(TricycleSteeringControllerTest, check_exported_interfaces); - FRIEND_TEST(TricycleSteeringControllerTest, activate_success); - FRIEND_TEST(TricycleSteeringControllerTest, update_success); - FRIEND_TEST(TricycleSteeringControllerTest, deactivate_success); - FRIEND_TEST(TricycleSteeringControllerTest, reactivate_success); - FRIEND_TEST(TricycleSteeringControllerTest, test_update_logic); - FRIEND_TEST(TricycleSteeringControllerTest, test_update_logic_chained); - FRIEND_TEST(TricycleSteeringControllerTest, publish_status_success); - FRIEND_TEST(TricycleSteeringControllerTest, receive_message_and_publish_updated_status); + FRIEND_TEST(TricycleSteeringControllerDualTractionTest, all_parameters_set_configure_success); + FRIEND_TEST(TricycleSteeringControllerDualTractionTest, check_exported_interfaces); + FRIEND_TEST(TricycleSteeringControllerDualTractionTest, activate_success); + FRIEND_TEST(TricycleSteeringControllerDualTractionTest, update_success); + FRIEND_TEST(TricycleSteeringControllerDualTractionTest, deactivate_success); + FRIEND_TEST(TricycleSteeringControllerDualTractionTest, reactivate_success); + FRIEND_TEST(TricycleSteeringControllerDualTractionTest, test_update_logic); + FRIEND_TEST(TricycleSteeringControllerDualTractionTest, test_update_logic_chained); + FRIEND_TEST(TricycleSteeringControllerDualTractionTest, publish_status_success); + FRIEND_TEST( + TricycleSteeringControllerDualTractionTest, receive_message_and_publish_updated_status); public: controller_interface::CallbackReturn on_configure( @@ -113,7 +114,7 @@ class TestableTricycleSteeringController // We are using template class here for easier reuse of Fixture in specializations of controllers template -class TricycleSteeringControllerFixture : public ::testing::Test +class TricycleSteeringControllerDualTractionFixture : public ::testing::Test { public: static void SetUpTestCase() {} @@ -125,7 +126,7 @@ class TricycleSteeringControllerFixture : public ::testing::Test command_publisher_node_ = std::make_shared("command_publisher"); command_publisher_ = command_publisher_node_->create_publisher( - "/test_tricycle_steering_controller/reference", rclcpp::SystemDefaultsQoS()); + "/test_tricycle_steering_controller_dual_traction/reference", rclcpp::SystemDefaultsQoS()); } static void TearDownTestCase() {} @@ -133,7 +134,8 @@ class TricycleSteeringControllerFixture : public ::testing::Test void TearDown() { controller_.reset(nullptr); } protected: - void SetUpController(const std::string controller_name = "test_tricycle_steering_controller") + void SetUpController( + const std::string controller_name = "test_tricycle_steering_controller_dual_traction") { controller_interface::ControllerInterfaceParams params; params.controller_name = controller_name; @@ -171,7 +173,7 @@ class TricycleSteeringControllerFixture : public ::testing::Test command_itfs_.emplace_back( std::make_shared( steering_joints_names_[0], steering_interface_name_, - &joint_command_values_[CMD_STEER_WHEEL])); + &joint_command_values_[CMD_DUAL_TRACTION_STEER_WHEEL])); loaned_command_ifs.emplace_back(command_itfs_.back(), nullptr); std::vector loaned_state_ifs; @@ -193,7 +195,7 @@ class TricycleSteeringControllerFixture : public ::testing::Test state_itfs_.emplace_back( std::make_shared( steering_joints_names_[0], steering_interface_name_, - &joint_state_values_[STATE_STEER_AXIS])); + &joint_state_values_[STATE_DUAL_TRACTION_STEER_AXIS])); loaned_state_ifs.emplace_back(state_itfs_.back(), nullptr); controller_->assign_interfaces(std::move(loaned_command_ifs), std::move(loaned_state_ifs)); @@ -206,7 +208,7 @@ class TricycleSteeringControllerFixture : public ::testing::Test rclcpp::Node test_subscription_node("test_subscription_node"); auto subs_callback = [&](const ControllerStateMsg::SharedPtr cb_msg) { received_msg = cb_msg; }; auto subscription = test_subscription_node.create_subscription( - "/test_tricycle_steering_controller/controller_state", 10, subs_callback); + "/test_tricycle_steering_controller_dual_traction/controller_state", 10, subs_callback); rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(test_subscription_node.get_node_base_interface()); @@ -301,9 +303,9 @@ class TricycleSteeringControllerFixture : public ::testing::Test std::vector command_itfs_; // Test related parameters - std::unique_ptr controller_; + std::unique_ptr controller_; rclcpp::Node::SharedPtr command_publisher_node_; rclcpp::Publisher::SharedPtr command_publisher_; }; -#endif // TEST_TRICYCLE_STEERING_CONTROLLER_HPP_ +#endif // TEST_TRICYCLE_STEERING_CONTROLLER_DUAL_TRACTION_HPP_ diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller_preceding.cpp b/tricycle_steering_controller/test/test_tricycle_steering_controller_preceding.cpp index f730e74b53..ef1e0a2643 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller_preceding.cpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller_preceding.cpp @@ -17,15 +17,16 @@ #include #include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "test_tricycle_steering_controller.hpp" -class TricycleSteeringControllerTest -: public TricycleSteeringControllerFixture +#include "test_tricycle_steering_controller_dual_traction.hpp" +class TricycleSteeringControllerDualTractionTest +: public TricycleSteeringControllerDualTractionFixture< + TestableTricycleSteeringControllerDualTraction> { }; -TEST_F(TricycleSteeringControllerTest, all_parameters_set_configure_success) +TEST_F(TricycleSteeringControllerDualTractionTest, all_parameters_set_configure_success) { - SetUpController(); + SetUpController("test_tricycle_steering_controller_dual_traction_preceding"); ASSERT_TRUE(configure_succeeds(controller_)); @@ -43,9 +44,9 @@ TEST_F(TricycleSteeringControllerTest, all_parameters_set_configure_success) ASSERT_EQ(controller_->tricycle_params_.traction_track_width, traction_track_width_); } -TEST_F(TricycleSteeringControllerTest, check_exported_interfaces) +TEST_F(TricycleSteeringControllerDualTractionTest, check_exported_interfaces) { - SetUpController(); + SetUpController("test_tricycle_steering_controller_dual_traction_preceding"); ASSERT_TRUE(configure_succeeds(controller_)); @@ -58,7 +59,7 @@ TEST_F(TricycleSteeringControllerTest, check_exported_interfaces) cmd_if_conf.names[CMD_TRACTION_LEFT_WHEEL], preceding_prefix_ + "/" + traction_joints_names_[1] + "/" + traction_interface_name_); EXPECT_EQ( - cmd_if_conf.names[CMD_STEER_WHEEL], + cmd_if_conf.names[CMD_DUAL_TRACTION_STEER_WHEEL], preceding_prefix_ + "/" + steering_joints_names_[0] + "/" + steering_interface_name_); EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); @@ -71,7 +72,7 @@ TEST_F(TricycleSteeringControllerTest, check_exported_interfaces) state_if_conf.names[STATE_TRACTION_LEFT_WHEEL], controller_->traction_joints_state_names_[1] + "/" + traction_interface_name_); EXPECT_EQ( - state_if_conf.names[STATE_STEER_AXIS], + state_if_conf.names[STATE_DUAL_TRACTION_STEER_AXIS], controller_->steering_joints_state_names_[0] + "/" + steering_interface_name_); EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller_single_traction.cpp b/tricycle_steering_controller/test/test_tricycle_steering_controller_single_traction.cpp new file mode 100644 index 0000000000..513051dd23 --- /dev/null +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller_single_traction.cpp @@ -0,0 +1,262 @@ +// 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 +#include +#include + +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "test_tricycle_steering_controller_single_traction.hpp" + +class TricycleSteeringControllerSingleTractionTest +: public TricycleSteeringControllerSingleTractionFixture< + TestableTricycleSteeringControllerSingleTraction> +{ +}; + +TEST_F(TricycleSteeringControllerSingleTractionTest, all_parameters_set_configure_success) +{ + SetUpController(); + + ASSERT_TRUE(configure_succeeds(controller_)); + + ASSERT_THAT( + controller_->params_.traction_joints_names, testing::ElementsAreArray(traction_joints_names_)); + ASSERT_THAT( + controller_->params_.steering_joints_names, testing::ElementsAreArray(steering_joints_names_)); + ASSERT_EQ(controller_->params_.open_loop, open_loop_); + ASSERT_EQ(controller_->params_.velocity_rolling_window_size, velocity_rolling_window_size_); + ASSERT_EQ(controller_->params_.position_feedback, position_feedback_); + ASSERT_EQ(controller_->tricycle_params_.wheelbase, wheelbase_); + ASSERT_EQ(controller_->tricycle_params_.traction_wheels_radius, traction_wheels_radius_); + ASSERT_EQ( + controller_->tricycle_params_.traction_track_width, 0.0); // single traction has no track width +} + +TEST_F(TricycleSteeringControllerSingleTractionTest, check_exported_interfaces) +{ + SetUpController(); + + ASSERT_TRUE(configure_succeeds(controller_)); + + auto cmd_if_conf = controller_->command_interface_configuration(); + ASSERT_EQ(cmd_if_conf.names.size(), joint_command_values_.size()); + EXPECT_EQ( + cmd_if_conf.names[CMD_TRACTION_SINGLE_WHEEL], + traction_joints_names_[0] + "/" + traction_interface_name_); + EXPECT_EQ( + cmd_if_conf.names[CMD_SINGLE_TRACTION_STEER_WHEEL], + steering_joints_names_[0] + "/" + steering_interface_name_); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + + auto state_if_conf = controller_->state_interface_configuration(); + ASSERT_EQ(state_if_conf.names.size(), joint_state_values_.size()); + EXPECT_EQ( + state_if_conf.names[STATE_TRACTION_SINGLE_WHEEL], + controller_->traction_joints_state_names_[0] + "/" + traction_interface_name_); + EXPECT_EQ( + state_if_conf.names[STATE_SINGLE_TRACTION_STEER_AXIS], + controller_->steering_joints_state_names_[0] + "/" + steering_interface_name_); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + + // check ref itfs + auto reference_interfaces = controller_->export_reference_interfaces(); + ASSERT_EQ(reference_interfaces.size(), reference_interface_names_.size()); + for (size_t i = 0; i < reference_interface_names_.size(); ++i) + { + const std::string ref_itf_prefix_name = + std::string(controller_->get_node()->get_name()) + "/" + reference_interface_names_[i]; + EXPECT_EQ(reference_interfaces[i]->get_prefix_name(), ref_itf_prefix_name); + EXPECT_EQ( + reference_interfaces[i]->get_name(), + ref_itf_prefix_name + "/" + hardware_interface::HW_IF_VELOCITY); + EXPECT_EQ(reference_interfaces[i]->get_interface_name(), hardware_interface::HW_IF_VELOCITY); + } +} + +TEST_F(TricycleSteeringControllerSingleTractionTest, activate_success) +{ + SetUpController(); + + ASSERT_TRUE(configure_succeeds(controller_)); + ASSERT_TRUE(activate_succeeds(controller_)); + + // check that the message is reset + auto msg = controller_->input_ref_.get(); + EXPECT_TRUE(std::isnan(msg.twist.linear.x)); + EXPECT_TRUE(std::isnan(msg.twist.linear.y)); + EXPECT_TRUE(std::isnan(msg.twist.linear.z)); + EXPECT_TRUE(std::isnan(msg.twist.angular.x)); + EXPECT_TRUE(std::isnan(msg.twist.angular.y)); + EXPECT_TRUE(std::isnan(msg.twist.angular.z)); +} + +TEST_F(TricycleSteeringControllerSingleTractionTest, update_success) +{ + SetUpController(); + + ASSERT_TRUE(configure_succeeds(controller_)); + ASSERT_TRUE(activate_succeeds(controller_)); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); +} + +TEST_F(TricycleSteeringControllerSingleTractionTest, deactivate_success) +{ + SetUpController(); + + ASSERT_TRUE(configure_succeeds(controller_)); + ASSERT_TRUE(activate_succeeds(controller_)); + ASSERT_TRUE(deactivate_succeeds(controller_)); +} + +TEST_F(TricycleSteeringControllerSingleTractionTest, reactivate_success) +{ + SetUpController(); + + ASSERT_TRUE(configure_succeeds(controller_)); + ASSERT_TRUE(activate_succeeds(controller_)); + ASSERT_TRUE(deactivate_succeeds(controller_)); + ASSERT_TRUE(std::isnan(controller_->command_interfaces_[0].get_optional().value())); + ASSERT_TRUE(activate_succeeds(controller_)); + ASSERT_TRUE(std::isnan(controller_->command_interfaces_[0].get_optional().value())); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); +} + +TEST_F(TricycleSteeringControllerSingleTractionTest, test_update_logic) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + ASSERT_TRUE(configure_succeeds(controller_)); + controller_->set_chained_mode(false); + ASSERT_TRUE(activate_succeeds(controller_)); + ASSERT_FALSE(controller_->is_in_chained_mode()); + + // set command statically + ControllerReferenceMsg msg; + msg.header.stamp = controller_->get_node()->now(); + msg.twist.linear.x = 0.1; + msg.twist.angular.z = 0.2; + controller_->input_ref_.set(msg); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_NEAR( + controller_->command_interfaces_[CMD_TRACTION_SINGLE_WHEEL].get_optional().value(), + 0.22222222222222224, COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[CMD_SINGLE_TRACTION_STEER_WHEEL].get_optional().value(), + 1.4179821977774734, COMMON_THRESHOLD); + + EXPECT_FALSE(std::isnan(controller_->input_ref_.get().twist.linear.x)); + EXPECT_EQ(controller_->reference_interfaces_.size(), reference_interface_names_.size()); + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } +} + +TEST_F(TricycleSteeringControllerSingleTractionTest, test_update_logic_chained) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + ASSERT_TRUE(configure_succeeds(controller_)); + controller_->set_chained_mode(true); + ASSERT_TRUE(activate_succeeds(controller_)); + ASSERT_TRUE(controller_->is_in_chained_mode()); + + controller_->reference_interfaces_[0] = 0.1; + controller_->reference_interfaces_[1] = 0.2; + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + // we test with open_loop=false, but steering angle was not updated (is zero) -> same commands + EXPECT_NEAR( + controller_->command_interfaces_[CMD_TRACTION_SINGLE_WHEEL].get_optional().value(), + 0.22222222222222224, COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[CMD_SINGLE_TRACTION_STEER_WHEEL].get_optional().value(), + 1.4179821977774734, COMMON_THRESHOLD); + + EXPECT_TRUE(std::isnan(controller_->input_ref_.get().twist.linear.x)); + EXPECT_EQ(controller_->reference_interfaces_.size(), reference_interface_names_.size()); + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } +} + +TEST_F(TricycleSteeringControllerSingleTractionTest, receive_message_and_publish_updated_status) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + ASSERT_TRUE(configure_succeeds(controller_)); + ASSERT_TRUE(activate_succeeds(controller_)); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + ControllerStateMsg msg; + subscribe_and_get_messages(msg); + + // never received a valid command, linear velocity should have been reset + EXPECT_EQ(msg.traction_command[0], 0.0); + EXPECT_EQ(msg.steering_angle_command[0], 2.2); + + publish_commands(); + controller_->wait_for_commands(executor); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + // we test with open_loop=false, but steering angle was not updated (is zero) -> same commands + EXPECT_NEAR( + controller_->command_interfaces_[CMD_TRACTION_SINGLE_WHEEL].get_optional().value(), + 0.22222222222222224, COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[CMD_SINGLE_TRACTION_STEER_WHEEL].get_optional().value(), + 1.4179821977774734, COMMON_THRESHOLD); + + subscribe_and_get_messages(msg); + + // we test with open_loop=false, but steering angle was not updated (is zero) -> same commands + EXPECT_NEAR(msg.traction_command[0], 0.22222222222222224, COMMON_THRESHOLD); + EXPECT_NEAR(msg.steering_angle_command[0], 1.4179821977774734, COMMON_THRESHOLD); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller_single_traction.hpp b/tricycle_steering_controller/test/test_tricycle_steering_controller_single_traction.hpp new file mode 100644 index 0000000000..820541be22 --- /dev/null +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller_single_traction.hpp @@ -0,0 +1,286 @@ +// 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 TEST_TRICYCLE_STEERING_CONTROLLER_SINGLE_TRACTION_HPP_ +#define TEST_TRICYCLE_STEERING_CONTROLLER_SINGLE_TRACTION_HPP_ + +#include + +#include +#include +#include +#include +#include + +#include "controller_interface/test_utils.hpp" +#include "hardware_interface/loaned_command_interface.hpp" +#include "hardware_interface/loaned_state_interface.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors.hpp" +#include "rclcpp/time.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "tricycle_steering_controller/tricycle_steering_controller.hpp" + +using ControllerStateMsg = + steering_controllers_library::SteeringControllersLibrary::SteeringControllerStateMsg; +using ControllerReferenceMsg = + steering_controllers_library::SteeringControllersLibrary::ControllerTwistReferenceMsg; + +// name constants for state interfaces +using tricycle_steering_controller::STATE_SINGLE_TRACTION_STEER_AXIS; +using tricycle_steering_controller::STATE_TRACTION_SINGLE_WHEEL; + +// name constants for command interfaces +using tricycle_steering_controller::CMD_SINGLE_TRACTION_STEER_WHEEL; +using tricycle_steering_controller::CMD_TRACTION_SINGLE_WHEEL; + +using controller_interface::activate_succeeds; +using controller_interface::configure_succeeds; +using controller_interface::deactivate_succeeds; + +namespace +{ +const double COMMON_THRESHOLD = 1e-6; +} // namespace + +// subclassing and friending so we can access member variables +class TestableTricycleSteeringControllerSingleTraction +: public tricycle_steering_controller::TricycleSteeringController +{ + FRIEND_TEST(TricycleSteeringControllerSingleTractionTest, all_parameters_set_configure_success); + FRIEND_TEST(TricycleSteeringControllerSingleTractionTest, check_exported_interfaces); + FRIEND_TEST(TricycleSteeringControllerSingleTractionTest, activate_success); + FRIEND_TEST(TricycleSteeringControllerSingleTractionTest, update_success); + FRIEND_TEST(TricycleSteeringControllerSingleTractionTest, deactivate_success); + FRIEND_TEST(TricycleSteeringControllerSingleTractionTest, reactivate_success); + FRIEND_TEST(TricycleSteeringControllerSingleTractionTest, test_update_logic); + FRIEND_TEST(TricycleSteeringControllerSingleTractionTest, test_update_logic_chained); + FRIEND_TEST( + TricycleSteeringControllerSingleTractionTest, receive_message_and_publish_updated_status); + +public: + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override + { + return tricycle_steering_controller::TricycleSteeringController::on_configure(previous_state); + } + + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override + { + auto ref_itfs = on_export_reference_interfaces(); + return tricycle_steering_controller::TricycleSteeringController::on_activate(previous_state); + } + + /** + * @brief wait_for_command blocks until a new ControllerReferenceMsg is received. + * Requires that the executor is not spinned elsewhere between the + * message publication and the call to this function. + */ + void wait_for_command( + rclcpp::Executor & executor, + const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) + { + auto until = get_node()->get_clock()->now() + timeout; + while (get_node()->get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } + } + + void wait_for_commands( + rclcpp::Executor & executor, + const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) + { + return wait_for_command(executor, timeout); + } +}; + +// We are using template class here for easier reuse of Fixture in specializations of controllers +template +class TricycleSteeringControllerSingleTractionFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() {} + + void SetUp() + { + // initialize controller + controller_ = std::make_unique(); + + command_publisher_node_ = std::make_shared("command_publisher"); + command_publisher_ = command_publisher_node_->create_publisher( + "/test_tricycle_steering_controller_single_traction/reference", rclcpp::SystemDefaultsQoS()); + } + + static void TearDownTestCase() {} + + void TearDown() { controller_.reset(nullptr); } + +protected: + void SetUpController( + const std::string controller_name = "test_tricycle_steering_controller_single_traction") + { + controller_interface::ControllerInterfaceParams params; + params.controller_name = controller_name; + params.robot_description = ""; + params.update_rate = 0; + params.node_namespace = ""; + params.node_options = controller_->define_custom_node_options(); + ASSERT_EQ(controller_->init(params), controller_interface::return_type::OK); + + if (position_feedback_ == true) + { + traction_interface_name_ = "position"; + } + else + { + traction_interface_name_ = "velocity"; + } + + std::vector loaned_command_ifs; + command_itfs_.reserve(joint_command_values_.size()); + loaned_command_ifs.reserve(joint_command_values_.size()); + + command_itfs_.emplace_back( + std::make_shared( + traction_joints_names_[0], traction_interface_name_, + &joint_command_values_[CMD_TRACTION_SINGLE_WHEEL])); + loaned_command_ifs.emplace_back(command_itfs_.back(), nullptr); + + command_itfs_.emplace_back( + std::make_shared( + steering_joints_names_[0], steering_interface_name_, + &joint_command_values_[CMD_SINGLE_TRACTION_STEER_WHEEL])); + loaned_command_ifs.emplace_back(command_itfs_.back(), nullptr); + + std::vector loaned_state_ifs; + state_itfs_.reserve(joint_state_values_.size()); + loaned_state_ifs.reserve(joint_state_values_.size()); + + state_itfs_.emplace_back( + std::make_shared( + traction_joints_names_[0], traction_interface_name_, + &joint_state_values_[STATE_TRACTION_SINGLE_WHEEL])); + loaned_state_ifs.emplace_back(state_itfs_.back(), nullptr); + + state_itfs_.emplace_back( + std::make_shared( + steering_joints_names_[0], steering_interface_name_, + &joint_state_values_[STATE_SINGLE_TRACTION_STEER_AXIS])); + loaned_state_ifs.emplace_back(state_itfs_.back(), nullptr); + + controller_->assign_interfaces(std::move(loaned_command_ifs), std::move(loaned_state_ifs)); + } + + void subscribe_and_get_messages(ControllerStateMsg & msg) + { + // create a new subscriber + ControllerStateMsg::SharedPtr received_msg; + rclcpp::Node test_subscription_node("test_subscription_node"); + auto subs_callback = [&](const ControllerStateMsg::SharedPtr cb_msg) { received_msg = cb_msg; }; + auto subscription = test_subscription_node.create_subscription( + "/test_tricycle_steering_controller_single_traction/controller_state", 10, subs_callback); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(test_subscription_node.get_node_base_interface()); + + // call update to publish the test value + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + // call update to publish the test value + // since update doesn't guarantee a published message, republish until received + int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop + while (max_sub_check_loop_count--) + { + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)); + const auto timeout = std::chrono::milliseconds{5}; + const auto until = test_subscription_node.get_clock()->now() + timeout; + while (!received_msg && test_subscription_node.get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } + // check if message has been received + if (received_msg.get()) + { + break; + } + } + ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " + "controller/broadcaster update loop"; + ASSERT_TRUE(received_msg); + + // take message from subscription + msg = *received_msg; + } + + void publish_commands(const double linear = 0.1, const double angular = 0.2) + { + auto wait_for_topic = [&](const auto topic_name) + { + size_t wait_count = 0; + while (command_publisher_node_->count_subscribers(topic_name) == 0) + { + if (wait_count >= 5) + { + auto error_msg = + std::string("publishing to ") + topic_name + " but no node subscribes to it"; + throw std::runtime_error(error_msg); + } + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + ++wait_count; + } + }; + + wait_for_topic(command_publisher_->get_topic_name()); + + ControllerReferenceMsg msg; + msg.twist.linear.x = linear; + msg.twist.angular.z = angular; + + command_publisher_->publish(msg); + } + +protected: + // Controller-related parameters + double reference_timeout_ = 2.0; + bool open_loop_ = false; + unsigned int velocity_rolling_window_size_ = 10; + bool position_feedback_ = false; + std::vector traction_joints_names_ = {"traction_wheel_joint"}; + std::vector steering_joints_names_ = {"steering_axis_joint"}; + + double wheelbase_ = 3.24644; + double traction_track_width_ = 1.212121; + double traction_wheels_radius_ = 0.45; + + std::array joint_state_values_{{0.5, 0.0}}; + std::array joint_command_values_{{1.1, 2.2}}; + std::array reference_interface_names_{{"linear", "angular"}}; + std::string steering_interface_name_ = "position"; + // defined in setup + std::string traction_interface_name_ = ""; + + std::vector state_itfs_; + std::vector command_itfs_; + + // Test related parameters + std::unique_ptr controller_; + rclcpp::Node::SharedPtr command_publisher_node_; + rclcpp::Publisher::SharedPtr command_publisher_; +}; + +#endif // TEST_TRICYCLE_STEERING_CONTROLLER_SINGLE_TRACTION_HPP_ diff --git a/tricycle_steering_controller/test/tricycle_steering_controller_params.yaml b/tricycle_steering_controller/test/tricycle_steering_controller_params.yaml index 968b24ea7b..d978d2d23c 100644 --- a/tricycle_steering_controller/test/tricycle_steering_controller_params.yaml +++ b/tricycle_steering_controller/test/tricycle_steering_controller_params.yaml @@ -1,4 +1,4 @@ -test_tricycle_steering_controller: +test_tricycle_steering_controller_dual_traction: ros__parameters: reference_timeout: 2.0 @@ -11,3 +11,16 @@ test_tricycle_steering_controller: wheelbase: 3.24644 traction_track_width: 1.212121 traction_wheels_radius: 0.45 + +test_tricycle_steering_controller_single_traction: + ros__parameters: + + reference_timeout: 2.0 + open_loop: false + velocity_rolling_window_size: 10 + position_feedback: false + traction_joints_names: [traction_wheel_joint] + steering_joints_names: [steering_axis_joint] + + wheelbase: 3.24644 + traction_wheels_radius: 0.45 diff --git a/tricycle_steering_controller/test/tricycle_steering_controller_preceding_params.yaml b/tricycle_steering_controller/test/tricycle_steering_controller_preceding_params.yaml index c3dce53798..5184d85d35 100644 --- a/tricycle_steering_controller/test/tricycle_steering_controller_preceding_params.yaml +++ b/tricycle_steering_controller/test/tricycle_steering_controller_preceding_params.yaml @@ -1,4 +1,4 @@ -test_tricycle_steering_controller: +test_tricycle_steering_controller_dual_traction_preceding: ros__parameters: reference_timeout: 2.0 open_loop: false