[MSA] Merge Upstream into feature/msa (#1119)

moveit_setup_assistant/moveit_setup_app_plugins/include/moveit_setup_app_plugins/perception.hpp

@@ -55,7 +55,7 @@ class Perception : public moveit_setup_framework::SetupStep
     return true;  // no dependencies
   }
 
-  std::vector<SensorParameters>& getSensorPluginConfig()
+  const std::vector<SensorParameters>& getSensorPluginConfig()
   {
     return perception_config_->getSensorPluginConfig();
   }

moveit_setup_assistant/moveit_setup_app_plugins/include/moveit_setup_app_plugins/perception_config.hpp

@@ -47,6 +47,9 @@ class PerceptionConfig : public moveit_setup_framework::SetupConfig
 public:
   void loadPrevious(const std::string& package_path, const YAML::Node& node) override;
 
+  /// Load perception sensor config
+  static std::vector<SensorParameters> load3DSensorsYAML(const std::string& file_path);
+
   bool isConfigured() const override
   {
     return !sensors_plugin_config_parameter_list_.empty();
@@ -55,7 +58,7 @@ class PerceptionConfig : public moveit_setup_framework::SetupConfig
   /**
    * \brief Used for adding a sensor plugin configuration parameter to the sensor plugin configuration parameter list
    */
-  std::vector<SensorParameters>& getSensorPluginConfig()
+  const std::vector<SensorParameters>& getSensorPluginConfig()
   {
     return sensors_plugin_config_parameter_list_;
   }
@@ -103,14 +106,6 @@ class PerceptionConfig : public moveit_setup_framework::SetupConfig
   }
 
 protected:
-  /**
-   * Input sensors_3d file - contains 3d sensors config data
-   *
-   * @param file_path path to sensors_3d yaml file in the config package
-   * @return true if the file was read correctly
-   */
-  bool input3DSensorsYAML(const std::string& file_path);
-
   /// Sensor plugin configuration parameter list, each sensor plugin type is a map of string pairs
   std::vector<SensorParameters> sensors_plugin_config_parameter_list_;
   bool changed_{ false };

moveit_setup_assistant/moveit_setup_app_plugins/src/perception_config.cpp

