[MSA] Merge main into feature/msa (Part II) (moveit#1240)

moveit_setup_assistant/moveit_setup_assistant/src/collisions_updater.cpp

@@ -90,7 +90,7 @@ int main(int argc, char* argv[])
 
   rclcpp::init(argc, argv);
   rclcpp::Node::SharedPtr node = std::make_shared<rclcpp::Node>("collision_updater");
-  rclcpp::Logger logger = node->get_logger();
+  static const rclcpp::Logger LOGGER = rclcpp::get_logger("collision_updater");
   moveit_setup_framework::DataWarehousePtr config_data = std::make_shared<moveit_setup_framework::DataWarehouse>(node);
 
   moveit_setup_srdf_plugins::DefaultCollisions setup_step;
@@ -105,18 +105,18 @@ int main(int argc, char* argv[])
     }
     catch (const std::runtime_error& e)
     {
-      RCLCPP_ERROR_STREAM(logger, "Could not load config at '" << config_pkg_path << "'. " << e.what());
+      RCLCPP_ERROR_STREAM(LOGGER, "Could not load config at '" << config_pkg_path << "'. " << e.what());
       return 1;
     }
   }
   else if (urdf_path.empty() || srdf_path.empty())
   {
-    RCLCPP_ERROR_STREAM(logger, "Please provide config package or URDF and SRDF path");
+    RCLCPP_ERROR_STREAM(LOGGER, "Please provide config package or URDF and SRDF path");
     return 1;
   }
   else if (rdf_loader::RDFLoader::isXacroFile(srdf_path) && output_path.empty())
   {
-    RCLCPP_ERROR_STREAM(logger, "Please provide a different output file for SRDF xacro input file");
+    RCLCPP_ERROR_STREAM(LOGGER, "Please provide a different output file for SRDF xacro input file");
     return 1;
   }
 
@@ -139,7 +139,9 @@ int main(int argc, char* argv[])
   }
 
   if (!keep_old)
-    srdf_config->getDisabledCollisions().clear();
+  {
+    srdf_config->clearCollisionData();
+  }
 
   setup_step.startGenerationThread(never_trials, min_collision_fraction, verbose);
   int thread_progress;
@@ -148,12 +150,12 @@ int main(int argc, char* argv[])
   {
     if (thread_progress - last_progress > 10)
     {
-      RCLCPP_INFO(logger, "%d%% complete...", thread_progress);
+      RCLCPP_INFO(LOGGER, "%d%% complete...", thread_progress);
       last_progress = thread_progress;
     }
   }
   setup_step.joinGenerationThread();
-  RCLCPP_INFO(logger, "100%% complete...");
+  RCLCPP_INFO(LOGGER, "100%% complete...");
 
   size_t skip_mask = 0;
   if (!include_default)

moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/config.hpp

@@ -137,7 +137,7 @@ class SetupConfig
    *
    * @param[out] packages Names of ROS packages
    */
-  virtual void collectDependencies(std::set<std::string>& packages) const
+  virtual void collectDependencies(std::set<std::string>& /*packages*/) const
   {
   }
 

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

@@ -72,7 +72,8 @@ class SRDFConfig : public SetupConfig
 
   /// Load SRDF File
   void loadSRDFFile(const std::string& package_path, const std::string& relative_path);
