Skip to content

Commit bdeee82

Browse files
urfeexurfeex
committed
Humble-specific changes
1 parent fa8f982 commit bdeee82

File tree

3 files changed

+47
-404
lines changed

3 files changed

+47
-404
lines changed

‎ur_controllers/src/tool_contact_controller.cpp‎

Lines changed: 15 additions & 52 deletions
Original file line numberDiff line numberDiff line change
@@ -40,10 +40,8 @@
4040

4141
#include <optional>
4242
#include <ur_controllers/tool_contact_controller.hpp>
43-
#include "controller_interface/helpers.hpp"
4443
#include <rclcpp/rclcpp.hpp>
4544
#include <rclcpp/logging.hpp>
46-
#include "std_msgs/msg/bool.hpp"
4745
#include <lifecycle_msgs/msg/state.hpp>
4846

4947
namespace ur_controllers
@@ -94,19 +92,12 @@ controller_interface::InterfaceConfiguration ToolContactController::state_interf
9492
controller_interface::CallbackReturn
9593
ToolContactController::on_activate(const rclcpp_lifecycle::State& /* previous_state */)
9694
{
97-
std::optional<double> val;
9895
{
9996
const std::string interface_name = tool_contact_params_.tf_prefix + "tool_contact/tool_contact_state";
10097
auto it = std::find_if(state_interfaces_.begin(), state_interfaces_.end(),
10198
[&](auto& interface) { return (interface.get_name() == interface_name); });
10299
if (it != state_interfaces_.end()) {
103100
tool_contact_state_interface_ = *it;
104-
val = tool_contact_state_interface_->get().get_optional();
105-
if (!val) {
106-
RCLCPP_ERROR(get_node()->get_logger(),
107-
"Failed to read '%s' state interface, aborting activation of controller.", interface_name.c_str());
108-
return controller_interface::CallbackReturn::ERROR;
109-
}
110101
} else {
111102
RCLCPP_ERROR(get_node()->get_logger(), "Did not find '%s' in state interfaces.", interface_name.c_str());
112103
return controller_interface::CallbackReturn::ERROR;
@@ -118,12 +109,7 @@ ToolContactController::on_activate(const rclcpp_lifecycle::State& /* previous_st
118109
[&](auto& interface) { return (interface.get_name() == interface_name); });
119110
if (it != command_interfaces_.end()) {
120111
tool_contact_set_state_interface_ = *it;
121-
if (!tool_contact_set_state_interface_->get().set_value(TOOL_CONTACT_STANDBY)) {
122-
RCLCPP_ERROR(get_node()->get_logger(),
123-
"Failed to set '%s' command interface, aborting activation of controller.",
124-
interface_name.c_str());
125-
return controller_interface::CallbackReturn::ERROR;
126-
}
112+
tool_contact_set_state_interface_->get().set_value(TOOL_CONTACT_STANDBY);
127113
} else {
128114
RCLCPP_ERROR(get_node()->get_logger(), "Did not find '%s' in command interfaces.", interface_name.c_str());
129115
return controller_interface::CallbackReturn::ERROR;
@@ -135,12 +121,6 @@ ToolContactController::on_activate(const rclcpp_lifecycle::State& /* previous_st
135121
[&](auto& interface) { return (interface.get_name() == interface_name); });
136122
if (it != state_interfaces_.end()) {
137123
tool_contact_result_interface_ = *it;
138-
val = tool_contact_result_interface_->get().get_optional();
139-
if (!val) {
140-
RCLCPP_ERROR(get_node()->get_logger(),
141-
"Failed to read '%s' state interface, aborting activation of controller.", interface_name.c_str());
142-
return controller_interface::CallbackReturn::ERROR;
143-
}
144124
} else {
145125
RCLCPP_ERROR(get_node()->get_logger(), "Did not find '%s' in state interfaces.", interface_name.c_str());
146126
return controller_interface::CallbackReturn::ERROR;
@@ -152,13 +132,8 @@ ToolContactController::on_activate(const rclcpp_lifecycle::State& /* previous_st
152132
[&](auto& interface) { return (interface.get_name() == interface_name); });
153133
if (it != state_interfaces_.end()) {
154134
major_version_state_interface_ = *it;
155-
std::optional<double> major_version = major_version_state_interface_->get().get_optional();
156-
if (!major_version) {
157-
RCLCPP_ERROR(get_node()->get_logger(),
158-
"Failed to read '%s' state interface, aborting activation of controller.", interface_name.c_str());
159-
return controller_interface::CallbackReturn::ERROR;
160-
}
161-
if (major_version.value() < 5) {
135+
double major_version = major_version_state_interface_->get().get_value();
136+
if (major_version < 5) {
162137
RCLCPP_ERROR(get_node()->get_logger(), "This feature is not supported on CB3 robots, controller will not be "
163138
"started.");
164139
return controller_interface::CallbackReturn::ERROR;
@@ -204,7 +179,7 @@ rclcpp_action::GoalResponse ToolContactController::goal_received_callback(
204179
{
205180
RCLCPP_INFO(get_node()->get_logger(), "New goal received.");
206181

207-
if (get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) {
182+
if (get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) {
208183
RCLCPP_ERROR(get_node()->get_logger(), "Tool contact controller is not in active state, can not accept action "
209184
"goals.");
210185
return rclcpp_action::GoalResponse::REJECT;
@@ -270,26 +245,18 @@ rclcpp_action::CancelResponse ToolContactController::goal_canceled_callback(
270245
controller_interface::return_type ToolContactController::update(const rclcpp::Time& /* time */,
271246
const rclcpp::Duration& /* period */)
272247
{
273-
static bool write_success = true;
274-
275248
// Abort takes priority
276249
if (tool_contact_abort_) {
277250
tool_contact_abort_ = false;
278251
tool_contact_enable_ = false;
279-
write_success &= tool_contact_set_state_interface_->get().set_value(TOOL_CONTACT_WAITING_END);
252+
tool_contact_set_state_interface_->get().set_value(TOOL_CONTACT_WAITING_END);
280253
} else if (tool_contact_enable_) {
281254
tool_contact_enable_ = false;
282-
write_success &= tool_contact_set_state_interface_->get().set_value(TOOL_CONTACT_WAITING_BEGIN);
255+
tool_contact_set_state_interface_->get().set_value(TOOL_CONTACT_WAITING_BEGIN);
283256
}
284257

285258
const auto active_goal = *rt_active_goal_.readFromRT();
286-
std::optional<double> state_optional = tool_contact_state_interface_->get().get_optional();
287-
288-
if (!state_optional) {
289-
RCLCPP_FATAL(get_node()->get_logger(), "Controller failed to read state interface, aborting.");
290-
return controller_interface::return_type::ERROR;
291-
}
292-
const int state = static_cast<int>(state_optional.value());
259+
const int state = static_cast<int>(tool_contact_state_interface_->get().get_value());
293260

294261
switch (state) {
295262
case static_cast<int>(TOOL_CONTACT_EXECUTING):
@@ -299,26 +266,26 @@ controller_interface::return_type ToolContactController::update(const rclcpp::Ti
299266
RCLCPP_INFO(get_node()->get_logger(), "Tool contact enabled successfully.");
300267
logged_once_ = true;
301268
}
302-
std::optional<double> result = tool_contact_result_interface_->get().get_optional();
269+
double result = tool_contact_result_interface_->get().get_value();
303270
if (!result) {
304271
RCLCPP_FATAL(get_node()->get_logger(), "Controller failed to read result interface, aborting.");
305272
return controller_interface::return_type::ERROR;
306273
}
307-
if (result.value() == 0.0) {
274+
if (result == 0.0) {
308275
tool_contact_active_ = false;
309276
RCLCPP_INFO(get_node()->get_logger(), "Tool contact finished successfully.");
310277

311-
write_success &= tool_contact_set_state_interface_->get().set_value(TOOL_CONTACT_WAITING_END);
278+
tool_contact_set_state_interface_->get().set_value(TOOL_CONTACT_WAITING_END);
312279
if (active_goal) {
313280
auto result = std::make_shared<ur_msgs::action::ToolContact::Result>();
314281
active_goal->setSucceeded(result);
315282
should_reset_goal = true;
316283
}
317-
} else if (result.value() == 1.0) {
284+
} else if (result == 1.0) {
318285
tool_contact_active_ = false;
319286
RCLCPP_ERROR(get_node()->get_logger(), "Tool contact aborted by hardware.");
320287

321-
write_success &= tool_contact_set_state_interface_->get().set_value(TOOL_CONTACT_STANDBY);
288+
tool_contact_set_state_interface_->get().set_value(TOOL_CONTACT_STANDBY);
322289
if (active_goal) {
323290
auto result = std::make_shared<ur_msgs::action::ToolContact::Result>();
324291
active_goal->setAborted(result);
@@ -331,7 +298,7 @@ controller_interface::return_type ToolContactController::update(const rclcpp::Ti
331298
{
332299
RCLCPP_ERROR(get_node()->get_logger(), "Tool contact could not be enabled.");
333300
tool_contact_active_ = false;
334-
write_success &= tool_contact_set_state_interface_->get().set_value(TOOL_CONTACT_STANDBY);
301+
tool_contact_set_state_interface_->get().set_value(TOOL_CONTACT_STANDBY);
335302

336303
if (active_goal) {
337304
auto result = std::make_shared<ur_msgs::action::ToolContact::Result>();
@@ -346,15 +313,15 @@ controller_interface::return_type ToolContactController::update(const rclcpp::Ti
346313
RCLCPP_INFO(get_node()->get_logger(), "Tool contact disabled successfully.");
347314
tool_contact_active_ = false;
348315

349-
write_success &= tool_contact_set_state_interface_->get().set_value(TOOL_CONTACT_STANDBY);
316+
tool_contact_set_state_interface_->get().set_value(TOOL_CONTACT_STANDBY);
350317
}
351318
} break;
352319

353320
case static_cast<int>(TOOL_CONTACT_FAILURE_END):
354321
{
355322
RCLCPP_ERROR(get_node()->get_logger(), "Tool contact could not be disabled.");
356323

357-
write_success &= tool_contact_set_state_interface_->get().set_value(TOOL_CONTACT_STANDBY);
324+
tool_contact_set_state_interface_->get().set_value(TOOL_CONTACT_STANDBY);
358325

359326
if (active_goal) {
360327
auto result = std::make_shared<ur_msgs::action::ToolContact::Result>();
@@ -372,10 +339,6 @@ controller_interface::return_type ToolContactController::update(const rclcpp::Ti
372339
active_goal->setFeedback(feedback_);
373340
}
374341

375-
if (!write_success) {
376-
RCLCPP_FATAL(get_node()->get_logger(), "Controller failed to update or read command/state interface.");
377-
return controller_interface::return_type::ERROR;
378-
}
379342
return controller_interface::return_type::OK;
380343
}
381344

0 commit comments

Comments
 (0)