Merge remote-tracking branch 'origin/master' into development

IFL-CAMP · Nov 23, 2016 · a90a22e · a90a22e
2 parents ff1e465 + c63a3b6
commit a90a22e
Show file tree

Hide file tree

Showing 4 changed files with 33 additions and 5 deletions.
diff --git a/iiwa_control/launch/iiwa_control.launch b/iiwa_control/launch/iiwa_control.launch
@@ -14,7 +14,7 @@
 
     <!-- Loads the controllers -->
     <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
-          output="screen" args="$(arg controllers)" />
+          output="screen" args="$(arg controllers) --shutdown-timeout 2" />
 
     <!-- Converts joint states to TF transforms for rviz, etc -->
     <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"

diff --git a/iiwa_description/urdf/iiwa.transmission.xacro b/iiwa_description/urdf/iiwa.transmission.xacro
@@ -4,6 +4,7 @@
   <xacro:macro name="iiwa_transmission" params="hardware_interface">
 
     <transmission name="iiwa_tran_1">
+      <robotNamespace>/${robot_name}</robotNamespace>
       <type>transmission_interface/SimpleTransmission</type>
       <joint name="iiwa_joint_1">
 	<hardwareInterface>${hardware_interface}</hardwareInterface>
@@ -15,6 +16,7 @@
     </transmission>
 
     <transmission name="iiwa_tran_2">
+      <robotNamespace>/${robot_name}</robotNamespace>
       <type>transmission_interface/SimpleTransmission</type>
       <joint name="iiwa_joint_2">
 	<hardwareInterface>${hardware_interface}</hardwareInterface>
@@ -26,6 +28,7 @@
     </transmission>
 
     <transmission name="iiwa_tran_3">
+      <robotNamespace>/${robot_name}</robotNamespace>
       <type>transmission_interface/SimpleTransmission</type>
       <joint name="iiwa_joint_3">
 	<hardwareInterface>${hardware_interface}</hardwareInterface>
@@ -37,6 +40,7 @@
     </transmission>
 
     <transmission name="iiwa_tran_4">
+      <robotNamespace>/${robot_name}</robotNamespace>
       <type>transmission_interface/SimpleTransmission</type>
       <joint name="iiwa_joint_4">
 	<hardwareInterface>${hardware_interface}</hardwareInterface>
@@ -48,6 +52,7 @@
     </transmission>
 
     <transmission name="iiwa_tran_5">
+      <robotNamespace>/${robot_name}</robotNamespace>
       <type>transmission_interface/SimpleTransmission</type>
       <joint name="iiwa_joint_5">
 	<hardwareInterface>${hardware_interface}</hardwareInterface>
@@ -59,6 +64,7 @@
     </transmission>
 
     <transmission name="iiwa_tran_6">
+      <robotNamespace>/${robot_name}</robotNamespace>
       <type>transmission_interface/SimpleTransmission</type>
       <joint name="iiwa_joint_6">
 	<hardwareInterface>${hardware_interface}</hardwareInterface>
@@ -70,6 +76,7 @@
     </transmission>
 
     <transmission name="iiwa_tran_7">
+      <robotNamespace>/${robot_name}</robotNamespace>
       <type>transmission_interface/SimpleTransmission</type>
       <joint name="iiwa_joint_7">
 	<hardwareInterface>${hardware_interface}</hardwareInterface>

