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

ttl_driver
Author(s): Clement Cocquempot
autogenerated on Wed May 21 2025 08:30:14