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

Major refactor

Browse Source
This commit is contained in:
Simon Rasmussen
2017-04-27 06:40:03 +02:00
parent 46f4e493cf
commit c59bfc78cc
22 changed files with 825 additions and 423 deletions
Download Patch File Download Diff File Expand all files Collapse all files

112
src/ros/action_server.cpp
View File

@@ -15,7 +15,14 @@ ActionServer::ActionServer(TrajectoryFollower& follower, std::vector<std::string
, state_(RobotState::Error)
, follower_(follower)
{
}
void ActionServer::start()
{
if(running_)
return;
running_ = true;
tj_thread_ = thread(&ActionServer::trajectoryThread, this);
}
void ActionServer::onRobotStateChange(RobotState state)
@@ -34,7 +41,14 @@ void ActionServer::onGoal(GoalHandle gh)
void ActionServer::onCancel(GoalHandle gh)
{
interrupt_traj_ = true;
//wait for goal to be interrupted
std::lock_guard<std::mutex> lock(tj_mutex_);
Result res;
res.error_code = -100;
res.error_string = "Goal cancelled by client";
gh.setCanceled(res);
}
bool ActionServer::validate(GoalHandle& gh, Result& res)
@@ -125,9 +139,17 @@ bool ActionServer::validateTrajectory(GoalHandle& gh, Result& res)
}
}
//todo validate start position?
return true;
}
inline std::chrono::microseconds convert(const ros::Duration &dur)
{
return std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::seconds(dur.sec))
+ std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::nanoseconds(dur.nsec));
}
bool ActionServer::try_execute(GoalHandle& gh, Result& res)
{
if(!running_)
@@ -137,27 +159,20 @@ bool ActionServer::try_execute(GoalHandle& gh, Result& res)
}
if(!tj_mutex_.try_lock())
{
has_goal_ = false;
//stop_trajectory();
interrupt_traj_ = true;
res.error_string = "Received another trajectory";
curr_gh_.setAborted(res, res.error_string);
tj_mutex_.lock();
//todo: make configurable
std::this_thread::sleep_for(std::chrono::milliseconds(250));
}
//locked here
curr_gh_ = gh;
interrupt_traj_ = false;
has_goal_ = true;
tj_mutex_.unlock();
tj_cv_.notify_one();
}
inline double ActionServer::interp_cubic(double t, double T, double p0_pos, double p1_pos, double p0_vel, double p1_vel)
{
using std::pow;
double a = p0_pos;
double b = p0_vel;
double c = (-3 * a + 3 * p1_pos - 2 * T * b - T * p1_vel) / pow(T, 2);
double d = (2 * a - 2 * p1_pos + T * b + T * p1_vel) / pow(T, 3);
return a + b * t + c * pow(t, 2) + d * pow(t, 3);
return true;
}
std::vector<size_t> ActionServer::reorderMap(std::vector<std::string> goal_joints)
@@ -179,53 +194,56 @@ std::vector<size_t> ActionServer::reorderMap(std::vector<std::string> goal_joint
void ActionServer::trajectoryThread()
{
follower_.start(); //todo check error
//as_.start();
while(running_)
{
std::unique_lock<std::mutex> lk(tj_mutex_);
if(!tj_cv_.wait_for(lk, std::chrono::milliseconds(100), [&]{return running_ && has_goal_;}))
continue;
auto g = curr_gh_.getGoal();
auto const& traj = g->trajectory;
auto const& points = traj.points;
size_t len = points.size();
auto const& last_point = points[points.size() - 1];
double end_time = last_point.time_from_start.toSec();
LOG_DEBUG("Trajectory received and accepted");
curr_gh_.setAccepted();
auto goal = curr_gh_.getGoal();
auto mapping = reorderMap(goal->trajectory.joint_names);
std::vector<TrajectoryPoint> trajectory(goal->trajectory.points.size());
auto mapping = reorderMap(traj.joint_names);
std::chrono::high_resolution_clock::time_point t0, t;
t = t0 = std::chrono::high_resolution_clock::now();
size_t i = 0;
while(end_time >= toSec(t - t0) && has_goal_)
for(auto const& point : goal->trajectory.points)
{
while(points[i].time_from_start.toSec() <= toSec(t - t0) && i < len)
i++;
auto const& pp = points[i-1];
auto const& p = points[i];
auto pp_t = pp.time_from_start.toSec();
auto p_t =p.time_from_start.toSec();
std::array<double, 6> pos;
for(size_t j = 0; j < pos.size(); j++)
std::array<double, 6> pos, vel;
for(size_t i = 0; i < 6; i++)
{
pos[i] = interp_cubic(
toSec(t - t0) - pp_t,
p_t - pp_t,
pp.positions[j],
p.positions[j],
pp.velocities[j],
p.velocities[j]
);
//joint names of the goal might have a different ordering compared
//to what URScript expects so need to map between the two
size_t idx = mapping[i];
pos[idx] = point.positions[i];
vel[idx] = point.velocities[i];
}
trajectory.push_back(TrajectoryPoint(pos, vel, convert(point.time_from_start)));
}
follower_.execute(pos);
//std::this_thread::sleep_for(std::chrono::milliseconds((int)((servoj_time_ * 1000) / 4.)));
t = std::chrono::high_resolution_clock::now();
Result res;
if(follower_.execute(trajectory, interrupt_traj_))
{
//interrupted goals must be handled by interrupt trigger
if(!interrupt_traj_)
{
LOG_DEBUG("Trajectory executed successfully");
res.error_code = Result::SUCCESSFUL;
curr_gh_.setSucceeded(res);
}
}
else
{
LOG_DEBUG("Trajectory failed");
res.error_code = -100;
res.error_string = "Connection to robot was lost";
curr_gh_.setAborted(res, res.error_string);
}
has_goal_ = false;
lk.unlock();
}
follower_.stop();
}

29
src/ros/controller.cpp
View File

@@ -20,6 +20,8 @@ void ROSController::setupConsumer()
void ROSController::doSwitch(const std::list<hardware_interface::ControllerInfo>& start_list, const std::list<hardware_interface::ControllerInfo>& stop_list)
{
LOG_INFO("Switching hardware interface");
if (active_interface_ != nullptr && stop_list.size() > 0)
{
LOG_INFO("Stopping active interface");
@@ -54,6 +56,14 @@ bool ROSController::write()
return active_interface_->write();
}
void ROSController::reset()
{
if (active_interface_ == nullptr)
return;
active_interface_->reset();
}
void ROSController::read(RTShared& packet)
{
joint_interface_.update(packet);
@@ -68,17 +78,32 @@ bool ROSController::update(RTShared& state)
lastUpdate_ = time;
read(state);
controller_.update(time, diff);
controller_.update(time, diff, !service_enabled_);
//emergency stop and such should not kill the pipeline
//but still prevent writes
if(!service_enabled_)
{
reset();
return true;
}
//allow the controller to update x times before allowing writes again
if(service_cooldown_ > 0)
{
service_cooldown_ -= 1;
return true;
}
return write();
}
void ROSController::onRobotStateChange(RobotState state)
{
service_enabled_ = (state == RobotState::Running);
bool next = (state == RobotState::Running);
if(next == service_enabled_)
return;
service_enabled_ = next;
service_cooldown_ = 125;
}

66
src/ros/hardware_interface.cpp
View File

@@ -1,61 +1,69 @@
#include "ur_modern_driver/ros/hardware_interface.h"
#include "ur_modern_driver/log.h"
JointInterface::JointInterface(std::vector<std::string> &joint_names)
{
for (size_t i = 0; i < 6; i++)
{
registerHandle(hardware_interface::JointStateHandle(joint_names[i], &positions_[i], &velocities_[i], &efforts_[i]));
}
for (size_t i = 0; i < 6; i++)
{
registerHandle(hardware_interface::JointStateHandle(joint_names[i], &positions_[i], &velocities_[i], &efforts_[i]));
}
}
void JointInterface::update(RTShared &packet)
{
positions_ = packet.q_actual;
velocities_ = packet.qd_actual;
efforts_ = packet.i_actual;
positions_ = packet.q_actual;
velocities_ = packet.qd_actual;
efforts_ = packet.i_actual;
}
WrenchInterface::WrenchInterface()
{
registerHandle(hardware_interface::ForceTorqueSensorHandle("wrench", "", tcp_.begin(), tcp_.begin() + 3));
}
registerHandle(hardware_interface::ForceTorqueSensorHandle("wrench", "", tcp_.begin(), tcp_.begin() + 3));
}
void WrenchInterface::update(RTShared &packet)
{
tcp_ = packet.tcp_force;
tcp_ = packet.tcp_force;
}
VelocityInterface::VelocityInterface(URCommander &commander, hardware_interface::JointStateInterface &js_interface, std::vector<std::string> &joint_names, double max_vel_change)
: commander_(commander), max_vel_change_(max_vel_change)
: commander_(commander), max_vel_change_(max_vel_change)
{
for (size_t i = 0; i < 6; i++)
{
registerHandle(JointHandle(js_interface.getHandle(joint_names[i]), &velocity_cmd_[i]));
}
for (size_t i = 0; i < 6; i++)
{
registerHandle(JointHandle(js_interface.getHandle(joint_names[i]), &velocity_cmd_[i]));
}
}
bool VelocityInterface::write()
{
for (size_t i = 0; i < 6; i++)
{
double prev = prev_velocity_cmd_[i];
double lo = prev - max_vel_change_;
double hi = prev + max_vel_change_;
// clamp value to ±max_vel_change
prev_velocity_cmd_[i] = std::max(lo, std::min(velocity_cmd_[i], hi));
}
for (size_t i = 0; i < 6; i++)
{
// clamp value to ±max_vel_change
double prev = prev_velocity_cmd_[i];
double lo = prev - max_vel_change_;
double hi = prev + max_vel_change_;
prev_velocity_cmd_[i] = std::max(lo, std::min(velocity_cmd_[i], hi));
}
return commander_.speedj(prev_velocity_cmd_, max_vel_change_);
}
return commander_.speedj(prev_velocity_cmd_, max_vel_change_);
void VelocityInterface::reset()
{
for(auto &val : prev_velocity_cmd_)
{
val = 0;
}
}
PositionInterface:: PositionInterface(URCommander &commander, hardware_interface::JointStateInterface &js_interface, std::vector<std::string> &joint_names)
: commander_(commander)
: commander_(commander)
{
for (size_t i = 0; i < 6; i++)
{
registerHandle(JointHandle(js_interface.getHandle(joint_names[i]), &position_cmd_[i]));
}
for (size_t i = 0; i < 6; i++)
{
registerHandle(JointHandle(js_interface.getHandle(joint_names[i]), &position_cmd_[i]));
}
}
bool PositionInterface::write()

226
src/ros/trajectory_follower.cpp
View File

@@ -1,83 +1,96 @@
#include <endian.h>
#include "ur_modern_driver/ros/trajectory_follower.h"
TrajectoryFollower::TrajectoryFollower(URCommander &commander, int reverse_port, bool version_3)
: running_(false)
, commander_(commander)
, server_(reverse_port)
, program_(buildProgram(version_3))
{
}
static const int32_t MULT_JOINTSTATE_ = 1000000;
static const std::string JOINT_STATE_REPLACE("{{JOINT_STATE_REPLACE}}");
static const std::string SERVO_J_REPLACE("{{SERVO_J_REPLACE}}");
static const std::string SERVER_IP_REPLACE("{{SERVER_IP_REPLACE}}");
static const std::string SERVER_PORT_REPLACE("{{SERVER_PORT_REPLACE}}");
static const std::string POSITION_PROGRAM = R"(
def driverProg():
MULT_jointstate = {{JOINT_STATE_REPLACE}}
MULT_jointstate = {{JOINT_STATE_REPLACE}}
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, {{SERVO_J_REPLACE}})
else:
sync()
end
end
end
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, {{SERVO_J_REPLACE}})
else:
sync()
end
end
end
socket_open(\"{{SERVER_IP_REPLACE}}\", {{SERVER_PORT_REPLACE}})
thread_servo = run servoThread()
keepalive = 1
while keepalive > 0:
params_mult = socket_read_binary_integer(6+1)
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]
keepalive = params_mult[7]
set_servo_setpoint(q)
end
params_mult = socket_read_binary_integer(6+1)
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]
keepalive = params_mult[7]
set_servo_setpoint(q)
end
end
sleep(.1)
socket_close()
kill thread_servo
end
)";
std::string TrajectoryFollower::buildProgram(bool version_3)
TrajectoryFollower::TrajectoryFollower(URCommander &commander, int reverse_port, bool version_3)
: running_(false)
, commander_(commander)
, reverse_port_(reverse_port)
, server_(reverse_port)
{
std::string res(POSITION_PROGRAM);
size_t js_idx = POSITION_PROGRAM.find(JOINT_STATE_REPLACE);
size_t sj_idx = POSITION_PROGRAM.find(SERVO_J_REPLACE);
res.replace(res.find(JOINT_STATE_REPLACE), JOINT_STATE_REPLACE.length(), std::to_string(MULT_JOINTSTATE_));
std::ostringstream out;
out << "t=" << std::fixed << std::setprecision(4) << servoj_time_;
if(version_3)
out << ", lookahead_time=" << servoj_lookahead_time_ << ", gain=" << servoj_gain_;
res.replace(js_idx, JOINT_STATE_REPLACE.length(), std::to_string(MULT_JOINTSTATE_));
res.replace(sj_idx, SERVO_J_REPLACE.length(), out.str());
res.replace(res.find(SERVO_J_REPLACE), SERVO_J_REPLACE.length(), out.str());
program_ = res;
}
std::string TrajectoryFollower::buildProgram()
{
std::string res(program_);
std::string IP(server_.getIP());
LOG_INFO("Local IP: %s ", IP.c_str());
res.replace(res.find(SERVER_IP_REPLACE), SERVER_IP_REPLACE.length(), "127.0.0.1");
res.replace(res.find(SERVER_PORT_REPLACE), SERVER_PORT_REPLACE.length(), std::to_string(reverse_port_));
return res;
}
@@ -86,12 +99,31 @@ bool TrajectoryFollower::start()
if(running_)
return true; //not sure
//TODO
std::string prog(""); // buildProg();
if(!commander_.uploadProg(prog))
if(!server_.bind())
{
LOG_ERROR("Failed to bind server");
return false;
}
stream_ = std::move(server_.accept()); //todo: pointer instead?
LOG_INFO("Uploading trajectory program to robot");
std::string prog(buildProgram());
//std::string prog = "socket_open(\"127.0.0.1\", 50001)\n";
if(!commander_.uploadProg(prog))
{
LOG_ERROR("Program upload failed!");
return false;
}
LOG_INFO("Awaiting incomming robot connection");
if(!server_.accept())
{
LOG_ERROR("Failed to accept incomming robot connection");
return false;
}
LOG_INFO("Robot successfully connected");
return (running_ = true);
}
@@ -115,8 +147,18 @@ bool TrajectoryFollower::execute(std::array<double, 6> &positions, bool keep_ali
int32_t val = htobe32(static_cast<int32_t>(keep_alive));
append(idx, val);
ssize_t res = stream_.send(buf, sizeof(buf));
return res > 0 && res == sizeof(buf);
size_t written;
return server_.write(buf, sizeof(buf), written);
}
double TrajectoryFollower::interpolate(double t, double T, double p0_pos, double p1_pos, double p0_vel, double p1_vel)
{
using std::pow;
double a = p0_pos;
double b = p0_vel;
double c = (-3 * a + 3 * p1_pos - 2 * T * b - T * p1_vel) / pow(T, 2);
double d = (2 * a - 2 * p1_pos + T * b + T * p1_vel) / pow(T, 3);
return a + b * t + c * pow(t, 2) + d * pow(t, 3);
}
bool TrajectoryFollower::execute(std::array<double, 6> &positions)
@@ -124,6 +166,68 @@ bool TrajectoryFollower::execute(std::array<double, 6> &positions)
return execute(positions, true);
}
bool TrajectoryFollower::execute(std::vector<TrajectoryPoint> &trajectory, std::atomic<bool> &interrupt)
{
if(!running_)
return false;
using namespace std::chrono;
typedef duration<double> double_seconds;
typedef high_resolution_clock Clock;
typedef Clock::time_point Time;
auto const& last = trajectory[trajectory.size()-1];
auto& prev = trajectory[0];
Time t0 = Clock::now();
Time latest = t0;
std::array<double, 6> positions;
for(auto const& point : trajectory)
{
//skip t0
if(&point == &prev)
continue;
auto duration = point.time_from_start - prev.time_from_start;
double d_s = duration_cast<double_seconds>(duration).count();
//interpolation loop
while(!interrupt)
{
latest = Clock::now();
auto elapsed = latest - t0;
if(point.time_from_start <= elapsed || last.time_from_start >= elapsed)
break;
double elapsed_s = duration_cast<double_seconds>(elapsed - prev.time_from_start).count();
//double prev_seconds
for(size_t j = 0; j < positions.size(); j++)
{
positions[j] = interpolate(
elapsed_s,
d_s,
prev.positions[j],
point.positions[j],
prev.velocities[j],
point.velocities[j]
);
}
if(!execute(positions, true))
return false;
std::this_thread::sleep_for(double_seconds(servoj_time_));
}
prev = point;
}
return true;
}
void TrajectoryFollower::stop()
{
if(!running_)
@@ -132,6 +236,6 @@ void TrajectoryFollower::stop()
std::array<double, 6> empty;
execute(empty, false);
stream_.disconnect();
//server_.disconnect();
running_ = false;
}

