Update code to use ROS services.

c8f06408 · Erik Jansson · f02a36e3 · c8f06408 · c8f06408 · c8f06408
Commit c8f06408 authored May 26, 2014 by Erik Jansson
--- a/.gitignore
+++ b/.gitignore
@@ -14,3 +14,4 @@ srv_gen/
 src/signals.py
 src/statemachine.py
 *.pyc
+.*.swp
--- a/automatica_statemachine.xml
+++ b/automatica_statemachine.xml
 <?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"/>

--- a/jgc_bridge_conf.xml
+++ b/jgc_bridge_conf.xml
 <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>


--- a/src/example.py
+++ b/src/example.py
@@ -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

--- a/srv/ExtCtrlStart.srv
+++ b/srv/ExtCtrlStart.srv
 uint32 aua_id                   # Controller ID (0 to deactivate)
-geometry_msgs/PoseStamped pose
 ---
 uint32 status