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 
40 
41 #if (MOVEIT_FCL_VERSION >= FCL_VERSION_CHECK(0, 6, 0))
42 #include <fcl/geometry/geometric_shape_to_BVH_model.h>
43 #include <fcl/narrowphase/detail/traversal/collision/bvh_collision_traversal_node.h>
44 #include <fcl/narrowphase/detail/traversal/collision_node.h>
45 #include <fcl/broadphase/broadphase_dynamic_AABB_tree.h>
46 #else
47 #include <fcl/shape/geometric_shape_to_BVH_model.h>
48 #include <fcl/traversal/traversal_node_bvhs.h>
49 #include <fcl/traversal/traversal_node_setup.h>
50 #include <fcl/collision_node.h>
51 #endif
52 
53 #include <boost/bind.hpp>
54 
55 namespace collision_detection
56 {
57 const std::string CollisionDetectorAllocatorFCL::NAME("FCL");
58 
60 {
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 }
68 
70 {
72  // m->tree_init_level = 2;
73  manager_.reset(m);
74 
75  // request notifications about changes to new world
76  observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionWorldFCL::notifyObjectChange, this, _1, _2));
77  getWorld()->notifyObserverAllObjects(observer_handle_, World::CREATE);
78 }
79 
80 CollisionWorldFCL::CollisionWorldFCL(const CollisionWorldFCL& other, const WorldPtr& world)
81  : CollisionWorld(other, world)
82 {
84  // m->tree_init_level = 2;
85  manager_.reset(m);
86 
87  fcl_objs_ = other.fcl_objs_;
88  for (auto& fcl_obj : fcl_objs_)
89  fcl_obj.second.registerTo(manager_.get());
90  // manager_->update();
91 
92  // request notifications about changes to new world
93  observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionWorldFCL::notifyObjectChange, this, _1, _2));
94 }
95 
97 {
98  getWorld()->removeObserver(observer_handle_);
99 }
100 
102  const CollisionRobot& robot, const robot_state::RobotState& state) const
103 {
104  checkRobotCollisionHelper(req, res, robot, state, nullptr);
105 }
106 
108  const CollisionRobot& robot, const robot_state::RobotState& state,
109  const AllowedCollisionMatrix& acm) const
110 {
111  checkRobotCollisionHelper(req, res, robot, state, &acm);
112 }
113 
115  const CollisionRobot& robot, const robot_state::RobotState& state1,
116  const robot_state::RobotState& state2) const
117 {
118  ROS_ERROR_NAMED("collision_detection.fcl", "FCL continuous collision checking not yet implemented");
119 }
120 
122  const CollisionRobot& robot, const robot_state::RobotState& state1,
123  const robot_state::RobotState& state2,
124  const AllowedCollisionMatrix& acm) const
125 {
126  ROS_ERROR_NAMED("collision_detection.fcl", "FCL continuous collision checking not yet implemented");
127 }
128 
130  const CollisionRobot& robot, const robot_state::RobotState& state,
131  const AllowedCollisionMatrix* acm) const
132 {
133  const CollisionRobotFCL& robot_fcl = dynamic_cast<const CollisionRobotFCL&>(robot);
134  FCLObject fcl_obj;
135  robot_fcl.constructFCLObject(state, fcl_obj);
136 
137  CollisionData cd(&req, &res, acm);
138  cd.enableGroup(robot.getRobotModel());
139  for (std::size_t i = 0; !cd.done_ && i < fcl_obj.collision_objects_.size(); ++i)
140  manager_->collide(fcl_obj.collision_objects_[i].get(), &cd, &collisionCallback);
141 
142  if (req.distance)
143  {
144  DistanceRequest dreq;
145  DistanceResult dres;
146 
147  dreq.group_name = req.group_name;
148  dreq.acm = acm;
149  dreq.enableGroup(robot.getRobotModel());
150  distanceRobot(dreq, dres, robot, state);
152  }
153 }
154 
156  const CollisionWorld& other_world) const
157 {
158  checkWorldCollisionHelper(req, res, other_world, nullptr);
159 }
160 
162  const CollisionWorld& other_world, const AllowedCollisionMatrix& acm) const
163 {
164  checkWorldCollisionHelper(req, res, other_world, &acm);
165 }
166 
168  const CollisionWorld& other_world,
169  const AllowedCollisionMatrix* acm) const
170 {
171  const CollisionWorldFCL& other_fcl_world = dynamic_cast<const CollisionWorldFCL&>(other_world);
172  CollisionData cd(&req, &res, acm);
173  manager_->collide(other_fcl_world.manager_.get(), &cd, &collisionCallback);
174 
175  if (req.distance)
176  {
177  DistanceRequest dreq;
178  DistanceResult dres;
179 
180  dreq.group_name = req.group_name;
181  dreq.acm = acm;
182  distanceWorld(dreq, dres, other_world);
184  }
185 }
186 
188 {
189  for (std::size_t i = 0; i < obj->shapes_.size(); ++i)
190  {
191  FCLGeometryConstPtr g = createCollisionGeometry(obj->shapes_[i], obj);
192  if (g)
193  {
194  auto co = new fcl::CollisionObjectd(g->collision_geometry_, transform2fcl(obj->shape_poses_[i]));
195  fcl_obj.collision_objects_.push_back(FCLCollisionObjectPtr(co));
196  fcl_obj.collision_geometry_.push_back(g);
197  }
198  }
199 }
200 
201 void CollisionWorldFCL::updateFCLObject(const std::string& id)
202 {
203  // remove FCL objects that correspond to this object
204  auto jt = fcl_objs_.find(id);
205  if (jt != fcl_objs_.end())
206  {
207  jt->second.unregisterFrom(manager_.get());
208  jt->second.clear();
209  }
210 
211  // check to see if we have this object
212  auto it = getWorld()->find(id);
213  if (it != getWorld()->end())
214  {
215  // construct FCL objects that correspond to this object
216  if (jt != fcl_objs_.end())
217  {
218  constructFCLObject(it->second.get(), jt->second);
219  jt->second.registerTo(manager_.get());
220  }
221  else
222  {
223  constructFCLObject(it->second.get(), fcl_objs_[id]);
224  fcl_objs_[id].registerTo(manager_.get());
225  }
226  }
227  else
228  {
229  if (jt != fcl_objs_.end())
230  fcl_objs_.erase(jt);
231  }
232 
233  // manager_->update();
234 }
235 
236 void CollisionWorldFCL::setWorld(const WorldPtr& world)
237 {
238  if (world == getWorld())
239  return;
240 
241  // turn off notifications about old world
242  getWorld()->removeObserver(observer_handle_);
243 
244  // clear out objects from old world
245  manager_->clear();
246  fcl_objs_.clear();
248 
250 
251  // request notifications about changes to new world
252  observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionWorldFCL::notifyObjectChange, this, _1, _2));
253 
254  // get notifications any objects already in the new world
255  getWorld()->notifyObserverAllObjects(observer_handle_, World::CREATE);
256 }
257 
259 {
260  if (action == World::DESTROY)
261  {
262  auto it = fcl_objs_.find(obj->id_);
263  if (it != fcl_objs_.end())
264  {
265  it->second.unregisterFrom(manager_.get());
266  it->second.clear();
267  fcl_objs_.erase(it);
268  }
270  }
271  else
272  {
273  updateFCLObject(obj->id_);
274  if (action & (World::DESTROY | World::REMOVE_SHAPE))
276  }
277 }
278 
280  const robot_state::RobotState& state) const
281 {
282  const CollisionRobotFCL& robot_fcl = dynamic_cast<const CollisionRobotFCL&>(robot);
283  FCLObject fcl_obj;
284  robot_fcl.constructFCLObject(state, fcl_obj);
285 
286  DistanceData drd(&req, &res);
287  for (std::size_t i = 0; !drd.done && i < fcl_obj.collision_objects_.size(); ++i)
288  manager_->distance(fcl_obj.collision_objects_[i].get(), &drd, &distanceCallback);
289 }
290 
292  const CollisionWorld& world) const
293 {
294  const CollisionWorldFCL& other_fcl_world = dynamic_cast<const CollisionWorldFCL&>(world);
295  DistanceData drd(&req, &res);
296  manager_->distance(other_fcl_world.manager_.get(), &drd, &distanceCallback);
297 }
298 
299 } // end of namespace collision_detection
fcl::CollisionObject CollisionObjectd
Definition: fcl_compat.h:51
void updateFCLObject(const std::string &id)
EigenSTL::vector_Isometry3d shape_poses_
The poses of the corresponding entries in shapes_.
Definition: world.h:107
bool done_
Flag indicating whether collision checking is complete.
FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr &shape, const robot_model::LinkModel *link, int shape_index)
Create new FCLGeometry object out of robot link model.
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
void distanceWorld(const DistanceRequest &req, DistanceResult &res, const CollisionWorld &world) const override
Compute the distance between another world.
void checkWorldCollisionHelper(const CollisionRequest &req, CollisionResult &res, const CollisionWorld &other_world, const AllowedCollisionMatrix *acm) const
DistanceResultsData minimum_distance
ResultsData for the two objects with the minimum distance.
std::shared_ptr< fcl::CollisionObjectd > FCLCollisionObjectPtr
Represents an action that occurred on an object in the world. Several bits may be set indicating seve...
Definition: world.h:196
void setWorld(const WorldPtr &world) override
std::unique_ptr< fcl::BroadPhaseCollisionManagerd > manager_
Generic interface to collision detection.
Data structure which is passed to the collision callback function of the collision manager...
void enableGroup(const robot_model::RobotModelConstPtr &robot_model)
Compute active_components_only_ based on the joint group specified in req_.
Data structure which is passed to the distance callback function of the collision manager...
World::ObjectConstPtr ObjectConstPtr
void checkRobotCollision(const CollisionRequest &req, CollisionResult &res, const CollisionRobot &robot, const robot_state::RobotState &state) const override
Check whether the robot model is in collision with the world. Any collisions between a robot link and...
void transform2fcl(const Eigen::Isometry3d &b, fcl::Transform3d &f)
Transforms an Eigen Isometry3d to FCL coordinate transformation.
A representation of an object.
Definition: world.h:80
Definition of a structure for the allowed collision matrix. All elements in the collision world are r...
void constructFCLObject(const World::Object *obj, FCLObject &fcl_obj) const
void cleanCollisionGeometryCache()
Increases the counter of the caches which can trigger the cleaning of expired entries from them...
void enableGroup(const robot_model::RobotModelConstPtr &robot_model)
Compute active_components_only_ based on req_.
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) ...
const robot_model::RobotModelConstPtr & getRobotModel() const
The kinematic model corresponding to this collision model.
std::vector< FCLGeometryConstPtr > collision_geometry_
Geometry data corresponding to collision_objects_.
double distance
The distance between two objects. If two objects are in collision, distance <= 0. ...
bool distanceCallback(fcl::CollisionObjectd *o1, fcl::CollisionObjectd *o2, void *data, double &min_dist)
Callback function used by the FCLManager used for each pair of collision objects to calculate collisi...
void checkRobotCollisionHelper(const CollisionRequest &req, CollisionResult &res, const CollisionRobot &robot, const robot_state::RobotState &state, const AllowedCollisionMatrix *acm) const
virtual void setWorld(const WorldPtr &world)
bool collisionCallback(fcl::CollisionObjectd *o1, fcl::CollisionObjectd *o2, void *data)
Callback function used by the FCLManager used for each pair of collision objects to calculate object ...
fcl::DynamicAABBTreeCollisionManager DynamicAABBTreeCollisionManagerd
Definition: fcl_compat.h:98
Perform collision checking with the environment. The collision world maintains a representation of th...
#define ROS_ERROR_NAMED(name,...)
Representation of a robot&#39;s state. This includes position, velocity, acceleration and effort...
Definition: robot_state.h:122
void notifyObjectChange(const ObjectConstPtr &obj, World::Action action)
A general high-level object which consists of multiple FCLCollisionObjects. It is the top level data ...
void checkWorldCollision(const CollisionRequest &req, CollisionResult &res, const CollisionWorld &other_world) const override
Check whether a given set of objects is in collision with objects from another world. Any contacts are considered.
std::map< std::string, FCLObject > fcl_objs_
void constructFCLObject(const robot_state::RobotState &state, FCLObject &fcl_obj) const


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Sat Jul 11 2020 03:51:20