Skip to content

Commit

Permalink
(Entity) Add collision shape (#161)
Browse files Browse the repository at this point in the history
  • Loading branch information
@P-ict0
P-ict0 authored May 6, 2023
2 parents 6ac5521 + 2d3a650 commit c2ab283
Show file tree
Hide file tree
Showing 19 changed files with 381 additions and 266 deletions.
37 changes: 23 additions & 14 deletions include/ed/entity.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -53,14 +53,24 @@ class Entity

void addMeasurement(MeasurementConstPtr measurement);

inline geo::ShapeConstPtr shape() const { return shape_; }
void setShape(const geo::ShapeConstPtr& shape);
[[deprecated("Use visual() or collision() instead.")]]
inline geo::ShapeConstPtr shape() const { return visual(); }
inline geo::ShapeConstPtr visual() const { return visual_; }
inline geo::ShapeConstPtr collision() const { return collision_; }
[[deprecated("Use setVisual() or setCollision() instead.")]]
inline void setShape(const geo::ShapeConstPtr& shape) { setVisual(shape); }
void setVisual(const geo::ShapeConstPtr& visual);
void setCollision(const geo::ShapeConstPtr& collision);

inline const std::map<std::string, geo::ShapeConstPtr>& volumes() const { return volumes_; }
inline void addVolume(const std::string& volume_name, const geo::ShapeConstPtr& volume_shape) { volumes_[volume_name] = volume_shape; ++shape_revision_; }
inline void removeVolume(const std::string& volume_name) { volumes_.erase(volume_name); ++shape_revision_; }
inline void addVolume(const std::string& volume_name, const geo::ShapeConstPtr& volume_shape) { volumes_[volume_name] = volume_shape; ++volumes_revision_; }
inline void removeVolume(const std::string& volume_name) { volumes_.erase(volume_name); ++volumes_revision_; }

inline unsigned long shapeRevision() const{ return shape_ ? shape_revision_ : 0; }
[[deprecated("Use visualRevision(), collisionRevision() or volumesRevision() instead.")]]
inline unsigned long shapeRevision() const{ return visualRevision(); }
inline unsigned long visualRevision() const{ return visual_ ? visual_revision_ : 0; }
inline unsigned long collisionRevision() const{ return collision_ ? collision_revision_ : 0; }
inline unsigned long volumesRevision() const{ return !volumes_.empty() ? volumes_revision_ : 0; }

inline const ConvexHull& convexHull() const { return convex_hull_new_; }

Expand Down Expand Up @@ -94,8 +104,8 @@ class Entity
inline void setPose(const geo::Pose3D& pose)
{
pose_ = pose;
if (shape_)
updateConvexHullFromShape();
if (visual_)
updateConvexHullFromVisual();

has_pose_ = true;
}
Expand All @@ -105,10 +115,6 @@ class Entity
inline const tue::config::DataConstPointer& data() const { return config_; }
inline void setData(const tue::config::DataConstPointer& data) { config_ = data; }

//! For debugging purposes
bool in_frustrum;
bool object_in_front;

// inline double creationTime() const { return creation_time_; }

inline void setRelationTo(Idx child_idx, Idx r_idx) { relations_to_[child_idx] = r_idx; }
Expand Down Expand Up @@ -237,9 +243,12 @@ class Entity
MeasurementConstPtr best_measurement_;
unsigned int measurements_seq_;

geo::ShapeConstPtr shape_;
geo::ShapeConstPtr visual_;
geo::ShapeConstPtr collision_;
std::map<std::string, geo::ShapeConstPtr> volumes_;
unsigned long shape_revision_;
unsigned long visual_revision_;
unsigned long collision_revision_;
unsigned long volumes_revision_;

std::map<std::string, MeasurementConvexHull> convex_hull_map_;
ConvexHull convex_hull_new_;
Expand All @@ -259,7 +268,7 @@ class Entity

void updateConvexHull();

void updateConvexHullFromShape();
void updateConvexHullFromVisual();

std::set<std::string> flags_;

Expand Down
4 changes: 2 additions & 2 deletions include/ed/helpers/msg_conversions.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -78,7 +78,7 @@ void convert(const ed::Entity& e, ed_msgs::EntityInfo& msg) {
msg.z_max = 0;
}

msg.has_shape = (e.shape() != nullptr);
msg.has_shape = (e.visual() != nullptr);
msg.has_pose = e.has_pose();
if (e.has_pose())
{
Expand Down Expand Up @@ -119,7 +119,7 @@ void convert(const ed::Entity& e, ed_msgs::EntityInfo& msg) {

ed_msgs::SubVolume sub_volume;
convert(shape_tr, sub_volume);
sub_volume.center_point.header.frame_id = "/" + e.id().str();
sub_volume.center_point.header.frame_id = e.id().str();
volume.subvolumes.push_back(sub_volume);
}
}
Expand Down
145 changes: 5 additions & 140 deletions include/ed/models/shape_loader.h
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@
#ifndef ED_SHAPE_LOADER_H_
#define ED_SHAPE_LOADER_H_
#ifndef ED_MODELS_SHAPE_LOADER_H_
#define ED_MODELS_SHAPE_LOADER_H_

#include <geolib/datatypes.h>
#include <geolib/Mesh.h>
Expand All @@ -22,43 +22,8 @@ namespace models
* @param height height of the cylinder
* @param num_corners divided the circumference in N points and N+1 lines
*/
void createCylinder(geo::Shape& shape, double radius, double height, int num_corners = 12)
{
geo::Mesh mesh;

// Calculate vertices
for(int i = 0; i < num_corners; ++i)
{
double a = 2 * M_PI * i / num_corners;
double x = sin(a) * radius;
double y = cos(a) * radius;

mesh.addPoint(x, y, -height / 2);
mesh.addPoint(x, y, height / 2);
}

// Calculate top and bottom triangles
for(int i = 1; i < num_corners - 1; ++i)
{
int i2 = 2 * i;

// bottom
mesh.addTriangle(0, i2, i2 + 2);

// top
mesh.addTriangle(1, i2 + 3, i2 + 1);
}
void createCylinder(geo::Shape& shape, double radius, double height, int num_corners = 12);

// Calculate side triangles
for(int i = 0; i < num_corners; ++i)
{
int j = (i + 1) % num_corners;
mesh.addTriangle(i * 2, i * 2 + 1, j * 2);
mesh.addTriangle(i * 2 + 1, j * 2 + 1, j * 2);
}

shape.setMesh(mesh);
}
/**
* @brief getMiddlePoint Gets the middle point of two points in a mesh of a sphere. Uses a cache to not create double points.
* The new point is placed on the radius of the sphere.
Expand All @@ -69,115 +34,15 @@ void createCylinder(geo::Shape& shape, double radius, double height, int num_cor
* @param radius radius of teh sphere
* @return index of the inserted point
*/
uint getMiddlePoint(geo::Mesh& mesh, uint i1, uint i2, std::map<unsigned long, uint> cache, double radius)
{
// first check if we have it already
bool firstIsSmaller = i1 < i2;
unsigned long smallerIndex = firstIsSmaller ? i1 : i2;
unsigned long greaterIndex = firstIsSmaller ? i2 : i1;
unsigned long key = (smallerIndex << 32) + greaterIndex;

std::map<unsigned long, uint>::const_iterator it = cache.find(key);
if (it != cache.end())
return it->second;
uint getMiddlePoint(geo::Mesh& mesh, uint i1, uint i2, std::map<unsigned long, uint> cache, double radius);

// not in cache, calculate it
const std::vector<geo::Vec3>& points = mesh.getPoints();
geo::Vec3 p1 = points[i1];
geo::Vec3 p2 = points[i2];
geo::Vec3 p3((p1+p2)/2);
p3 = p3.normalized() * radius;

// add vertex makes sure point is on unit sphere
uint i3 = mesh.addPoint(p3);

// store it, return index
cache.insert(std::pair<unsigned long, uint>(key, i3));
return i3;
}
/**
* @brief createSphere Create a shape of sphere
* @param shape Shape object to be filled
* @param radius radius of the sphere
* @param recursion_level number of recursions to smooth the mesh, but rapidly increases the mesh.
*/
void createSphere(geo::Shape& shape, double radius, uint recursion_level = 2)
{
geo::Mesh mesh;

// create 12 vertices of a icosahedron
double t = (1.0 + sqrt(5.0)) / 2.0;

mesh.addPoint(geo::Vec3(-1, t, 0).normalized()*radius);
mesh.addPoint(geo::Vec3( 1, t, 0).normalized()*radius);
mesh.addPoint(geo::Vec3(-1, -t, 0).normalized()*radius);
mesh.addPoint(geo::Vec3( 1, -t, 0).normalized()*radius);

mesh.addPoint(geo::Vec3( 0, -1, t).normalized()*radius);
mesh.addPoint(geo::Vec3( 0, 1, t).normalized()*radius);
mesh.addPoint(geo::Vec3( 0, -1, -t).normalized()*radius);
mesh.addPoint(geo::Vec3( 0, 1, -t).normalized()*radius);

mesh.addPoint(geo::Vec3( t, 0, -1).normalized()*radius);
mesh.addPoint(geo::Vec3( t, 0, 1).normalized()*radius);
mesh.addPoint(geo::Vec3(-t, 0, -1).normalized()*radius);
mesh.addPoint(geo::Vec3(-t, 0, 1).normalized()*radius);

// create 20 triangles of the icosahedron
// 5 faces around point 0
mesh.addTriangle(0, 11, 5);
mesh.addTriangle(0, 5, 1);
mesh.addTriangle(0, 1, 7);
mesh.addTriangle(0, 7, 10);
mesh.addTriangle(0, 10, 11);

// 5 adjacent faces
mesh.addTriangle(1, 5, 9);
mesh.addTriangle(5, 11, 4);
mesh.addTriangle(11, 10, 2);
mesh.addTriangle(10, 7, 6);
mesh.addTriangle(7, 1, 8);

// 5 faces around point 3
mesh.addTriangle(3, 9, 4);
mesh.addTriangle(3, 4, 2);
mesh.addTriangle(3, 2, 6);
mesh.addTriangle(3, 6, 8);
mesh.addTriangle(3, 8, 9);

// 5 adjacent faces
mesh.addTriangle(4, 9, 5);
mesh.addTriangle(2, 4, 11);
mesh.addTriangle(6, 2, 10);
mesh.addTriangle(8, 6, 7);
mesh.addTriangle(9, 8, 1);

for (uint i = 0; i < recursion_level; i++)
{
geo::Mesh mesh2;
std::map<unsigned long, uint> cache;

const std::vector<geo::Vec3>& points = mesh.getPoints();
for (std::vector<geo::Vec3>::const_iterator it = points.begin(); it != points.end(); ++it)
mesh2.addPoint(*it);

const std::vector<geo::TriangleI>& triangleIs = mesh.getTriangleIs();
for (std::vector<geo::TriangleI>::const_iterator it = triangleIs.begin(); it != triangleIs.end(); ++it)
{
// replace triangle by 4 triangles
uint a = getMiddlePoint(mesh2, it->i1_, it->i2_, cache, radius);
uint b = getMiddlePoint(mesh2, it->i2_, it->i3_, cache, radius);
uint c = getMiddlePoint(mesh2, it->i3_, it->i1_, cache, radius);

mesh2.addTriangle(it->i1_, a, c);
mesh2.addTriangle(it->i2_, b, a);
mesh2.addTriangle(it->i3_, c, b);
mesh2.addTriangle(a, b, c);
}
mesh = mesh2;
}
shape.setMesh(mesh);
}
void createSphere(geo::Shape& shape, double radius, uint recursion_level = 2);

} // end namespace models

