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/README.md b/README.md index a113f8f..c374df0 100644 --- a/README.md +++ b/README.md @@ -9,7 +9,7 @@ ## 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. ## Documentation @@ -19,8 +19,8 @@ Refer to the `staubli_val3_driver` [readme](./staubli_val3_driver/README.md) for ## 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. diff --git a/staubli_val3_driver/README.md b/staubli_val3_driver/README.md index 0e5ebf3..08b6b82 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 recommended) ## 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 diff --git a/staubli_val3_driver/val3/ros_server/dataReceiver.pgx b/staubli_val3_driver/val3/ros_server/dataReceiver.pgx index c73b43a..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=0 + + // 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==0) + if(l_nMsgRecvState==nRecHeader) call recvMsgHeader(l_nMsgRecvState) endIf nHeaderTime = clock() - l_nStartTime - if (l_nMsgRecvState==1) + if(l_nMsgRecvState==nDecHeader) call decodeMsgHeader(l_nMsgRecvState) endIf - if (l_nMsgRecvState==2) + if(l_nMsgRecvState==nRecBody) call recvMsgBody(l_nMsgRecvState) endIf - if (l_nMsgRecvState==3) + if(l_nMsgRecvState==nDecBody) call decodeMsgBody(l_nMsgRecvState) endIf - if (l_nMsgRecvState==4) + if(l_nMsgRecvState==nPushMotion) call pushMotion(l_nMsgRecvState) endIf - if (l_nMsgRecvState==5) + if(l_nMsgRecvState==nAck) 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_nMsgRecvStatenAck) popUpMsg("Data reception state machine error") endIf @@ -75,4 +85,4 @@ endWhile end]]> - + \ 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..54ffc40 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,7 +40,7 @@ // 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 @@ -48,14 +48,14 @@ // 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 // 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 +66,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..4fe4df1 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 @@ -70,7 +70,7 @@ // velocities[10] for l_nIndex=0 to 9 step 1 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 @@ -79,7 +79,7 @@ // 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 +91,4 @@ x_bFlag=true 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..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 11 + case nStdMsgType["JOINT_TRAJ_PT"] call decTrajPt(l_bFlag) break - case 14 + case nStdMsgType["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=nPushMotion else // error whilst decoding msg body... if (rosGenericMsg.header.nCommType==2) switch rosGenericMsg.header.nMsgType - case 11 + case nStdMsgType["JOINT_TRAJ_PT"] rosTrajPtAck.header.nReplyCode=2 break - case 14 + case nStdMsgType["JOINT_TRAJ_PT_FULL"] rosTrajPtFAck.header.nReplyCode=2 break default break endSwitch // jump to state 5: sendAck() - x_nMsgRecvState=5 + x_nMsgRecvState=nAck else // reset state machine, since no ACK is required - x_nMsgRecvState=0 + x_nMsgRecvState=nReset 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..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=0 + 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=0 + 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=0 + 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=0 + x_nMsgRecvState=nReset return endIf // no errors detected whilst decoding prefix and header // move on to state 2: receive msg body - x_nMsgRecvState=2 + x_nMsgRecvState=nRecBody 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..cac2239 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/interpolateVel.pgx b/staubli_val3_driver/val3/ros_server/interpolateVel.pgx new file mode 100644 index 0000000..2ca0b75 --- /dev/null +++ b/staubli_val3_driver/val3/ros_server/interpolateVel.pgx @@ -0,0 +1,21 @@ + + + + + + + + + \ 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..9165d3d 100644 --- a/staubli_val3_driver/val3/ros_server/motionControl.pgx +++ b/staubli_val3_driver/val3/ros_server/motionControl.pgx @@ -5,8 +5,6 @@ - - 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 @@ -45,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 @@ -79,4 +85,4 @@ endWhile 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 dedcb37..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 11 + 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 14 + case nStdMsgType["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==nCommType["SERVICE_REQUEST"]) switch rosGenericMsg.header.nMsgType - case 11 + case nStdMsgType["JOINT_TRAJ_PT"] rosTrajPtAck.header.nReplyCode=2 break - case 14 + case nStdMsgType["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 == nCommType["SERVICE_REQUEST"]) // comm type request requires ACK - x_nMsgRecvState = 5 + x_nMsgRecvState = nAck else // other comm types (e.g., topic) does not require ACK, hence skip state - x_nMsgRecvState = 0 + x_nMsgRecvState = nReset 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..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 11 + case nStdMsgType["JOINT_TRAJ_PT"] call recvTrajPt(l_bFlag) break - case 14 + case nStdMsgType["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=nDecBody else // error whilst receiving msg body, reset state machine - x_nMsgRecvState=0 + x_nMsgRecvState=nReset 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..2abd46d 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=nDecHeader else // error: reset state to 0: receive header - x_nMsgRecvState=0 + x_nMsgRecvState=nReset 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..0ded1f8 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,31 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + - + \ 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 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 diff --git a/staubli_val3_driver/val3/ros_server/sendAck.pgx b/staubli_val3_driver/val3/ros_server/sendAck.pgx index d42dfca..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 11 + case nStdMsgType["JOINT_TRAJ_PT"] call sendRosMsg(siTcpIpMotion, rosTrajPtAck.prefix, rosTrajPtAck.header, rosTrajPtAck.body, nInConnFlag) break - case 14 + case nStdMsgType["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=nReset end]]> - + \ 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