From 3cebe0bdaa6fa0ed4753ab59c9fecc911370ea2f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marija=20Golubovi=C4=87?= <93673097+MarijaGolubovic@users.noreply.github.com> Date: Fri, 26 May 2023 09:45:27 +0200 Subject: [PATCH] Bug fix and new features (#280) * Fix blue agresive strategy * Add ignore obstacles in translate * Add parametar in groot * Add new start positions * Add new start positions * Print map resolution anddelete print for table params * Parse color for different start fields * Fix start positions * Add gentle attack * Fix gentle attack * Impruve old startegy with new features * Fix big strategy, replace move stack by other move comand * Fix big blue startegy and add stack * Add post and pri conditions * Fix green startegy * Try fix Client requested to cancel the goal * Fix box driver * Fix green strategy, disable detection on the plate * Fix and test blue strategy, disable detection on the plate * Try implement agresive strategy * Fix agresive strategy * Try impleement big agresive staregy * Fix finish cordinate * Try impleement green strategy * Fix green agresive strategy * Try implement big green agresive straetgy * Fix green side for agresive staretgy * Try implement big blue simple safe strategy(put cake on close plate) * Calibrate small robot * Change start point for small robot * Add ax in small description * Fix small startegy * Fix small staretgy for blue side * Try implement green side for smalll * Add blue_1 start field and implement pick cherry green start * Try implement all small straetgy for green side * Fix homologation strategy * Fix big homologation and safe big blue * Fix safe blue staretgy for big robot * Implement green safe staregy-first part * Implement green safe staregy-first part-fix * Fix strategy * Implement green safe staregy * Try fix staryegy in vs code * Fix staretgy * Fix safe staretgy * Implement green small staretgy for small * Fix safe blue strategy * Fix small staretgy for bouth side and chaneg launch pi for new small start position * Cnegne robot radiius for small 18->25 * Fix green staretgy for big safe startegy * Add base for smal safe startegy * Try to fix blue side * Go backweard when put first set of cakes * Fix both staretgy * Fix strategy, fix lifting lift brown cake * Fix agresive strategy for blue seide, fix position cake taking * Delete points for basket and change cordinates for waiting points * Fix blue agresive strategy * Fix green agresive strategy and add green_a in robot_launch.py * Implement safe straetgy for blue side * Add patern for turbin * Fix small sefe staretgy, add ramp for turbin * Change robot radius for small robot * Fix small waiting time and cordinates for big * Fix safe staretgy for small * Fix safe stareygy for big * Funny action blue * Fix cordinates for funny * Add green funny * Fix funny staretgys * Fix time for strategy * Add cordinatest for small robot in robot_launch.py * Add dispensers for big * Fix dispenser for big * Fix points for big and safe for small * Fix points for agresive * Fix cordinates for dispenser for small safe strategy * Fix finish points small * Fix staregy for smalla agresive --- .../include/mep3_behavior/bt_action_node.hpp | 36 +- .../include/mep3_behavior/move_action.hpp | 11 +- mep3_behavior/src/mep3_behavior_tree.cpp | 13 +- mep3_behavior/strategies/big_blue.xml | 103 +++- mep3_behavior/strategies/big_homologation.xml | 18 +- mep3_behavior/strategies/big_strategy.xml | 179 ++++--- mep3_behavior/strategies/big_strategy_a.xml | 294 +++++++++++ .../strategies/big_strategy_green.xml | 199 ++++---- .../strategies/big_strategy_green_a.xml | 284 +++++++++++ .../strategies/big_strategy_green_s.xml | 267 ++++++++++ mep3_behavior/strategies/big_strategy_s.xml | 276 ++++++++++ .../strategies/big_strategy_simple.xml | 478 ++++++++++-------- .../strategies/big_strategy_simple_a.xml | 351 +++++++++++++ .../strategies/big_strategy_simple_green.xml | 415 ++++++++------- .../big_strategy_simple_green_a.xml | 340 +++++++++++++ .../big_strategy_simple_green_s.xml | 416 +++++++++++++++ .../strategies/big_strategy_simple_s.xml | 417 +++++++++++++++ mep3_behavior/strategies/common.xml | 42 +- mep3_behavior/strategies/detection.xml | 6 +- mep3_behavior/strategies/duvanje.xml | 33 ++ mep3_behavior/strategies/funny_blue.xml | 138 +++++ mep3_behavior/strategies/funny_green.xml | 138 +++++ mep3_behavior/strategies/gentle_attack.xml | 59 +++ mep3_behavior/strategies/izduvavanje.xml | 41 ++ .../strategies/move_homologation.xml | 8 +- mep3_behavior/strategies/move_wait.xml | 4 +- mep3_behavior/strategies/project.btproj | 21 +- .../strategies/skill_basket_blue.xml | 81 ++- .../strategies/skill_basket_green.xml | 117 +++++ mep3_behavior/strategies/skill_lift_cake.xml | 2 +- .../strategies/skill_lift_cake_right.xml | 2 +- .../strategies/skill_put_one_cherry.xml | 4 +- .../skill_take_cherry_blue_start.xml | 72 ++- .../skill_take_cherry_green_start.xml | 91 ++++ .../strategies/skill_take_side_cherry.xml | 66 ++- .../skill_take_side_cherry_green.xml | 112 ++++ .../strategies/small_blue_simple.xml | 36 +- .../strategies/small_blue_simple_s.xml | 174 +++++++ .../strategies/small_green_simple.xml | 36 +- .../strategies/small_green_simple_s.xml | 173 +++++++ .../strategies/small_homologation.xml | 71 ++- mep3_behavior/strategies/small_strategy.xml | 63 ++- .../strategies/small_strategy_green.xml | 138 ++++- mep3_bringup/launch/robot_launch.py | 12 +- mep3_hardware/mep3_hardware/box_driver.py | 4 +- mep3_hardware/resource/big_controllers.yaml | 4 +- mep3_hardware/resource/small_description.urdf | 16 + mep3_navigation/params/nav2_params_big.yaml | 8 +- mep3_navigation/params/nav2_params_small.yaml | 6 +- .../src/dilation_layer/dilation_layer.cpp | 1 - 50 files changed, 5135 insertions(+), 741 deletions(-) create mode 100644 mep3_behavior/strategies/big_strategy_a.xml create mode 100644 mep3_behavior/strategies/big_strategy_green_a.xml create mode 100644 mep3_behavior/strategies/big_strategy_green_s.xml create mode 100644 mep3_behavior/strategies/big_strategy_s.xml create mode 100644 mep3_behavior/strategies/big_strategy_simple_a.xml create mode 100644 mep3_behavior/strategies/big_strategy_simple_green_a.xml create mode 100644 mep3_behavior/strategies/big_strategy_simple_green_s.xml create mode 100644 mep3_behavior/strategies/big_strategy_simple_s.xml create mode 100644 mep3_behavior/strategies/duvanje.xml create mode 100644 mep3_behavior/strategies/funny_blue.xml create mode 100644 mep3_behavior/strategies/funny_green.xml create mode 100644 mep3_behavior/strategies/gentle_attack.xml create mode 100644 mep3_behavior/strategies/izduvavanje.xml create mode 100644 mep3_behavior/strategies/skill_basket_green.xml create mode 100644 mep3_behavior/strategies/skill_take_cherry_green_start.xml create mode 100644 mep3_behavior/strategies/skill_take_side_cherry_green.xml create mode 100644 mep3_behavior/strategies/small_blue_simple_s.xml create mode 100644 mep3_behavior/strategies/small_green_simple_s.xml 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_); }