Strategy mirroring improvements (#215)

mep3_behavior_tree/include/mep3_behavior_tree/dynamixel_command_action.hpp

@@ -51,6 +51,7 @@ class DynamixelCommandAction
       BT::InputPort<_Float64>("velocity"),
       BT::InputPort<_Float64>("tolerance"),
       BT::InputPort<_Float64>("timeout"),
+      BT::InputPort<std::string>("mirror"),
       BT::OutputPort<int8_t>("result")
     });
 
@@ -85,17 +86,16 @@ void DynamixelCommandAction::on_tick()
               << "' on table '" << table << "' detected" << std::endl;
   }
 
-  if (g_StrategyMirror.server_name_requires_mirroring(action_name_)) {
-    g_StrategyMirror.remap_server_name(action_name_);
-    g_StrategyMirror.mirror_angle(position, true);
-  }
+  std::string mirror;
+  getInput("mirror", mirror);
 
-  // ovo je testirano za small, mozda treba obrisati
-  if (g_StrategyMirror.server_name_requires_mirroring1(action_name_)) {
+  if (g_StrategyMirror.server_name_requires_mirroring(action_name_, mirror)) {
     g_StrategyMirror.remap_server_name(action_name_);
-    g_StrategyMirror.mirror_angle(position, true);
   }
 
+  if (g_StrategyMirror.angle_requires_mirroring(action_name_, mirror)) {
+    g_StrategyMirror.invert_angle(position);
+  }
 
   goal_.position = position;
   goal_.velocity = velocity;

mep3_behavior_tree/include/mep3_behavior_tree/motion_command_action.hpp

@@ -92,9 +92,9 @@ void MotionCommandAction::on_tick()
   }
 
   if (command == "rotate_relative") {
-    g_StrategyMirror.mirror_angle(value, true);
+    g_StrategyMirror.invert_angle(value);
   } else if (command == "rotate_absolute") {
-    g_StrategyMirror.mirror_angle(value, false);
+    g_StrategyMirror.mirror_angle(value);
   }
 
   goal_.command = command;

mep3_behavior_tree/include/mep3_behavior_tree/navigate_through_action.hpp

@@ -48,6 +48,7 @@ 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")
       });
 
@@ -77,10 +78,16 @@ namespace mep3_behavior_tree
                 << table << "' detected" << std::endl;
     }
 
+    std::string mirror;
+    getInput("mirror", mirror);
+    bool requires_mirroring = g_StrategyMirror.requires_mirroring(mirror);
+
     goal_.behavior_tree = behavior_tree;
     for (auto &pose : poses)
     {
-      g_StrategyMirror.mirror_pose(pose);
+      if (requires_mirroring) {
+        g_StrategyMirror.mirror_pose(pose);
+      }
 
       geometry_msgs::msg::PoseStamped pose_stamped;
       pose_stamped.header.frame_id = "map";

mep3_behavior_tree/include/mep3_behavior_tree/navigate_to_action.hpp

@@ -47,6 +47,7 @@ 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")
       });
 
@@ -75,12 +76,18 @@ namespace mep3_behavior_tree
                 << table << "' detected" << std::endl;
       goal += goal_offset;
     }
+
+    std::string mirror;
+    getInput("mirror", mirror);
+
+    if (g_StrategyMirror.requires_mirroring(mirror)) {
+      g_StrategyMirror.mirror_pose(goal);
+    }
+
     std::cout << "Navigating to x=" << goal.x \
               << " y=" << goal.y \
               << " θ=" << goal.theta << "°" << std::endl;
 
-    g_StrategyMirror.mirror_pose(goal);
-
     goal_.pose.header.frame_id = "map";
     goal_.pose.header.stamp = node_->get_clock()->now();
 

mep3_behavior_tree/include/mep3_behavior_tree/precise_navigate_to_action.hpp

@@ -46,6 +46,7 @@ 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")
       });
 
@@ -74,12 +75,18 @@ namespace mep3_behavior_tree
     //             << table << "' detected" << std::endl;
     //   goal += goal_offset;
     // }
+
+    std::string mirror;
+    getInput("mirror", mirror);
+
+    if (g_StrategyMirror.requires_mirroring(mirror)) {
+      g_StrategyMirror.mirror_pose(goal);
+    }
+
     std::cout << "Precise navigating to x=" << goal.x \
               << " y=" << goal.y \
               << " θ=" << goal.theta << "°" << std::endl;
 
