modelec/modelec-marcel-ROS
1
0
Fork 0
mirror of https://github.com/modelec/modelec-marcel-ROS.git synced 2026-01-18 16:47:43 +01:00
Code Issues Packages Projects Releases Wiki Activity

some patch

Browse Source
This commit is contained in:
acki
2025-05-29 20:03:56 -04:00
parent bbe6228a34
commit 867fbf9c23
4 changed files with 44 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
simulated_pcb/odo.py
View File

@@ -56,12 +56,12 @@ class SimulatedPCB:
self.waypoint_order.pop(0) self.waypoint_order.pop(0)
del self.waypoints[wp['id']] del self.waypoints[wp['id']]
else: else:
speed = min(300.0, distance * 3) speed = min(150.0, distance)
self.vx = speed * math.cos(angle) self.vx = speed * math.cos(angle)
self.vy = speed * math.sin(angle) self.vy = speed * math.sin(angle)
self.vtheta = max(-9.0, min(9.0, angle_diff * 2)) self.vtheta = max(-9.0, min(9.0, angle_diff * 2))
else: else:
speed = min(600.0, distance * 4) speed = min(200.0, distance * 2)
self.vx = speed * math.cos(angle) self.vx = speed * math.cos(angle)
self.vy = speed * math.sin(angle) self.vy = speed * math.sin(angle)
self.vtheta = 0.0 self.vtheta = 0.0

43
src/modelec_strat/src/enemy_manager.cpp
View File

@@ -61,8 +61,8 @@ namespace Modelec
{ {
game_stated_ = true; game_stated_ = true;
} }
else if (game_stated_ && msg->state ==
if (game_stated_ && msg->state == modelec_interfaces::msg::StratState::STOP) modelec_interfaces::msg::StratState::STOP)
{ {
game_stated_ = false; game_stated_ = false;
} }
@@ -91,6 +91,7 @@ namespace Modelec
{ {
if (!game_stated_) if (!game_stated_)
{ {
RCLCPP_INFO(this->get_logger(), "Game not started, ignoring Lidar data");
return; return;
} }
@@ -112,7 +113,8 @@ namespace Modelec
double best_x = 0.0; double best_x = 0.0;
double best_y = 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); // 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) for (size_t i = 0; i < msg->ranges.size(); ++i)
@@ -127,17 +129,18 @@ namespace Modelec
continue; continue;
} }
double range_mm = range * 1000.0; // conversion en mm double range_mm = range * 1000.0; // conversion en mm
if (range_mm < robot_radius_) 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; angle += msg->angle_increment;
continue; continue;
} }
// EMERGENCY detection: obstacle very close but not the robot itself // 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 // Convert to local robot frame
double x_local = range * std::cos(angle) * 1000.0; // meters -> mm 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)); double y_global = robot_y + (x_local * std::sin(robot_theta) + y_local * std::cos(robot_theta));
// Check if in bounds // 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;
modelec_interfaces::msg::OdometryPos emergency_msg; emergency_msg.y = y_global;
emergency_msg.x = x_global; emergency_msg.theta = 0.0;
emergency_msg.y = y_global;
emergency_msg.theta = 0.0;
close_enemy_pos_pub_->publish(emergency_msg); 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); 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 // Convert to local robot frame
double x_local = range * std::cos(angle) * 1000.0; // meters -> mm 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)); double y_global = robot_y + (x_local * std::sin(robot_theta) + y_local * std::cos(robot_theta));
// Ignore points outside of the table // 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); // RCLCPP_INFO(this->get_logger(), "Lidar point out of bounds: x=%.2f, y=%.2f", x_global, y_global);

7
src/modelec_strat/src/missions/test.go_home_mission.cpp
View File

@@ -20,7 +20,7 @@ namespace Modelec
auto curr = nav_->GetCurrentPos(); auto curr = nav_->GetCurrentPos();
RCLCPP_INFO(node_->get_logger(), "GoHomeMission: Starting at position (%f, %f)", curr->x, curr->y); 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(); go_home_time_ = node_->now();
@@ -29,6 +29,11 @@ namespace Modelec
void GoHomeMission::Update() void GoHomeMission::Update()
{ {
/*if (!nav_->HasArrived())
{
return;
}*/
switch (step_) switch (step_)
{ {
case GO_FRONT: case GO_FRONT:

24
src/modelec_strat/src/navigation_helper.cpp
View File

@@ -551,19 +551,23 @@ namespace Modelec
pathfinding_->OnEnemyPosition(msg); pathfinding_->OnEnemyPosition(msg);
last_enemy_pos_ = *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 (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..."); last_was_close_enemy_ = false;
if (Replan(false))
{
last_was_close_enemy_ = false;
}
}
else
{
RCLCPP_INFO(node_->get_logger(), "Enemy is still close, waiting for more information...");
} }
return; return;