Skip to content

Commit 6cfa794

Browse files
urfeexurfeex
committed
Add a wrapper to handle all robot setup
1 parent 09c6908 commit 6cfa794

File tree

4 files changed

+171
-77
lines changed

4 files changed

+171
-77
lines changed

‎CMakeLists.txt‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -39,6 +39,7 @@ add_library(urcl SHARED
3939
src/ur/version_information.cpp
4040
src/rtde/rtde_writer.cpp
4141
src/default_log_handler.cpp
42+
src/example_robot_wrapper.cpp
4243
src/log.cpp
4344
src/helpers.cpp
4445
)

‎examples/freedrive_example.cpp‎

Lines changed: 13 additions & 77 deletions
Original file line numberDiff line numberDiff line change
@@ -36,6 +36,7 @@
3636
#include <ur_client_library/ur/dashboard_client.h>
3737
#include <ur_client_library/ur/ur_driver.h>
3838
#include <ur_client_library/types.h>
39+
#include <ur_client_library/example_robot_wrapper.h>
3940

4041
#include <chrono>
4142
#include <cstdlib>
@@ -47,27 +48,17 @@
4748
using namespace urcl;
4849

4950
const std::string DEFAULT_ROBOT_IP = "192.168.56.101";
50-
const std::string SCRIPT_FILE = "resources/external_control.urscript";
5151
const std::string OUTPUT_RECIPE = "examples/resources/rtde_output_recipe.txt";
5252
const std::string INPUT_RECIPE = "examples/resources/rtde_input_recipe.txt";
53-
const std::string CALIBRATION_CHECKSUM = "calib_12788084448423163542";
5453

55-
std::unique_ptr<UrDriver> g_my_driver;
56-
std::unique_ptr<DashboardClient> g_my_dashboard;
57-
58-
// We need a callback function to register. See UrDriver's parameters for details.
59-
void handleRobotProgramState(bool program_running)
60-
{
61-
// Print the text in green so we see it better
62-
std::cout << "\033[1;32mProgram running: " << std::boolalpha << program_running << "\033[0m\n" << std::endl;
63-
}
54+
std::unique_ptr<ExampleRobotWrapper> g_my_robot;
6455

6556
void sendFreedriveMessageOrDie(const control::FreedriveControlMessage freedrive_action)
6657
{
67-
bool ret = g_my_driver->writeFreedriveControlMessage(freedrive_action);
58+
bool ret = g_my_robot->ur_driver_->writeFreedriveControlMessage(freedrive_action);
6859
if (!ret)
6960
{
70-
URCL_LOG_ERROR("Could not send joint command. Is the robot in remote control?");
61+
URCL_LOG_ERROR("Could not send freedrive command. Is the robot in remote control?");
7162
exit(1);
7263
}
7364
}
@@ -89,86 +80,31 @@ int main(int argc, char* argv[])
8980
second_to_run = std::chrono::seconds(std::stoi(argv[2]));
9081
}
9182

92-
// Making the robot ready for the program by:
93-
// Connect the robot Dashboard
94-
g_my_dashboard.reset(new DashboardClient(robot_ip));
95-
if (!g_my_dashboard->connect())
96-
{
97-
URCL_LOG_ERROR("Could not connect to dashboard");
98-
return 1;
99-
}
100-
101-
// // Stop program, if there is one running
102-
if (!g_my_dashboard->commandStop())
103-
{
104-
URCL_LOG_ERROR("Could not send stop program command");
105-
return 1;
106-
}
107-
108-
// Power it off
109-
if (!g_my_dashboard->commandPowerOff())
110-
{
111-
URCL_LOG_ERROR("Could not send Power off command");
112-
return 1;
113-
}
83+
g_my_robot = std::make_unique<ExampleRobotWrapper>(robot_ip, OUTPUT_RECIPE, INPUT_RECIPE, true);
11484

115-
// Power it on
116-
if (!g_my_dashboard->commandPowerOn())
85+
if (!g_my_robot->waitForProgramRunning())
11786
{
118-
URCL_LOG_ERROR("Could not send Power on command");
87+
URCL_LOG_ERROR("External Control script not running.");
11988
return 1;
12089
}
12190

