From 3f4f83e25ea676fdca9cac4886963bff604d0261 Mon Sep 17 00:00:00 2001 From: acki Date: Wed, 17 Dec 2025 19:04:30 +0100 Subject: [PATCH 01/43] log --- src/modelec_com/src/pcb_odo_interface.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/modelec_com/src/pcb_odo_interface.cpp b/src/modelec_com/src/pcb_odo_interface.cpp index d535304..b5a4f62 100644 --- a/src/modelec_com/src/pcb_odo_interface.cpp +++ b/src/modelec_com/src/pcb_odo_interface.cpp @@ -201,6 +201,7 @@ namespace Modelec } else if (tokens[1] == "WAYPOINT") { + RCLCPP_INFO(this->get_logger(), "Waypoint added successfully."); } else if (tokens[1] == "PID") { @@ -255,9 +256,9 @@ namespace Modelec { if (IsOk()) { - // RCLCPP_INFO(this->get_logger(), "Sending to PCB: %s", trim(data).c_str()); - RCLCPP_INFO_ONCE(this->get_logger(), "Sending to PCB: %s", trim(data).c_str()); - RCLCPP_DEBUG_SKIPFIRST(this->get_logger(), "Sending to PCB: %s", data.c_str()); + RCLCPP_INFO(this->get_logger(), "Sending to PCB: %s", trim(data).c_str()); + // RCLCPP_INFO_ONCE(this->get_logger(), "Sending to PCB: %s", trim(data).c_str()); + // RCLCPP_DEBUG_SKIPFIRST(this->get_logger(), "Sending to PCB: %s", data.c_str()); this->write(data); } } From d4a9f97848bf444583ac3b7664fa0708317bd55a Mon Sep 17 00:00:00 2001 From: acki Date: Wed, 17 Dec 2025 19:18:00 +0100 Subject: [PATCH 02/43] log --- src/modelec_com/src/pcb_odo_interface.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modelec_com/src/pcb_odo_interface.cpp b/src/modelec_com/src/pcb_odo_interface.cpp index b5a4f62..4116371 100644 --- a/src/modelec_com/src/pcb_odo_interface.cpp +++ b/src/modelec_com/src/pcb_odo_interface.cpp @@ -170,6 +170,8 @@ namespace Modelec } else if (tokens[1] == "WAYPOINT") { + RCLCPP_INFO(this->get_logger(), "Waypoint reached: ID %s", tokens[2].c_str()); + int id = std::stoi(tokens[2]); auto message = modelec_interfaces::msg::OdometryWaypoint(); From ccde341c48e0bea3b4c5a6bbbafafe30912f8e56 Mon Sep 17 00:00:00 2001 From: acki Date: Wed, 17 Dec 2025 19:20:34 +0100 Subject: [PATCH 03/43] log --- src/modelec_strat/src/navigation_helper.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/modelec_strat/src/navigation_helper.cpp b/src/modelec_strat/src/navigation_helper.cpp index f61ae0d..7f8ba82 100644 --- a/src/modelec_strat/src/navigation_helper.cpp +++ b/src/modelec_strat/src/navigation_helper.cpp @@ -380,6 +380,8 @@ namespace Modelec auto p = Point(wl[0].x, wl[0].y, wl[0].theta); if (RotateTo(p)) { + RCLCPP_INFO(node_->get_logger(), "Rotating first before going to target"); + await_rotate_ = true; send_back_waypoints_.clear(); @@ -391,6 +393,8 @@ namespace Modelec } else { + RCLCPP_INFO(node_->get_logger(), "No need to rotate first, going directly to target"); + waypoints_.clear(); for (auto& w : wl) From 9696385a9e505633a36aae082a08155bedac3e3f Mon Sep 17 00:00:00 2001 From: acki Date: Wed, 17 Dec 2025 19:34:51 +0100 Subject: [PATCH 04/43] log --- src/modelec_com/src/pcb_odo_interface.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modelec_com/src/pcb_odo_interface.cpp b/src/modelec_com/src/pcb_odo_interface.cpp index 4116371..6a17ea9 100644 --- a/src/modelec_com/src/pcb_odo_interface.cpp +++ b/src/modelec_com/src/pcb_odo_interface.cpp @@ -117,9 +117,9 @@ namespace Modelec void PCBOdoInterface::read(const std::string& msg) { - // RCLCPP_INFO(this->get_logger(), "Received from PCB: %s", trim(msg).c_str()); - RCLCPP_INFO_ONCE(this->get_logger(), "Received from PCB: %s", trim(msg).c_str()); - RCLCPP_DEBUG_SKIPFIRST(this->get_logger(), "Received from PCB: %s", msg.c_str()); + RCLCPP_INFO(this->get_logger(), "Received from PCB: %s", trim(msg).c_str()); + // RCLCPP_INFO_ONCE(this->get_logger(), "Received from PCB: %s", trim(msg).c_str()); + // RCLCPP_DEBUG_SKIPFIRST(this->get_logger(), "Received from PCB: %s", msg.c_str()); std::vector tokens = split(trim(msg), ';'); if (tokens.size() < 2) { From c4a8c282fe20e60e9ec858fb9c573c4a4bdde893 Mon Sep 17 00:00:00 2001 From: acki Date: Wed, 17 Dec 2025 20:11:22 +0100 Subject: [PATCH 05/43] ask waypoint to pcb --- .../include/modelec_com/pcb_odo_interface.hpp | 1 + src/modelec_com/src/pcb_odo_interface.cpp | 41 ++++++++++++++++--- .../modelec_strat/navigation_helper.hpp | 5 ++- .../src/missions/free_mission.cpp | 5 +++ .../src/missions/go_home_mission.cpp | 5 +++ .../src/missions/take_mission.cpp | 7 +++- src/modelec_strat/src/navigation_helper.cpp | 12 ++++-- 7 files changed, 64 insertions(+), 12 deletions(-) diff --git a/src/modelec_com/include/modelec_com/pcb_odo_interface.hpp b/src/modelec_com/include/modelec_com/pcb_odo_interface.hpp index f5a5b09..4e5ab2e 100644 --- a/src/modelec_com/include/modelec_com/pcb_odo_interface.hpp +++ b/src/modelec_com/include/modelec_com/pcb_odo_interface.hpp @@ -56,6 +56,7 @@ namespace Modelec rclcpp::Subscription::SharedPtr odo_add_waypoints_subscriber_; rclcpp::Subscription::SharedPtr odo_set_pos_subscriber_; rclcpp::Subscription::SharedPtr odo_set_pid_subscriber_; + rclcpp::Subscription::SharedPtr odo_ask_active_waypoint_subscriber_; rclcpp::Subscription::SharedPtr joy_subscriber_; void AddWaypointCallback(const modelec_interfaces::msg::OdometryWaypoint::SharedPtr msg); diff --git a/src/modelec_com/src/pcb_odo_interface.cpp b/src/modelec_com/src/pcb_odo_interface.cpp index 6a17ea9..c74b777 100644 --- a/src/modelec_com/src/pcb_odo_interface.cpp +++ b/src/modelec_com/src/pcb_odo_interface.cpp @@ -69,6 +69,13 @@ namespace Modelec SetPIDCallback(msg); }); + odo_ask_active_waypoint_subscriber_ = this->create_subscription( + "odometry/ask_active_waypoint", 10, + [this](const std_msgs::msg::Empty::SharedPtr) + { + GetData("WAYPOINT", {"ACTIVE"}); + }); + joy_subscriber_ = this->create_subscription( "joy", 30, [this](const sensor_msgs::msg::Joy::SharedPtr msg) @@ -170,14 +177,38 @@ namespace Modelec } else if (tokens[1] == "WAYPOINT") { - RCLCPP_INFO(this->get_logger(), "Waypoint reached: ID %s", tokens[2].c_str()); + if (tokens[2] == "REACH") + { + RCLCPP_INFO(this->get_logger(), "Waypoint reached: ID %s", tokens[2].c_str()); - int id = std::stoi(tokens[2]); + int id = std::stoi(tokens[2]); - auto message = modelec_interfaces::msg::OdometryWaypoint(); - message.id = id; + auto message = modelec_interfaces::msg::OdometryWaypoint(); + message.id = id; - odo_waypoint_reach_publisher_->publish(message); + odo_waypoint_reach_publisher_->publish(message); + } + else if (tokens.size() >= 7) + { + int id = std::stoi(tokens[2]); + bool is_end = tokens[3] == "1"; + long x = std::stol(tokens[4]); + long y = std::stol(tokens[5]); + double theta = std::stod(tokens[6]); + bool reach = tokens.size() > 7 ? tokens[7] == "1" : false; + + if (reach) + { + auto message = modelec_interfaces::msg::OdometryWaypoint(); + message.id = id; + message.is_end = is_end; + message.x = x; + message.y = y; + message.theta = theta; + + odo_waypoint_reach_publisher_->publish(message); + } + } } else if (tokens[1] == "PID") { diff --git a/src/modelec_strat/include/modelec_strat/navigation_helper.hpp b/src/modelec_strat/include/modelec_strat/navigation_helper.hpp index 1932dab..c89b32e 100644 --- a/src/modelec_strat/include/modelec_strat/navigation_helper.hpp +++ b/src/modelec_strat/include/modelec_strat/navigation_helper.hpp @@ -113,6 +113,8 @@ namespace Modelec Point GetSpawn() const; + void AskWaypoint(); + protected: void OnWaypointReach(const WaypointMsg::SharedPtr msg); @@ -171,8 +173,7 @@ namespace Modelec rclcpp::Publisher::SharedPtr spawn_pub_; rclcpp::Service::SharedPtr ask_spawn_srv_; - rclcpp::Publisher::SharedPtr odo_get_pos_pub_; - rclcpp::Time last_odo_get_pos_time_; + rclcpp::Publisher::SharedPtr odo_ask_waypoint_pub_; }; diff --git a/src/modelec_strat/src/missions/free_mission.cpp b/src/modelec_strat/src/missions/free_mission.cpp index ac1d60e..eef69cd 100644 --- a/src/modelec_strat/src/missions/free_mission.cpp +++ b/src/modelec_strat/src/missions/free_mission.cpp @@ -24,6 +24,11 @@ namespace Modelec { if (!nav_->HasArrived()) { + if ((node_->now() - go_timeout_).seconds() < 2) + { + nav_->AskWaypoint(); + return; + } if ((node_->now() - go_timeout_).seconds() < 10) { return; diff --git a/src/modelec_strat/src/missions/go_home_mission.cpp b/src/modelec_strat/src/missions/go_home_mission.cpp index 7693465..356b64f 100644 --- a/src/modelec_strat/src/missions/go_home_mission.cpp +++ b/src/modelec_strat/src/missions/go_home_mission.cpp @@ -27,6 +27,11 @@ namespace Modelec { if (!nav_->HasArrived()) { + if ((node_->now() - go_timeout_).seconds() < 2) + { + nav_->AskWaypoint(); + return; + } if ((node_->now() - go_timeout_).seconds() < 10) { return; diff --git a/src/modelec_strat/src/missions/take_mission.cpp b/src/modelec_strat/src/missions/take_mission.cpp index 8d9eccb..dd16480 100644 --- a/src/modelec_strat/src/missions/take_mission.cpp +++ b/src/modelec_strat/src/missions/take_mission.cpp @@ -24,7 +24,12 @@ namespace Modelec { if (!nav_->HasArrived()) { - if ((node_->now() - go_timeout_).seconds() < 20) + if ((node_->now() - go_timeout_).seconds() < 2) + { + nav_->AskWaypoint(); + return; + } + if ((node_->now() - go_timeout_).seconds() < 10) { return; } diff --git a/src/modelec_strat/src/navigation_helper.cpp b/src/modelec_strat/src/navigation_helper.cpp index 7f8ba82..6ec2321 100644 --- a/src/modelec_strat/src/navigation_helper.cpp +++ b/src/modelec_strat/src/navigation_helper.cpp @@ -97,10 +97,8 @@ namespace Modelec } }); - odo_get_pos_pub_ = node_->create_publisher( - "odometry/get/pos", 30); - - last_odo_get_pos_time_ = node_->now(); + odo_ask_waypoint_pub_ = node_->create_publisher( + "odometry/ask_active_waypoint", 30); } void NavigationHelper::ReInit() @@ -724,6 +722,12 @@ namespace Modelec return spawn_; } + void NavigationHelper::AskWaypoint() + { + std_msgs::msg::Empty msg; + odo_ask_waypoint_pub_->publish(msg); + } + void NavigationHelper::OnWaypointReach(const WaypointMsg::SharedPtr msg) { for (auto& waypoint : waypoints_) From 83cd0f751175ed18dd1c7318ecb97697403c4017 Mon Sep 17 00:00:00 2001 From: acki Date: Wed, 17 Dec 2025 20:19:18 +0100 Subject: [PATCH 06/43] ask waypoint to pcb --- src/modelec_com/src/pcb_odo_interface.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/modelec_com/src/pcb_odo_interface.cpp b/src/modelec_com/src/pcb_odo_interface.cpp index c74b777..97a5e36 100644 --- a/src/modelec_com/src/pcb_odo_interface.cpp +++ b/src/modelec_com/src/pcb_odo_interface.cpp @@ -179,9 +179,9 @@ namespace Modelec { if (tokens[2] == "REACH") { - RCLCPP_INFO(this->get_logger(), "Waypoint reached: ID %s", tokens[2].c_str()); + RCLCPP_INFO(this->get_logger(), "Waypoint reached: ID %s", tokens[3].c_str()); - int id = std::stoi(tokens[2]); + int id = std::stoi(tokens[3]); auto message = modelec_interfaces::msg::OdometryWaypoint(); message.id = id; @@ -190,12 +190,12 @@ namespace Modelec } else if (tokens.size() >= 7) { - int id = std::stoi(tokens[2]); - bool is_end = tokens[3] == "1"; - long x = std::stol(tokens[4]); - long y = std::stol(tokens[5]); - double theta = std::stod(tokens[6]); - bool reach = tokens.size() > 7 ? tokens[7] == "1" : false; + int id = std::stoi(tokens[3]); + bool is_end = tokens[4] == "1"; + long x = std::stol(tokens[5]); + long y = std::stol(tokens[6]); + double theta = std::stod(tokens[7]); + bool reach = tokens.size() > 7 ? tokens[8] == "1" : false; if (reach) { From cdbf41ecddd740379aa3d6c2779f182236ed3915 Mon Sep 17 00:00:00 2001 From: acki Date: Wed, 17 Dec 2025 20:24:54 +0100 Subject: [PATCH 07/43] ask waypoint to pcb --- src/modelec_strat/src/missions/free_mission.cpp | 2 +- src/modelec_strat/src/missions/go_home_mission.cpp | 2 +- src/modelec_strat/src/missions/take_mission.cpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modelec_strat/src/missions/free_mission.cpp b/src/modelec_strat/src/missions/free_mission.cpp index eef69cd..a691449 100644 --- a/src/modelec_strat/src/missions/free_mission.cpp +++ b/src/modelec_strat/src/missions/free_mission.cpp @@ -26,7 +26,7 @@ namespace Modelec { { if ((node_->now() - go_timeout_).seconds() < 2) { - nav_->AskWaypoint(); + // nav_->AskWaypoint(); return; } if ((node_->now() - go_timeout_).seconds() < 10) diff --git a/src/modelec_strat/src/missions/go_home_mission.cpp b/src/modelec_strat/src/missions/go_home_mission.cpp index 356b64f..64b670a 100644 --- a/src/modelec_strat/src/missions/go_home_mission.cpp +++ b/src/modelec_strat/src/missions/go_home_mission.cpp @@ -29,7 +29,7 @@ namespace Modelec { if ((node_->now() - go_timeout_).seconds() < 2) { - nav_->AskWaypoint(); + // nav_->AskWaypoint(); return; } if ((node_->now() - go_timeout_).seconds() < 10) diff --git a/src/modelec_strat/src/missions/take_mission.cpp b/src/modelec_strat/src/missions/take_mission.cpp index dd16480..d2ddbcb 100644 --- a/src/modelec_strat/src/missions/take_mission.cpp +++ b/src/modelec_strat/src/missions/take_mission.cpp @@ -26,7 +26,7 @@ namespace Modelec { { if ((node_->now() - go_timeout_).seconds() < 2) { - nav_->AskWaypoint(); + // nav_->AskWaypoint(); return; } if ((node_->now() - go_timeout_).seconds() < 10) From 8858e6d9a4e96e27c9fbb9f71045aab3b79f17eb Mon Sep 17 00:00:00 2001 From: acki Date: Wed, 17 Dec 2025 20:54:09 +0100 Subject: [PATCH 08/43] log --- src/modelec_com/src/pcb_odo_interface.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/modelec_com/src/pcb_odo_interface.cpp b/src/modelec_com/src/pcb_odo_interface.cpp index 97a5e36..8c79f74 100644 --- a/src/modelec_com/src/pcb_odo_interface.cpp +++ b/src/modelec_com/src/pcb_odo_interface.cpp @@ -124,9 +124,9 @@ namespace Modelec void PCBOdoInterface::read(const std::string& msg) { - RCLCPP_INFO(this->get_logger(), "Received from PCB: %s", trim(msg).c_str()); - // RCLCPP_INFO_ONCE(this->get_logger(), "Received from PCB: %s", trim(msg).c_str()); - // RCLCPP_DEBUG_SKIPFIRST(this->get_logger(), "Received from PCB: %s", msg.c_str()); + // RCLCPP_INFO(this->get_logger(), "Received from PCB: %s", trim(msg).c_str()); + RCLCPP_INFO_ONCE(this->get_logger(), "Received from PCB: %s", trim(msg).c_str()); + RCLCPP_DEBUG_SKIPFIRST(this->get_logger(), "Received from PCB: %s", msg.c_str()); std::vector tokens = split(trim(msg), ';'); if (tokens.size() < 2) { @@ -179,7 +179,7 @@ namespace Modelec { if (tokens[2] == "REACH") { - RCLCPP_INFO(this->get_logger(), "Waypoint reached: ID %s", tokens[3].c_str()); + // RCLCPP_INFO(this->get_logger(), "Waypoint reached: ID %s", tokens[3].c_str()); int id = std::stoi(tokens[3]); @@ -234,7 +234,7 @@ namespace Modelec } else if (tokens[1] == "WAYPOINT") { - RCLCPP_INFO(this->get_logger(), "Waypoint added successfully."); + // RCLCPP_INFO(this->get_logger(), "Waypoint added successfully."); } else if (tokens[1] == "PID") { @@ -289,9 +289,9 @@ namespace Modelec { if (IsOk()) { - RCLCPP_INFO(this->get_logger(), "Sending to PCB: %s", trim(data).c_str()); - // RCLCPP_INFO_ONCE(this->get_logger(), "Sending to PCB: %s", trim(data).c_str()); - // RCLCPP_DEBUG_SKIPFIRST(this->get_logger(), "Sending to PCB: %s", data.c_str()); + // RCLCPP_INFO(this->get_logger(), "Sending to PCB: %s", trim(data).c_str()); + RCLCPP_INFO_ONCE(this->get_logger(), "Sending to PCB: %s", trim(data).c_str()); + RCLCPP_DEBUG_SKIPFIRST(this->get_logger(), "Sending to PCB: %s", data.c_str()); this->write(data); } } From f1e0a74185b72a984821c4bc8cfed60c2a2f4afc Mon Sep 17 00:00:00 2001 From: acki Date: Wed, 17 Dec 2025 20:58:28 +0100 Subject: [PATCH 09/43] log --- src/modelec_strat/src/navigation_helper.cpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/src/modelec_strat/src/navigation_helper.cpp b/src/modelec_strat/src/navigation_helper.cpp index 6ec2321..0dbed84 100644 --- a/src/modelec_strat/src/navigation_helper.cpp +++ b/src/modelec_strat/src/navigation_helper.cpp @@ -378,8 +378,6 @@ namespace Modelec auto p = Point(wl[0].x, wl[0].y, wl[0].theta); if (RotateTo(p)) { - RCLCPP_INFO(node_->get_logger(), "Rotating first before going to target"); - await_rotate_ = true; send_back_waypoints_.clear(); @@ -391,8 +389,6 @@ namespace Modelec } else { - RCLCPP_INFO(node_->get_logger(), "No need to rotate first, going directly to target"); - waypoints_.clear(); for (auto& w : wl) From c2711a153c29d64755db093418a76fd48c1ed63f Mon Sep 17 00:00:00 2001 From: acki Date: Thu, 18 Dec 2025 09:14:12 +0100 Subject: [PATCH 10/43] servo value --- src/modelec_strat/src/action/free_action.cpp | 4 ++-- src/modelec_strat/src/action/take_action.cpp | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modelec_strat/src/action/free_action.cpp b/src/modelec_strat/src/action/free_action.cpp index ca1b78d..e9443a8 100644 --- a/src/modelec_strat/src/action/free_action.cpp +++ b/src/modelec_strat/src/action/free_action.cpp @@ -38,8 +38,8 @@ void Modelec::FreeAction::Next() msg.items.resize(1); msg.items[0].id = n_ + (front_ ? 3 : 11); - msg.items[0].start_angle = front_ ? 3 : 0; - msg.items[0].end_angle = front_ ? 0 : 0; + msg.items[0].start_angle = front_ ? 2.5 : 0; + msg.items[0].end_angle = front_ ? 0.8 : 0; msg.items[0].duration_s = 0.5; action_executor_->MoveServoTimed(msg); } diff --git a/src/modelec_strat/src/action/take_action.cpp b/src/modelec_strat/src/action/take_action.cpp index 2b36272..c67978b 100644 --- a/src/modelec_strat/src/action/take_action.cpp +++ b/src/modelec_strat/src/action/take_action.cpp @@ -38,8 +38,8 @@ void Modelec::TakeAction::Next() msg.items.resize(1); msg.items[0].id = n_ + (front_ ? 3 : 11); - msg.items[0].start_angle = front_ ? 0 : 0; - msg.items[0].end_angle = front_ ? 3 : 0; + msg.items[0].start_angle = front_ ? 0.8 : 0; + msg.items[0].end_angle = front_ ? 2.5 : 0; msg.items[0].duration_s = 0.5; action_executor_->MoveServoTimed(msg); } From e92b128c412ce9bad1f056a7b57d05e8b93e6367 Mon Sep 17 00:00:00 2001 From: acki Date: Thu, 18 Dec 2025 18:22:16 +0100 Subject: [PATCH 11/43] servo value --- src/modelec_com/src/pcb_action_interface.cpp | 8 +++---- src/modelec_strat/src/action/down_action.cpp | 24 ++++++++++---------- src/modelec_strat/src/action/up_action.cpp | 24 ++++++++++---------- 3 files changed, 28 insertions(+), 28 deletions(-) diff --git a/src/modelec_com/src/pcb_action_interface.cpp b/src/modelec_com/src/pcb_action_interface.cpp index 00054b0..a4a3301 100644 --- a/src/modelec_com/src/pcb_action_interface.cpp +++ b/src/modelec_com/src/pcb_action_interface.cpp @@ -263,10 +263,10 @@ namespace Modelec };*/ servo_value_ = { - {0, 0}, - {1, 3}, - {2, 1.45}, - {3, 1.6}, + {0, 2.95}, + {1, 0.93}, + {2, 3}, + {3, 0}, {4, 0}, }; diff --git a/src/modelec_strat/src/action/down_action.cpp b/src/modelec_strat/src/action/down_action.cpp index 9e9c7d8..1ab94fa 100644 --- a/src/modelec_strat/src/action/down_action.cpp +++ b/src/modelec_strat/src/action/down_action.cpp @@ -36,24 +36,24 @@ void Modelec::DownAction::Next() msg.items.resize(4); msg.items[0].id = front_ ? 0 : 8; - msg.items[0].start_angle = front_ ? 1.49 : 0; - msg.items[0].end_angle = front_ ? 0 : 0; - msg.items[0].duration_s = 1; + msg.items[0].start_angle = front_ ? 1.95 : 0; + msg.items[0].end_angle = front_ ? 2.95 : 0; + msg.items[0].duration_s = 5; msg.items[1].id = front_ ? 1 : 9; - msg.items[1].start_angle = front_ ? 1.5 : 0; - msg.items[1].end_angle = front_ ? 3 : 0; - msg.items[1].duration_s = 1; + msg.items[1].start_angle = front_ ? 1.9 : 0; + msg.items[1].end_angle = front_ ? 0.93 : 0; + msg.items[1].duration_s = 5; msg.items[2].id = front_ ? 2 : 10; - msg.items[2].start_angle = front_ ? 3 : 0; - msg.items[2].end_angle = front_ ? 1.45 : 0; - msg.items[2].duration_s = 1; + msg.items[2].start_angle = front_ ? 2.3 : 0; + msg.items[2].end_angle = front_ ? 3 : 0; + msg.items[2].duration_s = 5; msg.items[3].id = front_ ? 3 : 11; - msg.items[3].start_angle = front_ ? 0 : 0; - msg.items[3].end_angle = front_ ? 1.6 : 0; - msg.items[3].duration_s = 1; + msg.items[3].start_angle = front_ ? 0.8 : 0; + msg.items[3].end_angle = front_ ? 0 : 0; + msg.items[3].duration_s = 5; action_executor_->MoveServoTimed(msg); } diff --git a/src/modelec_strat/src/action/up_action.cpp b/src/modelec_strat/src/action/up_action.cpp index 6d8ca66..9cc7f69 100644 --- a/src/modelec_strat/src/action/up_action.cpp +++ b/src/modelec_strat/src/action/up_action.cpp @@ -36,24 +36,24 @@ void Modelec::UPAction::Next() msg.items.resize(4); msg.items[0].id = front_ ? 0 : 8; - msg.items[0].start_angle = front_ ? 0 : 0; - msg.items[0].end_angle = front_ ? 1.49 : 0; - msg.items[0].duration_s = 1; + msg.items[0].start_angle = front_ ? 2.95 : 0; + msg.items[0].end_angle = front_ ? 1.95 : 0; + msg.items[0].duration_s = 5; msg.items[1].id = front_ ? 1 : 9; - msg.items[1].start_angle = front_ ? 3 : 0; - msg.items[1].end_angle = front_ ? 1.5 : 0; - msg.items[1].duration_s = 1; + msg.items[1].start_angle = front_ ? 0.93 : 0; + msg.items[1].end_angle = front_ ? 1.9 : 0; + msg.items[1].duration_s = 5; msg.items[2].id = front_ ? 2 : 10; - msg.items[2].start_angle = front_ ? 1.45 : 0; - msg.items[2].end_angle = front_ ? 3 : 0; - msg.items[2].duration_s = 1; + msg.items[2].start_angle = front_ ? 3 : 0; + msg.items[2].end_angle = front_ ? 2.3 : 0; + msg.items[2].duration_s = 5; msg.items[3].id = front_ ? 3 : 11; - msg.items[3].start_angle = front_ ? 1.6 : 0; - msg.items[3].end_angle = front_ ? 0 : 0; - msg.items[3].duration_s = 1; + msg.items[3].start_angle = front_ ? 0 : 0; + msg.items[3].end_angle = front_ ? 0.8 : 0; + msg.items[3].duration_s = 5; action_executor_->MoveServoTimed(msg); } From 524619882c00396de55341676bab8d7768a6dabe Mon Sep 17 00:00:00 2001 From: acki Date: Thu, 18 Dec 2025 18:24:39 +0100 Subject: [PATCH 12/43] servo value --- src/modelec_com/src/pcb_action_interface.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modelec_com/src/pcb_action_interface.cpp b/src/modelec_com/src/pcb_action_interface.cpp index a4a3301..5236a72 100644 --- a/src/modelec_com/src/pcb_action_interface.cpp +++ b/src/modelec_com/src/pcb_action_interface.cpp @@ -265,7 +265,7 @@ namespace Modelec servo_value_ = { {0, 2.95}, {1, 0.93}, - {2, 3}, + {2, 3.2}, {3, 0}, {4, 0}, }; From 0bbf669882580f9cc32917c7fa4445c1d2b2d825 Mon Sep 17 00:00:00 2001 From: acki Date: Thu, 18 Dec 2025 18:25:58 +0100 Subject: [PATCH 13/43] servo value --- src/modelec_strat/src/action/down_action.cpp | 2 +- src/modelec_strat/src/action/up_action.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modelec_strat/src/action/down_action.cpp b/src/modelec_strat/src/action/down_action.cpp index 1ab94fa..51e2356 100644 --- a/src/modelec_strat/src/action/down_action.cpp +++ b/src/modelec_strat/src/action/down_action.cpp @@ -47,7 +47,7 @@ void Modelec::DownAction::Next() msg.items[2].id = front_ ? 2 : 10; msg.items[2].start_angle = front_ ? 2.3 : 0; - msg.items[2].end_angle = front_ ? 3 : 0; + msg.items[2].end_angle = front_ ? 3.2 : 0; msg.items[2].duration_s = 5; msg.items[3].id = front_ ? 3 : 11; diff --git a/src/modelec_strat/src/action/up_action.cpp b/src/modelec_strat/src/action/up_action.cpp index 9cc7f69..89e132c 100644 --- a/src/modelec_strat/src/action/up_action.cpp +++ b/src/modelec_strat/src/action/up_action.cpp @@ -46,7 +46,7 @@ void Modelec::UPAction::Next() msg.items[1].duration_s = 5; msg.items[2].id = front_ ? 2 : 10; - msg.items[2].start_angle = front_ ? 3 : 0; + msg.items[2].start_angle = front_ ? 3.2 : 0; msg.items[2].end_angle = front_ ? 2.3 : 0; msg.items[2].duration_s = 5; From b6736f3ef55fbf953f1802ba5ec51606bf0aa3f9 Mon Sep 17 00:00:00 2001 From: acki Date: Thu, 18 Dec 2025 18:29:19 +0100 Subject: [PATCH 14/43] servo value --- src/modelec_strat/src/action/down_action.cpp | 8 ++++---- src/modelec_strat/src/action/up_action.cpp | 8 ++++---- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/modelec_strat/src/action/down_action.cpp b/src/modelec_strat/src/action/down_action.cpp index 51e2356..37952e2 100644 --- a/src/modelec_strat/src/action/down_action.cpp +++ b/src/modelec_strat/src/action/down_action.cpp @@ -38,22 +38,22 @@ void Modelec::DownAction::Next() msg.items[0].id = front_ ? 0 : 8; msg.items[0].start_angle = front_ ? 1.95 : 0; msg.items[0].end_angle = front_ ? 2.95 : 0; - msg.items[0].duration_s = 5; + msg.items[0].duration_s = 1; msg.items[1].id = front_ ? 1 : 9; msg.items[1].start_angle = front_ ? 1.9 : 0; msg.items[1].end_angle = front_ ? 0.93 : 0; - msg.items[1].duration_s = 5; + msg.items[1].duration_s = 1; msg.items[2].id = front_ ? 2 : 10; msg.items[2].start_angle = front_ ? 2.3 : 0; msg.items[2].end_angle = front_ ? 3.2 : 0; - msg.items[2].duration_s = 5; + msg.items[2].duration_s = 1; msg.items[3].id = front_ ? 3 : 11; msg.items[3].start_angle = front_ ? 0.8 : 0; msg.items[3].end_angle = front_ ? 0 : 0; - msg.items[3].duration_s = 5; + msg.items[3].duration_s = 1; action_executor_->MoveServoTimed(msg); } diff --git a/src/modelec_strat/src/action/up_action.cpp b/src/modelec_strat/src/action/up_action.cpp index 89e132c..79b0cea 100644 --- a/src/modelec_strat/src/action/up_action.cpp +++ b/src/modelec_strat/src/action/up_action.cpp @@ -38,22 +38,22 @@ void Modelec::UPAction::Next() msg.items[0].id = front_ ? 0 : 8; msg.items[0].start_angle = front_ ? 2.95 : 0; msg.items[0].end_angle = front_ ? 1.95 : 0; - msg.items[0].duration_s = 5; + msg.items[0].duration_s = 1; msg.items[1].id = front_ ? 1 : 9; msg.items[1].start_angle = front_ ? 0.93 : 0; msg.items[1].end_angle = front_ ? 1.9 : 0; - msg.items[1].duration_s = 5; + msg.items[1].duration_s = 1; msg.items[2].id = front_ ? 2 : 10; msg.items[2].start_angle = front_ ? 3.2 : 0; msg.items[2].end_angle = front_ ? 2.3 : 0; - msg.items[2].duration_s = 5; + msg.items[2].duration_s = 1; msg.items[3].id = front_ ? 3 : 11; msg.items[3].start_angle = front_ ? 0 : 0; msg.items[3].end_angle = front_ ? 0.8 : 0; - msg.items[3].duration_s = 5; + msg.items[3].duration_s = 1; action_executor_->MoveServoTimed(msg); } From 8241d16d546d3ee9c17edc8d69718ab77b4ae516 Mon Sep 17 00:00:00 2001 From: acki Date: Thu, 18 Dec 2025 18:39:31 +0100 Subject: [PATCH 15/43] servo value --- src/modelec_com/src/pcb_action_interface.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/modelec_com/src/pcb_action_interface.cpp b/src/modelec_com/src/pcb_action_interface.cpp index 5236a72..7b7f6f2 100644 --- a/src/modelec_com/src/pcb_action_interface.cpp +++ b/src/modelec_com/src/pcb_action_interface.cpp @@ -267,7 +267,10 @@ namespace Modelec {1, 0.93}, {2, 3.2}, {3, 0}, - {4, 0}, + {4, 0.8}, + {5, 0.8}, + {6, 0.8}, + {7, 0.8}, }; std::string data = "MOV;SERVO;" + std::to_string(servo_value_.size()) + ";"; From 51dcf3a5023d4466a131b4411a0bc6d0aeba4627 Mon Sep 17 00:00:00 2001 From: Modelec Date: Thu, 18 Dec 2025 18:43:35 +0100 Subject: [PATCH 16/43] fix --- src/modelec_core/launch/modelec.launch.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modelec_core/launch/modelec.launch.py b/src/modelec_core/launch/modelec.launch.py index bc34bf9..90be96b 100644 --- a/src/modelec_core/launch/modelec.launch.py +++ b/src/modelec_core/launch/modelec.launch.py @@ -99,7 +99,7 @@ def launch_com(context, *args, **kwargs): executable='pcb_odo_interface', name='pcb_odo_interface', parameters=[{ - 'serial_port': "/tmp/USB_ODO", + 'serial_port': "/dev/USB_ODO", 'baudrate': 115200, 'name': "pcb_odo", }] @@ -109,7 +109,7 @@ def launch_com(context, *args, **kwargs): executable='pcb_action_interface', name='pcb_action_interface', parameters=[{ - 'serial_port': "/tmp/USB_ACTION", + 'serial_port': "/dev/USB_ACTION", 'baudrate': 115200, 'name': "pcb_action", }] @@ -172,4 +172,4 @@ def launch_joy(context, *args, **kwargs): OpaqueFunction(function=launch_joy), ]) -# to run in debug : , prefix=['xterm -e gdb -ex run --args'] \ No newline at end of file +# to run in debug : , prefix=['xterm -e gdb -ex run --args'] From 96df284ca88314f434ac5b944f0a3aa2473b3a44 Mon Sep 17 00:00:00 2001 From: Modelec Date: Thu, 18 Dec 2025 18:44:25 +0100 Subject: [PATCH 17/43] fix --- no_lidar.ros2_launch_marcel.desktop | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/no_lidar.ros2_launch_marcel.desktop b/no_lidar.ros2_launch_marcel.desktop index b59f89d..c01e2fc 100644 --- a/no_lidar.ros2_launch_marcel.desktop +++ b/no_lidar.ros2_launch_marcel.desktop @@ -2,7 +2,7 @@ Type=Application Name=No Lidar Comment=Lance le système ROS 2 avec GUI -Exec=/home/modelec/modelec-marcel-ROS/start_ros2.sh with_joy:=false with_lidar:=false +Exec=/home/modelec/modelec-marcel-ROS/start_ros2.sh with_joy:=false with_rplidar:=false Icon=utilities-terminal Terminal=true Categories=Utility; From c2f967aee7b5462b9cc5c47f1e987a8b2b7799fa Mon Sep 17 00:00:00 2001 From: acki Date: Thu, 18 Dec 2025 19:06:57 +0100 Subject: [PATCH 18/43] up and down value --- src/modelec_strat/src/action/down_action.cpp | 10 ---------- src/modelec_strat/src/action/up_action.cpp | 10 ---------- 2 files changed, 20 deletions(-) diff --git a/src/modelec_strat/src/action/down_action.cpp b/src/modelec_strat/src/action/down_action.cpp index 37952e2..4ab2b09 100644 --- a/src/modelec_strat/src/action/down_action.cpp +++ b/src/modelec_strat/src/action/down_action.cpp @@ -45,16 +45,6 @@ void Modelec::DownAction::Next() msg.items[1].end_angle = front_ ? 0.93 : 0; msg.items[1].duration_s = 1; - msg.items[2].id = front_ ? 2 : 10; - msg.items[2].start_angle = front_ ? 2.3 : 0; - msg.items[2].end_angle = front_ ? 3.2 : 0; - msg.items[2].duration_s = 1; - - msg.items[3].id = front_ ? 3 : 11; - msg.items[3].start_angle = front_ ? 0.8 : 0; - msg.items[3].end_angle = front_ ? 0 : 0; - msg.items[3].duration_s = 1; - action_executor_->MoveServoTimed(msg); } break; diff --git a/src/modelec_strat/src/action/up_action.cpp b/src/modelec_strat/src/action/up_action.cpp index 79b0cea..4050f5a 100644 --- a/src/modelec_strat/src/action/up_action.cpp +++ b/src/modelec_strat/src/action/up_action.cpp @@ -45,16 +45,6 @@ void Modelec::UPAction::Next() msg.items[1].end_angle = front_ ? 1.9 : 0; msg.items[1].duration_s = 1; - msg.items[2].id = front_ ? 2 : 10; - msg.items[2].start_angle = front_ ? 3.2 : 0; - msg.items[2].end_angle = front_ ? 2.3 : 0; - msg.items[2].duration_s = 1; - - msg.items[3].id = front_ ? 3 : 11; - msg.items[3].start_angle = front_ ? 0 : 0; - msg.items[3].end_angle = front_ ? 0.8 : 0; - msg.items[3].duration_s = 1; - action_executor_->MoveServoTimed(msg); } break; From ca9da5b1e1d15c951e0f9dc5c372318846670b99 Mon Sep 17 00:00:00 2001 From: acki Date: Thu, 18 Dec 2025 19:14:39 +0100 Subject: [PATCH 19/43] setup joy --- src/modelec_strat/CMakeLists.txt | 2 +- src/modelec_strat/include/modelec_strat/action_executor.hpp | 3 +++ src/modelec_strat/src/action_executor.cpp | 6 ++++++ 3 files changed, 10 insertions(+), 1 deletion(-) diff --git a/src/modelec_strat/CMakeLists.txt b/src/modelec_strat/CMakeLists.txt index 8c8fc9f..b51d79a 100644 --- a/src/modelec_strat/CMakeLists.txt +++ b/src/modelec_strat/CMakeLists.txt @@ -37,7 +37,7 @@ set(strat_fsm_sources ) add_executable(strat_fsm ${strat_fsm_sources}) -ament_target_dependencies(strat_fsm rclcpp std_msgs std_srvs modelec_interfaces ament_index_cpp) +ament_target_dependencies(strat_fsm rclcpp std_msgs sensor_msgs std_srvs modelec_interfaces ament_index_cpp) target_link_libraries(strat_fsm modelec_utils::utils modelec_utils::config) target_include_directories(strat_fsm PUBLIC $ diff --git a/src/modelec_strat/include/modelec_strat/action_executor.hpp b/src/modelec_strat/include/modelec_strat/action_executor.hpp index 50cd0f7..264105a 100644 --- a/src/modelec_strat/include/modelec_strat/action_executor.hpp +++ b/src/modelec_strat/include/modelec_strat/action_executor.hpp @@ -7,6 +7,8 @@ #include #include +#include + namespace Modelec { class BaseAction; @@ -60,6 +62,7 @@ namespace Modelec rclcpp::Subscription::SharedPtr servo_timed_move_res_sub_; rclcpp::Subscription::SharedPtr action_exec_sub_; + rclcpp::Subscription::SharedPtr joy_sub_; std::shared_ptr action_; diff --git a/src/modelec_strat/src/action_executor.cpp b/src/modelec_strat/src/action_executor.cpp index ec266f8..f1292f0 100644 --- a/src/modelec_strat/src/action_executor.cpp +++ b/src/modelec_strat/src/action_executor.cpp @@ -79,6 +79,12 @@ namespace Modelec step_running_ -= msg->items.size(); Update(); }); + + joy_sub_ = node_->create_subscription( + "/joy", 10, [this](const sensor_msgs::msg::Joy::SharedPtr) + { + // use game controller to manually control all the action. make it carefully + }); } rclcpp::Node::SharedPtr ActionExecutor::GetNode() const From 267d28f594800322d918ab2cf3b86d0175d26032 Mon Sep 17 00:00:00 2001 From: acki Date: Thu, 18 Dec 2025 19:40:39 +0100 Subject: [PATCH 20/43] servo --- src/modelec_strat/src/action/down_action.cpp | 4 ++-- src/modelec_strat/src/action/up_action.cpp | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modelec_strat/src/action/down_action.cpp b/src/modelec_strat/src/action/down_action.cpp index 4ab2b09..576f32f 100644 --- a/src/modelec_strat/src/action/down_action.cpp +++ b/src/modelec_strat/src/action/down_action.cpp @@ -37,12 +37,12 @@ void Modelec::DownAction::Next() msg.items[0].id = front_ ? 0 : 8; msg.items[0].start_angle = front_ ? 1.95 : 0; - msg.items[0].end_angle = front_ ? 2.95 : 0; + msg.items[0].end_angle = front_ ? 2.75 : 0; msg.items[0].duration_s = 1; msg.items[1].id = front_ ? 1 : 9; msg.items[1].start_angle = front_ ? 1.9 : 0; - msg.items[1].end_angle = front_ ? 0.93 : 0; + msg.items[1].end_angle = front_ ? 1.13 : 0; msg.items[1].duration_s = 1; action_executor_->MoveServoTimed(msg); diff --git a/src/modelec_strat/src/action/up_action.cpp b/src/modelec_strat/src/action/up_action.cpp index 4050f5a..e3ed862 100644 --- a/src/modelec_strat/src/action/up_action.cpp +++ b/src/modelec_strat/src/action/up_action.cpp @@ -36,12 +36,12 @@ void Modelec::UPAction::Next() msg.items.resize(4); msg.items[0].id = front_ ? 0 : 8; - msg.items[0].start_angle = front_ ? 2.95 : 0; + msg.items[0].start_angle = front_ ? 2.75 : 0; msg.items[0].end_angle = front_ ? 1.95 : 0; msg.items[0].duration_s = 1; msg.items[1].id = front_ ? 1 : 9; - msg.items[1].start_angle = front_ ? 0.93 : 0; + msg.items[1].start_angle = front_ ? 1.13 : 0; msg.items[1].end_angle = front_ ? 1.9 : 0; msg.items[1].duration_s = 1; From 0b649a980bb534f4fb6dc54149f14224007d9914 Mon Sep 17 00:00:00 2001 From: acki Date: Thu, 18 Dec 2025 20:12:29 +0100 Subject: [PATCH 21/43] joy --- src/modelec_strat/src/action_executor.cpp | 21 ++++++++++++++++++++- 1 file changed, 20 insertions(+), 1 deletion(-) diff --git a/src/modelec_strat/src/action_executor.cpp b/src/modelec_strat/src/action_executor.cpp index f1292f0..574f934 100644 --- a/src/modelec_strat/src/action_executor.cpp +++ b/src/modelec_strat/src/action_executor.cpp @@ -81,9 +81,28 @@ namespace Modelec }); joy_sub_ = node_->create_subscription( - "/joy", 10, [this](const sensor_msgs::msg::Joy::SharedPtr) + "/joy", 10, [this](const sensor_msgs::msg::Joy::SharedPtr msg) { // use game controller to manually control all the action. make it carefully + if (msg->buttons.size() >= 5) + { + if (msg->buttons[0] == 1) // A button + { + Down(true); + } + else if (msg->buttons[1] == 1) // B button + { + Up(true); + } + else if (msg->buttons[3] == 1) // X button + { + Down(false); + } + else if (msg->buttons[4] == 1) // Y button + { + Up(false); + } + } }); } From 344aec65e428ba2dfeeccf5b14caf153cc4f4f1a Mon Sep 17 00:00:00 2001 From: acki Date: Thu, 18 Dec 2025 20:21:59 +0100 Subject: [PATCH 22/43] servo value --- src/modelec_strat/src/action/down_action.cpp | 6 +++--- src/modelec_strat/src/action/up_action.cpp | 4 ++-- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/modelec_strat/src/action/down_action.cpp b/src/modelec_strat/src/action/down_action.cpp index 576f32f..e45466b 100644 --- a/src/modelec_strat/src/action/down_action.cpp +++ b/src/modelec_strat/src/action/down_action.cpp @@ -37,12 +37,12 @@ void Modelec::DownAction::Next() msg.items[0].id = front_ ? 0 : 8; msg.items[0].start_angle = front_ ? 1.95 : 0; - msg.items[0].end_angle = front_ ? 2.75 : 0; + msg.items[0].end_angle = front_ ? 2.95 : 0; msg.items[0].duration_s = 1; msg.items[1].id = front_ ? 1 : 9; - msg.items[1].start_angle = front_ ? 1.9 : 0; - msg.items[1].end_angle = front_ ? 1.13 : 0; + msg.items[1].start_angle = front_ ? 0.9 : 0; + msg.items[1].end_angle = front_ ? 1.9 : 0; msg.items[1].duration_s = 1; action_executor_->MoveServoTimed(msg); diff --git a/src/modelec_strat/src/action/up_action.cpp b/src/modelec_strat/src/action/up_action.cpp index e3ed862..b8273ab 100644 --- a/src/modelec_strat/src/action/up_action.cpp +++ b/src/modelec_strat/src/action/up_action.cpp @@ -36,12 +36,12 @@ void Modelec::UPAction::Next() msg.items.resize(4); msg.items[0].id = front_ ? 0 : 8; - msg.items[0].start_angle = front_ ? 2.75 : 0; + msg.items[0].start_angle = front_ ? 2.95 : 0; msg.items[0].end_angle = front_ ? 1.95 : 0; msg.items[0].duration_s = 1; msg.items[1].id = front_ ? 1 : 9; - msg.items[1].start_angle = front_ ? 1.13 : 0; + msg.items[1].start_angle = front_ ? 0.9 : 0; msg.items[1].end_angle = front_ ? 1.9 : 0; msg.items[1].duration_s = 1; From bcfdb94f492ff46a9aec8c4aced59093f47edefc Mon Sep 17 00:00:00 2001 From: acki Date: Thu, 18 Dec 2025 20:34:55 +0100 Subject: [PATCH 23/43] servo value --- src/modelec_com/src/pcb_action_interface.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modelec_com/src/pcb_action_interface.cpp b/src/modelec_com/src/pcb_action_interface.cpp index 7b7f6f2..aa28560 100644 --- a/src/modelec_com/src/pcb_action_interface.cpp +++ b/src/modelec_com/src/pcb_action_interface.cpp @@ -265,8 +265,8 @@ namespace Modelec servo_value_ = { {0, 2.95}, {1, 0.93}, - {2, 3.2}, - {3, 0}, + {2, 0}, + {3, 3}, {4, 0.8}, {5, 0.8}, {6, 0.8}, From 507020eda157bb48c46ceaf2cf7e6ca48bf3b3f5 Mon Sep 17 00:00:00 2001 From: acki Date: Thu, 18 Dec 2025 20:36:29 +0100 Subject: [PATCH 24/43] servo value --- src/modelec_strat/src/action/down_action.cpp | 10 ++++++++++ src/modelec_strat/src/action/up_action.cpp | 10 ++++++++++ 2 files changed, 20 insertions(+) diff --git a/src/modelec_strat/src/action/down_action.cpp b/src/modelec_strat/src/action/down_action.cpp index e45466b..13b7ec5 100644 --- a/src/modelec_strat/src/action/down_action.cpp +++ b/src/modelec_strat/src/action/down_action.cpp @@ -45,6 +45,16 @@ void Modelec::DownAction::Next() msg.items[1].end_angle = front_ ? 1.9 : 0; msg.items[1].duration_s = 1; + msg.items[1].id = front_ ? 2 : 10; + msg.items[1].start_angle = front_ ? 0.3 : 0; + msg.items[1].end_angle = front_ ? 0 : 0; + msg.items[1].duration_s = 1; + + msg.items[1].id = front_ ? 3 : 11; + msg.items[1].start_angle = front_ ? 2.7 : 0; + msg.items[1].end_angle = front_ ? 3 : 0; + msg.items[1].duration_s = 1; + action_executor_->MoveServoTimed(msg); } break; diff --git a/src/modelec_strat/src/action/up_action.cpp b/src/modelec_strat/src/action/up_action.cpp index b8273ab..7e48400 100644 --- a/src/modelec_strat/src/action/up_action.cpp +++ b/src/modelec_strat/src/action/up_action.cpp @@ -45,6 +45,16 @@ void Modelec::UPAction::Next() msg.items[1].end_angle = front_ ? 1.9 : 0; msg.items[1].duration_s = 1; + msg.items[1].id = front_ ? 2 : 10; + msg.items[1].start_angle = front_ ? 0 : 0; + msg.items[1].end_angle = front_ ? 0.3 : 0; + msg.items[1].duration_s = 1; + + msg.items[1].id = front_ ? 3 : 11; + msg.items[1].start_angle = front_ ? 3 : 0; + msg.items[1].end_angle = front_ ? 2.7 : 0; + msg.items[1].duration_s = 1; + action_executor_->MoveServoTimed(msg); } break; From 6e235feeda1e5c811f67913318e5cd2bec6cb8b0 Mon Sep 17 00:00:00 2001 From: acki Date: Thu, 18 Dec 2025 20:44:26 +0100 Subject: [PATCH 25/43] servo value --- src/modelec_strat/src/action/down_action.cpp | 16 ++++++++-------- src/modelec_strat/src/action/up_action.cpp | 16 ++++++++-------- 2 files changed, 16 insertions(+), 16 deletions(-) diff --git a/src/modelec_strat/src/action/down_action.cpp b/src/modelec_strat/src/action/down_action.cpp index 13b7ec5..a40667b 100644 --- a/src/modelec_strat/src/action/down_action.cpp +++ b/src/modelec_strat/src/action/down_action.cpp @@ -45,15 +45,15 @@ void Modelec::DownAction::Next() msg.items[1].end_angle = front_ ? 1.9 : 0; msg.items[1].duration_s = 1; - msg.items[1].id = front_ ? 2 : 10; - msg.items[1].start_angle = front_ ? 0.3 : 0; - msg.items[1].end_angle = front_ ? 0 : 0; - msg.items[1].duration_s = 1; + msg.items[2].id = front_ ? 2 : 10; + msg.items[2].start_angle = front_ ? 0.3 : 0; + msg.items[2].end_angle = front_ ? 0 : 0; + msg.items[2].duration_s = 1; - msg.items[1].id = front_ ? 3 : 11; - msg.items[1].start_angle = front_ ? 2.7 : 0; - msg.items[1].end_angle = front_ ? 3 : 0; - msg.items[1].duration_s = 1; + msg.items[3].id = front_ ? 3 : 11; + msg.items[3].start_angle = front_ ? 2.7 : 0; + msg.items[3].end_angle = front_ ? 3 : 0; + msg.items[3].duration_s = 1; action_executor_->MoveServoTimed(msg); } diff --git a/src/modelec_strat/src/action/up_action.cpp b/src/modelec_strat/src/action/up_action.cpp index 7e48400..3a37974 100644 --- a/src/modelec_strat/src/action/up_action.cpp +++ b/src/modelec_strat/src/action/up_action.cpp @@ -45,15 +45,15 @@ void Modelec::UPAction::Next() msg.items[1].end_angle = front_ ? 1.9 : 0; msg.items[1].duration_s = 1; - msg.items[1].id = front_ ? 2 : 10; - msg.items[1].start_angle = front_ ? 0 : 0; - msg.items[1].end_angle = front_ ? 0.3 : 0; - msg.items[1].duration_s = 1; + msg.items[2].id = front_ ? 2 : 10; + msg.items[2].start_angle = front_ ? 0 : 0; + msg.items[2].end_angle = front_ ? 0.3 : 0; + msg.items[2].duration_s = 1; - msg.items[1].id = front_ ? 3 : 11; - msg.items[1].start_angle = front_ ? 3 : 0; - msg.items[1].end_angle = front_ ? 2.7 : 0; - msg.items[1].duration_s = 1; + msg.items[3].id = front_ ? 3 : 11; + msg.items[3].start_angle = front_ ? 3 : 0; + msg.items[3].end_angle = front_ ? 2.7 : 0; + msg.items[3].duration_s = 1; action_executor_->MoveServoTimed(msg); } From 9f6da67b8093ecfcc92c41ac77d18583ca51f5ba Mon Sep 17 00:00:00 2001 From: acki Date: Thu, 18 Dec 2025 20:47:20 +0100 Subject: [PATCH 26/43] servo value --- src/modelec_strat/src/action/down_action.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modelec_strat/src/action/down_action.cpp b/src/modelec_strat/src/action/down_action.cpp index a40667b..e84c78c 100644 --- a/src/modelec_strat/src/action/down_action.cpp +++ b/src/modelec_strat/src/action/down_action.cpp @@ -41,8 +41,8 @@ void Modelec::DownAction::Next() msg.items[0].duration_s = 1; msg.items[1].id = front_ ? 1 : 9; - msg.items[1].start_angle = front_ ? 0.9 : 0; - msg.items[1].end_angle = front_ ? 1.9 : 0; + msg.items[1].start_angle = front_ ? 1.9 : 0; + msg.items[1].end_angle = front_ ? 0.9 : 0; msg.items[1].duration_s = 1; msg.items[2].id = front_ ? 2 : 10; From bd77cccdba680153ae8b8a3aaa2d893ace7322f9 Mon Sep 17 00:00:00 2001 From: acki Date: Thu, 18 Dec 2025 20:54:01 +0100 Subject: [PATCH 27/43] servo value --- src/modelec_strat/src/action/free_action.cpp | 2 +- src/modelec_strat/src/action/take_action.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modelec_strat/src/action/free_action.cpp b/src/modelec_strat/src/action/free_action.cpp index e9443a8..0e79fd5 100644 --- a/src/modelec_strat/src/action/free_action.cpp +++ b/src/modelec_strat/src/action/free_action.cpp @@ -38,7 +38,7 @@ void Modelec::FreeAction::Next() msg.items.resize(1); msg.items[0].id = n_ + (front_ ? 3 : 11); - msg.items[0].start_angle = front_ ? 2.5 : 0; + msg.items[0].start_angle = front_ ? 3 : 0; msg.items[0].end_angle = front_ ? 0.8 : 0; msg.items[0].duration_s = 0.5; action_executor_->MoveServoTimed(msg); diff --git a/src/modelec_strat/src/action/take_action.cpp b/src/modelec_strat/src/action/take_action.cpp index c67978b..3889eb8 100644 --- a/src/modelec_strat/src/action/take_action.cpp +++ b/src/modelec_strat/src/action/take_action.cpp @@ -39,7 +39,7 @@ void Modelec::TakeAction::Next() msg.items[0].id = n_ + (front_ ? 3 : 11); msg.items[0].start_angle = front_ ? 0.8 : 0; - msg.items[0].end_angle = front_ ? 2.5 : 0; + msg.items[0].end_angle = front_ ? 3 : 0; msg.items[0].duration_s = 0.5; action_executor_->MoveServoTimed(msg); } From b802d22e54727b2ac61739ef2956782dcced5cf2 Mon Sep 17 00:00:00 2001 From: acki Date: Thu, 18 Dec 2025 20:59:59 +0100 Subject: [PATCH 28/43] servo value --- src/modelec_strat/src/action/free_action.cpp | 2 +- src/modelec_strat/src/action/take_action.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modelec_strat/src/action/free_action.cpp b/src/modelec_strat/src/action/free_action.cpp index 0e79fd5..2705343 100644 --- a/src/modelec_strat/src/action/free_action.cpp +++ b/src/modelec_strat/src/action/free_action.cpp @@ -38,7 +38,7 @@ void Modelec::FreeAction::Next() msg.items.resize(1); msg.items[0].id = n_ + (front_ ? 3 : 11); - msg.items[0].start_angle = front_ ? 3 : 0; + msg.items[0].start_angle = front_ ? 2.7 : 0; msg.items[0].end_angle = front_ ? 0.8 : 0; msg.items[0].duration_s = 0.5; action_executor_->MoveServoTimed(msg); diff --git a/src/modelec_strat/src/action/take_action.cpp b/src/modelec_strat/src/action/take_action.cpp index 3889eb8..0892fd8 100644 --- a/src/modelec_strat/src/action/take_action.cpp +++ b/src/modelec_strat/src/action/take_action.cpp @@ -39,7 +39,7 @@ void Modelec::TakeAction::Next() msg.items[0].id = n_ + (front_ ? 3 : 11); msg.items[0].start_angle = front_ ? 0.8 : 0; - msg.items[0].end_angle = front_ ? 3 : 0; + msg.items[0].end_angle = front_ ? 2.7 : 0; msg.items[0].duration_s = 0.5; action_executor_->MoveServoTimed(msg); } From 94b85ff9426e84cfef2b285a66a509ff2f8922a8 Mon Sep 17 00:00:00 2001 From: acki Date: Fri, 19 Dec 2025 16:09:40 +0100 Subject: [PATCH 29/43] servo value --- .../include/modelec_strat/action/base_action.hpp | 4 ---- .../include/modelec_strat/action/down_action.hpp | 1 - .../include/modelec_strat/action/free_action.hpp | 1 - .../include/modelec_strat/action/take_action.hpp | 2 +- .../include/modelec_strat/action/up_action.hpp | 1 - src/modelec_strat/src/action/down_action.cpp | 4 ---- src/modelec_strat/src/action/free_action.cpp | 6 +----- src/modelec_strat/src/action/take_action.cpp | 10 ++++------ src/modelec_strat/src/action/up_action.cpp | 6 +----- 9 files changed, 7 insertions(+), 28 deletions(-) diff --git a/src/modelec_strat/include/modelec_strat/action/base_action.hpp b/src/modelec_strat/include/modelec_strat/action/base_action.hpp index ee727df..b30866e 100644 --- a/src/modelec_strat/include/modelec_strat/action/base_action.hpp +++ b/src/modelec_strat/include/modelec_strat/action/base_action.hpp @@ -41,13 +41,9 @@ namespace Modelec { } virtual ~BaseAction() = default; - virtual void Execute() = 0; virtual void Next() = 0; virtual bool IsDone() const { return done_; } virtual void Init(const std::vector& params) = 0; - - // static constexpr std::string_view Name = "BaseAction"; - static Ptr CreateAction( const std::string& action_name, const std::shared_ptr& action_executor); diff --git a/src/modelec_strat/include/modelec_strat/action/down_action.hpp b/src/modelec_strat/include/modelec_strat/action/down_action.hpp index ad784e5..e32d6c0 100644 --- a/src/modelec_strat/include/modelec_strat/action/down_action.hpp +++ b/src/modelec_strat/include/modelec_strat/action/down_action.hpp @@ -11,7 +11,6 @@ namespace Modelec DownAction(const std::shared_ptr& action_executor); DownAction(const std::shared_ptr& action_executor, bool front); - void Execute() override; void Next() override; void Init(const std::vector& params) override; void SetFront(bool front); diff --git a/src/modelec_strat/include/modelec_strat/action/free_action.hpp b/src/modelec_strat/include/modelec_strat/action/free_action.hpp index 20326c2..2167139 100644 --- a/src/modelec_strat/include/modelec_strat/action/free_action.hpp +++ b/src/modelec_strat/include/modelec_strat/action/free_action.hpp @@ -11,7 +11,6 @@ namespace Modelec FreeAction(const std::shared_ptr& action_executor); FreeAction(const std::shared_ptr& action_executor, bool front, int n); - void Execute() override; void Next() override; void Init(const std::vector& params) override; void SetFront(bool front); diff --git a/src/modelec_strat/include/modelec_strat/action/take_action.hpp b/src/modelec_strat/include/modelec_strat/action/take_action.hpp index 9808919..4ae0008 100644 --- a/src/modelec_strat/include/modelec_strat/action/take_action.hpp +++ b/src/modelec_strat/include/modelec_strat/action/take_action.hpp @@ -11,9 +11,9 @@ namespace Modelec TakeAction(const std::shared_ptr& action_executor); TakeAction(const std::shared_ptr& action_executor, bool front, int n); - void Execute() override; void Next() override; void Init(const std::vector& params) override; + void Init(bool front, int n); void SetFront(bool front); void SetN(int n); diff --git a/src/modelec_strat/include/modelec_strat/action/up_action.hpp b/src/modelec_strat/include/modelec_strat/action/up_action.hpp index 174b21d..866a8f2 100644 --- a/src/modelec_strat/include/modelec_strat/action/up_action.hpp +++ b/src/modelec_strat/include/modelec_strat/action/up_action.hpp @@ -11,7 +11,6 @@ namespace Modelec UPAction(const std::shared_ptr& action_executor); UPAction(const std::shared_ptr& action_executor, bool front); - void Execute() override; void Next() override; void Init(const std::vector& params) override; void SetFront(bool front); diff --git a/src/modelec_strat/src/action/down_action.cpp b/src/modelec_strat/src/action/down_action.cpp index e84c78c..706fc3a 100644 --- a/src/modelec_strat/src/action/down_action.cpp +++ b/src/modelec_strat/src/action/down_action.cpp @@ -13,10 +13,6 @@ Modelec::DownAction::DownAction(const std::shared_ptr& action_ex front_ = front; } -void Modelec::DownAction::Execute() -{ -} - void Modelec::DownAction::Next() { if (steps_.empty()) diff --git a/src/modelec_strat/src/action/free_action.cpp b/src/modelec_strat/src/action/free_action.cpp index 2705343..7ab3b24 100644 --- a/src/modelec_strat/src/action/free_action.cpp +++ b/src/modelec_strat/src/action/free_action.cpp @@ -14,10 +14,6 @@ Modelec::FreeAction::FreeAction(const std::shared_ptr& action_ex n_ = n; } -void Modelec::FreeAction::Execute() -{ -} - void Modelec::FreeAction::Next() { if (steps_.empty()) @@ -38,7 +34,7 @@ void Modelec::FreeAction::Next() msg.items.resize(1); msg.items[0].id = n_ + (front_ ? 3 : 11); - msg.items[0].start_angle = front_ ? 2.7 : 0; + msg.items[0].start_angle = front_ ? 2.5 : 0; msg.items[0].end_angle = front_ ? 0.8 : 0; msg.items[0].duration_s = 0.5; action_executor_->MoveServoTimed(msg); diff --git a/src/modelec_strat/src/action/take_action.cpp b/src/modelec_strat/src/action/take_action.cpp index 0892fd8..be11108 100644 --- a/src/modelec_strat/src/action/take_action.cpp +++ b/src/modelec_strat/src/action/take_action.cpp @@ -4,8 +4,6 @@ Modelec::TakeAction::TakeAction(const std::shared_ptr& action_executor) : BaseAction(action_executor) { - steps_.push(ActionExec::TAKE_STEP); - steps_.push(ActionExec::DONE_STEP); } Modelec::TakeAction::TakeAction(const std::shared_ptr& action_executor, bool front, int n) : TakeAction(action_executor) @@ -14,10 +12,6 @@ Modelec::TakeAction::TakeAction(const std::shared_ptr& action_ex n_ = n; } -void Modelec::TakeAction::Execute() -{ -} - void Modelec::TakeAction::Next() { if (steps_.empty()) @@ -63,6 +57,10 @@ void Modelec::TakeAction::Init(const std::vector& params) } } +void Modelec::TakeAction::Init(bool front, int n) +{ +} + void Modelec::TakeAction::SetFront(bool front) { front_ = front; diff --git a/src/modelec_strat/src/action/up_action.cpp b/src/modelec_strat/src/action/up_action.cpp index 3a37974..7cfa960 100644 --- a/src/modelec_strat/src/action/up_action.cpp +++ b/src/modelec_strat/src/action/up_action.cpp @@ -13,10 +13,6 @@ Modelec::UPAction::UPAction(const std::shared_ptr& action_execut front_ = front; } -void Modelec::UPAction::Execute() -{ -} - void Modelec::UPAction::Next() { if (steps_.empty()) @@ -52,7 +48,7 @@ void Modelec::UPAction::Next() msg.items[3].id = front_ ? 3 : 11; msg.items[3].start_angle = front_ ? 3 : 0; - msg.items[3].end_angle = front_ ? 2.7 : 0; + msg.items[3].end_angle = front_ ? 2.5 : 0; msg.items[3].duration_s = 1; action_executor_->MoveServoTimed(msg); From 980beaa8bf2833735927a5f5154c0094b0a5f28f Mon Sep 17 00:00:00 2001 From: acki Date: Fri, 19 Dec 2025 16:22:10 +0100 Subject: [PATCH 30/43] dist --- src/modelec_strat/src/missions/free_mission.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modelec_strat/src/missions/free_mission.cpp b/src/modelec_strat/src/missions/free_mission.cpp index a691449..09ffb77 100644 --- a/src/modelec_strat/src/missions/free_mission.cpp +++ b/src/modelec_strat/src/missions/free_mission.cpp @@ -42,7 +42,7 @@ namespace Modelec { auto currPos = nav_->GetCurrentPos(); auto dist = std::clamp(Point::distance(Point(currPos->x, currPos->y, currPos->theta), - nav_->GetClosestDepositeZone(nav_->GetCurrentPos())->GetPosition()), 0.0, 200.0); + nav_->GetClosestDepositeZone(nav_->GetCurrentPos())->GetPosition()), 0.0, 300.0); target_deposite_zone_ = nav_->GetClosestDepositeZone(nav_->GetCurrentPos(), {}, true); From ec7fca430db7c3d6870aba8ff6a7493fa20afd1f Mon Sep 17 00:00:00 2001 From: acki Date: Fri, 19 Dec 2025 16:56:01 +0100 Subject: [PATCH 31/43] new Front Back system --- .../modelec_strat/action/base_action.hpp | 7 ++ .../modelec_strat/action/down_action.hpp | 6 +- .../modelec_strat/action/up_action.hpp | 6 +- .../include/modelec_strat/action_executor.hpp | 14 +--- .../modelec_strat/missions/free_mission.hpp | 2 + .../modelec_strat/missions/take_mission.hpp | 2 + src/modelec_strat/src/action/down_action.cpp | 67 +++++++++++++------ src/modelec_strat/src/action/up_action.cpp | 67 +++++++++++++------ src/modelec_strat/src/action_executor.cpp | 4 +- .../src/missions/free_mission.cpp | 30 +++++++-- .../src/missions/take_mission.cpp | 26 +++++-- src/modelec_strat/src/strat_fms.cpp | 4 ++ 12 files changed, 165 insertions(+), 70 deletions(-) diff --git a/src/modelec_strat/include/modelec_strat/action/base_action.hpp b/src/modelec_strat/include/modelec_strat/action/base_action.hpp index b30866e..cb4b103 100644 --- a/src/modelec_strat/include/modelec_strat/action/base_action.hpp +++ b/src/modelec_strat/include/modelec_strat/action/base_action.hpp @@ -25,6 +25,13 @@ namespace Modelec class BaseAction { public: + enum Front + { + FRONT = 1, + BACK = 0, + BOTH = -1, + }; + using Ptr = std::shared_ptr; using FactoryFn = std::function&)>; diff --git a/src/modelec_strat/include/modelec_strat/action/down_action.hpp b/src/modelec_strat/include/modelec_strat/action/down_action.hpp index e32d6c0..814e01e 100644 --- a/src/modelec_strat/include/modelec_strat/action/down_action.hpp +++ b/src/modelec_strat/include/modelec_strat/action/down_action.hpp @@ -9,16 +9,16 @@ namespace Modelec { public: DownAction(const std::shared_ptr& action_executor); - DownAction(const std::shared_ptr& action_executor, bool front); + DownAction(const std::shared_ptr& action_executor, Front front); void Next() override; void Init(const std::vector& params) override; - void SetFront(bool front); + void SetFront(Front front); inline static const std::string Name = ActionExec::DOWN; private: - bool front_; + Front front_; std::queue steps_; diff --git a/src/modelec_strat/include/modelec_strat/action/up_action.hpp b/src/modelec_strat/include/modelec_strat/action/up_action.hpp index 866a8f2..95b8ea8 100644 --- a/src/modelec_strat/include/modelec_strat/action/up_action.hpp +++ b/src/modelec_strat/include/modelec_strat/action/up_action.hpp @@ -9,16 +9,16 @@ namespace Modelec { public: UPAction(const std::shared_ptr& action_executor); - UPAction(const std::shared_ptr& action_executor, bool front); + UPAction(const std::shared_ptr& action_executor, Front front); void Next() override; void Init(const std::vector& params) override; - void SetFront(bool front); + void SetFront(Front front); inline static const std::string Name = ActionExec::UP; private: - bool front_; + Front front_; std::queue steps_; diff --git a/src/modelec_strat/include/modelec_strat/action_executor.hpp b/src/modelec_strat/include/modelec_strat/action_executor.hpp index 264105a..b9cbc15 100644 --- a/src/modelec_strat/include/modelec_strat/action_executor.hpp +++ b/src/modelec_strat/include/modelec_strat/action_executor.hpp @@ -16,16 +16,6 @@ namespace Modelec class ActionExecutor : public std::enable_shared_from_this { public: - enum Action - { - NONE, - UP, - DOWN, - - TAKE, - FREE - }; - ActionExecutor(); ActionExecutor(const rclcpp::Node::SharedPtr& node); @@ -38,9 +28,9 @@ namespace Modelec void ReInit(); - void Down(bool front = true); + void Down(BaseAction::Front front); - void Up(bool front = true); + void Up(BaseAction::Front front); void Take(bool front = true, int n = 0); diff --git a/src/modelec_strat/include/modelec_strat/missions/free_mission.hpp b/src/modelec_strat/include/modelec_strat/missions/free_mission.hpp index d56a4a3..9571bd9 100644 --- a/src/modelec_strat/include/modelec_strat/missions/free_mission.hpp +++ b/src/modelec_strat/include/modelec_strat/missions/free_mission.hpp @@ -20,7 +20,9 @@ namespace Modelec { enum Step { GO_TO_FREE, + DOWN, FREE, + UP, DONE, } step_; diff --git a/src/modelec_strat/include/modelec_strat/missions/take_mission.hpp b/src/modelec_strat/include/modelec_strat/missions/take_mission.hpp index d194339..4893be1 100644 --- a/src/modelec_strat/include/modelec_strat/missions/take_mission.hpp +++ b/src/modelec_strat/include/modelec_strat/missions/take_mission.hpp @@ -20,7 +20,9 @@ namespace Modelec { enum Step { GO_TO_TAKE, + DOWN, TAKE, + UP, DONE, } step_; diff --git a/src/modelec_strat/src/action/down_action.cpp b/src/modelec_strat/src/action/down_action.cpp index 706fc3a..2d666ed 100644 --- a/src/modelec_strat/src/action/down_action.cpp +++ b/src/modelec_strat/src/action/down_action.cpp @@ -8,7 +8,7 @@ Modelec::DownAction::DownAction(const std::shared_ptr& action_ex steps_.push(ActionExec::DONE_STEP); } -Modelec::DownAction::DownAction(const std::shared_ptr& action_executor, bool front) : DownAction(action_executor) +Modelec::DownAction::DownAction(const std::shared_ptr& action_executor, Front front) : DownAction(action_executor) { front_ = front; } @@ -29,27 +29,54 @@ void Modelec::DownAction::Next() case ActionExec::DOWN_STEP: { ActionServoTimedArray msg; - msg.items.resize(4); + msg.items.resize(front_ == BOTH ? 8 : 4); - msg.items[0].id = front_ ? 0 : 8; - msg.items[0].start_angle = front_ ? 1.95 : 0; - msg.items[0].end_angle = front_ ? 2.95 : 0; - msg.items[0].duration_s = 1; + if (front_ == FRONT || front_ == BOTH) + { + msg.items[0].id = 0; + msg.items[0].start_angle = 2.95; + msg.items[0].end_angle = 1.95; + msg.items[0].duration_s = 1; - msg.items[1].id = front_ ? 1 : 9; - msg.items[1].start_angle = front_ ? 1.9 : 0; - msg.items[1].end_angle = front_ ? 0.9 : 0; - msg.items[1].duration_s = 1; + msg.items[1].id = 1; + msg.items[1].start_angle = 0.9; + msg.items[1].end_angle = 1.9; + msg.items[1].duration_s = 1; - msg.items[2].id = front_ ? 2 : 10; - msg.items[2].start_angle = front_ ? 0.3 : 0; - msg.items[2].end_angle = front_ ? 0 : 0; - msg.items[2].duration_s = 1; + msg.items[2].id = 2; + msg.items[2].start_angle = 0; + msg.items[2].end_angle = 0.3; + msg.items[2].duration_s = 1; - msg.items[3].id = front_ ? 3 : 11; - msg.items[3].start_angle = front_ ? 2.7 : 0; - msg.items[3].end_angle = front_ ? 3 : 0; - msg.items[3].duration_s = 1; + msg.items[3].id = 3; + msg.items[3].start_angle = 3; + msg.items[3].end_angle = 2.7; + msg.items[3].duration_s = 1; + } + + if (front_ == BACK || front_ == BOTH) + { + int i = front_ == BOTH ? 4 : 0; + msg.items[i].id = 8; + msg.items[i].start_angle = 0; + msg.items[i].end_angle = 0; + msg.items[i].duration_s = 1; + + msg.items[i+1].id = 9; + msg.items[i+1].start_angle = 0; + msg.items[i+1].end_angle = 0; + msg.items[i+1].duration_s = 1; + + msg.items[i+2].id = 10; + msg.items[i+2].start_angle = 0; + msg.items[i+2].end_angle = 0; + msg.items[i+2].duration_s = 1; + + msg.items[i+3].id = 11; + msg.items[i+3].start_angle = 0; + msg.items[i+3].end_angle = 0; + msg.items[i+3].duration_s = 1; + } action_executor_->MoveServoTimed(msg); } @@ -68,11 +95,11 @@ void Modelec::DownAction::Init(const std::vector& params) { if (!params.empty()) { - SetFront(params[1] == "1" || params[1] == "true" || params[1] == "front"); + SetFront(static_cast(std::stoi(params[1]))); } } -void Modelec::DownAction::SetFront(bool front) +void Modelec::DownAction::SetFront(Front front) { front_ = front; } diff --git a/src/modelec_strat/src/action/up_action.cpp b/src/modelec_strat/src/action/up_action.cpp index 7cfa960..4774027 100644 --- a/src/modelec_strat/src/action/up_action.cpp +++ b/src/modelec_strat/src/action/up_action.cpp @@ -8,7 +8,7 @@ Modelec::UPAction::UPAction(const std::shared_ptr& action_execut steps_.push(ActionExec::DONE_STEP); } -Modelec::UPAction::UPAction(const std::shared_ptr& action_executor, bool front) : UPAction(action_executor) +Modelec::UPAction::UPAction(const std::shared_ptr& action_executor, front front) : UPAction(action_executor) { front_ = front; } @@ -29,27 +29,54 @@ void Modelec::UPAction::Next() case ActionExec::UP_STEP: { ActionServoTimedArray msg; - msg.items.resize(4); + msg.items.resize(front_ == BOTH ? 8 : 4); - msg.items[0].id = front_ ? 0 : 8; - msg.items[0].start_angle = front_ ? 2.95 : 0; - msg.items[0].end_angle = front_ ? 1.95 : 0; - msg.items[0].duration_s = 1; + if (front_ == FRONT || front_ == BOTH) + { + msg.items[0].id = 0; + msg.items[0].start_angle = 2.95; + msg.items[0].end_angle = 1.95; + msg.items[0].duration_s = 1; - msg.items[1].id = front_ ? 1 : 9; - msg.items[1].start_angle = front_ ? 0.9 : 0; - msg.items[1].end_angle = front_ ? 1.9 : 0; - msg.items[1].duration_s = 1; + msg.items[1].id = 1; + msg.items[1].start_angle = 0.9; + msg.items[1].end_angle = 1.9; + msg.items[1].duration_s = 1; - msg.items[2].id = front_ ? 2 : 10; - msg.items[2].start_angle = front_ ? 0 : 0; - msg.items[2].end_angle = front_ ? 0.3 : 0; - msg.items[2].duration_s = 1; + msg.items[2].id = 2; + msg.items[2].start_angle = 0; + msg.items[2].end_angle = 0.3; + msg.items[2].duration_s = 1; - msg.items[3].id = front_ ? 3 : 11; - msg.items[3].start_angle = front_ ? 3 : 0; - msg.items[3].end_angle = front_ ? 2.5 : 0; - msg.items[3].duration_s = 1; + msg.items[3].id = 3; + msg.items[3].start_angle = 3; + msg.items[3].end_angle = 2.5; + msg.items[3].duration_s = 1; + } + + if (front_ == BACK || front_ == BOTH) { + int i = front_ == BOTH ? 4 : 0; + + msg.items[i].id = 8; + msg.items[i].start_angle = 0; + msg.items[i].end_angle = 0; + msg.items[i].duration_s = 1; + + msg.items[i+1].id = 9; + msg.items[i+1].start_angle = 0; + msg.items[i+1].end_angle = 0; + msg.items[i+1].duration_s = 1; + + msg.items[i+2].id = 10; + msg.items[i+2].start_angle = 0; + msg.items[i+2].end_angle = 0; + msg.items[i+2].duration_s = 1; + + msg.items[i+3].id = 11; + msg.items[i+3].start_angle = 0; + msg.items[i+3].end_angle = 0; + msg.items[i+3].duration_s = 1; + } action_executor_->MoveServoTimed(msg); } @@ -68,11 +95,11 @@ void Modelec::UPAction::Init(const std::vector& params) { if (!params.empty()) { - SetFront(params[1] == "1" || params[1] == "true" || params[1] == "front"); + SetFront(static_cast(std::stoi(params[1]))); } } -void Modelec::UPAction::SetFront(bool front) +void Modelec::UPAction::SetFront(front front) { front_ = front; } diff --git a/src/modelec_strat/src/action_executor.cpp b/src/modelec_strat/src/action_executor.cpp index 574f934..13380bb 100644 --- a/src/modelec_strat/src/action_executor.cpp +++ b/src/modelec_strat/src/action_executor.cpp @@ -149,7 +149,7 @@ namespace Modelec action_ = nullptr; } - void ActionExecutor::Down(bool front) { + void ActionExecutor::Down(BaseAction::front front) { if (action_done_) { action_ = std::make_shared(shared_from_this(), front); @@ -160,7 +160,7 @@ namespace Modelec } } - void ActionExecutor::Up(bool front) { + void ActionExecutor::Up(BaseAction::front front) { if (action_done_) { action_ = std::make_shared(shared_from_this(), front); diff --git a/src/modelec_strat/src/missions/free_mission.cpp b/src/modelec_strat/src/missions/free_mission.cpp index 09ffb77..dd6ad37 100644 --- a/src/modelec_strat/src/missions/free_mission.cpp +++ b/src/modelec_strat/src/missions/free_mission.cpp @@ -1,5 +1,7 @@ #include +#include "modelec_strat/action/base_action.hpp" + namespace Modelec { FreeMission::FreeMission(const std::shared_ptr& nav, const std::shared_ptr& action_executor) : step_(GO_TO_FREE), status_(MissionStatus::READY), nav_(nav), action_executor_(action_executor) { @@ -24,9 +26,9 @@ namespace Modelec { if (!nav_->HasArrived()) { - if ((node_->now() - go_timeout_).seconds() < 2) + if ((node_->now() - go_timeout_).seconds() < 5) { - // nav_->AskWaypoint(); + nav_->AskWaypoint(); return; } if ((node_->now() - go_timeout_).seconds() < 10) @@ -55,17 +57,33 @@ namespace Modelec { angle + M_PI), true, Pathfinding::FREE); go_timeout_ = node_->now(); + } - step_ = FREE; + step_ = FREE; + break; + case DOWN: + { + action_executor_->Down(BaseAction::FRONT); + deploy_time_ = node_->now(); } + + step_ = FREE; break; case FREE: { - action_executor_->Down(); + action_executor_->Free(); deploy_time_ = node_->now(); + } - step_ = DONE; + step_ = UP; + break; + case UP: + { + action_executor_->Up(BaseAction::FRONT); + deploy_time_ = node_->now(); } + + step_ = DONE; break; case DONE: { @@ -88,4 +106,4 @@ namespace Modelec { return status_; } -} \ No newline at end of file +} diff --git a/src/modelec_strat/src/missions/take_mission.cpp b/src/modelec_strat/src/missions/take_mission.cpp index d2ddbcb..9d2f54a 100644 --- a/src/modelec_strat/src/missions/take_mission.cpp +++ b/src/modelec_strat/src/missions/take_mission.cpp @@ -1,5 +1,7 @@ #include +#include "modelec_strat/action/base_action.hpp" + namespace Modelec { TakeMission::TakeMission(const std::shared_ptr& nav, const std::shared_ptr& action_executor) : step_(GO_TO_TAKE), status_(MissionStatus::READY), nav_(nav), action_executor_(action_executor) { @@ -24,9 +26,9 @@ namespace Modelec { if (!nav_->HasArrived()) { - if ((node_->now() - go_timeout_).seconds() < 2) + if ((node_->now() - go_timeout_).seconds() < 5) { - // nav_->AskWaypoint(); + nav_->AskWaypoint(); return; } if ((node_->now() - go_timeout_).seconds() < 10) @@ -51,11 +53,27 @@ namespace Modelec { go_timeout_ = node_->now(); } + step_ = DOWN; + break; + case DOWN: + { + action_executor_->Up(BaseAction::FRONT); + deploy_time_ = node_->now(); + } + step_ = TAKE; break; case TAKE: { - action_executor_->Up(); + action_executor_->Take(); + deploy_time_ = node_->now(); + } + + step_ = UP; + break; + case UP: + { + action_executor_->Up(BaseAction::FRONT); deploy_time_ = node_->now(); } @@ -78,4 +96,4 @@ namespace Modelec { return status_; } -} \ No newline at end of file +} diff --git a/src/modelec_strat/src/strat_fms.cpp b/src/modelec_strat/src/strat_fms.cpp index 316587b..e5282d1 100644 --- a/src/modelec_strat/src/strat_fms.cpp +++ b/src/modelec_strat/src/strat_fms.cpp @@ -7,6 +7,8 @@ #include #include +#include "modelec_strat/action/base_action.hpp" + namespace Modelec { @@ -157,6 +159,8 @@ namespace Modelec .count(); start_time_pub_->publish(msg); + action_executor_->Up(BaseAction::Front::BOTH); + Transition(State::SELECT_MISSION, "Match started"); } break; From eea5746adbffa85e7ceff0f39cef1e133dae27a1 Mon Sep 17 00:00:00 2001 From: acki Date: Fri, 19 Dec 2025 22:26:10 +0100 Subject: [PATCH 32/43] rewrite action i'm not 100% ok with it but gonna test it --- .../modelec_strat/action/free_action.hpp | 10 ++-- .../modelec_strat/action/take_action.hpp | 11 +++-- .../include/modelec_strat/action_executor.hpp | 11 +++-- src/modelec_strat/src/action/free_action.cpp | 48 ++++++++++++++----- src/modelec_strat/src/action/take_action.cpp | 46 ++++++++++++------ src/modelec_strat/src/action/up_action.cpp | 6 +-- src/modelec_strat/src/action_executor.cpp | 48 ++++++++++++------- .../src/missions/free_mission.cpp | 2 +- .../src/missions/take_mission.cpp | 2 +- src/modelec_strat/src/strat_fms.cpp | 6 ++- 10 files changed, 125 insertions(+), 65 deletions(-) diff --git a/src/modelec_strat/include/modelec_strat/action/free_action.hpp b/src/modelec_strat/include/modelec_strat/action/free_action.hpp index 2167139..acb123a 100644 --- a/src/modelec_strat/include/modelec_strat/action/free_action.hpp +++ b/src/modelec_strat/include/modelec_strat/action/free_action.hpp @@ -10,17 +10,19 @@ namespace Modelec public: FreeAction(const std::shared_ptr& action_executor); FreeAction(const std::shared_ptr& action_executor, bool front, int n); + FreeAction(const std::shared_ptr& action_executor, std::pair servo); + FreeAction(const std::shared_ptr& action_executor, std::vector> servos); void Next() override; void Init(const std::vector& params) override; - void SetFront(bool front); - void SetN(int n); + void AddServo(int id, bool front); + void AddServo(std::pair servo); + void AddServos(const std::vector>& servos); inline static const std::string Name = ActionExec::FREE; private: - bool front_; - int n_; + std::vector> servos_; std::queue steps_; diff --git a/src/modelec_strat/include/modelec_strat/action/take_action.hpp b/src/modelec_strat/include/modelec_strat/action/take_action.hpp index 4ae0008..fd9f226 100644 --- a/src/modelec_strat/include/modelec_strat/action/take_action.hpp +++ b/src/modelec_strat/include/modelec_strat/action/take_action.hpp @@ -10,18 +10,19 @@ namespace Modelec public: TakeAction(const std::shared_ptr& action_executor); TakeAction(const std::shared_ptr& action_executor, bool front, int n); + TakeAction(const std::shared_ptr& action_executor, std::pair servo); + TakeAction(const std::shared_ptr& action_executor, std::vector> servos); void Next() override; void Init(const std::vector& params) override; - void Init(bool front, int n); - void SetFront(bool front); - void SetN(int n); + void AddServo(int id, bool front); + void AddServo(std::pair servo); + void AddServos(const std::vector>& servos); inline static const std::string Name = ActionExec::TAKE; private: - bool front_; - int n_; + std::vector> servos_; std::queue steps_; diff --git a/src/modelec_strat/include/modelec_strat/action_executor.hpp b/src/modelec_strat/include/modelec_strat/action_executor.hpp index b9cbc15..46c7e84 100644 --- a/src/modelec_strat/include/modelec_strat/action_executor.hpp +++ b/src/modelec_strat/include/modelec_strat/action_executor.hpp @@ -9,6 +9,8 @@ #include +#include "action/base_action.hpp" + namespace Modelec { class BaseAction; @@ -28,14 +30,13 @@ namespace Modelec void ReInit(); - void Down(BaseAction::Front front); - - void Up(BaseAction::Front front); + void Down(BaseAction::Front front, bool force = false); - void Take(bool front = true, int n = 0); + void Up(BaseAction::Front front, bool force = false); - void Free(bool front = true, int n = 0); + void Take(std::vector> servos, bool force = false); + void Free(std::vector> servos, bool force = false); void MoveServoTimed(const modelec_interfaces::msg::ActionServoTimedArray& msg); diff --git a/src/modelec_strat/src/action/free_action.cpp b/src/modelec_strat/src/action/free_action.cpp index 7ab3b24..fcc18a1 100644 --- a/src/modelec_strat/src/action/free_action.cpp +++ b/src/modelec_strat/src/action/free_action.cpp @@ -10,8 +10,17 @@ Modelec::FreeAction::FreeAction(const std::shared_ptr& action_ex Modelec::FreeAction::FreeAction(const std::shared_ptr& action_executor, bool front, int n) : FreeAction(action_executor) { - front_ = front; - n_ = n; + AddServo(n, front); +} + +Modelec::FreeAction::FreeAction(const std::shared_ptr& action_executor, std::pair servo) : FreeAction(action_executor) +{ + AddServo(servo.first, servo.second); +} + +Modelec::FreeAction::FreeAction(const std::shared_ptr& action_executor, std::vector> servos) : FreeAction(action_executor) +{ + AddServos(servos); } void Modelec::FreeAction::Next() @@ -31,12 +40,16 @@ void Modelec::FreeAction::Next() { modelec_interfaces::msg::ActionServoTimedArray msg; - msg.items.resize(1); + msg.items.resize(servos_.size()); + + for (size_t i = 0; i < servos_.size(); i++) + { + msg.items[i].id = servos_[i].first + (servos_[i].second ? 3 : 11); + msg.items[i].start_angle = servos_[i].second ? 0.8 : 0; + msg.items[i].end_angle = servos_[i].second ? 2.5 : 0; + msg.items[i].duration_s = 0.5; + } - msg.items[0].id = n_ + (front_ ? 3 : 11); - msg.items[0].start_angle = front_ ? 2.5 : 0; - msg.items[0].end_angle = front_ ? 0.8 : 0; - msg.items[0].duration_s = 0.5; action_executor_->MoveServoTimed(msg); } break; @@ -54,17 +67,26 @@ void Modelec::FreeAction::Init(const std::vector& params) { if (params.size() >= 2) { - SetFront(params[1] == "1" || params[1] == "true" || params[1] == "front"); - SetN(std::stoi(params[2])); + for (size_t i = 1; i < params.size(); i += 2) + { + int id = std::stoi(params[i]); + bool front = (i + 1 < params.size()) ? (params[i + 1] == "1" || params[i + 1] == "true" || params[i + 1] == "front") : true; + AddServo(id, front); + } } } -void Modelec::FreeAction::SetFront(bool front) +void Modelec::FreeAction::AddServo(int id, bool front) +{ + servos_.emplace_back(id, front); +} + +void Modelec::FreeAction::AddServo(std::pair servo) { - front_ = front; + servos_.emplace_back(servo); } -void Modelec::FreeAction::SetN(int n) +void Modelec::FreeAction::AddServos(const std::vector>& servos) { - n_ = n; + servos_.insert(servos_.end(), servos.begin(), servos.end()); } diff --git a/src/modelec_strat/src/action/take_action.cpp b/src/modelec_strat/src/action/take_action.cpp index be11108..01a1469 100644 --- a/src/modelec_strat/src/action/take_action.cpp +++ b/src/modelec_strat/src/action/take_action.cpp @@ -8,8 +8,17 @@ Modelec::TakeAction::TakeAction(const std::shared_ptr& action_ex Modelec::TakeAction::TakeAction(const std::shared_ptr& action_executor, bool front, int n) : TakeAction(action_executor) { - front_ = front; - n_ = n; + AddServo(n, front); +} + +Modelec::TakeAction::TakeAction(const std::shared_ptr& action_executor, std::pair servo) : TakeAction(action_executor) +{ + AddServo(servo); +} + +Modelec::TakeAction::TakeAction(const std::shared_ptr& action_executor, std::vector> servos) : TakeAction(action_executor) +{ + AddServos(servos); } void Modelec::TakeAction::Next() @@ -29,12 +38,16 @@ void Modelec::TakeAction::Next() { modelec_interfaces::msg::ActionServoTimedArray msg; - msg.items.resize(1); + msg.items.resize(servos_.size()); + + for (size_t i = 0; i < servos_.size(); i++) + { + msg.items[i].id = servos_[i].first + (servos_[i].second ? 3 : 11); + msg.items[i].start_angle = servos_[i].second ? 0.8 : 0; + msg.items[i].end_angle = servos_[i].second ? 2.7 : 0; + msg.items[i].duration_s = 0.5; + } - msg.items[0].id = n_ + (front_ ? 3 : 11); - msg.items[0].start_angle = front_ ? 0.8 : 0; - msg.items[0].end_angle = front_ ? 2.7 : 0; - msg.items[0].duration_s = 0.5; action_executor_->MoveServoTimed(msg); } break; @@ -52,21 +65,26 @@ void Modelec::TakeAction::Init(const std::vector& params) { if (params.size() >= 2) { - SetFront(params[1] == "1" || params[1] == "true" || params[1] == "front"); - SetN(std::stoi(params[2])); + for (size_t i = 1; i < params.size(); i += 2) + { + int id = std::stoi(params[i]); + bool front = (i + 1 < params.size()) ? (params[i + 1] == "1" || params[i + 1] == "true" || params[i + 1] == "front") : true; + AddServo(id, front); + } } } -void Modelec::TakeAction::Init(bool front, int n) +void Modelec::TakeAction::AddServo(int id, bool front) { + servos_.emplace_back(id, front); } -void Modelec::TakeAction::SetFront(bool front) +void Modelec::TakeAction::AddServo(std::pair servo) { - front_ = front; + servos_.emplace_back(servo); } -void Modelec::TakeAction::SetN(int n) +void Modelec::TakeAction::AddServos(const std::vector>& servos) { - n_ = n; + servos_.insert(servos_.end(), servos.begin(), servos.end()); } diff --git a/src/modelec_strat/src/action/up_action.cpp b/src/modelec_strat/src/action/up_action.cpp index 4774027..1cbeb21 100644 --- a/src/modelec_strat/src/action/up_action.cpp +++ b/src/modelec_strat/src/action/up_action.cpp @@ -8,7 +8,7 @@ Modelec::UPAction::UPAction(const std::shared_ptr& action_execut steps_.push(ActionExec::DONE_STEP); } -Modelec::UPAction::UPAction(const std::shared_ptr& action_executor, front front) : UPAction(action_executor) +Modelec::UPAction::UPAction(const std::shared_ptr& action_executor, Front front) : UPAction(action_executor) { front_ = front; } @@ -95,11 +95,11 @@ void Modelec::UPAction::Init(const std::vector& params) { if (!params.empty()) { - SetFront(static_cast(std::stoi(params[1]))); + SetFront(static_cast(std::stoi(params[1]))); } } -void Modelec::UPAction::SetFront(front front) +void Modelec::UPAction::SetFront(Front front) { front_ = front; } diff --git a/src/modelec_strat/src/action_executor.cpp b/src/modelec_strat/src/action_executor.cpp index 13380bb..0d36e83 100644 --- a/src/modelec_strat/src/action_executor.cpp +++ b/src/modelec_strat/src/action_executor.cpp @@ -88,19 +88,19 @@ namespace Modelec { if (msg->buttons[0] == 1) // A button { - Down(true); + Down(BaseAction::BOTH); } else if (msg->buttons[1] == 1) // B button { - Up(true); + Up(BaseAction::BOTH); } else if (msg->buttons[3] == 1) // X button { - Down(false); + Take({{0, true}, {1, true}, {2, true}, {3, true}}); } else if (msg->buttons[4] == 1) // Y button { - Up(false); + Free({{0, true}, {1, true}, {2, true}, {3, true}}); } } }); @@ -149,45 +149,57 @@ namespace Modelec action_ = nullptr; } - void ActionExecutor::Down(BaseAction::front front) { - if (action_done_) + void ActionExecutor::Down(BaseAction::Front front, bool force) { + if (action_done_ || force) { action_ = std::make_shared(shared_from_this(), front); + if (action_done_) + { + step_running_ = 0; + } action_done_ = false; - step_running_ = 0; Update(); } } - void ActionExecutor::Up(BaseAction::front front) { - if (action_done_) + void ActionExecutor::Up(BaseAction::Front front, bool force) { + if (action_done_ || force) { action_ = std::make_shared(shared_from_this(), front); + if (action_done_) + { + step_running_ = 0; + } action_done_ = false; - step_running_ = 0; Update(); } } - void ActionExecutor::Take(bool front, int n) { - if (action_done_) + void ActionExecutor::Take(std::vector> servos, bool force) { + if (action_done_ || force) { - action_ = std::make_shared(shared_from_this(), front, n); + action_ = std::make_shared(shared_from_this(), servos); + if (action_done_) + { + step_running_ = 0; + } action_done_ = false; - step_running_ = 0; Update(); } } - void ActionExecutor::Free(bool front, int n) { - if (action_done_) + void ActionExecutor::Free(std::vector> servos, bool force) { + if (action_done_ || force) { - action_ = std::make_shared(shared_from_this(), front, n); + action_ = std::make_shared(shared_from_this(), servos); + if (action_done_) + { + step_running_ = 0; + } action_done_ = false; - step_running_ = 0; Update(); } diff --git a/src/modelec_strat/src/missions/free_mission.cpp b/src/modelec_strat/src/missions/free_mission.cpp index dd6ad37..31fcb5b 100644 --- a/src/modelec_strat/src/missions/free_mission.cpp +++ b/src/modelec_strat/src/missions/free_mission.cpp @@ -71,7 +71,7 @@ namespace Modelec { break; case FREE: { - action_executor_->Free(); + action_executor_->Free({{0, true}, {1, true}, {2, true}, {3, true}}); deploy_time_ = node_->now(); } diff --git a/src/modelec_strat/src/missions/take_mission.cpp b/src/modelec_strat/src/missions/take_mission.cpp index 9d2f54a..3f9892c 100644 --- a/src/modelec_strat/src/missions/take_mission.cpp +++ b/src/modelec_strat/src/missions/take_mission.cpp @@ -65,7 +65,7 @@ namespace Modelec { break; case TAKE: { - action_executor_->Take(); + action_executor_->Take({{0, true}, {1, true}, {2, true}, {3, true}}); deploy_time_ = node_->now(); } diff --git a/src/modelec_strat/src/strat_fms.cpp b/src/modelec_strat/src/strat_fms.cpp index e5282d1..414218c 100644 --- a/src/modelec_strat/src/strat_fms.cpp +++ b/src/modelec_strat/src/strat_fms.cpp @@ -159,7 +159,11 @@ namespace Modelec .count(); start_time_pub_->publish(msg); - action_executor_->Up(BaseAction::Front::BOTH); + action_executor_->Up(BaseAction::Front::BOTH, true); + action_executor_->Free({ + {0, true}, {1, true}, {2, true}, {3, true}, + {0, false}, {1, false}, {2, false}, {3, false}, + }, true); Transition(State::SELECT_MISSION, "Match started"); } From 8b1e2637589c63da3ff047f7dec93f13d45c519c Mon Sep 17 00:00:00 2001 From: acki Date: Sat, 20 Dec 2025 22:01:40 +0100 Subject: [PATCH 33/43] rewrite action hope this work --- simulated_pcb/odo.py | 174 +++++++++--------- src/modelec_core/launch/modelec.launch.py | 10 +- .../include/modelec_gui/pages/home_page.hpp | 3 +- src/modelec_gui/src/pages/home_page.cpp | 100 +++++----- src/modelec_gui/src/pages/map_page.cpp | 1 - src/modelec_gui/src/pages/test_map_page.cpp | 1 - .../include/modelec_strat/action_executor.hpp | 4 + .../modelec_strat/missions/free_mission.hpp | 3 + .../modelec_strat/missions/take_mission.hpp | 2 + .../modelec_strat/navigation_helper.hpp | 11 +- .../include/modelec_strat/obstacle/box.hpp | 4 - .../modelec_strat/obstacle/obstacle.hpp | 55 +++--- .../include/modelec_strat/pami_manager.hpp | 2 +- .../src/missions/free_mission.cpp | 32 +++- .../src/missions/take_mission.cpp | 22 ++- src/modelec_strat/src/navigation_helper.cpp | 24 --- src/modelec_strat/src/obstacle/box.cpp | 10 - src/modelec_strat/src/obstacle/obstacle.cpp | 75 ++++++++ src/modelec_strat/src/pami_manager.cpp | 19 +- 19 files changed, 308 insertions(+), 244 deletions(-) diff --git a/simulated_pcb/odo.py b/simulated_pcb/odo.py index 3361c37..0ede088 100644 --- a/simulated_pcb/odo.py +++ b/simulated_pcb/odo.py @@ -14,16 +14,20 @@ def __init__(self, port='/dev/pts/6', baud=115200): self.x = 1225.0 self.y = 300.0 self.theta = math.pi / 2 + self.vx = 0.0 self.vy = 0.0 self.vtheta = 0.0 + self.last_update = time.time() self.last_send = time.time() self.pid = [0, 0, 0] self.tof = {0: 1000, 1: 1000, 2: 1000} - self.waypoints = {} # waypoints by id - self.waypoint_order = [] # ordered list of ids + + # --- waypoint system --- + self.waypoints = {} # id -> waypoint + self.waypoint_queue = [] # FIFO execution self.current_wp_id = None self.thread = threading.Thread(target=self.run) @@ -37,52 +41,53 @@ def update_position(self): dt = now - self.last_update self.last_update = now - if self.start: - if self.waypoint_order: - wp = self.waypoints[self.waypoint_order[0]] - dx = wp['x'] - self.x - dy = wp['y'] - self.y - distance = math.hypot(dx, dy) - angle = math.atan2(dy, dx) - angle_diff = self.normalize_angle(wp['theta'] - self.theta) - - if wp['type'] == 1: - if distance < 5.0 and abs(angle_diff) < 0.15: - self.vx = 0.0 - self.vy = 0.0 - self.vtheta = 0.0 - self.ser.write(f'SET;WAYPOINT;{wp["id"]}\n'.encode('utf-8')) - self.current_wp_id = wp['id'] - self.waypoint_order.pop(0) - del self.waypoints[wp['id']] - else: - speed = min(100.0, distance / 2) - self.vx = speed * math.cos(angle) - self.vy = speed * math.sin(angle) - self.vtheta = max(-9.0, min(9.0, angle_diff * 2)) - else: - speed = min(150.0, distance / 2) - self.vx = speed * math.cos(angle) - self.vy = speed * math.sin(angle) - self.vtheta = 0.0 - if distance < 100: - self.ser.write(f'SET;WAYPOINT;{wp["id"]}\n'.encode('utf-8')) - self.current_wp_id = wp['id'] - self.waypoint_order.pop(0) - del self.waypoints[wp['id']] - else: - self.vx = 0.0 - self.vy = 0.0 + if self.start and self.waypoint_queue: + wp_id = self.waypoint_queue[0] + wp = self.waypoints[wp_id] + + dx = wp['x'] - self.x + dy = wp['y'] - self.y + distance = math.hypot(dx, dy) + + target_angle = math.atan2(dy, dx) + theta_error = self.normalize_angle(wp['theta'] - self.theta) + + # --- motion --- + if wp['type'] == 1: # precise waypoint + speed = min(120.0, distance * 1) + self.vtheta = max(-6.0, min(6.0, theta_error * 2)) + else: # transit waypoint + speed = min(180.0, distance * 2) self.vtheta = 0.0 - self.x += self.vx * dt - self.y += self.vy * dt - self.theta += self.vtheta * dt - self.theta = self.normalize_angle(self.theta) + self.vx = speed * math.cos(target_angle) + self.vy = speed * math.sin(target_angle) + + reached_pos = distance < 5.0 + reached_theta = abs(theta_error) < 0.15 or wp['type'] == 0 + + if reached_pos and reached_theta: + self.vx = self.vy = self.vtheta = 0.0 + self.current_wp_id = wp_id + + self.ser.write(f"SET;WAYPOINT;REACH;{wp_id}\n".encode()) - if now - self.last_send > 0.1 and self.start: - print(f'[TX] SET;POS;{int(self.x)};{int(self.y)};{self.theta:.5f}') - self.ser.write(f'SET;POS;{int(self.x)};{int(self.y)};{self.theta:.5f}\n'.encode()) + self.waypoint_queue.pop(0) + del self.waypoints[wp_id] + + else: + self.vx = self.vy = self.vtheta = 0.0 + + # --- integrate --- + self.x += self.vx * dt + self.y += self.vy * dt + self.theta = self.normalize_angle(self.theta + self.vtheta * dt) + + # --- periodic position --- + if self.start and now - self.last_send > 0.1: + self.ser.write( + f"SET;POS;{int(self.x)};{int(self.y)};{self.theta:.5f}\n".encode() + ) self.last_send = now def run(self): @@ -98,73 +103,62 @@ def run(self): def handle_message(self, msg): if msg == "GET;POS": - print(f'[TX] SET;POS;{int(self.x)};{int(self.y)};{self.theta:.3f}') - self.ser.write(f'SET;POS;{int(self.x)};{int(self.y)};{self.theta:.3f}\n'.encode()) + self.ser.write( + f"SET;POS;{int(self.x)};{int(self.y)};{self.theta:.3f}\n".encode() + ) elif msg == "GET;SPEED": - print(f'[TX] SET;SPEED;{int(self.vx)};{int(self.vy)};{int(self.vtheta)}') - self.ser.write(f'SET;SPEED;{int(self.vx)};{int(self.vy)};{int(self.vtheta)}\n'.encode()) + self.ser.write( + f"SET;SPEED;{int(self.vx)};{int(self.vy)};{int(self.vtheta)}\n".encode() + ) elif msg.startswith("GET;DIST;"): n = int(msg.split(';')[2]) dist = self.tof.get(n, 0) - print(f'[TX] SET;DIST;{n};{dist}') - self.ser.write(f'SET;DIST;{n};{dist}\n'.encode()) + self.ser.write(f"SET;DIST;{n};{dist}\n".encode()) elif msg == "GET;PID": - print(f'[TX] SET;PID;{self.pid[0]};{self.pid[1]};{self.pid[2]}') - self.ser.write(f'SET;PID;{self.pid[0]};{self.pid[1]};{self.pid[2]}\n'.encode()) + self.ser.write(f"SET;PID;{self.pid[0]};{self.pid[1]};{self.pid[2]}\n".encode()) elif msg.startswith("SET;PID"): - print(f'[TX] OK;PID;1') - self.ser.write(f'OK;PID;1\n'.encode()) + self.ser.write(b"OK;PID;1\n") elif msg.startswith("SET;START;"): self.start = msg.split(';')[2] == '1' - print(f'[TX] OK;START;1') - self.ser.write(f'OK;START;1\n'.encode()) + self.ser.write(b"OK;START;1\n") + # --- MULTI WAYPOINT (clears previous) --- elif msg.startswith("SET;WAYPOINT;"): - parts = msg.split(';') - if len(parts) >= 7: + parts = msg.split(';')[2:] + + if len(parts) % 5 != 0: + self.ser.write(b"ERR;WAYPOINT;0\n") + return + + # clear existing waypoints + self.waypoints.clear() + self.waypoint_queue.clear() + + for i in range(0, len(parts), 5): wp = { - 'id': int(parts[2]), - 'type': int(parts[3]), - 'x': float(parts[4]), - 'y': float(parts[5]), - 'theta': float(parts[6]) + 'id': int(parts[i]), + 'type': int(parts[i + 1]), + 'x': float(parts[i + 2]), + 'y': float(parts[i + 3]), + 'theta': float(parts[i + 4]), } + self.waypoints[wp['id']] = wp - if wp['id'] not in self.waypoint_order: - self.waypoint_order.append(wp['id']) - self.ser.write(f'OK;WAYPOINT;{wp["id"]}\n'.encode()) + self.waypoint_queue.append(wp['id']) - elif msg.startswith("SET;WAYPOINTS;"): - parts = msg.split(';') - if len(parts) / 7 >= 1: - self.waypoints.clear() - self.waypoint_order.clear() - for i in range(2, len(parts), 5): - wp = { - 'id': int(parts[i]), - 'type': int(parts[i + 1]), - 'x': float(parts[i + 2]), - 'y': float(parts[i + 3]), - 'theta': float(parts[i + 4]) - } - self.waypoints[wp['id']] = wp - if wp['id'] not in self.waypoint_order: - self.waypoint_order.append(wp['id']) - self.ser.write(f'OK;WAYPOINT;{wp["id"]}\n'.encode()) - self.ser.write(b'OK;WAYPOINTS;1\n') + self.ser.write(f"OK;WAYPOINT;{wp['id']}\n".encode()) elif msg.startswith("SET;POS"): parts = msg.split(';') self.x = float(parts[2]) self.y = float(parts[3]) self.theta = float(parts[4]) - print(f'[TX] OK;POS;1') - self.ser.write(b'OK;POS;1\n') + self.ser.write(b"OK;POS;1\n") def stop(self): self.running = False @@ -174,13 +168,15 @@ def stop(self): if __name__ == '__main__': parser = argparse.ArgumentParser(description="Simulated PCB") - parser.add_argument('--port', type=str, default='/dev/pts/6', help='Serial port to use') + parser.add_argument('--port', type=str, default='/dev/pts/6') args = parser.parse_args() sim = None try: sim = SimulatedPCB(port=args.port) + while True: + time.sleep(1) except KeyboardInterrupt: if sim: sim.stop() - print("Simulation stopped") \ No newline at end of file + print("Simulation stopped") diff --git a/src/modelec_core/launch/modelec.launch.py b/src/modelec_core/launch/modelec.launch.py index 90be96b..5af9289 100644 --- a/src/modelec_core/launch/modelec.launch.py +++ b/src/modelec_core/launch/modelec.launch.py @@ -99,7 +99,7 @@ def launch_com(context, *args, **kwargs): executable='pcb_odo_interface', name='pcb_odo_interface', parameters=[{ - 'serial_port': "/dev/USB_ODO", + 'serial_port': "/tmp/USB_ODO", 'baudrate': 115200, 'name': "pcb_odo", }] @@ -109,7 +109,7 @@ def launch_com(context, *args, **kwargs): executable='pcb_action_interface', name='pcb_action_interface', parameters=[{ - 'serial_port': "/dev/USB_ACTION", + 'serial_port': "/tmp/USB_ACTION", 'baudrate': 115200, 'name': "pcb_action", }] @@ -138,11 +138,7 @@ def launch_joy(context, *args, **kwargs): package='joy', executable='joy_node', name='joy_node', - output='screen', - parameters=[{ - 'dev': '/dev/input/js0', # optional: specify your joystick device - 'deadzone': 0.05 - }] + output='screen' ) return [joy] return [] diff --git a/src/modelec_gui/include/modelec_gui/pages/home_page.hpp b/src/modelec_gui/include/modelec_gui/pages/home_page.hpp index 1f423dd..7de1f4e 100644 --- a/src/modelec_gui/include/modelec_gui/pages/home_page.hpp +++ b/src/modelec_gui/include/modelec_gui/pages/home_page.hpp @@ -40,6 +40,7 @@ namespace ModelecGUI QSvgRenderer* renderer_; - std::vector spawn_buttons_; + QPushButton* yellow_spawn_buttons_; + QPushButton* blue_spawn_buttons_; }; } diff --git a/src/modelec_gui/src/pages/home_page.cpp b/src/modelec_gui/src/pages/home_page.cpp index a8c6e45..5bc00ac 100644 --- a/src/modelec_gui/src/pages/home_page.cpp +++ b/src/modelec_gui/src/pages/home_page.cpp @@ -12,52 +12,64 @@ namespace ModelecGUI { spawn_pub_ = node_->create_publisher("strat/spawn", 10); - auto w = Modelec::Config::get("config.spawn.width_mm"); - auto h = Modelec::Config::get("config.spawn.height_mm"); - - spawn_sub_ = node_->create_subscription("nav/spawn", 10, - [this, w, h](const modelec_interfaces::msg::Spawn::SharedPtr msg) - { - auto ratioX = 1200 / 3000.0f; - auto ratioY = 800 / 2000.0f; - - auto* button = new QPushButton(this); - spawn_buttons_.push_back(button); - - button->setText(msg->team_id == 0 ? "Yellow" : "Blue"); - button->setStyleSheet( - msg->team_id == 0 - ? "background-color: rgba(255, 255, 0, 128); border: none; color: black; font-size: 24px;" - : "background-color: rgba(0, 0, 255, 128); border: none; color: white; font-size: 24px;" - ); - - button->move( - static_cast(msg->x * ratioX - (w * ratioX) / 2), - static_cast(800 - msg->y * ratioY - (h * ratioY) / 2) - ); - - button->setFixedSize(w * ratioX, h * ratioY); - - button->show(); - - connect(button, &QPushButton::clicked, this, [this, msg]() - { - modelec_interfaces::msg::Spawn team_msg; - team_msg.team_id = msg->team_id; - team_msg.name = modelec_interfaces::msg::Spawn::TOP; - spawn_pub_->publish(team_msg); - - emit TeamChoose(); - }); - }); + auto* layout = new QHBoxLayout(this); + layout->setContentsMargins(0, 0, 0, 0); + layout->setSpacing(0); + + yellow_spawn_buttons_ = new QPushButton("Yellow", this); + blue_spawn_buttons_ = new QPushButton("Blue", this); + + yellow_spawn_buttons_->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); + blue_spawn_buttons_->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); + + yellow_spawn_buttons_->setStyleSheet( + "QPushButton {" + " background-color: rgba(255, 215, 0, 140);" + " border: none;" + " font-size: 32px;" + " font-weight: bold;" + " color: black;" + "}" + "QPushButton:hover {" + " background-color: rgba(255, 215, 0, 180);" + "}" + ); + + blue_spawn_buttons_->setStyleSheet( + "QPushButton {" + " background-color: rgba(0, 90, 200, 140);" + " border: none;" + " font-size: 32px;" + " font-weight: bold;" + " color: white;" + "}" + "QPushButton:hover {" + " background-color: rgba(0, 90, 200, 180);" + "}" + ); + + layout->addWidget(yellow_spawn_buttons_, 1); + layout->addWidget(blue_spawn_buttons_, 1); + + connect(yellow_spawn_buttons_, &QPushButton::clicked, this, [this]() + { + modelec_interfaces::msg::Spawn msg; + msg.team_id = 0; + msg.name = modelec_interfaces::msg::Spawn::TOP; + spawn_pub_->publish(msg); + emit TeamChoose(); + }); + + connect(blue_spawn_buttons_, &QPushButton::clicked, this, [this]() + { + modelec_interfaces::msg::Spawn msg; + msg.team_id = 1; + msg.name = modelec_interfaces::msg::Spawn::TOP; + spawn_pub_->publish(msg); + emit TeamChoose(); + }); reset_strat_pub_ = node_->create_publisher("strat/reset", 10); - - ask_spawn_client_ = node_->create_client("nav/ask_spawn"); - ask_spawn_client_->wait_for_service(); - auto ask_spawn_request_ = std::make_shared(); - auto res = ask_spawn_client_->async_send_request(ask_spawn_request_); - rclcpp::spin_until_future_complete(node_->get_node_base_interface(), res); } void HomePage::Init() diff --git a/src/modelec_gui/src/pages/map_page.cpp b/src/modelec_gui/src/pages/map_page.cpp index c32d62f..9a2fe1f 100644 --- a/src/modelec_gui/src/pages/map_page.cpp +++ b/src/modelec_gui/src/pages/map_page.cpp @@ -167,7 +167,6 @@ namespace ModelecGUI { if (auto res = result.get()) { - RCLCPP_INFO(node_->get_logger(), "Map received: %d x %d", res->width, res->height); map_width_ = res->width; map_height_ = res->height; } diff --git a/src/modelec_gui/src/pages/test_map_page.cpp b/src/modelec_gui/src/pages/test_map_page.cpp index b4c1f56..8d09f26 100644 --- a/src/modelec_gui/src/pages/test_map_page.cpp +++ b/src/modelec_gui/src/pages/test_map_page.cpp @@ -118,7 +118,6 @@ namespace ModelecGUI { if (auto res = result.get()) { - RCLCPP_INFO(node_->get_logger(), "Map received: %d x %d", res->width, res->height); map_width_ = res->width; map_height_ = res->height; } diff --git a/src/modelec_strat/include/modelec_strat/action_executor.hpp b/src/modelec_strat/include/modelec_strat/action_executor.hpp index 46c7e84..795c015 100644 --- a/src/modelec_strat/include/modelec_strat/action_executor.hpp +++ b/src/modelec_strat/include/modelec_strat/action_executor.hpp @@ -10,6 +10,7 @@ #include #include "action/base_action.hpp" +#include "obstacle/box.hpp" namespace Modelec { @@ -41,6 +42,9 @@ namespace Modelec void MoveServoTimed(const modelec_interfaces::msg::ActionServoTimedArray& msg); void MoveServo(const modelec_interfaces::msg::ActionServoPosArray& msg); + + std::array, 2> box_obstacles_; + protected: rclcpp::Publisher::SharedPtr asc_move_pub_; rclcpp::Publisher::SharedPtr servo_move_pub_; diff --git a/src/modelec_strat/include/modelec_strat/missions/free_mission.hpp b/src/modelec_strat/include/modelec_strat/missions/free_mission.hpp index 9571bd9..34738a0 100644 --- a/src/modelec_strat/include/modelec_strat/missions/free_mission.hpp +++ b/src/modelec_strat/include/modelec_strat/missions/free_mission.hpp @@ -23,6 +23,7 @@ namespace Modelec { DOWN, FREE, UP, + GO_BACK, DONE, } step_; @@ -33,6 +34,8 @@ namespace Modelec { std::shared_ptr target_deposite_zone_; + double angle_; + rclcpp::Time go_timeout_; rclcpp::Time deploy_time_; }; diff --git a/src/modelec_strat/include/modelec_strat/missions/take_mission.hpp b/src/modelec_strat/include/modelec_strat/missions/take_mission.hpp index 4893be1..64f7db5 100644 --- a/src/modelec_strat/include/modelec_strat/missions/take_mission.hpp +++ b/src/modelec_strat/include/modelec_strat/missions/take_mission.hpp @@ -20,12 +20,14 @@ namespace Modelec { enum Step { GO_TO_TAKE, + GO_TO_TAKE_CLOSE, DOWN, TAKE, UP, DONE, } step_; + std::shared_ptr closestBox; MissionStatus status_; std::shared_ptr nav_; std::shared_ptr action_executor_; diff --git a/src/modelec_strat/include/modelec_strat/navigation_helper.hpp b/src/modelec_strat/include/modelec_strat/navigation_helper.hpp index c89b32e..0e0a209 100644 --- a/src/modelec_strat/include/modelec_strat/navigation_helper.hpp +++ b/src/modelec_strat/include/modelec_strat/navigation_helper.hpp @@ -188,11 +188,14 @@ namespace Modelec { if (auto obs = std::dynamic_pointer_cast(obstacle.second)) { - auto dist = Point::distance(robotPos, obs->GetPosition()); - if (dist < distance) + if (!obs->IsAtObjective()) { - distance = dist; - closest_obstacle = obs; + auto dist = Point::distance(robotPos, obs->GetPosition()); + if (dist < distance) + { + distance = dist; + closest_obstacle = obs; + } } } } diff --git a/src/modelec_strat/include/modelec_strat/obstacle/box.hpp b/src/modelec_strat/include/modelec_strat/obstacle/box.hpp index 4124eb8..18f5f8a 100644 --- a/src/modelec_strat/include/modelec_strat/obstacle/box.hpp +++ b/src/modelec_strat/include/modelec_strat/obstacle/box.hpp @@ -15,16 +15,12 @@ namespace Modelec BoxObstacle(tinyxml2::XMLElement* obstacleElem); BoxObstacle(const modelec_interfaces::msg::Obstacle& msg); - bool IsAtObjective() const; - void SetAtObjective(bool atObjective); - Point GetOptimizedGetPos(const modelec_interfaces::msg::OdometryPos::SharedPtr& msg) const; Point GetOptimizedGetPos(const Point& currentPos) const; std::vector GetAllPossiblePositions() const; protected: - bool isAtObjective = false; std::vector possible_angles_; }; diff --git a/src/modelec_strat/include/modelec_strat/obstacle/obstacle.hpp b/src/modelec_strat/include/modelec_strat/obstacle/obstacle.hpp index 3676e1d..130c3ed 100644 --- a/src/modelec_strat/include/modelec_strat/obstacle/obstacle.hpp +++ b/src/modelec_strat/include/modelec_strat/obstacle/obstacle.hpp @@ -26,46 +26,37 @@ namespace Modelec virtual modelec_interfaces::msg::Obstacle toMsg() const; - int GetId() const { return id_; } - int GetX() const { return x_; } - int GetY() const { return y_; } - double GetTheta() const { return theta_; } - int GetWidth() const { return w_; } - int GetHeight() const { return h_; } - const std::string& Type() const { return type_; } - Point GetPosition() const { return Point(x_, y_, theta_); } + int GetId() const; + int GetX() const; + int GetY() const; + double GetTheta() const; + int GetWidth() const; + int GetHeight() const; + const std::string& Type() const; + Point GetPosition() const; - void SetId(int id) { id_ = id; } - void SetX(int x) { x_ = x; } - void SetY(int y) { y_ = y; } - void SetTheta(double theta) { theta_ = theta; } - void SetWidth(int w) { w_ = w; } - void SetHeight(int h) { h_ = h; } - void SetType(const std::string& type) { type_ = type; } + void SetId(int id); + void SetX(int x); + void SetY(int y); + void SetTheta(double theta); + void SetWidth(int w); + void SetHeight(int h); + void SetType(const std::string& type); - void SetPosition(int x, int y, double theta) - { - x_ = x; - y_ = y; - theta_ = theta; - } + void SetPosition(int x, int y, double theta); - void SetPosition(const Point& p) - { - x_ = p.x; - y_ = p.y; - theta_ = p.theta; - } + void SetPosition(const Point& p); - void SetSize(int w, int h) - { - w_ = w; - h_ = h; - } + void SetSize(int w, int h); + + bool IsAtObjective() const; + void SetAtObjective(bool atObjective); protected: int id_, x_, y_, w_, h_; double theta_; std::string type_; + + bool isAtObjective = false; }; } diff --git a/src/modelec_strat/include/modelec_strat/pami_manager.hpp b/src/modelec_strat/include/modelec_strat/pami_manager.hpp index c3aecdb..2dee336 100644 --- a/src/modelec_strat/include/modelec_strat/pami_manager.hpp +++ b/src/modelec_strat/include/modelec_strat/pami_manager.hpp @@ -35,7 +35,7 @@ namespace Modelec int score_free_zone_ = 0; rclcpp::TimerBase::SharedPtr timer_add_; - rclcpp::TimerBase::SharedPtr timer_remove_; + // rclcpp::TimerBase::SharedPtr timer_remove_; rclcpp::Subscription::SharedPtr start_time_sub_; rclcpp::Publisher::SharedPtr add_obs_pub_; diff --git a/src/modelec_strat/src/missions/free_mission.cpp b/src/modelec_strat/src/missions/free_mission.cpp index 31fcb5b..6bdc344 100644 --- a/src/modelec_strat/src/missions/free_mission.cpp +++ b/src/modelec_strat/src/missions/free_mission.cpp @@ -28,8 +28,8 @@ namespace Modelec { { if ((node_->now() - go_timeout_).seconds() < 5) { - nav_->AskWaypoint(); - return; + // nav_->AskWaypoint(); + // return; } if ((node_->now() - go_timeout_).seconds() < 10) { @@ -50,11 +50,11 @@ namespace Modelec { auto depoPoint = target_deposite_zone_->GetPosition(); - auto angle = atan2(depoPoint.y - currPos->y, + angle_ = atan2(depoPoint.y - currPos->y, depoPoint.x - currPos->x); nav_->GoToRotateFirst(depoPoint.GetTakePosition(dist, - angle + M_PI), true, Pathfinding::FREE); + angle_ + M_PI), true, Pathfinding::FREE); go_timeout_ = node_->now(); } @@ -73,6 +73,20 @@ namespace Modelec { { action_executor_->Free({{0, true}, {1, true}, {2, true}, {3, true}}); deploy_time_ = node_->now(); + + auto obs = action_executor_->box_obstacles_[0]; + action_executor_->box_obstacles_[0] = nullptr; + + auto pos = nav_->GetCurrentPos(); + + obs->SetPosition( + pos->x + 250 * cos(pos->theta), + pos->y + 250 * sin(pos->theta), + pos->theta); + + obs->SetAtObjective(true); + + nav_->GetPathfinding()->AddObstacle(obs); } step_ = UP; @@ -83,6 +97,16 @@ namespace Modelec { deploy_time_ = node_->now(); } + step_ = GO_BACK; + break; + case GO_BACK: + { + nav_->GoToRotateFirst(target_deposite_zone_->GetPosition().GetTakePosition(350, + angle_ + M_PI), true, Pathfinding::FREE | Pathfinding::OBSTACLE); + + go_timeout_ = node_->now(); + } + step_ = DONE; break; case DONE: diff --git a/src/modelec_strat/src/missions/take_mission.cpp b/src/modelec_strat/src/missions/take_mission.cpp index 3f9892c..a33c59e 100644 --- a/src/modelec_strat/src/missions/take_mission.cpp +++ b/src/modelec_strat/src/missions/take_mission.cpp @@ -28,8 +28,8 @@ namespace Modelec { { if ((node_->now() - go_timeout_).seconds() < 5) { - nav_->AskWaypoint(); - return; + // nav_->AskWaypoint(); + // return; } if ((node_->now() - go_timeout_).seconds() < 10) { @@ -42,17 +42,28 @@ namespace Modelec { case GO_TO_TAKE: { - auto closestBox = nav_->GetClosestObstacle(nav_->GetCurrentPos()); + closestBox = nav_->GetClosestObstacle(nav_->GetCurrentPos()); - auto pos = closestBox->GetOptimizedGetPos(nav_->GetCurrentPos()).GetTakeBasePosition(); + action_executor_->box_obstacles_[0] = closestBox; - nav_->GetPathfinding()->RemoveObstacle(closestBox->GetId()); + auto pos = closestBox->GetOptimizedGetPos(nav_->GetCurrentPos()).GetTakeBasePosition(); nav_->GoToRotateFirst(pos, true, Pathfinding::FREE | Pathfinding::WALL); go_timeout_ = node_->now(); } + step_ = GO_TO_TAKE_CLOSE; + break; + case GO_TO_TAKE_CLOSE: + { + auto pos = closestBox->GetOptimizedGetPos(nav_->GetCurrentPos()).GetTakeClosePosition(); + + nav_->GoToRotateFirst(pos, true, Pathfinding::FREE | Pathfinding::WALL | Pathfinding::OBSTACLE); + + go_timeout_ = node_->now(); + } + step_ = DOWN; break; case DOWN: @@ -80,6 +91,7 @@ namespace Modelec { step_ = DONE; break; case DONE: + nav_->GetPathfinding()->RemoveObstacle(closestBox->GetId()); status_ = MissionStatus::DONE; break; default: diff --git a/src/modelec_strat/src/navigation_helper.cpp b/src/modelec_strat/src/navigation_helper.cpp index 0dbed84..525c249 100644 --- a/src/modelec_strat/src/navigation_helper.cpp +++ b/src/modelec_strat/src/navigation_helper.cpp @@ -761,34 +761,10 @@ namespace Modelec Config::get("config.spawn.yellow.top@theta") ); - spawn_yellow_["side"] = Point( - Config::get("config.spawn.yellow.side@x"), - Config::get("config.spawn.yellow.side@y"), - Config::get("config.spawn.yellow.side@theta") - ); - - spawn_yellow_["bottom"] = Point( - Config::get("config.spawn.yellow.bottom@x"), - Config::get("config.spawn.yellow.bottom@y"), - Config::get("config.spawn.yellow.bottom@theta") - ); - spawn_blue_["top"] = Point( Config::get("config.spawn.blue.top@x"), Config::get("config.spawn.blue.top@y"), Config::get("config.spawn.blue.top@theta") ); - - spawn_blue_["side"] = Point( - Config::get("config.spawn.blue.side@x"), - Config::get("config.spawn.blue.side@y"), - Config::get("config.spawn.blue.side@theta") - ); - - spawn_blue_["bottom"] = Point( - Config::get("config.spawn.blue.bottom@x"), - Config::get("config.spawn.blue.bottom@y"), - Config::get("config.spawn.blue.bottom@theta") - ); } } \ No newline at end of file diff --git a/src/modelec_strat/src/obstacle/box.cpp b/src/modelec_strat/src/obstacle/box.cpp index b3dee90..31a29d3 100644 --- a/src/modelec_strat/src/obstacle/box.cpp +++ b/src/modelec_strat/src/obstacle/box.cpp @@ -24,16 +24,6 @@ namespace Modelec { } - bool BoxObstacle::IsAtObjective() const - { - return isAtObjective; - } - - void BoxObstacle::SetAtObjective(bool atObjective) - { - isAtObjective = atObjective; - } - Point BoxObstacle::GetOptimizedGetPos(const modelec_interfaces::msg::OdometryPos::SharedPtr& msg) const { Point p = Point(msg->x, msg->y, msg->theta); diff --git a/src/modelec_strat/src/obstacle/obstacle.cpp b/src/modelec_strat/src/obstacle/obstacle.cpp index 5fbdb1f..575e524 100644 --- a/src/modelec_strat/src/obstacle/obstacle.cpp +++ b/src/modelec_strat/src/obstacle/obstacle.cpp @@ -43,4 +43,79 @@ namespace Modelec return msg; } + + int Obstacle::GetId() const + { return id_; } + + int Obstacle::GetX() const + { return x_; } + + int Obstacle::GetY() const + { return y_; } + + double Obstacle::GetTheta() const + { return theta_; } + + int Obstacle::GetWidth() const + { return w_; } + + int Obstacle::GetHeight() const + { return h_; } + + const std::string& Obstacle::Type() const + { return type_; } + + Point Obstacle::GetPosition() const + { return Point(x_, y_, theta_); } + + void Obstacle::SetId(int id) + { id_ = id; } + + void Obstacle::SetX(int x) + { x_ = x; } + + void Obstacle::SetY(int y) + { y_ = y; } + + void Obstacle::SetTheta(double theta) + { theta_ = theta; } + + void Obstacle::SetWidth(int w) + { w_ = w; } + + void Obstacle::SetHeight(int h) + { h_ = h; } + + void Obstacle::SetType(const std::string& type) + { type_ = type; } + + void Obstacle::SetPosition(int x, int y, double theta) + { + x_ = x; + y_ = y; + theta_ = theta; + } + + void Obstacle::SetPosition(const Point& p) + { + x_ = p.x; + y_ = p.y; + theta_ = p.theta; + } + + void Obstacle::SetSize(int w, int h) + { + w_ = w; + h_ = h; + } + + bool Obstacle::IsAtObjective() const + { + return isAtObjective; + } + + void Obstacle::SetAtObjective(bool atObjective) + { + isAtObjective = atObjective; + } } diff --git a/src/modelec_strat/src/pami_manager.cpp b/src/modelec_strat/src/pami_manager.cpp index adfdb6b..7b13442 100644 --- a/src/modelec_strat/src/pami_manager.cpp +++ b/src/modelec_strat/src/pami_manager.cpp @@ -37,21 +37,6 @@ namespace Modelec auto goal = std::chrono::nanoseconds(msg->data) + std::chrono::seconds(time_to_remove_top_pot_); auto second_goal = std::chrono::nanoseconds(msg->data) + std::chrono::seconds(time_to_put_zone_); - timer_remove_ = create_wall_timer( - goal - now, - [this]() - { - modelec_interfaces::msg::Obstacle topLeft; - topLeft.id = 10; - remove_obs_pub_->publish(topLeft); - - modelec_interfaces::msg::Obstacle topRight; - topRight.id = 20; - remove_obs_pub_->publish(topRight); - - timer_remove_->cancel(); - }); - timer_add_ = create_wall_timer( second_goal - now, [this]() @@ -84,10 +69,10 @@ namespace Modelec { timer_add_->cancel(); } - if (timer_remove_) + /*if (timer_remove_) { timer_remove_->cancel(); - } + }*/ }); } From c55ab84cf14db7cf9a9aac4560745a26f5ea7228 Mon Sep 17 00:00:00 2001 From: acki Date: Sat, 17 Jan 2026 10:42:44 +0100 Subject: [PATCH 34/43] start using a top level cmake so it's easier with CLion --- CMakeLists.txt | 45 +++++++++++++++++++++++++++++++++++++++++++++ test.zsh | 6 ++++++ 2 files changed, 51 insertions(+) create mode 100644 CMakeLists.txt create mode 100644 test.zsh diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..84e9a7d --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,45 @@ +cmake_minimum_required(VERSION 3.14) +project("ROS2 Master") + +# usually I put this in a separate file include("/opt/ros/_common/Colcon.cmake") +function(colcon_add_subdirectories) + cmake_parse_arguments(PARSE_ARGV 0 "ARG" "" "BUILD_BASE;BASE_PATHS" "") + + message("search criteria: ${ARGV}") + + execute_process(COMMAND colcon list + --paths-only + --base-paths ${ARG_BASE_PATHS} + --topological-order + ${ARG_UNPARSED_ARGUMENTS} + OUTPUT_VARIABLE paths) + string(STRIP "${paths}" paths) + string(REPLACE "\n" ";" paths "${paths}") + + MESSAGE("colcon shows paths ${paths}") + + foreach(path IN LISTS paths) + message("...examining ${path}") + # if(EXISTS "${path}/CMakeLists.txt") + execute_process(COMMAND colcon info --paths "${path}" OUTPUT_VARIABLE package_info) + if(NOT "${package_info}" MATCHES "type:[ \t]+(cmake|ros.ament_cmake|ros.cmake)") + message("skipping non-cmake project") + elseif(NOT "${package_info}" MATCHES "name:[ \t]+([^ \r\n\t]*)") + message(WARNING "could not identify package at ${path}") + else() + set(name "${CMAKE_MATCH_1}") + message("...adding package ${name} from path ${path}") + MESSAGE("package info: ${package_info}") + + get_filename_component(BUILD_PATH "${name}" ABSOLUTE BASE_DIR "${ARG_BUILD_BASE}") + + add_subdirectory("${path}" "${BUILD_PATH}") + endif() + endforeach() +endfunction() + +colcon_add_subdirectories( + BUILD_BASE "${PROJECT_SOURCE_DIR}/build" + BASE_PATHS "${PROJECT_SOURCE_DIR}/src/" + --packages-select modelec_utils +) \ No newline at end of file diff --git a/test.zsh b/test.zsh new file mode 100644 index 0000000..2f3dd65 --- /dev/null +++ b/test.zsh @@ -0,0 +1,6 @@ +ros_env="AMENT_PREFIX_PATH CMAKE_PREFIX_PATH COLCON_PREFIX_PATH PKG_CONFIG_PATH PYTHONPATH LD_LIBRARY_PATH PATH ROS_DISTRO ROS_PYTHON_VERSION ROS_LOCALHOST_ONLY ROS_VERSION" + +for e in ${=ros_env}; do + # In Zsh, ${(P)e} performs the indirect expansion + echo "$e=${(P)e}" +done \ No newline at end of file From d4905f3eb12ebe134a65be705ea9b63fd02d84f0 Mon Sep 17 00:00:00 2001 From: Modelec Date: Tue, 20 Jan 2026 18:28:03 +0100 Subject: [PATCH 35/43] change to real serial port --- src/modelec_core/launch/modelec.launch.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modelec_core/launch/modelec.launch.py b/src/modelec_core/launch/modelec.launch.py index 5af9289..62a073d 100644 --- a/src/modelec_core/launch/modelec.launch.py +++ b/src/modelec_core/launch/modelec.launch.py @@ -99,7 +99,7 @@ def launch_com(context, *args, **kwargs): executable='pcb_odo_interface', name='pcb_odo_interface', parameters=[{ - 'serial_port': "/tmp/USB_ODO", + 'serial_port': "/dev/USB_ODO", 'baudrate': 115200, 'name': "pcb_odo", }] @@ -109,7 +109,7 @@ def launch_com(context, *args, **kwargs): executable='pcb_action_interface', name='pcb_action_interface', parameters=[{ - 'serial_port': "/tmp/USB_ACTION", + 'serial_port': "/dev/USB_ACTION", 'baudrate': 115200, 'name': "pcb_action", }] From 2078e8f6a8aaa1763fe00a928fd7e7b324fc6107 Mon Sep 17 00:00:00 2001 From: acki Date: Tue, 20 Jan 2026 19:12:44 +0100 Subject: [PATCH 36/43] update mission --- CMakeLists.txt | 45 ------------------- simulated_pcb/action.py | 8 ++-- .../src/missions/take_mission.cpp | 2 +- test.zsh | 6 --- 4 files changed, 5 insertions(+), 56 deletions(-) delete mode 100644 CMakeLists.txt delete mode 100644 test.zsh diff --git a/CMakeLists.txt b/CMakeLists.txt deleted file mode 100644 index 84e9a7d..0000000 --- a/CMakeLists.txt +++ /dev/null @@ -1,45 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project("ROS2 Master") - -# usually I put this in a separate file include("/opt/ros/_common/Colcon.cmake") -function(colcon_add_subdirectories) - cmake_parse_arguments(PARSE_ARGV 0 "ARG" "" "BUILD_BASE;BASE_PATHS" "") - - message("search criteria: ${ARGV}") - - execute_process(COMMAND colcon list - --paths-only - --base-paths ${ARG_BASE_PATHS} - --topological-order - ${ARG_UNPARSED_ARGUMENTS} - OUTPUT_VARIABLE paths) - string(STRIP "${paths}" paths) - string(REPLACE "\n" ";" paths "${paths}") - - MESSAGE("colcon shows paths ${paths}") - - foreach(path IN LISTS paths) - message("...examining ${path}") - # if(EXISTS "${path}/CMakeLists.txt") - execute_process(COMMAND colcon info --paths "${path}" OUTPUT_VARIABLE package_info) - if(NOT "${package_info}" MATCHES "type:[ \t]+(cmake|ros.ament_cmake|ros.cmake)") - message("skipping non-cmake project") - elseif(NOT "${package_info}" MATCHES "name:[ \t]+([^ \r\n\t]*)") - message(WARNING "could not identify package at ${path}") - else() - set(name "${CMAKE_MATCH_1}") - message("...adding package ${name} from path ${path}") - MESSAGE("package info: ${package_info}") - - get_filename_component(BUILD_PATH "${name}" ABSOLUTE BASE_DIR "${ARG_BUILD_BASE}") - - add_subdirectory("${path}" "${BUILD_PATH}") - endif() - endforeach() -endfunction() - -colcon_add_subdirectories( - BUILD_BASE "${PROJECT_SOURCE_DIR}/build" - BASE_PATHS "${PROJECT_SOURCE_DIR}/src/" - --packages-select modelec_utils -) \ No newline at end of file diff --git a/simulated_pcb/action.py b/simulated_pcb/action.py index cab53ea..edc6ae2 100644 --- a/simulated_pcb/action.py +++ b/simulated_pcb/action.py @@ -210,7 +210,7 @@ def stop(self): sim.stop() print("Stopped.") -# socat -d -d pty,raw,echo=0,link=/tmp/ACTION_SIM pty,raw,echo=0,link=/tmp/ACTION_USB -# python3 simulated_pcb/action.py --port /tmp/ACTION_SIM -# socat -d -d pty,raw,echo=0,link=/tmp/ODO_SIM pty,raw,echo=0,link=/tmp/ODO_USB -# python3 simulated_pcb/action.py --port /tmp/ODO_SIM +# socat -d -d pty,raw,echo=0,link=/tmp/SIM_ACTION pty,raw,echo=0,link=/tmp/USB_ACTION +# python3 simulated_pcb/action.py --port /tmp/SIM_ACTION +# socat -d -d pty,raw,echo=0,link=/tmp/SIM_ODO pty,raw,echo=0,link=/tmp/USB_ODO +# python3 simulated_pcb/action.py --port /tmp/SIM_ODO diff --git a/src/modelec_strat/src/missions/take_mission.cpp b/src/modelec_strat/src/missions/take_mission.cpp index a33c59e..a94245f 100644 --- a/src/modelec_strat/src/missions/take_mission.cpp +++ b/src/modelec_strat/src/missions/take_mission.cpp @@ -68,7 +68,7 @@ namespace Modelec { break; case DOWN: { - action_executor_->Up(BaseAction::FRONT); + action_executor_->Down(BaseAction::FRONT); deploy_time_ = node_->now(); } diff --git a/test.zsh b/test.zsh deleted file mode 100644 index 2f3dd65..0000000 --- a/test.zsh +++ /dev/null @@ -1,6 +0,0 @@ -ros_env="AMENT_PREFIX_PATH CMAKE_PREFIX_PATH COLCON_PREFIX_PATH PKG_CONFIG_PATH PYTHONPATH LD_LIBRARY_PATH PATH ROS_DISTRO ROS_PYTHON_VERSION ROS_LOCALHOST_ONLY ROS_VERSION" - -for e in ${=ros_env}; do - # In Zsh, ${(P)e} performs the indirect expansion - echo "$e=${(P)e}" -done \ No newline at end of file From 3c99f7a00fc3a28a308a0d1eeabe33533519746a Mon Sep 17 00:00:00 2001 From: acki Date: Wed, 21 Jan 2026 18:06:23 +0100 Subject: [PATCH 37/43] update mission + init strat --- src/modelec_strat/src/action/down_action.cpp | 16 ++++++++-------- src/modelec_strat/src/strat_fms.cpp | 12 ++++++------ 2 files changed, 14 insertions(+), 14 deletions(-) diff --git a/src/modelec_strat/src/action/down_action.cpp b/src/modelec_strat/src/action/down_action.cpp index 2d666ed..19a6dde 100644 --- a/src/modelec_strat/src/action/down_action.cpp +++ b/src/modelec_strat/src/action/down_action.cpp @@ -34,23 +34,23 @@ void Modelec::DownAction::Next() if (front_ == FRONT || front_ == BOTH) { msg.items[0].id = 0; - msg.items[0].start_angle = 2.95; - msg.items[0].end_angle = 1.95; + msg.items[0].start_angle = 1.95; + msg.items[0].end_angle = 2.95; msg.items[0].duration_s = 1; msg.items[1].id = 1; - msg.items[1].start_angle = 0.9; - msg.items[1].end_angle = 1.9; + msg.items[1].start_angle = 1.9; + msg.items[1].end_angle = 0.9; msg.items[1].duration_s = 1; msg.items[2].id = 2; - msg.items[2].start_angle = 0; - msg.items[2].end_angle = 0.3; + msg.items[2].start_angle = 0.3; + msg.items[2].end_angle = 0; msg.items[2].duration_s = 1; msg.items[3].id = 3; - msg.items[3].start_angle = 3; - msg.items[3].end_angle = 2.7; + msg.items[3].start_angle = 2.7; + msg.items[3].end_angle = 3; msg.items[3].duration_s = 1; } diff --git a/src/modelec_strat/src/strat_fms.cpp b/src/modelec_strat/src/strat_fms.cpp index 414218c..c95c054 100644 --- a/src/modelec_strat/src/strat_fms.cpp +++ b/src/modelec_strat/src/strat_fms.cpp @@ -143,6 +143,12 @@ namespace Modelec arm_msg.data = true; tir_arm_set_pub_->publish(arm_msg); + action_executor_->Up(BaseAction::Front::BOTH, true); + action_executor_->Free({ + {0, true}, {1, true}, {2, true}, {3, true}, + {0, false}, {1, false}, {2, false}, {3, false}, + }, true); + Transition(State::WAIT_START, "System ready"); } break; @@ -159,12 +165,6 @@ namespace Modelec .count(); start_time_pub_->publish(msg); - action_executor_->Up(BaseAction::Front::BOTH, true); - action_executor_->Free({ - {0, true}, {1, true}, {2, true}, {3, true}, - {0, false}, {1, false}, {2, false}, {3, false}, - }, true); - Transition(State::SELECT_MISSION, "Match started"); } break; From 60e361b15f2976ed60fea39207680510458ce4f3 Mon Sep 17 00:00:00 2001 From: acki Date: Wed, 21 Jan 2026 18:23:44 +0100 Subject: [PATCH 38/43] nav --- src/modelec_strat/src/missions/free_mission.cpp | 12 ++++++++---- src/modelec_strat/src/missions/take_mission.cpp | 8 +++++++- 2 files changed, 15 insertions(+), 5 deletions(-) diff --git a/src/modelec_strat/src/missions/free_mission.cpp b/src/modelec_strat/src/missions/free_mission.cpp index 6bdc344..d31ffa8 100644 --- a/src/modelec_strat/src/missions/free_mission.cpp +++ b/src/modelec_strat/src/missions/free_mission.cpp @@ -53,8 +53,12 @@ namespace Modelec { angle_ = atan2(depoPoint.y - currPos->y, depoPoint.x - currPos->x); - nav_->GoToRotateFirst(depoPoint.GetTakePosition(dist, - angle_ + M_PI), true, Pathfinding::FREE); + if (nav_->GoToRotateFirst(depoPoint.GetTakePosition(dist, + angle_ + M_PI), true, Pathfinding::FREE)) + { + nav_->GoToRotateFirst(depoPoint.GetTakePosition(dist, + angle_ + M_PI), true, Pathfinding::FREE | Pathfinding::OBSTACLE); + } go_timeout_ = node_->now(); } @@ -101,8 +105,8 @@ namespace Modelec { break; case GO_BACK: { - nav_->GoToRotateFirst(target_deposite_zone_->GetPosition().GetTakePosition(350, - angle_ + M_PI), true, Pathfinding::FREE | Pathfinding::OBSTACLE); + nav_->GoTo(target_deposite_zone_->GetPosition().GetTakePosition(500, + nav_->GetCurrentPos()->theta), true, Pathfinding::FREE | Pathfinding::OBSTACLE); go_timeout_ = node_->now(); } diff --git a/src/modelec_strat/src/missions/take_mission.cpp b/src/modelec_strat/src/missions/take_mission.cpp index a94245f..6bcf895 100644 --- a/src/modelec_strat/src/missions/take_mission.cpp +++ b/src/modelec_strat/src/missions/take_mission.cpp @@ -48,7 +48,13 @@ namespace Modelec { auto pos = closestBox->GetOptimizedGetPos(nav_->GetCurrentPos()).GetTakeBasePosition(); - nav_->GoToRotateFirst(pos, true, Pathfinding::FREE | Pathfinding::WALL); + if (nav_->GoToRotateFirst(pos, false, Pathfinding::FREE | Pathfinding::WALL)) + { + if (nav_->GoToRotateFirst(pos, true, Pathfinding::FREE | Pathfinding::WALL)) + { + nav_->GoToRotateFirst(pos, true, Pathfinding::FREE | Pathfinding::WALL | Pathfinding::OBSTACLE); + } + } go_timeout_ = node_->now(); } From b75bc451988755e6773623df9e8d15fc06f8c323 Mon Sep 17 00:00:00 2001 From: acki Date: Wed, 21 Jan 2026 18:30:50 +0100 Subject: [PATCH 39/43] nav --- src/modelec_strat/src/missions/free_mission.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modelec_strat/src/missions/free_mission.cpp b/src/modelec_strat/src/missions/free_mission.cpp index d31ffa8..b2ccbf4 100644 --- a/src/modelec_strat/src/missions/free_mission.cpp +++ b/src/modelec_strat/src/missions/free_mission.cpp @@ -106,7 +106,7 @@ namespace Modelec { case GO_BACK: { nav_->GoTo(target_deposite_zone_->GetPosition().GetTakePosition(500, - nav_->GetCurrentPos()->theta), true, Pathfinding::FREE | Pathfinding::OBSTACLE); + angle_ + M_PI), true, Pathfinding::FREE | Pathfinding::OBSTACLE); go_timeout_ = node_->now(); } From 3c1cea05adba3b9deabcdaf954a4745f4586f368 Mon Sep 17 00:00:00 2001 From: acki Date: Wed, 21 Jan 2026 19:18:29 +0100 Subject: [PATCH 40/43] hope it works --- src/modelec_strat/data/deposite_zone.xml | 39 ++++++++++++++++++ .../include/modelec_strat/deposite_zone.hpp | 4 ++ src/modelec_strat/src/deposite_zone.cpp | 41 +++++++++++++++++++ .../src/missions/free_mission.cpp | 14 ++----- 4 files changed, 88 insertions(+), 10 deletions(-) diff --git a/src/modelec_strat/data/deposite_zone.xml b/src/modelec_strat/data/deposite_zone.xml index 99fd522..5decfea 100644 --- a/src/modelec_strat/data/deposite_zone.xml +++ b/src/modelec_strat/data/deposite_zone.xml @@ -1,32 +1,71 @@ + + -1.5708 + + + -1.5708 + + + 0 + + + 0 + 1.5708 + -1.5708 + 3.1415 + + + 0 + 1.5708 + -1.5708 + 3.1415 + + + 0 + 1.5708 + -1.5708 + 3.1415 + + + 3.1415 + + + 1.5708 + + + 1.5708 + + + 1.5708 + diff --git a/src/modelec_strat/include/modelec_strat/deposite_zone.hpp b/src/modelec_strat/include/modelec_strat/deposite_zone.hpp index 5179646..4bd48b6 100644 --- a/src/modelec_strat/include/modelec_strat/deposite_zone.hpp +++ b/src/modelec_strat/include/modelec_strat/deposite_zone.hpp @@ -14,6 +14,8 @@ namespace Modelec Point GetPosition() const; + Point GetBestTakePosition(const Point& currentPos) const; + void Validate(bool valid); bool Validate() const; @@ -29,6 +31,8 @@ namespace Modelec int w_, h_; Point position_; + std::vector take_angle_; + bool has_box_ = false; }; } diff --git a/src/modelec_strat/src/deposite_zone.cpp b/src/modelec_strat/src/deposite_zone.cpp index 64159ec..f39ac10 100644 --- a/src/modelec_strat/src/deposite_zone.cpp +++ b/src/modelec_strat/src/deposite_zone.cpp @@ -1,3 +1,4 @@ +#include #include #include @@ -15,6 +16,16 @@ namespace Modelec posElem->QueryDoubleAttribute("theta", &position_.theta); posElem->QueryIntAttribute("w", &w_); posElem->QueryIntAttribute("h", &h_); + + if (auto TakeAngleElem = obstacleElem->FirstChildElement("TakeAngle")) + { + for (auto takePos = TakeAngleElem->FirstChildElement("Angle"); takePos; takePos = takePos->NextSiblingElement("Angle")) + { + double angle; + takePos->QueryDoubleAttribute("value", &angle); + take_angle_.push_back(angle); + } + } } } @@ -23,6 +34,36 @@ namespace Modelec return position_; } + Point DepositeZone::GetBestTakePosition(const Point& currentPos) const + { + if (take_angle_.empty()) + { + RCLCPP_WARN(rclcpp::get_logger("DepositeZone"), "No take angles defined for DepositeZone id=%d", id_); + return position_; + } + + double min_distance = std::numeric_limits::max(); + double best_angle = take_angle_.front(); + + for (const auto& angle : take_angle_) + { + double dx = currentPos.x - position_.x; + double dy = currentPos.y - position_.y; + double angle_to_zone = std::atan2(dy, dx); + double distance = std::fabs(angle_to_zone - angle); + + if (distance < min_distance) + { + min_distance = distance; + best_angle = angle; + } + } + + Point best_position = position_; + best_position.theta = best_angle; + return best_position; + } + void DepositeZone::Validate(bool valid) { has_box_ = valid; diff --git a/src/modelec_strat/src/missions/free_mission.cpp b/src/modelec_strat/src/missions/free_mission.cpp index b2ccbf4..2162158 100644 --- a/src/modelec_strat/src/missions/free_mission.cpp +++ b/src/modelec_strat/src/missions/free_mission.cpp @@ -48,16 +48,11 @@ namespace Modelec { target_deposite_zone_ = nav_->GetClosestDepositeZone(nav_->GetCurrentPos(), {}, true); - auto depoPoint = target_deposite_zone_->GetPosition(); + auto depoPoint = target_deposite_zone_->GetBestTakePosition(Point(currPos->x, currPos->y, currPos->theta)); - angle_ = atan2(depoPoint.y - currPos->y, - depoPoint.x - currPos->x); - - if (nav_->GoToRotateFirst(depoPoint.GetTakePosition(dist, - angle_ + M_PI), true, Pathfinding::FREE)) + if (nav_->GoToRotateFirst(depoPoint.GetTakePosition(dist), true, Pathfinding::FREE)) { - nav_->GoToRotateFirst(depoPoint.GetTakePosition(dist, - angle_ + M_PI), true, Pathfinding::FREE | Pathfinding::OBSTACLE); + nav_->GoToRotateFirst(depoPoint.GetTakePosition(dist), true, Pathfinding::FREE | Pathfinding::OBSTACLE); } go_timeout_ = node_->now(); @@ -105,8 +100,7 @@ namespace Modelec { break; case GO_BACK: { - nav_->GoTo(target_deposite_zone_->GetPosition().GetTakePosition(500, - angle_ + M_PI), true, Pathfinding::FREE | Pathfinding::OBSTACLE); + nav_->GoTo(target_deposite_zone_->GetPosition().GetTakePosition(500), true, Pathfinding::FREE | Pathfinding::OBSTACLE); go_timeout_ = node_->now(); } From 1481a1042bb5412e5bc1c2e02532642e1e44af19 Mon Sep 17 00:00:00 2001 From: acki Date: Wed, 21 Jan 2026 20:12:16 +0100 Subject: [PATCH 41/43] hope it works --- .../include/modelec_strat/deposite_zone.hpp | 3 +-- src/modelec_strat/src/deposite_zone.cpp | 12 +++++++----- src/modelec_utils/include/modelec_utils/point.hpp | 6 ++++++ src/modelec_utils/src/point.cpp | 14 ++++++++++++-- 4 files changed, 26 insertions(+), 9 deletions(-) diff --git a/src/modelec_strat/include/modelec_strat/deposite_zone.hpp b/src/modelec_strat/include/modelec_strat/deposite_zone.hpp index 4bd48b6..1876e40 100644 --- a/src/modelec_strat/include/modelec_strat/deposite_zone.hpp +++ b/src/modelec_strat/include/modelec_strat/deposite_zone.hpp @@ -1,6 +1,5 @@ #pragma once -#include #include #include #include @@ -14,7 +13,7 @@ namespace Modelec Point GetPosition() const; - Point GetBestTakePosition(const Point& currentPos) const; + Point GetBestTakePosition(const Point& currentPos, int dist = CLOSE_DISTANCE) const; void Validate(bool valid); diff --git a/src/modelec_strat/src/deposite_zone.cpp b/src/modelec_strat/src/deposite_zone.cpp index f39ac10..654daf5 100644 --- a/src/modelec_strat/src/deposite_zone.cpp +++ b/src/modelec_strat/src/deposite_zone.cpp @@ -34,7 +34,7 @@ namespace Modelec return position_; } - Point DepositeZone::GetBestTakePosition(const Point& currentPos) const + Point DepositeZone::GetBestTakePosition(const Point& currentPos, int dist) const { if (take_angle_.empty()) { @@ -47,10 +47,12 @@ namespace Modelec for (const auto& angle : take_angle_) { - double dx = currentPos.x - position_.x; - double dy = currentPos.y - position_.y; - double angle_to_zone = std::atan2(dy, dx); - double distance = std::fabs(angle_to_zone - angle); + Point p = Point( + static_cast(position_.x + dist * std::cos(angle)), + static_cast(position_.y + dist * std::sin(angle)), + angle); + + double distance = p.distance(currentPos); if (distance < min_distance) { diff --git a/src/modelec_utils/include/modelec_utils/point.hpp b/src/modelec_utils/include/modelec_utils/point.hpp index aa6b17c..578286f 100644 --- a/src/modelec_utils/include/modelec_utils/point.hpp +++ b/src/modelec_utils/include/modelec_utils/point.hpp @@ -1,5 +1,8 @@ #pragma once +#define CLOSE_DISTANCE 150 +#define BASE_DISTANCE 290 + namespace Modelec { struct Point { @@ -13,6 +16,9 @@ namespace Modelec static double distance(const Point& p1, const Point& p2); static double angleDiff(const Point& p1, const Point& p2); + double distance(const Point& p2) const; + double angleDiff(const Point& p2) const; + [[nodiscard]] Point GetTakePosition(int distance, double angle) const; [[nodiscard]] Point GetTakePosition(int distance) const; diff --git a/src/modelec_utils/src/point.cpp b/src/modelec_utils/src/point.cpp index bfa8888..9580774 100644 --- a/src/modelec_utils/src/point.cpp +++ b/src/modelec_utils/src/point.cpp @@ -15,6 +15,16 @@ namespace Modelec return diff - M_PI; } + double Point::distance(const Point& p2) const + { + return distance(*this, p2); + } + + double Point::angleDiff(const Point& p2) const + { + return angleDiff(*this, p2); + } + Point Point::GetTakePosition(int distance, double angle) const { Point pos; @@ -31,11 +41,11 @@ namespace Modelec Point Point::GetTakeBasePosition() const { - return GetTakePosition(290, theta); + return GetTakePosition(BASE_DISTANCE, theta); } Point Point::GetTakeClosePosition() const { - return GetTakePosition(150, theta); + return GetTakePosition(CLOSE_DISTANCE, theta); } } From 0834053569f34da7388414943dfa371819506593 Mon Sep 17 00:00:00 2001 From: acki Date: Wed, 21 Jan 2026 21:12:11 +0100 Subject: [PATCH 42/43] install --- install.sh | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/install.sh b/install.sh index f516425..3e7cbef 100755 --- a/install.sh +++ b/install.sh @@ -27,9 +27,9 @@ git submodule init git submodule update echo "source /opt/ros/jazzy/setup.bash -source ~/modelec-marcel-ROS/install/setup.bash +source ~/Modelec-ROS/install/setup.bash export RMW_IMPLEMENTATION=rmw_fastrtps_cpp -export FASTRTPS_DEFAULT_PROFILES_FILE=~/modelec-marcel-ROS/fastdds_setup.xml +export FASTRTPS_DEFAULT_PROFILES_FILE=~/Modelec-ROS/fastdds_setup.xml export ROS_DOMAIN_ID=128" >> ~/.bashrc source ~/.bashrc From c085793ae60336e5c14828a5326844e30b414ce1 Mon Sep 17 00:00:00 2001 From: acki Date: Thu, 22 Jan 2026 18:26:44 +0100 Subject: [PATCH 43/43] BackFront --- .../modelec_strat/action/free_action.hpp | 14 +++---- .../modelec_strat/action/take_action.hpp | 14 +++---- .../include/modelec_strat/action_executor.hpp | 4 +- .../modelec_strat/missions/free_mission.hpp | 5 ++- .../modelec_strat/missions/take_mission.hpp | 6 ++- src/modelec_strat/src/action/free_action.cpp | 14 +++---- src/modelec_strat/src/action/take_action.cpp | 14 +++---- src/modelec_strat/src/action_executor.cpp | 8 ++-- .../src/missions/free_mission.cpp | 11 ++++-- .../src/missions/take_mission.cpp | 36 ++++++++++++----- src/modelec_strat/src/strat_fms.cpp | 39 +++++++++++++++++-- 11 files changed, 111 insertions(+), 54 deletions(-) diff --git a/src/modelec_strat/include/modelec_strat/action/free_action.hpp b/src/modelec_strat/include/modelec_strat/action/free_action.hpp index acb123a..3a4532d 100644 --- a/src/modelec_strat/include/modelec_strat/action/free_action.hpp +++ b/src/modelec_strat/include/modelec_strat/action/free_action.hpp @@ -9,20 +9,20 @@ namespace Modelec { public: FreeAction(const std::shared_ptr& action_executor); - FreeAction(const std::shared_ptr& action_executor, bool front, int n); - FreeAction(const std::shared_ptr& action_executor, std::pair servo); - FreeAction(const std::shared_ptr& action_executor, std::vector> servos); + FreeAction(const std::shared_ptr& action_executor, Front front, int n); + FreeAction(const std::shared_ptr& action_executor, std::pair servo); + FreeAction(const std::shared_ptr& action_executor, std::vector> servos); void Next() override; void Init(const std::vector& params) override; - void AddServo(int id, bool front); - void AddServo(std::pair servo); - void AddServos(const std::vector>& servos); + void AddServo(int id, Front front); + void AddServo(std::pair servo); + void AddServos(const std::vector>& servos); inline static const std::string Name = ActionExec::FREE; private: - std::vector> servos_; + std::vector> servos_; std::queue steps_; diff --git a/src/modelec_strat/include/modelec_strat/action/take_action.hpp b/src/modelec_strat/include/modelec_strat/action/take_action.hpp index fd9f226..68cd7a6 100644 --- a/src/modelec_strat/include/modelec_strat/action/take_action.hpp +++ b/src/modelec_strat/include/modelec_strat/action/take_action.hpp @@ -9,20 +9,20 @@ namespace Modelec { public: TakeAction(const std::shared_ptr& action_executor); - TakeAction(const std::shared_ptr& action_executor, bool front, int n); - TakeAction(const std::shared_ptr& action_executor, std::pair servo); - TakeAction(const std::shared_ptr& action_executor, std::vector> servos); + TakeAction(const std::shared_ptr& action_executor, Front front, int n); + TakeAction(const std::shared_ptr& action_executor, std::pair servo); + TakeAction(const std::shared_ptr& action_executor, std::vector> servos); void Next() override; void Init(const std::vector& params) override; - void AddServo(int id, bool front); - void AddServo(std::pair servo); - void AddServos(const std::vector>& servos); + void AddServo(int id, Front front); + void AddServo(std::pair servo); + void AddServos(const std::vector>& servos); inline static const std::string Name = ActionExec::TAKE; private: - std::vector> servos_; + std::vector> servos_; std::queue steps_; diff --git a/src/modelec_strat/include/modelec_strat/action_executor.hpp b/src/modelec_strat/include/modelec_strat/action_executor.hpp index 795c015..c8185f2 100644 --- a/src/modelec_strat/include/modelec_strat/action_executor.hpp +++ b/src/modelec_strat/include/modelec_strat/action_executor.hpp @@ -35,9 +35,9 @@ namespace Modelec void Up(BaseAction::Front front, bool force = false); - void Take(std::vector> servos, bool force = false); + void Take(std::vector> servos, bool force = false); - void Free(std::vector> servos, bool force = false); + void Free(std::vector> servos, bool force = false); void MoveServoTimed(const modelec_interfaces::msg::ActionServoTimedArray& msg); diff --git a/src/modelec_strat/include/modelec_strat/missions/free_mission.hpp b/src/modelec_strat/include/modelec_strat/missions/free_mission.hpp index 34738a0..bb741ad 100644 --- a/src/modelec_strat/include/modelec_strat/missions/free_mission.hpp +++ b/src/modelec_strat/include/modelec_strat/missions/free_mission.hpp @@ -8,7 +8,8 @@ namespace Modelec { class FreeMission : public Mission { public: FreeMission(const std::shared_ptr& nav, - const std::shared_ptr& action_executor); + const std::shared_ptr& action_executor, + BaseAction::Front front = BaseAction::FRONT); void Start(rclcpp::Node::SharedPtr node) override; void Update() override; @@ -27,6 +28,8 @@ namespace Modelec { DONE, } step_; + BaseAction::Front front_; + MissionStatus status_; std::shared_ptr nav_; std::shared_ptr action_executor_; diff --git a/src/modelec_strat/include/modelec_strat/missions/take_mission.hpp b/src/modelec_strat/include/modelec_strat/missions/take_mission.hpp index 64f7db5..a27b4fe 100644 --- a/src/modelec_strat/include/modelec_strat/missions/take_mission.hpp +++ b/src/modelec_strat/include/modelec_strat/missions/take_mission.hpp @@ -8,7 +8,8 @@ namespace Modelec { class TakeMission : public Mission { public: TakeMission(const std::shared_ptr& nav, - const std::shared_ptr& action_executor); + const std::shared_ptr& action_executor, + BaseAction::Front front = BaseAction::FRONT); void Start(rclcpp::Node::SharedPtr node) override; void Update() override; @@ -27,7 +28,8 @@ namespace Modelec { DONE, } step_; - std::shared_ptr closestBox; + BaseAction::Front front_; + MissionStatus status_; std::shared_ptr nav_; std::shared_ptr action_executor_; diff --git a/src/modelec_strat/src/action/free_action.cpp b/src/modelec_strat/src/action/free_action.cpp index fcc18a1..912194a 100644 --- a/src/modelec_strat/src/action/free_action.cpp +++ b/src/modelec_strat/src/action/free_action.cpp @@ -8,17 +8,17 @@ Modelec::FreeAction::FreeAction(const std::shared_ptr& action_ex steps_.push(ActionExec::DONE_STEP); } -Modelec::FreeAction::FreeAction(const std::shared_ptr& action_executor, bool front, int n) : FreeAction(action_executor) +Modelec::FreeAction::FreeAction(const std::shared_ptr& action_executor, Front front, int n) : FreeAction(action_executor) { AddServo(n, front); } -Modelec::FreeAction::FreeAction(const std::shared_ptr& action_executor, std::pair servo) : FreeAction(action_executor) +Modelec::FreeAction::FreeAction(const std::shared_ptr& action_executor, std::pair servo) : FreeAction(action_executor) { AddServo(servo.first, servo.second); } -Modelec::FreeAction::FreeAction(const std::shared_ptr& action_executor, std::vector> servos) : FreeAction(action_executor) +Modelec::FreeAction::FreeAction(const std::shared_ptr& action_executor, std::vector> servos) : FreeAction(action_executor) { AddServos(servos); } @@ -71,22 +71,22 @@ void Modelec::FreeAction::Init(const std::vector& params) { int id = std::stoi(params[i]); bool front = (i + 1 < params.size()) ? (params[i + 1] == "1" || params[i + 1] == "true" || params[i + 1] == "front") : true; - AddServo(id, front); + AddServo(id, front ? FRONT : BACK); } } } -void Modelec::FreeAction::AddServo(int id, bool front) +void Modelec::FreeAction::AddServo(int id, Front front) { servos_.emplace_back(id, front); } -void Modelec::FreeAction::AddServo(std::pair servo) +void Modelec::FreeAction::AddServo(std::pair servo) { servos_.emplace_back(servo); } -void Modelec::FreeAction::AddServos(const std::vector>& servos) +void Modelec::FreeAction::AddServos(const std::vector>& servos) { servos_.insert(servos_.end(), servos.begin(), servos.end()); } diff --git a/src/modelec_strat/src/action/take_action.cpp b/src/modelec_strat/src/action/take_action.cpp index 01a1469..014e8d6 100644 --- a/src/modelec_strat/src/action/take_action.cpp +++ b/src/modelec_strat/src/action/take_action.cpp @@ -6,17 +6,17 @@ Modelec::TakeAction::TakeAction(const std::shared_ptr& action_ex { } -Modelec::TakeAction::TakeAction(const std::shared_ptr& action_executor, bool front, int n) : TakeAction(action_executor) +Modelec::TakeAction::TakeAction(const std::shared_ptr& action_executor, Front front, int n) : TakeAction(action_executor) { AddServo(n, front); } -Modelec::TakeAction::TakeAction(const std::shared_ptr& action_executor, std::pair servo) : TakeAction(action_executor) +Modelec::TakeAction::TakeAction(const std::shared_ptr& action_executor, std::pair servo) : TakeAction(action_executor) { AddServo(servo); } -Modelec::TakeAction::TakeAction(const std::shared_ptr& action_executor, std::vector> servos) : TakeAction(action_executor) +Modelec::TakeAction::TakeAction(const std::shared_ptr& action_executor, std::vector> servos) : TakeAction(action_executor) { AddServos(servos); } @@ -69,22 +69,22 @@ void Modelec::TakeAction::Init(const std::vector& params) { int id = std::stoi(params[i]); bool front = (i + 1 < params.size()) ? (params[i + 1] == "1" || params[i + 1] == "true" || params[i + 1] == "front") : true; - AddServo(id, front); + AddServo(id, front ? FRONT : BACK); } } } -void Modelec::TakeAction::AddServo(int id, bool front) +void Modelec::TakeAction::AddServo(int id, Front front) { servos_.emplace_back(id, front); } -void Modelec::TakeAction::AddServo(std::pair servo) +void Modelec::TakeAction::AddServo(std::pair servo) { servos_.emplace_back(servo); } -void Modelec::TakeAction::AddServos(const std::vector>& servos) +void Modelec::TakeAction::AddServos(const std::vector>& servos) { servos_.insert(servos_.end(), servos.begin(), servos.end()); } diff --git a/src/modelec_strat/src/action_executor.cpp b/src/modelec_strat/src/action_executor.cpp index 0d36e83..eb2760a 100644 --- a/src/modelec_strat/src/action_executor.cpp +++ b/src/modelec_strat/src/action_executor.cpp @@ -96,11 +96,11 @@ namespace Modelec } else if (msg->buttons[3] == 1) // X button { - Take({{0, true}, {1, true}, {2, true}, {3, true}}); + Take({{0, BaseAction::FRONT}, {1, BaseAction::FRONT}, {2, BaseAction::FRONT}, {3, BaseAction::FRONT}}); } else if (msg->buttons[4] == 1) // Y button { - Free({{0, true}, {1, true}, {2, true}, {3, true}}); + Free({{0, BaseAction::FRONT}, {1, BaseAction::FRONT}, {2, BaseAction::FRONT}, {3, BaseAction::FRONT}}); } } }); @@ -177,7 +177,7 @@ namespace Modelec } } - void ActionExecutor::Take(std::vector> servos, bool force) { + void ActionExecutor::Take(std::vector> servos, bool force) { if (action_done_ || force) { action_ = std::make_shared(shared_from_this(), servos); @@ -191,7 +191,7 @@ namespace Modelec } } - void ActionExecutor::Free(std::vector> servos, bool force) { + void ActionExecutor::Free(std::vector> servos, bool force) { if (action_done_ || force) { action_ = std::make_shared(shared_from_this(), servos); diff --git a/src/modelec_strat/src/missions/free_mission.cpp b/src/modelec_strat/src/missions/free_mission.cpp index 2162158..a72783a 100644 --- a/src/modelec_strat/src/missions/free_mission.cpp +++ b/src/modelec_strat/src/missions/free_mission.cpp @@ -3,8 +3,11 @@ #include "modelec_strat/action/base_action.hpp" namespace Modelec { - FreeMission::FreeMission(const std::shared_ptr& nav, const std::shared_ptr& action_executor) - : step_(GO_TO_FREE), status_(MissionStatus::READY), nav_(nav), action_executor_(action_executor) { + FreeMission::FreeMission(const std::shared_ptr& nav, + const std::shared_ptr& action_executor, + BaseAction::Front front) + : step_(GO_TO_FREE), front_(front), status_(MissionStatus::READY), nav_(nav), action_executor_(action_executor) + { } void FreeMission::Start(rclcpp::Node::SharedPtr node) @@ -70,11 +73,11 @@ namespace Modelec { break; case FREE: { - action_executor_->Free({{0, true}, {1, true}, {2, true}, {3, true}}); + action_executor_->Free({{0, front_}, {1, front_}, {2, front_}, {3, front_}}); deploy_time_ = node_->now(); auto obs = action_executor_->box_obstacles_[0]; - action_executor_->box_obstacles_[0] = nullptr; + action_executor_->box_obstacles_[front_] = nullptr; auto pos = nav_->GetCurrentPos(); diff --git a/src/modelec_strat/src/missions/take_mission.cpp b/src/modelec_strat/src/missions/take_mission.cpp index 6bcf895..6d4d968 100644 --- a/src/modelec_strat/src/missions/take_mission.cpp +++ b/src/modelec_strat/src/missions/take_mission.cpp @@ -3,8 +3,11 @@ #include "modelec_strat/action/base_action.hpp" namespace Modelec { - TakeMission::TakeMission(const std::shared_ptr& nav, const std::shared_ptr& action_executor) - : step_(GO_TO_TAKE), status_(MissionStatus::READY), nav_(nav), action_executor_(action_executor) { + TakeMission::TakeMission(const std::shared_ptr& nav, + const std::shared_ptr& action_executor, + BaseAction::Front front) + : step_(GO_TO_TAKE), front_(front), status_(MissionStatus::READY), nav_(nav), action_executor_(action_executor) + { } void TakeMission::Start(rclcpp::Node::SharedPtr node) @@ -42,9 +45,9 @@ namespace Modelec { case GO_TO_TAKE: { - closestBox = nav_->GetClosestObstacle(nav_->GetCurrentPos()); + auto closestBox = nav_->GetClosestObstacle(nav_->GetCurrentPos()); - action_executor_->box_obstacles_[0] = closestBox; + action_executor_->box_obstacles_[front_] = closestBox; auto pos = closestBox->GetOptimizedGetPos(nav_->GetCurrentPos()).GetTakeBasePosition(); @@ -63,7 +66,13 @@ namespace Modelec { break; case GO_TO_TAKE_CLOSE: { - auto pos = closestBox->GetOptimizedGetPos(nav_->GetCurrentPos()).GetTakeClosePosition(); + if (action_executor_->box_obstacles_[front_] == nullptr) + { + status_ = MissionStatus::FAILED; + break; + } + + auto pos = action_executor_->box_obstacles_[0]->GetOptimizedGetPos(nav_->GetCurrentPos()).GetTakeClosePosition(); nav_->GoToRotateFirst(pos, true, Pathfinding::FREE | Pathfinding::WALL | Pathfinding::OBSTACLE); @@ -74,7 +83,7 @@ namespace Modelec { break; case DOWN: { - action_executor_->Down(BaseAction::FRONT); + action_executor_->Down(front_); deploy_time_ = node_->now(); } @@ -82,7 +91,7 @@ namespace Modelec { break; case TAKE: { - action_executor_->Take({{0, true}, {1, true}, {2, true}, {3, true}}); + action_executor_->Take({{0, front_}, {1, front_}, {2, front_}, {3, front_}}); deploy_time_ = node_->now(); } @@ -90,14 +99,23 @@ namespace Modelec { break; case UP: { - action_executor_->Up(BaseAction::FRONT); + action_executor_->Up(front_); deploy_time_ = node_->now(); } step_ = DONE; break; case DONE: - nav_->GetPathfinding()->RemoveObstacle(closestBox->GetId()); + { + if (action_executor_->box_obstacles_[front_] == nullptr) + { + status_ = MissionStatus::FAILED; + break; + } + + nav_->GetPathfinding()->RemoveObstacle(action_executor_->box_obstacles_[front_]->GetId()); + } + status_ = MissionStatus::DONE; break; default: diff --git a/src/modelec_strat/src/strat_fms.cpp b/src/modelec_strat/src/strat_fms.cpp index c95c054..b6e615e 100644 --- a/src/modelec_strat/src/strat_fms.cpp +++ b/src/modelec_strat/src/strat_fms.cpp @@ -145,8 +145,8 @@ namespace Modelec action_executor_->Up(BaseAction::Front::BOTH, true); action_executor_->Free({ - {0, true}, {1, true}, {2, true}, {3, true}, - {0, false}, {1, false}, {2, false}, {3, false}, + {0, BaseAction::FRONT}, {1, BaseAction::FRONT}, {2, BaseAction::FRONT}, {3, BaseAction::FRONT}, + {0, BaseAction::BACK}, {1, BaseAction::BACK}, {2, BaseAction::BACK}, {3, BaseAction::BACK}, }, true); Transition(State::WAIT_START, "System ready"); @@ -207,7 +207,23 @@ namespace Modelec case State::TAKE_MISSION: if (!current_mission_) { - current_mission_ = std::make_unique(nav_, action_executor_); + if (action_executor_->box_obstacles_[BaseAction::FRONT] != nullptr) + { + if (action_executor_->box_obstacles_[BaseAction::BACK] != nullptr) + { + RCLCPP_WARN(get_logger(), "Both front and back box obstacles are occupied!"); + current_mission_.reset(); + Transition(State::SELECT_MISSION, "Cannot take box, both sides occupied"); + break; + } + + RCLCPP_INFO(get_logger(), "Front box obstacle is occupied, taking from back"); + current_mission_ = std::make_unique(nav_, action_executor_, BaseAction::BACK); + current_mission_->Start(shared_from_this()); + break; + } + + current_mission_ = std::make_unique(nav_, action_executor_, BaseAction::FRONT); current_mission_->Start(shared_from_this()); } current_mission_->Update(); @@ -221,7 +237,22 @@ namespace Modelec case State::FREE_MISSION: if (!current_mission_) { - current_mission_ = std::make_unique(nav_, action_executor_); + if (action_executor_->box_obstacles_[BaseAction::FRONT] == nullptr) + { + if (action_executor_->box_obstacles_[BaseAction::BACK] == nullptr) + { + RCLCPP_WARN(get_logger(), "Both front and back box obstacles are free!"); + Transition(State::SELECT_MISSION, "Cannot free box, both sides empty"); + break; + } + + RCLCPP_INFO(get_logger(), "Front box obstacle is occupied, taking from back"); + current_mission_ = std::make_unique(nav_, action_executor_, BaseAction::BACK); + current_mission_->Start(shared_from_this()); + break; + } + + current_mission_ = std::make_unique(nav_, action_executor_, BaseAction::FRONT); current_mission_->Start(shared_from_this()); } current_mission_->Update();