BehaviorTree runtime optimizations and mirroring fix (#219)

mep3_behavior_tree/assets/strategies/big/purple_strategy.xml

@@ -2,8 +2,11 @@
     <!-- ////////// -->
     <BehaviorTree ID="BehaviorTree">
         <Sequence>
-            <WaitMatchStart state="2" />
             <DefaultTeamColor color="purple" />
+            <WaitMatchStart state="1" />
+            <!-- Robot setup (pre-match state) -->
+            <WaitMatchStart state="2" />
+            <!-- Robot strategy (match state) -->
             <Wait duration="10000000.0" name="Wait forever as big robot" />
         </Sequence>
     </BehaviorTree>

mep3_behavior_tree/assets/strategies/small/purple_strategy.xml

@@ -2,8 +2,11 @@
     <!-- ////////// -->
     <BehaviorTree ID="BehaviorTree">
         <Sequence>
-            <WaitMatchStart state="2" />
             <DefaultTeamColor color="purple" />
+            <WaitMatchStart state="1" />
+            <!-- Robot setup (pre-match state) -->
+            <WaitMatchStart state="2" />
+            <!-- Robot strategy (match state) -->
             <Wait duration="10000000.0" name="Wait forever as small robot" />
         </Sequence>
     </BehaviorTree>

mep3_behavior_tree/include/mep3_behavior_tree/bt_action_node.hpp

@@ -19,8 +19,8 @@
 #include <string>
 
 #include "behaviortree_cpp_v3/action_node.h"
-#include "mep3_behavior_tree/team_color_strategy_mirror.hpp"
 #include "rclcpp/executors/single_threaded_executor.hpp"
+#include "mep3_behavior_tree/team_color_strategy_mirror.hpp"
 #include "rclcpp_action/rclcpp_action.hpp"
 
 namespace mep3_behavior_tree
@@ -65,7 +65,23 @@ class BtActionNode : public BT::ActionNodeBase
       // Unfortunately, `name` is a reserved property, so we have to use `label`.
       action_name_ = action_name + "/" + raw_name;
     }
-    g_StrategyMirror.remap_server_name(action_name_);
+
+    std::string raw_mirror;
+    if (!getInput("mirror", raw_mirror))
+      raw_mirror = "default";
+    mirror_ = StrategyMirror::string_to_mirror_enum(raw_mirror);
+
+    original_action_name_ = action_name_;
+    if (g_StrategyMirror.server_name_requires_mirroring(action_name_, mirror_)) {
+      g_StrategyMirror.remap_server_name(action_name_);
+      RCLCPP_INFO(
+        node_->get_logger(),
+        "Remapping \"%s\" action server to \"%s\"",
+        original_action_name_.c_str(),
+        action_name_.c_str()
+      );
+    }
+
     createActionClient(action_name_);
 
     // Give the derive class a chance to do any initialization
@@ -104,7 +120,9 @@ class BtActionNode : public BT::ActionNodeBase
     BT::PortsList basic = {
       BT::InputPort<std::string>("server_name", "Action server name"),
       BT::InputPort<std::string>("label", "Action name suffix"),
-      BT::InputPort<std::chrono::milliseconds>("server_timeout")};
+      BT::InputPort<std::string>("mirror", "Action mirroring flag"),
+      BT::InputPort<std::chrono::milliseconds>("server_timeout")
+    };
     basic.insert(addition.begin(), addition.end());
 
     return basic;
@@ -383,7 +401,8 @@ class BtActionNode : public BT::ActionNodeBase
     config().blackboard->template set<int>("number_recoveries", recovery_count);  // NOLINT
   }
 
-  std::string action_name_;
+  std::string action_name_, original_action_name_;
+  MirrorParam mirror_;
   typename std::shared_ptr<rclcpp_action::Client<ActionT>> action_client_;
 
   // All ROS2 actions have a goal and a result

mep3_behavior_tree/include/mep3_behavior_tree/dynamixel_command_action.hpp

@@ -36,6 +36,27 @@ class DynamixelCommandAction
   : mep3_behavior_tree::BtActionNode<mep3_msgs::action::DynamixelCommand>(
       xml_tag_name, "dynamixel_command", config)
   {
+    if (!getInput("position", this->position))
+      throw BT::RuntimeError(
+        "Dynamixel action requires 'position' argument"
+      ); 
+    if (!getInput("velocity", this->velocity))
+      this->velocity = 220;
+    if (!getInput("tolerance", this->tolerance))
+      this->tolerance = 9;
+    if (!getInput("timeout", this->timeout))
+      this->timeout = 5;
+
+    std::string table = this->config().blackboard->get<std::string>("table");
+    _Float64 position_offset;
+    if (table.length() > 0 && getInput("position_" + table, position_offset)) {
+      position += position_offset;
+    }
+
+    if (g_StrategyMirror.angle_requires_mirroring(original_action_name_, mirror_)) {
+      g_StrategyMirror.invert_angle(position);
+    }
+
   }
 
   void on_tick() override;
