Obbart/universal_robots_ros_driver
1
0
Fork 0
mirror of https://gitlab.com/obbart/universal_robots_ros_driver.git synced 2026-04-12 11:00:47 +02:00
Code Releases Activity

moved calibration to own repository

Browse Source
This commit is contained in:
Felix Mauch
2019-05-27 16:30:21 +02:00
parent ceb00d8d6d
commit 312fe8b1b7
110 changed files with 250 additions and 46 deletions
Download Patch File Download Diff File Expand all files Collapse all files

157
ur_rtde_driver/src/comm/server.cpp Normal file
View File

@@ -0,0 +1,157 @@
/*
* Copyright 2017, 2018 Jarek Potiuk (low bandwidth trajectory follower)
*
* Copyright 2017, 2018 Simon Rasmussen (refactor)
*
* Copyright 2015, 2016 Thomas Timm Andersen (original version)
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#include "ur_rtde_driver/comm/server.h"
#include <arpa/inet.h>
#include <netinet/tcp.h>
#include <unistd.h>
#include <cstring>
#include "ur_rtde_driver/log.h"
namespace ur_driver
{
namespace comm
{
URServer::URServer(int port) : port_(port)
{
}
URServer::~URServer()
{
TCPSocket::close();
}
std::string URServer::getIP()
{
sockaddr_in name;
socklen_t len = sizeof(name);
int res = ::getsockname(getSocketFD(), (sockaddr*)&name, &len);
if (res < 0)
{
LOG_ERROR("Could not get local IP");
return std::string();
}
char buf[128];
inet_ntop(AF_INET, &name.sin_addr, buf, sizeof(buf));
return std::string(buf);
}
bool URServer::open(int socket_fd, struct sockaddr* address, size_t address_len)
{
int flag = 1;
setsockopt(socket_fd, SOL_SOCKET, SO_REUSEADDR, &flag, sizeof(int));
return ::bind(socket_fd, address, address_len) == 0;
}
bool URServer::bind()
{
std::string empty;
bool res = TCPSocket::setup(empty, port_);
if (!res)
return false;
if (::listen(getSocketFD(), 1) < 0)
return false;
return true;
}
bool URServer::accept()
{
if (TCPSocket::getState() != comm::SocketState::Connected || client_.getSocketFD() > 0)
return false;
struct sockaddr addr;
socklen_t addr_len;
int client_fd = -1;
int retry = 0;
while ((client_fd = ::accept(getSocketFD(), &addr, &addr_len)) == -1)
{
LOG_ERROR("Accepting socket connection failed. (errno: %d)", errno);
if (retry++ >= 5)
return false;
}
TCPSocket::setOptions(client_fd);
return client_.setSocketFD(client_fd);
}
void URServer::disconnectClient()
{
if (client_.getState() != comm::SocketState::Connected)
return;
client_.close();
}
bool URServer::write(const uint8_t* buf, size_t buf_len, size_t& written)
{
return client_.write(buf, buf_len, written);
}
bool URServer::readLine(char* buffer, size_t buf_len)
{
char* current_pointer = buffer;
char ch;
size_t total_read;
if (buf_len <= 0 || buffer == NULL)
{
return false;
}
total_read = 0;
for (;;)
{
if (client_.read(&ch))
{
if (total_read < buf_len - 1) // just in case ...
{
total_read++;
*current_pointer++ = ch;
}
if (ch == '\n')
{
break;
}
}
else
{
if (total_read == 0)
{
return false;
}
else
{
break;
}
}
}
*current_pointer = '\0';
return true;
}
} // namespace comm
} // namespace ur_driver

192
ur_rtde_driver/src/comm/tcp_socket.cpp Normal file
View File

@@ -0,0 +1,192 @@
/*
* Copyright 2017, 2018 Jarek Potiuk (low bandwidth trajectory follower)
*
* Copyright 2017, 2018 Simon Rasmussen (refactor)
*
* Copyright 2015, 2016 Thomas Timm Andersen (original version)
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#include <arpa/inet.h>
#include <endian.h>
#include <netinet/tcp.h>
#include <unistd.h>
#include <cstring>
#include "ur_rtde_driver/log.h"
#include "ur_rtde_driver/comm/tcp_socket.h"
namespace ur_driver
{
namespace comm
{
TCPSocket::TCPSocket() : socket_fd_(-1), state_(SocketState::Invalid)
{
}
TCPSocket::~TCPSocket()
{
close();
}
void TCPSocket::setOptions(int socket_fd)
{
int flag = 1;
setsockopt(socket_fd, IPPROTO_TCP, TCP_NODELAY, &flag, sizeof(int));
setsockopt(socket_fd, IPPROTO_TCP, TCP_QUICKACK, &flag, sizeof(int));
}
bool TCPSocket::setup(std::string& host, int port)
{
if (state_ == SocketState::Connected)
return false;
LOG_INFO("Setting up connection: %s:%d", host.c_str(), port);
// gethostbyname() is deprecated so use getadderinfo() as described in:
// http://www.beej.us/guide/bgnet/output/html/multipage/syscalls.html#getaddrinfo
const char* host_name = host.empty() ? nullptr : host.c_str();
std::string service = std::to_string(port);
struct addrinfo hints, *result;
std::memset(&hints, 0, sizeof(hints));
hints.ai_family = AF_UNSPEC;
hints.ai_socktype = SOCK_STREAM;
hints.ai_flags = AI_PASSIVE;
if (getaddrinfo(host_name, service.c_str(), &hints, &result) != 0)
{
LOG_ERROR("Failed to get address for %s:%d", host.c_str(), port);
return false;
}
bool connected = false;
// loop through the list of addresses untill we find one that's connectable
for (struct addrinfo* p = result; p != nullptr; p = p->ai_next)
{
socket_fd_ = ::socket(p->ai_family, p->ai_socktype, p->ai_protocol);
if (socket_fd_ != -1 && open(socket_fd_, p->ai_addr, p->ai_addrlen))
{
connected = true;
break;
}
}
freeaddrinfo(result);
if (!connected)
{
state_ = SocketState::Invalid;
LOG_ERROR("Connection setup failed for %s:%d", host.c_str(), port);
}
else
{
setOptions(socket_fd_);
state_ = SocketState::Connected;
LOG_INFO("Connection established for %s:%d", host.c_str(), port);
}
return connected;
}
bool TCPSocket::setSocketFD(int socket_fd)
{
if (state_ == SocketState::Connected)
return false;
socket_fd_ = socket_fd;
state_ = SocketState::Connected;
return true;
}
void TCPSocket::close()
{
if (state_ != SocketState::Connected)
return;
state_ = SocketState::Closed;
::close(socket_fd_);
socket_fd_ = -1;
}
std::string TCPSocket::getIP()
{
sockaddr_in name;
socklen_t len = sizeof(name);
int res = ::getsockname(socket_fd_, (sockaddr*)&name, &len);
if (res < 0)
{
LOG_ERROR("Could not get local IP");
return std::string();
}
char buf[128];
inet_ntop(AF_INET, &name.sin_addr, buf, sizeof(buf));
return std::string(buf);
}
bool TCPSocket::read(char* character)
{
size_t read_chars;
// It's inefficient, but in our case we read very small messages
// and the overhead connected with reading character by character is
// negligible - adding buffering would complicate the code needlessly.
return read((uint8_t*)character, 1, read_chars);
}
bool TCPSocket::read(uint8_t* buf, const size_t buf_len, size_t& read)
{
read = 0;
if (state_ != SocketState::Connected)
return false;
ssize_t res = ::recv(socket_fd_, buf, buf_len, 0);
if (res == 0)
{
state_ = SocketState::Disconnected;
return false;
}
else if (res < 0)
return false;
read = static_cast<size_t>(res);
return true;
}
bool TCPSocket::write(const uint8_t* buf, const size_t buf_len, size_t& written)
{
written = 0;
if (state_ != SocketState::Connected)
return false;
size_t remaining = buf_len;
// handle partial sends
while (written < buf_len)
{
ssize_t sent = ::send(socket_fd_, buf + written, remaining, 0);
if (sent <= 0)
return false;
written += sent;
remaining -= sent;
}
return true;
}
} // namespace comm
} // namespace ur_driver

40
ur_rtde_driver/src/main_plain_driver.cpp Normal file
View File

@@ -0,0 +1,40 @@
// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
// -- BEGIN LICENSE BLOCK ----------------------------------------------
// -- END LICENSE BLOCK ------------------------------------------------
//----------------------------------------------------------------------
/*!\file
*
* \author Felix Mauch mauch@fzi.de
* \date 2019-04-11
*
*/
//----------------------------------------------------------------------
#include <ur_rtde_driver/ur/ur_driver.h>
using namespace ur_driver;
int main(int argc, char* argv[])
{
std::string ROBOT_IP = "192.168.56.101";
if (argc > 1)
{
ROBOT_IP = argv[1];
}
UrDriver driver(ROBOT_IP);
while (true)
{
sleep(1);
std::unique_ptr<rtde_interface::DataPackage> data_pkg = driver.getDataPackage();
if (data_pkg)
{
data_pkg->toString();
}
}
return 0;
}

