[MSA] SRDF Setup (#1057)

moveit_setup_assistant/MIGRATION.md

@@ -10,10 +10,13 @@ This file should be deleted once all tasks are complete. Also delete `SETUP_STEP
  * `moveit_setup_srdf_plugins`
    * [x] Self-Collisions
    * [x] Virtual Joints
-   * [ ] Planning Groups
-   * [ ] Robot Poses
-   * [ ] End Effectors
-   * [ ] Passive Joints
+   * [x] Planning Groups
+     * BUG: Creating a new group does not properly create a new JointModelGroup
+     * TODO: Need method for getting a list of OMPL Planners
+   * [x] Robot Poses
+     * TODO: Need to re-add sleeping code
+   * [x] End Effectors
+   * [x] Passive Joints
  * `moveit_setup_app_plugins`
    * [ ] ROS Control
    * [ ] Simulation

moveit_setup_assistant/extra_generated_files_code.txt

@@ -19,14 +19,6 @@
   file.gen_func_ = boost::bind(&MoveItConfigData::outputCHOMPPlanningYAML, config_data_, _1);
   file.write_on_changes = MoveItConfigData::GROUPS;  // need to double check if this is actually correct!
 
-  // kinematics.yaml  --------------------------------------------------------------------------------------
-  file.file_name_ = "kinematics.yaml";
-  file.rel_path_ = config_data_->appendPaths(config_path, file.file_name_);
-  file.description_ = "Specifies which kinematic solver plugin to use for each planning group in the SRDF, as well as "
-                      "the kinematic solver search resolution.";
-  file.gen_func_ = boost::bind(&MoveItConfigData::outputKinematicsYAML, config_data_, _1);
-  file.write_on_changes = MoveItConfigData::GROUPS | MoveItConfigData::GROUP_KINEMATICS;
-
   // joint_limits.yaml --------------------------------------------------------------------------------------
   file.file_name_ = "joint_limits.yaml";
   file.rel_path_ = config_data_->appendPaths(config_path, file.file_name_);

moveit_setup_assistant/extra_initialization_code.txt

@@ -1,25 +1,6 @@
-  // Load the allowed collision matrix
-  config_data_->loadAllowedCollisionMatrix();
-
-  // Load kinematics yaml file if available --------------------------------------------------
-  fs::path kinematics_yaml_path = config_data_->config_pkg_path_;
-  kinematics_yaml_path /= "config/kinematics.yaml";
-
-  if (!config_data_->inputKinematicsYAML(kinematics_yaml_path.make_preferred().string()))
-  {
-    QMessageBox::warning(this, "No Kinematic YAML File",
-                         QString("Failed to parse kinematics yaml file. This file is not critical but any previous "
-                                 "kinematic solver settings have been lost. To re-populate this file edit each "
-                                 "existing planning group and choose a solver, then save each change. \n\nFile error "
-                                 "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());
-  }
+  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 ros controllers yaml file if available-----------------------------------------------
   fs::path ros_controllers_yaml_path = config_data_->config_pkg_path_;

moveit_setup_assistant/extra_template_variables.txt

@@ -14,21 +14,3 @@
     }
     variables.push_back(TemplateVariable("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;
-  }
-  variables.push_back(TemplateVariable("KINEMATICS_PARAMETERS_FILE_NAMES_BLOCK", kinematics_parameters_files_block));

moveit_setup_assistant/moveit_setup_app_plugins/src/perception_widget.cpp

@@ -283,7 +283,7 @@ void PerceptionWidget::loadSensorPluginsComboBox()
   sensor_plugin_field_->addItem("Depth Map");
 
   // Load default config, or use the one in the config package if exists
-  auto sensors_vec_map = setup_step_.getSensorPluginConfig();
+  auto& sensors_vec_map = setup_step_.getSensorPluginConfig();
   for (auto& sensor_plugin_config : sensors_vec_map)
   {
     if (sensor_plugin_config["sensor_plugin"] == std::string("occupancy_map_monitor/PointCloudOctomapUpdater"))

moveit_setup_assistant/moveit_setup_assistant/src/setup_assistant_widget.cpp

@@ -92,6 +92,10 @@ SetupAssistantWidget::SetupAssistantWidget(rviz_common::ros_integration::RosNode
   setup_steps.push_back("moveit_setup_core_plugins::StartScreenWidget");
   setup_steps.push_back("moveit_setup_srdf_plugins::DefaultCollisionsWidget");
   setup_steps.push_back("moveit_setup_srdf_plugins::VirtualJointsWidget");
+  setup_steps.push_back("moveit_setup_srdf_plugins::PlanningGroupsWidget");
+  setup_steps.push_back("moveit_setup_srdf_plugins::RobotPosesWidget");
+  setup_steps.push_back("moveit_setup_srdf_plugins::EndEffectorsWidget");
+  setup_steps.push_back("moveit_setup_srdf_plugins::PassiveJointsWidget");
   setup_steps.push_back("moveit_setup_app_plugins::PerceptionWidget");
   setup_steps.push_back("moveit_setup_core_plugins::AuthorInformationWidget");
   setup_steps.push_back("moveit_setup_core_plugins::ConfigurationFilesWidget");

moveit_setup_assistant/moveit_setup_framework/CMakeLists.txt

@@ -26,10 +26,12 @@ qt5_wrap_cpp(MOC_FILES
     include/moveit_setup_framework/qt/helper_widgets.hpp
     include/moveit_setup_framework/qt/setup_step_widget.hpp
     include/moveit_setup_framework/qt/rviz_panel.hpp
+    include/moveit_setup_framework/qt/double_list_widget.hpp
 )
 
 add_library(${PROJECT_NAME}
     src/helper_widgets.cpp
+    src/double_list_widget.cpp
     src/urdf_config.cpp
     src/package_settings_config.cpp
     src/srdf_config.cpp

moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/data/srdf_config.hpp

@@ -43,6 +43,20 @@
 
 namespace moveit_setup_framework
 {
+// bits of information that can be changed in the SRDF
+enum InformationFields
+{
+  NONE = 0,
+  COLLISIONS = 1 << 1,
+  VIRTUAL_JOINTS = 1 << 2,
+  GROUPS = 1 << 3,
+  GROUP_CONTENTS = 1 << 4,
+  POSES = 1 << 5,
+  END_EFFECTORS = 1 << 6,
+  PASSIVE_JOINTS = 1 << 7,
+  OTHER = 1 << 8,
+};
+
 class SRDFConfig : public SetupConfig
 {
 public:
@@ -71,7 +85,9 @@ class SRDFConfig : public SetupConfig
   /// Provide a shared planning scene
   planning_scene::PlanningScenePtr getPlanningScene();
 
-  void updateRobotModel(bool mark_as_changed = false);
+  /// Update the robot model with the new SRDF, AND mark the changes that have been made to the model
+  /// changed_information should be composed of InformationFields
+  void updateRobotModel(long changed_information = 0L);
 
   std::vector<std::string> getLinkNames() const;
 
@@ -90,11 +106,39 @@ class SRDFConfig : public SetupConfig
     return srdf_.groups_;
   }
 
+  std::vector<std::string> getGroupNames() const
+  {
+    std::vector<std::string> group_names;
+    for (const srdf::Model::Group& group : srdf_.groups_)
+    {
+      group_names.push_back(group.name_);
+    }
+    return group_names;
+  }
+
+  std::vector<srdf::Model::GroupState>& getGroupStates()
+  {
+    return srdf_.group_states_;
+  }
+
   std::vector<srdf::Model::VirtualJoint>& getVirtualJoints()
   {
     return srdf_.virtual_joints_;
   }
 
+  std::vector<srdf::Model::PassiveJoint>& getPassiveJoints()
+  {
+    return srdf_.passive_joints_;
+  }
+
+  /**
+   * @brief Return the name of the child link of a joint
+   * @return empty string if joint is not found
+   */
+  std::string getChildOfJoint(const std::string& joint_name) const;
+
+  void removePoseByName(const std::string& pose_name, const std::string& group_name);
+
   class GeneratedSRDF : public GeneratedFile
   {
   public:
@@ -118,7 +162,7 @@ class SRDFConfig : public SetupConfig
 
     bool hasChanges() const override
     {
-      return parent_.has_changes_;
+      return parent_.changes_ > 0;
     }
 
     bool write() override
@@ -173,6 +217,7 @@ class SRDFConfig : public SetupConfig
   /// Shared planning scene
   planning_scene::PlanningScenePtr planning_scene_;
 
-  bool has_changes_;
+  // bitfield of changes (composed of InformationFields)
+  unsigned long changes_;
 };
 }  // namespace moveit_setup_framework

moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/qt/double_list_widget.hpp

@@ -37,16 +37,12 @@
 #pragma once
 
 #include <QWidget>
-class QLabel;
-class QTableWidget;
-class QTableWidgetItem;
-class QItemSelection;
+#include <QLabel>
+#include <QTableWidget>
+#include <QTableWidgetItem>
+#include <QItemSelection>
 
-#ifndef Q_MOC_RUN
-#include <moveit/setup_assistant/tools/moveit_config_data.h>
-#endif
-
-namespace moveit_setup_assistant
+namespace moveit_setup_framework
 {
 class DoubleListWidget : public QWidget
 {
@@ -61,8 +57,7 @@ class DoubleListWidget : public QWidget
   // ******************************************************************************************
 
   /// Constructor
-  DoubleListWidget(QWidget* parent, const MoveItConfigDataPtr& config_data, const QString& long_name,
-                   const QString& short_name, bool add_ok_cancel = true);
+  DoubleListWidget(QWidget* parent, const QString& long_name, const QString& short_name, bool add_ok_cancel = true);
 
   /// Loads the available data list
   void setAvailable(const std::vector<std::string>& items);
@@ -78,6 +73,9 @@ class DoubleListWidget : public QWidget
   /// Set the names of the two columns in the widget
   void setColumnNames(const QString& col1, const QString& col2);
 
+  /// Return all the values that are in the "selected" subset
+  std::vector<std::string> getSelectedValues() const;
+
   // ******************************************************************************************
   // Qt Components
   // ******************************************************************************************
@@ -130,9 +128,6 @@ private Q_SLOTS:
   // Variables
   // ******************************************************************************************
 
-  /// Contains all the configuration data for the setup assistant
-  moveit_setup_assistant::MoveItConfigDataPtr config_data_;
-
   // ******************************************************************************************
   // Private Functions
   // ******************************************************************************************
@@ -141,4 +136,4 @@ private Q_SLOTS:
   void previewSelected(const QList<QTableWidgetItem*>& selected);
 };
 
-}  // namespace moveit_setup_assistant
+}  // namespace moveit_setup_framework

moveit_setup_assistant/moveit_setup_framework/src/double_list_widget.cpp

@@ -45,16 +45,16 @@
 #include <QString>
 #include <QTableWidget>
 #include <QVBoxLayout>
-#include "double_list_widget.h"
+#include <moveit_setup_framework/qt/double_list_widget.hpp>
 
-namespace moveit_setup_assistant
+namespace moveit_setup_framework
 {
 // ******************************************************************************************
 //
 // ******************************************************************************************
-DoubleListWidget::DoubleListWidget(QWidget* parent, const MoveItConfigDataPtr& config_data, const QString& long_name,
-                                   const QString& short_name, bool add_ok_cancel)
-  : QWidget(parent), long_name_(long_name), short_name_(short_name), config_data_(config_data)
+DoubleListWidget::DoubleListWidget(QWidget* parent, const QString& long_name, const QString& short_name,
+                                   bool add_ok_cancel)
+  : QWidget(parent), long_name_(long_name), short_name_(short_name)
 {
   // Basic widget container
   QVBoxLayout* layout = new QVBoxLayout();
@@ -334,4 +334,14 @@ void DoubleListWidget::previewSelected(const QList<QTableWidgetItem*>& selected)
   Q_EMIT(previewSelected(selected_vector));
 }
 
-}  // namespace moveit_setup_assistant
+std::vector<std::string> DoubleListWidget::getSelectedValues() const
+{
+  std::vector<std::string> values(selected_data_table_->rowCount());
+  for (int i = 0; i < selected_data_table_->rowCount(); ++i)
+  {
+    values.push_back(selected_data_table_->item(i, 0)->text().toStdString());
+  }
+  return values;
+}
+
+}  // namespace moveit_setup_framework

moveit_setup_assistant/moveit_setup_framework/src/srdf_config.cpp

@@ -41,7 +41,7 @@ namespace moveit_setup_framework
 void SRDFConfig::onInit()
 {
   parent_node_->declare_parameter("robot_description_semantic", rclcpp::ParameterType::PARAMETER_STRING);
-  has_changes_ = false;
+  changes_ = 0L;
 }
 
 void SRDFConfig::loadPrevious(const std::string& package_path, const YAML::Node& node)
@@ -135,19 +135,24 @@ void SRDFConfig::getRelativePath()
   srdf_pkg_relative_path_ = appendPaths("config", urdf_model_->getName() + ".srdf");
 }
 
-void SRDFConfig::updateRobotModel(bool mark_as_changed)
+void SRDFConfig::updateRobotModel(long changed_information)
 {
   // Initialize with a URDF Model Interface and a SRDF Model
   loadURDFModel();
+  if (changed_information > 0)
+  {
+    srdf_.updateSRDFModel(*urdf_model_);
+  }
+
   robot_model_ = std::make_shared<moveit::core::RobotModel>(urdf_model_, srdf_.srdf_model_);
 
   if (srdf_pkg_relative_path_.empty())
   {
     srdf_pkg_relative_path_ = appendPaths("config", urdf_model_->getName() + ".srdf");
     srdf_.robot_name_ = urdf_model_->getName();
-    has_changes_ = true;
+    changes_ |= OTHER;
   }
-  has_changes_ |= mark_as_changed;
+  changes_ |= changed_information;
 
   // Reset the planning scene
   planning_scene_.reset();
@@ -179,6 +184,31 @@ std::vector<std::string> SRDFConfig::getLinkNames() const
   return names;
 }
 
+std::string SRDFConfig::getChildOfJoint(const std::string& joint_name) const
+{
+  const moveit::core::JointModel* joint_model = getRobotModel()->getJointModel(joint_name);
+  // Check that a joint model was found
+  if (!joint_model)
+  {
+    return "";
+  }
+  return joint_model->getChildLinkModel()->getName();
+}
+
+void SRDFConfig::removePoseByName(const std::string& pose_name, const std::string& group_name)
+{
+  for (std::vector<srdf::Model::GroupState>::iterator pose_it = srdf_.group_states_.begin();
+       pose_it != srdf_.group_states_.end(); ++pose_it)
+  {
+    if (pose_it->name_ == pose_name && pose_it->group_ == group_name)
+    {
+      srdf_.group_states_.erase(pose_it);
+      updateRobotModel(moveit_setup_framework::POSES);
+      return;
+    }
+  }
+}
+
 void SRDFConfig::collectVariables(std::vector<TemplateVariable>& variables)
 {
   variables.push_back(TemplateVariable("ROBOT_NAME", srdf_.robot_name_));