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 a check to incoming goals to see if there are any points in the trajectory

Browse Source
This commit is contained in:
Thomas Timm Andersen
2015-10-08 13:44:24 +02:00
parent 22e7d799c2
commit f592d2189a
1 changed files with 18 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

18
src/ur_ros_wrapper.cpp
View File

@@ -190,6 +190,13 @@ private:
print_error(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";
as_.setAborted(result_, result_.error_string);
print_error(result_.error_string);
}
if (!has_velocities()) { if (!has_velocities()) {
result_.error_code = result_.INVALID_GOAL; result_.error_code = result_.INVALID_GOAL;
result_.error_string = "Received a goal without velocities"; result_.error_string = "Received a goal without velocities";
@@ -325,6 +332,17 @@ private:
return true; return true;
} }
bool has_positions() {
if (goal_.trajectory.points.size() == 0)
return false;
for (unsigned int i = 0; i < goal_.trajectory.points.size(); i++) {
if (goal_.trajectory.points[i].positions.size()
!= goal_.trajectory.joint_names.size())
return false;
}
return true;
}
bool has_limited_velocities() { bool has_limited_velocities() {
for (unsigned int i = 0; i < goal_.trajectory.points.size(); i++) { for (unsigned int i = 0; i < goal_.trajectory.points.size(); i++) {
for (unsigned int j = 0; for (unsigned int j = 0;