From 867fbf9c23243d7cc42b948fa193535f48104fba Mon Sep 17 00:00:00 2001 From: acki Date: Thu, 29 May 2025 20:03:56 -0400 Subject: [PATCH] some patch --- simulated_pcb/odo.py | 4 +- src/modelec_strat/src/enemy_manager.cpp | 43 ++++++++++--------- .../src/missions/test.go_home_mission.cpp | 7 ++- src/modelec_strat/src/navigation_helper.cpp | 24 ++++++----- 4 files changed, 44 insertions(+), 34 deletions(-) diff --git a/simulated_pcb/odo.py b/simulated_pcb/odo.py index 004332c..cd71e23 100644 --- a/simulated_pcb/odo.py +++ b/simulated_pcb/odo.py @@ -56,12 +56,12 @@ class SimulatedPCB: self.waypoint_order.pop(0) del self.waypoints[wp['id']] else: - speed = min(300.0, distance * 3) + speed = min(150.0, distance) 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(600.0, distance * 4) + speed = min(200.0, distance * 2) self.vx = speed * math.cos(angle) self.vy = speed * math.sin(angle) self.vtheta = 0.0 diff --git a/src/modelec_strat/src/enemy_manager.cpp b/src/modelec_strat/src/enemy_manager.cpp index 805726b..0fe2927 100644 --- a/src/modelec_strat/src/enemy_manager.cpp +++ b/src/modelec_strat/src/enemy_manager.cpp @@ -61,8 +61,8 @@ namespace Modelec { game_stated_ = true; } - - if (game_stated_ && msg->state == modelec_interfaces::msg::StratState::STOP) + else if (game_stated_ && msg->state == + modelec_interfaces::msg::StratState::STOP) { game_stated_ = false; } @@ -91,6 +91,7 @@ namespace Modelec { if (!game_stated_) { + RCLCPP_INFO(this->get_logger(), "Game not started, ignoring Lidar data"); return; } @@ -112,7 +113,8 @@ namespace Modelec double best_x = 0.0; double best_y = 0.0; - // RCLCPP_INFO(this->get_logger(), "Processing Lidar min %f | %f | %f", msg->range_min, msg->range_max, msg->ranges[1]); + RCLCPP_INFO(this->get_logger(), "Processing Lidar min %f | %f | %f", msg->range_min, msg->range_max, + msg->ranges[1]); // RCLCPP_INFO(this->get_logger(), "Robot pos %f | %f | %f", robot_x, robot_y, robot_theta); for (size_t i = 0; i < msg->ranges.size(); ++i) @@ -127,17 +129,18 @@ namespace Modelec continue; } - double range_mm = range * 1000.0; // conversion en mm + double range_mm = range * 1000.0; // conversion en mm if (range_mm < robot_radius_) { - RCLCPP_INFO(this->get_logger(), "Ignoring Lidar point too close to robot body: range=%.2f mm", range_mm); + RCLCPP_INFO(this->get_logger(), "Ignoring Lidar point too close to robot body: range=%.2f mm", + range_mm); angle += msg->angle_increment; continue; } // EMERGENCY detection: obstacle very close but not the robot itself - if (range_mm < min_emergency_distance_) + /*if (range_mm < min_emergency_distance_) { // Convert to local robot frame double x_local = range * std::cos(angle) * 1000.0; // meters -> mm @@ -148,24 +151,21 @@ namespace Modelec double y_global = robot_y + (x_local * std::sin(robot_theta) + y_local * std::cos(robot_theta)); // Check if in bounds - if (x_global >= 0 && x_global <= (map_width_ - margin_detection_table_) && y_global >= 0 && y_global <= (map_height_ - margin_detection_table_)) + if (x_global >= 0 && x_global <= (map_width_ - margin_detection_table_) && y_global >= 0 && y_global <= + (map_height_ - margin_detection_table_)) { - if (!is_enemy_close_) - { - modelec_interfaces::msg::OdometryPos emergency_msg; - emergency_msg.x = x_global; - emergency_msg.y = y_global; - emergency_msg.theta = 0.0; + modelec_interfaces::msg::OdometryPos emergency_msg; + emergency_msg.x = x_global; + emergency_msg.y = y_global; + emergency_msg.theta = 0.0; - close_enemy_pos_pub_->publish(emergency_msg); - RCLCPP_WARN(this->get_logger(), "EMERGENCY CLOSE OBJECT DETECTED at x=%.2f y=%.2f (%.1f mm)", x_global, y_global, range_mm); + close_enemy_pos_pub_->publish(emergency_msg); + RCLCPP_WARN(this->get_logger(), "EMERGENCY CLOSE OBJECT DETECTED at x=%.2f y=%.2f (%.1f mm)", + x_global, y_global, range_mm); - is_enemy_close_ = true; - } - - return; + // return; } - } + }*/ // Convert to local robot frame double x_local = range * std::cos(angle) * 1000.0; // meters -> mm @@ -176,7 +176,8 @@ namespace Modelec double y_global = robot_y + (x_local * std::sin(robot_theta) + y_local * std::cos(robot_theta)); // Ignore points outside of the table - if (x_global < 0 || x_global > (map_width_ - margin_detection_table_) || y_global < 0 || y_global > (map_height_ - margin_detection_table_)) + if (x_global < 0 || x_global > (map_width_ - margin_detection_table_) || y_global < 0 || y_global > ( + map_height_ - margin_detection_table_)) { // RCLCPP_INFO(this->get_logger(), "Lidar point out of bounds: x=%.2f, y=%.2f", x_global, y_global); diff --git a/src/modelec_strat/src/missions/test.go_home_mission.cpp b/src/modelec_strat/src/missions/test.go_home_mission.cpp index c3b0afd..11513ee 100644 --- a/src/modelec_strat/src/missions/test.go_home_mission.cpp +++ b/src/modelec_strat/src/missions/test.go_home_mission.cpp @@ -20,7 +20,7 @@ namespace Modelec auto curr = nav_->GetCurrentPos(); RCLCPP_INFO(node_->get_logger(), "GoHomeMission: Starting at position (%f, %f)", curr->x, curr->y); - nav_->GoTo(375, 1000, -M_PI_2, true, Pathfinding::FREE | Pathfinding::WALL | Pathfinding::OBSTACLE); + nav_->GoTo(375, 500, -M_PI_2, true, Pathfinding::FREE | Pathfinding::WALL | Pathfinding::OBSTACLE); go_home_time_ = node_->now(); @@ -29,6 +29,11 @@ namespace Modelec void GoHomeMission::Update() { + /*if (!nav_->HasArrived()) + { + return; + }*/ + switch (step_) { case GO_FRONT: diff --git a/src/modelec_strat/src/navigation_helper.cpp b/src/modelec_strat/src/navigation_helper.cpp index ff83cb4..4c61dc7 100644 --- a/src/modelec_strat/src/navigation_helper.cpp +++ b/src/modelec_strat/src/navigation_helper.cpp @@ -551,19 +551,23 @@ namespace Modelec pathfinding_->OnEnemyPosition(msg); last_enemy_pos_ = *msg; + if (Point::distance(Point(msg->x, msg->y, msg->theta), Point(current_pos_->x, current_pos_->y, current_pos_->theta)) < 600) + { + std_msgs::msg::Bool start_odo_msg; + start_odo_msg.data = false; + start_odo_pub_->publish(start_odo_msg); + + last_was_close_enemy_ = true; + + return; + } + if (last_was_close_enemy_) { - if (Point::distance(Point(msg->x, msg->y, msg->theta), Point(current_pos_->x, current_pos_->y, current_pos_->theta)) > 600) + RCLCPP_INFO(node_->get_logger(), "Enemy was close try to replanning..."); + if (Replan(false)) { - RCLCPP_INFO(node_->get_logger(), "Enemy was close try to replanning..."); - if (Replan(false)) - { - last_was_close_enemy_ = false; - } - } - else - { - RCLCPP_INFO(node_->get_logger(), "Enemy is still close, waiting for more information..."); + last_was_close_enemy_ = false; } return;