├── .clang-format ├── CMakeLists.txt ├── LICENSE ├── README.md ├── docs ├── 2d_arrow.png └── 3d_arrow.png ├── icons └── classes │ └── PointCloud2Normal.png ├── package.xml ├── plugin_description.xml └── src ├── arrow.cpp ├── arrow.h ├── pix_arrow.cpp ├── pix_arrow.h ├── point_cloud2_normal_display.cpp ├── point_cloud2_normal_display.h ├── point_cloud_normal.cpp ├── point_cloud_normal.h ├── shape.cpp └── shape.h /.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | Language: Cpp 3 | BasedOnStyle: Google 4 | ColumnLimit: 120 5 | ContinuationIndentWidth: 4 6 | DerivePointerAlignment: false 7 | IndentWidth: 2 8 | MaxEmptyLinesToKeep: 1 9 | PointerAlignment: Left 10 | SortIncludes: false 11 | TabWidth: 2 12 | UseTab: Never 13 | ... 14 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(pointcloud2_normal_rviz_plugin) 3 | find_package(catkin REQUIRED COMPONENTS rviz) 4 | catkin_package() 5 | include_directories(${catkin_INCLUDE_DIRS}) 6 | link_directories(${catkin_LIBRARY_DIRS}) 7 | 8 | set(CMAKE_AUTOMOC ON) 9 | 10 | if(rviz_QT_VERSION VERSION_LESS "5") 11 | message(STATUS "Using Qt4 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}") 12 | find_package(Qt4 ${rviz_QT_VERSION} EXACT REQUIRED QtCore QtGui) 13 | include(${QT_USE_FILE}) 14 | else() 15 | message(STATUS "Using Qt5 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}") 16 | find_package(Qt5 ${rviz_QT_VERSION} EXACT REQUIRED Core Widgets) 17 | set(QT_LIBRARIES Qt5::Widgets) 18 | endif() 19 | 20 | ## I prefer the Qt signals and slots to avoid defining "emit", "slots", 21 | ## etc because they can conflict with boost signals, so define QT_NO_KEYWORDS here. 22 | add_definitions(-DQT_NO_KEYWORDS) 23 | 24 | set(SRC_FILES 25 | src/point_cloud2_normal_display.h 26 | src/point_cloud2_normal_display.cpp 27 | src/point_cloud_normal.h 28 | src/point_cloud_normal.cpp 29 | src/pix_arrow.h 30 | src/pix_arrow.cpp 31 | src/arrow.h 32 | src/arrow.cpp 33 | src/shape.h 34 | src/shape.cpp 35 | ) 36 | 37 | ## An rviz plugin is just a shared library, so here we declare the 38 | ## library to be called ``${PROJECT_NAME}`` (which is 39 | ## "rviz_plugin_tutorials", or whatever your version of this project 40 | ## is called) and specify the list of source files we collected above 41 | ## in ``${SRC_FILES}``. 42 | add_library(${PROJECT_NAME} ${SRC_FILES}) 43 | 44 | ## Link the myviz executable with whatever Qt libraries have been defined by 45 | ## the ``find_package(Qt4 ...)`` line above, or by the 46 | ## ``set(QT_LIBRARIES Qt5::Widgets)``, and with whatever libraries 47 | ## catkin has included. 48 | ## 49 | ## Although this puts "rviz_plugin_tutorials" (or whatever you have 50 | ## called the project) as the name of the library, cmake knows it is a 51 | ## library and names the actual file something like 52 | ## "librviz_plugin_tutorials.so", or whatever is appropriate for your 53 | ## particular OS. 54 | target_link_libraries(${PROJECT_NAME} ${QT_LIBRARIES} ${catkin_LIBRARIES}) 55 | ## END_TUTORIAL 56 | 57 | ## Install rules 58 | 59 | install(TARGETS 60 | ${PROJECT_NAME} 61 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 62 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 63 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 64 | ) 65 | 66 | install(FILES 67 | plugin_description.xml 68 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 69 | 70 | install(DIRECTORY icons/ 71 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/icons) 72 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2023, Autonomous Robots and Control Systems (ARCS) Lab 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 11 | 2. Redistributions in binary form must reproduce the above copyright notice, 12 | this list of conditions and the following disclaimer in the documentation 13 | and/or other materials provided with the distribution. 14 | 15 | 3. Neither the name of the copyright holder nor the names of its 16 | contributors may be used to endorse or promote products derived from 17 | this software without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 23 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 24 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 25 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 26 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 27 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 28 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # PointCloud2 Normal Rviz Plugin 2 | RViz plugin to display normal vectors of points in a point cloud, if available. 3 | 4 | ## Installation 5 | Just place this ROS package under a ROS workspace, and run `catkin_make install`. 6 | 7 | ## Usage 8 | The default RViz plugin can display points in a point cloud of the type sensor_msgs::PointCloud2. 9 | This plugin can be used along with the default plugin to further display the normal vectors in 3D space. 10 | 11 | It supports two styles of visualization for arrows (3D Arrow and 2D Arrow), as well as some common configurations 12 | (color, size, etc.) 13 | 14 | ![2D Arrow](docs/2d_arrow.png) 15 | -------------------------------------------------------------------------------- /docs/2d_arrow.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UCR-Robotics/pointcloud2_normal_rviz_plugin/ddfc86ec002d2f0371bd80f320c0f1877f20f7b7/docs/2d_arrow.png -------------------------------------------------------------------------------- /docs/3d_arrow.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UCR-Robotics/pointcloud2_normal_rviz_plugin/ddfc86ec002d2f0371bd80f320c0f1877f20f7b7/docs/3d_arrow.png -------------------------------------------------------------------------------- /icons/classes/PointCloud2Normal.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UCR-Robotics/pointcloud2_normal_rviz_plugin/ddfc86ec002d2f0371bd80f320c0f1877f20f7b7/icons/classes/PointCloud2Normal.png -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | pointcloud2_normal_rviz_plugin 3 | 1.0.0 4 | RViz plugin to display normal vectors of points in a point cloud, if available. 5 | Yipeng Wang 6 | Hanzhe Teng 7 | BSD 8 | 9 | catkin 10 | 11 | qtbase5-dev 12 | rviz 13 | 14 | libqt5-core 15 | libqt5-gui 16 | libqt5-widgets 17 | rviz 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /plugin_description.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | Display normal vectors of points in a point cloud (sensor_msgs::PointCloud2), if available. 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /src/arrow.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2008, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #include "arrow.h" 31 | #include "shape.h" 32 | 33 | #include 34 | #include 35 | #include 36 | #include 37 | 38 | #include 39 | 40 | namespace rviz 41 | { 42 | Arrow2::Arrow2(Ogre::SceneManager* scene_manager, 43 | Ogre::SceneNode* parent_node, 44 | float shaft_length, 45 | float shaft_diameter, 46 | float head_length, 47 | float head_diameter) 48 | : Object(scene_manager) 49 | { 50 | if (!parent_node) 51 | { 52 | parent_node = scene_manager_->getRootSceneNode(); 53 | } 54 | 55 | scene_node_ = parent_node->createChildSceneNode(); 56 | 57 | shaft_ = new Shape(Shape::Cylinder, scene_manager_, scene_node_); 58 | head_ = new Shape(Shape::Cone, scene_manager_, scene_node_); 59 | head_->setOffset(Ogre::Vector3(0.0f, 0.5f, 0.0f)); 60 | 61 | set(shaft_length, shaft_diameter, head_length, head_diameter); 62 | 63 | Arrow2::setOrientation(Ogre::Quaternion::IDENTITY); 64 | } 65 | 66 | Arrow2::~Arrow2() 67 | { 68 | delete shaft_; 69 | delete head_; 70 | 71 | scene_manager_->destroySceneNode(scene_node_); 72 | } 73 | 74 | void Arrow2::set(float shaft_length, float shaft_diameter, float head_length, float head_diameter) 75 | { 76 | shaft_->setScale(Ogre::Vector3(shaft_diameter, shaft_length, shaft_diameter)); 77 | shaft_->setPosition(Ogre::Vector3(0.0f, shaft_length / 2.0f, 0.0f)); 78 | 79 | head_->setScale(Ogre::Vector3(head_diameter, head_length, head_diameter)); 80 | head_->setPosition(Ogre::Vector3(0.0f, shaft_length, 0.0f)); 81 | } 82 | 83 | void Arrow2::setColor(const Ogre::ColourValue& c) 84 | { 85 | setShaftColor(c); 86 | setHeadColor(c); 87 | } 88 | 89 | void Arrow2::setColor(float r, float g, float b, float a) 90 | { 91 | setColor(Ogre::ColourValue(r, g, b, a)); 92 | } 93 | 94 | void Arrow2::setShaftColor(const Ogre::ColourValue& c) 95 | { 96 | shaft_->setColor(c); 97 | } 98 | 99 | void Arrow2::setHeadColor(const Ogre::ColourValue& c) 100 | { 101 | head_->setColor(c); 102 | } 103 | 104 | void Arrow2::setShaftColor(float r, float g, float b, float a) 105 | { 106 | setShaftColor(Ogre::ColourValue(r, g, b, a)); 107 | } 108 | 109 | void Arrow2::setHeadColor(float r, float g, float b, float a) 110 | { 111 | setHeadColor(Ogre::ColourValue(r, g, b, a)); 112 | } 113 | 114 | void Arrow2::setPosition(const Ogre::Vector3& position) 115 | { 116 | scene_node_->setPosition(position); 117 | } 118 | 119 | void Arrow2::setOrientation(const Ogre::Quaternion& orientation) 120 | { 121 | // "forward" (negative z) should always be our identity orientation 122 | // ... wouldn't need to mangle the orientation if we just fix the cylinders! 123 | scene_node_->setOrientation(orientation); 124 | } 125 | 126 | void Arrow2::setDirection(const Ogre::Vector3& direction) 127 | { 128 | if (!direction.isZeroLength()) 129 | { 130 | // setOrientation(Ogre::Vector3::NEGATIVE_UNIT_Z.getRotationTo(direction) * Ogre::Quaternion(Ogre::Degree(-90), Ogre::Vector3::UNIT_X)); 131 | setOrientation(Ogre::Vector3::UNIT_Y.getRotationTo(direction)); 132 | } 133 | } 134 | 135 | void Arrow2::setScale(const Ogre::Vector3& scale) 136 | { 137 | // Have to mangle the scale because of the default orientation of the cylinders :( 138 | scene_node_->setScale(Ogre::Vector3(scale.z, scale.x, scale.y)); 139 | } 140 | 141 | const Ogre::Vector3& Arrow2::getPosition() 142 | { 143 | return scene_node_->getPosition(); 144 | } 145 | 146 | const Ogre::Quaternion& Arrow2::getOrientation() 147 | { 148 | return scene_node_->getOrientation(); 149 | } 150 | 151 | void Arrow2::setUserData(const Ogre::Any& data) 152 | { 153 | head_->setUserData(data); 154 | shaft_->setUserData(data); 155 | } 156 | 157 | } // namespace rviz -------------------------------------------------------------------------------- /src/arrow.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2008, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #include 31 | 32 | #ifndef OGRE_TOOLS_ARROW_H 33 | #define OGRE_TOOLS_ARROW_H 34 | 35 | #include 36 | 37 | namespace Ogre 38 | { 39 | class Any; 40 | } // namespace Ogre 41 | 42 | 43 | namespace rviz 44 | { 45 | class Shape; 46 | 47 | /** 48 | * \class Arrow 49 | * \brief An arrow consisting of a cylinder and a cone. 50 | * 51 | * The base of the arrow is at the position sent to setPosition(). 52 | * The arrow points in the direction of the negative Z axis by 53 | * default, and -Z is the identity direction of it. To set a 54 | * different direction, call setOrientation() with a rotation from -Z 55 | * to the desired vector. 56 | */ 57 | class Arrow2 : public Object 58 | { 59 | public: 60 | /** 61 | * \brief Constructor 62 | * 63 | * @param scene_manager The scene manager to use to construct any necessary objects 64 | * @param parent_node A scene node to use as the parent of this object. If NULL, uses the root scene 65 | * node. 66 | * @param shaft_length Length of the arrow's shaft 67 | * @param shaft_diameter Diameter of the arrow's shaft 68 | * @param head_length Length of the arrow's head 69 | * @param head_diameter Diameter of the arrow's head 70 | */ 71 | Arrow2(Ogre::SceneManager* scene_manager, 72 | Ogre::SceneNode* parent_node = nullptr, 73 | float shaft_length = 1.0f, 74 | float shaft_diameter = 0.1f, 75 | float head_length = 0.3f, 76 | float head_diameter = 0.2f); 77 | ~Arrow2() override; 78 | 79 | /** 80 | * \brief Set the parameters for this arrow 81 | * 82 | * @param shaft_length Length of the arrow's shaft 83 | * @param shaft_diameter Diameter of the arrow's shaft 84 | * @param head_length Length of the arrow's head 85 | * @param head_diameter Diameter of the arrow's head 86 | */ 87 | void set(float shaft_length, float shaft_diameter, float head_length, float head_diameter); 88 | 89 | /** 90 | * \brief Set the color of this arrow. Sets both the head and shaft color to the same value. Values 91 | * are in the range [0, 1] 92 | * 93 | * @param r Red component 94 | * @param g Green component 95 | * @param b Blue component 96 | */ 97 | void setColor(float r, float g, float b, float a) override; 98 | void setColor(const Ogre::ColourValue& color); 99 | 100 | /** 101 | * \brief Set the color of the arrow's head. Values are in the range [0, 1] 102 | * 103 | * @param r Red component 104 | * @param g Green component 105 | * @param b Blue component 106 | */ 107 | void setHeadColor(float r, float g, float b, float a = 1.0f); 108 | void setHeadColor(const Ogre::ColourValue& color); 109 | /** 110 | * \brief Set the color of the arrow's shaft. Values are in the range [0, 1] 111 | * 112 | * @param r Red component 113 | * @param g Green component 114 | * @param b Blue component 115 | */ 116 | void setShaftColor(float r, float g, float b, float a = 1.0f); 117 | void setShaftColor(const Ogre::ColourValue& color); 118 | 119 | /** @brief Set the orientation. 120 | * 121 | * Note that negative Z is the identity orientation. 122 | * 123 | * Both setOrientation() and setDirection() change the direction the arrow points. */ 124 | void setOrientation(const Ogre::Quaternion& orientation) override; 125 | 126 | /** @brief Set the position of the base of the arrow */ 127 | void setPosition(const Ogre::Vector3& position) override; 128 | 129 | /** @brief Set the direction of the arrow. 130 | * 131 | * @param direction The direction the arrow should point, in the 132 | * coordinate frame of the parent Ogre::SceneNode. 133 | * 134 | * If direction is zero, this does not change the arrow. 135 | * 136 | * Both setOrientation() and setDirection() change the direction the arrow points. */ 137 | void setDirection(const Ogre::Vector3& direction); 138 | 139 | void setScale(const Ogre::Vector3& scale) override; 140 | const Ogre::Vector3& getPosition() override; 141 | const Ogre::Quaternion& getOrientation() override; 142 | 143 | /** 144 | * \brief Get the scene node associated with this arrow 145 | * @return the scene node associated with this arrow 146 | */ 147 | Ogre::SceneNode* getSceneNode() 148 | { 149 | return scene_node_; 150 | } 151 | 152 | /** 153 | * \brief Sets user data on all ogre objects we own 154 | */ 155 | void setUserData(const Ogre::Any& data) override; 156 | 157 | Shape* getShaft() 158 | { 159 | return shaft_; 160 | } 161 | Shape* getHead() 162 | { 163 | return head_; 164 | } 165 | 166 | private: 167 | Ogre::SceneNode* scene_node_; 168 | 169 | Shape* shaft_; ///< Cylinder used for the shaft of the arrow 170 | Shape* head_; ///< Cone used for the head of the arrow 171 | }; 172 | 173 | } // namespace rviz 174 | 175 | #endif /* OGRE_TOOLS_ARROW_H */ -------------------------------------------------------------------------------- /src/pix_arrow.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2023, ARCS Lab 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the ARCS Lab nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #include "pix_arrow.h" 31 | 32 | #include 33 | 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | 40 | namespace rviz { 41 | PixArrow::PixArrow(Ogre::SceneManager* manager, Ogre::SceneNode* parent_node) : Object(manager) { 42 | if (!parent_node) { 43 | parent_node = manager->getRootSceneNode(); 44 | } 45 | manual_object_ = manager->createManualObject(); 46 | scene_node_ = parent_node->createChildSceneNode(); 47 | 48 | static int count = 0; 49 | std::stringstream ss; 50 | ss << "PixArrowMaterial" << count++; 51 | 52 | // NOTE: The second parameter to the create method is the resource group the material will be added to. 53 | // If the group you name does not exist (in your resources.cfg file) the library will assert() and your 54 | // program will crash 55 | manual_object_material_ = 56 | Ogre::MaterialManager::getSingleton().create(ss.str(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); 57 | manual_object_material_->setReceiveShadows(false); 58 | manual_object_material_->getTechnique(0)->setLightingEnabled(true); 59 | manual_object_material_->getTechnique(0)->getPass(0)->setDiffuse(0, 0, 0, 0); 60 | manual_object_material_->getTechnique(0)->getPass(0)->setAmbient(1, 1, 1); 61 | 62 | scene_node_->attachObject(manual_object_); 63 | } 64 | 65 | PixArrow::~PixArrow() { 66 | if (scene_node_->getParentSceneNode()) { 67 | scene_node_->getParentSceneNode()->removeChild(scene_node_); 68 | } 69 | scene_manager_->destroySceneNode(scene_node_); 70 | scene_manager_->destroyManualObject(manual_object_); 71 | Ogre::MaterialManager::getSingleton().remove(manual_object_material_->getName()); 72 | } 73 | 74 | void PixArrow::set(float shaft_length, float head_length) { 75 | Ogre::Vector3 start(0, 0, 0); 76 | Ogre::Vector3 end(0, shaft_length, 0); 77 | // 50 degrees arrow head, 25 degrees each side. sin(25deg)=0.4226, cos(25deg)=0.9063 78 | Ogre::Vector3 left(-0.4226 * head_length, shaft_length - 0.9063 * head_length, 0); 79 | Ogre::Vector3 right(0.4226 * head_length, shaft_length - 0.9063 * head_length, 0); 80 | 81 | manual_object_->clear(); 82 | manual_object_->begin(manual_object_material_->getName(), Ogre::RenderOperation::OT_LINE_LIST); 83 | manual_object_->position(start); 84 | manual_object_->position(end); 85 | manual_object_->position(left); 86 | manual_object_->position(end); 87 | manual_object_->position(right); 88 | manual_object_->position(end); 89 | manual_object_->end(); 90 | setVisible(true); 91 | } 92 | 93 | void PixArrow::setVisible(bool visible) { scene_node_->setVisible(visible, true); } 94 | 95 | void PixArrow::setPosition(const Ogre::Vector3& position) { scene_node_->setPosition(position); } 96 | 97 | void PixArrow::setDirection(const Ogre::Vector3& direction) { 98 | if (!direction.isZeroLength()) { 99 | setOrientation(Ogre::Vector3::UNIT_Y.getRotationTo(direction)); 100 | } 101 | } 102 | 103 | void PixArrow::setOrientation(const Ogre::Quaternion& orientation) { scene_node_->setOrientation(orientation); } 104 | 105 | void PixArrow::setScale(const Ogre::Vector3& scale) { scene_node_->setScale(scale); } 106 | 107 | void PixArrow::setColor(const Ogre::ColourValue& c) { 108 | // this is consistent with the behaviour in the Shape class. 109 | 110 | // manual_object_material_->getTechnique(0)->setAmbient(c * 0.5); 111 | // manual_object_material_->getTechnique(0)->setDiffuse(c); 112 | manual_object_material_->getTechnique(0)->getPass(0)->setEmissive(c); 113 | 114 | if (c.a < 0.9998) { 115 | manual_object_material_->getTechnique(0)->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA); 116 | manual_object_material_->getTechnique(0)->setDepthWriteEnabled(false); 117 | } else { 118 | manual_object_material_->getTechnique(0)->setSceneBlending(Ogre::SBT_REPLACE); 119 | manual_object_material_->getTechnique(0)->setDepthWriteEnabled(true); 120 | } 121 | } 122 | 123 | void PixArrow::setColor(float r, float g, float b, float a) { setColor(Ogre::ColourValue(r, g, b, a)); } 124 | 125 | // where are the void Line::setColour(...) convenience methods??? ;) 126 | 127 | const Ogre::Vector3& PixArrow::getPosition() { return scene_node_->getPosition(); } 128 | 129 | const Ogre::Quaternion& PixArrow::getOrientation() { return scene_node_->getOrientation(); } 130 | 131 | void PixArrow::setUserData(const Ogre::Any& data) { manual_object_->getUserObjectBindings().setUserAny(data); } 132 | 133 | } // namespace rviz -------------------------------------------------------------------------------- /src/pix_arrow.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2023, ARCS Lab 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the ARCS Lab nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #ifndef ARCS_RVIZ_PIX_ARROW_H 31 | #define ARCS_RVIZ_PIX_ARROW_H 32 | 33 | #include 34 | 35 | #include 36 | #include 37 | #include 38 | 39 | namespace Ogre { 40 | class Any; 41 | } // namespace Ogre 42 | 43 | namespace rviz { 44 | /* Represents a straight wireframe line between two points. */ 45 | class PixArrow : public Object { 46 | public: 47 | /** 48 | * \brief Constructor 49 | * @param manager Scene manager this object is a part of 50 | * @param parent_node A scene node to use as the parent of this object. If NULL, uses the root scene 51 | * node. 52 | */ 53 | PixArrow(Ogre::SceneManager* manager, Ogre::SceneNode* parent_node = nullptr); 54 | 55 | ~PixArrow() override; 56 | 57 | /** 58 | * \brief Set the shaft length and head length of the arrow 59 | */ 60 | void set(float shaft_length, float head_length); 61 | 62 | void setVisible(bool visible); 63 | 64 | /** 65 | * \brief Set the position of this object 66 | * @param Position vector position to set to. 67 | */ 68 | void setPosition(const Ogre::Vector3& position) override; 69 | 70 | /** @brief Set the direction of the arrow. 71 | * 72 | * @param direction The direction the arrow should point, in the 73 | * coordinate frame of the parent Ogre::SceneNode. 74 | * 75 | * If direction is zero, this does not change the arrow. 76 | * 77 | * Both setOrientation() and setDirection() change the direction the arrow points. */ 78 | void setDirection(const Ogre::Vector3& direction); 79 | 80 | /** 81 | * \brief Set the orientation of the object 82 | * @param Orientation quaternion orientation to set to. 83 | */ 84 | void setOrientation(const Ogre::Quaternion& orientation) override; 85 | 86 | /** 87 | * \brief Set the scale of the object. Always relative to the identity orientation of the object. 88 | * @param Scale vector scale to set to. 89 | */ 90 | void setScale(const Ogre::Vector3& scale) override; 91 | 92 | /** 93 | * \brief Set the color of the object. Values are in the range [0, 1] 94 | * @param r Red component 95 | * @param g Green component 96 | * @param b Blue component 97 | */ 98 | void setColor(float r, float g, float b, float a) override; 99 | 100 | /** 101 | * \brief Set the color of the object using ogre colour definitions. 102 | * 103 | * @param c : ogre colour type. 104 | */ 105 | virtual void setColor(const Ogre::ColourValue& c); 106 | 107 | /** 108 | * \brief Get the local position of this object 109 | * @return The position 110 | */ 111 | const Ogre::Vector3& getPosition() override; 112 | /** 113 | * \brief Get the local orientation of this object 114 | * @return The orientation 115 | */ 116 | const Ogre::Quaternion& getOrientation() override; 117 | 118 | /** 119 | * \brief Set the user data on this object 120 | * @param data 121 | */ 122 | void setUserData(const Ogre::Any& data) override; 123 | 124 | protected: 125 | Ogre::SceneNode* scene_node_; 126 | Ogre::ManualObject* manual_object_; 127 | Ogre::MaterialPtr manual_object_material_; 128 | }; 129 | 130 | } // namespace rviz 131 | 132 | #endif /* ARCS_RVIZ_PIX_ARROW_H */ 133 | -------------------------------------------------------------------------------- /src/point_cloud2_normal_display.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2023, ARCS Lab 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the ARCS Lab nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #include 31 | #include 32 | 33 | #include 34 | 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | 42 | #include "point_cloud_normal.h" 43 | #include "point_cloud2_normal_display.h" 44 | 45 | namespace rviz { 46 | PointCloud2NormalDisplay::PointCloud2NormalDisplay() : point_cloud_normal_(new PointCloudNormal(this)) {} 47 | 48 | PointCloud2NormalDisplay::~PointCloud2NormalDisplay() { 49 | PointCloud2NormalDisplay::unsubscribe(); 50 | delete point_cloud_normal_; 51 | } 52 | 53 | void PointCloud2NormalDisplay::onInitialize() { 54 | // Use the threaded queue for processing of incoming messages 55 | update_nh_.setCallbackQueue(context_->getThreadedQueue()); 56 | 57 | MFDClass::onInitialize(); 58 | point_cloud_normal_->initialize(context_, scene_node_); 59 | } 60 | 61 | void PointCloud2NormalDisplay::processMessage(const sensor_msgs::PointCloud2ConstPtr& cloud) { 62 | // Filter any nan values out of the cloud. Any nan values that make it through to PointCloudBase 63 | // will get their points put off in lala land, but it means they still do get processed/rendered 64 | // which can be a big performance hit 65 | sensor_msgs::PointCloud2Ptr filtered(new sensor_msgs::PointCloud2); 66 | 67 | int32_t xi = findChannelIndex(cloud, "x"); 68 | int32_t yi = findChannelIndex(cloud, "y"); 69 | int32_t zi = findChannelIndex(cloud, "z"); 70 | int32_t normal_xi = findChannelIndex(cloud, "normal_x"); 71 | int32_t normal_yi = findChannelIndex(cloud, "normal_y"); 72 | int32_t normal_zi = findChannelIndex(cloud, "normal_z"); 73 | if (normal_xi < 0 || normal_yi < 0 || normal_zi < 0 || xi < 0 || yi < 0 || zi < 0) { 74 | setStatusStd(StatusProperty::Error, "Message", "Position or normal information not found or not complete."); 75 | return; 76 | } 77 | 78 | const uint32_t xoff = cloud->fields[xi].offset; 79 | const uint32_t yoff = cloud->fields[yi].offset; 80 | const uint32_t zoff = cloud->fields[zi].offset; 81 | const uint32_t normal_xoff = cloud->fields[normal_xi].offset; 82 | const uint32_t normal_yoff = cloud->fields[normal_yi].offset; 83 | const uint32_t normal_zoff = cloud->fields[normal_zi].offset; 84 | const uint32_t point_step = cloud->point_step; 85 | const size_t point_count = cloud->width * cloud->height; 86 | 87 | if (point_count * point_step != cloud->data.size()) { 88 | std::stringstream ss; 89 | ss << "Data size (" << cloud->data.size() << " bytes) does not match width (" << cloud->width << ") times height (" 90 | << cloud->height << ") times point_step (" << point_step << "). Dropping message."; 91 | setStatusStd(StatusProperty::Error, "Message", ss.str()); 92 | return; 93 | } 94 | 95 | filtered->data.resize(cloud->data.size()); 96 | uint32_t output_count; 97 | if (point_count == 0) { 98 | output_count = 0; 99 | } else { 100 | uint8_t* output_ptr = &filtered->data.front(); 101 | const uint8_t *ptr = &cloud->data.front(), *ptr_end = &cloud->data.back(), *ptr_init; 102 | size_t points_to_copy = 0; 103 | for (; ptr < ptr_end; ptr += point_step) { 104 | float x = *reinterpret_cast(ptr + xoff); 105 | float y = *reinterpret_cast(ptr + yoff); 106 | float z = *reinterpret_cast(ptr + zoff); 107 | float normal_x = *reinterpret_cast(ptr + normal_xoff); 108 | float normal_y = *reinterpret_cast(ptr + normal_yoff); 109 | float normal_z = *reinterpret_cast(ptr + normal_zoff); 110 | if (validateFloats(normal_x) && validateFloats(normal_y) && validateFloats(normal_z) && validateFloats(x) && 111 | validateFloats(y) && validateFloats(z)) { 112 | if (points_to_copy == 0) { 113 | // Only memorize where to start copying from 114 | ptr_init = ptr; 115 | points_to_copy = 1; 116 | } else { 117 | ++points_to_copy; 118 | } 119 | } else { 120 | if (points_to_copy) { 121 | // Copy all the points that need to be copied 122 | memcpy(output_ptr, ptr_init, point_step * points_to_copy); 123 | output_ptr += point_step * points_to_copy; 124 | points_to_copy = 0; 125 | } 126 | } 127 | } 128 | // Don't forget to flush what needs to be copied 129 | if (points_to_copy) { 130 | memcpy(output_ptr, ptr_init, point_step * points_to_copy); 131 | output_ptr += point_step * points_to_copy; 132 | } 133 | output_count = (output_ptr - &filtered->data.front()) / point_step; 134 | } 135 | 136 | filtered->header = cloud->header; 137 | filtered->fields = cloud->fields; 138 | filtered->data.resize(output_count * point_step); 139 | filtered->height = 1; 140 | filtered->width = output_count; 141 | filtered->is_bigendian = cloud->is_bigendian; 142 | filtered->point_step = point_step; 143 | filtered->row_step = output_count; 144 | 145 | point_cloud_normal_->addMessage(filtered); 146 | } 147 | 148 | void PointCloud2NormalDisplay::update(float wall_dt, float ros_dt) {} 149 | 150 | void PointCloud2NormalDisplay::reset() { 151 | MFDClass::reset(); 152 | point_cloud_normal_->reset(); 153 | } 154 | 155 | } // namespace rviz 156 | 157 | #include 158 | PLUGINLIB_EXPORT_CLASS(rviz::PointCloud2NormalDisplay, rviz::Display) -------------------------------------------------------------------------------- /src/point_cloud2_normal_display.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2023, ARCS Lab 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the ARCS Lab nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #ifndef ARCS_RVIZ_POINT_CLOUD2_NORMAL_DISPLAY_H 31 | #define ARCS_RVIZ_POINT_CLOUD2_NORMAL_DISPLAY_H 32 | 33 | #include 34 | 35 | #include 36 | 37 | namespace rviz { 38 | class IntProperty; 39 | class PointCloudNormal; 40 | 41 | /** 42 | * \class PointCloud2NormalDisplay 43 | * \brief Displays normals of a point cloud of type sensor_msgs::PointCloud2 44 | */ 45 | class PointCloud2NormalDisplay : public MessageFilterDisplay { 46 | Q_OBJECT 47 | public: 48 | PointCloud2NormalDisplay(); 49 | ~PointCloud2NormalDisplay() override; 50 | 51 | void reset() override; 52 | 53 | void update(float wall_dt, float ros_dt) override; 54 | 55 | protected: 56 | /** @brief Do initialization. Overridden from MessageFilterDisplay. */ 57 | void onInitialize() override; 58 | 59 | /** @brief Process a single message. Overridden from MessageFilterDisplay. */ 60 | void processMessage(const sensor_msgs::PointCloud2ConstPtr& cloud) override; 61 | 62 | PointCloudNormal* point_cloud_normal_; 63 | }; 64 | 65 | } // namespace rviz 66 | 67 | #endif -------------------------------------------------------------------------------- /src/point_cloud_normal.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2023, ARCS Lab 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the ARCS Lab nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #include 31 | 32 | #include 33 | #include 34 | #include 35 | 36 | #include 37 | 38 | #include 39 | 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | #include 50 | #include 51 | #include 52 | #include 53 | 54 | #include "arrow.h" 55 | #include "pix_arrow.h" 56 | #include "point_cloud_normal.h" 57 | 58 | namespace rviz { 59 | 60 | PointCloudNormal::PointCloudNormal(Display* display) : display_(display), buffer_index_(0), current_mode_(NM_ARROW) { 61 | arrow_color_property_ = new ColorProperty("Arrow Color", QColor(0x66, 0xcc, 0xff), "Color of the normal arrows", 62 | display_, SLOT(updateArrowColor()), this); 63 | 64 | alpha_property_ = new FloatProperty("Alpha", 1.0, 65 | "Amount of transparency to apply to the points. " 66 | "Note that this is experimental and does not always look correct.", 67 | display_, SLOT(updateArrowColor()), this); 68 | alpha_property_->setMin(0); 69 | alpha_property_->setMax(1); 70 | 71 | buffer_length_property_ = 72 | new IntProperty("Buffer Length", 1, "Number of frames to display.", display_, SLOT(updateBufferLength()), this); 73 | buffer_length_property_->setMin(1); 74 | 75 | style_property_ = new EnumProperty("Style", "Arrow", "Rendering mode to use, in order of computational complexity.", 76 | display_, SLOT(updateStyle()), this); 77 | style_property_->addOption("2D Arrow", PointCloudNormal::NM_2D_ARROW); 78 | style_property_->addOption("Arrow", PointCloudNormal::NM_ARROW); 79 | 80 | arrow_shaft_length_property_ = new FloatProperty("Shaft Length", 0.3, "Length of the arrow shaft.", style_property_, 81 | SLOT(updateArrowGeometry()), this); 82 | arrow_head_length_property_ = new FloatProperty("Head Length", 0.2, "Length of the arrow head.", style_property_, 83 | SLOT(updateArrowGeometry()), this); 84 | arrow_shaft_diameter_property_ = new FloatProperty("Shaft Diameter", 0.1, "Diameter of the arrow shaft.", 85 | style_property_, SLOT(updateArrowGeometry()), this); 86 | arrow_head_diameter_property_ = new FloatProperty("Head Diameter", 0.3, "Diameter of the arrow head.", 87 | style_property_, SLOT(updateArrowGeometry()), this); 88 | 89 | pix_arrow_shaft_length_property_ = new FloatProperty("2D Arrow Shaft Length", 0.5, "Length of the 2D arrow shaft.", 90 | style_property_, SLOT(updatePixArrowGeometry()), this); 91 | pix_arrow_head_length_property_ = new FloatProperty("2D Arrow Head Length", 0.2, "Length of the 2D arrow head.", 92 | style_property_, SLOT(updatePixArrowGeometry()), this); 93 | pix_arrow_shaft_length_property_->hide(); 94 | pix_arrow_head_length_property_->hide(); 95 | } 96 | 97 | void PointCloudNormal::initialize(DisplayContext* context, Ogre::SceneNode* scene_node) { 98 | context_ = context; 99 | scene_node_ = scene_node; 100 | 101 | updateStyle(); 102 | updateArrowColor(); 103 | updateArrowGeometry(); 104 | updatePixArrowGeometry(); 105 | updateBufferLength(); 106 | } 107 | 108 | PointCloudNormal::~PointCloudNormal() { 109 | destroyArrowChain(); 110 | destroyPixArrowChain(); 111 | } 112 | 113 | void PointCloudNormal::updateBufferLength() { 114 | int new_buffer_length = buffer_length_property_->getInt(); 115 | if (new_buffer_length == arrow_chain_.size()) return; 116 | 117 | // create new containers 118 | std::vector > arrow_chain(new_buffer_length); 119 | std::vector > pix_arrow_chain(new_buffer_length); 120 | std::vector cloud_chain(new_buffer_length); 121 | 122 | if (new_buffer_length > arrow_chain_.size()) { 123 | size_t offset = new_buffer_length - arrow_chain_.size(); 124 | for (size_t i = 0; i < arrow_chain_.size(); ++i) { 125 | arrow_chain[i + offset].swap(arrow_chain_[(i + buffer_index_) % arrow_chain_.size()]); 126 | pix_arrow_chain[i + offset].swap(pix_arrow_chain_[(i + buffer_index_) % arrow_chain_.size()]); 127 | cloud_chain[i + offset].swap(cloud_chain_[(i + buffer_index_) % arrow_chain_.size()]); 128 | } 129 | buffer_index_ = 0; 130 | } else if (new_buffer_length < arrow_chain_.size()) { 131 | for (size_t i = 0; i < arrow_chain_.size(); ++i) { 132 | arrow_chain[i % new_buffer_length].swap(arrow_chain_[(i + buffer_index_) % arrow_chain_.size()]); 133 | pix_arrow_chain[i % new_buffer_length].swap(pix_arrow_chain_[(i + buffer_index_) % arrow_chain_.size()]); 134 | cloud_chain[i % new_buffer_length].swap(cloud_chain_[(i + buffer_index_) % arrow_chain_.size()]); 135 | } 136 | buffer_index_ = 0; 137 | } 138 | // destroy unused arrows 139 | destroyArrowChain(); 140 | destroyPixArrowChain(); 141 | 142 | // replace old buffer by new buffer 143 | arrow_chain_.swap(arrow_chain); 144 | pix_arrow_chain_.swap(pix_arrow_chain); 145 | cloud_chain_.swap(cloud_chain); 146 | 147 | context_->queueRender(); 148 | } 149 | 150 | void PointCloudNormal::arrow2PixArrow() { 151 | destroyPixArrowChain(); 152 | 153 | float head_length = pix_arrow_head_length_property_->getFloat(); 154 | float shaft_length = pix_arrow_shaft_length_property_->getFloat(); 155 | 156 | QColor color = arrow_color_property_->getColor(); 157 | float alpha = alpha_property_->getFloat(); 158 | 159 | pix_arrow_chain_.resize(arrow_chain_.size()); 160 | for (size_t i = 0; i < arrow_chain_.size(); ++i) { 161 | auto& arrow_vect = arrow_chain_[i]; 162 | auto& pix_arrow_vect = pix_arrow_chain_[i]; 163 | allocatePixArrowVector(pix_arrow_vect, arrow_vect.size()); 164 | for (size_t j = 0; j < arrow_vect.size(); ++j) { 165 | pix_arrow_vect[j]->set(shaft_length, head_length); 166 | pix_arrow_vect[j]->setPosition(arrow_vect[j]->getPosition()); 167 | pix_arrow_vect[j]->setOrientation(arrow_vect[j]->getOrientation()); 168 | pix_arrow_vect[j]->setColor(color.redF(), color.greenF(), color.blueF(), alpha); 169 | } 170 | } 171 | } 172 | 173 | void PointCloudNormal::pixArrow2Arrow() { 174 | destroyArrowChain(); 175 | 176 | float shaft_length = arrow_shaft_length_property_->getFloat(); 177 | float shaft_diameter = arrow_shaft_diameter_property_->getFloat(); 178 | float head_length = arrow_head_length_property_->getFloat(); 179 | float head_diameter = arrow_head_diameter_property_->getFloat(); 180 | 181 | QColor color = arrow_color_property_->getColor(); 182 | float alpha = alpha_property_->getFloat(); 183 | 184 | arrow_chain_.resize(pix_arrow_chain_.size()); 185 | for (size_t i = 0; i < pix_arrow_chain_.size(); ++i) { 186 | auto& pix_arrow_vect = pix_arrow_chain_[i]; 187 | auto& arrow_vect = arrow_chain_[i]; 188 | allocateArrowVector(arrow_vect, pix_arrow_vect.size()); 189 | for (size_t j = 0; j < pix_arrow_vect.size(); ++j) { 190 | arrow_vect[j]->set(shaft_length, shaft_diameter, head_length, head_diameter); 191 | arrow_vect[j]->setPosition(pix_arrow_vect[j]->getPosition()); 192 | arrow_vect[j]->setOrientation(pix_arrow_vect[j]->getOrientation()); 193 | arrow_vect[j]->setColor(color.redF(), color.greenF(), color.blueF(), alpha); 194 | } 195 | } 196 | } 197 | 198 | void PointCloudNormal::updateStyle() { 199 | PointCloudNormal::NormalMode mode = (PointCloudNormal::NormalMode)style_property_->getOptionInt(); 200 | if (mode == current_mode_) return; 201 | 202 | if (mode == PointCloudNormal::NM_ARROW) { 203 | arrow_head_diameter_property_->show(); 204 | arrow_head_length_property_->show(); 205 | arrow_shaft_diameter_property_->show(); 206 | arrow_shaft_length_property_->show(); 207 | pix_arrow_shaft_length_property_->hide(); 208 | pix_arrow_head_length_property_->hide(); 209 | 210 | pixArrow2Arrow(); 211 | destroyPixArrowChain(); 212 | } else { 213 | arrow_head_diameter_property_->hide(); 214 | arrow_head_length_property_->hide(); 215 | arrow_shaft_diameter_property_->hide(); 216 | arrow_shaft_length_property_->hide(); 217 | pix_arrow_shaft_length_property_->show(); 218 | pix_arrow_head_length_property_->show(); 219 | 220 | arrow2PixArrow(); 221 | destroyArrowChain(); 222 | } 223 | current_mode_ = mode; 224 | context_->queueRender(); 225 | } 226 | 227 | void PointCloudNormal::updateArrowGeometry() { 228 | float shaft_length = arrow_shaft_length_property_->getFloat(); 229 | float shaft_diameter = arrow_shaft_diameter_property_->getFloat(); 230 | float head_length = arrow_head_length_property_->getFloat(); 231 | float head_diameter = arrow_head_diameter_property_->getFloat(); 232 | for (size_t i = 0; i < arrow_chain_.size(); ++i) { 233 | auto& arrow_vect = arrow_chain_[i]; 234 | for (size_t j = 0; j < arrow_vect.size(); ++j) { 235 | arrow_vect[j]->set(shaft_length, shaft_diameter, head_length, head_diameter); 236 | } 237 | } 238 | context_->queueRender(); 239 | } 240 | 241 | void PointCloudNormal::updatePixArrowGeometry() { 242 | float head_length = pix_arrow_head_length_property_->getFloat(); 243 | float shaft_length = pix_arrow_shaft_length_property_->getFloat(); 244 | 245 | for (size_t i = 0; i < pix_arrow_chain_.size(); ++i) { 246 | auto& pix_arrow_vect = pix_arrow_chain_[i]; 247 | for (size_t j = 0; j < pix_arrow_vect.size(); ++j) { 248 | pix_arrow_vect[j]->set(shaft_length, head_length); 249 | } 250 | } 251 | context_->queueRender(); 252 | } 253 | 254 | void PointCloudNormal::updateArrowColor() { 255 | QColor color = arrow_color_property_->getColor(); 256 | float alpha = alpha_property_->getFloat(); 257 | 258 | PointCloudNormal::NormalMode mode = (PointCloudNormal::NormalMode)style_property_->getOptionInt(); 259 | if (mode == PointCloudNormal::NM_ARROW) { 260 | for (size_t i = 0; i < arrow_chain_.size(); ++i) { 261 | auto& arrow_vect = arrow_chain_[i]; 262 | for (size_t j = 0; j < arrow_vect.size(); ++j) { 263 | arrow_vect[j]->setColor(color.redF(), color.greenF(), color.blueF(), alpha); 264 | } 265 | } 266 | } else { 267 | for (size_t i = 0; i < pix_arrow_chain_.size(); ++i) { 268 | auto& pix_arrow_vect = pix_arrow_chain_[i]; 269 | for (size_t j = 0; j < pix_arrow_vect.size(); ++j) { 270 | pix_arrow_vect[j]->setColor(color.redF(), color.greenF(), color.blueF(), alpha); 271 | } 272 | } 273 | } 274 | context_->queueRender(); 275 | } 276 | 277 | void PointCloudNormal::reset() { 278 | destroyArrowChain(); 279 | destroyPixArrowChain(); 280 | cloud_chain_.clear(); 281 | 282 | size_t buffer_length = buffer_length_property_->getInt(); 283 | arrow_chain_.resize(buffer_length); 284 | pix_arrow_chain_.resize(buffer_length); 285 | cloud_chain_.resize(buffer_length); 286 | } 287 | 288 | void PointCloudNormal::allocateArrowVector(std::vector& arrow_vect, size_t num) { 289 | if (num > arrow_vect.size()) { 290 | for (size_t i = arrow_vect.size(); i < num; ++i) { 291 | rviz::Arrow2* arrow = new rviz::Arrow2(context_->getSceneManager(), scene_node_); 292 | arrow_vect.push_back(arrow); 293 | } 294 | } else if (num < arrow_vect.size()) { 295 | for (size_t i = num; i < arrow_vect.size(); ++i) { 296 | delete arrow_vect[i]; 297 | } 298 | arrow_vect.resize(num); 299 | } 300 | } 301 | 302 | void PointCloudNormal::allocatePixArrowVector(std::vector& pix_arrow_vect, size_t num) { 303 | if (num > pix_arrow_vect.size()) { 304 | for (size_t i = pix_arrow_vect.size(); i < num; ++i) { 305 | PixArrow* pix_arrow = new PixArrow(context_->getSceneManager(), scene_node_); 306 | pix_arrow_vect.push_back(pix_arrow); 307 | } 308 | } else if (num < pix_arrow_vect.size()) { 309 | for (size_t i = num; i < pix_arrow_vect.size(); ++i) { 310 | delete pix_arrow_vect[i]; 311 | } 312 | pix_arrow_vect.resize(num); 313 | } 314 | } 315 | 316 | void PointCloudNormal::destroyArrowChain() { 317 | for (size_t i = 0; i < arrow_chain_.size(); ++i) { 318 | allocateArrowVector(arrow_chain_[i], 0); 319 | } 320 | arrow_chain_.resize(0); 321 | } 322 | 323 | void PointCloudNormal::destroyPixArrowChain() { 324 | for (size_t i = 0; i < pix_arrow_chain_.size(); ++i) { 325 | allocatePixArrowVector(pix_arrow_chain_[i], 0); 326 | } 327 | pix_arrow_chain_.resize(0); 328 | } 329 | 330 | void PointCloudNormal::updateStatus() { 331 | std::stringstream ss; 332 | // ss << "Showing [" << total_point_count_ << "] points from [" << clouds_.size() << "] messages"; 333 | display_->setStatusStd(StatusProperty::Ok, "Points", ss.str()); 334 | } 335 | 336 | void PointCloudNormal::processMessage(const sensor_msgs::PointCloud2ConstPtr& cloud) { 337 | size_t buffer_length = buffer_length_property_->getInt(); 338 | cloud_chain_[buffer_index_] = cloud; 339 | 340 | Ogre::Vector3 position; 341 | Ogre::Quaternion orientation; 342 | if (!context_->getFrameManager()->getTransform(cloud->header, position, orientation)) { 343 | ROS_ERROR("Error transforming from frame '%s' to frame '%s'", cloud->header.frame_id.c_str(), 344 | qPrintable(context_->getFixedFrame())); 345 | } 346 | 347 | Ogre::Matrix4 transform(orientation); 348 | transform.setTrans(position); 349 | 350 | Ogre::ColourValue color = arrow_color_property_->getOgreColor(); 351 | color.a = alpha_property_->getFloat(); 352 | 353 | NormalMode mode = (NormalMode)style_property_->getOptionInt(); 354 | 355 | uint32_t num_points = cloud->width; 356 | 357 | int32_t xi = findChannelIndex(cloud, "x"); 358 | int32_t yi = findChannelIndex(cloud, "y"); 359 | int32_t zi = findChannelIndex(cloud, "z"); 360 | int32_t normal_xi = findChannelIndex(cloud, "normal_x"); 361 | int32_t normal_yi = findChannelIndex(cloud, "normal_y"); 362 | int32_t normal_zi = findChannelIndex(cloud, "normal_z"); 363 | const uint32_t xoff = cloud->fields[xi].offset; 364 | const uint32_t yoff = cloud->fields[yi].offset; 365 | const uint32_t zoff = cloud->fields[zi].offset; 366 | const uint32_t normal_xoff = cloud->fields[normal_xi].offset; 367 | const uint32_t normal_yoff = cloud->fields[normal_yi].offset; 368 | const uint32_t normal_zoff = cloud->fields[normal_zi].offset; 369 | const uint32_t point_step = cloud->point_step; 370 | 371 | if (mode == NM_ARROW) { 372 | float shaft_length = arrow_shaft_length_property_->getFloat(); 373 | float shaft_diameter = arrow_shaft_diameter_property_->getFloat(); 374 | float head_length = arrow_head_length_property_->getFloat(); 375 | float head_diameter = arrow_head_diameter_property_->getFloat(); 376 | 377 | auto& arrow_vect = arrow_chain_[buffer_index_]; 378 | allocateArrowVector(arrow_vect, num_points); 379 | for (uint32_t i = 0; i < num_points; ++i) { 380 | const uint8_t* ptr = cloud->data.data() + i * point_step; 381 | float x = *reinterpret_cast(ptr + xoff); 382 | float y = *reinterpret_cast(ptr + yoff); 383 | float z = *reinterpret_cast(ptr + zoff); 384 | float normal_x = *reinterpret_cast(ptr + normal_xoff); 385 | float normal_y = *reinterpret_cast(ptr + normal_yoff); 386 | float normal_z = *reinterpret_cast(ptr + normal_zoff); 387 | 388 | Ogre::Vector3 xpos = transform * Ogre::Vector3(x, y, z); 389 | Ogre::Vector3 xdir = orientation * Ogre::Vector3(normal_x, normal_y, normal_z); 390 | arrow_vect[i]->setColor(color); 391 | arrow_vect[i]->set(shaft_length, shaft_diameter, head_length, head_diameter); 392 | arrow_vect[i]->setPosition(xpos); 393 | arrow_vect[i]->setDirection(xdir); 394 | } 395 | } else if (mode == NM_2D_ARROW) { 396 | float head_length = pix_arrow_head_length_property_->getFloat(); 397 | float shaft_length = pix_arrow_shaft_length_property_->getFloat(); 398 | 399 | auto& pix_arrow_vect = pix_arrow_chain_[buffer_index_]; 400 | allocatePixArrowVector(pix_arrow_vect, num_points); 401 | for (size_t i = 0; i < pix_arrow_vect.size(); ++i) { 402 | const uint8_t* ptr = cloud->data.data() + i * point_step; 403 | float x = *reinterpret_cast(ptr + xoff); 404 | float y = *reinterpret_cast(ptr + yoff); 405 | float z = *reinterpret_cast(ptr + zoff); 406 | float normal_x = *reinterpret_cast(ptr + normal_xoff); 407 | float normal_y = *reinterpret_cast(ptr + normal_yoff); 408 | float normal_z = *reinterpret_cast(ptr + normal_zoff); 409 | 410 | Ogre::Vector3 xpos = transform * Ogre::Vector3(x, y, z); 411 | Ogre::Vector3 xdir = orientation * Ogre::Vector3(normal_x, normal_y, normal_z); 412 | pix_arrow_vect[i]->set(shaft_length, head_length); 413 | pix_arrow_vect[i]->setColor(color); 414 | pix_arrow_vect[i]->setPosition(xpos); 415 | // pix_arrow_vect[i]->setOrientation(Ogre::Vector3::NEGATIVE_UNIT_Y.getRotationTo(xdir)); 416 | pix_arrow_vect[i]->setDirection(xdir); 417 | } 418 | } else { 419 | ROS_ERROR("Error arrow style '%d'", mode); 420 | } 421 | context_->queueRender(); 422 | 423 | buffer_index_ += 1; 424 | buffer_index_ %= buffer_length; 425 | } 426 | 427 | void PointCloudNormal::addMessage(const sensor_msgs::PointCloud2ConstPtr& cloud) { processMessage(cloud); } 428 | 429 | void PointCloudNormal::fixedFrameChanged() { reset(); } 430 | 431 | } // namespace rviz -------------------------------------------------------------------------------- /src/point_cloud_normal.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2023, ARCS Lab 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the ARCS Lab nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #ifndef ARCS_RVIZ_POINT_CLOUD_NORMAL_H 31 | #define ARCS_RVIZ_POINT_CLOUD_NORMAL_H 32 | 33 | #ifndef Q_MOC_RUN // See: https://bugreports.qt-project.org/browse/QTBUG-22829 34 | #include 35 | #include 36 | #include 37 | 38 | #include 39 | #include 40 | 41 | #include 42 | #include 43 | #include 44 | 45 | #include 46 | 47 | #include 48 | 49 | #include 50 | #include 51 | 52 | #include 53 | #include 54 | #endif 55 | 56 | namespace rviz { 57 | class BoolProperty; 58 | class Display; 59 | class DisplayContext; 60 | class EnumProperty; 61 | class FloatProperty; 62 | class IntProperty; 63 | class ColorProperty; 64 | class Arrow2; 65 | class PixArrow; 66 | 67 | typedef std::vector V_string; 68 | 69 | /** 70 | * \class PointCloudNormal 71 | * \brief Displays normals of a point cloud of type sensor_msgs::PointCloud 72 | */ 73 | class PointCloudNormal : public QObject { 74 | Q_OBJECT 75 | public: 76 | enum NormalMode { 77 | NM_2D_ARROW, 78 | NM_ARROW, 79 | }; 80 | 81 | PointCloudNormal(Display* display); 82 | ~PointCloudNormal() override; 83 | 84 | void initialize(DisplayContext* context, Ogre::SceneNode* scene_node); 85 | 86 | void fixedFrameChanged(); 87 | void reset(); 88 | 89 | void addMessage(const sensor_msgs::PointCloud2ConstPtr& cloud); 90 | 91 | Display* getDisplay() { return display_; } 92 | 93 | EnumProperty* style_property_; 94 | ColorProperty* arrow_color_property_; 95 | FloatProperty* alpha_property_; 96 | IntProperty* buffer_length_property_; 97 | // for 3D arrow 98 | FloatProperty* arrow_shaft_length_property_; 99 | FloatProperty* arrow_head_length_property_; 100 | FloatProperty* arrow_shaft_diameter_property_; 101 | FloatProperty* arrow_head_diameter_property_; 102 | // for 2D arrow 103 | FloatProperty* pix_arrow_shaft_length_property_; 104 | FloatProperty* pix_arrow_head_length_property_; 105 | 106 | private Q_SLOTS: 107 | void updateStyle(); 108 | void updateArrowColor(); 109 | void updateArrowGeometry(); 110 | void updatePixArrowGeometry(); 111 | void updateBufferLength(); 112 | 113 | private: 114 | void processMessage(const sensor_msgs::PointCloud2ConstPtr& cloud); 115 | void updateStatus(); 116 | 117 | void allocateArrowVector(std::vector& arrow_vect, size_t num); 118 | void allocatePixArrowVector(std::vector& pix_arrow_vect, size_t num); 119 | void destroyArrowChain(); 120 | void destroyPixArrowChain(); 121 | 122 | void pixArrow2Arrow(); 123 | void arrow2PixArrow(); 124 | 125 | std::vector > arrow_chain_; 126 | std::vector > pix_arrow_chain_; 127 | std::vector cloud_chain_; 128 | 129 | Ogre::SceneNode* scene_node_; 130 | 131 | Display* display_; 132 | DisplayContext* context_; 133 | 134 | size_t buffer_index_; 135 | NormalMode current_mode_; 136 | }; 137 | 138 | } // namespace rviz 139 | 140 | #endif // RVIZ_POINT_CLOUD_COMMON_H -------------------------------------------------------------------------------- /src/shape.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2008, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #include "shape.h" 31 | #include 32 | 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | 44 | namespace rviz 45 | { 46 | Ogre::Entity* Shape::createEntity(const std::string& name, Type type, Ogre::SceneManager* scene_manager) 47 | { 48 | if (type == Mesh) 49 | return nullptr; // the entity is initialized after the vertex data was specified 50 | 51 | std::string mesh_name; 52 | switch (type) 53 | { 54 | case Cone: 55 | mesh_name = "rviz_cone.mesh"; 56 | break; 57 | 58 | case Cube: 59 | mesh_name = "rviz_cube.mesh"; 60 | break; 61 | 62 | case Cylinder: 63 | mesh_name = "rviz_cylinder.mesh"; 64 | break; 65 | 66 | case Sphere: 67 | mesh_name = "rviz_sphere.mesh"; 68 | break; 69 | 70 | default: 71 | ROS_BREAK(); 72 | } 73 | 74 | return scene_manager->createEntity(name, mesh_name); 75 | } 76 | 77 | Shape::Shape(Type type, Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node) 78 | : Object(scene_manager), type_(type) 79 | { 80 | static uint32_t count = 0; 81 | std::stringstream ss; 82 | ss << "Shape" << count++; 83 | 84 | entity_ = createEntity(ss.str(), type, scene_manager); 85 | 86 | if (!parent_node) 87 | { 88 | parent_node = scene_manager_->getRootSceneNode(); 89 | } 90 | 91 | scene_node_ = parent_node->createChildSceneNode(); 92 | offset_node_ = scene_node_->createChildSceneNode(); 93 | if (entity_) 94 | offset_node_->attachObject(entity_); 95 | 96 | ss << "Material"; 97 | material_name_ = ss.str(); 98 | material_ = Ogre::MaterialManager::getSingleton().create( 99 | material_name_, Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); 100 | material_->setReceiveShadows(false); 101 | material_->getTechnique(0)->setLightingEnabled(true); 102 | material_->getTechnique(0)->setAmbient(0.5, 0.5, 0.5); 103 | 104 | if (entity_) 105 | entity_->setMaterial(material_); 106 | } 107 | 108 | Shape::~Shape() 109 | { 110 | scene_manager_->destroySceneNode(scene_node_); 111 | scene_manager_->destroySceneNode(offset_node_); 112 | 113 | if (entity_) 114 | scene_manager_->destroyEntity(entity_); 115 | 116 | Ogre::MaterialManager::getSingleton().remove(material_->getName()); 117 | } 118 | 119 | void Shape::setColor(const Ogre::ColourValue& c) 120 | { 121 | material_->getTechnique(0)->setAmbient(c * 0.5); 122 | material_->getTechnique(0)->setDiffuse(c); 123 | 124 | if (c.a < 0.9998) 125 | { 126 | material_->getTechnique(0)->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA); 127 | material_->getTechnique(0)->setDepthWriteEnabled(false); 128 | } 129 | else 130 | { 131 | material_->getTechnique(0)->setSceneBlending(Ogre::SBT_REPLACE); 132 | material_->getTechnique(0)->setDepthWriteEnabled(true); 133 | } 134 | } 135 | 136 | void Shape::setColor(float r, float g, float b, float a) 137 | { 138 | setColor(Ogre::ColourValue(r, g, b, a)); 139 | } 140 | 141 | void Shape::setOffset(const Ogre::Vector3& offset) 142 | { 143 | offset_node_->setPosition(offset); 144 | } 145 | 146 | void Shape::setPosition(const Ogre::Vector3& position) 147 | { 148 | scene_node_->setPosition(position); 149 | } 150 | 151 | void Shape::setOrientation(const Ogre::Quaternion& orientation) 152 | { 153 | scene_node_->setOrientation(orientation); 154 | } 155 | 156 | void Shape::setScale(const Ogre::Vector3& scale) 157 | { 158 | scene_node_->setScale(scale); 159 | } 160 | 161 | const Ogre::Vector3& Shape::getPosition() 162 | { 163 | return scene_node_->getPosition(); 164 | } 165 | 166 | const Ogre::Quaternion& Shape::getOrientation() 167 | { 168 | return scene_node_->getOrientation(); 169 | } 170 | 171 | void Shape::setUserData(const Ogre::Any& data) 172 | { 173 | if (entity_) 174 | entity_->getUserObjectBindings().setUserAny(data); 175 | else 176 | ROS_ERROR("Shape not yet fully constructed. Cannot set user data. Did you add triangles to the mesh already?"); 177 | } 178 | 179 | } // namespace rviz -------------------------------------------------------------------------------- /src/shape.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2008, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #ifndef OGRE_TOOLS_SHAPE_H 31 | #define OGRE_TOOLS_SHAPE_H 32 | 33 | #include 34 | 35 | #include 36 | #include 37 | #include 38 | 39 | namespace Ogre 40 | { 41 | class SceneManager; 42 | class SceneNode; 43 | class Any; 44 | class Entity; 45 | } // namespace Ogre 46 | 47 | namespace rviz 48 | { 49 | /** 50 | */ 51 | class Shape : public Object 52 | { 53 | public: 54 | enum Type 55 | { 56 | Cone, 57 | Cube, 58 | Cylinder, 59 | Sphere, 60 | Mesh, 61 | }; 62 | 63 | /** 64 | * \brief Constructor 65 | * 66 | * @param scene_manager The scene manager this object is associated with 67 | * @param parent_node A scene node to use as the parent of this object. If NULL, uses the root scene 68 | * node. 69 | */ 70 | Shape(Type shape_type, Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node = nullptr); 71 | ~Shape() override; 72 | 73 | Type getType() 74 | { 75 | return type_; 76 | } 77 | 78 | /** 79 | * \brief Set the offset for this shape 80 | * 81 | * The default is no offset, which puts the pivot point directly in the center of the object. 82 | * 83 | * @param offset Amount to offset the center of the object from the pivot point 84 | */ 85 | void setOffset(const Ogre::Vector3& offset); 86 | 87 | void setColor(float r, float g, float b, float a) override; 88 | void setColor(const Ogre::ColourValue& c); 89 | void setPosition(const Ogre::Vector3& position) override; 90 | void setOrientation(const Ogre::Quaternion& orientation) override; 91 | void setScale(const Ogre::Vector3& scale) override; 92 | const Ogre::Vector3& getPosition() override; 93 | const Ogre::Quaternion& getOrientation() override; 94 | 95 | /** 96 | * \brief Get the root scene node (pivot node) for this object 97 | * 98 | * @return The root scene node of this object 99 | */ 100 | Ogre::SceneNode* getRootNode() 101 | { 102 | return scene_node_; 103 | } 104 | 105 | /** 106 | * \brief Sets user data on all ogre objects we own 107 | */ 108 | void setUserData(const Ogre::Any& data) override; 109 | 110 | Ogre::Entity* getEntity() 111 | { 112 | return entity_; 113 | } 114 | 115 | Ogre::MaterialPtr getMaterial() 116 | { 117 | return material_; 118 | } 119 | 120 | static Ogre::Entity* 121 | createEntity(const std::string& name, Type shape_type, Ogre::SceneManager* scene_manager); 122 | 123 | protected: 124 | Ogre::SceneNode* scene_node_; 125 | Ogre::SceneNode* offset_node_; 126 | Ogre::Entity* entity_; 127 | Ogre::MaterialPtr material_; 128 | std::string material_name_; 129 | 130 | Type type_; 131 | }; 132 | 133 | } // namespace rviz 134 | 135 | #endif 136 | --------------------------------------------------------------------------------