From b6091f33d825d5821a103858f15bae4d7b613a23 Mon Sep 17 00:00:00 2001 From: Robert Vollmer Date: Fri, 7 Jun 2024 22:36:52 +0200 Subject: [PATCH 1/3] refactor: move repeated code to function --- src/coverage_planner.cpp | 304 ++++++++++++++------------------------- 1 file changed, 108 insertions(+), 196 deletions(-) diff --git a/src/coverage_planner.cpp b/src/coverage_planner.cpp index ed81b62..d99ed31 100644 --- a/src/coverage_planner.cpp +++ b/src/coverage_planner.cpp @@ -211,6 +211,112 @@ void traverse_from_right(std::vector &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; + + 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 + + // 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 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); + + if (areaLastPoint != nullptr) { + *areaLastPoint = *lastPoint; + } + } + return path; +} + bool planPath(slic3r_coverage_planner::PlanPathRequest &req, slic3r_coverage_planner::PlanPathResponse &res) { Slic3r::Polygon outline_poly; @@ -415,105 +521,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); } @@ -554,104 +562,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); } From dfdf89f4ca72fa163f4f00e5929bfd8bc4152702 Mon Sep 17 00:00:00 2001 From: Robert Vollmer Date: Fri, 7 Jun 2024 23:21:52 +0200 Subject: [PATCH 2/3] Alternative implementation of loop transitions Instead of finding a transition in the recorded polygon's points, look at the equally spaced points created from the polygon. This fixes issues with sparse polygons (caused by map editing or with automatic point recording disabled). For dense polygons, which anyway have one point for every 10 cm, the result should be very similar as before. --- src/coverage_planner.cpp | 51 +++++++++++++++++++--------------------- 1 file changed, 24 insertions(+), 27 deletions(-) diff --git a/src/coverage_planner.cpp b/src/coverage_planner.cpp index d99ed31..a8f516a 100644 --- a/src/coverage_planner.cpp +++ b/src/coverage_planner.cpp @@ -217,23 +217,26 @@ slic3r_coverage_planner::Path determinePathForOutline(std_msgs::Header &header, path.path.header = header; for (int i = 0; i < group.size(); i++) { - auto &poly = group[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"); - // 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 { + if (!path.path.poses.empty()) { + // 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. + // 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 + + // Find the closest point in the current poly. 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]; + 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_pose.pose.position.x) * (pt_x - last_pose.pose.position.x) + @@ -241,16 +244,15 @@ slic3r_coverage_planner::Path determinePathForOutline(std_msgs::Header &header, if (distance < min_distance) { min_distance = distance; - min_split_index = split_index; + 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_split_index = (min_split_index + 2) % poly.points.size(); + int smooth_transition_idx = (closest_idx + 3) % 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]; @@ -258,26 +260,21 @@ slic3r_coverage_planner::Path determinePathForOutline(std_msgs::Header &header, // we are in the outermost line, use outline for collision check next_outer_poly = &outline_poly; } - Line connection(line.first_point(), + Line connection(points[smooth_transition_idx], 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); + // intersection, we need to transition at closest point + smooth_transition_idx = closest_idx; } - } - 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; + if (smooth_transition_idx > 0) { + std::rotate(points.begin(), points.begin() + smooth_transition_idx, points.end()); + } } - ROS_INFO_STREAM("Got " << equally_spaced_points.size() << " points"); Point *lastPoint = nullptr; - for (auto &pt: equally_spaced_points) { + for (auto &pt: points) { if (lastPoint == nullptr) { lastPoint = &pt; continue; From 91bcaf6b1412bb4a29c355d2949a22570edb9633 Mon Sep 17 00:00:00 2001 From: Robert Vollmer Date: Mon, 29 Jul 2024 23:02:59 +0200 Subject: [PATCH 3/3] Fix orientation of the last point in each loop --- src/coverage_planner.cpp | 61 ++++++++++++++++++++-------------------- 1 file changed, 30 insertions(+), 31 deletions(-) diff --git a/src/coverage_planner.cpp b/src/coverage_planner.cpp index a8f516a..e466885 100644 --- a/src/coverage_planner.cpp +++ b/src/coverage_planner.cpp @@ -216,6 +216,8 @@ slic3r_coverage_planner::Path determinePathForOutline(std_msgs::Header &header, 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) { @@ -224,24 +226,21 @@ slic3r_coverage_planner::Path determinePathForOutline(std_msgs::Header &header, } ROS_INFO_STREAM("Got " << points.size() << " points"); - // Cannot use i==0 here, because we might skip a path later ("Skipping single dot") - if (!path.path.poses.empty()) { + 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. - // 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. + // 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_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)); - + 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; @@ -260,8 +259,7 @@ slic3r_coverage_planner::Path determinePathForOutline(std_msgs::Header &header, // we are in the outermost line, use outline for collision check next_outer_poly = &outline_poly; } - Line connection(points[smooth_transition_idx], - Point(scale_(last_pose.pose.position.x), scale_(last_pose.pose.position.y))); + 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 @@ -273,17 +271,17 @@ slic3r_coverage_planner::Path determinePathForOutline(std_msgs::Header &header, } } - Point *lastPoint = nullptr; for (auto &pt: points) { - if (lastPoint == nullptr) { - lastPoint = &pt; + 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; + auto dir = isObstacle ? lastPoint - pt : pt - lastPoint; double orientation = atan2(dir.y, dir.x); tf2::Quaternion q(0.0, 0.0, orientation); @@ -291,26 +289,27 @@ slic3r_coverage_planner::Path determinePathForOutline(std_msgs::Header &header, 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.x = unscale(lastPoint.x); + pose.pose.position.y = unscale(lastPoint.y); pose.pose.position.z = 0; path.path.poses.push_back(pose); - lastPoint = &pt; + 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); - - if (areaLastPoint != nullptr) { - *areaLastPoint = *lastPoint; - } + // 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; } @@ -604,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;