Skip to content

Commit

Permalink
Fixed models
Browse files Browse the repository at this point in the history 
Signed-off-by: Marco Lampacrescia <[email protected]>
  • Loading branch information
@MarcoLm993
MarcoLm993 committed Nov 29, 2024
1 parent f668748 commit 4be98dc
Show file tree
Hide file tree
Showing 2 changed files with 20 additions and 14 deletions.
7 changes: 5 additions & 2 deletions ...jani_generator/_test_data/grid_robot_blackboard/bt_update_goal_and_current_position.scxml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,7 +14,8 @@
<bt_declare_port_in key="curr_topic" type="string" />

<datamodel>
<data id="is_init" expr="false" type="bool" />
<data id="is_goal_init" expr="false" type="bool" />
<data id="is_pose_init" expr="false" type="bool" />
<data id="goal_x" expr="0" type="int32" />
<data id="goal_y" expr="0" type="int32" />
<data id="curr_x" expr="0" type="int32" />
Expand All @@ -38,15 +39,17 @@
<ros_topic_callback name="goal_sub" target="running">
<assign location="goal_x" expr="_msg.x" />
<assign location="goal_y" expr="_msg.y" />
<assign location="is_goal_init" expr="true" />
</ros_topic_callback>
<!-- New robot pose -->
<ros_topic_callback name="curr_sub" target="running">
<assign location="curr_x" expr="_msg.x" />
<assign location="curr_y" expr="_msg.y" />
<assign location="is_pose_init" expr="true" />
</ros_topic_callback>
<!-- Leaf node tick -->
<bt_tick target="running">
<if cond="is_init">
<if cond="is_goal_init &amp;&amp; is_pose_init">
<bt_set_output key="goal_x" expr="goal_x" />
<bt_set_output key="goal_y" expr="goal_y" />
<bt_set_output key="curr_x" expr="curr_x" />
Expand Down
27 changes: 15 additions & 12 deletions test/jani_generator/_test_data/grid_robot_blackboard/world.scxml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,8 +9,8 @@
<data id="n" type="int32" expr="10" /> <!-- grid size -->
<data id="pose_x" type="int32" expr="0" /> <!-- TODO: Math.floor(Math.random() * n) -->
<data id="pose_y" type="int32" expr="0" /> <!-- TODO: Math.floor(Math.random() * n) -->
<data id="goal_x" type="int32" expr="4" />
<data id="goal_y" type="int32" expr="9" />
<data id="goal_x" type="int32" expr="0" />
<data id="goal_y" type="int32" expr="0" />
</datamodel>

<ros_topic_subscriber topic="move" type="std_msgs/Int16" />
Expand All @@ -25,17 +25,21 @@

<state id="init">
<onentry>
<!-- if at goal -->
<if cond="pose_x == goal_x &amp;&amp; pose_y == goal_y">
<assign location="goal_x" expr="goal_x + 1" /> <!-- TODO: Math.floor(Math.random() * n) -->
<assign location="goal_y" expr="goal_y - 1" /> <!-- TODO: Math.floor(Math.random() * n) -->
<ros_topic_publish name="goal">
<field name="x" expr="goal_x" />
<field name="y" expr="goal_y" />
</ros_topic_publish>
</if>
<assign location="goal_x" expr="4" /> <!-- TODO: Math.floor(Math.random() * n) -->
<assign location="goal_y" expr="9" /> <!-- TODO: Math.floor(Math.random() * n) -->
<ros_topic_publish name="goal">
<field name="x" expr="goal_x" />
<field name="y" expr="goal_y" />
</ros_topic_publish>
<ros_topic_publish name="pose">
<field name="x" expr="pose_x" />
<field name="y" expr="pose_y" />
</ros_topic_publish>
</onentry>
<transition target="running" />
</state>

<state id="running">
<ros_topic_callback name="move" target="init">
<if cond="_msg.data == 0 &amp;&amp; pose_x &lt; n - 1">
<!-- right -->
Expand All @@ -55,6 +59,5 @@
<field name="y" expr="pose_y" />
</ros_topic_publish>
</ros_topic_callback>

</state>
</scxml>

0 comments on commit 4be98dc

Please sign in to comment.