How to edit PlanningScene collision checking method to add objects to the environment?

MoveIt
1

Hello all,

I’ve been using MoveIt! to add an extra constraint during the collision checking process. To do this, I add a rectangular prism to the environment based on the position of the End Effector of my manipulator. I planned on adding the geometry by the method:
processCollisionObjectMsg()

I would call this method from:
void PlanningScene::checkCollision(const collision_detection::CollisionRequest& req,
collision_detection::CollisionResult& res,
const robot_state::RobotState& kstate) const

However, checkCollision is const and processCollisionObjectMsg() is not, and thus is uncallable. I am using OMPL (and the default fcl collision detection) to plan my paths. What is a good way for me add a simple rectangular prism into the collision environment ? Note, each rectangular prism should only apply to its respective sample; one sample’s rectangular prism should not affect another sample’s collision checking.

2

I’ve found the answer!

There’s a similar method

void PlanningScene::checkCollision(const collision_detection::CollisionRequest& req,
collision_detection::CollisionResult& res,
robot_state::RobotState& kstate) const

without a constant robotstate that calls the checkCollision method I mentioned above in planning_scene. RobotState can be altered to include an attached Body, which will be used for collision checking (make sure to call update() on the robot state afterwards).

I imagine you could create an imaginary link frame if you wanted to add the object to the robot for full robot collision-detection against the primitive.