@@ -45,30 +45,37 @@ void PerceptionConfig::loadPrevious(const std::string& package_path, const YAML:
 {
   // Loads parameters values from sensors_3d yaml file if available
   std::string sensors_3d_yaml_path = moveit_setup_framework::appendPaths(package_path, "config/sensors_3d.yaml");
-  if (boost::filesystem::is_regular_file(sensors_3d_yaml_path))
+  if (!boost::filesystem::is_regular_file(sensors_3d_yaml_path))
   {
-    input3DSensorsYAML(sensors_3d_yaml_path);
+    std::string pkg_path = ament_index_cpp::get_package_share_directory("moveit_setup_app_plugins");
+    std::string templates_folder = moveit_setup_framework::appendPaths(pkg_path, "templates");
+    sensors_3d_yaml_path = moveit_setup_framework::appendPaths(templates_folder, "config/sensors_3d.yaml");
   }
-  else
+  try
+  {
+    sensors_plugin_config_parameter_list_ = load3DSensorsYAML(sensors_3d_yaml_path);
+  }
+  catch (const std::runtime_error& e)
   {
-    // TODO: Handle defaults
-    // fs::path default_sensors_3d_yaml_path = "templates/moveit_config_pkg_template/config/sensors_3d.yaml";
-    // Load from default
-    // input3DSensorsYAML(default_sensors_3d_yaml_path.make_preferred().string());
+    RCLCPP_ERROR_STREAM((*logger_), e.what());
   }
 }
 
 // ******************************************************************************************
 // Input sensors_3d yaml file
 // ******************************************************************************************
-bool PerceptionConfig::input3DSensorsYAML(const std::string& file_path)
+std::vector<SensorParameters> PerceptionConfig::load3DSensorsYAML(const std::string& file_path)
 {
+  std::vector<SensorParameters> config;
+  // Is there a sensors config in the package?
+  if (file_path.empty())
+    return config;
+
   // Load file
   std::ifstream input_stream(file_path.c_str());
   if (!input_stream.good())
   {
-    RCLCPP_ERROR_STREAM((*logger_), "Unable to open file for reading " << file_path);
-    return false;
+    throw std::runtime_error("Unable to open file for reading " + file_path);
   }
 
   // Begin parsing
@@ -81,33 +88,28 @@ bool PerceptionConfig::input3DSensorsYAML(const std::string& file_path)
     // Make sure that the sensors are written as a sequence
     if (sensors_node && sensors_node.IsSequence())
     {
-      SensorParameters sensor_map;
-      bool empty_node = true;
-
       // Loop over the sensors available in the file
       for (const YAML::Node& sensor : sensors_node)
       {
-        if (const YAML::Node& sensor_node = sensor)
+        SensorParameters sensor_map;
+        bool empty_node = true;
+
+        for (YAML::const_iterator sensor_it = sensor.begin(); sensor_it != sensor.end(); ++sensor_it)
         {
-          for (YAML::const_iterator sensor_it = sensor_node.begin(); sensor_it != sensor_node.end(); ++sensor_it)
-          {
-            empty_node = false;
-            sensor_map[sensor_it->first.as<std::string>()] = sensor_it->second.as<std::string>();
-          }
-          // Don't push empty nodes
-          if (!empty_node)
-            sensors_plugin_config_parameter_list_.push_back(sensor_map);
+          empty_node = false;
+          sensor_map[sensor_it->first.as<std::string>()] = sensor_it->second.as<std::string>();
         }
+        // Don't push empty nodes
+        if (!empty_node)
+          config.push_back(sensor_map);
       }
     }
-    return true;
+    return config;
   }
-  catch (YAML::ParserException& e)  // Catch errors
+  catch (YAML::ParserException& e)  // Catch errors, rethrow as runtime_error
   {
-    RCLCPP_ERROR_STREAM((*logger_), "Error parsing sensors yaml: " << e.what());
+    throw std::runtime_error(std::string("Error parsing sensors yaml: ") + e.what());
   }
-
-  return false;  // if it gets to this point an error has occurred
 }
 
 // ******************************************************************************************
