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

Clean up of old driver files

Browse Source
This commit is contained in:
Simon Rasmussen
2017-07-09 02:45:02 +02:00
parent 40fc986e7b
commit fdaaacfe2c
17 changed files with 5 additions and 2863 deletions
Download Patch File Download Diff File Expand all files Collapse all files

62
src/do_output.cpp
View File

@@ -1,62 +0,0 @@
/*
* do_output.cpp
*
* Copyright 2015 Thomas Timm Andersen
*
* 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_modern_driver/do_output.h"
void print_debug(std::string inp)
{
#ifdef ROS_BUILD
ROS_DEBUG("%s", inp.c_str());
#else
printf("DEBUG: %s\n", inp.c_str());
#endif
}
void print_info(std::string inp)
{
#ifdef ROS_BUILD
ROS_INFO("%s", inp.c_str());
#else
printf("INFO: %s\n", inp.c_str());
#endif
}
void print_warning(std::string inp)
{
#ifdef ROS_BUILD
ROS_WARN("%s", inp.c_str());
#else
printf("WARNING: %s\n", inp.c_str());
#endif
}
void print_error(std::string inp)
{
#ifdef ROS_BUILD
ROS_ERROR("%s", inp.c_str());
#else
printf("ERROR: %s\n", inp.c_str());
#endif
}
void print_fatal(std::string inp)
{
#ifdef ROS_BUILD
ROS_FATAL("%s", inp.c_str());
ros::shutdown();
#else
printf("FATAL: %s\n", inp.c_str());
exit(1);
#endif
}

30
src/ros/robot_hardware.cpp
View File

@@ -1,30 +0,0 @@
#include "ur_modern_driver/ros/robot_hardware.h"
/*
bool RobotHardware::canSwitch(const std::list<ControllerInfo>& start_list,
const std::list<ControllerInfo>& stop_list) const
{
bool running = active_interface_ != nullptr;
size_t start_size = start_list.size();
size_t stop_size = stop_list.size();
for (auto const& ci : stop_list)
{
auto it = interfaces_.find(ci.hardware_interface);
if(it == interfaces_.end() || it->second != active_interface_)
return false;
}
for (auto const& ci : start_list)
{
auto it = interfaces_.find(ci.hardware_interface);
//we can only start a controller that's already running if we stop it first
if(it == interfaces_.end() || (it->second == active_interface_ && stop_size == 0))
return false;
}
return true;
}
*/

184
src/ur_communication.cpp
View File

