diff --git a/staubli_val3_driver/val3/ros_server/dataReceiver.pgx b/staubli_val3_driver/val3/ros_server/dataReceiver.pgx index 92f1ece..ef6bb3e 100644 --- a/staubli_val3_driver/val3/ros_server/dataReceiver.pgx +++ b/staubli_val3_driver/val3/ros_server/dataReceiver.pgx @@ -1,88 +1,95 @@ - - - - - - - - nAck) - popUpMsg("Data reception state machine error") - endIf - - // sequence task - delay(0) - - - endWhile -end]]> - + + + + + + + + nAck) + popUpMsg("Data reception state machine error") + endIf + + // sequence task + delay(0) + + + endWhile +end]]> + \ No newline at end of file diff --git a/staubli_val3_driver/val3/ros_server/dataStreamer.pgx b/staubli_val3_driver/val3/ros_server/dataStreamer.pgx index ea67070..82612e3 100644 --- a/staubli_val3_driver/val3/ros_server/dataStreamer.pgx +++ b/staubli_val3_driver/val3/ros_server/dataStreamer.pgx @@ -1,54 +1,59 @@ - - - - - - - - - - + + + + + + + + + + \ 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 9165d3d..5c28bf3 100644 --- a/staubli_val3_driver/val3/ros_server/motionControl.pgx +++ b/staubli_val3_driver/val3/ros_server/motionControl.pgx @@ -1,88 +1,93 @@ - - - - - - - - - - 0) - call libQueueFuncs:pop(qTrajPtBuffer,l_mTrajPt, l_bFlag) - if(l_bFlag == true) - nPtsPopped = nPtsPopped + 1 - call libQueueFuncs:push(qMoveBuffer,l_mTrajPt,l_bFlag) - endIf - endIf - - // 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) - call libQueueFuncs:pop(qMoveBuffer,l_motion,l_bFlag) - 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 - // start executing the segment towards 'l_motion'). - // Checking for dist(l_motion, herej()) (pseudo code) would not be correct as there will - // always be a delay in the data coming out of herej() and the actual pose of the robot. - // @TODO potentially set move id according to sequence value - nMoveId = movej(l_motion.jJointRx, tTool, l_motion.mDesc) - endIf - endIf - else - // stop immediately -- resetMotion() also resets moveId - resetMotion() - // clear buffers - call libQueueFuncs:clear(qTrajPtBuffer) - call libQueueFuncs:clear(qMoveBuffer) - bStopNow = false - - // debug - nPtsPopped = 0 - nMovePts = 0 - endIf - - nMotionProgress = getMoveId() - - // sequence task - delay(0) - endWhile -end]]> - + + + + + + + + + + 0) + call libQueueFuncs:pop(qTrajPtBuffer,l_mTrajPt, l_bFlag) + if(l_bFlag == true) + nPtsPopped = nPtsPopped + 1 + call libQueueFuncs:push(qMoveBuffer,l_mTrajPt,l_bFlag) + endIf + endIf + + // 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) + call libQueueFuncs:pop(qMoveBuffer,l_motion,l_bFlag) + 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 + // start executing the segment towards 'l_motion'). + // Checking for dist(l_motion, herej()) (pseudo code) would not be correct as there will + // always be a delay in the data coming out of herej() and the actual pose of the robot. + // @TODO potentially set move id according to sequence value + nMoveId = movej(l_motion.jJointRx, tTool, l_motion.mDesc) + endIf + endIf + else + // stop immediately -- resetMotion() also resets moveId + resetMotion() + // clear buffers + call libQueueFuncs:clear(qTrajPtBuffer) + call libQueueFuncs:clear(qMoveBuffer) + bStopNow = false + + // debug + nPtsPopped = 0 + nMovePts = 0 + endIf + + nMotionProgress = getMoveId() + + // sequence task + delay(0) + endWhile +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 71a4f3a..fd1982c 100644 --- a/staubli_val3_driver/val3/ros_server/ros_server.dtx +++ b/staubli_val3_driver/val3/ros_server/ros_server.dtx @@ -1,483 +1,491 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/staubli_val3_driver/val3/ros_server/ros_server.pjx b/staubli_val3_driver/val3/ros_server/ros_server.pjx index 3b5f2dd..12ae19d 100644 --- a/staubli_val3_driver/val3/ros_server/ros_server.pjx +++ b/staubli_val3_driver/val3/ros_server/ros_server.pjx @@ -1,58 +1,59 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ 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 3eeb66c..c5769cd 100644 --- a/staubli_val3_driver/val3/ros_server/start.pgx +++ b/staubli_val3_driver/val3/ros_server/start.pgx @@ -1,141 +1,133 @@ - - - - - - - - - - - - - - - - = 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))) - // VAL 3 version is too old - popUpMsg("Error: VAL 3 version is < 7.7, socket communication will not work!") - taskKill("ros_server~") - endIf - libInterface:libLoad("ros_libs/UserInterface/interfaceCS8") - elseIf(mid(getVersion("System"),1,1) == "8") - // CS9 - VAL 3 rev 8 - libInterface:libLoad("ros_libs/UserInterface/interfaceCS9") - endIf - - // feedback task will run every 20ms (50Hz) - l_nFbkPeriod=0.02 - //l_nFbkPeriod = 0.032 // 30Hz - //l_nFbkPeriod = 0.05 // 20Hz - //l_nFbkPeriod = 1.0 // 1Hz - - // motion control task will run every 4ms (250Hz) - l_nMotionPeriod = 0.004 - //l_nMotionPeriod=0.012 // 83Hz - //l_nMotionPeriod = 0.05 // 20Hz - - // data (trajectory) reception task will run every 4ms (250Hz) - l_nTrajPeriod = 0.004 - // not in use at the moment - l_nScreenUpdate = 1.0 - - sStreamTaskName = "streamerTask" - sTrajPtTaskName = "trajPtTask" - sMotionTaskName = "motionTask" - sScreenTaskName = "screenTask" - sHtbtTaskName = "htbtTask" - - // initialise TCP/IP parameters - call initParams() - // setup prefix and header of ROS messages - call setupRosMsgs() - - // socket connection status flags: initially disconnected - nInConnFlag = 0 - nOutConnFlag = 0 - - // flag defining whether velocity to be overwritten to max. vel. - bOverwriteVel = false - - nPtsPopped = 0 - nMovePts = 0 - - // flush buffer of incoming socket - clearBuffer(siTcpIpMotion) - // flush buffer of outgoing socket - clearBuffer(siTcpIpFbk) - - // check if robot is not in emergency stop and wait until estop is reset - 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) - do - enablePower() - delay(2) - until isPowered() - else - popUpMsg("Power must be enabled manually") - endIf - endIf - - // reset motion, potentially setting a start joint position for calling autoConnect() in the future - resetMotion() - - // create joint state and robot status streamer task to send data to industrial_robot_client - taskCreateSync sStreamTaskName, l_nFbkPeriod, bFbkOverrun, dataStreamer() - - // create trajectory point task to receive data from industrial_robot_client - taskCreateSync sTrajPtTaskName, l_nTrajPeriod, bTrajPtOverrun, dataReceiver() - //taskCreate sTrajPtTaskName, 100, dataReceiver() - - // create motion task to synchronously apply motion commands to robot - taskCreateSync sMotionTaskName, l_nMotionPeriod, bMotionOverrun, motionControl() - - // create synchronous task to update screen - //taskCreateSync sScreenTaskName, l_nScreenUpdate, bScreenOverrun, screenUpdate() - taskCreate sScreenTaskName, 10, screenUpdate() - - // create asynchronous task to receive heartbeat on port 11002 - taskCreate sHtbtTaskName, 10, recvHeartbeat() - -end]]> - + + + + + + + + + + + + + + = 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") + call stop() + endIf + if( !((l_nVersionMajor == 7) and (l_nVersionMinor >= 7))) + // VAL 3 version is too old + popUpMsg("Error: VAL 3 version is < 7.7, socket communication will not work!") + call stop() + endIf + nLibLoadError = libInterface:libLoad("ros_libs/UserInterface/interfaceCS8") + elseIf(mid(getVersion("System"),1,1) == "8") + // CS9 - VAL 3 rev 8 + nLibLoadError = libInterface:libLoad("ros_libs/UserInterface/interfaceCS9") + endIf + + // check for libLoad error + if nLibLoadError != 0 + popUpMsg("Error loading UserPage!") + call stop() + else + // feedback task will run every 20ms (50Hz) + nFbkPeriod=0.02 + //l_nFbkPeriod = 0.032 // 30Hz + //l_nFbkPeriod = 0.05 // 20Hz + //l_nFbkPeriod = 1.0 // 1Hz + + // motion control task will run every 4ms (250Hz) + nMotionPeriod = 0.004 + //l_nMotionPeriod=0.012 // 83Hz + //l_nMotionPeriod = 0.05 // 20Hz + + // data (trajectory) reception task will run every 4ms (250Hz) + nTrajPeriod = 0.004 + // not in use at the moment + nScreenUpdate = 1.0 + + sStreamTaskName = "streamerTask" + sTrajPtTaskName = "trajPtTask" + sMotionTaskName = "motionTask" + sScreenTaskName = "screenTask" + sHtbtTaskName = "htbtTask" + + // initialise TCP/IP parameters + call initParams() + // setup prefix and header of ROS messages + call setupRosMsgs() + + // socket connection status flags: initially disconnected + nInConnFlag = 0 + nOutConnFlag = 0 + + // flag defining whether velocity to be overwritten to max. vel. + bOverwriteVel = false + + nPtsPopped = 0 + nMovePts = 0 + + // flush buffer of incoming socket + clearBuffer(siTcpIpMotion) + // flush buffer of outgoing socket + clearBuffer(siTcpIpFbk) + + // check if robot is not in emergency stop and wait until estop is reset + 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) + do + enablePower() + delay(2) + until isPowered() + else + popUpMsg("Power must be enabled manually") + endIf + endIf + + // reset motion, potentially setting a start joint position for calling autoConnect() in the future + resetMotion() + + //start task for the taskManager: + taskCreate "taskManager", 50, taskManager() + wait(taskStatus("taskManager") != -1) + endIf + + +end]]> + \ No newline at end of file diff --git a/staubli_val3_driver/val3/ros_server/stop.pgx b/staubli_val3_driver/val3/ros_server/stop.pgx index 53fec5d..aa020db 100644 --- a/staubli_val3_driver/val3/ros_server/stop.pgx +++ b/staubli_val3_driver/val3/ros_server/stop.pgx @@ -1,26 +1,37 @@ - - - - - - \ No newline at end of file + + + + + + diff --git a/staubli_val3_driver/val3/ros_server/taskManager.pgx b/staubli_val3_driver/val3/ros_server/taskManager.pgx new file mode 100644 index 0000000..14db520 --- /dev/null +++ b/staubli_val3_driver/val3/ros_server/taskManager.pgx @@ -0,0 +1,60 @@ + + + + 1 : VAL3 runtime error + + do + + // create joint state and robot status streamer task to send data to industrial_robot_client + if taskStatus(sStreamTaskName) == -1 + taskCreateSync sStreamTaskName, nFbkPeriod, bFbkOverrun, dataStreamer() + wait(taskStatus(sStreamTaskName) != -1) + endIf + + // create trajectory point task to receive data from industrial_robot_client + if taskStatus(sTrajPtTaskName) == -1 + taskCreateSync sTrajPtTaskName, nTrajPeriod, bTrajPtOverrun, dataReceiver() + wait(taskStatus(sTrajPtTaskName) != -1) + endIf + + // create motion task to synchronously apply motion commands to robot + if taskStatus(sMotionTaskName) == -1 + taskCreateSync sMotionTaskName, nMotionPeriod, bMotionOverrun, motionControl() + wait(taskStatus(sMotionTaskName) != -1) + endIf + + // create asynchronous task to update screen + if taskStatus(sScreenTaskName) == -1 + taskCreate sScreenTaskName, 10, screenUpdate() + wait(taskStatus(sScreenTaskName) != -1) + endIf + + // create asynchronous task to receive heartbeat on port 11002 + if taskStatus(sHtbtTaskName) == -1 + taskCreate sHtbtTaskName, 10, recvHeartbeat() + wait(taskStatus(sHtbtTaskName) != -1) + endIf + + until false +end]]> + + \ No newline at end of file