Commit c8f06408 authored by Erik Jansson's avatar Erik Jansson
Browse files

Update code to use ROS services.

parent f02a36e3
......@@ -14,3 +14,4 @@ srv_gen/
src/signals.py
src/statemachine.py
*.pyc
.*.swp
<?xml version="1.0" encoding="UTF-8" standalone="yes"?>
<GCDocument color="-1" dimTicks="25" dpwsInterface="" dpwsPort="-1" height="600" horizontalScrollBar="1" modifiable="1" name="J2" saveVersion="7" scale="1.0" simulationMode="1" socketHost="" socketIsServer="0" socketPort="-1" socketSendMode="Changed" threadSpeed="40" tokenLuminance="0" verticalScrollBar="1" viewPositionX="0" viewPositionY="0" width="863" x="0" y="0">
<GCDocument color="-1" dimTicks="25" dpwsInterface="" dpwsPort="-1" height="625" horizontalScrollBar="1" modifiable="1" name="J2" saveVersion="7" scale="1.0" simulationMode="1" socketHost="" socketIsServer="0" socketPort="-1" socketSendMode="Changed" threadSpeed="40" tokenLuminance="0" verticalScrollBar="1" viewPositionX="0" viewPositionY="0" width="1070" x="0" y="0">
<GCInitialStep actionBlockVisible="0" actionText=";" fileName="" height="70" id="8e87c019-c4a8-4f9a-a1f0-593a1d891361" name="" useIcon="0" width="60" x="320" y="35"/>
<GCStep actionBlockVisible="1" actionText=";" fileName="" height="70" id="3934db7f-5f1e-458d-98b2-2cc8fa65f120" name="" useIcon="0" width="200" x="320" y="285"/>
<WorkspaceObject enabled="1" fileName="" height="60" horizontalScrollBar="1" name="i" ownerClass="" scanCycle="1" useIcon="0" verticalScrollBar="1" width="60" x="100" y="140">
......@@ -1853,18 +1853,18 @@
</GCLink>
</GCDocument>
</MacroStep>
<GCTransition CGURI="" actionText="LC_ROS.S__ec_start.aua_id == 1" conditionVisible="1" height="25" id="f15b2300-592b-4c0c-acf5-b4667ee051ba" width="30" x="65" y="400"/>
<GCTransition CGURI="" actionText="LC_ROS.S__ec_start.aua_id == 2" conditionVisible="1" height="25" id="3dba4889-d9d2-450a-832a-c96c5a5868a1" width="30" x="375" y="430"/>
<GCTransition CGURI="" actionText="LC_ROS.S__ec_start.aua_id == 3" conditionVisible="1" height="25" id="d0dd24f6-c3e0-460d-8708-67003372c738" width="30" x="645" y="380"/>
<LabCommObject height="60" isSocketServer="1" name="LC_ROS" socketHost="" socketPort="8888" specification="sample struct{&#10; int aua_id;&#10;} S__ec_start;&#10;&#10;sample struct{&#10; int status;&#10;} S__ec_status;" width="60" x="130" y="30"/>
<GCStep actionBlockVisible="1" actionText="S o.startExtCtrl1 = 1;&#10;S LC_ROS.S__ec_status.status = 1;&#10;X lcSend(LC_ROS, &quot;S__ec_status&quot;);" fileName="" height="70" id="423b1328-c71a-454b-be83-5c9c7c1ddaeb" name="" useIcon="0" width="200" x="320" y="165"/>
<GCTransition CGURI="" actionText="LC_ROS.S__ec_start.aua_id &gt; 0" conditionVisible="1" height="25" id="45862eed-29e2-4817-bd62-1e55a052e414" width="30" x="335" y="130"/>
<GCTransition CGURI="" actionText="LC_ROS.ExtCtrl_PAR.aua_id == 1" conditionVisible="1" height="25" id="f15b2300-592b-4c0c-acf5-b4667ee051ba" width="30" x="65" y="400"/>
<GCTransition CGURI="" actionText="LC_ROS.ExtCtrl_PAR.aua_id == 2" conditionVisible="1" height="25" id="3dba4889-d9d2-450a-832a-c96c5a5868a1" width="30" x="375" y="430"/>
<GCTransition CGURI="" actionText="LC_ROS.ExtCtrl_PAR.aua_id == 3" conditionVisible="1" height="25" id="d0dd24f6-c3e0-460d-8708-67003372c738" width="30" x="645" y="380"/>
<LabCommObject height="60" isSocketServer="1" name="LC_ROS" socketHost="" socketPort="8888" specification="sample struct {&#10; long aua_id;&#10;} ExtCtrl_PAR;&#10;&#10;sample struct {&#10; long status;&#10;} ExtCtrl_RET;&#10;" width="60" x="130" y="30"/>
<GCStep actionBlockVisible="1" actionText="S o.startExtCtrl1 = 1;&#10;S LC_ROS.ExtCtrl_RET.status = 1;&#10;S lcSend(LC_ROS, &quot;ExtCtrl_RET&quot;);" fileName="" height="70" id="423b1328-c71a-454b-be83-5c9c7c1ddaeb" name="" useIcon="0" width="200" x="320" y="165"/>
<GCTransition CGURI="" actionText="LC_ROS.ExtCtrl_PAR.aua_id &gt; 0" conditionVisible="1" height="25" id="45862eed-29e2-4817-bd62-1e55a052e414" width="30" x="335" y="130"/>
<GCTransition CGURI="" actionText="i.runExtctrl1 &gt; 0.5" conditionVisible="1" height="25" id="4f140457-0a2c-4540-8b32-bba8e46e1fc3" width="30" x="335" y="260"/>
<GCStep actionBlockVisible="1" actionText="S o.startExtCtrl1 = 0;&#10;S LC_ROS.S__ec_start.aua_id = 0;&#10;S LC_ROS.S__ec_status.status = 2;&#10;S lcSend(LC_ROS, &quot;S__ec_status&quot;);" fileName="" height="70" id="dfa412ae-f2e3-4e57-aea9-dad69de460ec" name="" useIcon="0" width="200" x="800" y="705"/>
<GCStep actionBlockVisible="1" actionText="S o.startExtCtrl1 = 0;&#10;S LC_ROS.ExtCtrl_PAR.aua_id = 0;&#10;S LC_ROS.ExtCtrl_RET.status = 2;&#10;S lcSend(LC_ROS, &quot;ExtCtrl_RET&quot;);" fileName="" height="70" id="dfa412ae-f2e3-4e57-aea9-dad69de460ec" name="" useIcon="0" width="200" x="800" y="705"/>
<GCTransition CGURI="" actionText="1" conditionVisible="1" height="25" id="faf87ef2-9065-4bff-93a5-499ccecd815d" width="30" x="115" y="590"/>
<GCTransition CGURI="" actionText="1" conditionVisible="1" height="25" id="2c375e9a-fb80-46c0-9033-54463ad96a83" width="30" x="445" y="580"/>
<GCTransition CGURI="" actionText="1" conditionVisible="1" height="25" id="4c9319c0-a030-4ca9-a99b-573aa63d7f2a" width="30" x="705" y="560"/>
<GCTransition CGURI="" actionText="LC_ROS.S__ec_start.aua_id &gt; 0" conditionVisible="1" height="25" id="dcfeaccb-b22c-4ce9-9660-a12ac56f7645" width="30" x="965" y="720"/>
<GCTransition CGURI="" actionText="LC_ROS.ExtCtrl_PAR.aua_id &gt; 0" conditionVisible="1" height="25" id="dcfeaccb-b22c-4ce9-9660-a12ac56f7645" width="30" x="965" y="720"/>
<GCLink fromObject="3934db7f-5f1e-458d-98b2-2cc8fa65f120" manAdj="0" toObject="3dba4889-d9d2-450a-832a-c96c5a5868a1">
<Stroke x="350" y="356"/>
<Stroke x="350" y="366"/>
......
<bridge name="jgc_bridge" port="7357">
<imports>
<topic name="/ec_status" />
<service name="/extctrl_service" file="srv/ExtCtrl.srv" />
</imports>
<exports>
<topic name="/ec_start" />
</exports>
<static-connections>
<target addr="127.0.0.1:8888">
<subscribe name="/ec_start" />
<publish name="/ec_status" />
</target>
</static-connections>
......
......@@ -3,16 +3,16 @@
import roslib; roslib.load_manifest('ros_extctrl')
import rospy
from ros_extctrl.srv import ExtCtrlStart
from geometry_msgs.msg import PoseStamped
from ros_extctrl.srv import ExtCtrl
def interact():
while not rospy.is_shutdown():
rospy.wait_for_service('ExtCtrlStart')
start_controller = rospy.ServiceProxy('ExtCtrlStart', ExtCtrlStart)
rospy.wait_for_service('extctrl_service')
start_controller = rospy.ServiceProxy('extctrl_service', ExtCtrl)
line = ''
try:
line = raw_input("Choose a controller to activate:")
line = raw_input("Choose a controller to activate: ")
except EOFError:
print('\nBye')
break
......@@ -22,17 +22,9 @@ def interact():
except ValueError:
print("%s is not a number" % line)
continue
pose = PoseStamped()
pose.pose.position.x = 0.0
pose.pose.position.y = 0.0
pose.pose.position.z = 0.0
pose.pose.orientation.x = 0.0
pose.pose.orientation.y = 0.0
pose.pose.orientation.z = 0.0
pose.pose.orientation.w = 0.0
resp = None
try:
resp = start_controller(aua_id, pose)
resp = start_controller(aua_id)
except rospy.ServiceException as exc:
print("Service did not process request: " + str(exc))
continue
......
uint32 aua_id # Controller ID (0 to deactivate)
geometry_msgs/PoseStamped pose
---
uint32 status
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment