Skip to content

Commit 8a4de39

Browse files
committed
WIP: Use a config struct
1 parent f717f83 commit 8a4de39

File tree

5 files changed

+308
-106
lines changed

5 files changed

+308
-106
lines changed

include/ur_client_library/ur/ur_driver.h

Lines changed: 138 additions & 26 deletions
Original file line numberDiff line numberDiff line change
@@ -44,6 +44,34 @@
4444

4545
namespace urcl
4646
{
47+
struct UrDriverConfiguration
48+
{
49+
std::string robot_ip;
50+
std::string script_file;
51+
std::string output_recipe_file;
52+
std::string input_recipe_file;
53+
std::function<void(bool)> handle_program_state;
54+
bool headless_mode;
55+
56+
std::unique_ptr<ToolCommSetup> tool_comm_setup = nullptr;
57+
58+
uint32_t reverse_port = 50001;
59+
uint32_t script_sender_port = 50002;
60+
uint32_t trajectory_port = 50003;
61+
uint32_t script_command_port = 50004;
62+
std::string reverse_ip = "";
63+
64+
int servoj_gain = 2000;
65+
double servoj_lookahead_time = 0.03;
66+
67+
bool non_blocking_read = false;
68+
69+
// TODO: Remove on 2027-05
70+
std::string calibration_checksum = "";
71+
double force_mode_damping = 0.025;
72+
double force_mode_gain_scaling = 0.5;
73+
};
74+
4775
/*!
4876
* \brief This is the main class for interfacing the driver.
4977
*
@@ -54,6 +82,11 @@ namespace urcl
5482
class UrDriver
5583
{
5684
public:
85+
explicit UrDriver(const UrDriverConfiguration& config)
86+
{
87+
init(config);
88+
}
89+
5790
/*!
5891
* \brief Constructs a new UrDriver object.
5992
* Upon initialization this class will check the calibration checksum reported from the robot and
@@ -94,12 +127,34 @@ class UrDriver
94127
* \param script_command_port Port used for forwarding script commands to the robot. The script commands will be
95128
* executed locally on the robot.
96129
*/
130+
// Called sigA in tests
131+
[[deprecated("Initializing a UrDriver object with an argument list is deprecated. Please use UrDriver(const "
132+
"UrDriverConfiguration& config) instead. This function will be removed in May 2027.")]]
97133
UrDriver(const std::string& robot_ip, const std::string& script_file, const std::string& output_recipe_file,
98134
const std::string& input_recipe_file, std::function<void(bool)> handle_program_state, bool headless_mode,
99135
std::unique_ptr<ToolCommSetup> tool_comm_setup, const uint32_t reverse_port = 50001,
100136
const uint32_t script_sender_port = 50002, int servoj_gain = 2000, double servoj_lookahead_time = 0.03,
101137
bool non_blocking_read = false, const std::string& reverse_ip = "", const uint32_t trajectory_port = 50003,
102-
const uint32_t script_command_port = 50004);
138+
const uint32_t script_command_port = 50004)
139+
{
140+
UrDriverConfiguration config;
141+
config.robot_ip = robot_ip;
142+
config.script_file = script_file;
143+
config.output_recipe_file = output_recipe_file;
144+
config.input_recipe_file = input_recipe_file;
145+
config.handle_program_state = handle_program_state;
146+
config.headless_mode = headless_mode;
147+
config.tool_comm_setup = std::move(tool_comm_setup);
148+
config.reverse_port = reverse_port;
149+
config.script_sender_port = script_sender_port;
150+
config.servoj_gain = servoj_gain;
151+
config.servoj_lookahead_time = servoj_lookahead_time;
152+
config.non_blocking_read = non_blocking_read;
153+
config.reverse_ip = reverse_ip;
154+
config.trajectory_port = trajectory_port;
155+
config.script_command_port = script_command_port;
156+
init(config);
157+
}
103158