@@ -1,184 +0,0 @@
/*
* ur_communication.cpp
*
* Copyright 2015 Thomas Timm Andersen
*
* 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_modern_driver/ur_communication.h"
UrCommunication::UrCommunication(std::condition_variable& msg_cond, std::string host)
{
robot_state_ = new RobotState(msg_cond);
bzero((char*)&pri_serv_addr_, sizeof(pri_serv_addr_));
bzero((char*)&sec_serv_addr_, sizeof(sec_serv_addr_));
pri_sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
if (pri_sockfd_ < 0)
{
print_fatal("ERROR opening socket pri_sockfd");
}
sec_sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
if (sec_sockfd_ < 0)
{
print_fatal("ERROR opening socket sec_sockfd");
}
server_ = gethostbyname(host.c_str());
if (server_ == NULL)
{
print_fatal("ERROR, unknown host");
}
pri_serv_addr_.sin_family = AF_INET;
sec_serv_addr_.sin_family = AF_INET;
bcopy((char*)server_->h_addr, (char*)&pri_serv_addr_.sin_addr.s_addr, server_->h_length);
bcopy((char*)server_->h_addr, (char*)&sec_serv_addr_.sin_addr.s_addr, server_->h_length);
pri_serv_addr_.sin_port = htons(30001);
sec_serv_addr_.sin_port = htons(30002);
flag_ = 1;
setsockopt(pri_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag_, sizeof(int));
setsockopt(pri_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, sizeof(int));
setsockopt(pri_sockfd_, SOL_SOCKET, SO_REUSEADDR, (char*)&flag_, sizeof(int));
setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag_, sizeof(int));
setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, sizeof(int));
setsockopt(sec_sockfd_, SOL_SOCKET, SO_REUSEADDR, (char*)&flag_, sizeof(int));
fcntl(sec_sockfd_, F_SETFL, O_NONBLOCK);
connected_ = false;
keepalive_ = false;
}
bool UrCommunication::start()
{
keepalive_ = true;
uint8_t buf[512];
unsigned int bytes_read;
std::string cmd;
bzero(buf, 512);
print_debug("Acquire firmware version: Connecting...");
if (connect(pri_sockfd_, (struct sockaddr*)&pri_serv_addr_, sizeof(pri_serv_addr_)) < 0)
{
print_fatal("Error connecting to get firmware version");
return false;
}
print_debug("Acquire firmware version: Got connection");
bytes_read = read(pri_sockfd_, buf, 512);
setsockopt(pri_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, sizeof(int));
robot_state_->unpack(buf, bytes_read);
// wait for some traffic so the UR socket doesn't die in version 3.1.
std::this_thread::sleep_for(std::chrono::milliseconds(500));
char tmp[64];
sprintf(tmp, "Firmware version detected: %.7f", robot_state_->getVersion());
print_debug(tmp);
close(pri_sockfd_);
print_debug("Switching to secondary interface for masterboard data: Connecting...");
fd_set writefds;
struct timeval timeout;
connect(sec_sockfd_, (struct sockaddr*)&sec_serv_addr_, sizeof(sec_serv_addr_));
FD_ZERO(&writefds);
FD_SET(sec_sockfd_, &writefds);
timeout.tv_sec = 10;
timeout.tv_usec = 0;
select(sec_sockfd_ + 1, NULL, &writefds, NULL, &timeout);
unsigned int flag_len;
getsockopt(sec_sockfd_, SOL_SOCKET, SO_ERROR, &flag_, &flag_len);
if (flag_ < 0)
{
print_fatal("Error connecting to secondary interface");
return false;
}
print_debug("Secondary interface: Got connection");
comThread_ = std::thread(&UrCommunication::run, this);
return true;
}
void UrCommunication::halt()
{
keepalive_ = false;
comThread_.join();
}
void UrCommunication::run()
{
uint8_t buf[2048];
int bytes_read;
bzero(buf, 2048);
struct timeval timeout;
fd_set readfds;
FD_ZERO(&readfds);
FD_SET(sec_sockfd_, &readfds);
connected_ = true;
while (keepalive_)
{
while (connected_ && keepalive_)
{
timeout.tv_sec = 0; // do this each loop as selects modifies timeout
timeout.tv_usec = 500000; // timeout of 0.5 sec
select(sec_sockfd_ + 1, &readfds, NULL, NULL, &timeout);
bytes_read = read(sec_sockfd_, buf, 2048); // usually only up to 1295 bytes
if (bytes_read > 0)
{
setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, sizeof(int));
robot_state_->unpack(buf, bytes_read);
}
else
{
connected_ = false;
robot_state_->setDisconnected();
close(sec_sockfd_);
}
}
if (keepalive_)
{
// reconnect
print_warning("Secondary port: No connection. Is controller crashed? Will try to reconnect in 10 seconds...");
sec_sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
if (sec_sockfd_ < 0)
{
print_fatal("ERROR opening secondary socket");
}
flag_ = 1;
setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag_, sizeof(int));
setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, sizeof(int));
setsockopt(sec_sockfd_, SOL_SOCKET, SO_REUSEADDR, (char*)&flag_, sizeof(int));
fcntl(sec_sockfd_, F_SETFL, O_NONBLOCK);
while (keepalive_ && !connected_)
{
std::this_thread::sleep_for(std::chrono::seconds(10));
fd_set writefds;
connect(sec_sockfd_, (struct sockaddr*)&sec_serv_addr_, sizeof(sec_serv_addr_));
FD_ZERO(&writefds);
FD_SET(sec_sockfd_, &writefds);
select(sec_sockfd_ + 1, NULL, &writefds, NULL, NULL);
unsigned int flag_len;
getsockopt(sec_sockfd_, SOL_SOCKET, SO_ERROR, &flag_, &flag_len);
if (flag_ < 0)
{
print_error("Error re-connecting to port 30002. Is controller started? Will try to reconnect in 10 "
"seconds...");
}
else
{
connected_ = true;
print_info("Secondary port: Reconnected");
}
}
}
}
// wait for some traffic so the UR socket doesn't die in version 3.1.
std::this_thread::sleep_for(std::chrono::milliseconds(500));
close(sec_sockfd_);
}

420
src/ur_driver.cpp
View File

@@ -1,420 +0,0 @@
/*
* ur_driver.cpp
*
* Copyright 2015 Thomas Timm Andersen
*
* 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_modern_driver/ur_driver.h"
UrDriver::UrDriver(std::condition_variable& rt_msg_cond, std::condition_variable& msg_cond, std::string host,
unsigned int reverse_port, double servoj_time, unsigned int safety_count_max, double max_time_step,
double min_payload, double max_payload, double servoj_lookahead_time, double servoj_gain)
: REVERSE_PORT_(reverse_port)
, maximum_time_step_(max_time_step)
, minimum_payload_(min_payload)
, maximum_payload_(max_payload)
, servoj_time_(servoj_time)
, servoj_lookahead_time_(servoj_lookahead_time)
, servoj_gain_(servoj_gain)
{
char buffer[256];
struct sockaddr_in serv_addr;
int n, flag;
firmware_version_ = 0;
reverse_connected_ = false;
executing_traj_ = false;
rt_interface_ = new UrRealtimeCommunication(rt_msg_cond, host, safety_count_max);
new_sockfd_ = -1;
sec_interface_ = new UrCommunication(msg_cond, host);
incoming_sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
if (incoming_sockfd_ < 0)
{
print_fatal("ERROR opening socket for reverse communication");
}
bzero((char*)&serv_addr, sizeof(serv_addr));
serv_addr.sin_family = AF_INET;
serv_addr.sin_addr.s_addr = INADDR_ANY;
serv_addr.sin_port = htons(REVERSE_PORT_);
flag = 1;
setsockopt(incoming_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag, sizeof(int));
setsockopt(incoming_sockfd_, SOL_SOCKET, SO_REUSEADDR, &flag, sizeof(int));
if (bind(incoming_sockfd_, (struct sockaddr*)&serv_addr, sizeof(serv_addr)) < 0)
{
print_fatal("ERROR on binding socket for reverse communication");
}
listen(incoming_sockfd_, 5);
}
std::vector<double> UrDriver::interp_cubic(double t, double T, std::vector<double> p0_pos, std::vector<double> p1_pos,
std::vector<double> p0_vel, std::vector<double> p1_vel)
{
/*Returns positions of the joints at time 't' */
std::vector<double> positions;
for (unsigned int i = 0; i < p0_pos.size(); i++)
{
double a = p0_pos[i];
double b = p0_vel[i];
double c = (-3 * p0_pos[i] + 3 * p1_pos[i] - 2 * T * p0_vel[i] - T * p1_vel[i]) / pow(T, 2);
double d = (2 * p0_pos[i] - 2 * p1_pos[i] + T * p0_vel[i] + T * p1_vel[i]) / pow(T, 3);
positions.push_back(a + b * t + c * pow(t, 2) + d * pow(t, 3));
}
return positions;
}
bool UrDriver::doTraj(std::vector<double> inp_timestamps, std::vector<std::vector<double>> inp_positions,
std::vector<std::vector<double>> inp_velocities)
{
std::chrono::high_resolution_clock::time_point t0, t;
std::vector<double> positions;
unsigned int j;
if (!UrDriver::uploadProg())
{
return false;
}
executing_traj_ = true;
t0 = std::chrono::high_resolution_clock::now();
t = t0;
j = 0;
while ((inp_timestamps[inp_timestamps.size() - 1] >=
std::chrono::duration_cast<std::chrono::duration<double>>(t - t0).count()) and
executing_traj_)
{
while (inp_timestamps[j] <= std::chrono::duration_cast<std::chrono::duration<double>>(t - t0).count() &&
j < inp_timestamps.size() - 1)
{
j += 1;
}
positions = UrDriver::interp_cubic(std::chrono::duration_cast<std::chrono::duration<double>>(t - t0).count() -
inp_timestamps[j - 1],
inp_timestamps[j] - inp_timestamps[j - 1], inp_positions[j - 1],
inp_positions[j], inp_velocities[j - 1], inp_velocities[j]);
UrDriver::servoj(positions);
// oversample with 4 * sample_time
std::this_thread::sleep_for(std::chrono::milliseconds((int)((servoj_time_ * 1000) / 4.)));
t = std::chrono::high_resolution_clock::now();
}
executing_traj_ = false;
// Signal robot to stop driverProg()
UrDriver::closeServo(positions);
return true;
}
void UrDriver::servoj(std::vector<double> positions, int keepalive)
{
if (!reverse_connected_)
{
print_error("UrDriver::servoj called without a reverse connection present. Keepalive: " +
std::to_string(keepalive));
return;
}
unsigned int bytes_written;
int tmp;
unsigned char buf[28];
for (int i = 0; i < 6; i++)
{
tmp = htonl((int)(positions[i] * MULT_JOINTSTATE_));
buf[i * 4] = tmp & 0xff;
buf[i * 4 + 1] = (tmp >> 8) & 0xff;
buf[i * 4 + 2] = (tmp >> 16) & 0xff;
buf[i * 4 + 3] = (tmp >> 24) & 0xff;
}
tmp = htonl((int)keepalive);
buf[6 * 4] = tmp & 0xff;
buf[6 * 4 + 1] = (tmp >> 8) & 0xff;
buf[6 * 4 + 2] = (tmp >> 16) & 0xff;
buf[6 * 4 + 3] = (tmp >> 24) & 0xff;
bytes_written = write(new_sockfd_, buf, 28);
}
void UrDriver::stopTraj()
{
executing_traj_ = false;
rt_interface_->addCommandToQueue("stopj(10)\n");
}
bool UrDriver::uploadProg()
{
std::string cmd_str;
char buf[128];
cmd_str = "def driverProg():\n";
sprintf(buf, "\tMULT_jointstate = %i\n", MULT_JOINTSTATE_);
cmd_str += buf;
cmd_str += "\tSERVO_IDLE = 0\n";
cmd_str += "\tSERVO_RUNNING = 1\n";
cmd_str += "\tcmd_servo_state = SERVO_IDLE\n";
cmd_str += "\tcmd_servo_q = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]\n";
cmd_str += "\tdef set_servo_setpoint(q):\n";
cmd_str += "\t\tenter_critical\n";
cmd_str += "\t\tcmd_servo_state = SERVO_RUNNING\n";
cmd_str += "\t\tcmd_servo_q = q\n";
cmd_str += "\t\texit_critical\n";
cmd_str += "\tend\n";
cmd_str += "\tthread servoThread():\n";
cmd_str += "\t\tstate = SERVO_IDLE\n";
cmd_str += "\t\twhile True:\n";
cmd_str += "\t\t\tenter_critical\n";
cmd_str += "\t\t\tq = cmd_servo_q\n";
cmd_str += "\t\t\tdo_brake = False\n";
cmd_str += "\t\t\tif (state == SERVO_RUNNING) and ";
cmd_str += "(cmd_servo_state == SERVO_IDLE):\n";
cmd_str += "\t\t\t\tdo_brake = True\n";
cmd_str += "\t\t\tend\n";
cmd_str += "\t\t\tstate = cmd_servo_state\n";
cmd_str += "\t\t\tcmd_servo_state = SERVO_IDLE\n";
cmd_str += "\t\t\texit_critical\n";
cmd_str += "\t\t\tif do_brake:\n";
cmd_str += "\t\t\t\tstopj(1.0)\n";
cmd_str += "\t\t\t\tsync()\n";
cmd_str += "\t\t\telif state == SERVO_RUNNING:\n";
if (sec_interface_->robot_state_->getVersion() >= 3.1)
sprintf(buf, "\t\t\t\tservoj(q, t=%.4f, lookahead_time=%.4f, gain=%.0f)\n", servoj_time_, servoj_lookahead_time_,
servoj_gain_);
else
sprintf(buf, "\t\t\t\tservoj(q, t=%.4f)\n", servoj_time_);
cmd_str += buf;
cmd_str += "\t\t\telse:\n";
cmd_str += "\t\t\t\tsync()\n";
cmd_str += "\t\t\tend\n";
cmd_str += "\t\tend\n";
cmd_str += "\tend\n";
sprintf(buf, "\tsocket_open(\"%s\", %i)\n", ip_addr_.c_str(), REVERSE_PORT_);
cmd_str += buf;
cmd_str += "\tthread_servo = run servoThread()\n";
cmd_str += "\tkeepalive = 1\n";
cmd_str += "\twhile keepalive > 0:\n";
cmd_str += "\t\tparams_mult = socket_read_binary_integer(6+1)\n";
cmd_str += "\t\tif params_mult[0] > 0:\n";
cmd_str += "\t\t\tq = [params_mult[1] / MULT_jointstate, ";
cmd_str += "params_mult[2] / MULT_jointstate, ";
cmd_str += "params_mult[3] / MULT_jointstate, ";
cmd_str += "params_mult[4] / MULT_jointstate, ";
cmd_str += "params_mult[5] / MULT_jointstate, ";
cmd_str += "params_mult[6] / MULT_jointstate]\n";
cmd_str += "\t\t\tkeepalive = params_mult[7]\n";
cmd_str += "\t\t\tset_servo_setpoint(q)\n";
cmd_str += "\t\tend\n";
cmd_str += "\tend\n";
cmd_str += "\tsleep(.1)\n";
cmd_str += "\tsocket_close()\n";
cmd_str += "\tkill thread_servo\n";
cmd_str += "end\n";
rt_interface_->addCommandToQueue(cmd_str);
return UrDriver::openServo();
}
bool UrDriver::openServo()
{
struct sockaddr_in cli_addr;
socklen_t clilen;
clilen = sizeof(cli_addr);
new_sockfd_ = accept(incoming_sockfd_, (struct sockaddr*)&cli_addr, &clilen);
if (new_sockfd_ < 0)
{
print_fatal("ERROR on accepting reverse communication");
return false;
}
reverse_connected_ = true;
return true;
}
void UrDriver::closeServo(std::vector<double> positions)
{
if (positions.size() != 6)
UrDriver::servoj(rt_interface_->robot_state_->getQActual(), 0);
else
UrDriver::servoj(positions, 0);
reverse_connected_ = false;
close(new_sockfd_);
}
bool UrDriver::start()
{
if (!sec_interface_->start())
return false;
firmware_version_ = sec_interface_->robot_state_->getVersion();
rt_interface_->robot_state_->setVersion(firmware_version_);
if (!rt_interface_->start())
return false;
ip_addr_ = rt_interface_->getLocalIp();
print_debug("Listening on " + ip_addr_ + ":" + std::to_string(REVERSE_PORT_) + "\n");
return true;
}
void UrDriver::halt()
{
if (executing_traj_)
{
UrDriver::stopTraj();
}
sec_interface_->halt();
rt_interface_->halt();
close(incoming_sockfd_);
}
void UrDriver::setSpeed(double q0, double q1, double q2, double q3, double q4, double q5, double acc)
{
rt_interface_->setSpeed(q0, q1, q2, q3, q4, q5, acc);
}
std::vector<std::string> UrDriver::getJointNames()
{
return joint_names_;
}
void UrDriver::setJointNames(std::vector<std::string> jn)
{
joint_names_ = jn;
}
void UrDriver::setToolVoltage(unsigned int v)
{
char buf[256];
sprintf(buf, "sec setOut():\n\tset_tool_voltage(%d)\nend\n", v);
rt_interface_->addCommandToQueue(buf);
print_debug(buf);
}
void UrDriver::setFlag(unsigned int n, bool b)
{
char buf[256];
sprintf(buf, "sec setOut():\n\tset_flag(%d, %s)\nend\n", n, b ? "True" : "False");
rt_interface_->addCommandToQueue(buf);
print_debug(buf);
}
void UrDriver::setDigitalOut(unsigned int n, bool b)
{
char buf[256];
if (firmware_version_ < 2)
{
sprintf(buf, "sec setOut():\n\tset_digital_out(%d, %s)\nend\n", n, b ? "True" : "False");
}
else if (n > 15)
{
sprintf(buf, "sec setOut():\n\tset_tool_digital_out(%d, %s)\nend\n", n - 16, b ? "True" : "False");
}
else if (n > 7)
{
sprintf(buf, "sec setOut():\n\tset_configurable_digital_out(%d, %s)\nend\n", n - 8, b ? "True" : "False");
}
else
{
sprintf(buf, "sec setOut():\n\tset_standard_digital_out(%d, %s)\nend\n", n, b ? "True" : "False");
}
rt_interface_->addCommandToQueue(buf);
print_debug(buf);
}
void UrDriver::setAnalogOut(unsigned int n, double f)
{
char buf[256];
if (firmware_version_ < 2)
{
sprintf(buf, "sec setOut():\n\tset_analog_out(%d, %1.4f)\nend\n", n, f);
}
else
{
sprintf(buf, "sec setOut():\n\tset_standard_analog_out(%d, %1.4f)\nend\n", n, f);
}
rt_interface_->addCommandToQueue(buf);
print_debug(buf);
}
bool UrDriver::setPayload(double m)
{
if ((m < maximum_payload_) && (m > minimum_payload_))
{
char buf[256];
sprintf(buf, "sec setOut():\n\tset_payload(%1.3f)\nend\n", m);
rt_interface_->addCommandToQueue(buf);
print_debug(buf);
return true;
}
else
return false;
}
void UrDriver::setMinPayload(double m)
{
if (m > 0)
{
minimum_payload_ = m;
}
else
{
minimum_payload_ = 0;
}
}
void UrDriver::setMaxPayload(double m)
{
maximum_payload_ = m;
}
void UrDriver::setServojTime(double t)
{
if (t > 0.008)
{
servoj_time_ = t;
}
else
{
servoj_time_ = 0.008;
}
}
void UrDriver::setServojLookahead(double t)
{
if (t > 0.03)
{
if (t < 0.2)
{
servoj_lookahead_time_ = t;
}
else
{
servoj_lookahead_time_ = 0.2;
}
}
else
{
servoj_lookahead_time_ = 0.03;
}
}
void UrDriver::setServojGain(double g)
{
if (g > 100)
{
if (g < 2000)
{
servoj_gain_ = g;
}
else
{
servoj_gain_ = 2000;
}
}
else
{
servoj_gain_ = 100;
}
}

