Assistant: load additional file with kinematics parameters

moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.h

@@ -69,6 +69,7 @@ struct GroupMetaData
   std::string kinematics_solver_;               // Name of kinematics plugin to use
   double kinematics_solver_search_resolution_;  // resolution to use with solver
   double kinematics_solver_timeout_;            // solver timeout
+  std::string kinematics_parameters_file_;      // file for additional kinematics parameters
   std::string default_planner_;                 // Name of the default planner to use
 };
 
@@ -362,6 +363,13 @@ class MoveItConfigData
    */
   bool inputKinematicsYAML(const std::string& file_path);
 
+  /**
+   * Input planning_context.launch for editing its values
+   * @param file_path path to planning_context.launch in the input package
+   * @return true if the file was read correctly
+   */
+  bool inputPlanningContextLaunch(const std::string& file_path);
+
   /**
    * Helper function for parsing ros_controllers.yaml file
    * @param YAML::Node - individual controller to be parsed
@@ -396,6 +404,16 @@ class MoveItConfigData
    */
   bool setPackagePath(const std::string& pkg_path);
 
+  /**
+   * determine the package name containing a given file path
+   * @param path to a file
+   * @param package_name holds the ros package name if found
+   * @param relative_filepath holds the relative path of the file to the package root
+   * @return whether the file belongs to a known package
+   */
+  bool extractPackageNameFromPath(const std::string& path, std::string& package_name,
+                                  std::string& relative_filepath) const;
+
   /**
    * Resolve path to .setup_assistant file
    * @param path resolved path

moveit_setup_assistant/src/tools/moveit_config_data.cpp

@@ -1360,6 +1360,46 @@ bool MoveItConfigData::inputKinematicsYAML(const std::string& file_path)
   return true;  // file created successfully
 }
 
+// ******************************************************************************************
+// Input planning_context.launch file
+// ******************************************************************************************
+bool MoveItConfigData::inputPlanningContextLaunch(const std::string& file_path)
+{
+  TiXmlDocument launch_document(file_path);
+  if (!launch_document.LoadFile())
+  {
+    ROS_ERROR_STREAM("Failed parsing " << file_path);
+    return false;
+  }
+
+  // find the kinematics section
+  TiXmlHandle doc(&launch_document);
+  TiXmlElement* kinematics_group = doc.FirstChild("launch").FirstChild("group").ToElement();
+  while (kinematics_group && kinematics_group->Attribute("ns") != std::string("$(arg robot_description)_kinematics"))
+  {
+    kinematics_group = kinematics_group->NextSiblingElement("group");
+  }
+  if (!kinematics_group)
+  {
+    ROS_ERROR("<group ns=\"$(arg robot_description)_kinematics\"> not found");
+    return false;
+  }
+
+  // iterate over all <rosparam namespace="group" file="..."/> elements
+  // and if 'group' matches an existing group, copy the filename
+  for (TiXmlElement* kinematics_parameter_file = kinematics_group->FirstChildElement("rosparam");
+       kinematics_parameter_file; kinematics_parameter_file = kinematics_parameter_file->NextSiblingElement("rosparam"))
+  {
+    const char* ns = kinematics_parameter_file->Attribute("ns");
+    if (ns && (group_meta_data_.find(ns) != group_meta_data_.end()))
+    {
+      group_meta_data_[ns].kinematics_parameters_file_ = kinematics_parameter_file->Attribute("file");
+    }
+  }
+
+  return true;
+}
+
 // ******************************************************************************************
 // Helper function for parsing an individual ROSController from ros_controllers yaml file
 // ******************************************************************************************
@@ -1550,6 +1590,46 @@ bool MoveItConfigData::setPackagePath(const std::string& pkg_path)
   return true;
 }
 
+// ******************************************************************************************
+// Extract the package/stack name from an absolute file path
+// Input:  path
+// Output: package name and relative path
+// ******************************************************************************************
+bool MoveItConfigData::extractPackageNameFromPath(const std::string& path, std::string& package_name,
+                                                  std::string& relative_filepath) const
+{
+  fs::path sub_path = path;  // holds the directory less one folder
+  fs::path relative_path;    // holds the path after the sub_path
+
+  bool package_found = false;
+
+  // truncate path step by step and check if it contains a package.xml
+  while (!sub_path.empty())
+  {
+    ROS_DEBUG_STREAM("checking in " << sub_path.make_preferred().string());
+    if (fs::is_regular_file(sub_path / "package.xml"))
+    {
+      ROS_DEBUG_STREAM("Found package.xml in " << sub_path.make_preferred().string());
+      package_found = true;
+      relative_filepath = relative_path.string();
+      package_name = sub_path.leaf().string();
+      break;
+    }
+    relative_path = sub_path.leaf() / relative_path;
+    sub_path.remove_leaf();
+  }
+
+  // Assign data to moveit_config_data
+  if (!package_found)
+  {
+    // No package name found, we must be outside ROS
+    return false;
+  }
+
+  ROS_DEBUG_STREAM("Package name for file \"" << path << "\" is \"" << package_name << "\"");
+  return true;
+}
+
 // ******************************************************************************************
 // Resolve path to .setup_assistant file
 // ******************************************************************************************

moveit_setup_assistant/src/widgets/configuration_files_widget.cpp

@@ -962,6 +962,9 @@ bool ConfigurationFilesWidget::generatePackage()
     absolute_path = config_data_->appendPaths(new_package_path, file->rel_path_);
     ROS_DEBUG_STREAM("Creating file " << absolute_path);
 
+    // Clear template strings in case export is run multiple times with changes in between
+    template_strings_.clear();
+
     // Run the generate function
     if (!file->gen_func_(absolute_path))
     {
@@ -1118,6 +1121,24 @@ void ConfigurationFilesWidget::loadTemplateStrings()
     addTemplateString("[ROS_CONTROLLERS]", controllers.str());
   }
 
+  // Pair 10 - Add parameter files for the kinematics solvers that should be loaded
+  // in addition to kinematics.yaml by planning_context.launch
+  std::string kinematics_parameters_files_block;
+  for (const auto& groups : config_data_->group_meta_data_)
+  {
+    if (groups.second.kinematics_parameters_file_.empty())
+      continue;
+
+    // add a linebreak if we have more than one entry
+    if (!kinematics_parameters_files_block.empty())
+      kinematics_parameters_files_block += "\n";
+
+    std::string line = "    <rosparam command=\"load\" ns=\"" + groups.first + "\" file=\"" +
+                       groups.second.kinematics_parameters_file_ + "\"/>";
+    kinematics_parameters_files_block += line;
+  }
+  addTemplateString("[KINEMATICS_PARAMETERS_FILE_NAMES_BLOCK]", kinematics_parameters_files_block);
+
   addTemplateString("[AUTHOR_NAME]", config_data_->author_name_);
   addTemplateString("[AUTHOR_EMAIL]", config_data_->author_email_);
 }

moveit_setup_assistant/src/widgets/group_edit_widget.cpp

@@ -37,6 +37,7 @@
 #include <QVBoxLayout>
 #include <QHBoxLayout>
 #include <QMessageBox>
+#include <QFileDialog>
 #include <QFormLayout>
 #include <QString>
 #include <QGroupBox>
@@ -88,6 +89,20 @@ GroupEditWidget::GroupEditWidget(QWidget* parent, const MoveItConfigDataPtr& con
   kinematics_timeout_field_->setMaximumWidth(400);
   form_layout->addRow("Kin. Search Timeout (sec):", kinematics_timeout_field_);
 
+  // file to load additional parameters from
+  kinematics_parameters_file_field_ = new QLineEdit(this);
+  kinematics_parameters_file_field_->setMaximumWidth(400);
+  QPushButton* kinematics_parameters_file_button = new QPushButton("...", this);
+  kinematics_parameters_file_button->setMaximumWidth(50);
+  connect(kinematics_parameters_file_button, SIGNAL(clicked()), this, SLOT(selectKinematicsFile()));
+  QBoxLayout* kinematics_parameters_file_layout = new QHBoxLayout(this);
+  kinematics_parameters_file_layout->addWidget(kinematics_parameters_file_field_);
+  kinematics_parameters_file_layout->addWidget(kinematics_parameters_file_button);
+  kinematics_parameters_file_layout->setContentsMargins(0, 0, 0, 0);
+  QWidget* container = new QWidget(this);
+  container->setLayout(kinematics_parameters_file_layout);
+  form_layout->addRow("Kin. parameters file:", container);
+
   group1->setLayout(form_layout);
 
   // OMPL Planner form --------------------------------------------
@@ -258,6 +273,9 @@ void GroupEditWidget::setSelected(const std::string& group_name)
     kinematics_solver_field_->setCurrentIndex(index);
   }
 
+  kinematics_parameters_file_field_->setText(
+      config_data_->group_meta_data_[group_name].kinematics_parameters_file_.c_str());
+
   // Set default planner
   std::string default_planner = config_data_->group_meta_data_[group_name].default_planner_;
 
@@ -338,4 +356,26 @@ void GroupEditWidget::loadKinematicPlannersComboBox()
   }
 }
 
+void GroupEditWidget::selectKinematicsFile()
+{
+  QString filename = QFileDialog::getOpenFileName(this, "Select a parameter file", "", "YAML files (*.yaml)");
+
+  if (filename.isEmpty())
+  {
+    return;
+  }
+
+  std::string package_name;
+  std::string relative_filename;
+  bool package_found =
+      config_data_->extractPackageNameFromPath(filename.toStdString(), package_name, relative_filename);
+
+  QString lookup_path = filename;
+  if (package_found)
+  {
+    lookup_path = QString("$(find %1)/%2").arg(package_name.c_str()).arg(relative_filename.c_str());
+  }
+  kinematics_parameters_file_field_->setText(lookup_path);
+}
+
 }  // namespace moveit_setup_assistant

moveit_setup_assistant/src/widgets/group_edit_widget.h

@@ -75,6 +75,7 @@ class GroupEditWidget : public QWidget
   QComboBox* kinematics_solver_field_;
   QLineEdit* kinematics_resolution_field_;
   QLineEdit* kinematics_timeout_field_;
+  QLineEdit* kinematics_parameters_file_field_;
   QComboBox* default_planner_field_;
   QPushButton* btn_delete_;      // this button is hidden for new groups
   QPushButton* btn_save_;        // this button is hidden for new groups
@@ -86,6 +87,9 @@ private Q_SLOTS:
   // Slot Event Functions
   // ******************************************************************************************
 
+  /// Shows a file dialog to select an additional parameter file for kinematics
+  void selectKinematicsFile();
+
 Q_SIGNALS:
 
   // ******************************************************************************************

moveit_setup_assistant/src/widgets/planning_groups_widget.cpp

@@ -1084,6 +1084,8 @@ bool PlanningGroupsWidget::saveGroupScreen()
   const std::string& default_planner = group_edit_widget_->default_planner_field_->currentText().toStdString();
   const std::string& kinematics_resolution = group_edit_widget_->kinematics_resolution_field_->text().toStdString();
   const std::string& kinematics_timeout = group_edit_widget_->kinematics_timeout_field_->text().toStdString();
+  const std::string& kinematics_parameters_file =
+      group_edit_widget_->kinematics_parameters_file_field_->text().toStdString();
 
   // Used for editing existing groups
   srdf::Model::Group* searched_group = nullptr;
@@ -1234,6 +1236,7 @@ bool PlanningGroupsWidget::saveGroupScreen()
   config_data_->group_meta_data_[group_name].kinematics_solver_ = kinematics_solver;
   config_data_->group_meta_data_[group_name].kinematics_solver_search_resolution_ = kinematics_resolution_double;
   config_data_->group_meta_data_[group_name].kinematics_solver_timeout_ = kinematics_timeout_double;
+  config_data_->group_meta_data_[group_name].kinematics_parameters_file_ = kinematics_parameters_file;
   config_data_->group_meta_data_[group_name].default_planner_ = default_planner;
   config_data_->changes |= MoveItConfigData::GROUP_KINEMATICS;
 

moveit_setup_assistant/src/widgets/start_screen_widget.cpp

@@ -416,6 +416,12 @@ bool StartScreenWidget::loadExistingFiles()
                                  "at location ")
                              .append(kinematics_yaml_path.make_preferred().string().c_str()));
   }
+  else
+  {
+    fs::path planning_context_launch_path = config_data_->config_pkg_path_;
+    planning_context_launch_path /= "launch/planning_context.launch";
+    config_data_->inputPlanningContextLaunch(planning_context_launch_path.make_preferred().string());
+  }
 
   // Load 3d_sensors config file
   load3DSensorsFile();
@@ -636,65 +642,10 @@ bool StartScreenWidget::setSRDFFile(const std::string& srdf_string)
 // ******************************************************************************************
 bool StartScreenWidget::extractPackageNameFromPath()
 {
-  // Get the path to urdf, save filename
-  fs::path urdf_path = config_data_->urdf_path_;
-  fs::path urdf_directory = urdf_path;
-  urdf_directory.remove_filename();
-
-  fs::path sub_path;         // holds the directory less one folder
-  fs::path relative_path;    // holds the path after the sub_path
-  std::string package_name;  // result
-
-  // Paths for testing if files exist
-  fs::path package_path;
-
-  std::vector<std::string> path_parts;  // holds each folder name in vector
-
-  // Copy path into vector of parts
-  for (fs::path::iterator it = urdf_directory.begin(); it != urdf_directory.end(); ++it)
-    path_parts.push_back(it->string());
-
-  bool package_found = false;
-
-  // reduce the generated directoy path's folder count by 1 each loop
-  for (int segment_length = path_parts.size(); segment_length > 0; --segment_length)
-  {
-    // Reset the sub_path
-    sub_path.clear();
+  std::string relative_path;  // holds the path after the sub_path
+  std::string package_name;   // result
 
-    // Create a subpath based on the outer loops length
-    for (int segment_count = 0; segment_count < segment_length; ++segment_count)
-    {
-      sub_path /= path_parts[segment_count];
-
-      // decide if we should remember this directory name because it is topmost, in case it is the package/stack name
-      if (segment_count == segment_length - 1)
-      {
-        package_name = path_parts[segment_count];
-      }
-    }
-
-    // check if this directory has a package.xml
-    package_path = sub_path;
-    package_path /= "package.xml";
-    ROS_DEBUG_STREAM("Checking for " << package_path.make_preferred().string());
-
-    // Check if the files exist
-    if (fs::is_regular_file(package_path) || fs::is_regular_file(sub_path / "manifest.xml"))
-    {
-      // now generate the relative path
-      for (std::size_t relative_count = segment_length; relative_count < path_parts.size(); ++relative_count)
-        relative_path /= path_parts[relative_count];
-
-      // add the URDF filename at end of relative path
-      relative_path /= urdf_path.filename();
-
-      // end the search
-      segment_length = 0;
-      package_found = true;
-      break;
-    }
-  }
+  bool package_found = config_data_->extractPackageNameFromPath(config_data_->urdf_path_, package_name, relative_path);
 
   // Assign data to moveit_config_data
   if (!package_found)
@@ -706,7 +657,7 @@ bool StartScreenWidget::extractPackageNameFromPath()
   else
   {
     // Check that ROS can find the package
-    const std::string robot_desc_pkg_path = ros::package::getPath(config_data_->urdf_pkg_name_) + "/";
+    const std::string robot_desc_pkg_path = ros::package::getPath(package_name);
 
     if (robot_desc_pkg_path.empty())
     {
@@ -718,7 +669,7 @@ bool StartScreenWidget::extractPackageNameFromPath()
 
     // Success
     config_data_->urdf_pkg_name_ = package_name;
-    config_data_->urdf_pkg_relative_path_ = relative_path.make_preferred().string();
+    config_data_->urdf_pkg_relative_path_ = relative_path;
   }
 
   ROS_DEBUG_STREAM("URDF Package Name: " << config_data_->urdf_pkg_name_);

moveit_setup_assistant/templates/moveit_config_pkg_template/launch/moveit_rviz.launch

@@ -10,7 +10,6 @@
 
   <node name="$(anon rviz)" launch-prefix="$(arg launch_prefix)" pkg="rviz" type="rviz" respawn="false"
 	args="$(arg command_args)" output="screen">
-    <rosparam command="load" file="$(find [GENERATED_PACKAGE_NAME])/config/kinematics.yaml"/>
   </node>
 
 </launch>

moveit_setup_assistant/templates/moveit_config_pkg_template/launch/planning_context.launch

@@ -19,6 +19,7 @@
   <!-- Load default settings for kinematics; these settings are overridden by settings in a node's namespace -->
   <group ns="$(arg robot_description)_kinematics">
     <rosparam command="load" file="$(find [GENERATED_PACKAGE_NAME])/config/kinematics.yaml"/>
+[KINEMATICS_PARAMETERS_FILE_NAMES_BLOCK]
   </group>
 
 </launch>

moveit_setup_assistant/templates/moveit_config_pkg_template/launch/run_benchmark_ompl.launch

@@ -15,7 +15,6 @@
 
   <!-- Start Benchmark Executable -->
   <node name="$(anon moveit_benchmark)" pkg="moveit_ros_benchmarks" type="moveit_run_benchmark" args="$(arg cfg) --benchmark-planners" respawn="false" output="screen">
-    <rosparam command="load" file="$(find [GENERATED_PACKAGE_NAME])/config/kinematics.yaml"/>
     <rosparam command="load" file="$(find [GENERATED_PACKAGE_NAME])/config/ompl_planning.yaml"/>
   </node>