54
ur_rtde_driver/src/primary/primary_package.cpp Normal file
View File

@@ -0,0 +1,54 @@
// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
// -- BEGIN LICENSE BLOCK ----------------------------------------------
// Copyright 2019 FZI Forschungszentrum Informatik
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// -- END LICENSE BLOCK ------------------------------------------------
//----------------------------------------------------------------------
/*!\file
*
* \author Felix Mauch mauch@fzi.de
* \date 2019-04-09
*
*/
//----------------------------------------------------------------------
#include "ur_rtde_driver/primary/primary_package.h"
namespace ur_driver
{
namespace primary_interface
{
bool PrimaryPackage::parseWith(comm::BinParser& bp)
{
bp.rawData(buffer_, buffer_length_);
return true;
}
std::string PrimaryPackage::toString() const
{
std::stringstream ss;
ss << "Raw byte stream: ";
for (size_t i = 0; i < buffer_length_; ++i)
{
uint8_t* buf = buffer_.get();
ss << std::hex << static_cast<int>(buf[i]) << " ";
}
ss << std::endl;
return ss.str();
}
} // namespace primary_interface
} // namespace ur_driver

49
ur_rtde_driver/src/primary/robot_message.cpp Normal file
View File

@@ -0,0 +1,49 @@
// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
// -- BEGIN LICENSE BLOCK ----------------------------------------------
// Copyright 2019 FZI Forschungszentrum Informatik (ur_rtde_driver)
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// -- END LICENSE BLOCK ------------------------------------------------
//----------------------------------------------------------------------
/*!\file
*
* \author Felix Mauch mauch@fzi.de
* \date 2019-04-09
*
*/
//----------------------------------------------------------------------
#include "ur_rtde_driver/primary/robot_message.h"
namespace ur_driver
{
namespace primary_interface
{
bool RobotMessage::parseWith(comm::BinParser& bp)
{
return true;
}
std::string RobotMessage::toString() const
{
std::stringstream ss;
ss << "timestamp: " << timestamp_ << std::endl;
ss << "source: " << static_cast<int>(source_) << std::endl;
ss << "message_type: " << static_cast<int>(message_type_) << std::endl;
return ss.str();
}
} // namespace primary_interface
} // namespace ur_driver

61
ur_rtde_driver/src/primary/robot_message/version_message.cpp Normal file
View File

@@ -0,0 +1,61 @@
// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
// -- BEGIN LICENSE BLOCK ----------------------------------------------
// Copyright 2019 FZI Forschungszentrum Informatik (ur_rtde_driver)
// Copyright 2017, 2018 Simon Rasmussen (refactor)
//
// Copyright 2015, 2016 Thomas Timm Andersen (original version)
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// -- END LICENSE BLOCK ------------------------------------------------
//----------------------------------------------------------------------
/*!\file
*
* \author Felix Mauch mauch@fzi.de
* \date 2019-04-08
*
*/
//----------------------------------------------------------------------
#include "ur_rtde_driver/log.h"
#include "ur_rtde_driver/primary/robot_message/version_message.h"
namespace ur_driver
{
namespace primary_interface
{
bool VersionMessage::parseWith(comm::BinParser& bp)
{
bp.parse(project_name_length_);
bp.parse(project_name_, project_name_length_);
bp.parse(major_version_);
bp.parse(minor_version_);
bp.parse(svn_version_);
bp.parse(build_number_);
bp.parseRemainder(build_date_);
return true; // not really possible to check dynamic size packets
}
std::string VersionMessage::toString() const
{
std::stringstream ss;
ss << "project name: " << project_name_ << std::endl;
ss << "version: " << unsigned(major_version_) << "." << unsigned(minor_version_) << "." << svn_version_ << std::endl;
ss << "build date: " << build_date_;
return ss.str();
}
} // namespace primary_interface
} // namespace ur_driver

76
ur_rtde_driver/src/primary/robot_state/kinematics_info.cpp Normal file
View File

@@ -0,0 +1,76 @@
// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
// -- BEGIN LICENSE BLOCK ----------------------------------------------
// -- END LICENSE BLOCK ------------------------------------------------
//----------------------------------------------------------------------
/*!\file
*
* \author Felix Mauch mauch@fzi.de
* \date 2019-04-08
*
*/
//----------------------------------------------------------------------
#include "ur_rtde_driver/log.h"
#include "ur_rtde_driver/primary/robot_state/kinematics_info.h"
namespace ur_driver
{
namespace primary_interface
{
bool KinematicsInfo::parseWith(comm::BinParser& bp)
{
bp.parse(checksum_);
bp.parse(dh_theta_);
bp.parse(dh_a_);
bp.parse(dh_d_);
bp.parse(dh_alpha_);
bp.parse(calibration_status_);
return true;
}
std::string KinematicsInfo::toString() const
{
std::stringstream os;
os << "checksum: [";
for (size_t i = 0; i < checksum_.size(); ++i)
{
os << checksum_[i] << " ";
}
os << "]" << std::endl;
os << "dh_theta: [";
for (size_t i = 0; i < dh_theta_.size(); ++i)
{
os << dh_theta_[i] << " ";
}
os << "]" << std::endl;
os << "dh_a: [";
for (size_t i = 0; i < dh_a_.size(); ++i)
{
os << dh_a_[i] << " ";
}
os << "]" << std::endl;
os << "dh_d: [";
for (size_t i = 0; i < dh_d_.size(); ++i)
{
os << dh_d_[i] << " ";
}
os << "]" << std::endl;
os << "dh_alpha: [";
for (size_t i = 0; i < dh_alpha_.size(); ++i)
{
os << dh_alpha_[i] << " ";
}
os << "]" << std::endl;
os << "calibration_status: " << calibration_status_ << std::endl;
return os.str();
}
} // namespace primary_interface
} // namespace ur_driver

57
ur_rtde_driver/src/producer.cpp Normal file
View File

@@ -0,0 +1,57 @@
// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
// -- BEGIN LICENSE BLOCK ----------------------------------------------
// -- END LICENSE BLOCK ------------------------------------------------
//----------------------------------------------------------------------
/*!\file
*
* \author Felix Mauch mauch@fzi.de
* \date 2018-12-11
*
*/
//----------------------------------------------------------------------
#include <ur_rtde_driver/comm/producer.h>
#include <ur_rtde_driver/comm/shell_consumer.h>
#include <ur_rtde_driver/comm/stream.h>
#include <ur_rtde_driver/comm/parser.h>
#include <ur_rtde_driver/comm/pipeline.h>
#include <ur_rtde_driver/primary/package_header.h>
#include <ur_rtde_driver/primary/primary_parser.h>
static const int UR_PRIMARY_PORT = 30001;
static const int UR_SECONDARY_PORT = 30002;
static const int UR_RT_PORT = 30003;
using namespace ur_driver;
using namespace primary_interface;
int main(int argc, char* argv[])
{
std::string ROBOT_IP = "192.168.56.101";
// std::string ROBOT_IP = "192.168.0.104";
comm::URStream<PackageHeader> stream(ROBOT_IP, UR_PRIMARY_PORT);
if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug))
{
ros::console::notifyLoggerLevelsChanged();
}
primary_interface::PrimaryParser parser;
comm::URProducer<PackageHeader> prod(stream, parser);
comm::ShellConsumer<PackageHeader> consumer;
comm::INotifier notifier;
comm::Pipeline<PackageHeader> pipeline(prod, consumer, "Pipeline", notifier);
LOG_INFO("Running now");
pipeline.run();
while (true)
{
sleep(1);
// LOG_INFO("Still running");
}
return 0;
}

