adding untested BT nodes and navigator plugin for coverage navigation complete

Author: SteveMacenski

nav2_complete_coverage_msgs/CMakeLists.txt

@@ -19,6 +19,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/Swath.msg"
   "msg/PathComponents.msg"
   "action/ComputeCoveragePath.action"
+  "action/NavigateCompleteCoverage.action"
   DEPENDENCIES builtin_interfaces geometry_msgs std_msgs action_msgs nav_msgs
 )
 

nav2_complete_coverage_msgs/action/ComputeCoveragePath.action

@@ -9,7 +9,6 @@ bool generate_path True
 # If providing gml file with multiple fields, specify which with gml_field_id
 # If using polygons, bounding polygon must be first, followed by inner cutouts
 # Both must specify if the data is cartesian or GPS coordinates
-bool frame_cartesian True
 bool use_gml_file False
 string gml_field
 uint16 gml_field_id 0

nav2_complete_coverage_msgs/action/NavigateCompleteCoverage.action

@@ -0,0 +1,26 @@
+#goal definition
+
+# Define the field as either a filepath to a GML file or as a vector of polygons
+# Whereas the first polygon is the outer field and subsequent polygons are internal voids.
+# When both are specified, the file is used in ComputeCoveragePath BT Node.
+string field_filepath
+geometry_msgs/Polygon[] polygons
+string frame_id map # Specify the frame of reference of the polygon field
+
+string behavior_tree
+---
+#result definition
+
+# Error codes
+# Note: The expected priority order of the errors should match the message order
+uint16 NONE=0
+
+uint16 error_code
+---
+#feedback definition
+
+geometry_msgs/PoseStamped current_pose
+builtin_interfaces/Duration navigation_time
+builtin_interfaces/Duration estimated_time_remaining
+int16 number_of_recoveries
+float32 distance_remaining

nav2_coverage/README.md

@@ -56,6 +56,7 @@ Walk through
   - Use GPS, Cartesian; files or direct coordinates
   - Error codes for contextual failures to know when failures what to do about it
   - Return: PathComponents, NavPath, error code, compute time for metrics
+  - BT nodes + XML using the nav path from coverage + Navigator type for semantic information of request + demo sim use
 
   - Tester to demo
     - Basic call
@@ -64,10 +65,13 @@ Walk through
     - RQT
 
 Future
-  - Use coverage exceptions
+  - Test BT nodes / XML / Navigator // unit tests
+  - Create simulation to testing / demo
+
+  - A couple of utilities for the BT nodes to iterate through the swath-turn combos (optional)
+
+
+
+Navigators require Iron+
+Options: (A) upgrade, (B) use Navigate To Pose and hardcode the field file path and ignore the pose action request, (C) call the server manually from your application
 
-  - BT nodes to interact with in in the Nav2 autonomy framework
-  - A couple of utilities for the BT nodes to iterate through the swath-turn combos
-  - Turn tester into Python API for it
-  - A Navigator API plugin for coverage specific tasks
-  - A BT XML to pair with the navigator + BT nodes to initial system demos

nav2_coverage/include/nav2_coverage/coverage_server.hpp

@@ -128,6 +128,7 @@ class CoverageServer : public nav2_util::LifecycleNode
   std::unique_ptr<RouteGenerator> route_gen_;
   std::unique_ptr<PathGenerator> path_gen_;
   std::unique_ptr<Visualizer> visualizer_;
+  bool cartesian_frame_;
 };
 
 }  // namespace nav2_coverage

nav2_coverage/src/coverage_server.cpp

@@ -40,6 +40,12 @@ CoverageServer::on_configure(const rclcpp_lifecycle::State & /*state*/)
   path_gen_ = std::make_unique<PathGenerator>(node, robot_params_.get());
   visualizer_ = std::make_unique<Visualizer>();
 
+  // If in GPS coordinates, we must convert to a CRS to compute coverage
+  // Then, reconvert back to GPS for the user.
+  nav2_util::declare_parameter_if_not_declared(
+    node, "coordinates_in_cartesian_frame", rclcpp::ParameterValue(true));
+  get_parameter("coordinates_in_cartesian_frame", cartesian_frame_);
+
   double action_server_result_timeout;
   nav2_util::declare_parameter_if_not_declared(
     node, "action_server_result_timeout", rclcpp::ParameterValue(10.0));
