Merge remote-tracking branch 'upstream/main' into feature/msa

.github/workflows/ci.yaml

@@ -95,7 +95,7 @@ jobs:
         with:
           file: moveit2.repos
       - name: Cache upstream workspace
-        uses: pat-s/always-upload-cache@v2.1.5
+        uses: pat-s/always-upload-cache@v3.0.1
         with:
           path: ${{ env.BASEDIR }}/upstream_ws
           key: ${{ env.CACHE_PREFIX }}-${{ github.run_id }}
@@ -106,15 +106,15 @@ jobs:
       # that comes from the checkout.  See "prepare target_ws for cache" task below
       - name: Cache target workspace
         if: "!matrix.env.CCOV"
-        uses: pat-s/always-upload-cache@v2.1.5
+        uses: pat-s/always-upload-cache@v3.0.1
         with:
           path: ${{ env.BASEDIR }}/target_ws
           key: ${{ env.CACHE_PREFIX }}-${{ github.run_id }}
           restore-keys: ${{ env.CACHE_PREFIX }}
         env:
           CACHE_PREFIX: target_ws${{ matrix.env.CCOV && '-ccov' || '' }}-${{ matrix.env.IMAGE }}-${{ hashFiles('**/CMakeLists.txt', '**/package.xml', '.github/workflows/ci.yaml') }}
       - name: Cache ccache
-        uses: pat-s/always-upload-cache@v2.1.5
+        uses: pat-s/always-upload-cache@v3.0.1
         with:
           path: ${{ env.CCACHE_DIR }}
           key: ${{ env.CACHE_PREFIX }}-${{ github.sha }}-${{ github.run_id }}

.github/workflows/format.yaml

@@ -16,7 +16,9 @@ jobs:
     runs-on: ubuntu-22.04
     steps:
     - uses: actions/checkout@v3
-    - uses: actions/setup-python@v2
+    - uses: actions/setup-python@v4
+      with:
+        python-version: '3.10'
     - name: Install clang-format-12
       run: sudo apt-get install clang-format-12
     - uses: pre-commit/[email protected]

moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h

@@ -36,8 +36,8 @@
 
 #pragma once
 
-#include <boost/array.hpp>
-#include <boost/function.hpp>
+#include <array>
+#include <functional>
 #include <vector>
 #include <string>
 #include <map>
@@ -110,10 +110,10 @@ struct Contact
 struct CostSource
 {
   /// The minimum bound of the AABB defining the volume responsible for this partial cost
-  boost::array<double, 3> aabb_min;
+  std::array<double, 3> aabb_min;
 
   /// The maximum bound of the AABB defining the volume responsible for this partial cost
-  boost::array<double, 3> aabb_max;
+  std::array<double, 3> aabb_max;
 
   /// The partial cost (the probability of existence for the object there is a collision with)
   double cost;
@@ -220,7 +220,7 @@ struct CollisionRequest
   std::size_t max_cost_sources;
 
   /** \brief Function call that decides whether collision detection should stop. */
-  boost::function<bool(const CollisionResult&)> is_done;
+  std::function<bool(const CollisionResult&)> is_done;
 
   /** \brief Flag indicating whether information about detected collisions should be reported */
   bool verbose;

moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h

@@ -68,7 +68,7 @@ enum Type
 
 /** \brief Signature of predicate that decides whether a contact is allowed or not (when AllowedCollision::Type is
  * CONDITIONAL) */
-using DecideContactFn = boost::function<bool(collision_detection::Contact&)>;
+using DecideContactFn = std::function<bool(collision_detection::Contact&)>;
 
 MOVEIT_CLASS_FORWARD(AllowedCollisionMatrix);  // Defines AllowedCollisionMatrixPtr, ConstPtr, WeakPtr... etc
 

moveit_core/collision_detection/include/moveit/collision_detection/occupancy_map.h

@@ -38,12 +38,11 @@
 
 #include <octomap/octomap.h>
 
-#include <boost/thread/locks.hpp>
-#include <boost/thread/shared_mutex.hpp>
-#include <boost/function.hpp>
-
 #include <memory>
 #include <string>
+#include <shared_mutex>
+#include <mutex>
+#include <functional>
 
 namespace collision_detection
 {
@@ -86,8 +85,8 @@ class OccMapTree : public octomap::OcTree
     tree_mutex_.unlock();
   }
 
-  using ReadLock = boost::shared_lock<boost::shared_mutex>;
-  using WriteLock = boost::unique_lock<boost::shared_mutex>;
+  using ReadLock = std::shared_lock<std::shared_mutex>;
+  using WriteLock = std::unique_lock<std::shared_mutex>;
 
   ReadLock reading()
   {
@@ -106,14 +105,14 @@ class OccMapTree : public octomap::OcTree
   }
 
   /** @brief Set the callback to trigger when updates are received */
-  void setUpdateCallback(const boost::function<void()>& update_callback)
+  void setUpdateCallback(const std::function<void()>& update_callback)
   {
     update_callback_ = update_callback;
   }
 
 private:
-  boost::shared_mutex tree_mutex_;
-  boost::function<void()> update_callback_;
+  std::shared_mutex tree_mutex_;
+  std::function<void()> update_callback_;
 };
 
 using OccMapTreePtr = std::shared_ptr<OccMapTree>;

