moveit · AndyZe · Jun 17, 2022 · Jun 7, 2022 · Jun 8, 2022 · Jun 8, 2022
diff --git a/moveit_core/collision_detection/CMakeLists.txt b/moveit_core/collision_detection/CMakeLists.txt
@@ -24,7 +24,6 @@ ament_target_dependencies(${MOVEIT_LIB_NAME}
   visualization_msgs
   tf2_eigen
   geometric_shapes
-  Boost
   OCTOMAP
 )
 

diff --git a/moveit_core/collision_detection/src/collision_matrix.cpp b/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)
   {

diff --git a/moveit_core/collision_detection/src/world.cpp b/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));

diff --git a/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp b/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp
@@ -40,7 +40,6 @@
 #include <moveit/robot_state/conversions.h>
 #include <moveit/collision_detection_fcl/collision_env_fcl.h>
 #include <geometric_shapes/check_isometry.h>
-#include <boost/math/constants/constants.hpp>
 #include <rclcpp/logger.hpp>
 #include <rclcpp/logging.hpp>
 #include <rclcpp/time.hpp>
@@ -51,6 +50,7 @@
 #endif
 #include <functional>
 #include <limits>
+#include <math.h>
 #include <memory>
 #include <typeinfo>
 
@@ -64,11 +64,11 @@ static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_kinematic_constr
 
 static double normalizeAngle(double angle)
 {
-  double v = fmod(angle, 2.0 * boost::math::constants::pi<double>());
-  if (v < -boost::math::constants::pi<double>())
-    v += 2.0 * boost::math::constants::pi<double>();
-  else if (v > boost::math::constants::pi<double>())
-    v -= 2.0 * boost::math::constants::pi<double>();
+  double v = fmod(angle, 2.0 * M_PI);
+  if (v < -M_PI)
+    v += 2.0 * M_PI;
+  else if (v > M_PI)
+    v -= 2.0 * M_PI;
   return v;
 }
 
