Skip to content

Commit 56fe942

Browse files
urfeexurfeex
committed
Make robot_state_helper use primary_client whenever possible
1 parent 246d4a9 commit 56fe942

File tree

6 files changed

+81
-49
lines changed

6 files changed

+81
-49
lines changed

‎ur_robot_driver/CMakeLists.txt‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -102,6 +102,7 @@ ament_target_dependencies(controller_stopper_node ${${PROJECT_NAME}_EXPORTED_TAR
102102
add_executable(robot_state_helper
103103
src/robot_state_helper.cpp
104104
src/robot_state_helper_node.cpp
105+
src/urcl_log_handler.cpp
105106
)
106107
target_link_libraries(robot_state_helper ur_client_library::urcl)
107108
ament_target_dependencies(robot_state_helper ${${PROJECT_NAME}_EXPORTED_TARGETS} ${THIS_PACKAGE_INCLUDE_DEPENDS})

‎ur_robot_driver/include/ur_robot_driver/robot_state_helper.hpp‎

Lines changed: 6 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -29,7 +29,7 @@
2929
#ifndef UR_ROBOT_DRIVER__ROBOT_STATE_HELPER_HPP_
3030
#define UR_ROBOT_DRIVER__ROBOT_STATE_HELPER_HPP_
3131

32-
#include <chrono>
32+
#include <string>
3333
#include <memory>
3434

3535
#include "rclcpp/rclcpp.hpp"
@@ -41,6 +41,7 @@
4141
#include "ur_dashboard_msgs/msg/safety_mode.hpp"
4242
#include "ur_dashboard_msgs/msg/robot_mode.hpp"
4343
#include "ur_client_library/ur/datatypes.h"
44+
#include "ur_client_library/primary/primary_client.h"
4445

4546
namespace ur_robot_driver
4647
{
@@ -90,6 +91,10 @@ class RobotStateHelper
9091
std::atomic<bool> program_running_;
9192
std::mutex goal_mutex_;
9293

94+
std::string robot_ip_;
95+
urcl::comm::INotifier notifier_;
96+
std::shared_ptr<urcl::primary_interface::PrimaryClient> primary_client_;
97+
9398
rclcpp_action::Server<ur_dashboard_msgs::action::SetMode>::SharedPtr set_mode_as_;
9499

95100
rclcpp::CallbackGroup::SharedPtr robot_mode_sub_cb_;
@@ -100,12 +105,7 @@ class RobotStateHelper
100105

101106
rclcpp::CallbackGroup::SharedPtr service_cb_grp_;
102107

103-
rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr unlock_protective_stop_srv_;
104108
rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr restart_safety_srv_;
105-
rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr power_on_srv_;
106-
rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr power_off_srv_;
107-
rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr brake_release_srv_;
108-
rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr stop_program_srv_;
109109
rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr play_program_srv_;
110110
rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr resend_robot_program_srv_;
111111
};

‎ur_robot_driver/launch/ur_control.launch.py‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -96,8 +96,10 @@ def launch_setup(context):
9696
executable="robot_state_helper",
9797
name="ur_robot_state_helper",
9898
output="screen",
99+
condition=UnlessCondition(use_mock_hardware),
99100
parameters=[
100101
{"headless_mode": headless_mode},
102+
{"robot_ip": robot_ip},
101103
],
102104
)
103105

‎ur_robot_driver/src/dashboard_client_ros.cpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -51,7 +51,7 @@ DashboardClientROS::DashboardClientROS(const rclcpp::Node::SharedPtr& node, cons
5151
{
5252
node_->declare_parameter<double>("receive_timeout", 1);
5353

54-
primary_client_.start(10, std::chrono::seconds(2));
54+
primary_client_.start(10, std::chrono::seconds(10));
5555
auto robot_version = primary_client_.getRobotVersion();
5656

5757
if (robot_version->major > 5) {

‎ur_robot_driver/src/robot_state_helper.cpp‎

Lines changed: 63 additions & 41 deletions
Original file line numberDiff line numberDiff line change
@@ -27,6 +27,8 @@
2727
// POSSIBILITY OF SUCH DAMAGE.
2828

2929
#include <mutex>
30+
#include <rclcpp/logger.hpp>
31+
#include <rclcpp/logging.hpp>
3032
#include <ur_robot_driver/robot_state_helper.hpp>
3133

3234
#include "rclcpp/rclcpp.hpp"
@@ -66,28 +68,27 @@ RobotStateHelper::RobotStateHelper(const rclcpp::Node::SharedPtr& node)
6668
node->declare_parameter("headless_mode", false);
6769
headless_mode_ = node->get_parameter("headless_mode").as_bool();
6870

69-
// Service to unlock protective stop
70-
unlock_protective_stop_srv_ = node_->create_client<std_srvs::srv::Trigger>(
71-
"dashboard_client/unlock_protective_stop", rclcpp::QoS(rclcpp::KeepLast(10)), service_cb_grp_);
72-
// Service to restart safety
73-
restart_safety_srv_ = node_->create_client<std_srvs::srv::Trigger>(
74-
"dashboard_client/restart_safety", rclcpp::QoS(rclcpp::KeepLast(10)), service_cb_grp_);
75-
// Service to power on the robot
76-
power_on_srv_ = node_->create_client<std_srvs::srv::Trigger>("dashboard_client/power_on",
77-
rclcpp::QoS(rclcpp::KeepLast(10)), service_cb_grp_);
78-
// Service to power off the robot
79-
power_off_srv_ = node_->create_client<std_srvs::srv::Trigger>("dashboard_client/power_off",
80-
rclcpp::QoS(rclcpp::KeepLast(10)), service_cb_grp_);
81-
// Service to release the robot's brakes
82-
brake_release_srv_ = node_->create_client<std_srvs::srv::Trigger>("dashboard_client/brake_release",
83-
rclcpp::QoS(rclcpp::KeepLast(10)), service_cb_grp_);
84-
// Service to stop UR program execution on the robot
85-
stop_program_srv_ = node_->create_client<std_srvs::srv::Trigger>("dashboard_client/stop",
86-
rclcpp::QoS(rclcpp::KeepLast(10)), service_cb_grp_);
87-
// Service to start UR program execution on the robot
88-
play_program_srv_ = node_->create_client<std_srvs::srv::Trigger>("dashboard_client/play",
89-
rclcpp::QoS(rclcpp::KeepLast(10)), service_cb_grp_);
90-
play_program_srv_->wait_for_service();
71+
node->declare_parameter("robot_ip", "192.168.56.101");
72+
robot_ip_ = node->get_parameter("robot_ip").as_string();
73+
74+
primary_client_ = std::make_shared<urcl::primary_interface::PrimaryClient>(robot_ip_, notifier_);
75+
76+
primary_client_->start(0, std::chrono::seconds(10));
77+
auto robot_version = primary_client_->getRobotVersion();
78+
79+
if (robot_version->major > 5) {
80+
RCLCPP_WARN(rclcpp::get_logger("robot_state_helper"), "Running on a PolyScopeX robot. The dashboard server is not "
81+
"available, therefore the robot_state_helper cannot start "
82+
"PolyScope programs.");
83+
} else {
84+
// Service to restart safety
85+
restart_safety_srv_ = node_->create_client<std_srvs::srv::Trigger>(
86+
"dashboard_client/restart_safety", rclcpp::QoS(rclcpp::KeepLast(10)), service_cb_grp_);
87+
// Service to start UR program execution on the robot
88+
play_program_srv_ = node_->create_client<std_srvs::srv::Trigger>(
89+
"dashboard_client/play", rclcpp::QoS(rclcpp::KeepLast(10)), service_cb_grp_);
90+
play_program_srv_->wait_for_service();
91+
}
9192

9293
resend_robot_program_srv_ = node_->create_client<std_srvs::srv::Trigger>(
9394
"io_and_status_controller/resend_robot_program", rclcpp::QoS(rclcpp::KeepLast(10)), service_cb_grp_);
@@ -136,7 +137,13 @@ bool RobotStateHelper::recoverFromSafety()
136137
{
137138
switch (safety_mode_) {
138139
case urcl::SafetyMode::PROTECTIVE_STOP:
139-
return safeDashboardTrigger(this->unlock_protective_stop_srv_);
140+
try {
141+
primary_client_->commandUnlockProtectiveStop();
142+
} catch (const urcl::UrException& e) {
143+
RCLCPP_WARN_STREAM(rclcpp::get_logger("robot_state_helper"), e.what());
144+
return false;
145+
}
146+
return true;
140147
case urcl::SafetyMode::SYSTEM_EMERGENCY_STOP:;
141148
case urcl::SafetyMode::ROBOT_EMERGENCY_STOP:
142149
RCLCPP_WARN_STREAM(rclcpp::get_logger("robot_state_helper"), "The robot is currently in safety mode."
@@ -145,7 +152,11 @@ bool RobotStateHelper::recoverFromSafety()
145152
return false;
146153
case urcl::SafetyMode::VIOLATION:;
147154
case urcl::SafetyMode::FAULT:
148-
return safeDashboardTrigger(this->restart_safety_srv_);
155+
if (restart_safety_srv_ != nullptr) {
156+
return safeDashboardTrigger(this->restart_safety_srv_);
157+
} else {
158+
return false;
159+
}
149160
default:
150161
// nothing to do
151162
RCLCPP_DEBUG_STREAM(rclcpp::get_logger("robot_state_helper"), "No safety recovery needed.");
@@ -155,15 +166,22 @@ bool RobotStateHelper::recoverFromSafety()
155166

156167
bool RobotStateHelper::jumpToRobotMode(const urcl::RobotMode target_mode)
157168
{
158-
switch (target_mode) {
159-
case urcl::RobotMode::POWER_OFF:
160-
return safeDashboardTrigger(this->power_off_srv_);
161-
case urcl::RobotMode::IDLE:
162-
return safeDashboardTrigger(this->power_on_srv_);
163-
case urcl::RobotMode::RUNNING:
164-
return safeDashboardTrigger(this->brake_release_srv_);
165-
default:
166-
RCLCPP_ERROR_STREAM(rclcpp::get_logger("robot_state_helper"), "Unreachable target robot mode.");
169+
try {
170+
switch (target_mode) {
171+
case urcl::RobotMode::POWER_OFF:
172+
primary_client_->commandPowerOff();
173+
return true;
174+
case urcl::RobotMode::IDLE:
175+
primary_client_->commandPowerOn();
176+
return true;
177+
case urcl::RobotMode::RUNNING:
178+
primary_client_->commandBrakeRelease();
179+
return true;
180+
default:
181+
RCLCPP_ERROR_STREAM(rclcpp::get_logger("robot_state_helper"), "Unreachable target robot mode.");
182+
}
183+
} catch (const urcl::UrException& e) {
184+
RCLCPP_ERROR_STREAM(rclcpp::get_logger("robot_state_helper"), e.what());
167185
}
168186
return false;
169187
}
@@ -247,14 +265,13 @@ void RobotStateHelper::setModeAcceptCallback(const std::shared_ptr<RobotStateHel
247265

248266
bool RobotStateHelper::stopProgram()
249267
{
250-
if (safeDashboardTrigger(this->stop_program_srv_)) {
251-
auto start = std::chrono::steady_clock::now();
252-
while (program_running_ && std::chrono::steady_clock::now() - start < std::chrono::seconds(1)) {
253-
std::this_thread::sleep_for(std::chrono::milliseconds(2));
254-
return true;
255-
}
268+
try {
269+
primary_client_->commandStop();
270+
} catch (const urcl::UrException& e) {
271+
RCLCPP_ERROR_STREAM(rclcpp::get_logger("robot_state_helper"), e.what());
272+
return false;
256273
}
257-
return false;
274+
return true;
258275
}
259276

260277
void RobotStateHelper::setModeExecute(const std::shared_ptr<RobotStateHelper::SetModeGoalHandle> goal_handle)
@@ -346,7 +363,12 @@ void RobotStateHelper::setModeExecute(const std::shared_ptr<RobotStateHelper::Se
346363
} else {
347364
// The dashboard denies playing immediately after switching the mode to RUNNING
348365
sleep(1);
349-
result_->success = safeDashboardTrigger(this->play_program_srv_);
366+
if (play_program_srv_ == nullptr) {
367+
result_->success = false;
368+
result_->message = "Play program service not available on this robot.";
369+
} else {
370+
result_->success = safeDashboardTrigger(this->play_program_srv_);
371+
}
350372
}
351373
}
352374
if (result_->success) {

‎ur_robot_driver/src/robot_state_helper_node.cpp‎

Lines changed: 8 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -27,12 +27,19 @@
2727
// POSSIBILITY OF SUCH DAMAGE.
2828

2929
#include "ur_robot_driver/robot_state_helper.hpp"
30+
#include "ur_robot_driver/urcl_log_handler.hpp"
3031

3132
int main(int argc, char** argv)
3233
{
3334
rclcpp::init(argc, argv);
3435
rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("robot_state_helper");
35-
ur_robot_driver::RobotStateHelper state_helper(node);
36+
ur_robot_driver::registerUrclLogHandler(""); // Set empty tf_prefix at the moment
37+
std::shared_ptr<ur_robot_driver::RobotStateHelper> robot_state_helper;
38+
try {
39+
robot_state_helper = std::make_shared<ur_robot_driver::RobotStateHelper>(node);
40+
} catch (const urcl::UrException& e) {
41+
RCLCPP_ERROR(rclcpp::get_logger("robot_state_helper"), "%s", e.what());
42+
}
3643

3744
rclcpp::executors::MultiThreadedExecutor executor;
3845
executor.add_node(node);

0 commit comments

Comments
 (0)