Merge moveit/moveit@779b7c8

.clang-tidy

@@ -9,6 +9,8 @@ Checks:          '-*,
                   modernize-use-default,
                   modernize-use-override,
                   modernize-loop-convert,
+                  modernize-make-shared,
+                  modernize-make-unique,
                   readability-named-parameter,
                   readability-redundant-smartptr-get,
                   readability-redundant-string-cstr,

moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h

@@ -74,11 +74,11 @@ class CollisionDetectorPandaTest : public testing::Test
 protected:
   void SetUp() override
   {
-    value_.reset(new CollisionAllocatorType);
+    value_ = std::make_shared<CollisionAllocatorType>();
     robot_model_ = moveit::core::loadTestingRobotModel("panda");
     robot_model_ok_ = static_cast<bool>(robot_model_);
 
-    acm_.reset(new collision_detection::AllowedCollisionMatrix());
+    acm_ = std::make_shared<collision_detection::AllowedCollisionMatrix>();
     // Use default collision operations in the SRDF to setup the acm
     const std::vector<std::string>& collision_links = robot_model_->getLinkModelNamesWithCollisionGeometry();
     acm_->setEntry(collision_links, collision_links, false);
@@ -90,7 +90,7 @@ class CollisionDetectorPandaTest : public testing::Test
 
     cenv_ = value_->allocateEnv(robot_model_);
 
-    robot_state_.reset(new moveit::core::RobotState(robot_model_));
+    robot_state_ = std::make_shared<moveit::core::RobotState>(robot_model_);
     setToHome(*robot_state_);
   }
 
@@ -336,13 +336,13 @@ TYPED_TEST_P(DistanceFullPandaTest, DistancePoints)
   this->cenv_->distanceRobot(req, res, *this->robot_state_);
 
   // Checks a particular point is inside the box
