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

Split servoj into several functions for reuseability

Browse Source
This commit is contained in:
Thomas Timm Andersen
2015-09-24 16:51:18 +02:00
parent 402f5c0170
commit ba96d0f098
2 changed files with 42 additions and 45 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
include/ur_modern_driver/ur_driver.h
View File

@@ -37,6 +37,7 @@ private:
const int MULT_TIME_ = 1000000;
const unsigned int REVERSE_PORT_;
int incoming_sockfd_;
int new_sockfd_;
public:
UrRealtimeCommunication* rt_interface_;
UrCommunication* sec_interface_;
@@ -58,9 +59,13 @@ public:
void doTraj(std::vector<double> inp_timestamps,
std::vector<std::vector<double> > inp_positions,
std::vector<std::vector<double> > inp_velocities);
void servoj(std::vector<double> positions, double time, int keepalive);
void stopTraj();
void uploadProg();
void openServo();
void closeServo();
std::vector<double> interp_cubic(double t, double T,
std::vector<double> p0_pos, std::vector<double> p1_pos,

92
src/ur_driver.cpp
View File

@@ -128,28 +128,13 @@ void UrDriver::addTraj(std::vector<double> inp_timestamps,
void UrDriver::doTraj(std::vector<double> inp_timestamps,
std::vector<std::vector<double> > inp_positions,
std::vector<std::vector<double> > inp_velocities) {
struct sockaddr_in cli_addr;
socklen_t clilen;
int new_sockfd;
std::chrono::high_resolution_clock::time_point t0, t;
std::vector<double> positions;
unsigned int j, bytes_written;
int tmp;
unsigned char buf[32];
unsigned int j;
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;
@@ -165,6 +150,24 @@ void UrDriver::doTraj(std::vector<double> inp_timestamps,
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, 0.008, 1);
// oversample with 4 * sample_time
std::this_thread::sleep_for(std::chrono::milliseconds(2));
t = std::chrono::high_resolution_clock::now();
}
//Signal robot to stop driverProg()
UrDriver::servoj(positions, 0.008, 0);
UrDriver::closeServo();
}
void UrDriver::servoj(std::vector<double> positions, double time,
int keepalive) {
unsigned int bytes_written;
int tmp;
unsigned char buf[32];
for (int i = 0; i < 6; i++) {
tmp = htonl((int) (positions[i] * MULT_JOINTSTATE_));
buf[i * 4] = tmp & 0xff;
@@ -172,47 +175,17 @@ void UrDriver::doTraj(std::vector<double> inp_timestamps,
buf[i * 4 + 2] = (tmp >> 16) & 0xff;
buf[i * 4 + 3] = (tmp >> 24) & 0xff;
}
tmp = htonl((int) (0.008 * MULT_TIME_));
tmp = htonl((int) (time * 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);
tmp = htonl((int) keepalive);
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 4 * sample_time
std::this_thread::sleep_for(std::chrono::milliseconds(2));
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);
bytes_written = write(new_sockfd_, buf, 32);
}
void UrDriver::stopTraj() {
@@ -294,6 +267,25 @@ void UrDriver::uploadProg() {
rt_interface_->addCommandToQueue(cmd_str);
UrDriver::openServo();
}
void 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) {
#ifdef ROS_BUILD
ROS_FATAL("ERROR on accepting reverse communication");
#else
printf("ERROR on accepting reverse communication");
#endif
}
}
void UrDriver::closeServo() {
close(new_sockfd_);
}
bool UrDriver::start() {