ougy.srong
/
autopilot
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

autopilot_v3

This commit is contained in:
“SRONG 2024-11-07 20:13:35 +01:00
parent 612d094d53
commit eebcaf4c92
1 changed files with 8 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
src/autopilot.cpp
View File

@ -85,8 +85,8 @@ int main(int argc, char **argv)
float angular_speed = 0.0; float angular_speed = 0.0;
// Thresholds for obstacle detection // Thresholds for obstacle detection
float front_threshold = 0.7; float front_threshold = 0.65;
float side_threshold = 0.5; float side_threshold = 0.35;
// Obstacle avoidance logic // Obstacle avoidance logic
if (min_front < front_threshold) if (min_front < front_threshold)
@ -104,7 +104,7 @@ int main(int argc, char **argv)
if (left_clearance > side_threshold && left_clearance > right_clearance) if (left_clearance > side_threshold && left_clearance > right_clearance)
{ {
// Turn left while moving forward slowly // Turn left while moving forward slowly
vel_msg.linear.x = 0.05; // Slow forward speed vel_msg.linear.x = 0.01; // Slow forward speed
angular_speed = max_angular_speed * (front_threshold - min_front) / front_threshold; angular_speed = max_angular_speed * (front_threshold - min_front) / front_threshold;
angular_speed = std::max(angular_speed, min_angular_speed); angular_speed = std::max(angular_speed, min_angular_speed);
vel_msg.angular.z = angular_speed; vel_msg.angular.z = angular_speed;
@ -113,7 +113,7 @@ int main(int argc, char **argv)
else if (right_clearance > side_threshold && right_clearance > left_clearance) else if (right_clearance > side_threshold && right_clearance > left_clearance)
{ {
// Turn right while moving forward slowly // Turn right while moving forward slowly
vel_msg.linear.x = 0.05; // Slow forward speed vel_msg.linear.x = 0.01; // Slow forward speed
angular_speed = -max_angular_speed * (front_threshold - min_front) / front_threshold; angular_speed = -max_angular_speed * (front_threshold - min_front) / front_threshold;
angular_speed = std::min(angular_speed, -min_angular_speed); angular_speed = std::min(angular_speed, -min_angular_speed);
vel_msg.angular.z = angular_speed; vel_msg.angular.z = angular_speed;
@ -122,7 +122,7 @@ int main(int argc, char **argv)
else if (left_clearance > side_threshold) else if (left_clearance > side_threshold)
{ {
// Turn left even if right is blocked // Turn left even if right is blocked
vel_msg.linear.x = 0.05; // Slow forward speed vel_msg.linear.x = 0.01; // Slow forward speed
angular_speed = max_angular_speed * (front_threshold - min_front) / front_threshold; angular_speed = max_angular_speed * (front_threshold - min_front) / front_threshold;
angular_speed = std::max(angular_speed, min_angular_speed); angular_speed = std::max(angular_speed, min_angular_speed);
vel_msg.angular.z = angular_speed; vel_msg.angular.z = angular_speed;
@ -131,7 +131,7 @@ int main(int argc, char **argv)
else if (right_clearance > side_threshold) else if (right_clearance > side_threshold)
{ {
// Turn right even if left is blocked // Turn right even if left is blocked
vel_msg.linear.x = 0.05; // Slow forward speed vel_msg.linear.x = 0.01; // Slow forward speed
angular_speed = -max_angular_speed * (front_threshold - min_front) / front_threshold; angular_speed = -max_angular_speed * (front_threshold - min_front) / front_threshold;
angular_speed = std::min(angular_speed, -min_angular_speed); angular_speed = std::min(angular_speed, -min_angular_speed);
vel_msg.angular.z = angular_speed; vel_msg.angular.z = angular_speed;
@ -140,8 +140,8 @@ int main(int argc, char **argv)
else else
{ {
// Both sides are blocked; perform escape maneuver // Both sides are blocked; perform escape maneuver
vel_msg.linear.x = -0.05; // Move backward slowly vel_msg.linear.x = -0.05;
vel_msg.angular.z = 0.5; // Rotate to try to find a clear path vel_msg.angular.z = 1.0; // Rotate to try to find a clear path
ROS_WARN("No clear path ahead; moving backward and rotating."); ROS_WARN("No clear path ahead; moving backward and rotating.");
} }
} }