collision_common_distance_field.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2012, 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 the 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: E. Gil Jones */
36 
38 #include <ros/console.h>
39 #include <boost/thread/mutex.hpp>
41 #include <memory>
42 
43 namespace collision_detection
44 {
46 {
47  using Comperator = std::owner_less<std::weak_ptr<const shapes::Shape>>;
48  using Map = std::map<std::weak_ptr<const shapes::Shape>, BodyDecompositionConstPtr, Comperator>;
49 
51  {
52  }
53  static const unsigned int MAX_CLEAN_COUNT = 100;
55  unsigned int clean_count_;
56  boost::mutex lock_;
57 };
58 
60 {
61  static BodyDecompositionCache cache;
62  return cache;
63 }
64 
65 BodyDecompositionConstPtr getBodyDecompositionCacheEntry(const shapes::ShapeConstPtr& shape, double resolution)
66 {
67  // TODO - deal with changing resolution?
69  std::weak_ptr<const shapes::Shape> wptr(shape);
70  {
71  boost::mutex::scoped_lock slock(cache.lock_);
72  BodyDecompositionCache::Map::const_iterator cache_it = cache.map_.find(wptr);
73  if (cache_it != cache.map_.end())
74  {
75  return cache_it->second;
76  }
77  }
78 
79  BodyDecompositionConstPtr bdcp(new BodyDecomposition(shape, resolution));
80  {
81  boost::mutex::scoped_lock slock(cache.lock_);
82  cache.map_[wptr] = bdcp;
83  cache.clean_count_++;
84  return bdcp;
85  }
86  // TODO - clean cache
87 }
88 
89 PosedBodyPointDecompositionVectorPtr getCollisionObjectPointDecomposition(const collision_detection::World::Object& obj,
90  double resolution)
91 {
92  PosedBodyPointDecompositionVectorPtr ret(new PosedBodyPointDecompositionVector());
93  for (unsigned int i = 0; i < obj.shapes_.size(); i++)
94  {
95  PosedBodyPointDecompositionPtr pbd(
97  ret->addToVector(pbd);
98  ret->updatePose(ret->getSize() - 1, obj.shape_poses_[i]);
99  }
100  return ret;
101 }
102 
103 PosedBodySphereDecompositionVectorPtr getAttachedBodySphereDecomposition(const robot_state::AttachedBody* att,
104  double resolution)
105 {
106  PosedBodySphereDecompositionVectorPtr ret(new PosedBodySphereDecompositionVector());
107  for (unsigned int i = 0; i < att->getShapes().size(); i++)
108  {
109  PosedBodySphereDecompositionPtr pbd(
111  pbd->updatePose(att->getGlobalCollisionBodyTransforms()[i]);
112  ret->addToVector(pbd);
113  }
114  return ret;
115 }
116 
117 PosedBodyPointDecompositionVectorPtr getAttachedBodyPointDecomposition(const robot_state::AttachedBody* att,
118  double resolution)
119 {
120  PosedBodyPointDecompositionVectorPtr ret(new PosedBodyPointDecompositionVector());
121  for (unsigned int i = 0; i < att->getShapes().size(); i++)
122  {
123  PosedBodyPointDecompositionPtr pbd(
125  ret->addToVector(pbd);
126  ret->updatePose(ret->getSize() - 1, att->getGlobalCollisionBodyTransforms()[i]);
127  }
128  return ret;
129 }
130 
131 void getBodySphereVisualizationMarkers(GroupStateRepresentationConstPtr& gsr, std::string reference_frame,
132  visualization_msgs::MarkerArray& body_marker_array)
133 {
134  // creating namespaces
135  std::string robot_ns = gsr->dfce_->group_name_ + "_sphere_decomposition";
136  std::string attached_ns = "attached_sphere_decomposition";
137 
138  // creating colors
139  std_msgs::ColorRGBA robot_color;
140  robot_color.r = 0;
141  robot_color.b = 0.8f;
142  robot_color.g = 0;
143  robot_color.a = 0.5;
144 
145  std_msgs::ColorRGBA attached_color;
146  attached_color.r = 1;
147  attached_color.g = 1;
148  attached_color.b = 0;
149  attached_color.a = 0.5;
150 
151  // creating sphere marker
152  visualization_msgs::Marker sphere_marker;
153  sphere_marker.header.frame_id = reference_frame;
154  sphere_marker.header.stamp = ros::Time(0);
155  sphere_marker.ns = robot_ns;
156  sphere_marker.id = 0;
157  sphere_marker.type = visualization_msgs::Marker::SPHERE;
158  sphere_marker.action = visualization_msgs::Marker::ADD;
159  sphere_marker.pose.orientation.x = 0;
160  sphere_marker.pose.orientation.y = 0;
161  sphere_marker.pose.orientation.z = 0;
162  sphere_marker.pose.orientation.w = 1;
163  sphere_marker.color = robot_color;
164  sphere_marker.lifetime = ros::Duration(0);
165 
166  const moveit::core::RobotState& state = *(gsr->dfce_->state_);
167  unsigned int id = 0;
168  for (unsigned int i = 0; i < gsr->dfce_->link_names_.size(); i++)
169  {
170  const moveit::core::LinkModel* ls = state.getLinkModel(gsr->dfce_->link_names_[i]);
171  if (gsr->dfce_->link_has_geometry_[i])
172  {
173  gsr->link_body_decompositions_[i]->updatePose(state.getFrameTransform(ls->getName()));
174 
175  collision_detection::PosedBodySphereDecompositionConstPtr sphere_representation =
176  gsr->link_body_decompositions_[i];
177  for (unsigned int j = 0; j < sphere_representation->getCollisionSpheres().size(); j++)
178  {
179  tf::pointEigenToMsg(sphere_representation->getSphereCenters()[j], sphere_marker.pose.position);
180  sphere_marker.scale.x = sphere_marker.scale.y = sphere_marker.scale.z =
181  sphere_representation->getCollisionSpheres()[j].radius_;
182  sphere_marker.id = id;
183  id++;
184 
185  body_marker_array.markers.push_back(sphere_marker);
186  }
187  }
188  }
189 
190  sphere_marker.ns = attached_ns;
191  sphere_marker.color = attached_color;
192  for (unsigned int i = 0; i < gsr->dfce_->attached_body_names_.size(); i++)
193  {
194  const moveit::core::AttachedBody* att = state.getAttachedBody(gsr->dfce_->attached_body_names_[i]);
195  if (!att)
196  {
197  ROS_WARN("Attached body '%s' was not found, skipping sphere "
198  "decomposition visualization",
199  gsr->dfce_->attached_body_names_[i].c_str());
200  continue;
201  }
202 
203  if (gsr->attached_body_decompositions_[i]->getSize() != att->getShapes().size())
204  {
205  ROS_WARN("Attached body size discrepancy");
206  continue;
207  }
208 
209  for (unsigned int j = 0; j < att->getShapes().size(); j++)
210  {
211  PosedBodySphereDecompositionVectorPtr sphere_decp = gsr->attached_body_decompositions_[i];
212  sphere_decp->updatePose(j, att->getGlobalCollisionBodyTransforms()[j]);
213 
214  tf::pointEigenToMsg(sphere_decp->getSphereCenters()[j], sphere_marker.pose.position);
215  sphere_marker.scale.x = sphere_marker.scale.y = sphere_marker.scale.z =
216  sphere_decp->getCollisionSpheres()[j].radius_;
217  sphere_marker.id = id;
218  body_marker_array.markers.push_back(sphere_marker);
219  id++;
220  }
221  }
222 }
223 }
const std::string & getName() const
The name of this link.
Definition: link_model.h:81
PosedBodySphereDecompositionVectorPtr getAttachedBodySphereDecomposition(const robot_state::AttachedBody *att, double resolution)
std::vector< shapes::ShapeConstPtr > shapes_
All the shapes making up this object.
Definition: world.h:102
const Eigen::Affine3d & getFrameTransform(const std::string &id)
Get the transformation matrix from the model frame to the frame identified by id. ...
void getBodySphereVisualizationMarkers(GroupStateRepresentationPtr &gsr, std::string reference_frame, visualization_msgs::MarkerArray &body_marker_array)
BodyDecompositionConstPtr getBodyDecompositionCacheEntry(const shapes::ShapeConstPtr &shape, double resolution)
#define ROS_WARN(...)
Generic interface to collision detection.
const AttachedBody * getAttachedBody(const std::string &name) const
Get the attached body named name. Return NULL if not found.
const std::vector< shapes::ShapeConstPtr > & getShapes() const
Get the shapes that make up this attached body.
Definition: attached_body.h:88
PosedBodyPointDecompositionVectorPtr getAttachedBodyPointDecomposition(const robot_state::AttachedBody *att, double resolution)
A representation of an object.
Definition: world.h:80
static const double resolution
std::map< std::weak_ptr< const shapes::Shape >, BodyDecompositionConstPtr, Comperator > Map
BodyDecompositionCache & getBodyDecompositionCache()
PosedBodyPointDecompositionVectorPtr getCollisionObjectPointDecomposition(const collision_detection::World::Object &obj, double resolution)
const EigenSTL::vector_Affine3d & getGlobalCollisionBodyTransforms() const
Get the global transforms for the collision bodies.
void pointEigenToMsg(const Eigen::Vector3d &e, geometry_msgs::Point &m)
const LinkModel * getLinkModel(const std::string &link) const
Get the model of a particular link.
Definition: robot_state.h:156
std::owner_less< std::weak_ptr< const shapes::Shape >> Comperator
Representation of a robot&#39;s state. This includes position, velocity, acceleration and effort...
Definition: robot_state.h:123
A link from the robot. Contains the constant transform applied to the link and its geometry...
Definition: link_model.h:72
EigenSTL::vector_Affine3d shape_poses_
The poses of the corresponding entries in shapes_.
Definition: world.h:107
Object defining bodies that can be attached to robot links. This is useful when handling objects pick...
Definition: attached_body.h:56
std::shared_ptr< const Shape > ShapeConstPtr


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Sun Oct 18 2020 13:16:33