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

Added stability to reverse connection. Resolves #6

Browse Source
This commit is contained in:
Thomas Timm Andersen
2015-10-20 13:21:23 +02:00
parent 6c3cade12f
commit f856487e9f
2 changed files with 25 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
include/ur_modern_driver/ur_driver.h
View File

@@ -46,6 +46,7 @@ private:
const unsigned int REVERSE_PORT_; const unsigned int REVERSE_PORT_;
int incoming_sockfd_; int incoming_sockfd_;
int new_sockfd_; int new_sockfd_;
bool reverse_connected_;
double servoj_time_; double servoj_time_;
bool executing_traj_; bool executing_traj_;
public: public:
@@ -66,15 +67,15 @@ public:
std::vector<double> inp_timestamps, //DEPRECATED std::vector<double> inp_timestamps, //DEPRECATED
std::vector<std::vector<double> > positions, std::vector<std::vector<double> > positions,
std::vector<std::vector<double> > velocities); */ std::vector<std::vector<double> > velocities); */
void doTraj(std::vector<double> inp_timestamps, bool doTraj(std::vector<double> inp_timestamps,
std::vector<std::vector<double> > inp_positions, std::vector<std::vector<double> > inp_positions,
std::vector<std::vector<double> > inp_velocities); std::vector<std::vector<double> > inp_velocities);
void servoj(std::vector<double> positions, int keepalive = 1, double time = 0); void servoj(std::vector<double> positions, int keepalive = 1, double time = 0);
void stopTraj(); void stopTraj();
void uploadProg(); bool uploadProg();
void openServo(); bool openServo();
void closeServo(std::vector<double> positions); void closeServo(std::vector<double> positions);
std::vector<double> interp_cubic(double t, double T, std::vector<double> interp_cubic(double t, double T,

28
src/ur_driver.cpp
View File

@@ -29,7 +29,8 @@ UrDriver::UrDriver(std::condition_variable& rt_msg_cond,
char buffer[256]; char buffer[256];
struct sockaddr_in serv_addr; struct sockaddr_in serv_addr;
int n, flag; int n, flag;
//char *ip_addr;
reverse_connected_ = false;
executing_traj_ = false; executing_traj_ = false;
rt_interface_ = new UrRealtimeCommunication(rt_msg_cond, host, rt_interface_ = new UrRealtimeCommunication(rt_msg_cond, host,
safety_count_max); safety_count_max);
@@ -127,15 +128,17 @@ std::vector<double> UrDriver::interp_cubic(double t, double T,
} }
*/ */
void UrDriver::doTraj(std::vector<double> inp_timestamps, bool UrDriver::doTraj(std::vector<double> inp_timestamps,
std::vector<std::vector<double> > inp_positions, std::vector<std::vector<double> > inp_positions,
std::vector<std::vector<double> > inp_velocities) { std::vector<std::vector<double> > inp_velocities) {
std::chrono::high_resolution_clock::time_point t0, t; std::chrono::high_resolution_clock::time_point t0, t;
std::vector<double> positions; std::vector<double> positions;
unsigned int j; unsigned int j;
if (!UrDriver::uploadProg()) {
return false;
}
executing_traj_ = true; executing_traj_ = true;
UrDriver::uploadProg();
t0 = std::chrono::high_resolution_clock::now(); t0 = std::chrono::high_resolution_clock::now();
t = t0; t = t0;
j = 0; j = 0;
@@ -159,12 +162,18 @@ void UrDriver::doTraj(std::vector<double> inp_timestamps,
std::chrono::milliseconds((int) ((servoj_time_ * 1000) / 4.))); std::chrono::milliseconds((int) ((servoj_time_ * 1000) / 4.)));
t = std::chrono::high_resolution_clock::now(); t = std::chrono::high_resolution_clock::now();
} }
executing_traj_ = false;
//Signal robot to stop driverProg() //Signal robot to stop driverProg()
UrDriver::closeServo(positions); UrDriver::closeServo(positions);
return true;
} }
void UrDriver::servoj(std::vector<double> positions, int keepalive, void UrDriver::servoj(std::vector<double> positions, int keepalive,
double time) { double time) {
if (!reverse_connected_) {
print_error("UrDriver::servoj called without a reverse connection present. Keepalive: " + std::to_string(keepalive));
return;
}
unsigned int bytes_written; unsigned int bytes_written;
int tmp; int tmp;
unsigned char buf[28]; unsigned char buf[28];
@@ -191,7 +200,7 @@ void UrDriver::stopTraj() {
rt_interface_->addCommandToQueue("stopj(10)\n"); rt_interface_->addCommandToQueue("stopj(10)\n");
} }
void UrDriver::uploadProg() { bool UrDriver::uploadProg() {
std::string cmd_str; std::string cmd_str;
char buf[128]; char buf[128];
cmd_str = "def driverProg():\n"; cmd_str = "def driverProg():\n";
@@ -255,15 +264,16 @@ void UrDriver::uploadProg() {
cmd_str += "\t\t\tset_servo_setpoint(q)\n"; cmd_str += "\t\t\tset_servo_setpoint(q)\n";
cmd_str += "\t\tend\n"; cmd_str += "\t\tend\n";
cmd_str += "\tend\n"; cmd_str += "\tend\n";
cmd_str += "\tstopj(10)\n";
cmd_str += "\tsleep(.1)\n"; cmd_str += "\tsleep(.1)\n";
cmd_str += "\tsocket_close()\n"; cmd_str += "\tsocket_close()\n";
cmd_str += "end\n"; cmd_str += "end\n";
rt_interface_->addCommandToQueue(cmd_str); rt_interface_->addCommandToQueue(cmd_str);
UrDriver::openServo(); return UrDriver::openServo();
} }
void UrDriver::openServo() { bool UrDriver::openServo() {
struct sockaddr_in cli_addr; struct sockaddr_in cli_addr;
socklen_t clilen; socklen_t clilen;
clilen = sizeof(cli_addr); clilen = sizeof(cli_addr);
@@ -271,14 +281,18 @@ void UrDriver::openServo() {
&clilen); &clilen);
if (new_sockfd_ < 0) { if (new_sockfd_ < 0) {
print_fatal("ERROR on accepting reverse communication"); print_fatal("ERROR on accepting reverse communication");
return false;
} }
reverse_connected_ = true;
return true;
} }
void UrDriver::closeServo(std::vector<double> positions) { void UrDriver::closeServo(std::vector<double> positions) {
if (positions.size() != 6) if (positions.size() != 6)
UrDriver::servoj(rt_interface_->robot_state_->getQActual(), 0); UrDriver::servoj(rt_interface_->robot_state_->getQActual(), 0);
else else
UrDriver::servoj(positions, 0); UrDriver::servoj(positions, 0);
executing_traj_ = false;
reverse_connected_ = false;
close(new_sockfd_); close(new_sockfd_);
} }