(roseus_bt) Update t08_multimaster

jsk-ros-pkg · Apr 26, 2022 · 9b0b65d · 9b0b65d
1 parent 29ae622
commit 9b0b65d
Show file tree

Hide file tree

Showing 3 changed files with 45 additions and 14 deletions.
diff --git a/roseus_bt/include/roseus_bt/tutorial_parser.h b/roseus_bt/include/roseus_bt/tutorial_parser.h
@@ -39,7 +39,8 @@ std::string TutorialParser::format_node_body(const XMLElement* node, int padding
   // Conditions
   if (id == "CheckTrue") return std::string("(send value :data)").insert(0, padding, ' ');
   if (id == "atTable")   return std::string("(at-spot \"table-front\")").insert(0, padding, ' ');
-  if (id == "atSpot")    return fmt::format("{}(at-spot {})", pad, param_list.at(0));
+  if (id == "atSpot" || id == "atTableSpot" || id == "atBroomSpot")
+    return fmt::format("{}(at-spot {})", pad, param_list.at(0));
   if (id == "CheckCoords") return fmt::format(
     "{}(not (equal (instance geometry_msgs::Pose :init) {}))", pad, param_list.at(0));
 

diff --git a/roseus_bt/sample/README.md b/roseus_bt/sample/README.md
@@ -234,8 +234,7 @@ The `{var}` notation also works.
 This example shows how to use the rosbridge interface to assign different hosts to each action in a multimaster application.
 https://github.com/Affonso-Gui/jsk_roseus/blob/roseus_bt/roseus_bt/sample/models/t08_multimaster.xml
 
-To do this we declare the actions with the `<RemoteAction/>` tag in the `<TreeNodesModel/>`, and add a `host_name` and `host_port` field to it. As a normal action, it also requires to have the `server_name` field set.
-The `host_name` and `host_port` can be set either statically (in the `TreeNodesModel` definition) or dynamically (within the `BehaviorTree` node instance), although we recommend to set it statically to avoid possible runtime errors.
+To do this we declare the actions with the `<RemoteAction/>` and conditions with the `<RemoteCondition/>` tag in the `<TreeNodesModel/>`, and add a `host_name` and `host_port` field to them.
 
 #### Run the code
 
@@ -247,7 +246,7 @@ roslaunch rosbridge_server rosbridge_websocket.launch
 Run the first roseus server:
 ```bash
 roscd roseus_bt_tutorials/euslisp
-roseus t08_multimaster-action-server.l
+roseus t08_multimaster_localhost9090-action-server.l
 ```
 
 Run the second rosbridge_server:
@@ -260,7 +259,7 @@ Run the second roseus server:
 ```bash
 export ROS_MASTER_URI=http://localhost:11312
 roscd roseus_bt_tutorials/euslisp
-roseus t08_multimaster-action-server.l
+roseus t08_multimaster_localhost9091-action-server.l
 ```
 
 Run the cpp client

diff --git a/roseus_bt/sample/models/t08_multimaster.xml b/roseus_bt/sample/models/t08_multimaster.xml
@@ -5,25 +5,56 @@
         <Parallel failure_threshold="1" success_threshold="2">
             <Sequence name="Robot1">
                 <Action ID="Init"/>
-                <Action ID="MoveToTable"/>
+                <Fallback>
+                    <Condition ID="atTableSpot" spot="table-front"/>
+                    <Action ID="MoveToTable"/>
+                </Fallback>
+                <Action ID="PickBottle"/>
+                <Action ID="PourBottle"/>
+                <Action ID="PlaceBottle"/>
             </Sequence>
             <Sequence name="Robot2">
-                <Action ID="InitWithBroom"
-                        host_name="localhost" host_port="9091"/>
-                <Action ID="MoveToBroom"
-                        host_name="localhost" host_port="9091"/>
+                <Action ID="InitWithBroom"/>
+                <Fallback>
+                    <Condition ID="atBroomSpot" spot="broom-front"/>
+                    <Action ID="MoveToBroom"/>
+                </Fallback>
+                <Timeout msec="5000">
+                    <Action ID="SweepFloor"/>
+                </Timeout>
             </Sequence>
         </Parallel>
     </BehaviorTree>
     <!-- ////////// -->
     <TreeNodesModel>
-        <RemoteAction ID="Init" server_name="/robot1/init"
+        <!-- Robot1 -->
+        <RemoteAction ID="Init" server_name="/t08_multimaster/robot1/init"
                       host_name="localhost" host_port="9090"/>
-        <RemoteAction ID="MoveToTable" server_name="/robot1/move"
+        <RemoteAction ID="MoveToTable" server_name="/t08_multimaster/robot1/move"
                       host_name="localhost" host_port="9090"/>
+        <RemoteAction ID="PickBottle" server_name="/t08_multimaster/robot1/pick"
+                      host_name="localhost" host_port="9090"/>
+        <RemoteAction ID="PourBottle" server_name="/t08_multimaster/robot1/pour"
+                      host_name="localhost" host_port="9090"/>
+        <RemoteAction ID="PlaceBottle" server_name="/t08_multimaster/robot1/place"
+                      host_name="localhost" host_port="9090"/>
+        <RemoteCondition ID="atTableSpot" service_name="/t08_multimaster/robot1/atspot"
+                         host_name="localhost" host_port="9090">
+          <input_port name="spot" type="string"/>
+        </RemoteCondition>
+
+        <!-- Robot2 -->
+        <RemoteAction ID="InitWithBroom" server_name="/t08_multimaster/robot2/init"
+                      host_name="localhost" host_port="9091"/>
+        <RemoteAction ID="MoveToBroom" server_name="/t08_multimaster/robot2/move"
+                      host_name="localhost" host_port="9091"/>
+        <RemoteAction ID="SweepFloor" server_name="/t08_multimaster/robot2/sweep"
+                      host_name="localhost" host_port="9091"/>
+        <RemoteCondition ID="atBroomSpot" service_name="/t08_multimaster/robot2/atspot"
+                         host_name="localhost" host_port="9091">
+          <input_port name="spot" type="string"/>
+        </RemoteCondition>
 
-        <RemoteAction ID="InitWithBroom" server_name="/robot2/init"/>
-        <RemoteAction ID="MoveToBroom" server_name="/robot2/move"/>
     </TreeNodesModel>
     <!-- ////////// -->
 </root>