39 #include <boost/thread/mutex.hpp> 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>;
69 std::weak_ptr<const shapes::Shape> wptr(shape);
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())
75 return cache_it->second;
81 boost::mutex::scoped_lock slock(cache.
lock_);
82 cache.
map_[wptr] = bdcp;
93 for (
unsigned int i = 0; i < obj.
shapes_.size(); i++)
95 PosedBodyPointDecompositionPtr pbd(
97 ret->addToVector(pbd);
98 ret->updatePose(ret->getSize() - 1, obj.
shape_poses_[i]);
107 for (
unsigned int i = 0; i < att->
getShapes().size(); i++)
109 PosedBodySphereDecompositionPtr pbd(
112 ret->addToVector(pbd);
121 for (
unsigned int i = 0; i < att->
getShapes().size(); i++)
123 PosedBodyPointDecompositionPtr pbd(
125 ret->addToVector(pbd);
132 visualization_msgs::MarkerArray& body_marker_array)
135 std::string robot_ns = gsr->dfce_->group_name_ +
"_sphere_decomposition";
136 std::string attached_ns =
"attached_sphere_decomposition";
139 std_msgs::ColorRGBA robot_color;
141 robot_color.b = 0.8f;
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;
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;
168 for (
unsigned int i = 0; i < gsr->dfce_->link_names_.size(); i++)
171 if (gsr->dfce_->link_has_geometry_[i])
175 collision_detection::PosedBodySphereDecompositionConstPtr sphere_representation =
176 gsr->link_body_decompositions_[i];
177 for (
unsigned int j = 0; j < sphere_representation->getCollisionSpheres().size(); j++)
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;
185 body_marker_array.markers.push_back(sphere_marker);
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++)
197 ROS_WARN(
"Attached body '%s' was not found, skipping sphere " 198 "decomposition visualization",
199 gsr->dfce_->attached_body_names_[i].c_str());
203 if (gsr->attached_body_decompositions_[i]->getSize() != att->
getShapes().size())
205 ROS_WARN(
"Attached body size discrepancy");
209 for (
unsigned int j = 0; j < att->
getShapes().size(); j++)
211 PosedBodySphereDecompositionVectorPtr sphere_decp = gsr->attached_body_decompositions_[i];
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);
const std::string & getName() const
The name of this link.
PosedBodySphereDecompositionVectorPtr getAttachedBodySphereDecomposition(const robot_state::AttachedBody *att, double resolution)
unsigned int clean_count_
std::vector< shapes::ShapeConstPtr > shapes_
All the shapes making up this object.
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)
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.
PosedBodyPointDecompositionVectorPtr getAttachedBodyPointDecomposition(const robot_state::AttachedBody *att, double resolution)
A representation of an object.
static const double resolution
std::map< std::weak_ptr< const shapes::Shape >, BodyDecompositionConstPtr, Comperator > Map
static const unsigned int MAX_CLEAN_COUNT
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.
std::owner_less< std::weak_ptr< const shapes::Shape >> Comperator
Representation of a robot's state. This includes position, velocity, acceleration and effort...
A link from the robot. Contains the constant transform applied to the link and its geometry...
EigenSTL::vector_Affine3d shape_poses_
The poses of the corresponding entries in shapes_.
Object defining bodies that can be attached to robot links. This is useful when handling objects pick...
std::shared_ptr< const Shape > ShapeConstPtr