36 #define BOOST_PARAMETER_MAX_ARITY 7 57 "Color", QColor(25, 255, 0),
58 "Color to draw the polygons.",
62 "Amount of transparency to apply to the polygon.",
66 "only shows the borders of polygons",
70 "show normal direction",
73 "enable lighting",
true,
94 for (
size_t i = 0; i <
lines_.size(); i++) {
98 for (
size_t i = 0; i <
materials_.size(); i++) {
123 static uint32_t
count = 0;
126 for (
size_t i =
materials_.size(); num > i; i++) {
127 std::stringstream ss;
128 ss <<
"PolygonArrayMaterial" << count++;
129 Ogre::MaterialPtr material
130 = Ogre::MaterialManager::getSingleton().create(
131 ss.str(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME);
132 material->setReceiveShadows(
false);
134 material->getTechnique(0)->setAmbient(0.5, 0.5, 0.5);
144 for (
size_t i = 0; i < msg.polygons.size(); i++) {
160 const jsk_recognition_msgs::PolygonArray::ConstPtr& msg)
162 int scale_factor = 2;
168 i < msg->polygons.size() * scale_factor;
170 Ogre::SceneNode* scene_node
172 Ogre::ManualObject* manual_object
174 manual_object->setDynamic(
true);
175 scene_node->attachObject(manual_object);
180 else if (msg->polygons.size() * scale_factor <
manual_objects_.size()) {
181 for (
size_t i = msg->polygons.size() * scale_factor;
188 for (
size_t i =
arrow_objects_.size(); i < msg->polygons.size(); i++) {
189 Ogre::SceneNode* scene_node =
scene_node_->createChildSceneNode();
191 scene_node->setVisible(
false);
197 for (
size_t i = msg->polygons.size(); i <
arrow_nodes_.size(); i++) {
206 if (num >
lines_.size()) {
207 for (
size_t i =
lines_.size(); i < num; i++) {
216 for (
size_t i = 0; i <
lines_.size(); i++) {
223 Ogre::ColourValue color;
226 color.r = ros_color.r;
227 color.g = ros_color.g;
228 color.b = ros_color.b;
229 color.a = ros_color.a;
239 "Message does not have lieklihood fields");
242 std_msgs::ColorRGBA ros_color
244 color.r = ros_color.r;
245 color.g = ros_color.g;
246 color.b = ros_color.b;
247 color.a = ros_color.a;
255 "Message does not have lebels fields");
258 std_msgs::ColorRGBA ros_color
260 color.r = ros_color.r;
261 color.g = ros_color.g;
262 color.b = ros_color.b;
263 color.a = ros_color.a;
272 const geometry_msgs::PolygonStamped& polygon)
276 Ogre::Vector3 position;
277 Ogre::Quaternion orientation;
278 if (!
getTransform(polygon.header, position, orientation))
280 scene_node->setPosition(position);
281 scene_node->setOrientation(orientation);
286 Ogre::ColourValue color =
getColor(i);
287 line->
setColor(color.r, color.g, color.b, color.a);
289 for (
size_t i = 0; i < polygon.polygon.points.size(); ++i) {
290 Ogre::Vector3 step_position;
291 step_position.x = polygon.polygon.points[i].x;
292 step_position.y = polygon.polygon.points[i].y;
293 step_position.z = polygon.polygon.points[i].z;
296 Ogre::Vector3 step_position;
297 step_position.x = polygon.polygon.points[0].x;
298 step_position.y = polygon.polygon.points[0].y;
299 step_position.z = polygon.polygon.points[0].z;
305 Ogre::ColourValue color =
getColor(i);
307 materials_[i]->getTechnique(0)->setAmbient(color * 0.5);
308 materials_[i]->getTechnique(0)->setDiffuse(color);
309 if (color.a < 0.9998) {
310 materials_[i]->getTechnique(0)->setSceneBlending(
311 Ogre::SBT_TRANSPARENT_ALPHA);
312 materials_[i]->getTechnique(0)->setDepthWriteEnabled(
false);
315 materials_[i]->getTechnique(0)->setSceneBlending(Ogre::SBT_REPLACE);
316 materials_[i]->getTechnique(0)->setDepthWriteEnabled(
true);
319 materials_[i]->getTechnique(0)->setAmbient(color * 0.5);
320 materials_[i]->getTechnique(0)->setDiffuse(color);
324 const size_t i,
const geometry_msgs::PolygonStamped& polygon)
326 Ogre::Vector3 position;
327 Ogre::Quaternion orientation;
328 if (!
getTransform(polygon.header, position, orientation))
334 Ogre::ColourValue color =
getColor(i);
335 scene_node->setPosition(position);
336 scene_node->setOrientation(orientation);
337 manual_object->clear();
338 manual_object->setVisible(
true);
342 std::vector<jsk_recognition_utils::Polygon::Ptr>
345 uint32_t num_points = 0;
346 for (
size_t j = 0; j < triangles.size(); j++) {
347 num_points += triangles[j]->getNumVertices();
350 manual_object->estimateVertexCount(num_points * 2);
351 manual_object->begin(
353 for (
size_t ii = 0; ii < triangles.size(); ii++) {
355 size_t num_vertices = triangle->getNumVertices();
356 for (
size_t j = 0; j < num_vertices; j++) {
357 Eigen::Vector3f v = triangle->getVertex(j);
358 manual_object->position(v[0], v[1], v[2]);
359 manual_object->colour(color.r, color.g, color.b, color.a);
361 for (
int j = num_vertices - 1; j >= 0; j--) {
362 Eigen::Vector3f v = triangle->getVertex(j);
363 manual_object->position(v[0], v[1], v[2]);
364 manual_object->colour(color.r, color.g, color.b, color.a);
367 manual_object->end();
373 const size_t i,
const geometry_msgs::PolygonStamped& polygon)
376 scene_node->setVisible(
true);
378 Ogre::Vector3 position;
379 Ogre::Quaternion orientation;
380 if (!
getTransform(polygon.header, position, orientation))
382 scene_node->setPosition(position);
383 scene_node->setOrientation(orientation);
388 Eigen::Vector3f centroid(0, 0, 0);
389 if (vertices.size() == 0) {
393 for (
size_t j = 0; j < vertices.size(); j++) {
394 centroid = vertices[j] + centroid;
396 centroid = centroid / vertices.size();
398 Ogre::Vector3
pos(centroid[0], centroid[1], centroid[2]);
399 Eigen::Vector3f normal = geo_polygon.
getNormal();
400 Ogre::Vector3 direction(normal[0], normal[1], normal[2]);
401 if (std::isnan(direction[0]) || std::isnan(direction[1]) || std::isnan(direction[2])) {
402 ROS_ERROR(
"failed to compute normal direction");
403 Ogre::Vector3 zeroscale(0, 0, 0);
404 arrow->setScale(zeroscale);
408 arrow->setPosition(pos);
409 arrow->setDirection(direction);
411 arrow->setScale(scale);
416 const jsk_recognition_msgs::PolygonArray::ConstPtr& msg)
421 "Message contained invalid floating point values" 438 for (
size_t i = 0; i < msg->polygons.size(); i++) {
439 geometry_msgs::PolygonStamped polygon = msg->polygons[i];
440 if (polygon.polygon.points.size() >= 3) {
446 for (
size_t i = 0; i < msg->polygons.size(); i++) {
450 for (
size_t i = 0; i < msg->polygons.size(); i++) {
451 geometry_msgs::PolygonStamped polygon = msg->polygons[i];
457 for (
size_t i = 0; i < msg->polygons.size(); i++) {
458 geometry_msgs::PolygonStamped polygon = msg->polygons[i];
459 if (polygon.polygon.points.size() >= 3) {
467 const std_msgs::Header &
header,
468 Ogre::Vector3 &position, Ogre::Quaternion &orientation)
471 header.frame_id, header.stamp,
472 position, orientation);
474 std::ostringstream oss;
475 oss <<
"Error transforming from frame '";
476 oss << header.frame_id <<
"' to frame '";
480 "Transform", QString::fromStdString(oss.str()));
rviz::EnumProperty * coloring_property_
virtual bool getTransform(const std_msgs::Header &header, Ogre::Vector3 &position, Ogre::Quaternion &orientation)
std::vector< Ogre::SceneNode * > arrow_nodes_
rviz::ColorProperty * color_property_
virtual void processMessage(const jsk_recognition_msgs::PolygonArray::ConstPtr &msg)
void addPoint(const Ogre::Vector3 &point)
void updateEnableLighting()
PLUGINLIB_EXPORT_CLASS(jsk_rviz_plugins::PictogramArrayDisplay, rviz::Display)
DisplayContext * context_
virtual QColor getColor() const
virtual void processLine(const size_t i, const geometry_msgs::PolygonStamped &polygon)
virtual Vertices getVertices()
virtual void allocateMaterials(int num)
virtual void updateLines(int num)
std::string coloring_method_
static Polygon fromROSMsg(const geometry_msgs::Polygon &polygon)
Ogre::SceneNode * scene_node_
rviz::BoolProperty * only_border_property_
jsk_recognition_msgs::PolygonArray::ConstPtr latest_msg_
rviz::BoolProperty * enable_lighting_property_
virtual void processPolygonMaterial(const size_t i)
Ogre::ColourValue qtToOgre(const QColor &c)
virtual void processPolygon(const size_t i, const geometry_msgs::PolygonStamped &polygon)
virtual void processNormal(const size_t i, const geometry_msgs::PolygonStamped &polygon)
virtual void addOption(const QString &option, int value=0)
virtual void updateSceneNodes(const jsk_recognition_msgs::PolygonArray::ConstPtr &msg)
std::vector< Ogre::SceneNode * > scene_nodes_
rviz::FloatProperty * normal_length_property_
bool validateFloats(const sensor_msgs::CameraInfo &msg)
virtual QString getName() const
std::vector< ArrowPtr > arrow_objects_
virtual FrameManager * getFrameManager() const=0
virtual ~PolygonArrayDisplay()
void setColor(float r, float g, float b, float a) override
bool validateFloats(const visualization_msgs::InteractiveMarker &msg)
void setNumLines(uint32_t num)
#define ROS_DEBUG_STREAM(args)
void setMaxPointsPerLine(uint32_t max)
Ogre::SceneManager * scene_manager_
void updateNormalLength()
std::vector< Eigen::Vector3f, Eigen::aligned_allocator< Eigen::Vector3f > > Vertices
virtual Ogre::SceneManager * getSceneManager() const=0
std::vector< rviz::BillboardLine * > lines_
rviz::FloatProperty * alpha_property_
std::vector< Ogre::MaterialPtr > materials_
virtual float getFloat() const
bool getTransform(const Header &header, Ogre::Vector3 &position, Ogre::Quaternion &orientation)
void onInitialize() override
rviz::BoolProperty * show_normal_property_
std::vector< Ogre::ManualObject * > manual_objects_
virtual Ogre::ColourValue getColor(size_t index)
virtual void onInitialize()
virtual bool getBool() const
virtual int getOptionInt()
virtual void setStatus(StatusProperty::Level level, const QString &name, const QString &text)
virtual std::vector< Polygon::Ptr > decomposeToTriangles()
virtual Eigen::Vector3f getNormal()
void setLineWidth(float width)