Skip to content

Commit f5bcaea

Browse files
urfeexurfeex
committed
Add tool_contact_controllert to controller_switch tests
1 parent d3c228c commit f5bcaea

File tree

2 files changed

+95
-6
lines changed

2 files changed

+95
-6
lines changed

‎ur_robot_driver/src/hardware_interface.cpp‎

Lines changed: 6 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -933,7 +933,7 @@ void URPositionHardwareInterface::check_tool_contact_controller()
933933

934934
if (ur_driver_ != nullptr) {
935935
if (cmd_state == 2.0) {
936-
bool success = static_cast<double>(ur_driver_->startToolContact());
936+
bool success = ur_driver_->startToolContact();
937937
if (success) {
938938
// TOOL_CONTACT_EXECUTING
939939
tool_contact_state_ = 3.0;
@@ -1131,7 +1131,7 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod
11311131
return hardware_interface::return_type::ERROR;
11321132
}
11331133
start_modes_[i].push_back(FREEDRIVE_MODE_GPIO);
1134-
} else if (key == tf_prefix + TOOL_CONTACT_GPIO + "/tool_contact_status") {
1134+
} else if (key == tf_prefix + TOOL_CONTACT_GPIO + "/tool_contact_set_state") {
11351135
if (std::any_of(start_modes_[i].begin(), start_modes_[i].end(), [&](const std::string& item) {
11361136
return item == FORCE_MODE_GPIO || item == FREEDRIVE_MODE_GPIO;
11371137
})) {
@@ -1213,11 +1213,11 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod
12131213
(std::any_of(start_modes_[0].begin(), start_modes_[0].end(),
12141214
[this](auto& item) {
12151215
return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION ||
1216-
item == FREEDRIVE_MODE_GPIO);
1216+
item == FREEDRIVE_MODE_GPIO || item == TOOL_CONTACT_GPIO);
12171217
}) ||
12181218
std::any_of(control_modes[0].begin(), control_modes[0].end(), [this](auto& item) {
12191219
return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION ||
1220-
item == FORCE_MODE_GPIO || item == FREEDRIVE_MODE_GPIO);
1220+
item == FORCE_MODE_GPIO || item == FREEDRIVE_MODE_GPIO || item == TOOL_CONTACT_GPIO);
12211221
}))) {
12221222
RCLCPP_ERROR(get_logger(), "Attempting to start force mode control while there is either position or "
12231223
"velocity mode running.");
@@ -1230,11 +1230,11 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod
12301230
(std::any_of(start_modes_[0].begin(), start_modes_[0].end(),
12311231
[this](auto& item) {
12321232
return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION ||
1233-
item == PASSTHROUGH_GPIO || item == FORCE_MODE_GPIO);
1233+
item == PASSTHROUGH_GPIO || item == FORCE_MODE_GPIO || item == TOOL_CONTACT_GPIO);
12341234
}) ||
12351235
std::any_of(control_modes[0].begin(), control_modes[0].end(), [this](auto& item) {
12361236
return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION ||
1237-
item == PASSTHROUGH_GPIO || item == FORCE_MODE_GPIO);
1237+
item == PASSTHROUGH_GPIO || item == FORCE_MODE_GPIO || item == TOOL_CONTACT_GPIO);
12381238
}))) {
12391239
RCLCPP_ERROR(get_logger(), "Attempting to start force mode control while there is either position or "
12401240
"velocity mode running.");

‎ur_robot_driver/test/integration_test_controller_switch.py‎

Lines changed: 89 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -405,3 +405,92 @@ def test_force_mode_and_trajectory_passthrough_controller_are_compatible(self):
405405
],
406406
).ok
407407
)
408+
409+
def test_tool_contact_compatibility(self):
410+
# Deactivate all writing controllers
411+
self.assertTrue(
412+
self._controller_manager_interface.switch_controller(
413+
strictness=SwitchController.Request.BEST_EFFORT,
414+
deactivate_controllers=[
415+
"scaled_joint_trajectory_controller",
416+
"joint_trajectory_controller",
417+
"forward_position_controller",
418+
"forward_velocity_controller",
419+
"passthrough_trajectory_controller",
420+
"force_mode_controller",
421+
"tool_contact_controller",
422+
],
423+
).ok
424+
)
425+
426+
time.sleep(3)
427+
# Start tool contact controller and JTC
428+
self.assertTrue(
429+
self._controller_manager_interface.switch_controller(
430+
strictness=SwitchController.Request.STRICT,
431+
activate_controllers=[
432+
"scaled_joint_trajectory_controller",
433+
"tool_contact_controller",
434+
],
435+
).ok
436+
)
437+
self.assertTrue(
438+
self._controller_manager_interface.switch_controller(
439+
strictness=SwitchController.Request.STRICT,
440+
deactivate_controllers=[
441+
"scaled_joint_trajectory_controller",
442+
"tool_contact_controller",
443+
],
444+
).ok
445+
)
446+
447+
# Start tool contact controller and passthrough trajectory
448+
self.assertTrue(
449+
self._controller_manager_interface.switch_controller(
450+
strictness=SwitchController.Request.STRICT,
451+
activate_controllers=[
452+
"passthrough_trajectory_controller",
453+
"tool_contact_controller",
454+
],
455+
).ok
456+
)
457+
self.assertTrue(
458+
self._controller_manager_interface.switch_controller(
459+
strictness=SwitchController.Request.STRICT,
460+
deactivate_controllers=[
461+
"passthrough_trajectory_controller",
462+
"tool_contact_controller",
463+
],
464+
).ok
465+
)
466+
467+
# tool contact should not start with force_mode
468+
self.assertFalse(
469+
self._controller_manager_interface.switch_controller(
470+
strictness=SwitchController.Request.STRICT,
471+
activate_controllers=[
472+
"force_mode_controller",
473+
"tool_contact_controller",
474+
],
475+
).ok
476+
)
477+
self.assertFalse(
478+
self._controller_manager_interface.switch_controller(
479+
strictness=SwitchController.Request.STRICT,
480+
activate_controllers=[
481+
"tool_contact_controller",
482+
"force_mode_controller",
483+
],
484+
).ok
485+
)
486+
487+
# tool contact should not start with freedrive
488+
self.assertFalse(
489+
self._controller_manager_interface.switch_controller(
490+
strictness=SwitchController.Request.STRICT,
491+
activate_controllers=[
492+
"tool_contact_controller",
493+
"freedrive_mode_controller",
494+
],
495+
).ok
496+
)

0 commit comments

Comments
 (0)