Skip to content

Commit a62e842

Browse files
URJalaVinDp
URJala
authored and
VinDp
committed
Implemented tool contact
1 parent c1940cf commit a62e842

File tree

5 files changed

+115
-0
lines changed

5 files changed

+115
-0
lines changed

‎ur_controllers/include/ur_controllers/gpio_controller.hpp‎

Lines changed: 11 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -79,6 +79,10 @@ enum CommandInterfaces
7979
ZERO_FTSENSOR_ASYNC_SUCCESS = 32,
8080
HAND_BACK_CONTROL_CMD = 33,
8181
HAND_BACK_CONTROL_ASYNC_SUCCESS = 34,
82+
START_TOOL_CONTACT_CMD = 35,
83+
START_TOOL_CONTACT_ASYNC_SUCCESS = 36,
84+
END_TOOL_CONTACT_CMD = 37,
85+
END_TOOL_CONTACT_ASYNC_SUCCESS = 38,
8286
};
8387

8488
enum StateInterfaces
@@ -136,6 +140,11 @@ class GPIOController : public controller_interface::ControllerInterface
136140

137141
bool zeroFTSensor(std_srvs::srv::Trigger::Request::SharedPtr req, std_srvs::srv::Trigger::Response::SharedPtr resp);
138142

143+
bool startToolContact(std_srvs::srv::Trigger::Request::SharedPtr req,
144+
std_srvs::srv::Trigger::Response::SharedPtr resp);
145+
146+
bool endToolContact(std_srvs::srv::Trigger::Request::SharedPtr req, std_srvs::srv::Trigger::Response::SharedPtr resp);
147+
139148
void publishIO();
140149

141150
void publishToolData();
@@ -163,6 +172,8 @@ class GPIOController : public controller_interface::ControllerInterface
163172
rclcpp::Service<ur_msgs::srv::SetIO>::SharedPtr set_io_srv_;
164173
rclcpp::Service<ur_msgs::srv::SetPayload>::SharedPtr set_payload_srv_;
165174
rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr tare_sensor_srv_;
175+
rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr start_tool_contact_srv_;
176+
rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr end_tool_contact_srv_;
166177

167178
std::shared_ptr<rclcpp::Publisher<ur_msgs::msg::IOStates>> io_pub_;
168179
std::shared_ptr<rclcpp::Publisher<ur_msgs::msg::ToolDataMsg>> tool_data_pub_;

‎ur_controllers/src/gpio_controller.cpp‎

Lines changed: 67 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -97,6 +97,12 @@ controller_interface::InterfaceConfiguration GPIOController::command_interface_c
9797
config.names.emplace_back(tf_prefix + "hand_back_control/hand_back_control_cmd");
9898
config.names.emplace_back(tf_prefix + "hand_back_control/hand_back_control_async_success");
9999

100+
// Start or stop tool contact functionality
101+
config.names.emplace_back(tf_prefix + "start_tool_contact/start_tool_contact_cmd");
102+
config.names.emplace_back(tf_prefix + "start_tool_contact/start_tool_contact_async_success");
103+
config.names.emplace_back(tf_prefix + "end_tool_contact/end_tool_contact_cmd");
104+
config.names.emplace_back(tf_prefix + "end_tool_contact/end_tool_contact_async_success");
105+
100106
return config;
101107
}
102108

