From 3978dd3ed794dba69c03290471800141e8c19312 Mon Sep 17 00:00:00 2001 From: marshallpowell97 Date: Mon, 20 Jan 2020 12:45:36 -0500 Subject: [PATCH 01/14] Added velocity interpolation to smooth out jerk at last point in trajectory If Val3 driver recieves default value from industrial_robot_client (0.1), it replaces it with new value interpolated from previous velocities. May slow down robot at the end of trajectory if incorrect max velocities/accelerations are used in arm URDF file. --- .../val3/ros_server/.outlining.json | 101 ++++++++++++++++++ .../val3/ros_server/decTrajPt.pgx | 33 ++++-- .../val3/ros_server/decTrajPtFull.pgx | 37 ++++--- 3 files changed, 148 insertions(+), 23 deletions(-) create mode 100644 staubli_val3_driver/val3/ros_server/.outlining.json diff --git a/staubli_val3_driver/val3/ros_server/.outlining.json b/staubli_val3_driver/val3/ros_server/.outlining.json new file mode 100644 index 0000000..cbef38f --- /dev/null +++ b/staubli_val3_driver/val3/ros_server/.outlining.json @@ -0,0 +1,101 @@ +{ + "programs": [ + { + "mode": 3, + "name": "decTrajPt", + "nodes": [ + { + "start": 865, + "end": 918, + "isCollapsed": false + }, + { + "start": 938, + "end": 1155, + "isCollapsed": false + }, + { + "start": 1087, + "end": 1146, + "isCollapsed": false + }, + { + "start": 1387, + "end": 1440, + "isCollapsed": false + }, + { + "start": 1782, + "end": 1951, + "isCollapsed": false + }, + { + "start": 2065, + "end": 2118, + "isCollapsed": false + } + ] + }, + { + "mode": 3, + "name": "decTrajPtFull", + "nodes": [ + { + "start": 875, + "end": 928, + "isCollapsed": false + }, + { + "start": 1038, + "end": 1091, + "isCollapsed": false + }, + { + "start": 1213, + "end": 1266, + "isCollapsed": false + }, + { + "start": 1374, + "end": 1427, + "isCollapsed": false + }, + { + "start": 1450, + "end": 1677, + "isCollapsed": false + }, + { + "start": 1609, + "end": 1668, + "isCollapsed": false + }, + { + "start": 1701, + "end": 2580, + "isCollapsed": false + }, + { + "start": 1985, + "end": 2044, + "isCollapsed": false + }, + { + "start": 2398, + "end": 2571, + "isCollapsed": false + }, + { + "start": 2607, + "end": 2838, + "isCollapsed": false + }, + { + "start": 2770, + "end": 2829, + "isCollapsed": false + } + ] + } + ] +} \ No newline at end of file diff --git a/staubli_val3_driver/val3/ros_server/decTrajPt.pgx b/staubli_val3_driver/val3/ros_server/decTrajPt.pgx index 8cbd974..4fa2762 100644 --- a/staubli_val3_driver/val3/ros_server/decTrajPt.pgx +++ b/staubli_val3_driver/val3/ros_server/decTrajPt.pgx @@ -1,8 +1,8 @@ - - - - - + + + + + @@ -32,7 +32,7 @@ // sequence l_nRetVal=fromBinary(rosTrajPtMsg.body.nData,4,"-4l",rosTrajPtMsg.jointTrajPt.nSeq) - if (l_nRetVal!=1) + if(l_nRetVal!=1) x_bFlag=false return endIf @@ -40,22 +40,35 @@ // joints[10] for l_nIndex=0 to 9 step 1 l_nRetVal=fromBinary(rosTrajPtMsg.body.nData[(l_nIndex*4)+4],4,"4.0l",rosTrajPtMsg.jointTrajPt.nJoints[l_nIndex]) - if (l_nRetVal!=1) + if(l_nRetVal!=1) x_bFlag=false return endIf endFor + // Velocity buffer to smooth out last traj point + nVel[1] = nVel[0] + nVel[0] = rosTrajPtMsg.jointTrajPt.nVelocity + // velocity l_nRetVal=fromBinary(rosTrajPtMsg.body.nData[44],4,"4.0l",rosTrajPtMsg.jointTrajPt.nVelocity) - if (l_nRetVal!=1) + if(l_nRetVal!=1) x_bFlag=false return endIf + // Round to remove trailing decimal places from fromBinary() + rosTrajPtMsg.jointTrajPt.nVelocity = rosTrajPtMsg.jointTrajPt.nVelocity * 10000 + rosTrajPtMsg.jointTrajPt.nVelocity = round(rosTrajPtMsg.jointTrajPt.nVelocity) + rosTrajPtMsg.jointTrajPt.nVelocity = rosTrajPtMsg.jointTrajPt.nVelocity / 10000 + // Smooth last point in traj + if((rosTrajPtMsg.jointTrajPt.nVelocity == 0.1) and (rosTrajPtMsg.jointTrajPt.nSeq != 0)) + rosTrajPtMsg.jointTrajPt.nVelocity = (nVel[0] - (nVel[1] - nVel[0])) + endIf + // duration l_nRetVal=fromBinary(rosTrajPtMsg.body.nData[48],4,"4.0l",rosTrajPtMsg.jointTrajPt.nDuration) - if (l_nRetVal!=1) + if(l_nRetVal!=1) x_bFlag=false return endIf @@ -66,4 +79,4 @@ x_bFlag=true end]]> - + \ No newline at end of file diff --git a/staubli_val3_driver/val3/ros_server/decTrajPtFull.pgx b/staubli_val3_driver/val3/ros_server/decTrajPtFull.pgx index 1317294..d303c84 100644 --- a/staubli_val3_driver/val3/ros_server/decTrajPtFull.pgx +++ b/staubli_val3_driver/val3/ros_server/decTrajPtFull.pgx @@ -1,8 +1,8 @@ - - - - - + + + + + @@ -32,28 +32,28 @@ // robotId l_nRetVal=fromBinary(rosTrajPtFMsg.body.nData,4,"-4l",rosTrajPtFMsg.jointTrajPtFull.nRobotId) - if (l_nRetVal!=1) + if(l_nRetVal!=1) x_bFlag=false return endIf // sequence l_nRetVal=fromBinary(rosTrajPtFMsg.body.nData,4,"-4l",rosTrajPtFMsg.jointTrajPtFull.nSeq) - if (l_nRetVal!=1) + if(l_nRetVal!=1) x_bFlag=false return endIf // valid fields l_nRetVal=fromBinary(rosTrajPtFMsg.body.nData,4,"-4l",rosTrajPtFMsg.jointTrajPtFull.nValidFields) - if (l_nRetVal!=1) + if(l_nRetVal!=1) x_bFlag=false return endIf // time l_nRetVal=fromBinary(rosTrajPtFMsg.body.nData,4,"4.0l",rosTrajPtFMsg.jointTrajPtFull.nTime) - if (l_nRetVal!=1) + if(l_nRetVal!=1) x_bFlag=false return endIf @@ -61,7 +61,7 @@ // positions[10] for l_nIndex=0 to 9 step 1 l_nRetVal=fromBinary(rosTrajPtFMsg.body.nData[(l_nIndex*4)+16],4,"4.0l",rosTrajPtFMsg.jointTrajPtFull.nPositions[l_nIndex]) - if (l_nRetVal!=1) + if(l_nRetVal!=1) x_bFlag=false return endIf @@ -69,17 +69,28 @@ // velocities[10] for l_nIndex=0 to 9 step 1 + // Velocity buffer to smooth out last traj point + nVel[1] = nVel[0] + nVel[0] = rosTrajPtMsg.jointTrajPt.nVelocity l_nRetVal=fromBinary(rosTrajPtFMsg.body.nData[(l_nIndex*4)+56],4,"4.0l",rosTrajPtFMsg.jointTrajPtFull.nVelocities[l_nIndex]) - if (l_nRetVal!=1) + if(l_nRetVal!=1) x_bFlag=false return endIf + // Round to remove trailing decimal places from fromBinary() + rosTrajPtMsg.jointTrajPt.nVelocity = rosTrajPtMsg.jointTrajPt.nVelocity * 10000 + rosTrajPtMsg.jointTrajPt.nVelocity = round(rosTrajPtMsg.jointTrajPt.nVelocity) + rosTrajPtMsg.jointTrajPt.nVelocity = rosTrajPtMsg.jointTrajPt.nVelocity / 10000 + // Smooth last point in traj + if((rosTrajPtMsg.jointTrajPt.nVelocity == 0.1) and (rosTrajPtMsg.jointTrajPt.nSeq != 0)) + rosTrajPtMsg.jointTrajPt.nVelocity = (nVel[0] - (nVel[1] - nVel[0])) + endIf endFor // accelerations[10] for l_nIndex=0 to 9 step 1 l_nRetVal=fromBinary(rosTrajPtFMsg.body.nData[(l_nIndex*4)+96],4,"4.0l",rosTrajPtFMsg.jointTrajPtFull.nAccelerations[l_nIndex]) - if (l_nRetVal!=1) + if(l_nRetVal!=1) x_bFlag=false return endIf @@ -91,4 +102,4 @@ x_bFlag=true end]]> - + \ No newline at end of file From 4cbb7f92143987360ae85ea0bcb825275c08acf0 Mon Sep 17 00:00:00 2001 From: marshallpowell97 Date: Mon, 20 Jan 2020 16:38:30 -0500 Subject: [PATCH 02/14] Remove magic numbers, unused variables --- .../val3/ros_server/.outlining.json | 137 ++++++++++++++++++ .../val3/ros_server/dataReceiver.pgx | 18 +-- .../val3/ros_server/decodeMsgBody.pgx | 16 +- .../val3/ros_server/decodeMsgHeader.pgx | 12 +- .../val3/ros_server/encodeAck.pgx | 12 +- .../val3/ros_server/motionControl.pgx | 4 +- .../val3/ros_server/pushMotion.pgx | 32 ++-- .../val3/ros_server/recvHeartbeat.pgx | 4 +- .../val3/ros_server/recvMsgBody.pgx | 10 +- .../val3/ros_server/recvMsgHeader.pgx | 6 +- .../val3/ros_server/ros_server.dtx | 20 ++- .../val3/ros_server/sendAck.pgx | 8 +- 12 files changed, 214 insertions(+), 65 deletions(-) diff --git a/staubli_val3_driver/val3/ros_server/.outlining.json b/staubli_val3_driver/val3/ros_server/.outlining.json index cbef38f..863b444 100644 --- a/staubli_val3_driver/val3/ros_server/.outlining.json +++ b/staubli_val3_driver/val3/ros_server/.outlining.json @@ -96,6 +96,143 @@ "isCollapsed": false } ] + }, + { + "mode": 3, + "name": "dataReceiver", + "nodes": [ + { + "start": 869, + "end": 2334, + "isCollapsed": false + }, + { + "start": 1488, + "end": 1584, + "isCollapsed": false + }, + { + "start": 1630, + "end": 1728, + "isCollapsed": false + }, + { + "start": 1733, + "end": 1818, + "isCollapsed": false + }, + { + "start": 1823, + "end": 1898, + "isCollapsed": false + }, + { + "start": 1903, + "end": 1975, + "isCollapsed": false + }, + { + "start": 1980, + "end": 2072, + "isCollapsed": false + }, + { + "start": 2180, + "end": 2286, + "isCollapsed": false + } + ] + }, + { + "mode": 3, + "name": "pushMotion", + "nodes": [ + { + "start": 660, + "end": 2357, + "isCollapsed": false + }, + { + "start": 701, + "end": 1410, + "isCollapsed": false + }, + { + "start": 1415, + "end": 2323, + "isCollapsed": false + }, + { + "start": 2328, + "end": 2345, + "isCollapsed": false + }, + { + "start": 2466, + "end": 3895, + "isCollapsed": false + }, + { + "start": 2550, + "end": 3887, + "isCollapsed": false + }, + { + "start": 2649, + "end": 2942, + "isCollapsed": false + }, + { + "start": 3155, + "end": 3316, + "isCollapsed": false + }, + { + "start": 3282, + "end": 3306, + "isCollapsed": false + }, + { + "start": 3326, + "end": 3887, + "isCollapsed": false + }, + { + "start": 3474, + "end": 3877, + "isCollapsed": false + }, + { + "start": 3553, + "end": 3865, + "isCollapsed": false + }, + { + "start": 3600, + "end": 3698, + "isCollapsed": false + }, + { + "start": 3709, + "end": 3813, + "isCollapsed": false + }, + { + "start": 3824, + "end": 3847, + "isCollapsed": false + }, + { + "start": 3936, + "end": 4218, + "isCollapsed": false + }, + { + "start": 4088, + "end": 4210, + "isCollapsed": false + } + ] } ] } \ No newline at end of file diff --git a/staubli_val3_driver/val3/ros_server/dataReceiver.pgx b/staubli_val3_driver/val3/ros_server/dataReceiver.pgx index c73b43a..5740a47 100644 --- a/staubli_val3_driver/val3/ros_server/dataReceiver.pgx +++ b/staubli_val3_driver/val3/ros_server/dataReceiver.pgx @@ -29,7 +29,7 @@ // 3: decode body // 4: push motion into buffer // 5: send ACK - l_nMsgRecvState=0 + l_nMsgRecvState=nMsgState["RecHeader"] while (true) // usually for state machines a switch-case statement is a neat implementation @@ -41,30 +41,30 @@ // into buffer and then send ACK to client WITHIN ONE ITERATION of while(), // hence the if statement sequence below l_nStartTime = clock() - if (l_nMsgRecvState==0) + if (l_nMsgRecvState==nMsgState["RecHeader"]) call recvMsgHeader(l_nMsgRecvState) endIf nHeaderTime = clock() - l_nStartTime - if (l_nMsgRecvState==1) + if (l_nMsgRecvState==nMsgState["DecHeader"]) call decodeMsgHeader(l_nMsgRecvState) endIf - if (l_nMsgRecvState==2) + if (l_nMsgRecvState==nMsgState["RecBody"]) call recvMsgBody(l_nMsgRecvState) endIf - if (l_nMsgRecvState==3) + if (l_nMsgRecvState==nMsgState["DecBody"]) call decodeMsgBody(l_nMsgRecvState) endIf - if (l_nMsgRecvState==4) + if (l_nMsgRecvState==nMsgState["PushMotion"]) call pushMotion(l_nMsgRecvState) endIf - if (l_nMsgRecvState==5) + if (l_nMsgRecvState==nMsgState["Ack"]) call encodeAck() call sendAck(l_nMsgRecvState) endIf nElapsedTime = clock() - l_nStartTime - nHeaderTime // assert state machine is working OK - if (l_nMsgRecvState<0 or l_nMsgRecvState>5) + if (l_nMsgRecvStatenMsgState["Ack"]) popUpMsg("Data reception state machine error") endIf @@ -75,4 +75,4 @@ endWhile end]]> - + \ No newline at end of file diff --git a/staubli_val3_driver/val3/ros_server/decodeMsgBody.pgx b/staubli_val3_driver/val3/ros_server/decodeMsgBody.pgx index dd7ae26..7e0b0a3 100644 --- a/staubli_val3_driver/val3/ros_server/decodeMsgBody.pgx +++ b/staubli_val3_driver/val3/ros_server/decodeMsgBody.pgx @@ -27,10 +27,10 @@ l_bFlag=false switch rosGenericMsg.header.nMsgType - case 11 + case nGenericMsgType["JOINT_TRAJ_PT"] call decTrajPt(l_bFlag) break - case 14 + case nGenericMsgType["JOINT_TRAJ_PT_FULL"] call decTrajPtFull(l_bFlag) break default @@ -39,27 +39,27 @@ if (l_bFlag == true) // move on to state 4: push motion into buffer - x_nMsgRecvState=4 + x_nMsgRecvState=nMsgState["PushMotion"] else // error whilst decoding msg body... if (rosGenericMsg.header.nCommType==2) switch rosGenericMsg.header.nMsgType - case 11 + case nGenericMsgType["JOINT_TRAJ_PT"] rosTrajPtAck.header.nReplyCode=2 break - case 14 + case nGenericMsgType["JOINT_TRAJ_PT_FULL"] rosTrajPtFAck.header.nReplyCode=2 break default break endSwitch // jump to state 5: sendAck() - x_nMsgRecvState=5 + x_nMsgRecvState=nMsgState["Ack"] else // reset state machine, since no ACK is required - x_nMsgRecvState=0 + x_nMsgRecvState=nMsgState["Reset"] endIf endIf end]]> - + \ No newline at end of file diff --git a/staubli_val3_driver/val3/ros_server/decodeMsgHeader.pgx b/staubli_val3_driver/val3/ros_server/decodeMsgHeader.pgx index 6bfd597..ecdff4d 100644 --- a/staubli_val3_driver/val3/ros_server/decodeMsgHeader.pgx +++ b/staubli_val3_driver/val3/ros_server/decodeMsgHeader.pgx @@ -28,31 +28,31 @@ l_nRetVal=fromBinary(rosGenericMsg.prefix.nData,4,"-4l",rosGenericMsg.prefix.nLength) if (l_nRetVal!=1) // error decoding prefix, reset state machine - x_nMsgRecvState=0 + x_nMsgRecvState=nMsgState["Reset"] return endIf l_nRetVal=fromBinary(rosGenericMsg.header.nData,4,"-4l",rosGenericMsg.header.nMsgType) if (l_nRetVal!=1) // error decoding header (msg type), reset state machine - x_nMsgRecvState=0 + x_nMsgRecvState=nMsgState["Reset"] return endIf l_nRetVal=fromBinary(rosGenericMsg.header.nData[4],4,"-4l",rosGenericMsg.header.nCommType) if (l_nRetVal!=1) // error decoding header (comm type), reset state machine - x_nMsgRecvState=0 + x_nMsgRecvState=nMsgState["Reset"] return endIf l_nRetVal=fromBinary(rosGenericMsg.header.nData[8],4,"-4l",rosGenericMsg.header.nReplyCode) if (l_nRetVal!=1) // error decoding header (reply code), reset state machine - x_nMsgRecvState=0 + x_nMsgRecvState=nMsgState["Reset"] return endIf // no errors detected whilst decoding prefix and header // move on to state 2: receive msg body - x_nMsgRecvState=2 + x_nMsgRecvState=nMsgState["RecBody"] end]]> - + \ No newline at end of file diff --git a/staubli_val3_driver/val3/ros_server/encodeAck.pgx b/staubli_val3_driver/val3/ros_server/encodeAck.pgx index 300fbdd..1dfdd10 100644 --- a/staubli_val3_driver/val3/ros_server/encodeAck.pgx +++ b/staubli_val3_driver/val3/ros_server/encodeAck.pgx @@ -1,6 +1,6 @@ - - - + + + - + \ No newline at end of file diff --git a/staubli_val3_driver/val3/ros_server/motionControl.pgx b/staubli_val3_driver/val3/ros_server/motionControl.pgx index 9f18d4b..4ec0f51 100644 --- a/staubli_val3_driver/val3/ros_server/motionControl.pgx +++ b/staubli_val3_driver/val3/ros_server/motionControl.pgx @@ -5,8 +5,6 @@ - - - + \ No newline at end of file diff --git a/staubli_val3_driver/val3/ros_server/pushMotion.pgx b/staubli_val3_driver/val3/ros_server/pushMotion.pgx index dedcb37..07242e1 100644 --- a/staubli_val3_driver/val3/ros_server/pushMotion.pgx +++ b/staubli_val3_driver/val3/ros_server/pushMotion.pgx @@ -28,7 +28,7 @@ switch rosGenericMsg.header.nMsgType - case 11 + case nGenericMsgType["JOINT_TRAJ_PT"] l_mTrajPt.nSequence=rosTrajPtMsg.jointTrajPt.nSeq // ROS joint position values are in rad, but VAL3 uses degrees @@ -43,7 +43,7 @@ l_mTrajPt.nDuration=rosTrajPtMsg.jointTrajPt.nDuration break - case 14 + case nGenericMsgType["JOINT_TRAJ_PT_FULL"] l_mTrajPt.nSequence=rosTrajPtFMsg.jointTrajPtFull.nSeq l_mTrajPt.nDuration=rosTrajPtFMsg.jointTrajPtFull.nTime @@ -65,36 +65,36 @@ endSwitch // see github.com/ros-industrial/industrial_core/simple_message/include/simple_message/joint_traj_pt.h - if (l_mTrajPt.nSequence==-4) + if(l_mTrajPt.nSequence==-4) // set flag to stop immediately bStopNow=true else // check whether velocity value should be overwritten with value set on teach pendant - if (bOverwriteVel == true) + if(bOverwriteVel == true) // overwrite velocity (and acceleration - ?) - l_mTrajPt.mDesc.vel = getMonitorSpeed() + //l_mTrajPt.mDesc.vel = getMonitorSpeed() // explicitly setting accel and decel to VAL3 default values - l_mTrajPt.mDesc.accel = 9999 - l_mTrajPt.mDesc.decel = 9999 + l_mTrajPt.mDesc.accel = mNomSpeed.accel + l_mTrajPt.mDesc.decel = mNomSpeed.decel endIf // only push motion into buffer if not a STOP message // and if joint positions are within software joint limits // and if velocity is greater than zero (beucase movej() will not accept vel <= 0) - if (isInRange(l_mTrajPt.jJointRx) and l_mTrajPt.mDesc.vel > 0) + if(isInRange(l_mTrajPt.jJointRx) and l_mTrajPt.mDesc.vel > 0) call libQueueFuncs:push(qTrajPtBuffer,l_mTrajPt,l_bOk) else l_bOk = false endIf - if (l_bOk==false) + if(l_bOk==false) // could not push trajectory point into buffer // thus ACK should flag FAILURE, but only if comm type is request - if (rosGenericMsg.header.nCommType==2) + if(rosGenericMsg.header.nCommType==nGenericMsgType["SERVICE_REQUEST"]) switch rosGenericMsg.header.nMsgType - case 11 + case nGenericMsgType["JOINT_TRAJ_PT"] rosTrajPtAck.header.nReplyCode=2 break - case 14 + case nGenericMsgType["JOINT_TRAJ_PT_FULL"] rosTrajPtFAck.header.nReplyCode=2 break default @@ -104,13 +104,13 @@ endIf endIf // check whether ACK should be sent - if (rosGenericMsg.header.nCommType == 2) + if(rosGenericMsg.header.nCommType == nGenericMsgType["SERVICE_REQUEST"]) // comm type request requires ACK - x_nMsgRecvState = 5 + x_nMsgRecvState = nMsgState["Ack"] else // other comm types (e.g., topic) does not require ACK, hence skip state - x_nMsgRecvState = 0 + x_nMsgRecvState = nMsgState["Reset"] endIf end]]> - + \ No newline at end of file diff --git a/staubli_val3_driver/val3/ros_server/recvHeartbeat.pgx b/staubli_val3_driver/val3/ros_server/recvHeartbeat.pgx index c11d6ba..8f4d456 100644 --- a/staubli_val3_driver/val3/ros_server/recvHeartbeat.pgx +++ b/staubli_val3_driver/val3/ros_server/recvHeartbeat.pgx @@ -4,8 +4,6 @@ - - - + \ No newline at end of file diff --git a/staubli_val3_driver/val3/ros_server/recvMsgBody.pgx b/staubli_val3_driver/val3/ros_server/recvMsgBody.pgx index 528d359..df095b7 100644 --- a/staubli_val3_driver/val3/ros_server/recvMsgBody.pgx +++ b/staubli_val3_driver/val3/ros_server/recvMsgBody.pgx @@ -27,10 +27,10 @@ l_bFlag=false switch rosGenericMsg.header.nMsgType - case 11 + case nGenericMsgType["JOINT_TRAJ_PT"] call recvTrajPt(l_bFlag) break - case 14 + case nGenericMsgType["JOINT_TRAJ_PT_FULL"] call recvTrajPtFull(l_bFlag) break default @@ -39,11 +39,11 @@ if (l_bFlag == true) // move on to state 3: decode msg body - x_nMsgRecvState=3 + x_nMsgRecvState=nMsgState["DecBody"] else // error whilst receiving msg body, reset state machine - x_nMsgRecvState=0 + x_nMsgRecvState=nMsgState["Reset"] endIf end]]> - + \ No newline at end of file diff --git a/staubli_val3_driver/val3/ros_server/recvMsgHeader.pgx b/staubli_val3_driver/val3/ros_server/recvMsgHeader.pgx index 3ce7eea..02c034e 100644 --- a/staubli_val3_driver/val3/ros_server/recvMsgHeader.pgx +++ b/staubli_val3_driver/val3/ros_server/recvMsgHeader.pgx @@ -32,11 +32,11 @@ if (l_bFlag == true) // move on to state 1: decode header - x_nMsgRecvState=1 + x_nMsgRecvState=nMsgState["DecHeader"] else // error: reset state to 0: receive header - x_nMsgRecvState=0 + x_nMsgRecvState=nMsgState["Reset"] endIf end]]> - + \ No newline at end of file diff --git a/staubli_val3_driver/val3/ros_server/ros_server.dtx b/staubli_val3_driver/val3/ros_server/ros_server.dtx index 3119e49..72b5906 100644 --- a/staubli_val3_driver/val3/ros_server/ros_server.dtx +++ b/staubli_val3_driver/val3/ros_server/ros_server.dtx @@ -32,7 +32,7 @@ - + @@ -455,5 +455,21 @@ + + + + + + + + + + + + + + + + - + \ No newline at end of file diff --git a/staubli_val3_driver/val3/ros_server/sendAck.pgx b/staubli_val3_driver/val3/ros_server/sendAck.pgx index d42dfca..6661b6a 100644 --- a/staubli_val3_driver/val3/ros_server/sendAck.pgx +++ b/staubli_val3_driver/val3/ros_server/sendAck.pgx @@ -22,10 +22,10 @@ switch rosGenericMsg.header.nMsgType - case 11 + case nGenericMsgType["JOINT_TRAJ_PT"] call sendRosMsg(siTcpIpMotion, rosTrajPtAck.prefix, rosTrajPtAck.header, rosTrajPtAck.body, nInConnFlag) break - case 14 + case nGenericMsgType["JOINT_TRAJ_PT_FULL"] call sendRosMsg(siTcpIpMotion, rosTrajPtFAck.prefix, rosTrajPtFAck.header, rosTrajPtFAck.body, nInConnFlag) break default @@ -34,7 +34,7 @@ // regardless of result from sending ACK // reset state machine to receive new messages - x_nMsgRecvState=0 + x_nMsgRecvState=nMsgState["Reset"] end]]> - + \ No newline at end of file From d75f20ba09c0714cdb76385277597ad03e190c93 Mon Sep 17 00:00:00 2001 From: marshallpowell97 <48460127+marshallpowell97@users.noreply.github.com> Date: Tue, 21 Jan 2020 08:20:14 -0500 Subject: [PATCH 03/14] Update README.md with Staubli terminology --- staubli_val3_driver/README.md | 33 +++++++++++++++++---------------- 1 file changed, 17 insertions(+), 16 deletions(-) diff --git a/staubli_val3_driver/README.md b/staubli_val3_driver/README.md index 0e5ebf3..59e7b03 100644 --- a/staubli_val3_driver/README.md +++ b/staubli_val3_driver/README.md @@ -2,20 +2,19 @@ ## Overview -This ROS-I driver was developed in Staubli's VAL3 language for use with 6-axis Staubli robot manipulators. +This ROS-I driver was developed in Staubli's VAL 3 language for use with 6-axis Staubli robot manipulators. -It is advisable to try this driver on Staubli's emulator first. +It is advisable to try this driver on the emulator in [Staubli Robotics Suite (SRS)](https://www.staubli.com/en-us/robotics/product-range/robot-software/pc-robot-programming-srs/) first. ## Requirements * Staubli 6-axis robot manipulator -* Staubli CS8 controller - * it may work with other controllers, but this driver was tested on CS8 only +* Staubli CS8/CS8C controller * VAL3 version s7.7.2 or greater * this is very important, since this implementation uses return values of `sioGet()` only available from s7.7.2 onwards - +* Staubli Robotics Suite 2019 (not required but strongly reccomended) ## Installation @@ -29,20 +28,22 @@ Clone [staubli_val3_driver](https://github.com/ros-industrial/staubli_val3_drive git clone https://github.com/ros-industrial/staubli_val3_driver.git ``` -### Transfer driver to Staubli CS8 controller +### Transfer driver to Staubli CS8/CS8C controller + +There are 2 ways of transferring the VAL 3 application to the controller: -There are several ways of transferring VAL3 applications to a Staubli controller. -The simplest method is probably copying the contents of `val3` folder into a USB memory stick (<2GB given the CS8 limitations), plugging the stick to the controller and using the teach pendant to copy the folders. +1. Copy the contents of `val3` folder onto a USB memory stick (<2GB given the CS8/CS8C limitations), plugging the stick into the controller and using the teach pendant to copy the folders + +1. Use the Transfer Manager in SRS to copy the VAL 3 applications to the controller. (Home -> Controller -> Transfer Manager) ### Open the VAL3 application with Staubli SRS -Although it is possible to edit the source files with any text editor (they are essentially XML files), it is advisable to use the Staubli Robotics Suite: +Although it is possible to edit the source files with any text editor (they are essentially XML files), it is advisable to use [Staubli Robotics Suite](https://www.staubli.com/en-us/robotics/product-range/robot-software/pc-robot-programming-srs/): * Copy contents of folder `val3` into the `usrapp` folder of the Staubli cell * Open the `ros_server` VAL3 project located inside the `ros_server` folder -SRS offers autocompletion, syntax highlighting and syntax checking, amongst other useful features, such as a Staubli controller/teach pendant emulator. - +SRS offers autocompletion, syntax highlighting and syntax checking, amongst other useful features, such as a Staubli controller/teach pendant emulator. ## Usage @@ -55,7 +56,7 @@ From `Main menu`: ### Configuration -The TCP sockets on the CS8 controller/emulator must be configured prior to using the driver, otherwise a runtime error will be displayed on the teach pendant and the driver will not work. +The TCP sockets on the CS8/CS8C controller/emulator must be configured prior to use, otherwise a runtime error will be generated and the driver will not work. Two sockets (TCP Servers) are required. From `Main menu`: @@ -73,17 +74,17 @@ have been transferred to the Staubli controller 2. The VAL3 application `ros_server` has been loaded 3. Both TCP Server sockets have been configured properly -Then simply press the `Run` button, ensure that `ros_server` is highlighted, then press `F8` (Ok). +Press the `Run` button, ensure that `ros_server` is highlighted, then press `F8` (Ok). -Notice that depending on which mode of operation is currently active, the motors may need to be enabled manually (a message will pop up on the screen). -Likewise, the robot will only move if the `Move` button has been pressed (or is kept pressed if in manual mode). +Depending on which working mode the controller is in, arm power may need to be enabled manually (a message will pop up on the screen). +Once arm power is enabled, the robot will only move if the `Move` button has been pressed (or is kept pressed if in manual mode). ### Run the industrial_robot_client node (ROS-I client) The `indigo-devel` branch provides launch files (within the `staubli_val3_driver` ROS package). Simply run: ```shell -roslaunch staubli_val3_driver robot_interface_streaming.launch robot_ip:= +roslaunch staubli_val3_driver robot_interface_streaming.launch robot_ip:= ``` ## Bugs, suggestions and feature requests From 64edc9ee7c2d5477e1483360a1a98bf5fe91fdde Mon Sep 17 00:00:00 2001 From: marshallpowell97 <48460127+marshallpowell97@users.noreply.github.com> Date: Tue, 21 Jan 2020 08:22:00 -0500 Subject: [PATCH 04/14] Update README.md --- README.md | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/README.md b/README.md index 486fb5d..f6bc405 100644 --- a/README.md +++ b/README.md @@ -9,13 +9,13 @@ ## Overview This repository contains the `staubli_val3_driver` package which provides a set of VAL3 libraries and an application which together implement a [simple_message][] compatible server implementation. -Together with the nodes in [industrial_robot_client][], this server can be used as a ROS 1 driver that allows motion control of Staubli CS8 controlled robots, by exposing a [FollowJointTrajectory][] [action][] server, which is compatible with MoveIt and other nodes that implement an action client. +Together with the nodes in [industrial_robot_client][], this server can be used as a ROS 1 driver that allows motion control of Staubli CS8/CS8C controlled robots, by exposing a [FollowJointTrajectory][] [action][] server, which is compatible with MoveIt and other nodes that implement an action client. ## Compatibility -The current version of the driver is compatible with CS8 controllers only. -Future work may extend this to support CS9 controllers as well. +The current version of the driver is compatible with CS8/CS8C controllers only. +Future work is planned to extend this to support CS9 controllers as well. From fec591f0b7c79e8385200e45b2db504aa5159c9b Mon Sep 17 00:00:00 2001 From: marshallpowell97 Date: Tue, 21 Jan 2020 09:37:47 -0500 Subject: [PATCH 05/14] Check for VAL 3 version >= 7.7 --- .../val3/ros_server/.outlining.json | 41 +++++++++++++++++++ staubli_val3_driver/val3/ros_server/start.pgx | 29 ++++++++++--- 2 files changed, 65 insertions(+), 5 deletions(-) diff --git a/staubli_val3_driver/val3/ros_server/.outlining.json b/staubli_val3_driver/val3/ros_server/.outlining.json index 863b444..7803647 100644 --- a/staubli_val3_driver/val3/ros_server/.outlining.json +++ b/staubli_val3_driver/val3/ros_server/.outlining.json @@ -233,6 +233,47 @@ "isCollapsed": false } ] + }, + { + "mode": 3, + "name": "start", + "nodes": [ + { + "start": 899, + "end": 1009, + "isCollapsed": false + }, + { + "start": 1012, + "end": 1151, + "isCollapsed": false + }, + { + "start": 2340, + "end": 2428, + "isCollapsed": false + }, + { + "start": 2514, + "end": 2705, + "isCollapsed": false + }, + { + "start": 2535, + "end": 2697, + "isCollapsed": false + }, + { + "start": 2564, + "end": 2629, + "isCollapsed": false + }, + { + "start": 2634, + "end": 2687, + "isCollapsed": false + } + ] } ] } \ No newline at end of file diff --git a/staubli_val3_driver/val3/ros_server/start.pgx b/staubli_val3_driver/val3/ros_server/start.pgx index 4959ebd..46369c9 100644 --- a/staubli_val3_driver/val3/ros_server/start.pgx +++ b/staubli_val3_driver/val3/ros_server/start.pgx @@ -6,8 +6,14 @@ + + + + + + - = 7.7 + l_sVersionMajor = mid(getVersion("VAL3"),1,1) + l_sVersionMinor = mid(getVersion("VAL3"),2,1) + toNum(l_sVersionMajor, l_nVersionMajor, l_bOkMajor) + toNum(l_sVersionMinor, l_nVersionMinor, l_bOkMinor) + if(!l_bOkMajor or !l_bOkMinor) + popUpMsg("Error reading VAL 3 version") + taskKill("ros_server~") + endIf + if( !((l_nVersionMajor == 7) and (l_nVersionMinor >= 7))) + popUpMsg("Error: VAL 3 version is < 7.7") + taskKill("ros_server~") + endIf // feedback task will run every 20ms (50Hz) l_nFbkPeriod=0.02 @@ -67,14 +86,14 @@ clearBuffer(siTcpIpFbk) // check if robot is not in emergency stop and wait until estop is reset - if (esStatus() == 2) + if(esStatus() == 2) popUpMsg("Please reset E-Stop") wait(esStatus() < 2) endIf // before anything else, enable power (only works if robot is in remote mode) - if (!isPowered()) - if (workingMode() >= 4) + if(!isPowered()) + if(workingMode() >= 4) do enablePower() delay(2) @@ -106,4 +125,4 @@ end]]> - + \ No newline at end of file From a61e87100242b9e55dfd616bd0cd1fdff30d1505 Mon Sep 17 00:00:00 2001 From: marshallpowell97 <48460127+marshallpowell97@users.noreply.github.com> Date: Mon, 10 Feb 2020 14:34:50 -0500 Subject: [PATCH 06/14] Update staubli_val3_driver/README.md Co-Authored-By: G.A. vd. Hoorn --- staubli_val3_driver/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/staubli_val3_driver/README.md b/staubli_val3_driver/README.md index 59e7b03..08b6b82 100644 --- a/staubli_val3_driver/README.md +++ b/staubli_val3_driver/README.md @@ -14,7 +14,7 @@ It is advisable to try this driver on the emulator in [Staubli Robotics Suite (S * VAL3 version s7.7.2 or greater * this is very important, since this implementation uses return values of `sioGet()` only available from s7.7.2 onwards -* Staubli Robotics Suite 2019 (not required but strongly reccomended) +* Staubli Robotics Suite 2019 (not required but strongly recommended) ## Installation From 3096af5299afa0ec59ea6bbb45850e6a1fb2fe1f Mon Sep 17 00:00:00 2001 From: marshallpowell97 Date: Wed, 12 Feb 2020 11:17:12 -0500 Subject: [PATCH 07/14] Separate message state variables from index Change from indexing through array (nMsgState) to having individual variables as "constants" --- .gitignore | 1 + .../val3/ros_server/.outlining.json | 245 +++++++++++++++--- .../val3/ros_server/dataReceiver.pgx | 28 +- .../val3/ros_server/decodeMsgBody.pgx | 6 +- .../val3/ros_server/decodeMsgHeader.pgx | 10 +- .../val3/ros_server/pushMotion.pgx | 4 +- .../val3/ros_server/recvMsgBody.pgx | 4 +- .../val3/ros_server/recvMsgHeader.pgx | 4 +- .../val3/ros_server/ros_server.dtx | 26 +- .../val3/ros_server/sendAck.pgx | 2 +- 10 files changed, 262 insertions(+), 68 deletions(-) create mode 100644 .gitignore diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..404c407 --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +staubli_val3_driver/val3/ros_server/.outlining.json diff --git a/staubli_val3_driver/val3/ros_server/.outlining.json b/staubli_val3_driver/val3/ros_server/.outlining.json index 7803647..3f57556 100644 --- a/staubli_val3_driver/val3/ros_server/.outlining.json +++ b/staubli_val3_driver/val3/ros_server/.outlining.json @@ -99,46 +99,133 @@ }, { "mode": 3, - "name": "dataReceiver", + "name": "start", "nodes": [ { - "start": 869, - "end": 2334, + "start": 899, + "end": 1009, "isCollapsed": false }, { - "start": 1488, - "end": 1584, + "start": 1012, + "end": 1151, "isCollapsed": false }, { - "start": 1630, - "end": 1728, + "start": 2340, + "end": 2428, "isCollapsed": false }, { - "start": 1733, - "end": 1818, + "start": 2514, + "end": 2705, "isCollapsed": false }, { - "start": 1823, - "end": 1898, + "start": 2535, + "end": 2697, "isCollapsed": false }, { - "start": 1903, - "end": 1975, + "start": 2564, + "end": 2629, "isCollapsed": false }, { - "start": 1980, - "end": 2072, + "start": 2634, + "end": 2687, + "isCollapsed": false + } + ] + }, + { + "mode": 3, + "name": "decodeMsgBody", + "nodes": [ + { + "start": 677, + "end": 920, + "isCollapsed": false + }, + { + "start": 718, + "end": 795, + "isCollapsed": false + }, + { + "start": 800, + "end": 886, + "isCollapsed": false + }, + { + "start": 891, + "end": 908, "isCollapsed": false }, { - "start": 2180, - "end": 2286, + "start": 924, + "end": 1593, + "isCollapsed": false + }, + { + "start": 1030, + "end": 1585, + "isCollapsed": false + }, + { + "start": 1080, + "end": 1585, + "isCollapsed": false + }, + { + "start": 1125, + "end": 1419, + "isCollapsed": false + }, + { + "start": 1170, + "end": 1264, + "isCollapsed": false + }, + { + "start": 1273, + "end": 1373, + "isCollapsed": false + }, + { + "start": 1382, + "end": 1403, + "isCollapsed": false + }, + { + "start": 1487, + "end": 1575, + "isCollapsed": false + } + ] + }, + { + "mode": 3, + "name": "decodeMsgHeader", + "nodes": [ + { + "start": 797, + "end": 910, + "isCollapsed": false + }, + { + "start": 1002, + "end": 1126, + "isCollapsed": false + }, + { + "start": 1222, + "end": 1347, + "isCollapsed": false + }, + { + "start": 1444, + "end": 1570, "isCollapsed": false } ] @@ -224,53 +311,141 @@ }, { "start": 3936, - "end": 4218, + "end": 4194, "isCollapsed": false }, { - "start": 4088, - "end": 4210, + "start": 4076, + "end": 4186, "isCollapsed": false } ] }, { "mode": 3, - "name": "start", + "name": "recvMsgBody", "nodes": [ { - "start": 899, - "end": 1009, + "start": 677, + "end": 922, "isCollapsed": false }, { - "start": 1012, - "end": 1151, + "start": 718, + "end": 796, "isCollapsed": false }, { - "start": 2340, - "end": 2428, + "start": 801, + "end": 888, "isCollapsed": false }, { - "start": 2514, - "end": 2705, + "start": 893, + "end": 910, "isCollapsed": false }, { - "start": 2535, - "end": 2697, + "start": 926, + "end": 1120, "isCollapsed": false }, { - "start": 2564, - "end": 2629, + "start": 1021, + "end": 1112, + "isCollapsed": false + } + ] + }, + { + "mode": 3, + "name": "recvMsgHeader", + "nodes": [ + { + "start": 788, + "end": 896, "isCollapsed": false }, { - "start": 2634, - "end": 2687, + "start": 900, + "end": 1081, + "isCollapsed": false + }, + { + "start": 995, + "end": 1073, + "isCollapsed": false + } + ] + }, + { + "mode": 3, + "name": "sendAck", + "nodes": [ + { + "start": 660, + "end": 1064, + "isCollapsed": false + }, + { + "start": 701, + "end": 859, + "isCollapsed": false + }, + { + "start": 864, + "end": 1030, + "isCollapsed": false + }, + { + "start": 1035, + "end": 1052, + "isCollapsed": false + } + ] + }, + { + "mode": 3, + "name": "dataReceiver", + "nodes": [ + { + "start": 1022, + "end": 2482, + "isCollapsed": false + }, + { + "start": 1640, + "end": 1723, + "isCollapsed": false + }, + { + "start": 1769, + "end": 1854, + "isCollapsed": false + }, + { + "start": 1859, + "end": 1938, + "isCollapsed": false + }, + { + "start": 1943, + "end": 2024, + "isCollapsed": false + }, + { + "start": 2029, + "end": 2110, + "isCollapsed": false + }, + { + "start": 2115, + "end": 2209, + "isCollapsed": false + }, + { + "start": 2317, + "end": 2434, "isCollapsed": false } ] diff --git a/staubli_val3_driver/val3/ros_server/dataReceiver.pgx b/staubli_val3_driver/val3/ros_server/dataReceiver.pgx index 5740a47..92f1ece 100644 --- a/staubli_val3_driver/val3/ros_server/dataReceiver.pgx +++ b/staubli_val3_driver/val3/ros_server/dataReceiver.pgx @@ -29,9 +29,19 @@ // 3: decode body // 4: push motion into buffer // 5: send ACK - l_nMsgRecvState=nMsgState["RecHeader"] + + // Set variable constants to make sure they were not changed + nReset=0 + nRecHeader=0 + nDecHeader=1 + nRecBody=2 + nDecBody=3 + nPushMotion=4 + nAck=5 + + l_nMsgRecvState=nRecHeader - while (true) + while(true) // usually for state machines a switch-case statement is a neat implementation // HOWEVER, at every iteration of while() this task is sequenced and only called // after whatever ms is was defined when created. @@ -41,30 +51,30 @@ // into buffer and then send ACK to client WITHIN ONE ITERATION of while(), // hence the if statement sequence below l_nStartTime = clock() - if (l_nMsgRecvState==nMsgState["RecHeader"]) + if(l_nMsgRecvState==nRecHeader) call recvMsgHeader(l_nMsgRecvState) endIf nHeaderTime = clock() - l_nStartTime - if (l_nMsgRecvState==nMsgState["DecHeader"]) + if(l_nMsgRecvState==nDecHeader) call decodeMsgHeader(l_nMsgRecvState) endIf - if (l_nMsgRecvState==nMsgState["RecBody"]) + if(l_nMsgRecvState==nRecBody) call recvMsgBody(l_nMsgRecvState) endIf - if (l_nMsgRecvState==nMsgState["DecBody"]) + if(l_nMsgRecvState==nDecBody) call decodeMsgBody(l_nMsgRecvState) endIf - if (l_nMsgRecvState==nMsgState["PushMotion"]) + if(l_nMsgRecvState==nPushMotion) call pushMotion(l_nMsgRecvState) endIf - if (l_nMsgRecvState==nMsgState["Ack"]) + if(l_nMsgRecvState==nAck) call encodeAck() call sendAck(l_nMsgRecvState) endIf nElapsedTime = clock() - l_nStartTime - nHeaderTime // assert state machine is working OK - if (l_nMsgRecvStatenMsgState["Ack"]) + if(l_nMsgRecvStatenAck) popUpMsg("Data reception state machine error") endIf diff --git a/staubli_val3_driver/val3/ros_server/decodeMsgBody.pgx b/staubli_val3_driver/val3/ros_server/decodeMsgBody.pgx index 7e0b0a3..ad34fb1 100644 --- a/staubli_val3_driver/val3/ros_server/decodeMsgBody.pgx +++ b/staubli_val3_driver/val3/ros_server/decodeMsgBody.pgx @@ -39,7 +39,7 @@ if (l_bFlag == true) // move on to state 4: push motion into buffer - x_nMsgRecvState=nMsgState["PushMotion"] + x_nMsgRecvState=nPushMotion else // error whilst decoding msg body... if (rosGenericMsg.header.nCommType==2) @@ -54,10 +54,10 @@ break endSwitch // jump to state 5: sendAck() - x_nMsgRecvState=nMsgState["Ack"] + x_nMsgRecvState=nAck else // reset state machine, since no ACK is required - x_nMsgRecvState=nMsgState["Reset"] + x_nMsgRecvState=nReset endIf endIf end]]> diff --git a/staubli_val3_driver/val3/ros_server/decodeMsgHeader.pgx b/staubli_val3_driver/val3/ros_server/decodeMsgHeader.pgx index ecdff4d..28e8a38 100644 --- a/staubli_val3_driver/val3/ros_server/decodeMsgHeader.pgx +++ b/staubli_val3_driver/val3/ros_server/decodeMsgHeader.pgx @@ -28,31 +28,31 @@ l_nRetVal=fromBinary(rosGenericMsg.prefix.nData,4,"-4l",rosGenericMsg.prefix.nLength) if (l_nRetVal!=1) // error decoding prefix, reset state machine - x_nMsgRecvState=nMsgState["Reset"] + x_nMsgRecvState=nReset return endIf l_nRetVal=fromBinary(rosGenericMsg.header.nData,4,"-4l",rosGenericMsg.header.nMsgType) if (l_nRetVal!=1) // error decoding header (msg type), reset state machine - x_nMsgRecvState=nMsgState["Reset"] + x_nMsgRecvState=nReset return endIf l_nRetVal=fromBinary(rosGenericMsg.header.nData[4],4,"-4l",rosGenericMsg.header.nCommType) if (l_nRetVal!=1) // error decoding header (comm type), reset state machine - x_nMsgRecvState=nMsgState["Reset"] + x_nMsgRecvState=nReset return endIf l_nRetVal=fromBinary(rosGenericMsg.header.nData[8],4,"-4l",rosGenericMsg.header.nReplyCode) if (l_nRetVal!=1) // error decoding header (reply code), reset state machine - x_nMsgRecvState=nMsgState["Reset"] + x_nMsgRecvState=nReset return endIf // no errors detected whilst decoding prefix and header // move on to state 2: receive msg body - x_nMsgRecvState=nMsgState["RecBody"] + x_nMsgRecvState=nRecBody end]]> \ No newline at end of file diff --git a/staubli_val3_driver/val3/ros_server/pushMotion.pgx b/staubli_val3_driver/val3/ros_server/pushMotion.pgx index 07242e1..de76bd9 100644 --- a/staubli_val3_driver/val3/ros_server/pushMotion.pgx +++ b/staubli_val3_driver/val3/ros_server/pushMotion.pgx @@ -106,10 +106,10 @@ // check whether ACK should be sent if(rosGenericMsg.header.nCommType == nGenericMsgType["SERVICE_REQUEST"]) // comm type request requires ACK - x_nMsgRecvState = nMsgState["Ack"] + x_nMsgRecvState = nAck else // other comm types (e.g., topic) does not require ACK, hence skip state - x_nMsgRecvState = nMsgState["Reset"] + x_nMsgRecvState = nReset endIf end]]> diff --git a/staubli_val3_driver/val3/ros_server/recvMsgBody.pgx b/staubli_val3_driver/val3/ros_server/recvMsgBody.pgx index df095b7..3389328 100644 --- a/staubli_val3_driver/val3/ros_server/recvMsgBody.pgx +++ b/staubli_val3_driver/val3/ros_server/recvMsgBody.pgx @@ -39,10 +39,10 @@ if (l_bFlag == true) // move on to state 3: decode msg body - x_nMsgRecvState=nMsgState["DecBody"] + x_nMsgRecvState=nDecBody else // error whilst receiving msg body, reset state machine - x_nMsgRecvState=nMsgState["Reset"] + x_nMsgRecvState=nReset endIf end]]> diff --git a/staubli_val3_driver/val3/ros_server/recvMsgHeader.pgx b/staubli_val3_driver/val3/ros_server/recvMsgHeader.pgx index 02c034e..2abd46d 100644 --- a/staubli_val3_driver/val3/ros_server/recvMsgHeader.pgx +++ b/staubli_val3_driver/val3/ros_server/recvMsgHeader.pgx @@ -32,10 +32,10 @@ if (l_bFlag == true) // move on to state 1: decode header - x_nMsgRecvState=nMsgState["DecHeader"] + x_nMsgRecvState=nDecHeader else // error: reset state to 0: receive header - x_nMsgRecvState=nMsgState["Reset"] + x_nMsgRecvState=nReset endIf end]]> diff --git a/staubli_val3_driver/val3/ros_server/ros_server.dtx b/staubli_val3_driver/val3/ros_server/ros_server.dtx index 72b5906..7e97f5e 100644 --- a/staubli_val3_driver/val3/ros_server/ros_server.dtx +++ b/staubli_val3_driver/val3/ros_server/ros_server.dtx @@ -456,20 +456,28 @@ - - - - - - - - - + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/staubli_val3_driver/val3/ros_server/sendAck.pgx b/staubli_val3_driver/val3/ros_server/sendAck.pgx index 6661b6a..f1f7ca9 100644 --- a/staubli_val3_driver/val3/ros_server/sendAck.pgx +++ b/staubli_val3_driver/val3/ros_server/sendAck.pgx @@ -34,7 +34,7 @@ // regardless of result from sending ACK // reset state machine to receive new messages - x_nMsgRecvState=nMsgState["Reset"] + x_nMsgRecvState=nReset end]]> \ No newline at end of file From b0639ffa9f058419549a9462c7fdab07fea1f3e4 Mon Sep 17 00:00:00 2001 From: marshallpowell97 Date: Wed, 12 Feb 2020 13:22:59 -0500 Subject: [PATCH 08/14] Move velocity interpolation to higher level Added `interpolateVel` program, called as a part of `pushMotion` --- .../val3/ros_server/.outlining.json | 260 +++++++++--------- .../val3/ros_server/decTrajPt.pgx | 11 +- .../val3/ros_server/interpolateVel.pgx | 19 ++ .../val3/ros_server/pushMotion.pgx | 3 + .../val3/ros_server/ros_server.pjx | 5 +- 5 files changed, 159 insertions(+), 139 deletions(-) create mode 100644 staubli_val3_driver/val3/ros_server/interpolateVel.pgx diff --git a/staubli_val3_driver/val3/ros_server/.outlining.json b/staubli_val3_driver/val3/ros_server/.outlining.json index 3f57556..23f526c 100644 --- a/staubli_val3_driver/val3/ros_server/.outlining.json +++ b/staubli_val3_driver/val3/ros_server/.outlining.json @@ -1,41 +1,5 @@ { "programs": [ - { - "mode": 3, - "name": "decTrajPt", - "nodes": [ - { - "start": 865, - "end": 918, - "isCollapsed": false - }, - { - "start": 938, - "end": 1155, - "isCollapsed": false - }, - { - "start": 1087, - "end": 1146, - "isCollapsed": false - }, - { - "start": 1387, - "end": 1440, - "isCollapsed": false - }, - { - "start": 1782, - "end": 1951, - "isCollapsed": false - }, - { - "start": 2065, - "end": 2118, - "isCollapsed": false - } - ] - }, { "mode": 3, "name": "decTrajPtFull", @@ -230,97 +194,6 @@ } ] }, - { - "mode": 3, - "name": "pushMotion", - "nodes": [ - { - "start": 660, - "end": 2357, - "isCollapsed": false - }, - { - "start": 701, - "end": 1410, - "isCollapsed": false - }, - { - "start": 1415, - "end": 2323, - "isCollapsed": false - }, - { - "start": 2328, - "end": 2345, - "isCollapsed": false - }, - { - "start": 2466, - "end": 3895, - "isCollapsed": false - }, - { - "start": 2550, - "end": 3887, - "isCollapsed": false - }, - { - "start": 2649, - "end": 2942, - "isCollapsed": false - }, - { - "start": 3155, - "end": 3316, - "isCollapsed": false - }, - { - "start": 3282, - "end": 3306, - "isCollapsed": false - }, - { - "start": 3326, - "end": 3887, - "isCollapsed": false - }, - { - "start": 3474, - "end": 3877, - "isCollapsed": false - }, - { - "start": 3553, - "end": 3865, - "isCollapsed": false - }, - { - "start": 3600, - "end": 3698, - "isCollapsed": false - }, - { - "start": 3709, - "end": 3813, - "isCollapsed": false - }, - { - "start": 3824, - "end": 3847, - "isCollapsed": false - }, - { - "start": 3936, - "end": 4194, - "isCollapsed": false - }, - { - "start": 4076, - "end": 4186, - "isCollapsed": false - } - ] - }, { "mode": 3, "name": "recvMsgBody", @@ -449,6 +322,139 @@ "isCollapsed": false } ] + }, + { + "mode": 3, + "name": "decTrajPt", + "nodes": [ + { + "start": 865, + "end": 918, + "isCollapsed": false + }, + { + "start": 938, + "end": 1155, + "isCollapsed": false + }, + { + "start": 1087, + "end": 1146, + "isCollapsed": false + }, + { + "start": 1388, + "end": 1441, + "isCollapsed": false + }, + { + "start": 1555, + "end": 1608, + "isCollapsed": false + } + ] + }, + { + "mode": 3, + "name": "interpolateVel", + "nodes": [ + { + "start": 469, + "end": 638, + "isCollapsed": false + } + ] + }, + { + "mode": 3, + "name": "pushMotion", + "nodes": [ + { + "start": 660, + "end": 2445, + "isCollapsed": false + }, + { + "start": 701, + "end": 1498, + "isCollapsed": false + }, + { + "start": 1503, + "end": 2411, + "isCollapsed": false + }, + { + "start": 2416, + "end": 2433, + "isCollapsed": false + }, + { + "start": 2554, + "end": 3983, + "isCollapsed": false + }, + { + "start": 2638, + "end": 3975, + "isCollapsed": false + }, + { + "start": 2737, + "end": 3030, + "isCollapsed": false + }, + { + "start": 3243, + "end": 3404, + "isCollapsed": false + }, + { + "start": 3370, + "end": 3394, + "isCollapsed": false + }, + { + "start": 3414, + "end": 3975, + "isCollapsed": false + }, + { + "start": 3562, + "end": 3965, + "isCollapsed": false + }, + { + "start": 3641, + "end": 3953, + "isCollapsed": false + }, + { + "start": 3688, + "end": 3786, + "isCollapsed": false + }, + { + "start": 3797, + "end": 3901, + "isCollapsed": false + }, + { + "start": 3912, + "end": 3935, + "isCollapsed": false + }, + { + "start": 4024, + "end": 4282, + "isCollapsed": false + }, + { + "start": 4164, + "end": 4274, + "isCollapsed": false + } + ] } ] } \ No newline at end of file diff --git a/staubli_val3_driver/val3/ros_server/decTrajPt.pgx b/staubli_val3_driver/val3/ros_server/decTrajPt.pgx index 4fa2762..5aa860f 100644 --- a/staubli_val3_driver/val3/ros_server/decTrajPt.pgx +++ b/staubli_val3_driver/val3/ros_server/decTrajPt.pgx @@ -46,7 +46,7 @@ endIf endFor - // Velocity buffer to smooth out last traj point + // Velocity buffer to smooth out last traj point nVel[1] = nVel[0] nVel[0] = rosTrajPtMsg.jointTrajPt.nVelocity @@ -57,15 +57,6 @@ return endIf - // Round to remove trailing decimal places from fromBinary() - rosTrajPtMsg.jointTrajPt.nVelocity = rosTrajPtMsg.jointTrajPt.nVelocity * 10000 - rosTrajPtMsg.jointTrajPt.nVelocity = round(rosTrajPtMsg.jointTrajPt.nVelocity) - rosTrajPtMsg.jointTrajPt.nVelocity = rosTrajPtMsg.jointTrajPt.nVelocity / 10000 - // Smooth last point in traj - if((rosTrajPtMsg.jointTrajPt.nVelocity == 0.1) and (rosTrajPtMsg.jointTrajPt.nSeq != 0)) - rosTrajPtMsg.jointTrajPt.nVelocity = (nVel[0] - (nVel[1] - nVel[0])) - endIf - // duration l_nRetVal=fromBinary(rosTrajPtMsg.body.nData[48],4,"4.0l",rosTrajPtMsg.jointTrajPt.nDuration) if(l_nRetVal!=1) diff --git a/staubli_val3_driver/val3/ros_server/interpolateVel.pgx b/staubli_val3_driver/val3/ros_server/interpolateVel.pgx new file mode 100644 index 0000000..13e7de8 --- /dev/null +++ b/staubli_val3_driver/val3/ros_server/interpolateVel.pgx @@ -0,0 +1,19 @@ + + + + + + \ No newline at end of file diff --git a/staubli_val3_driver/val3/ros_server/pushMotion.pgx b/staubli_val3_driver/val3/ros_server/pushMotion.pgx index de76bd9..42a5cae 100644 --- a/staubli_val3_driver/val3/ros_server/pushMotion.pgx +++ b/staubli_val3_driver/val3/ros_server/pushMotion.pgx @@ -34,6 +34,9 @@ // ROS joint position values are in rad, but VAL3 uses degrees call toJointRxDeg(rosTrajPtMsg.jointTrajPt.nJoints, l_mTrajPt.jJointRx) + // Interpolate velocity for last point in trajectory + call interpolateVel() + // from industrial_robot_client/src/joint_trajectory_interface.cpp // JointTrajectoryInterface::trajectory_to_msgs() // JointTrajectoryInterface::calc_speed() diff --git a/staubli_val3_driver/val3/ros_server/ros_server.pjx b/staubli_val3_driver/val3/ros_server/ros_server.pjx index f55594b..d15ee8a 100644 --- a/staubli_val3_driver/val3/ros_server/ros_server.pjx +++ b/staubli_val3_driver/val3/ros_server/ros_server.pjx @@ -1,6 +1,6 @@  - + @@ -16,6 +16,7 @@ + @@ -53,4 +54,4 @@ - + \ No newline at end of file From 7a517065cae414c40fd5a76fdcb35442bb3aa099 Mon Sep 17 00:00:00 2001 From: marshallpowell97 Date: Mon, 17 Feb 2020 10:12:28 -0500 Subject: [PATCH 09/14] Move velocity interpolation logic to motionControl() --- .../val3/ros_server/.outlining.json | 158 +++++++++++------- .../val3/ros_server/decTrajPt.pgx | 4 - .../val3/ros_server/interpolateVel.pgx | 16 +- .../val3/ros_server/motionControl.pgx | 20 ++- .../val3/ros_server/pushMotion.pgx | 3 - 5 files changed, 120 insertions(+), 81 deletions(-) diff --git a/staubli_val3_driver/val3/ros_server/.outlining.json b/staubli_val3_driver/val3/ros_server/.outlining.json index 23f526c..602fd19 100644 --- a/staubli_val3_driver/val3/ros_server/.outlining.json +++ b/staubli_val3_driver/val3/ros_server/.outlining.json @@ -325,133 +325,169 @@ }, { "mode": 3, - "name": "decTrajPt", + "name": "pushMotion", "nodes": [ { - "start": 865, - "end": 918, + "start": 660, + "end": 2357, "isCollapsed": false }, { - "start": 938, - "end": 1155, + "start": 701, + "end": 1410, "isCollapsed": false }, { - "start": 1087, - "end": 1146, + "start": 1415, + "end": 2323, "isCollapsed": false }, { - "start": 1388, - "end": 1441, + "start": 2328, + "end": 2345, "isCollapsed": false }, { - "start": 1555, - "end": 1608, + "start": 2466, + "end": 3895, "isCollapsed": false - } - ] - }, - { - "mode": 3, - "name": "interpolateVel", - "nodes": [ + }, { - "start": 469, - "end": 638, + "start": 2550, + "end": 3887, "isCollapsed": false - } - ] - }, - { - "mode": 3, - "name": "pushMotion", - "nodes": [ + }, { - "start": 660, - "end": 2445, + "start": 2649, + "end": 2942, "isCollapsed": false }, { - "start": 701, - "end": 1498, + "start": 3155, + "end": 3316, + "isCollapsed": false + }, + { + "start": 3282, + "end": 3306, + "isCollapsed": false + }, + { + "start": 3326, + "end": 3887, + "isCollapsed": false + }, + { + "start": 3474, + "end": 3877, "isCollapsed": false }, { - "start": 1503, - "end": 2411, + "start": 3553, + "end": 3865, "isCollapsed": false }, { - "start": 2416, - "end": 2433, + "start": 3600, + "end": 3698, "isCollapsed": false }, { - "start": 2554, - "end": 3983, + "start": 3709, + "end": 3813, "isCollapsed": false }, { - "start": 2638, - "end": 3975, + "start": 3824, + "end": 3847, "isCollapsed": false }, { - "start": 2737, - "end": 3030, + "start": 3936, + "end": 4194, "isCollapsed": false }, { - "start": 3243, - "end": 3404, + "start": 4076, + "end": 4186, + "isCollapsed": false + } + ] + }, + { + "mode": 3, + "name": "decTrajPt", + "nodes": [ + { + "start": 865, + "end": 918, "isCollapsed": false }, { - "start": 3370, - "end": 3394, + "start": 938, + "end": 1155, "isCollapsed": false }, { - "start": 3414, - "end": 3975, + "start": 1087, + "end": 1146, "isCollapsed": false }, { - "start": 3562, - "end": 3965, + "start": 1269, + "end": 1322, "isCollapsed": false }, { - "start": 3641, - "end": 3953, + "start": 1436, + "end": 1489, + "isCollapsed": false + } + ] + }, + { + "mode": 3, + "name": "motionControl", + "nodes": [ + { + "start": 676, + "end": 2942, "isCollapsed": false }, { - "start": 3688, - "end": 3786, + "start": 751, + "end": 2855, "isCollapsed": false }, { - "start": 3797, - "end": 3901, + "start": 948, + "end": 1186, "isCollapsed": false }, { - "start": 3912, - "end": 3935, + "start": 1038, + "end": 1174, "isCollapsed": false }, { - "start": 4024, - "end": 4282, + "start": 1411, + "end": 2556, "isCollapsed": false }, { - "start": 4164, - "end": 4274, + "start": 1497, + "end": 2544, + "isCollapsed": false + } + ] + }, + { + "mode": 3, + "name": "interpolateVel", + "nodes": [ + { + "start": 377, + "end": 506, "isCollapsed": false } ] diff --git a/staubli_val3_driver/val3/ros_server/decTrajPt.pgx b/staubli_val3_driver/val3/ros_server/decTrajPt.pgx index 5aa860f..54ffc40 100644 --- a/staubli_val3_driver/val3/ros_server/decTrajPt.pgx +++ b/staubli_val3_driver/val3/ros_server/decTrajPt.pgx @@ -46,10 +46,6 @@ endIf endFor - // Velocity buffer to smooth out last traj point - nVel[1] = nVel[0] - nVel[0] = rosTrajPtMsg.jointTrajPt.nVelocity - // velocity l_nRetVal=fromBinary(rosTrajPtMsg.body.nData[44],4,"4.0l",rosTrajPtMsg.jointTrajPt.nVelocity) if(l_nRetVal!=1) diff --git a/staubli_val3_driver/val3/ros_server/interpolateVel.pgx b/staubli_val3_driver/val3/ros_server/interpolateVel.pgx index 13e7de8..2ca0b75 100644 --- a/staubli_val3_driver/val3/ros_server/interpolateVel.pgx +++ b/staubli_val3_driver/val3/ros_server/interpolateVel.pgx @@ -1,18 +1,20 @@  + + + diff --git a/staubli_val3_driver/val3/ros_server/motionControl.pgx b/staubli_val3_driver/val3/ros_server/motionControl.pgx index 4ec0f51..9165d3d 100644 --- a/staubli_val3_driver/val3/ros_server/motionControl.pgx +++ b/staubli_val3_driver/val3/ros_server/motionControl.pgx @@ -26,15 +26,15 @@ nMoveId = -1 - while (true) + while(true) // check connection status, and whether to stop motion - if (nInConnFlag == 1 and nOutConnFlag == 1 and bStopNow == false) + if(nInConnFlag == 1 and nOutConnFlag == 1 and bStopNow == false) // process trajectory point from trajectory buffer call libQueueFuncs:getNumElems(qTrajPtBuffer, l_nElems) - if (l_nElems > 0) + if(l_nElems > 0) call libQueueFuncs:pop(qTrajPtBuffer,l_mTrajPt, l_bFlag) - if (l_bFlag == true) + if(l_bFlag == true) nPtsPopped = nPtsPopped + 1 call libQueueFuncs:push(qMoveBuffer,l_mTrajPt,l_bFlag) endIf @@ -43,10 +43,18 @@ // process motion command from move buffer // @TODO move buffer should have elements previously interpolated to ensure they are 4 ms apart call libQueueFuncs:getNumElems(qMoveBuffer, l_nElems) - if (l_nElems > 0) + if(l_nElems > 0) call libQueueFuncs:pop(qMoveBuffer,l_motion,l_bFlag) - if (l_bFlag == true) + if(l_bFlag == true) nMovePts = nMovePts + 1 + + // Velocity buffer to smooth out last traj point + nVel[1] = nVel[0] + nVel[0] = rosTrajPtMsg.jointTrajPt.nVelocity + + // Interpolate velocity for last point in trajectory + call interpolateVel(l_motion) + // contrary to what we did earlier, we unconditionally add the traj pt to the motion queue: // there doesn't appear to be a way to compare the 'current pose' of the robot to the pts // in the trajectory at the appropriate time (ie: when the robot is in motion and about to diff --git a/staubli_val3_driver/val3/ros_server/pushMotion.pgx b/staubli_val3_driver/val3/ros_server/pushMotion.pgx index 42a5cae..de76bd9 100644 --- a/staubli_val3_driver/val3/ros_server/pushMotion.pgx +++ b/staubli_val3_driver/val3/ros_server/pushMotion.pgx @@ -34,9 +34,6 @@ // ROS joint position values are in rad, but VAL3 uses degrees call toJointRxDeg(rosTrajPtMsg.jointTrajPt.nJoints, l_mTrajPt.jJointRx) - // Interpolate velocity for last point in trajectory - call interpolateVel() - // from industrial_robot_client/src/joint_trajectory_interface.cpp // JointTrajectoryInterface::trajectory_to_msgs() // JointTrajectoryInterface::calc_speed() From 397e5bc20576175976ff11c3a889007f3b9a938b Mon Sep 17 00:00:00 2001 From: gavanderhoorn Date: Thu, 12 Mar 2020 11:22:34 +0100 Subject: [PATCH 10/14] Don't include outlining files. --- .../val3/ros_server/.outlining.json | 496 ------------------ 1 file changed, 496 deletions(-) delete mode 100644 staubli_val3_driver/val3/ros_server/.outlining.json diff --git a/staubli_val3_driver/val3/ros_server/.outlining.json b/staubli_val3_driver/val3/ros_server/.outlining.json deleted file mode 100644 index 602fd19..0000000 --- a/staubli_val3_driver/val3/ros_server/.outlining.json +++ /dev/null @@ -1,496 +0,0 @@ -{ - "programs": [ - { - "mode": 3, - "name": "decTrajPtFull", - "nodes": [ - { - "start": 875, - "end": 928, - "isCollapsed": false - }, - { - "start": 1038, - "end": 1091, - "isCollapsed": false - }, - { - "start": 1213, - "end": 1266, - "isCollapsed": false - }, - { - "start": 1374, - "end": 1427, - "isCollapsed": false - }, - { - "start": 1450, - "end": 1677, - "isCollapsed": false - }, - { - "start": 1609, - "end": 1668, - "isCollapsed": false - }, - { - "start": 1701, - "end": 2580, - "isCollapsed": false - }, - { - "start": 1985, - "end": 2044, - "isCollapsed": false - }, - { - "start": 2398, - "end": 2571, - "isCollapsed": false - }, - { - "start": 2607, - "end": 2838, - "isCollapsed": false - }, - { - "start": 2770, - "end": 2829, - "isCollapsed": false - } - ] - }, - { - "mode": 3, - "name": "start", - "nodes": [ - { - "start": 899, - "end": 1009, - "isCollapsed": false - }, - { - "start": 1012, - "end": 1151, - "isCollapsed": false - }, - { - "start": 2340, - "end": 2428, - "isCollapsed": false - }, - { - "start": 2514, - "end": 2705, - "isCollapsed": false - }, - { - "start": 2535, - "end": 2697, - "isCollapsed": false - }, - { - "start": 2564, - "end": 2629, - "isCollapsed": false - }, - { - "start": 2634, - "end": 2687, - "isCollapsed": false - } - ] - }, - { - "mode": 3, - "name": "decodeMsgBody", - "nodes": [ - { - "start": 677, - "end": 920, - "isCollapsed": false - }, - { - "start": 718, - "end": 795, - "isCollapsed": false - }, - { - "start": 800, - "end": 886, - "isCollapsed": false - }, - { - "start": 891, - "end": 908, - "isCollapsed": false - }, - { - "start": 924, - "end": 1593, - "isCollapsed": false - }, - { - "start": 1030, - "end": 1585, - "isCollapsed": false - }, - { - "start": 1080, - "end": 1585, - "isCollapsed": false - }, - { - "start": 1125, - "end": 1419, - "isCollapsed": false - }, - { - "start": 1170, - "end": 1264, - "isCollapsed": false - }, - { - "start": 1273, - "end": 1373, - "isCollapsed": false - }, - { - "start": 1382, - "end": 1403, - "isCollapsed": false - }, - { - "start": 1487, - "end": 1575, - "isCollapsed": false - } - ] - }, - { - "mode": 3, - "name": "decodeMsgHeader", - "nodes": [ - { - "start": 797, - "end": 910, - "isCollapsed": false - }, - { - "start": 1002, - "end": 1126, - "isCollapsed": false - }, - { - "start": 1222, - "end": 1347, - "isCollapsed": false - }, - { - "start": 1444, - "end": 1570, - "isCollapsed": false - } - ] - }, - { - "mode": 3, - "name": "recvMsgBody", - "nodes": [ - { - "start": 677, - "end": 922, - "isCollapsed": false - }, - { - "start": 718, - "end": 796, - "isCollapsed": false - }, - { - "start": 801, - "end": 888, - "isCollapsed": false - }, - { - "start": 893, - "end": 910, - "isCollapsed": false - }, - { - "start": 926, - "end": 1120, - "isCollapsed": false - }, - { - "start": 1021, - "end": 1112, - "isCollapsed": false - } - ] - }, - { - "mode": 3, - "name": "recvMsgHeader", - "nodes": [ - { - "start": 788, - "end": 896, - "isCollapsed": false - }, - { - "start": 900, - "end": 1081, - "isCollapsed": false - }, - { - "start": 995, - "end": 1073, - "isCollapsed": false - } - ] - }, - { - "mode": 3, - "name": "sendAck", - "nodes": [ - { - "start": 660, - "end": 1064, - "isCollapsed": false - }, - { - "start": 701, - "end": 859, - "isCollapsed": false - }, - { - "start": 864, - "end": 1030, - "isCollapsed": false - }, - { - "start": 1035, - "end": 1052, - "isCollapsed": false - } - ] - }, - { - "mode": 3, - "name": "dataReceiver", - "nodes": [ - { - "start": 1022, - "end": 2482, - "isCollapsed": false - }, - { - "start": 1640, - "end": 1723, - "isCollapsed": false - }, - { - "start": 1769, - "end": 1854, - "isCollapsed": false - }, - { - "start": 1859, - "end": 1938, - "isCollapsed": false - }, - { - "start": 1943, - "end": 2024, - "isCollapsed": false - }, - { - "start": 2029, - "end": 2110, - "isCollapsed": false - }, - { - "start": 2115, - "end": 2209, - "isCollapsed": false - }, - { - "start": 2317, - "end": 2434, - "isCollapsed": false - } - ] - }, - { - "mode": 3, - "name": "pushMotion", - "nodes": [ - { - "start": 660, - "end": 2357, - "isCollapsed": false - }, - { - "start": 701, - "end": 1410, - "isCollapsed": false - }, - { - "start": 1415, - "end": 2323, - "isCollapsed": false - }, - { - "start": 2328, - "end": 2345, - "isCollapsed": false - }, - { - "start": 2466, - "end": 3895, - "isCollapsed": false - }, - { - "start": 2550, - "end": 3887, - "isCollapsed": false - }, - { - "start": 2649, - "end": 2942, - "isCollapsed": false - }, - { - "start": 3155, - "end": 3316, - "isCollapsed": false - }, - { - "start": 3282, - "end": 3306, - "isCollapsed": false - }, - { - "start": 3326, - "end": 3887, - "isCollapsed": false - }, - { - "start": 3474, - "end": 3877, - "isCollapsed": false - }, - { - "start": 3553, - "end": 3865, - "isCollapsed": false - }, - { - "start": 3600, - "end": 3698, - "isCollapsed": false - }, - { - "start": 3709, - "end": 3813, - "isCollapsed": false - }, - { - "start": 3824, - "end": 3847, - "isCollapsed": false - }, - { - "start": 3936, - "end": 4194, - "isCollapsed": false - }, - { - "start": 4076, - "end": 4186, - "isCollapsed": false - } - ] - }, - { - "mode": 3, - "name": "decTrajPt", - "nodes": [ - { - "start": 865, - "end": 918, - "isCollapsed": false - }, - { - "start": 938, - "end": 1155, - "isCollapsed": false - }, - { - "start": 1087, - "end": 1146, - "isCollapsed": false - }, - { - "start": 1269, - "end": 1322, - "isCollapsed": false - }, - { - "start": 1436, - "end": 1489, - "isCollapsed": false - } - ] - }, - { - "mode": 3, - "name": "motionControl", - "nodes": [ - { - "start": 676, - "end": 2942, - "isCollapsed": false - }, - { - "start": 751, - "end": 2855, - "isCollapsed": false - }, - { - "start": 948, - "end": 1186, - "isCollapsed": false - }, - { - "start": 1038, - "end": 1174, - "isCollapsed": false - }, - { - "start": 1411, - "end": 2556, - "isCollapsed": false - }, - { - "start": 1497, - "end": 2544, - "isCollapsed": false - } - ] - }, - { - "mode": 3, - "name": "interpolateVel", - "nodes": [ - { - "start": 377, - "end": 506, - "isCollapsed": false - } - ] - } - ] -} \ No newline at end of file From 9ee7d2e408853b2e0c1e3a4464063581460e675b Mon Sep 17 00:00:00 2001 From: gavanderhoorn Date: Thu, 12 Mar 2020 11:26:07 +0100 Subject: [PATCH 11/14] Remove vel smoothing from decTrajPtFull. Similar to decTrajPt: this code has been moved to interpolateVel and motionControl. Ref: 7a517065cae414c40fd5a76fdcb35442bb3aa099. --- staubli_val3_driver/val3/ros_server/decTrajPtFull.pgx | 11 ----------- 1 file changed, 11 deletions(-) diff --git a/staubli_val3_driver/val3/ros_server/decTrajPtFull.pgx b/staubli_val3_driver/val3/ros_server/decTrajPtFull.pgx index d303c84..4fe4df1 100644 --- a/staubli_val3_driver/val3/ros_server/decTrajPtFull.pgx +++ b/staubli_val3_driver/val3/ros_server/decTrajPtFull.pgx @@ -69,22 +69,11 @@ // velocities[10] for l_nIndex=0 to 9 step 1 - // Velocity buffer to smooth out last traj point - nVel[1] = nVel[0] - nVel[0] = rosTrajPtMsg.jointTrajPt.nVelocity l_nRetVal=fromBinary(rosTrajPtFMsg.body.nData[(l_nIndex*4)+56],4,"4.0l",rosTrajPtFMsg.jointTrajPtFull.nVelocities[l_nIndex]) if(l_nRetVal!=1) x_bFlag=false return endIf - // Round to remove trailing decimal places from fromBinary() - rosTrajPtMsg.jointTrajPt.nVelocity = rosTrajPtMsg.jointTrajPt.nVelocity * 10000 - rosTrajPtMsg.jointTrajPt.nVelocity = round(rosTrajPtMsg.jointTrajPt.nVelocity) - rosTrajPtMsg.jointTrajPt.nVelocity = rosTrajPtMsg.jointTrajPt.nVelocity / 10000 - // Smooth last point in traj - if((rosTrajPtMsg.jointTrajPt.nVelocity == 0.1) and (rosTrajPtMsg.jointTrajPt.nSeq != 0)) - rosTrajPtMsg.jointTrajPt.nVelocity = (nVel[0] - (nVel[1] - nVel[0])) - endIf endFor // accelerations[10] From 021155cf702c65b2d59c7a6b294d1c385fee4f2b Mon Sep 17 00:00:00 2001 From: gavanderhoorn Date: Thu, 12 Mar 2020 11:33:17 +0100 Subject: [PATCH 12/14] This is actually a CommType, not a StandardMsgType. --- staubli_val3_driver/val3/ros_server/pushMotion.pgx | 4 ++-- staubli_val3_driver/val3/ros_server/ros_server.dtx | 2 ++ 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/staubli_val3_driver/val3/ros_server/pushMotion.pgx b/staubli_val3_driver/val3/ros_server/pushMotion.pgx index de76bd9..1aacdd5 100644 --- a/staubli_val3_driver/val3/ros_server/pushMotion.pgx +++ b/staubli_val3_driver/val3/ros_server/pushMotion.pgx @@ -89,7 +89,7 @@ if(l_bOk==false) // could not push trajectory point into buffer // thus ACK should flag FAILURE, but only if comm type is request - if(rosGenericMsg.header.nCommType==nGenericMsgType["SERVICE_REQUEST"]) + if(rosGenericMsg.header.nCommType==nCommType["SERVICE_REQUEST"]) switch rosGenericMsg.header.nMsgType case nGenericMsgType["JOINT_TRAJ_PT"] rosTrajPtAck.header.nReplyCode=2 @@ -104,7 +104,7 @@ endIf endIf // check whether ACK should be sent - if(rosGenericMsg.header.nCommType == nGenericMsgType["SERVICE_REQUEST"]) + if(rosGenericMsg.header.nCommType == nCommType["SERVICE_REQUEST"]) // comm type request requires ACK x_nMsgRecvState = nAck else diff --git a/staubli_val3_driver/val3/ros_server/ros_server.dtx b/staubli_val3_driver/val3/ros_server/ros_server.dtx index 7e97f5e..6eaf761 100644 --- a/staubli_val3_driver/val3/ros_server/ros_server.dtx +++ b/staubli_val3_driver/val3/ros_server/ros_server.dtx @@ -459,6 +459,8 @@ + + From 6089826fcc5bea5dd8007540931f7f34e17d4c69 Mon Sep 17 00:00:00 2001 From: gavanderhoorn Date: Thu, 12 Mar 2020 11:51:22 +0100 Subject: [PATCH 13/14] Use simple_message naming for protocol constants. --- staubli_val3_driver/val3/ros_server/decodeMsgBody.pgx | 8 ++++---- staubli_val3_driver/val3/ros_server/encodeAck.pgx | 4 ++-- staubli_val3_driver/val3/ros_server/pushMotion.pgx | 8 ++++---- staubli_val3_driver/val3/ros_server/recvMsgBody.pgx | 4 ++-- staubli_val3_driver/val3/ros_server/ros_server.dtx | 2 +- staubli_val3_driver/val3/ros_server/sendAck.pgx | 4 ++-- 6 files changed, 15 insertions(+), 15 deletions(-) diff --git a/staubli_val3_driver/val3/ros_server/decodeMsgBody.pgx b/staubli_val3_driver/val3/ros_server/decodeMsgBody.pgx index ad34fb1..6961910 100644 --- a/staubli_val3_driver/val3/ros_server/decodeMsgBody.pgx +++ b/staubli_val3_driver/val3/ros_server/decodeMsgBody.pgx @@ -27,10 +27,10 @@ l_bFlag=false switch rosGenericMsg.header.nMsgType - case nGenericMsgType["JOINT_TRAJ_PT"] + case nStandardMsgType["JOINT_TRAJ_PT"] call decTrajPt(l_bFlag) break - case nGenericMsgType["JOINT_TRAJ_PT_FULL"] + case nStandardMsgType["JOINT_TRAJ_PT_FULL"] call decTrajPtFull(l_bFlag) break default @@ -44,10 +44,10 @@ // error whilst decoding msg body... if (rosGenericMsg.header.nCommType==2) switch rosGenericMsg.header.nMsgType - case nGenericMsgType["JOINT_TRAJ_PT"] + case nStandardMsgType["JOINT_TRAJ_PT"] rosTrajPtAck.header.nReplyCode=2 break - case nGenericMsgType["JOINT_TRAJ_PT_FULL"] + case nStandardMsgType["JOINT_TRAJ_PT_FULL"] rosTrajPtFAck.header.nReplyCode=2 break default diff --git a/staubli_val3_driver/val3/ros_server/encodeAck.pgx b/staubli_val3_driver/val3/ros_server/encodeAck.pgx index 1dfdd10..73ed0f7 100644 --- a/staubli_val3_driver/val3/ros_server/encodeAck.pgx +++ b/staubli_val3_driver/val3/ros_server/encodeAck.pgx @@ -19,10 +19,10 @@ switch rosGenericMsg.header.nMsgType - case nGenericMsgType["JOINT_TRAJ_PT"] + case nStandardMsgType["JOINT_TRAJ_PT"] call encTrajPtAck() break - case nGenericMsgType["JOINT_TRAJ_PT_FULL"] + case nStandardMsgType["JOINT_TRAJ_PT_FULL"] call encTrajPtFAck() break default diff --git a/staubli_val3_driver/val3/ros_server/pushMotion.pgx b/staubli_val3_driver/val3/ros_server/pushMotion.pgx index 1aacdd5..ea3036f 100644 --- a/staubli_val3_driver/val3/ros_server/pushMotion.pgx +++ b/staubli_val3_driver/val3/ros_server/pushMotion.pgx @@ -28,7 +28,7 @@ switch rosGenericMsg.header.nMsgType - case nGenericMsgType["JOINT_TRAJ_PT"] + case nStandardMsgType["JOINT_TRAJ_PT"] l_mTrajPt.nSequence=rosTrajPtMsg.jointTrajPt.nSeq // ROS joint position values are in rad, but VAL3 uses degrees @@ -43,7 +43,7 @@ l_mTrajPt.nDuration=rosTrajPtMsg.jointTrajPt.nDuration break - case nGenericMsgType["JOINT_TRAJ_PT_FULL"] + case nStandardMsgType["JOINT_TRAJ_PT_FULL"] l_mTrajPt.nSequence=rosTrajPtFMsg.jointTrajPtFull.nSeq l_mTrajPt.nDuration=rosTrajPtFMsg.jointTrajPtFull.nTime @@ -91,10 +91,10 @@ // thus ACK should flag FAILURE, but only if comm type is request if(rosGenericMsg.header.nCommType==nCommType["SERVICE_REQUEST"]) switch rosGenericMsg.header.nMsgType - case nGenericMsgType["JOINT_TRAJ_PT"] + case nStandardMsgType["JOINT_TRAJ_PT"] rosTrajPtAck.header.nReplyCode=2 break - case nGenericMsgType["JOINT_TRAJ_PT_FULL"] + case nStandardMsgType["JOINT_TRAJ_PT_FULL"] rosTrajPtFAck.header.nReplyCode=2 break default diff --git a/staubli_val3_driver/val3/ros_server/recvMsgBody.pgx b/staubli_val3_driver/val3/ros_server/recvMsgBody.pgx index 3389328..39be1f4 100644 --- a/staubli_val3_driver/val3/ros_server/recvMsgBody.pgx +++ b/staubli_val3_driver/val3/ros_server/recvMsgBody.pgx @@ -27,10 +27,10 @@ l_bFlag=false switch rosGenericMsg.header.nMsgType - case nGenericMsgType["JOINT_TRAJ_PT"] + case nStandardMsgType["JOINT_TRAJ_PT"] call recvTrajPt(l_bFlag) break - case nGenericMsgType["JOINT_TRAJ_PT_FULL"] + case nStandardMsgType["JOINT_TRAJ_PT_FULL"] call recvTrajPtFull(l_bFlag) break default diff --git a/staubli_val3_driver/val3/ros_server/ros_server.dtx b/staubli_val3_driver/val3/ros_server/ros_server.dtx index 6eaf761..a80e2c5 100644 --- a/staubli_val3_driver/val3/ros_server/ros_server.dtx +++ b/staubli_val3_driver/val3/ros_server/ros_server.dtx @@ -456,7 +456,7 @@ - + diff --git a/staubli_val3_driver/val3/ros_server/sendAck.pgx b/staubli_val3_driver/val3/ros_server/sendAck.pgx index f1f7ca9..a7c6d72 100644 --- a/staubli_val3_driver/val3/ros_server/sendAck.pgx +++ b/staubli_val3_driver/val3/ros_server/sendAck.pgx @@ -22,10 +22,10 @@ switch rosGenericMsg.header.nMsgType - case nGenericMsgType["JOINT_TRAJ_PT"] + case nStandardMsgType["JOINT_TRAJ_PT"] call sendRosMsg(siTcpIpMotion, rosTrajPtAck.prefix, rosTrajPtAck.header, rosTrajPtAck.body, nInConnFlag) break - case nGenericMsgType["JOINT_TRAJ_PT_FULL"] + case nStandardMsgType["JOINT_TRAJ_PT_FULL"] call sendRosMsg(siTcpIpMotion, rosTrajPtFAck.prefix, rosTrajPtFAck.header, rosTrajPtFAck.body, nInConnFlag) break default From 1f24ea9bc6d6db705413620bfaee2dc68034bc3a Mon Sep 17 00:00:00 2001 From: marshallpowell97 Date: Thu, 12 Mar 2020 09:36:40 -0400 Subject: [PATCH 14/14] Shorten nStandardMsgType to fit 15-character limit --- staubli_val3_driver/val3/ros_server/decodeMsgBody.pgx | 8 ++++---- staubli_val3_driver/val3/ros_server/encodeAck.pgx | 4 ++-- staubli_val3_driver/val3/ros_server/pushMotion.pgx | 8 ++++---- staubli_val3_driver/val3/ros_server/recvMsgBody.pgx | 4 ++-- staubli_val3_driver/val3/ros_server/ros_server.dtx | 2 +- staubli_val3_driver/val3/ros_server/sendAck.pgx | 4 ++-- 6 files changed, 15 insertions(+), 15 deletions(-) diff --git a/staubli_val3_driver/val3/ros_server/decodeMsgBody.pgx b/staubli_val3_driver/val3/ros_server/decodeMsgBody.pgx index 6961910..318b451 100644 --- a/staubli_val3_driver/val3/ros_server/decodeMsgBody.pgx +++ b/staubli_val3_driver/val3/ros_server/decodeMsgBody.pgx @@ -27,10 +27,10 @@ l_bFlag=false switch rosGenericMsg.header.nMsgType - case nStandardMsgType["JOINT_TRAJ_PT"] + case nStdMsgType["JOINT_TRAJ_PT"] call decTrajPt(l_bFlag) break - case nStandardMsgType["JOINT_TRAJ_PT_FULL"] + case nStdMsgType["JOINT_TRAJ_PT_FULL"] call decTrajPtFull(l_bFlag) break default @@ -44,10 +44,10 @@ // error whilst decoding msg body... if (rosGenericMsg.header.nCommType==2) switch rosGenericMsg.header.nMsgType - case nStandardMsgType["JOINT_TRAJ_PT"] + case nStdMsgType["JOINT_TRAJ_PT"] rosTrajPtAck.header.nReplyCode=2 break - case nStandardMsgType["JOINT_TRAJ_PT_FULL"] + case nStdMsgType["JOINT_TRAJ_PT_FULL"] rosTrajPtFAck.header.nReplyCode=2 break default diff --git a/staubli_val3_driver/val3/ros_server/encodeAck.pgx b/staubli_val3_driver/val3/ros_server/encodeAck.pgx index 73ed0f7..cac2239 100644 --- a/staubli_val3_driver/val3/ros_server/encodeAck.pgx +++ b/staubli_val3_driver/val3/ros_server/encodeAck.pgx @@ -19,10 +19,10 @@ switch rosGenericMsg.header.nMsgType - case nStandardMsgType["JOINT_TRAJ_PT"] + case nStdMsgType["JOINT_TRAJ_PT"] call encTrajPtAck() break - case nStandardMsgType["JOINT_TRAJ_PT_FULL"] + case nStdMsgType["JOINT_TRAJ_PT_FULL"] call encTrajPtFAck() break default diff --git a/staubli_val3_driver/val3/ros_server/pushMotion.pgx b/staubli_val3_driver/val3/ros_server/pushMotion.pgx index ea3036f..43eea10 100644 --- a/staubli_val3_driver/val3/ros_server/pushMotion.pgx +++ b/staubli_val3_driver/val3/ros_server/pushMotion.pgx @@ -28,7 +28,7 @@ switch rosGenericMsg.header.nMsgType - case nStandardMsgType["JOINT_TRAJ_PT"] + case nStdMsgType["JOINT_TRAJ_PT"] l_mTrajPt.nSequence=rosTrajPtMsg.jointTrajPt.nSeq // ROS joint position values are in rad, but VAL3 uses degrees @@ -43,7 +43,7 @@ l_mTrajPt.nDuration=rosTrajPtMsg.jointTrajPt.nDuration break - case nStandardMsgType["JOINT_TRAJ_PT_FULL"] + case nStdMsgType["JOINT_TRAJ_PT_FULL"] l_mTrajPt.nSequence=rosTrajPtFMsg.jointTrajPtFull.nSeq l_mTrajPt.nDuration=rosTrajPtFMsg.jointTrajPtFull.nTime @@ -91,10 +91,10 @@ // thus ACK should flag FAILURE, but only if comm type is request if(rosGenericMsg.header.nCommType==nCommType["SERVICE_REQUEST"]) switch rosGenericMsg.header.nMsgType - case nStandardMsgType["JOINT_TRAJ_PT"] + case nStdMsgType["JOINT_TRAJ_PT"] rosTrajPtAck.header.nReplyCode=2 break - case nStandardMsgType["JOINT_TRAJ_PT_FULL"] + case nStdMsgType["JOINT_TRAJ_PT_FULL"] rosTrajPtFAck.header.nReplyCode=2 break default diff --git a/staubli_val3_driver/val3/ros_server/recvMsgBody.pgx b/staubli_val3_driver/val3/ros_server/recvMsgBody.pgx index 39be1f4..f6c0593 100644 --- a/staubli_val3_driver/val3/ros_server/recvMsgBody.pgx +++ b/staubli_val3_driver/val3/ros_server/recvMsgBody.pgx @@ -27,10 +27,10 @@ l_bFlag=false switch rosGenericMsg.header.nMsgType - case nStandardMsgType["JOINT_TRAJ_PT"] + case nStdMsgType["JOINT_TRAJ_PT"] call recvTrajPt(l_bFlag) break - case nStandardMsgType["JOINT_TRAJ_PT_FULL"] + case nStdMsgType["JOINT_TRAJ_PT_FULL"] call recvTrajPtFull(l_bFlag) break default diff --git a/staubli_val3_driver/val3/ros_server/ros_server.dtx b/staubli_val3_driver/val3/ros_server/ros_server.dtx index a80e2c5..0ded1f8 100644 --- a/staubli_val3_driver/val3/ros_server/ros_server.dtx +++ b/staubli_val3_driver/val3/ros_server/ros_server.dtx @@ -456,7 +456,7 @@ - + diff --git a/staubli_val3_driver/val3/ros_server/sendAck.pgx b/staubli_val3_driver/val3/ros_server/sendAck.pgx index a7c6d72..ae4be59 100644 --- a/staubli_val3_driver/val3/ros_server/sendAck.pgx +++ b/staubli_val3_driver/val3/ros_server/sendAck.pgx @@ -22,10 +22,10 @@ switch rosGenericMsg.header.nMsgType - case nStandardMsgType["JOINT_TRAJ_PT"] + case nStdMsgType["JOINT_TRAJ_PT"] call sendRosMsg(siTcpIpMotion, rosTrajPtAck.prefix, rosTrajPtAck.header, rosTrajPtAck.body, nInConnFlag) break - case nStandardMsgType["JOINT_TRAJ_PT_FULL"] + case nStdMsgType["JOINT_TRAJ_PT_FULL"] call sendRosMsg(siTcpIpMotion, rosTrajPtFAck.prefix, rosTrajPtFAck.header, rosTrajPtFAck.body, nInConnFlag) break default