From 6106f285d6fc78bf7b59bbb98cd9cafb10b00283 Mon Sep 17 00:00:00 2001 From: Atsushi Watanabe Date: Fri, 22 Mar 2024 14:03:03 +0900 Subject: [PATCH] Simulate motion on --without-device mode (#185) --- CMakeLists.txt | 20 ++++ include/command.h | 2 + include/control.h | 3 + src/control_vehicle.c | 44 +++++++++ src/ypspur-coordinator.c | 12 +-- test/e2e.cpp | 208 +++++++++++++++++++++++++++++++++++++++ test/joint.param | 44 +++++++++ test/vehicle.param | 41 ++++++++ 8 files changed, 365 insertions(+), 9 deletions(-) create mode 100644 test/e2e.cpp create mode 100644 test/joint.param create mode 100644 test/vehicle.param diff --git a/CMakeLists.txt b/CMakeLists.txt index 3196e32..17bdc99 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -136,6 +136,26 @@ if(GTEST_FOUND) ) add_test(YPSpurParamTest param_load_test) + + add_executable(e2e_test + test/e2e.cpp + ) + add_dependencies(e2e_test + ypspur-coordinator + ypspur + ) + target_link_libraries(e2e_test + ${GTEST_LIBRARIES} + ${THREAD_LIBRARIES} + ypspur + ) + if(CMAKE_VERSION VERSION_LESS "3.1") + set_property(TARGET e2e_test PROPERTY COMPILE_OPTIONS "-std=c++11") + else() + set_property(TARGET e2e_test PROPERTY CXX_STANDARD 11) + endif() + + add_test(E2ETest e2e_test ${CMAKE_SOURCE_DIR}/test/) endif(GTEST_FOUND) diff --git a/include/command.h b/include/command.h index a61518f..2cfa5ed 100644 --- a/include/command.h +++ b/include/command.h @@ -21,6 +21,8 @@ #ifndef COMMAND_H #define COMMAND_H +#include + #include #include #include diff --git a/include/control.h b/include/control.h index bf255b2..df46fdd 100644 --- a/include/control.h +++ b/include/control.h @@ -21,6 +21,8 @@ #ifndef CONTROL_H #define CONTROL_H +#include + #include #include @@ -29,6 +31,7 @@ void robot_speed(SpurUserParamsPtr spur); int motor_control(SpurUserParamsPtr spur); void apply_motor_torque(SpurUserParamsPtr spur); void apply_motor_speed(SpurUserParamsPtr spur); +void simulate_control(OdometryPtr odm, SpurUserParamsPtr spur); void update_ref_speed(SpurUserParamsPtr spur); diff --git a/src/control_vehicle.c b/src/control_vehicle.c index 39990e4..7a46ad3 100644 --- a/src/control_vehicle.c +++ b/src/control_vehicle.c @@ -457,6 +457,40 @@ void control_loop_cleanup(void* data) yprintf(OUTPUT_LV_INFO, "Trajectory control loop stopped.\n"); } +void simulate_control(OdometryPtr odm, SpurUserParamsPtr spur) +{ + const double dt = p(YP_PARAM_CONTROL_CYCLE, 0); + ParametersPtr param = get_param_ptr(); + odm->time = get_time(); + + int i; + for (i = 0; i < YP_PARAM_MAX_MOTOR_NUM; i++) + { + if (!param->motor_enable[i]) + continue; + + switch (spur->wheel_mode[i]) + { + case MOTOR_CONTROL_OPENFREE: + case MOTOR_CONTROL_FREE: + odm->wvel[i] = 0; + break; + default: + odm->wvel[i] = spur->wheel_vel_smooth[i]; + odm->wang[i] += odm->wvel[i] * dt; + break; + } + } + + odm->v = p(YP_PARAM_RADIUS, MOTOR_RIGHT) * odm->wvel[MOTOR_RIGHT] / 2.0 + + p(YP_PARAM_RADIUS, MOTOR_LEFT) * odm->wvel[MOTOR_LEFT] / 2.0; + odm->w = p(YP_PARAM_RADIUS, MOTOR_RIGHT) * odm->wvel[MOTOR_RIGHT] / p(YP_PARAM_TREAD, 0) - + p(YP_PARAM_RADIUS, MOTOR_LEFT) * odm->wvel[MOTOR_LEFT] / p(YP_PARAM_TREAD, 0); + odm->x = odm->x + odm->v * cos(odm->theta) * dt; + odm->y = odm->y + odm->v * sin(odm->theta) * dt; + odm->theta = odm->theta + odm->w * dt; +} + /* 20msごとの割り込みで軌跡追従制御処理を呼び出す */ void control_loop(void) { @@ -487,6 +521,11 @@ void control_loop(void) coordinate_synchronize(odometry, spur); run_control(*odometry, spur); + if ((option(OPTION_WITHOUT_DEVICE))) + { + simulate_control(*odometry, spur); + } + // スレッドの停止要求チェック pthread_testcancel(); } @@ -500,6 +539,11 @@ void control_loop(void) coordinate_synchronize(odometry, spur); run_control(*odometry, spur); + if ((option(OPTION_WITHOUT_DEVICE))) + { + simulate_control(odometry, spur); + } + // スレッドの停止要求チェック pthread_testcancel(); } diff --git a/src/ypspur-coordinator.c b/src/ypspur-coordinator.c index 66bc075..c0698c8 100644 --- a/src/ypspur-coordinator.c +++ b/src/ypspur-coordinator.c @@ -486,15 +486,9 @@ int main(int argc, char* argv[]) init_command_thread(&command_thread); command_thread_en = 1; - if (!(option(OPTION_WITHOUT_DEVICE))) - { - init_control_thread(&control_thread); - control_thread_en = 1; - } - else - { - control_thread_en = 0; - } + init_control_thread(&control_thread); + control_thread_en = 1; + if (option(OPTION_UPDATE_PARAM)) { yprintf(OUTPUT_LV_WARNING, "==================== Warning! ====================\n"); diff --git a/test/e2e.cpp b/test/e2e.cpp new file mode 100644 index 0000000..e6bf09d --- /dev/null +++ b/test/e2e.cpp @@ -0,0 +1,208 @@ +// Copyright (c) 2024 The YP-Spur Authors, except where otherwise indicated. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to +// deal in the Software without restriction, including without limitation the +// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or +// sell copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#include +#include +#include + +#include +#include +#include + +#include + +#include + +char* test_dir; + +class E2E : public ::testing::Test +{ +protected: + void initCoordinator(const std::string& param_name) + { + coordinator_pid_ = fork(); + ASSERT_GE(coordinator_pid_, 0); + if (coordinator_pid_ == 0) + { + const std::string param_file = std::string(test_dir) + "/" + param_name; + execl("./ypspur-coordinator", "ypspur-coordinator", "--without-device", "--param", param_file.c_str(), nullptr); + } + sleep(1); + ASSERT_GE(YPSpur_md_init(&cli_), 0) << "Failed to connect to ypspur-coordinator"; + } + void TearDown() + { + kill(coordinator_pid_, 2); + wait(nullptr); + } + + YPSpur cli_; + pid_t coordinator_pid_; +}; + +TEST_F(E2E, Vel) +{ + ASSERT_NO_FATAL_FAILURE(initCoordinator("vehicle.param")); + + ASSERT_GE(YPSpur_md_set_vel(&cli_, 2.0), 0); + ASSERT_GE(YPSpur_md_set_accel(&cli_, 0.5), 0); + ASSERT_GE(YPSpur_md_set_angvel(&cli_, 2.0), 0); + ASSERT_GE(YPSpur_md_set_angaccel(&cli_, 0.5), 0); + ASSERT_GE(YPSpur_md_vel(&cli_, 0.7, 0.9), 0); + + double v, w; + sleep(1); + ASSERT_GE(YPSpur_md_get_vel(&cli_, &v, &w), 0); + ASSERT_NEAR(v, 0.5, 0.1); + ASSERT_NEAR(w, 0.5, 0.1); + + sleep(1); + ASSERT_GE(YPSpur_md_get_vel(&cli_, &v, &w), 0); + ASSERT_NEAR(v, 0.7, 0.1); + ASSERT_NEAR(w, 0.9, 0.1); +} + +TEST_F(E2E, LineSpin) +{ + ASSERT_NO_FATAL_FAILURE(initCoordinator("vehicle.param")); + + ASSERT_GE(YPSpur_md_set_vel(&cli_, 4.0), 0); + ASSERT_GE(YPSpur_md_set_accel(&cli_, 8.0), 0); + ASSERT_GE(YPSpur_md_set_angvel(&cli_, 4.0), 0); + ASSERT_GE(YPSpur_md_set_angaccel(&cli_, 8.0), 0); + + ASSERT_GE(YPSpur_md_stop_line(&cli_, CS_GL, 0.5, 0.0, 0.0), 0); + sleep(1); + + double x, y, th; + ASSERT_GE(YPSpur_md_get_pos(&cli_, CS_GL, &x, &y, &th), 0); + ASSERT_NEAR(x, 0.5, 0.05); + ASSERT_NEAR(y, 0.0, 0.05); + ASSERT_NEAR(th, 0.0, 0.05); + + ASSERT_GE(YPSpur_md_spin(&cli_, CS_GL, 1.5708), 0); + sleep(2); + + ASSERT_GE(YPSpur_md_get_pos(&cli_, CS_GL, &x, &y, &th), 0); + ASSERT_NEAR(x, 0.5, 0.05); + ASSERT_NEAR(y, 0.0, 0.05); + ASSERT_NEAR(th, 1.5708, 0.05); + + ASSERT_GE(YPSpur_md_stop_line(&cli_, CS_GL, 0.5, -0.5, 1.5708), 0); + sleep(1); + + ASSERT_GE(YPSpur_md_get_pos(&cli_, CS_GL, &x, &y, &th), 0); + ASSERT_NEAR(x, 0.5, 0.05); + ASSERT_NEAR(y, -0.5, 0.05); + ASSERT_NEAR(th, 1.5708, 0.05); +} + +TEST_F(E2E, SetPos) +{ + ASSERT_NO_FATAL_FAILURE(initCoordinator("vehicle.param")); + + ASSERT_GE(YPSpur_md_set_vel(&cli_, 4.0), 0); + ASSERT_GE(YPSpur_md_set_accel(&cli_, 8.0), 0); + ASSERT_GE(YPSpur_md_set_angvel(&cli_, 4.0), 0); + ASSERT_GE(YPSpur_md_set_angaccel(&cli_, 8.0), 0); + + double x, y, th; + ASSERT_GE(YPSpur_md_get_pos(&cli_, CS_GL, &x, &y, &th), 0); + ASSERT_NEAR(x, 0.0, 0.01); + ASSERT_NEAR(y, 0.0, 0.01); + ASSERT_NEAR(th, 0.0, 0.01); + + ASSERT_GE(YPSpur_md_set_pos(&cli_, CS_GL, 10.0, 20.0, 1.0), 0); + + ASSERT_GE(YPSpur_md_get_pos(&cli_, CS_GL, &x, &y, &th), 0); + ASSERT_NEAR(x, 10.0, 0.01); + ASSERT_NEAR(y, 20.0, 0.01); + ASSERT_NEAR(th, 1.0, 0.01); +} + +TEST_F(E2E, JointAngle) +{ + ASSERT_NO_FATAL_FAILURE(initCoordinator("joint.param")); + + for (int i = 0; i < 2; ++i) + { + SCOPED_TRACE("joint_id=" + std::to_string(i)); + + double a; + ASSERT_GE(YP_md_get_joint_ang(&cli_, i, &a), 0); + ASSERT_NEAR(a, 0.0, 0.05); + + ASSERT_GE(YP_md_set_joint_vel(&cli_, i, 20), 0); + ASSERT_GE(YP_md_set_joint_accel(&cli_, i, 30), 0); + + ASSERT_GE(YP_md_joint_ang(&cli_, i, 7.0), 0); + sleep(1); + + ASSERT_GE(YP_md_get_joint_ang(&cli_, i, &a), 0); + ASSERT_NEAR(a, 7.0, 0.05); + } +} + +TEST_F(E2E, JointAngleVel) +{ + ASSERT_NO_FATAL_FAILURE(initCoordinator("joint.param")); + + for (int i = 0; i < 2; ++i) + { + SCOPED_TRACE("joint_id=" + std::to_string(i)); + + ASSERT_GE(YP_md_set_joint_vel(&cli_, i, 20), 0); + ASSERT_GE(YP_md_set_joint_accel(&cli_, i, 30), 0); + + ASSERT_GE(YP_md_joint_ang_vel(&cli_, i, 8.0, 3.0), 0); + + double v_diff_min = 3.0; + for (int t = 0; t < 100; ++t) + { + double a, v; + usleep(20000); + ASSERT_GE(YP_md_get_joint_ang(&cli_, i, &a), 0); + ASSERT_GE(YP_md_get_joint_vel(&cli_, i, &v), 0); + if (8.0 < a && a < 8.2) + { + const double v_diff = 3.0 - v; + if (std::abs(v_diff_min) > std::abs(v_diff)) + { + v_diff_min = v_diff; + } + } + } + ASSERT_NEAR(v_diff_min, 0.0, 0.05); + } +} + +int main(int argc, char** argv) +{ + if (argc < 2) + { + std::cerr << "test dir path should be provided as a first argument" << std::endl; + return 1; + } + test_dir = argv[1]; + + testing::InitGoogleTest(&argc, argv); + + return RUN_ALL_TESTS(); +} diff --git a/test/joint.param b/test/joint.param new file mode 100644 index 0000000..c1fde68 --- /dev/null +++ b/test/joint.param @@ -0,0 +1,44 @@ +VERSION 4 +VOLT 16 +CONTROL_CYCLE 0.02 +TORQUE_FINENESS 0.00000001 + +MOTOR_PHASE 3 +COUNT_REV 1000 +ENCODER_TYPE 2 +ENCODER_DIV 1 +CYCLE 0.001 +MOTOR_R 1 +TORQUE_MAX 1 +TORQUE_LIMIT 1 +MOTOR_VC 600 +MOTOR_TC 0.0159 + +GEAR 1 +TREAD 1 +RADIUS 1 +MASS 0.0 +MOMENT_INERTIA 0 +TIRE_M_INERTIA 1 +MOTOR_M_INERTIA 0 + +L_C1 0.01 +L_K1 800 +L_K2 300 +L_K3 200 +L_DIST 0.6 + +MAX_VEL 1 +MAX_W 2 +MAX_ACC_V 2 +MAX_ACC_W 4 +MAX_CENTRI_ACC 1.96 + +GAIN_KP 100 +GAIN_KI 1000 +INTEGRAL_MAX 0.01 +TORQUE_NEWTON 0 +TORQUE_VISCOS 0 + +VEHICLE_CONTROL[0] 0 +VEHICLE_CONTROL[1] 0 diff --git a/test/vehicle.param b/test/vehicle.param new file mode 100644 index 0000000..5402752 --- /dev/null +++ b/test/vehicle.param @@ -0,0 +1,41 @@ +VERSION 4 +VOLT 16 +CONTROL_CYCLE 0.02 +TORQUE_FINENESS 0.00000001 + +MOTOR_PHASE 3 +COUNT_REV 1000 +ENCODER_TYPE 2 +ENCODER_DIV 1 +CYCLE 0.001 +MOTOR_R 1 +TORQUE_MAX 1 +TORQUE_LIMIT 1 +MOTOR_VC 600 +MOTOR_TC 0.0159 + +GEAR 1 +TREAD 1 +RADIUS 1 +MASS 0.0 +MOMENT_INERTIA 0 +TIRE_M_INERTIA 1 +MOTOR_M_INERTIA 0 + +L_C1 0.01 +L_K1 800 +L_K2 300 +L_K3 200 +L_DIST 0.6 + +MAX_VEL 1 +MAX_W 2 +MAX_ACC_V 2 +MAX_ACC_W 4 +MAX_CENTRI_ACC 1.96 + +GAIN_KP 100 +GAIN_KI 1000 +INTEGRAL_MAX 0.01 +TORQUE_NEWTON 0 +TORQUE_VISCOS 0