283
src/ur_hardware_interface.cpp
View File

@@ -1,283 +0,0 @@
/*
* ur_hardware_control_loop.cpp
*
* Copyright 2015 Thomas Timm Andersen
*
* 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.
*/
/* Based on original source from University of Colorado, Boulder. License copied below. */
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2015, University of Colorado, Boulder
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Univ of CO, Boulder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************
Author: Dave Coleman
*/
#include <ur_modern_driver/ur_hardware_interface.h>
namespace ros_control_ur
{
UrHardwareInterface::UrHardwareInterface(ros::NodeHandle& nh, UrDriver* robot) : nh_(nh), robot_(robot)
{
// Initialize shared memory and interfaces here
init(); // this implementation loads from rosparam
max_vel_change_ = 0.12; // equivalent of an acceleration of 15 rad/sec^2
ROS_INFO_NAMED("ur_hardware_interface", "Loaded ur_hardware_interface.");
}
void UrHardwareInterface::init()
{
ROS_INFO_STREAM_NAMED("ur_hardware_interface", "Reading rosparams from namespace: " << nh_.getNamespace());
// Get joint names
nh_.getParam("hardware_interface/joints", joint_names_);
if (joint_names_.size() == 0)
{
ROS_FATAL_STREAM_NAMED("ur_hardware_interface",
"No joints found on parameter server for controller, did you load the proper yaml file?"
<< " Namespace: " << nh_.getNamespace());
exit(-1);
}
num_joints_ = joint_names_.size();
// Resize vectors
joint_position_.resize(num_joints_);
joint_velocity_.resize(num_joints_);
joint_effort_.resize(num_joints_);
joint_position_command_.resize(num_joints_);
joint_velocity_command_.resize(num_joints_);
prev_joint_velocity_command_.resize(num_joints_);
// Initialize controller
for (std::size_t i = 0; i < num_joints_; ++i)
{
ROS_DEBUG_STREAM_NAMED("ur_hardware_interface", "Loading joint name: " << joint_names_[i]);
// Create joint state interface
joint_state_interface_.registerHandle(hardware_interface::JointStateHandle(joint_names_[i], &joint_position_[i],
&joint_velocity_[i], &joint_effort_[i]));
// Create position joint interface
position_joint_interface_.registerHandle(hardware_interface::JointHandle(
joint_state_interface_.getHandle(joint_names_[i]), &joint_position_command_[i]));
// Create velocity joint interface
velocity_joint_interface_.registerHandle(hardware_interface::JointHandle(
joint_state_interface_.getHandle(joint_names_[i]), &joint_velocity_command_[i]));
prev_joint_velocity_command_[i] = 0.;
}
// Create force torque interface
force_torque_interface_.registerHandle(
hardware_interface::ForceTorqueSensorHandle("wrench", "", robot_force_, robot_torque_));
registerInterface(&joint_state_interface_); // From RobotHW base class.
registerInterface(&position_joint_interface_); // From RobotHW base class.
registerInterface(&velocity_joint_interface_); // From RobotHW base class.
registerInterface(&force_torque_interface_); // From RobotHW base class.
velocity_interface_running_ = false;
position_interface_running_ = false;
}
void UrHardwareInterface::read()
{
std::vector<double> pos, vel, current, tcp;
pos = robot_->rt_interface_->robot_state_->getQActual();
vel = robot_->rt_interface_->robot_state_->getQdActual();
current = robot_->rt_interface_->robot_state_->getIActual();
tcp = robot_->rt_interface_->robot_state_->getTcpForce();
for (std::size_t i = 0; i < num_joints_; ++i)
{
joint_position_[i] = pos[i];
joint_velocity_[i] = vel[i];
joint_effort_[i] = current[i];
}
for (std::size_t i = 0; i < 3; ++i)
{
robot_force_[i] = tcp[i];
robot_torque_[i] = tcp[i + 3];
}
}
void UrHardwareInterface::setMaxVelChange(double inp)
{
max_vel_change_ = inp;
}
void UrHardwareInterface::write()
{
if (velocity_interface_running_)
{
std::vector<double> cmd;
// do some rate limiting
cmd.resize(joint_velocity_command_.size());
for (unsigned int i = 0; i < joint_velocity_command_.size(); i++)
{
cmd[i] = joint_velocity_command_[i];
if (cmd[i] > prev_joint_velocity_command_[i] + max_vel_change_)
{
cmd[i] = prev_joint_velocity_command_[i] + max_vel_change_;
}
else if (cmd[i] < prev_joint_velocity_command_[i] - max_vel_change_)
{
cmd[i] = prev_joint_velocity_command_[i] - max_vel_change_;
}
prev_joint_velocity_command_[i] = cmd[i];
}
robot_->setSpeed(cmd[0], cmd[1], cmd[2], cmd[3], cmd[4], cmd[5], max_vel_change_ * 125);
}
else if (position_interface_running_)
{
robot_->servoj(joint_position_command_);
}
}
bool UrHardwareInterface::canSwitch(const std::list<hardware_interface::ControllerInfo>& start_list,
const std::list<hardware_interface::ControllerInfo>& stop_list) const
{
for (std::list<hardware_interface::ControllerInfo>::const_iterator controller_it = start_list.begin();
controller_it != start_list.end(); ++controller_it)
{
if (controller_it->hardware_interface == "hardware_interface::VelocityJointInterface")
{
if (velocity_interface_running_)
{
ROS_ERROR("%s: An interface of that type (%s) is already running", controller_it->name.c_str(),
controller_it->hardware_interface.c_str());
return false;
}
if (position_interface_running_)
{
bool error = true;
for (std::list<hardware_interface::ControllerInfo>::const_iterator stop_controller_it = stop_list.begin();
stop_controller_it != stop_list.end(); ++stop_controller_it)
{
if (stop_controller_it->hardware_interface == "hardware_interface::PositionJointInterface")
{
error = false;
break;
}
}
if (error)
{
ROS_ERROR("%s (type %s) can not be run simultaneously with a PositionJointInterface",
controller_it->name.c_str(), controller_it->hardware_interface.c_str());
return false;
}
}
}
else if (controller_it->hardware_interface == "hardware_interface::PositionJointInterface")
{
if (position_interface_running_)
{
ROS_ERROR("%s: An interface of that type (%s) is already running", controller_it->name.c_str(),
controller_it->hardware_interface.c_str());
return false;
}
if (velocity_interface_running_)
{
bool error = true;
for (std::list<hardware_interface::ControllerInfo>::const_iterator stop_controller_it = stop_list.begin();
stop_controller_it != stop_list.end(); ++stop_controller_it)
{
if (stop_controller_it->hardware_interface == "hardware_interface::VelocityJointInterface")
{
error = false;
break;
}
}
if (error)
{
ROS_ERROR("%s (type %s) can not be run simultaneously with a VelocityJointInterface",
controller_it->name.c_str(), controller_it->hardware_interface.c_str());
return false;
}
}
}
}
// we can always stop a controller
return true;
}
void UrHardwareInterface::doSwitch(const std::list<hardware_interface::ControllerInfo>& start_list,
const std::list<hardware_interface::ControllerInfo>& stop_list)
{
for (std::list<hardware_interface::ControllerInfo>::const_iterator controller_it = stop_list.begin();
controller_it != stop_list.end(); ++controller_it)
{
if (controller_it->hardware_interface == "hardware_interface::VelocityJointInterface")
{
velocity_interface_running_ = false;
ROS_DEBUG("Stopping velocity interface");
}
if (controller_it->hardware_interface == "hardware_interface::PositionJointInterface")
{
position_interface_running_ = false;
std::vector<double> tmp;
robot_->closeServo(tmp);
ROS_DEBUG("Stopping position interface");
}
}
for (std::list<hardware_interface::ControllerInfo>::const_iterator controller_it = start_list.begin();
controller_it != start_list.end(); ++controller_it)
{
if (controller_it->hardware_interface == "hardware_interface::VelocityJointInterface")
{
velocity_interface_running_ = true;
ROS_DEBUG("Starting velocity interface");
}
if (controller_it->hardware_interface == "hardware_interface::PositionJointInterface")
{
position_interface_running_ = true;
robot_->uploadProg();
ROS_DEBUG("Starting position interface");
}
}
}
} // namespace

