@@ -440,8 +440,8 @@ void PlanningScene::checkCollision(const collision_detection::CollisionRequest&
440
440
" transforms before checking collision." );
441
441
}
442
442
// check collision with the world using the padded version
443
- req.use_padded_collision_environment ? getCollisionEnv ()->checkRobotCollision (req, res, robot_state, acm) :
444
- getCollisionEnvUnpadded ()->checkRobotCollision (req, res, robot_state, acm);
443
+ req.pad_environment_collisions ? getCollisionEnv ()->checkRobotCollision (req, res, robot_state, acm) :
444
+ getCollisionEnvUnpadded ()->checkRobotCollision (req, res, robot_state, acm);
445
445
446
446
// Return early if a collision was found or the maximum number of allowed contacts is exceeded
447
447
if (res.collision || (req.contacts && res.contacts .size () >= req.max_contacts ))
@@ -450,23 +450,23 @@ void PlanningScene::checkCollision(const collision_detection::CollisionRequest&
450
450
}
451
451
452
452
// do self-collision checking with the unpadded version of the robot
453
- req.use_padded_self_collision ? getCollisionEnv ()->checkSelfCollision (req, res, robot_state, acm) :
454
- getCollisionEnvUnpadded ()->checkSelfCollision (req, res, robot_state, acm);
453
+ req.pad_self_collisions ? getCollisionEnv ()->checkSelfCollision (req, res, robot_state, acm) :
454
+ getCollisionEnvUnpadded ()->checkSelfCollision (req, res, robot_state, acm);
455
455
}
456
456
457
457
void PlanningScene::checkCollisionUnpadded (const collision_detection::CollisionRequest& req,
458
458
collision_detection::CollisionResult& res)
459
459
{
460
460
collision_detection::CollisionRequest new_req = req;
461
- new_req.use_padded_collision_environment = false ;
461
+ new_req.pad_environment_collisions = false ;
462
462
checkCollision (req, res, getCurrentStateNonConst (), getAllowedCollisionMatrix ());
463
463
}
464
464
465
465
void PlanningScene::checkCollisionUnpadded (const collision_detection::CollisionRequest& req,
466
466
collision_detection::CollisionResult& res) const
467
467
{
468
468
collision_detection::CollisionRequest new_req = req;
469
- new_req.use_padded_collision_environment = false ;
469
+ new_req.pad_environment_collisions = false ;
470
470
checkCollision (new_req, res, getCurrentState (), getAllowedCollisionMatrix ());
471
471
}
472
472
@@ -475,7 +475,7 @@ void PlanningScene::checkCollisionUnpadded(const collision_detection::CollisionR
475
475
const moveit::core::RobotState& robot_state) const
476
476
{
477
477
collision_detection::CollisionRequest new_req = req;
478
- new_req.use_padded_collision_environment = false ;
478
+ new_req.pad_environment_collisions = false ;
479
479
checkCollision (new_req, res, robot_state, getAllowedCollisionMatrix ());
480
480
}
481
481
@@ -484,7 +484,7 @@ void PlanningScene::checkCollisionUnpadded(const collision_detection::CollisionR
484
484
moveit::core::RobotState& robot_state) const
485
485
{
486
486
collision_detection::CollisionRequest new_req = req;
487
- new_req.use_padded_collision_environment = false ;
487
+ new_req.pad_environment_collisions = false ;
488
488
checkCollision (new_req, res, static_cast <const moveit::core::RobotState&>(robot_state), getAllowedCollisionMatrix ());
489
489
}
490
490
@@ -495,7 +495,7 @@ void PlanningScene::checkCollisionUnpadded(const collision_detection::CollisionR
495
495
{
496
496
robot_state.updateCollisionBodyTransforms ();
497
497
collision_detection::CollisionRequest new_req = req;
498
- new_req.use_padded_collision_environment = false ;
498
+ new_req.pad_environment_collisions = false ;
499
499
checkCollision (new_req, res, static_cast <const moveit::core::RobotState&>(robot_state), acm);
500
500
}
501
501
@@ -505,7 +505,7 @@ void PlanningScene::checkCollisionUnpadded(const collision_detection::CollisionR
505
505
const collision_detection::AllowedCollisionMatrix& acm) const
506
506
{
507
507
collision_detection::CollisionRequest new_req = req;
508
- new_req.use_padded_collision_environment = false ;
508
+ new_req.pad_environment_collisions = false ;
509
509
checkCollision (req, res, robot_state, acm);
510
510
}
511
511
@@ -515,7 +515,6 @@ void PlanningScene::checkSelfCollision(const collision_detection::CollisionReque
515
515
checkSelfCollision (req, res, getCurrentStateNonConst ());
516
516
}
517
517
518
- /* * \brief Check whether the current state is in self collision */
519
518
void PlanningScene::checkSelfCollision (const collision_detection::CollisionRequest& req,
520
519
collision_detection::CollisionResult& res) const
521
520
{
@@ -556,8 +555,8 @@ void PlanningScene::checkSelfCollision(const collision_detection::CollisionReque
556
555
const moveit::core::RobotState& robot_state,
557
556
const collision_detection::AllowedCollisionMatrix& acm) const
558
557
{
559
- req.use_padded_self_collision ? getCollisionEnv ()->checkSelfCollision (req, res, robot_state, acm) :
560
- getCollisionEnvUnpadded ()->checkSelfCollision (req, res, robot_state, acm);
558
+ req.pad_self_collisions ? getCollisionEnv ()->checkSelfCollision (req, res, robot_state, acm) :
559
+ getCollisionEnvUnpadded ()->checkSelfCollision (req, res, robot_state, acm);
561
560
}
562
561
563
562
void PlanningScene::getCollidingPairs (collision_detection::CollisionResult::ContactMap& contacts)
0 commit comments