Skip to content

simulation and real system_check update #47

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
2006wu wants to merge 1 commit into
base: develop
Choose a base branch
from
Open

simulation and real system_check update #47

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
197 changes: 63 additions & 134 deletions src/navigation2_run/src/system_check.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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"
Expand All @@ -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<std_msgs::msg::String>(
"/robot/startup/plan", 10,
std::bind(&SystemCheck::readyCallback, this, std::placeholders::_1));
are_you_ready_sub_ = this->create_subscription<std_msgs::msg::Bool>(
"/robot/startup/are_you_ready", 10,
std::bind(&SystemCheck::areYouReadyCallback, this, std::placeholders::_1));

costmap_subscription_ = this->create_subscription<nav_msgs::msg::OccupancyGrid>(
"/global_costmap/costmap", rclcpp::QoS(10),
Expand All @@ -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<std_msgs::msg::String>::SharedPtr ready_sub_;
rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr are_you_ready_sub_;
rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::SharedPtr costmap_subscription_;
nav_msgs::msg::OccupancyGrid::SharedPtr latest_costmap_;
rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr subscription_pose_;
Expand All @@ -68,105 +65,71 @@ class SystemCheck : public rclcpp::Node
rclcpp_action::Client<NavigateToPose>::SharedPtr navigate_to_pose_client_;
rclcpp_action::Client<DockRobot>::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<NavigateToPose>::SendGoalOptions();
send_goal_options.result_callback =
[this](const rclcpp_action::ClientGoalHandle<NavigateToPose>::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;
}

void obstacleCheckTimer()
{
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
}
}

Expand All @@ -181,78 +144,46 @@ 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<int>(latest_pose_->pose.pose.position.x * 100.0);
int mapY = static_cast<int>(latest_pose_->pose.pose.position.y * 100.0);
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<int>(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<double>();
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<double>();
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());
}
}
};
Expand All @@ -261,9 +192,7 @@ int main(int argc, char **argv)
{
rclcpp::init(argc, argv);
auto node = std::make_shared<SystemCheck>();
// 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;
}
Loading