Expand Down
2 changes: 1 addition & 1 deletion include/ed/plugin_container.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -99,7 +99,7 @@ class PluginContainer

UpdateRequestPtr update_request_;

boost::shared_ptr<boost::thread> thread_;
ed::shared_ptr<boost::thread> thread_;

bool step_finished_;

Expand Down
15 changes: 9 additions & 6 deletions include/ed/update_request.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -46,14 +46,17 @@ class UpdateRequest
}


// SHAPES
// VISUALS
std::map<UUID, geo::ShapeConstPtr> visuals;
[[deprecated("Use setVisual() or setCollision() instead.")]]
void setShape(const UUID& id, const geo::ShapeConstPtr& shape) { setVisual(id, shape); }
void setVisual(const UUID& id, const geo::ShapeConstPtr& visual) { visuals[id] = visual; flagUpdated(id); }

std::map<UUID, geo::ShapeConstPtr> shapes;
void setShape(const UUID& id, const geo::ShapeConstPtr& shape) { shapes[id] = shape; flagUpdated(id); }


//VOLUMES
// COLLISIONS
std::map<UUID, geo::ShapeConstPtr> collisions;
void setCollision(const UUID& id, const geo::ShapeConstPtr& collision) { collisions[id] = collision; flagUpdated(id); }