moveit_core/collision_detection/include/moveit/collision_detection/world.h

@@ -40,7 +40,7 @@
 #include <string>
 #include <vector>
 #include <map>
-#include <boost/function.hpp>
+#include <functional>
 #include <Eigen/Geometry>
 #include <eigen_stl_containers/eigen_stl_vector_container.h>
 #include <moveit/transforms/transforms.h>
@@ -298,7 +298,7 @@ class World
     friend class World;
   };
 
-  using ObserverCallbackFn = boost::function<void(const ObjectConstPtr&, Action)>;
+  using ObserverCallbackFn = std::function<void(const ObjectConstPtr&, Action)>;
 
   /** \brief register a callback function for notification of changes.
    * \e callback will be called right after any change occurs to any Object.

moveit_core/collision_detection/src/collision_matrix.cpp

@@ -147,7 +147,7 @@ void AllowedCollisionMatrix::setEntry(const std::string& name1, const std::strin
   const AllowedCollision::Type v = allowed ? AllowedCollision::ALWAYS : AllowedCollision::NEVER;
   entries_[name1][name2] = entries_[name2][name1] = v;
 
-  // remove boost::function pointers, if any
+  // remove function pointers, if any
   auto it = allowed_contacts_.find(name1);
   if (it != allowed_contacts_.end())
   {

moveit_core/collision_detection/src/world.cpp

@@ -140,7 +140,8 @@ bool World::knowsTransform(const std::string& name) const
     for (const std::pair<const std::string, ObjectPtr>& object : objects_)
     {
       // if "object name/" matches start of object_id, we found the matching object
-      if (boost::starts_with(name, object.first) && name[object.first.length()] == '/')
+      // rfind searches name for object.first in the first index (returns 0 if found)
+      if (name.rfind(object.first, 0) == 0 && name[object.first.length()] == '/')
       {
         return object.second->global_subframe_poses_.find(name.substr(object.first.length() + 1)) !=
                object.second->global_subframe_poses_.end();
@@ -174,7 +175,8 @@ const Eigen::Isometry3d& World::getTransform(const std::string& name, bool& fram
     for (const std::pair<const std::string, ObjectPtr>& object : objects_)
     {
       // if "object name/" matches start of object_id, we found the matching object
-      if (boost::starts_with(name, object.first) && name[object.first.length()] == '/')
+      // name.rfind is in service of removing the call to boost::starts_with and does the same thing
+      if (name.rfind(object.first, 0) == 0 && name[object.first.length()] == '/')
       {
         auto it = object.second->global_subframe_poses_.find(name.substr(object.first.length() + 1));
         if (it != object.second->global_subframe_poses_.end())

moveit_core/collision_detection_bullet/CMakeLists.txt

@@ -22,7 +22,6 @@ ament_target_dependencies(${MOVEIT_LIB_NAME}
   urdf
   urdfdom
   urdfdom_headers
-  Boost
   visualization_msgs
   octomap_msgs
 )

moveit_core/collision_detection_fcl/CMakeLists.txt

@@ -15,7 +15,6 @@ ament_target_dependencies(${MOVEIT_LIB_NAME}
   urdfdom
   urdfdom_headers
   LIBFCL
-  Boost
   visualization_msgs
 )
 target_link_libraries(${MOVEIT_LIB_NAME}

moveit_core/collision_detection_fcl/src/collision_common.cpp

@@ -49,9 +49,9 @@
 #include <fcl/octree.h>
 #endif
 
-#include <boost/thread/mutex.hpp>
 #include <memory>
 #include <type_traits>
+#include <mutex>
 
 namespace collision_detection
 {

moveit_core/collision_distance_field/CMakeLists.txt

@@ -35,7 +35,6 @@ if(BUILD_TESTING)
 
   ament_add_gtest(test_collision_distance_field test/test_collision_distance_field.cpp)
   ament_target_dependencies(test_collision_distance_field
-    Boost
     geometric_shapes
     OCTOMAP
     srdfdom

moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_env_distance_field.h

@@ -41,8 +41,8 @@
 #include <moveit/collision_distance_field/collision_common_distance_field.h>
 #include <moveit/collision_detection/collision_env.h>
 #include <moveit/planning_scene/planning_scene.h>
-#include <boost/thread/mutex.hpp>
 #include "rclcpp/rclcpp.hpp"
+#include <mutex>
 
 namespace collision_detection
 {
@@ -296,14 +296,14 @@ class CollisionEnvDistanceField : public CollisionEnv
   std::vector<BodyDecompositionConstPtr> link_body_decomposition_vector_;
   std::map<std::string, unsigned int> link_body_decomposition_index_map_;
 
-  mutable boost::mutex update_cache_lock_;
+  mutable std::mutex update_cache_lock_;
   DistanceFieldCacheEntryPtr distance_field_cache_entry_;
   std::map<std::string, std::map<std::string, bool>> in_group_update_map_;
   std::map<std::string, GroupStateRepresentationPtr> pregenerated_group_state_representation_map_;
 
   planning_scene::PlanningScenePtr planning_scene_;
 
-  mutable boost::mutex update_cache_lock_world_;
+  mutable std::mutex update_cache_lock_world_;
   DistanceFieldCacheEntryWorldPtr distance_field_cache_entry_world_;
   GroupStateRepresentationPtr last_gsr_;
   World::ObserverHandle observer_handle_;

moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_env_hybrid.h

@@ -41,8 +41,6 @@
 #include <moveit/collision_distance_field/collision_common_distance_field.h>
 #include <moveit/collision_distance_field/collision_env_distance_field.h>
 
-#include <boost/thread/mutex.hpp>
-
 namespace collision_detection
 {
 /** \brief This hybrid collision environment combines FCL and a distance field. Both can be used to calculate

moveit_core/collision_distance_field/src/collision_common_distance_field.cpp

@@ -35,7 +35,6 @@
 /* Author: E. Gil Jones */
 
 #include <moveit/collision_distance_field/collision_common_distance_field.h>