28
src/ros_main.cpp
View File

@@ -6,10 +6,12 @@
#include "ur_modern_driver/log.h"
#include "ur_modern_driver/pipeline.h"
#include "ur_modern_driver/ros/action_server.h"
#include "ur_modern_driver/ros/controller.h"
#include "ur_modern_driver/ros/io_service.h"
#include "ur_modern_driver/ros/mb_publisher.h"
#include "ur_modern_driver/ros/controller.h"
#include "ur_modern_driver/ros/rt_publisher.h"
#include "ur_modern_driver/ros/trajectory_follower.h"
#include "ur_modern_driver/ros/service_stopper.h"
#include "ur_modern_driver/ur/commander.h"
#include "ur_modern_driver/ur/factory.h"
@@ -63,7 +65,7 @@ bool parse_args(ProgArgs &args)
return true;
}
#include "ur_modern_driver/event_counter.h"
#include "ur_modern_driver/ur/server.h"
int main(int argc, char **argv)
{
@@ -75,6 +77,7 @@ int main(int argc, char **argv)
return EXIT_FAILURE;
}
URFactory factory(args.host);
vector<Service*> services;
@@ -84,17 +87,27 @@ int main(int argc, char **argv)
URStream rt_stream(args.host, UR_RT_PORT);
URProducer<RTPacket> rt_prod(rt_stream, *rt_parser);
RTPublisher rt_pub(args.prefix, args.base_frame, args.tool_frame, args.use_ros_control);
URCommander rt_commander(rt_stream);
auto rt_commander = factory.getCommander(rt_stream);
vector<IConsumer<RTPacket> *> rt_vec{&rt_pub};
TrajectoryFollower traj_follower(*rt_commander, args.reverse_port, factory.isVersion3());
ROSController *controller(nullptr);
ActionServer *action_server(nullptr);
if (args.use_ros_control)
{
LOG_INFO("ROS control enabled");
controller = new ROSController(rt_commander, args.joint_names, args.max_vel_change);
controller = new ROSController(*rt_commander, args.joint_names, args.max_vel_change);
rt_vec.push_back(controller);
services.push_back(controller);
}
else
{
LOG_INFO("ActionServer enabled");
action_server = new ActionServer(traj_follower, args.joint_names, args.max_velocity);
//rt_vec.push_back(action_server);
services.push_back(action_server);
}
MultiConsumer<RTPacket> rt_cons(rt_vec);
Pipeline<RTPacket> rt_pl(rt_prod, rt_cons);
@@ -116,8 +129,11 @@ int main(int argc, char **argv)
rt_pl.run();
state_pl.run();
URCommander state_commander(state_stream);
IOService io_service(state_commander);
auto state_commander = factory.getCommander(state_stream);
IOService io_service(*state_commander);
if(action_server)
action_server->start();
ros::spin();