122-
// Release the brakes
123-
if (!g_my_dashboard->commandBrakeRelease())
124-
{
125-
URCL_LOG_ERROR("Could not send BrakeRelease command");
126-
return 1;
127-
}
128-
129-
// Now the robot is ready to receive a program
130-
std::unique_ptr<ToolCommSetup> tool_comm_setup;
131-
const bool headless = true;
132-
g_my_driver.reset(new UrDriver(robot_ip, SCRIPT_FILE, OUTPUT_RECIPE, INPUT_RECIPE, &handleRobotProgramState, headless,
133-
std::move(tool_comm_setup), CALIBRATION_CHECKSUM));
134-
135-
// Once RTDE communication is started, we have to make sure to read from the interface buffer, as
136-
// otherwise we will get pipeline overflows. Therefore, do this directly before starting your main
137-
// loop.
138-
g_my_driver->startRTDECommunication();
91+
URCL_LOG_INFO("Starting freedrive mode");
92+
sendFreedriveMessageOrDie(control::FreedriveControlMessage::FREEDRIVE_START);
13993

14094
std::chrono::duration<double> time_done(0);
14195
std::chrono::duration<double> timeout(second_to_run);
14296
auto stopwatch_last = std::chrono::steady_clock::now();
14397
auto stopwatch_now = stopwatch_last;
144-
g_my_driver->writeKeepalive();
145-
sendFreedriveMessageOrDie(control::FreedriveControlMessage::FREEDRIVE_START);
14698

147-
while (true)
99+
while (time_done < timeout || second_to_run.count() == 0)
148100
{
149-
// Read latest RTDE package. This will block for a hard-coded timeout (see UrDriver), so the
150-
// robot will effectively be in charge of setting the frequency of this loop.
151-
// In a real-world application this thread should be scheduled with real-time priority in order
152-
// to ensure that this is called in time.
153-
std::unique_ptr<rtde_interface::DataPackage> data_pkg = g_my_driver->getDataPackage();
154-
if (data_pkg)
155-
{
156-
sendFreedriveMessageOrDie(control::FreedriveControlMessage::FREEDRIVE_NOOP);
157-
158-
if (time_done > timeout && second_to_run.count() != 0)
159-
{
160-
URCL_LOG_INFO("Timeout reached.");
161-
break;
162-
}
163-
}
164-
else
165-
{
166-
URCL_LOG_WARN("Could not get fresh data package from robot");
167-
}
101+
sendFreedriveMessageOrDie(control::FreedriveControlMessage::FREEDRIVE_NOOP);
168102

169103
stopwatch_now = std::chrono::steady_clock::now();
170104
time_done += stopwatch_now - stopwatch_last;
171105
stopwatch_last = stopwatch_now;
172106
}
107+
108+
URCL_LOG_INFO("Stopping freedrive mode");
173109
sendFreedriveMessageOrDie(control::FreedriveControlMessage::FREEDRIVE_STOP);
174110
}

‎include/ur_client_library/example_robot_wrapper.h‎

Lines changed: 48 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,48 @@
1+
#ifndef UR_CLIENT_LIBRARY_EXAMPLE_ROBOT_WRAPPER_H_INCLUDED
2+
#define UR_CLIENT_LIBRARY_EXAMPLE_ROBOT_WRAPPER_H_INCLUDED
3+
4+
#include <ur_client_library/ur/dashboard_client.h>
5+
#include <ur_client_library/ur/ur_driver.h>
6+
#include <ur_client_library/log.h>
7+
8+
namespace urcl
9+
{
10+
class ExampleRobotWrapper
11+
{
12+
public:
13+
inline static const std::string DEFAULT_ROBOT_IP = "192.168.56.101";
14+
inline static const std::string SCRIPT_FILE = "resources/external_control.urscript";
15+
16+
ExampleRobotWrapper() = delete;
17+
ExampleRobotWrapper(const std::string& robot_ip, const std::string& output_recipe_file,
18+
const std::string& input_recipe_file, const bool headless_mode = true,
19+
const std::string& script_file = SCRIPT_FILE);
20+
21+
void initializeRobotWithDashboard();
22+
23+
void startRTDECommununication(const bool consume_data = false);
24+
void startConsumingRTDEData();
25+
void stopConsumingRTDEData();
26+
27+
bool waitForProgramRunning(int milliseconds = 100);
28+
29+
std::shared_ptr<urcl::DashboardClient> dashboard_client_;
30+
std::shared_ptr<urcl::UrDriver> ur_driver_;
31+
32+
private:
33+
void handleRobotProgramState(bool program_running);
34+
35+
std::atomic<bool> rtde_communication_started_ = false;
36+
std::atomic<bool> consume_rtde_packages_ = false;
37+
std::mutex read_package_mutex_;
38+
std::unique_ptr<rtde_interface::DataPackage> data_pkg_;
39+
40+
bool program_running_;
41+
std::condition_variable program_running_cv_;
42+
std::mutex program_running_mutex_;
43+
44+
std::thread rtde_consumer_thread_;
45+
};
46+
} // namespace urcl
47+
48+
#endif

