From f35f40b45d1163344968531aabca1ce539aa35db Mon Sep 17 00:00:00 2001 From: Simon Rasmussen Date: Fri, 7 Jul 2017 15:26:39 +0200 Subject: [PATCH] WIP --- include/ur_modern_driver/bin_parser.h | 2 +- .../ros/trajectory_follower.h | 3 +- include/ur_modern_driver/tcp_socket.h | 6 ++- include/ur_modern_driver/ur/producer.h | 4 ++ include/ur_modern_driver/ur/server.h | 2 +- include/ur_modern_driver/ur/state_parser.h | 8 +-- include/ur_modern_driver/ur/stream.h | 2 +- src/ros/action_server.cpp | 54 ++++++++++++------- src/ros/trajectory_follower.cpp | 50 ++++++----------- src/ros_main.cpp | 12 +++-- src/tcp_socket.cpp | 18 +++++++ src/ur/server.cpp | 24 +++++++-- 12 files changed, 114 insertions(+), 71 deletions(-) diff --git a/include/ur_modern_driver/bin_parser.h b/include/ur_modern_driver/bin_parser.h index dbf6df5..2cfce45 100644 --- a/include/ur_modern_driver/bin_parser.h +++ b/include/ur_modern_driver/bin_parser.h @@ -68,7 +68,7 @@ public: template T peek() { - assert(buf_pos_ <= buf_end_); + assert(buf_pos_ + sizeof(T) <= buf_end_); T val; std::memcpy(&val, buf_pos_, sizeof(T)); return decode(val); diff --git a/include/ur_modern_driver/ros/trajectory_follower.h b/include/ur_modern_driver/ros/trajectory_follower.h index 74588af..be59a56 100644 --- a/include/ur_modern_driver/ros/trajectory_follower.h +++ b/include/ur_modern_driver/ros/trajectory_follower.h @@ -49,12 +49,11 @@ private: return s; } - std::string buildProgram(); bool execute(std::array &positions, bool keep_alive); double interpolate(double t, double T, double p0_pos, double p1_pos, double p0_vel, double p1_vel); public: - TrajectoryFollower(URCommander &commander, int reverse_port, bool version_3); + TrajectoryFollower(URCommander &commander, std::string& reverse_ip, int reverse_port, bool version_3); bool start(); bool execute(std::array &positions); diff --git a/include/ur_modern_driver/tcp_socket.h b/include/ur_modern_driver/tcp_socket.h index 9822424..4230ac0 100644 --- a/include/ur_modern_driver/tcp_socket.h +++ b/include/ur_modern_driver/tcp_socket.h @@ -29,8 +29,6 @@ protected: bool setup(std::string &host, int port); - void close(); - public: TCPSocket(); @@ -41,6 +39,10 @@ public: int getSocketFD() { return socket_fd_; } bool setSocketFD(int socket_fd); + std::string getIP(); + bool read(uint8_t* buf, size_t buf_len, size_t &read); bool write(const uint8_t* buf, size_t buf_len, size_t &written); + + void close(); }; diff --git a/include/ur_modern_driver/ur/producer.h b/include/ur_modern_driver/ur/producer.h index b3bb475..df4a61a 100644 --- a/include/ur_modern_driver/ur/producer.h +++ b/include/ur_modern_driver/ur/producer.h @@ -50,6 +50,10 @@ public: LOG_WARN("Failed to read from stream, reconnecting in %ld seconds...", timeout_.count()); std::this_thread::sleep_for(timeout_); + + if(stream_.connect()) + continue; + auto next = timeout_ * 2; if(next <= std::chrono::seconds(120)) timeout_ = next; diff --git a/include/ur_modern_driver/ur/server.h b/include/ur_modern_driver/ur/server.h index c5c8412..daff4d3 100644 --- a/include/ur_modern_driver/ur/server.h +++ b/include/ur_modern_driver/ur/server.h @@ -12,7 +12,6 @@ class URServer : private TCPSocket { private: int port_; - SocketState state_; TCPSocket client_; protected: @@ -28,5 +27,6 @@ public: std::string getIP(); bool bind(); bool accept(); + void disconnectClient(); bool write(const uint8_t* buf, size_t buf_len, size_t &written); }; \ No newline at end of file diff --git a/include/ur_modern_driver/ur/state_parser.h b/include/ur_modern_driver/ur/state_parser.h index 1e93314..494558f 100644 --- a/include/ur_modern_driver/ur/state_parser.h +++ b/include/ur_modern_driver/ur/state_parser.h @@ -34,7 +34,7 @@ public: bp.parse(type); //quietly ignore the intial version message - if (type == message_type::ROBOT_MESSAGE) + if (type == message_type::ROBOT_MESSAGE || ((int)type) == 5) { bp.consume(); return true; @@ -42,7 +42,7 @@ public: if (type != message_type::ROBOT_STATE) { - LOG_WARN("Invalid message type recieved: %u", static_cast(type)); + LOG_WARN("Invalid state message type recieved: %u", static_cast(type)); return false; } @@ -60,7 +60,7 @@ public: return false; } - LOG_DEBUG("sub-packet size: %" PRIu32, sub_size); + //LOG_DEBUG("sub-packet size: %" PRIu32, sub_size); // deconstruction of a sub parser will increment the position of the parent parser BinParser sbp(bp, sub_size); @@ -73,7 +73,7 @@ public: if (packet == nullptr) { sbp.consume(); - LOG_DEBUG("Skipping sub-packet of type %d", type); + //LOG_DEBUG("Skipping sub-packet of type %d", type); continue; } diff --git a/include/ur_modern_driver/ur/stream.h b/include/ur_modern_driver/ur/stream.h index 60619a8..3d5bc81 100644 --- a/include/ur_modern_driver/ur/stream.h +++ b/include/ur_modern_driver/ur/stream.h @@ -8,7 +8,7 @@ #include "ur_modern_driver/log.h" #include "ur_modern_driver/tcp_socket.h" -class URStream : private TCPSocket +class URStream : public TCPSocket { private: std::string host_; diff --git a/src/ros/action_server.cpp b/src/ros/action_server.cpp index 1ec7217..fc3f7ad 100644 --- a/src/ros/action_server.cpp +++ b/src/ros/action_server.cpp @@ -51,12 +51,12 @@ void ActionServer::onRobotStateChange(RobotState state) } interrupt_traj_ = true; - //wait for goal to be interrupted + //wait for goal to be interrupted and automagically unlock when going out of scope std::lock_guard lock(tj_mutex_); Result res; res.error_code = -100; - res.error_string = "Received another trajectory"; + res.error_string = "Robot safety stop"; curr_gh_.setAborted(res, res.error_string); } @@ -212,8 +212,8 @@ bool ActionServer::validateTrajectory(GoalHandle& gh, Result& res) inline std::chrono::microseconds convert(const ros::Duration &dur) { - return std::chrono::duration_cast(std::chrono::seconds(dur.sec)) - + std::chrono::duration_cast(std::chrono::nanoseconds(dur.nsec)); + return std::chrono::duration_cast(std::chrono::seconds(dur.sec) + + std::chrono::nanoseconds(dur.nsec)); } bool ActionServer::try_execute(GoalHandle& gh, Result& res) @@ -261,7 +261,7 @@ std::vector ActionServer::reorderMap(std::vector goal_joint void ActionServer::trajectoryThread() { LOG_INFO("Trajectory thread started"); - follower_.start(); //todo check error + while(running_) { std::unique_lock lk(tj_mutex_); @@ -272,7 +272,8 @@ void ActionServer::trajectoryThread() curr_gh_.setAccepted(); auto goal = curr_gh_.getGoal(); - std::vector trajectory(goal->trajectory.points.size()); + std::vector trajectory; + trajectory.reserve(goal->trajectory.points.size()+1); //joint names of the goal might have a different ordering compared //to what URScript expects so need to map between the two @@ -281,7 +282,7 @@ void ActionServer::trajectoryThread() LOG_INFO("Translating trajectory"); auto const& fp = goal->trajectory.points[0]; - auto fpt = convert(fp.time_from_start) + auto fpt = convert(fp.time_from_start); //make sure we have a proper t0 position if(fpt > std::chrono::microseconds(0)) @@ -290,6 +291,7 @@ void ActionServer::trajectoryThread() trajectory.push_back(TrajectoryPoint(q_actual_, qd_actual_, std::chrono::microseconds(0))); } + int j = 0; for(auto const& point : goal->trajectory.points) { std::array pos, vel; @@ -299,35 +301,49 @@ void ActionServer::trajectoryThread() pos[idx] = point.positions[i]; vel[idx] = point.velocities[i]; } - trajectory.push_back(TrajectoryPoint(pos, vel, convert(point.time_from_start))); + auto t = convert(point.time_from_start); + LOG_DEBUG("P[%d] = %ds, %dns -> %dus", j++, point.time_from_start.sec, point.time_from_start.nsec, t.count()); + trajectory.push_back(TrajectoryPoint(pos, vel, t)); } double t = std::chrono::duration_cast>(trajectory[trajectory.size()-1].time_from_start).count(); - LOG_INFO("Trajectory with %d points and duration of %f constructed, executing now", trajectory.size(), t); + LOG_INFO("Executing trajectory with %d points and duration of %4.3fs", trajectory.size(), t); Result res; - if(follower_.execute(trajectory, interrupt_traj_)) + + if(follower_.start()) { - //interrupted goals must be handled by interrupt trigger - if(!interrupt_traj_) + if(follower_.execute(trajectory, interrupt_traj_)) { - LOG_INFO("Trajectory executed successfully"); - res.error_code = Result::SUCCESSFUL; - curr_gh_.setSucceeded(res); + //interrupted goals must be handled by interrupt trigger + if(!interrupt_traj_) + { + LOG_INFO("Trajectory executed successfully"); + res.error_code = Result::SUCCESSFUL; + curr_gh_.setSucceeded(res); + } + else + LOG_INFO("Trajectory interrupted"); } else - LOG_INFO("Trajectory interrupted"); + { + LOG_INFO("Trajectory failed"); + res.error_code = -100; + res.error_string = "Connection to robot was lost"; + curr_gh_.setAborted(res, res.error_string); + } + + follower_.stop(); } else { - LOG_INFO("Trajectory failed"); + LOG_ERROR("Failed to start trajectory follower!"); res.error_code = -100; - res.error_string = "Connection to robot was lost"; + res.error_string = "Robot connection could not be established"; curr_gh_.setAborted(res, res.error_string); } has_goal_ = false; lk.unlock(); } - follower_.stop(); } \ No newline at end of file diff --git a/src/ros/trajectory_follower.cpp b/src/ros/trajectory_follower.cpp index c6752b1..099d84a 100644 --- a/src/ros/trajectory_follower.cpp +++ b/src/ros/trajectory_follower.cpp @@ -65,7 +65,7 @@ def driverProg(): end )"; -TrajectoryFollower::TrajectoryFollower(URCommander &commander, int reverse_port, bool version_3) +TrajectoryFollower::TrajectoryFollower(URCommander &commander, std::string& reverse_ip, int reverse_port, bool version_3) : running_(false) , commander_(commander) , reverse_port_(reverse_port) @@ -75,7 +75,6 @@ TrajectoryFollower::TrajectoryFollower(URCommander &commander, int reverse_port, , server_(reverse_port) { std::string res(POSITION_PROGRAM); - res.replace(res.find(JOINT_STATE_REPLACE), JOINT_STATE_REPLACE.length(), std::to_string(MULT_JOINTSTATE_)); std::ostringstream out; @@ -85,17 +84,16 @@ TrajectoryFollower::TrajectoryFollower(URCommander &commander, int reverse_port, res.replace(res.find(SERVO_J_REPLACE), SERVO_J_REPLACE.length(), out.str()); - program_ = res; -} + if(!server_.bind()) + { + LOG_ERROR("Failed to bind server, the port %d is likely already in use", reverse_port); + std::exit(-1); + } -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_IP_REPLACE), SERVER_IP_REPLACE.length(), reverse_ip); res.replace(res.find(SERVER_PORT_REPLACE), SERVER_PORT_REPLACE.length(), std::to_string(reverse_port_)); - return res; + + program_ = res; } bool TrajectoryFollower::start() @@ -103,23 +101,15 @@ bool TrajectoryFollower::start() if(running_) return true; //not sure - if(!server_.bind()) - { - LOG_ERROR("Failed to bind server"); - return false; - } - 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)) + + if(!commander_.uploadProg(program_)) { LOG_ERROR("Program upload failed!"); return false; } - LOG_INFO("Awaiting incomming robot connection"); + LOG_DEBUG("Awaiting incomming robot connection"); if(!server_.accept()) { @@ -127,7 +117,7 @@ bool TrajectoryFollower::start() return false; } - LOG_INFO("Robot successfully connected"); + LOG_DEBUG("Robot successfully connected"); return (running_ = true); } @@ -182,7 +172,7 @@ bool TrajectoryFollower::execute(std::vector &trajectory, std:: typedef high_resolution_clock Clock; typedef Clock::time_point Time; - auto const& last = trajectory[trajectory.size()-1]; + auto& last = trajectory[trajectory.size()-1]; auto& prev = trajectory[0]; Time t0 = Clock::now(); @@ -202,8 +192,6 @@ bool TrajectoryFollower::execute(std::vector &trajectory, std:: auto duration = point.time_from_start - prev.time_from_start; double d_s = duration_cast(duration).count(); - LOG_INFO("Point duration: %f", d_s); - //interpolation loop while(!interrupt) { @@ -243,13 +231,7 @@ bool TrajectoryFollower::execute(std::vector &trajectory, std:: //the interpolation loop above but rather some position between //t[N-1] and t[N] where N is the number of trajectory points. //To make sure this does not happen the last position is sent - //if not interrupted. - if(!interrupt) - { - return execute(last.positions, true); - } - - return true; + return execute(last.positions, true); } void TrajectoryFollower::stop() @@ -260,6 +242,6 @@ void TrajectoryFollower::stop() std::array empty; execute(empty, false); - //server_.disconnect(); + server_.disconnectClient(); running_ = false; } \ No newline at end of file diff --git a/src/ros_main.cpp b/src/ros_main.cpp index 38634c9..dc5255c 100644 --- a/src/ros_main.cpp +++ b/src/ros_main.cpp @@ -74,7 +74,11 @@ bool parse_args(ProgArgs &args) return true; } -#include "ur_modern_driver/ur/server.h" +std::string getLocalIPAccessibleFromHost(std::string& host) +{ + URStream stream(host, UR_RT_PORT); + return stream.connect() ? stream.getIP() : std::string(); +} int main(int argc, char **argv) { @@ -86,9 +90,9 @@ int main(int argc, char **argv) return EXIT_FAILURE; } - + std::string local_ip(getLocalIPAccessibleFromHost(args.host)); + URFactory factory(args.host); - vector services; // RT packets @@ -99,7 +103,7 @@ int main(int argc, char **argv) auto rt_commander = factory.getCommander(rt_stream); vector *> rt_vec{&rt_pub}; - TrajectoryFollower traj_follower(*rt_commander, args.reverse_port, factory.isVersion3()); + TrajectoryFollower traj_follower(*rt_commander, local_ip, args.reverse_port, factory.isVersion3()); ROSController *controller(nullptr); ActionServer *action_server(nullptr); diff --git a/src/tcp_socket.cpp b/src/tcp_socket.cpp index f0049ec..a7f8cc4 100644 --- a/src/tcp_socket.cpp +++ b/src/tcp_socket.cpp @@ -1,5 +1,6 @@ #include #include +#include #include #include @@ -94,6 +95,23 @@ void TCPSocket::close() socket_fd_ = -1; } +std::string TCPSocket::getIP() +{ + sockaddr_in name; + socklen_t len = sizeof(name); + int res = ::getsockname(socket_fd_, (sockaddr*)&name, &len); + + if(res < 0) + { + LOG_ERROR("Could not get local IP"); + return std::string(); + } + + char buf[128]; + inet_ntop(AF_INET, &name.sin_addr, buf, sizeof(buf)); + return std::string(buf); +} + bool TCPSocket::read(uint8_t *buf, size_t buf_len, size_t &read) { read = 0; diff --git a/src/ur/server.cpp b/src/ur/server.cpp index fd929fe..1d53ed4 100644 --- a/src/ur/server.cpp +++ b/src/ur/server.cpp @@ -1,5 +1,6 @@ #include #include +#include #include #include "ur_modern_driver/log.h" #include "ur_modern_driver/ur/server.h" @@ -19,8 +20,18 @@ void URServer::setOptions(int socket_fd) std::string URServer::getIP() { + sockaddr_in name; + socklen_t len = sizeof(name); + int res = ::getsockname(getSocketFD(), (sockaddr*)&name, &len); + + if(res < 0) + { + LOG_ERROR("Could not get local IP"); + return std::string(); + } + char buf[128]; - int res = ::gethostname(buf, sizeof(buf)); + inet_ntop(AF_INET, &name.sin_addr, buf, sizeof(buf)); return std::string(buf); } @@ -28,7 +39,6 @@ bool URServer::bind() { std::string empty; bool res = TCPSocket::setup(empty, port_); - state_ = TCPSocket::getState(); if(!res) return false; @@ -41,7 +51,7 @@ bool URServer::bind() bool URServer::accept() { - if(state_ != SocketState::Connected || client_.getSocketFD() > 0) + if(TCPSocket::getState() != SocketState::Connected || client_.getSocketFD() > 0) return false; struct sockaddr addr; @@ -54,6 +64,14 @@ bool URServer::accept() return client_.setSocketFD(client_fd); } +void URServer::disconnectClient() +{ + if(client_.getState() != SocketState::Connected) + return; + + client_.close(); +} + bool URServer::write(const uint8_t* buf, size_t buf_len, size_t &written) { return client_.write(buf, buf_len, written);