From 04f4cde84c3cc4578a6e3a0985f7464fb01adc61 Mon Sep 17 00:00:00 2001 From: Filip Parag Date: Sun, 22 May 2022 15:46:51 +0200 Subject: [PATCH] Strategy mirroring improvements (#215) --- .../dynamixel_command_action.hpp | 14 +-- .../motion_command_action.hpp | 4 +- .../navigate_through_action.hpp | 9 +- .../mep3_behavior_tree/navigate_to_action.hpp | 11 +- .../precise_navigate_to_action.hpp | 11 +- .../team_color_strategy_mirror.hpp | 119 ++++++++++++++---- .../vacuum_pump_command_action.hpp | 11 +- mep3_behavior_tree/src/mep3_behavior_tree.cpp | 32 ++++- mep3_bringup/launch/robot_launch.py | 20 ++- 9 files changed, 184 insertions(+), 47 deletions(-) diff --git a/mep3_behavior_tree/include/mep3_behavior_tree/dynamixel_command_action.hpp b/mep3_behavior_tree/include/mep3_behavior_tree/dynamixel_command_action.hpp index 9ffdbba4d..041407240 100644 --- a/mep3_behavior_tree/include/mep3_behavior_tree/dynamixel_command_action.hpp +++ b/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("mirror"), BT::OutputPort("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; diff --git a/mep3_behavior_tree/include/mep3_behavior_tree/motion_command_action.hpp b/mep3_behavior_tree/include/mep3_behavior_tree/motion_command_action.hpp index 78f3cde35..b1cda45e2 100644 --- a/mep3_behavior_tree/include/mep3_behavior_tree/motion_command_action.hpp +++ b/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; diff --git a/mep3_behavior_tree/include/mep3_behavior_tree/navigate_through_action.hpp b/mep3_behavior_tree/include/mep3_behavior_tree/navigate_through_action.hpp index d8eeaa19d..43a43f6b4 100644 --- a/mep3_behavior_tree/include/mep3_behavior_tree/navigate_through_action.hpp +++ b/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>("goal"), + BT::InputPort("mirror"), BT::InputPort("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"; diff --git a/mep3_behavior_tree/include/mep3_behavior_tree/navigate_to_action.hpp b/mep3_behavior_tree/include/mep3_behavior_tree/navigate_to_action.hpp index b12c22bc7..f5d590f7f 100644 --- a/mep3_behavior_tree/include/mep3_behavior_tree/navigate_to_action.hpp +++ b/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("goal"), + BT::InputPort("mirror"), BT::InputPort("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(); diff --git a/mep3_behavior_tree/include/mep3_behavior_tree/precise_navigate_to_action.hpp b/mep3_behavior_tree/include/mep3_behavior_tree/precise_navigate_to_action.hpp index f00a441e4..3f4a32f5f 100644 --- a/mep3_behavior_tree/include/mep3_behavior_tree/precise_navigate_to_action.hpp +++ b/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("goal"), + BT::InputPort("mirror"), BT::InputPort("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(); diff --git a/mep3_behavior_tree/include/mep3_behavior_tree/team_color_strategy_mirror.hpp b/mep3_behavior_tree/include/mep3_behavior_tree/team_color_strategy_mirror.hpp index 5470c1634..cf6955c56 100644 --- a/mep3_behavior_tree/include/mep3_behavior_tree/team_color_strategy_mirror.hpp +++ b/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 #include #include +#include #include "behaviortree_cpp_v3/action_node.h" #include "mep3_behavior_tree/pose_2d.hpp" @@ -29,10 +33,17 @@ 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; } @@ -40,6 +51,14 @@ class StrategyMirror { this->color = StrategyMirror::string_to_color_enum(color); } + void set_angle_blacklist(const std::vector& list) { + this->mirror_angle_blacklist = list; + } + + void set_name_blacklist(const std::vector& 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 - 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 + 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 mirror_angle_blacklist, mirror_name_blacklist; + }; // Globally shared singleton diff --git a/mep3_behavior_tree/include/mep3_behavior_tree/vacuum_pump_command_action.hpp b/mep3_behavior_tree/include/mep3_behavior_tree/vacuum_pump_command_action.hpp index 82d75052f..29ab39c3c 100644 --- a/mep3_behavior_tree/include/mep3_behavior_tree/vacuum_pump_command_action.hpp +++ b/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("connect"), BT::OutputPort("result")}); + return providedBasicPorts({ + BT::InputPort("connect"), + BT::InputPort("mirror"), + BT::OutputPort("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_); } diff --git a/mep3_behavior_tree/src/mep3_behavior_tree.cpp b/mep3_behavior_tree/src/mep3_behavior_tree.cpp index 09ad28906..16b8e704b 100644 --- a/mep3_behavior_tree/src/mep3_behavior_tree.cpp +++ b/mep3_behavior_tree/src/mep3_behavior_tree.cpp @@ -107,6 +107,32 @@ int main(int argc, char **argv) predefined_tables.as_string_array() ); + // Set color + node->declare_parameter("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>("mirror_angle_blacklist", std::vector({})); + node->declare_parameter>("mirror_name_blacklist", std::vector({})); + rclcpp::Parameter mirror_angle_blacklist( + "mirror_angle_blacklist", + std::vector({}) + ); + rclcpp::Parameter mirror_name_blacklist( + "mirror_name_blacklist", + std::vector({}) + ); + 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( "bt_loop_duration", std::chrono::milliseconds(10)); @@ -114,12 +140,6 @@ int main(int argc, char **argv) "server_timeout", std::chrono::milliseconds(1000)); - // Set color - node->declare_parameter("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; diff --git a/mep3_bringup/launch/robot_launch.py b/mep3_bringup/launch/robot_launch.py index e2b11ae88..26437968e 100644 --- a/mep3_bringup/launch/robot_launch.py +++ b/mep3_bringup/launch/robot_launch.py @@ -27,6 +27,22 @@ 'table2' ] +ANGLE_MIRRORING_BLACKLIST = [ + 'box', + 'flipper_left', + 'flipper_right', + 'base', + 'mid', + 'gripper' +] + +SERVER_NAME_MIRRORING_BLACKLIST = [ + 'box', + 'base', + 'mid', + 'gripper' +] + def verify_color(context, *args, **kwargs): if LaunchConfiguration('color').perform(context) \ not in ['purple', 'yellow']: @@ -126,7 +142,9 @@ def generate_launch_description(): 'color': color, 'table': table, 'strategy': strategy, - 'predefined_tables': PREDEFINED_TABLE_NAMES + 'predefined_tables': PREDEFINED_TABLE_NAMES, + 'mirror_angle_blacklist': ANGLE_MIRRORING_BLACKLIST, + 'mirror_name_blacklist': SERVER_NAME_MIRRORING_BLACKLIST }], namespace=namespace, condition=launch.conditions.IfCondition(use_behavior_tree))