unit_tests_ned_one.cpp
Go to the documentation of this file.
1 /*
2  ttl_driver_unit_tests.cpp
3  Copyright (C) 2020 Niryo
4  All rights reserved.
5  This program is free software: you can redistribute it and/or modify
6  it under the terms of the GNU General Public License as published by
7  the Free Software Foundation, either version 3 of the License, or
8  (at your option) any later version.
9  This program is distributed in the hope that it will be useful,
10  but WITHOUT ANY WARRANTY; without even the implied warranty of
11  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12  GNU General Public License for more details.
13  You should have received a copy of the GNU General Public License
14  along with this program. If not, see <http:// www.gnu.org/licenses/>.
15 */
16 
17 // Bring in my package's API, which is what I'm testing
18 #include "common/model/abstract_motor_state.hpp"
19 #include "common/model/bus_protocol_enum.hpp"
20 #include "common/model/dxl_command_type_enum.hpp"
21 #include "common/model/dxl_motor_state.hpp"
22 #include "common/model/hardware_type_enum.hpp"
23 #include "common/model/joint_state.hpp"
24 #include "common/model/single_motor_cmd.hpp"
25 #include "common/model/stepper_command_type_enum.hpp"
26 #include "common/model/stepper_motor_state.hpp"
27 #include "common/model/synchronize_motor_cmd.hpp"
28 #include "dynamixel_sdk/dynamixel_sdk.h"
29 #include "ros/node_handle.h"
30 #include "ttl_driver/ttl_interface_core.hpp"
31 #include "ttl_driver/ttl_manager.hpp"
32 
33 // Bring in gtest
34 #include <cassert>
35 #include <gtest/gtest.h>
36 #include <memory>
37 #include <ros/console.h>
38 #include <string>
39 #include <utility>
40 
41 using ::common::model::BusProtocolEnum;
42 using ::common::model::DxlMotorState;
43 using ::common::model::EBusProtocol;
44 using ::common::model::EHardwareType;
45 using ::common::model::HardwareTypeEnum;
46 using ::common::model::StepperMotorState;
47 using ::std::string;
48 using ::std::to_string;
49 
54 void addJointToTtlInterface(const std::shared_ptr<ttl_driver::TtlInterfaceCore> &ttl_interface)
55 {
56  size_t nb_joints = 0;
57 
58  ros::NodeHandle robot_hwnh("joints_interface");
59  // retrieve nb joints with checking that the config param exists for both name and id
60  while (robot_hwnh.hasParam("joint_" + to_string(nb_joints + 1) + "/id") &&
61  robot_hwnh.hasParam("joint_" + to_string(nb_joints + 1) + "/name") &&
62  robot_hwnh.hasParam("joint_" + to_string(nb_joints + 1) + "/type") &&
63  robot_hwnh.hasParam("joint_" + to_string(nb_joints + 1) + "/bus"))
64  nb_joints++;
65 
66  // connect and register joint state interface
67 
68  int currentIdDxl = 1;
69  int currentIdStepper = 1;
70 
71  for (size_t j = 0; j < nb_joints; j++)
72  {
73  int joint_id_config = 0;
74  string joint_name;
75  string joint_type;
76  string joint_bus;
77 
78  robot_hwnh.getParam("joint_" + to_string(j + 1) + "/id", joint_id_config);
79  robot_hwnh.getParam("joint_" + to_string(j + 1) + "/name", joint_name);
80  robot_hwnh.getParam("joint_" + to_string(j + 1) + "/type", joint_type);
81  robot_hwnh.getParam("joint_" + to_string(j + 1) + "/bus", joint_bus);
82  HardwareTypeEnum eType = HardwareTypeEnum(joint_type.c_str());
83  BusProtocolEnum eBusProto = BusProtocolEnum(joint_bus.c_str());
84 
85  if (eType == EHardwareType::STEPPER || eType == EHardwareType::FAKE_STEPPER_MOTOR)
86  { // stepper
87  std::string currentStepperNamespace = "steppers/stepper_" + to_string(currentIdStepper);
88 
89  auto stepperState = std::make_shared<StepperMotorState>(joint_name, eType, common::model::EComponentType::JOINT,
90  eBusProto, static_cast<uint8_t>(joint_id_config));
91  if (stepperState)
92  {
93  double offsetPos = 0.0;
94  double gear_ratio = 1.0;
95  int direction = 1;
96  double max_effort = 0.0;
97  double home_position = 0.0;
98  double limit_position_min = 0.0;
99  double limit_position_max = 0.0;
100  double motor_ratio = 0.0;
101 
102  robot_hwnh.getParam(currentStepperNamespace + "/offset_position", offsetPos);
103  robot_hwnh.getParam(currentStepperNamespace + "/gear_ratio", gear_ratio);
104  robot_hwnh.getParam(currentStepperNamespace + "/direction", direction);
105  robot_hwnh.getParam(currentStepperNamespace + "/max_effort", max_effort);
106  robot_hwnh.getParam(currentStepperNamespace + "/default_home_position", home_position);
107  robot_hwnh.getParam(currentStepperNamespace + "/limit_position_min", limit_position_min);
108  robot_hwnh.getParam(currentStepperNamespace + "/limit_position_max", limit_position_max);
109  robot_hwnh.getParam(currentStepperNamespace + "/motor_ratio", motor_ratio);
110 
111  // acceleration and velocity profiles
112  common::model::VelocityProfile profile{};
113  int data{};
114  if (robot_hwnh.hasParam(currentStepperNamespace + "/v_start"))
115  {
116  robot_hwnh.getParam(currentStepperNamespace + "/v_start", data);
117  profile.v_start = static_cast<uint32_t>(data);
118  }
119 
120  if (robot_hwnh.hasParam(currentStepperNamespace + "/a_1"))
121  {
122  robot_hwnh.getParam(currentStepperNamespace + "/a_1", data);
123  profile.a_1 = static_cast<uint32_t>(data);
124  }
125  if (robot_hwnh.hasParam(currentStepperNamespace + "/v_1"))
126  {
127  robot_hwnh.getParam(currentStepperNamespace + "/v_1", data);
128  profile.v_1 = static_cast<uint32_t>(data);
129  }
130  if (robot_hwnh.hasParam(currentStepperNamespace + "/a_max"))
131  {
132  robot_hwnh.getParam(currentStepperNamespace + "/a_max", data);
133  profile.a_max = static_cast<uint32_t>(data);
134  }
135  if (robot_hwnh.hasParam(currentStepperNamespace + "/v_max"))
136  {
137  robot_hwnh.getParam(currentStepperNamespace + "/v_max", data);
138  profile.v_max = static_cast<uint32_t>(data);
139  }
140  if (robot_hwnh.hasParam(currentStepperNamespace + "/d_max"))
141  {
142  robot_hwnh.getParam(currentStepperNamespace + "/d_max", data);
143  profile.d_max = static_cast<uint32_t>(data);
144  }
145  if (robot_hwnh.hasParam(currentStepperNamespace + "/d_1"))
146  {
147  robot_hwnh.getParam(currentStepperNamespace + "/d_1", data);
148  profile.d_1 = static_cast<uint32_t>(data);
149  }
150  if (robot_hwnh.hasParam(currentStepperNamespace + "/v_stop"))
151  {
152  robot_hwnh.getParam(currentStepperNamespace + "/v_stop", data);
153  profile.v_stop = static_cast<uint32_t>(data);
154  }
155 
156  // add parameters
157  stepperState->setOffsetPosition(offsetPos);
158  stepperState->setGearRatio(gear_ratio);
159  stepperState->setDirection(static_cast<int8_t>(direction));
160  stepperState->setMaxEffort(max_effort);
161  stepperState->setHomePosition(home_position);
162  stepperState->setLimitPositionMax(limit_position_max);
163  stepperState->setLimitPositionMin(limit_position_min);
164  stepperState->setMotorRatio(motor_ratio);
165  stepperState->setVelocityProfile(profile);
166 
167  if (eBusProto == EBusProtocol::TTL)
168  ttl_interface->addJoint(stepperState);
169 
170  currentIdStepper++;
171  }
172  }
173  else if (eType != EHardwareType::UNKNOWN)
174  { // dynamixel
175  auto dxlState = std::make_shared<DxlMotorState>(joint_name, eType, common::model::EComponentType::JOINT,
176  static_cast<uint8_t>(joint_id_config));
177  if (dxlState)
178  {
179  double offsetPos = 0.0;
180  int direction = 1;
181  int positionPGain = 0;
182  int positionIGain = 0;
183  int positionDGain = 0;
184  int velocityPGain = 0;
185  int velocityIGain = 0;
186  int FF1Gain = 0;
187  int FF2Gain = 0;
188  int velocityProfile = 0;
189  int accelerationProfile = 0;
190  double limit_position_min = 0.0;
191  double limit_position_max = 0.0;
192  double home_position = 0.0;
193 
194  std::string currentDxlNamespace = "dynamixels/dxl_" + to_string(currentIdDxl);
195 
196  robot_hwnh.getParam(currentDxlNamespace + "/offset_position", offsetPos);
197  robot_hwnh.getParam(currentDxlNamespace + "/direction", direction);
198 
199  robot_hwnh.getParam(currentDxlNamespace + "/position_P_gain", positionPGain);
200  robot_hwnh.getParam(currentDxlNamespace + "/position_I_gain", positionIGain);
201  robot_hwnh.getParam(currentDxlNamespace + "/position_D_gain", positionDGain);
202 
203  robot_hwnh.getParam(currentDxlNamespace + "/velocity_P_gain", velocityPGain);
204  robot_hwnh.getParam(currentDxlNamespace + "/velocity_I_gain", velocityIGain);
205 
206  robot_hwnh.getParam(currentDxlNamespace + "/FF1_gain", FF1Gain);
207  robot_hwnh.getParam(currentDxlNamespace + "/FF2_gain", FF2Gain);
208 
209  robot_hwnh.getParam(currentDxlNamespace + "/velocity_profile", velocityProfile);
210  robot_hwnh.getParam(currentDxlNamespace + "/acceleration_profile", accelerationProfile);
211 
212  robot_hwnh.getParam(currentDxlNamespace + "/default_home_position", home_position);
213  robot_hwnh.getParam(currentDxlNamespace + "/limit_position_min", limit_position_min);
214  robot_hwnh.getParam(currentDxlNamespace + "/limit_position_max", limit_position_max);
215 
216  dxlState->setOffsetPosition(offsetPos);
217  dxlState->setDirection(static_cast<int8_t>(direction));
218 
219  dxlState->setPositionPGain(static_cast<uint32_t>(positionPGain));
220  dxlState->setPositionIGain(static_cast<uint32_t>(positionIGain));
221  dxlState->setPositionDGain(static_cast<uint32_t>(positionDGain));
222 
223  dxlState->setVelocityPGain(static_cast<uint32_t>(velocityPGain));
224  dxlState->setVelocityIGain(static_cast<uint32_t>(velocityIGain));
225 
226  dxlState->setFF1Gain(static_cast<uint32_t>(FF1Gain));
227  dxlState->setFF2Gain(static_cast<uint32_t>(FF2Gain));
228 
229  dxlState->setVelProfile(static_cast<uint32_t>(velocityProfile));
230  dxlState->setAccProfile(static_cast<uint32_t>(accelerationProfile));
231 
232  dxlState->setLimitPositionMin(limit_position_min);
233  dxlState->setLimitPositionMax(limit_position_max);
234 
235  dxlState->setHomePosition(home_position);
236 
237  if (eBusProto == EBusProtocol::TTL)
238  ttl_interface->addJoint(dxlState);
239 
240  currentIdDxl++;
241  }
242  }
243  } // end for (size_t j = 0; j < nb_joints; j++)
244 }
245 
250 void addJointToTtlManager(const std::shared_ptr<ttl_driver::TtlManager> &ttl_drv)
251 {
252  size_t nb_joints = 0;
253 
254  ros::NodeHandle robot_hwnh("joints_interface");
255  // retrieve nb joints with checking that the config param exists for both name and id
256  while (robot_hwnh.hasParam("joint_" + to_string(nb_joints + 1) + "/id") &&
257  robot_hwnh.hasParam("joint_" + to_string(nb_joints + 1) + "/name") &&
258  robot_hwnh.hasParam("joint_" + to_string(nb_joints + 1) + "/type") &&
259  robot_hwnh.hasParam("joint_" + to_string(nb_joints + 1) + "/bus"))
260  nb_joints++;
261 
262  // connect and register joint state interface
263  int currentIdStepper = 1;
264  int currentIdDxl = 1;
265 
266  for (size_t j = 0; j < nb_joints; j++)
267  {
268  int joint_id_config = 0;
269  string joint_name;
270  string joint_type;
271  string joint_bus;
272 
273  robot_hwnh.getParam("joint_" + to_string(j + 1) + "/id", joint_id_config);
274  robot_hwnh.getParam("joint_" + to_string(j + 1) + "/name", joint_name);
275  robot_hwnh.getParam("joint_" + to_string(j + 1) + "/type", joint_type);
276  robot_hwnh.getParam("joint_" + to_string(j + 1) + "/bus", joint_bus);
277 
278  HardwareTypeEnum eType = HardwareTypeEnum(joint_type.c_str());
279  BusProtocolEnum eBusProto = BusProtocolEnum(joint_bus.c_str());
280 
281  if (eType == EHardwareType::STEPPER || eType == EHardwareType::FAKE_STEPPER_MOTOR)
282  { // stepper
283  std::string currentStepperNamespace = "steppers/stepper_" + to_string(currentIdStepper);
284 
285  auto stepperState = std::make_shared<StepperMotorState>(joint_name, eType, common::model::EComponentType::JOINT,
286  eBusProto, static_cast<uint8_t>(joint_id_config));
287  if (stepperState)
288  {
289  double offsetPos = 0.0;
290  double gear_ratio = 1.0;
291  int direction = 1;
292  double max_effort = 0.0;
293  double home_position = 0.0;
294  double limit_position_min = 0.0;
295  double limit_position_max = 0.0;
296  double motor_ratio = 0.0;
297 
298  robot_hwnh.getParam(currentStepperNamespace + "/offset_position", offsetPos);
299  robot_hwnh.getParam(currentStepperNamespace + "/gear_ratio", gear_ratio);
300  robot_hwnh.getParam(currentStepperNamespace + "/direction", direction);
301  robot_hwnh.getParam(currentStepperNamespace + "/max_effort", max_effort);
302  robot_hwnh.getParam(currentStepperNamespace + "/default_home_position", home_position);
303  robot_hwnh.getParam(currentStepperNamespace + "/limit_position_min", limit_position_min);
304  robot_hwnh.getParam(currentStepperNamespace + "/limit_position_max", limit_position_max);
305  robot_hwnh.getParam(currentStepperNamespace + "/motor_ratio", motor_ratio);
306 
307  // acceleration and velocity profiles
308  common::model::VelocityProfile profile{};
309  int data{};
310  if (robot_hwnh.hasParam(currentStepperNamespace + "/v_start"))
311  {
312  robot_hwnh.getParam(currentStepperNamespace + "/v_start", data);
313  profile.v_start = static_cast<uint32_t>(data);
314  }
315 
316  if (robot_hwnh.hasParam(currentStepperNamespace + "/a_1"))
317  {
318  robot_hwnh.getParam(currentStepperNamespace + "/a_1", data);
319  profile.a_1 = static_cast<uint32_t>(data);
320  }
321  if (robot_hwnh.hasParam(currentStepperNamespace + "/v_1"))
322  {
323  robot_hwnh.getParam(currentStepperNamespace + "/v_1", data);
324  profile.v_1 = static_cast<uint32_t>(data);
325  }
326  if (robot_hwnh.hasParam(currentStepperNamespace + "/a_max"))
327  {
328  robot_hwnh.getParam(currentStepperNamespace + "/a_max", data);
329  profile.a_max = static_cast<uint32_t>(data);
330  }
331  if (robot_hwnh.hasParam(currentStepperNamespace + "/v_max"))
332  {
333  robot_hwnh.getParam(currentStepperNamespace + "/v_max", data);
334  profile.v_max = static_cast<uint32_t>(data);
335  }
336  if (robot_hwnh.hasParam(currentStepperNamespace + "/d_max"))
337  {
338  robot_hwnh.getParam(currentStepperNamespace + "/d_max", data);
339  profile.d_max = static_cast<uint32_t>(data);
340  }
341  if (robot_hwnh.hasParam(currentStepperNamespace + "/d_1"))
342  {
343  robot_hwnh.getParam(currentStepperNamespace + "/d_1", data);
344  profile.d_1 = static_cast<uint32_t>(data);
345  }
346  if (robot_hwnh.hasParam(currentStepperNamespace + "/v_stop"))
347  {
348  robot_hwnh.getParam(currentStepperNamespace + "/v_stop", data);
349  profile.v_stop = static_cast<uint32_t>(data);
350  }
351 
352  // add parameters
353  stepperState->setOffsetPosition(offsetPos);
354  stepperState->setGearRatio(gear_ratio);
355  stepperState->setDirection(static_cast<int8_t>(direction));
356  stepperState->setMaxEffort(max_effort);
357  stepperState->setHomePosition(home_position);
358  stepperState->setLimitPositionMax(limit_position_max);
359  stepperState->setLimitPositionMin(limit_position_min);
360  stepperState->setMotorRatio(motor_ratio);
361  stepperState->setVelocityProfile(profile);
362 
363  if (eBusProto == EBusProtocol::TTL)
364  {
365  ttl_drv->addHardwareComponent(stepperState);
366  }
367  currentIdStepper++;
368  }
369  }
370  else if (eType != EHardwareType::UNKNOWN)
371  { // dynamixel
372  auto dxlState = std::make_shared<DxlMotorState>(joint_name, eType, common::model::EComponentType::JOINT,
373  static_cast<uint8_t>(joint_id_config));
374  if (dxlState)
375  {
376  double offsetPos = 0.0;
377  int direction = 1;
378  int positionPGain = 0;
379  int positionIGain = 0;
380  int positionDGain = 0;
381  int velocityPGain = 0;
382  int velocityIGain = 0;
383  int FF1Gain = 0;
384  int FF2Gain = 0;
385  int velocityProfile = 0;
386  int accelerationProfile = 0;
387  double limit_position_min = 0.0;
388  double limit_position_max = 0.0;
389  double home_position = 0.0;
390 
391  std::string currentDxlNamespace = "dynamixels/dxl_" + to_string(currentIdDxl);
392 
393  robot_hwnh.getParam(currentDxlNamespace + "/offset_position", offsetPos);
394  robot_hwnh.getParam(currentDxlNamespace + "/direction", direction);
395 
396  robot_hwnh.getParam(currentDxlNamespace + "/position_P_gain", positionPGain);
397  robot_hwnh.getParam(currentDxlNamespace + "/position_I_gain", positionIGain);
398  robot_hwnh.getParam(currentDxlNamespace + "/position_D_gain", positionDGain);
399 
400  robot_hwnh.getParam(currentDxlNamespace + "/velocity_P_gain", velocityPGain);
401  robot_hwnh.getParam(currentDxlNamespace + "/velocity_I_gain", velocityIGain);
402 
403  robot_hwnh.getParam(currentDxlNamespace + "/FF1_gain", FF1Gain);
404  robot_hwnh.getParam(currentDxlNamespace + "/FF2_gain", FF2Gain);
405 
406  robot_hwnh.getParam(currentDxlNamespace + "/velocity_profile", velocityProfile);
407  robot_hwnh.getParam(currentDxlNamespace + "/acceleration_profile", accelerationProfile);
408 
409  robot_hwnh.getParam(currentDxlNamespace + "/default_home_position", home_position);
410  robot_hwnh.getParam(currentDxlNamespace + "/limit_position_min", limit_position_min);
411  robot_hwnh.getParam(currentDxlNamespace + "/limit_position_max", limit_position_max);
412 
413  dxlState->setOffsetPosition(offsetPos);
414  dxlState->setDirection(static_cast<int8_t>(direction));
415 
416  dxlState->setPositionPGain(static_cast<uint32_t>(positionPGain));
417  dxlState->setPositionIGain(static_cast<uint32_t>(positionIGain));
418  dxlState->setPositionDGain(static_cast<uint32_t>(positionDGain));
419 
420  dxlState->setVelocityPGain(static_cast<uint32_t>(velocityPGain));
421  dxlState->setVelocityIGain(static_cast<uint32_t>(velocityIGain));
422 
423  dxlState->setFF1Gain(static_cast<uint32_t>(FF1Gain));
424  dxlState->setFF2Gain(static_cast<uint32_t>(FF2Gain));
425 
426  dxlState->setVelProfile(static_cast<uint32_t>(velocityProfile));
427  dxlState->setAccProfile(static_cast<uint32_t>(accelerationProfile));
428 
429  dxlState->setLimitPositionMin(limit_position_min);
430  dxlState->setLimitPositionMax(limit_position_max);
431 
432  dxlState->setHomePosition(home_position);
433 
434  if (eBusProto == EBusProtocol::TTL)
435  {
436  ttl_drv->addHardwareComponent(dxlState);
437  }
438  currentIdDxl++;
439  }
440  }
441  } // end for (size_t j = 0; j < nb_joints; j++)
442 }
443 
444 // Declare a test
445 /******************************************************/
446 /************ Tests of ttl interface ******************/
447 /******************************************************/
451 class TtlInterfaceTestSuite : public ::testing::Test
452 {
453 protected:
454  static void SetUpTestCase()
455  {
456  ros::NodeHandle nh("ttl_driver");
457  ros::Duration(5.0).sleep();
458 
459  ttl_interface = std::make_shared<ttl_driver::TtlInterfaceCore>(nh);
460 
461  addJointToTtlInterface(ttl_interface);
462  // check connections
463  EXPECT_TRUE(ttl_interface->isConnectionOk());
464  EXPECT_TRUE(ttl_interface->scanMotorId(2));
465  EXPECT_TRUE(ttl_interface->scanMotorId(3));
466  EXPECT_TRUE(ttl_interface->scanMotorId(6));
467  }
468 
469  static void TearDownTestCase()
470  {
471  ros::shutdown();
472  }
473 
474  static std::shared_ptr<ttl_driver::TtlInterfaceCore> ttl_interface;
475 };
476 
477 std::shared_ptr<ttl_driver::TtlInterfaceCore> TtlInterfaceTestSuite::ttl_interface;
478 
479 // Test reboot motors
480 TEST_F(TtlInterfaceTestSuite, testRebootMotors)
481 {
482  int resutl = ttl_interface->rebootHardware(ttl_interface->getJointState(2));
483  EXPECT_EQ(resutl, static_cast<int>(niryo_robot_msgs::CommandStatus::SUCCESS));
484 }
485 
486 // Test reboot motor with wrong id
487 TEST_F(TtlInterfaceTestSuite, testRebootMotorsWrongID)
488 {
489  bool result;
490  result = ttl_interface->rebootHardware(std::make_shared<common::model::DxlMotorState>());
491  EXPECT_FALSE(result);
492 }
493 
497 class TtlManagerTestSuite : public ::testing::Test
498 {
499 protected:
500  static void SetUpTestCase()
501  {
502  ros::NodeHandle nh("ttl_driver");
503 
504  ttl_drv = std::make_shared<ttl_driver::TtlManager>(nh);
505 
506  addJointToTtlManager(ttl_drv);
507  // check connections
508  EXPECT_TRUE(ttl_drv->ping(2));
509  EXPECT_TRUE(ttl_drv->ping(3));
510  EXPECT_TRUE(ttl_drv->ping(6));
511  }
512 
513  static std::shared_ptr<ttl_driver::TtlManager> ttl_drv;
514 };
515 
516 std::shared_ptr<ttl_driver::TtlManager> TtlManagerTestSuite::ttl_drv;
517 
518 /******************************************************/
519 /************** Tests of ttl manager ******************/
520 /******************************************************/
521 
522 // Test driver received cmd
523 
524 TEST_F(TtlManagerTestSuite, testSingleCmds)
525 {
526  auto cmd_1 = std::make_unique<common::model::DxlSingleCmd>(common::model::EDxlCommandType::CMD_TYPE_TORQUE, 2,
527  std::initializer_list<uint32_t>{ 1 });
528  EXPECT_EQ(ttl_drv->writeSingleCommand(std::move(cmd_1)), COMM_SUCCESS);
529  ros::Duration(0.01).sleep();
530 
531  // wrong id
532  auto cmd_2 = std::make_unique<common::model::DxlSingleCmd>(common::model::EDxlCommandType::CMD_TYPE_TORQUE, 20,
533  std::initializer_list<uint32_t>{ 1 });
534  EXPECT_NE(ttl_drv->writeSingleCommand(std::move(cmd_2)), COMM_SUCCESS);
535  ros::Duration(0.01).sleep();
536 
537  // wrong type cmd
538  auto cmd_3 = std::make_unique<common::model::DxlSingleCmd>(common::model::EDxlCommandType::CMD_TYPE_UNKNOWN, 2,
539  std::initializer_list<uint32_t>{ 1 });
540  EXPECT_NE(ttl_drv->writeSingleCommand(std::move(cmd_3)), COMM_SUCCESS);
541  ros::Duration(0.01).sleep();
542 
543  // wrong type of cmd object
544  auto cmd_4 = std::make_unique<common::model::StepperTtlSingleCmd>(common::model::EStepperCommandType::CMD_TYPE_TORQUE,
545  5, std::initializer_list<uint32_t>{ 1 });
546  EXPECT_NE(ttl_drv->writeSingleCommand(std::move(cmd_4)), COMM_SUCCESS);
547 }
548 
549 // Test control cmds
550 TEST_F(TtlManagerTestSuite, testSingleControlCmds)
551 {
552  auto cmd_1_torque = std::make_unique<common::model::DxlSingleCmd>(common::model::EDxlCommandType::CMD_TYPE_TORQUE, 2,
553  std::initializer_list<uint32_t>{ 1 });
554 
555  EXPECT_EQ(ttl_drv->writeSingleCommand(std::move(cmd_1_torque)), COMM_SUCCESS);
556  ros::Duration(0.01).sleep();
557 
558  auto cmd_2_torque = std::make_unique<common::model::DxlSingleCmd>(common::model::EDxlCommandType::CMD_TYPE_TORQUE, 3,
559  std::initializer_list<uint32_t>{ 1 });
560  EXPECT_EQ(ttl_drv->writeSingleCommand(std::move(cmd_2_torque)), COMM_SUCCESS);
561  ros::Duration(0.01).sleep();
562 
563  auto cmd_3_torque = std::make_unique<common::model::DxlSingleCmd>(common::model::EDxlCommandType::CMD_TYPE_TORQUE, 6,
564  std::initializer_list<uint32_t>{ 1 });
565  EXPECT_EQ(ttl_drv->writeSingleCommand(std::move(cmd_3_torque)), COMM_SUCCESS);
566  ros::Duration(0.01).sleep();
567 
568  ttl_drv->readJointsStatus();
569  auto state_motor_2 = std::dynamic_pointer_cast<common::model::JointState>(ttl_drv->getHardwareState(2));
570  assert(state_motor_2);
571  auto state_motor_3 = std::dynamic_pointer_cast<common::model::JointState>(ttl_drv->getHardwareState(3));
572  assert(state_motor_3);
573  auto state_motor_6 = std::dynamic_pointer_cast<common::model::JointState>(ttl_drv->getHardwareState(6));
574  assert(state_motor_6);
575 
576  auto new_pos_2 = static_cast<uint32_t>(state_motor_2->to_motor_pos(state_motor_2->getHomePosition()));
577  auto new_pos_3 = static_cast<uint32_t>(state_motor_3->to_motor_pos(state_motor_3->getHomePosition()));
578  auto new_pos_6 = static_cast<uint32_t>(state_motor_6->to_motor_pos(state_motor_6->getHomePosition()));
579 
580  // single control cmd for stepper ttl id 2
581  auto cmd_1 = std::make_unique<common::model::DxlSingleCmd>(common::model::EDxlCommandType::CMD_TYPE_POSITION, 2,
582  std::initializer_list<uint32_t>{ new_pos_2 });
583  EXPECT_EQ(ttl_drv->writeSingleCommand(std::move(cmd_1)), COMM_SUCCESS);
584  ros::Duration(0.5).sleep();
585 
586  // single control cmd for stepper ttl id 3
587  auto cmd_2 = std::make_unique<common::model::DxlSingleCmd>(common::model::EDxlCommandType::CMD_TYPE_POSITION, 3,
588  std::initializer_list<uint32_t>{ new_pos_3 });
589  EXPECT_EQ(ttl_drv->writeSingleCommand(std::move(cmd_2)), COMM_SUCCESS);
590  ros::Duration(0.5).sleep();
591 
592  // single control cmd for dxl ttl id 6
593  auto cmd_3 = std::make_unique<common::model::DxlSingleCmd>(common::model::EDxlCommandType::CMD_TYPE_POSITION, 6,
594  std::initializer_list<uint32_t>{ new_pos_6 });
595  EXPECT_EQ(ttl_drv->writeSingleCommand(std::move(cmd_3)), COMM_SUCCESS);
596  ros::Duration(0.5).sleep();
597 
598  ttl_drv->readJointsStatus();
599 
600  // can miss 1 2 steps
601  EXPECT_NEAR(state_motor_2->getPosition(), new_pos_2, 2);
602  EXPECT_NEAR(state_motor_3->getPosition(), new_pos_3, 2);
603  EXPECT_NEAR(state_motor_6->getPosition(), new_pos_6, 2);
604 }
605 
606 TEST_F(TtlManagerTestSuite, testSyncCmds)
607 {
608  common::model::EHardwareType dxl_type;
609 
610  dxl_type = common::model::EHardwareType::FAKE_DXL_MOTOR;
611 
612  // sync cmd
613  auto dynamixel_cmd_1 = std::make_unique<common::model::DxlSyncCmd>(common::model::EDxlCommandType::CMD_TYPE_TORQUE);
614  dynamixel_cmd_1->addMotorParam(dxl_type, 2, 1);
615  dynamixel_cmd_1->addMotorParam(dxl_type, 6, 1);
616 
617  EXPECT_EQ(ttl_drv->writeSynchronizeCommand(std::move(dynamixel_cmd_1)), COMM_SUCCESS);
618  ros::Duration(0.5).sleep();
619 
620  // redondant id
621  auto dynamixel_cmd_3 = std::make_unique<common::model::DxlSyncCmd>(common::model::EDxlCommandType::CMD_TYPE_TORQUE);
622  dynamixel_cmd_3->addMotorParam(dxl_type, 2, 1);
623  dynamixel_cmd_3->addMotorParam(dxl_type, 2, 1);
624 
625  EXPECT_NE(ttl_drv->writeSynchronizeCommand(std::move(dynamixel_cmd_3)), COMM_SUCCESS);
626 
627  // wrong cmd type
628  auto dynamixel_cmd_4 = std::make_unique<common::model::DxlSyncCmd>(common::model::EDxlCommandType::CMD_TYPE_UNKNOWN);
629  dynamixel_cmd_4->addMotorParam(dxl_type, 2, 1);
630  dynamixel_cmd_4->addMotorParam(dxl_type, 6, 1);
631 
632  EXPECT_NE(ttl_drv->writeSynchronizeCommand(std::move(dynamixel_cmd_4)), COMM_SUCCESS);
633 }
634 
635 TEST_F(TtlManagerTestSuite, testSyncControlCmds)
636 {
637  auto state_motor_2 = std::dynamic_pointer_cast<common::model::JointState>(ttl_drv->getHardwareState(2));
638  assert(state_motor_2);
639  auto state_motor_3 = std::dynamic_pointer_cast<common::model::JointState>(ttl_drv->getHardwareState(3));
640  assert(state_motor_3);
641  auto state_motor_6 = std::dynamic_pointer_cast<common::model::JointState>(ttl_drv->getHardwareState(6));
642  assert(state_motor_6);
643 
644  // sync cmd
645  auto cmd_1_torque = std::make_unique<common::model::DxlSyncCmd>(common::model::EDxlCommandType::CMD_TYPE_TORQUE);
646  cmd_1_torque->addMotorParam(state_motor_2->getHardwareType(), 2, 1);
647  cmd_1_torque->addMotorParam(state_motor_3->getHardwareType(), 3, 1);
648  cmd_1_torque->addMotorParam(state_motor_6->getHardwareType(), 6, 1);
649 
650  EXPECT_EQ(ttl_drv->writeSynchronizeCommand(std::move(cmd_1_torque)), COMM_SUCCESS);
651  ros::Duration(0.01).sleep();
652 
653  ttl_drv->readJointsStatus();
654 
655  auto new_pos_2 = static_cast<uint32_t>(state_motor_2->to_motor_pos(0.47));
656  auto new_pos_3 = static_cast<uint32_t>(state_motor_3->to_motor_pos(0.577));
657  auto new_pos_6 = static_cast<uint32_t>(state_motor_6->to_motor_pos(1.08));
658 
659  auto cmd_1 = std::make_unique<common::model::DxlSyncCmd>(common::model::EDxlCommandType::CMD_TYPE_POSITION);
660  cmd_1->addMotorParam(state_motor_2->getHardwareType(), 2, new_pos_2);
661  cmd_1->addMotorParam(state_motor_3->getHardwareType(), 3, new_pos_3);
662  cmd_1->addMotorParam(state_motor_6->getHardwareType(), 6, new_pos_6);
663 
664  EXPECT_EQ(ttl_drv->writeSynchronizeCommand(std::move(cmd_1)), COMM_SUCCESS);
665  ros::Duration(0.5).sleep();
666 
667  ttl_drv->readJointsStatus();
668 
669  EXPECT_EQ(static_cast<uint32_t>(state_motor_2->getPosition()), new_pos_2);
670  EXPECT_EQ(static_cast<uint32_t>(state_motor_3->getPosition()), new_pos_3);
671  EXPECT_EQ(static_cast<uint32_t>(state_motor_6->getPosition()), new_pos_6);
672 }
673 
674 TEST_F(TtlManagerTestSuite, testSyncControlCmdsReturnHome)
675 {
676  auto state_motor_2 = std::dynamic_pointer_cast<common::model::JointState>(ttl_drv->getHardwareState(2));
677  assert(state_motor_2);
678  auto state_motor_3 = std::dynamic_pointer_cast<common::model::JointState>(ttl_drv->getHardwareState(3));
679  assert(state_motor_3);
680  auto state_motor_6 = std::dynamic_pointer_cast<common::model::JointState>(ttl_drv->getHardwareState(6));
681  assert(state_motor_6);
682 
683  // sync cmd
684  auto cmd_1_torque = std::make_unique<common::model::DxlSyncCmd>(common::model::EDxlCommandType::CMD_TYPE_TORQUE);
685  cmd_1_torque->addMotorParam(state_motor_2->getHardwareType(), 2, 1);
686  cmd_1_torque->addMotorParam(state_motor_3->getHardwareType(), 3, 1);
687  cmd_1_torque->addMotorParam(state_motor_6->getHardwareType(), 6, 1);
688 
689  EXPECT_EQ(ttl_drv->writeSynchronizeCommand(std::move(cmd_1_torque)), COMM_SUCCESS);
690  ros::Duration(0.01).sleep();
691 
692  auto new_pos_2 = static_cast<uint32_t>(state_motor_2->to_motor_pos(state_motor_2->getHomePosition()));
693  auto new_pos_3 = static_cast<uint32_t>(state_motor_3->to_motor_pos(state_motor_3->getHomePosition()));
694  auto new_pos_6 = static_cast<uint32_t>(state_motor_6->to_motor_pos(state_motor_6->getHomePosition()));
695 
696  auto cmd_1 = std::make_unique<common::model::DxlSyncCmd>(common::model::EDxlCommandType::CMD_TYPE_POSITION);
697  cmd_1->addMotorParam(state_motor_2->getHardwareType(), 2, new_pos_2);
698  cmd_1->addMotorParam(state_motor_3->getHardwareType(), 3, new_pos_3);
699  cmd_1->addMotorParam(state_motor_6->getHardwareType(), 6, new_pos_6);
700 
701  EXPECT_EQ(ttl_drv->writeSynchronizeCommand(std::move(cmd_1)), COMM_SUCCESS);
702  ros::Duration(0.5).sleep();
703 
704  ttl_drv->readJointsStatus();
705 
706  EXPECT_EQ(static_cast<uint32_t>(state_motor_6->getPosition()), new_pos_6);
707  EXPECT_EQ(static_cast<uint32_t>(state_motor_2->getPosition()), new_pos_2);
708  EXPECT_EQ(static_cast<uint32_t>(state_motor_3->getPosition()), new_pos_3);
709 }
710 
711 // Test driver scan motors
712 TEST_F(TtlManagerTestSuite, scanTest)
713 {
714  EXPECT_EQ(ttl_drv->scanAndCheck(), COMM_SUCCESS);
715 }
716 
717 // Run all the tests that were declared with TEST()
718 int main(int argc, char **argv)
719 {
720  testing::InitGoogleTest(&argc, argv);
721  ros::init(argc, argv, "ttl_driver_unit_tests");
722 
723  std::string hardware_version;
724 
725  return RUN_ALL_TESTS();
726 }
TtlManagerTestSuite
The TtlManagerTestSuite class.
Definition: unit_tests_ned2.cpp:503
ttl_interface_core.hpp
TtlInterfaceTestSuite
The TtlInterfaceTestSuite class.
Definition: unit_tests_ned2.cpp:454
TtlInterfaceTestSuite::ttl_interface
static std::shared_ptr< ttl_driver::TtlInterfaceCore > ttl_interface
Definition: unit_tests_ned2.cpp:480
TtlInterfaceTestSuite::SetUpTestCase
static void SetUpTestCase()
Definition: unit_tests_ned_one.cpp:454
TtlManagerTestSuite::SetUpTestCase
static void SetUpTestCase()
Definition: unit_tests_ned_one.cpp:500
addJointToTtlInterface
void addJointToTtlInterface(const std::shared_ptr< ttl_driver::TtlInterfaceCore > &ttl_interface)
addJointToTtlInterface
Definition: unit_tests_ned_one.cpp:54
TtlInterfaceTestSuite::TearDownTestCase
static void TearDownTestCase()
Definition: unit_tests_ned_one.cpp:469
nh
static std::unique_ptr< ros::NodeHandle > nh
Definition: service_client_ned2.cpp:31
addJointToTtlManager
void addJointToTtlManager(const std::shared_ptr< ttl_driver::TtlManager > &ttl_drv)
addJointToTtlManager
Definition: unit_tests_ned_one.cpp:250
TEST_F
TEST_F(TtlInterfaceTestSuite, testRebootMotors)
Definition: unit_tests_ned_one.cpp:480
TtlManagerTestSuite::ttl_drv
static std::shared_ptr< ttl_driver::TtlManager > ttl_drv
Definition: unit_tests_ned2.cpp:596
ttl_manager.hpp
main
int main(int argc, char **argv)
Definition: unit_tests_ned_one.cpp:718

ttl_driver
Author(s): Clement Cocquempot
autogenerated on Fri Mar 6 2026 15:24:15