Removing some boost usage

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

@@ -36,8 +36,6 @@
 
 #pragma once
 
-#include <boost/array.hpp>
-#include <boost/function.hpp>
 #include <vector>
 #include <string>
 #include <map>
@@ -110,10 +108,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 +218,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,10 @@
 
 #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>
 
 namespace collision_detection
 {
@@ -86,8 +84,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 +104,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,6 @@
 #include <string>
 #include <vector>
 #include <map>
-#include <boost/function.hpp>
 #include <Eigen/Geometry>
 #include <eigen_stl_containers/eigen_stl_vector_container.h>
 #include <moveit/transforms/transforms.h>
@@ -298,7 +297,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/world.cpp

@@ -139,7 +139,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()] == '/')
+      // 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()] == '/')
       {
         return object.second->global_subframe_poses_.find(name.substr(object.first.length() + 1)) !=
                object.second->global_subframe_poses_.end();
@@ -173,7 +174,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_fcl/src/collision_common.cpp

@@ -47,7 +47,6 @@
 #include <fcl/octree.h>
 #endif
 
-#include <boost/thread/mutex.hpp>
 #include <memory>
 #include <type_traits>
 

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

@@ -41,7 +41,6 @@
 #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"
 
 namespace collision_detection
@@ -296,14 +295,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

@@ -36,7 +36,6 @@
 
 #include "rclcpp/rclcpp.hpp"
 #include <moveit/collision_distance_field/collision_common_distance_field.h>
-#include <boost/thread/mutex.hpp>
 #if __has_include(<tf2_eigen/tf2_eigen.hpp>)
 #include <tf2_eigen/tf2_eigen.hpp>
 #else
@@ -59,7 +58,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()
@@ -74,7 +73,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())
     {
@@ -84,7 +83,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

@@ -166,7 +166,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/include/moveit/kinematics_base/kinematics_base.h

@@ -40,7 +40,6 @@
 #include <moveit_msgs/msg/move_it_error_codes.hpp>
 #include <moveit/macros/class_forward.h>
 #include "rclcpp/rclcpp.hpp"
-#include <boost/function.hpp>
 #include <string>
 
 #include "moveit_kinematics_base_export.h"
@@ -150,8 +149,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

@@ -47,7 +47,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();
@@ -87,7 +87,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,6 @@
 /* Author: Ioan Sucan */
 
 #include <moveit/planning_interface/planning_interface.h>
-#include <boost/thread/mutex.hpp>
 #include <set>
 
 namespace planning_interface
@@ -47,7 +46,7 @@ namespace
 // keep track of currently active contexts
 struct ActiveContexts
 {
-  boost::mutex mutex_;
+  std::mutex mutex_;
   std::set<PlanningContext*> contexts_;
 };
 
@@ -61,14 +60,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);
 }
 
@@ -124,7 +123,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();
 }

moveit_core/planning_request_adapter/include/moveit/planning_request_adapter/planning_request_adapter.h

@@ -39,7 +39,6 @@
 #include <moveit/macros/class_forward.h>
 #include <moveit/planning_interface/planning_interface.h>
 #include <moveit/planning_scene/planning_scene.h>
-#include <boost/function.hpp>
 
 /** \brief Generic interface to adapting motion planning requests */
 namespace planning_request_adapter
