renaming robot_params + unit tests

SteveMacenski · SteveMacenski · commit befaccd4e045 · 2023-10-24T15:01:12.000-07:00
diff --git a/nav2_coverage/README.md b/nav2_coverage/README.md
@@ -84,3 +84,7 @@ Q
 Leader follower 2-3 months out to kick off 
 a bunch of other stuff --> projects interest me. open nav hire to fill!
 
+
+
+TODO
+  - Use coverage exception
diff --git a/nav2_coverage/include/nav2_coverage/coverage_server.hpp b/nav2_coverage/include/nav2_coverage/coverage_server.hpp
@@ -122,7 +122,7 @@ class CoverageServer : public nav2_util::LifecycleNode
 
   std::unique_ptr<ActionServer> action_server_;
 
-  std::unique_ptr<RobotParams> robot_;
+  std::unique_ptr<RobotParams> robot_params_;
   std::unique_ptr<HeadlandGenerator> headland_gen_;
   std::unique_ptr<SwathGenerator> swath_gen_;
   std::unique_ptr<RouteGenerator> route_gen_;
diff --git a/nav2_coverage/include/nav2_coverage/path_generator.hpp b/nav2_coverage/include/nav2_coverage/path_generator.hpp
@@ -27,7 +27,7 @@
 #include "nav2_complete_coverage_msgs/msg/path_mode.hpp"
 #include "nav2_coverage/utils.hpp"
 #include "nav2_coverage/types.hpp"
-#include "nav2_coverage/robot.hpp"
+#include "nav2_coverage/robot_params.hpp"
 
 namespace nav2_coverage
 {
@@ -43,10 +43,10 @@ class PathGenerator
    * @param node A node to get the swath type from
    */
   template<typename NodeT>
-  explicit PathGenerator(const NodeT & node, RobotParams * robot)
+  explicit PathGenerator(const NodeT & node, RobotParams * robot_params)
   {
     logger_ = node->get_logger();
-    robot_ = robot;
+    robot_params_ = robot_params;
 
     nav2_util::declare_parameter_if_not_declared(
       node, "default_path_type", rclcpp::ParameterValue("DUBIN"));
@@ -127,7 +127,7 @@ class PathGenerator
   float default_turn_point_distance_;
   TurningBasePtr default_curve_;
   std::unique_ptr<f2c::pp::PathPlanning> generator_;
-  RobotParams * robot_;
+  RobotParams * robot_params_;
   rclcpp::Logger logger_{rclcpp::get_logger("SwathGenerator")};
 };
 
diff --git a/nav2_coverage/include/nav2_coverage/robot_params.hpp b/nav2_coverage/include/nav2_coverage/robot_params.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef NAV2_COVERAGE__ROBOT_HPP_
-#define NAV2_COVERAGE__ROBOT_HPP_
+#ifndef NAV2_COVERAGE__ROBOT_PARAMS_HPP_
+#define NAV2_COVERAGE__ROBOT_PARAMS_HPP_
 
 #include <vector>
 #include <string>
@@ -91,4 +91,4 @@ class RobotParams
 
 }  // namespace nav2_coverage
 
-#endif  // NAV2_COVERAGE__ROBOT_HPP_
+#endif  // NAV2_COVERAGE__ROBOT_PARAMS_HPP_
diff --git a/nav2_coverage/include/nav2_coverage/swath_generator.hpp b/nav2_coverage/include/nav2_coverage/swath_generator.hpp
@@ -27,7 +27,7 @@
 #include "nav2_complete_coverage_msgs/msg/swath_mode.hpp"
 #include "nav2_coverage/utils.hpp"
 #include "nav2_coverage/types.hpp"
