Merge pull request #9 from smart-robotics/fix/driver

Fix/driver

CMakeLists.txt

@@ -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}
 )
 
@@ -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

README.md

@@ -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.
@@ -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.
 
@@ -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
+

include/ur_modern_driver/ur_realtime_communication.h

@@ -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);

launch/ur3_ros_control.launch

@@ -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" />
@@ -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"/>

package.xml

@@ -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="[email protected]">Jane Doe</maintainer> -->
   <maintainer email="[email protected]">Thomas Timm Andersen</maintainer>
@@ -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>

src/robot_state_RT.cpp

@@ -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;
 	}

src/ur_driver.cpp

@@ -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) {

src/ur_realtime_communication.cpp

@@ -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");
@@ -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);

src/ur_ros_wrapper.cpp

@@ -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_;
@@ -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",
@@ -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()) {
@@ -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 = "";
@@ -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_);