@@ -158,7 +164,6 @@ void CoverageServer::computeCoveragePath()
 
   try {
     // (0) Obtain field to use
-    bool cartesian_frame = goal->frame_cartesian;
     Field field;
     F2CField master_field;
     std::string frame_id;
@@ -173,7 +178,7 @@ void CoverageServer::computeCoveragePath()
       frame_id = goal->frame_id;
     }
 
-    if (!cartesian_frame) {
+    if (!cartesian_frame_) {
       f2c::Transform::transformToUTM(master_field);
     }
     field = master_field.field.getGeometry(0);
@@ -204,15 +209,15 @@ void CoverageServer::computeCoveragePath()
       if (goal->generate_path) {
         Path path = path_gen_->generatePath(route, goal->path_mode);
         result->coverage_path =
-          util::toCoveragePathMsg(path, master_field, header, cartesian_frame);
-        result->nav_path = util::toNavPathMsg(path, master_field, header, cartesian_frame);
+          util::toCoveragePathMsg(path, master_field, header, cartesian_frame_);
+        result->nav_path = util::toNavPathMsg(path, master_field, header, cartesian_frame_);
       } else {
         result->coverage_path =
-          util::toCoveragePathMsg(route, master_field, true, header, cartesian_frame);
+          util::toCoveragePathMsg(route, master_field, true, header, cartesian_frame_);
       }
     } else {
       result->coverage_path =
-        util::toCoveragePathMsg(swaths, master_field, false, header, cartesian_frame);
+        util::toCoveragePathMsg(swaths, master_field, false, header, cartesian_frame_);
     }
 
     auto cycle_duration = this->now() - start_time;
@@ -277,6 +282,8 @@ CoverageServer::dynamicParametersCallback(std::vector<rclcpp::Parameter> paramet
     } else if (type == ParameterType::PARAMETER_BOOL) {
       if (name == "default_allow_overlap") {
         swath_gen_->setOVerlap(parameter.as_bool());
+      } else if (name == "coordinates_in_cartesian_frame") {
+        cartesian_frame_ = parameter.as_bool();
       }
     } else if (type == ParameterType::PARAMETER_INTEGER) {
       if (name == "default_spiral_n") {

nav2_coverage/src/headland_generator.cpp

@@ -39,7 +39,7 @@ Field HeadlandGenerator::generateHeadlands(
 
 
   if (!generator) {
-    throw std::runtime_error("No valid headlands mode set! Options: CONSTANT.");
+    throw CoverageException("No valid headlands mode set! Options: CONSTANT.");
   }
 
   RCLCPP_DEBUG(logger_, "Generating Headland with generator: %s", toString(action_type).c_str());

nav2_coverage/src/path_generator.cpp

@@ -40,7 +40,7 @@ Path PathGenerator::generatePath(
   }
 
   if (!curve) {
-    throw std::runtime_error("No valid path mode set!");
+    throw CoverageException("No valid path mode set!");
   }
 
   RCLCPP_DEBUG(

nav2_coverage/src/route_generator.cpp

@@ -41,7 +41,7 @@ Swaths RouteGenerator::generateRoute(
   }
 
   if (!generator) {
-    throw std::runtime_error(
+    throw CoverageException(
             "No valid route mode set! Options: BOUSTROPHEDON, SNAKE, SPIRAL, CUSTOM.");
   } else if (action_type == RouteType::SPIRAL) {
     dynamic_cast<f2c::rp::SpiralOrder *>(generator.get())->setSpiralSize(spiral_n);

nav2_coverage/src/swath_generator.cpp

@@ -49,14 +49,14 @@ Swaths SwathGenerator::generateSwaths(
   switch (action_angle_type) {
     case SwathAngleType::BRUTE_FORCE:
       if (!objective) {
-        throw std::runtime_error("No valid swath mode set! Options: LENGTH, NUMBER, COVERAGE.");
+        throw CoverageException("No valid swath mode set! Options: LENGTH, NUMBER, COVERAGE.");
       }
       generator_->step_angle = step_angle;
       return generator_->generateBestSwaths(*objective, robot_params_->getOperationWidth(), field);
     case SwathAngleType::SET_ANGLE:
       return generator_->generateSwaths(swath_angle, robot_params_->getOperationWidth(), field);
     default:
-      throw std::runtime_error("No valid swath angle mode set! Options: BRUTE_FORCE, SET_ANGLE.");
+      throw CoverageException("No valid swath angle mode set! Options: BRUTE_FORCE, SET_ANGLE.");
   }
 }