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

autopilot_v2

This commit is contained in:
“SRONG 2024-11-07 19:22:16 +01:00
parent 0a09811d85
commit 612d094d53
3 changed files with 128 additions and 70 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
.vscode/settings.json vendored
View File

@ -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"
}
}

1
CMakeLists.txt
View File

@ -12,6 +12,7 @@ find_package(catkin REQUIRED COMPONENTS
rospy
std_msgs
geometry_msgs
sensor_msgs
)
## System dependencies are found with CMake's conventions

184
src/autopilot.cpp
View File

@ -5,67 +5,67 @@
#include <limits>
#include <cmath>
// Global variables to share data between the callback and main loop
float min_range = std::numeric_limits<float>::infinity();
std::string obstacle_direction = "none";
// Global variables for min ranges in sectors
float min_front = std::numeric_limits<float>::infinity();
float min_front_left = std::numeric_limits<float>::infinity();
float min_front_right = std::numeric_limits<float>::infinity();
float min_left = std::numeric_limits<float>::infinity();
float min_right = std::numeric_limits<float>::infinity();
void scanCallback(const sensor_msgs::LaserScan::ConstPtr& msg)
{
min_range = std::numeric_limits<float>::infinity();
int min_index = -1;
// Reset the min ranges
min_front = min_front_left = min_front_right = min_left = min_right = std::numeric_limits<float>::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();