104159
/*!
105160
* \brief Constructs a new UrDriver object.
@@ -129,25 +184,38 @@ class UrDriver
129184
* \param force_mode_damping The damping parameter used when the robot is in force mode, range [0,1]
130185
* \param force_mode_gain_scaling Scales the gain used when the robot is in force mode, range [0,2] (only e-series)
131186
*/
132-
[[deprecated(
133-
"Specifying the force mode damping factor and the force mode gain scaling factor at driver creation has "
134-
"been deprecated. Force mode parameters should be specified with each activiation of force mode, and "
135-
"can be set in the function call to start force mode.")]] UrDriver(const std::string& robot_ip,
136-
const std::string& script_file,
137-
const std::string& output_recipe_file,
138-
const std::string& input_recipe_file,
139-
std::function<void(bool)> handle_program_state,
140-
bool headless_mode,
141-
std::unique_ptr<ToolCommSetup> tool_comm_setup,
142-
const uint32_t reverse_port,
143-
const uint32_t script_sender_port,
144-
int servoj_gain, double servoj_lookahead_time,
145-
bool non_blocking_read,
146-
const std::string& reverse_ip,
147-
const uint32_t trajectory_port,
148-
const uint32_t script_command_port,
149-
double force_mode_damping,
150-
double force_mode_gain_scaling = 0.5);
187+
// Called sigB in tests
188+
[[deprecated("Specifying the force mode damping factor and the force mode gain scaling factor at driver creation has "
189+
"been deprecated. Force mode parameters should be specified with each activiation of force mode, and "
190+
"can be set in the function call to start force mode. This function will be removed in May "
191+
"2027.")]]
192+
UrDriver(const std::string& robot_ip, const std::string& script_file, const std::string& output_recipe_file,
193+
const std::string& input_recipe_file, std::function<void(bool)> handle_program_state, bool headless_mode,
194+
std::unique_ptr<ToolCommSetup> tool_comm_setup, const uint32_t reverse_port,
195+
const uint32_t script_sender_port, int servoj_gain, double servoj_lookahead_time, bool non_blocking_read,
196+
const std::string& reverse_ip, const uint32_t trajectory_port, const uint32_t script_command_port,
197+
double force_mode_damping, double force_mode_gain_scaling = 0.5)
198+
{
199+
UrDriverConfiguration config;
200+
config.robot_ip = robot_ip;
201+
config.script_file = script_file;
202+
config.output_recipe_file = output_recipe_file;
203+
config.input_recipe_file = input_recipe_file;
204+
config.handle_program_state = handle_program_state;
205+
config.headless_mode = headless_mode;
206+
config.tool_comm_setup = std::move(tool_comm_setup);
207+
config.reverse_port = reverse_port;
208+
config.script_sender_port = script_sender_port;
209+
config.servoj_gain = servoj_gain;
210+
config.servoj_lookahead_time = servoj_lookahead_time;
211+
config.non_blocking_read = non_blocking_read;
212+
config.reverse_ip = reverse_ip;
213+
config.trajectory_port = trajectory_port;
214+
config.script_command_port = script_command_port;
215+
config.force_mode_damping = force_mode_damping;
216+
config.force_mode_gain_scaling = force_mode_gain_scaling;
217+
init(config);
218+
}
151219