-  auto checkInBox = [&](const Eigen::Vector3d& p) {
-    Eigen::Vector3d inBox = pos.inverse() * p;
+  auto check_in_box = [&](const Eigen::Vector3d& p) {
+    Eigen::Vector3d in_box = pos.inverse() * p;
 
-    constexpr double EPS = 1e-5;
-    EXPECT_LE(std::abs(inBox.x()), shape->size[0] + EPS);
-    EXPECT_LE(std::abs(inBox.y()), shape->size[1] + EPS);
-    EXPECT_LE(std::abs(inBox.z()), shape->size[2] + EPS);
+    constexpr double eps = 1e-5;
+    EXPECT_LE(std::abs(in_box.x()), shape->size[0] + eps);
+    EXPECT_LE(std::abs(in_box.y()), shape->size[1] + eps);
+    EXPECT_LE(std::abs(in_box.z()), shape->size[2] + eps);
   };
 
   // Check that all points reported on "box" are actually on the box and not
@@ -352,9 +352,9 @@ TYPED_TEST_P(DistanceFullPandaTest, DistancePoints)
     for (auto& pair : distance.second)
     {
       if (pair.link_names[0] == "box")
-        checkInBox(pair.nearest_points[0]);
+        check_in_box(pair.nearest_points[0]);
       else if (pair.link_names[1] == "box")
-        checkInBox(pair.nearest_points[1]);
+        check_in_box(pair.nearest_points[1]);
       else
         ADD_FAILURE() << "Unrecognized link names";
     }

moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_pr2.h

@@ -62,12 +62,12 @@ class CollisionDetectorTest : public ::testing::Test
 
   void SetUp() override
   {
-    value_.reset(new CollisionAllocatorType);
+    value_ = std::make_shared<CollisionAllocatorType>();
     robot_model_ = moveit::core::loadTestingRobotModel("pr2");
     robot_model_ok_ = static_cast<bool>(robot_model_);
     kinect_dae_resource_ = "package://moveit_resources_pr2_description/urdf/meshes/sensors/kinect_v0/kinect.dae";
 
-    acm_.reset(new collision_detection::AllowedCollisionMatrix(robot_model_->getLinkModelNames(), true));
+    acm_ = std::make_shared<collision_detection::AllowedCollisionMatrix>(robot_model_->getLinkModelNames(), true);
 
     cenv_ = value_->allocateEnv(robot_model_);
   }
@@ -195,7 +195,8 @@ TYPED_TEST_P(CollisionDetectorTest, ContactReporting)
 
   req.max_contacts = 10;
   req.max_contacts_per_pair = 2;
-  this->acm_.reset(new collision_detection::AllowedCollisionMatrix(this->robot_model_->getLinkModelNames(), false));
+  this->acm_ =
+      std::make_shared<collision_detection::AllowedCollisionMatrix>(this->robot_model_->getLinkModelNames(), false);
   this->cenv_->checkSelfCollision(req, res, robot_state, *this->acm_);
   ASSERT_TRUE(res.collision);
   EXPECT_LE(res.contacts.size(), 10u);
@@ -278,7 +279,8 @@ TYPED_TEST_P(CollisionDetectorTest, AttachedBodyTester)
   collision_detection::CollisionRequest req;
   collision_detection::CollisionResult res;
 
-  this->acm_.reset(new collision_detection::AllowedCollisionMatrix(this->robot_model_->getLinkModelNames(), true));
+  this->acm_ =
+      std::make_shared<collision_detection::AllowedCollisionMatrix>(this->robot_model_->getLinkModelNames(), true);
 
   moveit::core::RobotState robot_state(this->robot_model_);
   robot_state.setToDefaultValues();
@@ -320,7 +322,7 @@ TYPED_TEST_P(CollisionDetectorTest, AttachedBodyTester)
 
   touch_links.push_back("r_gripper_palm_link");
   touch_links.push_back("r_gripper_motor_accelerometer_link");
-  shapes[0].reset(new shapes::Box(.1, .1, .1));
+  shapes[0] = std::make_shared<shapes::Box>(.1, .1, .1);
   robot_state.attachBody("box", shapes, poses, touch_links, "r_gripper_palm_link");
   robot_state.update();
 

moveit_core/collision_detection/src/collision_env.cpp

@@ -287,7 +287,7 @@ void CollisionEnv::setWorld(const WorldPtr& world)
 {
   world_ = world;
   if (!world_)
-    world_.reset(new World());
+    world_ = std::make_shared<World>();
 
   world_const_ = world;
 }

moveit_core/collision_detection/src/world.cpp

@@ -85,7 +85,7 @@ void World::addToObject(const std::string& id, const std::vector<shapes::ShapeCo
   ObjectPtr& obj = objects_[id];
   if (!obj)
   {
-    obj.reset(new Object(id));
+    obj = std::make_shared<Object>(id);
     action |= CREATE;
   }
 
@@ -104,7 +104,7 @@ void World::addToObject(const std::string& id, const shapes::ShapeConstPtr& shap
   ObjectPtr& obj = objects_[id];
   if (!obj)
   {
-    obj.reset(new Object(id));
+    obj = std::make_shared<Object>(id);
     action |= CREATE;
   }
 
@@ -134,7 +134,7 @@ World::ObjectConstPtr World::getObject(const std::string& object_id) const
 void World::ensureUnique(ObjectPtr& obj)
 {
   if (obj && !obj.unique())
-    obj.reset(new Object(*obj));
+    obj = std::make_shared<Object>(*obj);
 }
 
 bool World::hasObject(const std::string& object_id) const

moveit_core/collision_detection/test/test_all_valid.cpp

@@ -44,11 +44,11 @@ TEST(AllValid, Instantiate)
   CollisionDetectorAllocatorAllValid allocator;
 
   urdf::ModelInterfaceSharedPtr urdf_model;
-  urdf_model.reset(new urdf::ModelInterface());
+  urdf_model = std::make_shared<urdf::ModelInterface>();
   srdf::ModelConstSharedPtr srdf_model;
-  srdf_model.reset(new srdf::Model());
+  srdf_model = std::make_shared<srdf::Model>();
   moveit::core::RobotModelConstPtr robot_model;
-  robot_model.reset(new moveit::core::RobotModel(urdf_model, srdf_model));
+  robot_model = std::make_shared<moveit::core::RobotModel>(urdf_model, srdf_model);
   CollisionEnvAllValid env(robot_model);
 }
 

moveit_core/collision_detection_bullet/src/bullet_integration/bullet_bvh_manager.cpp

@@ -39,7 +39,7 @@ namespace collision_detection_bullet
 {
 BulletBVHManager::BulletBVHManager()
 {
-  dispatcher_.reset(new btCollisionDispatcher(&coll_config_));
+  dispatcher_ = std::make_unique<btCollisionDispatcher>(&coll_config_);
 
   dispatcher_->registerCollisionCreateFunc(BOX_SHAPE_PROXYTYPE, BOX_SHAPE_PROXYTYPE,
                                            coll_config_.getCollisionAlgorithmCreateFunc(CONVEX_SHAPE_PROXYTYPE,
@@ -48,7 +48,7 @@ BulletBVHManager::BulletBVHManager()
   dispatcher_->setDispatcherFlags(dispatcher_->getDispatcherFlags() &
                                   ~btCollisionDispatcher::CD_USE_RELATIVE_CONTACT_BREAKING_THRESHOLD);
 
-  broadphase_.reset(new btDbvtBroadphase());
+  broadphase_ = std::make_unique<btDbvtBroadphase>();
 
   broadphase_->getOverlappingPairCache()->setOverlapFilterCallback(&filter_callback_);
 

moveit_core/collision_detection_bullet/test/test_bullet_continuous_collision_checking.cpp

@@ -78,7 +78,7 @@ class BulletCollisionDetectionTester : public testing::Test
     robot_model_ = moveit::core::loadTestingRobotModel("panda");
     robot_model_ok_ = static_cast<bool>(robot_model_);
 
-    acm_.reset(new collision_detection::AllowedCollisionMatrix());
+    acm_ = std::make_shared<collision_detection::AllowedCollisionMatrix>();
     // Use default collision operations in the SRDF to setup the acm
     const std::vector<std::string>& collision_links = robot_model_->getLinkModelNamesWithCollisionGeometry();
     acm_->setEntry(collision_links, collision_links, false);
@@ -88,9 +88,9 @@ class BulletCollisionDetectionTester : public testing::Test
     for (const srdf::Model::DisabledCollision& it : dc)
       acm_->setEntry(it.link1_, it.link2_, true);
 
-    cenv_.reset(new collision_detection::CollisionEnvBullet(robot_model_));
+    cenv_ = std::make_shared<collision_detection::CollisionEnvBullet>(robot_model_);
 
-    robot_state_.reset(new moveit::core::RobotState(robot_model_));
+    robot_state_ = std::make_shared<moveit::core::RobotState>(robot_model_);
 
     setToHome(*robot_state_);
   }
@@ -186,8 +186,7 @@ void addCollisionObjectsMesh(cb::BulletCastBVHManager& checker)
   s_pose.setIdentity();
 
   std::string kinect = "package://moveit_resources_panda_description/meshes/collision/hand.stl";
-  shapes::ShapeConstPtr s;
-  s.reset(shapes::createMeshFromResource(kinect));
+  auto s = std::shared_ptr<shapes::Shape>{ shapes::createMeshFromResource(kinect) };
   obj2_shapes.push_back(s);
   obj2_types.push_back(cb::CollisionObjectType::CONVEX_HULL);
   obj2_poses.push_back(s_pose);

moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_common.h

@@ -233,7 +233,7 @@ struct FCLGeometry
     if (!newType && collision_geometry_data_)
       if (collision_geometry_data_->ptr.raw == reinterpret_cast<const void*>(data))
         return;
-    collision_geometry_data_.reset(new CollisionGeometryData(data, shape_index));
+    collision_geometry_data_ = std::make_shared<CollisionGeometryData>(data, shape_index);
     collision_geometry_->setUserData(collision_geometry_data_.get());
   }
 

moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp

@@ -102,9 +102,7 @@ CollisionEnvFCL::CollisionEnvFCL(const moveit::core::RobotModelConstPtr& model,
         RCLCPP_ERROR(LOGGER, "Unable to construct collision geometry for link '%s'", link->getName().c_str());
     }
 
-  auto m = new fcl::DynamicAABBTreeCollisionManagerd();
-  // m->tree_init_level = 2;
-  manager_.reset(m);
+  manager_ = std::make_unique<fcl::DynamicAABBTreeCollisionManagerd>();
 
   // request notifications about changes to new world
   observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionEnvFCL::notifyObjectChange, this, _1, _2));
@@ -139,9 +137,7 @@ CollisionEnvFCL::CollisionEnvFCL(const moveit::core::RobotModelConstPtr& model,
         RCLCPP_ERROR(LOGGER, "Unable to construct collision geometry for link '%s'", link->getName().c_str());
     }
 
-  auto m = new fcl::DynamicAABBTreeCollisionManagerd();
-  // m->tree_init_level = 2;
-  manager_.reset(m);
+  manager_ = std::make_unique<fcl::DynamicAABBTreeCollisionManagerd>();
 
   // request notifications about changes to new world
   observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionEnvFCL::notifyObjectChange, this, _1, _2));
