diff --git a/mep3_behavior_tree/assets/strategies/big/purple_strategy.xml b/mep3_behavior_tree/assets/strategies/big/purple_strategy.xml index d8ad92df2..7ab5a2cd8 100644 --- a/mep3_behavior_tree/assets/strategies/big/purple_strategy.xml +++ b/mep3_behavior_tree/assets/strategies/big/purple_strategy.xml @@ -2,8 +2,11 @@ - + + + + diff --git a/mep3_behavior_tree/assets/strategies/small/purple_strategy.xml b/mep3_behavior_tree/assets/strategies/small/purple_strategy.xml index 939022bd6..28b9d1a31 100644 --- a/mep3_behavior_tree/assets/strategies/small/purple_strategy.xml +++ b/mep3_behavior_tree/assets/strategies/small/purple_strategy.xml @@ -2,8 +2,11 @@ - + + + + diff --git a/mep3_behavior_tree/include/mep3_behavior_tree/bt_action_node.hpp b/mep3_behavior_tree/include/mep3_behavior_tree/bt_action_node.hpp index 1dc18bce7..9dbdd18bf 100644 --- a/mep3_behavior_tree/include/mep3_behavior_tree/bt_action_node.hpp +++ b/mep3_behavior_tree/include/mep3_behavior_tree/bt_action_node.hpp @@ -19,8 +19,8 @@ #include #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("server_name", "Action server name"), BT::InputPort("label", "Action name suffix"), - BT::InputPort("server_timeout")}; + BT::InputPort("mirror", "Action mirroring flag"), + BT::InputPort("server_timeout") + }; basic.insert(addition.begin(), addition.end()); return basic; @@ -383,7 +401,8 @@ class BtActionNode : public BT::ActionNodeBase config().blackboard->template set("number_recoveries", recovery_count); // NOLINT } - std::string action_name_; + std::string action_name_, original_action_name_; + MirrorParam mirror_; typename std::shared_ptr> action_client_; // All ROS2 actions have a goal and a result 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 041407240..8535950d4 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 @@ -36,6 +36,27 @@ class DynamixelCommandAction : mep3_behavior_tree::BtActionNode( 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("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("mirror"), BT::OutputPort("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("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 << "°" <( 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("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("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; 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 43a43f6b4..24e34dad8 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,7 +48,6 @@ namespace mep3_behavior_tree // Static parameters BT::PortsList port_list = providedBasicPorts({ BT::InputPort>("goal"), - BT::InputPort("mirror"), BT::InputPort("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) 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 f5d590f7f..41c36970b 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 @@ -37,6 +37,21 @@ namespace mep3_behavior_tree : mep3_behavior_tree::BtActionNode( 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("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("goal"), - BT::InputPort("mirror"), BT::InputPort("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("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() 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 3f4a32f5f..96a5f47eb 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 @@ -36,6 +36,22 @@ namespace mep3_behavior_tree : mep3_behavior_tree::BtActionNode( xml_tag_name, "precise_navigate_to_pose", config) { + if (!getInput("goal", this->goal)) + throw BT::RuntimeError( + "PreciseNavigate action requires 'goal' argument" + ); + getInput("behavior_tree", this->behavior_tree); + + // Warning: might break for a still undetermined reason + std::string table = this->config().blackboard->get("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; @@ -46,7 +62,6 @@ namespace mep3_behavior_tree // Static parameters BT::PortsList port_list = providedBasicPorts({ BT::InputPort("goal"), - BT::InputPort("mirror"), BT::InputPort("behavior_tree") }); @@ -59,30 +74,14 @@ namespace mep3_behavior_tree return port_list; } + + private: + BT::Pose2D goal; + std::string behavior_tree; }; void PreciseNavigateToAction::on_tick() { - BT::Pose2D goal; - std::string behavior_tree; - getInput("goal", goal); - getInput("behavior_tree", behavior_tree); - - // std::string table = config().blackboard->get("table"); - // BT::Pose2D goal_offset; - // if (table.length() > 0 && getInput("goal_" + table, goal_offset)) { - // std::cout << "Precise 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); - } - std::cout << "Precise navigating to x=" << goal.x \ << " y=" << goal.y \ << " θ=" << goal.theta << "°" << std::endl; @@ -97,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 PreciseNavigateToAction::on_success() diff --git a/mep3_behavior_tree/include/mep3_behavior_tree/resistance_meter_action.hpp b/mep3_behavior_tree/include/mep3_behavior_tree/resistance_meter_action.hpp index caa1d669e..22cd28244 100644 --- a/mep3_behavior_tree/include/mep3_behavior_tree/resistance_meter_action.hpp +++ b/mep3_behavior_tree/include/mep3_behavior_tree/resistance_meter_action.hpp @@ -40,6 +40,26 @@ namespace mep3_behavior_tree name, "resistance_meter", config ) { + if (!getInput("resistance", this->expected)) + throw BT::RuntimeError( + "ResistanceMeter action requires 'resistance' argument" + ); + if (!getInput("tolerance", this->tolerance)) + throw BT::RuntimeError( + "ResistanceMeter action requires 'tolerance' argument" + ); + + tolerance /= 100.0; + + std::string table = this->config().blackboard->get("table"); + int32_t resistance_offset; + if (table.length() > 0 && getInput("resistance_" + table, resistance_offset)) { + expected += resistance_offset; + } + + if (g_StrategyMirror.requires_mirroring(mirror_)) { + g_StrategyMirror.mirror_resistance(expected); + } } void on_tick() override; @@ -64,32 +84,24 @@ namespace mep3_behavior_tree return port_list; } + + private: + int32_t measured, expected; + _Float32 tolerance; }; void ResistanceMeterAction::on_tick() { + std::cout << "Measuring resistance on '" \ + << this->action_name_ << "'" << std::endl; } BT::NodeStatus ResistanceMeterAction::on_success() { int32_t measured = result_.result->resistance; - - int32_t expected; - _Float32 tolerance; - getInput("resistance", expected); - getInput("tolerance", tolerance); - tolerance /= 100.0; - - std::string table = config().blackboard->get("table"); - int32_t resistance_offset; - if (table.length() > 0 && getInput("resistance_" + table, resistance_offset)) { - expected += resistance_offset; - std::cout << "Resistance offset for table '" \ - << table << "' detected" << std::endl; - } - - g_StrategyMirror.mirror_resistance(expected); - + std::cout << "Expected " << expected << "±" \ + << tolerance * expected << "Ω, " \ + << "measured " << 0 << "Ω" << std::endl; if ( measured >= expected * (1.0 - tolerance) && \ measured <= expected * (1.0 + tolerance) 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 cf6955c56..111395379 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 @@ -90,18 +90,26 @@ class StrategyMirror { } bool requires_mirroring( - const std::string& mirror + const MirrorParam mirror ) { - MirrorParam m = StrategyMirror::string_to_mirror_enum(mirror); - return m != MirrorParam::False; + switch (mirror) { + case MirrorParam::True: + return true; + case MirrorParam::False: + return false; + default: + if (this->color == this->default_color) + return false; + else + return true; + } } bool server_name_requires_mirroring( const std::string& server_name, - const std::string& mirror + const MirrorParam mirror ) { - MirrorParam m = StrategyMirror::string_to_mirror_enum(mirror); - switch (m) { + switch (mirror) { case MirrorParam::True: return true; case MirrorParam::False: @@ -113,17 +121,16 @@ class StrategyMirror { return std::find( this->mirror_name_blacklist.begin(), this->mirror_name_blacklist.end(), - server_name + StrategyMirror::strip_server_name(server_name) ) == this->mirror_name_blacklist.end(); } } bool angle_requires_mirroring( const std::string& server_name, - const std::string& mirror + const MirrorParam mirror ) { - MirrorParam m = StrategyMirror::string_to_mirror_enum(mirror); - switch (m) { + switch (mirror) { case MirrorParam::True: return true; case MirrorParam::False: @@ -135,23 +142,19 @@ class StrategyMirror { return std::find( this->mirror_angle_blacklist.begin(), this->mirror_angle_blacklist.end(), - server_name + StrategyMirror::strip_server_name(server_name) ) == this->mirror_angle_blacklist.end(); } } template void invert_angle(Number& angle) { - if (this->color == this->default_color) - return; // 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 { @@ -161,8 +164,6 @@ class StrategyMirror { template void mirror_resistance(Number& resistance) { - if (this->color == this->default_color) - return; switch (resistance) { case RESISTANCE_VALUE_YELLOW: resistance = RESISTANCE_VALUE_PURPLE; @@ -172,6 +173,16 @@ class StrategyMirror { return; } } + + 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; + } + } private: static TeamColor string_to_color_enum(const std::string& color) { @@ -184,14 +195,13 @@ class StrategyMirror { } } - 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; + static std::string strip_server_name(const std::string& full_name) { + std::string stripped_name = full_name; + auto separator = full_name.find_last_of("/"); + if (separator != std::string::npos && separator < stripped_name.length() - 1) { + stripped_name = stripped_name.substr(separator + 1, stripped_name.length() - (separator + 1)); } + return stripped_name; } TeamColor color, default_color; 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 29ab39c3c..19bf0be28 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 @@ -34,6 +34,10 @@ class VacuumPumpCommandAction : mep3_behavior_tree::BtActionNode( xml_tag_name, "vacuum_pump_command", config) { + if (!getInput("connect", this->connect)) + throw BT::RuntimeError( + "VacuumPump action requires 'connect' argument" + ); } void on_tick() override; @@ -45,25 +49,18 @@ class VacuumPumpCommandAction { return providedBasicPorts({ BT::InputPort("connect"), - BT::InputPort("mirror"), BT::OutputPort("result") }); } + +private: + _Float64 connect; }; void VacuumPumpCommandAction::on_tick() { - _Float64 connect; - - getInput("connect", connect); - - std::string mirror; - getInput("mirror", mirror); - - if (g_StrategyMirror.server_name_requires_mirroring(action_name_, mirror)) { - g_StrategyMirror.remap_server_name(action_name_); - } - + std::cout << ((this->connect == 1) ? "C" : "Disc") << "onnecting vacuum pump '" \ + << this->action_name_ << "'" << std::endl; goal_.connect = connect; } diff --git a/mep3_bringup/launch/robot_launch.py b/mep3_bringup/launch/robot_launch.py index 26437968e..fdcc49983 100644 --- a/mep3_bringup/launch/robot_launch.py +++ b/mep3_bringup/launch/robot_launch.py @@ -28,19 +28,27 @@ ] ANGLE_MIRRORING_BLACKLIST = [ + # Florian (big) 'box', 'flipper_left', 'flipper_right', + # Kosta (small) 'base', 'mid', - 'gripper' + 'gripper', + 'rail' ] SERVER_NAME_MIRRORING_BLACKLIST = [ + # Florian (big) 'box', + # Kosta (small) 'base', 'mid', - 'gripper' + 'gripper', + 'rail', + 'fork_left', + 'fork_right' ] def verify_color(context, *args, **kwargs):