@@ -146,7 +148,6 @@ void PerceptionConfig::setConfig(const SensorParameters& parameters)
 bool PerceptionConfig::GeneratedSensorConfig::writeYaml(YAML::Emitter& emitter)
 {
   emitter << YAML::BeginMap;
-  emitter << YAML::Comment("The name of this file shouldn't be changed, or else the Setup Assistant won't detect it");
   emitter << YAML::Key << "sensors";
   emitter << YAML::Value << YAML::BeginSeq;
   for (auto& sensor_config : parent_.sensors_plugin_config_parameter_list_)

moveit_setup_assistant/moveit_setup_app_plugins/src/perception_widget.cpp

@@ -272,51 +272,42 @@ void PerceptionWidget::loadSensorPluginsComboBox()
     return;
   has_loaded = true;
 
-  // Remove all old items
-  sensor_plugin_field_->clear();
-
   // Add None option, the default
   sensor_plugin_field_->addItem("None");
+  sensor_plugin_field_->setCurrentIndex(0);
 
   // Add the two available plugins to combo box
   sensor_plugin_field_->addItem("Point Cloud");
   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();
+  const 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"))
+    if (sensor_plugin_config.at("sensor_plugin") == std::string("occupancy_map_monitor/PointCloudOctomapUpdater"))
     {
       sensor_plugin_field_->setCurrentIndex(1);
-      point_cloud_topic_field_->setText(QString(sensor_plugin_config["point_cloud_topic"].c_str()));
-      max_range_field_->setText(QString(sensor_plugin_config["max_range"].c_str()));
-      point_subsample_field_->setText(QString(sensor_plugin_config["point_subsample"].c_str()));
-      padding_offset_field_->setText(QString(sensor_plugin_config["padding_offset"].c_str()));
-      padding_scale_field_->setText(QString(sensor_plugin_config["padding_scale"].c_str()));
-      max_update_rate_field_->setText(QString(sensor_plugin_config["max_update_rate"].c_str()));
-      filtered_cloud_topic_field_->setText(QString(sensor_plugin_config["filtered_cloud_topic"].c_str()));
+      point_cloud_topic_field_->setText(QString(sensor_plugin_config.at("point_cloud_topic").c_str()));
+      max_range_field_->setText(QString(sensor_plugin_config.at("max_range").c_str()));
+      point_subsample_field_->setText(QString(sensor_plugin_config.at("point_subsample").c_str()));
+      padding_offset_field_->setText(QString(sensor_plugin_config.at("padding_offset").c_str()));
+      padding_scale_field_->setText(QString(sensor_plugin_config.at("padding_scale").c_str()));
+      max_update_rate_field_->setText(QString(sensor_plugin_config.at("max_update_rate").c_str()));
+      filtered_cloud_topic_field_->setText(QString(sensor_plugin_config.at("filtered_cloud_topic").c_str()));
     }
-    else if (sensor_plugin_config["sensor_plugin"] == std::string("occupancy_map_monitor/DepthImageOctomapUpdater"))
+    else if (sensor_plugin_config.at("sensor_plugin") == std::string("occupancy_map_monitor/DepthImageOctomapUpdater"))
     {
       sensor_plugin_field_->setCurrentIndex(2);
-      image_topic_field_->setText(QString(sensor_plugin_config["image_topic"].c_str()));
-      queue_size_field_->setText(QString(sensor_plugin_config["queue_size"].c_str()));
-      near_clipping_field_->setText(QString(sensor_plugin_config["near_clipping_plane_distance"].c_str()));
-      far_clipping_field_->setText(QString(sensor_plugin_config["far_clipping_plane_distance"].c_str()));
-      shadow_threshold_field_->setText(QString(sensor_plugin_config["shadow_threshold"].c_str()));
-      depth_padding_scale_field_->setText(QString(sensor_plugin_config["padding_scale"].c_str()));
-      depth_padding_offset_field_->setText(QString(sensor_plugin_config["padding_offset"].c_str()));
-      depth_filtered_cloud_topic_field_->setText(QString(sensor_plugin_config["filtered_cloud_topic"].c_str()));
-      depth_max_update_rate_field_->setText(QString(sensor_plugin_config["max_update_rate"].c_str()));
+      image_topic_field_->setText(QString(sensor_plugin_config.at("image_topic").c_str()));
+      queue_size_field_->setText(QString(sensor_plugin_config.at("queue_size").c_str()));
+      near_clipping_field_->setText(QString(sensor_plugin_config.at("near_clipping_plane_distance").c_str()));
+      far_clipping_field_->setText(QString(sensor_plugin_config.at("far_clipping_plane_distance").c_str()));
+      shadow_threshold_field_->setText(QString(sensor_plugin_config.at("shadow_threshold").c_str()));
+      depth_padding_scale_field_->setText(QString(sensor_plugin_config.at("padding_scale").c_str()));
+      depth_padding_offset_field_->setText(QString(sensor_plugin_config.at("padding_offset").c_str()));
+      depth_filtered_cloud_topic_field_->setText(QString(sensor_plugin_config.at("filtered_cloud_topic").c_str()));
+      depth_max_update_rate_field_->setText(QString(sensor_plugin_config.at("max_update_rate").c_str()));
     }
   }
-
-  // If no sensor config exists, default to None
-  if (sensors_vec_map.size() == 2)
-  {
-    sensor_plugin_field_->setCurrentIndex(0);
-  }
 }
 
 }  // namespace moveit_setup_app_plugins

moveit_setup_assistant/moveit_setup_core_plugins/include/moveit_setup_core_plugins/configuration_files.hpp

@@ -105,6 +105,11 @@ class ConfigurationFiles : public moveit_setup_framework::SetupStep
 
   std::string getInvalidGroupName() const;
 
