Merge branch 'ClemensElflein:main' into main

ene9ba · Apr 25, 2024 · b8a0897 · b8a0897
2 parents 1632874 + 06e7f05
commit b8a0897
Show file tree

Hide file tree

Showing 3 changed files with 28 additions and 9 deletions.
diff --git a/src/lib/slic3r_coverage_planner b/src/lib/slic3r_coverage_planner
diff --git a/src/mower_utils/launch/planner_test.launch b/src/mower_utils/launch/planner_test.launch
@@ -4,7 +4,17 @@
 <launch>
     <node name="rviz" pkg="rviz" type="rviz" args="-d $(find mower_utils)/rviz/planner_test.rviz" required="true" />
     <node pkg="mower_map" type="mower_map_service" name="mower_map" required="true" />
-    <node pkg="slic3r_coverage_planner" type="slic3r_coverage_planner" name="slic3r_coverage_planner" required="true" />
-    <node pkg="mower_utils" type="planner_test" name="planner_test" required="false" />
+    <node pkg="slic3r_coverage_planner" type="slic3r_coverage_planner" name="slic3r_coverage_planner" required="false" />
+
+    <node
+            if="$(eval optenv('START_PLANNER', True))"
+            pkg="mower_utils"
+            type="planner_test"
+            name="planner_test"
+            required="false"
+    >
+        <param name="area_index" value="$(eval optenv('AREA_INDEX', 0))"/>
+        <param name="outline_count" value="$(eval optenv('OM_OUTLINE_COUNT', 10))"/>
+    </node>
 <!--    <node pkg="mower_logic" type="mower_logic" name="mower_logic" required="true" />-->
 </launch>
diff --git a/src/mower_utils/src/planner_test.cpp b/src/mower_utils/src/planner_test.cpp
@@ -14,6 +14,8 @@ int main(int argc, char **argv) {
     ros::NodeHandle n;
     ros::NodeHandle paramNh("~");
 
+    int area_index = paramNh.param("area_index", 0);
+    int outline_count = paramNh.param("outline_count", 4);
 
     ros::Publisher path_pub;
 
@@ -33,7 +35,7 @@ int main(int argc, char **argv) {
 
 
     mower_map::GetMowingAreaSrv mapSrv;
-    mapSrv.request.index = 0;
+    mapSrv.request.index = area_index;
 
     if (!mapClient.call(mapSrv)) {
         ROS_ERROR_STREAM("Error loading mowing area");
@@ -42,16 +44,23 @@ int main(int argc, char **argv) {
 
     slic3r_coverage_planner::PlanPath pathSrv;
     pathSrv.request.angle = 0;
-    pathSrv.request.outline_count = 1;
+    pathSrv.request.outline_count = outline_count;
     pathSrv.request.outline = mapSrv.response.area.area;
     pathSrv.request.holes = mapSrv.response.area.obstacles;
     pathSrv.request.fill_type = slic3r_coverage_planner::PlanPathRequest::FILL_LINEAR;
-    pathSrv.request.distance = 1.0;
+    pathSrv.request.distance = 0.13;
+    pathSrv.request.outer_offset = 0.05;
 
-    if (!pathClient.call(pathSrv)) {
-        ROS_ERROR_STREAM("Error getting path area");
-        return 1;
+    ros::Duration loop_time(1);
+    while(ros::ok()) {
+        if (!pathClient.call(pathSrv)) {
+            ROS_ERROR_STREAM("Error getting path area");
+        } else {
+            ROS_INFO_STREAM("Got path");
+        }
+        loop_time.sleep();
     }
+
     return 0;
 }