├── .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 | 
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 |
--------------------------------------------------------------------------------