143
ur_rtde_driver/src/ros/hardware_interface.cpp Normal file
View File

@@ -0,0 +1,143 @@
// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
// -- BEGIN LICENSE BLOCK ----------------------------------------------
// Copyright 2019 FZI Forschungszentrum Informatik
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// -- END LICENSE BLOCK ------------------------------------------------
//----------------------------------------------------------------------
/*!\file
*
* \author Felix Mauch mauch@fzi.de
* \date 2019-04-11
*
*/
//----------------------------------------------------------------------
#include "ur_rtde_driver/ros/hardware_interface.h"
namespace ur_driver
{
HardwareInterface::HardwareInterface()
: joint_position_command_{ 0, 0, 0, 0, 0, 0 }
, joint_positions_{ 0, 0, 0, 0, 0, 0 }
, joint_velocities_{ 0, 0, 0, 0, 0, 0 }
, joint_efforts_{ 0, 0, 0, 0, 0, 0 }
, joint_names_(6)
, position_controller_running_(false)
{
}
bool HardwareInterface ::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh)
{
joint_velocities_ = { 0, 0, 0, 0, 0, 0 };
joint_efforts_ = { 0, 0, 0, 0, 0, 0 };
std::string ROBOT_IP = robot_hw_nh.param<std::string>("robot_ip", "192.168.56.101");
ROS_INFO_STREAM("Initializing urdriver");
ur_driver_.reset(new UrDriver(ROBOT_IP));
if (!root_nh.getParam("hardware_interface/joints", joint_names_))
{
ROS_ERROR_STREAM("Cannot find required parameter " << root_nh.resolveName("hardware_interface/joints")
<< " on the parameter server.");
throw std::runtime_error("Cannot find required parameter "
"'controller_joint_names' on the parameter server.");
}
// Create ros_control interfaces
for (std::size_t i = 0; i < joint_positions_.size(); ++i)
{
// Create joint state interface for all joints
js_interface_.registerHandle(hardware_interface::JointStateHandle(joint_names_[i], &joint_positions_[i],
&joint_velocities_[i], &joint_efforts_[i]));
// Create joint position control interface
pj_interface_.registerHandle(
hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[i]), &joint_position_command_[i]));
}
// Register interfaces
registerInterface(&js_interface_);
registerInterface(&pj_interface_);
ROS_INFO_STREAM_NAMED("hardware_interface", "Loaded ur_rtde_driver hardware_interface");
return true;
}
void HardwareInterface ::read(const ros::Time& time, const ros::Duration& period)
{
std::unique_ptr<rtde_interface::DataPackage> data_pkg = ur_driver_->getDataPackage();
if (data_pkg)
{
if (!data_pkg->getData("actual_q", joint_positions_))
{
// This throwing should never happen unless misconfigured
throw std::runtime_error("Did not find joint position in data sent from robot. This should not happen!");
}
if (!data_pkg->getData("actual_qd", joint_velocities_))
{
// This throwing should never happen unless misconfigured
throw std::runtime_error("Did not find joint velocity in data sent from robot. This should not happen!");
}
}
else
{
ROS_ERROR("Could not get fresh data package from robot");
}
}
void HardwareInterface ::write(const ros::Time& time, const ros::Duration& period)
{
if (position_controller_running_)
{
ur_driver_->writeJointCommand(joint_position_command_);
}
}
bool HardwareInterface ::prepareSwitch(const std::list<hardware_interface::ControllerInfo>& start_list,
const std::list<hardware_interface::ControllerInfo>& stop_list)
{
// TODO: Implement
return true;
}
void HardwareInterface ::doSwitch(const std::list<hardware_interface::ControllerInfo>& start_list,
const std::list<hardware_interface::ControllerInfo>& stop_list)
{
position_controller_running_ = false;
for (auto& controller_it : start_list)
{
for (auto& resource_it : controller_it.claimed_resources)
{
if (resource_it.hardware_interface == "hardware_interface::PositionJointInterface")
{
position_controller_running_ = true;
}
}
}
}
uint32_t HardwareInterface ::getControlFrequency() const
{
if (ur_driver_ != nullptr)
{
return ur_driver_->getControlFrequency();
}
// TODO: Do this nicer than throwing an exception
throw std::runtime_error("ur_driver is not yet initialized");
}
} // namespace ur_driver

86
ur_rtde_driver/src/ros/hardware_interface_node.cpp Normal file
View File

@@ -0,0 +1,86 @@
// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
// -- BEGIN LICENSE BLOCK ----------------------------------------------
// Copyright 2019 FZI Forschungszentrum Informatik
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// -- END LICENSE BLOCK ------------------------------------------------
//----------------------------------------------------------------------
/*!\file
*
* \author Felix Mauch mauch@fzi.de
* \date 2019-04-11
*
*/
//----------------------------------------------------------------------
#include <ros/ros.h>
#include <controller_manager/controller_manager.h>
#include <ur_rtde_driver/ros/hardware_interface.h>
int main(int argc, char** argv)
{
// Set up ROS.
ros::init(argc, argv, "ur_hardware_interface");
ros::AsyncSpinner spinner(2);
spinner.start();
ros::NodeHandle nh;
ros::NodeHandle nh_priv("~");
ur_driver::HardwareInterface hw_interface;
// Set up timers
ros::Time timestamp;
ros::Duration period;
auto stopwatch_last = std::chrono::steady_clock::now();
auto stopwatch_now = stopwatch_last;
hw_interface.init(nh, nh_priv);
controller_manager::ControllerManager cm(&hw_interface, nh);
// Get current time and elapsed time since last read
timestamp = ros::Time::now();
stopwatch_now = std::chrono::steady_clock::now();
period.fromSec(std::chrono::duration_cast<std::chrono::duration<double>>(stopwatch_now - stopwatch_last).count());
stopwatch_last = stopwatch_now;
ros::Rate control_rate(static_cast<double>(hw_interface.getControlFrequency()));
// Run as fast as possible
while (ros::ok())
{
// Receive current state from robot
hw_interface.read(timestamp, period);
// Get current time and elapsed time since last read
timestamp = ros::Time::now();
stopwatch_now = std::chrono::steady_clock::now();
period.fromSec(std::chrono::duration_cast<std::chrono::duration<double>>(stopwatch_now - stopwatch_last).count());
stopwatch_last = stopwatch_now;
cm.update(timestamp, period);
hw_interface.write(timestamp, period);
if (!control_rate.sleep())
{
ROS_WARN_STREAM("Could not keep cycle rate of " << control_rate.expectedCycleTime().toNSec() / 1000000.0 << "ms");
}
}
spinner.stop();
ROS_INFO_STREAM_NAMED("hardware_interface", "Shutting down.");
return 0;
}

97
ur_rtde_driver/src/ros/service_stopper.cpp Normal file
View File

