From 3f4f83e25ea676fdca9cac4886963bff604d0261 Mon Sep 17 00:00:00 2001 From: acki Date: Wed, 17 Dec 2025 19:04:30 +0100 Subject: [PATCH 01/79] 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/79] 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/79] 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/79] 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/79] 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/79] 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/79] 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/79] 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/79] 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/79] 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/79] 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/79] 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/79] 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/79] 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/79] 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/79] 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/79] 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/79] 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/79] 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/79] 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/79] 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/79] 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/79] 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/79] 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/79] 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/79] 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/79] 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/79] 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/79] 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/79] 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/79] 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/79] 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/79] 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/79] 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/79] 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/79] 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/79] 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/79] 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/79] 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/79] 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/79] 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/79] 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/79] 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(); From b6cbb7ee9376e0466d4efaa569f14c481e626526 Mon Sep 17 00:00:00 2001 From: acki Date: Thu, 22 Jan 2026 18:52:22 +0100 Subject: [PATCH 44/79] start dynamic mission --- .../include/modelec_strat/action_executor.hpp | 10 ++++++ src/modelec_strat/src/action_executor.cpp | 25 +++++++++++++ src/modelec_strat/src/strat_fms.cpp | 36 ++++++++++--------- 3 files changed, 54 insertions(+), 17 deletions(-) diff --git a/src/modelec_strat/include/modelec_strat/action_executor.hpp b/src/modelec_strat/include/modelec_strat/action_executor.hpp index c8185f2..4728a65 100644 --- a/src/modelec_strat/include/modelec_strat/action_executor.hpp +++ b/src/modelec_strat/include/modelec_strat/action_executor.hpp @@ -45,6 +45,16 @@ namespace Modelec std::array, 2> box_obstacles_; + bool IsEmpty() const; + + bool HasBox(BaseAction::Front front) const; + + bool HasFrontBox() const; + + bool HasBackBox() const; + + bool HasOneBox() const; + protected: rclcpp::Publisher::SharedPtr asc_move_pub_; rclcpp::Publisher::SharedPtr servo_move_pub_; diff --git a/src/modelec_strat/src/action_executor.cpp b/src/modelec_strat/src/action_executor.cpp index eb2760a..d7a09e1 100644 --- a/src/modelec_strat/src/action_executor.cpp +++ b/src/modelec_strat/src/action_executor.cpp @@ -222,4 +222,29 @@ namespace Modelec servo_move_pub_->publish(msg); step_running_ += msg.items.size(); } + + bool ActionExecutor::IsEmpty() const + { + return box_obstacles_[0] == nullptr && box_obstacles_[1] == nullptr; + } + + bool ActionExecutor::HasBox(BaseAction::Front front) const + { + return box_obstacles_[front] != nullptr; + } + + bool ActionExecutor::HasFrontBox() const + { + return HasBox(BaseAction::FRONT); + } + + bool ActionExecutor::HasBackBox() const + { + return HasBox(BaseAction::BACK); + } + + bool ActionExecutor::HasOneBox() const + { + return HasFrontBox() != HasBackBox(); + } } diff --git a/src/modelec_strat/src/strat_fms.cpp b/src/modelec_strat/src/strat_fms.cpp index b6e615e..8c86238 100644 --- a/src/modelec_strat/src/strat_fms.cpp +++ b/src/modelec_strat/src/strat_fms.cpp @@ -191,15 +191,15 @@ namespace Modelec } case State::SELECT_GAME_ACTION: { - if (game_action_sequence_.empty()) + if (action_executor_->IsEmpty() || action_executor_->HasOneBox()) { - Transition(State::DO_GO_HOME, "No more game actions"); + RCLCPP_INFO(get_logger(), "No box on robot, selecting TAKE mission"); + Transition(State::TAKE_MISSION, "Selecting TAKE mission"); } else { - auto action = game_action_sequence_.front(); - game_action_sequence_.pop(); - Transition(action, "Selected game action"); + RCLCPP_INFO(get_logger(), "Box present on robot, selecting FREE mission"); + Transition(State::FREE_MISSION, "Selecting FREE mission"); } } @@ -207,9 +207,9 @@ namespace Modelec case State::TAKE_MISSION: if (!current_mission_) { - if (action_executor_->box_obstacles_[BaseAction::FRONT] != nullptr) + if (action_executor_->HasFrontBox()) { - if (action_executor_->box_obstacles_[BaseAction::BACK] != nullptr) + if (action_executor_->HasBackBox()) { RCLCPP_WARN(get_logger(), "Both front and back box obstacles are occupied!"); current_mission_.reset(); @@ -220,11 +220,12 @@ namespace Modelec 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()); + else + { + current_mission_ = std::make_unique(nav_, action_executor_, BaseAction::FRONT); + current_mission_->Start(shared_from_this()); + } } current_mission_->Update(); if (current_mission_->GetStatus() == MissionStatus::DONE) @@ -237,9 +238,9 @@ namespace Modelec case State::FREE_MISSION: if (!current_mission_) { - if (action_executor_->box_obstacles_[BaseAction::FRONT] == nullptr) + if (!action_executor_->HasFrontBox()) { - if (action_executor_->box_obstacles_[BaseAction::BACK] == nullptr) + if (!action_executor_->HasBackBox()) { RCLCPP_WARN(get_logger(), "Both front and back box obstacles are free!"); Transition(State::SELECT_MISSION, "Cannot free box, both sides empty"); @@ -249,11 +250,12 @@ namespace Modelec 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()); + else + { + current_mission_ = std::make_unique(nav_, action_executor_, BaseAction::FRONT); + current_mission_->Start(shared_from_this()); + } } current_mission_->Update(); if (current_mission_->GetStatus() == MissionStatus::DONE) From fe913d5ff3665d77aed6d84b935b5ccd6159b11b Mon Sep 17 00:00:00 2001 From: acki Date: Fri, 23 Jan 2026 08:56:10 +0100 Subject: [PATCH 45/79] front/back --- .../include/modelec_strat/action_executor.hpp | 2 ++ .../modelec_strat/navigation_helper.hpp | 10 +++++----- src/modelec_strat/src/action_executor.cpp | 5 +++++ .../src/missions/free_mission.cpp | 12 ++++++++--- .../src/missions/take_mission.cpp | 10 ++++++---- src/modelec_strat/src/navigation_helper.cpp | 20 +++++++++---------- src/modelec_strat/src/strat_fms.cpp | 6 +++--- 7 files changed, 40 insertions(+), 25 deletions(-) diff --git a/src/modelec_strat/include/modelec_strat/action_executor.hpp b/src/modelec_strat/include/modelec_strat/action_executor.hpp index 4728a65..1f3b3f7 100644 --- a/src/modelec_strat/include/modelec_strat/action_executor.hpp +++ b/src/modelec_strat/include/modelec_strat/action_executor.hpp @@ -47,6 +47,8 @@ namespace Modelec bool IsEmpty() const; + bool IsFull() const; + bool HasBox(BaseAction::Front front) const; bool HasFrontBox() const; diff --git a/src/modelec_strat/include/modelec_strat/navigation_helper.hpp b/src/modelec_strat/include/modelec_strat/navigation_helper.hpp index 0e0a209..eabaa3a 100644 --- a/src/modelec_strat/include/modelec_strat/navigation_helper.hpp +++ b/src/modelec_strat/include/modelec_strat/navigation_helper.hpp @@ -60,17 +60,17 @@ namespace Modelec bool HasArrived() const; - bool RotateTo(const PosMsg::SharedPtr& pos); - bool RotateTo(const Point& pos); + bool RotateTo(const PosMsg::SharedPtr& pos, bool front = true); + bool RotateTo(const Point& pos, bool front = true); void Rotate(double angle); int GoTo(const PosMsg::SharedPtr& goal, bool isClose = false, int collisionMask = Pathfinding::FREE); int GoTo(int x, int y, double theta, bool isClose = false, int collisionMask = Pathfinding::FREE); int GoTo(const Point& goal, bool isClose = false, int collisionMask = Pathfinding::FREE); - int GoToRotateFirst(const PosMsg::SharedPtr& goal, bool isClose = false, int collisionMask = Pathfinding::FREE); - int GoToRotateFirst(int x, int y, double theta, bool isClose = false, int collisionMask = Pathfinding::FREE); - int GoToRotateFirst(const Point& goal, bool isClose = false, int collisionMask = Pathfinding::FREE); + int GoToRotateFirst(const PosMsg::SharedPtr& goal, bool isClose = false, int collisionMask = Pathfinding::FREE, bool front = true); + int GoToRotateFirst(int x, int y, double theta, bool isClose = false, int collisionMask = Pathfinding::FREE, bool front = true); + int GoToRotateFirst(const Point& goal, bool isClose = false, int collisionMask = Pathfinding::FREE, bool front = true); int CanGoTo(const PosMsg::SharedPtr& goal, bool isClose = false, int collisionMask = Pathfinding::FREE); int CanGoTo(int x, int y, double theta, bool isClose = false, int collisionMask = Pathfinding::FREE); diff --git a/src/modelec_strat/src/action_executor.cpp b/src/modelec_strat/src/action_executor.cpp index d7a09e1..27a6dc7 100644 --- a/src/modelec_strat/src/action_executor.cpp +++ b/src/modelec_strat/src/action_executor.cpp @@ -228,6 +228,11 @@ namespace Modelec return box_obstacles_[0] == nullptr && box_obstacles_[1] == nullptr; } + bool ActionExecutor::IsFull() const + { + return box_obstacles_[0] != nullptr && box_obstacles_[1] != nullptr; + } + bool ActionExecutor::HasBox(BaseAction::Front front) const { return box_obstacles_[front] != nullptr; diff --git a/src/modelec_strat/src/missions/free_mission.cpp b/src/modelec_strat/src/missions/free_mission.cpp index a72783a..8b5ce6d 100644 --- a/src/modelec_strat/src/missions/free_mission.cpp +++ b/src/modelec_strat/src/missions/free_mission.cpp @@ -53,9 +53,12 @@ namespace Modelec { auto depoPoint = target_deposite_zone_->GetBestTakePosition(Point(currPos->x, currPos->y, currPos->theta)); - if (nav_->GoToRotateFirst(depoPoint.GetTakePosition(dist), true, Pathfinding::FREE)) + auto pos = depoPoint.GetTakePosition(dist); + pos.theta += front_ == BaseAction::FRONT ? M_PI : 0; + + if (nav_->GoToRotateFirst(pos, true, Pathfinding::FREE, front_ == BaseAction::FRONT)) { - nav_->GoToRotateFirst(depoPoint.GetTakePosition(dist), true, Pathfinding::FREE | Pathfinding::OBSTACLE); + nav_->GoToRotateFirst(pos, true, Pathfinding::FREE | Pathfinding::OBSTACLE, front_ == BaseAction::FRONT); } go_timeout_ = node_->now(); @@ -103,7 +106,10 @@ namespace Modelec { break; case GO_BACK: { - nav_->GoTo(target_deposite_zone_->GetPosition().GetTakePosition(500), true, Pathfinding::FREE | Pathfinding::OBSTACLE); + auto pos = target_deposite_zone_->GetPosition().GetTakePosition(500); + pos.theta += front_ == BaseAction::FRONT ? M_PI : 0; + + nav_->GoTo(pos, 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 6d4d968..4bc5f84 100644 --- a/src/modelec_strat/src/missions/take_mission.cpp +++ b/src/modelec_strat/src/missions/take_mission.cpp @@ -50,12 +50,13 @@ namespace Modelec { action_executor_->box_obstacles_[front_] = closestBox; auto pos = closestBox->GetOptimizedGetPos(nav_->GetCurrentPos()).GetTakeBasePosition(); + pos.theta += (front_ == BaseAction::FRONT ? M_PI : 0); - if (nav_->GoToRotateFirst(pos, false, Pathfinding::FREE | Pathfinding::WALL)) + if (nav_->GoToRotateFirst(pos, false, Pathfinding::FREE | Pathfinding::WALL, front_ == BaseAction::FRONT)) { - if (nav_->GoToRotateFirst(pos, true, Pathfinding::FREE | Pathfinding::WALL)) + if (nav_->GoToRotateFirst(pos, true, Pathfinding::FREE | Pathfinding::WALL, front_ == BaseAction::FRONT)) { - nav_->GoToRotateFirst(pos, true, Pathfinding::FREE | Pathfinding::WALL | Pathfinding::OBSTACLE); + nav_->GoToRotateFirst(pos, true, Pathfinding::FREE | Pathfinding::WALL | Pathfinding::OBSTACLE, front_ == BaseAction::FRONT); } } @@ -73,8 +74,9 @@ namespace Modelec { } auto pos = action_executor_->box_obstacles_[0]->GetOptimizedGetPos(nav_->GetCurrentPos()).GetTakeClosePosition(); + pos.theta += (front_ == BaseAction::FRONT ? M_PI : 0); - nav_->GoToRotateFirst(pos, true, Pathfinding::FREE | Pathfinding::WALL | Pathfinding::OBSTACLE); + nav_->GoToRotateFirst(pos, true, Pathfinding::FREE | Pathfinding::WALL | Pathfinding::OBSTACLE, front_ == BaseAction::FRONT); go_timeout_ = node_->now(); } diff --git a/src/modelec_strat/src/navigation_helper.cpp b/src/modelec_strat/src/navigation_helper.cpp index 525c249..021fe26 100644 --- a/src/modelec_strat/src/navigation_helper.cpp +++ b/src/modelec_strat/src/navigation_helper.cpp @@ -286,11 +286,11 @@ namespace Modelec return waypoint_queue_.empty() && waypoints_.back().reached; } - bool NavigationHelper::RotateTo(const PosMsg::SharedPtr& pos) + bool NavigationHelper::RotateTo(const PosMsg::SharedPtr& pos, bool front) { double angle = std::atan2(pos->y - current_pos_->y, pos->x - current_pos_->x); - if (std::abs(angle - current_pos_->theta) > M_PI / 3) + if (std::abs(angle - (current_pos_->theta + (front ? 0 : M_PI))) > M_PI / 4) { Rotate(angle); return true; @@ -298,11 +298,11 @@ namespace Modelec return false; } - bool NavigationHelper::RotateTo(const Point& pos) + bool NavigationHelper::RotateTo(const Point& pos, bool front) { double angle = std::atan2(pos.y - current_pos_->y, pos.x - current_pos_->x); - if (std::abs(angle - current_pos_->theta) > M_PI / 4) + if (std::abs(angle - (current_pos_->theta + (front ? 0 : M_PI))) > M_PI / 4) { Rotate(angle); return true; @@ -364,7 +364,7 @@ namespace Modelec return GoTo(goal.x, goal.y, goal.theta, isClose, collisionMask); } - int NavigationHelper::GoToRotateFirst(const PosMsg::SharedPtr& goal, bool isClose, int collisionMask) + int NavigationHelper::GoToRotateFirst(const PosMsg::SharedPtr& goal, bool isClose, int collisionMask, bool front) { last_go_to_ = {goal, isClose, collisionMask}; @@ -376,7 +376,7 @@ namespace Modelec } auto p = Point(wl[0].x, wl[0].y, wl[0].theta); - if (RotateTo(p)) + if (RotateTo(p, front)) { await_rotate_ = true; @@ -403,18 +403,18 @@ namespace Modelec return res; } - int NavigationHelper::GoToRotateFirst(int x, int y, double theta, bool isClose, int collisionMask) + int NavigationHelper::GoToRotateFirst(int x, int y, double theta, bool isClose, int collisionMask, bool front) { PosMsg::SharedPtr goal = std::make_shared(); goal->x = x; goal->y = y; goal->theta = theta; - return GoToRotateFirst(goal, isClose, collisionMask); + return GoToRotateFirst(goal, isClose, collisionMask, front); } - int NavigationHelper::GoToRotateFirst(const Point& goal, bool isClose, int collisionMask) + int NavigationHelper::GoToRotateFirst(const Point& goal, bool isClose, int collisionMask, bool front) { - return GoToRotateFirst(goal.x, goal.y, goal.theta, isClose, collisionMask); + return GoToRotateFirst(goal.x, goal.y, goal.theta, isClose, collisionMask, front); } int NavigationHelper::CanGoTo(const PosMsg::SharedPtr& goal, bool isClose, int collisionMask) diff --git a/src/modelec_strat/src/strat_fms.cpp b/src/modelec_strat/src/strat_fms.cpp index 8c86238..0678c5e 100644 --- a/src/modelec_strat/src/strat_fms.cpp +++ b/src/modelec_strat/src/strat_fms.cpp @@ -191,14 +191,14 @@ namespace Modelec } case State::SELECT_GAME_ACTION: { - if (action_executor_->IsEmpty() || action_executor_->HasOneBox()) + if (!action_executor_->IsFull()) { - RCLCPP_INFO(get_logger(), "No box on robot, selecting TAKE mission"); + RCLCPP_INFO(get_logger(), "Missing box on robot, selecting TAKE mission"); Transition(State::TAKE_MISSION, "Selecting TAKE mission"); } else { - RCLCPP_INFO(get_logger(), "Box present on robot, selecting FREE mission"); + RCLCPP_INFO(get_logger(), "All box are present on robot, selecting FREE mission"); Transition(State::FREE_MISSION, "Selecting FREE mission"); } } From f039b9cca311a1fde40655d918c47272971d1793 Mon Sep 17 00:00:00 2001 From: acki Date: Fri, 23 Jan 2026 09:07:37 +0100 Subject: [PATCH 46/79] front/back --- src/modelec_strat/src/missions/free_mission.cpp | 4 ++-- src/modelec_strat/src/missions/take_mission.cpp | 2 +- 2 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 8b5ce6d..508e2bd 100644 --- a/src/modelec_strat/src/missions/free_mission.cpp +++ b/src/modelec_strat/src/missions/free_mission.cpp @@ -54,7 +54,7 @@ namespace Modelec { auto depoPoint = target_deposite_zone_->GetBestTakePosition(Point(currPos->x, currPos->y, currPos->theta)); auto pos = depoPoint.GetTakePosition(dist); - pos.theta += front_ == BaseAction::FRONT ? M_PI : 0; + pos.theta += front_ == BaseAction::FRONT ? 0 : M_PI; if (nav_->GoToRotateFirst(pos, true, Pathfinding::FREE, front_ == BaseAction::FRONT)) { @@ -107,7 +107,7 @@ namespace Modelec { case GO_BACK: { auto pos = target_deposite_zone_->GetPosition().GetTakePosition(500); - pos.theta += front_ == BaseAction::FRONT ? M_PI : 0; + pos.theta += front_ == BaseAction::FRONT ? 0 : M_PI; nav_->GoTo(pos, true, Pathfinding::FREE | Pathfinding::OBSTACLE); diff --git a/src/modelec_strat/src/missions/take_mission.cpp b/src/modelec_strat/src/missions/take_mission.cpp index 4bc5f84..267dcfd 100644 --- a/src/modelec_strat/src/missions/take_mission.cpp +++ b/src/modelec_strat/src/missions/take_mission.cpp @@ -74,7 +74,7 @@ namespace Modelec { } auto pos = action_executor_->box_obstacles_[0]->GetOptimizedGetPos(nav_->GetCurrentPos()).GetTakeClosePosition(); - pos.theta += (front_ == BaseAction::FRONT ? M_PI : 0); + pos.theta += front_ == BaseAction::FRONT ? 0 : M_PI; nav_->GoToRotateFirst(pos, true, Pathfinding::FREE | Pathfinding::WALL | Pathfinding::OBSTACLE, front_ == BaseAction::FRONT); From 9f582c2007403321c301828515e1e983b6ac83f0 Mon Sep 17 00:00:00 2001 From: acki Date: Fri, 23 Jan 2026 17:08:38 +0100 Subject: [PATCH 47/79] some fix --- src/modelec_strat/src/missions/free_mission.cpp | 2 +- src/modelec_strat/src/missions/take_mission.cpp | 4 ++-- src/modelec_strat/src/obstacle/box.cpp | 2 +- src/modelec_strat/src/strat_fms.cpp | 2 +- 4 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/modelec_strat/src/missions/free_mission.cpp b/src/modelec_strat/src/missions/free_mission.cpp index 508e2bd..cca80ff 100644 --- a/src/modelec_strat/src/missions/free_mission.cpp +++ b/src/modelec_strat/src/missions/free_mission.cpp @@ -79,7 +79,7 @@ namespace Modelec { action_executor_->Free({{0, front_}, {1, front_}, {2, front_}, {3, front_}}); deploy_time_ = node_->now(); - auto obs = action_executor_->box_obstacles_[0]; + auto obs = action_executor_->box_obstacles_[front_]; 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 267dcfd..c4db0df 100644 --- a/src/modelec_strat/src/missions/take_mission.cpp +++ b/src/modelec_strat/src/missions/take_mission.cpp @@ -50,7 +50,7 @@ namespace Modelec { action_executor_->box_obstacles_[front_] = closestBox; auto pos = closestBox->GetOptimizedGetPos(nav_->GetCurrentPos()).GetTakeBasePosition(); - pos.theta += (front_ == BaseAction::FRONT ? M_PI : 0); + pos.theta += (front_ == BaseAction::FRONT ? 0 : M_PI); if (nav_->GoToRotateFirst(pos, false, Pathfinding::FREE | Pathfinding::WALL, front_ == BaseAction::FRONT)) { @@ -73,7 +73,7 @@ namespace Modelec { break; } - auto pos = action_executor_->box_obstacles_[0]->GetOptimizedGetPos(nav_->GetCurrentPos()).GetTakeClosePosition(); + auto pos = action_executor_->box_obstacles_[front_]->GetOptimizedGetPos(nav_->GetCurrentPos()).GetTakeClosePosition(); pos.theta += front_ == BaseAction::FRONT ? 0 : M_PI; nav_->GoToRotateFirst(pos, true, Pathfinding::FREE | Pathfinding::WALL | Pathfinding::OBSTACLE, front_ == BaseAction::FRONT); diff --git a/src/modelec_strat/src/obstacle/box.cpp b/src/modelec_strat/src/obstacle/box.cpp index 31a29d3..fdae6b2 100644 --- a/src/modelec_strat/src/obstacle/box.cpp +++ b/src/modelec_strat/src/obstacle/box.cpp @@ -45,7 +45,7 @@ namespace Modelec } } - return Point(x_, y_, optimizedAngle); + return {x_, y_, optimizedAngle}; } std::vector BoxObstacle::GetAllPossiblePositions() const diff --git a/src/modelec_strat/src/strat_fms.cpp b/src/modelec_strat/src/strat_fms.cpp index 0678c5e..ec14e97 100644 --- a/src/modelec_strat/src/strat_fms.cpp +++ b/src/modelec_strat/src/strat_fms.cpp @@ -191,7 +191,7 @@ namespace Modelec } case State::SELECT_GAME_ACTION: { - if (!action_executor_->IsFull()) + if (action_executor_->IsEmpty()) { RCLCPP_INFO(get_logger(), "Missing box on robot, selecting TAKE mission"); Transition(State::TAKE_MISSION, "Selecting TAKE mission"); From 1b3bbf7363cc641881adac2c2bc35a37821254f2 Mon Sep 17 00:00:00 2001 From: acki Date: Fri, 23 Jan 2026 18:01:14 +0100 Subject: [PATCH 48/79] make it work --- src/modelec_strat/src/deposite_zone.cpp | 7 +++---- src/modelec_strat/src/missions/free_mission.cpp | 16 +++++++++++++--- src/modelec_strat/src/missions/take_mission.cpp | 4 ++-- 3 files changed, 18 insertions(+), 9 deletions(-) diff --git a/src/modelec_strat/src/deposite_zone.cpp b/src/modelec_strat/src/deposite_zone.cpp index 654daf5..cfd388b 100644 --- a/src/modelec_strat/src/deposite_zone.cpp +++ b/src/modelec_strat/src/deposite_zone.cpp @@ -21,8 +21,7 @@ namespace Modelec { for (auto takePos = TakeAngleElem->FirstChildElement("Angle"); takePos; takePos = takePos->NextSiblingElement("Angle")) { - double angle; - takePos->QueryDoubleAttribute("value", &angle); + double angle = takePos->DoubleText(0); take_angle_.push_back(angle); } } @@ -48,8 +47,8 @@ namespace Modelec for (const auto& angle : take_angle_) { Point p = Point( - static_cast(position_.x + dist * std::cos(angle)), - static_cast(position_.y + dist * std::sin(angle)), + position_.x + dist * std::cos(angle), + position_.y + dist * std::sin(angle), angle); double distance = p.distance(currentPos); diff --git a/src/modelec_strat/src/missions/free_mission.cpp b/src/modelec_strat/src/missions/free_mission.cpp index cca80ff..affc541 100644 --- a/src/modelec_strat/src/missions/free_mission.cpp +++ b/src/modelec_strat/src/missions/free_mission.cpp @@ -47,16 +47,22 @@ 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, 300.0); + nav_->GetClosestDepositeZone(nav_->GetCurrentPos())->GetPosition()), 0.0, 200.0); target_deposite_zone_ = nav_->GetClosestDepositeZone(nav_->GetCurrentPos(), {}, true); auto depoPoint = target_deposite_zone_->GetBestTakePosition(Point(currPos->x, currPos->y, currPos->theta)); auto pos = depoPoint.GetTakePosition(dist); + + RCLCPP_INFO( + node_->get_logger(), + "FreeMission: position (%.2d, %.2d) with distance %.2f", + pos.x, pos.y, dist); + pos.theta += front_ == BaseAction::FRONT ? 0 : M_PI; - if (nav_->GoToRotateFirst(pos, true, Pathfinding::FREE, front_ == BaseAction::FRONT)) + if (nav_->GoToRotateFirst(pos, true, Pathfinding::FREE, front_ == BaseAction::FRONT) != Pathfinding::FREE) { nav_->GoToRotateFirst(pos, true, Pathfinding::FREE | Pathfinding::OBSTACLE, front_ == BaseAction::FRONT); } @@ -106,7 +112,11 @@ namespace Modelec { break; case GO_BACK: { - auto pos = target_deposite_zone_->GetPosition().GetTakePosition(500); + auto currPos = nav_->GetCurrentPos(); + + auto depoPoint = target_deposite_zone_->GetBestTakePosition(Point(currPos->x, currPos->y, currPos->theta)); + + auto pos = depoPoint.GetTakePosition(300); pos.theta += front_ == BaseAction::FRONT ? 0 : M_PI; nav_->GoTo(pos, true, Pathfinding::FREE | Pathfinding::OBSTACLE); diff --git a/src/modelec_strat/src/missions/take_mission.cpp b/src/modelec_strat/src/missions/take_mission.cpp index c4db0df..839241b 100644 --- a/src/modelec_strat/src/missions/take_mission.cpp +++ b/src/modelec_strat/src/missions/take_mission.cpp @@ -52,9 +52,9 @@ namespace Modelec { auto pos = closestBox->GetOptimizedGetPos(nav_->GetCurrentPos()).GetTakeBasePosition(); pos.theta += (front_ == BaseAction::FRONT ? 0 : M_PI); - if (nav_->GoToRotateFirst(pos, false, Pathfinding::FREE | Pathfinding::WALL, front_ == BaseAction::FRONT)) + if (nav_->GoToRotateFirst(pos, false, Pathfinding::FREE | Pathfinding::WALL, front_ == BaseAction::FRONT) != Pathfinding::FREE) { - if (nav_->GoToRotateFirst(pos, true, Pathfinding::FREE | Pathfinding::WALL, front_ == BaseAction::FRONT)) + if (nav_->GoToRotateFirst(pos, true, Pathfinding::FREE | Pathfinding::WALL, front_ == BaseAction::FRONT) != Pathfinding::FREE) { nav_->GoToRotateFirst(pos, true, Pathfinding::FREE | Pathfinding::WALL | Pathfinding::OBSTACLE, front_ == BaseAction::FRONT); } From 8c605b8825fbbc7629e474371bbeaaf041136a26 Mon Sep 17 00:00:00 2001 From: acki Date: Fri, 23 Jan 2026 18:06:59 +0100 Subject: [PATCH 49/79] fix path --- joy.ros2_launch_marcel.desktop | 2 +- no_lidar.joy.ros2_launch_marcel.desktop | 2 +- no_lidar.ros2_launch_marcel.desktop | 2 +- ros2_launch_marcel.desktop | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/joy.ros2_launch_marcel.desktop b/joy.ros2_launch_marcel.desktop index 204eef5..e321000 100644 --- a/joy.ros2_launch_marcel.desktop +++ b/joy.ros2_launch_marcel.desktop @@ -2,7 +2,7 @@ Type=Application Name=Joy Comment=Lance le système ROS 2 avec GUI -Exec=/home/modelec/modelec-marcel-ROS/start_ros2.sh +Exec=/home/modelec/Modelec-ROS2/start_ros2.sh Icon=utilities-terminal Terminal=true Categories=Utility; diff --git a/no_lidar.joy.ros2_launch_marcel.desktop b/no_lidar.joy.ros2_launch_marcel.desktop index 60e6063..e323a0d 100644 --- a/no_lidar.joy.ros2_launch_marcel.desktop +++ b/no_lidar.joy.ros2_launch_marcel.desktop @@ -2,7 +2,7 @@ Type=Application Name=Joy no Lidar Comment=Lance le système ROS 2 avec GUI -Exec=/home/modelec/modelec-marcel-ROS/start_ros2.sh with_rplidar:=false +Exec=/home/modelec/Modelec-ROS2/start_ros2.sh with_rplidar:=false Icon=utilities-terminal Terminal=true Categories=Utility; diff --git a/no_lidar.ros2_launch_marcel.desktop b/no_lidar.ros2_launch_marcel.desktop index c01e2fc..268ed94 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_rplidar:=false +Exec=/home/modelec/Modelec-ROS2/start_ros2.sh with_joy:=false with_rplidar:=false Icon=utilities-terminal Terminal=true Categories=Utility; diff --git a/ros2_launch_marcel.desktop b/ros2_launch_marcel.desktop index 6eed291..b919b14 100644 --- a/ros2_launch_marcel.desktop +++ b/ros2_launch_marcel.desktop @@ -2,7 +2,7 @@ Type=Application Name=Lancer ROS 2 Comment=Lance le système ROS 2 avec GUI -Exec=/home/modelec/modelec-marcel-ROS/start_ros2.sh with_joy:=false +Exec=/home/modelec/Modelec-ROS2/start_ros2.sh with_joy:=false Icon=utilities-terminal Terminal=true Categories=Utility; From 3a6ac05c6120a17b7c073918ca9564a1949006ac Mon Sep 17 00:00:00 2001 From: acki Date: Fri, 23 Jan 2026 18:07:27 +0100 Subject: [PATCH 50/79] fix path --- start_ros2.sh | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/start_ros2.sh b/start_ros2.sh index 56c9bba..25ed44d 100755 --- a/start_ros2.sh +++ b/start_ros2.sh @@ -2,11 +2,11 @@ set -e source /opt/ros/jazzy/setup.bash -source /home/modelec/modelec-marcel-ROS/install/setup.bash +source /home/modelec/Modelec-ROS2/install/setup.bash export RCL_LOG_LEVEL=info export RMW_IMPLEMENTATION=rmw_fastrtps_cpp -export FASTRTPS_DEFAULT_PROFILES_FILE=/home/modelec/modelec-marcel-ROS/fastdds_setup.xml +export FASTRTPS_DEFAULT_PROFILES_FILE=/home/modelec/Modelec-ROS2/fastdds_setup.xml export ROS_DOMAIN_ID=128 exec ros2 launch modelec_core modelec.launch.py "$@" From 6bf12478c35af22d07756a8d6a950e1424cc1d7b Mon Sep 17 00:00:00 2001 From: acki Date: Fri, 23 Jan 2026 18:13:03 +0100 Subject: [PATCH 51/79] ip --- fastdds_setup.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/fastdds_setup.xml b/fastdds_setup.xml index de973c3..bc22a6e 100644 --- a/fastdds_setup.xml +++ b/fastdds_setup.xml @@ -23,7 +23,7 @@ -
100.113.219.4
+
100.84.205.108
 From 775f4776b9f3cef63f310c7f9e7719c4e1730398 Mon Sep 17 00:00:00 2001 From: acki Date: Fri, 23 Jan 2026 21:26:17 +0100 Subject: [PATCH 52/79] remove front back but working good --- src/modelec_strat/data/obstacles.xml | 2 +- src/modelec_strat/src/missions/free_mission.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modelec_strat/data/obstacles.xml b/src/modelec_strat/data/obstacles.xml index 0d46ad6..151de31 100644 --- a/src/modelec_strat/data/obstacles.xml +++ b/src/modelec_strat/data/obstacles.xml @@ -18,7 +18,7 @@ - + diff --git a/src/modelec_strat/src/missions/free_mission.cpp b/src/modelec_strat/src/missions/free_mission.cpp index affc541..964f732 100644 --- a/src/modelec_strat/src/missions/free_mission.cpp +++ b/src/modelec_strat/src/missions/free_mission.cpp @@ -91,8 +91,8 @@ namespace Modelec { auto pos = nav_->GetCurrentPos(); obs->SetPosition( - pos->x + 250 * cos(pos->theta), - pos->y + 250 * sin(pos->theta), + pos->x + 200 * cos(pos->theta), + pos->y + 200 * sin(pos->theta), pos->theta); obs->SetAtObjective(true); From 184c05adc50b8fb9399ce2befc47ea55f450e58f Mon Sep 17 00:00:00 2001 From: acki Date: Fri, 23 Jan 2026 22:02:08 +0100 Subject: [PATCH 53/79] better dynamic choice --- src/modelec_strat/src/strat_fms.cpp | 56 ++++++++++++++++++++++++++--- 1 file changed, 51 insertions(+), 5 deletions(-) diff --git a/src/modelec_strat/src/strat_fms.cpp b/src/modelec_strat/src/strat_fms.cpp index ec14e97..642bf0e 100644 --- a/src/modelec_strat/src/strat_fms.cpp +++ b/src/modelec_strat/src/strat_fms.cpp @@ -178,7 +178,11 @@ namespace Modelec Transition(State::STOP, "All missions done"); } - else if (elapsed.seconds() < 70) + else if (elapsed.seconds() < 60 && !action_executor_->IsEmpty()) + { + Transition(State::FREE_MISSION, "Selecting a game action"); + } + else if (elapsed.seconds() < 80) { Transition(State::SELECT_GAME_ACTION, "Selecting a game action"); } @@ -191,15 +195,57 @@ namespace Modelec } case State::SELECT_GAME_ACTION: { - if (action_executor_->IsEmpty()) + if (action_executor_->IsFull()) { - RCLCPP_INFO(get_logger(), "Missing box on robot, selecting TAKE mission"); + RCLCPP_INFO(get_logger(), "All box are present on robot, selecting FREE mission"); + Transition(State::FREE_MISSION, "Selecting FREE mission"); + } + else if (action_executor_->IsEmpty()) + { + RCLCPP_INFO(get_logger(), "No box present on robot, selecting TAKE mission"); Transition(State::TAKE_MISSION, "Selecting TAKE mission"); } else { - RCLCPP_INFO(get_logger(), "All box are present on robot, selecting FREE mission"); - Transition(State::FREE_MISSION, "Selecting FREE mission"); + auto pos = nav_->GetCurrentPos(); + auto closestBox = nav_->GetClosestObstacle(pos); + auto closestDeposite = nav_->GetClosestDepositeZone(pos); + + if (closestBox && closestDeposite) + { + double distToBox = Point::distance(Point(pos->x, pos->y, pos->theta), + closestBox->GetPosition()); + double distToDeposite = Point::distance(Point(pos->x, pos->y, pos->theta), + closestDeposite->GetPosition()); + + if (distToBox < distToDeposite) + { + RCLCPP_INFO(get_logger(), "Choosing TAKE mission (dist to box: %.2f < dist to deposite: %.2f)", + distToBox, distToDeposite); + Transition(State::TAKE_MISSION, "Selecting TAKE mission"); + } + else + { + RCLCPP_INFO(get_logger(), "Choosing FREE mission (dist to deposite: %.2f < dist to box: %.2f)", + distToDeposite, distToBox); + Transition(State::FREE_MISSION, "Selecting FREE mission"); + } + } + else if (closestBox) + { + RCLCPP_INFO(get_logger(), "Only box available, selecting TAKE mission"); + Transition(State::TAKE_MISSION, "Selecting TAKE mission"); + } + else if (closestDeposite) + { + RCLCPP_INFO(get_logger(), "Only deposite available, selecting FREE mission"); + Transition(State::FREE_MISSION, "Selecting FREE mission"); + } + else + { + RCLCPP_WARN(get_logger(), "No box or deposite available, cannot select mission!"); + Transition(State::STOP, "No missions available"); + } } } From 1d093edd3309d612322b0ed3b4d6e4edf86c09f2 Mon Sep 17 00:00:00 2001 From: acki Date: Sat, 24 Jan 2026 10:11:43 +0100 Subject: [PATCH 54/79] front/back --- src/modelec_strat/src/missions/free_mission.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modelec_strat/src/missions/free_mission.cpp b/src/modelec_strat/src/missions/free_mission.cpp index 964f732..fa491cf 100644 --- a/src/modelec_strat/src/missions/free_mission.cpp +++ b/src/modelec_strat/src/missions/free_mission.cpp @@ -74,7 +74,7 @@ namespace Modelec { break; case DOWN: { - action_executor_->Down(BaseAction::FRONT); + action_executor_->Down(front_); deploy_time_ = node_->now(); } @@ -104,7 +104,7 @@ namespace Modelec { break; case UP: { - action_executor_->Up(BaseAction::FRONT); + action_executor_->Up(front_); deploy_time_ = node_->now(); } From c4342e3ff49e8738f799d83a5238b6e892e910f2 Mon Sep 17 00:00:00 2001 From: acki Date: Sat, 24 Jan 2026 10:14:26 +0100 Subject: [PATCH 55/79] servo --- 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 912194a..d5b83d8 100644 --- a/src/modelec_strat/src/action/free_action.cpp +++ b/src/modelec_strat/src/action/free_action.cpp @@ -44,7 +44,7 @@ void Modelec::FreeAction::Next() for (size_t i = 0; i < servos_.size(); i++) { - msg.items[i].id = servos_[i].first + (servos_[i].second ? 3 : 11); + msg.items[i].id = servos_[i].first + (servos_[i].second ? 4 : 12); 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; diff --git a/src/modelec_strat/src/action/take_action.cpp b/src/modelec_strat/src/action/take_action.cpp index 014e8d6..696b4ca 100644 --- a/src/modelec_strat/src/action/take_action.cpp +++ b/src/modelec_strat/src/action/take_action.cpp @@ -42,7 +42,7 @@ void Modelec::TakeAction::Next() for (size_t i = 0; i < servos_.size(); i++) { - msg.items[i].id = servos_[i].first + (servos_[i].second ? 3 : 11); + msg.items[i].id = servos_[i].first + (servos_[i].second ? 4 : 12); 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; From 101dc0a2df125aeb5265fe54edaba8a67972de5e Mon Sep 17 00:00:00 2001 From: acki Date: Sat, 24 Jan 2026 10:30:18 +0100 Subject: [PATCH 56/79] serial action message --- src/modelec_com/src/pcb_action_interface.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/modelec_com/src/pcb_action_interface.cpp b/src/modelec_com/src/pcb_action_interface.cpp index aa28560..6ee069f 100644 --- a/src/modelec_com/src/pcb_action_interface.cpp +++ b/src/modelec_com/src/pcb_action_interface.cpp @@ -446,6 +446,10 @@ namespace Modelec relay_move_res_pub_->publish(relay_msg); } + else if (tokens[1] == "TIR") + { + // Do nothing for now + } else { RCLCPP_WARN(this->get_logger(), "Unknown message format for OK response: '%s'", trim(msg).c_str()); From eb8a193f7061da774e47beb6a2ed0eba05a75ff9 Mon Sep 17 00:00:00 2001 From: acki Date: Mon, 26 Jan 2026 17:58:57 +0100 Subject: [PATCH 57/79] servo --- src/modelec_strat/include/modelec_strat/action_executor.hpp | 4 ++-- src/modelec_strat/src/action_executor.cpp | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modelec_strat/include/modelec_strat/action_executor.hpp b/src/modelec_strat/include/modelec_strat/action_executor.hpp index 1f3b3f7..6186b01 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(const std::vector>& servos, bool force = false); - void Free(std::vector> servos, bool force = false); + void Free(const std::vector>& servos, bool force = false); void MoveServoTimed(const modelec_interfaces::msg::ActionServoTimedArray& msg); diff --git a/src/modelec_strat/src/action_executor.cpp b/src/modelec_strat/src/action_executor.cpp index 27a6dc7..d41a7cc 100644 --- a/src/modelec_strat/src/action_executor.cpp +++ b/src/modelec_strat/src/action_executor.cpp @@ -177,7 +177,7 @@ namespace Modelec } } - void ActionExecutor::Take(std::vector> servos, bool force) { + void ActionExecutor::Take(const 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(const std::vector>& servos, bool force) { if (action_done_ || force) { action_ = std::make_shared(shared_from_this(), servos); From 11e64e263603ec6283841091934ca11120a6123c Mon Sep 17 00:00:00 2001 From: acki Date: Mon, 26 Jan 2026 18:19:21 +0100 Subject: [PATCH 58/79] rewrite mission for future use --- .../modelec_strat/missions/free_mission.hpp | 2 +- .../missions/go_home_mission.hpp | 9 +++---- .../modelec_strat/missions/mission_base.hpp | 3 +++ .../modelec_strat/missions/take_mission.hpp | 2 +- .../src/missions/free_mission.cpp | 26 ++++++++++--------- .../src/missions/go_home_mission.cpp | 25 ++++++++++-------- .../src/missions/take_mission.cpp | 26 ++++++++++--------- 7 files changed, 50 insertions(+), 43 deletions(-) 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 bb741ad..e7652dd 100644 --- a/src/modelec_strat/include/modelec_strat/missions/free_mission.hpp +++ b/src/modelec_strat/include/modelec_strat/missions/free_mission.hpp @@ -26,7 +26,7 @@ namespace Modelec { UP, GO_BACK, DONE, - } step_; + }; BaseAction::Front front_; diff --git a/src/modelec_strat/include/modelec_strat/missions/go_home_mission.hpp b/src/modelec_strat/include/modelec_strat/missions/go_home_mission.hpp index a61d684..ba3073f 100644 --- a/src/modelec_strat/include/modelec_strat/missions/go_home_mission.hpp +++ b/src/modelec_strat/include/modelec_strat/missions/go_home_mission.hpp @@ -20,14 +20,11 @@ namespace Modelec private: enum Step { - GO_FRONT, - AWAIT_95S, - GO_HOME, - DONE, - ROTATE_TO_HOME, + GO_HOME, GO_CLOSE, - } step_; + DONE, + }; MissionStatus status_; std::shared_ptr nav_; diff --git a/src/modelec_strat/include/modelec_strat/missions/mission_base.hpp b/src/modelec_strat/include/modelec_strat/missions/mission_base.hpp index 21033b5..fa0e360 100644 --- a/src/modelec_strat/include/modelec_strat/missions/mission_base.hpp +++ b/src/modelec_strat/include/modelec_strat/missions/mission_base.hpp @@ -24,5 +24,8 @@ namespace Modelec virtual void Clear() = 0; virtual MissionStatus GetStatus() const = 0; virtual std::string GetName() const = 0; + + protected: + std::queue steps_; }; } 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 a27b4fe..0e0df54 100644 --- a/src/modelec_strat/include/modelec_strat/missions/take_mission.hpp +++ b/src/modelec_strat/include/modelec_strat/missions/take_mission.hpp @@ -26,7 +26,7 @@ namespace Modelec { TAKE, UP, DONE, - } step_; + }; BaseAction::Front front_; diff --git a/src/modelec_strat/src/missions/free_mission.cpp b/src/modelec_strat/src/missions/free_mission.cpp index fa491cf..19ede8c 100644 --- a/src/modelec_strat/src/missions/free_mission.cpp +++ b/src/modelec_strat/src/missions/free_mission.cpp @@ -6,7 +6,7 @@ namespace Modelec { 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) + : front_(front), status_(MissionStatus::READY), nav_(nav), action_executor_(action_executor) { } @@ -17,7 +17,16 @@ namespace Modelec { go_timeout_ = node_->now(); status_ = MissionStatus::RUNNING; - step_ = GO_TO_FREE; + + std::queue empty; + std::swap(steps_, empty); + + steps_.push(GO_TO_FREE); + steps_.push(DOWN); + steps_.push(FREE); + steps_.push(UP); + steps_.push(GO_BACK); + steps_.push(DONE); } void FreeMission::Update() @@ -40,6 +49,9 @@ namespace Modelec { } } + auto step_ = steps_.front(); + steps_.pop(); + switch (step_) { case GO_TO_FREE: @@ -69,16 +81,12 @@ namespace Modelec { go_timeout_ = node_->now(); } - - step_ = FREE; break; case DOWN: { action_executor_->Down(front_); deploy_time_ = node_->now(); } - - step_ = FREE; break; case FREE: { @@ -99,16 +107,12 @@ namespace Modelec { nav_->GetPathfinding()->AddObstacle(obs); } - - step_ = UP; break; case UP: { action_executor_->Up(front_); deploy_time_ = node_->now(); } - - step_ = GO_BACK; break; case GO_BACK: { @@ -123,8 +127,6 @@ namespace Modelec { go_timeout_ = node_->now(); } - - step_ = DONE; break; case DONE: { diff --git a/src/modelec_strat/src/missions/go_home_mission.cpp b/src/modelec_strat/src/missions/go_home_mission.cpp index 64b670a..1068de7 100644 --- a/src/modelec_strat/src/missions/go_home_mission.cpp +++ b/src/modelec_strat/src/missions/go_home_mission.cpp @@ -5,7 +5,7 @@ namespace Modelec { GoHomeMission::GoHomeMission(const std::shared_ptr& nav, const rclcpp::Time& start_time) : - step_(ROTATE_TO_HOME), status_(MissionStatus::READY), nav_(nav), start_time_(start_time) + status_(MissionStatus::READY), nav_(nav), start_time_(start_time) { } @@ -20,7 +20,14 @@ namespace Modelec go_timeout_ = node_->now(); status_ = MissionStatus::RUNNING; - step_ = ROTATE_TO_HOME; + + std::queue empty; + std::swap(steps_, empty); + + steps_.push(ROTATE_TO_HOME); + steps_.push(GO_HOME); + steps_.push(GO_CLOSE); + steps_.push(DONE); } void GoHomeMission::Update() @@ -38,6 +45,9 @@ namespace Modelec } } + auto step_ = steps_.front(); + steps_.pop(); + switch (step_) { case ROTATE_TO_HOME: @@ -55,8 +65,6 @@ namespace Modelec nav_->RotateTo(home_point_); go_timeout_ = node_->now(); - - step_ = GO_HOME; } break; case GO_HOME: @@ -71,23 +79,18 @@ namespace Modelec } go_timeout_ = node_->now(); - - step_ = GO_CLOSE; } break; case GO_CLOSE: { - if ((node_->now() - start_time_).seconds() < 94) + /*if ((node_->now() - start_time_).seconds() < 94) { break; - } + }*/ nav_->GoTo(home_point_, true); 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 839241b..8388c3e 100644 --- a/src/modelec_strat/src/missions/take_mission.cpp +++ b/src/modelec_strat/src/missions/take_mission.cpp @@ -6,7 +6,7 @@ namespace Modelec { 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) + : front_(front), status_(MissionStatus::READY), nav_(nav), action_executor_(action_executor) { } @@ -17,7 +17,16 @@ namespace Modelec { go_timeout_ = node_->now(); status_ = MissionStatus::RUNNING; - step_ = GO_TO_TAKE; + + std::queue empty; + std::swap(steps_, empty); + + steps_.push(GO_TO_TAKE); + steps_.push(GO_TO_TAKE_CLOSE); + steps_.push(DOWN); + steps_.push(TAKE); + steps_.push(UP); + steps_.push(DONE); } void TakeMission::Update() @@ -40,6 +49,9 @@ namespace Modelec { } } + auto step_ = steps_.front(); + steps_.pop(); + switch (step_) { case GO_TO_TAKE: @@ -62,8 +74,6 @@ namespace Modelec { go_timeout_ = node_->now(); } - - step_ = GO_TO_TAKE_CLOSE; break; case GO_TO_TAKE_CLOSE: { @@ -80,32 +90,24 @@ namespace Modelec { go_timeout_ = node_->now(); } - - step_ = DOWN; break; case DOWN: { action_executor_->Down(front_); deploy_time_ = node_->now(); } - - step_ = TAKE; break; case TAKE: { action_executor_->Take({{0, front_}, {1, front_}, {2, front_}, {3, front_}}); deploy_time_ = node_->now(); } - - step_ = UP; break; case UP: { action_executor_->Up(front_); deploy_time_ = node_->now(); } - - step_ = DONE; break; case DONE: { From 57c81765303a6ad5ded6ee23d62922726af5950b Mon Sep 17 00:00:00 2001 From: acki Date: Mon, 26 Jan 2026 18:24:47 +0100 Subject: [PATCH 59/79] rewrite mission for future use --- src/modelec_strat/src/missions/free_mission.cpp | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/src/modelec_strat/src/missions/free_mission.cpp b/src/modelec_strat/src/missions/free_mission.cpp index 19ede8c..6d0d14c 100644 --- a/src/modelec_strat/src/missions/free_mission.cpp +++ b/src/modelec_strat/src/missions/free_mission.cpp @@ -76,7 +76,11 @@ namespace Modelec { if (nav_->GoToRotateFirst(pos, true, Pathfinding::FREE, front_ == BaseAction::FRONT) != Pathfinding::FREE) { - nav_->GoToRotateFirst(pos, true, Pathfinding::FREE | Pathfinding::OBSTACLE, front_ == BaseAction::FRONT); + if (nav_->GoToRotateFirst(pos, true, Pathfinding::FREE | Pathfinding::OBSTACLE, front_ == BaseAction::FRONT) != Pathfinding::FREE) + { + status_ = MissionStatus::FAILED; + return; + } } go_timeout_ = node_->now(); @@ -123,7 +127,11 @@ namespace Modelec { auto pos = depoPoint.GetTakePosition(300); pos.theta += front_ == BaseAction::FRONT ? 0 : M_PI; - nav_->GoTo(pos, true, Pathfinding::FREE | Pathfinding::OBSTACLE); + if (nav_->GoTo(pos, true, Pathfinding::FREE | Pathfinding::OBSTACLE) != Pathfinding::FREE) + { + status_ = MissionStatus::FAILED; + return; + } go_timeout_ = node_->now(); } From 2a0f61c628618a7228d8a805e51ae81073abc90e Mon Sep 17 00:00:00 2001 From: acki Date: Mon, 26 Jan 2026 20:24:10 +0100 Subject: [PATCH 60/79] add more things to action --- .../modelec_strat/missions/free_mission.hpp | 4 +++ .../missions/go_home_mission.hpp | 4 +++ .../modelec_strat/missions/take_mission.hpp | 4 +++ .../src/missions/free_mission.cpp | 24 +++++++++++-- .../src/missions/go_home_mission.cpp | 18 ++++++++-- .../src/missions/take_mission.cpp | 34 ++++++++++++++++--- 6 files changed, 78 insertions(+), 10 deletions(-) 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 e7652dd..0bdd862 100644 --- a/src/modelec_strat/include/modelec_strat/missions/free_mission.hpp +++ b/src/modelec_strat/include/modelec_strat/missions/free_mission.hpp @@ -41,5 +41,9 @@ namespace Modelec { rclcpp::Time go_timeout_; rclcpp::Time deploy_time_; + + std::optional min_time_; + + rclcpp::Time last_ask_waypoint_time_; }; } \ No newline at end of file diff --git a/src/modelec_strat/include/modelec_strat/missions/go_home_mission.hpp b/src/modelec_strat/include/modelec_strat/missions/go_home_mission.hpp index ba3073f..0ba87a9 100644 --- a/src/modelec_strat/include/modelec_strat/missions/go_home_mission.hpp +++ b/src/modelec_strat/include/modelec_strat/missions/go_home_mission.hpp @@ -35,5 +35,9 @@ namespace Modelec rclcpp::Publisher::SharedPtr score_pub_; int mission_score_ = 0; rclcpp::Time go_timeout_; + + std::optional min_time_; + + rclcpp::Time last_ask_waypoint_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 0e0df54..01894da 100644 --- a/src/modelec_strat/include/modelec_strat/missions/take_mission.hpp +++ b/src/modelec_strat/include/modelec_strat/missions/take_mission.hpp @@ -37,5 +37,9 @@ namespace Modelec { rclcpp::Time go_timeout_; rclcpp::Time deploy_time_; + + std::optional min_time_; + + rclcpp::Time last_ask_waypoint_time_; }; } \ No newline at end of file diff --git a/src/modelec_strat/src/missions/free_mission.cpp b/src/modelec_strat/src/missions/free_mission.cpp index 6d0d14c..b0ad5e2 100644 --- a/src/modelec_strat/src/missions/free_mission.cpp +++ b/src/modelec_strat/src/missions/free_mission.cpp @@ -15,6 +15,8 @@ namespace Modelec { node_ = node; go_timeout_ = node_->now(); + deploy_time_ = node_->now(); + last_ask_waypoint_time_ = node_->now(); status_ = MissionStatus::RUNNING; @@ -38,10 +40,11 @@ namespace Modelec { if (!nav_->HasArrived()) { - if ((node_->now() - go_timeout_).seconds() < 5) + if ((node_->now() - go_timeout_).seconds() < 2 && (node_->now() - last_ask_waypoint_time_).seconds() > 1) { - // nav_->AskWaypoint(); - // return; + nav_->AskWaypoint(); + last_ask_waypoint_time_ = node_->now(); + return; } if ((node_->now() - go_timeout_).seconds() < 10) { @@ -49,6 +52,19 @@ namespace Modelec { } } + if (min_time_.has_value()) + { + if ((node_->now() - min_time_.value()).seconds() < 0.1) + { + return; + } + else + { + min_time_.reset(); + } + } + + auto step_ = steps_.front(); steps_.pop(); @@ -110,6 +126,8 @@ namespace Modelec { obs->SetAtObjective(true); nav_->GetPathfinding()->AddObstacle(obs); + + min_time_ = node_->now() + rclcpp::Duration::from_seconds(0.5); } break; case UP: diff --git a/src/modelec_strat/src/missions/go_home_mission.cpp b/src/modelec_strat/src/missions/go_home_mission.cpp index 1068de7..6d70e4b 100644 --- a/src/modelec_strat/src/missions/go_home_mission.cpp +++ b/src/modelec_strat/src/missions/go_home_mission.cpp @@ -18,6 +18,7 @@ namespace Modelec score_pub_ = node_->create_publisher("/strat/score", 10); go_timeout_ = node_->now(); + last_ask_waypoint_time_ = node_->now(); status_ = MissionStatus::RUNNING; @@ -34,9 +35,10 @@ namespace Modelec { if (!nav_->HasArrived()) { - if ((node_->now() - go_timeout_).seconds() < 2) + if ((node_->now() - go_timeout_).seconds() < 2 && (node_->now() - last_ask_waypoint_time_).seconds() > 1) { - // nav_->AskWaypoint(); + nav_->AskWaypoint(); + last_ask_waypoint_time_ = node_->now(); return; } if ((node_->now() - go_timeout_).seconds() < 10) @@ -45,6 +47,18 @@ namespace Modelec } } + if (min_time_.has_value()) + { + if ((node_->now() - min_time_.value()).seconds() > 0.0) + { + return; + } + else + { + min_time_.reset(); + } + } + auto step_ = steps_.front(); steps_.pop(); diff --git a/src/modelec_strat/src/missions/take_mission.cpp b/src/modelec_strat/src/missions/take_mission.cpp index 8388c3e..e18b9cd 100644 --- a/src/modelec_strat/src/missions/take_mission.cpp +++ b/src/modelec_strat/src/missions/take_mission.cpp @@ -15,6 +15,8 @@ namespace Modelec { node_ = node; go_timeout_ = node_->now(); + deploy_time_ = node_->now(); + last_ask_waypoint_time_ = node_->now(); status_ = MissionStatus::RUNNING; @@ -38,10 +40,11 @@ namespace Modelec { if (!nav_->HasArrived()) { - if ((node_->now() - go_timeout_).seconds() < 5) + if ((node_->now() - go_timeout_).seconds() < 2 && (node_->now() - last_ask_waypoint_time_).seconds() > 1) { - // nav_->AskWaypoint(); - // return; + nav_->AskWaypoint(); + last_ask_waypoint_time_ = node_->now(); + return; } if ((node_->now() - go_timeout_).seconds() < 10) { @@ -49,6 +52,18 @@ namespace Modelec { } } + if (min_time_.has_value()) + { + if ((node_->now() - min_time_.value()).seconds() < 0.1) + { + return; + } + else + { + min_time_.reset(); + } + } + auto step_ = steps_.front(); steps_.pop(); @@ -68,7 +83,11 @@ namespace Modelec { { if (nav_->GoToRotateFirst(pos, true, Pathfinding::FREE | Pathfinding::WALL, front_ == BaseAction::FRONT) != Pathfinding::FREE) { - nav_->GoToRotateFirst(pos, true, Pathfinding::FREE | Pathfinding::WALL | Pathfinding::OBSTACLE, front_ == BaseAction::FRONT); + if (nav_->GoToRotateFirst(pos, true, Pathfinding::FREE | Pathfinding::WALL | Pathfinding::OBSTACLE, front_ == BaseAction::FRONT) != Pathfinding::FREE) + { + status_ = MissionStatus::FAILED; + break; + } } } @@ -86,7 +105,11 @@ namespace Modelec { auto pos = action_executor_->box_obstacles_[front_]->GetOptimizedGetPos(nav_->GetCurrentPos()).GetTakeClosePosition(); pos.theta += front_ == BaseAction::FRONT ? 0 : M_PI; - nav_->GoToRotateFirst(pos, true, Pathfinding::FREE | Pathfinding::WALL | Pathfinding::OBSTACLE, front_ == BaseAction::FRONT); + if (nav_->GoToRotateFirst(pos, true, Pathfinding::FREE | Pathfinding::WALL | Pathfinding::OBSTACLE, front_ == BaseAction::FRONT) != Pathfinding::FREE) + { + status_ = MissionStatus::FAILED; + break; + } go_timeout_ = node_->now(); } @@ -101,6 +124,7 @@ namespace Modelec { { action_executor_->Take({{0, front_}, {1, front_}, {2, front_}, {3, front_}}); deploy_time_ = node_->now(); + min_time_ = node_->now() + rclcpp::Duration::from_seconds(0.5); } break; case UP: From 556934b0a7a042621141c4f0f6e865b26f1ec771 Mon Sep 17 00:00:00 2001 From: acki Date: Mon, 26 Jan 2026 20:50:51 +0100 Subject: [PATCH 61/79] add more things to action --- src/modelec_strat/src/missions/go_home_mission.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modelec_strat/src/missions/go_home_mission.cpp b/src/modelec_strat/src/missions/go_home_mission.cpp index 6d70e4b..8e8026e 100644 --- a/src/modelec_strat/src/missions/go_home_mission.cpp +++ b/src/modelec_strat/src/missions/go_home_mission.cpp @@ -49,7 +49,7 @@ namespace Modelec if (min_time_.has_value()) { - if ((node_->now() - min_time_.value()).seconds() > 0.0) + if ((node_->now() - min_time_.value()).seconds() < 0.1) { return; } From d69d59443c2093c02aa6a9a8dc00bb626a0b2cc6 Mon Sep 17 00:00:00 2001 From: acki Date: Tue, 27 Jan 2026 11:40:04 +0100 Subject: [PATCH 62/79] ask waypoint threshold --- 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 b0ad5e2..a90df5a 100644 --- a/src/modelec_strat/src/missions/free_mission.cpp +++ b/src/modelec_strat/src/missions/free_mission.cpp @@ -40,7 +40,7 @@ namespace Modelec { if (!nav_->HasArrived()) { - if ((node_->now() - go_timeout_).seconds() < 2 && (node_->now() - last_ask_waypoint_time_).seconds() > 1) + if ((node_->now() - go_timeout_).seconds() < 2 && (node_->now() - last_ask_waypoint_time_).seconds() > 2) { nav_->AskWaypoint(); last_ask_waypoint_time_ = node_->now(); diff --git a/src/modelec_strat/src/missions/go_home_mission.cpp b/src/modelec_strat/src/missions/go_home_mission.cpp index 8e8026e..a3722c4 100644 --- a/src/modelec_strat/src/missions/go_home_mission.cpp +++ b/src/modelec_strat/src/missions/go_home_mission.cpp @@ -35,7 +35,7 @@ namespace Modelec { if (!nav_->HasArrived()) { - if ((node_->now() - go_timeout_).seconds() < 2 && (node_->now() - last_ask_waypoint_time_).seconds() > 1) + if ((node_->now() - go_timeout_).seconds() < 2 && (node_->now() - last_ask_waypoint_time_).seconds() > 2) { nav_->AskWaypoint(); last_ask_waypoint_time_ = node_->now(); diff --git a/src/modelec_strat/src/missions/take_mission.cpp b/src/modelec_strat/src/missions/take_mission.cpp index e18b9cd..7daaed3 100644 --- a/src/modelec_strat/src/missions/take_mission.cpp +++ b/src/modelec_strat/src/missions/take_mission.cpp @@ -40,7 +40,7 @@ namespace Modelec { if (!nav_->HasArrived()) { - if ((node_->now() - go_timeout_).seconds() < 2 && (node_->now() - last_ask_waypoint_time_).seconds() > 1) + if ((node_->now() - go_timeout_).seconds() < 2 && (node_->now() - last_ask_waypoint_time_).seconds() > 2) { nav_->AskWaypoint(); last_ask_waypoint_time_ = node_->now(); From 5cf02712b9703f90cb6fe7ca04200ab359258161 Mon Sep 17 00:00:00 2001 From: acki Date: Tue, 27 Jan 2026 11:46:06 +0100 Subject: [PATCH 63/79] ask waypoint threshold --- 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 a90df5a..b0ad5e2 100644 --- a/src/modelec_strat/src/missions/free_mission.cpp +++ b/src/modelec_strat/src/missions/free_mission.cpp @@ -40,7 +40,7 @@ namespace Modelec { if (!nav_->HasArrived()) { - if ((node_->now() - go_timeout_).seconds() < 2 && (node_->now() - last_ask_waypoint_time_).seconds() > 2) + if ((node_->now() - go_timeout_).seconds() < 2 && (node_->now() - last_ask_waypoint_time_).seconds() > 1) { nav_->AskWaypoint(); last_ask_waypoint_time_ = node_->now(); diff --git a/src/modelec_strat/src/missions/go_home_mission.cpp b/src/modelec_strat/src/missions/go_home_mission.cpp index a3722c4..8e8026e 100644 --- a/src/modelec_strat/src/missions/go_home_mission.cpp +++ b/src/modelec_strat/src/missions/go_home_mission.cpp @@ -35,7 +35,7 @@ namespace Modelec { if (!nav_->HasArrived()) { - if ((node_->now() - go_timeout_).seconds() < 2 && (node_->now() - last_ask_waypoint_time_).seconds() > 2) + if ((node_->now() - go_timeout_).seconds() < 2 && (node_->now() - last_ask_waypoint_time_).seconds() > 1) { nav_->AskWaypoint(); last_ask_waypoint_time_ = node_->now(); diff --git a/src/modelec_strat/src/missions/take_mission.cpp b/src/modelec_strat/src/missions/take_mission.cpp index 7daaed3..e18b9cd 100644 --- a/src/modelec_strat/src/missions/take_mission.cpp +++ b/src/modelec_strat/src/missions/take_mission.cpp @@ -40,7 +40,7 @@ namespace Modelec { if (!nav_->HasArrived()) { - if ((node_->now() - go_timeout_).seconds() < 2 && (node_->now() - last_ask_waypoint_time_).seconds() > 2) + if ((node_->now() - go_timeout_).seconds() < 2 && (node_->now() - last_ask_waypoint_time_).seconds() > 1) { nav_->AskWaypoint(); last_ask_waypoint_time_ = node_->now(); From 9ef5d7ddce330c7d0f76fb405b9fb57eb3d94f25 Mon Sep 17 00:00:00 2001 From: acki Date: Tue, 27 Jan 2026 17:50:57 +0100 Subject: [PATCH 64/79] start --- src/modelec_strat/src/strat_fms.cpp | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/src/modelec_strat/src/strat_fms.cpp b/src/modelec_strat/src/strat_fms.cpp index 642bf0e..e649c22 100644 --- a/src/modelec_strat/src/strat_fms.cpp +++ b/src/modelec_strat/src/strat_fms.cpp @@ -278,6 +278,12 @@ namespace Modelec { current_mission_.reset(); Transition(State::SELECT_MISSION, "Take done"); + } + else if (current_mission_->GetStatus() == MissionStatus::FAILED) + { + current_mission_.reset(); + RCLCPP_ERROR(get_logger(), "Take mission failed!"); + Transition(State::SELECT_MISSION, "Take mission failed"); } break; @@ -308,6 +314,12 @@ namespace Modelec { current_mission_.reset(); Transition(State::SELECT_MISSION, "Free done"); + } + else if (current_mission_->GetStatus() == MissionStatus::FAILED) + { + current_mission_.reset(); + RCLCPP_ERROR(get_logger(), "Free mission failed!"); + Transition(State::SELECT_MISSION, "Free mission failed"); } break; @@ -322,6 +334,12 @@ namespace Modelec { current_mission_.reset(); Transition(State::STOP, "Cleanup done"); + } + else if (current_mission_->GetStatus() == MissionStatus::FAILED) + { + current_mission_.reset(); + RCLCPP_ERROR(get_logger(), "Go Home mission failed!"); + Transition(State::STOP, "Go Home mission failed"); } break; From bb3d9e59695b508be547ef187b2774352fb18c72 Mon Sep 17 00:00:00 2001 From: acki Date: Tue, 27 Jan 2026 18:31:35 +0100 Subject: [PATCH 65/79] log --- src/modelec_strat/src/action_executor.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modelec_strat/src/action_executor.cpp b/src/modelec_strat/src/action_executor.cpp index d41a7cc..6d4bda4 100644 --- a/src/modelec_strat/src/action_executor.cpp +++ b/src/modelec_strat/src/action_executor.cpp @@ -96,10 +96,12 @@ namespace Modelec } else if (msg->buttons[3] == 1) // X button { + RCLCPP_WARN(node_->get_logger(), "Manual Take action triggered"); Take({{0, BaseAction::FRONT}, {1, BaseAction::FRONT}, {2, BaseAction::FRONT}, {3, BaseAction::FRONT}}); } else if (msg->buttons[4] == 1) // Y button { + RCLCPP_WARN(node_->get_logger(), "Manual Free action triggered"); Free({{0, BaseAction::FRONT}, {1, BaseAction::FRONT}, {2, BaseAction::FRONT}, {3, BaseAction::FRONT}}); } } From b29a6a673f2bbe5e09a3a1a669ba50df080d8f87 Mon Sep 17 00:00:00 2001 From: acki Date: Tue, 27 Jan 2026 18:35:54 +0100 Subject: [PATCH 66/79] step --- src/modelec_strat/src/action/take_action.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modelec_strat/src/action/take_action.cpp b/src/modelec_strat/src/action/take_action.cpp index 696b4ca..c23e5df 100644 --- a/src/modelec_strat/src/action/take_action.cpp +++ b/src/modelec_strat/src/action/take_action.cpp @@ -4,6 +4,8 @@ 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, Front front, int n) : TakeAction(action_executor) From ee06352e5dc2bd024aafbcda9b4dfb68c46e5805 Mon Sep 17 00:00:00 2001 From: acki Date: Tue, 27 Jan 2026 18:37:46 +0100 Subject: [PATCH 67/79] log --- src/modelec_strat/src/action_executor.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/modelec_strat/src/action_executor.cpp b/src/modelec_strat/src/action_executor.cpp index 6d4bda4..d41a7cc 100644 --- a/src/modelec_strat/src/action_executor.cpp +++ b/src/modelec_strat/src/action_executor.cpp @@ -96,12 +96,10 @@ namespace Modelec } else if (msg->buttons[3] == 1) // X button { - RCLCPP_WARN(node_->get_logger(), "Manual Take action triggered"); Take({{0, BaseAction::FRONT}, {1, BaseAction::FRONT}, {2, BaseAction::FRONT}, {3, BaseAction::FRONT}}); } else if (msg->buttons[4] == 1) // Y button { - RCLCPP_WARN(node_->get_logger(), "Manual Free action triggered"); Free({{0, BaseAction::FRONT}, {1, BaseAction::FRONT}, {2, BaseAction::FRONT}, {3, BaseAction::FRONT}}); } } From 7f5150a112f651bd1dc6d787b877c1dd4a9c51c8 Mon Sep 17 00:00:00 2001 From: acki Date: Tue, 27 Jan 2026 18:38:42 +0100 Subject: [PATCH 68/79] angle value --- src/modelec_strat/src/action/free_action.cpp | 4 ++-- 1 file 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 d5b83d8..f4d7423 100644 --- a/src/modelec_strat/src/action/free_action.cpp +++ b/src/modelec_strat/src/action/free_action.cpp @@ -45,8 +45,8 @@ void Modelec::FreeAction::Next() for (size_t i = 0; i < servos_.size(); i++) { msg.items[i].id = servos_[i].first + (servos_[i].second ? 4 : 12); - 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].start_angle = servos_[i].second ? 2.7 : 0; + msg.items[i].end_angle = servos_[i].second ? 0.8 : 0; msg.items[i].duration_s = 0.5; } From 854afbe5b343c561a93ffae858b79ecb22ff7872 Mon Sep 17 00:00:00 2001 From: acki Date: Tue, 27 Jan 2026 18:44:59 +0100 Subject: [PATCH 69/79] static strat --- .../include/modelec_strat/strat_fms.hpp | 1 + src/modelec_strat/src/strat_fms.cpp | 19 ++++++++++++++++--- 2 files changed, 17 insertions(+), 3 deletions(-) diff --git a/src/modelec_strat/include/modelec_strat/strat_fms.hpp b/src/modelec_strat/include/modelec_strat/strat_fms.hpp index d552887..2069550 100644 --- a/src/modelec_strat/include/modelec_strat/strat_fms.hpp +++ b/src/modelec_strat/include/modelec_strat/strat_fms.hpp @@ -62,6 +62,7 @@ namespace Modelec int team_id_ = 0; std::queue game_action_sequence_; + bool static_strat_ = false; std::shared_ptr nav_; std::shared_ptr action_executor_; diff --git a/src/modelec_strat/src/strat_fms.cpp b/src/modelec_strat/src/strat_fms.cpp index e649c22..0fb6528 100644 --- a/src/modelec_strat/src/strat_fms.cpp +++ b/src/modelec_strat/src/strat_fms.cpp @@ -67,9 +67,6 @@ namespace Modelec } game_action_sequence_.push(State::TAKE_MISSION); - game_action_sequence_.push(State::FREE_MISSION); - game_action_sequence_.push(State::TAKE_MISSION); - game_action_sequence_.push(State::FREE_MISSION); } void StratFMS::Init() @@ -195,6 +192,22 @@ namespace Modelec } case State::SELECT_GAME_ACTION: { + + if (!game_action_sequence_.empty() || static_strat_) { + if (!game_action_sequence_.empty()) { + Transition(State::STOP, "No more game actions in sequence"); + return; + } + + static_strat_ = true; + + auto next_action = game_action_sequence_.front(); + game_action_sequence_.pop(); + + Transition(next_action, "Selecting next game action from sequence"); + return; + } + if (action_executor_->IsFull()) { RCLCPP_INFO(get_logger(), "All box are present on robot, selecting FREE mission"); From e2f3304b3413b8ca7194561b33aa0b11bd3efa23 Mon Sep 17 00:00:00 2001 From: acki Date: Tue, 27 Jan 2026 18:51:07 +0100 Subject: [PATCH 70/79] static strat --- src/modelec_strat/src/strat_fms.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modelec_strat/src/strat_fms.cpp b/src/modelec_strat/src/strat_fms.cpp index 0fb6528..116f495 100644 --- a/src/modelec_strat/src/strat_fms.cpp +++ b/src/modelec_strat/src/strat_fms.cpp @@ -194,7 +194,7 @@ namespace Modelec { if (!game_action_sequence_.empty() || static_strat_) { - if (!game_action_sequence_.empty()) { + if (game_action_sequence_.empty()) { Transition(State::STOP, "No more game actions in sequence"); return; } From 2f2b888b28f5b93e91b318706bba0c7295d9a4dd Mon Sep 17 00:00:00 2001 From: acki Date: Tue, 27 Jan 2026 18:55:51 +0100 Subject: [PATCH 71/79] message --- src/modelec_strat/src/strat_fms.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modelec_strat/src/strat_fms.cpp b/src/modelec_strat/src/strat_fms.cpp index 116f495..5d052f4 100644 --- a/src/modelec_strat/src/strat_fms.cpp +++ b/src/modelec_strat/src/strat_fms.cpp @@ -177,7 +177,7 @@ namespace Modelec else if (elapsed.seconds() < 60 && !action_executor_->IsEmpty()) { - Transition(State::FREE_MISSION, "Selecting a game action"); + Transition(State::FREE_MISSION, "No Time left, freeing boxes"); } else if (elapsed.seconds() < 80) { From b08cac558bfb262ce29996b4c93182255810baba Mon Sep 17 00:00:00 2001 From: acki Date: Tue, 27 Jan 2026 18:57:17 +0100 Subject: [PATCH 72/79] static --- src/modelec_strat/src/strat_fms.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/modelec_strat/src/strat_fms.cpp b/src/modelec_strat/src/strat_fms.cpp index 5d052f4..0588ea7 100644 --- a/src/modelec_strat/src/strat_fms.cpp +++ b/src/modelec_strat/src/strat_fms.cpp @@ -67,6 +67,7 @@ namespace Modelec } game_action_sequence_.push(State::TAKE_MISSION); + static_strat_ = true; } void StratFMS::Init() @@ -193,14 +194,12 @@ namespace Modelec case State::SELECT_GAME_ACTION: { - if (!game_action_sequence_.empty() || static_strat_) { + if (static_strat_) { if (game_action_sequence_.empty()) { Transition(State::STOP, "No more game actions in sequence"); return; } - static_strat_ = true; - auto next_action = game_action_sequence_.front(); game_action_sequence_.pop(); From d608eb0ceae4814e835b88c80fdcbddac523634b Mon Sep 17 00:00:00 2001 From: acki Date: Tue, 27 Jan 2026 19:00:43 +0100 Subject: [PATCH 73/79] static --- src/modelec_strat/src/strat_fms.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modelec_strat/src/strat_fms.cpp b/src/modelec_strat/src/strat_fms.cpp index 0588ea7..4ce0294 100644 --- a/src/modelec_strat/src/strat_fms.cpp +++ b/src/modelec_strat/src/strat_fms.cpp @@ -176,7 +176,7 @@ namespace Modelec Transition(State::STOP, "All missions done"); } - else if (elapsed.seconds() < 60 && !action_executor_->IsEmpty()) + else if (elapsed.seconds() > 60 && !action_executor_->IsEmpty()) { Transition(State::FREE_MISSION, "No Time left, freeing boxes"); } From 22f2f84c4534246244fec47b6674a8637fe4fbaa Mon Sep 17 00:00:00 2001 From: acki Date: Tue, 27 Jan 2026 19:22:16 +0100 Subject: [PATCH 74/79] static --- src/modelec_strat/src/strat_fms.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modelec_strat/src/strat_fms.cpp b/src/modelec_strat/src/strat_fms.cpp index 4ce0294..155f049 100644 --- a/src/modelec_strat/src/strat_fms.cpp +++ b/src/modelec_strat/src/strat_fms.cpp @@ -196,7 +196,7 @@ namespace Modelec if (static_strat_) { if (game_action_sequence_.empty()) { - Transition(State::STOP, "No more game actions in sequence"); + Transition(State::DO_GO_HOME, "No more game actions in sequence"); return; } From f480c062882d25481b25a8e52922a8539a645044 Mon Sep 17 00:00:00 2001 From: acki Date: Tue, 27 Jan 2026 19:36:41 +0100 Subject: [PATCH 75/79] value --- src/modelec_utils/include/modelec_utils/point.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modelec_utils/include/modelec_utils/point.hpp b/src/modelec_utils/include/modelec_utils/point.hpp index 578286f..90850ac 100644 --- a/src/modelec_utils/include/modelec_utils/point.hpp +++ b/src/modelec_utils/include/modelec_utils/point.hpp @@ -1,6 +1,6 @@ #pragma once -#define CLOSE_DISTANCE 150 +#define CLOSE_DISTANCE 165 #define BASE_DISTANCE 290 namespace Modelec From bc225bc913a49a1d3a0d04b30dfbb9ea5f5ec4dc Mon Sep 17 00:00:00 2001 From: acki Date: Tue, 27 Jan 2026 19:52:03 +0100 Subject: [PATCH 76/79] servo value --- src/modelec_strat/src/action/down_action.cpp | 4 ++-- src/modelec_strat/src/action/free_action.cpp | 4 ++-- src/modelec_strat/src/action/take_action.cpp | 4 ++-- src/modelec_strat/src/action/up_action.cpp | 4 ++-- 4 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 19a6dde..889ff80 100644 --- a/src/modelec_strat/src/action/down_action.cpp +++ b/src/modelec_strat/src/action/down_action.cpp @@ -35,12 +35,12 @@ void Modelec::DownAction::Next() { msg.items[0].id = 0; msg.items[0].start_angle = 1.95; - msg.items[0].end_angle = 2.95; + msg.items[0].end_angle = 3; msg.items[0].duration_s = 1; msg.items[1].id = 1; msg.items[1].start_angle = 1.9; - msg.items[1].end_angle = 0.9; + msg.items[1].end_angle = 0.85; msg.items[1].duration_s = 1; msg.items[2].id = 2; diff --git a/src/modelec_strat/src/action/free_action.cpp b/src/modelec_strat/src/action/free_action.cpp index f4d7423..52fe403 100644 --- a/src/modelec_strat/src/action/free_action.cpp +++ b/src/modelec_strat/src/action/free_action.cpp @@ -45,8 +45,8 @@ void Modelec::FreeAction::Next() for (size_t i = 0; i < servos_.size(); i++) { msg.items[i].id = servos_[i].first + (servos_[i].second ? 4 : 12); - msg.items[i].start_angle = servos_[i].second ? 2.7 : 0; - msg.items[i].end_angle = servos_[i].second ? 0.8 : 0; + msg.items[i].start_angle = servos_[i].second ? 3 : 0; + msg.items[i].end_angle = servos_[i].second ? 1 : 0; msg.items[i].duration_s = 0.5; } diff --git a/src/modelec_strat/src/action/take_action.cpp b/src/modelec_strat/src/action/take_action.cpp index c23e5df..25849a6 100644 --- a/src/modelec_strat/src/action/take_action.cpp +++ b/src/modelec_strat/src/action/take_action.cpp @@ -45,8 +45,8 @@ void Modelec::TakeAction::Next() for (size_t i = 0; i < servos_.size(); i++) { msg.items[i].id = servos_[i].first + (servos_[i].second ? 4 : 12); - 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].start_angle = servos_[i].second ? 1 : 0; + msg.items[i].end_angle = servos_[i].second ? 3 : 0; msg.items[i].duration_s = 0.5; } diff --git a/src/modelec_strat/src/action/up_action.cpp b/src/modelec_strat/src/action/up_action.cpp index 1cbeb21..8cb3a45 100644 --- a/src/modelec_strat/src/action/up_action.cpp +++ b/src/modelec_strat/src/action/up_action.cpp @@ -34,12 +34,12 @@ void Modelec::UPAction::Next() if (front_ == FRONT || front_ == BOTH) { msg.items[0].id = 0; - msg.items[0].start_angle = 2.95; + msg.items[0].start_angle = 3; msg.items[0].end_angle = 1.95; msg.items[0].duration_s = 1; msg.items[1].id = 1; - msg.items[1].start_angle = 0.9; + msg.items[1].start_angle = 0.85; msg.items[1].end_angle = 1.9; msg.items[1].duration_s = 1; From c1ca8daa40b3eca5ff59f21501dc13c29d52a6f5 Mon Sep 17 00:00:00 2001 From: acki Date: Tue, 27 Jan 2026 20:05:15 +0100 Subject: [PATCH 77/79] servo value --- src/modelec_strat/src/action/down_action.cpp | 2 +- src/modelec_strat/src/action/up_action.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modelec_strat/src/action/down_action.cpp b/src/modelec_strat/src/action/down_action.cpp index 889ff80..fd843f9 100644 --- a/src/modelec_strat/src/action/down_action.cpp +++ b/src/modelec_strat/src/action/down_action.cpp @@ -50,7 +50,7 @@ void Modelec::DownAction::Next() msg.items[3].id = 3; msg.items[3].start_angle = 2.7; - msg.items[3].end_angle = 3; + msg.items[3].end_angle = 2.9; msg.items[3].duration_s = 1; } diff --git a/src/modelec_strat/src/action/up_action.cpp b/src/modelec_strat/src/action/up_action.cpp index 8cb3a45..0907bba 100644 --- a/src/modelec_strat/src/action/up_action.cpp +++ b/src/modelec_strat/src/action/up_action.cpp @@ -49,8 +49,8 @@ void Modelec::UPAction::Next() msg.items[2].duration_s = 1; msg.items[3].id = 3; - msg.items[3].start_angle = 3; - msg.items[3].end_angle = 2.5; + msg.items[3].start_angle = 2.9; + msg.items[3].end_angle = 2.7; msg.items[3].duration_s = 1; } From 2f68d8404e04554142236a7ee9141cee2d52b1ca Mon Sep 17 00:00:00 2001 From: acki Date: Wed, 28 Jan 2026 08:48:00 +0100 Subject: [PATCH 78/79] angle --- src/modelec_strat/src/missions/free_mission.cpp | 4 ++-- src/modelec_strat/src/strat_fms.cpp | 6 ++++++ 2 files changed, 8 insertions(+), 2 deletions(-) diff --git a/src/modelec_strat/src/missions/free_mission.cpp b/src/modelec_strat/src/missions/free_mission.cpp index b0ad5e2..42975bd 100644 --- a/src/modelec_strat/src/missions/free_mission.cpp +++ b/src/modelec_strat/src/missions/free_mission.cpp @@ -119,8 +119,8 @@ namespace Modelec { auto pos = nav_->GetCurrentPos(); obs->SetPosition( - pos->x + 200 * cos(pos->theta), - pos->y + 200 * sin(pos->theta), + pos->x + 200 * cos(pos->theta + (front_ == BaseAction::FRONT ? 0 : M_PI)), + pos->y + 200 * sin(pos->theta + (front_ == BaseAction::FRONT ? 0 : M_PI)), pos->theta); obs->SetAtObjective(true); diff --git a/src/modelec_strat/src/strat_fms.cpp b/src/modelec_strat/src/strat_fms.cpp index 155f049..be23ccf 100644 --- a/src/modelec_strat/src/strat_fms.cpp +++ b/src/modelec_strat/src/strat_fms.cpp @@ -67,6 +67,9 @@ namespace Modelec } game_action_sequence_.push(State::TAKE_MISSION); + game_action_sequence_.push(State::TAKE_MISSION); + game_action_sequence_.push(State::FREE_MISSION); + game_action_sequence_.push(State::FREE_MISSION); static_strat_ = true; } @@ -207,6 +210,9 @@ namespace Modelec return; } + + // TODO : If close to border, do the side mission (thermometre) + if (action_executor_->IsFull()) { RCLCPP_INFO(get_logger(), "All box are present on robot, selecting FREE mission"); From f638db03511d8cd51685fa58b08ad550d0536b04 Mon Sep 17 00:00:00 2001 From: acki Date: Wed, 28 Jan 2026 08:53:00 +0100 Subject: [PATCH 79/79] fix merge --- .../src/missions/free_mission.cpp | 18 ------------------ 1 file changed, 18 deletions(-) diff --git a/src/modelec_strat/src/missions/free_mission.cpp b/src/modelec_strat/src/missions/free_mission.cpp index 16e396c..f9e0359 100644 --- a/src/modelec_strat/src/missions/free_mission.cpp +++ b/src/modelec_strat/src/missions/free_mission.cpp @@ -156,24 +156,6 @@ namespace Modelec { go_timeout_ = node_->now(); } - step_ = UP; - break; - case UP: - { - action_executor_->Up(BaseAction::FRONT); - deploy_time_ = node_->now(); - } - - step_ = GO_BACK; - break; - case GO_BACK: - { - nav_->GoTo(target_deposite_zone_->GetPosition().GetTakePosition(500), true, Pathfinding::FREE | Pathfinding::OBSTACLE); - - go_timeout_ = node_->now(); - } - - step_ = DONE; break; case DONE: {