Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Alternative implementation of loop transitions #11

Merged
merged 3 commits into from
Jul 30, 2024
Merged

Alternative implementation of loop transitions #11

Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
b6091f3
refactor: move repeated code to function
rovo89 Jun 7, 2024
dfdf89f
Alternative implementation of loop transitions
rovo89 Jun 7, 2024
91bcaf6
Fix orientation of the last point in each loop
rovo89 Jul 29, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
302 changes: 105 additions & 197 deletions src/coverage_planner.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -211,6 +211,108 @@ void traverse_from_right(std::vector<PerimeterGeneratorLoop> &contours, std::vec
}
}

slic3r_coverage_planner::Path determinePathForOutline(std_msgs::Header &header, Slic3r::Polygon &outline_poly, Slic3r::Polygons &group, bool isObstacle, Point *areaLastPoint) {
slic3r_coverage_planner::Path path;
path.is_outline = true;
path.path.header = header;

Point lastPoint;
bool is_first_point = true;
for (int i = 0; i < group.size(); i++) {
auto points = group[i].equally_spaced_points(scale_(0.1));
if (points.size() < 2) {
ROS_INFO("Skipping single dot");
continue;
}
ROS_INFO_STREAM("Got " << points.size() << " points");

if (!is_first_point) {
// Find a good transition point between the loops.
// It should be close to the last split point, so that we don't need to traverse a lot.

// Find the point in the current poly which is closest to the last point of the last group
// (which is the next inner poly from this point of view).
const auto last_x = unscale(lastPoint.x);
const auto last_y = unscale(lastPoint.y);
double min_distance = INFINITY;
int closest_idx = 0;
for (int idx = 0; idx < points.size(); ++idx) {
const auto &pt = points[idx];
const auto pt_x = unscale(pt.x);
const auto pt_y = unscale(pt.y);
double distance = sqrt((pt_x - last_x) * (pt_x - last_x) + (pt_y - last_y) * (pt_y - last_y));
if (distance < min_distance) {
min_distance = distance;
closest_idx = idx;
}
}

// In order to smooth the transition we skip some points (think spiral movement of the mower).
// Check, that the skip did not break the path (cross the outer poly during transition).
// If it's fine, use the smoothed path, otherwise use the shortest point to split.
int smooth_transition_idx = (closest_idx + 3) % points.size();

const Polygon *next_outer_poly;
if (i < group.size() - 1) {
next_outer_poly = &group[i + 1];
} else {
// we are in the outermost line, use outline for collision check
next_outer_poly = &outline_poly;
}
Line connection(points[smooth_transition_idx], lastPoint);
Point intersection_pt{};
if (next_outer_poly->intersection(connection, &intersection_pt)) {
// intersection, we need to transition at closest point
smooth_transition_idx = closest_idx;
}

if (smooth_transition_idx > 0) {
std::rotate(points.begin(), points.begin() + smooth_transition_idx, points.end());
}
}

for (auto &pt: points) {
if (is_first_point) {
lastPoint = pt;
is_first_point = false;
continue;
}

// calculate pose for "lastPoint" pointing to current point

// Direction for obstacle needs to be inversed compared to area outline, because we will reverse the point order later.
auto dir = isObstacle ? lastPoint - pt : pt - lastPoint;

double orientation = atan2(dir.y, dir.x);
tf2::Quaternion q(0.0, 0.0, orientation);

geometry_msgs::PoseStamped pose;
pose.header = header;
pose.pose.orientation = tf2::toMsg(q);
pose.pose.position.x = unscale(lastPoint.x);
pose.pose.position.y = unscale(lastPoint.y);
pose.pose.position.z = 0;
path.path.poses.push_back(pose);
lastPoint = pt;
}
}

// finally, we add the final pose for "lastPoint" with the same orientation as the last pose
geometry_msgs::PoseStamped pose;
pose.header = header;
pose.pose.orientation = path.path.poses.back().pose.orientation;
pose.pose.position.x = unscale(lastPoint.x);
pose.pose.position.y = unscale(lastPoint.y);
pose.pose.position.z = 0;
path.path.poses.push_back(pose);

if (areaLastPoint != nullptr) {
*areaLastPoint = lastPoint;
}

return path;
}

