Obbart/universal_robots_ros_driver
1
0
Fork 0
mirror of https://gitlab.com/obbart/universal_robots_ros_driver.git synced 2026-04-10 01:50:46 +02:00
Code Releases Activity

implemented full rtde handshake to initialize constant publishing to

Browse Source 
data packages by the robot
This commit is contained in:
Tristan Schnell
2019-04-11 13:58:34 +02:00
parent 8975957b5f
commit 2eea2bf8fc
2 changed files with 37 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
include/ur_rtde_driver/rtde/rtde_client.h
View File

@@ -36,6 +36,8 @@
#include "ur_rtde_driver/comm/producer.h"
#include "ur_rtde_driver/rtde/data_package.h"
#include "ur_rtde_driver/rtde/request_protocol_version.h"
#include "ur_rtde_driver/rtde/control_package_setup_outputs.h"
#include "ur_rtde_driver/rtde/control_package_start.h"
static const int UR_RTDE_PORT = 30004;
static const std::string PIPELINE_NAME = "RTDE Data Pipeline";
@@ -50,6 +52,8 @@ public:
RTDEClient() = delete;
RTDEClient(std::string ROBOT_IP, comm::INotifier& notifier);
~RTDEClient() = default;
bool init();
bool start();
bool getDataPackage(std::unique_ptr<comm::URPackage<PackageHeader>>& data_package, std::chrono::milliseconds timeout);
private:
@@ -57,6 +61,8 @@ private:
RTDEParser parser_;
comm::URProducer<PackageHeader> prod_;
comm::Pipeline<PackageHeader> pipeline_;
std::vector<std::string> readRecipe();
};
} // namespace rtde_interface

43
src/rtde/rtde_client.cpp
View File

@@ -34,24 +34,43 @@ namespace rtde_interface
RTDEClient::RTDEClient(std::string ROBOT_IP, comm::INotifier& notifier)
: stream_(ROBOT_IP, UR_RTDE_PORT), parser_(), prod_(stream_, parser_),
pipeline_(prod_, PIPELINE_NAME, notifier)
{
}
bool RTDEClient::init()
{
pipeline_.run();
sleep(1);
uint8_t buffer[4096];
size_t size;
size_t written;
size = RequestProtocolVersionRequest::generateSerializedRequest(buffer);
std::cout << "size: " << size << std::endl;
std::cout << "buffer: " << static_cast<int>(buffer[0]) << " - "
<< static_cast<int>(buffer[1]) << " - "
<< static_cast<int>(buffer[2]) << " - "
<< static_cast<int>(buffer[3]) << " - "
<< static_cast<int>(buffer[4]) << std::endl;
stream_.write(buffer, size, written);
size = RequestProtocolVersionRequest::generateSerializedRequest(buffer, 2);
stream_.write(buffer, size, written);
std::unique_ptr<comm::URPackage<PackageHeader>> package;
std::cout << "wrote: " << written << std::endl;
pipeline_.getLatestProduct(package, std::chrono::milliseconds(1000));
size = ControlPackageSetupOutputsRequest::generateSerializedRequest(buffer, 500.0, readRecipe());
stream_.write(buffer, size, written);
return pipeline_.getLatestProduct(package, std::chrono::milliseconds(1000));
}
bool RTDEClient::start()
{
uint8_t buffer[4096];
size_t size;
size_t written;
size = ControlPackageStartRequest::generateSerializedRequest(buffer);
std::unique_ptr<comm::URPackage<PackageHeader>> package;
stream_.write(buffer, size, written);
return pipeline_.getLatestProduct(package, std::chrono::milliseconds(1000));
}
std::vector<std::string> RTDEClient::readRecipe()
{
std::vector<std::string> recipe;
recipe.push_back("timestamp");
recipe.push_back("actual_q");
recipe.push_back("actual_qd");
recipe.push_back("speed_scaling");
recipe.push_back("target_speed_fraction");
return recipe;
}
bool RTDEClient::getDataPackage(