ROS Resources: Documentation | Support | Discussion Forum | Service Status | Q&A answers.ros.org

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

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.

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.