Skip to content

Commit 4610880

Browse files
committed
Align with the change in ur_msgs
1 parent b1e7ac7 commit 4610880

File tree

2 files changed

+29
-16
lines changed

2 files changed

+29
-16
lines changed

ur_controllers/src/friction_model_controller.cpp

Lines changed: 8 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -171,23 +171,25 @@ bool FrictionModelController::setFrictionModelParameters(
171171
return false;
172172
}
173173

174-
if (req->viscous_scale.size() != 6) {
174+
if (req->parameters.viscous_scale.size() != 6) {
175175
RCLCPP_ERROR(get_node()->get_logger(), "viscous_scale must have exactly 6 elements, got %zu.",
176-
req->viscous_scale.size());
176+
req->parameters.viscous_scale.size());
177177
resp->success = false;
178178
return false;
179179
}
180180

181-
if (req->coulomb_scale.size() != 6) {
181+
if (req->parameters.coulomb_scale.size() != 6) {
182182
RCLCPP_ERROR(get_node()->get_logger(), "coulomb_scale must have exactly 6 elements, got %zu.",
183-
req->coulomb_scale.size());
183+
req->parameters.coulomb_scale.size());
184184
resp->success = false;
185185
return false;
186186
}
187187

188188
FrictionModelParameters friction_model_parameters;
189-
std::copy(req->viscous_scale.begin(), req->viscous_scale.end(), friction_model_parameters.viscous_scale.begin());
190-
std::copy(req->coulomb_scale.begin(), req->coulomb_scale.end(), friction_model_parameters.coulomb_scale.begin());
189+
std::copy(req->parameters.viscous_scale.begin(), req->parameters.viscous_scale.end(),
190+
friction_model_parameters.viscous_scale.begin());
191+
std::copy(req->parameters.coulomb_scale.begin(), req->parameters.coulomb_scale.end(),
192+
friction_model_parameters.coulomb_scale.begin());
191193

192194
int tries = 0;
193195
while (!friction_model_params_buffer_.try_set(friction_model_parameters)) {

ur_robot_driver/test/integration_test_friction_model.py

Lines changed: 21 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -47,6 +47,7 @@
4747
from rclpy.node import Node
4848

4949
from controller_manager_msgs.srv import SwitchController
50+
from ur_msgs.msg import FrictionModelParameters
5051

5152
sys.path.append(os.path.dirname(__file__))
5253
from test_common import ( # noqa: E402
@@ -113,8 +114,10 @@ def test_set_friction_model_parameters(self, tf_prefix):
113114
self._friction_model_interface = FrictionModelInterface(self.node)
114115

115116
res = self._friction_model_interface.set_friction_model_parameters(
116-
viscous_scale=[0.9, 0.9, 0.8, 0.9, 0.9, 0.9],
117-
coulomb_scale=[0.8, 0.8, 0.7, 0.8, 0.8, 0.8],
117+
parameters=FrictionModelParameters(
118+
viscous_scale=[0.9, 0.9, 0.8, 0.9, 0.9, 0.9],
119+
coulomb_scale=[0.8, 0.8, 0.7, 0.8, 0.8, 0.8],
120+
),
118121
)
119122
self.assertTrue(res.success)
120123

@@ -145,15 +148,19 @@ def test_invalid_viscous_array_length(self, tf_prefix):
145148

146149
# Too few elements
147150
res = self._friction_model_interface.set_friction_model_parameters(
148-
viscous_scale=[0.9, 0.9, 0.8, 0.9, 0.9],
149-
coulomb_scale=[0.8, 0.8, 0.7, 0.8, 0.8, 0.8],
151+
parameters=FrictionModelParameters(
152+
viscous_scale=[0.9, 0.9, 0.8, 0.9, 0.9],
153+
coulomb_scale=[0.8, 0.8, 0.7, 0.8, 0.8, 0.8],
154+
),
150155
)
151156
self.assertFalse(res.success)
152157

153158
# Too many elements
154159
res = self._friction_model_interface.set_friction_model_parameters(
155-
viscous_scale=[0.9, 0.9, 0.8, 0.9, 0.9, 0.9, 0.9],
156-
coulomb_scale=[0.8, 0.8, 0.7, 0.8, 0.8, 0.8],
160+
parameters=FrictionModelParameters(
161+
viscous_scale=[0.9, 0.9, 0.8, 0.9, 0.9, 0.9, 0.9],
162+
coulomb_scale=[0.8, 0.8, 0.7, 0.8, 0.8, 0.8],
163+
),
157164
)
158165
self.assertFalse(res.success)
159166

@@ -183,8 +190,10 @@ def test_invalid_coulomb_array_length(self, tf_prefix):
183190
self._friction_model_interface = FrictionModelInterface(self.node)
184191

185192
res = self._friction_model_interface.set_friction_model_parameters(
186-
viscous_scale=[0.9, 0.9, 0.8, 0.9, 0.9, 0.9],
187-
coulomb_scale=[0.8, 0.8, 0.7],
193+
parameters=FrictionModelParameters(
194+
viscous_scale=[0.9, 0.9, 0.8, 0.9, 0.9, 0.9],
195+
coulomb_scale=[0.8, 0.8, 0.7],
196+
),
188197
)
189198
self.assertFalse(res.success)
190199

@@ -213,7 +222,9 @@ def test_set_friction_model_parameters_on_inactive_controller_fails(self, tf_pre
213222
self._friction_model_interface = FrictionModelInterface(self.node)
214223

215224
res = self._friction_model_interface.set_friction_model_parameters(
216-
viscous_scale=[0.9, 0.9, 0.8, 0.9, 0.9, 0.9],
217-
coulomb_scale=[0.8, 0.8, 0.7, 0.8, 0.8, 0.8],
225+
parameters=FrictionModelParameters(
226+
viscous_scale=[0.9, 0.9, 0.8, 0.9, 0.9, 0.9],
227+
coulomb_scale=[0.8, 0.8, 0.7, 0.8, 0.8, 0.8],
228+
),
218229
)
219230
self.assertFalse(res.success)

0 commit comments

Comments
 (0)