@@ -50,8 +49,8 @@ class PlanningRequestAdapter
 {
 public:
   using PlannerFn =
-      boost::function<bool(const planning_scene::PlanningSceneConstPtr&, const planning_interface::MotionPlanRequest&,
-                           planning_interface::MotionPlanResponse&)>;
+      std::function<bool(const planning_scene::PlanningSceneConstPtr&, const planning_interface::MotionPlanRequest&,
+                         planning_interface::MotionPlanResponse&)>;
 
   PlanningRequestAdapter()
   {

moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h

@@ -52,7 +52,6 @@
 #include <moveit_msgs/msg/planning_scene_components.hpp>
 #include <octomap_msgs/msg/octomap_with_pose.hpp>
 #include <boost/noncopyable.hpp>
-#include <boost/function.hpp>
 #include <boost/concept_check.hpp>
 #include <memory>
 #include "rclcpp/rclcpp.hpp"
@@ -68,15 +67,14 @@ MOVEIT_CLASS_FORWARD(PlanningScene);  // Defines PlanningScenePtr, ConstPtr, Wea
    respecting constraints and collision avoidance).
     The first argument is the state to check the feasibility for, the second one is whether the check should be verbose
    or not. */
-typedef boost::function<bool(const moveit::core::RobotState&, bool)> StateFeasibilityFn;
+typedef std::function<bool(const moveit::core::RobotState&, bool)> StateFeasibilityFn;
 
 /** \brief This is the function signature for additional feasibility checks to be imposed on motions segments between
    states (in addition to respecting constraints and collision avoidance).
     The order of the arguments matters: the notion of feasibility is to be checked for motion segments that start at the
    first state and end at the second state. The third argument indicates
     whether the check should be verbose or not. */
-using MotionFeasibilityFn =
-    boost::function<bool(const moveit::core::RobotState&, const moveit::core::RobotState&, bool)>;
+using MotionFeasibilityFn = std::function<bool(const moveit::core::RobotState&, const moveit::core::RobotState&, bool)>;
 
 /** \brief A map from object names (e.g., attached bodies, collision objects) to their colors */
 using ObjectColorMap = std::map<std::string, std_msgs::msg::ColorRGBA>;

moveit_core/planning_scene/test/test_collision_objects.cpp

@@ -42,7 +42,6 @@
 #include <fstream>
 #include <sstream>
 #include <string>
-#include <boost/filesystem/path.hpp>
 
 #if __has_include(<tf2_eigen/tf2_eigen.hpp>)
 #include <tf2_eigen/tf2_eigen.hpp>

moveit_core/planning_scene/test/test_planning_scene.cpp

@@ -43,7 +43,6 @@
 #include <fstream>
 #include <sstream>
 #include <string>
-#include <boost/filesystem/path.hpp>
 #include <tf2_eigen/tf2_eigen.hpp>
 
 #include <moveit/collision_detection/collision_common.h>

moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h

@@ -41,7 +41,6 @@
 #include <moveit/robot_model/link_model.h>
 #include <moveit/kinematics_base/kinematics_base.h>
 #include <srdfdom/model.h>
-#include <boost/function.hpp>
 #include <set>
 
 namespace moveit
@@ -52,7 +51,7 @@ class RobotModel;
 class JointModelGroup;
 
 /** \brief Function type that allocates a kinematics solver for a particular group */
-typedef boost::function<kinematics::KinematicsBasePtr(const JointModelGroup*)> SolverAllocatorFn;
+typedef std::function<kinematics::KinematicsBasePtr(const JointModelGroup*)> SolverAllocatorFn;
 
 /** \brief Map from group instances to allocator functions & bijections */
 using SolverAllocatorMapFn = std::map<const JointModelGroup*, SolverAllocatorFn>;

moveit_core/robot_model/test/test.cpp

@@ -38,7 +38,6 @@
 #include <urdf_parser/urdf_parser.h>
 #include <fstream>
 #include <gtest/gtest.h>
-#include <boost/filesystem/path.hpp>
 
 #include <moveit/utils/robot_model_test_utils.h>
 

moveit_core/robot_state/include/moveit/robot_state/attached_body.h

@@ -40,7 +40,6 @@
 #include <moveit/transforms/transforms.h>
 #include <geometric_shapes/check_isometry.h>
 #include <eigen_stl_containers/eigen_stl_containers.h>
-#include <boost/function.hpp>
 #include <trajectory_msgs/msg/joint_trajectory.hpp>
 #include <set>
 
@@ -49,7 +48,7 @@ namespace moveit
 namespace core
 {
 class AttachedBody;
-typedef boost::function<void(AttachedBody* body, bool attached)> AttachedBodyCallback;
+typedef std::function<void(AttachedBody* body, bool attached)> AttachedBodyCallback;
 
 /** @brief Object defining bodies that can be attached to robot links.
  *