-#include "nav2_coverage/robot.hpp"
+#include "nav2_coverage/robot_params.hpp"
 
 namespace nav2_coverage
 {
@@ -43,10 +43,10 @@ class SwathGenerator
    * @param node A node to get the swath type from
    */
   template<typename NodeT>
-  explicit SwathGenerator(const NodeT & node, RobotParams * robot)
+  explicit SwathGenerator(const NodeT & node, RobotParams * robot_params)
   {
     logger_ = node->get_logger();
-    robot_ = robot;
+    robot_params_ = robot_params;
 
     nav2_util::declare_parameter_if_not_declared(
       node, "default_swath_type", rclcpp::ParameterValue("LENGTH"));
@@ -151,7 +151,7 @@ class SwathGenerator
   double default_step_angle_;
   bool default_allow_overlap_;
   std::unique_ptr<f2c::sg::BruteForce> generator_;
-  RobotParams * robot_;
+  RobotParams * robot_params_;
   rclcpp::Logger logger_{rclcpp::get_logger("SwathGenerator")};
 };
 
diff --git a/nav2_coverage/include/nav2_coverage/types.hpp b/nav2_coverage/include/nav2_coverage/types.hpp
@@ -29,6 +29,7 @@ namespace nav2_coverage
 typedef F2CCells Fields;
 typedef F2CCell Field;
 typedef F2CSwaths Swaths;
+typedef F2CSwath Swath;
 typedef F2CPath Path;
 typedef F2CRobot Robot;
 typedef F2CLinearRing Polygon;
diff --git a/nav2_coverage/package.xml b/nav2_coverage/package.xml
@@ -4,7 +4,6 @@
   <name>nav2_coverage</name>
   <version>0.0.1</version>
   <description>A Task Server for complete coverage planning</description>
-  <author email="steve@opennav.org">Steve Macenski</author>
   <maintainer email="steve@opennav.org">Steve Macenski</maintainer>
   <license>Apache-2.0</license>
 
diff --git a/nav2_coverage/src/coverage_server.cpp b/nav2_coverage/src/coverage_server.cpp
@@ -33,11 +33,11 @@ CoverageServer::on_configure(const rclcpp_lifecycle::State & /*state*/)
   RCLCPP_INFO(get_logger(), "Configuring %s", get_name());
   auto node = shared_from_this();
 
-  robot_ = std::make_unique<RobotParams>(node);
+  robot_params_ = std::make_unique<RobotParams>(node);
   headland_gen_ = std::make_unique<HeadlandGenerator>(node);
-  swath_gen_ = std::make_unique<SwathGenerator>(node, robot_.get());
+  swath_gen_ = std::make_unique<SwathGenerator>(node, robot_params_.get());
   route_gen_ = std::make_unique<RouteGenerator>(node);
-  path_gen_ = std::make_unique<PathGenerator>(node, robot_.get());
+  path_gen_ = std::make_unique<PathGenerator>(node, robot_params_.get());
   visualizer_ = std::make_unique<Visualizer>();
 
   double action_server_result_timeout;
@@ -101,7 +101,7 @@ CoverageServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/)
   route_gen_.reset();
   swath_gen_.reset();
   headland_gen_.reset();
-  robot_.reset();
+  robot_params_.reset();
   return nav2_util::CallbackReturn::SUCCESS;
 }
 
@@ -243,10 +243,10 @@ CoverageServer::dynamicParametersCallback(std::vector<rclcpp::Parameter> paramet
       } else if (name == "default_turn_point_distance") {
         path_gen_->setTurnPointDistance(parameter.as_double());
       } else if (name == "robot_width") {
-        auto & robot = robot_->getRobot();
+        auto & robot = robot_params_->getRobot();
         robot.robot_width = parameter.as_double();
       } else if (name == "operation_width") {
-        auto & robot = robot_->getRobot();
+        auto & robot = robot_params_->getRobot();
         robot.op_width = parameter.as_double();
       }
     } else if (type == ParameterType::PARAMETER_STRING) {
diff --git a/nav2_coverage/src/path_generator.cpp b/nav2_coverage/src/path_generator.cpp
@@ -47,7 +47,7 @@ Path PathGenerator::generatePath(
     logger_,
     "Generating path with curve: %s", toString(action_type, action_continuity_type).c_str());
   generator_->turn_point_dist = turn_point_distance;
-  return generator_->searchBestPath(robot_->getRobot(), swaths, *curve);
+  return generator_->searchBestPath(robot_params_->getRobot(), swaths, *curve);
 }
 
 void PathGenerator::setPathMode(const std::string & new_mode)
diff --git a/nav2_coverage/src/swath_generator.cpp b/nav2_coverage/src/swath_generator.cpp
@@ -52,9 +52,9 @@ Swaths SwathGenerator::generateSwaths(
         throw std::runtime_error("No valid swath mode set! Options: LENGTH, NUMBER, COVERAGE.");
       }
       generator_->step_angle = step_angle;
-      return generator_->generateBestSwaths(*objective, robot_->getOperationWidth(), field);
+      return generator_->generateBestSwaths(*objective, robot_params_->getOperationWidth(), field);
     case SwathAngleType::SET_ANGLE:
-      return generator_->generateSwaths(swath_angle, robot_->getOperationWidth(), field);
+      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.");
   }
diff --git a/nav2_coverage/src/visualizer.cpp b/nav2_coverage/src/visualizer.cpp
@@ -47,7 +47,7 @@ void Visualizer::visualize(
     for (unsigned int i = 0; i != boundary.size(); i++) {
       field_polygon->polygon.points.push_back(util::toMsg(boundary.getGeometry(i)));
     }
-    headlands_pub_->publish(std::move(field_polygon));    
+    headlands_pub_->publish(std::move(field_polygon));
   }
 
   // Visualize field for planning (after headland removed)
diff --git a/nav2_coverage/test/CMakeLists.txt b/nav2_coverage/test/CMakeLists.txt
@@ -0,0 +1,65 @@
+# Test utils
+ament_add_gtest(test_utils
+  test_utils.cpp
+)
+ament_target_dependencies(test_utils
+  ${dependencies}
+)
+target_link_libraries(test_utils
+  ${library_name}
+)
+
+# Test robot
+ament_add_gtest(test_robot
+  test_robot.cpp
+)
+ament_target_dependencies(test_robot
+  ${dependencies}
+)
+target_link_libraries(test_robot
+  ${library_name}
+)
+
+# Test visualizations
+ament_add_gtest(test_viz
+  test_viz.cpp
+)
+ament_target_dependencies(test_viz
+  ${dependencies}
+)
+target_link_libraries(test_viz
+  ${library_name}
+)
+
+# Test headland
+ament_add_gtest(test_headland
+  test_headland.cpp
+)
+ament_target_dependencies(test_headland
+  ${dependencies}
+)
+target_link_libraries(test_headland
+  ${library_name}
+)
+
+# Test swath
+ament_add_gtest(test_swath
+  test_swath.cpp
+)
+ament_target_dependencies(test_swath
+  ${dependencies}
+)
+target_link_libraries(test_swath
+  ${library_name}
+)
+
+# Test route
+ament_add_gtest(test_route
+  test_route.cpp
+)
+ament_target_dependencies(test_route
+  ${dependencies}
+)
+target_link_libraries(test_route
+  ${library_name}
+)
diff --git a/nav2_coverage/test/test_headland.cpp b/nav2_coverage/test/test_headland.cpp
@@ -0,0 +1,92 @@
+// Copyright (c) 2023 Open Navigation LLC
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "gtest/gtest.h"
+#include "rclcpp/rclcpp.hpp"
+#include "nav2_coverage/headland_generator.hpp"
+#include "tf2/utils.h"
+#include "fields2cover/utils/random.h"
+
+// Luckily, F2C has very high test coverage so we only need to test what we touch
+
+class RosLockGuard
+{
+public:
+  RosLockGuard() {rclcpp::init(0, nullptr);}
+  ~RosLockGuard() {rclcpp::shutdown();}
+};
+RosLockGuard g_rclcpp;
+
+namespace nav2_coverage
+{
+
+class HeadlandShim : public HeadlandGenerator
+{
+public:
+  template<typename NodeT>
+  explicit HeadlandShim(const NodeT & node)
+  : HeadlandGenerator(node)
+  {}
+
+  HeadlandGeneratorPtr createGeneratorShim(const HeadlandType & type)
+  {
+    return createGenerator(type);
+  }
+
+  std::string toStringShim(const HeadlandType & type)
+  {
+    return toString(type);
+  }
+
+  HeadlandType toTypeShim(const std::string & str)
+  {
+    return toType(str);
+  }
+};
+
+TEST(HeadlandTests, TestheadlandUtils)
+{
+  auto node = std::make_shared<rclcpp::Node>("test_node");
+  auto generator = HeadlandShim(node);
+
+  EXPECT_EQ(generator.toStringShim(HeadlandType::UNKNOWN), std::string("Unknown"));
+  EXPECT_EQ(generator.toStringShim(HeadlandType::CONSTANT), std::string("Constant"));
+
+  EXPECT_EQ(generator.toTypeShim("FAKE"), HeadlandType::UNKNOWN);
+  EXPECT_EQ(generator.toTypeShim("constant"), HeadlandType::CONSTANT);
+  EXPECT_EQ(generator.toTypeShim("CONSTANT"), HeadlandType::CONSTANT);
+
+  EXPECT_TRUE(generator.createGeneratorShim(HeadlandType::CONSTANT));
+  EXPECT_FALSE(generator.createGeneratorShim(HeadlandType::UNKNOWN));
+
+  generator.setMode("constant");
+}
+
+TEST(HeadlandTests, TestheadlandGeneration)
+{
+  auto node = std::make_shared<rclcpp::Node>("test_node");
+  auto generator = HeadlandShim(node);
+
+  // Generate some toy field
+  f2c::Random rand;
+  auto field = rand.generateRandField(5, 1e5);
+
+  // Shouldn't throw, results in valid output
+  nav2_complete_coverage_msgs::msg::HeadlandMode settings;
+  auto field_out = generator.generateHeadlands(field.field.getGeometry(0), settings);
+  settings.mode = "CONSTANT";
+  auto field_out2 = generator.generateHeadlands(field.field.getGeometry(0), settings);
+}
+
+}  // namespace nav2_coverage
diff --git a/nav2_coverage/test/test_robot.cpp b/nav2_coverage/test/test_robot.cpp
@@ -0,0 +1,43 @@
+// Copyright (c) 2023 Open Navigation LLC
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "gtest/gtest.h"
+#include "rclcpp/rclcpp.hpp"
+#include "nav2_coverage/robot_params.hpp"
+#include "tf2/utils.h"
+
+// Luckily, F2C has very high test coverage so we only need to test what we touch
+
+class RosLockGuard
+{
+public:
+  RosLockGuard() {rclcpp::init(0, nullptr);}
+  ~RosLockGuard() {rclcpp::shutdown();}
+};
+RosLockGuard g_rclcpp;
+
+namespace nav2_coverage
+{
+
+TEST(RobotTests, Testrobot)
+{
+  auto node = std::make_shared<rclcpp::Node>("test_node");
+  RobotParams robot(node);
+
+  EXPECT_EQ(robot.getWidth(), 2.1);
+  EXPECT_EQ(robot.getOperationWidth(), 2.5);
+  EXPECT_EQ(robot.getRobot().linear_curv_change, 2.0);
+}
+
+}  // namespace nav2_coverage
diff --git a/nav2_coverage/test/test_route.cpp b/nav2_coverage/test/test_route.cpp
diff --git a/nav2_coverage/test/test_swath.cpp b/nav2_coverage/test/test_swath.cpp
diff --git a/nav2_coverage/test/test_utils.cpp b/nav2_coverage/test/test_utils.cpp
diff --git a/nav2_coverage/test/test_viz.cpp b/nav2_coverage/test/test_viz.cpp
diff --git a/nav2_coverage/test/tester.py b/nav2_coverage/test/tester.py

Original file line number	Diff line number	Diff line change
`@@ -47,7 +47,7 @@ Path PathGenerator::generatePath(`
`47`	`47`	`logger_,`
`48`	`48`	`"Generating path with curve: %s", toString(action_type, action_continuity_type).c_str());`
`49`	`49`	`generator_->turn_point_dist = turn_point_distance;`
`50`		`- return generator_->searchBestPath(robot_->getRobot(), swaths, *curve);`
	`50`	`+ return generator_->searchBestPath(robot_params_->getRobot(), swaths, *curve);`
`51`	`51`	`}`
`52`	`52`
`53`	`53`	`void PathGenerator::setPathMode(const std::string & new_mode)`
Original file line number	Diff line number	Diff line change
`@@ -52,9 +52,9 @@ Swaths SwathGenerator::generateSwaths(`
`52`	`52`	`throw std::runtime_error("No valid swath mode set! Options: LENGTH, NUMBER, COVERAGE.");`
`53`	`53`	`}`
`54`	`54`	`generator_->step_angle = step_angle;`
`55`		`- return generator_->generateBestSwaths(*objective, robot_->getOperationWidth(), field);`
	`55`	`+ return generator_->generateBestSwaths(*objective, robot_params_->getOperationWidth(), field);`
`56`	`56`	`case SwathAngleType::SET_ANGLE:`
`57`		`- return generator_->generateSwaths(swath_angle, robot_->getOperationWidth(), field);`
	`57`	`+ return generator_->generateSwaths(swath_angle, robot_params_->getOperationWidth(), field);`
`58`	`58`	`default:`
`59`	`59`	`throw std::runtime_error("No valid swath angle mode set! Options: BRUTE_FORCE, SET_ANGLE.");`
`60`	`60`	`}`