+  void setGenerationTime()
+  {
+    package_settings_->setGenerationTime();
+  }
+
 protected:
   bool hasMatchingFileStatus(FileStatus status) const;
 

moveit_setup_assistant/moveit_setup_core_plugins/include/moveit_setup_core_plugins/configuration_files_widget.hpp

@@ -102,6 +102,9 @@ private Q_SLOTS:
   /// Disable or enable item in gen_files_ array
   void changeCheckedState(QListWidgetItem* item);
 
+  /// Set checked state of all selected items
+  void setCheckSelected(bool checked);
+
   // When the configuration path changes
   void onPackagePathChanged(const QString& path);
 

moveit_setup_assistant/moveit_setup_core_plugins/src/configuration_files_widget.cpp

@@ -178,6 +178,12 @@ void ConfigurationFilesWidget::onInit()
   this->setLayout(layout);
 }
 
+void ConfigurationFilesWidget::setCheckSelected(bool checked)
+{
+  for (const QModelIndex& row : action_list_->selectionModel()->selectedRows())
+    action_list_->model()->setData(row, checked ? Qt::Checked : Qt::Unchecked, Qt::CheckStateRole);
+}
+
 void ConfigurationFilesWidget::onPackagePathChanged(const QString& path)
 {
   setup_step_.setPackagePath(path.toStdString());
@@ -443,6 +449,8 @@ bool ConfigurationFilesWidget::generatePackage()
     }
   }
 
+  setup_step_.setGenerationTime();
+
   // Begin to create files and folders ----------------------------------------------------------------------
   std::string absolute_path;
 

moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/data/package_settings_config.hpp

@@ -195,6 +195,8 @@ class PackageSettingsConfig : public SetupConfig
     author_email_ = email;
   }
 
+  void setGenerationTime();
+
 protected:
   /// Loaded configuration package path - if an existing package was loaded, holds that path
   std::string config_pkg_path_;

moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/utilities.hpp

@@ -123,7 +123,7 @@ template <typename T>
 inline bool getYamlProperty(const YAML::Node& node, const std::string& key, T& storage, const T& default_value = T())
 {
   const YAML::Node& n = node[key];
-  bool valid = n;
+  bool valid = n.IsDefined();
   storage = valid ? n.as<T>() : default_value;
   return valid;
 }

moveit_setup_assistant/moveit_setup_framework/src/package_settings_config.cpp

@@ -55,7 +55,7 @@ YAML::Node PackageSettingsConfig::saveToYaml() const
   YAML::Node node;
   node["author_name"] = author_name_;
   node["author_email"] = author_email_;
-  node["generated_timestamp"] = std::time(nullptr);  // TODO: is this cross-platform?
+  node["generated_timestamp"] = config_pkg_generated_timestamp_;
   return node;
 }
 
@@ -195,6 +195,12 @@ bool PackageSettingsConfig::hasValidEmail() const
 {
   return std::regex_match(author_email_, MAIL_REGEX);
 }
+
+void PackageSettingsConfig::setGenerationTime()
+{
+  config_pkg_generated_timestamp_ = std::time(nullptr);  // TODO: is this cross-platform?
+}
+
 }  // namespace moveit_setup_framework
 
 #include <pluginlib/class_list_macros.hpp>  // NOLINT

moveit_setup_assistant/moveit_setup_framework/src/srdf_config.cpp

