From 3069a69ef021008c80925ab718ba31b12a64b6e6 Mon Sep 17 00:00:00 2001 From: 2006wu Date: Mon, 9 Feb 2026 23:43:33 +0800 Subject: [PATCH] simulation and real system_check update --- src/navigation2_run/src/system_check.cpp | 197 ++++++------------ .../src/system_check_test_server.cpp | 79 +++++-- 2 files changed, 126 insertions(+), 150 deletions(-) diff --git a/src/navigation2_run/src/system_check.cpp b/src/navigation2_run/src/system_check.cpp index 41f6543..0938fcc 100644 --- a/src/navigation2_run/src/system_check.cpp +++ b/src/navigation2_run/src/system_check.cpp @@ -1,5 +1,5 @@ #include "rclcpp/rclcpp.hpp" -#include "std_msgs/msg/string.hpp" +#include "std_msgs/msg/bool.hpp" #include "btcpp_ros2_interfaces/srv/start_up_srv.hpp" #include "rclcpp_action/rclcpp_action.hpp" #include "nav2_msgs/action/navigate_to_pose.hpp" @@ -19,12 +19,15 @@ class SystemCheck : public rclcpp::Node using NavigateToPose = nav2_msgs::action::NavigateToPose; using DockRobot = opennav_docking_msgs::action::DockRobot; + const int GROUP_NAVIGATION = 3; + const int STATE_READY = 1; + SystemCheck() : Node("system_check"), running_(false), obstacle_check_count_(10) { - ready_sub_ = this->create_subscription( - "/robot/startup/plan", 10, - std::bind(&SystemCheck::readyCallback, this, std::placeholders::_1)); + are_you_ready_sub_ = this->create_subscription( + "/robot/startup/are_you_ready", 10, + std::bind(&SystemCheck::areYouReadyCallback, this, std::placeholders::_1)); costmap_subscription_ = this->create_subscription( "/global_costmap/costmap", rclcpp::QoS(10), @@ -49,17 +52,11 @@ class SystemCheck : public rclcpp::Node this->declare_parameter("external_rival_data_path", ""); this->get_parameter("external_rival_data_path", external_rival_data_path_); - // send goal - this->declare_parameter("goal_x", 1.2); - this->declare_parameter("goal_y", 0.4); - this->get_parameter("goal_x", goal_x); - this->get_parameter("goal_y", goal_y); - RCLCPP_INFO(this->get_logger(), "\033[1;35m SystemCheck started, waiting for startup plan... \033[0m"); } private: - rclcpp::Subscription::SharedPtr ready_sub_; + rclcpp::Subscription::SharedPtr are_you_ready_sub_; rclcpp::Subscription::SharedPtr costmap_subscription_; nav_msgs::msg::OccupancyGrid::SharedPtr latest_costmap_; rclcpp::Subscription::SharedPtr subscription_pose_; @@ -68,84 +65,54 @@ class SystemCheck : public rclcpp::Node rclcpp_action::Client::SharedPtr navigate_to_pose_client_; rclcpp_action::Client::SharedPtr dock_robot_client_; rclcpp::TimerBase::SharedPtr obstacle_timer_; - rclcpp::TimerBase::SharedPtr ready_signal_timer_; bool running_; int costmap_tolerance_; std::string external_rival_data_path_; int obstacle_check_count_; - bool warn_once_ = true; - int fail_count_ = 0; - double goal_x = 0.0; - double goal_y = 0.0; - void readyCallback(const std_msgs::msg::String::SharedPtr msg) + void areYouReadyCallback(const std_msgs::msg::Bool::SharedPtr msg) { - if (msg == nullptr || running_) - return; + if (!msg->data || running_) return; running_ = true; // Set running state to true to prevent re-entrance + RCLCPP_INFO(this->get_logger(), "[NAVIGATION] are_you_ready received, starting system check..."); // Wait for services and action servers - while (!ready_srv_client_->wait_for_service(1s)) { - RCLCPP_WARN(this->get_logger(), "Waiting for ReadySignal service..."); - if (!rclcpp::ok()) return; - } - while (!navigate_to_pose_client_->wait_for_action_server(1s)) { - RCLCPP_WARN(this->get_logger(), "Waiting for navigate_to_pose action server..."); - if (!rclcpp::ok()) return; - } - while (!dock_robot_client_->wait_for_action_server(1s)) { - RCLCPP_WARN(this->get_logger(), "Waiting for dock_robot action server..."); - if (!rclcpp::ok()) return; + if (!dependenciesReady()) { + RCLCPP_WARN(this->get_logger(), "[NAVIGATION] Dependencies not ready"); + running_ = false; // Reset running state to allow retry + return; } // Start obstacle check timer if in obstacle if (inObstacle()) { RCLCPP_WARN(this->get_logger(), "Robot is in obstacles, waiting for manual action..."); obstacle_check_count_ = 10; - obstacle_timer_ = this->create_wall_timer( - 1s, std::bind(&SystemCheck::obstacleCheckTimer, this)); - return; - } - // test a point - if(sendGoal(goal_x, goal_y)) { - RCLCPP_INFO(this->get_logger(), "\033[1;32m Goal reach successfully! \033[0m"); - } else { - RCLCPP_ERROR(this->get_logger(), "Failed to send goal."); + if (!obstacle_timer_){ + obstacle_timer_ = this->create_wall_timer(1s, std::bind(&SystemCheck::obstacleCheckTimer, this)); + } return; } - - // All systems ready - RCLCPP_INFO(this->get_logger(), "\033[1;32m All systems ready !!! \033[0m"); - sendReadySignal(3, 3); // group = 3 (navigation), state = 3 (START) + sendReadySignal(GROUP_NAVIGATION, STATE_READY); + running_ = false; } - - bool sendGoal(double map_x, double map_y) - { - RCLCPP_INFO(this->get_logger(), "Sending goal to navigate to pose: (%.2f, %.2f)", map_x, map_y); - auto goal_msg = NavigateToPose::Goal(); - goal_msg.pose.header.frame_id = "map"; - goal_msg.pose.header.stamp = this->now(); - goal_msg.pose.pose.position.x = map_x; - goal_msg.pose.pose.position.y = map_y; - goal_msg.pose.pose.orientation.w = 1.0; - - auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); - send_goal_options.result_callback = - [this](const rclcpp_action::ClientGoalHandle::WrappedResult & result) { - if (result.code == rclcpp_action::ResultCode::SUCCEEDED) { - RCLCPP_INFO(this->get_logger(), "\033[1;32m Goal reached successfully! \033[0m"); - return true; - } else { - RCLCPP_ERROR(this->get_logger(), "Failed to reach goal."); - return false; - } - }; - - navigate_to_pose_client_->async_send_goal(goal_msg, send_goal_options); + bool dependenciesReady() { + // Check if the service and action servers are available + if (!ready_srv_client_->wait_for_service(0s)) { + RCLCPP_WARN(this->get_logger(), "Ready service not available after waiting"); + return false; + } + if (!navigate_to_pose_client_->wait_for_action_server(0s)) { + RCLCPP_WARN(this->get_logger(), "NavigateToPose action server not available after waiting"); + return false; + } + if (!dock_robot_client_->wait_for_action_server(0s)) { + RCLCPP_WARN(this->get_logger(), "DockRobot action server not available after waiting"); + return false; + } return true; } @@ -153,20 +120,16 @@ class SystemCheck : public rclcpp::Node { if (!inObstacle()) { obstacle_timer_->cancel(); - RCLCPP_INFO(this->get_logger(), "\033[1;32m All systems ready after obstacle cleared! \033[0m"); - sendReadySignal(3, 3); + obstacle_timer_.reset(); + RCLCPP_INFO(this->get_logger(), "\033[1;32m [NAVIGATION] Obstacle cleared \033[0m"); + sendReadySignal(GROUP_NAVIGATION, STATE_READY); + running_ = false; return; } - if (obstacle_check_count_ > 0) { - RCLCPP_INFO(this->get_logger(), "\033[1;35m Automatic shrinking process is going to activate after %d seconds... \033[0m", obstacle_check_count_); - obstacle_check_count_--; - } else { + if (obstacle_check_count_-- <= 0) { shrinkRivalRadius(); - // After shrinking, check again in 0.1s - obstacle_timer_->cancel(); - obstacle_timer_ = this->create_wall_timer( - 100ms, std::bind(&SystemCheck::obstacleCheckTimer, this)); + obstacle_check_count_ = 10; // Reset the counter after shrinking } } @@ -181,39 +144,16 @@ class SystemCheck : public rclcpp::Node auto response = future.get(); if (response->success) { RCLCPP_INFO(this->get_logger(), "\033[1;32m ReadySignal SUCCESS: group=%d \033[0m", response->group); - // RCLCPP_INFO(this->get_logger(), "\033[1;32m Check again after 5 second... \033[0m"); - // Wait 5 seconds before next action - ready_signal_timer_ = this->create_wall_timer( - 5s, [this]() { - ready_signal_timer_->cancel(); - // running_ = false; - warn_once_ = true; // Reset warning flag for next signal - }); } else { - RCLCPP_WARN(this->get_logger(), "ReadySignal FAILED, retrying after 1 second..."); - fail_count_++; - // Wait 1 second before next action - ready_signal_timer_ = this->create_wall_timer( - 1s, [this, group, state]() { - ready_signal_timer_->cancel(); - // Retry sending the signal after 1 second - if(fail_count_ >= 3) { - RCLCPP_ERROR(this->get_logger(), "Failed to send ReadySignal 3 times, aborting..."); - // running_ = false; // Reset running state - warn_once_ = true; // Reset warning flag for next signal - fail_count_ = 0; // Reset fail count - return; - } - sendReadySignal(group, state); - }); + RCLCPP_WARN(this->get_logger(), "[NAVIGATION] READY rejected"); } }); } bool inObstacle() { if (!latest_costmap_ || !latest_pose_) { - RCLCPP_WARN(this->get_logger(), "No costmap or pose received yet."); - return false; + RCLCPP_WARN(this->get_logger(), "[NAVIGATION] No costmap or pose received yet."); + return true; } int mapX = static_cast(latest_pose_->pose.pose.position.x * 100.0); @@ -221,38 +161,29 @@ class SystemCheck : public rclcpp::Node int width = latest_costmap_->info.width; int index = mapY * width + mapX; - if (latest_costmap_->data[index] > costmap_tolerance_) { - // RCLCPP_INFO(this->get_logger(), "Obstacle data is [%d]", latest_costmap_->data[index]); - return true; - } - return false; + if (index < 0 || index >= static_cast(latest_costmap_->data.size())) + return false; + + return latest_costmap_->data[index] > costmap_tolerance_; } void shrinkRivalRadius() { + if (external_rival_data_path_.empty()) return; + try { - YAML::Node config = YAML::LoadFile(external_rival_data_path_); - if (config["nav_rival_parameters"] && config["nav_rival_parameters"]["rival_inscribed_radius"]) { - double rival_inscribed_radius = config["nav_rival_parameters"]["rival_inscribed_radius"].as(); - if (rival_inscribed_radius > 0.0) { - rival_inscribed_radius = std::max(0.0, std::round((rival_inscribed_radius - 0.01) * 100.0) / 100.0); - config["nav_rival_parameters"]["rival_inscribed_radius"] = rival_inscribed_radius; - std::ofstream fout(external_rival_data_path_); - fout << config; - fout.close(); - } else { - if(warn_once_) { - RCLCPP_WARN(this->get_logger(), "rival_inscribed_radius is already at 0, cannot shrink further"); - RCLCPP_WARN(this->get_logger(), "Aborting automatic shrinking process..."); - warn_once_ = false; // Prevent further warnings - } else { - return; // Avoid repeated warnings - } - } - } else { - RCLCPP_WARN(this->get_logger(), "rival_inscribed_radius not found in YAML file, cannot shrink automatically"); - } + YAML::Node config = YAML::LoadFile(external_rival_data_path_); + YAML::Node node = config["nav_rival_parameters"]["rival_inscribed_radius"]; + if (!node) return; + + double r = node.as(); + r = std::max(0.0, r - 0.01); // Decrease radius but not below 0.05 + config["nav_rival_parameters"]["rival_inscribed_radius"] = r; + std::ofstream out(external_rival_data_path_); + out << config; + + RCLCPP_INFO(this->get_logger(), "[NAVIGATION] shrink rival_inscribed_radius to %.2f", r); } catch (const std::exception &e) { - RCLCPP_ERROR(this->get_logger(), "%s %s -> Failed to update YAML file, cannot shrink automatically", e.what(), external_rival_data_path_.c_str()); + RCLCPP_WARN(this->get_logger(), "Failed to shrink rival radius %s", e.what()); } } }; @@ -261,9 +192,7 @@ int main(int argc, char **argv) { rclcpp::init(argc, argv); auto node = std::make_shared(); - // Use MultiThreadedExecutor for safety if you add more timers or callbacks - rclcpp::executors::MultiThreadedExecutor exec; - exec.add_node(node); - exec.spin(); + rclcpp::spin(node); + rclcpp::shutdown(); return 0; } \ No newline at end of file diff --git a/src/navigation2_run/src/system_check_test_server.cpp b/src/navigation2_run/src/system_check_test_server.cpp index c99e5fa..0062a12 100644 --- a/src/navigation2_run/src/system_check_test_server.cpp +++ b/src/navigation2_run/src/system_check_test_server.cpp @@ -1,40 +1,87 @@ #include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/bool.hpp" #include "btcpp_ros2_interfaces/srv/start_up_srv.hpp" -class ReadySignalServer : public rclcpp::Node +using namespace std::chrono_literals; + +class SimSystemCheck : public rclcpp::Node { public: - ReadySignalServer() : Node("ready_signal_server") + const int GROUP_NAVIGATION = 3; + const int STATE_READY = 1; + + SimSystemCheck() : Node("sim_system_check") { - server_ = this->create_service( - "/robot/startup/ready_signal", - std::bind(&ReadySignalServer::handleRequest, this, std::placeholders::_1, std::placeholders::_2)); + are_you_ready_sub_ = this->create_subscription( + "/robot/startup/are_you_ready", 10, + std::bind(&SimSystemCheck::areYouReadyCallback, this, std::placeholders::_1)); + + ready_client_ = this->create_client( + "/robot/startup/ready_signal"); - RCLCPP_INFO(this->get_logger(), "ReadySignal service server started and ready!"); + this->declare_parameter("ready_delay_sec_", 0.5); + ready_delay_sec_ = this->get_parameter("ready_delay_sec_").as_double(); + RCLCPP_INFO(this->get_logger(), "SystemCheck ready, waiting for are_you_ready"); } private: - rclcpp::Service::SharedPtr server_; + rclcpp::Subscription::SharedPtr are_you_ready_sub_; + rclcpp::Client::SharedPtr ready_client_; + rclcpp::TimerBase::SharedPtr delay_timer_; + + bool ready_sent_; + double ready_delay_sec_; - void handleRequest( - const std::shared_ptr request, - std::shared_ptr response) + void areYouReadyCallback(const std_msgs::msg::Bool::SharedPtr msg) { + if (!msg->data || ready_sent_) + return; + + if (!ready_client_->wait_for_service(0s)) { + RCLCPP_WARN(this->get_logger(), + "[SIM] ready_signal service not available"); + return; + } + RCLCPP_INFO(this->get_logger(), - "Received ReadySignal Request -> group: %d, state: %d", request->group, request->state); + "[SIM] are_you_ready received, scheduling READY"); + + delay_timer_ = this->create_wall_timer( + std::chrono::duration(ready_delay_sec_), + std::bind(&SimSystemCheck::sendReadySignal, this)); + } + + void sendReadySignal() + { + delay_timer_->cancel(); + ready_sent_ = true; + + auto req = std::make_shared(); + + req->group = GROUP_NAVIGATION; + req->state = STATE_READY; - response->group = request->group; - response->success = true; + using ResponseFuture = rclcpp::Client::SharedFuture; - RCLCPP_INFO(this->get_logger(), "Sent response: success = true"); + ready_client_->async_send_request(req,[this](ResponseFuture future) { + auto res = future.get(); + if (res->success) { + RCLCPP_INFO(this->get_logger(), + "[SIM] READY acknowledged by Startup"); + } else { + RCLCPP_WARN(this->get_logger(), + "[SIM] READY rejected"); + ready_sent_ = false; // allow retry + } + }); } }; int main(int argc, char **argv) { rclcpp::init(argc, argv); - auto node = std::make_shared(); + auto node = std::make_shared(); rclcpp::spin(node); rclcpp::shutdown(); return 0; -} +} \ No newline at end of file