@@ -0,0 +1,97 @@
/*
* Copyright 2017, 2018 Simon Rasmussen (refactor)
*
* Copyright 2015, 2016 Thomas Timm Andersen (original version)
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#include "ur_rtde_driver/ros/service_stopper.h"
namespace ur_driver
{
ServiceStopper::ServiceStopper(std::vector<Service*> services)
: enable_service_(nh_.advertiseService("ur_driver/robot_enable", &ServiceStopper::enableCallback, this))
, services_(services)
, last_state_(RobotState::Error)
, activation_mode_(ActivationMode::Never)
{
std::string mode;
ros::param::param("~require_activation", mode, std::string("Never"));
if (mode == "Always")
{
activation_mode_ = ActivationMode::Always;
}
else if (mode == "OnStartup")
{
activation_mode_ = ActivationMode::OnStartup;
}
else
{
if (mode != "Never")
{
LOG_WARN("Found invalid value for param require_activation: '%s'\nShould be one of Never, OnStartup, Always",
mode.c_str());
mode = "Never";
}
notify_all(RobotState::Running);
}
LOG_INFO("Service 'ur_driver/robot_enable' activation mode: %s", mode.c_str());
}
bool ServiceStopper::enableCallback(std_srvs::EmptyRequest& req, std_srvs::EmptyResponse& resp)
{
// After the startup call OnStartup and Never behave the same
if (activation_mode_ == ActivationMode::OnStartup)
activation_mode_ = ActivationMode::Never;
notify_all(RobotState::Running);
return true;
}
void ServiceStopper::notify_all(RobotState state)
{
if (last_state_ == state)
return;
for (auto const service : services_)
{
service->onRobotStateChange(state);
}
last_state_ = state;
}
bool ServiceStopper::handle(SharedRobotModeData& data, bool error)
{
if (data.emergency_stopped)
{
notify_all(RobotState::EmergencyStopped);
}
else if (data.protective_stopped)
{
notify_all(RobotState::ProtectiveStopped);
}
else if (error)
{
notify_all(RobotState::Error);
}
else if (activation_mode_ == ActivationMode::Never)
{
// No error encountered, the user requested automatic reactivation
notify_all(RobotState::Running);
}
return true;
}
} // namespace ur_driver

48
ur_rtde_driver/src/rtde/control_package_pause.cpp Normal file
View File

@@ -0,0 +1,48 @@
// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
// -- BEGIN LICENSE BLOCK ----------------------------------------------
// Copyright 2019 FZI Forschungszentrum Informatik
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// -- END LICENSE BLOCK ------------------------------------------------
//----------------------------------------------------------------------
/*!\file
*
* \author Tristan Schnell schnell@fzi.de
* \date 2019-04-09
*
*/
//----------------------------------------------------------------------
#include "ur_rtde_driver/rtde/control_package_pause.h"
namespace ur_driver
{
namespace rtde_interface
{
bool ControlPackagePause::parseWith(comm::BinParser& bp)
{
bp.parse(accepted_);
return true;
}
std::string ControlPackagePause::toString() const
{
std::stringstream ss;
ss << "accepted: " << static_cast<int>(accepted_);
return ss.str();
}
} // namespace rtde_interface
} // namespace ur_driver

50
ur_rtde_driver/src/rtde/control_package_setup_inputs.cpp Normal file
View File

@@ -0,0 +1,50 @@
// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
// -- BEGIN LICENSE BLOCK ----------------------------------------------
// Copyright 2019 FZI Forschungszentrum Informatik
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// -- END LICENSE BLOCK ------------------------------------------------
//----------------------------------------------------------------------
/*!\file
*
* \author Tristan Schnell schnell@fzi.de
* \date 2019-04-09
*
*/
//----------------------------------------------------------------------
#include "ur_rtde_driver/rtde/control_package_setup_inputs.h"
namespace ur_driver
{
namespace rtde_interface
{
bool ControlPackageSetupInputs::parseWith(comm::BinParser& bp)
{
bp.parse(input_recipe_id_);
bp.parseRemainder(variable_types_);
return true;
}
std::string ControlPackageSetupInputs::toString() const
{
std::stringstream ss;
ss << "input recipe id: " << static_cast<int>(input_recipe_id_) << std::endl;
ss << "variable types: " << variable_types_;
return ss.str();
}
} // namespace rtde_interface
} // namespace ur_driver

92
ur_rtde_driver/src/rtde/control_package_setup_outputs.cpp Normal file
View File

@@ -0,0 +1,92 @@
// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
// -- BEGIN LICENSE BLOCK ----------------------------------------------
// Copyright 2019 FZI Forschungszentrum Informatik
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// -- END LICENSE BLOCK ------------------------------------------------
//----------------------------------------------------------------------
/*!\file
*
* \author Tristan Schnell schnell@fzi.de
* \date 2019-04-09
*
*/
//----------------------------------------------------------------------
#include "ur_rtde_driver/rtde/control_package_setup_outputs.h"
namespace ur_driver
{
namespace rtde_interface
{
bool ControlPackageSetupOutputs::parseWith(comm::BinParser& bp)
{
bp.parse(output_recipe_id_);
bp.parseRemainder(variable_types_);
return true;
}
std::string ControlPackageSetupOutputs::toString() const
{
std::stringstream ss;
ss << "output recipe id: " << static_cast<int>(output_recipe_id_) << std::endl;
ss << "variable types: " << variable_types_;
return ss.str();
}
size_t ControlPackageSetupOutputsRequest::generateSerializedRequest(uint8_t* buffer, double output_frequency,
std::vector<std::string> variable_names)
{
if (variable_names.size() == 0)
{
return 0;
}
std::string variables;
for (const auto& piece : variable_names)
variables += (piece + ",");
variables.pop_back();
uint16_t payload_size = sizeof(double) + variables.size();
size_t size = 0;
size += PackageHeader::serializeHeader(buffer, PACKAGE_TYPE, payload_size);
size += comm::PackageSerializer::serialize(buffer + size, output_frequency);
size += comm::PackageSerializer::serialize(buffer + size, variables);
return size;
}
size_t ControlPackageSetupOutputsRequest::generateSerializedRequest(uint8_t* buffer,
std::vector<std::string> variable_names)
{
if (variable_names.size() == 0)
{
return 0;
}
std::string variables;
for (const auto& piece : variable_names)
variables += (piece + ",");
variables.pop_back();
uint16_t payload_size = sizeof(double) + variables.size();
size_t size = 0;
size += PackageHeader::serializeHeader(buffer, PACKAGE_TYPE, payload_size);
size += comm::PackageSerializer::serialize(buffer + size, variables);
return size;
}
} // namespace rtde_interface
} // namespace ur_driver

53
ur_rtde_driver/src/rtde/control_package_start.cpp Normal file
View File

@@ -0,0 +1,53 @@
// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
// -- BEGIN LICENSE BLOCK ----------------------------------------------
// Copyright 2019 FZI Forschungszentrum Informatik
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// -- END LICENSE BLOCK ------------------------------------------------
//----------------------------------------------------------------------
/*!\file
*
* \author Tristan Schnell schnell@fzi.de
* \date 2019-04-09
*
*/
//----------------------------------------------------------------------
#include "ur_rtde_driver/rtde/control_package_start.h"
namespace ur_driver
{
namespace rtde_interface
{
bool ControlPackageStart::parseWith(comm::BinParser& bp)
{
bp.parse(accepted_);
return true;
}
std::string ControlPackageStart::toString() const
{
std::stringstream ss;
ss << "accepted: " << static_cast<int>(accepted_);
return ss.str();
}
size_t ControlPackageStartRequest::generateSerializedRequest(uint8_t* buffer)
{
return PackageHeader::serializeHeader(buffer, PACKAGE_TYPE, PAYLOAD_SIZE);
}
} // namespace rtde_interface
} // namespace ur_driver

222
ur_rtde_driver/src/rtde/data_package.cpp Normal file
View File

