From c4a8c282fe20e60e9ec858fb9c573c4a4bdde893 Mon Sep 17 00:00:00 2001 From: acki Date: Wed, 17 Dec 2025 20:11:22 +0100 Subject: [PATCH] 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_)