Removing more boost usage

moveit_core/collision_detection/CMakeLists.txt

@@ -24,7 +24,6 @@ ament_target_dependencies(${MOVEIT_LIB_NAME}
   visualization_msgs
   tf2_eigen
   geometric_shapes
-  Boost
   OCTOMAP
 )
 

moveit_core/collision_detection/src/collision_matrix.cpp

@@ -368,7 +368,7 @@ void AllowedCollisionMatrix::getMessage(moveit_msgs::msg::AllowedCollisionMatrix
   for (std::size_t i = 0; i < msg.entry_names.size(); ++i)
     msg.entry_values[i].enabled.resize(msg.entry_names.size(), false);
 
-  // there is an approximation here: if we use a boost function to decide
+  // there is an approximation here: if we use a function to decide
   // whether a collision is allowed or not, we just assume the collision is not allowed.
   for (std::size_t i = 0; i < msg.entry_names.size(); ++i)
   {

moveit_core/collision_detection/src/world.cpp

@@ -36,7 +36,6 @@
 
 #include <moveit/collision_detection/world.h>
 #include <geometric_shapes/check_isometry.h>
-#include <boost/algorithm/string/predicate.hpp>
 #include <rclcpp/logger.hpp>
 #include <rclcpp/logging.hpp>
 
@@ -175,7 +174,7 @@ 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
-      // name.rfind is in service of removing the call to boost::starts_with and does the same thing
+      // 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()] == '/')
       {
         auto it = object.second->global_subframe_poses_.find(name.substr(object.first.length() + 1));

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

@@ -51,10 +51,10 @@
 #include <moveit_msgs/msg/constraints.hpp>
 #include <moveit_msgs/msg/planning_scene_components.hpp>
 #include <octomap_msgs/msg/octomap_with_pose.hpp>
-#include <boost/noncopyable.hpp>
-#include <boost/concept_check.hpp>
 #include <memory>
 #include <functional>
+#include <thread>
+#include <variant>
 #include "rclcpp/rclcpp.hpp"
 
 #include "moveit_planning_scene_export.h"
@@ -86,10 +86,19 @@ using ObjectTypeMap = std::map<std::string, object_recognition_msgs::msg::Object
 /** \brief This class maintains the representation of the
     environment as seen by a planning instance. The environment
     geometry, the robot geometry and state are maintained. */
-class MOVEIT_PLANNING_SCENE_EXPORT PlanningScene : private boost::noncopyable,
-                                                   public std::enable_shared_from_this<PlanningScene>
+class MOVEIT_PLANNING_SCENE_EXPORT PlanningScene : public std::enable_shared_from_this<PlanningScene>
 {
 public:
+  /**
+   * @brief PlanningScene cannot be copy-constructed
+   */
+  PlanningScene(const PlanningScene&) = delete;
+
+  /**
+   * @brief PlanningScene cannot be copy-assigned
+   */
+  PlanningScene& operator=(const PlanningScene&) = delete;
+
   /** \brief construct using an existing RobotModel */
   PlanningScene(const moveit::core::RobotModelConstPtr& robot_model,
                 const collision_detection::WorldPtr& world = std::make_shared<collision_detection::World>());

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

@@ -43,6 +43,7 @@
 #include <srdfdom/model.h>
 #include <functional>
 #include <set>
+#include <string>
 
 namespace moveit
 {

moveit_core/robot_model/src/joint_model_group.cpp

@@ -39,7 +39,6 @@
 #include <moveit/robot_model/joint_model_group.h>
 #include <moveit/robot_model/revolute_joint_model.h>
 #include <moveit/exceptions/exceptions.h>
-#include <boost/lexical_cast.hpp>
 #include <rclcpp/logger.hpp>
 #include <rclcpp/logging.hpp>
 #include <algorithm>
@@ -369,9 +368,9 @@ void JointModelGroup::getVariableRandomPositionsNearBy(random_numbers::RandomNum
 {
   assert(active_joint_bounds.size() == active_joint_model_vector_.size());
   if (distances.size() != active_joint_model_vector_.size())
-    throw Exception("When sampling random values nearby for group '" + name_ + "', distances vector should be of size " +
-                    boost::lexical_cast<std::string>(active_joint_model_vector_.size()) + ", but it is of size " +
-                    boost::lexical_cast<std::string>(distances.size()));
+    throw Exception("When sampling random values nearby for group '" + name_ +
+                    "', distances vector should be of size " + std::to_string(active_joint_model_vector_.size()) +
+                    ", but it is of size " + std::to_string(distances.size()));
   for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
     active_joint_model_vector_[i]->getVariableRandomPositionsNearBy(rng, values + active_joint_model_start_index_[i],
                                                                     *active_joint_bounds[i],

moveit_core/robot_state/include/moveit/robot_state/cartesian_interpolator.h

@@ -40,8 +40,6 @@
 
 #include <moveit/robot_state/robot_state.h>
 
-#include <boost/assert.hpp>
-
 namespace moveit
 {
 namespace core

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

@@ -46,7 +46,6 @@
 #include <geometry_msgs/msg/twist.hpp>
 #include <cassert>
 
-#include <boost/assert.hpp>
 #include <rclcpp/duration.hpp>
 
 /* Terminology
@@ -1356,7 +1355,7 @@ class RobotState
     {
       throw Exception("Invalid link");
     }
-    BOOST_VERIFY(checkLinkTransforms());
+    assert(checkLinkTransforms());
     return global_link_transforms_[link->getLinkIndex()];
   }
 
@@ -1388,7 +1387,7 @@ class RobotState
 
   const Eigen::Isometry3d& getCollisionBodyTransform(const LinkModel* link, std::size_t index) const
   {
-    BOOST_VERIFY(checkCollisionTransforms());
+    assert(checkCollisionTransforms());
     return global_collision_body_transforms_[link->getFirstCollisionBodyTransformIndex() + index];
   }
 
@@ -1416,7 +1415,7 @@ class RobotState
 
   const Eigen::Isometry3d& getJointTransform(const JointModel* joint) const
   {
-    BOOST_VERIFY(checkJointTransforms(joint));
+    assert(checkJointTransforms(joint));
     return variable_joint_transforms_[joint->getJointIndex()];
   }
 

moveit_core/robot_state/src/attached_body.cpp

@@ -37,7 +37,6 @@
 #include <moveit/robot_state/attached_body.h>
 #include <geometric_shapes/check_isometry.h>
 #include <geometric_shapes/shapes.h>
-#include <boost/algorithm/string/predicate.hpp>
 
 namespace moveit
 {

moveit_core/robot_state/src/conversions.cpp

@@ -44,7 +44,7 @@
 #else
 #include <tf2_eigen/tf2_eigen.h>
 #endif
-#include <boost/lexical_cast.hpp>
+#include <string>
 
 namespace moveit
 {
@@ -560,7 +560,7 @@ void streamToRobotState(RobotState& state, const std::string& line, const std::s
     // Get a variable
     if (!std::getline(line_stream, cell, separator[0]))
       RCLCPP_ERROR(LOGGER, "Missing variable %s", state.getVariableNames()[i].c_str());
-    state.getVariablePositions()[i] = boost::lexical_cast<double>(cell.c_str());
+    state.getVariablePositions()[i] = std::stod(cell);
   }
 }
 

moveit_core/robot_state/src/robot_state.cpp

@@ -52,6 +52,7 @@
 #include <moveit/macros/console_colors.h>
 #include <functional>
 #include <moveit/robot_model/aabb.h>
+#include <cassert>
 
 namespace moveit
 {
@@ -1132,7 +1133,7 @@ const Eigen::Isometry3d& RobotState::getFrameInfo(const std::string& frame_id, c
   }
   if ((robot_link = robot_model_->getLinkModel(frame_id, &frame_found)))
   {
-    BOOST_VERIFY(checkLinkTransforms());
+    assert(checkLinkTransforms());
     return global_link_transforms_[robot_link->getLinkIndex()];
   }
   robot_link = nullptr;
@@ -1144,7 +1145,7 @@ const Eigen::Isometry3d& RobotState::getFrameInfo(const std::string& frame_id, c
     const Eigen::Isometry3d& transform = jt->second->getGlobalPose();
     robot_link = jt->second->getAttachedLink();
     frame_found = true;
-    BOOST_VERIFY(checkLinkTransforms());
+    assert(checkLinkTransforms());
     return transform;
   }
 
@@ -1155,7 +1156,7 @@ const Eigen::Isometry3d& RobotState::getFrameInfo(const std::string& frame_id, c
     if (frame_found)
     {
       robot_link = body.second->getAttachedLink();
-      BOOST_VERIFY(checkLinkTransforms());
+      assert(checkLinkTransforms());
       return transform;
     }
   }
@@ -1284,7 +1285,7 @@ bool RobotState::getJacobian(const JointModelGroup* group, const LinkModel* link
                              const Eigen::Vector3d& reference_point_position, Eigen::MatrixXd& jacobian,
                              bool use_quaternion_representation) const
 {
-  BOOST_VERIFY(checkLinkTransforms());
+  assert(checkLinkTransforms());
 
   if (!group->isChain())
   {
@@ -2049,7 +2050,7 @@ double RobotState::computeCartesianPath(const JointModelGroup* group, std::vecto
 
 void RobotState::computeAABB(std::vector<double>& aabb) const
 {
-  BOOST_VERIFY(checkLinkTransforms());
+  assert(checkLinkTransforms());
 
   core::AABB bounding_box;
   std::vector<const LinkModel*> links = robot_model_->getLinkModelsWithCollisionGeometry();

moveit_core/robot_state/test/test_aabb.cpp

@@ -39,6 +39,7 @@
 #include <moveit/robot_state/robot_state.h>
 #include <urdf_parser/urdf_parser.h>
 #include <fstream>
+#include <string>
 #include <gtest/gtest.h>
 #if __has_include(<tf2_geometry_msgs/tf2_geometry_msgs.hpp>)
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
@@ -240,7 +241,7 @@ TEST_F(TestAABB, TestPR2)
       aabb.extendWithTransformedBox(transforms[i], extents);
 
       // Publish AABB
-      msg->ns = (*it)->getName() + boost::lexical_cast<std::string>(i);
+      msg->ns = (*it)->getName() + std::to_string(i);
       msg->pose.position.x = transforms[i].translation()[0];
       msg->pose.position.y = transforms[i].translation()[1];
       msg->pose.position.z = transforms[i].translation()[2];

moveit_core/transforms/include/moveit/transforms/transforms.h

@@ -38,7 +38,6 @@
 
 #include <geometry_msgs/msg/transform_stamped.hpp>
 #include <Eigen/Geometry>
-#include <boost/noncopyable.hpp>
 #include <moveit/macros/class_forward.h>
 #include <map>
 
@@ -56,9 +55,19 @@ using FixedTransformsMap = std::map<std::string, Eigen::Isometry3d, std::less<st
 /** @brief Provides an implementation of a snapshot of a transform tree that can be easily queried for
     transforming different quantities. Transforms are maintained as a list of transforms to a particular frame.
     All stored transforms are considered fixed. */
-class Transforms : private boost::noncopyable
+class Transforms
 {
 public:
+  /**
+   * @brief Transforms cannot be copy-constructed
+   */
+  Transforms(const Transforms&) = delete;
+
+  /**
+   * @brief Transforms cannot be copy-assigned
+   */
+  Transforms& operator=(const Transforms&) = delete;
+
   /**
    * @brief Construct a transform list
    */

moveit_core/utils/src/robot_model_test_utils.cpp

@@ -34,7 +34,6 @@
 
 /* Author: Bryce Willey */
 
-#include <boost/algorithm/string_regex.hpp>
 #include <boost/math/constants/constants.hpp>
 #include <geometry_msgs/msg/pose.hpp>
 #include <urdf_parser/urdf_parser.h>
@@ -43,6 +42,7 @@
 #include <filesystem>
 #include <rclcpp/logger.hpp>
 #include <rclcpp/logging.hpp>
+#include <regex>
 
 namespace moveit
 {
@@ -117,8 +117,11 @@ RobotModelBuilder::RobotModelBuilder(const std::string& name, const std::string&
 void RobotModelBuilder::addChain(const std::string& section, const std::string& type,
                                  const std::vector<geometry_msgs::msg::Pose>& joint_origins, urdf::Vector3 joint_axis)
 {
-  std::vector<std::string> link_names;
-  boost::split_regex(link_names, section, boost::regex("->"));
+  // https://stackoverflow.com/questions/53849/how-do-i-tokenize-a-string-in-c
+  std::regex re("->");
+  auto const link_names = std::vector<std::string>(std::sregex_token_iterator{ begin(section), end(section), re, -1 },
+                                                   std::sregex_token_iterator{});
+
   if (link_names.empty())
   {
     RCLCPP_ERROR(LOGGER, "No links specified (empty section?)");

moveit_kinematics/cached_ik_kinematics_plugin/CMakeLists.txt

@@ -1,16 +1,13 @@
 find_package(trac_ik_kinematics_plugin QUIET)
 find_package(ur_kinematics QUIET)
 
-find_package(Boost COMPONENTS filesystem program_options REQUIRED)
-
 set(MOVEIT_LIB_NAME moveit_cached_ik_kinematics_base)
 add_library(${MOVEIT_LIB_NAME} SHARED src/ik_cache.cpp)
 set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
 ament_target_dependencies(${MOVEIT_LIB_NAME}
   rclcpp
   moveit_core
   moveit_msgs
-  Boost
 )
 
 if(trac_ik_kinematics_plugin_FOUND)

moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/detail/GreedyKCenters.h

@@ -38,9 +38,9 @@
 
 #pragma once
 
-#include <boost/numeric/ublas/matrix.hpp>
 #include <functional>
 #include <random>
+#include <eigen3/Eigen/Core>
 
 namespace cached_ik_kinematics_plugin
 {
@@ -54,7 +54,7 @@ class GreedyKCenters
   /** \brief The definition of a distance function */
   using DistanceFunction = std::function<double(const _T&, const _T&)>;
   /** \brief A matrix type for storing distances between points and centers */
-  using Matrix = boost::numeric::ublas::matrix<double>;
+  using Matrix = Eigen::MatrixXd;
 
   GreedyKCenters() = default;
 
@@ -88,8 +88,8 @@ class GreedyKCenters
 
     centers.clear();
     centers.reserve(k);
-    if (dists.size1() < data.size() || dists.size2() < k)
-      dists.resize(std::max(2 * dists.size1() + 1, data.size()), k, false);
+    if (((long unsigned int)dists.rows()) < data.size() || ((long unsigned int)dists.cols()) < k)
+      dists.resize(std::max(2 * ((long unsigned int)dists.rows()) + 1, data.size()), k);
     // first center is picked randomly
     centers.push_back(std::uniform_int_distribution<size_t>{ 0, data.size() - 1 }(generator_));
     for (unsigned i = 1; i < k; ++i)

moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/detail/NearestNeighborsGNAT.h

@@ -45,6 +45,7 @@
 #include <utility>
 #include "GreedyKCenters.h"
 #include "NearestNeighbors.h"
+#include <iostream>
 
 namespace cached_ik_kinematics_plugin
 {