Skip to content

Commit d4947aa

Browse files
fdurchdewaldFelix Durchdewaldurfeex
authored
Port robot_state_helper to ROS2 (#933)
--------- Co-authored-by: Felix Durchdewald <[email protected]> Co-authored-by: Felix Exner <[email protected]>
1 parent 7c63057 commit d4947aa

File tree

10 files changed

+676
-5
lines changed

10 files changed

+676
-5
lines changed

‎ur_dashboard_msgs/action/SetMode.action

Lines changed: 4 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,10 @@
11
# This action is for setting the robot into a desired mode (e.g. RUNNING) and safety mode into a
22
# non-critical state (e.g. NORMAL or REDUCED), for example after a safety incident happened.
33

4-
# goal
4+
# Target modes can be one of
5+
# - 3: ROBOT_MODE_POWER_OFF
6+
# - 5: ROBOT_MODE_IDLE
7+
# - 7: ROBOT_MODE_RUNNING
58
int8 target_robot_mode
69

710
# Stop program execution before restoring the target mode. Can be used together with 'play_program'.

‎ur_robot_driver/CMakeLists.txt

Lines changed: 13 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -58,6 +58,7 @@ add_library(ur_robot_driver_plugin
5858
src/dashboard_client_ros.cpp
5959
src/hardware_interface.cpp
6060
src/urcl_log_handler.cpp
61+
src/robot_state_helper.cpp
6162
)
6263
target_link_libraries(
6364
ur_robot_driver_plugin
@@ -83,7 +84,7 @@ add_executable(dashboard_client
8384
src/dashboard_client_node.cpp
8485
src/urcl_log_handler.cpp
8586
)
86-
target_link_libraries(dashboard_client ${catkin_LIBRARIES} ur_client_library::urcl)
87+
target_link_libraries(dashboard_client ur_client_library::urcl)
8788
ament_target_dependencies(dashboard_client ${${PROJECT_NAME}_EXPORTED_TARGETS} ${THIS_PACKAGE_INCLUDE_DEPENDS})
8889

8990
#
@@ -95,13 +96,23 @@ add_executable(controller_stopper_node
9596
)
9697
ament_target_dependencies(controller_stopper_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${THIS_PACKAGE_INCLUDE_DEPENDS})
9798

99+
#
100+
# robot_state_helper
101+
#
102+
add_executable(robot_state_helper
103+
src/robot_state_helper.cpp
104+
src/robot_state_helper_node.cpp
105+
)
106+
target_link_libraries(robot_state_helper ur_client_library::urcl)
107+
ament_target_dependencies(robot_state_helper ${${PROJECT_NAME}_EXPORTED_TARGETS} ${THIS_PACKAGE_INCLUDE_DEPENDS})
108+
98109
add_executable(urscript_interface
99110
src/urscript_interface.cpp
100111
)
101112
ament_target_dependencies(urscript_interface ${${PROJECT_NAME}_EXPORTED_TARGETS} ${THIS_PACKAGE_INCLUDE_DEPENDS})
102113

103114
install(
104-
TARGETS dashboard_client controller_stopper_node urscript_interface
115+
TARGETS dashboard_client controller_stopper_node urscript_interface robot_state_helper
105116
DESTINATION lib/${PROJECT_NAME}
106117
)
107118

‎ur_robot_driver/doc/controller_stopper.rst

Lines changed: 29 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,29 @@
1+
.. _controller_stopper:
2+
3+
Controller stopper
4+
==================
5+
6+
As explained in the section :ref:`robot_startup_program`, the robot needs to run a program in order
7+
to receive motion commands from the ROS driver. When the program is not running, commands sent to
8+
the robot will have no effect.
9+
10+
To make that transparent, the ``controller_stopper`` node mirrors that state in the ROS
11+
controller's state. It listens to the ``/io_and_status_controller/robot_program_running`` topic and
12+
deactivates all motion controllers (or any controller not explicitly marked as "consistent", see
13+
below)when the program is not running.
14+
15+
Once the program is running again, any previously active motion controller will be activated again.
16+
17+
This way, when sending commands to an inactive controller the caller should be transparently
18+
informed, that the controller cannot accept commands at the moment.
19+
20+
In the same way, any running action on the ROS controller will be aborted, as the controller gets
21+
deactivated by the controller_stopper.
22+
23+
Parameters
24+
----------
25+
26+
- ``~consistent_controllers`` (list of strings, default: ``[]``)
27+
28+
A list of controller names that should not be stopped when the program is not running. Any
29+
controller that doesn't require the robot program to be running should be in that list.

‎ur_robot_driver/doc/index.rst

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -17,3 +17,5 @@ ur_robot_driver
1717
setup_tool_communication
1818
hardware_interface_parameters
1919
dashboard_client
20+
robot_state_helper
21+
controller_stopper

‎ur_robot_driver/doc/robot_state_helper.rst

Lines changed: 59 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,59 @@
1+
.. _robot_state_helper:
2+
3+
Robot state helper
4+
==================
5+
After switching on the robot, it has to be manually started, the brakes have to be released and a
6+
program has to be started in order to make the robot ready to use. This is usually done using the
7+
robot's teach pendant.
8+
9+
Whenever the robot encounters an error, manual intervention is required to resolve the issue. For
10+
example, if the robot goes into a protective stop, the error has to be acknowledged and the robot
11+
program has to be unpaused.
12+
13+
When the robot is in :ref:`remote_control_mode <operation_modes>`, most interaction with the robot can be done
14+
without using the teach pendant, many of that through the :ref:`dashboard client
15+
<dashboard_client_ros2>`.
16+
17+
The ROS driver provides a helper node that can be used to automate some of these tasks. The
18+
``robot_state_helper`` node can be used to start the robot, release the brakes, and (re-)start the
19+
program through an action call. It is started by default and provides a
20+
`dashboard_msgs/action/SetMode
21+
<https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/blob/main/ur_dashboard_msgs/action/SetMode.action>`_ action.
22+
23+
For example, to make the robot ready to be used by the ROS driver, call
24+
25+
.. code-block:: console
26+
27+
$ ros2 action send_goal /ur_robot_state_helper/set_mode ur_dashboard_msgs/action/SetMode "{ target_robot_mode: 7, stop_program: true, play_program: true}"
28+
29+
The ``target_robot_mode`` can be one of the following:
30+
31+
.. table:: target_robot_mode
32+
:widths: auto
33+
34+
===== =====
35+
index meaning
36+
===== =====
37+
3 POWER_OFF -- Robot is powered off
38+
5 IDLE -- Robot is powered on, but brakes are engaged
39+
7 RUNNING -- Robot is powered on, brakes are released, ready to run a program
40+
===== =====
41+
42+
.. note::
43+
44+
When the ROBOT_STATE is in ``RUNNING``, that is equivalent to the robot showing the green dot in
45+
the lower left corner of the teach pendant (On PolyScope 5). The program state is independent of
46+
that and shows with the text next to that button.
47+
48+
The ``stop_program`` flag is used to stop the currently running program before changing the robot
49+
state. In combination with the :ref:`controller_stopper`, this will deactivate any motion
50+
controller and therefore stop any ROS action being active on those controllers.
51+
52+
.. warning::
53+
A robot's protective stop or emergency stop is only pausing the running program. If the program
54+
is resumed after the P-Stop or EM-Stop is released, the robot will continue executing what it
55+
has been doing. Therefore, it is advised to stop and re-start the program when recovering from a
56+
fault.
57+
58+
The ``play_program`` flag is used to start the program after the robot state has been set. This has
59+
the same effects as explained in :ref:`continuation_after_interruptions`.

‎ur_robot_driver/doc/usage/startup.rst

Lines changed: 5 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -71,12 +71,12 @@ Depending on the :ref:`robot control mode<operation_modes>` do the following:
7171
* When the driver is started with ``headless_mode:=true`` nothing is needed. The driver is running
7272
already.
7373

74-
.. _continuation_after_interruptions:
74+
75+
.. _verify_calibration:
7576

7677
Verify calibration info is being used correctly
7778
-----------------------------------------------
7879

79-
.. _verify_calibration:
8080

8181
If you passed a path to an extracted calibration via the *kinematics_params_file*
8282
parameter, ensure that the loaded calibration matches that of the robot by inspecting the console
@@ -98,9 +98,12 @@ Alternatively, search for the term *checksum* in the console output after launch
9898
Verify that the printed checksum matches that on the final line of your extracted calibration file.
9999

100100

101+
.. _continuation_after_interruptions:
102+
101103
Continuation after interruptions
102104
--------------------------------
103105

106+
104107
Whenever the *External Control URCap* program gets interrupted, it has to be unpaused / restarted.
105108

106109
If that happens, you will see the output ``Connection to reverse interface dropped.``

‎ur_robot_driver/include/ur_robot_driver/robot_state_helper.hpp

Lines changed: 114 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,114 @@
1+
// Copyright 2024, FZI Forschungszentrum Informatik, Created on behalf of Universal Robots A/S
2+
//
3+
// Redistribution and use in source and binary forms, with or without
4+
// modification, are permitted provided that the following conditions are met:
5+
//
6+
// * Redistributions of source code must retain the above copyright
7+
// notice, this list of conditions and the following disclaimer.
8+
//
9+
// * Redistributions in binary form must reproduce the above copyright
10+
// notice, this list of conditions and the following disclaimer in the
11+
// documentation and/or other materials provided with the distribution.
12+
//
13+
// * Neither the name of the {copyright_holder} nor the names of its
14+
// contributors may be used to endorse or promote products derived from
15+
// this software without specific prior written permission.
16+
//
17+
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18+
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19+
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20+
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
21+
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22+
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23+
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24+
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25+
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26+
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27+
// POSSIBILITY OF SUCH DAMAGE.
28+
29+
#ifndef UR_ROBOT_DRIVER__ROBOT_STATE_HELPER_HPP_
30+
#define UR_ROBOT_DRIVER__ROBOT_STATE_HELPER_HPP_
31+
32+
#include <chrono>
33+
#include <memory>
34+
35+
#include "rclcpp/rclcpp.hpp"
36+
#include "rclcpp_action/create_server.hpp"
37+
#include "std_msgs/msg/bool.hpp"
38+
#include "std_srvs/srv/trigger.hpp"
39+
40+
#include "ur_dashboard_msgs/action/set_mode.hpp"
41+
#include "ur_dashboard_msgs/msg/safety_mode.hpp"
42+
#include "ur_dashboard_msgs/msg/robot_mode.hpp"
43+
#include "ur_client_library/ur/datatypes.h"
44+
45+
namespace ur_robot_driver
46+
{
47+
class RobotStateHelper
48+
{
49+
public:
50+
using SetModeGoalHandle = rclcpp_action::ServerGoalHandle<ur_dashboard_msgs::action::SetMode>;
51+
52+
explicit RobotStateHelper(const rclcpp::Node::SharedPtr& node);
53+
RobotStateHelper() = delete;
54+
virtual ~RobotStateHelper() = default;
55+
56+
private:
57+
rclcpp::Node::SharedPtr node_;
58+
59+
void robotModeCallback(ur_dashboard_msgs::msg::RobotMode::SharedPtr msg);
60+
void safetyModeCallback(ur_dashboard_msgs::msg::SafetyMode::SharedPtr msg);
61+
62+
void updateRobotState();
63+
64+
bool recoverFromSafety();
65+
bool doTransition(const urcl::RobotMode target_mode);
66+
bool jumpToRobotMode(const urcl::RobotMode target_mode);
67+
68+
bool safeDashboardTrigger(rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr srv);
69+
70+
bool stopProgram();
71+
72+
void setModeAcceptCallback(const std::shared_ptr<SetModeGoalHandle> goal_handle);
73+
rclcpp_action::GoalResponse setModeGoalCallback(const rclcpp_action::GoalUUID& uuid,
74+
std::shared_ptr<const ur_dashboard_msgs::action::SetMode::Goal> goal);
75+
rclcpp_action::CancelResponse setModeCancelCallback(const std::shared_ptr<SetModeGoalHandle> goal_handle);
76+
77+
void setModeExecute(const std::shared_ptr<SetModeGoalHandle> goal_handle);
78+
79+
bool headless_mode_;
80+
81+
std::shared_ptr<ur_dashboard_msgs::action::SetMode::Result> result_;
82+
std::shared_ptr<ur_dashboard_msgs::action::SetMode::Feedback> feedback_;
83+
std::shared_ptr<const ur_dashboard_msgs::action::SetMode::Goal> goal_;
84+
std::shared_ptr<SetModeGoalHandle> current_goal_handle_;
85+
86+
std::atomic<urcl::RobotMode> robot_mode_;
87+
std::atomic<urcl::SafetyMode> safety_mode_;
88+
std::atomic<bool> error_ = false;
89+
std::atomic<bool> in_action_;
90+
std::atomic<bool> program_running_;
91+
std::mutex goal_mutex_;
92+
93+
rclcpp_action::Server<ur_dashboard_msgs::action::SetMode>::SharedPtr set_mode_as_;
94+
95+
rclcpp::CallbackGroup::SharedPtr robot_mode_sub_cb_;
96+
97+
rclcpp::Subscription<ur_dashboard_msgs::msg::RobotMode>::SharedPtr robot_mode_sub_;
98+
rclcpp::Subscription<ur_dashboard_msgs::msg::SafetyMode>::SharedPtr safety_mode_sub_;
99+
rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr program_running_sub;
100+
101+
rclcpp::CallbackGroup::SharedPtr service_cb_grp_;
102+
103+
rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr unlock_protective_stop_srv_;
104+
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_;
109+
rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr play_program_srv_;
110+
rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr resend_robot_program_srv_;
111+
};
112+
} // namespace ur_robot_driver
113+
114+
#endif // UR_ROBOT_DRIVER__ROBOT_STATE_HELPER_HPP_

‎ur_robot_driver/launch/ur_control.launch.py

Lines changed: 11 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -91,6 +91,16 @@ def launch_setup(context):
9191
parameters=[{"robot_ip": robot_ip}],
9292
)
9393

94+
robot_state_helper_node = Node(
95+
package="ur_robot_driver",
96+
executable="robot_state_helper",
97+
name="ur_robot_state_helper",
98+
output="screen",
99+
parameters=[
100+
{"headless_mode": headless_mode},
101+
],
102+
)
103+
94104
tool_communication_node = Node(
95105
package="ur_robot_driver",
96106
condition=IfCondition(use_tool_communication),
@@ -202,6 +212,7 @@ def controller_spawner(controllers, active=True):
202212
nodes_to_start = [
203213
control_node,
204214
dashboard_client_node,
215+
robot_state_helper_node,
205216
tool_communication_node,
206217
controller_stopper_node,
207218
urscript_interface,

0 commit comments

Comments
 (0)