@@ -51,7 +72,6 @@ class DynamixelCommandAction
       BT::InputPort<_Float64>("velocity"),
       BT::InputPort<_Float64>("tolerance"),
       BT::InputPort<_Float64>("timeout"),
-      BT::InputPort<std::string>("mirror"),
       BT::OutputPort<int8_t>("result")
     });
 
@@ -64,39 +84,14 @@ class DynamixelCommandAction
 
     return port_list;
   }
+
+private:
+  _Float64 position, velocity, tolerance, timeout;
 };
 
 void DynamixelCommandAction::on_tick()
 {
-  _Float64 position, velocity, tolerance, timeout;
-
-  getInput("position", position);
-  if (!getInput("velocity", velocity))
-    velocity = 220;
-  if (!getInput("tolerance", tolerance))
-    tolerance = 9;
-  if (!getInput("timeout", timeout))
-    timeout = 5;
-
-  std::string table = config().blackboard->get<std::string>("table");
-  _Float64 position_offset;
-  if (table.length() > 0 && getInput("position_" + table, position_offset)) {
-    position += position_offset;
-    std::cout << "Position offset for '" << action_name_ \
-              << "' on table '" << table << "' detected" << std::endl;
-  }
-
-  std::string mirror;
-  getInput("mirror", mirror);
-
-  if (g_StrategyMirror.server_name_requires_mirroring(action_name_, mirror)) {
-    g_StrategyMirror.remap_server_name(action_name_);
-  }
-
-  if (g_StrategyMirror.angle_requires_mirroring(action_name_, mirror)) {
-    g_StrategyMirror.invert_angle(position);
-  }
-
+  std::cout << "Set '" << this->action_name_ << "' to " << this->position << "°" <<std::endl;
   goal_.position = position;
   goal_.velocity = velocity;
   goal_.tolerance = tolerance;

mep3_behavior_tree/include/mep3_behavior_tree/motion_command_action.hpp

@@ -36,6 +36,40 @@ class MotionCommandAction
   : mep3_behavior_tree::BtActionNode<mep3_msgs::action::MotionCommand>(
       xml_tag_name, "motion_command", config)
   {
+    if (!getInput("command", this->command))
+      throw BT::RuntimeError(
+        "Motion action requires 'command' argument"
+      );
+    if (!getInput("value", this->value))
+      throw BT::RuntimeError(
+        "Motion action requires 'value' argument"
+      );
+    if (!getInput("velocity_linear", velocity_linear))
+      velocity_linear = 0;
+    if (!getInput("acceleration_linear", acceleration_linear))
+      acceleration_linear = 0;
+    if (!getInput("velocity_angular", velocity_angular))
+      velocity_angular = 0;
+    if (!getInput("acceleration_angular", acceleration_angular))
+      acceleration_angular = 0;
+
+    std::string table = this->config().blackboard->get<std::string>("table");
+    _Float64 value_offset;
+    if (table.length() > 0 && getInput("value_" + table, value_offset)) {
+      value += value_offset;
+    }
+
+    if (
+      g_StrategyMirror.requires_mirroring(mirror_) && \
+      command == "rotate_relative"
+    ) {
+      g_StrategyMirror.invert_angle(value);
+    } else if (
+      g_StrategyMirror.requires_mirroring(mirror_) && \
+      command == "rotate_absolute"
+    ) {
+      g_StrategyMirror.mirror_angle(value);
+    }
   }
 
   void on_tick() override;
@@ -63,40 +97,18 @@ class MotionCommandAction
 
     return port_list;
   }
-};
 
-void MotionCommandAction::on_tick()
-{
+private:
   std::string command;
   _Float64 value, velocity_linear, acceleration_linear, 
                   velocity_angular, acceleration_angular;
+};
 
