Skip to content

Commit 3403344

Browse files
urfeexurfeex
committed
Use ExampleRobotWrapper for initialization in all examples
1 parent 316b1b3 commit 3403344

File tree

6 files changed

+112
-457
lines changed

6 files changed

+112
-457
lines changed

‎examples/force_mode_example.cpp‎

Lines changed: 26 additions & 91 deletions
Original file line numberDiff line numberDiff line change
@@ -33,6 +33,7 @@
3333
// In a real-world example it would be better to get those values from command line parameters / a
3434
// better configuration system such as Boost.Program_options
3535

36+
#include <ur_client_library/example_robot_wrapper.h>
3637
#include <ur_client_library/ur/dashboard_client.h>
3738
#include <ur_client_library/ur/ur_driver.h>
3839
#include <ur_client_library/types.h>
@@ -52,47 +53,18 @@ const std::string OUTPUT_RECIPE = "examples/resources/rtde_output_recipe.txt";
5253
const std::string INPUT_RECIPE = "examples/resources/rtde_input_recipe.txt";
5354
const std::string CALIBRATION_CHECKSUM = "calib_12788084448423163542";
5455

55-
std::unique_ptr<UrDriver> g_my_driver;
56-
std::unique_ptr<DashboardClient> g_my_dashboard;
57-
58-
std::condition_variable g_program_running_cv;
59-
std::mutex g_program_running_mutex;
60-
bool g_program_running;
61-
62-
// We need a callback function to register. See UrDriver's parameters for details.
63-
void handleRobotProgramState(bool program_running)
64-
{
65-
// Print the text in green so we see it better
66-
std::cout << "\033[1;32mProgram running: " << std::boolalpha << program_running << "\033[0m\n" << std::endl;
67-
if (program_running)
68-
{
69-
std::lock_guard<std::mutex> lk(g_program_running_mutex);
70-
g_program_running = program_running;
71-
g_program_running_cv.notify_one();
72-
}
73-
}
56+
std::unique_ptr<ExampleRobotWrapper> g_my_robot;
7457

7558
void sendFreedriveMessageOrDie(const control::FreedriveControlMessage freedrive_action)
7659
{
77-
bool ret = g_my_driver->writeFreedriveControlMessage(freedrive_action);
60+
bool ret = g_my_robot->ur_driver_->writeFreedriveControlMessage(freedrive_action);
7861
if (!ret)
7962
{
8063
URCL_LOG_ERROR("Could not send joint command. Is the robot in remote control?");
8164
exit(1);
8265
}
8366
}
8467

85-
bool waitForProgramRunning(int milliseconds = 100)
86-
{
87-
std::unique_lock<std::mutex> lk(g_program_running_mutex);
88-
if (g_program_running_cv.wait_for(lk, std::chrono::milliseconds(milliseconds)) == std::cv_status::no_timeout ||
89-
g_program_running == true)
90-
{
91-
return true;
92-
}
93-
return false;
94-
}
95-
9668
int main(int argc, char* argv[])
9769
{
9870
urcl::setLogLevel(urcl::LogLevel::INFO);
@@ -110,50 +82,11 @@ int main(int argc, char* argv[])
11082
second_to_run = std::chrono::seconds(std::stoi(argv[2]));
11183
}
11284

113-
// Making the robot ready for the program by:
114-
// Connect the robot Dashboard
115-
g_my_dashboard.reset(new DashboardClient(robot_ip));
116-
if (!g_my_dashboard->connect())
117-
{
118-
URCL_LOG_ERROR("Could not connect to dashboard");
119-
return 1;
120-
}
121-
122-
// // Stop program, if there is one running
123-
if (!g_my_dashboard->commandStop())
124-
{
125-
URCL_LOG_ERROR("Could not send stop program command");
126-
return 1;
127-
}
85+
bool headless_mode = true;
86+
g_my_robot = std::make_unique<ExampleRobotWrapper>(robot_ip, OUTPUT_RECIPE, INPUT_RECIPE, headless_mode,
87+
"external_control.urp");
12888

