Implement BT-based nav controller for demo

Signed-off-by: Marco Lampacrescia <[email protected]>

jani_generator/test/_test_data/robot_navigation_with_bt/bt.xml

@@ -0,0 +1,13 @@
+<root BTCPP_format="4">
+    <BehaviorTree>
+        <!-- If the robot goal is in front (goal.x > goal.y), go forward (direction=0). -->
+        <!-- Otherwise, rotate in place (direction=1). -->
+        <Fallback>
+            <Sequence>
+                <Action ID="GoalCheck" name="goal_in_front" topic="/goal_position" />
+                <Action ID="DriveRobot" name="drive_fwd" direction="0" topic="/cmd_vel" />
+            </Sequence>
+            <Action ID="DriveRobot" name="drive_rotate" direction="1" topic="/cmd_vel" />
+        </Fallback>
+    </BehaviorTree>
+</root>

jani_generator/test/_test_data/robot_navigation_with_bt/bt_drive_robot.scxml

@@ -0,0 +1,57 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<scxml 
+    initial="init" 
+    version="1.0" 
+    name="DriveRobot"
+    model_src=""
+    xmlns="http://www.w3.org/2005/07/scxml">
+
+    <bt_declare_port_in key="topic" type="string" />
+    <bt_declare_port_in key="direction" type="int8" />
+
+    <ros_topic_publisher name="cmd_pub" type="geometry_msgs/Twist">
+        <topic>
+            <bt_get_input key="topic" />
+        </topic>
+    </ros_topic_subscriber>
+
+    <datamodel>
+        <data id="control_type" type="int8">
+            <expr>
+                <!-- 0 = drive_fwd, 1 = rotate_ccw -->
+                <bt_get_input key="direction" />
+            </expr>
+        </data>
+        <data id="x_vel" expr="0" type="float64" />
+        <data id="theta_vel" expr="0" type="float64" />
+    </datamodel>
+
+    <state id="init">
+        <transition target="wait_for_tick">
+            <if cond="control_type == 0">
+                <assign location="x_vel" expr="0.5" />
+                <assign location="theta_vel" expr="0.0" />
+            <else />
+                <assign location="x_vel" expr="0.0" />
+                <assign location="theta_vel" expr="0.5" />
+            </if>
+        </transition>
+    </state>
+
+    <state id="wait_for_tick">
+        <transition event="bt_tick" target="wait_for_tick">
+            <ros_topic_publish name="cmd_vel">
+                <!-- 2D robots require only x and theta controls -->
+                <field name="linear.x" expr="x_vel" />
+                <field name="angular.z" expr="theta_vel" />
+                <!-- Additional fields are set to 0 -->
+                <field name="linear.y" expr="0.0" />
+                <field name="linear.z" expr="0.0" />
+                <field name="angular.x" expr="0.0" />
+                <field name="angular.y" expr="0.0" />
+            </ros_topic_publish>
+            <send event="bt_success" />
+        </transition>
+    </state>
+
+</scxml>

jani_generator/test/_test_data/robot_navigation_with_bt/bt_goal_check.scxml

