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)
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

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

@@ -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);

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

@@ -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:

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

@@ -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;