Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fix/driver #9

Merged
merged 21 commits into from
Feb 19, 2018
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
21 commits
Select commit Hold shift + click to select a range
9a94eb5
Remove dependecy on ros_controllers metapackage.
miguelprada Feb 24, 2017
b72aac3
ur_ros_wrapper: introduce a bit of thread safety into goal handling
xqms Jun 25, 2016
c52eb64
ur_ros_wrapper: more thread safety
xqms Jun 25, 2016
2212d9b
even more thread safety (ur_realtime_comm)
xqms Jun 30, 2016
94b1ebf
ur_ros_wrapper: protect against simultaneous action callbacks
xqms Jun 30, 2016
50380df
Add time parameter back to speedj for SW >= 3.3.
miguelprada Mar 22, 2017
6eb529e
Merge pull request #100 from tecnalia-advancedmanufacturing-robotics/…
ThomasTimm Apr 25, 2017
e272a85
Copy config folder on install
Jul 7, 2017
50510ec
Correct controller names. Fixes ThomasTimm/ur_modern_driver#98
Jul 7, 2017
7290b1e
added installation and runtime execution for absolute beginners
maxgittelman Aug 4, 2017
a2294c8
Merge pull request #127 from maxgittelman/master
ThomasTimm Aug 21, 2017
a99248d
Merge pull request #117 from mrjogo/controller_names
ThomasTimm Aug 21, 2017
457863c
Merge pull request #116 from mrjogo/cmakelists_install
ThomasTimm Aug 21, 2017
b47a15a
Merge pull request #94 from tecnalia-advancedmanufacturing-robotics/r…
ThomasTimm Aug 21, 2017
9397f47
Updated readme.md
ThomasTimm Oct 10, 2017
5551cc8
Update robot_state_RT.cpp
hellochenwang Oct 13, 2017
92ace77
complete merge
Feb 19, 2018
efed9fd
Add data length for 3.5+
Feb 19, 2018
fe7424c
do not print all wrong message lengths
Feb 19, 2018
5eaf150
Merge branch 'thread_safety' of https://github.com/xqms/ur_modern_dri…
Feb 19, 2018
e353b21
disable goal and as lock
Feb 19, 2018
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 2 additions & 1 deletion CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -134,7 +134,7 @@ endif()
## Specify additional locations of header files
## Your package locations should be listed before other locations
# include_directories(include)
include_directories(include
include_directories(include
${catkin_INCLUDE_DIRS}
)

Expand Down Expand Up @@ -177,6 +177,7 @@ target_link_libraries(ur_driver
#############

install(DIRECTORY launch/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch)
install(DIRECTORY config/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/config)

## Mark executables and/or libraries for installation
install(TARGETS ur_driver ur_hardware_interface
Expand Down
18 changes: 18 additions & 0 deletions README.md
Original file line number Diff line number Diff line change
Expand Up @@ -39,6 +39,8 @@ A new driver for the UR3/UR5/UR10 robot arms from Universal Robots. It is design

## Installation

**As the driver communicates with the robot via ethernet and depends on reliable continous communication, it is not possible to reliably control a UR from a virtual machine.**

Just clone the repository into your catkin working directory and make it with ```catkin_make```.

Note that this package depends on ur_msgs, hardware_interface, and controller_manager so it cannot directly be used with ROS versions prior to hydro.
Expand All @@ -49,6 +51,21 @@ The driver is designed to be a drop-in replacement of the ur\_driver package. It

If you want to test it in your current setup, just use the modified launch files included in this package instead of those in ur\_bringup. Everything else should work as usual.

If you would like to run this package to connect to the hardware, you only need to run the following launch file.
```
roslaunch ur_modern_driver urXX_bringup.launch robot_ip:=ROBOT_IP_ADDRESS
```

Where ROBOT_IP_ADDRESS is your UR arm's IP and XX is '5' or '10' depending on your robot. The above launch file makes calls to both roscore and the launch file to the urXX_description so that ROS's parameter server has information on your robot arm. If you do not have your ```ur_description``` installed please do so via:
```
sudo apt install ros-<distro>-ur-description
```

Where <distro> is the ROS distribution your machine is running on. You may want to run MoveIt to plan and execute actions on the arm. You can do so by simply entering the following commands after launching ```ur_modern_driver```:
```
roslaunch urXX_moveit_config ur5_moveit_planning_executing.launch
roslaunch urXX_moveit_config moveit_rviz.launch config:=true
```
---
If you would like to use the ros\_control-based approach, use the launch files urXX\_ros\_control.launch, where XX is '5' or '10' depending on your robot.

Expand Down Expand Up @@ -143,3 +160,4 @@ Please cite the following report if using this driver


The report can be downloaded from http://orbit.dtu.dk/en/publications/optimizing-the-universal-robots-ros-driver(20dde139-7e87-4552-8658-dbf2cdaab24b).html

1 change: 1 addition & 0 deletions include/ur_modern_driver/ur_realtime_communication.h
Original file line number Diff line number Diff line change
Expand Up @@ -60,6 +60,7 @@ class UrRealtimeCommunication {
public:
bool connected_;
RobotStateRT* robot_state_;
std::mutex mutex_;

UrRealtimeCommunication(std::condition_variable& msg_cond, std::string host,
unsigned int safety_count_max = 12);
Expand Down
6 changes: 3 additions & 3 deletions launch/ur3_ros_control.launch
Original file line number Diff line number Diff line change
Expand Up @@ -10,7 +10,7 @@
<arg name="limited" default="false"/>
<arg name="min_payload" default="0.0"/>
<arg name="max_payload" default="3.0"/>
<arg name="prefix" default="" />
<arg name="prefix" default="" />
<arg name="max_velocity" default="10.0"/> <!-- [rad/s] -->
<arg name="base_frame" default="$(arg prefix)base" />
<arg name="tool_frame" default="$(arg prefix)tool0_controller" />
Expand Down Expand Up @@ -38,11 +38,11 @@

<!-- spawn controller manager -->
<node name="ros_control_controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
output="screen" args="joint_state_controller force_torque_sensor_controller velocity_based_position_trajectory_controller" />
output="screen" args="joint_state_controller force_torque_sensor_controller pos_based_pos_traj_controller" />

<!-- load other controller -->
<node name="ros_control_controller_manager" pkg="controller_manager" type="controller_manager" respawn="false"
output="screen" args="load position_based_position_trajectory_controller" />
output="screen" args="load vel_based_pos_traj_controller" />

<!-- Convert joint states to /tf tranforms -->
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/>
Expand Down
3 changes: 1 addition & 2 deletions package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,7 @@
<version>0.0.1</version>
<description>The new driver for the UR3/UR5/UR10 robot arms from universal robots</description>

<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="thomas.timm.dk@gmail.com">Thomas Timm Andersen</maintainer>
Expand Down Expand Up @@ -55,7 +55,6 @@
<build_depend>control_panel_ur</build_depend>
<run_depend>hardware_interface</run_depend>
<run_depend>controller_manager</run_depend>
<run_depend>ros_controllers</run_depend>
<run_depend>actionlib</run_depend>
<run_depend>control_msgs</run_depend>
<run_depend>geometry_msgs</run_depend>
Expand Down
40 changes: 22 additions & 18 deletions src/robot_state_RT.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -338,26 +338,30 @@ void RobotStateRT::unpack(uint8_t * buf) {
len = ntohl(len);

//Check the correct message length is received
bool len_good = true;
if (version_ >= 1.6 && version_ < 1.7) { //v1.6
if (len != 756)
len_good = false;
} else if (version_ >= 1.7 && version_ < 1.8) { //v1.7
if (len != 764)
len_good = false;
} else if (version_ >= 1.8 && version_ < 1.9) { //v1.8
if (len != 812)
len_good = false;
} else if (version_ >= 3.0 && version_ < 3.2) { //v3.0 & v3.1
if (len != 1044)
len_good = false;
} else if (version_ >= 3.2 && version_ < 3.3) { //v3.2
if (len != 1060)
len_good = false;
}
bool len_good = false;
if (version_ >= 1.6 && version_ < 1.7) { //v1.6
if (len == 756)
len_good = true;
} else if (version_ >= 1.7 && version_ < 1.8) { //v1.7
if (len == 764)
len_good = true;
} else if (version_ >= 1.8 && version_ < 1.9) { //v1.8
if (len == 812)
len_good = true;
} else if (version_ >= 3.0 && version_ < 3.2) { //v3.0 & v3.1
if (len == 1044)
len_good = true;
} else if (version_ >= 3.2 && version_ < 3.5) { //v3.2 ~ v3.4
if (len == 1060)
len_good = true;
} else if (version_ >= 3.5 && version_ < 3.6) { //v3.5
if (len == 1108)
len_good = true;
}

if (!len_good) {
printf("Wrong length of message on RT interface: %i\n", len);
if (len != 0 && std::abs(len) <= 100000) // ignoring empty and often repeating wrong messages.
printf("Wrong length of message on RT interface: %i\n", len);
val_lock_.unlock();
return;
}
Expand Down
6 changes: 5 additions & 1 deletion src/ur_driver.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -112,7 +112,11 @@ bool UrDriver::doTraj(std::vector<double> inp_timestamps,
executing_traj_ = false;
//Signal robot to stop driverProg()
UrDriver::closeServo(positions);
return true;

if(inp_timestamps[inp_timestamps.size() - 1] < std::chrono::duration_cast<std::chrono::duration<double>>(t - t0).count())
return true;
else
return false;
}

void UrDriver::servoj(std::vector<double> positions, int keepalive) {
Expand Down
9 changes: 8 additions & 1 deletion src/ur_realtime_communication.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -85,6 +85,8 @@ void UrRealtimeCommunication::halt() {
}

void UrRealtimeCommunication::addCommandToQueue(std::string inp) {
std::unique_lock<std::mutex> lock(mutex_);

int bytes_written;
if (inp.back() != '\n') {
inp.append("\n");
Expand All @@ -98,7 +100,12 @@ void UrRealtimeCommunication::addCommandToQueue(std::string inp) {
void UrRealtimeCommunication::setSpeed(double q0, double q1, double q2,
double q3, double q4, double q5, double acc) {
char cmd[1024];
if( robot_state_->getVersion() >= 3.1 ) {
if( robot_state_->getVersion() >= 3.3 ) {
sprintf(cmd,
"speedj([%1.5f, %1.5f, %1.5f, %1.5f, %1.5f, %1.5f], %f, 0.008)\n",
q0, q1, q2, q3, q4, q5, acc);
}
else if( robot_state_->getVersion() >= 3.1 ) {
sprintf(cmd,
"speedj([%1.5f, %1.5f, %1.5f, %1.5f, %1.5f, %1.5f], %f)\n",
q0, q1, q2, q3, q4, q5, acc);
Expand Down
74 changes: 55 additions & 19 deletions src/ur_ros_wrapper.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -67,9 +67,7 @@ class RosWrapper {
ros::NodeHandle nh_;
actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction> as_;
actionlib::ServerGoalHandle<control_msgs::FollowJointTrajectoryAction> goal_handle_;
bool has_goal_;
control_msgs::FollowJointTrajectoryFeedback feedback_;
control_msgs::FollowJointTrajectoryResult result_;

ros::Subscriber speed_sub_;
ros::Subscriber urscript_sub_;
ros::ServiceServer io_srv_;
Expand All @@ -86,6 +84,20 @@ class RosWrapper {
boost::shared_ptr<ros_control_ur::UrHardwareInterface> hardware_interface_;
boost::shared_ptr<controller_manager::ControllerManager> controller_manager_;

// Thread semantics:
// * cancelCB and goalCB can be called simultaneously by the action server.
// We prevent that by locking the as_mutex_.
// * The has_goal_ variable is protected by the goal_mutex_, as it is
// accessed by trajThread() and the callbacks.

//std::mutex goal_mutex_;
bool has_goal_;
std::thread traj_thread_;
control_msgs::FollowJointTrajectoryFeedback feedback_;
control_msgs::FollowJointTrajectoryResult result_;

//std::mutex as_mutex_;

public:
RosWrapper(std::string host, int reverse_port) :
as_(nh_, "follow_joint_trajectory",
Expand Down Expand Up @@ -226,20 +238,28 @@ class RosWrapper {

}
private:
void trajThread(std::vector<double> timestamps,
void trajThread(
std::vector<double> timestamps,
std::vector<std::vector<double> > positions,
std::vector<std::vector<double> > velocities) {
std::vector<std::vector<double> > velocities)
{
bool finished = robot_.doTraj(timestamps, positions, velocities);

robot_.doTraj(timestamps, positions, velocities);
if (has_goal_) {
if (finished)
{
//std::unique_lock<std::mutex> lock(goal_mutex_);
result_.error_code = result_.SUCCESSFUL;
goal_handle_.setSucceeded(result_);
has_goal_ = false;
}
}

void goalCB(
actionlib::ServerGoalHandle<
control_msgs::FollowJointTrajectoryAction> gh) {

//std::unique_lock<std::mutex> asLock(as_mutex_);

std::string buf;
print_info("on_goal");
if (!robot_.sec_interface_->robot_state_->isReady()) {
Expand Down Expand Up @@ -287,16 +307,27 @@ class RosWrapper {

actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal =
*gh.getGoal(); //make a copy that we can modify
if (has_goal_) {
print_warning(
"Received new goal while still executing previous trajectory. Canceling previous trajectory");
has_goal_ = false;
robot_.stopTraj();
result_.error_code = -100; //nothing is defined for this...?
result_.error_string = "Received another trajectory";
goal_handle_.setAborted(result_, result_.error_string);
std::this_thread::sleep_for(std::chrono::milliseconds(250));

{
//std::unique_lock<std::mutex> lock(goal_mutex_);
if (has_goal_) {
print_warning(
"Received new goal while still executing previous trajectory. Canceling previous trajectory");
has_goal_ = false;
robot_.stopTraj();

result_.error_code = -100; //nothing is defined for this...?
result_.error_string = "Received another trajectory";
goal_handle_.setAborted(result_, result_.error_string);
std::this_thread::sleep_for(std::chrono::milliseconds(250));
}
}

if(traj_thread_.joinable())
traj_thread_.join();

//std::unique_lock<std::mutex> lock(goal_mutex_);

goal_handle_ = gh;
if (!validateJointNames()) {
std::string outp_joint_names = "";
Expand Down Expand Up @@ -377,21 +408,26 @@ class RosWrapper {

goal_handle_.setAccepted();
has_goal_ = true;
std::thread(&RosWrapper::trajThread, this, timestamps, positions,
velocities).detach();
traj_thread_ = std::thread(&RosWrapper::trajThread, this, timestamps, positions,
velocities);
}

void cancelCB(
actionlib::ServerGoalHandle<
control_msgs::FollowJointTrajectoryAction> gh) {
// set the action state to preempted

//std::unique_lock<std::mutex> asLock(as_mutex_);
//std::unique_lock<std::mutex> lock(goal_mutex_);

print_info("on_cancel");
if (has_goal_) {
if (gh == goal_handle_) {
robot_.stopTraj();
has_goal_ = false;
robot_.stopTraj();
}
}

result_.error_code = -100; //nothing is defined for this...?
result_.error_string = "Goal cancelled by client";
gh.setCanceled(result_);
Expand Down