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

Fixed an issue where canceling a trajectory wouldn't stop the robot.

Browse Source
This commit is contained in:
Thomas Timm Andersen
2015-10-09 10:47:29 +02:00
parent e354ada3ab
commit 80e344d167
2 changed files with 101 additions and 75 deletions
Download Patch File Download Diff File Expand all files Collapse all files

125
src/ur_driver.cpp
View File

@@ -13,15 +13,17 @@
UrDriver::UrDriver(std::condition_variable& rt_msg_cond,
std::condition_variable& msg_cond, std::string host,
unsigned int reverse_port, double servoj_time, unsigned int safety_count_max,
double max_time_step, double min_payload, double max_payload) :
unsigned int reverse_port, double servoj_time,
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), servoj_time_(servoj_time) {
min_payload), maximum_payload_(max_payload), servoj_time_(
servoj_time) {
char buffer[256];
struct sockaddr_in serv_addr;
int n, flag;
//char *ip_addr;
executing_traj_ = false;
rt_interface_ = new UrRealtimeCommunication(rt_msg_cond, host,
safety_count_max);
new_sockfd_ = -1;
@@ -63,77 +65,76 @@ std::vector<double> UrDriver::interp_cubic(double t, double T,
}
return positions;
}
/*
void UrDriver::addTraj(std::vector<double> inp_timestamps,
std::vector<std::vector<double> > inp_positions,
std::vector<std::vector<double> > inp_velocities) {
// DEPRECATED
printf("!! addTraj is deprecated !!\n");
std::vector<double> timestamps;
std::vector<std::vector<double> > positions;
std::string command_string = "def traj():\n";
/*
void UrDriver::addTraj(std::vector<double> inp_timestamps,
std::vector<std::vector<double> > inp_positions,
std::vector<std::vector<double> > inp_velocities) {
// DEPRECATED
printf("!! addTraj is deprecated !!\n");
std::vector<double> timestamps;
std::vector<std::vector<double> > positions;
std::string command_string = "def traj():\n";
for (unsigned int i = 1; i < inp_timestamps.size(); i++) {
timestamps.push_back(inp_timestamps[i - 1]);
double dt = inp_timestamps[i] - inp_timestamps[i - 1];
unsigned int steps = (unsigned int) ceil(dt / maximum_time_step_);
double step_size = dt / steps;
for (unsigned int j = 1; j < steps; j++) {
timestamps.push_back(inp_timestamps[i - 1] + step_size * j);
}
}
// //make sure we come to a smooth stop
// while (timestamps.back() < inp_timestamps.back()) {
// timestamps.push_back(timestamps.back() + 0.008);
// }
// timestamps.pop_back();
for (unsigned int i = 1; i < inp_timestamps.size(); i++) {
timestamps.push_back(inp_timestamps[i - 1]);
double dt = inp_timestamps[i] - inp_timestamps[i - 1];
unsigned int steps = (unsigned int) ceil(dt / maximum_time_step_);
double step_size = dt / steps;
for (unsigned int j = 1; j < steps; j++) {
timestamps.push_back(inp_timestamps[i - 1] + step_size * j);
}
}
// //make sure we come to a smooth stop
// while (timestamps.back() < inp_timestamps.back()) {
// timestamps.push_back(timestamps.back() + 0.008);
// }
// timestamps.pop_back();
unsigned int j = 0;
for (unsigned int i = 0; i < timestamps.size(); i++) {
while (inp_timestamps[j] <= timestamps[i]) {
j += 1;
}
positions.push_back(
UrDriver::interp_cubic(timestamps[i] - 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]));
}
unsigned int j = 0;
for (unsigned int i = 0; i < timestamps.size(); i++) {
while (inp_timestamps[j] <= timestamps[i]) {
j += 1;
}
positions.push_back(
UrDriver::interp_cubic(timestamps[i] - 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]));
}
timestamps.push_back(inp_timestamps[inp_timestamps.size() - 1]);
positions.push_back(inp_positions[inp_positions.size() - 1]);
/// This is actually faster than using a stringstream :-o
for (unsigned int i = 1; i < timestamps.size(); i++) {
char buf[128];
sprintf(buf,
"\tservoj([%1.5f, %1.5f, %1.5f, %1.5f, %1.5f, %1.5f], t=%1.5f)\n",
positions[i][0], positions[i][1], positions[i][2],
positions[i][3], positions[i][4], positions[i][5],
timestamps[i] - timestamps[i - 1]);
command_string += buf;
}
command_string += "end\n";
//printf("%s", command_string.c_str());
rt_interface_->addCommandToQueue(command_string);
timestamps.push_back(inp_timestamps[inp_timestamps.size() - 1]);
positions.push_back(inp_positions[inp_positions.size() - 1]);
/// This is actually faster than using a stringstream :-o
for (unsigned int i = 1; i < timestamps.size(); i++) {
char buf[128];
sprintf(buf,
"\tservoj([%1.5f, %1.5f, %1.5f, %1.5f, %1.5f, %1.5f], t=%1.5f)\n",
positions[i][0], positions[i][1], positions[i][2],
positions[i][3], positions[i][4], positions[i][5],
timestamps[i] - timestamps[i - 1]);
command_string += buf;
}
command_string += "end\n";
//printf("%s", command_string.c_str());
rt_interface_->addCommandToQueue(command_string);
}
*/
}
*/
void UrDriver::doTraj(std::vector<double> inp_timestamps,
std::vector<std::vector<double> > inp_positions,
std::vector<std::vector<double> > inp_velocities) {
std::chrono::high_resolution_clock::time_point t0, t;
std::vector<double> positions;
unsigned int j;
executing_traj_ = true;
UrDriver::uploadProg();
t0 = std::chrono::high_resolution_clock::now();
t = t0;
j = 0;
while ((inp_timestamps[inp_timestamps.size() - 1]
>= std::chrono::duration_cast<std::chrono::duration<double>>(t - t0).count()) and executing_traj_) {
>= std::chrono::duration_cast<std::chrono::duration<double>>(t - t0).count())
and executing_traj_) {
while (inp_timestamps[j]
<= std::chrono::duration_cast<std::chrono::duration<double>>(
t - t0).count() && j < inp_timestamps.size() - 1) {
@@ -144,19 +145,19 @@ 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);
// oversample with 4 * sample_time
std::this_thread::sleep_for(std::chrono::milliseconds((int) ((servoj_time_*1000)/4.)));
std::this_thread::sleep_for(
std::chrono::milliseconds((int) ((servoj_time_ * 1000) / 4.)));
t = std::chrono::high_resolution_clock::now();
}
//Signal robot to stop driverProg()
UrDriver::closeServo(positions);
}
void UrDriver::servoj(std::vector<double> positions,
int keepalive, double time) {
void UrDriver::servoj(std::vector<double> positions, int keepalive,
double time) {
unsigned int bytes_written;
int tmp;
unsigned char buf[32];

51
src/ur_ros_wrapper.cpp
View File

@@ -148,6 +148,7 @@ public:
"The control thread for this driver has been started");
} else {
//start actionserver
goal_handle_ = NULL;
as_.start();
//subscribe to the data topic of interest
@@ -180,8 +181,13 @@ private:
actionlib::ServerGoalHandle<
control_msgs::FollowJointTrajectoryAction> gh) {
print_info("on_goal");
actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal =
*gh.getGoal(); //make a copy that we can modify
if (goal_handle_ != NULL) {
RosWrapper::cancelCB(*goal_handle_);
}
goal_handle_ = &gh;
actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal = *goal_handle_->getGoal(); //make a copy that we can modify
if (!validateJointNames()) {
std::string outp_joint_names = "";
for (unsigned int i = 0; i < goal.trajectory.joint_names.size();
@@ -195,7 +201,6 @@ private:
gh.setRejected(result_, result_.error_string);
print_error(result_.error_string);
}
if (!has_positions()) {
result_.error_code = result_.INVALID_GOAL;
result_.error_string = "Received a goal without positions";
@@ -226,8 +231,15 @@ private:
}
reorder_traj_joints(goal.trajectory);
std::vector<double> timestamps;
std::vector<std::vector<double> > positions, velocities;
if (goal.trajectory.points[0].time_from_start.toSec() != 0.) {
print_warning("Trajectory's first point should be the current position, with time_from_start set to 0.0 - Inserting point in malformed trajectory");
timestamps.push_back(0.0);
positions.push_back(robot_.rt_interface_->robot_state_->getQActual());
velocities.push_back(robot_.rt_interface_->robot_state_->getQdActual());
}
for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) {
timestamps.push_back(
goal.trajectory.points[i].time_from_start.toSec());
@@ -235,10 +247,14 @@ private:
velocities.push_back(goal.trajectory.points[i].velocities);
}
gh.setAccepted();
goal_handle_->setAccepted();
robot_.doTraj(timestamps, positions, velocities);
result_.error_code = result_.SUCCESSFUL;
gh.setSucceeded(result_);
if (goal_handle_ != NULL) {
result_.error_code = result_.SUCCESSFUL;
goal_handle_->setSucceeded(result_);
goal_handle_ = NULL;
}
}
void cancelCB(
@@ -246,9 +262,13 @@ private:
control_msgs::FollowJointTrajectoryAction> gh) {
print_info("on_cancel");
// set the action state to preempted
robot_.stopTraj();
result_.error_code = result_.SUCCESSFUL;
result_.error_string = "Goal cancelled by client";
if (goal_handle_ != NULL) {
print_info("Stopping previous traj");
robot_.stopTraj();
result_.error_code = result_.SUCCESSFUL;
result_.error_string = "Goal cancelled by client";
goal_handle_ = NULL;
}
gh.setCanceled(result_);
}
@@ -285,7 +305,8 @@ private:
bool validateJointNames() {
std::vector<std::string> actual_joint_names = robot_.getJointNames();
actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal = *goal_handle_->getGoal();
actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal =
*goal_handle_->getGoal();
if (goal.trajectory.joint_names.size() != actual_joint_names.size())
return false;
@@ -336,7 +357,8 @@ private:
}
bool has_velocities() {
actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal = *goal_handle_->getGoal();
actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal =
*goal_handle_->getGoal();
for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) {
if (goal.trajectory.points[i].positions.size()
!= goal.trajectory.points[i].velocities.size())
@@ -346,7 +368,8 @@ private:
}
bool has_positions() {
actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal = *goal_handle_->getGoal();
actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal =
*goal_handle_->getGoal();
if (goal.trajectory.points.size() == 0)
return false;
for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) {
@@ -358,7 +381,8 @@ private:
}
bool has_limited_velocities() {
actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal = *goal_handle_->getGoal();
actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal =
*goal_handle_->getGoal();
for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) {
for (unsigned int j = 0;
j < goal.trajectory.points[i].velocities.size(); j++) {
@@ -371,7 +395,8 @@ private:
}
bool traj_is_finite() {
actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal = *goal_handle_->getGoal();
actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal =
*goal_handle_->getGoal();
for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) {
for (unsigned int j = 0;
j < goal.trajectory.points[i].velocities.size(); j++) {