-  void loadSRDFFile(const std::string& srdf_file_path);
+  void loadSRDFFile(const std::string& srdf_file_path,
+                    const std::vector<std::string>& xacro_args = std::vector<std::string>());
 
   moveit::core::RobotModelPtr getRobotModel() const
   {
@@ -88,9 +89,16 @@ class SRDFConfig : public SetupConfig
 
   std::vector<std::string> getLinkNames() const;
 
-  std::vector<srdf::Model::DisabledCollision>& getDisabledCollisions()
+  void clearCollisionData()
   {
-    return srdf_.disabled_collisions_;
+    srdf_.no_default_collision_links_.clear();
+    srdf_.enabled_collision_pairs_.clear();
+    srdf_.disabled_collision_pairs_.clear();
+  }
+
+  std::vector<srdf::Model::CollisionPair>& getDisabledCollisions()
+  {
+    return srdf_.disabled_collision_pairs_;
   }
 
   std::vector<srdf::Model::EndEffector>& getEndEffectors()

moveit_setup_assistant/moveit_setup_framework/src/srdf_config.cpp

@@ -81,15 +81,12 @@ void SRDFConfig::loadSRDFFile(const std::string& package_path, const std::string
 // ******************************************************************************************
 // Load SRDF File to Parameter Server
 // ******************************************************************************************
-void SRDFConfig::loadSRDFFile(const std::string& srdf_file_path)
+void SRDFConfig::loadSRDFFile(const std::string& srdf_file_path, const std::vector<std::string>& xacro_args)
 {
   srdf_path_ = srdf_file_path;
 
   loadURDFModel();
 
-  // empty
-  const std::vector<std::string> xacro_args;
-
   std::string srdf_string;
   if (!rdf_loader::RDFLoader::loadXmlFileToString(srdf_string, srdf_path_, xacro_args))
   {

moveit_setup_assistant/moveit_setup_srdf_plugins/include/moveit_setup_srdf_plugins/default_collisions.hpp

@@ -39,6 +39,15 @@
 #include <moveit_setup_srdf_plugins/compute_default_collisions.hpp>
 #include <boost/thread/thread.hpp>
 
+// less operation for two CollisionPairs
+struct CollisionPairLess
+{
+  bool operator()(const srdf::Model::CollisionPair& left, const srdf::Model::CollisionPair& right) const
+  {
+    return left.link1_ < right.link1_ && left.link2_ < right.link2_;
+  }
+};
+
 namespace moveit_setup_srdf_plugins
 {
 class DefaultCollisions : public SRDFStep

moveit_setup_assistant/moveit_setup_srdf_plugins/src/default_collisions.cpp

@@ -52,9 +52,9 @@ void DefaultCollisions::linkPairsToSRDF()
   disabled_list.clear();
 
   // Create temp disabled collision
-  srdf::Model::DisabledCollision dc;
+  srdf::Model::CollisionPair dc;
 
-  // copy the data in this class's LinkPairMap datastructure to srdf::Model::DisabledCollision format
+  // copy the data in this class's LinkPairMap datastructure to srdf::Model::CollisionPair format
   for (LinkPairMap::const_iterator pair_it = link_pairs_.begin(); pair_it != link_pairs_.end(); ++pair_it)
   {
     // Only copy those that are actually disabled
@@ -69,41 +69,21 @@ void DefaultCollisions::linkPairsToSRDF()
   srdf_config_->updateRobotModel(moveit_setup_framework::COLLISIONS);  // mark as changed
 }
 
-// ******************************************************************************************
-// Set list of collision link pairs in SRDF; sorted; with optional filter
-// ******************************************************************************************
-
-class SortableDisabledCollision
-{
-public:
-  SortableDisabledCollision(const srdf::Model::DisabledCollision& dc)
-    : dc_(dc), key_(dc.link1_ < dc.link2_ ? (dc.link1_ + "|" + dc.link2_) : (dc.link2_ + "|" + dc.link1_))
-  {
-  }
-  operator const srdf::Model::DisabledCollision&() const
-  {
-    return dc_;
-  }
-  bool operator<(const SortableDisabledCollision& other) const
-  {
-    return key_ < other.key_;
-  }
-
-private:
-  const srdf::Model::DisabledCollision dc_;
-  const std::string key_;
-};
-
 void DefaultCollisions::linkPairsToSRDFSorted(size_t skip_mask)
 {
   auto& disabled_list = srdf_config_->getDisabledCollisions();
   // Create temp disabled collision
-  srdf::Model::DisabledCollision dc;
+  srdf::Model::CollisionPair dc;
 
-  std::set<SortableDisabledCollision> disabled_collisions;
-  disabled_collisions.insert(disabled_list.begin(), disabled_list.end());
+  std::set<srdf::Model::CollisionPair, CollisionPairLess> disabled_collisions;
+  for (auto& p : disabled_list)
+  {
+    if (p.link1_ >= p.link2_)
+      std::swap(p.link1_, p.link2_);  // unify link1, link2 sorting
+    disabled_collisions.insert(p);
+  }
 
-  // copy the data in this class's LinkPairMap datastructure to srdf::Model::DisabledCollision format
+  // copy the data in this class's LinkPairMap datastructure to srdf::Model::CollisionPair format
   for (const std::pair<const std::pair<std::string, std::string>, LinkPairData>& link_pair : link_pairs_)
   {
     // Only copy those that are actually disabled
@@ -114,9 +94,11 @@ void DefaultCollisions::linkPairsToSRDFSorted(size_t skip_mask)
 
       dc.link1_ = link_pair.first.first;
       dc.link2_ = link_pair.first.second;
+      if (dc.link1_ >= dc.link2_)
+        std::swap(dc.link1_, dc.link2_);
       dc.reason_ = moveit_setup_srdf_plugins::disabledReasonToString(link_pair.second.reason);
 
-      disabled_collisions.insert(SortableDisabledCollision(dc));
+      disabled_collisions.insert(dc);
     }
   }
 
@@ -164,7 +146,8 @@ void DefaultCollisions::startGenerationThread(unsigned int num_trials, double mi
   progress_ = 0;
 
   // start worker thread
-  worker_ = boost::thread(std::bind(&DefaultCollisions::generateCollisionTable, this, num_trials, min_frac, verbose));
+  worker_ =
+      boost::thread([this, num_trials, min_frac, verbose] { generateCollisionTable(num_trials, min_frac, verbose); });
 }
 
 // ******************************************************************************************

moveit_setup_assistant/old_assistant/src/tools/moveit_config_data.cpp

@@ -299,6 +299,55 @@ bool MoveItConfigData::outputSTOMPPlanningYAML(const std::string& file_path)
   return true;  // file created successfully
 }
 
+// ******************************************************************************************
+// Output kinematic config files
+// ******************************************************************************************
+bool MoveItConfigData::outputKinematicsYAML(const std::string& file_path)
+{
+  YAML::Emitter emitter;
+  emitter << YAML::BeginMap;
+
+  // Output every group and the kinematic solver it can use ----------------------------------
+  for (srdf::Model::Group& group : srdf_->groups_)
+  {
+    // Only save kinematic data if the solver is not "None"
+    if (group_meta_data_[group.name_].kinematics_solver_.empty() ||
+        group_meta_data_[group.name_].kinematics_solver_ == "None")
+      continue;
+
+    emitter << YAML::Key << group.name_;
+    emitter << YAML::Value << YAML::BeginMap;
+
+    // Kinematic Solver
+    emitter << YAML::Key << "kinematics_solver";
+    emitter << YAML::Value << group_meta_data_[group.name_].kinematics_solver_;
+
+    // Search Resolution
+    emitter << YAML::Key << "kinematics_solver_search_resolution";
+    emitter << YAML::Value << group_meta_data_[group.name_].kinematics_solver_search_resolution_;
+
+    // Solver Timeout
+    emitter << YAML::Key << "kinematics_solver_timeout";
+    emitter << YAML::Value << group_meta_data_[group.name_].kinematics_solver_timeout_;
+
+    emitter << YAML::EndMap;
+  }
+
+  emitter << YAML::EndMap;
+
+  std::ofstream output_stream(file_path.c_str(), std::ios_base::trunc);
+  if (!output_stream.good())
+  {
+    RCLCPP_ERROR_STREAM(LOGGER, "Unable to open file for writing " << file_path);
+    return false;
+  }
+
+  output_stream << emitter.c_str();
+  output_stream.close();
+
+  return true;  // file created successfully
+}
+
 // ******************************************************************************************
 // Helper function to get the controller that is controlling the joint
 // ******************************************************************************************