diff --git a/mep3_behavior/include/mep3_behavior/bt_action_node.hpp b/mep3_behavior/include/mep3_behavior/bt_action_node.hpp index c86ec17f5..68ebf0097 100644 --- a/mep3_behavior/include/mep3_behavior/bt_action_node.hpp +++ b/mep3_behavior/include/mep3_behavior/bt_action_node.hpp @@ -102,6 +102,7 @@ class RosActionNode : public BT::ActionNodeBase void cancelGoal(); protected: + bool should_cancel_goal(); std::shared_ptr node_; std::string action_name_; @@ -335,16 +336,39 @@ template inline if (!goal_handle_) return; - auto future_cancel = action_client_->async_cancel_goal(goal_handle_); + if (should_cancel_goal()) { + auto future_cancel = action_client_->async_cancel_goal(goal_handle_); - if (rclcpp::spin_until_future_complete(node_, future_cancel, server_timeout_) != - rclcpp::FutureReturnCode::SUCCESS) + if (rclcpp::spin_until_future_complete(node_, future_cancel, server_timeout_) != + rclcpp::FutureReturnCode::SUCCESS) + { + RCLCPP_ERROR( node_->get_logger(), + "Failed to cancel action server for %s", action_name_.c_str()); + } + } +} + +template inline + bool RosActionNode::should_cancel_goal() +{ + // Shut the node down if it is currently running + if (status() != BT::NodeStatus::RUNNING) { + return false; + } + + rclcpp::spin_some(node_); + auto status = goal_handle_->get_status(); + + // Check if the goal is still executing + if (status == action_msgs::msg::GoalStatus::STATUS_ACCEPTED || + status == action_msgs::msg::GoalStatus::STATUS_EXECUTING) { - RCLCPP_ERROR( node_->get_logger(), - "Failed to cancel action server for %s", action_name_.c_str()); + return true; } + + return false; } } // namespace BT -#endif // BEHAVIOR_TREE_ROS2__BT_ACTION_NODE_HPP_ +#endif // BEHAVIOR_TREE_ROS2__BT_ACTION_NODE_HPP_ \ No newline at end of file diff --git a/mep3_behavior/include/mep3_behavior/move_action.hpp b/mep3_behavior/include/mep3_behavior/move_action.hpp index a2a2a4c9c..26784c09a 100644 --- a/mep3_behavior/include/mep3_behavior/move_action.hpp +++ b/mep3_behavior/include/mep3_behavior/move_action.hpp @@ -135,6 +135,8 @@ namespace mep3_behavior // ala ce robot da leti :) if (!getInput("max_acceleration", max_acceleration_)) max_velocity_ = 99999; + if (!getInput("ignore_obstacles", ignore_obstacles_)) + ignore_obstacles_=true; std::string table = this->config().blackboard->get("table"); double goal_offset; @@ -148,7 +150,8 @@ namespace mep3_behavior { std::cout << "Translate to " << target_position_ \ << "m max_velocity="<("goal"), BT::InputPort("max_velocity"), - BT::InputPort("max_acceleration")}; + BT::InputPort("max_acceleration"), + BT::InputPort("ignore_obstacles")}; // Dynamic parameters for (std::string table : BT::SharedBlackboard::access()->get>("predefined_tables")) @@ -187,6 +191,7 @@ namespace mep3_behavior double target_position_; double max_velocity_; double max_acceleration_; + bool ignore_obstacles_; }; diff --git a/mep3_behavior/src/mep3_behavior_tree.cpp b/mep3_behavior/src/mep3_behavior_tree.cpp index a7306c04d..b09227e02 100644 --- a/mep3_behavior/src/mep3_behavior_tree.cpp +++ b/mep3_behavior/src/mep3_behavior_tree.cpp @@ -116,8 +116,17 @@ int main(int argc, char **argv) // Set color node->declare_parameter("color", "blue"); - auto color = node->get_parameter("color"); - if (color.as_string() == "green") + auto color = node->get_parameter("color").as_string(); + + //Adapt different start field to default color + size_t found = color.find('_'); + if(found != std::string::npos){ + color.erase(found, color.size()); + } + + std::cout<<"ERASED COLOR: "<set("color", BT::TeamColor::GREEN); else blackboard->set("color", BT::TeamColor::BLUE); diff --git a/mep3_behavior/strategies/big_blue.xml b/mep3_behavior/strategies/big_blue.xml index 46448754f..07a356c24 100644 --- a/mep3_behavior/strategies/big_blue.xml +++ b/mep3_behavior/strategies/big_blue.xml @@ -2,7 +2,75 @@ - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 0 = unarmed; 1 = armed; 2 = started + diff --git a/mep3_behavior/strategies/big_homologation.xml b/mep3_behavior/strategies/big_homologation.xml index ba74c84a5..9faa2ba0f 100644 --- a/mep3_behavior/strategies/big_homologation.xml +++ b/mep3_behavior/strategies/big_homologation.xml @@ -42,8 +42,9 @@ color="{color}" table="{table}"/> + ignore_obstacles="false" + max_velocity="0.5" + max_acceleration="0.5"/> - + @@ -88,10 +90,12 @@ editable="true"> - + + diff --git a/mep3_behavior/strategies/big_strategy.xml b/mep3_behavior/strategies/big_strategy.xml index dde96cef2..294834c02 100644 --- a/mep3_behavior/strategies/big_strategy.xml +++ b/mep3_behavior/strategies/big_strategy.xml @@ -3,13 +3,13 @@ - - + + table="{table}"/> - - + - + table="{table}"/> + + + + + + + + + + + + + + + + + ignore_obstacles="false" + max_velocity="1.0" + max_acceleration="0.5"/> - - + + + + + - - + + table="{table}"/> @@ -121,8 +144,12 @@ feedback_position="feedback_position" result="result"/> - + + - + - + - - - - - - - - - - - - + + - - - + + + + + + + + + + + + + + + ignore_obstacles="false" + max_velocity="1.0" + max_acceleration="0.5"/> @@ -246,10 +277,16 @@ editable="true"> - + + + + + diff --git a/mep3_behavior/strategies/big_strategy_a.xml b/mep3_behavior/strategies/big_strategy_a.xml new file mode 100644 index 000000000..49a3a5c8d --- /dev/null +++ b/mep3_behavior/strategies/big_strategy_a.xml @@ -0,0 +1,294 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + points scored, can be negative + unique task name + + + + + + + + + + + + + + diff --git a/mep3_behavior/strategies/big_strategy_green.xml b/mep3_behavior/strategies/big_strategy_green.xml index 5cddb605b..76af2bffb 100644 --- a/mep3_behavior/strategies/big_strategy_green.xml +++ b/mep3_behavior/strategies/big_strategy_green.xml @@ -3,13 +3,13 @@ - - + + table="{table}"/> - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + - + table="{table}"/> - - + + table="{table}"/> - + @@ -153,40 +172,43 @@ feedback_effort="feedback_effort" feedback_position="feedback_position" result="result"/> - - - - - - - - - - - + + - - - - + + + + + + + + + + + + + + + ignore_obstacles="false" + max_velocity="1.0" + max_acceleration="0.5"/> @@ -237,10 +260,16 @@ editable="true"> - + + + + + diff --git a/mep3_behavior/strategies/big_strategy_green_a.xml b/mep3_behavior/strategies/big_strategy_green_a.xml new file mode 100644 index 000000000..6ba9aa553 --- /dev/null +++ b/mep3_behavior/strategies/big_strategy_green_a.xml @@ -0,0 +1,284 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + points scored, can be negative + unique task name + + + + + + + + + + + + + diff --git a/mep3_behavior/strategies/big_strategy_green_s.xml b/mep3_behavior/strategies/big_strategy_green_s.xml new file mode 100644 index 000000000..b5c3e5160 --- /dev/null +++ b/mep3_behavior/strategies/big_strategy_green_s.xml @@ -0,0 +1,267 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + points scored, can be negative + unique task name + + + + + + + + + + + + + diff --git a/mep3_behavior/strategies/big_strategy_s.xml b/mep3_behavior/strategies/big_strategy_s.xml new file mode 100644 index 000000000..b8c1c348b --- /dev/null +++ b/mep3_behavior/strategies/big_strategy_s.xml @@ -0,0 +1,276 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + points scored, can be negative + unique task name + + + + + + + + + + + + + + diff --git a/mep3_behavior/strategies/big_strategy_simple.xml b/mep3_behavior/strategies/big_strategy_simple.xml index a85b34702..69f0f3325 100644 --- a/mep3_behavior/strategies/big_strategy_simple.xml +++ b/mep3_behavior/strategies/big_strategy_simple.xml @@ -12,228 +12,286 @@ - - - + + + - + - - + - - - - - - - - - - - - - - - - - - - - - - - - - - - + + - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - + + + + + + + + + - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - - - - - - - - - - - - - - - - + - + - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + - - - - - - - - - - - - - + + + @@ -274,10 +332,12 @@ editable="true"> - + + diff --git a/mep3_behavior/strategies/big_strategy_simple_a.xml b/mep3_behavior/strategies/big_strategy_simple_a.xml new file mode 100644 index 000000000..bd2fd4097 --- /dev/null +++ b/mep3_behavior/strategies/big_strategy_simple_a.xml @@ -0,0 +1,351 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + points scored, can be negative + unique task name + + + + + + + + + + + + + 0 = unarmed; 1 = armed; 2 = started + + + + diff --git a/mep3_behavior/strategies/big_strategy_simple_green.xml b/mep3_behavior/strategies/big_strategy_simple_green.xml index 0d94c5967..9c3c6a2e2 100644 --- a/mep3_behavior/strategies/big_strategy_simple_green.xml +++ b/mep3_behavior/strategies/big_strategy_simple_green.xml @@ -14,122 +14,16 @@ task="KORPA"/> - + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + - + + + + + + + + + + + + + + + + + + + + + + + + + + - - + + + + + + + + + + + + + + - - - + + - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - - - - - - - - - - - - - - - - + + + - - + + + + + + + + + + - - - + + + + + + + + + + + + + + + + @@ -268,10 +321,12 @@ editable="true"> - + + diff --git a/mep3_behavior/strategies/big_strategy_simple_green_a.xml b/mep3_behavior/strategies/big_strategy_simple_green_a.xml new file mode 100644 index 000000000..2b6b39beb --- /dev/null +++ b/mep3_behavior/strategies/big_strategy_simple_green_a.xml @@ -0,0 +1,340 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + points scored, can be negative + unique task name + + + + + + + + + + + + + 0 = unarmed; 1 = armed; 2 = started + + + + diff --git a/mep3_behavior/strategies/big_strategy_simple_green_s.xml b/mep3_behavior/strategies/big_strategy_simple_green_s.xml new file mode 100644 index 000000000..b734071b3 --- /dev/null +++ b/mep3_behavior/strategies/big_strategy_simple_green_s.xml @@ -0,0 +1,416 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + points scored, can be negative + unique task name + + + + + + + + + + + + + 0 = unarmed; 1 = armed; 2 = started + + + + diff --git a/mep3_behavior/strategies/big_strategy_simple_s.xml b/mep3_behavior/strategies/big_strategy_simple_s.xml new file mode 100644 index 000000000..62864b453 --- /dev/null +++ b/mep3_behavior/strategies/big_strategy_simple_s.xml @@ -0,0 +1,417 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + points scored, can be negative + unique task name + + + + + + + + + + + + + 0 = unarmed; 1 = armed; 2 = started + + + + diff --git a/mep3_behavior/strategies/common.xml b/mep3_behavior/strategies/common.xml index 756244baa..47fd8b9f4 100644 --- a/mep3_behavior/strategies/common.xml +++ b/mep3_behavior/strategies/common.xml @@ -3,33 +3,45 @@ - + + + + + + + + + + + + ignore_obstacles="false" + max_velocity="0.8" + max_acceleration="0.5"/> + ignore_obstacles="false" + max_velocity="0.8" + max_acceleration="0.5"/> - + @@ -58,10 +70,12 @@ editable="true"> - + + diff --git a/mep3_behavior/strategies/detection.xml b/mep3_behavior/strategies/detection.xml index 52135b709..4b06c6571 100644 --- a/mep3_behavior/strategies/detection.xml +++ b/mep3_behavior/strategies/detection.xml @@ -2,12 +2,12 @@ - + diff --git a/mep3_behavior/strategies/duvanje.xml b/mep3_behavior/strategies/duvanje.xml new file mode 100644 index 000000000..26395b1e9 --- /dev/null +++ b/mep3_behavior/strategies/duvanje.xml @@ -0,0 +1,33 @@ + + + + + + + + + + + + + + + + + + For example, 0x12345 + For example, 0x32;0x34 + + + + + + + diff --git a/mep3_behavior/strategies/funny_blue.xml b/mep3_behavior/strategies/funny_blue.xml new file mode 100644 index 000000000..bedab4731 --- /dev/null +++ b/mep3_behavior/strategies/funny_blue.xml @@ -0,0 +1,138 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + For example, 0x12345 + For example, 0x32;0x34 + + + + + + + + + + + + + + + + + + + + points scored, can be negative + unique task name + + + + + + + 0 = unarmed; 1 = armed; 2 = started + + + + diff --git a/mep3_behavior/strategies/funny_green.xml b/mep3_behavior/strategies/funny_green.xml new file mode 100644 index 000000000..0a4a78810 --- /dev/null +++ b/mep3_behavior/strategies/funny_green.xml @@ -0,0 +1,138 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + For example, 0x12345 + For example, 0x32;0x34 + + + + + + + + + + + + + + + + + + + + points scored, can be negative + unique task name + + + + + + + 0 = unarmed; 1 = armed; 2 = started + + + + diff --git a/mep3_behavior/strategies/gentle_attack.xml b/mep3_behavior/strategies/gentle_attack.xml new file mode 100644 index 000000000..3a14b9468 --- /dev/null +++ b/mep3_behavior/strategies/gentle_attack.xml @@ -0,0 +1,59 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/mep3_behavior/strategies/izduvavanje.xml b/mep3_behavior/strategies/izduvavanje.xml new file mode 100644 index 000000000..0e0b09f46 --- /dev/null +++ b/mep3_behavior/strategies/izduvavanje.xml @@ -0,0 +1,41 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/mep3_behavior/strategies/move_homologation.xml b/mep3_behavior/strategies/move_homologation.xml index f71eac340..ab67f68d4 100644 --- a/mep3_behavior/strategies/move_homologation.xml +++ b/mep3_behavior/strategies/move_homologation.xml @@ -2,12 +2,12 @@ - + diff --git a/mep3_behavior/strategies/move_wait.xml b/mep3_behavior/strategies/move_wait.xml index 94f9fc713..9a09be61e 100644 --- a/mep3_behavior/strategies/move_wait.xml +++ b/mep3_behavior/strategies/move_wait.xml @@ -6,8 +6,8 @@ diff --git a/mep3_behavior/strategies/project.btproj b/mep3_behavior/strategies/project.btproj index 1ab7a152c..fb611c6e7 100644 --- a/mep3_behavior/strategies/project.btproj +++ b/mep3_behavior/strategies/project.btproj @@ -3,19 +3,33 @@ + + + + + + + + + + + + + + @@ -30,9 +44,13 @@ + + + + @@ -94,8 +112,9 @@ - + + diff --git a/mep3_behavior/strategies/skill_basket_blue.xml b/mep3_behavior/strategies/skill_basket_blue.xml index c94ce3cf0..caf4f7e2f 100644 --- a/mep3_behavior/strategies/skill_basket_blue.xml +++ b/mep3_behavior/strategies/skill_basket_blue.xml @@ -4,18 +4,15 @@ - - - + - + - - - - + + + + + + + + + + + + + + + + + + + @@ -78,10 +101,12 @@ editable="true"> - + + diff --git a/mep3_behavior/strategies/skill_basket_green.xml b/mep3_behavior/strategies/skill_basket_green.xml new file mode 100644 index 000000000..7583b1361 --- /dev/null +++ b/mep3_behavior/strategies/skill_basket_green.xml @@ -0,0 +1,117 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + For example, 0x12345 + For example, 0x32;0x34 + + + + + + + + + + + + + + + + + + + + + + + diff --git a/mep3_behavior/strategies/skill_lift_cake.xml b/mep3_behavior/strategies/skill_lift_cake.xml index cc7515d42..0320949eb 100644 --- a/mep3_behavior/strategies/skill_lift_cake.xml +++ b/mep3_behavior/strategies/skill_lift_cake.xml @@ -40,7 +40,7 @@ - - + - - - + + + + + + + + + + + + + + + - - + + + + + + @@ -37,8 +54,31 @@ For example, 0x12345 For example, 0x32;0x34 + + + + + + + + + + + + + diff --git a/mep3_behavior/strategies/skill_take_cherry_green_start.xml b/mep3_behavior/strategies/skill_take_cherry_green_start.xml new file mode 100644 index 000000000..b70625c5e --- /dev/null +++ b/mep3_behavior/strategies/skill_take_cherry_green_start.xml @@ -0,0 +1,91 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + For example, 0x12345 + For example, 0x32;0x34 + + + + + + + + + + + + + + + + + + + + + diff --git a/mep3_behavior/strategies/skill_take_side_cherry.xml b/mep3_behavior/strategies/skill_take_side_cherry.xml index 7c4ef9d8c..7dc3cf143 100644 --- a/mep3_behavior/strategies/skill_take_side_cherry.xml +++ b/mep3_behavior/strategies/skill_take_side_cherry.xml @@ -2,20 +2,42 @@ + + goal="0.26;1.0;90"/> + + message="251"/> - - + + + + + + + + + + + + @@ -31,11 +53,39 @@ For example, 0x12345 For example, 0x32;0x34 + + + + + + + + + + + + + + + + diff --git a/mep3_behavior/strategies/skill_take_side_cherry_green.xml b/mep3_behavior/strategies/skill_take_side_cherry_green.xml new file mode 100644 index 000000000..b5d2689c0 --- /dev/null +++ b/mep3_behavior/strategies/skill_take_side_cherry_green.xml @@ -0,0 +1,112 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + For example, 0x12345 + For example, 0x32;0x34 + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/mep3_behavior/strategies/small_blue_simple.xml b/mep3_behavior/strategies/small_blue_simple.xml index f8d770611..47b30288b 100644 --- a/mep3_behavior/strategies/small_blue_simple.xml +++ b/mep3_behavior/strategies/small_blue_simple.xml @@ -2,12 +2,10 @@ - - + - - + + + + - - + @@ -56,7 +59,7 @@ + goal="-0.50;-0.23;90"/> @@ -72,8 +75,6 @@ message="1"/> - @@ -127,6 +128,17 @@ + + + + + + diff --git a/mep3_behavior/strategies/small_blue_simple_s.xml b/mep3_behavior/strategies/small_blue_simple_s.xml new file mode 100644 index 000000000..d903f2b45 --- /dev/null +++ b/mep3_behavior/strategies/small_blue_simple_s.xml @@ -0,0 +1,174 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + For example, 0x12345 + For example, 0x32;0x34 + + + + + + + + + + + + + + + + + + + + + + + + points scored, can be negative + unique task name + + + + + + + + + + + + + 0 = unarmed; 1 = armed; 2 = started + + + + diff --git a/mep3_behavior/strategies/small_green_simple.xml b/mep3_behavior/strategies/small_green_simple.xml index 01ce141e1..c05d25dcb 100644 --- a/mep3_behavior/strategies/small_green_simple.xml +++ b/mep3_behavior/strategies/small_green_simple.xml @@ -2,12 +2,10 @@ - - + - - + + + + - - + @@ -56,7 +59,7 @@ + goal="0.50;-0.23;90"/> @@ -72,8 +75,6 @@ message="1"/> - @@ -127,6 +128,17 @@ + + + + + + diff --git a/mep3_behavior/strategies/small_green_simple_s.xml b/mep3_behavior/strategies/small_green_simple_s.xml new file mode 100644 index 000000000..5bdac9123 --- /dev/null +++ b/mep3_behavior/strategies/small_green_simple_s.xml @@ -0,0 +1,173 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + For example, 0x12345 + For example, 0x32;0x34 + + + + + + + + + + + + + + + + + + + + + + + + points scored, can be negative + unique task name + + + + + + + + + + + + + 0 = unarmed; 1 = armed; 2 = started + + + + diff --git a/mep3_behavior/strategies/small_homologation.xml b/mep3_behavior/strategies/small_homologation.xml index 7bca51574..a25cb0cb5 100644 --- a/mep3_behavior/strategies/small_homologation.xml +++ b/mep3_behavior/strategies/small_homologation.xml @@ -1,31 +1,46 @@ - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -41,10 +56,12 @@ editable="true"> - + + diff --git a/mep3_behavior/strategies/small_strategy.xml b/mep3_behavior/strategies/small_strategy.xml index 9a9eaf5d7..2bdc7a729 100644 --- a/mep3_behavior/strategies/small_strategy.xml +++ b/mep3_behavior/strategies/small_strategy.xml @@ -2,12 +2,10 @@ - - - - - - - + + + + + + + + + + + + + + + goal="-0.55; -0.58;-90"/> @@ -63,15 +73,13 @@ + - - @@ -113,6 +121,17 @@ + + + + + + diff --git a/mep3_behavior/strategies/small_strategy_green.xml b/mep3_behavior/strategies/small_strategy_green.xml index ac25aec14..9cdd7a87a 100644 --- a/mep3_behavior/strategies/small_strategy_green.xml +++ b/mep3_behavior/strategies/small_strategy_green.xml @@ -2,23 +2,145 @@ - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + For example, 0x12345 + For example, 0x32;0x34 + + + + + + + + + + + + + + + + + points scored, can be negative + unique task name + + + + + + + + + + 0 = unarmed; 1 = armed; 2 = started + diff --git a/mep3_bringup/launch/robot_launch.py b/mep3_bringup/launch/robot_launch.py index 8c5b8a9ad..13964b35c 100644 --- a/mep3_bringup/launch/robot_launch.py +++ b/mep3_bringup/launch/robot_launch.py @@ -22,9 +22,14 @@ INITIAL_POSE_MATRIX = [ ('big', 'blue', [-0.65, -0.43, 0]), - ('small', 'blue', [0.74, 0.28, -pi/2]), + ('small', 'blue', [-0.69, -0.405, -pi/2]), ('big', 'green', [-0.65, 0.43, 0]), - ('small', 'green', [-0.74, 0.28, -pi/2]), + ('small', 'blue_1', [0.69, -0.405, -pi/2]), + + ('big', 'blue_a', [0.83, 0.43, -pi]), + ('small', 'blue_a', [-0.80, 1.34, pi/2]), + ('big', 'green_a', [0.83, -0.43, -pi]), + ('small', 'blue_2', [0.80, 1.34, pi/2]), ] PREDEFINED_TABLE_NAMES = [ 'table1', @@ -47,7 +52,6 @@ def get_initial_pose_transform(namespace, color): row_namespace = row[0] row_color = row[1] row_pose = row[2] - transforms.append( Node(package='tf2_ros', executable='static_transform_publisher', @@ -84,7 +88,7 @@ def launch_setup(context, *args, **kwargs): color = LaunchConfiguration('color') table = LaunchConfiguration('table', default='') should_live_reload = ('live' in strategy.perform(context)) - if color.perform(context) not in ['blue', 'green']: + if color.perform(context) not in ['blue', 'green', 'blue_a', 'blue_2', 'blue_1', 'green_a']: print('ERROR: The `color` parameter must be either `blue` or `green`.') sys.exit(1) if namespace.perform(context) not in ['big', 'small']: diff --git a/mep3_hardware/mep3_hardware/box_driver.py b/mep3_hardware/mep3_hardware/box_driver.py index 70bcae565..d9a63a8ae 100755 --- a/mep3_hardware/mep3_hardware/box_driver.py +++ b/mep3_hardware/mep3_hardware/box_driver.py @@ -61,8 +61,8 @@ def publish_scoreboard(self): self.points = points self.publisher.publish(scoreboard_msg) self.get_logger().info('Published scoreboard: task=%s, points=%d' % (scoreboard_msg.task, scoreboard_msg.points)) - if not self.has_balls_in_box: - self.has_balls_in_box = True + if not self.balls_in_box: + self.balls_in_box = True scoreboard_msg = Scoreboard() scoreboard_msg.task = 'box_correct_prediction_5_points' scoreboard_msg.points = 5 diff --git a/mep3_hardware/resource/big_controllers.yaml b/mep3_hardware/resource/big_controllers.yaml index 6edb865ad..610304af2 100644 --- a/mep3_hardware/resource/big_controllers.yaml +++ b/mep3_hardware/resource/big_controllers.yaml @@ -38,8 +38,8 @@ big: odom_frame_id: odom base_frame_id: base_link - wheel_separation: 0.3175 - wheel_radius: 0.04080 + wheel_separation: 0.3176 + wheel_radius: 0.04075 right_wheel_radius_multiplier: 1.0 linear.x.has_velocity_limits: true diff --git a/mep3_hardware/resource/small_description.urdf b/mep3_hardware/resource/small_description.urdf index a2ac62925..6703cf32a 100644 --- a/mep3_hardware/resource/small_description.urdf +++ b/mep3_hardware/resource/small_description.urdf @@ -38,5 +38,21 @@ + + 2 + + + + + + + + 11 + + + + + + diff --git a/mep3_navigation/params/nav2_params_big.yaml b/mep3_navigation/params/nav2_params_big.yaml index 8e371511b..1470a11fb 100644 --- a/mep3_navigation/params/nav2_params_big.yaml +++ b/mep3_navigation/params/nav2_params_big.yaml @@ -104,7 +104,7 @@ obstacle_min_range: 0.0 dilation_layer: plugin: "mep3_navigation::DilationLayer" - size: 0.18 + size: 0.20 type: 2 always_send_full_costmap: True @@ -123,7 +123,7 @@ plugins: ["obstacle_layer", "dilation_layer", "temporal_obstacle_layer", "inflation_layer"] dilation_layer: plugin: "mep3_navigation::DilationLayer" - size: 0.18 + size: 0.20 type: 2 obstacle_layer: plugin: "nav2_costmap_2d::ObstacleLayer" @@ -156,8 +156,8 @@ side_right: '[[1.05, -1.55], [-1.05, -1.55], [-1.05, -1.75], [1.05, -1.75]]' cherry_dispenser_top: '[[-0.05, 1.5], [0.05, 1.5], [0.05, 1.15], [-0.05, 1.15]]' cherry_dispenser_bottom: '[[-0.05, -1.5], [0.05, -1.5], [0.05, -1.15], [-0.05, -1.15]]' - #cherry_dispenser_left: '[[-0.95, 0.1], [-0.85, 0.1], [-0.95, -0.1], [-0.95, 0.1]]' - #cherry_dispenser_right: '[[0.95, 0.1], [0.85, 0.1], [0.95, -0.1], [0.95, 0.1]]' + cherry_dispenser_left: '[[-0.975, 0.1], [-0.975, 0.1], [-0.975, -0.1], [-0.975, 0.1]]' + cherry_dispenser_right: '[[0.975, 0.1], [0.975, 0.1], [0.975, -0.1], [0.975, 0.1]]' always_send_full_costmap: True /big/planner_server: diff --git a/mep3_navigation/params/nav2_params_small.yaml b/mep3_navigation/params/nav2_params_small.yaml index 10d304753..3ccb4e860 100644 --- a/mep3_navigation/params/nav2_params_small.yaml +++ b/mep3_navigation/params/nav2_params_small.yaml @@ -83,7 +83,7 @@ width: 3 height: 3 resolution: 0.05 - robot_radius: 0.17 + robot_radius: 0.20 use_maximum: true plugins: ["obstacle_layer", "dilation_layer"] obstacle_layer: @@ -104,7 +104,7 @@ obstacle_min_range: 0.0 dilation_layer: plugin: "mep3_navigation::DilationLayer" - size: 0.18 + size: 0.25 type: 2 always_send_full_costmap: True @@ -123,7 +123,7 @@ plugins: ["obstacle_layer", "dilation_layer", "temporal_obstacle_layer", "inflation_layer"] dilation_layer: plugin: "mep3_navigation::DilationLayer" - size: 0.17 + size: 0.25 type: 2 obstacle_layer: plugin: "nav2_costmap_2d::ObstacleLayer" diff --git a/mep3_navigation/src/dilation_layer/dilation_layer.cpp b/mep3_navigation/src/dilation_layer/dilation_layer.cpp index c38826a56..9d5e1448c 100644 --- a/mep3_navigation/src/dilation_layer/dilation_layer.cpp +++ b/mep3_navigation/src/dilation_layer/dilation_layer.cpp @@ -68,7 +68,6 @@ namespace mep3_navigation kernel_ = cv::getStructuringElement(type_, cv::Size(size_ / master_grid.getResolution(), size_ / master_grid.getResolution())); kernel_initialized_ = true; } - cv::Mat mat(master_grid.getSizeInCellsY(), master_grid.getSizeInCellsX(), CV_8UC1, master_grid.getCharMap()); cv::dilate(mat, mat, kernel_); }