From c4af674faa739e5bfa3ff33f5cf5ddc264e2ae08 Mon Sep 17 00:00:00 2001 From: Thomas Timm Andersen Date: Tue, 22 Sep 2015 10:29:44 +0200 Subject: [PATCH] Changed how trajectories are handled to try and speed things up --- include/ur_modern_driver/ur_driver.h | 27 ++- .../ur_realtime_communication.h | 5 + src/ur_communication.cpp | 2 +- src/ur_driver.cpp | 226 +++++++++++++++++- src/ur_realtime_communication.cpp | 19 +- src/ur_ros_wrapper.cpp | 30 ++- 6 files changed, 291 insertions(+), 18 deletions(-) diff --git a/include/ur_modern_driver/ur_driver.h b/include/ur_modern_driver/ur_driver.h index 2cb2650..d6af6cd 100644 --- a/include/ur_modern_driver/ur_driver.h +++ b/include/ur_modern_driver/ur_driver.h @@ -19,6 +19,12 @@ #include #include #include +#include +#include +#include + +#include + class UrDriver { private: @@ -26,23 +32,36 @@ private: double minimum_payload_; double maximum_payload_; std::vector joint_names_; + std::string ip_addr_; + const int MULT_JOINTSTATE_ = 1000000; + const int MULT_TIME_ = 1000000; + const unsigned int REVERSE_PORT_; + int incoming_sockfd_; public: UrRealtimeCommunication* rt_interface_; UrCommunication* sec_interface_; - UrDriver(std::condition_variable& rt_msg_cond, std::condition_variable& msg_cond, std::string host, - unsigned int safety_count_max = 12, double max_time_step = 0.08, - double min_payload = 0., double max_payload = 1.); + UrDriver(std::condition_variable& rt_msg_cond, + std::condition_variable& msg_cond, std::string host, + unsigned int reverse_port = 50007, unsigned int safety_count_max = + 12, double max_time_step = 0.08, double min_payload = 0., + double max_payload = 1.); void start(); void halt(); void setSpeed(double q0, double q1, double q2, double q3, double q4, double q5, double acc = 100.); - void addTraj(std::vector inp_timestamps, + void addTraj( + std::vector inp_timestamps, //DEPRECATED std::vector > positions, std::vector > velocities); + void doTraj(std::vector inp_timestamps, + std::vector > inp_positions, + std::vector > inp_velocities); void stopTraj(); + void uploadProg(); + std::vector interp_cubic(double t, double T, std::vector p0_pos, std::vector p1_pos, std::vector p0_vel, std::vector p1_vel); diff --git a/include/ur_modern_driver/ur_realtime_communication.h b/include/ur_modern_driver/ur_realtime_communication.h index 77c6873..2e43e43 100644 --- a/include/ur_modern_driver/ur_realtime_communication.h +++ b/include/ur_modern_driver/ur_realtime_communication.h @@ -28,6 +28,8 @@ #include #include #include +#include +#include #ifdef ROS_BUILD #include @@ -39,6 +41,7 @@ private: int sockfd_; struct sockaddr_in serv_addr_; struct hostent *server_; + std::string local_ip_; bool keepalive_; std::thread comThread_; int flag_; @@ -47,6 +50,7 @@ private: unsigned int safety_count_; void run(); + public: bool connected_; RobotStateRT* robot_state_; @@ -59,6 +63,7 @@ public: double q5, double acc = 100.); void addCommandToQueue(std::string inp); void setSafetyCountMax(uint inp); + std::string getLocalIp(); }; diff --git a/src/ur_communication.cpp b/src/ur_communication.cpp index 2ceacd0..5b2a27f 100644 --- a/src/ur_communication.cpp +++ b/src/ur_communication.cpp @@ -98,7 +98,7 @@ void UrCommunication::start() { //wait for some traffic so the UR socket doesn't die in version 3.1. std::this_thread::sleep_for(std::chrono::milliseconds(500)); #ifdef ROS_BUILD - ROS_INFO("Firmware version detected: %1.7f", robot_state_->getVersion()); + ROS_DEBUG("Firmware version detected: %1.7f", robot_state_->getVersion()); #else printf("Firmware version detected: %f\n", robot_state_->getVersion()); #endif diff --git a/src/ur_driver.cpp b/src/ur_driver.cpp index a524b4d..cae94f2 100644 --- a/src/ur_driver.cpp +++ b/src/ur_driver.cpp @@ -11,15 +11,47 @@ #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 safety_count_max, double max_time_step, double min_payload, - double max_payload) : - maximum_time_step_(max_time_step), minimum_payload_(min_payload), maximum_payload_( - max_payload) { +UrDriver::UrDriver(std::condition_variable& rt_msg_cond, + std::condition_variable& msg_cond, std::string host, + unsigned int reverse_port, unsigned int safety_count_max, + double max_time_step, double min_payload, double max_payload) : + REVERSE_PORT_(reverse_port), maximum_time_step_(max_time_step), minimum_payload_( + min_payload), maximum_payload_(max_payload) { + char buffer[256]; + struct sockaddr_in serv_addr; + int n, flag; + //char *ip_addr; + rt_interface_ = new UrRealtimeCommunication(rt_msg_cond, host, safety_count_max); sec_interface_ = new UrCommunication(msg_cond, host); + incoming_sockfd_ = socket(AF_INET, SOCK_STREAM, 0); + if (incoming_sockfd_ < 0) { +#ifdef ROS_BUILD + ROS_FATAL("ERROR opening socket for reverse communication"); +#else + printf("ERROR opening socket for reverse communication"); +#endif + } + 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) { +#ifdef ROS_BUILD + ROS_FATAL("ERROR on binding socket for reverse communication"); +#else + printf("ERROR on binding socket for reverse communication"); +#endif + } + listen(incoming_sockfd_, 5); } std::vector UrDriver::interp_cubic(double t, double T, @@ -42,7 +74,8 @@ std::vector UrDriver::interp_cubic(double t, double T, void UrDriver::addTraj(std::vector inp_timestamps, std::vector > inp_positions, std::vector > inp_velocities) { - + /* DEPRECATED */ + printf("!! addTraj is deprecated !!\n"); std::vector timestamps; std::vector > positions; std::string command_string = "def traj():\n"; @@ -92,19 +125,194 @@ void UrDriver::addTraj(std::vector inp_timestamps, } +void UrDriver::doTraj(std::vector inp_timestamps, + std::vector > inp_positions, + std::vector > inp_velocities) { + struct sockaddr_in cli_addr; + socklen_t clilen; + int new_sockfd; + std::chrono::high_resolution_clock::time_point t0, t; + std::vector positions; + unsigned int j, bytes_written; + int tmp; + unsigned char buf[32]; + + UrDriver::uploadProg(); + + clilen = sizeof(cli_addr); + new_sockfd = accept(incoming_sockfd_, (struct sockaddr *) &cli_addr, + &clilen); + if (new_sockfd < 0) { +#ifdef ROS_BUILD + ROS_FATAL("ERROR on accepting reverse communication"); +#else + printf("ERROR on accepting reverse communication"); +#endif + } + + t0 = std::chrono::high_resolution_clock::now(); + t = t0; + j = 0; + while (inp_timestamps[inp_timestamps.size() - 1] + >= std::chrono::duration_cast>(t - t0).count()) { + while (inp_timestamps[j] + <= std::chrono::duration_cast>( + t - t0).count() && j < inp_timestamps.size() - 1) { + j += 1; + } + positions = UrDriver::interp_cubic( + std::chrono::duration_cast>( + 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]); + 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) (0.008 * MULT_TIME_)); + 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; + tmp = htonl((int) 1); + buf[7 * 4] = tmp & 0xff; + buf[7 * 4 + 1] = (tmp >> 8) & 0xff; + buf[7 * 4 + 2] = (tmp >> 16) & 0xff; + buf[7 * 4 + 3] = (tmp >> 24) & 0xff; + bytes_written = write(new_sockfd, buf, 32); + + // oversample with 2 * sample_time + std::this_thread::sleep_for(std::chrono::milliseconds(4)); + + t = std::chrono::high_resolution_clock::now(); + } + //Signal robot to stop driverProg() + for (int i = 0; i < 6; i++) { + tmp = htonl( + (int) (inp_positions[inp_positions.size() - 1][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) (0.008 * MULT_TIME_)); + 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; + tmp = htonl((int) 0); + buf[7 * 4] = tmp & 0xff; + buf[7 * 4 + 1] = (tmp >> 8) & 0xff; + buf[7 * 4 + 2] = (tmp >> 16) & 0xff; + buf[7 * 4 + 3] = (tmp >> 24) & 0xff; + bytes_written = write(new_sockfd, buf, 32); + + close(new_sockfd); + +} + void UrDriver::stopTraj() { rt_interface_->addCommandToQueue("stopj(10)\n"); } +void 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; + + sprintf(buf, "\tMULT_time = %i\n", MULT_TIME_); + 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_id = 0 # 0 = idle, -1 = stop\n"; + cmd_str += "\tcmd_servo_q = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]\n"; + cmd_str += "\tcmd_servo_dt = 0.0\n"; + cmd_str += "\tdef set_servo_setpoint(q, dt):\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\tcmd_servo_dt = dt\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\tdt = cmd_servo_dt\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"; + cmd_str += "\t\t\t\tservoj(q, t=dt)\n"; + 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+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\tt = params_mult[7] / MULT_time\n"; + cmd_str += "\t\t\tkeepalive = params_mult[8]\n"; + cmd_str += "\t\t\tset_servo_setpoint(q, t)\n"; + cmd_str += "\t\tend\n"; + cmd_str += "\tend\n"; + cmd_str += "\tsleep(.1)\n"; + cmd_str += "\tsocket_close()\n"; + cmd_str += "end\n"; + + rt_interface_->addCommandToQueue(cmd_str); + +} + void UrDriver::start() { sec_interface_->start(); - rt_interface_->robot_state_->setVersion(sec_interface_->robot_state_->getVersion()); + rt_interface_->robot_state_->setVersion( + sec_interface_->robot_state_->getVersion()); rt_interface_->start(); + ip_addr_ = rt_interface_->getLocalIp(); //inet_ntoa(serv_addr.sin_addr); +#ifdef ROS_BUILD + ROS_DEBUG("Listening on %s:%u\n", ip_addr_.c_str(), REVERSE_PORT_); +#else + printf("Listening on %s:%u\n", ip_addr_.c_str(), REVERSE_PORT_); +#endif } void UrDriver::halt() { sec_interface_->halt(); rt_interface_->halt(); + close(incoming_sockfd_); } void UrDriver::setSpeed(double q0, double q1, double q2, double q3, double q4, @@ -174,9 +382,9 @@ bool UrDriver::setPayload(double m) { char buf[256]; sprintf(buf, "sec setOut():\n\tset_payload(%1.3f)\nend\n", m); #ifdef ROS_BUILD - ROS_DEBUG("%s", buf); + ROS_DEBUG("%s", buf); #else - printf("%s", buf); + printf("%s", buf); #endif rt_interface_->addCommandToQueue(buf); return true; diff --git a/src/ur_realtime_communication.cpp b/src/ur_realtime_communication.cpp index 3f9665d..23ecfd2 100644 --- a/src/ur_realtime_communication.cpp +++ b/src/ur_realtime_communication.cpp @@ -56,12 +56,26 @@ void UrRealtimeCommunication::start() { printf("Realtime port: Connecting...\n"); #endif if (connect(sockfd_, (struct sockaddr *) &serv_addr_, sizeof(serv_addr_)) - < 0) + < 0) { #ifdef ROS_BUILD ROS_FATAL("Error connecting to RT port 30003"); #else printf("Error connecting to RT port 30003\n"); #endif + } + sockaddr_in name; + socklen_t namelen = sizeof(name); + int err = getsockname(sockfd_, (sockaddr*) &name, &namelen); + if (err < 0) { +#ifdef ROS_BUILD + ROS_FATAL("Could not get local IP - errno: %d (%s)", errno, strerror(errno)); +#else + printf("Could not get local IP - errno: %d (%s)", errno, strerror(errno)); +#endif + } + char str[18]; + inet_ntop(AF_INET, &name.sin_addr, str, 18); + local_ip_ = str; comThread_ = std::thread(&UrRealtimeCommunication::run, this); } @@ -119,3 +133,6 @@ void UrRealtimeCommunication::setSafetyCountMax(uint inp) { safety_count_max_ = inp; } +std::string UrRealtimeCommunication::getLocalIp() { + return local_ip_; +} diff --git a/src/ur_ros_wrapper.cpp b/src/ur_ros_wrapper.cpp index 5cd23ad..eb3b62f 100644 --- a/src/ur_ros_wrapper.cpp +++ b/src/ur_ros_wrapper.cpp @@ -95,7 +95,7 @@ public: } robot_.setMinPayload(min_payload); robot_.setMaxPayload(max_payload); - ROS_INFO("Bounds for set_payload service calls: [%f, %f]", + ROS_DEBUG("Bounds for set_payload service calls: [%f, %f]", min_payload, max_payload); } @@ -122,7 +122,31 @@ public: boost::bind(&RosWrapper::publishRTMsg, this)); mb_publish_thread_ = new std::thread( boost::bind(&RosWrapper::publishMbMsg, this)); - ROS_INFO("The action server for this driver has been started"); + ROS_DEBUG("The action server for this driver has been started"); + /*double pi = 3.141592653589793; + std::vector tmp, t; + std::vector > pos, vel; + tmp.push_back(-pi / 2); + tmp.push_back(-pi / 2); + tmp.push_back(-pi / 2); + tmp.push_back(0); + tmp.push_back(-pi / 2); + tmp.push_back(0); + pos.push_back(tmp); + tmp[5] = pi; + pos.push_back(tmp); + tmp[5] = 0; + pos.push_back(tmp); + for (int i = 0; i < 6; i++) { + tmp[i] = 0; + } + vel.push_back(tmp); + vel.push_back(tmp); + vel.push_back(tmp); + t.push_back(0.); + t.push_back(4.); + t.push_back(8.); + robot_.doTraj(t, pos, vel); */ } @@ -184,7 +208,7 @@ private: velocities.push_back(goal_.trajectory.points[i].velocities); } - robot_.addTraj(timestamps, positions, velocities); + robot_.doTraj(timestamps, positions, velocities); ros::Duration(timestamps.back()).sleep(); result_.error_code = result_.SUCCESSFUL;