// VOLUMES
std::map<UUID, std::map<std::string, geo::ShapeConstPtr> > volumes_added;
void addVolume(const UUID& id, const std::string Volume_name, const geo::ShapeConstPtr& Volume_shape)
{ std::map<UUID, std::map<std::string, geo::ShapeConstPtr> >::iterator it = volumes_added.find(id);
Expand Down
15 changes: 13 additions & 2 deletions include/ed/world_model.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -102,7 +102,14 @@ class WorldModel

const std::vector<unsigned long>& entity_revisions() const { return entity_revisions_; }

const std::vector<unsigned long>& entity_shape_revisions() const { return entity_shape_revisions_; }
[[deprecated("Use entity_visual_revisions(), entity_collision_revisions() or entity_volumes_revisions() instead.")]]
const std::vector<unsigned long>& entity_shape_revisions() const { return entity_visual_revisions(); }

const std::vector<unsigned long>& entity_visual_revisions() const { return entity_visual_revisions_; }

const std::vector<unsigned long>& entity_collision_revisions() const { return entity_collision_revisions_; }

const std::vector<unsigned long>& entity_volumes_revisions() const { return entity_volumes_revisions_; }

const PropertyKeyDBEntry* getPropertyInfo(const std::string& name) const;

Expand All @@ -116,7 +123,11 @@ class WorldModel

std::vector<unsigned long> entity_revisions_;

std::vector<unsigned long> entity_shape_revisions_;
std::vector<unsigned long> entity_visual_revisions_;

std::vector<unsigned long> entity_collision_revisions_;

std::vector<unsigned long> entity_volumes_revisions_;

std::queue<Idx> entity_empty_spots_;

Expand Down
6 changes: 3 additions & 3 deletions plugins/gui_plugin.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -273,7 +273,7 @@ ed::UUID GUIPlugin::getEntityFromClick(const cv::Point2i& p) const
for(ed::WorldModel::const_iterator it_entity = world_model_->begin(); it_entity != world_model_->end(); ++it_entity)
{
const ed::EntityConstPtr& e = *it_entity;
if (!e->shape())
if (!e->visual())
{
const pcl::PointCloud<pcl::PointXYZ>& chull_points = e->convexHull().chull;

Expand Down Expand Up @@ -307,14 +307,14 @@ void GUIPlugin::publishMapImage()
{
const ed::EntityConstPtr& e = *it_entity;

if (e->shape() && e->id() != "floor")
if (e->visual() && e->id() != "floor")
{
cv::Scalar color = idToColor(e->id());
cv::Vec3b color_vec(0.5 * color[0], 0.5 * color[1], 0.5 * color[2]);

geo::Pose3D pose = projector_pose_.inverse() * e->pose();
geo::RenderOptions opt;
opt.setMesh(e->shape()->getMesh(), pose);
opt.setMesh(e->visual()->getMesh(), pose);

ColorRenderResult res(map_image_, z_buffer, color_vec, projector_pose_.t.z - 3, projector_pose_.t.z + 1);

Expand Down
Loading

0 comments on commit c2ab283

Please sign in to comment.