@@ -0,0 +1,49 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<scxml 
+    initial="wait_first_msg" 
+    version="1.0" 
+    name="GoalCheck"
+    model_src=""
+    xmlns="http://www.w3.org/2005/07/scxml">
+
+    <bt_declare_port_in key="topic" type="string" />
+
+    <ros_topic_subscriber name="goal_sub" type="geometry_msgs/Point">
+        <topic>
+            <bt_get_input key="topic" />
+        </topic>
+    </ros_topic_subscriber>
+
+    <datamodel>
+        <data id="goal_x" expr="0" type="float64" />
+        <data id="goal_y" expr="0" type="float64" />
+    </datamodel>
+
+    <!-- Assumption: We get an event when the node is ticked by the BT, named "bt_tick". -->
+    <!-- Assumption: We have to send an event to the BT, that is either "bt_success" or "bt_failure". -->
+
+    <state id="wait_first_msg">
+        <transition event="bt_tick" target="wait_first_msg">
+            <send event="bt_running" />
+        </transition>
+        <ros_topic_callback name="goal_sub" target="ready_to_check">
+            <assign location="goal_x" expr="_msg.x" />
+            <assign location="goal_y" expr="_msg.y" />
+        </ros_topic_callback>
+    </state>
+
+    <state id="ready_to_check">
+        <transition event="bt_tick" target="ready_to_check">
+            <if cond="goal_x > goal_y">
+                <send event="bt_success" />
+            <else />
+                <send event="bt_failure" />
+            </if>
+        </transition>
+        <ros_topic_callback name="goal_sub" target="ready_to_check">
+            <assign location="goal_x" expr="_msg.x" />
+            <assign location="goal_y" expr="_msg.y" />
+        </ros_topic_callback>
+    </state>
+
+</scxml>

jani_generator/test/_test_data/robot_navigation_with_bt/main.xml

@@ -0,0 +1,19 @@
+<convince_mc_tc>
+    <mc_parameters>
+        <max_time value="100" unit="s" />
+    </mc_parameters>
+
+    <behavior_tree>
+        <input type="bt.cpp-xml" src="./bt.xml" />
+        <input type="bt-plugin-ros-scxml" src="./bt_goal_check.scxml" />
+        <input type="bt-plugin-ros-scxml" src="./bt_drive_robot.scxml" /> 
+    </behavior_tree>
+
+    <node_models>
+        <input type="ros-scxml" src="./robot_platform.scxml" />
+    </node_models>
+
+    <properties>
+        <input type="jani" src="./properties.jani" />
+    </properties>
+</convince_mc_tc>

jani_generator/test/_test_data/robot_navigation_with_bt/properties.jani

@@ -0,0 +1,67 @@
+{
+    "properties": [
+        {
+            "name": "goal_reached",
+            "expression": {
+              "op": "filter",
+              "fun": "values",
+              "values": {
+                "op": "Pmin",
+                "exp": {
+                  "op": "U",
+                  "left": {
+                    "op": "∧",
+                    "left": {
+                      "op": "<",
+                      "left": {
+                        "op": "abs",
+                        "exp": "topic_robot_pose_msg.ros_fields__x"
+                        },
+                      "right": 10.0
+                      },
+                    "right": {
+                      "op": "<",
+                      "left": {
+                        "op": "abs",
+                        "exp": "topic_robot_pose_msg.ros_fields__y"
+                        },
+                      "right": 10.0
+                    }
+                  },
+                  "right": {
+                    "comment": "Goal reached",
+                    "op": "∧",
+                    "left": {
+                      "op": "<",
+                      "left": {
+                        "op": "abs",
+                        "exp": {
+                          "op": "-",
+                          "left": "topic_robot_pose_msg.ros_fields__x",
+                          "right": 5.0
+                        }
+                      },
+                      "right": 0.5
+                    },
+                    "right": {
+                      "op": "<",
+                      "left": {
+                        "op": "abs",
+                        "exp": {
+                          "op": "-",
+                          "left": "topic_robot_pose_msg.ros_fields__y",
+                          "right": -2.0
+                        }
+                      },
+                      "right": 0.5
+                  }
+                }
+              }
+            },
+            "states": {
+              "op": "initial"
+            }
+          }
+        }
+    ]
+}

jani_generator/test/_test_data/robot_navigation_with_bt/robot_platform.scxml

