Skip to content

Commit

Permalink
Merge remote-tracking branch 'upstream/main' into feature/msa
Browse files Browse the repository at this point in the history
  • Loading branch information
Vatan Aksoy Tezer committed Jun 16, 2022
1 parent fcfe561 commit 1557db8
Show file tree
Hide file tree
Showing 121 changed files with 439 additions and 474 deletions.
6 changes: 3 additions & 3 deletions .github/workflows/ci.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -95,7 +95,7 @@ jobs:
with:
file: moveit2.repos
- name: Cache upstream workspace
uses: pat-s/always-upload-cache@v2.1.5
uses: pat-s/always-upload-cache@v3.0.1
with:
path: ${{ env.BASEDIR }}/upstream_ws
key: ${{ env.CACHE_PREFIX }}-${{ github.run_id }}
Expand All @@ -106,15 +106,15 @@ jobs:
# that comes from the checkout. See "prepare target_ws for cache" task below
- name: Cache target workspace
if: "!matrix.env.CCOV"
uses: pat-s/always-upload-cache@v2.1.5
uses: pat-s/always-upload-cache@v3.0.1
with:
path: ${{ env.BASEDIR }}/target_ws
key: ${{ env.CACHE_PREFIX }}-${{ github.run_id }}
restore-keys: ${{ env.CACHE_PREFIX }}
env:
CACHE_PREFIX: target_ws${{ matrix.env.CCOV && '-ccov' || '' }}-${{ matrix.env.IMAGE }}-${{ hashFiles('**/CMakeLists.txt', '**/package.xml', '.github/workflows/ci.yaml') }}
- name: Cache ccache
uses: pat-s/always-upload-cache@v2.1.5
uses: pat-s/always-upload-cache@v3.0.1
with:
path: ${{ env.CCACHE_DIR }}
key: ${{ env.CACHE_PREFIX }}-${{ github.sha }}-${{ github.run_id }}
Expand Down
4 changes: 3 additions & 1 deletion .github/workflows/format.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -16,7 +16,9 @@ jobs:
runs-on: ubuntu-22.04
steps:
- uses: actions/checkout@v3
- uses: actions/setup-python@v2
- uses: actions/setup-python@v4
with:
python-version: '3.10'
- name: Install clang-format-12
run: sudo apt-get install clang-format-12
- uses: pre-commit/[email protected]
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -36,8 +36,8 @@

#pragma once

#include <boost/array.hpp>
#include <boost/function.hpp>
#include <array>
#include <functional>
#include <vector>
#include <string>
#include <map>
Expand Down Expand Up @@ -110,10 +110,10 @@ struct Contact
struct CostSource
{
/// The minimum bound of the AABB defining the volume responsible for this partial cost
boost::array<double, 3> aabb_min;
std::array<double, 3> aabb_min;

/// The maximum bound of the AABB defining the volume responsible for this partial cost
boost::array<double, 3> aabb_max;
std::array<double, 3> aabb_max;

/// The partial cost (the probability of existence for the object there is a collision with)
double cost;
Expand Down Expand Up @@ -220,7 +220,7 @@ struct CollisionRequest
std::size_t max_cost_sources;

/** \brief Function call that decides whether collision detection should stop. */
boost::function<bool(const CollisionResult&)> is_done;
std::function<bool(const CollisionResult&)> is_done;

/** \brief Flag indicating whether information about detected collisions should be reported */
bool verbose;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -68,7 +68,7 @@ enum Type

/** \brief Signature of predicate that decides whether a contact is allowed or not (when AllowedCollision::Type is
* CONDITIONAL) */
using DecideContactFn = boost::function<bool(collision_detection::Contact&)>;
using DecideContactFn = std::function<bool(collision_detection::Contact&)>;

MOVEIT_CLASS_FORWARD(AllowedCollisionMatrix); // Defines AllowedCollisionMatrixPtr, ConstPtr, WeakPtr... etc

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -38,12 +38,11 @@

#include <octomap/octomap.h>

#include <boost/thread/locks.hpp>
#include <boost/thread/shared_mutex.hpp>
#include <boost/function.hpp>

#include <memory>
#include <string>
#include <shared_mutex>
#include <mutex>
#include <functional>

namespace collision_detection
{
Expand Down Expand Up @@ -86,8 +85,8 @@ class OccMapTree : public octomap::OcTree
tree_mutex_.unlock();
}

using ReadLock = boost::shared_lock<boost::shared_mutex>;
using WriteLock = boost::unique_lock<boost::shared_mutex>;
using ReadLock = std::shared_lock<std::shared_mutex>;
using WriteLock = std::unique_lock<std::shared_mutex>;

ReadLock reading()
{
Expand All @@ -106,14 +105,14 @@ class OccMapTree : public octomap::OcTree
}

/** @brief Set the callback to trigger when updates are received */
void setUpdateCallback(const boost::function<void()>& update_callback)
void setUpdateCallback(const std::function<void()>& update_callback)
{
update_callback_ = update_callback;
}

private:
boost::shared_mutex tree_mutex_;
boost::function<void()> update_callback_;
std::shared_mutex tree_mutex_;
std::function<void()> update_callback_;
};

