From 612d094d536475bc37c833ffa1da429efa211c64 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E2=80=9CSRONG?= <“ougy.srong@ecam.fr”> Date: Thu, 7 Nov 2024 19:22:16 +0100 Subject: [PATCH] autopilot_v2 --- .vscode/settings.json | 13 ++- CMakeLists.txt | 1 + src/autopilot.cpp | 184 ++++++++++++++++++++++++++---------------- 3 files changed, 128 insertions(+), 70 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index d98cab5..e2a5300 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -6,5 +6,16 @@ "python.analysis.extraPaths": [ "/home/ougy/catkin_ws/devel/lib/python3/dist-packages", "/opt/ros/noetic/lib/python3/dist-packages" - ] + ], + "files.associations": { + "*.ipp": "cpp", + "array": "cpp", + "bitset": "cpp", + "string_view": "cpp", + "hash_map": "cpp", + "hash_set": "cpp", + "initializer_list": "cpp", + "regex": "cpp", + "utility": "cpp" + } } \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index 9db5d33..a09fc79 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -12,6 +12,7 @@ find_package(catkin REQUIRED COMPONENTS rospy std_msgs geometry_msgs + sensor_msgs ) ## System dependencies are found with CMake's conventions diff --git a/src/autopilot.cpp b/src/autopilot.cpp index c0e7a8a..240d452 100644 --- a/src/autopilot.cpp +++ b/src/autopilot.cpp @@ -5,67 +5,67 @@ #include #include -// Global variables to share data between the callback and main loop -float min_range = std::numeric_limits::infinity(); -std::string obstacle_direction = "none"; +// Global variables for min ranges in sectors +float min_front = std::numeric_limits::infinity(); +float min_front_left = std::numeric_limits::infinity(); +float min_front_right = std::numeric_limits::infinity(); +float min_left = std::numeric_limits::infinity(); +float min_right = std::numeric_limits::infinity(); void scanCallback(const sensor_msgs::LaserScan::ConstPtr& msg) { - min_range = std::numeric_limits::infinity(); - int min_index = -1; + // Reset the min ranges + min_front = min_front_left = min_front_right = min_left = min_right = std::numeric_limits::infinity(); - // Find the minimum valid range for (size_t i = 0; i < msg->ranges.size(); ++i) { float range = msg->ranges[i]; - if (std::isfinite(range) && range < min_range) - { - min_range = range; - min_index = i; - } - } + if (!std::isfinite(range)) + continue; - if (min_index >= 0) - { - // Calculate the angle corresponding to the minimum range - float angle = msg->angle_min + min_index * msg->angle_increment; + float angle = msg->angle_min + i * msg->angle_increment; - // Normalize angle to be between -π and π + // Normalize angle to be between -PI and PI if (angle > M_PI) angle -= 2 * M_PI; else if (angle < -M_PI) angle += 2 * M_PI; - // Determine the direction - if (angle >= -M_PI / 4 && angle <= M_PI / 4) + // Front sector (-30 to +30 degrees) + if (angle > -M_PI / 6 && angle < M_PI / 6) { - obstacle_direction = "front"; + if (range < min_front) + min_front = range; } - else if (angle > M_PI / 4 && angle <= 3 * M_PI / 4) + // Front-left sector (+30 to +90 degrees) + else if (angle >= M_PI / 6 && angle < M_PI / 2) { - obstacle_direction = "left"; + if (range < min_front_left) + min_front_left = range; } - else if ((angle > 3 * M_PI / 4 && angle <= M_PI) || (angle >= -M_PI && angle < -3 * M_PI / 4)) + // Left sector (+90 to +150 degrees) + else if (angle >= M_PI / 2 && angle < 5 * M_PI / 6) { - obstacle_direction = "back"; + if (range < min_left) + min_left = range; } - else if (angle >= -3 * M_PI / 4 && angle < -M_PI / 4) + // Front-right sector (-90 to -30 degrees) + else if (angle > -M_PI / 2 && angle <= -M_PI / 6) { - obstacle_direction = "right"; + if (range < min_front_right) + min_front_right = range; } - else + // Right sector (-150 to -90 degrees) + else if (angle > -5 * M_PI / 6 && angle <= -M_PI / 2) { - obstacle_direction = "unknown"; + if (range < min_right) + min_right = range; } + } - // Print the obstacle information - ROS_INFO("Obstacle detected in %s at distance: %f meters", obstacle_direction.c_str(), min_range); - } - else - { - obstacle_direction = "none"; - ROS_WARN("No valid range measurements received."); - } + // For debugging purposes + ROS_INFO("Ranges - Front: %f, Front-Left: %f, Front-Right: %f, Left: %f, Right: %f", + min_front, min_front_left, min_front_right, min_left, min_right); } int main(int argc, char **argv) @@ -81,55 +81,101 @@ int main(int argc, char **argv) while (ros::ok()) { geometry_msgs::Twist vel_msg; + float linear_speed = 0.1; + float angular_speed = 0.0; + + // Thresholds for obstacle detection + float front_threshold = 0.7; + float side_threshold = 0.5; // Obstacle avoidance logic - if (obstacle_direction == "front" && min_range < 0.7) + if (min_front < front_threshold) { - // Obstacle ahead; stop and decide to turn left or right based on side obstacles - vel_msg.linear.x = 0.0; + // Obstacle ahead; decide which way to turn + // Calculate overall clearances + float left_clearance = std::min(min_front_left, min_left); + float right_clearance = std::min(min_front_right, min_right); - // Decide which way to turn based on side obstacles - if (obstacle_direction == "left") + // Define new maximum and minimum angular speeds + float max_angular_speed = 1.0; // Maximum angular speed (adjust as needed) + float min_angular_speed = 0.5; // Minimum angular speed for turning + + + if (left_clearance > side_threshold && left_clearance > right_clearance) { - // If obstacle on the left, turn right - vel_msg.angular.z = -0.5; - ROS_INFO("Turning right to avoid obstacle in front-left."); + // Turn left while moving forward slowly + vel_msg.linear.x = 0.05; // 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; + ROS_INFO("Turning left to avoid obstacle ahead."); } - else if (obstacle_direction == "right") + else if (right_clearance > side_threshold && right_clearance > left_clearance) { - // If obstacle on the right, turn left - vel_msg.angular.z = 0.5; - ROS_INFO("Turning left to avoid obstacle in front-right."); + // Turn right while moving forward slowly + vel_msg.linear.x = 0.05; // 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; + ROS_INFO("Turning right to avoid obstacle ahead."); + } + else if (left_clearance > side_threshold) + { + // Turn left even if right is blocked + vel_msg.linear.x = 0.05; // 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; + ROS_INFO("Turning left (right blocked) to avoid obstacle ahead."); + } + else if (right_clearance > side_threshold) + { + // Turn right even if left is blocked + vel_msg.linear.x = 0.05; // 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; + ROS_INFO("Turning right (left blocked) to avoid obstacle ahead."); } else { - // If obstacle directly ahead, turn in a random direction - vel_msg.angular.z = 0.5; // Turn left by default - ROS_INFO("Turning left to avoid obstacle directly ahead."); + // 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 + ROS_WARN("No clear path ahead; moving backward and rotating."); } } - else if (obstacle_direction == "left" && min_range < 0.5) - { - // Obstacle on the left; veer right - vel_msg.linear.x = 0.1; - vel_msg.angular.z = -0.3; - ROS_INFO("Veering right to avoid obstacle on the left."); - } - else if (obstacle_direction == "right" && min_range < 0.5) - { - // Obstacle on the right; veer left - vel_msg.linear.x = 0.1; - vel_msg.angular.z = 0.3; - ROS_INFO("Veering left to avoid obstacle on the right."); - } else { - // Path is clear; move forward - vel_msg.linear.x = 0.1; - vel_msg.angular.z = 0.0; - ROS_INFO("Moving forward."); + // Path is clear ahead; move forward + vel_msg.linear.x = linear_speed; + + // Adjust direction based on side obstacles + if (min_left < side_threshold) + { + // Obstacle on the left; veer right + angular_speed = -0.3 * (side_threshold - min_left) / side_threshold; + angular_speed = std::min(angular_speed, -0.1f); + vel_msg.angular.z = angular_speed; + ROS_INFO("Veering right to avoid obstacle on the left."); + } + else if (min_right < side_threshold) + { + // Obstacle on the right; veer left + angular_speed = 0.3 * (side_threshold - min_right) / side_threshold; + angular_speed = std::max(angular_speed, 0.1f); + vel_msg.angular.z = angular_speed; + ROS_INFO("Veering left to avoid obstacle on the right."); + } + else + { + // Path is clear, proceed straight + vel_msg.angular.z = 0.0; + ROS_INFO("Moving forward."); + } } + cmd_vel_pub.publish(vel_msg); ros::spinOnce();