133
src/tcp_socket.cpp Normal file
View File

@@ -0,0 +1,133 @@
#include <endian.h>
#include <netinet/tcp.h>
#include <unistd.h>
#include <cstring>
#include "ur_modern_driver/log.h"
#include "ur_modern_driver/tcp_socket.h"
TCPSocket::TCPSocket()
: socket_fd_(-1)
, state_(SocketState::Invalid)
{
}
TCPSocket::~TCPSocket()
{
close();
}
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
{
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;
::shutdown(socket_fd_, SHUT_RDWR);
socket_fd_ = -1;
}
bool TCPSocket::read(uint8_t *buf, 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, 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;
}

164
src/ur/commander.cpp
View File

@@ -3,10 +3,21 @@
bool URCommander::write(std::string& s)
{
size_t len = s.size();
const uint8_t* data = reinterpret_cast<const uint8_t*>(s.c_str());
ssize_t res = stream_.send(data, len);
return res > 0 && static_cast<size_t>(res) == len;
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(std::string &s)
@@ -14,59 +25,110 @@ bool URCommander::uploadProg(std::string &s)
return write(s);
}
bool URCommander::speedj(std::array<double, 6> &speeds, double acceleration)
{
std::ostringstream out;
out << std::fixed << std::setprecision(4);
out << "speedj([";
std::string mod;
for(auto const& val : speeds)
{
out << mod << val;
mod = ",";
}
out << "]," << acceleration << ")\n";
std::string s(out.str());
return write(s);
}
bool URCommander::stopj(double a)
{
}
bool URCommander::setAnalogOut(uint8_t pin, double value)
{
std::ostringstream out;
out << "set_analog_out(" << (int)pin << "," << std::fixed << std::setprecision(4) << value << ")\n";
std::string s(out.str());
return write(s);
}
bool URCommander::setDigitalOut(uint8_t pin, bool value)
{
std::ostringstream out;
out << "set_digital_out(" << (int)pin << "," << (value ? "True" : "False") << ")\n";
std::string s(out.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);
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(4) << value << ")\n";
std::string s(out.str());
return write(s);
}
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 << "set_analog_out(" << (int)pin << "," << std::fixed << std::setprecision(4) << value << ")\n";
std::string s(out.str());
return write(s);
}
bool URCommander_V1_X::setDigitalOut(uint8_t pin, bool value)
{
std::ostringstream out;
out << "set_digital_out(" << (int)pin << "," << (value ? "True" : "False") << ")\n";
std::string s(out.str());
return write(s);
}
bool URCommander_V3_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 << ")\n";
std::string s(out.str());
return write(s);
}
bool URCommander_V3_X::setAnalogOut(uint8_t pin, double value)
{
std::ostringstream out;
out << "set_standard_analog_out(" << (int)pin << "," << std::fixed << std::setprecision(5) << value << ")\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 << func << "(" << (int)pin << "," << (value ? "True" : "False") << ")\n";
std::string s(out.str());
return write(s);
}