‎src/example_robot_wrapper.cpp‎

Lines changed: 109 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,109 @@
1+
#include <ur_client_library/example_robot_wrapper.h>
2+
#include <iostream>
3+
4+
namespace urcl
5+
{
6+
ExampleRobotWrapper::ExampleRobotWrapper(const std::string& robot_ip, const std::string& output_recipe_file,
7+
const std::string& input_recipe_file, const bool headless_mode,
8+
const std::string& script_file)
9+
{
10+
dashboard_client_ = std::make_shared<DashboardClient>(robot_ip);
11+
12+
// Connect the robot Dashboard
13+
if (!dashboard_client_->connect())
14+
{
15+
URCL_LOG_ERROR("Could not connect to dashboard");
16+
}
17+
initializeRobotWithDashboard();
18+
19+
std::unique_ptr<ToolCommSetup> tool_comm_setup;
20+
ur_driver_ =
21+
std::make_shared<UrDriver>(robot_ip, script_file, output_recipe_file, input_recipe_file,
22+
std::bind(&ExampleRobotWrapper::handleRobotProgramState, this, std::placeholders::_1),
23+
headless_mode, std::move(tool_comm_setup));
24+
}
25+
void ExampleRobotWrapper::initializeRobotWithDashboard()
26+
{
27+
// // Stop program, if there is one running
28+
if (!dashboard_client_->commandStop())
29+
{
30+
URCL_LOG_ERROR("Could not send stop program command");
31+
}
32+
33+
// Power it off
34+
if (!dashboard_client_->commandPowerOff())
35+
{
36+
URCL_LOG_ERROR("Could not send Power off command");
37+
}
38+
39+
// Power it on
40+
if (!dashboard_client_->commandPowerOn())
41+
{
42+
URCL_LOG_ERROR("Could not send Power on command");
43+
}
44+
45+
// Release the brakes
46+
if (!dashboard_client_->commandBrakeRelease())
47+
{
48+
URCL_LOG_ERROR("Could not send BrakeRelease command");
49+
}
50+
51+
// Now the robot is ready to receive a program
52+
URCL_LOG_INFO("Robot ready to start a program");
53+
}
54+
55+
void ExampleRobotWrapper::handleRobotProgramState(bool program_running)
56+
{
57+
std::cout << "\033[1;32mProgram running: " << std::boolalpha << program_running << "\033[0m\n" << std::endl;
58+
program_running_ = program_running;
59+
if (program_running)
60+
{
61+
std::lock_guard<std::mutex> lk(program_running_mutex_);
62+
program_running_cv_.notify_one();
63+
}
64+
}
65+
66+
void ExampleRobotWrapper::startRTDECommununication(const bool consume_data)
67+
{
68+
if (!rtde_communication_started_)
69+
{
70+
ur_driver_->startRTDECommunication();
71+
rtde_communication_started_ = true;
72+
}
73+
if (consume_data)
74+
{
75+
startConsumingRTDEData();
76+
}
77+
}
78+
79+
void ExampleRobotWrapper::startConsumingRTDEData()
80+
{
81+
consume_rtde_packages_ = true;
82+
rtde_consumer_thread_ = std::thread([this]() {
83+
while (consume_rtde_packages_)
84+
{
85+
// Consume package to prevent pipeline overflow
86+
std::lock_guard<std::mutex> lk(read_package_mutex_);
87+
data_pkg_ = ur_driver_->getDataPackage();
88+
}
89+
});
90+
}
91+
92+
void ExampleRobotWrapper::stopConsumingRTDEData()
93+
{
94+
consume_rtde_packages_ = false;
95+
rtde_consumer_thread_.join();
96+
}
97+
98+
bool ExampleRobotWrapper::waitForProgramRunning(int milliseconds)
99+
{
100+
std::unique_lock<std::mutex> lk(program_running_mutex_);
101+
if (program_running_cv_.wait_for(lk, std::chrono::milliseconds(milliseconds)) == std::cv_status::no_timeout ||
102+
program_running_ == true)
103+
{
104+
return true;
105+
}
106+
return false;
107+
}
108+
109+
} // namespace urcl

0 commit comments

Comments
 (0)