@@ -312,6 +318,14 @@ ur_controllers::GPIOController::on_activate(const rclcpp_lifecycle::State& /*pre
312318
tare_sensor_srv_ = get_node()->create_service<std_srvs::srv::Trigger>(
313319
"~/zero_ftsensor",
314320
std::bind(&GPIOController::zeroFTSensor, this, std::placeholders::_1, std::placeholders::_2));
321+
322+
start_tool_contact_srv_ = get_node()->create_service<std_srvs::srv::Trigger>(
323+
"~/start_tool_contact",
324+
std::bind(&GPIOController::startToolContact, this, std::placeholders::_1, std::placeholders::_2));
325+
326+
end_tool_contact_srv_ = get_node()->create_service<std_srvs::srv::Trigger>(
327+
"~/end_tool_contact",
328+
std::bind(&GPIOController::endToolContact, this, std::placeholders::_1, std::placeholders::_2));
315329
} catch (...) {
316330
return LifecycleNodeInterface::CallbackReturn::ERROR;
317331
}
@@ -520,6 +534,59 @@ bool GPIOController::zeroFTSensor(std_srvs::srv::Trigger::Request::SharedPtr /*r
520534
return true;
521535
}
522536

537+
bool GPIOController::startToolContact(std_srvs::srv::Trigger::Request::SharedPtr /*req*/,
538+
std_srvs::srv::Trigger::Response::SharedPtr resp)
539+
{
540+
// reset success flag
541+
command_interfaces_[CommandInterfaces::START_TOOL_CONTACT_ASYNC_SUCCESS].set_value(ASYNC_WAITING);
542+
543+
// call the service in the hardware
544+
command_interfaces_[CommandInterfaces::START_TOOL_CONTACT_CMD].set_value(1.0);
545+
546+
if (!waitForAsyncCommand(
547+
[&]() { return command_interfaces_[CommandInterfaces::START_TOOL_CONTACT_ASYNC_SUCCESS].get_value(); })) {
548+
RCLCPP_WARN(get_node()->get_logger(), "Could not verify that tool contact was started.");
549+
}
550+
551+
resp->success =
552+
static_cast<bool>(command_interfaces_[CommandInterfaces::START_TOOL_CONTACT_ASYNC_SUCCESS].get_value());
553+
554+
if (resp->success) {
555+
RCLCPP_INFO(get_node()->get_logger(), "Successfully started tool contact");
556+
} else {
557+
RCLCPP_ERROR(get_node()->get_logger(), "Could not start tool contact");
558+
return false;
559+
}
560+
561+
return true;
562+
}
563+
564+
bool GPIOController::endToolContact(std_srvs::srv::Trigger::Request::SharedPtr /*req*/,
565+
std_srvs::srv::Trigger::Response::SharedPtr resp)
566+
{
567+
// reset success flag
568+
command_interfaces_[CommandInterfaces::END_TOOL_CONTACT_ASYNC_SUCCESS].set_value(ASYNC_WAITING);
569+
570+
// call the service in the hardware
571+
command_interfaces_[CommandInterfaces::END_TOOL_CONTACT_CMD].set_value(1.0);
572+
573+
if (!waitForAsyncCommand(
574+
[&]() { return command_interfaces_[CommandInterfaces::END_TOOL_CONTACT_ASYNC_SUCCESS].get_value(); })) {
575+
RCLCPP_WARN(get_node()->get_logger(), "Could not verify that tool contact was stopped.");
576+
}
577+
578+
resp->success = static_cast<bool>(command_interfaces_[CommandInterfaces::END_TOOL_CONTACT_ASYNC_SUCCESS].get_value());
579+
580+
if (resp->success) {
581+
RCLCPP_INFO(get_node()->get_logger(), "Successfully stopped tool contact");
582+
} else {
583+
RCLCPP_ERROR(get_node()->get_logger(), "Could not stop tool contact");
584+
return false;
585+
}
586+
587+
return true;
588+
}
589+
523590
void GPIOController::initMsgs()
524591
{
525592
io_msg_.digital_in_states.resize(standard_digital_output_cmd_.size());

‎ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -191,6 +191,10 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface
191191
bool initialized_;
192192
double system_interface_initialized_;
193193
bool async_thread_shutdown_;
194+
double start_tool_contact_cmd_;
195+
double start_tool_contact_async_success_;
196+
double end_tool_contact_cmd_;
197+
double end_tool_contact_async_success_;
194198

195199
// payload stuff
196200
urcl::vector3d_t payload_center_of_gravity_;

‎ur_robot_driver/src/hardware_interface.cpp‎

Lines changed: 23 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -298,6 +298,17 @@ std::vector<hardware_interface::CommandInterface> URPositionHardwareInterface::e
298298
command_interfaces.emplace_back(hardware_interface::CommandInterface(
299299
tf_prefix + "zero_ftsensor", "zero_ftsensor_async_success", &zero_ftsensor_async_success_));
300300

301+
command_interfaces.emplace_back(hardware_interface::CommandInterface(
302+
tf_prefix + "start_tool_contact", "start_tool_contact_cmd", &start_tool_contact_cmd_));
303+
304+
command_interfaces.emplace_back(hardware_interface::CommandInterface(
305+
tf_prefix + "start_tool_contact", "start_tool_contact_async_success", &start_tool_contact_async_success_));
306+
307+
command_interfaces.emplace_back(hardware_interface::CommandInterface(tf_prefix + "end_tool_contact",
308+
"end_tool_contact_cmd", &end_tool_contact_cmd_));
309+
310+
command_interfaces.emplace_back(hardware_interface::CommandInterface(
311+
tf_prefix + "end_tool_contact", "end_tool_contact_async_success", &end_tool_contact_async_success_));
301312
return command_interfaces;
302313
}
303314

@@ -603,6 +614,8 @@ hardware_interface::return_type URPositionHardwareInterface::read(const rclcpp::
603614
resend_robot_program_cmd_ = NO_NEW_CMD_;
604615
zero_ftsensor_cmd_ = NO_NEW_CMD_;
605616
hand_back_control_cmd_ = NO_NEW_CMD_;
617+
start_tool_contact_cmd_ = NO_NEW_CMD_;
618+
end_tool_contact_cmd_ = NO_NEW_CMD_;
606619
initialized_ = true;
607620
}
608621

@@ -727,6 +740,16 @@ void URPositionHardwareInterface::checkAsyncIO()
727740
zero_ftsensor_async_success_ = ur_driver_->zeroFTSensor();
728741
zero_ftsensor_cmd_ = NO_NEW_CMD_;
729742
}
743+
744+
if (!std::isnan(start_tool_contact_cmd_) && ur_driver_ != nullptr) {
745+
start_tool_contact_async_success_ = ur_driver_->startToolContact();
746+
start_tool_contact_cmd_ = NO_NEW_CMD_;
747+
}
748+
749+
if (!std::isnan(end_tool_contact_cmd_) && ur_driver_ != nullptr) {
750+
end_tool_contact_async_success_ = ur_driver_->endToolContact();
751+
end_tool_contact_cmd_ = NO_NEW_CMD_;
752+
}
730753
}
731754

732755
void URPositionHardwareInterface::updateNonDoubleValues()

‎ur_robot_driver/urdf/ur.ros2_control.xacro‎

Lines changed: 10 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -224,6 +224,16 @@
224224
<command_interface name="zero_ftsensor_async_success"/>
225225
</gpio>
226226

227+
<gpio name="${tf_prefix}start_tool_contact">
228+
<command_interface name="start_tool_contact_cmd"/>
229+
<command_interface name="start_tool_contact_async_success"/>
230+
</gpio>
231+
232+
<gpio name="${tf_prefix}end_tool_contact">
233+
<command_interface name="end_tool_contact_cmd"/>
234+
<command_interface name="end_tool_contact_async_success"/>
235+
</gpio>
236+
227237
<gpio name="${tf_prefix}system_interface">
228238
<state_interface name="initialized"/>
229239
</gpio>

0 commit comments

Comments
 (0)