75
src/ur/server.cpp
View File

@@ -5,47 +5,48 @@
#include "ur_modern_driver/ur/server.h"
URServer::URServer(int port)
: port_(port)
{
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(nullptr, service.c_str(), &hints, &result) != 0)
{
LOG_ERROR("Failed to setup recieving server");
return;
}
// 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) // socket error?
continue;
if (bind(socket_fd_, p->ai_addr, p->ai_addrlen) != 0)
continue;
// disable Nagle's algorithm to ensure we sent packets as fast as possible
int flag = 1;
setsockopt(socket_fd_, IPPROTO_TCP, TCP_NODELAY, &flag, sizeof(flag));
LOG_INFO("Server awaiting connection");
return;
}
LOG_ERROR("Failed to setup recieving server");
std::exit(EXIT_FAILURE);
}
URStream URServer::accept()
std::string URServer::getIP()
{
char buf[128];
int res = ::gethostname(buf, sizeof(buf));
return std::string(buf);
}
bool URServer::bind()
{
std::string empty;
bool res = TCPSocket::setup(empty, port_);
state_ = TCPSocket::getState();
if(!res)
return false;
if(::listen(getSocketFD(), 1) < 0)
return false;
return true;
}
bool URServer::accept()
{
if(state_ != SocketState::Connected || client_.getSocketFD() > 0)
return false;
struct sockaddr addr;
socklen_t addr_len;
int client_fd = ::accept(socket_fd_, &addr, &addr_len);
return URStream(client_fd);
int client_fd = ::accept(getSocketFD(), &addr, &addr_len);
if(client_fd <= 0)
return false;
return client_.setSocketFD(client_fd);
}
bool URServer::write(const uint8_t* buf, size_t buf_len, size_t &written)
{
return client_.write(buf, buf_len, written);
}