using OccMapTreePtr = std::shared_ptr<OccMapTree>;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -40,7 +40,7 @@
#include <string>
#include <vector>
#include <map>
#include <boost/function.hpp>
#include <functional>
#include <Eigen/Geometry>
#include <eigen_stl_containers/eigen_stl_vector_container.h>
#include <moveit/transforms/transforms.h>
Expand Down Expand Up @@ -298,7 +298,7 @@ class World
friend class World;
};

using ObserverCallbackFn = boost::function<void(const ObjectConstPtr&, Action)>;
using ObserverCallbackFn = std::function<void(const ObjectConstPtr&, Action)>;

/** \brief register a callback function for notification of changes.
* \e callback will be called right after any change occurs to any Object.
Expand Down
2 changes: 1 addition & 1 deletion moveit_core/collision_detection/src/collision_matrix.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -147,7 +147,7 @@ void AllowedCollisionMatrix::setEntry(const std::string& name1, const std::strin
const AllowedCollision::Type v = allowed ? AllowedCollision::ALWAYS : AllowedCollision::NEVER;
entries_[name1][name2] = entries_[name2][name1] = v;

// remove boost::function pointers, if any
// remove function pointers, if any
auto it = allowed_contacts_.find(name1);
if (it != allowed_contacts_.end())
{
Expand Down
6 changes: 4 additions & 2 deletions moveit_core/collision_detection/src/world.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -140,7 +140,8 @@ bool World::knowsTransform(const std::string& name) const
for (const std::pair<const std::string, ObjectPtr>& object : objects_)
{
// if "object name/" matches start of object_id, we found the matching object
if (boost::starts_with(name, object.first) && name[object.first.length()] == '/')
// rfind searches name for object.first in the first index (returns 0 if found)
if (name.rfind(object.first, 0) == 0 && name[object.first.length()] == '/')
{
return object.second->global_subframe_poses_.find(name.substr(object.first.length() + 1)) !=
object.second->global_subframe_poses_.end();
Expand Down Expand Up @@ -174,7 +175,8 @@ const Eigen::Isometry3d& World::getTransform(const std::string& name, bool& fram
for (const std::pair<const std::string, ObjectPtr>& object : objects_)
{
// if "object name/" matches start of object_id, we found the matching object
if (boost::starts_with(name, object.first) && name[object.first.length()] == '/')
// name.rfind is in service of removing the call to boost::starts_with and does the same thing
if (name.rfind(object.first, 0) == 0 && name[object.first.length()] == '/')
{
auto it = object.second->global_subframe_poses_.find(name.substr(object.first.length() + 1));
if (it != object.second->global_subframe_poses_.end())
Expand Down
1 change: 0 additions & 1 deletion moveit_core/collision_detection_bullet/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -22,7 +22,6 @@ ament_target_dependencies(${MOVEIT_LIB_NAME}
urdf
urdfdom
urdfdom_headers
Boost
visualization_msgs
octomap_msgs
)
Expand Down
1 change: 0 additions & 1 deletion moveit_core/collision_detection_fcl/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,6 @@ ament_target_dependencies(${MOVEIT_LIB_NAME}
urdfdom
urdfdom_headers
LIBFCL
Boost
visualization_msgs
)
target_link_libraries(${MOVEIT_LIB_NAME}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -49,9 +49,9 @@
#include <fcl/octree.h>
#endif

#include <boost/thread/mutex.hpp>
#include <memory>
#include <type_traits>
#include <mutex>

namespace collision_detection
{
Expand Down
1 change: 0 additions & 1 deletion moveit_core/collision_distance_field/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -35,7 +35,6 @@ if(BUILD_TESTING)

ament_add_gtest(test_collision_distance_field test/test_collision_distance_field.cpp)
ament_target_dependencies(test_collision_distance_field
Boost
geometric_shapes
OCTOMAP
srdfdom
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -41,8 +41,8 @@
#include <moveit/collision_distance_field/collision_common_distance_field.h>
#include <moveit/collision_detection/collision_env.h>
#include <moveit/planning_scene/planning_scene.h>
#include <boost/thread/mutex.hpp>
#include "rclcpp/rclcpp.hpp"
#include <mutex>

namespace collision_detection
{
Expand Down Expand Up @@ -296,14 +296,14 @@ class CollisionEnvDistanceField : public CollisionEnv
std::vector<BodyDecompositionConstPtr> link_body_decomposition_vector_;
std::map<std::string, unsigned int> link_body_decomposition_index_map_;

mutable boost::mutex update_cache_lock_;
mutable std::mutex update_cache_lock_;
DistanceFieldCacheEntryPtr distance_field_cache_entry_;
std::map<std::string, std::map<std::string, bool>> in_group_update_map_;
std::map<std::string, GroupStateRepresentationPtr> pregenerated_group_state_representation_map_;

planning_scene::PlanningScenePtr planning_scene_;

mutable boost::mutex update_cache_lock_world_;
mutable std::mutex update_cache_lock_world_;
DistanceFieldCacheEntryWorldPtr distance_field_cache_entry_world_;
GroupStateRepresentationPtr last_gsr_;
World::ObserverHandle observer_handle_;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -41,8 +41,6 @@
#include <moveit/collision_distance_field/collision_common_distance_field.h>
#include <moveit/collision_distance_field/collision_env_distance_field.h>

#include <boost/thread/mutex.hpp>

namespace collision_detection
{
/** \brief This hybrid collision environment combines FCL and a distance field. Both can be used to calculate
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -35,7 +35,6 @@
/* Author: E. Gil Jones */

