Skip to content

Commit 0f92d5f

Browse files
committed
Remove Aborted and preempted from result reports
1 parent 7f46156 commit 0f92d5f

File tree

1 file changed

+1
-6
lines changed

1 file changed

+1
-6
lines changed

ur_controllers/src/tool_contact_controller.cpp

Lines changed: 1 addition & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -179,7 +179,6 @@ ToolContactController::on_deactivate(const rclcpp_lifecycle::State& /* previous_
179179
RCLCPP_INFO(get_node()->get_logger(), "Aborting tool contact, as controller has been deactivated.");
180180
// Mark the current goal as abort
181181
auto result = std::make_shared<ur_msgs::action::ToolContact::Result>();
182-
result->result = ur_msgs::action::ToolContact::Result::ABORTED;
183182
active_goal->setAborted(result);
184183
should_reset_goal = true;
185184
}
@@ -260,7 +259,6 @@ rclcpp_action::CancelResponse ToolContactController::goal_cancelled_callback(
260259

261260
// Mark the current goal as canceled
262261
auto result = std::make_shared<ur_msgs::action::ToolContact::Result>();
263-
result->result = ur_msgs::action::ToolContact::Result::PREEMPTED;
264262
active_goal->setCanceled(result);
265263
rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr());
266264
tool_contact_abort_ = true;
@@ -304,7 +302,7 @@ controller_interface::return_type ToolContactController::update(const rclcpp::Ti
304302
write_success &= tool_contact_set_state_interface_->get().set_value(TOOL_CONTACT_WAITING_END);
305303
if (active_goal) {
306304
auto result = std::make_shared<ur_msgs::action::ToolContact::Result>();
307-
result->result = ur_msgs::action::ToolContact::Result::SUCCESS;
305+
result->result = ur_msgs::action::ToolContact::Result::TOOL_CONTACT_TRIGGERED;
308306
active_goal->setSucceeded(result);
309307
should_reset_goal = true;
310308
}
@@ -315,7 +313,6 @@ controller_interface::return_type ToolContactController::update(const rclcpp::Ti
315313
write_success &= tool_contact_set_state_interface_->get().set_value(TOOL_CONTACT_STANDBY);
316314
if (active_goal) {
317315
auto result = std::make_shared<ur_msgs::action::ToolContact::Result>();
318-
result->result = ur_msgs::action::ToolContact::Result::ABORTED;
319316
active_goal->setAborted(result);
320317
should_reset_goal = true;
321318
}
@@ -330,7 +327,6 @@ controller_interface::return_type ToolContactController::update(const rclcpp::Ti
330327

331328
if (active_goal) {
332329
auto result = std::make_shared<ur_msgs::action::ToolContact::Result>();
333-
result->result = ur_msgs::action::ToolContact::Result::ABORTED;
334330
active_goal->setAborted(result);
335331
should_reset_goal = true;
336332
}
@@ -352,7 +348,6 @@ controller_interface::return_type ToolContactController::update(const rclcpp::Ti
352348

353349
if (active_goal) {
354350
auto result = std::make_shared<ur_msgs::action::ToolContact::Result>();
355-
result->result = ur_msgs::action::ToolContact::Result::ABORTED;
356351
active_goal->setAborted(result);
357352
should_reset_goal = true;
358353
}

0 commit comments

Comments
 (0)