@@ -0,0 +1,70 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<scxml 
+    initial="publish_info" 
+    version="1.0" 
+    name="RobotPlatform"
+    model_src=""
+    xmlns="http://www.w3.org/2005/07/scxml">
+
+    <!-- TODO: Storing real numbers in STORM does not work. For now we have to use MODES -->
+    <datamodel>
+        <!-- The goal coordinates in the world frame -->
+        <data id="goal_world_x" expr="5.0" type="float32" />
+        <data id="goal_world_y" expr="-2.0" type="float32" />
+        <!-- The robot pose in the world frame -->
+        <data id="tf_world_robot_x" expr="0.0" type="float32" />
+        <data id="tf_world_robot_y" expr="0.0" type="float32" />
+        <data id="tf_world_robot_theta" expr="0.0" type="float32" />
+        <!-- The last command velocities received -->
+        <data id="last_cmd_lin" expr="0.0" type="float32" />
+        <data id="last_cmd_ang" expr="0.0" type="float32" />
+        <!-- Additional support variables -->
+        <data id="tf_robot_world_x" expr="0.0" type="float32" />
+        <data id="tf_robot_world_y" expr="0.0" type="float32" />
+        <data id="tf_robot_world_theta" expr="0.0" type="float32" />
+    </datamodel>
+
+    <ros_topic_publisher name="pose" topic="/robot_pose" type="geometry_msgs/Pose2D" />
+    <ros_topic_subscriber name="cmd_vel" topic="/cmd_vel" type="geometry_msgs/Twist" />
+    <ros_topic_publisher name="goal" topic="/goal_position" type="geometry_msgs/Point" />
+    <ros_time_rate name="sample_timer" rate_hz="0.5" />
+
+    <state id="publish_info">
+        <onentry>
+            <!-- Update the tf_robot_world variables -->
+            <!-- (tf_rob_w_x,  tf_rob_w_y)^T = R(tf_rob_w_theta) * (-(tf_w_rob_x, tf_w_rob_y)^T) -->
+            <assign location="tf_robot_world_theta" expr="-tf_world_robot_theta" />
+            <assign location="tf_robot_world_x"
+                    expr="(-tf_world_robot_x * cos(tf_robot_world_theta)) + (tf_world_robot_y * sin(tf_robot_world_theta))" />
+            <assign location="tf_robot_world_y"
+                    expr="(-tf_world_robot_x * sin(tf_robot_world_theta)) + (-tf_world_robot_y * cos(tf_robot_world_theta))" />
+            <!-- Publish the goal pose in the robot frame -->
+            <ros_topic_publish name="goal">
+                <field name="x" expr="goal_world_x * cos(tf_robot_world_theta) - goal_world_y * sin(tf_robot_world_theta) + tf_robot_world_x" />
+                <field name="y" expr="goal_world_x * sin(tf_robot_world_theta) + goal_world_y * cos(tf_robot_world_theta) + tf_robot_world_y" />
+                <field name="z" expr="0.0" />
+            </ros_topic_publish>
+            <!-- Publish the current robot pose -->
+            <ros_topic_publish name="pose">
+                <field name="x" expr="tf_world_robot_x" />
+                <field name="y" expr="tf_world_robot_y" />
+                <field name="theta" expr="tf_world_robot_theta" />
+            </ros_topic_publish>
+        </onentry>
+        <transition target="current" />
+    </state>
+
+
+    <state id="current">
+        <ros_topic_callback name="cmd_vel" target="current">
+            <!-- We assume the robot has a maximum lin. / ang. velocity of +-0.5  -->
+            <assign location="last_cmd_lin" expr="max(min(_msg.linear.x, 0.5), -0.5)" />
+            <assign location="last_cmd_ang" expr="max(min(_msg.angular.z, 0.5), -0.5)" />
+        </ros_topic_callback>
+        <ros_rate_callback name="sample_timer" target="publish_pose">
+            <assign location="tf_world_robot_x" expr="tf_world_robot_x + 0.5 * last_cmd_lin * cos(tf_world_robot_theta)" />
+            <assign location="tf_world_robot_y" expr="tf_world_robot_y + 0.5 * last_cmd_lin * sin(tf_world_robot_theta)" />
+            <assign location="tf_world_robot_theta" expr="tf_world_robot_theta + 0.5 * last_cmd_ang" />
+        </ros_rate_callback>
+    </state>
+</scxml>