Simulate motion on --without-device mode (#185)

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)
 
 

include/command.h

@@ -21,6 +21,8 @@
 #ifndef COMMAND_H
 #define COMMAND_H
 
+#include <pthread.h>
+
 #include <ipcommunication.h>
 #include <ypparam.h>
 #include <ypspur.h>

include/control.h

@@ -21,6 +21,8 @@
 #ifndef CONTROL_H
 #define CONTROL_H
 
+#include <pthread.h>
+
 #include <command.h>
 #include <odometry.h>
 
@@ -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);
 

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();
   }

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");

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 <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+
+#include <cmath>
+#include <stdexcept>
+#include <string>
+
+#include <gtest/gtest.h>
+
+#include <ypspur-md.h>
+
+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();
+}

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