152220
/*!
153221
* \brief Constructs a new UrDriver object.
@@ -178,13 +246,38 @@ class UrDriver
178246
* \param force_mode_damping The damping parameter used when the robot is in force mode, range [0,1]
179247
* \param force_mode_gain_scaling Scales the gain used when the robot is in force mode, range [0,2] (only e-series)
180248
*/
249+
// Called sigC in tests
250+
[[deprecated("Initializing a UrDriver object with an argument list is deprecated. Please use UrDriver(const "
251+
"UrDriverConfiguration& config) instead. This function will be removed in May 2027.")]]
181252
UrDriver(const std::string& robot_ip, const std::string& script_file, const std::string& output_recipe_file,
182253
const std::string& input_recipe_file, std::function<void(bool)> handle_program_state, bool headless_mode,
183254
std::unique_ptr<ToolCommSetup> tool_comm_setup, const std::string& calibration_checksum,
184255
const uint32_t reverse_port = 50001, const uint32_t script_sender_port = 50002, int servoj_gain = 2000,
185256
double servoj_lookahead_time = 0.03, bool non_blocking_read = false, const std::string& reverse_ip = "",
186257
const uint32_t trajectory_port = 50003, const uint32_t script_command_port = 50004,
187-
double force_mode_damping = 0.025, double force_mode_gain_scaling = 0.5);
258+
double force_mode_damping = 0.025, double force_mode_gain_scaling = 0.5)
259+
{
260+
UrDriverConfiguration config;
261+
config.robot_ip = robot_ip;
262+
config.script_file = script_file;
263+
config.output_recipe_file = output_recipe_file;
264+
config.input_recipe_file = input_recipe_file;
265+
config.handle_program_state = handle_program_state;
266+
config.headless_mode = headless_mode;
267+
config.calibration_checksum = calibration_checksum;
268+
config.tool_comm_setup = std::move(tool_comm_setup);
269+
config.reverse_port = reverse_port;
270+
config.script_sender_port = script_sender_port;
271+
config.servoj_gain = servoj_gain;
272+
config.servoj_lookahead_time = servoj_lookahead_time;
273+
config.non_blocking_read = non_blocking_read;
274+
config.reverse_ip = reverse_ip;
275+
config.trajectory_port = trajectory_port;
276+
config.script_command_port = script_command_port;
277+
config.force_mode_damping = force_mode_damping;
278+
config.force_mode_gain_scaling = force_mode_gain_scaling;
279+
init(config);
280+
}
188281
/*!
189282
* \brief Constructs a new UrDriver object.
190283
*
@@ -214,18 +307,36 @@ class UrDriver
214307
* \param force_mode_damping The damping parameter used when the robot is in force mode, range [0,1]
215308
* \param force_mode_gain_scaling Scales the gain used when the robot is in force mode, range [0,2] (only e-series)
216309
*/
310+
// Called sigD in tests
311+
[[deprecated("Initializing a UrDriver object with an argument list is deprecated. Please use UrDriver(const "
312+
"UrDriverConfiguration& config) instead. This function will be removed in May 2027.")]]
217313
UrDriver(const std::string& robot_ip, const std::string& script_file, const std::string& output_recipe_file,
218314
const std::string& input_recipe_file, std::function<void(bool)> handle_program_state, bool headless_mode,
219315
const std::string& calibration_checksum = "", const uint32_t reverse_port = 50001,
220316
const uint32_t script_sender_port = 50002, int servoj_gain = 2000, double servoj_lookahead_time = 0.03,
221317
bool non_blocking_read = false, const std::string& reverse_ip = "", const uint32_t trajectory_port = 50003,
222318
const uint32_t script_command_port = 50004, double force_mode_damping = 0.025,
223319
double force_mode_gain_scaling = 0.5)
224-
: UrDriver(robot_ip, script_file, output_recipe_file, input_recipe_file, handle_program_state, headless_mode,
225-
std::unique_ptr<ToolCommSetup>{}, calibration_checksum, reverse_port, script_sender_port, servoj_gain,
226-
servoj_lookahead_time, non_blocking_read, reverse_ip, trajectory_port, script_command_port,
227-
force_mode_damping, force_mode_gain_scaling)
228320
{
321+
UrDriverConfiguration config;
322+
config.robot_ip = robot_ip;
323+
config.script_file = script_file;
324+
config.output_recipe_file = output_recipe_file;
325+
config.input_recipe_file = input_recipe_file;
326+
config.handle_program_state = handle_program_state;
327+
config.headless_mode = headless_mode;
328+
config.calibration_checksum = calibration_checksum;
329+
config.reverse_port = reverse_port;
330+
config.script_sender_port = script_sender_port;
331+
config.servoj_gain = servoj_gain;
332+
config.servoj_lookahead_time = servoj_lookahead_time;
333+
config.non_blocking_read = non_blocking_read;
334+
config.reverse_ip = reverse_ip;
335+
config.trajectory_port = trajectory_port;
336+
config.script_command_port = script_command_port;
337+
config.force_mode_damping = force_mode_damping;
338+
config.force_mode_gain_scaling = force_mode_gain_scaling;
339+
init(config);
229340
}
230341

231342
virtual ~UrDriver() = default;
@@ -669,6 +780,7 @@ class UrDriver
669780

670781
private:
671782
static std::string readScriptFile(const std::string& filename);
783+
void init(const UrDriverConfiguration& config);
672784
/*!
673785
* \brief Reconnects the secondary stream used to send program to the robot.
674786
*
@@ -709,4 +821,4 @@ class UrDriver
709821
VersionInformation robot_version_;
710822
};
711823
} // namespace urcl
712-
#endif // ifndef UR_CLIENT_LIBRARY_UR_UR_DRIVER_H_INCLUDED
824+
#endif // ifndef UR_CLIENT_LIBRARY_UR_UR_DRIVER_H_INCLUDED

src/example_robot_wrapper.cpp

Lines changed: 9 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -53,11 +53,15 @@ ExampleRobotWrapper::ExampleRobotWrapper(const std::string& robot_ip, const std:
5353
throw UrException("Could not initialize robot with dashboard");
5454
}
5555

56-
std::unique_ptr<ToolCommSetup> tool_comm_setup;
57-
ur_driver_ =
58-
std::make_shared<UrDriver>(robot_ip, script_file, output_recipe_file, input_recipe_file,
59-
std::bind(&ExampleRobotWrapper::handleRobotProgramState, this, std::placeholders::_1),
60-
headless_mode, std::move(tool_comm_setup));
56+
UrDriverConfiguration driver_config;
57+
driver_config.robot_ip = robot_ip;
58+
driver_config.script_file = script_file;
59+
driver_config.output_recipe_file = output_recipe_file;
60+
driver_config.input_recipe_file = input_recipe_file;
61+
driver_config.handle_program_state =
62+
std::bind(&ExampleRobotWrapper::handleRobotProgramState, this, std::placeholders::_1);
63+
driver_config.headless_mode = headless_mode;
64+
ur_driver_ = std::make_shared<UrDriver>(driver_config);
6165

6266
if (!headless_mode && !std::empty(autostart_program))
6367
{

0 commit comments

Comments
 (0)