diff --git a/iiwa_ros_java/src/de/tum/in/camp/kuka/ros/iiwaMessageGenerator.java b/iiwa_ros_java/src/de/tum/in/camp/kuka/ros/iiwaMessageGenerator.java
@@ -53,6 +53,9 @@ public class iiwaMessageGenerator {
 	private static String baseFrameID;
 	private static final String baseFrameIDSuffix = "_link_0";
 	private static String[] joint_names;
+
+	private static double[] last_position;
+	private static long last_position_time_ns = 0;
 
 	// Objects to create ROS messages
 	private NodeConfiguration nodeConf = NodeConfiguration.newPrivate();
@@ -158,11 +161,19 @@ public void getCurrentJointPosition(iiwa_msgs.JointPosition currentJointPosition
 	 * @param currentJointPositionVelocity : the JointPositionVelocity message that will be created.
 	 * @param robot : an iiwa Robot, its current state is used to set the values of the message.
 	 */
-	// TODO : should we keep this?
-	public void getCurrentJointPositionVelocity(iiwa_msgs.JointPositionVelocity currentJointPositionVelocity, LBR robot) {
+	public void getCurrentJointPositionVelocity(iiwa_msgs.JointPositionVelocity currentJointPositionVelocity, LBR robot) {		
 		double[] position = robot.getCurrentJointPosition().getInternalArray();
-		double[] velocity = new double[robot.getJointCount()];  // TODO: read current joint velocity
-
+		long position_time_ns = System.nanoTime();
+		double[] velocity = new double[robot.getJointCount()];  
+
+		if (last_position_time_ns != 0) {
+			for (int i = 0; i < robot.getJointCount(); i++)
+				velocity[i] = (position[i] - last_position[i]) / ((double)(position_time_ns - last_position_time_ns) / 1000000000);
+		}
+
+		last_position = position;
+		last_position_time_ns = position_time_ns;
+
 		currentJointPositionVelocity.getHeader().setStamp(time.getCurrentTime());
 
 		vectorToJointQuantity(position, currentJointPositionVelocity.getPosition());

diff --git a/iiwa_ros_java/src/de/tum/in/camp/kuka/ros/iiwaPublisher.java b/iiwa_ros_java/src/de/tum/in/camp/kuka/ros/iiwaPublisher.java
@@ -26,6 +26,7 @@
 import geometry_msgs.PoseStamped;
 import geometry_msgs.WrenchStamped;
 import iiwa_msgs.JointPosition;
+import iiwa_msgs.JointPositionVelocity;
 import iiwa_msgs.JointStiffness;
 import iiwa_msgs.JointTorque;
 
@@ -50,6 +51,7 @@ public class iiwaPublisher extends AbstractNodeMain {
 	private Publisher<geometry_msgs.WrenchStamped> cartesianWrenchPublisher;
 	// Joint Message Publishers
 	private Publisher<iiwa_msgs.JointPosition> jointPositionPublisher;
+	private Publisher<iiwa_msgs.JointPositionVelocity> jointPositionVelocityPublisher;
 	private Publisher<iiwa_msgs.JointStiffness> jointStiffnessPublisher;
 	private Publisher<iiwa_msgs.JointDamping> jointDampingPublisher;
 	private Publisher<iiwa_msgs.JointTorque> jointTorquePublisher;
@@ -69,6 +71,7 @@ public class iiwaPublisher extends AbstractNodeMain {
 	private geometry_msgs.PoseStamped cp;
 	private geometry_msgs.WrenchStamped cw;
 	private iiwa_msgs.JointPosition jp;
+	private iiwa_msgs.JointPositionVelocity jpv;
 	private iiwa_msgs.JointStiffness jst;
 	private iiwa_msgs.JointDamping jd;
 	private iiwa_msgs.JointTorque jt;
@@ -89,6 +92,7 @@ public iiwaPublisher(String robotName) {
 		cp = helper.buildMessage(PoseStamped._TYPE);
 		cw = helper.buildMessage(WrenchStamped._TYPE);
 		jp = helper.buildMessage(JointPosition._TYPE);
+		jpv = helper.buildMessage(JointPositionVelocity._TYPE);
 		jst = helper.buildMessage(JointStiffness._TYPE);
 		jd = helper.buildMessage(iiwa_msgs.JointDamping._TYPE);
 		jt = helper.buildMessage(JointTorque._TYPE);
@@ -132,6 +136,7 @@ public void onStart(final ConnectedNode connectedNode) {
 		cartesianWrenchPublisher = connectedNode.newPublisher(iiwaName + "/state/CartesianWrench", geometry_msgs.WrenchStamped._TYPE);
 
 		jointPositionPublisher = connectedNode.newPublisher(iiwaName + "/state/JointPosition", iiwa_msgs.JointPosition._TYPE);
+		jointPositionVelocityPublisher = connectedNode.newPublisher(iiwaName + "/state/JointPositionVelocity", iiwa_msgs.JointPositionVelocity._TYPE);
 		jointStiffnessPublisher = connectedNode.newPublisher(iiwaName + "/state/JointStiffness", iiwa_msgs.JointStiffness._TYPE);
 		jointDampingPublisher = connectedNode.newPublisher(iiwaName + "/state/JointDamping", iiwa_msgs.JointDamping._TYPE);
 		jointTorquePublisher = connectedNode.newPublisher(iiwaName + "/state/JointTorque", iiwa_msgs.JointTorque._TYPE);
@@ -178,6 +183,11 @@ public void publishCurrentState(LBR robot, SmartServo motion, ObjectFrame frame)
 			helper.incrementSeqNumber(jp.getHeader());
 			jointPositionPublisher.publish(jp);
 		}
+		if (jointPositionVelocityPublisher.getNumberOfSubscribers() > 0) {
+			helper.getCurrentJointPositionVelocity(jpv, robot);
+			helper.incrementSeqNumber(jpv.getHeader());
+			jointPositionVelocityPublisher.publish(jpv);
+		}
 		if (jointTorquePublisher.getNumberOfSubscribers() > 0) {
 			helper.getCurrentJointTorque(jt, robot);
 			helper.incrementSeqNumber(jt.getHeader());