-#include <boost/thread/mutex.hpp>
 #include <rclcpp/duration.hpp>
 #include <rclcpp/logger.hpp>
 #include <rclcpp/logging.hpp>
@@ -46,6 +45,7 @@
 #include <tf2_eigen/tf2_eigen.h>
 #endif
 #include <memory>
+#include <mutex>
 
 namespace collision_detection
 {
@@ -62,7 +62,7 @@ struct BodyDecompositionCache
   static const unsigned int MAX_CLEAN_COUNT = 100;
   Map map_;
   unsigned int clean_count_;
-  boost::mutex lock_;
+  std::mutex lock_;
 };
 
 BodyDecompositionCache& getBodyDecompositionCache()
@@ -77,7 +77,7 @@ BodyDecompositionConstPtr getBodyDecompositionCacheEntry(const shapes::ShapeCons
   BodyDecompositionCache& cache = getBodyDecompositionCache();
   shapes::ShapeConstWeakPtr wptr(shape);
   {
-    boost::mutex::scoped_lock slock(cache.lock_);
+    std::scoped_lock slock(cache.lock_);
     BodyDecompositionCache::Map::const_iterator cache_it = cache.map_.find(wptr);
     if (cache_it != cache.map_.end())
     {
@@ -87,7 +87,7 @@ BodyDecompositionConstPtr getBodyDecompositionCacheEntry(const shapes::ShapeCons
 
   BodyDecompositionConstPtr bdcp = std::make_shared<const BodyDecomposition>(shape, resolution);
   {
-    boost::mutex::scoped_lock slock(cache.lock_);
+    std::scoped_lock slock(cache.lock_);
     cache.map_[wptr] = bdcp;
     cache.clean_count_++;
     return bdcp;

moveit_core/collision_distance_field/src/collision_env_distance_field.cpp

@@ -171,7 +171,7 @@ void CollisionEnvDistanceField::generateCollisionCheckingStructures(
     // DistanceFieldCacheEntry for CollisionRobot");
     DistanceFieldCacheEntryPtr new_dfce =
         generateDistanceFieldCacheEntry(group_name, state, acm, generate_distance_field);
-    boost::mutex::scoped_lock slock(update_cache_lock_);
+    std::scoped_lock slock(update_cache_lock_);
     (const_cast<CollisionEnvDistanceField*>(this))->distance_field_cache_entry_ = new_dfce;
     dfce = new_dfce;
   }

moveit_core/kinematics_base/CMakeLists.txt

@@ -15,7 +15,6 @@ ament_target_dependencies(
   srdfdom moveit_msgs
   geometric_shapes
   geometry_msgs
-  Boost
 )
 
 install(DIRECTORY include/ DESTINATION include)

moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h

@@ -39,10 +39,10 @@
 #include <geometry_msgs/msg/pose.hpp>
 #include <moveit_msgs/msg/move_it_error_codes.hpp>
 #include <moveit/macros/class_forward.h>
-#include <boost/function.hpp>
 #include <rclcpp/logging.hpp>
 #include <rclcpp/node.hpp>
 #include <string>
+#include <functional>
 
 #include "moveit_kinematics_base_export.h"
 
@@ -151,8 +151,8 @@ class MOVEIT_KINEMATICS_BASE_EXPORT KinematicsBase
   static const double DEFAULT_TIMEOUT;               /* = 1.0 */
 
   /** @brief Signature for a callback to validate an IK solution. Typically used for collision checking. */
-  using IKCallbackFn = boost::function<void(const geometry_msgs::msg::Pose&, const std::vector<double>&,
-                                            moveit_msgs::msg::MoveItErrorCodes&)>;
+  using IKCallbackFn = std::function<void(const geometry_msgs::msg::Pose&, const std::vector<double>&,
+                                          moveit_msgs::msg::MoveItErrorCodes&)>;
 
   /**
    * @brief Given a desired pose of the end-effector, compute the joint angles to reach it

moveit_core/kinematics_metrics/src/kinematics_metrics.cpp

@@ -40,6 +40,7 @@
 #include <Eigen/Dense>
 #include <Eigen/Eigenvalues>
 #include <boost/math/constants/constants.hpp>
+#include <limits>
 
 namespace kinematics_metrics
 {
@@ -48,7 +49,7 @@ static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_kinematics_metri
 double KinematicsMetrics::getJointLimitsPenalty(const moveit::core::RobotState& state,
                                                 const moveit::core::JointModelGroup* joint_model_group) const
 {
-  if (fabs(penalty_multiplier_) <= boost::math::tools::epsilon<double>())
+  if (fabs(penalty_multiplier_) <= std::numeric_limits<double>::min())
     return 1.0;
   double joint_limits_multiplier(1.0);
   const std::vector<const moveit::core::JointModel*>& joint_model_vector = joint_model_group->getJointModels();
@@ -88,7 +89,7 @@ double KinematicsMetrics::getJointLimitsPenalty(const moveit::core::RobotState&
     double lower_bound_distance = joint_model->distance(joint_values, &lower_bounds[0]);
     double upper_bound_distance = joint_model->distance(joint_values, &upper_bounds[0]);
     double range = lower_bound_distance + upper_bound_distance;
-    if (range <= boost::math::tools::epsilon<double>())
+    if (range <= std::numeric_limits<double>::min())
       continue;
     joint_limits_multiplier *= (lower_bound_distance * upper_bound_distance / (range * range));
   }

moveit_core/planning_interface/src/planning_interface.cpp

@@ -35,7 +35,7 @@
 /* Author: Ioan Sucan */
 
 #include <moveit/planning_interface/planning_interface.h>
-#include <boost/thread/mutex.hpp>
+#include <mutex>
 #include <rclcpp/logger.hpp>
 #include <rclcpp/logging.hpp>
 #include <set>
@@ -49,7 +49,7 @@ namespace
 // keep track of currently active contexts
 struct ActiveContexts
 {
-  boost::mutex mutex_;
+  std::mutex mutex_;
   std::set<PlanningContext*> contexts_;
 };
 
@@ -63,14 +63,14 @@ static ActiveContexts& getActiveContexts()
 PlanningContext::PlanningContext(const std::string& name, const std::string& group) : name_(name), group_(group)
 {
   ActiveContexts& ac = getActiveContexts();
-  boost::mutex::scoped_lock _(ac.mutex_);
+  std::scoped_lock _(ac.mutex_);
   ac.contexts_.insert(this);
 }
 
 PlanningContext::~PlanningContext()
 {
   ActiveContexts& ac = getActiveContexts();
-  boost::mutex::scoped_lock _(ac.mutex_);
+  std::scoped_lock _(ac.mutex_);
   ac.contexts_.erase(this);
 }
 
@@ -126,7 +126,7 @@ void PlannerManager::setPlannerConfigurations(const PlannerConfigurationMap& pcs
 void PlannerManager::terminate() const
 {
   ActiveContexts& ac = getActiveContexts();
-  boost::mutex::scoped_lock _(ac.mutex_);
+  std::scoped_lock _(ac.mutex_);
   for (PlanningContext* context : ac.contexts_)
     context->terminate();
 }