#include <moveit/collision_distance_field/collision_common_distance_field.h>
#include <boost/thread/mutex.hpp>
#include <rclcpp/duration.hpp>
#include <rclcpp/logger.hpp>
#include <rclcpp/logging.hpp>
Expand All @@ -46,6 +45,7 @@
#include <tf2_eigen/tf2_eigen.h>
#endif
#include <memory>
#include <mutex>

namespace collision_detection
{
Expand All @@ -62,7 +62,7 @@ struct BodyDecompositionCache
static const unsigned int MAX_CLEAN_COUNT = 100;
Map map_;
unsigned int clean_count_;
boost::mutex lock_;
std::mutex lock_;
};

BodyDecompositionCache& getBodyDecompositionCache()
Expand All @@ -77,7 +77,7 @@ BodyDecompositionConstPtr getBodyDecompositionCacheEntry(const shapes::ShapeCons
BodyDecompositionCache& cache = getBodyDecompositionCache();
shapes::ShapeConstWeakPtr wptr(shape);
{
boost::mutex::scoped_lock slock(cache.lock_);
std::scoped_lock slock(cache.lock_);
BodyDecompositionCache::Map::const_iterator cache_it = cache.map_.find(wptr);
if (cache_it != cache.map_.end())
{
Expand All @@ -87,7 +87,7 @@ BodyDecompositionConstPtr getBodyDecompositionCacheEntry(const shapes::ShapeCons

BodyDecompositionConstPtr bdcp = std::make_shared<const BodyDecomposition>(shape, resolution);
{
boost::mutex::scoped_lock slock(cache.lock_);
std::scoped_lock slock(cache.lock_);
cache.map_[wptr] = bdcp;
cache.clean_count_++;
return bdcp;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -171,7 +171,7 @@ void CollisionEnvDistanceField::generateCollisionCheckingStructures(
// DistanceFieldCacheEntry for CollisionRobot");
DistanceFieldCacheEntryPtr new_dfce =
generateDistanceFieldCacheEntry(group_name, state, acm, generate_distance_field);
boost::mutex::scoped_lock slock(update_cache_lock_);
std::scoped_lock slock(update_cache_lock_);
(const_cast<CollisionEnvDistanceField*>(this))->distance_field_cache_entry_ = new_dfce;
dfce = new_dfce;
}
Expand Down
1 change: 0 additions & 1 deletion moveit_core/kinematics_base/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,6 @@ ament_target_dependencies(
srdfdom moveit_msgs
geometric_shapes
geometry_msgs
Boost
)

install(DIRECTORY include/ DESTINATION include)
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -39,10 +39,10 @@
#include <geometry_msgs/msg/pose.hpp>
#include <moveit_msgs/msg/move_it_error_codes.hpp>
#include <moveit/macros/class_forward.h>
#include <boost/function.hpp>
#include <rclcpp/logging.hpp>
#include <rclcpp/node.hpp>
#include <string>
#include <functional>

#include "moveit_kinematics_base_export.h"

Expand Down Expand Up @@ -151,8 +151,8 @@ class MOVEIT_KINEMATICS_BASE_EXPORT KinematicsBase
static const double DEFAULT_TIMEOUT; /* = 1.0 */

/** @brief Signature for a callback to validate an IK solution. Typically used for collision checking. */
using IKCallbackFn = boost::function<void(const geometry_msgs::msg::Pose&, const std::vector<double>&,
moveit_msgs::msg::MoveItErrorCodes&)>;
using IKCallbackFn = std::function<void(const geometry_msgs::msg::Pose&, const std::vector<double>&,
moveit_msgs::msg::MoveItErrorCodes&)>;

/**
* @brief Given a desired pose of the end-effector, compute the joint angles to reach it
Expand Down
5 changes: 3 additions & 2 deletions moveit_core/kinematics_metrics/src/kinematics_metrics.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -40,6 +40,7 @@
#include <Eigen/Dense>
#include <Eigen/Eigenvalues>
#include <boost/math/constants/constants.hpp>
#include <limits>

namespace kinematics_metrics
{
Expand All @@ -48,7 +49,7 @@ static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_kinematics_metri
double KinematicsMetrics::getJointLimitsPenalty(const moveit::core::RobotState& state,
const moveit::core::JointModelGroup* joint_model_group) const
{
if (fabs(penalty_multiplier_) <= boost::math::tools::epsilon<double>())
if (fabs(penalty_multiplier_) <= std::numeric_limits<double>::min())
return 1.0;
double joint_limits_multiplier(1.0);
const std::vector<const moveit::core::JointModel*>& joint_model_vector = joint_model_group->getJointModels();
Expand Down Expand Up @@ -88,7 +89,7 @@ double KinematicsMetrics::getJointLimitsPenalty(const moveit::core::RobotState&
double lower_bound_distance = joint_model->distance(joint_values, &lower_bounds[0]);
double upper_bound_distance = joint_model->distance(joint_values, &upper_bounds[0]);
double range = lower_bound_distance + upper_bound_distance;
if (range <= boost::math::tools::epsilon<double>())
if (range <= std::numeric_limits<double>::min())
continue;
joint_limits_multiplier *= (lower_bound_distance * upper_bound_distance / (range * range));
}
Expand Down
10 changes: 5 additions & 5 deletions moveit_core/planning_interface/src/planning_interface.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -35,7 +35,7 @@
/* Author: Ioan Sucan */

#include <moveit/planning_interface/planning_interface.h>
#include <boost/thread/mutex.hpp>
#include <mutex>
#include <rclcpp/logger.hpp>
#include <rclcpp/logging.hpp>
#include <set>
Expand All @@ -49,7 +49,7 @@ namespace
// keep track of currently active contexts
struct ActiveContexts
{
boost::mutex mutex_;
std::mutex mutex_;
std::set<PlanningContext*> contexts_;
};

Expand All @@ -63,14 +63,14 @@ static ActiveContexts& getActiveContexts()
PlanningContext::PlanningContext(const std::string& name, const std::string& group) : name_(name), group_(group)
{
ActiveContexts& ac = getActiveContexts();
boost::mutex::scoped_lock _(ac.mutex_);
std::scoped_lock _(ac.mutex_);
ac.contexts_.insert(this);
}

PlanningContext::~PlanningContext()
{
ActiveContexts& ac = getActiveContexts();
boost::mutex::scoped_lock _(ac.mutex_);
std::scoped_lock _(ac.mutex_);
ac.contexts_.erase(this);
}

Expand Down Expand Up @@ -126,7 +126,7 @@ void PlannerManager::setPlannerConfigurations(const PlannerConfigurationMap& pcs
void PlannerManager::terminate() const
{
ActiveContexts& ac = getActiveContexts();
boost::mutex::scoped_lock _(ac.mutex_);
std::scoped_lock _(ac.mutex_);
for (PlanningContext* context : ac.contexts_)
context->terminate();
}
Expand Down
Loading

0 comments on commit 1557db8

Please sign in to comment.