Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add taskManager and check for libLoad errors #39

Open
wants to merge 2 commits into
base: master
Choose a base branch
from
Open

Add taskManager and check for libLoad errors #39

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
428eb7b
Add taskManager and check for libLoad errors
JohannesFriedrich Jul 12, 2021
050aa80
Update stop.pgx to first kill the taskmanager
JohannesFriedrich Jul 14, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
181 changes: 94 additions & 87 deletions staubli_val3_driver/val3/ros_server/dataReceiver.pgx
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,88 +1,95 @@
<?xml version="1.0" encoding="utf-8"?>
<Programs xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xmlns="http://www.staubli.com/robotics/VAL3/Program/2">
<Program name="dataReceiver" access="public">
<Locals>
<Local name="l_nStartTime" type="num" xsi:type="array" size="1" />
<Local name="l_nMsgRecvState" type="num" xsi:type="array" size="1" />
</Locals>
<Code><![CDATA[begin
// Copyright (c) 2016, Ocado Technology - Robotics Research Team
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.



// state machine at initial state
// 0: receive header
// 1: decode header
// 2: receive body
// 3: decode body
// 4: push motion into buffer
// 5: send ACK

// 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)
// 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.
// As a result, receiving one single message, pushing motion and sending ACK
// would take 6 * task period using switch-case statement.
// The desirable behaviour is to receive and decode a full message, push motion
// 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==nRecHeader)
call recvMsgHeader(l_nMsgRecvState)
endIf
nHeaderTime = clock() - l_nStartTime
if(l_nMsgRecvState==nDecHeader)
call decodeMsgHeader(l_nMsgRecvState)
endIf
if(l_nMsgRecvState==nRecBody)
call recvMsgBody(l_nMsgRecvState)
endIf
if(l_nMsgRecvState==nDecBody)
call decodeMsgBody(l_nMsgRecvState)
endIf
if(l_nMsgRecvState==nPushMotion)
call pushMotion(l_nMsgRecvState)
endIf
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<nRecHeader or l_nMsgRecvState>nAck)
popUpMsg("Data reception state machine error")
endIf

// sequence task
delay(0)


endWhile
end]]></Code>
</Program>
<?xml version="1.0" encoding="utf-8"?>
<Programs xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xmlns="http://www.staubli.com/robotics/VAL3/Program/2">
<Program name="dataReceiver" access="public">
<Locals>
<Local name="l_nStartTime" type="num" xsi:type="array" size="1" />
<Local name="l_nMsgRecvState" type="num" xsi:type="array" size="1" />
</Locals>
<Code><![CDATA[begin
// Copyright (c) 2016, Ocado Technology - Robotics Research Team
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.



// state machine at initial state
// 0: receive header
// 1: decode header
// 2: receive body
// 3: decode body
// 4: push motion into buffer
// 5: send ACK

// 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)
// 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.
// As a result, receiving one single message, pushing motion and sending ACK
// would take 6 * task period using switch-case statement.
// The desirable behaviour is to receive and decode a full message, push motion
// into buffer and then send ACK to client WITHIN ONE ITERATION of while(),
// hence the if statement sequence below

// check for overrun
if bTrajPtOverrun
nTrajPtOverrun = nTrajPtOverrun + 1
endIf


l_nStartTime = clock()
if(l_nMsgRecvState==nRecHeader)
call recvMsgHeader(l_nMsgRecvState)
endIf
nHeaderTime = clock() - l_nStartTime
if(l_nMsgRecvState==nDecHeader)
call decodeMsgHeader(l_nMsgRecvState)
endIf
if(l_nMsgRecvState==nRecBody)
call recvMsgBody(l_nMsgRecvState)
endIf
if(l_nMsgRecvState==nDecBody)
call decodeMsgBody(l_nMsgRecvState)
endIf
if(l_nMsgRecvState==nPushMotion)
call pushMotion(l_nMsgRecvState)
endIf
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<nRecHeader or l_nMsgRecvState>nAck)
popUpMsg("Data reception state machine error")
endIf

// sequence task
delay(0)


endWhile
end]]></Code>
</Program>
</Programs>
113 changes: 59 additions & 54 deletions staubli_val3_driver/val3/ros_server/dataStreamer.pgx
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,54 +1,59 @@
<?xml version="1.0" encoding="utf-8"?>
<Programs xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xmlns="http://www.staubli.com/robotics/VAL3/Program/2">
<Program name="dataStreamer" access="public">
<Locals>
<Local name="l_nThrottle" type="num" xsi:type="array" size="1" />
<Local name="l_jCurrPose" type="jointRx" xsi:type="array" size="1" />
</Locals>
<Code><![CDATA[begin
// Copyright (c) 2016, Ocado Technology - Robotics Research Team
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.



l_nThrottle = 0
while (true)
// only attempt to send data if flag indicates that connection is established
if (nOutConnFlag == 1)
// fetch current joint positions
l_jCurrPose=herej()
// encode and send ROS simple_message ID 10: joint state
call encodeJState(l_jCurrPose)
call sendRosMsg(siTcpIpFbk, rosJointMsg.prefix, rosJointMsg.header, rosJointMsg.body, nOutConnFlag)

// send Robot Status messages 5 times slower than Joint State
if (l_nThrottle == 5)
// fetch robot status
call fetchStatus()
// encode and send ROS simple_message ID 13: robot status
call encodeStatus()
call sendRosMsg(siTcpIpFbk, rosStatusMsg.prefix, rosStatusMsg.header, rosStatusMsg.body, nOutConnFlag)
l_nThrottle = 0
endIf
l_nThrottle = l_nThrottle + 1
else
delay(0.1)
endIf

// sequence task
delay(0)
endWhile
end]]></Code>
</Program>
</Programs>
<?xml version="1.0" encoding="utf-8"?>
<Programs xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xmlns="http://www.staubli.com/robotics/VAL3/Program/2">
<Program name="dataStreamer" access="public">
<Locals>
<Local name="l_nThrottle" type="num" xsi:type="array" size="1" />
<Local name="l_jCurrPose" type="jointRx" xsi:type="array" size="1" />
</Locals>
<Code><![CDATA[begin
// Copyright (c) 2016, Ocado Technology - Robotics Research Team
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.



l_nThrottle = 0
while(true)
// check for overrun
if bFbkOverrun
nFbkOverun = nFbkOverun + 1
endIf

// only attempt to send data if flag indicates that connection is established
if(nOutConnFlag == 1)
// fetch current joint positions
l_jCurrPose=herej()
// encode and send ROS simple_message ID 10: joint state
call encodeJState(l_jCurrPose)
call sendRosMsg(siTcpIpFbk, rosJointMsg.prefix, rosJointMsg.header, rosJointMsg.body, nOutConnFlag)

// send Robot Status messages 5 times slower than Joint State
if(l_nThrottle == 5)
// fetch robot status
call fetchStatus()
// encode and send ROS simple_message ID 13: robot status
call encodeStatus()
call sendRosMsg(siTcpIpFbk, rosStatusMsg.prefix, rosStatusMsg.header, rosStatusMsg.body, nOutConnFlag)
l_nThrottle = 0
endIf
l_nThrottle = l_nThrottle + 1
else
delay(0.1)
endIf

// sequence task
delay(0)
endWhile
end]]></Code>
</Program>
</Programs>
Loading