@@ -0,0 +1,222 @@
// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
// -- BEGIN LICENSE BLOCK ----------------------------------------------
// Copyright 2019 FZI Forschungszentrum Informatik
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// -- END LICENSE BLOCK ------------------------------------------------
//----------------------------------------------------------------------
/*!\file
*
* \author Felix Mauch mauch@fzi.de
* \date 2019-04-10
*
*/
//----------------------------------------------------------------------
#include "ur_rtde_driver/rtde/data_package.h"
namespace ur_driver
{
namespace rtde_interface
{
std::unordered_map<std::string, DataPackage::_rtde_type_variant> DataPackage::type_list_{
{ "timestamp", double() },
{ "target_q", vector6d_t() },
{ "target_qd", vector6d_t() },
{ "target_qdd", vector6d_t() },
{ "target_current", vector6d_t() },
{ "target_moment", vector6d_t() },
{ "actual_q", vector6d_t() },
{ "actual_qd", vector6d_t() },
{ "actual_qdd", vector6d_t() },
{ "actual_current", vector6d_t() },
{ "actual_moment", vector6d_t() },
{ "joint_control_output", vector6d_t() },
{ "actual_TCP_pose", vector6d_t() },
{ "actual_TCP_speed", vector6d_t() },
{ "actual_TCP_force", vector6d_t() },
{ "target_TCP_pose", vector6d_t() },
{ "target_TCP_speed", vector6d_t() },
{ "actual_digital_input_bits", uint64_t() },
{ "joint_temperatures", vector6d_t() },
{ "actual_execution_time", double() },
{ "robot_mode", int32_t() },
{ "joint_mode", vector6int32_t() },
{ "safety_mode", int32_t() },
{ "actual_tool_accelerometer", vector3d_t() },
{ "speed_scaling", double() },
{ "target_speed_fraction", double() },
{ "actual_momentum", double() },
{ "actial_main_voltage", double() },
{ "actual_robot_voltage", double() },
{ "actual_robot_current", double() },
{ "actual_joint_voltage", vector6d_t() },
{ "actual_digital_output_bits", uint64_t() },
{ "runtime_state", uint32_t() },
{ "elbow_position", vector3d_t() },
{ "elbow_velocity", vector3d_t() },
{ "robot_status_bits", uint32_t() },
{ "safety_status_bits", uint32_t() },
{ "analog_io_types", uint32_t() },
{ "standard_analog_input0", double() },
{ "standard_analog_input1", double() },
{ "standard_analog_output0", double() },
{ "standard_analog_output1", double() },
{ "io_current", double() },
{ "euromap67_input_bits", uint32_t() },
{ "euromap67_output_bits", uint32_t() },
{ "euromap67_24V_voltage", double() },
{ "euromap67_24V_current", double() },
{ "tool_mode", uint32_t() },
{ "tool_analog_input_types", uint32_t() },
{ "tool_analog_input0", double() },
{ "tool_analog_input1", double() },
{ "tool_output_voltage", int32_t() },
{ "tool_output_current", double() },
{ "tool_temperature", double() },
{ "tool_force_scalar", double() },
{ "output_bit_registers0_to_31", uint32_t() },
{ "output_bit_registers32_to_63", uint32_t() },
{ "output_int_register_0", int32_t() },
{ "output_int_register_1", int32_t() },
{ "output_int_register_2", int32_t() },
{ "output_int_register_3", int32_t() },
{ "output_int_register_4", int32_t() },
{ "output_int_register_5", int32_t() },
{ "output_int_register_6", int32_t() },
{ "output_int_register_7", int32_t() },
{ "output_int_register_8", int32_t() },
{ "output_int_register_9", int32_t() },
{ "output_int_register_10", int32_t() },
{ "output_int_register_11", int32_t() },
{ "output_int_register_12", int32_t() },
{ "output_int_register_13", int32_t() },
{ "output_int_register_14", int32_t() },
{ "output_int_register_15", int32_t() },
{ "output_int_register_16", int32_t() },
{ "output_int_register_17", int32_t() },
{ "output_int_register_18", int32_t() },
{ "output_int_register_19", int32_t() },
{ "output_int_register_20", int32_t() },
{ "output_int_register_21", int32_t() },
{ "output_int_register_22", int32_t() },
{ "output_int_register_23", int32_t() },
{ "output_double_register_0", double() },
{ "output_double_register_1", double() },
{ "output_double_register_2", double() },
{ "output_double_register_3", double() },
{ "output_double_register_4", double() },
{ "output_double_register_5", double() },
{ "output_double_register_6", double() },
{ "output_double_register_7", double() },
{ "output_double_register_8", double() },
{ "output_double_register_9", double() },
{ "output_double_register_10", double() },
{ "output_double_register_11", double() },
{ "output_double_register_12", double() },
{ "output_double_register_13", double() },
{ "output_double_register_14", double() },
{ "output_double_register_15", double() },
{ "output_double_register_16", double() },
{ "output_double_register_17", double() },
{ "output_double_register_18", double() },
{ "output_double_register_19", double() },
{ "output_double_register_20", double() },
{ "output_double_register_21", double() },
{ "output_double_register_22", double() },
{ "output_double_register_23", double() },
{ "input_bit_registers0_to_31", uint32_t() },
{ "input_bit_registers32_to_63", uint32_t() },
{ "input_int_register_0", int32_t() },
{ "input_int_register_1", int32_t() },
{ "input_int_register_2", int32_t() },
{ "input_int_register_3", int32_t() },
{ "input_int_register_4", int32_t() },
{ "input_int_register_5", int32_t() },
{ "input_int_register_6", int32_t() },
{ "input_int_register_7", int32_t() },
{ "input_int_register_8", int32_t() },
{ "input_int_register_9", int32_t() },
{ "input_int_register_10", int32_t() },
{ "input_int_register_11", int32_t() },
{ "input_int_register_12", int32_t() },
{ "input_int_register_13", int32_t() },
{ "input_int_register_14", int32_t() },
{ "input_int_register_15", int32_t() },
{ "input_int_register_16", int32_t() },
{ "input_int_register_17", int32_t() },
{ "input_int_register_18", int32_t() },
{ "input_int_register_19", int32_t() },
{ "input_int_register_20", int32_t() },
{ "input_int_register_21", int32_t() },
{ "input_int_register_22", int32_t() },
{ "input_int_register_23", int32_t() },
{ "input_double_register_0", double() },
{ "input_double_register_1", double() },
{ "input_double_register_2", double() },
{ "input_double_register_3", double() },
{ "input_double_register_4", double() },
{ "input_double_register_5", double() },
{ "input_double_register_6", double() },
{ "input_double_register_7", double() },
{ "input_double_register_8", double() },
{ "input_double_register_9", double() },
{ "input_double_register_10", double() },
{ "input_double_register_11", double() },
{ "input_double_register_12", double() },
{ "input_double_register_13", double() },
{ "input_double_register_14", double() },
{ "input_double_register_15", double() },
{ "input_double_register_16", double() },
{ "input_double_register_17", double() },
{ "input_double_register_18", double() },
{ "input_double_register_19", double() },
{ "input_double_register_20", double() },
{ "input_double_register_21", double() },
{ "input_double_register_22", double() },
{ "input_double_register_23", double() }
};
bool rtde_interface::DataPackage ::parseWith(comm::BinParser& bp)
{
bp.parse(recipe_id_);
for (auto& item : recipe_)
{
if (type_list_.find(item) != type_list_.end())
{
_rtde_type_variant entry = type_list_[item];
auto bound_visitor = std::bind(ParseVisitor(), std::placeholders::_1, bp);
boost::apply_visitor(bound_visitor, entry);
data_[item] = entry;
}
else
{
return false;
}
}
return true;
}
std::string rtde_interface::DataPackage::toString() const
{
std::stringstream ss;
for (auto& item : data_)
{
ss << item.first << ": ";
ss << boost::apply_visitor(StringVisitor{}, item.second) << std::endl;
}
return ss.str();
}
} // namespace rtde_interface
} // namespace ur_driver

57
ur_rtde_driver/src/rtde/get_urcontrol_version.cpp Normal file
View File

@@ -0,0 +1,57 @@
// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
// -- BEGIN LICENSE BLOCK ----------------------------------------------
// Copyright 2019 FZI Forschungszentrum Informatik
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// -- END LICENSE BLOCK ------------------------------------------------
//----------------------------------------------------------------------
/*!\file
*
* \author Tristan Schnell schnell@fzi.de
* \date 2019-04-09
*
*/
//----------------------------------------------------------------------
#include "ur_rtde_driver/rtde/get_urcontrol_version.h"
namespace ur_driver
{
namespace rtde_interface
{
bool GetUrcontrolVersion::parseWith(comm::BinParser& bp)
{
bp.parse(major_);
bp.parse(minor_);
bp.parse(bugfix_);
bp.parse(build_);
return true;
}
std::string GetUrcontrolVersion::toString() const
{
std::stringstream ss;
ss << "version: " << major_ << ".";
ss << minor_ << "." << bugfix_ << "." << build_;
return ss.str();
}
size_t GetUrcontrolVersionRequest::generateSerializedRequest(uint8_t* buffer)
{
return PackageHeader::serializeHeader(buffer, PACKAGE_TYPE, PAYLOAD_SIZE);
}
} // namespace rtde_interface
} // namespace ur_driver

57
ur_rtde_driver/src/rtde/request_protocol_version.cpp Normal file
View File

@@ -0,0 +1,57 @@
// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
// -- BEGIN LICENSE BLOCK ----------------------------------------------
// Copyright 2019 FZI Forschungszentrum Informatik
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// -- END LICENSE BLOCK ------------------------------------------------
//----------------------------------------------------------------------
/*!\file
*
* \author Tristan Schnell schnell@fzi.de
* \date 2019-04-09
*
*/
//----------------------------------------------------------------------
#include "ur_rtde_driver/rtde/request_protocol_version.h"
namespace ur_driver
{
namespace rtde_interface
{
bool RequestProtocolVersion::parseWith(comm::BinParser& bp)
{
bp.parse(accepted_);
return true;
}
std::string RequestProtocolVersion::toString() const
{
std::stringstream ss;
ss << "accepted: " << static_cast<int>(accepted_);
return ss.str();
}
size_t RequestProtocolVersionRequest::generateSerializedRequest(uint8_t* buffer, uint16_t version)
{
size_t size = 0;
size += PackageHeader::serializeHeader(buffer, PACKAGE_TYPE, PAYLOAD_SIZE);
size += comm::PackageSerializer::serialize(buffer + size, version);
return size;
}
} // namespace rtde_interface
} // namespace ur_driver