@@ -158,9 +154,7 @@ CollisionEnvFCL::CollisionEnvFCL(const CollisionEnvFCL& other, const WorldPtr& w
   robot_geoms_ = other.robot_geoms_;
   robot_fcl_objs_ = other.robot_fcl_objs_;
 
-  auto m = new fcl::DynamicAABBTreeCollisionManagerd();
-  // m->tree_init_level = 2;
-  manager_.reset(m);
+  manager_ = std::make_unique<fcl::DynamicAABBTreeCollisionManagerd>();
 
   fcl_objs_ = other.fcl_objs_;
   for (auto& fcl_obj : fcl_objs_)
@@ -230,7 +224,7 @@ void CollisionEnvFCL::constructFCLObjectRobot(const moveit::core::RobotState& st
       {
         transform2fcl(ab_t[k], fcl_tf);
         fcl_obj.collision_objects_.push_back(
-            FCLCollisionObjectPtr(new fcl::CollisionObjectd(objs[k]->collision_geometry_, fcl_tf)));
+            std::make_shared<fcl::CollisionObjectd>(objs[k]->collision_geometry_, fcl_tf));
         // we copy the shared ptr to the CollisionGeometryData, as this is not stored by the class itself,
         // and would be destroyed when objs goes out of scope.
         fcl_obj.collision_geometry_.push_back(objs[k]);
@@ -240,12 +234,10 @@ void CollisionEnvFCL::constructFCLObjectRobot(const moveit::core::RobotState& st
 
 void CollisionEnvFCL::allocSelfCollisionBroadPhase(const moveit::core::RobotState& state, FCLManager& manager) const
 {
-  auto m = new fcl::DynamicAABBTreeCollisionManagerd();
-  // m->tree_init_level = 2;
-  manager.manager_.reset(m);
+  manager.manager_ = std::make_unique<fcl::DynamicAABBTreeCollisionManagerd>();
+
   constructFCLObjectRobot(state, manager.object_);
   manager.object_.registerTo(manager.manager_.get());
-  // manager.manager_->update();
 }
 
 void CollisionEnvFCL::checkSelfCollision(const CollisionRequest& req, CollisionResult& res,

moveit_core/collision_detection_fcl/test/test_fcl_env.cpp

@@ -73,7 +73,7 @@ class CollisionDetectionEnvTest : public testing::Test
     robot_model_ = moveit::core::loadTestingRobotModel("panda");
     robot_model_ok_ = static_cast<bool>(robot_model_);
 
-    acm_.reset(new collision_detection::AllowedCollisionMatrix(robot_model_->getLinkModelNames(), false));
+    acm_ = std::make_shared<collision_detection::AllowedCollisionMatrix>(robot_model_->getLinkModelNames(), false);
 
     acm_->setEntry("panda_link0", "panda_link1", true);
     acm_->setEntry("panda_link1", "panda_link2", true);
@@ -89,9 +89,9 @@ class CollisionDetectionEnvTest : public testing::Test
     acm_->setEntry("panda_link5", "panda_link7", true);
     acm_->setEntry("panda_link6", "panda_hand", true);
 
-    c_env_.reset(new collision_detection::CollisionEnvFCL(robot_model_));
+    c_env_ = std::make_shared<collision_detection::CollisionEnvFCL>(robot_model_);
 
-    robot_state_.reset(new moveit::core::RobotState(robot_model_));
+    robot_state_ = std::make_shared<moveit::core::RobotState>(robot_model_);
 
     setToHome(*robot_state_);
   }

moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_common_distance_field.h

@@ -65,7 +65,8 @@ struct GroupStateRepresentation
     {
       if (gsr.link_body_decompositions_[i])
       {
-        link_body_decompositions_[i].reset(new PosedBodySphereDecomposition(*gsr.link_body_decompositions_[i]));
+        link_body_decompositions_[i] =
+            std::make_shared<PosedBodySphereDecomposition>(*gsr.link_body_decompositions_[i]);
       }
     }