From 840887c837208cc0280eaca487c7823d564591bf Mon Sep 17 00:00:00 2001 From: Henry Moore <44307180+henrygerardmoore@users.noreply.github.com> Date: Fri, 17 Jun 2022 10:30:24 -0600 Subject: [PATCH] Removing more boost usage (#1372) * initial pass on removing boost * removing boost filesystem usage * removing boost and changing calls to work without boost * got everything to build and fixed locked robot state test * made thread joining safer * remove install and log * auto formatting * fixed clang format * changes solution callback empty check to work with std::function * removed unnecessary comment * removing unnecessary boost inclusions * changed comment * removing forward declarations * removed reliance on indirect includes * final changes, removing unnecessary boost includes from CMakeLists * removed now-unnecessary package * added direct includes * fixed formatting * added back mistakenly removed cmake command * removing unnecessary includes thanks to Abi's review * resolved rebase conflicts * updated std::optional usage * changing variant usage to be consistent with std * fixed outdated calls to boost * updating comments and adding/removing includes * added commit and removed comment * change to std asserts * removing unnecessary boost usage * removed boost regex * fixed bug in string tokenizing * re-adding boost serializer * further boost removal * switching to eigen * fixing std rng usage * removing unnecessary includes * re-adding removed include * removing more includes * removing more includes * removing unnecessary includes * removing unnecessary includes * switched back to boost version * sorting includes alphabetically * changing to cmath constants instead of boost constants Co-authored-by: Vatan Aksoy Tezer --- .../collision_detection/CMakeLists.txt | 1 - .../src/collision_matrix.cpp | 2 +- moveit_core/collision_detection/src/world.cpp | 3 +- .../src/kinematic_constraint.cpp | 22 ++--- .../test/test_constraints.cpp | 4 +- .../test/test_orientation_constraints.cpp | 32 ++++---- .../src/kinematics_metrics.cpp | 13 ++- .../moveit/planning_scene/planning_scene.h | 17 +++- moveit_core/robot_model/CMakeLists.txt | 1 - .../moveit/robot_model/joint_model_group.h | 1 + .../robot_model/src/floating_joint_model.cpp | 11 ++- .../robot_model/src/joint_model_group.cpp | 7 +- .../robot_model/src/planar_joint_model.cpp | 48 ++++++----- .../robot_model/src/revolute_joint_model.cpp | 39 +++++---- moveit_core/robot_model/src/robot_model.cpp | 1 - .../robot_state/cartesian_interpolator.h | 2 - .../include/moveit/robot_state/robot_state.h | 7 +- moveit_core/robot_state/src/attached_body.cpp | 1 - moveit_core/robot_state/src/conversions.cpp | 4 +- moveit_core/robot_state/src/robot_state.cpp | 19 ++--- moveit_core/robot_state/test/test_aabb.cpp | 3 +- moveit_core/robot_trajectory/CMakeLists.txt | 1 - .../robot_trajectory/src/robot_trajectory.cpp | 18 ++--- .../include/moveit/transforms/transforms.h | 13 ++- .../utils/src/robot_model_test_utils.cpp | 5 +- .../CMakeLists.txt | 3 - .../detail/GreedyKCenters.h | 8 +- .../detail/NearestNeighborsGNAT.h | 1 + .../multivariate_gaussian.h | 20 ++--- .../detail/constraints_library.h | 1 + .../command_list_manager.h | 5 +- .../src/command_list_manager.cpp | 4 +- .../cartesianconfiguration.h | 18 ++--- .../command_types_typedef.h | 8 +- .../sequence.h | 7 +- .../src/sequence.cpp | 9 +-- .../src/xml_testdata_loader.cpp | 1 - .../benchmarks/src/benchmark_execution.cpp | 81 ++++++++++--------- .../benchmarks/src/BenchmarkExecutor.cpp | 16 ++-- .../include/moveit_servo/pose_tracking.h | 6 +- moveit_ros/moveit_servo/src/pose_tracking.cpp | 3 +- .../CMakeLists.txt | 2 - .../perception/mesh_filter/CMakeLists.txt | 1 - .../mesh_filter/test/mesh_filter_test.cpp | 4 +- .../pointcloud_octomap_updater/CMakeLists.txt | 2 - .../moveit/semantic_world/semantic_world.h | 2 - .../planning_scene_monitor.h | 13 ++- .../src/wrap_python_move_group.cpp | 1 + moveit_ros/robot_interaction/CMakeLists.txt | 7 +- .../robot_interaction/ConfigExtras.cmake | 3 - .../src/interaction_handler.cpp | 3 +- .../src/interactive_marker_helpers.cpp | 7 +- .../src/kinematic_options.cpp | 9 +-- .../src/robot_interaction.cpp | 6 +- .../planning_scene_rviz_plugin/CMakeLists.txt | 1 - .../background_processing.hpp | 13 ++- .../src/mesh_shape.cpp | 7 +- .../src/render_shapes.cpp | 8 +- .../src/trajectory_visualization.cpp | 11 +-- .../warehouse/src/initialize_demo_db.cpp | 4 +- .../warehouse/src/moveit_message_storage.cpp | 8 +- .../warehouse/src/planning_scene_storage.cpp | 8 +- .../warehouse/src/save_to_warehouse.cpp | 4 +- .../src/compute_default_collisions.cpp | 18 ++--- .../src/planning_groups_widget.cpp | 8 +- 65 files changed, 300 insertions(+), 316 deletions(-) delete mode 100644 moveit_ros/robot_interaction/ConfigExtras.cmake diff --git a/moveit_core/collision_detection/CMakeLists.txt b/moveit_core/collision_detection/CMakeLists.txt index 73685def49f..60ae3c713cf 100644 --- 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 index 77e601640e8..8de60b1f96d 100644 --- 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 index 1cb5baa66de..713b2997ab5 100644 --- a/moveit_core/collision_detection/src/world.cpp +++ b/moveit_core/collision_detection/src/world.cpp @@ -36,7 +36,6 @@ #include #include -#include #include #include @@ -175,7 +174,7 @@ const Eigen::Isometry3d& World::getTransform(const std::string& name, bool& fram for (const std::pair& 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 index a36d7038b4c..35263f059b0 100644 --- a/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp +++ b/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp @@ -40,7 +40,6 @@ #include #include #include -#include #include #include #include @@ -51,6 +50,7 @@ #endif #include #include +#include #include #include @@ -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()); - if (v < -boost::math::constants::pi()) - v += 2.0 * boost::math::constants::pi(); - else if (v > boost::math::constants::pi()) - v -= 2.0 * boost::math::constants::pi(); + 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()) - dif = 2.0 * boost::math::constants::pi() - dif; - else if (dif < -boost::math::constants::pi()) - dif += 2.0 * boost::math::constants::pi(); // 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)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 index c0535f233c4..8ee047a45c1 100644 --- a/moveit_core/kinematic_constraints/test/test_constraints.cpp +++ b/moveit_core/kinematic_constraints/test/test_constraints.cpp @@ -43,8 +43,8 @@ #else #include #endif +#include #include -#include 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(); + 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 index 2eac0842469..5cd7c02923a 100644 --- 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 #include #endif -#include +#include #include @@ -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(), 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(), 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(), 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(), 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(), 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(), 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(), 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(), 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(), 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(), 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(), 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(), 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(), 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(), 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(), 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 index 4c482af055f..a45829a177b 100644 --- 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 -#include -#include #include #include -#include #include +#include +#include +#include +#include namespace kinematics_metrics { @@ -68,9 +68,8 @@ double KinematicsMetrics::getJointLimitsPenalty(const moveit::core::RobotState& if (bounds[0].min_position_ == -std::numeric_limits::max() || bounds[0].max_position_ == std::numeric_limits::max() || bounds[1].min_position_ == -std::numeric_limits::max() || - bounds[1].max_position_ == std::numeric_limits::max() || - bounds[2].min_position_ == -boost::math::constants::pi() || - bounds[2].max_position_ == boost::math::constants::pi()) + bounds[1].max_position_ == std::numeric_limits::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 index 49d49a4ed10..339ec3b78ff 100644 --- 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 #include #include -#include -#include #include #include +#include +#include #include "rclcpp/rclcpp.hpp" #include "moveit_planning_scene_export.h" @@ -86,10 +86,19 @@ using ObjectTypeMap = std::map +class MOVEIT_PLANNING_SCENE_EXPORT PlanningScene : public std::enable_shared_from_this { 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()); diff --git a/moveit_core/robot_model/CMakeLists.txt b/moveit_core/robot_model/CMakeLists.txt index 1980af26359..05da550fd6a 100644 --- 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 index c7a1759bfb1..617adf33042 100644 --- 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 #include #include +#include namespace moveit { diff --git a/moveit_core/robot_model/src/floating_joint_model.cpp b/moveit_core/robot_model/src/floating_joint_model.cpp index 8bbe43c6542..f44a6e663f6 100644 --- 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 +#include #include -#include +#include +#include #include #include -#include -#include 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() * 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()) + 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 index 808025517ec..dd0db3c36d7 100644 --- 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 #include #include -#include #include #include #include @@ -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(active_joint_model_vector_.size()) + ", but it is of size " + - boost::lexical_cast(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], diff --git a/moveit_core/robot_model/src/planar_joint_model.cpp b/moveit_core/robot_model/src/planar_joint_model.cpp index 46784b5ed3f..8815c2fef3f 100644 --- a/moveit_core/robot_model/src/planar_joint_model.cpp +++ b/moveit_core/robot_model/src/planar_joint_model.cpp @@ -35,12 +35,11 @@ /* Author: Ioan Sucan */ -#include -#include #include -#include -#include #include +#include +#include +#include namespace moveit { @@ -72,8 +71,8 @@ PlanarJointModel::PlanarJointModel(const std::string& name, size_t joint_index, variable_bounds_[0].max_position_ = std::numeric_limits::infinity(); variable_bounds_[1].min_position_ = -std::numeric_limits::infinity(); variable_bounds_[1].max_position_ = std::numeric_limits::infinity(); - variable_bounds_[2].min_position_ = -boost::math::constants::pi(); - variable_bounds_[2].max_position_ = boost::math::constants::pi(); + variable_bounds_[2].min_position_ = -M_PI; + variable_bounds_[2].max_position_ = M_PI; computeVariableBoundsMsg(); } @@ -87,7 +86,7 @@ double PlanarJointModel::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_; - return sqrt(dx * dx + dy * dy) + boost::math::constants::pi() * angular_distance_weight_; + return sqrt(dx * dx + dy * dy) + M_PI * angular_distance_weight_; } void PlanarJointModel::getVariableDefaultPositions(double* values, const Bounds& bounds) const @@ -138,8 +137,8 @@ void PlanarJointModel::getVariableRandomPositionsNearBy(random_numbers::RandomNu double da = angular_distance_weight_ * distance; // limit the sampling range to 2pi to work correctly even if the distance is very large - if (da > boost::math::constants::pi()) - da = boost::math::constants::pi(); + if (da > M_PI) + da = M_PI; values[2] = rng.uniformReal(near[2] - da, near[2] + da); normalizeRotation(values); } @@ -162,8 +161,7 @@ void computeTurnDriveTurnGeometry(const double* from, const double* to, const do const double angle_straight_diff = std::hypot(dx, dy) > min_translational_distance ? angles::shortest_angular_distance(from[2], std::atan2(dy, dx)) : 0.0; - const double angle_backward_diff = - angles::normalize_angle(angle_straight_diff - boost::math::constants::pi()); + const double angle_backward_diff = angles::normalize_angle(angle_straight_diff - M_PI); const double move_straight_cost = std::abs(angle_straight_diff) + std::abs(angles::shortest_angular_distance(from[2] + angle_straight_diff, to[2])); const double move_backward_cost = @@ -190,20 +188,20 @@ void PlanarJointModel::interpolate(const double* from, const double* to, const d // interpolate angle double diff = to[2] - from[2]; - if (fabs(diff) <= boost::math::constants::pi()) + if (fabs(diff) <= M_PI) state[2] = from[2] + diff * t; else { if (diff > 0.0) - diff = 2.0 * boost::math::constants::pi() - diff; + diff = 2.0 * M_PI - diff; else - diff = -2.0 * boost::math::constants::pi() - diff; + diff = -2.0 * M_PI - diff; state[2] = from[2] - diff * t; // input states are within bounds, so the following check is sufficient - if (state[2] > boost::math::constants::pi()) - state[2] -= 2.0 * boost::math::constants::pi(); - else if (state[2] < -boost::math::constants::pi()) - state[2] += 2.0 * boost::math::constants::pi(); + if (state[2] > M_PI) + state[2] -= 2.0 * M_PI; + else if (state[2] < -M_PI) + state[2] += 2.0 * M_PI; } } else if (motion_model_ == DIFF_DRIVE) @@ -254,7 +252,7 @@ double PlanarJointModel::distance(const double* values1, const double* values2) double dy = values1[1] - values2[1]; double d = fabs(values1[2] - values2[2]); - d = (d > boost::math::constants::pi()) ? 2.0 * boost::math::constants::pi() - d : d; + d = (d > M_PI) ? 2.0 * M_PI - d : d; return sqrt(dx * dx + dy * dy) + angular_distance_weight_ * d; } else if (motion_model_ == DIFF_DRIVE) @@ -279,13 +277,13 @@ bool PlanarJointModel::satisfiesPositionBounds(const double* values, const Bound bool PlanarJointModel::normalizeRotation(double* values) const { double& v = values[2]; - if (v >= -boost::math::constants::pi() && v <= boost::math::constants::pi()) + if (v >= -M_PI && v <= M_PI) return false; - v = fmod(v, 2.0 * boost::math::constants::pi()); - if (v < -boost::math::constants::pi()) - v += 2.0 * boost::math::constants::pi(); - else if (v > boost::math::constants::pi()) - v -= 2.0 * boost::math::constants::pi(); + v = fmod(v, 2.0 * M_PI); + if (v < -M_PI) + v += 2.0 * M_PI; + else if (v > M_PI) + v -= 2.0 * M_PI; return true; } diff --git a/moveit_core/robot_model/src/revolute_joint_model.cpp b/moveit_core/robot_model/src/revolute_joint_model.cpp index bc0a8320ada..90fba07d903 100644 --- a/moveit_core/robot_model/src/revolute_joint_model.cpp +++ b/moveit_core/robot_model/src/revolute_joint_model.cpp @@ -37,7 +37,6 @@ #include #include -#include #include #include @@ -60,8 +59,8 @@ RevoluteJointModel::RevoluteJointModel(const std::string& name, size_t joint_ind variable_names_.push_back(getName()); variable_bounds_.resize(1); variable_bounds_[0].position_bounded_ = true; - variable_bounds_[0].min_position_ = -boost::math::constants::pi(); - variable_bounds_[0].max_position_ = boost::math::constants::pi(); + variable_bounds_[0].min_position_ = -M_PI; + variable_bounds_[0].max_position_ = M_PI; variable_index_map_[getName()] = 0; computeVariableBoundsMsg(); } @@ -88,8 +87,8 @@ void RevoluteJointModel::setContinuous(bool flag) if (flag) { variable_bounds_[0].position_bounded_ = false; - variable_bounds_[0].min_position_ = -boost::math::constants::pi(); - variable_bounds_[0].max_position_ = boost::math::constants::pi(); + variable_bounds_[0].min_position_ = -M_PI; + variable_bounds_[0].max_position_ = M_PI; } else variable_bounds_[0].position_bounded_ = true; @@ -135,20 +134,20 @@ void RevoluteJointModel::interpolate(const double* from, const double* to, const if (continuous_) { double diff = to[0] - from[0]; - if (fabs(diff) <= boost::math::constants::pi()) + if (fabs(diff) <= M_PI) state[0] = from[0] + diff * t; else { if (diff > 0.0) - diff = 2.0 * boost::math::constants::pi() - diff; + diff = 2.0 * M_PI - diff; else - diff = -2.0 * boost::math::constants::pi() - diff; + diff = -2.0 * M_PI - diff; state[0] = from[0] - diff * t; // input states are within bounds, so the following check is sufficient - if (state[0] > boost::math::constants::pi()) - state[0] -= 2.0 * boost::math::constants::pi(); - else if (state[0] < -boost::math::constants::pi()) - state[0] += 2.0 * boost::math::constants::pi(); + if (state[0] > M_PI) + state[0] -= 2.0 * M_PI; + else if (state[0] < -M_PI) + state[0] += 2.0 * M_PI; } } else @@ -159,8 +158,8 @@ double RevoluteJointModel::distance(const double* values1, const double* values2 { if (continuous_) { - double d = fmod(fabs(values1[0] - values2[0]), 2.0 * boost::math::constants::pi()); - return (d > boost::math::constants::pi()) ? 2.0 * boost::math::constants::pi() - d : d; + double d = fmod(fabs(values1[0] - values2[0]), 2.0 * M_PI); + return (d > M_PI) ? 2.0 * M_PI - d : d; } else return fabs(values1[0] - values2[0]); @@ -203,13 +202,13 @@ bool RevoluteJointModel::enforcePositionBounds(double* values, const Bounds& bou if (continuous_) { double& v = values[0]; - if (v <= -boost::math::constants::pi() || v > boost::math::constants::pi()) + if (v <= -M_PI || v > M_PI) { - v = fmod(v, 2.0 * boost::math::constants::pi()); - if (v <= -boost::math::constants::pi()) - v += 2.0 * boost::math::constants::pi(); - else if (v > boost::math::constants::pi()) - v -= 2.0 * boost::math::constants::pi(); + v = fmod(v, 2.0 * M_PI); + if (v <= -M_PI) + v += 2.0 * M_PI; + else if (v > M_PI) + v -= 2.0 * M_PI; return true; } } diff --git a/moveit_core/robot_model/src/robot_model.cpp b/moveit_core/robot_model/src/robot_model.cpp index 9c0ad5780f8..fa22ee73892 100644 --- a/moveit_core/robot_model/src/robot_model.cpp +++ b/moveit_core/robot_model/src/robot_model.cpp @@ -37,7 +37,6 @@ #include #include -#include #include #include #include diff --git a/moveit_core/robot_state/include/moveit/robot_state/cartesian_interpolator.h b/moveit_core/robot_state/include/moveit/robot_state/cartesian_interpolator.h index c3aea90ff6b..bd327bb7081 100644 --- a/moveit_core/robot_state/include/moveit/robot_state/cartesian_interpolator.h +++ b/moveit_core/robot_state/include/moveit/robot_state/cartesian_interpolator.h @@ -40,8 +40,6 @@ #include -#include - namespace moveit { namespace core diff --git a/moveit_core/robot_state/include/moveit/robot_state/robot_state.h b/moveit_core/robot_state/include/moveit/robot_state/robot_state.h index 7b108e63cad..44e0877b1b4 100644 --- a/moveit_core/robot_state/include/moveit/robot_state/robot_state.h +++ b/moveit_core/robot_state/include/moveit/robot_state/robot_state.h @@ -46,7 +46,6 @@ #include #include -#include #include /* 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()]; } diff --git a/moveit_core/robot_state/src/attached_body.cpp b/moveit_core/robot_state/src/attached_body.cpp index 829776fee1f..58611f33c27 100644 --- a/moveit_core/robot_state/src/attached_body.cpp +++ b/moveit_core/robot_state/src/attached_body.cpp @@ -37,7 +37,6 @@ #include #include #include -#include namespace moveit { diff --git a/moveit_core/robot_state/src/conversions.cpp b/moveit_core/robot_state/src/conversions.cpp index 3217b550cd7..5db0b68747f 100644 --- a/moveit_core/robot_state/src/conversions.cpp +++ b/moveit_core/robot_state/src/conversions.cpp @@ -44,7 +44,7 @@ #else #include #endif -#include +#include 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(cell.c_str()); + state.getVariablePositions()[i] = std::stod(cell); } } diff --git a/moveit_core/robot_state/src/robot_state.cpp b/moveit_core/robot_state/src/robot_state.cpp index 4a9022a497d..61934fea29e 100644 --- a/moveit_core/robot_state/src/robot_state.cpp +++ b/moveit_core/robot_state/src/robot_state.cpp @@ -35,11 +35,11 @@ /* Author: Ioan Sucan, Sachin Chitta, Acorn Pooley, Mario Prats, Dave Coleman */ -#include -#include -#include #include #include +#include +#include +#include #include #include #include @@ -49,8 +49,9 @@ #else #include #endif -#include +#include #include +#include #include 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& aabb) const { - BOOST_VERIFY(checkLinkTransforms()); + assert(checkLinkTransforms()); core::AABB bounding_box; std::vector links = robot_model_->getLinkModelsWithCollisionGeometry(); diff --git a/moveit_core/robot_state/test/test_aabb.cpp b/moveit_core/robot_state/test/test_aabb.cpp index bcb9889a9c5..09fb10b4b7b 100644 --- a/moveit_core/robot_state/test/test_aabb.cpp +++ b/moveit_core/robot_state/test/test_aabb.cpp @@ -39,6 +39,7 @@ #include #include #include +#include #include #if __has_include() #include @@ -240,7 +241,7 @@ TEST_F(TestAABB, TestPR2) aabb.extendWithTransformedBox(transforms[i], extents); // Publish AABB - msg->ns = (*it)->getName() + boost::lexical_cast(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]; diff --git a/moveit_core/robot_trajectory/CMakeLists.txt b/moveit_core/robot_trajectory/CMakeLists.txt index b0c07c08960..03aaad5dc3c 100644 --- a/moveit_core/robot_trajectory/CMakeLists.txt +++ b/moveit_core/robot_trajectory/CMakeLists.txt @@ -6,7 +6,6 @@ ament_target_dependencies(${MOVEIT_LIB_NAME} rclcpp urdfdom urdfdom_headers - Boost ) target_link_libraries(${MOVEIT_LIB_NAME} diff --git a/moveit_core/robot_trajectory/src/robot_trajectory.cpp b/moveit_core/robot_trajectory/src/robot_trajectory.cpp index fb1d19a570a..8bd38e05c8b 100644 --- a/moveit_core/robot_trajectory/src/robot_trajectory.cpp +++ b/moveit_core/robot_trajectory/src/robot_trajectory.cpp @@ -34,6 +34,7 @@ /* Author: Ioan Sucan, Adam Leeper */ +#include #include #include #include @@ -45,7 +46,6 @@ #else #include #endif -#include #include namespace robot_trajectory @@ -175,10 +175,10 @@ RobotTrajectory& RobotTrajectory::unwind() for (std::size_t j = 1; j < waypoints_.size(); ++j) { double current_value = waypoints_[j]->getJointPositions(cont_joint)[0]; - if (last_value > current_value + boost::math::constants::pi()) - running_offset += 2.0 * boost::math::constants::pi(); - else if (current_value > last_value + boost::math::constants::pi()) - running_offset -= 2.0 * boost::math::constants::pi(); + if (last_value > current_value + M_PI) + running_offset += 2.0 * M_PI; + else if (current_value > last_value + M_PI) + running_offset -= 2.0 * M_PI; last_value = current_value; if (running_offset > std::numeric_limits::epsilon() || @@ -223,10 +223,10 @@ RobotTrajectory& RobotTrajectory::unwind(const moveit::core::RobotState& state) for (std::size_t j = 1; j < waypoints_.size(); ++j) { double current_value = waypoints_[j]->getJointPositions(cont_joint)[0]; - if (last_value > current_value + boost::math::constants::pi()) - running_offset += 2.0 * boost::math::constants::pi(); - else if (current_value > last_value + boost::math::constants::pi()) - running_offset -= 2.0 * boost::math::constants::pi(); + if (last_value > current_value + M_PI) + running_offset += 2.0 * M_PI; + else if (current_value > last_value + M_PI) + running_offset -= 2.0 * M_PI; last_value = current_value; if (running_offset > std::numeric_limits::epsilon() || diff --git a/moveit_core/transforms/include/moveit/transforms/transforms.h b/moveit_core/transforms/include/moveit/transforms/transforms.h index 2f586616d17..7b31fbb4f47 100644 --- a/moveit_core/transforms/include/moveit/transforms/transforms.h +++ b/moveit_core/transforms/include/moveit/transforms/transforms.h @@ -38,7 +38,6 @@ #include #include -#include #include #include @@ -56,9 +55,19 @@ using FixedTransformsMap = std::map -#include #include #include #include @@ -191,8 +190,8 @@ void RobotModelBuilder::addChain(const std::string& section, const std::string& if (joint->type == urdf::Joint::REVOLUTE || joint->type == urdf::Joint::PRISMATIC) { auto limits = std::make_shared(); - limits->lower = -boost::math::constants::pi(); - limits->upper = boost::math::constants::pi(); + limits->lower = -M_PI; + limits->upper = M_PI; joint->limits = limits; } diff --git a/moveit_kinematics/cached_ik_kinematics_plugin/CMakeLists.txt b/moveit_kinematics/cached_ik_kinematics_plugin/CMakeLists.txt index 9263b4aabce..e78ee06f0a5 100644 --- a/moveit_kinematics/cached_ik_kinematics_plugin/CMakeLists.txt +++ b/moveit_kinematics/cached_ik_kinematics_plugin/CMakeLists.txt @@ -1,8 +1,6 @@ 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}") @@ -10,7 +8,6 @@ ament_target_dependencies(${MOVEIT_LIB_NAME} rclcpp moveit_core moveit_msgs - Boost ) if(trac_ik_kinematics_plugin_FOUND) diff --git a/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/detail/GreedyKCenters.h b/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/detail/GreedyKCenters.h index 1c4eef699fc..b1120c38647 100644 --- a/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/detail/GreedyKCenters.h +++ b/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/detail/GreedyKCenters.h @@ -38,9 +38,9 @@ #pragma once -#include #include #include +#include namespace cached_ik_kinematics_plugin { @@ -54,7 +54,7 @@ class GreedyKCenters /** \brief The definition of a distance function */ using DistanceFunction = std::function; /** \brief A matrix type for storing distances between points and centers */ - using Matrix = boost::numeric::ublas::matrix; + 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{ 0, data.size() - 1 }(generator_)); for (unsigned i = 1; i < k; ++i) diff --git a/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/detail/NearestNeighborsGNAT.h b/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/detail/NearestNeighborsGNAT.h index 495cf2d7c34..431d8b47e63 100644 --- a/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/detail/NearestNeighborsGNAT.h +++ b/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/detail/NearestNeighborsGNAT.h @@ -45,6 +45,7 @@ #include #include "GreedyKCenters.h" #include "NearestNeighbors.h" +#include namespace cached_ik_kinematics_plugin { diff --git a/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/multivariate_gaussian.h b/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/multivariate_gaussian.h index 23ae0b135fe..d9affb9b6a5 100644 --- a/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/multivariate_gaussian.h +++ b/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/multivariate_gaussian.h @@ -36,9 +36,7 @@ #pragma once -#include -#include -#include +#include #include #include #include @@ -63,9 +61,8 @@ class MultivariateGaussian Eigen::MatrixXd covariance_cholesky_; /**< Cholesky decomposition (LL^T) of the covariance */ int size_; - boost::mt19937 rng_; - boost::normal_distribution<> normal_dist_; - boost::variate_generator > gaussian_; + std::mt19937 rng_; + std::normal_distribution gaussian_; }; //////////////////////// template function definitions follow ////////////////////////////// @@ -73,14 +70,9 @@ class MultivariateGaussian template MultivariateGaussian::MultivariateGaussian(const Eigen::MatrixBase& mean, const Eigen::MatrixBase& covariance) - : mean_(mean) - , covariance_(covariance) - , covariance_cholesky_(covariance_.llt().matrixL()) - , rng_() - , normal_dist_(0.0, 1.0) - , gaussian_(rng_, normal_dist_) + : mean_(mean), covariance_(covariance), covariance_cholesky_(covariance_.llt().matrixL()), gaussian_(0.0, 1.0) { - rng_.seed(rand()); + rng_ = std::mt19937(std::random_device{}()); size_ = mean.rows(); } @@ -88,7 +80,7 @@ template void MultivariateGaussian::sample(Eigen::MatrixBase& output) { for (int i = 0; i < size_; ++i) - output(i) = gaussian_(); + output(i) = gaussian_(rng_); output = mean_ + covariance_cholesky_ * output; } } // namespace chomp diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constraints_library.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constraints_library.h index fcfdc9bdb0e..7893627c115 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constraints_library.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constraints_library.h @@ -36,6 +36,7 @@ #pragma once +#include #include #include #include diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/command_list_manager.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/command_list_manager.h index a2b0be38fab..638b63c986d 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/command_list_manager.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/command_list_manager.h @@ -36,7 +36,8 @@ #include -#include +#include +#include #include #include @@ -107,7 +108,7 @@ class CommandListManager private: using MotionResponseCont = std::vector; - using RobotState_OptRef = boost::optional; + using RobotState_OptRef = const std::optional>; using RadiiCont = std::vector; using GroupNamesCont = std::vector; diff --git a/moveit_planners/pilz_industrial_motion_planner/src/command_list_manager.cpp b/moveit_planners/pilz_industrial_motion_planner/src/command_list_manager.cpp index 34f41d206dd..8702caac224 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/command_list_manager.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/command_list_manager.cpp @@ -161,10 +161,10 @@ CommandListManager::getPreviousEndState(const MotionResponseCont& motion_plan_re { if (it->trajectory_->getGroupName() == group_name) { - return it->trajectory_->getLastWayPoint(); + return std::reference_wrapper(it->trajectory_->getLastWayPoint()); } } - return boost::none; + return {}; } void CommandListManager::setStartState(const MotionResponseCont& motion_plan_responses, const std::string& group_name, diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/cartesianconfiguration.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/cartesianconfiguration.h index 50edfc32912..b6057abdb33 100644 --- a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/cartesianconfiguration.h +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/cartesianconfiguration.h @@ -36,8 +36,8 @@ #include #include +#include -#include #include #include #include @@ -81,10 +81,10 @@ class CartesianConfiguration : public RobotConfiguration bool hasSeed() const; void setPoseTolerance(const double tol); - const boost::optional getPoseTolerance() const; + const std::optional getPoseTolerance() const; void setAngleTolerance(const double tol); - const boost::optional getAngleTolerance() const; + const std::optional getAngleTolerance() const; private: static geometry_msgs::msg::Pose toPose(const std::vector& pose); @@ -96,14 +96,14 @@ class CartesianConfiguration : public RobotConfiguration //! @brief The dimensions of the sphere associated with the target region //! of the position constraint. - boost::optional tolerance_pose_{ boost::none }; + std::optional tolerance_pose_; //! @brief The value to assign to the absolute tolerances of the //! orientation constraint. - boost::optional tolerance_angle_{ boost::none }; + std::optional tolerance_angle_; //! @brief The seed for computing the IK solution of the cartesian configuration. - boost::optional seed_{ boost::none }; + std::optional seed_; }; std::ostream& operator<<(std::ostream& /*os*/, const CartesianConfiguration& /*obj*/); @@ -158,7 +158,7 @@ inline const JointConfiguration& CartesianConfiguration::getSeed() const inline bool CartesianConfiguration::hasSeed() const { - return seed_.is_initialized(); + return seed_.has_value(); } inline void CartesianConfiguration::setPoseTolerance(const double tol) @@ -166,7 +166,7 @@ inline void CartesianConfiguration::setPoseTolerance(const double tol) tolerance_pose_ = tol; } -inline const boost::optional CartesianConfiguration::getPoseTolerance() const +inline const std::optional CartesianConfiguration::getPoseTolerance() const { return tolerance_pose_; } @@ -176,7 +176,7 @@ inline void CartesianConfiguration::setAngleTolerance(const double tol) tolerance_angle_ = tol; } -inline const boost::optional CartesianConfiguration::getAngleTolerance() const +inline const std::optional CartesianConfiguration::getAngleTolerance() const { return tolerance_angle_; } diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/command_types_typedef.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/command_types_typedef.h index dcf2cab7318..697cc5781f7 100644 --- a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/command_types_typedef.h +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/command_types_typedef.h @@ -34,8 +34,6 @@ #pragma once -#include - #include "ptp.h" #include "lin.h" #include "circ.h" @@ -44,6 +42,8 @@ #include "cartesianconfiguration.h" #include "circ_auxiliary_types.h" +#include + namespace pilz_industrial_motion_planner_testutils { typedef Ptp PtpJoint; @@ -60,7 +60,7 @@ typedef Circ C typedef Circ CircJointCenterCart; typedef Circ CircJointInterimCart; -typedef boost::variant +typedef std::variant CmdVariant; } // namespace pilz_industrial_motion_planner_testutils diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/sequence.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/sequence.h index 79ef318f816..e18d5170802 100644 --- a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/sequence.h +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/sequence.h @@ -43,6 +43,7 @@ #include "command_types_typedef.h" #include "motioncmd.h" +#include namespace pilz_industrial_motion_planner_testutils { @@ -112,13 +113,13 @@ inline size_t Sequence::size() const template inline T& Sequence::getCmd(const size_t index_cmd) { - return boost::get(cmds_.at(index_cmd).first); + return std::get(cmds_.at(index_cmd).first); } template inline const T& Sequence::getCmd(const size_t index_cmd) const { - return boost::get(cmds_.at(index_cmd).first); + return std::get(cmds_.at(index_cmd).first); } inline double Sequence::getBlendRadius(const size_t index_cmd) const @@ -139,6 +140,6 @@ inline void Sequence::setAllBlendRadiiToZero() template inline bool Sequence::cmdIsOfType(const size_t index_cmd) const { - return cmds_.at(index_cmd).first.type() == typeid(T); + return std::holds_alternative(cmds_.at(index_cmd).first); } } // namespace pilz_industrial_motion_planner_testutils diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/src/sequence.cpp b/moveit_planners/pilz_industrial_motion_planner_testutils/src/sequence.cpp index e6b30433145..4f4554196ae 100644 --- a/moveit_planners/pilz_industrial_motion_planner_testutils/src/sequence.cpp +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/src/sequence.cpp @@ -35,14 +35,13 @@ #include "pilz_industrial_motion_planner_testutils/sequence.h" #include -#include namespace pilz_industrial_motion_planner_testutils { /** * @brief Visitor transforming the stored command into a MotionPlanRequest. */ -class ToReqVisitor : public boost::static_visitor +class ToReqVisitor { public: template @@ -55,7 +54,7 @@ class ToReqVisitor : public boost::static_visitor +class ToBaseVisitor { public: template @@ -73,7 +72,7 @@ moveit_msgs::msg::MotionSequenceRequest Sequence::toRequest() const for (const auto& cmd : cmds_) { moveit_msgs::msg::MotionSequenceItem item; - item.req = boost::apply_visitor(ToReqVisitor(), cmd.first); + item.req = std::visit(ToReqVisitor(), cmd.first); if (std::find(group_names.begin(), group_names.end(), item.req.group_name) != group_names.end()) { @@ -114,7 +113,7 @@ void Sequence::erase(const size_t start, const size_t end) MotionCmd& Sequence::getCmd(const size_t index_cmd) { - return boost::apply_visitor(ToBaseVisitor(), cmds_.at(index_cmd).first); + return std::visit(ToBaseVisitor(), cmds_.at(index_cmd).first); } } // namespace pilz_industrial_motion_planner_testutils diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/src/xml_testdata_loader.cpp b/moveit_planners/pilz_industrial_motion_planner_testutils/src/xml_testdata_loader.cpp index 3fef3967ed2..4a6c9f8ab1c 100644 --- a/moveit_planners/pilz_industrial_motion_planner_testutils/src/xml_testdata_loader.cpp +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/src/xml_testdata_loader.cpp @@ -38,7 +38,6 @@ #include #include -#include #include "pilz_industrial_motion_planner_testutils/default_values.h" #include "pilz_industrial_motion_planner_testutils/exception_types.h" diff --git a/moveit_ros/benchmarks/benchmarks/src/benchmark_execution.cpp b/moveit_ros/benchmarks/benchmarks/src/benchmark_execution.cpp index b8885407690..b32c0a5471a 100644 --- a/moveit_ros/benchmarks/benchmarks/src/benchmark_execution.cpp +++ b/moveit_ros/benchmarks/benchmarks/src/benchmark_execution.cpp @@ -40,14 +40,12 @@ #include #include -#include #include #include #include #include #include #include -#include #include #include @@ -60,7 +58,11 @@ #include #include +#include #include +#include +#include +#include namespace moveit_benchmarks { @@ -229,13 +231,13 @@ void moveit_benchmarks::BenchmarkExecution::runAllBenchmarks(BenchmarkType type) std::vector start_states; if (!options_.start_regex.empty()) { - boost::regex start_regex(options_.start_regex); + std::regex start_regex(options_.start_regex); std::vector state_names; rs_.getKnownRobotStates(state_names); for (std::size_t i = 0; i < state_names.size(); ++i) { - boost::cmatch match; - if (boost::regex_match(state_names[i].c_str(), match, start_regex)) + std::smatch match; + if (std::regex_match(state_names[i], match, start_regex)) start_states.push_back(state_names[i]); } if (start_states.empty()) @@ -307,7 +309,7 @@ void moveit_benchmarks::BenchmarkExecution::runAllBenchmarks(BenchmarkType type) // update request given .cfg options if (start_state_to_use) req.motion_plan_request.start_state = *start_state_to_use; - req.filename = options_.output + "." + boost::lexical_cast(++n_call) + ".log"; + req.filename = options_.output + "." + std::to_string(++n_call) + ".log"; if (!options_.group_override.empty()) req.motion_plan_request.group_name = options_.group_override; @@ -404,7 +406,7 @@ void moveit_benchmarks::BenchmarkExecution::runAllBenchmarks(BenchmarkType type) checkConstrainedLink(req.motion_plan_request.goal_constraints[0], options_.default_constrained_link); if (!options_.planning_frame.empty()) checkHeader(req.motion_plan_request.goal_constraints[0], options_.planning_frame); - req.filename = options_.output + "." + boost::lexical_cast(++n_call) + ".log"; + req.filename = options_.output + "." + std::to_string(++n_call) + ".log"; ROS_INFO("Benchmarking goal '%s' (%d of %d)", cnames[i].c_str(), static_cast(i) + 1, static_cast(cnames.size())); @@ -493,7 +495,7 @@ void moveit_benchmarks::BenchmarkExecution::runAllBenchmarks(BenchmarkType type) req.motion_plan_request.group_name = options_.group_override; if (options_.timeout > 0.0) req.motion_plan_request.allowed_planning_time = options_.timeout; - req.filename = options_.output + ".trajectory." + boost::lexical_cast(i + 1) + ".log"; + req.filename = options_.output + ".trajectory." + std::to_string(i + 1) + ".log"; ROS_INFO("Benchmarking trajectory '%s' (%d of %d)", cnames[i].c_str(), static_cast(i) + 1, static_cast(cnames.size())); @@ -565,19 +567,19 @@ bool moveit_benchmarks::BenchmarkExecution::readOptions(const std::string& filen { memset(options_.offsets, 0, 6 * sizeof(double)); if (!declared_options["scene.goal_offset_x"].empty()) - options_.offsets[0] = boost::lexical_cast(declared_options["scene.goal_offset_x"]); + options_.offsets[0] = std::stod(declared_options["scene.goal_offset_x"]); if (!declared_options["scene.goal_offset_y"].empty()) - options_.offsets[1] = boost::lexical_cast(declared_options["scene.goal_offset_y"]); + options_.offsets[1] = std::stod(declared_options["scene.goal_offset_y"]); if (!declared_options["scene.goal_offset_z"].empty()) - options_.offsets[2] = boost::lexical_cast(declared_options["scene.goal_offset_z"]); + options_.offsets[2] = std::stod(declared_options["scene.goal_offset_z"]); if (!declared_options["scene.goal_offset_roll"].empty()) - options_.offsets[3] = boost::lexical_cast(declared_options["scene.goal_offset_roll"]); + options_.offsets[3] = std::stod(declared_options["scene.goal_offset_roll"]); if (!declared_options["scene.goal_offset_pitch"].empty()) - options_.offsets[4] = boost::lexical_cast(declared_options["scene.goal_offset_pitch"]); + options_.offsets[4] = std::stod(declared_options["scene.goal_offset_pitch"]); if (!declared_options["scene.goal_offset_yaw"].empty()) - options_.offsets[5] = boost::lexical_cast(declared_options["scene.goal_offset_yaw"]); + options_.offsets[5] = std::stod(declared_options["scene.goal_offset_yaw"]); } - catch (boost::bad_lexical_cast& ex) + catch (std::invalid_argument& ex) { ROS_WARN("%s", ex.what()); } @@ -603,14 +605,14 @@ bool moveit_benchmarks::BenchmarkExecution::readOptions(const std::string& filen // add workspace bounds if specified in the .cfg file options_.workspace_parameters.header.frame_id = declared_options["scene.workspace_frame"]; options_.workspace_parameters.header.stamp = ros::Time::now(); - options_.workspace_parameters.min_corner.x = boost::lexical_cast(strings[0]); - options_.workspace_parameters.min_corner.y = boost::lexical_cast(strings[1]); - options_.workspace_parameters.min_corner.z = boost::lexical_cast(strings[2]); - options_.workspace_parameters.max_corner.x = boost::lexical_cast(strings[3]); - options_.workspace_parameters.max_corner.y = boost::lexical_cast(strings[4]); - options_.workspace_parameters.max_corner.z = boost::lexical_cast(strings[5]); + options_.workspace_parameters.min_corner.x = std::stod(strings[0]); + options_.workspace_parameters.min_corner.y = std::stod(strings[1]); + options_.workspace_parameters.min_corner.z = std::stod(strings[2]); + options_.workspace_parameters.max_corner.x = std::stod(strings[3]); + options_.workspace_parameters.max_corner.y = std::stod(strings[4]); + options_.workspace_parameters.max_corner.z = std::stod(strings[5]); } - catch (boost::bad_lexical_cast& ex) + catch (std::invalid_argument& ex) { ROS_WARN("%s", ex.what()); } @@ -645,9 +647,9 @@ bool moveit_benchmarks::BenchmarkExecution::readOptions(const std::string& filen { try { - options_.timeout = boost::lexical_cast(declared_options["scene.timeout"]); + options_.timeout = std::stod(declared_options["scene.timeout"]); } - catch (boost::bad_lexical_cast& ex) + catch (std::invalid_argument& ex) { ROS_WARN("%s", ex.what()); } @@ -725,16 +727,16 @@ bool moveit_benchmarks::BenchmarkExecution::readOptions(const std::string& filen new_sweep.is_sweep = true; // not a fractional factorial analaysis try { - new_sweep.start = boost::lexical_cast(strings[0]); - new_sweep.step_size = boost::lexical_cast(strings[1]); - new_sweep.end = boost::lexical_cast(strings[2]); + new_sweep.start = std::stod(strings[0]); + new_sweep.step_size = std::stod(strings[1]); + new_sweep.end = std::stod(strings[2]); new_sweep.key = sweep_var; // for logging to file: std::ostringstream ss; ss << "param_" << sweep_var << " REAL"; new_sweep.log_key = ss.str(); } - catch (boost::bad_lexical_cast& ex) + catch (std::invalid_argument& ex) { ROS_WARN("%s", ex.what()); } @@ -804,8 +806,8 @@ void moveit_benchmarks::BenchmarkExecution::collectMetrics(RunData& rundata, const planning_interface::MotionPlanDetailedResponse& mp_res, bool solved, double total_time) { - rundata["total_time REAL"] = boost::lexical_cast(total_time); - rundata["solved BOOLEAN"] = boost::lexical_cast(solved); + rundata["total_time REAL"] = std::to_string(total_time); + rundata["solved BOOLEAN"] = solved ? "true" : "false"; double L = 0.0; double clearance = 0.0; double smoothness = 0.0; @@ -862,7 +864,7 @@ void moveit_benchmarks::BenchmarkExecution::collectMetrics(RunData& rundata, if (acosValue > -1.0 && acosValue < 1.0) { // the smoothness is actually the outside angle of the one we compute - double angle = (boost::math::constants::pi() - acos(acosValue)); + double angle = (M_PI - acos(acosValue)); // and we normalize by the length of the segments double u = 2.0 * angle; /// (a + b); @@ -872,17 +874,16 @@ void moveit_benchmarks::BenchmarkExecution::collectMetrics(RunData& rundata, } smoothness /= (double)p.getWayPointCount(); } - rundata["path_" + mp_res.description_[j] + "_correct BOOLEAN"] = boost::lexical_cast(correct); - rundata["path_" + mp_res.description_[j] + "_length REAL"] = boost::lexical_cast(L); - rundata["path_" + mp_res.description_[j] + "_clearance REAL"] = boost::lexical_cast(clearance); - rundata["path_" + mp_res.description_[j] + "_smoothness REAL"] = boost::lexical_cast(smoothness); - rundata["path_" + mp_res.description_[j] + "_time REAL"] = - boost::lexical_cast(mp_res.processing_time_[j]); + rundata["path_" + mp_res.description_[j] + "_correct BOOLEAN"] = correct ? "true" : "false"; + rundata["path_" + mp_res.description_[j] + "_length REAL"] = std::to_string(L); + rundata["path_" + mp_res.description_[j] + "_clearance REAL"] = std::to_string(clearance); + rundata["path_" + mp_res.description_[j] + "_smoothness REAL"] = std::to_string(smoothness); + rundata["path_" + mp_res.description_[j] + "_time REAL"] = std::to_string(mp_res.processing_time_[j]); process_time -= mp_res.processing_time_[j]; } if (process_time <= 0.0) process_time = 0.0; - rundata["process_time REAL"] = boost::lexical_cast(process_time); + rundata["process_time REAL"] = std::to_string(process_time); } } @@ -1425,9 +1426,9 @@ void moveit_benchmarks::BenchmarkExecution::modifyPlannerConfiguration(planning_ try { double value = param_combinations_[param_combinations_id_][param_options_[i].key]; - str_parameter_value = boost::lexical_cast(value); + str_parameter_value = std::to_string(value); } - catch (boost::bad_lexical_cast& ex) + catch (std::bad_alloc& ex) { ROS_WARN("%s", ex.what()); } diff --git a/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp b/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp index 5019d027708..dcf9df540b4 100644 --- a/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp +++ b/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp @@ -50,8 +50,8 @@ #include #include #undef BOOST_ALLOW_DEPRECATED_HEADERS -#include #include +#include #include #include #ifndef _WIN32 @@ -674,13 +674,13 @@ bool BenchmarkExecutor::loadStates(const std::string& regex, std::vector state_names; rs_->getKnownRobotStates(state_names); for (const std::string& state_name : state_names) { - boost::cmatch match; - if (boost::regex_match(state_name.c_str(), match, start_regex)) + std::smatch match; + if (std::regex_match(state_name, match, start_regex)) { moveit_warehouse::RobotStateWithMetadata robot_state; try @@ -874,7 +874,7 @@ void BenchmarkExecutor::collectMetrics(PlannerRunData& metrics, double total_time) { metrics["time REAL"] = moveit::core::toString(total_time); - metrics["solved BOOLEAN"] = boost::lexical_cast(solved); + metrics["solved BOOLEAN"] = solved ? "true" : "false"; if (solved) { @@ -935,7 +935,7 @@ void BenchmarkExecutor::collectMetrics(PlannerRunData& metrics, if (acos_value > -1.0 && acos_value < 1.0) { // the smoothness is actually the outside angle of the one we compute - double angle = (boost::math::constants::pi() - acos(acos_value)); + double angle = (M_PI - acos(acos_value)); // and we normalize by the length of the segments double u = 2.0 * angle; /// (a + b); @@ -945,7 +945,7 @@ void BenchmarkExecutor::collectMetrics(PlannerRunData& metrics, } smoothness /= (double)p.getWayPointCount(); } - metrics["path_" + mp_res.description_[j] + "_correct BOOLEAN"] = boost::lexical_cast(correct); + metrics["path_" + mp_res.description_[j] + "_correct BOOLEAN"] = correct ? "true" : "false"; metrics["path_" + mp_res.description_[j] + "_length REAL"] = moveit::core::toString(traj_len); metrics["path_" + mp_res.description_[j] + "_clearance REAL"] = moveit::core::toString(clearance); metrics["path_" + mp_res.description_[j] + "_smoothness REAL"] = moveit::core::toString(smoothness); @@ -953,7 +953,7 @@ void BenchmarkExecutor::collectMetrics(PlannerRunData& metrics, if (j == mp_res.trajectory_.size() - 1) { - metrics["final_path_correct BOOLEAN"] = boost::lexical_cast(correct); + metrics["final_path_correct BOOLEAN"] = correct ? "true" : "false"; metrics["final_path_length REAL"] = moveit::core::toString(traj_len); metrics["final_path_clearance REAL"] = moveit::core::toString(clearance); metrics["final_path_smoothness REAL"] = moveit::core::toString(smoothness); diff --git a/moveit_ros/moveit_servo/include/moveit_servo/pose_tracking.h b/moveit_ros/moveit_servo/include/moveit_servo/pose_tracking.h index 1a2abe1e347..4b4484ac3a5 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/pose_tracking.h +++ b/moveit_ros/moveit_servo/include/moveit_servo/pose_tracking.h @@ -39,11 +39,11 @@ #pragma once #include -#include #include #include -#include #include +#include +#include #if __has_include() #include #else @@ -191,7 +191,7 @@ class PoseTracking // Flag that a different thread has requested a stop. std::atomic stop_requested_; - boost::optional angular_error_; + std::optional angular_error_; }; // using alias diff --git a/moveit_ros/moveit_servo/src/pose_tracking.cpp b/moveit_ros/moveit_servo/src/pose_tracking.cpp index ee13f15272e..fed500dfad6 100644 --- a/moveit_ros/moveit_servo/src/pose_tracking.cpp +++ b/moveit_ros/moveit_servo/src/pose_tracking.cpp @@ -81,7 +81,6 @@ PoseTracking::PoseTracking(const rclcpp::Node::SharedPtr& node, const ServoParam , transform_buffer_(node_->get_clock()) , transform_listener_(transform_buffer_) , stop_requested_(false) - , angular_error_(boost::none) { readROSParams(); @@ -346,7 +345,7 @@ void PoseTracking::doPostMotionReset() { stopMotion(); stop_requested_ = false; - angular_error_ = boost::none; + angular_error_ = {}; // Reset error integrals and previous errors of PID controllers cartesian_position_pids_[0].reset(); diff --git a/moveit_ros/perception/depth_image_octomap_updater/CMakeLists.txt b/moveit_ros/perception/depth_image_octomap_updater/CMakeLists.txt index 0146061207c..013044d7e69 100644 --- a/moveit_ros/perception/depth_image_octomap_updater/CMakeLists.txt +++ b/moveit_ros/perception/depth_image_octomap_updater/CMakeLists.txt @@ -11,7 +11,6 @@ ament_target_dependencies(${MOVEIT_LIB_NAME}_core tf2_geometry_msgs geometric_shapes moveit_ros_occupancy_map_monitor - Boost ) target_link_libraries(${MOVEIT_LIB_NAME}_core moveit_lazy_free_space_updater moveit_mesh_filter) @@ -26,7 +25,6 @@ ament_target_dependencies(${MOVEIT_LIB_NAME} tf2_geometry_msgs geometric_shapes moveit_ros_occupancy_map_monitor - Boost pluginlib ) target_link_libraries(${MOVEIT_LIB_NAME} ${MOVEIT_LIB_NAME}_core) diff --git a/moveit_ros/perception/mesh_filter/CMakeLists.txt b/moveit_ros/perception/mesh_filter/CMakeLists.txt index 54b973d3178..d6b0916001f 100644 --- a/moveit_ros/perception/mesh_filter/CMakeLists.txt +++ b/moveit_ros/perception/mesh_filter/CMakeLists.txt @@ -16,7 +16,6 @@ ament_target_dependencies(${MOVEIT_LIB_NAME} moveit_core geometric_shapes Eigen3 - Boost ) target_link_libraries(${MOVEIT_LIB_NAME} ${gl_LIBS} ${SYSTEM_GL_LIBRARIES}) diff --git a/moveit_ros/perception/mesh_filter/test/mesh_filter_test.cpp b/moveit_ros/perception/mesh_filter/test/mesh_filter_test.cpp index 46acbcd8747..a8ee7b90866 100644 --- a/moveit_ros/perception/mesh_filter/test/mesh_filter_test.cpp +++ b/moveit_ros/perception/mesh_filter/test/mesh_filter_test.cpp @@ -79,8 +79,8 @@ class FilterTraits template class MeshFilterTest : public testing::TestWithParam { - BOOST_STATIC_ASSERT_MSG(FilterTraits::FILTER_GL_TYPE != GL_ZERO, "Only \"float\" and \"unsigned short int\" " - "are allowed."); + static_assert(FilterTraits::FILTER_GL_TYPE != GL_ZERO, "Only \"float\" and \"unsigned short int\" " + "are allowed."); public: MeshFilterTest(unsigned width = 500, unsigned height = 500, double near = 0.5, double far = 5.0, double shadow = 0.1, diff --git a/moveit_ros/perception/pointcloud_octomap_updater/CMakeLists.txt b/moveit_ros/perception/pointcloud_octomap_updater/CMakeLists.txt index 067554aafd6..c6d1330808c 100644 --- a/moveit_ros/perception/pointcloud_octomap_updater/CMakeLists.txt +++ b/moveit_ros/perception/pointcloud_octomap_updater/CMakeLists.txt @@ -11,7 +11,6 @@ ament_target_dependencies(${MOVEIT_LIB_NAME}_core moveit_ros_occupancy_map_monitor tf2_geometry_msgs tf2 - Boost ) target_link_libraries(${MOVEIT_LIB_NAME}_core moveit_point_containment_filter) set_target_properties(${MOVEIT_LIB_NAME}_core PROPERTIES COMPILE_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") @@ -28,7 +27,6 @@ ament_target_dependencies(${MOVEIT_LIB_NAME} moveit_ros_occupancy_map_monitor tf2_geometry_msgs tf2 - Boost pluginlib ) target_link_libraries(${MOVEIT_LIB_NAME} ${MOVEIT_LIB_NAME}_core) diff --git a/moveit_ros/perception/semantic_world/include/moveit/semantic_world/semantic_world.h b/moveit_ros/perception/semantic_world/include/moveit/semantic_world/semantic_world.h index a6b6f983e4e..66d4eda1841 100644 --- a/moveit_ros/perception/semantic_world/include/moveit/semantic_world/semantic_world.h +++ b/moveit_ros/perception/semantic_world/include/moveit/semantic_world/semantic_world.h @@ -150,8 +150,6 @@ class SemanticWorld std::map current_tables_in_collision_world_; - // boost::mutex table_lock_; - rclcpp::Subscription::SharedPtr table_subscriber_; rclcpp::Publisher::SharedPtr visualization_publisher_; diff --git a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h index 15c5bcd60a2..f1dce985633 100644 --- a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h +++ b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h @@ -46,7 +46,6 @@ #include #include #include -#include #include #include #include @@ -62,9 +61,19 @@ MOVEIT_CLASS_FORWARD(PlanningSceneMonitor); // Defines PlanningSceneMonitorPtr, /** * @brief PlanningSceneMonitor * Subscribes to the topic \e planning_scene */ -class MOVEIT_PLANNING_SCENE_MONITOR_EXPORT PlanningSceneMonitor : private boost::noncopyable +class MOVEIT_PLANNING_SCENE_MONITOR_EXPORT PlanningSceneMonitor { public: + /** + * @brief PlanningSceneMonitor cannot be copy-constructed + */ + PlanningSceneMonitor(const PlanningSceneMonitor&) = delete; + + /** + * @brief PlanningSceneMonitor cannot be copy-assigned + */ + PlanningSceneMonitor& operator=(const PlanningSceneMonitor&) = delete; + enum SceneUpdateType { /** \brief No update */ diff --git a/moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp b/moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp index 452edfea606..599b17d8029 100644 --- a/moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp +++ b/moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp @@ -58,6 +58,7 @@ #include #include +#include #include #include #include diff --git a/moveit_ros/robot_interaction/CMakeLists.txt b/moveit_ros/robot_interaction/CMakeLists.txt index e70b425cd0c..c3a8bb35ca7 100644 --- a/moveit_ros/robot_interaction/CMakeLists.txt +++ b/moveit_ros/robot_interaction/CMakeLists.txt @@ -13,13 +13,10 @@ find_package(tf2_geometry_msgs REQUIRED) find_package(tf2_ros REQUIRED) find_package(rclcpp REQUIRED) -# Finds Boost Components -include(ConfigExtras.cmake) set(MOVEIT_LIB_NAME moveit_robot_interaction) set(THIS_PACKAGE_INCLUDE_DEPENDS - Boost moveit_ros_planning interactive_markers tf2_geometry_msgs @@ -45,8 +42,6 @@ if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) ament_add_gtest(locked_robot_state_test test/locked_robot_state_test.cpp) target_link_libraries(locked_robot_state_test ${MOVEIT_LIB_NAME}) - ament_target_dependencies(locked_robot_state_test - Boost) endif() install( @@ -78,4 +73,4 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() endif() -ament_package(CONFIG_EXTRAS ConfigExtras.cmake) +ament_package() diff --git a/moveit_ros/robot_interaction/ConfigExtras.cmake b/moveit_ros/robot_interaction/ConfigExtras.cmake deleted file mode 100644 index 837e37df5e8..00000000000 --- a/moveit_ros/robot_interaction/ConfigExtras.cmake +++ /dev/null @@ -1,3 +0,0 @@ -# Extras module needed for dependencies to find boost components - -find_package(Boost REQUIRED thread) diff --git a/moveit_ros/robot_interaction/src/interaction_handler.cpp b/moveit_ros/robot_interaction/src/interaction_handler.cpp index a492bcd04bd..e4a59cb68e9 100644 --- a/moveit_ros/robot_interaction/src/interaction_handler.cpp +++ b/moveit_ros/robot_interaction/src/interaction_handler.cpp @@ -54,9 +54,8 @@ #else #include #endif -#include -#include #include +#include #include #include diff --git a/moveit_ros/robot_interaction/src/interactive_marker_helpers.cpp b/moveit_ros/robot_interaction/src/interactive_marker_helpers.cpp index 5b39aab24b5..98041c461be 100644 --- a/moveit_ros/robot_interaction/src/interactive_marker_helpers.cpp +++ b/moveit_ros/robot_interaction/src/interactive_marker_helpers.cpp @@ -34,6 +34,7 @@ /* Author: Ioan Sucan, Acorn Pooley, Adam Leeper */ +#include #include #include #if __has_include() @@ -42,8 +43,6 @@ #include #endif -#include - namespace robot_interaction { visualization_msgs::msg::InteractiveMarker @@ -73,7 +72,7 @@ void addTArrowMarker(visualization_msgs::msg::InteractiveMarker& im) // Arrow points along Z tf2::Quaternion imq, tmq; tf2::fromMsg(m.pose.orientation, imq); - tmq.setRPY(0, -boost::math::constants::pi() / 2.0, 0); + tmq.setRPY(0, -M_PI / 2.0, 0); imq = imq * tmq; m.pose.orientation = tf2::toMsg(imq); m.color.r = 0.0f; @@ -93,7 +92,7 @@ void addTArrowMarker(visualization_msgs::msg::InteractiveMarker& im) mc.pose = im.pose; // Cylinder points along Y tf2::fromMsg(mc.pose.orientation, imq); - tmq.setRPY(boost::math::constants::pi() / 2.0, 0, 0); + tmq.setRPY(M_PI / 2.0, 0, 0); imq = imq * tmq; mc.pose.orientation = tf2::toMsg(imq); mc.pose.position.x -= 0.04; diff --git a/moveit_ros/robot_interaction/src/kinematic_options.cpp b/moveit_ros/robot_interaction/src/kinematic_options.cpp index 357e9516aad..ea03b441191 100644 --- a/moveit_ros/robot_interaction/src/kinematic_options.cpp +++ b/moveit_ros/robot_interaction/src/kinematic_options.cpp @@ -35,7 +35,6 @@ /* Author: Acorn Pooley */ #include -#include #include static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros_robot_interaction.kinematic_options"); @@ -85,7 +84,7 @@ void robot_interaction::KinematicOptions::setOptions(const KinematicOptions& sou F(::kinematics::DiscretizationMethods::DiscretizationMethod, discretization_method, DISCRETIZATION_METHOD) // This structure should be identical to kinematics::KinematicsQueryOptions - // This is only used in the BOOST_STATIC_ASSERT below. + // This is only used in the static_assert below. struct DummyKinematicsQueryOptions { #define F(type, member, enumval) type member; @@ -93,7 +92,7 @@ void robot_interaction::KinematicOptions::setOptions(const KinematicOptions& sou #undef F }; // This structure should be identical to robot_interaction::KinematicOptions - // This is only used in the BOOST_STATIC_ASSERT below. + // This is only used in the static_assert below. struct DummyKinematicOptions { #define F(type, member, enumval) type member; @@ -106,8 +105,8 @@ void robot_interaction::KinematicOptions::setOptions(const KinematicOptions& sou // kinematics::KinematicsQueryOptions or robot_interaction::KinematicOptions // and not added to the O_FIELDS and QO_FIELDS definitions above. To fix add // any new fields to the definitions above. - BOOST_STATIC_ASSERT(sizeof(kinematics::KinematicsQueryOptions) == sizeof(DummyKinematicsQueryOptions)); - BOOST_STATIC_ASSERT(sizeof(KinematicOptions) == sizeof(DummyKinematicOptions)); + static_assert(sizeof(kinematics::KinematicsQueryOptions) == sizeof(DummyKinematicsQueryOptions)); + static_assert(sizeof(KinematicOptions) == sizeof(DummyKinematicOptions)); // copy fields from other to this if its bit is set in fields #define F(type, member, enumval) \ diff --git a/moveit_ros/robot_interaction/src/robot_interaction.cpp b/moveit_ros/robot_interaction/src/robot_interaction.cpp index 0caebce02b0..5e464e02039 100644 --- a/moveit_ros/robot_interaction/src/robot_interaction.cpp +++ b/moveit_ros/robot_interaction/src/robot_interaction.cpp @@ -53,12 +53,10 @@ #include #endif #include -#include -#include -#include #include #include +#include #include #include @@ -120,7 +118,7 @@ void RobotInteraction::addActiveComponent(const InteractiveMarkerConstructorFn& g.update_pose = update; g.process_feedback = process; // compute the suffix that will be added to the generated markers - g.marker_name_suffix = "_" + name + "_" + boost::lexical_cast(active_generic_.size()); + g.marker_name_suffix = "_" + name + "_" + std::to_string(active_generic_.size()); active_generic_.push_back(g); } diff --git a/moveit_ros/visualization/planning_scene_rviz_plugin/CMakeLists.txt b/moveit_ros/visualization/planning_scene_rviz_plugin/CMakeLists.txt index 858e5958c54..f71a603caa9 100644 --- a/moveit_ros/visualization/planning_scene_rviz_plugin/CMakeLists.txt +++ b/moveit_ros/visualization/planning_scene_rviz_plugin/CMakeLists.txt @@ -16,7 +16,6 @@ ament_target_dependencies(${MOVEIT_LIB_NAME}_core moveit_ros_planning moveit_msgs pluginlib - Boost rviz_ogre_vendor ) target_include_directories(${MOVEIT_LIB_NAME}_core PRIVATE "${OGRE_PREFIX_DIR}/include") diff --git a/moveit_ros/visualization/planning_scene_rviz_plugin/include/moveit/planning_scene_rviz_plugin/background_processing.hpp b/moveit_ros/visualization/planning_scene_rviz_plugin/include/moveit/planning_scene_rviz_plugin/background_processing.hpp index f2d1491305f..7f47a3331e1 100644 --- a/moveit_ros/visualization/planning_scene_rviz_plugin/include/moveit/planning_scene_rviz_plugin/background_processing.hpp +++ b/moveit_ros/visualization/planning_scene_rviz_plugin/include/moveit/planning_scene_rviz_plugin/background_processing.hpp @@ -38,7 +38,6 @@ #include #include -#include #include #include #include @@ -54,9 +53,19 @@ namespace tools /** \brief This class provides simple API for executing background jobs. A queue of jobs is created and the specified jobs are executed in order, one at a time. */ -class BackgroundProcessing : private boost::noncopyable +class BackgroundProcessing { public: + /** + * @brief BackgroundProcessing cannot be copy-constructed + */ + BackgroundProcessing(const BackgroundProcessing&) = delete; + + /** + * @brief BackgroundProcessing cannot be copy-assigned + */ + BackgroundProcessing& operator=(const BackgroundProcessing&) = delete; + /** \brief Events for jobs */ enum JobEvent { diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/src/mesh_shape.cpp b/moveit_ros/visualization/rviz_plugin_render_tools/src/mesh_shape.cpp index 06384107eb6..40afa630c6a 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/src/mesh_shape.cpp +++ b/moveit_ros/visualization/rviz_plugin_render_tools/src/mesh_shape.cpp @@ -37,8 +37,8 @@ #include #include -#include #include +#include namespace rviz_rendering { @@ -46,8 +46,7 @@ MeshShape::MeshShape(Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_ : Shape(Shape::Mesh, scene_manager, parent_node), started_(false) { static uint32_t count = 0; - manual_object_ = - scene_manager->createManualObject("MeshShape_ManualObject" + boost::lexical_cast(count++)); + manual_object_ = scene_manager->createManualObject("MeshShape_ManualObject" + std::to_string(count++)); material_->setCullingMode(Ogre::CULL_NONE); } @@ -121,7 +120,7 @@ void MeshShape::endTriangles() started_ = false; manual_object_->end(); static uint32_t count = 0; - std::string name = "ConvertedMeshShape@" + boost::lexical_cast(count++); + std::string name = "ConvertedMeshShape@" + std::to_string(count++); manual_object_->convertToMesh(name); entity_ = scene_manager_->createEntity(name); if (entity_) diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/src/render_shapes.cpp b/moveit_ros/visualization/rviz_plugin_render_tools/src/render_shapes.cpp index a27b38b0cc8..379d468f8e3 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/src/render_shapes.cpp +++ b/moveit_ros/visualization/rviz_plugin_render_tools/src/render_shapes.cpp @@ -48,10 +48,9 @@ #include #include -#include -#include - +#include #include +#include namespace moveit_rviz_plugin { @@ -179,8 +178,7 @@ void RenderShapes::renderShape(Ogre::SceneNode* node, const shapes::Shape* s, co { // in geometric shapes, the z axis of the cylinder is its height; // for the rviz shape, the y axis is the height; we add a transform to fix this - static Ogre::Quaternion fix(Ogre::Radian(boost::math::constants::pi() / 2.0), - Ogre::Vector3(1.0, 0.0, 0.0)); + static Ogre::Quaternion fix(Ogre::Radian(M_PI / 2.0), Ogre::Vector3(1.0, 0.0, 0.0)); orientation = orientation * fix; } diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_visualization.cpp b/moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_visualization.cpp index 7610776fbc2..c94ce94d09c 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_visualization.cpp +++ b/moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_visualization.cpp @@ -36,7 +36,6 @@ #include #include -#include #include @@ -57,6 +56,8 @@ #include #include +#include + using namespace std::placeholders; namespace moveit_rviz_plugin @@ -259,8 +260,8 @@ void TrajectoryVisualization::changedShowTrail() for (std::size_t i = 0; i < trajectory_trail_.size(); ++i) { int waypoint_i = std::min(i * stepsize, t->getWayPointCount() - 1); // limit to last trajectory point - auto r = std::make_unique(scene_node_, context_, - "Trail Robot " + boost::lexical_cast(i), nullptr); + auto r = + std::make_unique(scene_node_, context_, "Trail Robot " + std::to_string(i), nullptr); r->load(*robot_model_->getURDF()); r->setVisualVisible(display_path_visual_enabled_property_->getBool()); r->setCollisionVisible(display_path_collision_enabled_property_->getBool()); @@ -392,9 +393,9 @@ float TrajectoryVisualization::getStateDisplayTime() float value; try { - value = boost::lexical_cast(tm); + value = std::stof(tm); } - catch (const boost::bad_lexical_cast& ex) + catch (const std::invalid_argument& ex) { state_display_time_property_->setStdString(default_time_string); return default_time_value; diff --git a/moveit_ros/warehouse/warehouse/src/initialize_demo_db.cpp b/moveit_ros/warehouse/warehouse/src/initialize_demo_db.cpp index a3bfdedebad..3123e484281 100644 --- a/moveit_ros/warehouse/warehouse/src/initialize_demo_db.cpp +++ b/moveit_ros/warehouse/warehouse/src/initialize_demo_db.cpp @@ -34,6 +34,7 @@ /* Author: Ioan Sucan */ +#include #include #include #include @@ -45,7 +46,6 @@ #include #include #include -#include #include #include #include @@ -132,7 +132,7 @@ int main(int argc, char** argv) ocm.orientation.w = 1.0; ocm.absolute_x_axis_tolerance = 0.1; ocm.absolute_y_axis_tolerance = 0.1; - ocm.absolute_z_axis_tolerance = boost::math::constants::pi(); + ocm.absolute_z_axis_tolerance = M_PI; ocm.weight = 1.0; moveit_msgs::msg::Constraints cmsg; cmsg.orientation_constraints.resize(1, ocm); diff --git a/moveit_ros/warehouse/warehouse/src/moveit_message_storage.cpp b/moveit_ros/warehouse/warehouse/src/moveit_message_storage.cpp index 6b5f793fa6f..634074ef875 100644 --- a/moveit_ros/warehouse/warehouse/src/moveit_message_storage.cpp +++ b/moveit_ros/warehouse/warehouse/src/moveit_message_storage.cpp @@ -36,9 +36,9 @@ #include #include -#include #include #include +#include moveit_warehouse::MoveItMessageStorage::MoveItMessageStorage(warehouse_ros::DatabaseConnection::Ptr conn) : conn_(std::move(conn)) @@ -50,11 +50,11 @@ void moveit_warehouse::MoveItMessageStorage::filterNames(const std::string& rege if (!regex.empty()) { std::vector fnames; - boost::regex r(regex); + std::regex r(regex); for (std::string& name : names) { - boost::cmatch match; - if (boost::regex_match(name.c_str(), match, r)) + std::smatch match; + if (std::regex_match(name, match, r)) fnames.push_back(name); } names.swap(fnames); diff --git a/moveit_ros/warehouse/warehouse/src/planning_scene_storage.cpp b/moveit_ros/warehouse/warehouse/src/planning_scene_storage.cpp index 0e8d97348f9..158459c0f0f 100644 --- a/moveit_ros/warehouse/warehouse/src/planning_scene_storage.cpp +++ b/moveit_ros/warehouse/warehouse/src/planning_scene_storage.cpp @@ -35,9 +35,9 @@ /* Author: Ioan Sucan */ #include -#include #include #include +#include const std::string moveit_warehouse::PlanningSceneStorage::DATABASE_NAME = "moveit_planning_scenes"; @@ -286,11 +286,11 @@ void moveit_warehouse::PlanningSceneStorage::getPlanningQueriesNames(const std:: if (!regex.empty()) { std::vector fnames; - boost::regex r(regex); + std::regex r(regex); for (const std::string& query_name : query_names) { - boost::cmatch match; - if (boost::regex_match(query_name.c_str(), match, r)) + std::smatch match; + if (std::regex_match(query_name, match, r)) { fnames.push_back(query_name); } diff --git a/moveit_ros/warehouse/warehouse/src/save_to_warehouse.cpp b/moveit_ros/warehouse/warehouse/src/save_to_warehouse.cpp index 6d392cf23b8..5ed6e68984c 100644 --- a/moveit_ros/warehouse/warehouse/src/save_to_warehouse.cpp +++ b/moveit_ros/warehouse/warehouse/src/save_to_warehouse.cpp @@ -116,9 +116,9 @@ void onRobotState(const moveit_msgs::msg::RobotState& msg, moveit_warehouse::Rob rs.getKnownRobotStates(names); std::set names_set(names.begin(), names.end()); std::size_t n = names.size(); - while (names_set.find("S" + boost::lexical_cast(n)) != names_set.end()) + while (names_set.find("S" + std::to_string(n)) != names_set.end()) n++; - std::string name = "S" + boost::lexical_cast(n); + std::string name = "S" + std::to_string(n); RCLCPP_INFO(LOGGER, "Adding robot state '%s'", name.c_str()); rs.addRobotState(msg, name); } diff --git a/moveit_setup_assistant/moveit_setup_srdf_plugins/src/compute_default_collisions.cpp b/moveit_setup_assistant/moveit_setup_srdf_plugins/src/compute_default_collisions.cpp index 11a8cbacece..86b0f110c15 100644 --- a/moveit_setup_assistant/moveit_setup_srdf_plugins/src/compute_default_collisions.cpp +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/src/compute_default_collisions.cpp @@ -554,9 +554,8 @@ unsigned int disableNeverInCollision(const unsigned int num_trials, planning_sce StringPairSet& links_seen_colliding, unsigned int* progress) { unsigned int num_disabled = 0; - - boost::thread_group bgroup; // create a group of threads - std::mutex lock; // used for sharing the same data structures + std::vector bgroup; + std::mutex lock; // used for sharing the same data structures int num_threads = std::thread::hardware_concurrency(); // how many cores does this computer have? // RCLCPP_INFO_STREAM_STREAM(LOGGER, "Performing " << num_trials << " trials for 'always in collision' checking on " << @@ -565,19 +564,12 @@ unsigned int disableNeverInCollision(const unsigned int num_trials, planning_sce for (int i = 0; i < num_threads; ++i) { ThreadComputation tc(scene, req, i, num_trials / num_threads, &links_seen_colliding, &lock, progress); - bgroup.create_thread([tc] { return disableNeverInCollisionThread(tc); }); + bgroup.push_back(std::thread([tc] { return disableNeverInCollisionThread(tc); })); } - try - { - bgroup.join_all(); // wait for all threads to finish - } - catch (boost::thread_interrupted) + for (auto& thread : bgroup) { - RCLCPP_WARN_STREAM(LOGGER, "disableNeverInCollision interrupted"); - bgroup.interrupt_all(); - bgroup.join_all(); // wait for all threads to interrupt - throw; + thread.join(); } // Loop through every possible link pair and check if it has ever been seen in collision diff --git a/moveit_setup_assistant/moveit_setup_srdf_plugins/src/planning_groups_widget.cpp b/moveit_setup_assistant/moveit_setup_srdf_plugins/src/planning_groups_widget.cpp index 4fd262dae76..be7b6bf5a59 100644 --- a/moveit_setup_assistant/moveit_setup_srdf_plugins/src/planning_groups_widget.cpp +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/src/planning_groups_widget.cpp @@ -1093,9 +1093,9 @@ bool PlanningGroupsWidget::saveGroupScreen() double kinematics_resolution_double; try { - kinematics_resolution_double = boost::lexical_cast(kinematics_resolution); + kinematics_resolution_double = std::stod(kinematics_resolution); } - catch (boost::bad_lexical_cast&) + catch (std::invalid_argument&) { QMessageBox::warning(this, "Error Saving", "Unable to convert kinematics resolution to a double number."); return false; @@ -1105,9 +1105,9 @@ bool PlanningGroupsWidget::saveGroupScreen() double kinematics_timeout_double; try { - kinematics_timeout_double = boost::lexical_cast(kinematics_timeout); + kinematics_timeout_double = std::stod(kinematics_timeout); } - catch (boost::bad_lexical_cast&) + catch (std::invalid_argument&) { QMessageBox::warning(this, "Error Saving", "Unable to convert kinematics solver timeout to a double number."); return false;