124
ur_rtde_driver/src/rtde/rtde_client.cpp Normal file
View File

@@ -0,0 +1,124 @@
// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
// -- BEGIN LICENSE BLOCK ----------------------------------------------
// Copyright 2019 FZI Forschungszentrum Informatik
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// -- END LICENSE BLOCK ------------------------------------------------
//----------------------------------------------------------------------
/*!\file
*
* \author Tristan Schnell schnell@fzi.de
* \date 2019-04-10
*
*/
//----------------------------------------------------------------------
#include "ur_rtde_driver/rtde/rtde_client.h"
namespace ur_driver
{
namespace rtde_interface
{
RTDEClient::RTDEClient(std::string ROBOT_IP, comm::INotifier& notifier)
: stream_(ROBOT_IP, UR_RTDE_PORT)
, parser_(readRecipe())
, prod_(stream_, parser_)
, pipeline_(prod_, PIPELINE_NAME, notifier)
{
}
bool RTDEClient::init()
{
pipeline_.run();
uint8_t buffer[4096];
size_t size;
size_t written;
// negotiate version
uint16_t protocol_version = 2;
size = RequestProtocolVersionRequest::generateSerializedRequest(buffer, protocol_version);
stream_.write(buffer, size, written);
std::unique_ptr<comm::URPackage<PackageHeader>> package;
pipeline_.getLatestProduct(package, std::chrono::milliseconds(1000));
rtde_interface::RequestProtocolVersion* tmp_version =
dynamic_cast<rtde_interface::RequestProtocolVersion*>(package.get());
if (!tmp_version->accepted_)
{
protocol_version = 1;
size = RequestProtocolVersionRequest::generateSerializedRequest(buffer, protocol_version);
stream_.write(buffer, size, written);
pipeline_.getLatestProduct(package, std::chrono::milliseconds(1000));
tmp_version = dynamic_cast<rtde_interface::RequestProtocolVersion*>(package.get());
if (!tmp_version->accepted_)
{
LOG_ERROR("Could not negotiate protocol version");
return false;
}
}
// determine maximum frequency from ur-control version
double max_frequency = URE_MAX_FREQUENCY;
size = GetUrcontrolVersionRequest::generateSerializedRequest(buffer);
stream_.write(buffer, size, written);
pipeline_.getLatestProduct(package, std::chrono::milliseconds(1000));
rtde_interface::GetUrcontrolVersion* tmp_control_version =
dynamic_cast<rtde_interface::GetUrcontrolVersion*>(package.get());
if (tmp_control_version->major_ < 5)
{
max_frequency = CB3_MAX_FREQUENCY;
}
// sending output recipe
LOG_INFO("Setting up RTDE communication with frequency %f", max_frequency);
if (protocol_version == 2)
{
size = ControlPackageSetupOutputsRequest::generateSerializedRequest(buffer, max_frequency, readRecipe());
}
else
{
size = ControlPackageSetupOutputsRequest::generateSerializedRequest(buffer, 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);
pipeline_.getLatestProduct(package, std::chrono::milliseconds(1000));
rtde_interface::ControlPackageStart* tmp = dynamic_cast<rtde_interface::ControlPackageStart*>(package.get());
return tmp->accepted_;
}
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(std::unique_ptr<comm::URPackage<PackageHeader>>& data_package,
std::chrono::milliseconds timeout)
{
return pipeline_.getLatestProduct(data_package, timeout);
}
} // namespace rtde_interface
} // namespace ur_driver

53
ur_rtde_driver/src/rtde/rtde_package.cpp Normal file
View File

@@ -0,0 +1,53 @@
// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
// -- BEGIN LICENSE BLOCK ----------------------------------------------
// Copyright 2019 FZI Forschungszentrum Informatik
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// -- END LICENSE BLOCK ------------------------------------------------
//----------------------------------------------------------------------
/*!\file
*
* \author Felix Mauch mauch@fzi.de
* \date 2019-04-10
*
*/
//----------------------------------------------------------------------
#include "ur_rtde_driver/rtde/rtde_package.h"
namespace ur_driver
{
namespace rtde_interface
{
bool RTDEPackage::parseWith(comm::BinParser& bp)
{
bp.rawData(buffer_, buffer_length_);
return true;
}
std::string rtde_interface::RTDEPackage ::toString() const
{
std::stringstream ss;
ss << "Type: " << static_cast<int>(type_) << std::endl;
ss << "Raw byte stream: ";
for (size_t i = 0; i < buffer_length_; ++i)
{
uint8_t* buf = buffer_.get();
ss << std::hex << static_cast<int>(buf[i]) << " ";
}
ss << std::endl;
return ss.str();
}
} // namespace rtde_interface
} // namespace ur_driver

54
ur_rtde_driver/src/rtde/text_message.cpp Normal file
View File

@@ -0,0 +1,54 @@
// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
// -- BEGIN LICENSE BLOCK ----------------------------------------------
// Copyright 2019 FZI Forschungszentrum Informatik
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// -- END LICENSE BLOCK ------------------------------------------------
//----------------------------------------------------------------------
/*!\file
*
* \author Tristan Schnell schnell@fzi.de
* \date 2019-04-09
*
*/
//----------------------------------------------------------------------
#include "ur_rtde_driver/rtde/text_message.h"
namespace ur_driver
{
namespace rtde_interface
{
bool TextMessage::parseWith(comm::BinParser& bp)
{
bp.parse(message_length_);
bp.parse(message_, message_length_);
bp.parse(source_length_);
bp.parse(source_, source_length_);
bp.parse(warning_level_);
return true;
}
std::string TextMessage::toString() const
{
std::stringstream ss;
ss << "message: " << message_ << std::endl;
ss << "source: " << source_ << std::endl;
ss << "warning level: " << static_cast<int>(warning_level_);
return ss.str();
}
} // namespace rtde_interface
} // namespace ur_driver

173
ur_rtde_driver/src/ur/commander.cpp Normal file
View File

@@ -0,0 +1,173 @@
/*
* Copyright 2017, 2018 Simon Rasmussen (refactor)
*
* Copyright 2015, 2016 Thomas Timm Andersen (original version)
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#include "ur_rtde_driver/ur/commander.h"
#include "ur_rtde_driver/log.h"
namespace ur_driver
{
bool URCommander::write(const std::string& s)
{
size_t len = s.size();
const uint8_t* data = reinterpret_cast<const uint8_t*>(s.c_str());
size_t written;
return stream_.write(data, len, written);
}
void URCommander::formatArray(std::ostringstream& out, std::array<double, 6>& values)
{
std::string mod("[");
for (auto const& val : values)
{
out << mod << val;
mod = ",";
}
out << "]";
}
bool URCommander::uploadProg(const std::string& s)
{
LOG_DEBUG("Sending program [%s]", s.c_str());
return write(s);
}
bool URCommander::setToolVoltage(uint8_t voltage)
{
if (voltage != 0 || voltage != 12 || voltage != 24)
return false;
std::ostringstream out;
out << "set_tool_voltage(" << (int)voltage << ")\n";
std::string s(out.str());
return write(s);
}
bool URCommander::setFlag(uint8_t pin, bool value)
{
std::ostringstream out;
out << "set_flag(" << (int)pin << "," << (value ? "True" : "False") << ")\n";
std::string s(out.str());
return write(s);
}
bool URCommander::setPayload(double value)
{
std::ostringstream out;
out << "set_payload(" << std::fixed << std::setprecision(5) << value << ")\n";
std::string s(out.str());
return write(s);
}
bool URCommander::stopj(double a)
{
std::ostringstream out;
out << "stopj(" << std::fixed << std::setprecision(5) << a << ")\n";
std::string s(out.str());
return write(s);
}
bool URCommander_V1_X::speedj(std::array<double, 6>& speeds, double acceleration)
{
std::ostringstream out;
out << std::fixed << std::setprecision(5);
out << "speedj(";
formatArray(out, speeds);
out << "," << acceleration << "," << 0.02 << ")\n";
std::string s(out.str());
return write(s);
}
bool URCommander_V1_X::setAnalogOut(uint8_t pin, double value)
{
std::ostringstream out;
out << "sec io_fun():\n"
<< "set_analog_out(" << (int)pin << "," << std::fixed << std::setprecision(4) << value << ")\n"
<< "end\n";
std::string s(out.str());
return write(s);
}
bool URCommander_V1_X::setDigitalOut(uint8_t pin, bool value)
{
std::ostringstream out;
out << "sec io_fun():\n"
<< "set_digital_out(" << (int)pin << "," << (value ? "True" : "False") << ")\n"
<< "end\n";
std::string s(out.str());
return write(s);
}
bool URCommander_V3_X::setAnalogOut(uint8_t pin, double value)
{
std::ostringstream out;
out << "sec io_fun():\n"
<< "set_standard_analog_out(" << (int)pin << "," << std::fixed << std::setprecision(5) << value << ")\n"
<< "end\n";
std::string s(out.str());
return write(s);
}
bool URCommander_V3_X::setDigitalOut(uint8_t pin, bool value)
{
std::ostringstream out;
std::string func;
if (pin < 8)
{
func = "set_standard_digital_out";
}
else if (pin < 16)
{
func = "set_configurable_digital_out";
pin -= 8;
}
else if (pin < 18)
{
func = "set_tool_digital_out";
pin -= 16;
}
else
return false;
out << "sec io_fun():\n"
<< func << "(" << (int)pin << "," << (value ? "True" : "False") << ")\n"
<< "end\n";
std::string s(out.str());
return write(s);
}
bool URCommander_V3_1__2::speedj(std::array<double, 6>& speeds, double acceleration)
{
std::ostringstream out;
out << std::fixed << std::setprecision(5);
out << "speedj(";
formatArray(out, speeds);
out << "," << acceleration << ")\n";
std::string s(out.str());
return write(s);
}
bool URCommander_V3_3::speedj(std::array<double, 6>& speeds, double acceleration)
{
std::ostringstream out;
out << std::fixed << std::setprecision(5);
out << "speedj(";
formatArray(out, speeds);
out << "," << acceleration << "," << 0.008 << ")\n";
std::string s(out.str());
return write(s);
}
} // namespace ur_driver

124
ur_rtde_driver/src/ur/master_board.cpp Normal file
View File

@@ -0,0 +1,124 @@
/*
* Copyright 2017, 2018 Simon Rasmussen (refactor)
*
* Copyright 2015, 2016 Thomas Timm Andersen (original version)
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#include "ur_rtde_driver/ur/master_board.h"
#include "ur_rtde_driver/ur/consumer.h"
namespace ur_driver
{
bool SharedMasterBoardData::parseWith(BinParser& bp)
{
bp.parse(analog_input_range0);
bp.parse(analog_input_range1);
bp.parse(analog_input0);
bp.parse(analog_input1);
bp.parse(analog_output_domain0);
bp.parse(analog_output_domain1);
bp.parse(analog_output0);
bp.parse(analog_output1);
bp.parse(master_board_temperature);
bp.parse(robot_voltage_48V);
bp.parse(robot_current);
bp.parse(master_IO_current);
return true;
}
bool MasterBoardData_V1_X::parseWith(BinParser& bp)
{
if (!bp.checkSize<MasterBoardData_V1_X>())
return false;
bp.parse<uint16_t, 10>(digital_input_bits);
bp.parse<uint16_t, 10>(digital_output_bits);
SharedMasterBoardData::parseWith(bp);
bp.parse(master_safety_state);
bp.parse(master_on_off_state);
bp.parse(euromap67_interface_installed);
if (euromap67_interface_installed)
{
if (!bp.checkSize(MasterBoardData_V1_X::EURO_SIZE))
return false;
bp.parse(euromap_input_bits);
bp.parse(euromap_output_bits);
bp.parse(euromap_voltage);
bp.parse(euromap_current);
}
return true;
}
bool MasterBoardData_V3_0__1::parseWith(BinParser& bp)
{
if (!bp.checkSize<MasterBoardData_V3_0__1>())
return false;
bp.parse<uint32_t, 18>(digital_input_bits);
bp.parse<uint32_t, 18>(digital_output_bits);
SharedMasterBoardData::parseWith(bp);
bp.parse(safety_mode);
bp.parse(in_reduced_mode);
bp.parse(euromap67_interface_installed);
if (euromap67_interface_installed)
{
if (!bp.checkSize(MasterBoardData_V3_0__1::EURO_SIZE))
return false;
bp.parse(euromap_input_bits);
bp.parse(euromap_output_bits);
bp.parse(euromap_voltage);
bp.parse(euromap_current);
}
bp.consume(sizeof(uint32_t));
return true;
}
bool MasterBoardData_V3_2::parseWith(BinParser& bp)
{
if (!bp.checkSize<MasterBoardData_V3_2>())
return false;
MasterBoardData_V3_0__1::parseWith(bp);
bp.parse(operational_mode_selector_input);
bp.parse(three_position_enabling_device_input);
return true;
}
bool MasterBoardData_V1_X::consumeWith(URStatePacketConsumer& consumer)
{
return consumer.consume(*this);
}
bool MasterBoardData_V3_0__1::consumeWith(URStatePacketConsumer& consumer)
{
return consumer.consume(*this);
}
bool MasterBoardData_V3_2::consumeWith(URStatePacketConsumer& consumer)
{
return consumer.consume(*this);
}
} // namespace ur_driver

40
ur_rtde_driver/src/ur/messages.cpp Normal file
View File

@@ -0,0 +1,40 @@
/*
* Copyright 2017, 2018 Simon Rasmussen (refactor)
*
* Copyright 2015, 2016 Thomas Timm Andersen (original version)
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#include "ur_rtde_driver/ur/messages.h"
#include "ur_rtde_driver/ur/consumer.h"
namespace ur_driver
{
bool VersionMessage::parseWith(BinParser& bp)
{
bp.parse(project_name);
bp.parse(major_version);
bp.parse(minor_version);
bp.parse(svn_version);
bp.consume(sizeof(uint32_t)); // undocumented field??
bp.parseRemainder(build_date);
return true; // not really possible to check dynamic size packets
}
bool VersionMessage::consumeWith(URMessagePacketConsumer& consumer)
{
return consumer.consume(*this);
}
} // namespace ur_driver

105
ur_rtde_driver/src/ur/robot_mode.cpp Normal file
View File

@@ -0,0 +1,105 @@
/*
* Copyright 2017, 2018 Simon Rasmussen (refactor)
*
* Copyright 2015, 2016 Thomas Timm Andersen (original version)
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#include "ur_rtde_driver/ur/robot_mode.h"
#include "ur_rtde_driver/ur/consumer.h"
namespace ur_driver
{
bool SharedRobotModeData::parseWith(BinParser& bp)
{
bp.parse(timestamp);
bp.parse(physical_robot_connected);
bp.parse(real_robot_enabled);
bp.parse(robot_power_on);
bp.parse(emergency_stopped);
bp.parse(protective_stopped);
bp.parse(program_running);
bp.parse(program_paused);
return true;
}
bool RobotModeData_V1_X::parseWith(BinParser& bp)
{
if (!bp.checkSize<RobotModeData_V1_X>())
return false;
SharedRobotModeData::parseWith(bp);
bp.parse(robot_mode);
bp.parse(speed_fraction);
return true;
}
bool RobotModeData_V3_0__1::parseWith(BinParser& bp)
{
if (!bp.checkSize<RobotModeData_V3_0__1>())
return false;
SharedRobotModeData::parseWith(bp);
bp.parse(robot_mode);
bp.parse(control_mode);
bp.parse(target_speed_fraction);
bp.parse(speed_scaling);
return true;
}
bool RobotModeData_V3_2::parseWith(BinParser& bp)
{
if (!bp.checkSize<RobotModeData_V3_2>())
return false;
RobotModeData_V3_0__1::parseWith(bp);
bp.parse(target_speed_fraction_limit);
return true;
}
bool RobotModeData_V3_5::parseWith(BinParser& bp)
{
if (!bp.checkSize<RobotModeData_V3_5>())
return false;
RobotModeData_V3_2::parseWith(bp);
bp.parse(unknown_internal_use);
return true;
}
bool RobotModeData_V1_X::consumeWith(URStatePacketConsumer& consumer)
{
return consumer.consume(*this);
}
bool RobotModeData_V3_0__1::consumeWith(URStatePacketConsumer& consumer)
{
return consumer.consume(*this);
}
bool RobotModeData_V3_2::consumeWith(URStatePacketConsumer& consumer)
{
return consumer.consume(*this);
}
bool RobotModeData_V3_5::consumeWith(URStatePacketConsumer& consumer)
{
return consumer.consume(*this);
}
} // namespace ur_driver

138
ur_rtde_driver/src/ur/rt_state.cpp Normal file
View File

@@ -0,0 +1,138 @@
/*
* Copyright 2017, 2018 Simon Rasmussen (refactor)
*
* Copyright 2015, 2016 Thomas Timm Andersen (original version)
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#include "ur_rtde_driver/ur/rt_state.h"
#include "ur_rtde_driver/ur/consumer.h"
namespace ur_driver
{
void RTShared::parse_shared1(BinParser& bp)
{
bp.parse(time);
bp.parse(q_target);
bp.parse(qd_target);
bp.parse(qdd_target);
bp.parse(i_target);
bp.parse(m_target);
bp.parse(q_actual);
bp.parse(qd_actual);
bp.parse(i_actual);
}
void RTShared::parse_shared2(BinParser& bp)
{
bp.parse(digital_inputs);
bp.parse(motor_temperatures);
bp.parse(controller_time);
bp.consume(sizeof(double)); // Unused "Test value" field
bp.parse(robot_mode);
}
bool RTState_V1_6__7::parseWith(BinParser& bp)
{
if (!bp.checkSize<RTState_V1_6__7>())
return false;
parse_shared1(bp);
bp.parse(tool_accelerometer_values);
bp.consume(sizeof(double) * 15);
bp.parse(tcp_force);
bp.parse(tool_vector_actual);
bp.parse(tcp_speed_actual);
parse_shared2(bp);
return true;
}
bool RTState_V1_8::parseWith(BinParser& bp)
{
if (!bp.checkSize<RTState_V1_8>())
return false;
RTState_V1_6__7::parseWith(bp);
bp.parse(joint_modes);
return true;
}
bool RTState_V3_0__1::parseWith(BinParser& bp)
{
if (!bp.checkSize<RTState_V3_0__1>())
return false;
parse_shared1(bp);
bp.parse(i_control);
bp.parse(tool_vector_actual);
bp.parse(tcp_speed_actual);
bp.parse(tcp_force);
bp.parse(tool_vector_target);
bp.parse(tcp_speed_target);
parse_shared2(bp);
bp.parse(joint_modes);
bp.parse(safety_mode);
bp.consume(sizeof(double[6])); // skip undocumented
bp.parse(tool_accelerometer_values);
bp.consume(sizeof(double[6])); // skip undocumented
bp.parse(speed_scaling);
bp.parse(linear_momentum_norm);
bp.consume(sizeof(double)); // skip undocumented
bp.consume(sizeof(double)); // skip undocumented
bp.parse(v_main);
bp.parse(v_robot);
bp.parse(i_robot);
bp.parse(v_actual);
return true;
}
bool RTState_V3_2__3::parseWith(BinParser& bp)
{
if (!bp.checkSize<RTState_V3_2__3>())
return false;
RTState_V3_0__1::parseWith(bp);
bp.parse(digital_outputs);
bp.parse(program_state);
return true;
}
bool RTState_V1_6__7::consumeWith(URRTPacketConsumer& consumer)
{
return consumer.consume(*this);
}
bool RTState_V1_8::consumeWith(URRTPacketConsumer& consumer)
{
return consumer.consume(*this);
}
bool RTState_V3_0__1::consumeWith(URRTPacketConsumer& consumer)
{
return consumer.consume(*this);
}
bool RTState_V3_2__3::consumeWith(URRTPacketConsumer& consumer)
{
return consumer.consume(*this);
}
} // namespace ur_driver

156
ur_rtde_driver/src/ur/ur_driver.cpp Normal file
View File

@@ -0,0 +1,156 @@
// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
// -- BEGIN LICENSE BLOCK ----------------------------------------------
// Copyright 2019 FZI Forschungszentrum Informatik
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// -- END LICENSE BLOCK ------------------------------------------------
//----------------------------------------------------------------------
/*!\file
*
* \author Felix Mauch mauch@fzi.de
* \date 2019-04-11
*
*/
//----------------------------------------------------------------------
#include "ur_rtde_driver/ur/ur_driver.h"
#include <memory>
namespace ur_driver
{
static const std::string POSITION_PROGRAM = R"(
def myProg():
textmsg("hello")
MULT_jointstate = 1000000
SERVO_IDLE = 0
SERVO_RUNNING = 1
cmd_servo_state = SERVO_IDLE
cmd_servo_q = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
def set_servo_setpoint(q):
enter_critical
cmd_servo_state = SERVO_RUNNING
cmd_servo_q = q
exit_critical
end
thread servoThread():
state = SERVO_IDLE
while True:
enter_critical
q = cmd_servo_q
do_brake = False
if (state == SERVO_RUNNING) and (cmd_servo_state == SERVO_IDLE):
do_brake = True
end
state = cmd_servo_state
cmd_servo_state = SERVO_IDLE
exit_critical
if do_brake:
stopj(1.0)
sync()
elif state == SERVO_RUNNING:
servoj(q, t=0.008, lookahead_time=0.03, gain=300)
else:
sync()
end
end
end
socket_open("192.168.56.1", 50001)
thread_servo = run servoThread()
keepalive = -2
params_mult = socket_read_binary_integer(6+1)
keepalive = params_mult[7]
while keepalive > 0:
params_mult = socket_read_binary_integer(6+1)
keepalive = params_mult[7]
if keepalive > 0:
if params_mult[0] > 0:
q = [params_mult[1] / MULT_jointstate, params_mult[2] / MULT_jointstate, params_mult[3] / MULT_jointstate, params_mult[4] / MULT_jointstate, params_mult[5] / MULT_jointstate, params_mult[6] / MULT_jointstate]
set_servo_setpoint(q)
end
end
end
sleep(.1)
socket_close()
kill thread_servo
end
)";
ur_driver::UrDriver::UrDriver(const std::string& ROBOT_IP) : rtde_frequency_(125) // conservative CB3 default.
{
ROS_INFO_STREAM("Initializing RTDE client");
rtde_client_.reset(new rtde_interface::RTDEClient(ROBOT_IP, notifier_));
if (!rtde_client_->init())
{
throw std::runtime_error("initialization went wrong"); // TODO: be less harsh
}
comm::URStream<rtde_interface::PackageHeader> stream(ROBOT_IP, 30001);
stream.connect();
size_t len = POSITION_PROGRAM.size();
const uint8_t* data = reinterpret_cast<const uint8_t*>(POSITION_PROGRAM.c_str());
size_t written;
if (stream.write(data, len, written))
{
LOG_INFO("Sent program to robot");
}
else
{
LOG_ERROR("Could not send program to robot");
}
uint32_t reverse_port = 50001; // TODO: Make this a parameter
reverse_interface_.reset(new comm::ReverseInterface(reverse_port));
rtde_client_->start(); // TODO: Add extra start method (also to HW-Interface)
}
std::unique_ptr<rtde_interface::DataPackage> ur_driver::UrDriver::getDataPackage()
{
// TODO: This goes into the rtde_client
std::unique_ptr<comm::URPackage<rtde_interface::PackageHeader>> urpackage;
uint32_t period_ms = (1.0 / rtde_frequency_) * 1000;
std::chrono::milliseconds timeout(period_ms);
if (rtde_client_->getDataPackage(urpackage, timeout))
{
rtde_interface::DataPackage* tmp = dynamic_cast<rtde_interface::DataPackage*>(urpackage.get());
if (tmp != nullptr)
{
urpackage.release();
return std::move(std::unique_ptr<rtde_interface::DataPackage>(tmp));
}
}
return nullptr;
}
bool UrDriver::writeJointCommand(const vector6d_t& values)
{
if (reverse_interface_)
{
reverse_interface_->write(values);
}
else
{
return false;
}
return true;
}
} // namespace ur_driver