From 6f50646d1bbf3e61baee0ef1c1337f7dcf56ec04 Mon Sep 17 00:00:00 2001 From: Martin Idel Date: Wed, 13 Dec 2017 12:15:20 +0100 Subject: [PATCH 01/11] Move billboard_line to rviz2 for use in grid --- rviz_rendering/CMakeLists.txt | 1 + .../src/rviz_rendering}/billboard_line.cpp | 12 +++++------- .../src/rviz_rendering/billboard_line.hpp | 16 ++++++++-------- 3 files changed, 14 insertions(+), 15 deletions(-) rename {rviz/src/rviz/ogre_helpers => rviz_rendering/src/rviz_rendering}/billboard_line.cpp (97%) rename rviz/src/rviz/ogre_helpers/billboard_line.h => rviz_rendering/src/rviz_rendering/billboard_line.hpp (94%) diff --git a/rviz_rendering/CMakeLists.txt b/rviz_rendering/CMakeLists.txt index 826d853f4..216167f81 100644 --- a/rviz_rendering/CMakeLists.txt +++ b/rviz_rendering/CMakeLists.txt @@ -64,6 +64,7 @@ add_library(rviz_rendering SHARED src/rviz_rendering/apply_visibility_bits.cpp src/rviz_rendering/arrow.cpp src/rviz_rendering/axes.cpp + src/rviz_rendering/billboard_line.cpp # src/rviz_rendering/initialization.cpp src/rviz_rendering/logging.cpp src/rviz_rendering/geometry.cpp diff --git a/rviz/src/rviz/ogre_helpers/billboard_line.cpp b/rviz_rendering/src/rviz_rendering/billboard_line.cpp similarity index 97% rename from rviz/src/rviz/ogre_helpers/billboard_line.cpp rename to rviz_rendering/src/rviz_rendering/billboard_line.cpp index 44ea31077..412b84701 100644 --- a/rviz/src/rviz/ogre_helpers/billboard_line.cpp +++ b/rviz_rendering/src/rviz_rendering/billboard_line.cpp @@ -27,7 +27,7 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include "billboard_line.h" +#include "billboard_line.hpp" #include #include @@ -39,11 +39,9 @@ #include -#include - #define MAX_ELEMENTS (65536/4) -namespace rviz +namespace rviz_rendering { BillboardLine::BillboardLine( Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node ) @@ -191,7 +189,7 @@ void BillboardLine::newLine() { ++current_line_; - ROS_ASSERT(current_line_ < num_lines_); + assert(current_line_ < num_lines_); } void BillboardLine::addPoint( const Ogre::Vector3& point ) @@ -204,7 +202,7 @@ void BillboardLine::addPoint( const Ogre::Vector3& point, const Ogre::ColourValu ++num_elements_[current_line_]; ++total_elements_; - ROS_ASSERT(num_elements_[current_line_] <= max_points_per_line_); + assert(num_elements_[current_line_] <= max_points_per_line_); ++elements_in_current_chain_; if (elements_in_current_chain_ > MAX_ELEMENTS) @@ -294,4 +292,4 @@ const Ogre::Quaternion& BillboardLine::getOrientation() return scene_node_->getOrientation(); } -} // namespace rviz +} // namespace rviz_rendering diff --git a/rviz/src/rviz/ogre_helpers/billboard_line.h b/rviz_rendering/src/rviz_rendering/billboard_line.hpp similarity index 94% rename from rviz/src/rviz/ogre_helpers/billboard_line.h rename to rviz_rendering/src/rviz_rendering/billboard_line.hpp index 2edbbb230..690a6e1e6 100644 --- a/rviz/src/rviz/ogre_helpers/billboard_line.h +++ b/rviz_rendering/src/rviz_rendering/billboard_line.hpp @@ -27,19 +27,19 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef OGRE_TOOLS_BILLBOARD_LINE_H -#define OGRE_TOOLS_BILLBOARD_LINE_H - -#include "object.h" +#ifndef RVIZ_RENDERING__BILLBOARD_LINE_HPP_ +#define RVIZ_RENDERING__BILLBOARD_LINE_HPP_ #include - #include + #include #include #include #include +#include "rviz_rendering/object.hpp" + namespace Ogre { class SceneManager; @@ -49,7 +49,7 @@ class Any; class BillboardChain; } -namespace rviz +namespace rviz_rendering { /** @@ -126,8 +126,8 @@ class BillboardLine : public Object uint32_t elements_in_current_chain_; }; -} // namespace rviz +} // namespace rviz_rendering -#endif +#endif // RVIZ_RENDERING__BILLBOARD_LINE_HPP_ From eae3b5b56e6ab7a15d6c2b0ddd11b6c91998ffc0 Mon Sep 17 00:00:00 2001 From: Martin Idel Date: Wed, 13 Dec 2017 12:24:57 +0100 Subject: [PATCH 02/11] Restore billboard functionality in Grid --- .../displays/grid/grid_display.cpp | 10 +- .../include/rviz_rendering/grid.hpp | 6 +- .../src/rviz_rendering/billboard_line.cpp | 3 +- .../src/rviz_rendering/billboard_line.hpp | 2 +- rviz_rendering/src/rviz_rendering/grid.cpp | 115 +++++++++--------- 5 files changed, 68 insertions(+), 68 deletions(-) diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/grid/grid_display.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/grid/grid_display.cpp index 665ec4ba4..b7a6df4fd 100644 --- a/rviz_default_plugins/src/rviz_default_plugins/displays/grid/grid_display.cpp +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/grid/grid_display.cpp @@ -94,8 +94,7 @@ GridDisplay::GridDisplay() "The rendering operation to use to draw the grid lines.", this, SLOT(updateStyle())); style_property_->addOption("Lines", Grid::Lines); - // TODO(wjwwood): re-enable the billboards option for the grid - // style_property_->addOption("Billboards", Grid::Billboards); + style_property_->addOption("Billboards", Grid::Billboards); line_width_property_ = new FloatProperty("Line Width", 0.03, "The width, in meters, of each grid line.", @@ -210,10 +209,9 @@ void GridDisplay::updateStyle() grid_->setStyle(style); switch (style) { - // TODO(wjwwood): re-enable billboards option - // case Grid::Billboards: - // line_width_property_->show(); - // break; + case Grid::Billboards: + line_width_property_->show(); + break; case Grid::Lines: default: line_width_property_->hide(); diff --git a/rviz_rendering/include/rviz_rendering/grid.hpp b/rviz_rendering/include/rviz_rendering/grid.hpp index 5b668fc68..82ee2db23 100644 --- a/rviz_rendering/include/rviz_rendering/grid.hpp +++ b/rviz_rendering/include/rviz_rendering/grid.hpp @@ -53,7 +53,7 @@ class Any; namespace rviz_rendering { -// class BillboardLine; + class BillboardLine; /** * \class Grid @@ -67,7 +67,7 @@ class RVIZ_RENDERING_PUBLIC Grid enum Style { Lines, -// Billboards, + Billboards, }; /** @@ -122,7 +122,7 @@ class RVIZ_RENDERING_PUBLIC Grid Ogre::SceneNode * scene_node_; ///< The scene node that this grid is attached to Ogre::ManualObject * manual_object_; ///< The manual object used to draw the grid -// BillboardLine* billboard_line_; + BillboardLine* billboard_line_; Ogre::MaterialPtr material_; diff --git a/rviz_rendering/src/rviz_rendering/billboard_line.cpp b/rviz_rendering/src/rviz_rendering/billboard_line.cpp index 412b84701..9a2b81ac4 100644 --- a/rviz_rendering/src/rviz_rendering/billboard_line.cpp +++ b/rviz_rendering/src/rviz_rendering/billboard_line.cpp @@ -84,7 +84,7 @@ BillboardLine::~BillboardLine() scene_manager_->destroySceneNode( scene_node_->getName() ); - Ogre::MaterialManager::getSingleton().remove(material_->getName()); + Ogre::MaterialManager::getSingleton().remove(material_); } Ogre::BillboardChain* BillboardLine::createChain() @@ -250,6 +250,7 @@ void BillboardLine::setOrientation( const Ogre::Quaternion& orientation ) void BillboardLine::setScale( const Ogre::Vector3& scale ) { // Setting scale doesn't really make sense here + (void) scale; } void BillboardLine::setColor( float r, float g, float b, float a ) diff --git a/rviz_rendering/src/rviz_rendering/billboard_line.hpp b/rviz_rendering/src/rviz_rendering/billboard_line.hpp index 690a6e1e6..ce79c1a9f 100644 --- a/rviz_rendering/src/rviz_rendering/billboard_line.hpp +++ b/rviz_rendering/src/rviz_rendering/billboard_line.hpp @@ -94,7 +94,7 @@ class BillboardLine : public Object /** * \brief We have no objects that we can set user data on */ - void setUserData( const Ogre::Any& data ) {} + void setUserData( const Ogre::Any& data ) {(void) data; } Ogre::MaterialPtr getMaterial() { return material_; } diff --git a/rviz_rendering/src/rviz_rendering/grid.cpp b/rviz_rendering/src/rviz_rendering/grid.cpp index ae80df39c..4f620ac6e 100644 --- a/rviz_rendering/src/rviz_rendering/grid.cpp +++ b/rviz_rendering/src/rviz_rendering/grid.cpp @@ -28,7 +28,8 @@ */ #include "rviz_rendering/grid.hpp" -// #include "billboard_line.h" + +#include #include #include @@ -38,7 +39,7 @@ #include #include -#include +#include "billboard_line.hpp" namespace rviz_rendering { @@ -72,7 +73,7 @@ Grid::Grid( scene_node_ = parent_node->createChildSceneNode(); scene_node_->attachObject(manual_object_); -// billboard_line_ = new BillboardLine(scene_manager, scene_node_); + billboard_line_ = new BillboardLine(scene_manager, scene_node_); ss << "Material"; material_ = Ogre::MaterialManager::getSingleton().create( @@ -85,7 +86,7 @@ Grid::Grid( Grid::~Grid() { -// delete billboard_line_; + delete billboard_line_; scene_manager_->destroySceneNode(scene_node_->getName()); scene_manager_->destroyManualObject(manual_object_); @@ -146,24 +147,24 @@ void Grid::setHeight(uint32_t height) void Grid::create() { manual_object_->clear(); -// billboard_line_->clear(); + billboard_line_->clear(); float extent = (cell_length_ * static_cast(cell_count_)) / 2; -// if (style_ == Billboards) -// { -// billboard_line_->setColor(color_.r, color_.g, color_.b, color_.a); -// billboard_line_->setLineWidth(line_width_); -// billboard_line_->setMaxPointsPerLine(2); -// billboard_line_->setNumLines((cell_count_+1) * 2 * (height_ + 1) -// + ((cell_count_ + 1) * (cell_count_ + 1)) * height_); -// } -// else -// { + if (style_ == Billboards) + { + billboard_line_->setColor(color_.r, color_.g, color_.b, color_.a); + billboard_line_->setLineWidth(line_width_); + billboard_line_->setMaxPointsPerLine(2); + billboard_line_->setNumLines((cell_count_+1) * 2 * (height_ + 1) + + ((cell_count_ + 1) * (cell_count_ + 1)) * height_); + } + else + { manual_object_->estimateVertexCount(cell_count_ * 4 * (height_ + 1) + ((cell_count_ + 1) * (cell_count_ + 1) * height_)); manual_object_->begin(material_->getName(), Ogre::RenderOperation::OT_LINE_LIST); -// } + } for (uint32_t h = 0; h <= height_; ++h) { float h_real = (height_ / 2.0f - static_cast(h)) * cell_length_; @@ -175,33 +176,33 @@ void Grid::create() Ogre::Vector3 p3(-extent, h_real, inc); Ogre::Vector3 p4(extent, h_real, inc); -// if (style_ == Billboards) -// { -// if (h != 0 || i != 0) -// { -// billboard_line_->newLine(); -// } -// -// billboard_line_->addPoint(p1); -// billboard_line_->addPoint(p2); -// -// billboard_line_->newLine(); -// -// billboard_line_->addPoint(p3); -// billboard_line_->addPoint(p4); -// } -// else -// { - manual_object_->position(p1); - manual_object_->colour(color_); - manual_object_->position(p2); - manual_object_->colour(color_); - - manual_object_->position(p3); - manual_object_->colour(color_); - manual_object_->position(p4); - manual_object_->colour(color_); -// } + if (style_ == Billboards) + { + if (h != 0 || i != 0) + { + billboard_line_->newLine(); + } + + billboard_line_->addPoint(p1); + billboard_line_->addPoint(p2); + + billboard_line_->newLine(); + + billboard_line_->addPoint(p3); + billboard_line_->addPoint(p4); + } + else + { + manual_object_->position(p1); + manual_object_->colour(color_); + manual_object_->position(p2); + manual_object_->colour(color_); + + manual_object_->position(p3); + manual_object_->colour(color_); + manual_object_->position(p4); + manual_object_->colour(color_); + } } } @@ -214,20 +215,20 @@ void Grid::create() float y_top = (height_ / 2.0f) * cell_length_; float y_bottom = -y_top; -// if (style_ == Billboards) -// { -// billboard_line_->newLine(); -// -// billboard_line_->addPoint( Ogre::Vector3(x_real, y_bottom, z_real)); -// billboard_line_->addPoint( Ogre::Vector3(x_real, y_top, z_real)); -// } -// else -// { - manual_object_->position(x_real, y_bottom, z_real); - manual_object_->colour(color_); - manual_object_->position(x_real, y_top, z_real); - manual_object_->colour(color_); -// } + if (style_ == Billboards) + { + billboard_line_->newLine(); + + billboard_line_->addPoint( Ogre::Vector3(x_real, y_bottom, z_real)); + billboard_line_->addPoint( Ogre::Vector3(x_real, y_top, z_real)); + } + else + { + manual_object_->position(x_real, y_bottom, z_real); + manual_object_->colour(color_); + manual_object_->position(x_real, y_top, z_real); + manual_object_->colour(color_); + } } } } From 56c15ae4f70834e4315f336d4ae3229b88e3487f Mon Sep 17 00:00:00 2001 From: Martin Idel Date: Wed, 13 Dec 2017 12:26:32 +0100 Subject: [PATCH 03/11] Uncrustify + cpplint --- .../displays/grid/grid_display.cpp | 6 +- .../include/rviz_rendering/grid.hpp | 4 +- .../src/rviz_rendering/billboard_line.cpp | 136 ++++++++---------- .../src/rviz_rendering/billboard_line.hpp | 36 +++-- rviz_rendering/src/rviz_rendering/grid.cpp | 38 ++--- 5 files changed, 94 insertions(+), 126 deletions(-) diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/grid/grid_display.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/grid/grid_display.cpp index b7a6df4fd..66aae765f 100644 --- a/rviz_default_plugins/src/rviz_default_plugins/displays/grid/grid_display.cpp +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/grid/grid_display.cpp @@ -209,9 +209,9 @@ void GridDisplay::updateStyle() grid_->setStyle(style); switch (style) { - case Grid::Billboards: - line_width_property_->show(); - break; + case Grid::Billboards: + line_width_property_->show(); + break; case Grid::Lines: default: line_width_property_->hide(); diff --git a/rviz_rendering/include/rviz_rendering/grid.hpp b/rviz_rendering/include/rviz_rendering/grid.hpp index 82ee2db23..c68a58efe 100644 --- a/rviz_rendering/include/rviz_rendering/grid.hpp +++ b/rviz_rendering/include/rviz_rendering/grid.hpp @@ -53,7 +53,7 @@ class Any; namespace rviz_rendering { - class BillboardLine; +class BillboardLine; /** * \class Grid @@ -122,7 +122,7 @@ class RVIZ_RENDERING_PUBLIC Grid Ogre::SceneNode * scene_node_; ///< The scene node that this grid is attached to Ogre::ManualObject * manual_object_; ///< The manual object used to draw the grid - BillboardLine* billboard_line_; + BillboardLine * billboard_line_; Ogre::MaterialPtr material_; diff --git a/rviz_rendering/src/rviz_rendering/billboard_line.cpp b/rviz_rendering/src/rviz_rendering/billboard_line.cpp index 9a2b81ac4..0bae6bebd 100644 --- a/rviz_rendering/src/rviz_rendering/billboard_line.cpp +++ b/rviz_rendering/src/rviz_rendering/billboard_line.cpp @@ -39,24 +39,23 @@ #include -#define MAX_ELEMENTS (65536/4) +#define MAX_ELEMENTS (65536 / 4) namespace rviz_rendering { -BillboardLine::BillboardLine( Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node ) -: Object( scene_manager ) -, width_( 0.1f ) -, current_line_(0) -, total_elements_(0) -, num_lines_(1) -, max_points_per_line_(100) -, lines_per_chain_(0) -, current_chain_(0) -, elements_in_current_chain_(0) +BillboardLine::BillboardLine(Ogre::SceneManager * scene_manager, Ogre::SceneNode * parent_node) +: Object(scene_manager), + width_(0.1f), + current_line_(0), + total_elements_(0), + num_lines_(1), + max_points_per_line_(100), + lines_per_chain_(0), + current_chain_(0), + elements_in_current_chain_(0) { - if ( !parent_node ) - { + if (!parent_node) { parent_node = scene_manager_->getRootSceneNode(); } @@ -65,7 +64,8 @@ BillboardLine::BillboardLine( Ogre::SceneManager* scene_manager, Ogre::SceneNode static int count = 0; std::stringstream ss; ss << "BillboardLineMaterial" << count++; - material_ = Ogre::MaterialManager::getSingleton().create( ss.str(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME ); + material_ = Ogre::MaterialManager::getSingleton().create( + ss.str(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); material_->setReceiveShadows(false); material_->getTechnique(0)->setLightingEnabled(false); @@ -77,24 +77,23 @@ BillboardLine::~BillboardLine() { V_Chain::iterator it = chains_.begin(); V_Chain::iterator end = chains_.end(); - for (;it != end; ++it) - { + for (; it != end; ++it) { scene_manager_->destroyBillboardChain(*it); } - scene_manager_->destroySceneNode( scene_node_->getName() ); + scene_manager_->destroySceneNode(scene_node_->getName() ); Ogre::MaterialManager::getSingleton().remove(material_); } -Ogre::BillboardChain* BillboardLine::createChain() +Ogre::BillboardChain * BillboardLine::createChain() { std::stringstream ss; static int count = 0; ss << "BillboardLine chain" << count++; - Ogre::BillboardChain* chain = scene_manager_->createBillboardChain(ss.str()); - chain->setMaterialName( material_->getName() ); - scene_node_->attachObject( chain ); + Ogre::BillboardChain * chain = scene_manager_->createBillboardChain(ss.str()); + chain->setMaterialName(material_->getName() ); + scene_node_->attachObject(chain); chains_.push_back(chain); @@ -105,8 +104,7 @@ void BillboardLine::clear() { V_Chain::iterator it = chains_.begin(); V_Chain::iterator end = chains_.end(); - for (; it != end; ++it) - { + for (; it != end; ++it) { (*it)->clearAllChains(); } @@ -115,8 +113,7 @@ void BillboardLine::clear() current_chain_ = 0; elements_in_current_chain_ = 0; - for (V_uint32::iterator it = num_elements_.begin(); it != num_elements_.end(); ++it) - { + for (V_uint32::iterator it = num_elements_.begin(); it != num_elements_.end(); ++it) { *it = 0; } } @@ -125,13 +122,11 @@ void BillboardLine::setupChains() { uint32_t total_points = max_points_per_line_ * num_lines_; uint32_t num_chains = total_points / MAX_ELEMENTS; - if (total_points % MAX_ELEMENTS != 0) - { + if (total_points % MAX_ELEMENTS != 0) { ++num_chains; } - for (uint32_t i = chains_.size(); i < num_chains; ++i) - { + for (uint32_t i = chains_.size(); i < num_chains; ++i) { createChain(); } @@ -139,26 +134,20 @@ void BillboardLine::setupChains() V_Chain::iterator it = chains_.begin(); V_Chain::iterator end = chains_.end(); - for (;it != end; ++it) - { + for (; it != end; ++it) { (*it)->setMaxChainElements(max_points_per_line_); // shorten the number of chains in the last bbchain, to avoid memory wasteage - if (it + 1 == end) - { + if (it + 1 == end) { uint32_t lines_left = num_lines_ % lines_per_chain_; // Handle the case where num_lines_ is a multiple of lines_per_chain if (lines_left == 0) { - (*it)->setNumberOfChains(lines_per_chain_); + (*it)->setNumberOfChains(lines_per_chain_); + } else { + (*it)->setNumberOfChains(lines_left); } - else - { - (*it)->setNumberOfChains(lines_left); - } - } - else - { + } else { (*it)->setNumberOfChains(lines_per_chain_); } } @@ -179,8 +168,7 @@ void BillboardLine::setNumLines(uint32_t num) num_elements_.resize(num); - for (V_uint32::iterator it = num_elements_.begin(); it != num_elements_.end(); ++it) - { + for (V_uint32::iterator it = num_elements_.begin(); it != num_elements_.end(); ++it) { *it = 0; } } @@ -192,12 +180,12 @@ void BillboardLine::newLine() assert(current_line_ < num_lines_); } -void BillboardLine::addPoint( const Ogre::Vector3& point ) +void BillboardLine::addPoint(const Ogre::Vector3 & point) { addPoint(point, color_); } -void BillboardLine::addPoint( const Ogre::Vector3& point, const Ogre::ColourValue& color ) +void BillboardLine::addPoint(const Ogre::Vector3 & point, const Ogre::ColourValue & color) { ++num_elements_[current_line_]; ++total_elements_; @@ -205,8 +193,7 @@ void BillboardLine::addPoint( const Ogre::Vector3& point, const Ogre::ColourValu assert(num_elements_[current_line_] <= max_points_per_line_); ++elements_in_current_chain_; - if (elements_in_current_chain_ > MAX_ELEMENTS) - { + if (elements_in_current_chain_ > MAX_ELEMENTS) { ++current_chain_; elements_in_current_chain_ = 1; } @@ -215,20 +202,18 @@ void BillboardLine::addPoint( const Ogre::Vector3& point, const Ogre::ColourValu e.position = point; e.width = width_; e.colour = color; - chains_[current_chain_]->addChainElement(current_line_ % lines_per_chain_ , e); + chains_[current_chain_]->addChainElement(current_line_ % lines_per_chain_, e); } -void BillboardLine::setLineWidth( float width ) +void BillboardLine::setLineWidth(float width) { width_ = width; - for (uint32_t line = 0; line < num_lines_; ++line) - { + for (uint32_t line = 0; line < num_lines_; ++line) { uint32_t element_count = num_elements_[line]; - for ( uint32_t i = 0; i < element_count; ++i ) - { - Ogre::BillboardChain* c = chains_[line / lines_per_chain_]; + for (uint32_t i = 0; i < element_count; ++i) { + Ogre::BillboardChain * c = chains_[line / lines_per_chain_]; Ogre::BillboardChain::Element e = c->getChainElement(line % lines_per_chain_, i); e.width = width_; @@ -237,44 +222,39 @@ void BillboardLine::setLineWidth( float width ) } } -void BillboardLine::setPosition( const Ogre::Vector3& position ) +void BillboardLine::setPosition(const Ogre::Vector3 & position) { - scene_node_->setPosition( position ); + scene_node_->setPosition(position); } -void BillboardLine::setOrientation( const Ogre::Quaternion& orientation ) +void BillboardLine::setOrientation(const Ogre::Quaternion & orientation) { - scene_node_->setOrientation( orientation ); + scene_node_->setOrientation(orientation); } -void BillboardLine::setScale( const Ogre::Vector3& scale ) +void BillboardLine::setScale(const Ogre::Vector3 & scale) { // Setting scale doesn't really make sense here (void) scale; } -void BillboardLine::setColor( float r, float g, float b, float a ) +void BillboardLine::setColor(float r, float g, float b, float a) { - if ( a < 0.9998 ) - { - material_->getTechnique(0)->setSceneBlending( Ogre::SBT_TRANSPARENT_ALPHA ); - material_->getTechnique(0)->setDepthWriteEnabled( false ); - } - else - { - material_->getTechnique(0)->setSceneBlending( Ogre::SBT_REPLACE ); - material_->getTechnique(0)->setDepthWriteEnabled( true ); + if (a < 0.9998) { + material_->getTechnique(0)->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA); + material_->getTechnique(0)->setDepthWriteEnabled(false); + } else { + material_->getTechnique(0)->setSceneBlending(Ogre::SBT_REPLACE); + material_->getTechnique(0)->setDepthWriteEnabled(true); } - color_ = Ogre::ColourValue( r, g, b, a ); + color_ = Ogre::ColourValue(r, g, b, a); - for (uint32_t line = 0; line < num_lines_; ++line) - { + for (uint32_t line = 0; line < num_lines_; ++line) { uint32_t element_count = num_elements_[line]; - for ( uint32_t i = 0; i < element_count; ++i ) - { - Ogre::BillboardChain* c = chains_[line / lines_per_chain_]; + for (uint32_t i = 0; i < element_count; ++i) { + Ogre::BillboardChain * c = chains_[line / lines_per_chain_]; Ogre::BillboardChain::Element e = c->getChainElement(line % lines_per_chain_, i); e.colour = color_; @@ -283,14 +263,14 @@ void BillboardLine::setColor( float r, float g, float b, float a ) } } -const Ogre::Vector3& BillboardLine::getPosition() +const Ogre::Vector3 & BillboardLine::getPosition() { return scene_node_->getPosition(); } -const Ogre::Quaternion& BillboardLine::getOrientation() +const Ogre::Quaternion & BillboardLine::getOrientation() { return scene_node_->getOrientation(); } -} // namespace rviz_rendering +} // namespace rviz_rendering diff --git a/rviz_rendering/src/rviz_rendering/billboard_line.hpp b/rviz_rendering/src/rviz_rendering/billboard_line.hpp index ce79c1a9f..c9821277f 100644 --- a/rviz_rendering/src/rviz_rendering/billboard_line.hpp +++ b/rviz_rendering/src/rviz_rendering/billboard_line.hpp @@ -64,47 +64,47 @@ class BillboardLine : public Object * @param manager Scene manager this object is a part of * @param parent_node A scene node to use as the parent of this object. If NULL, uses the root scene node. */ - BillboardLine( Ogre::SceneManager* manager, Ogre::SceneNode* parent_node = NULL ); + explicit BillboardLine(Ogre::SceneManager * manager, Ogre::SceneNode * parent_node = NULL); virtual ~BillboardLine(); void clear(); void newLine(); - void addPoint(const Ogre::Vector3& point); - void addPoint(const Ogre::Vector3& point, const Ogre::ColourValue& color); + void addPoint(const Ogre::Vector3 & point); + void addPoint(const Ogre::Vector3 & point, const Ogre::ColourValue & color); - void setLineWidth( float width ); + void setLineWidth(float width); void setMaxPointsPerLine(uint32_t max); void setNumLines(uint32_t num); // overrides from Object - virtual void setOrientation( const Ogre::Quaternion& orientation ); - virtual void setPosition( const Ogre::Vector3& position ); - virtual void setScale( const Ogre::Vector3& scale ); - virtual void setColor( float r, float g, float b, float a ); - virtual const Ogre::Vector3& getPosition(); - virtual const Ogre::Quaternion& getOrientation(); + virtual void setOrientation(const Ogre::Quaternion & orientation); + virtual void setPosition(const Ogre::Vector3 & position); + virtual void setScale(const Ogre::Vector3 & scale); + virtual void setColor(float r, float g, float b, float a); + virtual const Ogre::Vector3 & getPosition(); + virtual const Ogre::Quaternion & getOrientation(); /** * \brief Get the scene node associated with this object * @return The scene node associated with this object */ - Ogre::SceneNode* getSceneNode() { return scene_node_; } + Ogre::SceneNode * getSceneNode() {return scene_node_;} /** * \brief We have no objects that we can set user data on */ - void setUserData( const Ogre::Any& data ) {(void) data; } + void setUserData(const Ogre::Any & data) {(void) data;} - Ogre::MaterialPtr getMaterial() { return material_; } + Ogre::MaterialPtr getMaterial() {return material_;} private: void setupChains(); - Ogre::BillboardChain* createChain(); + Ogre::BillboardChain * createChain(); - Ogre::SceneNode* scene_node_; + Ogre::SceneNode * scene_node_; - typedef std::vector V_Chain; + typedef std::vector V_Chain; V_Chain chains_; Ogre::MaterialPtr material_; @@ -126,8 +126,6 @@ class BillboardLine : public Object uint32_t elements_in_current_chain_; }; -} // namespace rviz_rendering +} // namespace rviz_rendering #endif // RVIZ_RENDERING__BILLBOARD_LINE_HPP_ - - diff --git a/rviz_rendering/src/rviz_rendering/grid.cpp b/rviz_rendering/src/rviz_rendering/grid.cpp index 4f620ac6e..b02f9603c 100644 --- a/rviz_rendering/src/rviz_rendering/grid.cpp +++ b/rviz_rendering/src/rviz_rendering/grid.cpp @@ -151,19 +151,16 @@ void Grid::create() float extent = (cell_length_ * static_cast(cell_count_)) / 2; - if (style_ == Billboards) - { + if (style_ == Billboards) { billboard_line_->setColor(color_.r, color_.g, color_.b, color_.a); billboard_line_->setLineWidth(line_width_); billboard_line_->setMaxPointsPerLine(2); - billboard_line_->setNumLines((cell_count_+1) * 2 * (height_ + 1) - + ((cell_count_ + 1) * (cell_count_ + 1)) * height_); - } - else - { - manual_object_->estimateVertexCount(cell_count_ * 4 * (height_ + 1) + - ((cell_count_ + 1) * (cell_count_ + 1) * height_)); - manual_object_->begin(material_->getName(), Ogre::RenderOperation::OT_LINE_LIST); + billboard_line_->setNumLines((cell_count_ + 1) * 2 * (height_ + 1) + + ((cell_count_ + 1) * (cell_count_ + 1)) * height_); + } else { + manual_object_->estimateVertexCount(cell_count_ * 4 * (height_ + 1) + + ((cell_count_ + 1) * (cell_count_ + 1) * height_)); + manual_object_->begin(material_->getName(), Ogre::RenderOperation::OT_LINE_LIST); } for (uint32_t h = 0; h <= height_; ++h) { @@ -176,10 +173,8 @@ void Grid::create() Ogre::Vector3 p3(-extent, h_real, inc); Ogre::Vector3 p4(extent, h_real, inc); - if (style_ == Billboards) - { - if (h != 0 || i != 0) - { + if (style_ == Billboards) { + if (h != 0 || i != 0) { billboard_line_->newLine(); } @@ -190,9 +185,7 @@ void Grid::create() billboard_line_->addPoint(p3); billboard_line_->addPoint(p4); - } - else - { + } else { manual_object_->position(p1); manual_object_->colour(color_); manual_object_->position(p2); @@ -215,15 +208,12 @@ void Grid::create() float y_top = (height_ / 2.0f) * cell_length_; float y_bottom = -y_top; - if (style_ == Billboards) - { + if (style_ == Billboards) { billboard_line_->newLine(); - billboard_line_->addPoint( Ogre::Vector3(x_real, y_bottom, z_real)); - billboard_line_->addPoint( Ogre::Vector3(x_real, y_top, z_real)); - } - else - { + billboard_line_->addPoint(Ogre::Vector3(x_real, y_bottom, z_real)); + billboard_line_->addPoint(Ogre::Vector3(x_real, y_top, z_real)); + } else { manual_object_->position(x_real, y_bottom, z_real); manual_object_->colour(color_); manual_object_->position(x_real, y_top, z_real); From b1bd2d1f9249f14039b0d54e88601be431bd3b46 Mon Sep 17 00:00:00 2001 From: Martin Idel Date: Wed, 13 Dec 2017 16:15:34 +0100 Subject: [PATCH 04/11] Add tests for billboard_line - need to expose getChains for testing - one test documents faulty behaviour of Ogre in test --- rviz_rendering/CMakeLists.txt | 25 ++- .../src/rviz_rendering/billboard_line.hpp | 5 +- .../src/test/billboard_line_test.cpp | 210 ++++++++++++++++++ 3 files changed, 231 insertions(+), 9 deletions(-) create mode 100644 rviz_rendering/src/test/billboard_line_test.cpp diff --git a/rviz_rendering/CMakeLists.txt b/rviz_rendering/CMakeLists.txt index 216167f81..63456c11d 100644 --- a/rviz_rendering/CMakeLists.txt +++ b/rviz_rendering/CMakeLists.txt @@ -185,14 +185,23 @@ if(BUILD_TESTING) # endif() # TODO(wjwwood): reenable this test when it can be run on CI - # ament_add_gtest(point_cloud_renderable_test_target src/test/point_cloud_renderable_test.cpp) - # if(TARGET point_cloud_renderable_test_target) - # target_link_libraries(point_cloud_renderable_test_target - # rviz_ogre_vendor::OgreMain - # rviz_rendering - # Qt5::Widgets # explicitly do this for include directories (not necessary for external use) - # ) - # endif() +# ament_add_gtest(point_cloud_renderable_test_target src/test/point_cloud_renderable_test.cpp) +# if(TARGET point_cloud_renderable_test_target) +# target_link_libraries(point_cloud_renderable_test_target +# rviz_ogre_vendor::OgreMain +# rviz_rendering +# Qt5::Widgets # explicitly do this for include directories (not necessary for external use) +# ) +# endif() + + ament_add_gtest(billboard_line_test_target src/test/billboard_line_test.cpp) + if(TARGET billboard_line_test_target) + target_link_libraries(billboard_line_test_target + rviz_ogre_vendor::OgreMain + rviz_rendering + Qt5::Widgets # explicitly do this for include directories (not necessary for external use) + ) + endif() endif() list(APPEND ${PROJECT_NAME}_CONFIG_EXTRAS diff --git a/rviz_rendering/src/rviz_rendering/billboard_line.hpp b/rviz_rendering/src/rviz_rendering/billboard_line.hpp index c9821277f..52c812330 100644 --- a/rviz_rendering/src/rviz_rendering/billboard_line.hpp +++ b/rviz_rendering/src/rviz_rendering/billboard_line.hpp @@ -98,13 +98,16 @@ class BillboardLine : public Object Ogre::MaterialPtr getMaterial() {return material_;} + typedef std::vector V_Chain; + /// exposed for testing + V_Chain getChains() {return chains_;} + private: void setupChains(); Ogre::BillboardChain * createChain(); Ogre::SceneNode * scene_node_; - typedef std::vector V_Chain; V_Chain chains_; Ogre::MaterialPtr material_; diff --git a/rviz_rendering/src/test/billboard_line_test.cpp b/rviz_rendering/src/test/billboard_line_test.cpp new file mode 100644 index 000000000..d1667b41e --- /dev/null +++ b/rviz_rendering/src/test/billboard_line_test.cpp @@ -0,0 +1,210 @@ +/* + * Copyright (c) 2017, Bosch Software Innovations GmbH. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include + +#include +#include +#include + +#include + +#include "ogre_testing_environment.hpp" +#include "../src/rviz_rendering/billboard_line.hpp" + +class BillboardLineTestFixture : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + testing_environment_ = std::make_shared(); + testing_environment_->setUpOgreTestEnvironment(); + } + + static std::shared_ptr testing_environment_; +}; + +std::shared_ptr +BillboardLineTestFixture::testing_environment_ = nullptr; + +static std::vector squareCenteredAtZero +{ + Ogre::Vector3(1, 1, 0), + Ogre::Vector3(1, -1, 0), + Ogre::Vector3(-1, 1, 0), + Ogre::Vector3(-1, -1, 0) +}; + +rviz_rendering::BillboardLine oneCellGrid() +{ + rviz_rendering::BillboardLine grid_cell(Ogre::Root::getSingletonPtr()->createSceneManager()); + + std::vector pts = squareCenteredAtZero; + grid_cell.setMaxPointsPerLine(2); + grid_cell.setNumLines(2 * 2); // grid of 1 cell needs 2 vertical and 2 horizontal lines + + grid_cell.addPoint(pts[0]); + grid_cell.addPoint(pts[1]); + grid_cell.newLine(); + grid_cell.addPoint(pts[2]); + grid_cell.addPoint(pts[3]); + grid_cell.newLine(); + grid_cell.addPoint(pts[0]); + grid_cell.addPoint(pts[2]); + grid_cell.newLine(); + grid_cell.addPoint(pts[1]); + grid_cell.addPoint(pts[3]); + + return grid_cell; +} + +rviz_rendering::BillboardLine +twoLines() +{ + rviz_rendering::BillboardLine two_lines(Ogre::Root::getSingletonPtr()->createSceneManager()); + std::vector pts = squareCenteredAtZero; + two_lines.setMaxPointsPerLine(2); + two_lines.setNumLines(2); + two_lines.addPoint(pts[0]); + two_lines.addPoint(pts[1]); + two_lines.newLine(); + two_lines.addPoint(pts[2]); + two_lines.addPoint(pts[3]); + return two_lines; +} + +TEST_F(BillboardLineTestFixture, new_billboard_consumes_only_as_much_space_as_necessary) { + auto grid_cell = oneCellGrid(); + + auto chains = grid_cell.getChains(); + + ASSERT_EQ(chains.size(), static_cast(1)); + ASSERT_EQ(chains[0]->getNumberOfChains(), static_cast(2 * 2)); + // A chainElement is exactly a line + for (unsigned int i = 0; i < chains[0]->getNumberOfChains(); i++) { + ASSERT_EQ(chains[0]->getNumChainElements(i), static_cast(2)); + } +} + +TEST_F(BillboardLineTestFixture, new_billboard_contains_correct_points_as_bounding_box_is_correct) { + auto grid_cell = oneCellGrid(); + + auto chains = grid_cell.getChains(); + + // chains are basically bounded by the square + auto bounding_box = Ogre::AxisAlignedBox(Ogre::Vector3(-1.1f, -1.1f, -0.1f), + Ogre::Vector3(1.1f, 1.1f, 0.1f)); + ASSERT_EQ(chains[0]->getBoundingBox(), bounding_box); +} + +TEST_F(BillboardLineTestFixture, setColor_results_in_change_of_color_for_all_chains) { + auto grid_cell = twoLines(); + + grid_cell.setColor(0.3f, 0.4f, 0.5f, 0.2f); + + auto chains = grid_cell.getChains(); + for (auto & chain : chains) { + for (unsigned int i = 0; i < chain->getNumberOfChains(); i++) { + for (unsigned int j = 0; j < chain->getNumChainElements(i); j++) { + ASSERT_EQ(chain->getChainElement(i, j).colour, Ogre::ColourValue(0.3f, 0.4f, 0.5f, 0.2f)); + } + } + } +} + +TEST_F(BillboardLineTestFixture, setWidth_results_in_change_of_width_for_all_chains) { + auto grid_cell = twoLines(); + + grid_cell.setLineWidth(0.4f); + + auto chains = grid_cell.getChains(); + for (auto & chain : chains) { + for (unsigned int i = 0; i < chain->getNumberOfChains(); i++) { + for (unsigned int j = 0; j < chain->getNumChainElements(i); j++) { + ASSERT_EQ(chain->getChainElement(i, j).width, 0.4f); + } + } + } +} + +TEST_F(BillboardLineTestFixture, clear_resets_all_chains) { + auto grid_cell = twoLines(); + + grid_cell.clear(); + + auto chains = grid_cell.getChains(); + ASSERT_EQ(chains.size(), static_cast(1)); // chains are reset, not destroyed + ASSERT_EQ(chains[0]->getNumberOfChains(), static_cast(2)); // reset, not destroyed + + // The following shows a bug in Ogre. The chains are cleared, but still have size. + ASSERT_EQ(chains[0]->getNumChainElements(0), static_cast(1)); + ASSERT_EQ(chains[0]->getChainElement(0, 0).position, Ogre::Vector3(1, 1, 0)); // shouldn't be + // accessible +} + +TEST_F(BillboardLineTestFixture, create_multiple_chains) { + rviz_rendering::BillboardLine grid_cell(Ogre::Root::getSingletonPtr()->createSceneManager()); + + grid_cell.setMaxPointsPerLine(2); + grid_cell.setNumLines(100000); + + for (unsigned int line = 0; line < 100000; line++) { + if (line != 0) { + grid_cell.newLine(); + } + grid_cell.addPoint(Ogre::Vector3(0, 0, line + 0.1f)); + grid_cell.addPoint(Ogre::Vector3(0, 0, line + 0.3f)); + } + + auto chains = grid_cell.getChains(); + // maximal no of elements per chain: 16384 (defined in billboard_line.cpp) + ASSERT_EQ(chains.size(), static_cast(200000 / 16384 + 1)); // +1 to accomodate rest + ASSERT_EQ(chains[0]->getNumberOfChains(), static_cast(16384 / 2)); // 2 lines per chain + ASSERT_EQ(chains[0]->getNumChainElements(0), static_cast(2)); +} + +TEST_F(BillboardLineTestFixture, create_chain_with_extremely_many_elements) { + rviz_rendering::BillboardLine grid_cell(Ogre::Root::getSingletonPtr()->createSceneManager()); + + grid_cell.setMaxPointsPerLine(100000); + grid_cell.setNumLines(2); + + for (unsigned int line = 0; line < 2; line++) { + if (line != 0) { + grid_cell.newLine(); + } + for (unsigned int point = 0; point < 100000; point++) { + grid_cell.addPoint(Ogre::Vector3(0, 0, line + point + 0.1f)); + } + } + + auto chains = grid_cell.getChains(); + ASSERT_EQ(chains.size(), static_cast(200000 / 16384 + 1)); + ASSERT_EQ(chains[0]->getNumberOfChains(), static_cast(1)); +} From 53eea38acc0c5221d54e8cbf5817b33a103a8940 Mon Sep 17 00:00:00 2001 From: Martin Idel Date: Thu, 14 Dec 2017 11:16:02 +0100 Subject: [PATCH 05/11] Refactoring of billboard_line - extract functions - fix functionality for huge numbers of elements per chain - add TODO comment to use internal Ogre functionality when fixed --- .../src/rviz_rendering/billboard_line.cpp | 150 +++++++++--------- .../src/rviz_rendering/billboard_line.hpp | 47 +++--- 2 files changed, 105 insertions(+), 92 deletions(-) diff --git a/rviz_rendering/src/rviz_rendering/billboard_line.cpp b/rviz_rendering/src/rviz_rendering/billboard_line.cpp index 0bae6bebd..1a6da769e 100644 --- a/rviz_rendering/src/rviz_rendering/billboard_line.cpp +++ b/rviz_rendering/src/rviz_rendering/billboard_line.cpp @@ -1,5 +1,6 @@ /* * Copyright (c) 2008, Willow Garage, Inc. + * Copyright (c) 2017, Bosch Software Innovations GmbH. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -37,9 +38,10 @@ #include #include +#include #include -#define MAX_ELEMENTS (65536 / 4) +static const uint32_t MAX_ELEMENTS = (65536 / 4); namespace rviz_rendering { @@ -47,13 +49,12 @@ namespace rviz_rendering BillboardLine::BillboardLine(Ogre::SceneManager * scene_manager, Ogre::SceneNode * parent_node) : Object(scene_manager), width_(0.1f), - current_line_(0), - total_elements_(0), num_lines_(1), max_points_per_line_(100), - lines_per_chain_(0), - current_chain_(0), - elements_in_current_chain_(0) + chains_per_container_(0), + current_line_(0), + current_chain_container_(0), + elements_in_current_chain_container_(0) { if (!parent_node) { parent_node = scene_manager_->getRootSceneNode(); @@ -75,10 +76,8 @@ BillboardLine::BillboardLine(Ogre::SceneManager * scene_manager, Ogre::SceneNode BillboardLine::~BillboardLine() { - V_Chain::iterator it = chains_.begin(); - V_Chain::iterator end = chains_.end(); - for (; it != end; ++it) { - scene_manager_->destroyBillboardChain(*it); + for (auto & chain : chain_containers_) { + scene_manager_->destroyBillboardChain(chain); } scene_manager_->destroySceneNode(scene_node_->getName() ); @@ -86,39 +85,22 @@ BillboardLine::~BillboardLine() Ogre::MaterialManager::getSingleton().remove(material_); } -Ogre::BillboardChain * BillboardLine::createChain() -{ - std::stringstream ss; - static int count = 0; - ss << "BillboardLine chain" << count++; - Ogre::BillboardChain * chain = scene_manager_->createBillboardChain(ss.str()); - chain->setMaterialName(material_->getName() ); - scene_node_->attachObject(chain); - - chains_.push_back(chain); - - return chain; -} - void BillboardLine::clear() { - V_Chain::iterator it = chains_.begin(); - V_Chain::iterator end = chains_.end(); - for (; it != end; ++it) { - (*it)->clearAllChains(); + for (auto & chain : chain_containers_) { + chain->clearAllChains(); } current_line_ = 0; - total_elements_ = 0; - current_chain_ = 0; - elements_in_current_chain_ = 0; + current_chain_container_ = 0; + elements_in_current_chain_container_ = 0; - for (V_uint32::iterator it = num_elements_.begin(); it != num_elements_.end(); ++it) { - *it = 0; + for (auto & num_element : num_elements_) { + num_element = 0; } } -void BillboardLine::setupChains() +void BillboardLine::setupChainContainers() { uint32_t total_points = max_points_per_line_ * num_lines_; uint32_t num_chains = total_points / MAX_ELEMENTS; @@ -126,29 +108,47 @@ void BillboardLine::setupChains() ++num_chains; } - for (uint32_t i = chains_.size(); i < num_chains; ++i) { + for (uint32_t i = chain_containers_.size(); i < num_chains; ++i) { createChain(); } - lines_per_chain_ = max_points_per_line_ > 0 ? MAX_ELEMENTS / max_points_per_line_ : 1; + chains_per_container_ = max_points_per_line_ > 0 ? MAX_ELEMENTS / max_points_per_line_ : 1; + if (max_points_per_line_ > MAX_ELEMENTS) { + chains_per_container_ = 1; + } + + setupChainsInChainContainers(); +} - V_Chain::iterator it = chains_.begin(); - V_Chain::iterator end = chains_.end(); +Ogre::BillboardChain * BillboardLine::createChain() +{ + std::stringstream ss; + static int count = 0; + ss << "BillboardLine chain" << count++; + Ogre::BillboardChain * chain = scene_manager_->createBillboardChain(ss.str()); + chain->setMaterialName(material_->getName() ); + scene_node_->attachObject(chain); + + chain_containers_.push_back(chain); + + return chain; +} + +void BillboardLine::setupChainsInChainContainers() const +{ + auto it = chain_containers_.begin(); + auto end = chain_containers_.end(); for (; it != end; ++it) { (*it)->setMaxChainElements(max_points_per_line_); // shorten the number of chains in the last bbchain, to avoid memory wasteage if (it + 1 == end) { - uint32_t lines_left = num_lines_ % lines_per_chain_; + uint32_t lines_left = num_lines_ % chains_per_container_; // Handle the case where num_lines_ is a multiple of lines_per_chain - if (lines_left == 0) { - (*it)->setNumberOfChains(lines_per_chain_); - } else { - (*it)->setNumberOfChains(lines_left); - } + (*it)->setNumberOfChains((lines_left == 0) ? chains_per_container_ : lines_left); } else { - (*it)->setNumberOfChains(lines_per_chain_); + (*it)->setNumberOfChains(chains_per_container_); } } } @@ -157,19 +157,19 @@ void BillboardLine::setMaxPointsPerLine(uint32_t max) { max_points_per_line_ = max; - setupChains(); + setupChainContainers(); } void BillboardLine::setNumLines(uint32_t num) { num_lines_ = num; - setupChains(); + setupChainContainers(); num_elements_.resize(num); - for (V_uint32::iterator it = num_elements_.begin(); it != num_elements_.end(); ++it) { - *it = 0; + for (auto & num_element : num_elements_) { + num_element = 0; } } @@ -188,38 +188,36 @@ void BillboardLine::addPoint(const Ogre::Vector3 & point) void BillboardLine::addPoint(const Ogre::Vector3 & point, const Ogre::ColourValue & color) { ++num_elements_[current_line_]; - ++total_elements_; assert(num_elements_[current_line_] <= max_points_per_line_); - ++elements_in_current_chain_; - if (elements_in_current_chain_ > MAX_ELEMENTS) { - ++current_chain_; - elements_in_current_chain_ = 1; - } + incrementChainContainerIfNecessary(); Ogre::BillboardChain::Element e; e.position = point; e.width = width_; e.colour = color; - chains_[current_chain_]->addChainElement(current_line_ % lines_per_chain_, e); + chain_containers_[current_chain_container_]->addChainElement( + current_line_ % chains_per_container_, e); +} + +void BillboardLine::incrementChainContainerIfNecessary() +{ + ++elements_in_current_chain_container_; + if (elements_in_current_chain_container_ > MAX_ELEMENTS) { + ++current_chain_container_; + elements_in_current_chain_container_ = 1; + } } void BillboardLine::setLineWidth(float width) { width_ = width; - for (uint32_t line = 0; line < num_lines_; ++line) { - uint32_t element_count = num_elements_[line]; - - for (uint32_t i = 0; i < element_count; ++i) { - Ogre::BillboardChain * c = chains_[line / lines_per_chain_]; - Ogre::BillboardChain::Element e = c->getChainElement(line % lines_per_chain_, i); - - e.width = width_; - c->updateChainElement(line % lines_per_chain_, i, e); - } - } + changeAllElements([width](Ogre::BillboardChain::Element element) { + element.width = width; + return element; + }); } void BillboardLine::setPosition(const Ogre::Vector3 & position) @@ -250,15 +248,25 @@ void BillboardLine::setColor(float r, float g, float b, float a) color_ = Ogre::ColourValue(r, g, b, a); + changeAllElements([this](Ogre::BillboardChain::Element element) { + element.colour = color_; + return element; + }); +} + +void BillboardLine::changeAllElements( + std::function change_element) +{ for (uint32_t line = 0; line < num_lines_; ++line) { uint32_t element_count = num_elements_[line]; for (uint32_t i = 0; i < element_count; ++i) { - Ogre::BillboardChain * c = chains_[line / lines_per_chain_]; - Ogre::BillboardChain::Element e = c->getChainElement(line % lines_per_chain_, i); + Ogre::BillboardChain * c = chain_containers_[line / chains_per_container_]; - e.colour = color_; - c->updateChainElement(line % lines_per_chain_, i, e); + uint32_t chain_index = line % chains_per_container_; + Ogre::BillboardChain::Element element = c->getChainElement(chain_index, i); + Ogre::BillboardChain::Element new_element = change_element(element); + c->updateChainElement(chain_index, i, new_element); } } } diff --git a/rviz_rendering/src/rviz_rendering/billboard_line.hpp b/rviz_rendering/src/rviz_rendering/billboard_line.hpp index 52c812330..1df52f06b 100644 --- a/rviz_rendering/src/rviz_rendering/billboard_line.hpp +++ b/rviz_rendering/src/rviz_rendering/billboard_line.hpp @@ -1,5 +1,6 @@ /* * Copyright (c) 2008, Willow Garage, Inc. + * Copyright (c) 2017, Bosch Software Innovations GmbH. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -30,9 +31,10 @@ #ifndef RVIZ_RENDERING__BILLBOARD_LINE_HPP_ #define RVIZ_RENDERING__BILLBOARD_LINE_HPP_ -#include +#include #include +#include #include #include #include @@ -64,8 +66,8 @@ class BillboardLine : public Object * @param manager Scene manager this object is a part of * @param parent_node A scene node to use as the parent of this object. If NULL, uses the root scene node. */ - explicit BillboardLine(Ogre::SceneManager * manager, Ogre::SceneNode * parent_node = NULL); - virtual ~BillboardLine(); + explicit BillboardLine(Ogre::SceneManager * manager, Ogre::SceneNode * parent_node = nullptr); + ~BillboardLine() override; void clear(); void newLine(); @@ -78,12 +80,12 @@ class BillboardLine : public Object void setNumLines(uint32_t num); // overrides from Object - virtual void setOrientation(const Ogre::Quaternion & orientation); - virtual void setPosition(const Ogre::Vector3 & position); - virtual void setScale(const Ogre::Vector3 & scale); - virtual void setColor(float r, float g, float b, float a); - virtual const Ogre::Vector3 & getPosition(); - virtual const Ogre::Quaternion & getOrientation(); + void setOrientation(const Ogre::Quaternion & orientation) override; + void setPosition(const Ogre::Vector3 & position) override; + void setScale(const Ogre::Vector3 & scale) override; + void setColor(float r, float g, float b, float a) override; + const Ogre::Vector3 & getPosition() override; + const Ogre::Quaternion & getOrientation() override; /** * \brief Get the scene node associated with this object @@ -94,39 +96,42 @@ class BillboardLine : public Object /** * \brief We have no objects that we can set user data on */ - void setUserData(const Ogre::Any & data) {(void) data;} + void setUserData(const Ogre::Any & data) override {(void) data;} Ogre::MaterialPtr getMaterial() {return material_;} - typedef std::vector V_Chain; + typedef std::vector V_ChainContainers; /// exposed for testing - V_Chain getChains() {return chains_;} + V_ChainContainers getChains() {return chain_containers_;} private: - void setupChains(); + void setupChainContainers(); Ogre::BillboardChain * createChain(); + void changeAllElements( + std::function change_element); + void incrementChainContainerIfNecessary(); + void setupChainsInChainContainers() const; Ogre::SceneNode * scene_node_; - V_Chain chains_; + V_ChainContainers chain_containers_; Ogre::MaterialPtr material_; Ogre::ColourValue color_; float width_; - uint32_t current_line_; - - // Ogre 1.4 doesn't have getNumChainElements() + // TODO(Martin-Idel-SI): Replace by using Ogre::BillboardChain::getNumberOfChains once available + // Current issue: https://github.com/OGRECave/ogre/issues/603 typedef std::vector V_uint32; V_uint32 num_elements_; - uint32_t total_elements_; uint32_t num_lines_; uint32_t max_points_per_line_; - uint32_t lines_per_chain_; + uint32_t chains_per_container_; - uint32_t current_chain_; - uint32_t elements_in_current_chain_; + uint32_t current_line_; + uint32_t current_chain_container_; + uint32_t elements_in_current_chain_container_; }; } // namespace rviz_rendering From de0a3e1ae2b30bce03e0e1c01ac8208b968d678b Mon Sep 17 00:00:00 2001 From: Martin Idel Date: Mon, 18 Dec 2017 13:39:25 +0100 Subject: [PATCH 06/11] Add tests for grid - need to expose functions for testing --- rviz_rendering/CMakeLists.txt | 9 ++ .../include/rviz_rendering/grid.hpp | 6 + rviz_rendering/src/rviz_rendering/grid.cpp | 1 + rviz_rendering/src/test/grid_test.cpp | 149 ++++++++++++++++++ 4 files changed, 165 insertions(+) create mode 100644 rviz_rendering/src/test/grid_test.cpp diff --git a/rviz_rendering/CMakeLists.txt b/rviz_rendering/CMakeLists.txt index 63456c11d..b86755807 100644 --- a/rviz_rendering/CMakeLists.txt +++ b/rviz_rendering/CMakeLists.txt @@ -202,6 +202,15 @@ if(BUILD_TESTING) Qt5::Widgets # explicitly do this for include directories (not necessary for external use) ) endif() + + ament_add_gtest(grid_test_target src/test/grid_test.cpp) + if(TARGET grid_test_target) + target_link_libraries(grid_test_target + rviz_ogre_vendor::OgreMain + rviz_rendering + Qt5::Widgets # explicitly do this for include directories (not necessary for external use) + ) + endif() endif() list(APPEND ${PROJECT_NAME}_CONFIG_EXTRAS diff --git a/rviz_rendering/include/rviz_rendering/grid.hpp b/rviz_rendering/include/rviz_rendering/grid.hpp index c68a58efe..f5a7507a4 100644 --- a/rviz_rendering/include/rviz_rendering/grid.hpp +++ b/rviz_rendering/include/rviz_rendering/grid.hpp @@ -1,5 +1,6 @@ /* * Copyright (c) 2008, Willow Garage, Inc. + * Copyright (c) 2017, Bosch Software Innovations GmbH. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -70,6 +71,7 @@ class RVIZ_RENDERING_PUBLIC Grid Billboards, }; + // TODO(Martin-Idel-SI): Adapt description /** * \brief Constructor * @@ -117,6 +119,10 @@ class RVIZ_RENDERING_PUBLIC Grid void setHeight(uint32_t count); uint32_t getHeight() {return height_;} + /// Exposed for testing + Ogre::ManualObject * getManualObject() {return manual_object_;} + BillboardLine * getBillboardLine() {return billboard_line_;} + private: Ogre::SceneManager * scene_manager_; Ogre::SceneNode * scene_node_; ///< The scene node that this grid is attached to diff --git a/rviz_rendering/src/rviz_rendering/grid.cpp b/rviz_rendering/src/rviz_rendering/grid.cpp index b02f9603c..4d7c13a69 100644 --- a/rviz_rendering/src/rviz_rendering/grid.cpp +++ b/rviz_rendering/src/rviz_rendering/grid.cpp @@ -1,5 +1,6 @@ /* * Copyright (c) 2008, Willow Garage, Inc. + * Copyright (c) 2017, Bosch Software Innovations GmbH. * All rights reserved. * * Redistribution and use in source and binary forms, with or without diff --git a/rviz_rendering/src/test/grid_test.cpp b/rviz_rendering/src/test/grid_test.cpp new file mode 100644 index 000000000..04f0f7590 --- /dev/null +++ b/rviz_rendering/src/test/grid_test.cpp @@ -0,0 +1,149 @@ +/* + * Copyright (c) 2017, Bosch Software Innovations GmbH. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include + +#include +#include +#include + +#include +#include + +#include "ogre_testing_environment.hpp" +#include "rviz_rendering/grid.hpp" +#include "../src/rviz_rendering/billboard_line.hpp" + +class GridTestFixture : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + testing_environment_ = std::make_shared(); + testing_environment_->setUpOgreTestEnvironment(); + } + + static std::shared_ptr testing_environment_; +}; + +std::shared_ptr +GridTestFixture::testing_environment_ = nullptr; + +rviz_rendering::Grid createGrid( + rviz_rendering::Grid::Style style, + uint32_t cell_count, + float cell_length, + float line_width) +{ + Ogre::ColourValue colour_value = Ogre::ColourValue(); + return rviz_rendering::Grid( + Ogre::Root::getSingletonPtr()->createSceneManager(), + nullptr, style, cell_count, cell_length, line_width, colour_value); +} + +Ogre::AxisAlignedBox expectedBoxForBillboards( + uint32_t cell_count, uint32_t height_count, + float cell_length, float line_width) +{ + return Ogre::AxisAlignedBox( + Ogre::Vector3(-cell_length * cell_count / 2 - line_width, + -cell_length * height_count / 2 - line_width, + -cell_length * cell_count / 2 - line_width), + Ogre::Vector3(cell_length * cell_count / 2 + line_width, + cell_length * height_count / 2 + line_width, + cell_length * cell_count / 2 + line_width)); +} + +Ogre::AxisAlignedBox expectedBoxForLines( + uint32_t cell_count, uint32_t height_count, + float cell_length) +{ + return expectedBoxForBillboards(cell_count, height_count, cell_length, 0); +} + +TEST_F(GridTestFixture, createGrid_returns_object_with_bounding_box_bounding_cells) { + uint32_t cell_count = 2; + float cell_length = 2.0f; + + auto grid = createGrid(rviz_rendering::Grid::Style::Lines, cell_count, cell_length, 1.0f); + + grid.create(); + + auto manual_object = grid.getManualObject(); + auto expected_bounding_box = expectedBoxForLines(cell_count, 0, cell_length); + ASSERT_EQ(manual_object->getBoundingBox(), expected_bounding_box); +} + +TEST_F(GridTestFixture, setHeight_creates_object_with_correct_height_in_bounding_box) { + uint32_t cell_count = 3; + float cell_length = 1.5f; + uint32_t height_count = 4; + + auto grid = createGrid(rviz_rendering::Grid::Style::Lines, cell_count, cell_length, 1.0f); + + grid.setHeight(height_count); + + auto manual_object = grid.getManualObject(); + auto expected_bounding_box = expectedBoxForLines(cell_count, height_count, cell_length); + ASSERT_EQ(manual_object->getBoundingBox(), expected_bounding_box); +} + +TEST_F(GridTestFixture, + createGrid_returns_billboard_object_with_bounding_box_bounding_cells_including_billboard_size) { + uint32_t cell_count = 2; + float cell_length = 2.0f; + float line_width = 0.1f; + + auto grid = createGrid(rviz_rendering::Grid::Style::Billboards, + cell_count, cell_length, line_width); + + grid.create(); + + auto billboard_object = grid.getBillboardLine(); + auto expected_bounding_box = expectedBoxForBillboards(cell_count, 0, cell_length, line_width); + ASSERT_EQ(billboard_object->getChains()[0]->getBoundingBox(), expected_bounding_box); +} + +TEST_F(GridTestFixture, setStyle_creates_a_new_grid_with_new_style) { + uint32_t cell_count = 2; + float cell_length = 2.0f; + float line_width = 0.1f; + + auto grid = createGrid(rviz_rendering::Grid::Style::Lines, + cell_count, cell_length, line_width); + grid.create(); + + grid.setStyle(rviz_rendering::Grid::Style::Billboards); + + auto billboard_object = grid.getBillboardLine(); + auto expected_bounding_box = expectedBoxForBillboards(cell_count, 0, cell_length, line_width); + ASSERT_EQ(billboard_object->getChains()[0]->getBoundingBox(), expected_bounding_box); + auto manual_object = grid.getManualObject(); + ASSERT_EQ(manual_object->getBoundingBox(), Ogre::AxisAlignedBox()); +} From 092c2665f969539c5e84c9e4d1b7992e711c1396 Mon Sep 17 00:00:00 2001 From: Martin Idel Date: Mon, 18 Dec 2017 14:41:15 +0100 Subject: [PATCH 07/11] Refactoring of grid --- .../include/rviz_rendering/grid.hpp | 25 ++-- rviz_rendering/src/rviz_rendering/grid.cpp | 140 ++++++++++-------- 2 files changed, 93 insertions(+), 72 deletions(-) diff --git a/rviz_rendering/include/rviz_rendering/grid.hpp b/rviz_rendering/include/rviz_rendering/grid.hpp index f5a7507a4..c941f6c61 100644 --- a/rviz_rendering/include/rviz_rendering/grid.hpp +++ b/rviz_rendering/include/rviz_rendering/grid.hpp @@ -31,8 +31,8 @@ #ifndef RVIZ_RENDERING__GRID_HPP_ #define RVIZ_RENDERING__GRID_HPP_ -#include - +#include +#include #include #include @@ -71,16 +71,14 @@ class RVIZ_RENDERING_PUBLIC Grid Billboards, }; - // TODO(Martin-Idel-SI): Adapt description /** * \brief Constructor * * @param manager The scene manager this object is part of * @param cell_count The number of cells to draw * @param cell_length The size of each cell - * @param r Red color component, in the range [0, 1] - * @param g Green color component, in the range [0, 1] - * @param b Blue color component, in the range [0, 1] + * @param line_width The line width of the cells if it is rendered in Billboards style + * @param color The color of the lines */ Grid( Ogre::SceneManager * manager, Ogre::SceneNode * parent_node, Style style, @@ -117,18 +115,25 @@ class RVIZ_RENDERING_PUBLIC Grid float getLineWidth() {return line_width_;} void setHeight(uint32_t count); - uint32_t getHeight() {return height_;} + uint32_t getHeight() {return height_count_;} /// Exposed for testing Ogre::ManualObject * getManualObject() {return manual_object_;} - BillboardLine * getBillboardLine() {return billboard_line_;} + std::shared_ptr getBillboardLine() {return billboard_line_;} private: + void prepareRenderables() const; + void createGridPlane(float extent, uint32_t height) const; + void createVerticalLinesBetweenPlanes(float extent) const; + void addBillboardLine(const Ogre::Vector3 & p3, const Ogre::Vector3 & p4) const; + void addManualLine(const Ogre::Vector3 & p1, const Ogre::Vector3 & p2) const; + Ogre::SceneManager * scene_manager_; Ogre::SceneNode * scene_node_; ///< The scene node that this grid is attached to Ogre::ManualObject * manual_object_; ///< The manual object used to draw the grid - BillboardLine * billboard_line_; + // TODO(Martin-Idel-SI): If we didn't need to expose for testing, we could use unique_ptr + std::shared_ptr billboard_line_; Ogre::MaterialPtr material_; @@ -136,7 +141,7 @@ class RVIZ_RENDERING_PUBLIC Grid uint32_t cell_count_; float cell_length_; float line_width_; - uint32_t height_; + uint32_t height_count_; Ogre::ColourValue color_; }; diff --git a/rviz_rendering/src/rviz_rendering/grid.cpp b/rviz_rendering/src/rviz_rendering/grid.cpp index 4d7c13a69..886dac496 100644 --- a/rviz_rendering/src/rviz_rendering/grid.cpp +++ b/rviz_rendering/src/rviz_rendering/grid.cpp @@ -30,12 +30,12 @@ #include "rviz_rendering/grid.hpp" +#include #include #include #include #include -#include #include #include #include @@ -58,7 +58,7 @@ Grid::Grid( cell_count_(cell_count), cell_length_(cell_length), line_width_(line_width), - height_(0), + height_count_(0), color_(color) { static uint32_t gridCount = 0; @@ -74,7 +74,7 @@ Grid::Grid( scene_node_ = parent_node->createChildSceneNode(); scene_node_->attachObject(manual_object_); - billboard_line_ = new BillboardLine(scene_manager, scene_node_); + billboard_line_ = std::make_shared(scene_manager, scene_node_); ss << "Material"; material_ = Ogre::MaterialManager::getSingleton().create( @@ -87,8 +87,6 @@ Grid::Grid( Grid::~Grid() { - delete billboard_line_; - scene_manager_->destroySceneNode(scene_node_->getName()); scene_manager_->destroyManualObject(manual_object_); @@ -140,7 +138,7 @@ void Grid::setStyle(Style style) void Grid::setHeight(uint32_t height) { - height_ = height; + height_count_ = height; create(); } @@ -150,83 +148,101 @@ void Grid::create() manual_object_->clear(); billboard_line_->clear(); - float extent = (cell_length_ * static_cast(cell_count_)) / 2; + prepareRenderables(); + + float plane_extent = (cell_length_ * static_cast(cell_count_)) / 2; + + for (uint32_t current_height = 0; current_height <= height_count_; ++current_height) { + createGridPlane(plane_extent, current_height); + } + + if (height_count_ > 0) { + createVerticalLinesBetweenPlanes(plane_extent); + } + + if (style_ == Lines) { + manual_object_->end(); + } +} +void Grid::prepareRenderables() const +{ if (style_ == Billboards) { billboard_line_->setColor(color_.r, color_.g, color_.b, color_.a); billboard_line_->setLineWidth(line_width_); billboard_line_->setMaxPointsPerLine(2); - billboard_line_->setNumLines((cell_count_ + 1) * 2 * (height_ + 1) + - ((cell_count_ + 1) * (cell_count_ + 1)) * height_); + billboard_line_->setNumLines((cell_count_ + 1) * 2 * (height_count_ + 1) + + ((cell_count_ + 1) * (cell_count_ + 1)) * height_count_); } else { - manual_object_->estimateVertexCount(cell_count_ * 4 * (height_ + 1) + - ((cell_count_ + 1) * (cell_count_ + 1) * height_)); + manual_object_->estimateVertexCount(cell_count_ * 4 * (height_count_ + 1) + + ((cell_count_ + 1) * (cell_count_ + 1) * height_count_)); manual_object_->begin(material_->getName(), Ogre::RenderOperation::OT_LINE_LIST); } +} - for (uint32_t h = 0; h <= height_; ++h) { - float h_real = (height_ / 2.0f - static_cast(h)) * cell_length_; - for (uint32_t i = 0; i <= cell_count_; i++) { - float inc = extent - ( i * cell_length_); +void Grid::createGridPlane(float extent, uint32_t height) const +{ + float real_height = (height_count_ / 2.0f - static_cast(height)) * cell_length_; + for (uint32_t i = 0; i <= cell_count_; i++) { + float inc = extent - ( i * cell_length_); - Ogre::Vector3 p1(inc, h_real, -extent); - Ogre::Vector3 p2(inc, h_real, extent); - Ogre::Vector3 p3(-extent, h_real, inc); - Ogre::Vector3 p4(extent, h_real, inc); + Ogre::Vector3 p1(inc, real_height, -extent); + Ogre::Vector3 p2(inc, real_height, extent); + Ogre::Vector3 p3(-extent, real_height, inc); + Ogre::Vector3 p4(extent, real_height, inc); - if (style_ == Billboards) { - if (h != 0 || i != 0) { - billboard_line_->newLine(); - } + if (style_ == Billboards) { + if (height != 0 || i != 0) { + billboard_line_->newLine(); + } - billboard_line_->addPoint(p1); - billboard_line_->addPoint(p2); + billboard_line_->addPoint(p1); + billboard_line_->addPoint(p2); - billboard_line_->newLine(); + addBillboardLine(p3, p4); - billboard_line_->addPoint(p3); - billboard_line_->addPoint(p4); - } else { - manual_object_->position(p1); - manual_object_->colour(color_); - manual_object_->position(p2); - manual_object_->colour(color_); - - manual_object_->position(p3); - manual_object_->colour(color_); - manual_object_->position(p4); - manual_object_->colour(color_); - } + } else { + addManualLine(p1, p2); + addManualLine(p3, p4); } } +} + +void Grid::createVerticalLinesBetweenPlanes(float extent) const +{ + for (uint32_t x = 0; x <= cell_count_; ++x) { + for (uint32_t z = 0; z <= cell_count_; ++z) { + float x_real = extent - x * cell_length_; + float z_real = extent - z * cell_length_; - if (height_ > 0) { - for (uint32_t x = 0; x <= cell_count_; ++x) { - for (uint32_t z = 0; z <= cell_count_; ++z) { - float x_real = extent - x * cell_length_; - float z_real = extent - z * cell_length_; - - float y_top = (height_ / 2.0f) * cell_length_; - float y_bottom = -y_top; - - if (style_ == Billboards) { - billboard_line_->newLine(); - - billboard_line_->addPoint(Ogre::Vector3(x_real, y_bottom, z_real)); - billboard_line_->addPoint(Ogre::Vector3(x_real, y_top, z_real)); - } else { - manual_object_->position(x_real, y_bottom, z_real); - manual_object_->colour(color_); - manual_object_->position(x_real, y_top, z_real); - manual_object_->colour(color_); - } + float y_top = (height_count_ / 2.0f) * cell_length_; + float y_bottom = -y_top; + + Ogre::Vector3 p1(x_real, y_bottom, z_real); + Ogre::Vector3 p2(x_real, y_top, z_real); + + if (style_ == Billboards) { + addBillboardLine(p1, p2); + } else { + addManualLine(p1, p2); } } } +} - if (style_ == Lines) { - manual_object_->end(); - } +void Grid::addManualLine(const Ogre::Vector3 & p1, const Ogre::Vector3 & p2) const +{ + manual_object_->position(p1); + manual_object_->colour(color_); + manual_object_->position(p2); + manual_object_->colour(color_); +} + +void Grid::addBillboardLine(const Ogre::Vector3 & p1, const Ogre::Vector3 & p2) const +{ + billboard_line_->newLine(); + billboard_line_->addPoint(p1); + billboard_line_->addPoint(p2); } void Grid::setUserData(const Ogre::Any & data) From 74fd1a10badc6e5a92ec6bced0ac9e91841803f0 Mon Sep 17 00:00:00 2001 From: Martin Idel Date: Mon, 18 Dec 2017 15:41:13 +0100 Subject: [PATCH 08/11] Propose additional newLine() possible in BillboardLine for ease of use - this gets rid of awkward "if(first element) don't add a new line" - simplifies usage - allows further refactoring of grid (for instance) --- .../include/rviz_rendering/grid.hpp | 11 ++- .../src/rviz_rendering/billboard_line.cpp | 3 +- rviz_rendering/src/rviz_rendering/grid.cpp | 92 ++++++++++--------- 3 files changed, 57 insertions(+), 49 deletions(-) diff --git a/rviz_rendering/include/rviz_rendering/grid.hpp b/rviz_rendering/include/rviz_rendering/grid.hpp index c941f6c61..f33aaf24e 100644 --- a/rviz_rendering/include/rviz_rendering/grid.hpp +++ b/rviz_rendering/include/rviz_rendering/grid.hpp @@ -122,17 +122,20 @@ class RVIZ_RENDERING_PUBLIC Grid std::shared_ptr getBillboardLine() {return billboard_line_;} private: - void prepareRenderables() const; - void createGridPlane(float extent, uint32_t height) const; - void createVerticalLinesBetweenPlanes(float extent) const; + typedef std::function AddLineFunction; + void createBillboardGrid() const; + void createManualGrid() const; + void createLines(AddLineFunction addLine) const; + void createGridPlane(float extent, uint32_t height, AddLineFunction addLine) const; + void createVerticalLinesBetweenPlanes(float extent, AddLineFunction addLine) const; void addBillboardLine(const Ogre::Vector3 & p3, const Ogre::Vector3 & p4) const; void addManualLine(const Ogre::Vector3 & p1, const Ogre::Vector3 & p2) const; + uint32_t numberOfVerticalLines() const; Ogre::SceneManager * scene_manager_; Ogre::SceneNode * scene_node_; ///< The scene node that this grid is attached to Ogre::ManualObject * manual_object_; ///< The manual object used to draw the grid - // TODO(Martin-Idel-SI): If we didn't need to expose for testing, we could use unique_ptr std::shared_ptr billboard_line_; Ogre::MaterialPtr material_; diff --git a/rviz_rendering/src/rviz_rendering/billboard_line.cpp b/rviz_rendering/src/rviz_rendering/billboard_line.cpp index 1a6da769e..9d967d7b3 100644 --- a/rviz_rendering/src/rviz_rendering/billboard_line.cpp +++ b/rviz_rendering/src/rviz_rendering/billboard_line.cpp @@ -177,7 +177,7 @@ void BillboardLine::newLine() { ++current_line_; - assert(current_line_ < num_lines_); + assert(current_line_ <= num_lines_); } void BillboardLine::addPoint(const Ogre::Vector3 & point) @@ -189,6 +189,7 @@ void BillboardLine::addPoint(const Ogre::Vector3 & point, const Ogre::ColourValu { ++num_elements_[current_line_]; + assert(current_line_ < num_lines_); assert(num_elements_[current_line_] <= max_points_per_line_); incrementChainContainerIfNecessary(); diff --git a/rviz_rendering/src/rviz_rendering/grid.cpp b/rviz_rendering/src/rviz_rendering/grid.cpp index 886dac496..d9c79b575 100644 --- a/rviz_rendering/src/rviz_rendering/grid.cpp +++ b/rviz_rendering/src/rviz_rendering/grid.cpp @@ -148,39 +148,59 @@ void Grid::create() manual_object_->clear(); billboard_line_->clear(); - prepareRenderables(); + if (style_ == Billboards) { + createBillboardGrid(); + } else { + createManualGrid(); + } +} - float plane_extent = (cell_length_ * static_cast(cell_count_)) / 2; +void Grid::createManualGrid() const +{ + AddLineFunction addLineFunction = bind( + &Grid::addManualLine, this, std::placeholders::_1, std::placeholders::_2); - for (uint32_t current_height = 0; current_height <= height_count_; ++current_height) { - createGridPlane(plane_extent, current_height); - } + const uint32_t number_of_vertices_in_plane = cell_count_ * 4 * (height_count_ + 1); + manual_object_->estimateVertexCount(number_of_vertices_in_plane + numberOfVerticalLines()); - if (height_count_ > 0) { - createVerticalLinesBetweenPlanes(plane_extent); - } + manual_object_->begin(material_->getName(), Ogre::RenderOperation::OT_LINE_LIST); + createLines(addLineFunction); + manual_object_->end(); +} - if (style_ == Lines) { - manual_object_->end(); - } +void Grid::createBillboardGrid() const +{ + AddLineFunction addLineFunction = bind( + &Grid::addBillboardLine, this, std::placeholders::_1, std::placeholders::_2); + + billboard_line_->setColor(color_.r, color_.g, color_.b, color_.a); + billboard_line_->setLineWidth(line_width_); + billboard_line_->setMaxPointsPerLine(2); + const uint32_t number_of_lines_in_plane = (cell_count_ + 1) * 2 * (height_count_ + 1); + billboard_line_->setNumLines(number_of_lines_in_plane + numberOfVerticalLines()); + + createLines(addLineFunction); } -void Grid::prepareRenderables() const +uint32_t Grid::numberOfVerticalLines() const { - if (style_ == Billboards) { - billboard_line_->setColor(color_.r, color_.g, color_.b, color_.a); - billboard_line_->setLineWidth(line_width_); - billboard_line_->setMaxPointsPerLine(2); - billboard_line_->setNumLines((cell_count_ + 1) * 2 * (height_count_ + 1) + - ((cell_count_ + 1) * (cell_count_ + 1)) * height_count_); - } else { - manual_object_->estimateVertexCount(cell_count_ * 4 * (height_count_ + 1) + - ((cell_count_ + 1) * (cell_count_ + 1) * height_count_)); - manual_object_->begin(material_->getName(), Ogre::RenderOperation::OT_LINE_LIST); + return (cell_count_ + 1) * (cell_count_ + 1) * height_count_; +} + +void Grid::createLines(AddLineFunction addLine) const +{ + float plane_extent = (cell_length_ * static_cast(cell_count_)) / 2; + + for (uint32_t current_height = 0; current_height <= height_count_; ++current_height) { + createGridPlane(plane_extent, current_height, addLine); + } + + if (height_count_ > 0) { + createVerticalLinesBetweenPlanes(plane_extent, addLine); } } -void Grid::createGridPlane(float extent, uint32_t height) const +void Grid::createGridPlane(float extent, uint32_t height, AddLineFunction addLine) const { float real_height = (height_count_ / 2.0f - static_cast(height)) * cell_length_; for (uint32_t i = 0; i <= cell_count_; i++) { @@ -191,24 +211,12 @@ void Grid::createGridPlane(float extent, uint32_t height) const Ogre::Vector3 p3(-extent, real_height, inc); Ogre::Vector3 p4(extent, real_height, inc); - if (style_ == Billboards) { - if (height != 0 || i != 0) { - billboard_line_->newLine(); - } - - billboard_line_->addPoint(p1); - billboard_line_->addPoint(p2); - - addBillboardLine(p3, p4); - - } else { - addManualLine(p1, p2); - addManualLine(p3, p4); - } + addLine(p1, p2); + addLine(p3, p4); } } -void Grid::createVerticalLinesBetweenPlanes(float extent) const +void Grid::createVerticalLinesBetweenPlanes(float extent, AddLineFunction addLine) const { for (uint32_t x = 0; x <= cell_count_; ++x) { for (uint32_t z = 0; z <= cell_count_; ++z) { @@ -221,11 +229,7 @@ void Grid::createVerticalLinesBetweenPlanes(float extent) const Ogre::Vector3 p1(x_real, y_bottom, z_real); Ogre::Vector3 p2(x_real, y_top, z_real); - if (style_ == Billboards) { - addBillboardLine(p1, p2); - } else { - addManualLine(p1, p2); - } + addLine(p1, p2); } } } @@ -240,9 +244,9 @@ void Grid::addManualLine(const Ogre::Vector3 & p1, const Ogre::Vector3 & p2) con void Grid::addBillboardLine(const Ogre::Vector3 & p1, const Ogre::Vector3 & p2) const { - billboard_line_->newLine(); billboard_line_->addPoint(p1); billboard_line_->addPoint(p2); + billboard_line_->newLine(); } void Grid::setUserData(const Ogre::Any & data) From 36d61c555f33694fef8dc701b82ae43235c27b01 Mon Sep 17 00:00:00 2001 From: Martin Idel Date: Mon, 18 Dec 2017 21:53:17 +0100 Subject: [PATCH 09/11] Minor refactoring of GridDisplay --- .../displays/grid/grid_display.cpp | 29 ++++++++----------- .../displays/grid/grid_display.hpp | 10 ++++--- 2 files changed, 18 insertions(+), 21 deletions(-) diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/grid/grid_display.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/grid/grid_display.cpp index 66aae765f..9c27f8e9b 100644 --- a/rviz_default_plugins/src/rviz_default_plugins/displays/grid/grid_display.cpp +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/grid/grid_display.cpp @@ -31,6 +31,7 @@ #include "./grid_display.hpp" #include +#include #include #ifndef _WIN32 @@ -68,7 +69,6 @@ using rviz_common::properties::VectorProperty; using rviz_rendering::Grid; GridDisplay::GridDisplay() -: Display() { frame_property_ = new TfFrameProperty("Reference Frame", TfFrameProperty::FIXED_FRAME_STRING, "The TF frame this grid will use for its origin.", @@ -123,20 +123,15 @@ GridDisplay::GridDisplay() this, SLOT(updateOffset())); } -GridDisplay::~GridDisplay() -{ - if (initialized() ) { - delete grid_; - } -} +GridDisplay::~GridDisplay() = default; void GridDisplay::onInitialize() { QColor color = color_property_->getColor(); color.setAlphaF(alpha_property_->getFloat() ); - frame_property_->setFrameManager(context_->getFrameManager() ); - grid_ = new Grid(scene_manager_, scene_node_, + frame_property_->setFrameManager(context_->getFrameManager()); + grid_ = std::make_unique(scene_manager_, scene_node_, (Grid::Style) style_property_->getOptionInt(), cell_count_property_->getInt(), cell_size_property_->getFloat(), @@ -174,38 +169,38 @@ void GridDisplay::update(float dt, float ros_dt) void GridDisplay::updateColor() { QColor color = color_property_->getColor(); - color.setAlphaF(alpha_property_->getFloat() ); + color.setAlphaF(alpha_property_->getFloat()); grid_->setColor(qtToOgre(color)); context_->queueRender(); } void GridDisplay::updateCellSize() { - grid_->setCellLength(cell_size_property_->getFloat() ); + grid_->setCellLength(cell_size_property_->getFloat()); context_->queueRender(); } void GridDisplay::updateCellCount() { - grid_->setCellCount(cell_count_property_->getInt() ); + grid_->setCellCount(cell_count_property_->getInt()); context_->queueRender(); } void GridDisplay::updateLineWidth() { - grid_->setLineWidth(line_width_property_->getFloat() ); + grid_->setLineWidth(line_width_property_->getFloat()); context_->queueRender(); } void GridDisplay::updateHeight() { - grid_->setHeight(height_property_->getInt() ); + grid_->setHeight(height_property_->getInt()); context_->queueRender(); } void GridDisplay::updateStyle() { - Grid::Style style = (Grid::Style) style_property_->getOptionInt(); + auto style = static_cast(style_property_->getOptionInt()); grid_->setStyle(style); switch (style) { @@ -222,14 +217,14 @@ void GridDisplay::updateStyle() void GridDisplay::updateOffset() { - grid_->getSceneNode()->setPosition(offset_property_->getVector() ); + grid_->getSceneNode()->setPosition(offset_property_->getVector()); context_->queueRender(); } void GridDisplay::updatePlane() { Ogre::Quaternion orient; - switch ( (Plane) plane_property_->getOptionInt() ) { + switch ( (Plane) plane_property_->getOptionInt()) { case XZ: orient = Ogre::Quaternion(1, 0, 0, 0); break; diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/grid/grid_display.hpp b/rviz_default_plugins/src/rviz_default_plugins/displays/grid/grid_display.hpp index 866963056..98c875244 100644 --- a/rviz_default_plugins/src/rviz_default_plugins/displays/grid/grid_display.hpp +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/grid/grid_display.hpp @@ -31,6 +31,8 @@ #ifndef RVIZ_DEFAULT_PLUGINS__DISPLAYS__GRID__GRID_DISPLAY_HPP_ #define RVIZ_DEFAULT_PLUGINS__DISPLAYS__GRID__GRID_DISPLAY_HPP_ +#include + #include "rviz_common/display.hpp" #include "rviz_common/properties/color_property.hpp" #include "rviz_common/properties/enum_property.hpp" @@ -70,11 +72,11 @@ class GridDisplay : public rviz_common::Display }; GridDisplay(); - virtual ~GridDisplay(); + ~GridDisplay() override; // Overrides from Display - virtual void onInitialize(); - virtual void update(float dt, float ros_dt); + void onInitialize() override; + void update(float dt, float ros_dt) override; private Q_SLOTS: void updateCellCount(); @@ -87,7 +89,7 @@ private Q_SLOTS: void updateStyle(); private: - rviz_rendering::Grid * grid_; ///< Handles actually drawing the grid + std::unique_ptr grid_; ///< Handles actually drawing the grid rviz_common::properties::TfFrameProperty * frame_property_; rviz_common::properties::IntProperty * cell_count_property_; From 787c461a60a1697c7ff8ba4b521ada0774563ef2 Mon Sep 17 00:00:00 2001 From: Martin Idel Date: Tue, 19 Dec 2017 17:45:19 +0100 Subject: [PATCH 10/11] Disable tests for now --- rviz_rendering/CMakeLists.txt | 52 ++++++++++++++++++----------------- 1 file changed, 27 insertions(+), 25 deletions(-) diff --git a/rviz_rendering/CMakeLists.txt b/rviz_rendering/CMakeLists.txt index b86755807..3a8b31f2f 100644 --- a/rviz_rendering/CMakeLists.txt +++ b/rviz_rendering/CMakeLists.txt @@ -185,32 +185,34 @@ if(BUILD_TESTING) # endif() # TODO(wjwwood): reenable this test when it can be run on CI -# ament_add_gtest(point_cloud_renderable_test_target src/test/point_cloud_renderable_test.cpp) -# if(TARGET point_cloud_renderable_test_target) -# target_link_libraries(point_cloud_renderable_test_target -# rviz_ogre_vendor::OgreMain -# rviz_rendering -# Qt5::Widgets # explicitly do this for include directories (not necessary for external use) -# ) -# endif() - - ament_add_gtest(billboard_line_test_target src/test/billboard_line_test.cpp) - if(TARGET billboard_line_test_target) - target_link_libraries(billboard_line_test_target - rviz_ogre_vendor::OgreMain - rviz_rendering - Qt5::Widgets # explicitly do this for include directories (not necessary for external use) - ) - endif() + # ament_add_gtest(point_cloud_renderable_test_target src/test/point_cloud_renderable_test.cpp) + # if(TARGET point_cloud_renderable_test_target) + # target_link_libraries(point_cloud_renderable_test_target + # rviz_ogre_vendor::OgreMain + # rviz_rendering + # Qt5::Widgets # explicitly do this for include directories (not necessary for external use) + # ) + # endif() - ament_add_gtest(grid_test_target src/test/grid_test.cpp) - if(TARGET grid_test_target) - target_link_libraries(grid_test_target - rviz_ogre_vendor::OgreMain - rviz_rendering - Qt5::Widgets # explicitly do this for include directories (not necessary for external use) - ) - endif() + # TODO(Martin-Idel-SI): reenable this test when it can be run on CI + # ament_add_gtest(billboard_line_test_target src/test/billboard_line_test.cpp) + # if(TARGET billboard_line_test_target) + # target_link_libraries(billboard_line_test_target + # rviz_ogre_vendor::OgreMain + # rviz_rendering + # Qt5::Widgets # explicitly do this for include directories (not necessary for external use) + # ) + # endif() + + # TODO(Martin-Idel-SI): reenable this test when it can be run on CI + # ament_add_gtest(grid_test_target src/test/grid_test.cpp) + # if(TARGET grid_test_target) + # target_link_libraries(grid_test_target + # rviz_ogre_vendor::OgreMain + # rviz_rendering + # Qt5::Widgets # explicitly do this for include directories (not necessary for external use) + # ) + # endif() endif() list(APPEND ${PROJECT_NAME}_CONFIG_EXTRAS From 5d5ec35b9e3e59e9f38d67d00bab5e3c1322dba1 Mon Sep 17 00:00:00 2001 From: Martin Idel Date: Wed, 20 Dec 2017 14:45:01 +0100 Subject: [PATCH 11/11] Add functional header and std:: for "bind" --- rviz_rendering/src/rviz_rendering/grid.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/rviz_rendering/src/rviz_rendering/grid.cpp b/rviz_rendering/src/rviz_rendering/grid.cpp index d9c79b575..4eca53ae9 100644 --- a/rviz_rendering/src/rviz_rendering/grid.cpp +++ b/rviz_rendering/src/rviz_rendering/grid.cpp @@ -30,6 +30,7 @@ #include "rviz_rendering/grid.hpp" +#include #include #include @@ -157,7 +158,7 @@ void Grid::create() void Grid::createManualGrid() const { - AddLineFunction addLineFunction = bind( + AddLineFunction addLineFunction = std::bind( &Grid::addManualLine, this, std::placeholders::_1, std::placeholders::_2); const uint32_t number_of_vertices_in_plane = cell_count_ * 4 * (height_count_ + 1); @@ -170,7 +171,7 @@ void Grid::createManualGrid() const void Grid::createBillboardGrid() const { - AddLineFunction addLineFunction = bind( + AddLineFunction addLineFunction = std::bind( &Grid::addBillboardLine, this, std::placeholders::_1, std::placeholders::_2); billboard_line_->setColor(color_.r, color_.g, color_.b, color_.a);