129
src/ur/stream.cpp
View File

@@ -6,139 +6,38 @@
#include "ur_modern_driver/log.h"
#include "ur_modern_driver/ur/stream.h"
bool URStream::connect()
bool URStream::write(const uint8_t* buf, size_t buf_len, size_t &written)
{
if (initialized_)
return false;
LOG_INFO("Connecting to UR @ %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
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_.c_str(), service.c_str(), &hints, &result) != 0)
{
LOG_ERROR("Failed to get host name");
return 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) // socket error?
continue;
if (::connect(socket_fd_, p->ai_addr, p->ai_addrlen) != 0)
{
if (stopping_)
break;
else
continue; // try next addrinfo if connect fails
}
// disable Nagle's algorithm to ensure we sent packets as fast as possible
int flag = 1;
setsockopt(socket_fd_, IPPROTO_TCP, TCP_NODELAY, &flag, sizeof(flag));
initialized_ = true;
LOG_INFO("Connection successfully established");
break;
}
freeaddrinfo(result);
if (!initialized_)
LOG_ERROR("Connection failed");
return initialized_;
std::lock_guard<std::mutex> lock(write_mutex_);
return TCPSocket::write(buf, buf_len, written);
}
void URStream::disconnect()
bool URStream::read(uint8_t* buf, size_t buf_len, size_t &total)
{
if (!initialized_ || stopping_)
return;
std::lock_guard<std::mutex> lock(read_mutex_);
LOG_INFO("Disconnecting from %s:%d", host_.c_str(), port_);
stopping_ = true;
close(socket_fd_);
initialized_ = false;
}
void URStream::reconnect()
{
disconnect();
stopping_ = false;
connect();
}
ssize_t URStream::send(const uint8_t* buf, size_t buf_len)
{
if (!initialized_)
return -1;
if (stopping_)
return 0;
std::lock_guard<std::mutex> lock(send_mutex_);
size_t total = 0;
size_t remaining = buf_len;
// TODO: handle reconnect?
// handle partial sends
while (total < buf_len)
{
ssize_t sent = ::send(socket_fd_, buf + total, remaining, 0);
if (sent <= 0)
return stopping_ ? 0 : sent;
total += sent;
remaining -= sent;
}
return total;
}
ssize_t URStream::receive(uint8_t* buf, size_t buf_len)
{
if (!initialized_)
return -1;
if (stopping_)
return 0;
std::lock_guard<std::mutex> lock(receive_mutex_);
size_t remainder = sizeof(int32_t);
uint8_t* buf_pos = buf;
bool initial = true;
uint8_t* buf_pos = buf;
size_t remainder = sizeof(int32_t);
size_t read = 0;
do
while(remainder > 0 && TCPSocket::read(buf_pos, remainder, read))
{
ssize_t read = recv(socket_fd_, buf_pos, remainder, 0);
if (read <= 0) // failed reading from socket
return stopping_ ? 0 : read;
if (initial)
{
remainder = be32toh(*(reinterpret_cast<int32_t*>(buf)));
if (remainder >= (buf_len - sizeof(int32_t)))
{
LOG_ERROR("Packet size %zd is larger than buffer %zu, discarding.", remainder, buf_len);
return -1;
return false;
}
initial = false;
}
total += read;
buf_pos += read;
remainder -= read;
} while (remainder > 0);
return buf_pos - buf;
}
return remainder == 0;
}