(roseus_bt) Fix tutorial SubTreePlus port remapping

jsk-ros-pkg · Feb 16, 2023 · bff5b60 · bff5b60
1 parent cc30486
commit bff5b60
Show file tree

Hide file tree

Showing 4 changed files with 18 additions and 13 deletions.
diff --git a/roseus_bt/sample/models/t05_subtrees.xml b/roseus_bt/sample/models/t05_subtrees.xml
@@ -6,7 +6,7 @@
             <Action ID="InitWithBroom" name="Initialize Task"/>
             <Action ID="setCoords" name="setBottleCoords" coords="${bottle_coords}"/>
             <Sequence>
-                <SubTreePlus ID="PourTask" coords="bottle_coords"/>
+                <SubTreePlus ID="PourTask" coords="${bottle_coords}"/>
                 <SubTreePlus ID="SweepTask"/>
             </Sequence>
         </SequenceStar>
@@ -30,14 +30,18 @@
                 <Condition ID="atSpot" spot="broom-front"/>
                 <Action ID="MoveTo" spot="broom-front"/>
             </Fallback>
-            <Timeout msec="5000">
-                <Action ID="SweepFloor"/>
-            </Timeout>
+            <ForceSuccess>
+                <Timeout msec="5000">
+                    <Action ID="SweepFloor"/>
+                </Timeout>
+            </ForceSuccess>
         </Sequence>
     </BehaviorTree>
     <!-- ////////// -->
     <TreeNodesModel>
-        <SubTreePlus ID="PourTask"/>
+        <SubTreePlus ID="PourTask">
+            <input_port name="coords" type="geometry_msgs/Pose"/>
+        </SubTreePlus>
         <SubTreePlus ID="SweepTask"/>
 
         <Action ID="InitWithBroom" server_name="/t05_subtrees/init"/>
@@ -60,4 +64,3 @@
     </TreeNodesModel>
     <!-- ////////// -->
 </root>
-
diff --git a/roseus_bt/sample/models/t06_reactive.xml b/roseus_bt/sample/models/t06_reactive.xml
@@ -11,7 +11,7 @@
                     <Condition ID="CheckTrue" value="${needs_drink}" name="NeedsDrink"/>
                     <SubTreePlus ID="SweepTask"/>
                 </ReactiveFallback>
-                <SubTreePlus ID="PourTask" coords="bottle_coords"/>
+                <SubTreePlus ID="PourTask" coords="${bottle_coords}"/>
             </Sequence>
         </SequenceStar>
     </BehaviorTree>
@@ -40,7 +40,7 @@
     <!-- ////////// -->
     <TreeNodesModel>
         <SubTreePlus ID="PourTask">
-            <input_port default="bottle_coords" name="coords" type=""/>
+            <input_port default="bottle_coords" name="coords" type="geometry_msgs/Pose"/>
         </SubTreePlus>
         <SubTreePlus ID="SweepTask"/>
 

diff --git a/roseus_bt/sample/models/t07_xacro.xml.xacro b/roseus_bt/sample/models/t07_xacro.xml.xacro
@@ -6,15 +6,15 @@
   <xacro:include filename="$(find roseus_bt)/sample/models/t07_xacro_sweep_task.xml.xacro"
                  ns="sweep_task"/>
 
-  <xacro:property name="namespace" value="/t07_xacro"/>
+  <xacro:property name="namespace" value="/t05_subtrees"/>
 
   <!-- ////////// -->
     <BehaviorTree ID="BehaviorTree">
         <SequenceStar name="InitSequence">
             <Action ID="InitWithBroom" name="Initialize Task"/>
             <Action ID="setCoords" name="setBottleCoords" coords="$${bottle_coords}"/>
             <Sequence>
-                <SubTreePlus ID="PourTask" coords="bottle_coords"/>
+                <SubTreePlus ID="PourTask" coords="$${bottle_coords}"/>
                 <SubTreePlus ID="SweepTask"/>
             </Sequence>
         </SequenceStar>

diff --git a/roseus_bt/sample/models/t07_xacro_sweep_task.xml.xacro b/roseus_bt/sample/models/t07_xacro_sweep_task.xml.xacro
@@ -10,9 +10,11 @@
                 <Condition ID="atSpot" spot="broom-front"/>
                 <Action ID="MoveTo" spot="broom-front"/>
             </Fallback>
-            <Timeout msec="5000">
-                <Action ID="SweepFloor"/>
-            </Timeout>
+            <ForceSuccess>
+                <Timeout msec="5000">
+                    <Action ID="SweepFloor"/>
+                </Timeout>
+            </ForceSuccess>
         </Sequence>
     </BehaviorTree>
   </xacro:macro>