collision_world_fcl.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Ioan Sucan */
36 
39 #include <fcl/shape/geometric_shape_to_BVH_model.h>
40 #include <fcl/traversal/traversal_node_bvhs.h>
41 #include <fcl/traversal/traversal_node_setup.h>
42 #include <fcl/collision_node.h>
43 #include <boost/bind.hpp>
44 
45 namespace collision_detection
46 {
47 const std::string CollisionDetectorAllocatorFCL::NAME_("FCL");
48 
50 {
51  auto m = new fcl::DynamicAABBTreeCollisionManager();
52  // m->tree_init_level = 2;
53  manager_.reset(m);
54 
55  // request notifications about changes to new world
56  observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionWorldFCL::notifyObjectChange, this, _1, _2));
57 }
58 
60 {
61  auto m = new fcl::DynamicAABBTreeCollisionManager();
62  // m->tree_init_level = 2;
63  manager_.reset(m);
64 
65  // request notifications about changes to new world
66  observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionWorldFCL::notifyObjectChange, this, _1, _2));
67  getWorld()->notifyObserverAllObjects(observer_handle_, World::CREATE);
68 }
69 
70 CollisionWorldFCL::CollisionWorldFCL(const CollisionWorldFCL& other, const WorldPtr& world)
71  : CollisionWorld(other, world)
72 {
73  auto m = new fcl::DynamicAABBTreeCollisionManager();
74  // m->tree_init_level = 2;
75  manager_.reset(m);
76 
77  fcl_objs_ = other.fcl_objs_;
78  for (auto& fcl_obj : fcl_objs_)
79  fcl_obj.second.registerTo(manager_.get());
80  // manager_->update();
81 
82  // request notifications about changes to new world
83  observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionWorldFCL::notifyObjectChange, this, _1, _2));
84 }
85 
87 {
88  getWorld()->removeObserver(observer_handle_);
89 }
90 
92  const CollisionRobot& robot, const robot_state::RobotState& state) const
93 {
94  checkRobotCollisionHelper(req, res, robot, state, nullptr);
95 }
96 
98  const CollisionRobot& robot, const robot_state::RobotState& state,
99  const AllowedCollisionMatrix& acm) const
100 {
101  checkRobotCollisionHelper(req, res, robot, state, &acm);
102 }
103 
105  const CollisionRobot& robot, const robot_state::RobotState& state1,
106  const robot_state::RobotState& state2) const
107 {
108  ROS_ERROR_NAMED("collision_detection.fcl", "FCL continuous collision checking not yet implemented");
109 }
110 
112  const CollisionRobot& robot, const robot_state::RobotState& state1,
113  const robot_state::RobotState& state2,
114  const AllowedCollisionMatrix& acm) const
115 {
116  ROS_ERROR_NAMED("collision_detection.fcl", "FCL continuous collision checking not yet implemented");
117 }
118 
120  const CollisionRobot& robot, const robot_state::RobotState& state,
121  const AllowedCollisionMatrix* acm) const
122 {
123  const CollisionRobotFCL& robot_fcl = dynamic_cast<const CollisionRobotFCL&>(robot);
124  FCLObject fcl_obj;
125  robot_fcl.constructFCLObject(state, fcl_obj);
126 
127  CollisionData cd(&req, &res, acm);
128  cd.enableGroup(robot.getRobotModel());
129  for (std::size_t i = 0; !cd.done_ && i < fcl_obj.collision_objects_.size(); ++i)
130  manager_->collide(fcl_obj.collision_objects_[i].get(), &cd, &collisionCallback);
131 
132  if (req.distance)
133  {
134  DistanceRequest dreq;
135  DistanceResult dres;
136 
137  dreq.group_name = req.group_name;
138  dreq.acm = acm;
139  dreq.enableGroup(robot.getRobotModel());
140  distanceRobot(dreq, dres, robot, state);
142  }
143 }
144 
146  const CollisionWorld& other_world) const
147 {
148  checkWorldCollisionHelper(req, res, other_world, nullptr);
149 }
150 
152  const CollisionWorld& other_world, const AllowedCollisionMatrix& acm) const
153 {
154  checkWorldCollisionHelper(req, res, other_world, &acm);
155 }
156 
158  const CollisionWorld& other_world,
159  const AllowedCollisionMatrix* acm) const
160 {
161  const CollisionWorldFCL& other_fcl_world = dynamic_cast<const CollisionWorldFCL&>(other_world);
162  CollisionData cd(&req, &res, acm);
163  manager_->collide(other_fcl_world.manager_.get(), &cd, &collisionCallback);
164 
165  if (req.distance)
166  {
167  DistanceRequest dreq;
168  DistanceResult dres;
169 
170  dreq.group_name = req.group_name;
171  dreq.acm = acm;
172  distanceWorld(dreq, dres, other_world);
174  }
175 }
176 
178 {
179  for (std::size_t i = 0; i < obj->shapes_.size(); ++i)
180  {
181  FCLGeometryConstPtr g = createCollisionGeometry(obj->shapes_[i], obj);
182  if (g)
183  {
184  auto co = new fcl::CollisionObject(g->collision_geometry_, transform2fcl(obj->shape_poses_[i]));
185  fcl_obj.collision_objects_.push_back(FCLCollisionObjectPtr(co));
186  fcl_obj.collision_geometry_.push_back(g);
187  }
188  }
189 }
190 
191 void CollisionWorldFCL::updateFCLObject(const std::string& id)
192 {
193  // remove FCL objects that correspond to this object
194  auto jt = fcl_objs_.find(id);
195  if (jt != fcl_objs_.end())
196  {
197  jt->second.unregisterFrom(manager_.get());
198  jt->second.clear();
199  }
200 
201  // check to see if we have this object
202  auto it = getWorld()->find(id);
203  if (it != getWorld()->end())
204  {
205  // construct FCL objects that correspond to this object
206  if (jt != fcl_objs_.end())
207  {
208  constructFCLObject(it->second.get(), jt->second);
209  jt->second.registerTo(manager_.get());
210  }
211  else
212  {
213  constructFCLObject(it->second.get(), fcl_objs_[id]);
214  fcl_objs_[id].registerTo(manager_.get());
215  }
216  }
217  else
218  {
219  if (jt != fcl_objs_.end())
220  fcl_objs_.erase(jt);
221  }
222 
223  // manager_->update();
224 }
225 
226 void CollisionWorldFCL::setWorld(const WorldPtr& world)
227 {
228  if (world == getWorld())
229  return;
230 
231  // turn off notifications about old world
232  getWorld()->removeObserver(observer_handle_);
233 
234  // clear out objects from old world
235  manager_->clear();
236  fcl_objs_.clear();
238 
240 
241  // request notifications about changes to new world
242  observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionWorldFCL::notifyObjectChange, this, _1, _2));
243 
244  // get notifications any objects already in the new world
245  getWorld()->notifyObserverAllObjects(observer_handle_, World::CREATE);
246 }
247 
249 {
250  if (action == World::DESTROY)
251  {
252  auto it = fcl_objs_.find(obj->id_);
253  if (it != fcl_objs_.end())
254  {
255  it->second.unregisterFrom(manager_.get());
256  it->second.clear();
257  fcl_objs_.erase(it);
258  }
260  }
261  else
262  {
263  updateFCLObject(obj->id_);
264  if (action & (World::DESTROY | World::REMOVE_SHAPE))
266  }
267 }
268 
270  const robot_state::RobotState& state) const
271 {
272  const CollisionRobotFCL& robot_fcl = dynamic_cast<const CollisionRobotFCL&>(robot);
273  FCLObject fcl_obj;
274  robot_fcl.constructFCLObject(state, fcl_obj);
275 
276  DistanceData drd(&req, &res);
277  for (std::size_t i = 0; !drd.done && i < fcl_obj.collision_objects_.size(); ++i)
278  manager_->distance(fcl_obj.collision_objects_[i].get(), &drd, &distanceCallback);
279 }
280 
282  const CollisionWorld& world) const
283 {
284  const CollisionWorldFCL& other_fcl_world = dynamic_cast<const CollisionWorldFCL&>(world);
285  DistanceData drd(&req, &res);
286  manager_->distance(other_fcl_world.manager_.get(), &drd, &distanceCallback);
287 }
288 
289 } // end of namespace collision_detection
void updateFCLObject(const std::string &id)
void constructFCLObject(const robot_state::RobotState &state, FCLObject &fcl_obj) const
bool done_
Flag indicating whether collision checking is complete.
void checkRobotCollisionHelper(const CollisionRequest &req, CollisionResult &res, const CollisionRobot &robot, const robot_state::RobotState &state, const AllowedCollisionMatrix *acm) const
FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr &shape, const robot_model::LinkModel *link, int shape_index)
virtual void distanceRobot(const DistanceRequest &req, DistanceResult &res, const CollisionRobot &robot, const robot_state::RobotState &state) const override
Compute the distance between a robot and the world.
const AllowedCollisionMatrix * acm
The allowed collision matrix used to filter checks.
std::vector< shapes::ShapeConstPtr > shapes_
All the shapes making up this object.
Definition: world.h:102
bool collisionCallback(fcl::CollisionObject *o1, fcl::CollisionObject *o2, void *data)
virtual void distanceWorld(const DistanceRequest &req, DistanceResult &res, const CollisionWorld &world) const override
Compute the distance between another world.
DistanceResultsData minimum_distance
ResultsData for the two objects with the minimum distance.
Represents an action that occurred on an object in the world. Several bits may be set indicating seve...
Definition: world.h:195
const robot_model::RobotModelConstPtr & getRobotModel() const
The kinematic model corresponding to this collision model.
std::unique_ptr< fcl::BroadPhaseCollisionManager > manager_
Generic interface to collision detection.
void enableGroup(const robot_model::RobotModelConstPtr &kmodel)
Compute active_components_only_ based on req_.
World::ObjectConstPtr ObjectConstPtr
virtual void setWorld(const WorldPtr &world)
A representation of an object.
Definition: world.h:80
void checkWorldCollisionHelper(const CollisionRequest &req, CollisionResult &res, const CollisionWorld &other_world, const AllowedCollisionMatrix *acm) const
virtual void checkWorldCollision(const CollisionRequest &req, CollisionResult &res, const CollisionWorld &other_world) const
Check whether a given set of objects is in collision with objects from another world. Any contacts are considered.
Definition of a structure for the allowed collision matrix. All elements in the collision world are r...
This class represents a collision model of the robot and can be used for self collision checks (to ch...
std::string group_name
The group name to check collisions for (optional; if empty, assume the complete robot) ...
double distance
The distance between two objects. If two objects are in collision, distance <= 0. ...
virtual void checkRobotCollision(const CollisionRequest &req, CollisionResult &res, const CollisionRobot &robot, const robot_state::RobotState &state) const
Check whether the robot model is in collision with the world. Any collisions between a robot link and...
virtual void setWorld(const WorldPtr &world)
Perform collision checking with the environment. The collision world maintains a representation of th...
void enableGroup(const robot_model::RobotModelConstPtr &kmodel)
Compute active_components_only_ based on req_.
void transform2fcl(const Eigen::Affine3d &b, fcl::Transform3f &f)
#define ROS_ERROR_NAMED(name,...)
Representation of a robot&#39;s state. This includes position, velocity, acceleration and effort...
Definition: robot_state.h:123
void notifyObjectChange(const ObjectConstPtr &obj, World::Action action)
bool distanceCallback(fcl::CollisionObject *o1, fcl::CollisionObject *o2, void *data, double &min_dist)
EigenSTL::vector_Affine3d shape_poses_
The poses of the corresponding entries in shapes_.
Definition: world.h:107
std::shared_ptr< fcl::CollisionObject > FCLCollisionObjectPtr
std::map< std::string, FCLObject > fcl_objs_
void constructFCLObject(const World::Object *obj, FCLObject &fcl_obj) const


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Sun Oct 21 2018 10:36:32