service_client_ned2.cpp
Go to the documentation of this file.
1 /*
2  ttl_driver_service_client.cpp
3  Copyright (C) 2020 Niryo
4  All rights reserved.
5 
6  This program is free software: you can redistribute it and/or modify
7  it under the terms of the GNU General Public License as published by
8  the Free Software Foundation, either version 3 of the License, or
9  (at your option) any later version.
10 
11  This program is distributed in the hope that it will be useful,
12  but WITHOUT ANY WARRANTY; without even the implied warranty of
13  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  GNU General Public License for more details.
15 
16  You should have received a copy of the GNU General Public License
17  along with this program. If not, see <http:// www.gnu.org/licenses/>.
18 */
19 
20 #include <ros/service_client.h>
21 
22 #include <gtest/gtest.h>
23 #include <ros/ros.h>
24 
25 #include "ros/duration.h"
26 #include "ttl_driver/ttl_interface_core.hpp"
27 
28 #include <memory>
29 #include <string>
30 
31 static std::unique_ptr<ros::NodeHandle> nh;
32 static bool simulation_mode;
33 
34 TEST(TESTSuite, SetLeds)
35 {
36  auto client = nh->serviceClient<niryo_robot_msgs::SetInt>("/niryo_robot/ttl_driver/set_dxl_leds");
37 
38  bool exists(client.waitForExistence(ros::Duration(1)));
39  EXPECT_TRUE(exists);
40 
41  niryo_robot_msgs::SetInt srv;
42  srv.request.value = 2;
43  client.call(srv);
44 
45  EXPECT_EQ(srv.response.status, niryo_robot_msgs::CommandStatus::SUCCESS);
46 }
47 
48 TEST(TESTSuite, WriteCustomValue)
49 {
50  auto client = nh->serviceClient<ttl_driver::WriteCustomValue>("/niryo_robot/ttl_driver/send_custom_value");
51 
52  bool exists(client.waitForExistence(ros::Duration(1)));
53  EXPECT_TRUE(exists);
54 
55  ttl_driver::WriteCustomValue srv;
56 
57  srv.request.id = 5;
58  srv.request.reg_address = 64; // Torque enable for xl430
59  srv.request.value = 1;
60  srv.request.byte_number = 1;
61 
62  client.call(srv);
63 
64  EXPECT_EQ(srv.response.status, niryo_robot_msgs::CommandStatus::SUCCESS);
65 }
66 
67 TEST(TESTSuite, WriteCustomValueWrongParam)
68 {
69  auto client = nh->serviceClient<ttl_driver::WriteCustomValue>("/niryo_robot/ttl_driver/send_custom_value");
70 
71  bool exists(client.waitForExistence(ros::Duration(1)));
72  EXPECT_TRUE(exists);
73 
74  ttl_driver::WriteCustomValue srv;
75 
76  srv.request.id = 50;
77  srv.request.reg_address = 64; // Torque enable for xl430
78  srv.request.value = 1;
79  srv.request.byte_number = 1;
80 
81  client.call(srv);
82 
83  EXPECT_EQ(srv.response.status, niryo_robot_msgs::CommandStatus::WRONG_MOTOR_TYPE);
84 }
85 
86 TEST(TESTSuite, ReadCustomValue)
87 {
88  auto client = nh->serviceClient<ttl_driver::ReadCustomValue>("/niryo_robot/ttl_driver/read_custom_value");
89 
90  bool exists(client.waitForExistence(ros::Duration(1)));
91  EXPECT_TRUE(exists);
92 
93  ttl_driver::ReadCustomValue srv;
94  srv.request.id = 5;
95  srv.request.reg_address = 64;
96  srv.request.byte_number = 1;
97 
98  client.call(srv);
99 
100  EXPECT_EQ(srv.response.status, niryo_robot_msgs::CommandStatus::SUCCESS);
101 
102  if (!simulation_mode)
103  {
104  EXPECT_EQ(srv.response.value, 1);
105  }
106 }
107 
108 TEST(TESTSuite, WritePIDValue)
109 {
110  auto client = nh->serviceClient<ttl_driver::WritePIDValue>("/niryo_robot/ttl_driver/write_pid_value");
111 
112  bool exists(client.waitForExistence(ros::Duration(1)));
113  EXPECT_TRUE(exists);
114 
115  ttl_driver::WritePIDValue srv;
116 
117  srv.request.id = 5;
118  srv.request.pos_p_gain = 1024;
119  srv.request.pos_i_gain = 3642;
120  srv.request.pos_d_gain = 11200;
121  srv.request.vel_p_gain = 0;
122  srv.request.vel_i_gain = 0;
123  srv.request.ff1_gain = 0;
124  srv.request.ff2_gain = 0;
125  srv.request.vel_profile = 0;
126  srv.request.acc_profile = 0;
127 
128  client.call(srv);
129 
130  EXPECT_EQ(srv.response.status, niryo_robot_msgs::CommandStatus::SUCCESS);
131  ros::Duration(1.0).sleep();
132 }
133 
134 TEST(TESTSuite, ReadPIDValue)
135 {
136  auto client = nh->serviceClient<ttl_driver::ReadPIDValue>("/niryo_robot/ttl_driver/read_pid_value");
137 
138  bool exists(client.waitForExistence(ros::Duration(1)));
139  EXPECT_TRUE(exists);
140 
141  ttl_driver::ReadPIDValue srv;
142 
143  srv.request.id = 5;
144 
145  bool res{false};
146  // try 3 times to read value
147  for (int i = 0; i < 5; i++)
148  {
149  client.call(srv);
150  if (srv.response.status == niryo_robot_msgs::CommandStatus::SUCCESS)
151  {
152  res = true;
153  break;
154  }
155  ros::Duration(0.5);
156  }
157 
158  ASSERT_TRUE(res);
159  EXPECT_EQ(srv.response.pos_p_gain, 1024);
160  EXPECT_EQ(srv.response.pos_i_gain, 3642);
161  EXPECT_EQ(srv.response.pos_d_gain, 11200);
162  EXPECT_EQ(srv.response.vel_p_gain, 0);
163  EXPECT_EQ(srv.response.vel_i_gain, 0);
164  EXPECT_EQ(srv.response.ff1_gain, 0);
165  EXPECT_EQ(srv.response.ff2_gain, 0);
166 }
167 
168 TEST(TESTSuite, ReadPIDValueWrongParam)
169 {
170  auto client = nh->serviceClient<ttl_driver::ReadPIDValue>("/niryo_robot/ttl_driver/read_pid_value");
171 
172  bool exists(client.waitForExistence(ros::Duration(1)));
173  EXPECT_TRUE(exists);
174 
175  ttl_driver::ReadPIDValue srv;
176 
177  srv.request.id = 20;
178 
179  client.call(srv);
180 
181  EXPECT_EQ(srv.response.status, niryo_robot_msgs::CommandStatus::FAILURE);
182 }
183 
184 TEST(TESTSuite, WriteVelocityProfile)
185 {
186  auto client = nh->serviceClient<ttl_driver::WriteVelocityProfile>("/niryo_robot/ttl_driver/write_velocity_profile");
187 
188  bool exists(client.waitForExistence(ros::Duration(1)));
189  EXPECT_TRUE(exists);
190 
191  ttl_driver::WriteVelocityProfile srv;
192 
193  srv.request.id = 2;
194  srv.request.v_start = 0;
195  srv.request.a_1 = 1260;
196  srv.request.v_1 = 500;
197  srv.request.a_max = 2500;
198  srv.request.v_max = 1500;
199  srv.request.d_max = 2500;
200  srv.request.d_1 = 1228;
201  srv.request.v_stop = 20;
202 
203  client.call(srv);
204 
205  EXPECT_EQ(srv.response.status, niryo_robot_msgs::CommandStatus::SUCCESS);
206 
207  ros::Duration(1.0).sleep();
208 }
209 
210 TEST(TESTSuite, ReadVelocityProfile)
211 {
212  auto client = nh->serviceClient<ttl_driver::ReadVelocityProfile>("/niryo_robot/ttl_driver/read_velocity_profile");
213 
214  bool exists(client.waitForExistence(ros::Duration(1)));
215  EXPECT_TRUE(exists);
216 
217  ttl_driver::ReadVelocityProfile srv;
218 
219  srv.request.id = 2;
220 
221  bool res{false};
222  // try 3 times to read value
223  for (int i = 0; i < 3; i++)
224  {
225  client.call(srv);
226  if (srv.response.status == niryo_robot_msgs::CommandStatus::SUCCESS)
227  {
228  res = true;
229  break;
230  }
231  ros::Duration(0.5);
232  }
233  ASSERT_TRUE(res);
234 
235  // In readVelocityProfile, using a convert function, so if we convert 2 times, there a very small error
236  EXPECT_DOUBLE_EQ(round(srv.response.v_start), 0.0);
237  EXPECT_DOUBLE_EQ(round(srv.response.a_1), 1260.0);
238  EXPECT_DOUBLE_EQ(round(srv.response.v_1), 500.0);
239  EXPECT_DOUBLE_EQ(round(srv.response.a_max), 2500.0);
240  EXPECT_DOUBLE_EQ(round(srv.response.v_max), 1500.0);
241  EXPECT_DOUBLE_EQ(round(srv.response.d_max), 2500.0);
242  EXPECT_DOUBLE_EQ(round(srv.response.d_1), 1228.0);
243  EXPECT_DOUBLE_EQ(round(srv.response.v_stop), 20.0);
244 }
245 
246 int main(int argc, char **argv)
247 {
248  ros::init(argc, argv, "ttl_driver_service_client");
249 
250  testing::InitGoogleTest(&argc, argv);
251 
252  nh = std::make_unique<ros::NodeHandle>("~");
253 
254  nh->getParam("simulation_mode", simulation_mode);
255 
256  ros::Duration(2.0).sleep();
257 
258  return RUN_ALL_TESTS();
259 }
ttl_interface_core.hpp
TEST
TEST(TESTSuite, SetLeds)
Definition: service_client_ned2.cpp:34
nh
static std::unique_ptr< ros::NodeHandle > nh
Definition: service_client_ned2.cpp:31
simulation_mode
static bool simulation_mode
Definition: service_client_ned2.cpp:32
main
int main(int argc, char **argv)
Definition: service_client_ned2.cpp:246

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