-    g_StrategyMirror.mirror_pose(goal);
-
     goal_.pose.header.frame_id = "map";
     goal_.pose.header.stamp = node_->get_clock()->now();
 

mep3_behavior_tree/include/mep3_behavior_tree/team_color_strategy_mirror.hpp

@@ -15,9 +15,13 @@
 #ifndef MEP3_BEHAVIOR_TREE__TEAM_COLOR_STRATEGY_MIRROR_HPP_
 #define MEP3_BEHAVIOR_TREE__TEAM_COLOR_STRATEGY_MIRROR_HPP_
 
+#define RESISTANCE_VALUE_YELLOW 1000
+#define RESISTANCE_VALUE_PURPLE 470
+
 #include <regex>
 #include <stdexcept>
 #include <string>
+#include <vector>
 
 #include "behaviortree_cpp_v3/action_node.h"
 #include "mep3_behavior_tree/pose_2d.hpp"
@@ -29,17 +33,32 @@ enum TeamColor {
   Yellow
 };
 
+enum MirrorParam {
+  True,
+  False,
+  Default
+};
+
 class StrategyMirror {
 public:
   StrategyMirror() {
     // Set to default color
+    this->default_color = TeamColor::Purple;
     this->color = TeamColor::Purple;
   }
 
   void set_color(const std::string& color) {
     this->color = StrategyMirror::string_to_color_enum(color);
   }
 
+  void set_angle_blacklist(const std::vector<std::string>& list) {
+    this->mirror_angle_blacklist = list;
+  }
+
+  void set_name_blacklist(const std::vector<std::string>& list) {
+    this->mirror_name_blacklist = list;
+  }
+
   void set_default_color(const std::string& color) {
     this->default_color = StrategyMirror::string_to_color_enum(color);
   }
@@ -70,32 +89,73 @@ class StrategyMirror {
     server_name = std::regex_replace(server_name, re_placeholder, "left");
   }
 
-  bool server_name_requires_mirroring(std::string server_name) {
-    if (this->color == this->default_color)
-      return false;
-    //const auto re_arm_base = std::regex("arm_[a-z]+_motor_base");
-    const auto re_hand = std::regex("hand_[a-z]+_(Dz|G)");
-    const auto re_vacuum = std::regex("[a-z]+_(left|right)_connector");
-    /* return std::regex_search(server_name, re_arm_base) || \*/
-    return       std::regex_search(server_name, re_hand) || \
-           std::regex_search(server_name, re_vacuum);
+  bool requires_mirroring(
+    const std::string& mirror
+  ) {
+    MirrorParam m = StrategyMirror::string_to_mirror_enum(mirror);
+    return m != MirrorParam::False;
   }
 
-  bool server_name_requires_mirroring1(std::string server_name) {
-    if (this->color == this->default_color)
-      return false;
-    const auto re_arm_base = std::regex("arm_[a-z]+_motor_base");
-    return std::regex_search(server_name, re_arm_base);
+  bool server_name_requires_mirroring(
+    const std::string& server_name,
+    const std::string& mirror
+  ) {
+    MirrorParam m = StrategyMirror::string_to_mirror_enum(mirror);
+    switch (m) {
+      case MirrorParam::True:
+        return true;
+      case MirrorParam::False:
+        return false;
+      default:
+        if (this->color == this->default_color)
+          return false;
+        // Return true if not found in blacklist array
+        return std::find(
+          this->mirror_name_blacklist.begin(),
+          this->mirror_name_blacklist.end(),
+          server_name
+        ) == this->mirror_name_blacklist.end();
+    }
+  }
+
+  bool angle_requires_mirroring(
+    const std::string& server_name,
+    const std::string& mirror
+  ) {
+    MirrorParam m = StrategyMirror::string_to_mirror_enum(mirror);
+    switch (m) {
+      case MirrorParam::True:
+        return true;
+      case MirrorParam::False:
+        return false;
+      default:
+        if (this->color == this->default_color)
+          return false;
+        // Return true if not found in blacklist array
+        return std::find(
+          this->mirror_angle_blacklist.begin(),
+          this->mirror_angle_blacklist.end(),
+          server_name
+        ) == this->mirror_angle_blacklist.end();
+    }
   }
 
   template<typename Number>
-  void mirror_angle(Number& angle, const bool invert) {
+  void invert_angle(Number& angle) {
     if (this->color == this->default_color)
       return;
-    if (invert) {
-      angle *= -1;
-    } else {
+    // Constraint by physical servo orientation
+    angle = 300.0 - angle;
+  }
+
+  template<typename Number>
+  void mirror_angle(Number& angle) {
+    if (this->color == this->default_color)
+      return;
+    if (angle >= 0) {
       angle = 180.0 - angle;
+    } else {
+      angle = -180.0 - angle;
     }
   }
 
@@ -104,11 +164,11 @@ class StrategyMirror {
     if (this->color == this->default_color)
       return;
     switch (resistance) {
-    case 420:
-      resistance = 1750;
+    case RESISTANCE_VALUE_YELLOW:
+      resistance = RESISTANCE_VALUE_PURPLE;
       return;
-    case 1750:
-      resistance = 420;
+    case RESISTANCE_VALUE_PURPLE:
+      resistance = RESISTANCE_VALUE_YELLOW;
       return;
     }
   }
@@ -124,8 +184,19 @@ class StrategyMirror {
     }
   }
 
-  TeamColor color;
-  TeamColor default_color;
+  static MirrorParam string_to_mirror_enum(const std::string& mirror) {
+    if (mirror == "false") {
+      return MirrorParam::False;
+    } else if (mirror == "true") {
+      return MirrorParam::True;
+    } else {
+      return MirrorParam::Default;
+    }
+  }
+
+  TeamColor color, default_color;
+  std::vector<std::string> mirror_angle_blacklist, mirror_name_blacklist;
+
 };
 
 // Globally shared singleton

mep3_behavior_tree/include/mep3_behavior_tree/vacuum_pump_command_action.hpp

@@ -43,7 +43,11 @@ class VacuumPumpCommandAction
 
   static BT::PortsList providedPorts()
   {
-    return providedBasicPorts({BT::InputPort<int8_t>("connect"), BT::OutputPort<int8_t>("result")});
+    return providedBasicPorts({
+      BT::InputPort<int8_t>("connect"),
+      BT::InputPort<std::string>("mirror"),
+      BT::OutputPort<int8_t>("result")
+    });
   }
 };
 
@@ -53,7 +57,10 @@ void VacuumPumpCommandAction::on_tick()
 
   getInput("connect", connect);
 
-  if (g_StrategyMirror.server_name_requires_mirroring(action_name_)) {
+  std::string mirror;
+  getInput("mirror", mirror);
+
+  if (g_StrategyMirror.server_name_requires_mirroring(action_name_, mirror)) {
     g_StrategyMirror.remap_server_name(action_name_);
   }
 

mep3_behavior_tree/src/mep3_behavior_tree.cpp

@@ -107,19 +107,39 @@ int main(int argc, char **argv)
     predefined_tables.as_string_array()
   );
 
+  // Set color
+  node->declare_parameter<std::string>("color", "purple");
+  auto color = node->get_parameter("color");
+  mep3_behavior_tree::g_StrategyMirror.set_color(color.as_string());
+  blackboard->set("color", color.as_string());
+
+  // Get mirroring blacklists
+  node->declare_parameter<std::vector<std::string>>("mirror_angle_blacklist", std::vector<std::string>({}));
+  node->declare_parameter<std::vector<std::string>>("mirror_name_blacklist", std::vector<std::string>({}));
+  rclcpp::Parameter mirror_angle_blacklist(
+    "mirror_angle_blacklist",
+    std::vector<std::string>({})
+  );
+  rclcpp::Parameter mirror_name_blacklist(
+    "mirror_name_blacklist",
+    std::vector<std::string>({})
+  );
+  node->get_parameter("mirror_angle_blacklist", mirror_angle_blacklist);
+  node->get_parameter("mirror_name_blacklist", mirror_name_blacklist);
+  mep3_behavior_tree::g_StrategyMirror.set_angle_blacklist(
+    mirror_angle_blacklist.as_string_array()
+  );
+  mep3_behavior_tree::g_StrategyMirror.set_name_blacklist(
+    mirror_name_blacklist.as_string_array()
+  );
+
   blackboard->set<std::chrono::milliseconds>(
       "bt_loop_duration",
       std::chrono::milliseconds(10));
   blackboard->set<std::chrono::milliseconds>(
       "server_timeout",
       std::chrono::milliseconds(1000));
 
-  // Set color
-  node->declare_parameter<std::string>("color", "purple");
-  auto color = node->get_parameter("color");
-  mep3_behavior_tree::g_StrategyMirror.set_color(color.as_string());
-  blackboard->set("color", color.as_string());
-
   BT::BehaviorTreeFactory factory;
 
   BT::SharedLibrary loader;