Skip to content

Commit d0f0462

Browse files
author
Felix Durchdewald
committed
use callback groups for services and topics
1 parent 7c72f7e commit d0f0462

File tree

3 files changed

+68
-46
lines changed

3 files changed

+68
-46
lines changed

‎ur_robot_driver/include/ur_robot_driver/robot_state_helper.hpp‎

Lines changed: 14 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -29,11 +29,11 @@ class RobotStateHelper
2929
void robotModeCallback(ur_dashboard_msgs::msg::RobotMode::SharedPtr msg);
3030
void safetyModeCallback(ur_dashboard_msgs::msg::SafetyMode::SharedPtr msg);
3131

32-
void updateRobotState(bool called_from_thread = false);
32+
void updateRobotState();
3333

34-
void doTransition(bool called_from_thread = false);
34+
void doTransition();
3535

36-
bool safeDashboardTrigger(rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr srv, bool called_from_thread = false);
36+
bool safeDashboardTrigger(rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr srv);
3737

3838
void setModeAcceptCallback(const std::shared_ptr<SetModeGoalHandle> goal_handle);
3939
rclcpp_action::GoalResponse setModeGoalCallback(const rclcpp_action::GoalUUID& uuid,
@@ -45,6 +45,7 @@ class RobotStateHelper
4545

4646
void startActionServer();
4747
bool is_started_;
48+
bool in_action_;
4849

4950
std::shared_ptr<ur_dashboard_msgs::action::SetMode::Result> result_;
5051
std::shared_ptr<ur_dashboard_msgs::action::SetMode::Feedback> feedback_;
@@ -56,9 +57,19 @@ class RobotStateHelper
5657

5758
rclcpp_action::Server<ur_dashboard_msgs::action::SetMode>::SharedPtr set_mode_as_;
5859

60+
rclcpp::CallbackGroup::SharedPtr robot_mode_sub_cb_;
61+
5962
rclcpp::Subscription<ur_dashboard_msgs::msg::RobotMode>::SharedPtr robot_mode_sub_;
6063
rclcpp::Subscription<ur_dashboard_msgs::msg::SafetyMode>::SharedPtr safety_mode_sub_;
6164

65+
rclcpp::CallbackGroup::SharedPtr unlock_cb_;
66+
rclcpp::CallbackGroup::SharedPtr restart_cb_;
67+
rclcpp::CallbackGroup::SharedPtr power_on_cb_;
68+
rclcpp::CallbackGroup::SharedPtr power_off_cb_;
69+
rclcpp::CallbackGroup::SharedPtr brake_release_cb_;
70+
rclcpp::CallbackGroup::SharedPtr stop_program_cb_;
71+
rclcpp::CallbackGroup::SharedPtr play_program_cb_;
72+
6273
rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr unlock_protective_stop_srv_;
6374
rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr restart_safety_srv_;
6475
rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr power_on_srv_;

‎ur_robot_driver/src/robot_state_helper.cpp‎

Lines changed: 49 additions & 42 deletions
Original file line numberDiff line numberDiff line change
@@ -14,32 +14,51 @@ namespace ur_robot_driver
1414
RobotStateHelper::RobotStateHelper(const rclcpp::Node::SharedPtr& node)
1515
: node_(node)
1616
, is_started_(false)
17+
, in_action_(false)
1718
, robot_mode_(urcl::RobotMode::UNKNOWN)
1819
, safety_mode_(urcl::SafetyMode::UNDEFINED_SAFETY_MODE)
1920
{
21+
robot_mode_sub_cb_ = node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
22+
rclcpp::SubscriptionOptions options;
23+
options.callback_group = robot_mode_sub_cb_;
2024
// Topic on which the robot_mode is published by the driver
2125
robot_mode_sub_ = node_->create_subscription<ur_dashboard_msgs::msg::RobotMode>(
22-
"io_and_status_controller/robot_mode", 1,
23-
std::bind(&RobotStateHelper::robotModeCallback, this, std::placeholders::_1));
26+
"io_and_status_controller/robot_mode", rclcpp::SensorDataQoS(),
27+
std::bind(&RobotStateHelper::robotModeCallback, this, std::placeholders::_1), options);
2428
// Topic on which the safety is published by the driver
2529
safety_mode_sub_ = node_->create_subscription<ur_dashboard_msgs::msg::SafetyMode>(
2630
"io_and_status_controller/safety_mode", 1,
2731
std::bind(&RobotStateHelper::safetyModeCallback, this, std::placeholders::_1));
2832

33+
unlock_cb_ = node_->create_callback_group(rclcpp::CallbackGroupType::Reentrant);
34+
restart_cb_ = unlock_cb_;
35+
power_on_cb_ = unlock_cb_;
36+
power_off_cb_ = unlock_cb_;
37+
brake_release_cb_ = unlock_cb_;
38+
stop_program_cb_ = unlock_cb_;
39+
play_program_cb_ = unlock_cb_;
40+
2941
// Service to unlock protective stop
30-
unlock_protective_stop_srv_ = node_->create_client<std_srvs::srv::Trigger>("dashboard_client/unlock_protective_stop");
42+
unlock_protective_stop_srv_ = node_->create_client<std_srvs::srv::Trigger>(
43+
"dashboard_client/unlock_protective_stop", rclcpp::QoS(rclcpp::KeepLast(10)), unlock_cb_);
3144
// Service to restart safety
32-
restart_safety_srv_ = node_->create_client<std_srvs::srv::Trigger>("dashboard_client/restart_safety");
45+
restart_safety_srv_ = node_->create_client<std_srvs::srv::Trigger>("dashboard_client/restart_safety",
46+
rclcpp::QoS(rclcpp::KeepLast(10)), restart_cb_);
3347
// Service to power on the robot
34-
power_on_srv_ = node_->create_client<std_srvs::srv::Trigger>("dashboard_client/power_on");
48+
power_on_srv_ = node_->create_client<std_srvs::srv::Trigger>("dashboard_client/power_on",
49+
rclcpp::QoS(rclcpp::KeepLast(10)), power_on_cb_);
3550
// Service to power off the robot
36-
power_off_srv_ = node_->create_client<std_srvs::srv::Trigger>("dashboard_client/power_off");
51+
power_off_srv_ = node_->create_client<std_srvs::srv::Trigger>("dashboard_client/power_off",
52+
rclcpp::QoS(rclcpp::KeepLast(10)), power_off_cb_);
3753
// Service to release the robot's brakes
38-
brake_release_srv_ = node_->create_client<std_srvs::srv::Trigger>("dashboard_client/brake_release");
54+
brake_release_srv_ = node_->create_client<std_srvs::srv::Trigger>(
55+
"dashboard_client/brake_release", rclcpp::QoS(rclcpp::KeepLast(10)), brake_release_cb_);
3956
// Service to stop UR program execution on the robot
40-
stop_program_srv_ = node_->create_client<std_srvs::srv::Trigger>("dashboard_client/stop");
57+
stop_program_srv_ = node_->create_client<std_srvs::srv::Trigger>("dashboard_client/stop",
58+
rclcpp::QoS(rclcpp::KeepLast(10)), stop_program_cb_);
4159
// Service to start UR program execution on the robot
42-
play_program_srv_ = node_->create_client<std_srvs::srv::Trigger>("dashboard_client/play");
60+
play_program_srv_ = node_->create_client<std_srvs::srv::Trigger>("dashboard_client/play",
61+
rclcpp::QoS(rclcpp::KeepLast(10)), play_program_cb_);
4362
play_program_srv_->wait_for_service();
4463

4564
feedback_ = std::make_shared<ur_dashboard_msgs::action::SetMode::Feedback>();
@@ -52,9 +71,11 @@ void RobotStateHelper::robotModeCallback(ur_dashboard_msgs::msg::RobotMode::Shar
5271
robot_mode_ = urcl::RobotMode(msg->mode);
5372
RCLCPP_INFO_STREAM(rclcpp::get_logger("robot_state_helper"),
5473
"The robot is currently in mode " << robotModeString(robot_mode_) << ".");
55-
updateRobotState();
56-
if (!is_started_) {
57-
startActionServer();
74+
if (in_action_) {
75+
updateRobotState();
76+
if (!is_started_) {
77+
startActionServer();
78+
}
5879
}
5980
}
6081
}
@@ -72,14 +93,14 @@ void RobotStateHelper::safetyModeCallback(ur_dashboard_msgs::msg::SafetyMode::Sh
7293
}
7394
}
7495