bool planPath(slic3r_coverage_planner::PlanPathRequest &req, slic3r_coverage_planner::PlanPathResponse &res) {

Slic3r::Polygon outline_poly;
Expand Down Expand Up @@ -415,105 +517,7 @@ bool planPath(slic3r_coverage_planner::PlanPathRequest &req, slic3r_coverage_pla

Point areaLastPoint;
for (auto &group: area_outlines) {
slic3r_coverage_planner::Path path;
path.is_outline = true;
path.path.header = header;

for (int i = 0; i < group.size(); i++) {
auto &poly = group[i];


// Find an appropriate point to split, this should be close to the last split point,
// so that we don't need to traverse a lot.
Polyline line;
// Cannot use i==0 here, because we might skip a path later ("Skipping single dot")
if (path.path.poses.empty()) {
// innermost group, split wherever
line = poly.split_at_first_point();
} else {
// Get last point of last group. this is the next inner poly from this point of view
const auto &last_pose = path.path.poses.back();
// Find the closest point in the current poly and split there
double min_distance = INFINITY;
int min_split_index = 0;
for (int split_index = 0; split_index < poly.points.size(); ++split_index) {
const auto &pt = poly.points[split_index];
const auto pt_x = unscale(pt.x);
const auto pt_y = unscale(pt.y);
double distance = sqrt((pt_x - last_pose.pose.position.x) * (pt_x - last_pose.pose.position.x) +
(pt_y - last_pose.pose.position.y) * (pt_y - last_pose.pose.position.y));

if (distance < min_distance) {
min_distance = distance;
min_split_index = split_index;
}
}

// In order to smooth the transition we skip some points (think spiral movement of the mower).
// Check, that the skip did not break the path (cross the outer poly during transition).
// If it's fine, use the smoothed path, otherwise use the shortest point to split.
int smooth_split_index = (min_split_index + 2) % poly.points.size();

line = poly.split_at_index(smooth_split_index);
const Polygon *next_outer_poly;
if (i < group.size() - 1) {
next_outer_poly = &group[i + 1];
} else {
// we are in the outermost line, use outline for collision check
next_outer_poly = &outline_poly;
}
Line connection(line.first_point(),
Point(scale_(last_pose.pose.position.x), scale_(last_pose.pose.position.y)));
Point intersection_pt{};
if (next_outer_poly->intersection(connection, &intersection_pt)) {
// intersection, we need to split at closest point
line = poly.split_at_index(min_split_index);
}
}
line.remove_duplicate_points();


auto equally_spaced_points = line.equally_spaced_points(scale_(0.1));
if (equally_spaced_points.size() < 2) {
ROS_INFO("Skipping single dot");
continue;
}
ROS_INFO_STREAM("Got " << equally_spaced_points.size() << " points");

Point *lastPoint = nullptr;
for (auto &pt: equally_spaced_points) {
if (lastPoint == nullptr) {
lastPoint = &pt;
continue;
}

// calculate pose for "lastPoint" pointing to current point

auto dir = pt - *lastPoint;
double orientation = atan2(dir.y, dir.x);
tf2::Quaternion q(0.0, 0.0, orientation);

geometry_msgs::PoseStamped pose;
pose.header = header;
pose.pose.orientation = tf2::toMsg(q);
pose.pose.position.x = unscale(lastPoint->x);
pose.pose.position.y = unscale(lastPoint->y);
pose.pose.position.z = 0;
path.path.poses.push_back(pose);
lastPoint = &pt;
}

// finally, we add the final pose for "lastPoint" with the same orientation as the last poe
geometry_msgs::PoseStamped pose;
pose.header = header;
pose.pose.orientation = path.path.poses.back().pose.orientation;
pose.pose.position.x = unscale(lastPoint->x);
pose.pose.position.y = unscale(lastPoint->y);
pose.pose.position.z = 0;
path.path.poses.push_back(pose);

areaLastPoint = *lastPoint;
}
auto path = determinePathForOutline(header, outline_poly, group, false, &areaLastPoint);
res.paths.push_back(path);
}

Expand Down Expand Up @@ -554,104 +558,8 @@ bool planPath(slic3r_coverage_planner::PlanPathRequest &req, slic3r_coverage_pla
// this is intentional, because then it's easier to find good traversal points.
// In order to make the mower approach the obstacle, we will reverse the path later.
for (auto &group: ordered_obstacle_outlines) {
slic3r_coverage_planner::Path path;
path.is_outline = true;
path.path.header = header;
for (int i = 0; i < group.size(); i++) {
auto &poly = group[i];


// Find an appropriate point to split, this should be close to the last split point,
// so that we don't need to traverse a lot.
Polyline line;
if (i == 0) {
// innermost group, split wherever
line = poly.split_at_first_point();
} else {
// Get last point of last group. this is the next inner poly from this point of view
const auto &last_pose = path.path.poses.back();
// Find the closest point in the current poly and split there
double min_distance = INFINITY;
int min_split_index = 0;
for (int split_index = 0; split_index < poly.points.size(); ++split_index) {
const auto &pt = poly.points[split_index];
const auto pt_x = unscale(pt.x);
const auto pt_y = unscale(pt.y);
double distance = sqrt((pt_x - last_pose.pose.position.x) * (pt_x - last_pose.pose.position.x) +
(pt_y - last_pose.pose.position.y) * (pt_y - last_pose.pose.position.y));

if (distance < min_distance) {
min_distance = distance;
min_split_index = split_index;
}
}

// In order to smooth the transition we skip some points (think spiral movement of the mower).
// Check, that the skip did not break the path (cross the outer poly during transition).
// If it's fine, use the smoothed path, otherwise use the shortest point to split.
int smooth_split_index = (min_split_index + 2) % poly.points.size();

line = poly.split_at_index(smooth_split_index);
const Polygon *next_outer_poly;
if (i < group.size() - 1) {
next_outer_poly = &group[i + 1];
} else {
// we are in the outermost line, use outline for collision check
next_outer_poly = &outline_poly;
}
Line connection(line.first_point(),
Point(scale_(last_pose.pose.position.x), scale_(last_pose.pose.position.y)));
Point intersection_pt{};
if (next_outer_poly->intersection(connection, &intersection_pt)) {
// intersection, we need to split at closest point
line = poly.split_at_index(min_split_index);
}
}
line.remove_duplicate_points();


auto equally_spaced_points = line.equally_spaced_points(scale_(0.1));
if (equally_spaced_points.size() < 2) {
ROS_INFO("Skipping single dot");
continue;
}
ROS_INFO_STREAM("Got " << equally_spaced_points.size() << " points");

Point *lastPoint = nullptr;
for (auto &pt: equally_spaced_points) {
if (lastPoint == nullptr) {
lastPoint = &pt;
continue;
}

// calculate pose for "lastPoint" pointing to current point

// Direction needs to be inversed compared to outline, because we will reverse the point order later.
auto dir = *lastPoint - pt;
double orientation = atan2(dir.y, dir.x);
tf2::Quaternion q(0.0, 0.0, orientation);

geometry_msgs::PoseStamped pose;
pose.header = header;
pose.pose.orientation = tf2::toMsg(q);
pose.pose.position.x = unscale(lastPoint->x);
pose.pose.position.y = unscale(lastPoint->y);
pose.pose.position.z = 0;
path.path.poses.push_back(pose);
lastPoint = &pt;
}

// finally, we add the final pose for "lastPoint" with the same orientation as the last poe
geometry_msgs::PoseStamped pose;
pose.header = header;
pose.pose.orientation = path.path.poses.back().pose.orientation;
pose.pose.position.x = unscale(lastPoint->x);
pose.pose.position.y = unscale(lastPoint->y);
pose.pose.position.z = 0;
path.path.poses.push_back(pose);

}
// Reverse here to make the mower approach the obstacle instead of starting close to the obstacle
auto path = determinePathForOutline(header, outline_poly, group, true, nullptr);
std::reverse(path.path.poses.begin(), path.path.poses.end());
res.paths.push_back(path);
}
Expand Down Expand Up @@ -695,7 +603,7 @@ bool planPath(slic3r_coverage_planner::PlanPathRequest &req, slic3r_coverage_pla
lastPoint = &pt;
}

// finally, we add the final pose for "lastPoint" with the same orientation as the last poe
// finally, we add the final pose for "lastPoint" with the same orientation as the last pose
geometry_msgs::PoseStamped pose;
pose.header = header;
pose.pose.orientation = path.path.poses.back().pose.orientation;
Expand Down