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