@@ -219,10 +219,6 @@ void SRDFConfig::collectVariables(std::vector<TemplateVariable>& variables)
   unsigned int counter = 0;
   for (const auto& vj : srdf_.virtual_joints_)
   {
-    if (vj.type_ == "fixed")
-    {
-      continue;
-    }
     vjb << "  <node pkg=\"tf2_ros\" type=\"static_transform_publisher\" name=\"virtual_joint_broadcaster_" << counter
         << "\" args=\"0 0 0 0 0 0 " << vj.parent_frame_ << " " << vj.child_link_ << "\" />" << std::endl;
     counter++;

moveit_setup_assistant/moveit_setup_srdf_plugins/src/default_collisions.cpp

@@ -164,7 +164,7 @@ void DefaultCollisions::startGenerationThread(unsigned int num_trials, double mi
   progress_ = 0;
 
   // start worker thread
-  worker_ = boost::thread(boost::bind(&DefaultCollisions::generateCollisionTable, this, num_trials, min_frac, verbose));
+  worker_ = boost::thread(std::bind(&DefaultCollisions::generateCollisionTable, this, num_trials, min_frac, verbose));
 }
 
 // ******************************************************************************************

moveit_setup_assistant/moveit_setup_srdf_plugins/src/robot_poses_widget.cpp

@@ -70,7 +70,7 @@ void RobotPosesWidget::onInit()
   auto header = new moveit_setup_framework::HeaderWidget(
       "Define Robot Poses",
       "Create poses for the robot. Poses are defined as sets of joint values for "
-      "particular planning groups. This is useful for things like <i>home position</i>."
+      "particular planning groups. This is useful for things like <i>home position</i>. "
       "The <i>first</i> listed pose will be the robot's initial pose in simulation.",
       this);
   layout->addWidget(header);

moveit_setup_assistant/moveit_setup_srdf_plugins/src/virtual_joints_widget.cpp

@@ -54,11 +54,11 @@ void VirtualJointsWidget::onInit()
 
   // Top Header Area ------------------------------------------------
 
-  auto header =
-      new moveit_setup_framework::HeaderWidget("Define Virtual Joints",
-                                               "Create a virtual joint between a robot link and an external frame of "
-                                               "reference (considered fixed with respect to the robot).",
-                                               this);
+  auto header = new moveit_setup_framework::HeaderWidget(
+      "Define Virtual Joints",
+      "Create a virtual joint between the base robot link and an external frame of reference. "
+      "This allows to place the robot in the world or on a mobile platform.",
+      this);
   layout->addWidget(header);
 
   // Create contents screens ---------------------------------------

moveit_setup_assistant/old_assistant/include/moveit/setup_assistant/tools/moveit_config_data.hpp

@@ -203,7 +203,7 @@ class MoveItConfigData
    * \brief Add a Follow Joint Trajectory action Controller for each Planning Group
    * \return true if controllers were added to the controller_configs_ data structure
    */
-  bool addDefaultControllers();
+  bool addDefaultControllers(const std::string& controller_type = "effort_controllers/JointTrajectoryController");
 
   /**
    * Helper Function for joining a file path and a file name, or two file paths, etc,
@@ -271,8 +271,8 @@ class MoveItConfigData
   // Private Vars
   // ******************************************************************************************
 
-  /// ROS Controllers config data
-  std::vector<ROSControlConfig> ros_controllers_config_;
+  /// Controllers config data
+  std::vector<ControllerConfig> controller_configs_;
 };
 
 }  // namespace moveit_setup_assistant

moveit_setup_assistant/old_assistant/src/tools/moveit_config_data.cpp

@@ -41,6 +41,7 @@
 #include <boost/filesystem/path.hpp>        // for creating folders/files
 #include <boost/filesystem/operations.hpp>  // is_regular_file, is_directory, etc.
 #include <boost/algorithm/string/trim.hpp>
+#include <boost/algorithm/string/predicate.hpp>
 
 // ROS
 #include <rclcpp/rclcpp.hpp>
@@ -1348,16 +1349,5 @@ bool MoveItConfigData::addController(const ControllerConfig& new_controller)
 }
 
 // ******************************************************************************************
-// Gets ros_controllers_config_ vector
-// ******************************************************************************************
-void MoveItConfigData::addGenericParameterToSensorPluginConfig(const std::string& name, const std::string& value,
-                                                               const std::string& /*comment*/)
-{
-  // Use index 0 since we only write one plugin
-  GenericParameter new_parameter;
-  new_parameter.setName(name);
-  new_parameter.setValue(value);
-  sensors_plugin_config_parameter_list_[0][name] = new_parameter;
-}
 
 }  // namespace moveit_setup_assistant