129-
// Power it off
130-
// if (!g_my_dashboard->commandPowerOff())
131-
//{
132-
// URCL_LOG_ERROR("Could not send Power off command");
133-
// return 1;
134-
//}
135-
136-
// Power it on
137-
// if (!g_my_dashboard->commandPowerOn())
138-
//{
139-
// URCL_LOG_ERROR("Could not send Power on command");
140-
// return 1;
141-
//}
142-
143-
// Release the brakes
144-
// if (!g_my_dashboard->commandBrakeRelease())
145-
//{
146-
// URCL_LOG_ERROR("Could not send BrakeRelease command");
147-
// return 1;
148-
//}
149-
150-
// Now the robot is ready to receive a program
151-
std::unique_ptr<ToolCommSetup> tool_comm_setup;
152-
const bool headless = true;
153-
g_my_driver.reset(new UrDriver(robot_ip, SCRIPT_FILE, OUTPUT_RECIPE, INPUT_RECIPE, &handleRobotProgramState, headless,
154-
std::move(tool_comm_setup)));
155-
156-
if (!g_my_driver->checkCalibration(CALIBRATION_CHECKSUM))
89+
if (!g_my_robot->ur_driver_->checkCalibration(CALIBRATION_CHECKSUM))
15790
{
15891
URCL_LOG_ERROR("Calibration checksum does not match actual robot.");
15992
URCL_LOG_ERROR("Use the ur_calibration tool to extract the correct calibration from the robot and pass that into "
@@ -163,7 +96,7 @@ int main(int argc, char* argv[])
16396
}
16497

16598
// Make sure that external control script is running
166-
if (!waitForProgramRunning())
99+
if (!g_my_robot->waitForProgramRunning())
167100
{
168101
URCL_LOG_ERROR("External Control script not running.");
169102
return 1;
@@ -175,22 +108,24 @@ int main(int argc, char* argv[])
175108
// Task frame at the robot's base with limits being large enough to cover the whole workspace
176109
// Compliance in z axis and rotation around z axis
177110
bool success;
178-
if (g_my_driver->getVersion().major < 5)
179-
success = g_my_driver->startForceMode({ 0, 0, 0, 0, 0, 0 }, // Task frame at the robot's base
180-
{ 0, 0, 1, 0, 0, 1 }, // Compliance in z axis and rotation around z axis
181-
{ 0, 0, -2, 0, 0, 0 }, // Press in -z direction
182-
2, // do not transform the force frame at all
183-
{ 0.1, 0.1, 1.5, 3.14, 3.14, 0.5 }, // limits
184-
0.005); // damping_factor. See ScriptManual for details.
111+
if (g_my_robot->ur_driver_->getVersion().major < 5)
112+
success =
113+
g_my_robot->ur_driver_->startForceMode({ 0, 0, 0, 0, 0, 0 }, // Task frame at the robot's base
114+
{ 0, 0, 1, 0, 0, 1 }, // Compliance in z axis and rotation around z axis
115+
{ 0, 0, -2, 0, 0, 0 }, // Press in -z direction
116+
2, // do not transform the force frame at all
117+
{ 0.1, 0.1, 1.5, 3.14, 3.14, 0.5 }, // limits
118+
0.005); // damping_factor. See ScriptManual for details.
185119
else
186120
{
187-
success = g_my_driver->startForceMode({ 0, 0, 0, 0, 0, 0 }, // Task frame at the robot's base
188-
{ 0, 0, 1, 0, 0, 1 }, // Compliance in z axis and rotation around z axis
189-
{ 0, 0, -2, 0, 0, 0 }, // Press in -z direction
190-
2, // do not transform the force frame at all
191-
{ 0.1, 0.1, 1.5, 3.14, 3.14, 0.5 }, // limits
192-
0.005, // damping_factor
193-
1.0); // gain_scaling. See ScriptManual for details.
121+
success =
122+
g_my_robot->ur_driver_->startForceMode({ 0, 0, 0, 0, 0, 0 }, // Task frame at the robot's base
123+
{ 0, 0, 1, 0, 0, 1 }, // Compliance in z axis and rotation around z axis
124+
{ 0, 0, -2, 0, 0, 0 }, // Press in -z direction
125+
2, // do not transform the force frame at all
126+
{ 0.1, 0.1, 1.5, 3.14, 3.14, 0.5 }, // limits
127+
0.005, // damping_factor
128+
1.0); // gain_scaling. See ScriptManual for details.
194129
}
195130
if (!success)
196131
{
@@ -204,13 +139,13 @@ int main(int argc, char* argv[])
204139
auto stopwatch_now = stopwatch_last;
205140
while (time_done < timeout || second_to_run.count() == 0)
206141
{
207-
g_my_driver->writeKeepalive();
142+
g_my_robot->ur_driver_->writeKeepalive();
208143

209144
stopwatch_now = std::chrono::steady_clock::now();
210145
time_done += stopwatch_now - stopwatch_last;
211146
stopwatch_last = stopwatch_now;
212147
std::this_thread::sleep_for(std::chrono::milliseconds(2));
213148
}
214149
URCL_LOG_INFO("Timeout reached.");
215-
g_my_driver->endForceMode();
150+
g_my_robot->ur_driver_->endForceMode();
216151
}

‎examples/full_driver.cpp‎

Lines changed: 12 additions & 80 deletions
Original file line numberDiff line numberDiff line change
@@ -28,6 +28,7 @@
2828
*/
2929
//----------------------------------------------------------------------
3030

31+
#include <ur_client_library/example_robot_wrapper.h>
3132
#include <ur_client_library/ur/dashboard_client.h>
3233
#include <ur_client_library/ur/ur_driver.h>
3334
#include <ur_client_library/types.h>
@@ -44,38 +45,9 @@ const std::string SCRIPT_FILE = "resources/external_control.urscript";
4445
const std::string OUTPUT_RECIPE = "examples/resources/rtde_output_recipe.txt";
4546
const std::string INPUT_RECIPE = "examples/resources/rtde_input_recipe.txt";
4647

47-
std::unique_ptr<UrDriver> g_my_driver;
48-
std::unique_ptr<DashboardClient> g_my_dashboard;
48+
std::unique_ptr<ExampleRobotWrapper> g_my_robot;
4949
vector6d_t g_joint_positions;
5050

51-
std::condition_variable g_program_running_cv;
52-
std::mutex g_program_running_mutex;
53-
bool g_program_running;
54-
55-
// We need a callback function to register. See UrDriver's parameters for details.
56-
void handleRobotProgramState(bool program_running)
57-
{
58-
// Print the text in green so we see it better
59-
std::cout << "\033[1;32mProgram running: " << std::boolalpha << program_running << "\033[0m\n" << std::endl;
60-
if (program_running)
61-
{
62-
std::lock_guard<std::mutex> lk(g_program_running_mutex);
63-
g_program_running = program_running;
64-
g_program_running_cv.notify_one();
65-
}
66-
}
67-
68-
bool waitForProgramRunning(int milliseconds = 100)
69-
{
70-
std::unique_lock<std::mutex> lk(g_program_running_mutex);
71-
if (g_program_running_cv.wait_for(lk, std::chrono::milliseconds(milliseconds)) == std::cv_status::no_timeout ||
72-
g_program_running == true)
73-
{
74-
return true;
75-
}
76-
return false;
77-
}
78-
7951
int main(int argc, char* argv[])
8052
{
8153
urcl::setLogLevel(urcl::LogLevel::INFO);
@@ -87,59 +59,19 @@ int main(int argc, char* argv[])
8759
robot_ip = std::string(argv[1]);
8860
}
8961

90-
// Making the robot ready for the program by:
91-
// Connect the the robot Dashboard
92-
g_my_dashboard.reset(new DashboardClient(robot_ip));
93-
if (!g_my_dashboard->connect())
94-
{
95-
URCL_LOG_ERROR("Could not connect to dashboard");
96-
return 1;
97-
}
98-
99-
// Stop program, if there is one running
100-
if (!g_my_dashboard->commandStop())
101-
{
102-
URCL_LOG_ERROR("Could not send stop program command");
103-
return 1;
104-
}
105-
106-
// Power it off
107-
if (!g_my_dashboard->commandPowerOff())
108-
{
109-
URCL_LOG_ERROR("Could not send Power off command");
110-
return 1;
111-
}
112-
113-
// Power it on
114-
if (!g_my_dashboard->commandPowerOn())
115-
{
116-
URCL_LOG_ERROR("Could not send Power on command");
117-
return 1;
118-
}
119-
120-
// Release the brakes
121-
if (!g_my_dashboard->commandBrakeRelease())
122-
{
123-
URCL_LOG_ERROR("Could not send BrakeRelease command");
124-
return 1;
125-
}
126-
127-
// Now the robot is ready to receive a program
128-
129-
std::unique_ptr<ToolCommSetup> tool_comm_setup;
130-
const bool headless = true;
131-
g_my_driver.reset(new UrDriver(robot_ip, SCRIPT_FILE, OUTPUT_RECIPE, INPUT_RECIPE, &handleRobotProgramState, headless,
132-
std::move(tool_comm_setup)));
62+
bool headless_mode = true;
63+
g_my_robot = std::make_unique<ExampleRobotWrapper>(robot_ip, OUTPUT_RECIPE, INPUT_RECIPE, headless_mode,
64+
"external_control.urp");
13365
// Make sure that external control script is running
134-
if (!waitForProgramRunning())
66+
if (!g_my_robot->waitForProgramRunning())
13567
{
13668
URCL_LOG_ERROR("External Control script not running.");
13769
return 1;
13870
}
13971

14072
// Increment depends on robot version
14173
double increment_constant = 0.0005;
142-
if (g_my_driver->getVersion().major < 5)
74+
if (g_my_robot->ur_driver_->getVersion().major < 5)
14375
{
14476
increment_constant = 0.002;
14577
}
@@ -154,14 +86,14 @@ int main(int argc, char* argv[])
15486
// Once RTDE communication is started, we have to make sure to read from the interface buffer, as
15587
// otherwise we will get pipeline overflows. Therefor, do this directly before starting your main
15688
// loop.
157-
g_my_driver->startRTDECommunication();
89+
g_my_robot->ur_driver_->startRTDECommunication();
15890
while (!(passed_positive_part && passed_negative_part))
15991
{
16092
// Read latest RTDE package. This will block for a hard-coded timeout (see UrDriver), so the
16193
// robot will effectively be in charge of setting the frequency of this loop.
16294
// In a real-world application this thread should be scheduled with real-time priority in order
16395
// to ensure that this is called in time.
164-
std::unique_ptr<rtde_interface::DataPackage> data_pkg = g_my_driver->getDataPackage();
96+
std::unique_ptr<rtde_interface::DataPackage> data_pkg = g_my_robot->ur_driver_->getDataPackage();
16597
if (!data_pkg)
16698
{
16799
URCL_LOG_WARN("Could not get fresh data package from robot");
@@ -201,16 +133,16 @@ int main(int argc, char* argv[])
201133
joint_target[5] += increment;
202134
// Setting the RobotReceiveTimeout time is for example purposes only. This will make the example running more
203135
// reliable on non-realtime systems. Use with caution in productive applications.
204-
bool ret = g_my_driver->writeJointCommand(joint_target, comm::ControlMode::MODE_SERVOJ,
205-
RobotReceiveTimeout::millisec(100));
136+
bool ret = g_my_robot->ur_driver_->writeJointCommand(joint_target, comm::ControlMode::MODE_SERVOJ,
137+
RobotReceiveTimeout::millisec(100));
206138
if (!ret)
207139
{
208140
URCL_LOG_ERROR("Could not send joint command. Is the robot in remote control?");
209141
return 1;
210142
}
211143
URCL_LOG_DEBUG("data_pkg:\n%s", data_pkg->toString().c_str());
212144
}
213-
g_my_driver->stopControl();
145+
g_my_robot->ur_driver_->stopControl();
214146
URCL_LOG_INFO("Movement done");
215147
return 0;
216148
}

0 commit comments

Comments
 (0)