-  getInput("command", command);
-  getInput("value", value);
-
-  if (!getInput("velocity_linear", velocity_linear))
-    velocity_linear = 0;
-  if (!getInput("acceleration_linear", acceleration_linear))
-    acceleration_linear = 0;
-  if (!getInput("velocity_angular", velocity_angular))
-    velocity_angular = 0;
-  if (!getInput("acceleration_angular", acceleration_angular))
-    acceleration_angular = 0;
-
-  std::string table = config().blackboard->get<std::string>("table");
-  _Float64 value_offset;
-  if (table.length() > 0 && getInput("value_" + table, value_offset)) {
-    value += value_offset;
-    std::cout << "Motion value offset for table '" \
-              << table << "' detected" << std::endl;
-  }
-
-  if (command == "rotate_relative") {
-    g_StrategyMirror.invert_angle(value);
-  } else if (command == "rotate_absolute") {
-    g_StrategyMirror.mirror_angle(value);
-  }
-
+void MotionCommandAction::on_tick()
+{
+  std::cout << "Motion " << this->command << " for " \
+            << this->value << ((this->command == "forward") ? "m" : "°") \
+            << std::endl;
   goal_.command = command;
   goal_.value = value;
   goal_.velocity_linear = velocity_linear;

mep3_behavior_tree/include/mep3_behavior_tree/navigate_through_action.hpp

@@ -48,7 +48,6 @@ namespace mep3_behavior_tree
       // Static parameters
       BT::PortsList port_list = providedBasicPorts({
         BT::InputPort<std::vector<BT::Pose2D>>("goal"),
-        BT::InputPort<std::string>("mirror"),
         BT::InputPort<std::string>("behavior_tree")
       });
 
@@ -78,9 +77,7 @@ namespace mep3_behavior_tree
                 << table << "' detected" << std::endl;
     }
 
-    std::string mirror;
-    getInput("mirror", mirror);
-    bool requires_mirroring = g_StrategyMirror.requires_mirroring(mirror);
+    bool requires_mirroring = g_StrategyMirror.requires_mirroring(mirror_);
 
     goal_.behavior_tree = behavior_tree;
     for (auto &pose : poses)

mep3_behavior_tree/include/mep3_behavior_tree/navigate_to_action.hpp

@@ -37,6 +37,21 @@ namespace mep3_behavior_tree
         : mep3_behavior_tree::BtActionNode<nav2_msgs::action::NavigateToPose>(
               xml_tag_name, "navigate_to_pose", config)
     {
+      if (!getInput("goal", this->goal))
+        throw BT::RuntimeError(
+          "Navigate action requires 'goal' argument"
+        );
+      getInput("behavior_tree", this->behavior_tree);
+
+      std::string table = this->config().blackboard->get<std::string>("table");
+      BT::Pose2D goal_offset;
+      if (table.length() > 0 && getInput("goal_" + table, goal_offset)) {
+        goal += goal_offset;
+      }
+
+      if (g_StrategyMirror.requires_mirroring(mirror_)) {
+        g_StrategyMirror.mirror_pose(goal);
+      }
     }
 
     void on_tick() override;
@@ -47,7 +62,6 @@ namespace mep3_behavior_tree
       // Static parameters
       BT::PortsList port_list = providedBasicPorts({
         BT::InputPort<BT::Pose2D>("goal"),
-        BT::InputPort<std::string>("mirror"),
         BT::InputPort<std::string>("behavior_tree")
       });
 
@@ -60,30 +74,14 @@ namespace mep3_behavior_tree
 
       return port_list;
     }
-  };
 
-  void NavigateToAction::on_tick()
-  {
+  private:
     BT::Pose2D goal;
     std::string behavior_tree;
-    getInput("goal", goal);
-    getInput("behavior_tree", behavior_tree);
-
-    std::string table = config().blackboard->get<std::string>("table");
-    BT::Pose2D goal_offset;
-    if (table.length() > 0 && getInput("goal_" + table, goal_offset)) {
-      std::cout << "Navigation goal offset for table '" \
-                << table << "' detected" << std::endl;
-      goal += goal_offset;
-    }
-
-    std::string mirror;
-    getInput("mirror", mirror);
-
-    if (g_StrategyMirror.requires_mirroring(mirror)) {
-      g_StrategyMirror.mirror_pose(goal);
-    }
+  };
 
+  void NavigateToAction::on_tick()
+  {
     std::cout << "Navigating to x=" << goal.x \
               << " y=" << goal.y \
               << " θ=" << goal.theta << "°" << std::endl;
@@ -98,10 +96,10 @@ namespace mep3_behavior_tree
 
     // Orientation (yaw)
     // Convert deg to rad
-    goal.theta = goal.theta * M_PI / 180.0;
+    double theta = goal.theta * M_PI / 180.0;
     // https://math.stackexchange.com/a/1972382
-    goal_.pose.pose.orientation.w = std::cos(goal.theta / 2.0);
-    goal_.pose.pose.orientation.z = std::sin(goal.theta / 2.0);
+    goal_.pose.pose.orientation.w = std::cos(theta / 2.0);
+    goal_.pose.pose.orientation.z = std::sin(theta / 2.0);
   }
 
   BT::NodeStatus NavigateToAction::on_success()