@@ -287,10 +287,10 @@ ConstraintEvaluationResult JointConstraint::decide(const moveit::core::RobotStat
   {
     dif = normalizeAngle(current_joint_position) - joint_position_;
 
-    if (dif > boost::math::constants::pi<double>())
-      dif = 2.0 * boost::math::constants::pi<double>() - dif;
-    else if (dif < -boost::math::constants::pi<double>())
-      dif += 2.0 * boost::math::constants::pi<double>();  // we include a sign change to have dif > 0
+    if (dif > M_PI)
+      dif = 2.0 * M_PI - dif;
+    else if (dif < -M_PI)
+      dif += 2.0 * M_PI;  // we include a sign change to have dif > 0
   }
   else
     dif = current_joint_position - joint_position_;
@@ -803,7 +803,7 @@ bool VisibilityConstraint::configure(const moveit_msgs::msg::VisibilityConstrain
 
   // compute the points on the base circle of the cone that make up the cone sides
   points_.clear();
-  double delta = 2.0 * boost::math::constants::pi<double>() / (double)cone_sides_;
+  double delta = 2.0 * M_PI / (double)cone_sides_;
   double a = 0.0;
   for (unsigned int i = 0; i < cone_sides_; ++i, a += delta)
   {

diff --git a/moveit_core/kinematic_constraints/test/test_constraints.cpp b/moveit_core/kinematic_constraints/test/test_constraints.cpp
@@ -43,8 +43,8 @@
 #else
 #include <tf2_eigen/tf2_eigen.h>
 #endif
+#include <math.h>
 #include <moveit/utils/robot_model_test_utils.h>
-#include <boost/math/constants/constants.hpp>
 
 class LoadPlanningModelsPr2 : public testing::Test
 {
@@ -659,7 +659,7 @@ TEST_F(LoadPlanningModelsPr2, OrientationConstraintsSimple)
   EXPECT_FALSE(oc.decide(robot_state).satisfied);
 
   // rotation by pi does not wrap to zero
-  jvals["r_wrist_roll_joint"] = boost::math::constants::pi<double>();
+  jvals["r_wrist_roll_joint"] = M_PI;
   robot_state.setVariablePositions(jvals);
   robot_state.update();
   EXPECT_FALSE(oc.decide(robot_state).satisfied);

diff --git a/moveit_core/kinematic_constraints/test/test_orientation_constraints.cpp b/moveit_core/kinematic_constraints/test/test_orientation_constraints.cpp
@@ -45,7 +45,7 @@
 #include <tf2_eigen/tf2_eigen.h>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 #endif
-#include <boost/math/constants/constants.hpp>
+#include <math.h>
 
 #include <moveit/utils/robot_model_test_utils.h>
 
@@ -202,24 +202,24 @@ TEST_F(SphericalRobot, Test2)
   moveit::core::RobotState robot_state(robot_model_);
   // Singularity: roll + yaw = theta
   // These violate either absolute_x_axis_tolerance or absolute_z_axis_tolerance
-  robot_state.setVariablePositions(getJointValues(0.15, boost::math::constants::half_pi<double>(), 0.15));
+  robot_state.setVariablePositions(getJointValues(0.15, M_PI_2, 0.15));
   robot_state.update();
   EXPECT_TRUE(oc.configure(ocm, tf));
   EXPECT_FALSE(oc.decide(robot_state).satisfied);
 
-  robot_state.setVariablePositions(getJointValues(0.21, boost::math::constants::half_pi<double>(), 0.0));
+  robot_state.setVariablePositions(getJointValues(0.21, M_PI_2, 0.0));
   robot_state.update();
   EXPECT_TRUE(oc.configure(ocm, tf));
   EXPECT_FALSE(oc.decide(robot_state).satisfied);
 
-  robot_state.setVariablePositions(getJointValues(0.0, boost::math::constants::half_pi<double>(), 0.21));
+  robot_state.setVariablePositions(getJointValues(0.0, M_PI_2, 0.21));
   robot_state.update();
   EXPECT_TRUE(oc.configure(ocm, tf));
   EXPECT_FALSE(oc.decide(robot_state).satisfied);
 
   // Singularity: roll - yaw = theta
   // This's identical to -pi/2 pitch rotation
-  robot_state.setVariablePositions(getJointValues(0.15, -boost::math::constants::half_pi<double>(), 0.15));
+  robot_state.setVariablePositions(getJointValues(0.15, -M_PI_2, 0.15));
   robot_state.update();
 
   EXPECT_TRUE(oc.configure(ocm, tf));
@@ -248,12 +248,12 @@ TEST_F(SphericalRobot, Test3)
   // Singularity: roll + yaw = theta
 
   // These tests violate absolute_x_axis_tolerance
-  robot_state.setVariablePositions(getJointValues(0.21, boost::math::constants::half_pi<double>(), 0.0));
+  robot_state.setVariablePositions(getJointValues(0.21, M_PI_2, 0.0));
   robot_state.update();
   EXPECT_TRUE(oc.configure(ocm, tf));
   EXPECT_FALSE(oc.decide(robot_state).satisfied);
 
-  robot_state.setVariablePositions(getJointValues(0.0, boost::math::constants::half_pi<double>(), 0.21));
+  robot_state.setVariablePositions(getJointValues(0.0, M_PI_2, 0.21));
   robot_state.update();
   EXPECT_TRUE(oc.configure(ocm, tf));
   EXPECT_FALSE(oc.decide(robot_state).satisfied);
@@ -263,12 +263,12 @@ TEST_F(SphericalRobot, Test3)
   ocm.absolute_z_axis_tolerance = 0.2;
 
   // These tests violate absolute_z_axis_tolerance
-  robot_state.setVariablePositions(getJointValues(0.21, boost::math::constants::half_pi<double>(), 0.0));
+  robot_state.setVariablePositions(getJointValues(0.21, M_PI_2, 0.0));
   robot_state.update();
   EXPECT_TRUE(oc.configure(ocm, tf));
   EXPECT_FALSE(oc.decide(robot_state).satisfied);
 
-  robot_state.setVariablePositions(getJointValues(0.0, boost::math::constants::half_pi<double>(), 0.21));
+  robot_state.setVariablePositions(getJointValues(0.0, M_PI_2, 0.21));
   robot_state.update();
   EXPECT_TRUE(oc.configure(ocm, tf));
   EXPECT_FALSE(oc.decide(robot_state).satisfied);
@@ -296,12 +296,12 @@ TEST_F(SphericalRobot, Test4)
   // Singularity: roll + yaw = theta
 
   // These tests violate absolute_x_axis_tolerance
-  robot_state.setVariablePositions(getJointValues(0.21, -boost::math::constants::half_pi<double>(), 0.0));
+  robot_state.setVariablePositions(getJointValues(0.21, -M_PI_2, 0.0));
   robot_state.update();
   EXPECT_TRUE(oc.configure(ocm, tf));
   EXPECT_FALSE(oc.decide(robot_state).satisfied);
 
-  robot_state.setVariablePositions(getJointValues(0.0, -boost::math::constants::half_pi<double>(), 0.21));
+  robot_state.setVariablePositions(getJointValues(0.0, -M_PI_2, 0.21));
   robot_state.update();
   EXPECT_TRUE(oc.configure(ocm, tf));
   EXPECT_FALSE(oc.decide(robot_state).satisfied);
@@ -311,17 +311,17 @@ TEST_F(SphericalRobot, Test4)
   ocm.absolute_z_axis_tolerance = 0.2;
 
   // These tests violate absolute_z_axis_tolerance
-  robot_state.setVariablePositions(getJointValues(0.21, -boost::math::constants::half_pi<double>(), 0.0));
+  robot_state.setVariablePositions(getJointValues(0.21, -M_PI_2, 0.0));
   robot_state.update();
   EXPECT_TRUE(oc.configure(ocm, tf));
   EXPECT_FALSE(oc.decide(robot_state).satisfied);
 
-  robot_state.setVariablePositions(getJointValues(0.0, -boost::math::constants::half_pi<double>(), 0.21));
+  robot_state.setVariablePositions(getJointValues(0.0, -M_PI_2, 0.21));
   robot_state.update();
   EXPECT_TRUE(oc.configure(ocm, tf));
   EXPECT_FALSE(oc.decide(robot_state).satisfied);
 
-  robot_state.setVariablePositions(getJointValues(0.5, -boost::math::constants::half_pi<double>(), 0.21));
+  robot_state.setVariablePositions(getJointValues(0.5, -M_PI_2, 0.21));
   robot_state.update();
   EXPECT_TRUE(oc.configure(ocm, tf));
   EXPECT_FALSE(oc.decide(robot_state).satisfied);
@@ -336,7 +336,7 @@ TEST_F(SphericalRobot, Test5)
   moveit_msgs::msg::OrientationConstraint ocm;
 
   moveit::core::RobotState robot_state(robot_model_);
-  robot_state.setVariablePositions(getJointValues(0.0, boost::math::constants::half_pi<double>(), 0.0));
+  robot_state.setVariablePositions(getJointValues(0.0, M_PI_2, 0.0));
   robot_state.update();
 
   ocm.link_name = "yaw";
@@ -347,7 +347,7 @@ TEST_F(SphericalRobot, Test5)
   ocm.absolute_z_axis_tolerance = 1.0;
   ocm.weight = 1.0;
 
-  robot_state.setVariablePositions(getJointValues(0.2, boost::math::constants::half_pi<double>(), 0.3));
+  robot_state.setVariablePositions(getJointValues(0.2, M_PI_2, 0.3));
   robot_state.update();
 
   EXPECT_TRUE(oc.configure(ocm, tf));

diff --git a/moveit_core/kinematics_metrics/src/kinematics_metrics.cpp b/moveit_core/kinematics_metrics/src/kinematics_metrics.cpp
@@ -34,13 +34,13 @@
 
 /* Author: Sachin Chitta */
 
-#include <moveit/kinematics_metrics/kinematics_metrics.h>
-#include <rclcpp/logger.hpp>
-#include <rclcpp/logging.hpp>
 #include <Eigen/Dense>
 #include <Eigen/Eigenvalues>
-#include <boost/math/constants/constants.hpp>
 #include <limits>
+#include <math.h>
+#include <moveit/kinematics_metrics/kinematics_metrics.h>
+#include <rclcpp/logger.hpp>
+#include <rclcpp/logging.hpp>
 
 namespace kinematics_metrics
 {
@@ -68,9 +68,8 @@ double KinematicsMetrics::getJointLimitsPenalty(const moveit::core::RobotState&
       if (bounds[0].min_position_ == -std::numeric_limits<double>::max() ||
           bounds[0].max_position_ == std::numeric_limits<double>::max() ||
           bounds[1].min_position_ == -std::numeric_limits<double>::max() ||
-          bounds[1].max_position_ == std::numeric_limits<double>::max() ||
-          bounds[2].min_position_ == -boost::math::constants::pi<double>() ||
-          bounds[2].max_position_ == boost::math::constants::pi<double>())
+          bounds[1].max_position_ == std::numeric_limits<double>::max() || bounds[2].min_position_ == -M_PI ||
+          bounds[2].max_position_ == M_PI)
         continue;
     }
     if (joint_model->getType() == moveit::core::JointModel::FLOATING)

diff --git a/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h b/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>());

diff --git a/moveit_core/robot_model/CMakeLists.txt b/moveit_core/robot_model/CMakeLists.txt
@@ -34,7 +34,6 @@ if(BUILD_TESTING)
   ament_add_gtest(test_robot_model test/test.cpp)
   ament_target_dependencies(test_robot_model
     rclcpp
-    Boost
   )
   target_link_libraries(test_robot_model moveit_test_utils ${MOVEIT_LIB_NAME})
 endif()

diff --git a/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h b/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
 {

diff --git a/moveit_core/robot_model/src/floating_joint_model.cpp b/moveit_core/robot_model/src/floating_joint_model.cpp
@@ -35,13 +35,12 @@
 
 /* Author: Ioan Sucan */
 
-#include <moveit/robot_model/floating_joint_model.h>
+#include <cmath>
 #include <geometric_shapes/check_isometry.h>
-#include <boost/math/constants/constants.hpp>
+#include <limits>
+#include <moveit/robot_model/floating_joint_model.h>
 #include <rclcpp/logger.hpp>
 #include <rclcpp/logging.hpp>
-#include <limits>
-#include <cmath>
 
 namespace moveit
 {
@@ -104,7 +103,7 @@ double FloatingJointModel::getMaximumExtent(const Bounds& other_bounds) const
   double dx = other_bounds[0].max_position_ - other_bounds[0].min_position_;
   double dy = other_bounds[1].max_position_ - other_bounds[1].min_position_;
   double dz = other_bounds[2].max_position_ - other_bounds[2].min_position_;
-  return sqrt(dx * dx + dy * dy + dz * dz) + boost::math::constants::pi<double>() * 0.5 * angular_distance_weight_;
+  return sqrt(dx * dx + dy * dy + dz * dz) + M_PI * 0.5 * angular_distance_weight_;
 }
 
 double FloatingJointModel::distance(const double* values1, const double* values2) const
@@ -312,7 +311,7 @@ void FloatingJointModel::getVariableRandomPositionsNearBy(random_numbers::Random
                                 std::min(bounds[2].max_position_, near[2] + distance));
 
   double da = angular_distance_weight_ * distance;
-  if (da >= .25 * boost::math::constants::pi<double>())
+  if (da >= .25 * M_PI)
   {
     double q[4];
     rng.quaternion(q);

diff --git a/moveit_core/robot_model/src/joint_model_group.cpp b/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],