211
src/ur_realtime_communication.cpp
View File

@@ -1,211 +0,0 @@
/*
* ur_realtime_communication.cpp
*
* Copyright 2015 Thomas Timm Andersen
*
* 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_modern_driver/ur_realtime_communication.h"
UrRealtimeCommunication::UrRealtimeCommunication(std::condition_variable& msg_cond, std::string host,
unsigned int safety_count_max)
{
robot_state_ = new RobotStateRT(msg_cond);
bzero((char*)&serv_addr_, sizeof(serv_addr_));
sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
if (sockfd_ < 0)
{
print_fatal("ERROR opening socket");
}
server_ = gethostbyname(host.c_str());
if (server_ == NULL)
{
print_fatal("ERROR, no such host");
}
serv_addr_.sin_family = AF_INET;
bcopy((char*)server_->h_addr, (char*)&serv_addr_.sin_addr.s_addr, server_->h_length);
serv_addr_.sin_port = htons(30003);
flag_ = 1;
setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag_, sizeof(int));
setsockopt(sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, sizeof(int));
setsockopt(sockfd_, SOL_SOCKET, SO_REUSEADDR, (char*)&flag_, sizeof(int));
fcntl(sockfd_, F_SETFL, O_NONBLOCK);
connected_ = false;
keepalive_ = false;
safety_count_ = safety_count_max + 1;
safety_count_max_ = safety_count_max;
}
bool UrRealtimeCommunication::start()
{
fd_set writefds;
struct timeval timeout;
keepalive_ = true;
print_debug("Realtime port: Connecting...");
connect(sockfd_, (struct sockaddr*)&serv_addr_, sizeof(serv_addr_));
FD_ZERO(&writefds);
FD_SET(sockfd_, &writefds);
timeout.tv_sec = 10;
timeout.tv_usec = 0;
select(sockfd_ + 1, NULL, &writefds, NULL, &timeout);
unsigned int flag_len;
getsockopt(sockfd_, SOL_SOCKET, SO_ERROR, &flag_, &flag_len);
if (flag_ < 0)
{
print_fatal("Error connecting to RT port 30003");
return false;
}
sockaddr_in name;
socklen_t namelen = sizeof(name);
int err = getsockname(sockfd_, (sockaddr*)&name, &namelen);
if (err < 0)
{
print_fatal("Could not get local IP");
close(sockfd_);
return false;
}
char str[18];
inet_ntop(AF_INET, &name.sin_addr, str, 18);
local_ip_ = str;
comThread_ = std::thread(&UrRealtimeCommunication::run, this);
return true;
}
void UrRealtimeCommunication::halt()
{
keepalive_ = false;
comThread_.join();
}
void UrRealtimeCommunication::addCommandToQueue(std::string inp)
{
int bytes_written;
if (inp.back() != '\n')
{
inp.append("\n");
}
if (connected_)
bytes_written = write(sockfd_, inp.c_str(), inp.length());
else
print_error("Could not send command \"" + inp + "\". The robot is not connected! Command is discarded");
}
void UrRealtimeCommunication::setSpeed(double q0, double q1, double q2, double q3, double q4, double q5, double acc)
{
char cmd[1024];
if (robot_state_->getVersion() >= 3.1)
{
sprintf(cmd, "speedj([%1.5f, %1.5f, %1.5f, %1.5f, %1.5f, %1.5f], %f)\n", q0, q1, q2, q3, q4, q5, acc);
}
else
{
sprintf(cmd, "speedj([%1.5f, %1.5f, %1.5f, %1.5f, %1.5f, %1.5f], %f, 0.02)\n", q0, q1, q2, q3, q4, q5, acc);
}
addCommandToQueue((std::string)(cmd));
if (q0 != 0. or q1 != 0. or q2 != 0. or q3 != 0. or q4 != 0. or q5 != 0.)
{
// If a joint speed is set, make sure we stop it again after some time if the user doesn't
safety_count_ = 0;
}
}
void UrRealtimeCommunication::run()
{
uint8_t buf[2048];
int bytes_read;
bzero(buf, 2048);
struct timeval timeout;
fd_set readfds;
FD_ZERO(&readfds);
FD_SET(sockfd_, &readfds);
print_debug("Realtime port: Got connection");
connected_ = true;
while (keepalive_)
{
while (connected_ && keepalive_)
{
timeout.tv_sec = 0; // do this each loop as selects modifies timeout
timeout.tv_usec = 500000; // timeout of 0.5 sec
select(sockfd_ + 1, &readfds, NULL, NULL, &timeout);
bytes_read = read(sockfd_, buf, 2048);
if (bytes_read > 0)
{
setsockopt(sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, sizeof(int));
robot_state_->unpack(buf);
if (safety_count_ == safety_count_max_)
{
setSpeed(0., 0., 0., 0., 0., 0.);
}
safety_count_ += 1;
}
else
{
connected_ = false;
close(sockfd_);
}
}
if (keepalive_)
{
// reconnect
print_warning("Realtime port: No connection. Is controller crashed? Will try to reconnect in 10 seconds...");
sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
if (sockfd_ < 0)
{
print_fatal("ERROR opening socket");
}
flag_ = 1;
setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag_, sizeof(int));
setsockopt(sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, sizeof(int));
setsockopt(sockfd_, SOL_SOCKET, SO_REUSEADDR, (char*)&flag_, sizeof(int));
fcntl(sockfd_, F_SETFL, O_NONBLOCK);
while (keepalive_ && !connected_)
{
std::this_thread::sleep_for(std::chrono::seconds(10));
fd_set writefds;
connect(sockfd_, (struct sockaddr*)&serv_addr_, sizeof(serv_addr_));
FD_ZERO(&writefds);
FD_SET(sockfd_, &writefds);
select(sockfd_ + 1, NULL, &writefds, NULL, NULL);
unsigned int flag_len;
getsockopt(sockfd_, SOL_SOCKET, SO_ERROR, &flag_, &flag_len);
if (flag_ < 0)
{
print_error("Error re-connecting to RT port 30003. Is controller started? Will try to reconnect in 10 "
"seconds...");
}
else
{
connected_ = true;
print_info("Realtime port: Reconnected");
}
}
}
}
setSpeed(0., 0., 0., 0., 0., 0.);
close(sockfd_);
}
void UrRealtimeCommunication::setSafetyCountMax(uint inp)
{
safety_count_max_ = inp;
}
std::string UrRealtimeCommunication::getLocalIp()
{
return local_ip_;
}

871
src/ur_ros_wrapper.cpp
View File

@@ -1,871 +0,0 @@
/*
* ur_driver.cpp
*
* Copyright 2015 Thomas Timm Andersen
*
* 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 <string.h>
#include <time.h>
#include <algorithm>
#include <chrono>
#include <cmath>
#include <condition_variable>
#include <mutex>
#include <thread>
#include <vector>
#include "ur_modern_driver/do_output.h"
#include "ur_modern_driver/ur_driver.h"
#include "ur_modern_driver/ur_hardware_interface.h"
#include <controller_manager/controller_manager.h>
#include <realtime_tools/realtime_publisher.h>
#include <ros/console.h>
#include "actionlib/server/action_server.h"
#include "actionlib/server/server_goal_handle.h"
#include "control_msgs/FollowJointTrajectoryAction.h"
#include "geometry_msgs/PoseStamped.h"
#include "geometry_msgs/WrenchStamped.h"
#include "ros/ros.h"
#include "sensor_msgs/JointState.h"
#include "std_msgs/String.h"
#include "trajectory_msgs/JointTrajectoryPoint.h"
#include "ur_msgs/Analog.h"
#include "ur_msgs/Digital.h"
#include "ur_msgs/IOStates.h"
#include "ur_msgs/SetIO.h"
#include "ur_msgs/SetIORequest.h"
#include "ur_msgs/SetIOResponse.h"
#include "ur_msgs/SetPayload.h"
#include "ur_msgs/SetPayloadRequest.h"
#include "ur_msgs/SetPayloadResponse.h"
/// TF
#include <tf/tf.h>
#include <tf/transform_broadcaster.h>
class RosWrapper
{
protected:
UrDriver robot_;
std::condition_variable rt_msg_cond_;
std::condition_variable msg_cond_;
ros::NodeHandle nh_;
actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction> as_;
actionlib::ServerGoalHandle<control_msgs::FollowJointTrajectoryAction> goal_handle_;
bool has_goal_;
control_msgs::FollowJointTrajectoryFeedback feedback_;
control_msgs::FollowJointTrajectoryResult result_;
ros::Subscriber speed_sub_;
ros::Subscriber urscript_sub_;
ros::ServiceServer io_srv_;
ros::ServiceServer payload_srv_;
std::thread* rt_publish_thread_;
std::thread* mb_publish_thread_;
double io_flag_delay_;
double max_velocity_;
std::vector<double> joint_offsets_;
std::string base_frame_;
std::string tool_frame_;
bool use_ros_control_;
std::thread* ros_control_thread_;
boost::shared_ptr<ros_control_ur::UrHardwareInterface> hardware_interface_;
boost::shared_ptr<controller_manager::ControllerManager> controller_manager_;
public:
RosWrapper(std::string host, int reverse_port)
: as_(nh_, "follow_joint_trajectory", boost::bind(&RosWrapper::goalCB, this, _1),
boost::bind(&RosWrapper::cancelCB, this, _1), false)
, robot_(rt_msg_cond_, msg_cond_, host, reverse_port, 0.03, 300)
, io_flag_delay_(0.05)
, joint_offsets_(6, 0.0)
{
std::string joint_prefix = "";
std::vector<std::string> joint_names;
char buf[256];
if (ros::param::get("~prefix", joint_prefix))
{
if (joint_prefix.length() > 0)
{
sprintf(buf, "Setting prefix to %s", joint_prefix.c_str());
print_info(buf);
}
}
joint_names.push_back(joint_prefix + "shoulder_pan_joint");
joint_names.push_back(joint_prefix + "shoulder_lift_joint");
joint_names.push_back(joint_prefix + "elbow_joint");
joint_names.push_back(joint_prefix + "wrist_1_joint");
joint_names.push_back(joint_prefix + "wrist_2_joint");
joint_names.push_back(joint_prefix + "wrist_3_joint");
robot_.setJointNames(joint_names);
use_ros_control_ = false;
ros::param::get("~use_ros_control", use_ros_control_);
if (use_ros_control_)
{
hardware_interface_.reset(new ros_control_ur::UrHardwareInterface(nh_, &robot_));
controller_manager_.reset(new controller_manager::ControllerManager(hardware_interface_.get(), nh_));
double max_vel_change = 0.12; // equivalent of an acceleration of 15 rad/sec^2
if (ros::param::get("~max_acceleration", max_vel_change))
{
max_vel_change = max_vel_change / 125;
}
sprintf(buf, "Max acceleration set to: %f [rad/sec²]", max_vel_change * 125);
print_debug(buf);
hardware_interface_->setMaxVelChange(max_vel_change);
}
// Using a very high value in order to not limit execution of trajectories being sent from MoveIt!
max_velocity_ = 10.;
if (ros::param::get("~max_velocity", max_velocity_))
{
sprintf(buf, "Max velocity accepted by ur_driver: %f [rad/s]", max_velocity_);
print_debug(buf);
}
// Bounds for SetPayload service
// Using a very conservative value as it should be set through the parameter server
double min_payload = 0.;
double max_payload = 1.;
if (ros::param::get("~min_payload", min_payload))
{
sprintf(buf, "Min payload set to: %f [kg]", min_payload);
print_debug(buf);
}
if (ros::param::get("~max_payload", max_payload))
{
sprintf(buf, "Max payload set to: %f [kg]", max_payload);
print_debug(buf);
}
robot_.setMinPayload(min_payload);
robot_.setMaxPayload(max_payload);
sprintf(buf, "Bounds for set_payload service calls: [%f, %f]", min_payload, max_payload);
print_debug(buf);
double servoj_time = 0.008;
if (ros::param::get("~servoj_time", servoj_time))
{
sprintf(buf, "Servoj_time set to: %f [sec]", servoj_time);
print_debug(buf);
}
robot_.setServojTime(servoj_time);
double servoj_lookahead_time = 0.03;
if (ros::param::get("~servoj_lookahead_time", servoj_lookahead_time))
{
sprintf(buf, "Servoj_lookahead_time set to: %f [sec]", servoj_lookahead_time);
print_debug(buf);
}
robot_.setServojLookahead(servoj_lookahead_time);
double servoj_gain = 300.;
if (ros::param::get("~servoj_gain", servoj_gain))
{
sprintf(buf, "Servoj_gain set to: %f [sec]", servoj_gain);
print_debug(buf);
}
robot_.setServojGain(servoj_gain);
// Base and tool frames
base_frame_ = joint_prefix + "base_link";
tool_frame_ = joint_prefix + "tool0_controller";
if (ros::param::get("~base_frame", base_frame_))
{
sprintf(buf, "Base frame set to: %s", base_frame_.c_str());
print_debug(buf);
}
if (ros::param::get("~tool_frame", tool_frame_))
{
sprintf(buf, "Tool frame set to: %s", tool_frame_.c_str());
print_debug(buf);
}
if (robot_.start())
{
if (use_ros_control_)
{
ros_control_thread_ = new std::thread(boost::bind(&RosWrapper::rosControlLoop, this));
print_debug("The control thread for this driver has been started");
}
else
{
// start actionserver
has_goal_ = false;
as_.start();
// subscribe to the data topic of interest
rt_publish_thread_ = new std::thread(boost::bind(&RosWrapper::publishRTMsg, this));
print_debug("The action server for this driver has been started");
}
mb_publish_thread_ = new std::thread(boost::bind(&RosWrapper::publishMbMsg, this));
speed_sub_ = nh_.subscribe("ur_driver/joint_speed", 1, &RosWrapper::speedInterface, this);
urscript_sub_ = nh_.subscribe("ur_driver/URScript", 1, &RosWrapper::urscriptInterface, this);
io_srv_ = nh_.advertiseService("ur_driver/set_io", &RosWrapper::setIO, this);
payload_srv_ = nh_.advertiseService("ur_driver/set_payload", &RosWrapper::setPayload, this);
}
}
void halt()
{
robot_.halt();
rt_publish_thread_->join();
}
private:
void trajThread(std::vector<double> timestamps, std::vector<std::vector<double>> positions,
std::vector<std::vector<double>> velocities)
{
robot_.doTraj(timestamps, positions, velocities);
if (has_goal_)
{
result_.error_code = result_.SUCCESSFUL;
goal_handle_.setSucceeded(result_);
has_goal_ = false;
}
}
void goalCB(actionlib::ServerGoalHandle<control_msgs::FollowJointTrajectoryAction> gh)
{
std::string buf;
print_info("on_goal");
if (!robot_.sec_interface_->robot_state_->isReady())
{
result_.error_code = -100; // nothing is defined for this...?
if (!robot_.sec_interface_->robot_state_->isPowerOnRobot())
{
result_.error_string = "Cannot accept new trajectories: Robot arm is not powered on";
gh.setRejected(result_, result_.error_string);
print_error(result_.error_string);
return;
}
if (!robot_.sec_interface_->robot_state_->isRealRobotEnabled())
{
result_.error_string = "Cannot accept new trajectories: Robot is not enabled";
gh.setRejected(result_, result_.error_string);
print_error(result_.error_string);
return;
}
result_.error_string = "Cannot accept new trajectories. (Debug: Robot mode is " +
std::to_string(robot_.sec_interface_->robot_state_->getRobotMode()) + ")";
gh.setRejected(result_, result_.error_string);
print_error(result_.error_string);
return;
}
if (robot_.sec_interface_->robot_state_->isEmergencyStopped())
{
result_.error_code = -100; // nothing is defined for this...?
result_.error_string = "Cannot accept new trajectories: Robot is emergency stopped";
gh.setRejected(result_, result_.error_string);
print_error(result_.error_string);
return;
}
if (robot_.sec_interface_->robot_state_->isProtectiveStopped())
{
result_.error_code = -100; // nothing is defined for this...?
result_.error_string = "Cannot accept new trajectories: Robot is protective stopped";
gh.setRejected(result_, result_.error_string);
print_error(result_.error_string);
return;
}
actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal =
*gh.getGoal(); // make a copy that we can modify
if (has_goal_)
{
print_warning("Received new goal while still executing previous trajectory. Canceling previous trajectory");
has_goal_ = false;
robot_.stopTraj();
result_.error_code = -100; // nothing is defined for this...?
result_.error_string = "Received another trajectory";
goal_handle_.setAborted(result_, result_.error_string);
std::this_thread::sleep_for(std::chrono::milliseconds(250));
}
goal_handle_ = gh;
if (!validateJointNames())
{
std::string outp_joint_names = "";
for (unsigned int i = 0; i < goal.trajectory.joint_names.size(); i++)
{
outp_joint_names += goal.trajectory.joint_names[i] + " ";
}
result_.error_code = result_.INVALID_JOINTS;
result_.error_string = "Received a goal with incorrect joint names: " + outp_joint_names;
gh.setRejected(result_, result_.error_string);
print_error(result_.error_string);
return;
}
if (!has_positions())
{
result_.error_code = result_.INVALID_GOAL;
result_.error_string = "Received a goal without positions";
gh.setRejected(result_, result_.error_string);
print_error(result_.error_string);
return;
}
if (!has_velocities())
{
result_.error_code = result_.INVALID_GOAL;
result_.error_string = "Received a goal without velocities";
gh.setRejected(result_, result_.error_string);
print_error(result_.error_string);
return;
}
if (!traj_is_finite())
{
result_.error_string = "Received a goal with infinities or NaNs";
result_.error_code = result_.INVALID_GOAL;
gh.setRejected(result_, result_.error_string);
print_error(result_.error_string);
return;
}
if (!has_limited_velocities())
{
result_.error_code = result_.INVALID_GOAL;
result_.error_string = "Received a goal with velocities that are higher than " + std::to_string(max_velocity_);
gh.setRejected(result_, result_.error_string);
print_error(result_.error_string);
return;
}
reorder_traj_joints(goal.trajectory);
if (!start_positions_match(goal.trajectory, 0.01))
{
result_.error_code = result_.INVALID_GOAL;
result_.error_string = "Goal start doesn't match current pose";
gh.setRejected(result_, result_.error_string);
print_error(result_.error_string);
return;
}
std::vector<double> timestamps;
std::vector<std::vector<double>> positions, velocities;
if (goal.trajectory.points[0].time_from_start.toSec() != 0.)
{
print_warning("Trajectory's first point should be the current position, with time_from_start set to 0.0 - "
"Inserting point in malformed trajectory");
timestamps.push_back(0.0);
positions.push_back(robot_.rt_interface_->robot_state_->getQActual());
velocities.push_back(robot_.rt_interface_->robot_state_->getQdActual());
}
for (unsigned int i = 0; i < goal.trajectory.points.size(); i++)
{
timestamps.push_back(goal.trajectory.points[i].time_from_start.toSec());
positions.push_back(goal.trajectory.points[i].positions);
velocities.push_back(goal.trajectory.points[i].velocities);
}
goal_handle_.setAccepted();
has_goal_ = true;
std::thread(&RosWrapper::trajThread, this, timestamps, positions, velocities).detach();
}
void cancelCB(actionlib::ServerGoalHandle<control_msgs::FollowJointTrajectoryAction> gh)
{
// set the action state to preempted
print_info("on_cancel");
if (has_goal_)
{
if (gh == goal_handle_)
{
robot_.stopTraj();
has_goal_ = false;
}
}
result_.error_code = -100; // nothing is defined for this...?
result_.error_string = "Goal cancelled by client";
gh.setCanceled(result_);
}
bool setIO(ur_msgs::SetIORequest& req, ur_msgs::SetIOResponse& resp)
{
resp.success = true;
// if (req.fun == ur_msgs::SetIO::Request::FUN_SET_DIGITAL_OUT) {
if (req.fun == 1)
{
robot_.setDigitalOut(req.pin, req.state > 0.0 ? true : false);
}
else if (req.fun == 2)
{
//} else if (req.fun == ur_msgs::SetIO::Request::FUN_SET_FLAG) {
robot_.setFlag(req.pin, req.state > 0.0 ? true : false);
// According to urdriver.py, set_flag will fail if called to rapidly in succession
ros::Duration(io_flag_delay_).sleep();
}
else if (req.fun == 3)
{
//} else if (req.fun == ur_msgs::SetIO::Request::FUN_SET_ANALOG_OUT) {
robot_.setAnalogOut(req.pin, req.state);
}
else if (req.fun == 4)
{
//} else if (req.fun == ur_msgs::SetIO::Request::FUN_SET_TOOL_VOLTAGE) {
robot_.setToolVoltage((int)req.state);
}
else
{
resp.success = false;
}
return resp.success;
}
bool setPayload(ur_msgs::SetPayloadRequest& req, ur_msgs::SetPayloadResponse& resp)
{
if (robot_.setPayload(req.payload))
resp.success = true;
else
resp.success = true;
return resp.success;
}
bool validateJointNames()
{
std::vector<std::string> actual_joint_names = robot_.getJointNames();
actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal = *goal_handle_.getGoal();
if (goal.trajectory.joint_names.size() != actual_joint_names.size())
return false;
for (unsigned int i = 0; i < goal.trajectory.joint_names.size(); i++)
{
unsigned int j;
for (j = 0; j < actual_joint_names.size(); j++)
{
if (goal.trajectory.joint_names[i] == actual_joint_names[j])
break;
}
if (goal.trajectory.joint_names[i] == actual_joint_names[j])
{
actual_joint_names.erase(actual_joint_names.begin() + j);
}
else
{
return false;
}
}
return true;
}
void reorder_traj_joints(trajectory_msgs::JointTrajectory& traj)
{
/* Reorders trajectory - destructive */
std::vector<std::string> actual_joint_names = robot_.getJointNames();
std::vector<unsigned int> mapping;
mapping.resize(actual_joint_names.size(), actual_joint_names.size());
for (unsigned int i = 0; i < traj.joint_names.size(); i++)
{
for (unsigned int j = 0; j < actual_joint_names.size(); j++)
{
if (traj.joint_names[i] == actual_joint_names[j])
mapping[j] = i;
}
}
traj.joint_names = actual_joint_names;
std::vector<trajectory_msgs::JointTrajectoryPoint> new_traj;
for (unsigned int i = 0; i < traj.points.size(); i++)
{
trajectory_msgs::JointTrajectoryPoint new_point;
for (unsigned int j = 0; j < traj.points[i].positions.size(); j++)
{
new_point.positions.push_back(traj.points[i].positions[mapping[j]]);
new_point.velocities.push_back(traj.points[i].velocities[mapping[j]]);
if (traj.points[i].accelerations.size() != 0)
new_point.accelerations.push_back(traj.points[i].accelerations[mapping[j]]);
}
new_point.time_from_start = traj.points[i].time_from_start;
new_traj.push_back(new_point);
}
traj.points = new_traj;
}
bool has_velocities()
{
actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal = *goal_handle_.getGoal();
for (unsigned int i = 0; i < goal.trajectory.points.size(); i++)
{
if (goal.trajectory.points[i].positions.size() != goal.trajectory.points[i].velocities.size())
return false;
}
return true;
}
bool has_positions()
{
actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal = *goal_handle_.getGoal();
if (goal.trajectory.points.size() == 0)
return false;
for (unsigned int i = 0; i < goal.trajectory.points.size(); i++)
{
if (goal.trajectory.points[i].positions.size() != goal.trajectory.joint_names.size())
return false;
}
return true;
}
bool start_positions_match(const trajectory_msgs::JointTrajectory& traj, double eps)
{
for (unsigned int i = 0; i < traj.points[0].positions.size(); i++)
{
std::vector<double> qActual = robot_.rt_interface_->robot_state_->getQActual();
if (fabs(traj.points[0].positions[i] - qActual[i]) > eps)
{
return false;
}
}
return true;
}
bool has_limited_velocities()
{
actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal = *goal_handle_.getGoal();
for (unsigned int i = 0; i < goal.trajectory.points.size(); i++)
{
for (unsigned int j = 0; j < goal.trajectory.points[i].velocities.size(); j++)
{
if (fabs(goal.trajectory.points[i].velocities[j]) > max_velocity_)
return false;
}
}
return true;
}
bool traj_is_finite()
{
actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal = *goal_handle_.getGoal();
for (unsigned int i = 0; i < goal.trajectory.points.size(); i++)
{
for (unsigned int j = 0; j < goal.trajectory.points[i].velocities.size(); j++)
{
if (!std::isfinite(goal.trajectory.points[i].positions[j]))
return false;
if (!std::isfinite(goal.trajectory.points[i].velocities[j]))
return false;
}
}
return true;
}
void speedInterface(const trajectory_msgs::JointTrajectory::Ptr& msg)
{
if (msg->points[0].velocities.size() == 6)
{
double acc = 100;
if (msg->points[0].accelerations.size() > 0)
acc = *std::max_element(msg->points[0].accelerations.begin(), msg->points[0].accelerations.end());
robot_.setSpeed(msg->points[0].velocities[0], msg->points[0].velocities[1], msg->points[0].velocities[2],
msg->points[0].velocities[3], msg->points[0].velocities[4], msg->points[0].velocities[5], acc);
}
}
void urscriptInterface(const std_msgs::String::ConstPtr& msg)
{
robot_.rt_interface_->addCommandToQueue(msg->data);
}
void rosControlLoop()
{
ros::Duration elapsed_time;
struct timespec last_time, current_time;
static const double BILLION = 1000000000.0;
realtime_tools::RealtimePublisher<tf::tfMessage> tf_pub(nh_, "/tf", 1);
geometry_msgs::TransformStamped tool_transform;
tool_transform.header.frame_id = base_frame_;
tool_transform.child_frame_id = tool_frame_;
tf_pub.msg_.transforms.push_back(tool_transform);
realtime_tools::RealtimePublisher<geometry_msgs::TwistStamped> tool_vel_pub(nh_, "tool_velocity", 1);
tool_vel_pub.msg_.header.frame_id = base_frame_;
clock_gettime(CLOCK_MONOTONIC, &last_time);
while (ros::ok())
{
std::mutex msg_lock; // The values are locked for reading in the class, so just use a dummy mutex
std::unique_lock<std::mutex> locker(msg_lock);
while (!robot_.rt_interface_->robot_state_->getControllerUpdated())
{
rt_msg_cond_.wait(locker);
}
// Input
hardware_interface_->read();
robot_.rt_interface_->robot_state_->setControllerUpdated();
// Control
clock_gettime(CLOCK_MONOTONIC, &current_time);
elapsed_time =
ros::Duration(current_time.tv_sec - last_time.tv_sec + (current_time.tv_nsec - last_time.tv_nsec) / BILLION);
ros::Time ros_time = ros::Time::now();
controller_manager_->update(ros_time, elapsed_time);
last_time = current_time;
// Output
hardware_interface_->write();
// Tool vector: Actual Cartesian coordinates of the tool: (x,y,z,rx,ry,rz), where rx, ry and rz is a rotation
// vector representation of the tool orientation
std::vector<double> tool_vector_actual = robot_.rt_interface_->robot_state_->getToolVectorActual();
// Compute rotation angle
double rx = tool_vector_actual[3];
double ry = tool_vector_actual[4];
double rz = tool_vector_actual[5];
double angle = std::sqrt(std::pow(rx, 2) + std::pow(ry, 2) + std::pow(rz, 2));
// Broadcast transform
if (tf_pub.trylock())
{
tf_pub.msg_.transforms[0].header.stamp = ros_time;
if (angle < 1e-16)
{
tf_pub.msg_.transforms[0].transform.rotation.x = 0;
tf_pub.msg_.transforms[0].transform.rotation.y = 0;
tf_pub.msg_.transforms[0].transform.rotation.z = 0;
tf_pub.msg_.transforms[0].transform.rotation.w = 1;
}
else
{
tf_pub.msg_.transforms[0].transform.rotation.x = (rx / angle) * std::sin(angle * 0.5);
tf_pub.msg_.transforms[0].transform.rotation.y = (ry / angle) * std::sin(angle * 0.5);
tf_pub.msg_.transforms[0].transform.rotation.z = (rz / angle) * std::sin(angle * 0.5);
tf_pub.msg_.transforms[0].transform.rotation.w = std::cos(angle * 0.5);
}
tf_pub.msg_.transforms[0].transform.translation.x = tool_vector_actual[0];
tf_pub.msg_.transforms[0].transform.translation.y = tool_vector_actual[1];
tf_pub.msg_.transforms[0].transform.translation.z = tool_vector_actual[2];
tf_pub.unlockAndPublish();
}
// Publish tool velocity
std::vector<double> tcp_speed = robot_.rt_interface_->robot_state_->getTcpSpeedActual();
if (tool_vel_pub.trylock())
{
tool_vel_pub.msg_.header.stamp = ros_time;
tool_vel_pub.msg_.twist.linear.x = tcp_speed[0];
tool_vel_pub.msg_.twist.linear.y = tcp_speed[1];
tool_vel_pub.msg_.twist.linear.z = tcp_speed[2];
tool_vel_pub.msg_.twist.angular.x = tcp_speed[3];
tool_vel_pub.msg_.twist.angular.y = tcp_speed[4];
tool_vel_pub.msg_.twist.angular.z = tcp_speed[5];
tool_vel_pub.unlockAndPublish();
}
}
}
void publishRTMsg()
{
ros::Publisher joint_pub = nh_.advertise<sensor_msgs::JointState>("joint_states", 1);
ros::Publisher wrench_pub = nh_.advertise<geometry_msgs::WrenchStamped>("wrench", 1);
ros::Publisher tool_vel_pub = nh_.advertise<geometry_msgs::TwistStamped>("tool_velocity", 1);
static tf::TransformBroadcaster br;
while (ros::ok())
{
sensor_msgs::JointState joint_msg;
joint_msg.name = robot_.getJointNames();
geometry_msgs::WrenchStamped wrench_msg;
geometry_msgs::PoseStamped tool_pose_msg;
std::mutex msg_lock; // The values are locked for reading in the class, so just use a dummy mutex
std::unique_lock<std::mutex> locker(msg_lock);
while (!robot_.rt_interface_->robot_state_->getDataPublished())
{
rt_msg_cond_.wait(locker);
}
joint_msg.header.stamp = ros::Time::now();
joint_msg.position = robot_.rt_interface_->robot_state_->getQActual();
for (unsigned int i = 0; i < joint_msg.position.size(); i++)
{
joint_msg.position[i] += joint_offsets_[i];
}
joint_msg.velocity = robot_.rt_interface_->robot_state_->getQdActual();
joint_msg.effort = robot_.rt_interface_->robot_state_->getIActual();
joint_pub.publish(joint_msg);
std::vector<double> tcp_force = robot_.rt_interface_->robot_state_->getTcpForce();
wrench_msg.header.stamp = joint_msg.header.stamp;
wrench_msg.wrench.force.x = tcp_force[0];
wrench_msg.wrench.force.y = tcp_force[1];
wrench_msg.wrench.force.z = tcp_force[2];
wrench_msg.wrench.torque.x = tcp_force[3];
wrench_msg.wrench.torque.y = tcp_force[4];
wrench_msg.wrench.torque.z = tcp_force[5];
wrench_pub.publish(wrench_msg);
// Tool vector: Actual Cartesian coordinates of the tool: (x,y,z,rx,ry,rz), where rx, ry and rz is a rotation
// vector representation of the tool orientation
std::vector<double> tool_vector_actual = robot_.rt_interface_->robot_state_->getToolVectorActual();
// Create quaternion
tf::Quaternion quat;
double rx = tool_vector_actual[3];
double ry = tool_vector_actual[4];
double rz = tool_vector_actual[5];
double angle = std::sqrt(std::pow(rx, 2) + std::pow(ry, 2) + std::pow(rz, 2));
if (angle < 1e-16)
{
quat.setValue(0, 0, 0, 1);
}
else
{
quat.setRotation(tf::Vector3(rx / angle, ry / angle, rz / angle), angle);
}
// Create and broadcast transform
tf::Transform transform;
transform.setOrigin(tf::Vector3(tool_vector_actual[0], tool_vector_actual[1], tool_vector_actual[2]));
transform.setRotation(quat);
br.sendTransform(tf::StampedTransform(transform, joint_msg.header.stamp, base_frame_, tool_frame_));
// Publish tool velocity
std::vector<double> tcp_speed = robot_.rt_interface_->robot_state_->getTcpSpeedActual();
geometry_msgs::TwistStamped tool_twist;
tool_twist.header.frame_id = base_frame_;
tool_twist.header.stamp = joint_msg.header.stamp;
tool_twist.twist.linear.x = tcp_speed[0];
tool_twist.twist.linear.y = tcp_speed[1];
tool_twist.twist.linear.z = tcp_speed[2];
tool_twist.twist.angular.x = tcp_speed[3];
tool_twist.twist.angular.y = tcp_speed[4];
tool_twist.twist.angular.z = tcp_speed[5];
tool_vel_pub.publish(tool_twist);
robot_.rt_interface_->robot_state_->setDataPublished();
}
}
void publishMbMsg()
{
bool warned = false;
ros::Publisher io_pub = nh_.advertise<ur_msgs::IOStates>("ur_driver/io_states", 1);
while (ros::ok())
{
ur_msgs::IOStates io_msg;
std::mutex msg_lock; // The values are locked for reading in the class, so just use a dummy mutex
std::unique_lock<std::mutex> locker(msg_lock);
while (!robot_.sec_interface_->robot_state_->getNewDataAvailable())
{
msg_cond_.wait(locker);
}
int i_max = 10;
if (robot_.sec_interface_->robot_state_->getVersion() > 3.0)
i_max = 18; // From version 3.0, there are up to 18 inputs and outputs
for (unsigned int i = 0; i < i_max; i++)
{
ur_msgs::Digital digi;
digi.pin = i;
digi.state = ((robot_.sec_interface_->robot_state_->getDigitalInputBits() & (1 << i)) >> i);
io_msg.digital_in_states.push_back(digi);
digi.state = ((robot_.sec_interface_->robot_state_->getDigitalOutputBits() & (1 << i)) >> i);
io_msg.digital_out_states.push_back(digi);
}
ur_msgs::Analog ana;
ana.pin = 0;
ana.state = robot_.sec_interface_->robot_state_->getAnalogInput0();
io_msg.analog_in_states.push_back(ana);
ana.pin = 1;
ana.state = robot_.sec_interface_->robot_state_->getAnalogInput1();
io_msg.analog_in_states.push_back(ana);
ana.pin = 0;
ana.state = robot_.sec_interface_->robot_state_->getAnalogOutput0();
io_msg.analog_out_states.push_back(ana);
ana.pin = 1;
ana.state = robot_.sec_interface_->robot_state_->getAnalogOutput1();
io_msg.analog_out_states.push_back(ana);
io_pub.publish(io_msg);
if (robot_.sec_interface_->robot_state_->isEmergencyStopped() or
robot_.sec_interface_->robot_state_->isProtectiveStopped())
{
if (robot_.sec_interface_->robot_state_->isEmergencyStopped() and !warned)
{
print_error("Emergency stop pressed!");
}
else if (robot_.sec_interface_->robot_state_->isProtectiveStopped() and !warned)
{
print_error("Robot is protective stopped!");
}
if (has_goal_)
{
print_error("Aborting trajectory");
robot_.stopTraj();
result_.error_code = result_.SUCCESSFUL;
result_.error_string = "Robot was halted";
goal_handle_.setAborted(result_, result_.error_string);
has_goal_ = false;
}
warned = true;
}
else
warned = false;
robot_.sec_interface_->robot_state_->finishedReading();
}
}
};
int main(int argc, char** argv)
{
bool use_sim_time = false;
std::string host;
int reverse_port = 50001;
ros::init(argc, argv, "ur_driver");
ros::NodeHandle nh;
if (ros::param::get("use_sim_time", use_sim_time))
{
print_warning("use_sim_time is set!!");
}
if (!(ros::param::get("~robot_ip_address", host)))
{
if (argc > 1)
{
print_warning("Please set the parameter robot_ip_address instead of giving it as a command line argument. This "
"method is DEPRECATED");
host = argv[1];
}
else
{
print_fatal("Could not get robot ip. Please supply it as command line parameter or on the parameter server as "
"robot_ip");
exit(1);
}
}
if ((ros::param::get("~reverse_port", reverse_port)))
{
if ((reverse_port <= 0) or (reverse_port >= 65535))
{
print_warning("Reverse port value is not valid (Use number between 1 and 65534. Using default value of 50001");
reverse_port = 50001;
}
}
else
reverse_port = 50001;
RosWrapper interface(host, reverse_port);
ros::AsyncSpinner spinner(3);
spinner.start();
ros::waitForShutdown();
interface.halt();
exit(0);
}