Changed time base for ros_control. Fixes #44

ros-industrial · May 3, 2016 · 5b22e46 · 5b22e46
1 parent c93d9dd
commit 5b22e46
Show file tree

Hide file tree

Showing 2 changed files with 13 additions and 10 deletions.
diff --git a/src/ur_ros_wrapper.cpp b/src/ur_ros_wrapper.cpp
@@ -560,19 +560,15 @@ class RosWrapper {
 			while (!robot_.rt_interface_->robot_state_->getControllerUpdated()) {
 				rt_msg_cond_.wait(locker);
 			}
-			clock_gettime(CLOCK_MONOTONIC, &current_time);
-			elapsed_time = ros::Duration(
-					current_time.tv_sec - last_time.tv_sec
-							+ (current_time.tv_nsec - last_time.tv_nsec)
-									/ BILLION);
-			last_time = current_time;
 			// Input
 			hardware_interface_->read();
 			robot_.rt_interface_->robot_state_->setControllerUpdated();
+
 			// Control
-			controller_manager_->update(
-					ros::Time(current_time.tv_sec, current_time.tv_nsec),
-					elapsed_time);
+			clock_gettime(CLOCK_MONOTONIC, &current_time);
+			elapsed_time = ros::Duration(current_time.tv_sec - last_time.tv_sec + (current_time.tv_nsec - last_time.tv_nsec)/ BILLION);
+			controller_manager_->update(ros::Time::now(), elapsed_time);
+			last_time = current_time;
 
 			// Output
 			hardware_interface_->write();

diff --git a/test_move.py b/test_move.py
@@ -102,8 +102,15 @@ def move_interrupt():
             JointTrajectoryPoint(positions=Q3, velocities=[0]*6, time_from_start=rospy.Duration(4.0))]
 
         client.send_goal(g)
-        time.sleep(2.0)
+        time.sleep(3.0)
         print "Interrupting"
+        joint_states = rospy.wait_for_message("joint_states", JointState)
+        joints_pos = joint_states.position
+        g.trajectory.points = [
+            JointTrajectoryPoint(positions=joints_pos, velocities=[0]*6, time_from_start=rospy.Duration(0.0)),
+            JointTrajectoryPoint(positions=Q1, velocities=[0]*6, time_from_start=rospy.Duration(2.0)),
+            JointTrajectoryPoint(positions=Q2, velocities=[0]*6, time_from_start=rospy.Duration(3.0)),
+            JointTrajectoryPoint(positions=Q3, velocities=[0]*6, time_from_start=rospy.Duration(4.0))]
         client.send_goal(g)
         client.wait_for_result()
     except KeyboardInterrupt: