[MSA] Testing Framework for MoveItSetupAssistant

moveit_setup_assistant/moveit_setup_app_plugins/CMakeLists.txt

@@ -46,6 +46,10 @@ ament_target_dependencies(${PROJECT_NAME}
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   ament_lint_auto_find_test_dependencies()
+
+  find_package(ament_cmake_gtest REQUIRED)
+  ament_add_gtest(test_perception test/test_perception.cpp)
+  target_link_libraries(test_perception ${PROJECT_NAME})
 endif()
 
 install(DIRECTORY templates DESTINATION share/${PROJECT_NAME})

moveit_setup_assistant/moveit_setup_app_plugins/package.xml

@@ -16,10 +16,11 @@
   <depend>pluginlib</depend>
   <depend>rclcpp</depend>
 
-  <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_clang_format</test_depend>
+  <test_depend>ament_cmake_gtest</test_depend>
   <test_depend>ament_cmake_lint_cmake</test_depend>
   <test_depend>ament_cmake_xmllint</test_depend>
+  <test_depend>ament_lint_auto</test_depend>
 
   <export>
     <build_type>ament_cmake</build_type>

moveit_setup_assistant/moveit_setup_app_plugins/src/perception_config.cpp

@@ -89,12 +89,13 @@ std::vector<SensorParameters> PerceptionConfig::load3DSensorsYAML(const std::fil
     if (sensors_node && sensors_node.IsSequence())
     {
       // Loop over the sensors available in the file
-      for (const YAML::Node& sensor_name : sensors_node)
+      for (const YAML::Node& sensor_name_node : sensors_node)
       {
-        const YAML::Node& sensor = doc[sensor_name.as<std::string>()];
+        std::string sensor_name = sensor_name_node.as<std::string>();
+        const YAML::Node& sensor = doc[sensor_name];
 
         SensorParameters sensor_map;
-        sensor_map["name"] = sensor_name.as<std::string>();
+        sensor_map["name"] = sensor_name;
         for (YAML::const_iterator sensor_it = sensor.begin(); sensor_it != sensor.end(); ++sensor_it)
         {
           sensor_map[sensor_it->first.as<std::string>()] = sensor_it->second.as<std::string>();

moveit_setup_assistant/moveit_setup_app_plugins/templates/config/sensors_3d.yaml

@@ -2,15 +2,19 @@
 # The name of this file shouldn't be changed, or else the Setup Assistant won't detect it.
 # Values may not be ideal defaults, original source unclear.
 sensors:
-  - sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater
+  - kinect_pointcloud
+  - kinect_depthimage
+kinect_pointcloud:
+    sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater
     point_cloud_topic: /head_mount_kinect/depth_registered/points
     max_range: 5.0
     point_subsample: 1
     padding_offset: 0.1
     padding_scale: 1.0
     max_update_rate: 1.0
     filtered_cloud_topic: filtered_cloud
-  - sensor_plugin: occupancy_map_monitor/DepthImageOctomapUpdater
+kinect_depthimage:
+    sensor_plugin: occupancy_map_monitor/DepthImageOctomapUpdater
     image_topic: /head_mount_kinect/depth_registered/image_raw
     queue_size: 5
     near_clipping_plane_distance: 0.3

moveit_setup_assistant/moveit_setup_app_plugins/test/test_perception.cpp

@@ -0,0 +1,108 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2022, Metro Robots
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Metro Robots nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+/* Author: David V. Lu!! */
+#include <moveit_setup_framework/testing_utils.hpp>
+#include <moveit_setup_app_plugins/perception_config.hpp>
+
+static const rclcpp::Logger LOGGER = rclcpp::get_logger("test_perception");
+
+using moveit_setup::expectYamlEquivalence;
+using moveit_setup::getSharePath;
+using moveit_setup::app::PerceptionConfig;
+
+class PerceptionTest : public moveit_setup::MoveItSetupTest
+{
+protected:
+  void SetUp() override
+  {
+    MoveItSetupTest::SetUp();
+    config_data_->registerType("sensors", "moveit_setup::app::PerceptionConfig");
+    templates_path_ = getSharePath("moveit_setup_app_plugins") / "templates";
+    default_yaml_path_ = templates_path_ / "config/sensors_3d.yaml";
+  }
+  std::filesystem::path templates_path_;
+  std::filesystem::path default_yaml_path_;
+};
+
+// This tests parsing of sensors_3d.yaml
+TEST_F(PerceptionTest, ReadingSensorsConfig)
+{
+  auto sensors_config = config_data_->get<PerceptionConfig>("sensors");
+
+  // Before parsing, no config available
+  EXPECT_EQ(sensors_config->getSensorPluginConfig().size(), 0u);
+
+  // Read the file containing the default config parameters
+  auto configs = sensors_config->load3DSensorsYAML(default_yaml_path_);
+
+  // Default config for the two available sensor plugins
+  // Make sure both are parsed correctly
+  ASSERT_EQ(configs.size(), 2u);
+
+  EXPECT_EQ(configs[0]["sensor_plugin"], std::string("occupancy_map_monitor/PointCloudOctomapUpdater"));
+
+  EXPECT_EQ(configs[1]["sensor_plugin"], std::string("occupancy_map_monitor/DepthImageOctomapUpdater"));
+}
+
+// This tests writing of sensors_3d.yaml
+TEST_F(PerceptionTest, WritingSensorsConfig)
+{
+  auto sensors_config = config_data_->get<PerceptionConfig>("sensors");
+
+  // Empty Config Should have No Sensors
+  EXPECT_EQ(sensors_config->getSensorPluginConfig().size(), 0u);
+
+  generateFiles<PerceptionConfig>("sensors");
+
+  std::filesystem::path generated = output_dir_ / "config/sensors_3d.yaml";
+
+  ASSERT_TRUE(std::filesystem::is_regular_file(generated));
+
+  sensors_config->loadPrevious(std::filesystem::path("fake_path"), YAML::Node());
+
+  // Should now have the defaults loaded
+  EXPECT_EQ(sensors_config->getSensorPluginConfig().size(), 2u);
+
+  generateFiles<PerceptionConfig>("sensors");
+
+  expectYamlEquivalence(generated, default_yaml_path_);
+}
+
+int main(int argc, char** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  rclcpp::init(argc, argv);
+  return RUN_ALL_TESTS();
+}

moveit_setup_assistant/moveit_setup_assistant/src/setup_assistant_widget.cpp

@@ -36,8 +36,6 @@
 
 // SA
 #include "moveit_setup_assistant/setup_assistant_widget.hpp"
-#include <moveit_setup_framework/data/urdf_config.hpp>
-#include <moveit_setup_framework/data/package_settings_config.hpp>
 
 // Qt
 #include <QApplication>
@@ -129,15 +127,11 @@ SetupAssistantWidget::SetupAssistantWidget(rviz_common::ros_integration::RosNode
   // Pass command arg values to start screen and show appropriate part of screen
   if (args.count("urdf_path"))
   {
-    std::filesystem::path urdf_path = args["urdf_path"].as<std::filesystem::path>();
-    auto config = config_data_->get<URDFConfig>("urdf");
-    config->loadFromPath(urdf_path);
+    config_data_->preloadWithURDFPath(args["urdf_path"].as<std::filesystem::path>());
   }
   if (args.count("config_pkg"))
   {
-    std::string config_pkg = args["config_pkg"].as<std::string>();
-    auto package_settings = config_data_->get<PackageSettingsConfig>("package_settings");
-    package_settings->loadExisting(config_pkg);
+    config_data_->preloadWithFullConfig(args["config_pkg"].as<std::string>());
   }
 
   // Navigation Left Pane --------------------------------------------------

moveit_setup_assistant/moveit_setup_controllers/CMakeLists.txt

@@ -51,6 +51,10 @@ ament_target_dependencies(${PROJECT_NAME}
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   ament_lint_auto_find_test_dependencies()
+
+  find_package(ament_cmake_gtest REQUIRED)
+  ament_add_gtest(test_controllers test/test_controllers.cpp)
+  target_link_libraries(test_controllers ${PROJECT_NAME})
 endif()
 
 install(DIRECTORY templates DESTINATION share/${PROJECT_NAME})

moveit_setup_assistant/moveit_setup_controllers/include/moveit_setup_controllers/moveit_controllers_config.hpp

@@ -92,6 +92,8 @@ class MoveItControllersConfig : public ControllersConfig
    * @return true if the file was read correctly
    */
   bool parseController(const std::string& name, const YAML::Node& controller);
+
+  std::map<std::string, YAML::Node> trajectory_parameters_;
 };
 }  // namespace controllers
 }  // namespace moveit_setup

moveit_setup_assistant/moveit_setup_controllers/package.xml

@@ -15,10 +15,13 @@
   <depend>pluginlib</depend>
   <depend>rclcpp</depend>
 
-  <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_clang_format</test_depend>
+  <test_depend>ament_cmake_gtest</test_depend>
   <test_depend>ament_cmake_lint_cmake</test_depend>
   <test_depend>ament_cmake_xmllint</test_depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>moveit_resources_fanuc_moveit_config</test_depend>
+  <test_depend>moveit_resources_panda_moveit_config</test_depend>
 
   <export>
     <build_type>ament_cmake</build_type>

moveit_setup_assistant/moveit_setup_controllers/src/moveit_controllers_config.cpp

@@ -45,6 +45,7 @@ void MoveItControllersConfig::loadPrevious(const std::filesystem::path& package_
 {
   changed_ = false;
   controllers_.clear();
+  trajectory_parameters_.clear();
 
   // Load moveit controllers yaml file if available-----------------------------------------------
   std::filesystem::path ros_controllers_yaml_path = package_path / MOVEIT_CONTROLLERS_YAML;
@@ -60,7 +61,8 @@ void MoveItControllersConfig::loadPrevious(const std::filesystem::path& package_
   {
     // Used in parsing controllers
     ControllerInfo control_setting;
-    YAML::Node controllers = YAML::Load(input_stream)["moveit_simple_controller_manager"];
+    YAML::Node doc = YAML::Load(input_stream);
+    YAML::Node controllers = doc["moveit_simple_controller_manager"];
 
     std::vector<std::string> controller_names;
     getYamlProperty(controllers, "controller_names", controller_names);
@@ -80,6 +82,15 @@ void MoveItControllersConfig::loadPrevious(const std::filesystem::path& package_
         return;
       }
     }
+
+    YAML::Node trajectory_execution = doc["trajectory_execution"];
+    if (trajectory_execution.IsDefined() && trajectory_execution.IsMap())
+    {
+      for (const auto& kv : trajectory_execution)
+      {
+        trajectory_parameters_[kv.first.as<std::string>()] = kv.second;
+      }
+    }
   }
   catch (YAML::ParserException& e)  // Catch errors
   {
@@ -139,10 +150,16 @@ bool MoveItControllersConfig::GeneratedControllersConfig::writeYaml(YAML::Emitte
   emitter << YAML::Newline;
   emitter << YAML::BeginMap;
   {
-    // TODO: Output possible trajectory_execution parameters including...
-    //       * allowed_execution_duration_scaling: 1.2
-    //       * allowed_goal_duration_margin: 0.5
-    //       * allowed_start_tolerance: 0.01
+    if (!parent_.trajectory_parameters_.empty())
+    {
+      emitter << YAML::Key << "trajectory_execution" << YAML::Value;
+      emitter << YAML::BeginMap;
+      for (const auto& kv : parent_.trajectory_parameters_)
+      {
+        emitter << YAML::Key << kv.first << YAML::Value << kv.second;
+      }
+      emitter << YAML::EndMap;
+    }
 
     emitter << YAML::Key << "moveit_controller_manager" << YAML::Value
             << "moveit_simple_controller_manager/MoveItSimpleControllerManager";