From ae906e733484cf25e0692e328938daa947eb101b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marija=20Golubovi=C4=87?= Date: Tue, 16 Apr 2024 18:46:17 +0200 Subject: [PATCH 1/8] Implement node for following path action --- .../mep3_behavior/navigate_to_action.hpp | 115 +++++++++++++++--- mep3_behavior/src/mep3_behavior_tree.cpp | 1 + 2 files changed, 100 insertions(+), 16 deletions(-) diff --git a/mep3_behavior/include/mep3_behavior/navigate_to_action.hpp b/mep3_behavior/include/mep3_behavior/navigate_to_action.hpp index 984f15dc3..d04ddb941 100644 --- a/mep3_behavior/include/mep3_behavior/navigate_to_action.hpp +++ b/mep3_behavior/include/mep3_behavior/navigate_to_action.hpp @@ -18,6 +18,7 @@ #include #include #include +#include #include "behaviortree_cpp/behavior_tree.h" #include "behaviortree_cpp/bt_factory.h" @@ -25,11 +26,13 @@ #include "mep3_behavior/pose_2d.hpp" #include "mep3_behavior/blackboard.hpp" #include "nav2_msgs/action/navigate_to_pose.hpp" +#include "nav2_msgs/action/follow_path.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" namespace mep3_behavior { class NavigateToAction - : public BT::RosActionNode + : public BT::RosActionNode { public: NavigateToAction(const std::string &name, @@ -40,36 +43,35 @@ namespace mep3_behavior { if (!getInput("goal", target_pose_)) throw BT::RuntimeError( - "Navigate action requires 'goal' argument" - ); + "Navigate action requires 'goal' argument"); getInput("behavior_tree", behavior_tree_); std::string table = this->config().blackboard->get("table"); BT::Pose2D goal_offset; - if (table.length() > 0 && getInput("goal_" + table, goal_offset)) { + if (table.length() > 0 && getInput("goal_" + table, goal_offset)) + { target_pose_ += goal_offset; } BT::TeamColor color = this->config().blackboard->get("color"); - if (color == BT::TeamColor::GREEN) { + if (color == BT::TeamColor::GREEN) + { target_pose_ = BT::mirrorPose(target_pose_); } - } static BT::PortsList providedPorts() { // Static parameters BT::PortsList port_list = { - BT::InputPort("goal"), - BT::InputPort("behavior_tree") - }; + BT::InputPort("goal"), + BT::InputPort("behavior_tree")}; // Dynamic parameters - for (std::string table : BT::SharedBlackboard::access()->get>("predefined_tables")) { + for (std::string table : BT::SharedBlackboard::access()->get>("predefined_tables")) + { port_list.insert( - BT::InputPort("goal_" + table) - ); + BT::InputPort("goal_" + table)); } return port_list; @@ -77,9 +79,9 @@ namespace mep3_behavior bool setGoal(Goal &goal) override { - std::cout << "Navigating to x=" << target_pose_.x \ - << " y=" << target_pose_.y \ - << " θ=" << target_pose_.theta << "°" << std::endl; + std::cout << "Navigating to x=" << target_pose_.x + << " y=" << target_pose_.y + << " θ=" << target_pose_.theta << "°" << std::endl; goal.pose.header.frame_id = "map"; goal.pose.header.stamp = node_->get_clock()->now(); @@ -98,7 +100,7 @@ namespace mep3_behavior return true; } - BT::NodeStatus onResultReceived(const WrappedResult &/*wr*/) override + BT::NodeStatus onResultReceived(const WrappedResult & /*wr*/) override { return BT::NodeStatus::SUCCESS; } @@ -108,6 +110,87 @@ namespace mep3_behavior std::string behavior_tree_; }; + class FollowPathAction + : public BT::RosActionNode + { + public: + FollowPathAction(const std::string &name, + const BT::NodeConfiguration &conf, + const BT::ActionNodeParams ¶ms, + typename std::shared_ptr action_client) + : BT::RosActionNode(name, conf, params, action_client) + { + if (!getInput("path", target_pose_)) + throw BT::RuntimeError( + "Follow path action requires 'path' argument"); + } + + static BT::PortsList providedPorts() + { + // Static parameters + BT::PortsList port_list = { + BT::InputPort("path")}; + + return port_list; + } + + bool setGoal(Goal &goal) override + { + // std::cout << "Foll to x=" << target_pose_.x + // << " y=" << target_pose_.y + // << " θ=" << target_pose_.theta << "°" << std::endl; + + // // Position + + std::istringstream iss(target_pose_); + std::string entry; + std::vector poses; + goal.path.header.frame_id = "map"; + goal.path.header.stamp = node_->get_clock()->now(); + + while (std::getline(iss, entry, '|')) + { + std::istringstream entryStream(entry); + std::string x, y, angle; + + std::getline(entryStream, x, ';'); + std::getline(entryStream, y, ';'); + std::getline(entryStream, angle, ';'); + double theta = std::stod(angle) * M_PI / 180.0; + auto pose_stamped = geometry_msgs::msg::PoseStamped(); + + pose_stamped.header.frame_id = "map"; + pose_stamped.header.stamp = node_->get_clock()->now(); + + pose_stamped.pose.position.x = std::stod(x); + pose_stamped.pose.position.y = std::stod(y); + pose_stamped.pose.position.z = 0.0; + + pose_stamped.pose.orientation.x = 0.0; + pose_stamped.pose.orientation.y = 0.0; + + // https://math.stackexchange.com/a/1972382 + pose_stamped.pose.orientation.z = std::sin(theta / 2.0); + pose_stamped.pose.orientation.w = std::cos(theta / 2.0); + + poses.push_back(pose_stamped); + + std::cout + << "x: " << x << " y: " << y << " theta: " << theta << std::endl; + } + goal.path.poses = poses; + return true; + } + + BT::NodeStatus onResultReceived(const WrappedResult & /*wr*/) override + { + return BT::NodeStatus::SUCCESS; + } + + private: + std::string target_pose_; + }; + } // namespace mep3_behavior #endif // MEP3_BEHAVIOR_TREE__NAVIGATE_TO_ACTION_HPP_ diff --git a/mep3_behavior/src/mep3_behavior_tree.cpp b/mep3_behavior/src/mep3_behavior_tree.cpp index 8db744322..103c9acf3 100644 --- a/mep3_behavior/src/mep3_behavior_tree.cpp +++ b/mep3_behavior/src/mep3_behavior_tree.cpp @@ -135,6 +135,7 @@ int main(int argc, char **argv) BT::RegisterRosAction(factory, "JointPosition", {node, "joint_position_command", std::chrono::seconds(30)}); BT::RegisterRosAction(factory, "Navigate", {node, "navigate_to_pose", std::chrono::seconds(30)}); + BT::RegisterRosAction(factory, "FollowPath", {node, "follow_path", std::chrono::seconds(30)}); BT::RegisterRosAction(factory, "Pump", {node, "pump", std::chrono::seconds(30)}); BT::RegisterRosAction(factory, "Translate", {node, "move/move", std::chrono::seconds(30)}); BT::RegisterRosAction(factory, "Rotate", {node, "move/move", std::chrono::seconds(30)}); From 7c33528d33e5f3c7dbb6f442e8fc06e374e8aad1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marija=20Golubovi=C4=87?= Date: Tue, 16 Apr 2024 18:54:17 +0200 Subject: [PATCH 2/8] change strategy in simulation --- mep3_bringup/launch/simulation_launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mep3_bringup/launch/simulation_launch.py b/mep3_bringup/launch/simulation_launch.py index 7571ae254..d95c62efb 100644 --- a/mep3_bringup/launch/simulation_launch.py +++ b/mep3_bringup/launch/simulation_launch.py @@ -14,7 +14,7 @@ def generate_launch_description(): use_behavior_tree = LaunchConfiguration('bt', default=True) - big_strategy = LaunchConfiguration('big_strategy', default='try_translate') + big_strategy = LaunchConfiguration('big_strategy', default='test') small_strategy = LaunchConfiguration('small_strategy', default='wait_everytime') color = LaunchConfiguration('color', default='blue') namespace = LaunchConfiguration('namespace', default='big') From a3882c366a99d7b7c56ba3121a6ad8759f4f1541 Mon Sep 17 00:00:00 2001 From: Marija Golubovic Date: Tue, 16 Apr 2024 20:43:00 +0000 Subject: [PATCH 3/8] add support for follow path in groot --- mep3_behavior/strategies/project.btproj | 3 ++ mep3_behavior/strategies/try_translate.xml | 44 ++++++++++++++-------- 2 files changed, 32 insertions(+), 15 deletions(-) diff --git a/mep3_behavior/strategies/project.btproj b/mep3_behavior/strategies/project.btproj index 840158fc2..8f68279c4 100644 --- a/mep3_behavior/strategies/project.btproj +++ b/mep3_behavior/strategies/project.btproj @@ -29,6 +29,9 @@ For example, 0x12345 For example, 0x32;0x34 + + + diff --git a/mep3_behavior/strategies/try_translate.xml b/mep3_behavior/strategies/try_translate.xml index c6ab7975e..db7e60d52 100644 --- a/mep3_behavior/strategies/try_translate.xml +++ b/mep3_behavior/strategies/try_translate.xml @@ -1,21 +1,31 @@ - - - - - - - - - - + + + + + + + + + + + + + + + @@ -282,6 +292,10 @@ + + + From a0ed640844875358ca5f3782fbd0e074c19e2916 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marija=20Golubovi=C4=87?= Date: Thu, 25 Apr 2024 22:06:45 +0200 Subject: [PATCH 4/8] fix simulation --- mep3_behavior/strategies/try_translate.xml | 30 +++++++++++++++++++ .../webots_data/controllers/judge/judge.py | 4 +-- .../webots_data/protos/StaticObjects.proto | 5 +--- .../webots_data/worlds/.eurobot.wbproj | 3 +- .../webots_data/worlds/eurobot.wbt | 8 ++--- 5 files changed, 39 insertions(+), 11 deletions(-) diff --git a/mep3_behavior/strategies/try_translate.xml b/mep3_behavior/strategies/try_translate.xml index db7e60d52..6ad6e55fa 100644 --- a/mep3_behavior/strategies/try_translate.xml +++ b/mep3_behavior/strategies/try_translate.xml @@ -26,6 +26,36 @@ + + + + + + + + + + + + + diff --git a/mep3_simulation/webots_data/controllers/judge/judge.py b/mep3_simulation/webots_data/controllers/judge/judge.py index b3c033504..9b05ba595 100644 --- a/mep3_simulation/webots_data/controllers/judge/judge.py +++ b/mep3_simulation/webots_data/controllers/judge/judge.py @@ -6,8 +6,8 @@ INITIAL_POSE_MATRIX = [ - ('big', 'blue', [0.03, -1.15, pi/2]), - ('small', 'blue', [0.72, -1.16, pi/2]), + ('big', 'blue', [0.815, -1.33, pi/2]), + ('small', 'blue', [-0.815, -1.33, pi/2]), ('big', 'green', [-0.72, 1.16, -pi/2]), ('small', 'green', [0.72, 1.16, -pi/2]), ] diff --git a/mep3_simulation/webots_data/protos/StaticObjects.proto b/mep3_simulation/webots_data/protos/StaticObjects.proto index eb2dac3af..4a44ad1a4 100644 --- a/mep3_simulation/webots_data/protos/StaticObjects.proto +++ b/mep3_simulation/webots_data/protos/StaticObjects.proto @@ -17,6 +17,7 @@ Group { contactMaterial "floor" size 2 3 tileSize 2 3 + rotation 0 0 1 3.1404 appearance PBRAppearance { baseColorMap ImageTexture { url [ @@ -32,10 +33,6 @@ Group { Walls {} FixedBeaconSupports {} CentralTrackingDevice {} - BasketSupport {} - BasketSupport { - teamColor "green" - } ] } } diff --git a/mep3_simulation/webots_data/worlds/.eurobot.wbproj b/mep3_simulation/webots_data/worlds/.eurobot.wbproj index f82f604c1..b44e8fcc7 100644 --- a/mep3_simulation/webots_data/worlds/.eurobot.wbproj +++ b/mep3_simulation/webots_data/worlds/.eurobot.wbproj @@ -1,10 +1,11 @@ Webots Project File version R2023b -perspectives: 000000ff00000000fd00000002000000010000011c00000307fc0200000001fb0000001400540065007800740045006400690074006f00720000000013000003070000003f00ffffff000000030000078000000039fc0100000001fb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c0100000000000007800000006900ffffff000007800000043300000001000000020000000100000008fc00000000 +perspectives: 000000ff00000000fd00000002000000010000011c00000307fc0200000001fb0000001400540065007800740045006400690074006f00720000000013000003070000003f00ffffff0000000300000780000000b6fc0100000001fb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c0100000000000007800000006900ffffff00000780000003b600000001000000020000000100000008fc00000000 simulationViewPerspectives: 000000ff000000010000000200000101000002c40100000002010000000100 sceneTreePerspectives: 000000ff00000001000000030000001c000000c0000000fa0100000002010000000200 maximizedDockId: -1 centralWidgetVisible: 1 orthographicViewHeight: 1 textFiles: -1 +globalOptionalRendering: CoordinateSystem consoles: Console:All:All renderingDevicePerspectives: camera_central:RasPi0;0;0.113021;0;0 diff --git a/mep3_simulation/webots_data/worlds/eurobot.wbt b/mep3_simulation/webots_data/worlds/eurobot.wbt index eed80cc85..0c45d3067 100644 --- a/mep3_simulation/webots_data/worlds/eurobot.wbt +++ b/mep3_simulation/webots_data/worlds/eurobot.wbt @@ -26,8 +26,8 @@ WorldInfo { ] } Viewpoint { - orientation 0.006249707430588943 0.9999401686365654 -0.008977767211780799 1.21630611235842 - position -1.8930923762430245 -0.048865452007669896 5.216846605544976 + orientation 0.4819430059392584 -0.005549339884766831 -0.8761849940812015 3.1538728487414 + position 2.988041384032802 0.28050982519477397 4.535328706366026 } TexturedBackground { } @@ -65,13 +65,13 @@ Robot { } DEF ROBOT_SMALL GenericRobot { name "robot_small" - translation 0.72 -1.16 0 + translation -0.75 -1.25 0 rotation 0 0 1 1.5708 arucoNumber 6 } DEF ROBOT_BIG GenericRobot { name "robot_big" - translation 0.09 -1.16 0 + translation 0.815 -1.33 0 rotation 0 0 1 1.5708 arucoNumber 7 baseSlot [ From 31fa9a1e1b331e796d26f0cb621e7820e54d158e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marija=20Golubovi=C4=87?= Date: Thu, 25 Apr 2024 22:10:06 +0200 Subject: [PATCH 5/8] fix simulation strategy --- mep3_behavior/strategies/try_translate.xml | 31 +--------------------- 1 file changed, 1 insertion(+), 30 deletions(-) diff --git a/mep3_behavior/strategies/try_translate.xml b/mep3_behavior/strategies/try_translate.xml index 6ad6e55fa..807dd709b 100644 --- a/mep3_behavior/strategies/try_translate.xml +++ b/mep3_behavior/strategies/try_translate.xml @@ -26,36 +26,7 @@ - - - - - - - - - - - - - + From 236bc8991473ec6d699c8da6695791b69775b78c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marija=20Golubovi=C4=87?= Date: Thu, 25 Apr 2024 22:13:23 +0200 Subject: [PATCH 6/8] fix simulation strategy --- mep3_behavior/strategies/try_translate.xml | 21 +++++++++------------ 1 file changed, 9 insertions(+), 12 deletions(-) diff --git a/mep3_behavior/strategies/try_translate.xml b/mep3_behavior/strategies/try_translate.xml index 807dd709b..99815cc6e 100644 --- a/mep3_behavior/strategies/try_translate.xml +++ b/mep3_behavior/strategies/try_translate.xml @@ -11,9 +11,7 @@ linear_velocity="0.5" linear_acceleration="0.8" enable_sensors="false"/> - - - + @@ -26,7 +24,6 @@ - @@ -219,17 +216,17 @@ + type="7" + goal="0.4;-0.5;-90"/> + type="7" + goal="-0.4;0.5;180"/> + type="7" + goal="-0.4;-0.5;0"/> + type="7" + goal="0.4;0.5;0"/> From 6a806d19f2e208312e71d6c45e65e51b9b0960a1 Mon Sep 17 00:00:00 2001 From: Marija Golubovic Date: Thu, 25 Apr 2024 20:45:10 +0000 Subject: [PATCH 7/8] calibrate robot and reduce speed for curve speed --- mep3_hardware/resource/big_controllers.yaml | 2 +- mep3_navigation/params/nav2_params_big.yaml | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/mep3_hardware/resource/big_controllers.yaml b/mep3_hardware/resource/big_controllers.yaml index f0de39c6d..3da139ec8 100644 --- a/mep3_hardware/resource/big_controllers.yaml +++ b/mep3_hardware/resource/big_controllers.yaml @@ -31,7 +31,7 @@ big: base_frame_id: base_link wheel_separation: 0.28670 - wheel_radius: 0.04053 + wheel_radius: 0.04051 right_wheel_radius_multiplier: 1.0 linear.x.has_velocity_limits: true diff --git a/mep3_navigation/params/nav2_params_big.yaml b/mep3_navigation/params/nav2_params_big.yaml index ea601cbcd..635d36f44 100644 --- a/mep3_navigation/params/nav2_params_big.yaml +++ b/mep3_navigation/params/nav2_params_big.yaml @@ -42,8 +42,8 @@ yaw_goal_tolerance: 0.05 FollowPath: plugin: "mep3_navigation::RegulatedPurePursuitController" - desired_linear_vel: 0.5 - max_linear_accel: 0.8 + desired_linear_vel: 0.35 + max_linear_accel: 0.6 max_linear_decel: 0.8 max_linear_jerk: 99999.0 max_angular_jerk: 99999.0 From b3f2aaf8d07fc56617e2f85715ab59a3602afc02 Mon Sep 17 00:00:00 2001 From: Marija Golubovic Date: Thu, 25 Apr 2024 23:56:10 +0000 Subject: [PATCH 8/8] calibrate robot and implement reversing move --- .../include/mep3_behavior/move_action.hpp | 5 + mep3_behavior/strategies/project.btproj | 1 + mep3_behavior/strategies/try_translate.xml | 127 +++++++++++------- mep3_bringup/launch/robot_launch.py | 5 +- mep3_hardware/resource/big_controllers.yaml | 4 +- mep3_navigation/launch/navigation_launch.py | 2 +- sensor.py | 55 ++++++++ 7 files changed, 146 insertions(+), 53 deletions(-) create mode 100644 sensor.py diff --git a/mep3_behavior/include/mep3_behavior/move_action.hpp b/mep3_behavior/include/mep3_behavior/move_action.hpp index c22cf081a..8d9734dbe 100644 --- a/mep3_behavior/include/mep3_behavior/move_action.hpp +++ b/mep3_behavior/include/mep3_behavior/move_action.hpp @@ -165,6 +165,7 @@ namespace mep3_behavior BT::InputPort("frame_id"), BT::InputPort("ignore_obstacles"), BT::InputPort("mode"), + BT::InputPort("reversing"), BT::OutputPort("error")}; } @@ -172,6 +173,7 @@ namespace mep3_behavior { double yaw_deg; int mode; + int reversing; std::string position; std::string token; @@ -183,6 +185,7 @@ namespace mep3_behavior getInput("angular_velocity", goal.angular_properties.max_velocity); getInput("angular_velocity", goal.angular_properties.max_acceleration); getInput("mode", mode); + getInput("reversing", reversing); std::istringstream iss(position); std::getline(iss, token, ';'); @@ -194,6 +197,7 @@ namespace mep3_behavior std::getline(iss, token, ';'); goal.target.theta = std::stod(token) * M_PI / 180.0; goal.mode = mode; + goal.reversing = reversing; std::cout << "RotateAction: setGoal" << std::endl; std::cout << " x: " << goal.target.x << std::endl; @@ -206,6 +210,7 @@ namespace mep3_behavior std::cout << " anguar_velocity: " << goal.angular_properties.max_velocity << std::endl; std::cout << " anguar_acceleration: " << goal.angular_properties.max_acceleration << std::endl; std::cout << " ignore_obstacles: " << goal.ignore_obstacles << std::endl; + std::cout << " reversing: " << goal.reversing << std::endl; return true; } diff --git a/mep3_behavior/strategies/project.btproj b/mep3_behavior/strategies/project.btproj index 8f68279c4..d82a16f39 100644 --- a/mep3_behavior/strategies/project.btproj +++ b/mep3_behavior/strategies/project.btproj @@ -51,6 +51,7 @@ + diff --git a/mep3_behavior/strategies/try_translate.xml b/mep3_behavior/strategies/try_translate.xml index 99815cc6e..ea111a0a8 100644 --- a/mep3_behavior/strategies/try_translate.xml +++ b/mep3_behavior/strategies/try_translate.xml @@ -4,24 +4,55 @@ + + + + + - - - - - - - + + + + + + + + + + @@ -53,7 +84,8 @@ angular_velocity="1.0" mode="7" linear_acceleration="0.8" - angular_acceleration="1.8"/> + angular_acceleration="1.8" + reversing="0"/> - - - + + - + angular_acceleration="1.8" + reversing="2"/> + + + @@ -209,26 +251,15 @@ - - - - - - - - - - + @@ -244,7 +275,8 @@ angular_velocity="1.0" mode="7" linear_acceleration="0.8" - angular_acceleration="1.8"/> + angular_acceleration="1.8" + reversing="0"/> + angular_acceleration="1.8" + reversing="0"/> + angular_acceleration="1.8" + reversing="0"/> + - - points scored, can be negative - unique task name -