75-
void RobotStateHelper::doTransition(bool called_from_thread)
96+
void RobotStateHelper::doTransition()
7697
{
7798
if (static_cast<urcl::RobotMode>(goal_->target_robot_mode) < robot_mode_) {
78-
safeDashboardTrigger(this->power_off_srv_, called_from_thread);
99+
safeDashboardTrigger(this->power_off_srv_);
79100
} else {
80101
switch (safety_mode_) {
81102
case urcl::SafetyMode::PROTECTIVE_STOP:
82-
safeDashboardTrigger(this->unlock_protective_stop_srv_, called_from_thread);
103+
safeDashboardTrigger(this->unlock_protective_stop_srv_);
83104
break;
84105
case urcl::SafetyMode::SYSTEM_EMERGENCY_STOP:;
85106
case urcl::SafetyMode::ROBOT_EMERGENCY_STOP:
@@ -89,7 +110,7 @@ void RobotStateHelper::doTransition(bool called_from_thread)
89110
break;
90111
case urcl::SafetyMode::VIOLATION:;
91112
case urcl::SafetyMode::FAULT:
92-
safeDashboardTrigger(this->restart_safety_srv_, called_from_thread);
113+
safeDashboardTrigger(this->restart_safety_srv_);
93114
break;
94115
default:
95116
switch (robot_mode_) {
@@ -106,7 +127,7 @@ void RobotStateHelper::doTransition(bool called_from_thread)
106127
"booted up...");
107128
break;
108129
case urcl::RobotMode::POWER_OFF:
109-
safeDashboardTrigger(this->power_on_srv_, called_from_thread);
130+
safeDashboardTrigger(this->power_on_srv_);
110131
break;
111132
case urcl::RobotMode::POWER_ON:
112133
RCLCPP_INFO_STREAM(rclcpp::get_logger("robot_state_helper"), "The robot is currently in mode "
@@ -116,7 +137,7 @@ void RobotStateHelper::doTransition(bool called_from_thread)
116137
<< robotModeString(urcl::RobotMode::IDLE));
117138
break;
118139
case urcl::RobotMode::IDLE:
119-
safeDashboardTrigger(this->brake_release_srv_, called_from_thread);
140+
safeDashboardTrigger(this->brake_release_srv_);
120141
break;
121142
case urcl::RobotMode::BACKDRIVE:
122143
RCLCPP_INFO_STREAM(rclcpp::get_logger("robot_state_helper"),
@@ -139,32 +160,17 @@ void RobotStateHelper::doTransition(bool called_from_thread)
139160
}
140161
}
141162

142-
bool RobotStateHelper::safeDashboardTrigger(rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr srv,
143-
bool called_from_thread)
163+
bool RobotStateHelper::safeDashboardTrigger(rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr srv)
144164
{
145-
RCLCPP_INFO(rclcpp::get_logger("robot_state_helper"), "trigger");
146165
assert(srv != nullptr);
147166
auto request = std::make_shared<std_srvs::srv::Trigger::Request>();
148167
auto future = srv->async_send_request(request);
149-
if (called_from_thread) {
150-
RCLCPP_INFO(rclcpp::get_logger("robot_state_helper"), "called from thread");
151-
future.wait();
152-
RCLCPP_INFO_STREAM(rclcpp::get_logger("robot_state_helper"),
153-
"Service response received: " << future.get()->message);
154-
} else {
155-
RCLCPP_INFO(rclcpp::get_logger("robot_state_helper"), "not from thread");
156-
if (rclcpp::spin_until_future_complete(node_, future) == rclcpp::FutureReturnCode::SUCCESS) {
157-
RCLCPP_INFO_STREAM(rclcpp::get_logger("robot_state_helper"),
158-
"Service response received: " << future.get()->message);
159-
} else {
160-
RCLCPP_ERROR(rclcpp::get_logger("robot_state_helper"), "Failed to call dashboard trigger service");
161-
return false;
162-
}
163-
}
168+
future.wait();
169+
RCLCPP_INFO_STREAM(rclcpp::get_logger("robot_state_helper"), "Service response received: " << future.get()->message);
164170
return true;
165171
}
166172

167-
void RobotStateHelper::updateRobotState(bool called_from_thread)
173+
void RobotStateHelper::updateRobotState()
168174
{
169175
if (is_started_) {
170176
// Update action feedback
@@ -177,20 +183,20 @@ void RobotStateHelper::updateRobotState(bool called_from_thread)
177183
if (robot_mode_ < static_cast<urcl::RobotMode>(goal_->target_robot_mode) ||
178184
safety_mode_ > urcl::SafetyMode::REDUCED) {
179185
// Transition to next mode
180-
RCLCPP_INFO(rclcpp::get_logger("robot_state_helper"), "transition");
181186
RCLCPP_DEBUG_STREAM(rclcpp::get_logger("ur_robot_state_helper"),
182187
"Current robot mode is "
183188
<< robotModeString(robot_mode_) << " while target mode is "
184189
<< robotModeString(static_cast<urcl::RobotMode>(goal_->target_robot_mode)));
185190
doTransition();
186191
} else if (robot_mode_ == static_cast<urcl::RobotMode>(goal_->target_robot_mode)) {
187192
// Final mode reached
193+
in_action_ = false;
188194
result_->success = true;
189195
result_->message = "Reached target robot mode.";
190196
if (robot_mode_ == urcl::RobotMode::RUNNING && goal_->play_program) {
191197
// The dashboard denies playing immediately after switching the mode to RUNNING
192198
sleep(1);
193-
safeDashboardTrigger(this->play_program_srv_, called_from_thread);
199+
safeDashboardTrigger(this->play_program_srv_);
194200
}
195201
current_goal_handle_->succeed(result_);
196202
} else {
@@ -222,6 +228,7 @@ void RobotStateHelper::setModeAcceptCallback(const std::shared_ptr<RobotStateHel
222228

223229
void RobotStateHelper::setModeExecute(const std::shared_ptr<RobotStateHelper::SetModeGoalHandle> goal_handle)
224230
{
231+
in_action_ = true;
225232
current_goal_handle_ = goal_handle;
226233
const auto goal = current_goal_handle_->get_goal();
227234
this->goal_ = goal;
@@ -240,11 +247,11 @@ void RobotStateHelper::setModeExecute(const std::shared_ptr<RobotStateHelper::Se
240247
if (robot_mode_ != static_cast<urcl::RobotMode>(goal_->target_robot_mode) ||
241248
safety_mode_ > urcl::SafetyMode::REDUCED) {
242249
if (goal_->stop_program) {
243-
safeDashboardTrigger(this->stop_program_srv_, true);
250+
safeDashboardTrigger(this->stop_program_srv_);
244251
}
245-
doTransition(true);
252+
doTransition();
246253
} else {
247-
updateRobotState(true);
254+
updateRobotState();
248255
}
249256
break;
250257
case urcl::RobotMode::NO_CONTROLLER:

‎ur_robot_driver/src/robot_state_helper_node.cpp‎

Lines changed: 5 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -7,7 +7,11 @@ int main(int argc, char** argv)
77
rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("robot_state_helper");
88
ur_robot_driver::RobotStateHelper state_helper(node);
99

10-
rclcpp::spin(node);
10+
rclcpp::executors::MultiThreadedExecutor executor;
11+
executor.add_node(node);
12+
executor.spin();
13+
14+
// rclcpp::spin(node);
1115

1216
return 0;
1317
}

0 commit comments

Comments
 (0)