├── icons └── classes │ └── SurfelCloud.png ├── shaders ├── include │ ├── pass_depth.vert │ ├── circle_impl.frag │ └── pack_depth.frag ├── depth_circle.frag ├── pass_color_circle.frag ├── pickcolor_circle.frag ├── flat_color_circle.frag ├── point_cloud_surfel.material ├── point.vert └── glsl120.program ├── plugin_description.xml ├── README.md ├── package.xml ├── CMakeLists.txt └── src ├── surfel_cloud_display.h ├── surfel_cloud_display.cpp ├── point_cloud_common.h ├── point_cloud.h ├── point_cloud.cpp └── point_cloud_common.cpp /icons/classes/SurfelCloud.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RMonica/surfel_cloud_rviz_plugin/HEAD/icons/classes/SurfelCloud.png -------------------------------------------------------------------------------- /shaders/include/pass_depth.vert: -------------------------------------------------------------------------------- 1 | #version 120 2 | 3 | uniform mat4 worldview_matrix; 4 | 5 | varying float depth; 6 | 7 | void passDepth( vec4 pos ) 8 | { 9 | vec4 pos_rel_view = worldview_matrix * pos; 10 | depth = -pos_rel_view.z; 11 | } 12 | -------------------------------------------------------------------------------- /shaders/depth_circle.frag: -------------------------------------------------------------------------------- 1 | #version 120 2 | 3 | // Draws a circle with the packed depth value 4 | 5 | // includes 6 | vec4 packDepth( ); 7 | void circleImpl( ); 8 | 9 | void main() 10 | { 11 | circleImpl(); 12 | 13 | gl_FragColor = packDepth(); 14 | } 15 | -------------------------------------------------------------------------------- /shaders/pass_color_circle.frag: -------------------------------------------------------------------------------- 1 | #version 120 2 | 3 | // Draws a circle in the fragment color 4 | 5 | varying vec3 normal; 6 | 7 | //includes: 8 | void circleImpl(); 9 | 10 | void main() 11 | { 12 | circleImpl(); 13 | gl_FragColor = gl_Color; 14 | } 15 | -------------------------------------------------------------------------------- /shaders/pickcolor_circle.frag: -------------------------------------------------------------------------------- 1 | #version 120 2 | 3 | // Draws a circle in the pick color 4 | 5 | //includes: 6 | void circleImpl(); 7 | 8 | uniform vec4 pick_color; 9 | 10 | void main() 11 | { 12 | circleImpl(); 13 | gl_FragColor = pick_color; 14 | } 15 | -------------------------------------------------------------------------------- /shaders/flat_color_circle.frag: -------------------------------------------------------------------------------- 1 | #version 120 2 | 3 | // Draws a circle in the fragment color, multiplying a with the alpha param 4 | 5 | uniform vec4 highlight; 6 | uniform float alpha; 7 | 8 | void circleImpl(); 9 | 10 | void main() 11 | { 12 | circleImpl(); 13 | vec3 col = gl_Color.xyz + gl_Color.xyz * highlight.xyz; 14 | gl_FragColor = vec4(col, alpha * gl_Color.a); 15 | } 16 | -------------------------------------------------------------------------------- /plugin_description.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | Displays a surfel cloud from sensor_msgs/PointCloud2 messages. 7 | 8 | sensor_msgs/PointCloud2 9 | 10 | 11 | -------------------------------------------------------------------------------- /shaders/include/circle_impl.frag: -------------------------------------------------------------------------------- 1 | #version 120 2 | 3 | varying vec3 position; 4 | varying vec3 normal; 5 | varying float sqr_radius; 6 | varying float billboard_half_side; 7 | 8 | void circleImpl() 9 | { 10 | vec2 billboard_d = (gl_PointCoord.st - vec2(0.5f)) * vec2(2.0f,-2.0f) * billboard_half_side; 11 | vec3 billboard_pos = position; 12 | billboard_pos.xy += billboard_d; 13 | vec3 l = normalize(billboard_pos); 14 | if (abs(dot(l,normal)) < 0.005f) 15 | discard; 16 | 17 | vec3 cpos = (dot(position, normal) / dot(l, normal)) * l; 18 | 19 | vec3 diff = cpos - position; 20 | if (dot(diff, diff) > sqr_radius) 21 | discard; 22 | } 23 | -------------------------------------------------------------------------------- /shaders/include/pack_depth.frag: -------------------------------------------------------------------------------- 1 | #version 120 2 | 3 | // Splits up a normalized depth value in the range (0..1) 4 | // into the vertex RGB values. 5 | // Alpha values below 1/255 are rendered transparent. 6 | 7 | uniform float alpha; 8 | uniform float far_clip_distance; 9 | 10 | const float minimum_alpha = 1.0 / 255.0; 11 | 12 | varying float depth; 13 | 14 | vec4 packDepth() 15 | { 16 | float normalized_depth = depth / far_clip_distance; 17 | 18 | // split up float into rgb components 19 | const vec3 shift = vec3(256.0 * 256.0, 256.0, 1.0); 20 | const vec3 mask = vec3(0.0, 1.0 / 256.0, 1.0 / 256.0); 21 | vec3 depth_packed = fract(normalized_depth * shift); 22 | depth_packed -= depth_packed.xxy * mask; 23 | 24 | return vec4( depth_packed.zyx, step( minimum_alpha, alpha )); 25 | } 26 | 27 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | Surfel Cloud RViz Plugin 2 | ------------------------ 3 | 4 | Plugin for the ROS (Robot Operating System) visualizer RViz, to display surfel clouds. 5 | Surfel clouds are point clouds where each point has a normal and a radius, i.e., it represents a small oriented disk. 6 | 7 | This plugin adds a new display type, named `SurfelCloud`. 8 | 9 | This plugin is an extension of the built-in PointCloud2 plugin, and likewise it receives messages of type `sensor_msgs/PointCloud2`. 10 | However, a new display style is present, named `Surfels`, selectable using the `Style` drop-down menu. 11 | 12 | The fields `normal_x`, `normal_y`, `normal_z` and `radius` must be present in the point cloud. This requirement is compatible with the `pcl::PointSurfel` point type from PCL (Point Cloud Library). 13 | 14 | The `surfel_cloud_rviz_plugin` was tested on ROS Kinetic and Melodic. 15 | 16 | 2019-08-08 -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | surfel_cloud_rviz_plugin 4 | 0.0.0 5 | The surfel_cloud_rviz_plugin package 6 | 7 | rimlab 8 | 9 | TODO 10 | 11 | catkin 12 | roscpp 13 | rviz 14 | sensor_msgs 15 | roslib 16 | roscpp 17 | rviz 18 | sensor_msgs 19 | roslib 20 | roscpp 21 | rviz 22 | sensor_msgs 23 | roslib 24 | 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /shaders/point_cloud_surfel.material: -------------------------------------------------------------------------------- 1 | material surfel_cloud_rviz_plugin/SurfelPointCloud 2 | { 3 | technique gp 4 | { 5 | pass 6 | { 7 | alpha_rejection greater_equal 1 8 | point_size_attenuation on 9 | point_sprites on 10 | vertex_program_ref surfel_cloud_rviz_plugin/point.vert {} 11 | fragment_program_ref surfel_cloud_rviz_plugin/flat_color_circle.frag {} 12 | } 13 | } 14 | 15 | technique depth 16 | { 17 | scheme Depth 18 | pass 19 | { 20 | point_size_attenuation on 21 | vertex_program_ref surfel_cloud_rviz_plugin/point.vert(with_depth) {} 22 | fragment_program_ref surfel_cloud_rviz_plugin/depth_circle.frag {} 23 | } 24 | } 25 | 26 | technique selection_first_pass 27 | { 28 | scheme Pick 29 | pass 30 | { 31 | point_size_attenuation on 32 | vertex_program_ref surfel_cloud_rviz_plugin/point.vert {} 33 | fragment_program_ref surfel_cloud_rviz_plugin/pickcolor_circle.frag {} 34 | } 35 | } 36 | 37 | technique selection_second_pass 38 | { 39 | scheme Pick1 40 | pass 41 | { 42 | point_size_attenuation on 43 | vertex_program_ref surfel_cloud_rviz_plugin/point.vert {} 44 | fragment_program_ref surfel_cloud_rviz_plugin/pass_color_circle.frag {} 45 | } 46 | } 47 | } 48 | -------------------------------------------------------------------------------- /shaders/point.vert: -------------------------------------------------------------------------------- 1 | #version 120 2 | 3 | uniform mat4 worldviewproj_matrix; 4 | uniform mat4 worldview_matrix; 5 | uniform mat4 inverse_transpose_worldview_matrix; 6 | uniform mat4 projection_matrix; 7 | uniform vec4 size; 8 | 9 | uniform float viewport_width; 10 | uniform float viewport_height; 11 | 12 | varying vec3 normal; 13 | varying vec3 position; 14 | varying float sqr_radius; 15 | varying float billboard_half_side; 16 | 17 | #ifdef WITH_DEPTH 18 | //include: 19 | void passDepth( vec4 pos ); 20 | #endif 21 | 22 | void main() 23 | { 24 | gl_Position = worldviewproj_matrix * gl_Vertex; 25 | vec3 proj3 = gl_Position.xyz / gl_Position.w; 26 | normal = mat3(inverse_transpose_worldview_matrix) * gl_Normal; 27 | vec4 pos4 = worldview_matrix * gl_Vertex; 28 | vec3 pos3 = pos4.xyz / pos4.w; 29 | position = pos3; 30 | 31 | // radius 32 | float radius = gl_SecondaryColor.x * size.x; 33 | vec3 pos3_dx = pos3; 34 | pos3_dx.x += radius; 35 | vec4 proj4_dx = projection_matrix * vec4(pos3_dx, 1.0f); 36 | vec3 proj3_dx = proj4_dx.xyz / proj4_dx.w; 37 | float dxmx = length((proj3_dx.xy - proj3.xy) * vec2(viewport_width, viewport_height)); 38 | gl_PointSize = dxmx * 1.2f; // 1.2 > 1/cos(FOV) 39 | billboard_half_side = radius * 1.2f; 40 | sqr_radius = radius * radius; 41 | 42 | gl_FrontColor = gl_Color; 43 | 44 | #ifdef WITH_DEPTH 45 | passDepth( gl_Vertex ); 46 | #endif 47 | } 48 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(surfel_cloud_rviz_plugin) 3 | 4 | add_compile_options(-std=c++11) 5 | 6 | find_package(catkin REQUIRED COMPONENTS 7 | roscpp 8 | rviz 9 | sensor_msgs 10 | roslib 11 | ) 12 | 13 | find_package(PCL REQUIRED) 14 | 15 | set(CMAKE_AUTOMOC ON) 16 | find_package(Qt5 REQUIRED COMPONENTS Core Widgets OpenGL) 17 | add_definitions( ${QT_DEFINITIONS} ) 18 | include_directories( ${CMAKE_BINARY_DIR} ) 19 | 20 | ################################################ 21 | ## Declare ROS messages, services and actions ## 22 | ################################################ 23 | 24 | ################################################ 25 | ## Declare ROS dynamic reconfigure parameters ## 26 | ################################################ 27 | 28 | ################################### 29 | ## catkin specific configuration ## 30 | ################################### 31 | ## The catkin_package macro generates cmake config files for your package 32 | ## Declare things to be passed to dependent projects 33 | ## INCLUDE_DIRS: uncomment this if your package contains header files 34 | ## LIBRARIES: libraries you create in this project that dependent projects also need 35 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 36 | ## DEPENDS: system dependencies of this project that dependent projects also need 37 | catkin_package( 38 | # INCLUDE_DIRS include 39 | # LIBRARIES surfel_cloud_rviz_plugin 40 | # CATKIN_DEPENDS roscpp rviz sensor_msgs 41 | # DEPENDS system_lib 42 | ) 43 | 44 | ########### 45 | ## Build ## 46 | ########### 47 | 48 | include_directories( 49 | # include 50 | ${catkin_INCLUDE_DIRS} 51 | ${PCL_INCLUDE_DIRS} 52 | ) 53 | 54 | add_library(surfel_cloud_rviz_plugin 55 | src/surfel_cloud_display.cpp 56 | src/point_cloud_common.cpp 57 | src/point_cloud.cpp 58 | ) 59 | 60 | add_dependencies(surfel_cloud_rviz_plugin ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 61 | 62 | target_link_libraries(surfel_cloud_rviz_plugin 63 | ${catkin_LIBRARIES} 64 | ${PCL_LIBRARIES} 65 | ) 66 | 67 | ############# 68 | ## Install ## 69 | ############# 70 | 71 | ############# 72 | ## Testing ## 73 | ############# 74 | 75 | -------------------------------------------------------------------------------- /shaders/glsl120.program: -------------------------------------------------------------------------------- 1 | 2 | //includes: 3 | fragment_program surfel_cloud_rviz_plugin/include/circle_impl.frag glsl { source include/circle_impl.frag } 4 | fragment_program surfel_cloud_rviz_plugin/include/pack_depth.frag glsl { source include/pack_depth.frag } 5 | vertex_program surfel_cloud_rviz_plugin/include/pass_depth.vert glsl { source include/pass_depth.vert } 6 | 7 | //all shaders, sorted by name 8 | 9 | 10 | fragment_program surfel_cloud_rviz_plugin/depth_circle.frag glsl 11 | { 12 | source depth_circle.frag 13 | attach surfel_cloud_rviz_plugin/include/pack_depth.frag 14 | attach surfel_cloud_rviz_plugin/include/circle_impl.frag 15 | default_params 16 | { 17 | param_named_auto alpha custom 1 18 | param_named_auto far_clip_distance far_clip_distance 19 | } 20 | } 21 | 22 | fragment_program surfel_cloud_rviz_plugin/flat_color_circle.frag glsl 23 | { 24 | source flat_color_circle.frag 25 | attach surfel_cloud_rviz_plugin/include/circle_impl.frag 26 | default_params 27 | { 28 | param_named_auto highlight custom 5 29 | param_named_auto alpha custom 1 30 | } 31 | } 32 | 33 | fragment_program surfel_cloud_rviz_plugin/pass_color_circle.frag glsl 34 | { 35 | source pass_color_circle.frag 36 | attach surfel_cloud_rviz_plugin/include/circle_impl.frag 37 | } 38 | 39 | fragment_program surfel_cloud_rviz_plugin/pickcolor_circle.frag glsl 40 | { 41 | source pickcolor_circle.frag 42 | attach surfel_cloud_rviz_plugin/include/circle_impl.frag 43 | default_params 44 | { 45 | param_named_auto pick_color custom 2 46 | } 47 | } 48 | 49 | vertex_program surfel_cloud_rviz_plugin/point.vert glsl 50 | { 51 | source point.vert 52 | default_params { 53 | param_named_auto worldviewproj_matrix worldviewproj_matrix 54 | param_named_auto worldview_matrix worldview_matrix 55 | param_named_auto projection_matrix projection_matrix 56 | param_named_auto inverse_transpose_worldview_matrix inverse_transpose_worldview_matrix 57 | param_named_auto viewport_width viewport_width 58 | param_named_auto viewport_height viewport_height 59 | param_named_auto size custom 0 60 | } 61 | } 62 | vertex_program surfel_cloud_rviz_plugin/point.vert(with_depth) glsl 63 | { 64 | source point.vert 65 | preprocessor_defines WITH_DEPTH=1 66 | attach surfel_cloud_rviz_plugin/include/pass_depth.vert 67 | default_params { 68 | param_named_auto worldviewproj_matrix worldviewproj_matrix 69 | param_named_auto worldview_matrix worldview_matrix 70 | param_named_auto projection_matrix projection_matrix 71 | param_named_auto inverse_transpose_worldview_matrix inverse_transpose_worldview_matrix 72 | param_named_auto viewport_width viewport_width 73 | param_named_auto viewport_height viewport_height 74 | param_named_auto size custom 0 75 | } 76 | } 77 | 78 | 79 | -------------------------------------------------------------------------------- /src/surfel_cloud_display.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2012, 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 IMU_DISPLAY_H 31 | #define IMU_DISPLAY_H 32 | 33 | #include 34 | 35 | #include 36 | #include 37 | 38 | namespace Ogre 39 | { 40 | class SceneNode; 41 | } 42 | 43 | namespace rviz 44 | { 45 | class ColorProperty; 46 | class FloatProperty; 47 | class IntProperty; 48 | } 49 | 50 | namespace surfel_cloud_rviz_plugin 51 | { 52 | 53 | class PointCloudCommon; 54 | 55 | class SurfelCloudDisplay: public rviz::MessageFilterDisplay 56 | { 57 | Q_OBJECT 58 | public: 59 | SurfelCloudDisplay(); 60 | ~SurfelCloudDisplay(); 61 | 62 | virtual void reset(); 63 | 64 | virtual void update( float wall_dt, float ros_dt ); 65 | 66 | private Q_SLOTS: 67 | void updateQueueSize(); 68 | 69 | protected: 70 | /** @brief Do initialization. Overridden from MessageFilterDisplay. */ 71 | virtual void onInitialize(); 72 | 73 | /** @brief Process a single message. Overridden from MessageFilterDisplay. */ 74 | virtual void processMessage( const sensor_msgs::PointCloud2ConstPtr& cloud ); 75 | 76 | rviz::IntProperty* queue_size_property_; 77 | 78 | std::shared_ptr point_cloud_common_; 79 | }; 80 | 81 | } 82 | 83 | #endif 84 | -------------------------------------------------------------------------------- /src/surfel_cloud_display.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2012, 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 | #include 32 | 33 | #include 34 | 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | 43 | #include 44 | 45 | #include "surfel_cloud_display.h" 46 | 47 | #include "point_cloud_common.h" 48 | 49 | namespace surfel_cloud_rviz_plugin 50 | { 51 | 52 | SurfelCloudDisplay::SurfelCloudDisplay() 53 | : point_cloud_common_( new PointCloudCommon( this )) 54 | { 55 | queue_size_property_ = new rviz::IntProperty( "Queue Size", 10, 56 | "Advanced: set the size of the incoming PointCloud2 message queue. " 57 | " Increasing this is useful if your incoming TF data is delayed significantly " 58 | "from your PointCloud2 data, but it can greatly increase memory usage if the messages are big.", 59 | this, SLOT( updateQueueSize() )); 60 | 61 | // PointCloudCommon sets up a callback queue with a thread for each 62 | // instance. Use that for processing incoming messages. 63 | update_nh_.setCallbackQueue( point_cloud_common_->getCallbackQueue() ); 64 | } 65 | 66 | void SurfelCloudDisplay::onInitialize() 67 | { 68 | MFDClass::onInitialize(); 69 | point_cloud_common_->initialize( context_, scene_node_ ); 70 | 71 | static bool resource_locations_added = false; 72 | if (!resource_locations_added) 73 | { 74 | const std::string my_path = ros::package::getPath(ROS_PACKAGE_NAME) + "/shaders"; 75 | Ogre::ResourceGroupManager::getSingleton().addResourceLocation(my_path, "FileSystem", ROS_PACKAGE_NAME); 76 | Ogre::ResourceGroupManager::getSingleton().initialiseAllResourceGroups(); 77 | resource_locations_added = true; 78 | } 79 | } 80 | 81 | SurfelCloudDisplay::~SurfelCloudDisplay() 82 | { 83 | } 84 | 85 | void SurfelCloudDisplay::updateQueueSize() 86 | { 87 | tf_filter_->setQueueSize( (uint32_t) queue_size_property_->getInt() ); 88 | } 89 | 90 | void SurfelCloudDisplay::processMessage( const sensor_msgs::PointCloud2ConstPtr& cloud ) 91 | { 92 | // Filter any nan values out of the cloud. Any nan values that make it through to PointCloudBase 93 | // will get their points put off in lala land, but it means they still do get processed/rendered 94 | // which can be a big performance hit 95 | sensor_msgs::PointCloud2Ptr filtered(new sensor_msgs::PointCloud2); 96 | int32_t xi = rviz::findChannelIndex(cloud, "x"); 97 | int32_t yi = rviz::findChannelIndex(cloud, "y"); 98 | int32_t zi = rviz::findChannelIndex(cloud, "z"); 99 | 100 | if (xi == -1 || yi == -1 || zi == -1) 101 | { 102 | return; 103 | } 104 | 105 | const uint32_t xoff = cloud->fields[xi].offset; 106 | const uint32_t yoff = cloud->fields[yi].offset; 107 | const uint32_t zoff = cloud->fields[zi].offset; 108 | const uint32_t point_step = cloud->point_step; 109 | const size_t point_count = cloud->width * cloud->height; 110 | 111 | if( point_count * point_step != cloud->data.size() ) 112 | { 113 | std::stringstream ss; 114 | ss << "Data size (" << cloud->data.size() << " bytes) does not match width (" << cloud->width 115 | << ") times height (" << cloud->height << ") times point_step (" << point_step << "). Dropping message."; 116 | setStatusStd( rviz::StatusProperty::Error, "Message", ss.str() ); 117 | return; 118 | } 119 | 120 | filtered->data.resize(cloud->data.size()); 121 | uint32_t output_count; 122 | if (point_count == 0) 123 | { 124 | output_count = 0; 125 | } else { 126 | uint8_t* output_ptr = &filtered->data.front(); 127 | const uint8_t* ptr = &cloud->data.front(), *ptr_end = &cloud->data.back(), *ptr_init; 128 | size_t points_to_copy = 0; 129 | for (; ptr < ptr_end; ptr += point_step) 130 | { 131 | float x = *reinterpret_cast(ptr + xoff); 132 | float y = *reinterpret_cast(ptr + yoff); 133 | float z = *reinterpret_cast(ptr + zoff); 134 | if (rviz::validateFloats(x) && rviz::validateFloats(y) && rviz::validateFloats(z)) 135 | { 136 | if (points_to_copy == 0) 137 | { 138 | // Only memorize where to start copying from 139 | ptr_init = ptr; 140 | points_to_copy = 1; 141 | } 142 | else 143 | { 144 | ++points_to_copy; 145 | } 146 | } 147 | else 148 | { 149 | if (points_to_copy) 150 | { 151 | // Copy all the points that need to be copied 152 | memcpy(output_ptr, ptr_init, point_step*points_to_copy); 153 | output_ptr += point_step*points_to_copy; 154 | points_to_copy = 0; 155 | } 156 | } 157 | } 158 | // Don't forget to flush what needs to be copied 159 | if (points_to_copy) 160 | { 161 | memcpy(output_ptr, ptr_init, point_step*points_to_copy); 162 | output_ptr += point_step*points_to_copy; 163 | } 164 | output_count = (output_ptr - &filtered->data.front()) / point_step; 165 | } 166 | 167 | filtered->header = cloud->header; 168 | filtered->fields = cloud->fields; 169 | filtered->data.resize(output_count * point_step); 170 | filtered->height = 1; 171 | filtered->width = output_count; 172 | filtered->is_bigendian = cloud->is_bigendian; 173 | filtered->point_step = point_step; 174 | filtered->row_step = output_count; 175 | 176 | point_cloud_common_->addMessage( filtered ); 177 | } 178 | 179 | 180 | void SurfelCloudDisplay::update( float wall_dt, float ros_dt ) 181 | { 182 | point_cloud_common_->update( wall_dt, ros_dt ); 183 | } 184 | 185 | void SurfelCloudDisplay::reset() 186 | { 187 | MFDClass::reset(); 188 | point_cloud_common_->reset(); 189 | } 190 | 191 | } 192 | 193 | #include 194 | PLUGINLIB_EXPORT_CLASS(surfel_cloud_rviz_plugin::SurfelCloudDisplay,rviz::Display ) 195 | -------------------------------------------------------------------------------- /src/point_cloud_common.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2012, 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 RVIZ_SURFEL_POINT_CLOUD_COMMON_H 31 | #define RVIZ_SURFEL_POINT_CLOUD_COMMON_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 | # include 47 | 48 | # include 49 | 50 | # include 51 | 52 | # include 53 | # include 54 | 55 | # include 56 | # include 57 | # include 58 | # include 59 | 60 | #include "point_cloud.h" 61 | #endif 62 | 63 | namespace rviz 64 | { 65 | class BoolProperty; 66 | class Display; 67 | class DisplayContext; 68 | class EnumProperty; 69 | class FloatProperty; 70 | class PointCloudTransformer; 71 | } 72 | 73 | namespace surfel_cloud_rviz_plugin 74 | { 75 | class PointCloudSelectionHandler; 76 | struct IndexAndMessage; 77 | typedef boost::shared_ptr PointCloudSelectionHandlerPtr; 78 | typedef boost::shared_ptr PointCloudTransformerPtr; 79 | typedef std::vector V_string; 80 | 81 | /** 82 | * \class PointCloudCommon 83 | * \brief Displays a point cloud of type sensor_msgs::PointCloud 84 | * 85 | * By default it will assume channel 0 of the cloud is an intensity value, and will color them by intensity. 86 | * If you set the channel's name to "rgb", it will interpret the channel as an integer rgb value, with r, g and b 87 | * all being 8 bits. 88 | */ 89 | class PointCloudCommon: public QObject 90 | { 91 | Q_OBJECT 92 | public: 93 | struct CloudInfo 94 | { 95 | CloudInfo(); 96 | ~CloudInfo(); 97 | 98 | // clear the point cloud, but keep selection handler around 99 | void clear(); 100 | 101 | ros::Time receive_time_; 102 | 103 | Ogre::SceneManager *manager_; 104 | 105 | sensor_msgs::PointCloud2ConstPtr message_; 106 | 107 | Ogre::SceneNode *scene_node_; 108 | boost::shared_ptr cloud_; 109 | PointCloudSelectionHandlerPtr selection_handler_; 110 | 111 | std::vector transformed_points_; 112 | 113 | Ogre::Quaternion orientation_; 114 | Ogre::Vector3 position_; 115 | }; 116 | 117 | typedef boost::shared_ptr CloudInfoPtr; 118 | typedef std::deque D_CloudInfo; 119 | typedef std::vector V_CloudInfo; 120 | typedef std::queue Q_CloudInfo; 121 | typedef std::list L_CloudInfo; 122 | 123 | PointCloudCommon( rviz::Display* display ); 124 | ~PointCloudCommon(); 125 | 126 | void initialize( rviz::DisplayContext* context, Ogre::SceneNode* scene_node ); 127 | 128 | void fixedFrameChanged(); 129 | void reset(); 130 | void update(float wall_dt, float ros_dt); 131 | 132 | void addMessage(const sensor_msgs::PointCloudConstPtr& cloud); 133 | void addMessage(const sensor_msgs::PointCloud2ConstPtr& cloud); 134 | 135 | ros::CallbackQueueInterface* getCallbackQueue() { return &cbqueue_; } 136 | 137 | rviz::Display* getDisplay() { return display_; } 138 | 139 | bool auto_size_; 140 | 141 | rviz::BoolProperty* selectable_property_; 142 | rviz::FloatProperty* point_world_size_property_; 143 | rviz::FloatProperty* point_pixel_size_property_; 144 | rviz::FloatProperty* point_scale_size_property_; 145 | rviz::FloatProperty* alpha_property_; 146 | rviz::EnumProperty* xyz_transformer_property_; 147 | rviz::EnumProperty* color_transformer_property_; 148 | rviz::EnumProperty* style_property_; 149 | rviz::FloatProperty* decay_time_property_; 150 | 151 | void setAutoSize( bool auto_size ); 152 | 153 | public Q_SLOTS: 154 | void causeRetransform(); 155 | 156 | private Q_SLOTS: 157 | void updateSelectable(); 158 | void updateStyle(); 159 | void updateBillboardSize(); 160 | void updateAlpha(); 161 | void updateXyzTransformer(); 162 | void updateColorTransformer(); 163 | void setXyzTransformerOptions( rviz::EnumProperty* prop ); 164 | void setColorTransformerOptions( rviz::EnumProperty* prop ); 165 | 166 | private: 167 | 168 | /** 169 | * \brief Transforms the cloud into the correct frame, and sets up our renderable cloud 170 | */ 171 | bool transformCloud(const CloudInfoPtr& cloud, bool fully_update_transformers); 172 | 173 | void processMessage(const sensor_msgs::PointCloud2ConstPtr& cloud); 174 | void updateStatus(); 175 | 176 | PointCloudTransformerPtr getXYZTransformer(const sensor_msgs::PointCloud2ConstPtr& cloud); 177 | PointCloudTransformerPtr getColorTransformer(const sensor_msgs::PointCloud2ConstPtr& cloud); 178 | void updateTransformers( const sensor_msgs::PointCloud2ConstPtr& cloud ); 179 | void retransform(); 180 | void onTransformerOptions(V_string& ops, uint32_t mask); 181 | 182 | void loadTransformers(); 183 | 184 | float getSelectionBoxSize(); 185 | void setPropertiesHidden( const QList& props, bool hide ); 186 | void fillTransformerOptions( rviz::EnumProperty* prop, uint32_t mask ); 187 | 188 | ros::AsyncSpinner spinner_; 189 | ros::CallbackQueue cbqueue_; 190 | 191 | D_CloudInfo cloud_infos_; 192 | 193 | Ogre::SceneNode* scene_node_; 194 | 195 | V_CloudInfo new_cloud_infos_; 196 | boost::mutex new_clouds_mutex_; 197 | 198 | L_CloudInfo obsolete_cloud_infos_; 199 | 200 | struct TransformerInfo 201 | { 202 | PointCloudTransformerPtr transformer; 203 | QList xyz_props; 204 | QList color_props; 205 | 206 | std::string readable_name; 207 | std::string lookup_name; 208 | }; 209 | typedef std::map M_TransformerInfo; 210 | 211 | boost::recursive_mutex transformers_mutex_; 212 | M_TransformerInfo transformers_; 213 | bool new_xyz_transformer_; 214 | bool new_color_transformer_; 215 | bool needs_retransform_; 216 | 217 | pluginlib::ClassLoader* transformer_class_loader_; 218 | 219 | rviz::Display* display_; 220 | rviz::DisplayContext* context_; 221 | 222 | friend class PointCloudSelectionHandler; 223 | }; 224 | 225 | class PointCloudSelectionHandler: public rviz::SelectionHandler 226 | { 227 | public: 228 | PointCloudSelectionHandler( float box_size, PointCloudCommon::CloudInfo* cloud_info, rviz::DisplayContext* context ); 229 | virtual ~PointCloudSelectionHandler(); 230 | 231 | virtual void createProperties( const rviz::Picked& obj, rviz::Property* parent_property ); 232 | virtual void destroyProperties( const rviz::Picked& obj, rviz::Property* parent_property ); 233 | 234 | virtual bool needsAdditionalRenderPass(uint32_t pass) 235 | { 236 | if (pass < 2) 237 | { 238 | return true; 239 | } 240 | 241 | return false; 242 | } 243 | 244 | virtual void preRenderPass(uint32_t pass); 245 | virtual void postRenderPass(uint32_t pass); 246 | 247 | virtual void onSelect(const rviz::Picked& obj); 248 | virtual void onDeselect(const rviz::Picked& obj); 249 | 250 | virtual void getAABBs(const rviz::Picked& obj, rviz::V_AABB& aabbs); 251 | 252 | void setBoxSize( float size ) { box_size_=size; } 253 | 254 | bool hasSelections() { return !boxes_.empty(); } 255 | 256 | private: 257 | PointCloudCommon::CloudInfo* cloud_info_; 258 | QHash property_hash_; 259 | float box_size_; 260 | }; 261 | 262 | } // namespace rviz 263 | 264 | #endif // RVIZ_POINT_CLOUD_COMMON_H 265 | -------------------------------------------------------------------------------- /src/point_cloud.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_OGRE_SURFEL_POINT_CLOUD_H 31 | #define OGRE_TOOLS_OGRE_SURFEL_POINT_CLOUD_H 32 | 33 | #ifndef _WIN32 34 | # pragma GCC diagnostic push 35 | # pragma GCC diagnostic ignored "-Wpedantic" 36 | # ifdef __clang__ 37 | # pragma clang diagnostic ignored "-Wdeprecated-register" 38 | # endif 39 | # pragma GCC diagnostic ignored "-Woverloaded-virtual" 40 | # pragma GCC diagnostic ignored "-Wunused-parameter" 41 | #endif 42 | 43 | #include 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | #include 50 | #include 51 | #include 52 | #include 53 | 54 | #ifndef _WIN32 55 | # pragma GCC diagnostic pop 56 | #endif 57 | 58 | #include 59 | 60 | #include 61 | 62 | #include 63 | 64 | namespace Ogre 65 | { 66 | class SceneManager; 67 | class ManualObject; 68 | class SceneNode; 69 | class RenderQueue; 70 | class Camera; 71 | class RenderSystem; 72 | class Matrix4; 73 | } 74 | 75 | namespace surfel_cloud_rviz_plugin 76 | { 77 | 78 | class PointCloud; 79 | class PointCloudRenderable : public Ogre::SimpleRenderable 80 | { 81 | public: 82 | PointCloudRenderable(PointCloud* parent, int num_points, bool use_tex_coords, bool use_normals, bool use_radius); 83 | ~PointCloudRenderable(); 84 | 85 | #ifndef _WIN32 86 | # pragma GCC diagnostic push 87 | # pragma GCC diagnostic ignored "-Woverloaded-virtual" 88 | #endif 89 | 90 | Ogre::RenderOperation* getRenderOperation() { return &mRenderOp; } 91 | 92 | #ifndef _WIN32 93 | # pragma GCC diagnostic pop 94 | #endif 95 | 96 | Ogre::HardwareVertexBufferSharedPtr getBuffer(); 97 | 98 | virtual Ogre::Real getBoundingRadius(void) const; 99 | virtual Ogre::Real getSquaredViewDepth(const Ogre::Camera* cam) const; 100 | virtual void _notifyCurrentCamera(Ogre::Camera* camera); 101 | virtual unsigned short getNumWorldTransforms() const { return 1; } 102 | virtual void getWorldTransforms(Ogre::Matrix4* xform) const; 103 | virtual const Ogre::LightList& getLights() const; 104 | 105 | private: 106 | Ogre::MaterialPtr material_; 107 | PointCloud* parent_; 108 | }; 109 | typedef boost::shared_ptr PointCloudRenderablePtr; 110 | typedef std::vector V_PointCloudRenderable; 111 | 112 | /** 113 | * \class PointCloud 114 | * \brief A visual representation of a set of points. 115 | * 116 | * Displays a set of points using any number of Ogre BillboardSets. PointCloud is optimized for sets of points that change 117 | * rapidly, rather than for large clouds that never change. 118 | * 119 | * Most of the functions in PointCloud are not safe to call from any thread but the render thread. Exceptions are clear() and addPoints(), which 120 | * are safe as long as we are not in the middle of a render (ie. Ogre::Root::renderOneFrame, or Ogre::RenderWindow::update) 121 | */ 122 | class PointCloud : public Ogre::MovableObject 123 | { 124 | public: 125 | enum RenderMode 126 | { 127 | RM_POINTS, 128 | RM_SQUARES, 129 | RM_FLAT_SQUARES, 130 | RM_SPHERES, 131 | RM_TILES, 132 | RM_BOXES, 133 | RM_SURFELS, 134 | }; 135 | 136 | PointCloud(); 137 | ~PointCloud(); 138 | 139 | /** 140 | * \brief Clear all the points 141 | */ 142 | void clear(); 143 | 144 | /** 145 | * \struct Point 146 | * \brief Representation of a point, with x/y/z position and r/g/b color 147 | */ 148 | struct Point 149 | { 150 | inline void setColor(float r, float g, float b, float a=1.0) 151 | { 152 | color=Ogre::ColourValue(r, g, b, a); 153 | } 154 | 155 | Ogre::Vector3 position; 156 | Ogre::ColourValue color; 157 | Ogre::Vector3 normal; 158 | float radius; 159 | }; 160 | 161 | /** 162 | * \brief Add points to this point cloud 163 | * 164 | * @param points An array of Point structures 165 | * @param num_points The number of points in the array 166 | */ 167 | void addPoints( Point* points, uint32_t num_points ); 168 | 169 | /** 170 | * \brief Remove a number of points from this point cloud 171 | * \param num_points The number of points to pop 172 | */ 173 | void popPoints( uint32_t num_points ); 174 | 175 | /** 176 | * \brief Set what type of rendering primitives should be used, currently points, billboards and boxes are supported 177 | */ 178 | void setRenderMode(RenderMode mode); 179 | /** 180 | * \brief Set the dimensions of the billboards used to render each point 181 | * @param width Width 182 | * @param height Height 183 | * @note width/height are only applicable to billboards and boxes, depth is only applicable to boxes 184 | */ 185 | void setDimensions( float width, float height, float depth ); 186 | 187 | /* 188 | * If set to true, the size of each point will be multiplied by it z component. 189 | * (Used for depth image based point clouds) 190 | */ 191 | void setAutoSize(bool auto_size); 192 | 193 | /// See Ogre::BillboardSet::setCommonDirection 194 | void setCommonDirection( const Ogre::Vector3& vec ); 195 | /// See Ogre::BillboardSet::setCommonUpVector 196 | void setCommonUpVector( const Ogre::Vector3& vec ); 197 | 198 | /// set alpha blending 199 | /// @param alpha global alpha value 200 | /// @param per_point_alpha indicates that each point will have an individual alpha value. 201 | /// if true, enables alpha blending regardless of the global alpha. 202 | void setAlpha( float alpha, bool per_point_alpha = false ); 203 | 204 | void setPickColor(const Ogre::ColourValue& color); 205 | void setColorByIndex(bool set); 206 | 207 | void setHighlightColor( float r, float g, float b ); 208 | 209 | virtual const Ogre::String& getMovableType() const { return sm_Type; } 210 | virtual const Ogre::AxisAlignedBox& getBoundingBox() const; 211 | virtual float getBoundingRadius() const; 212 | virtual void getWorldTransforms( Ogre::Matrix4* xform ) const; 213 | virtual unsigned short getNumWorldTransforms() const { return 1; } 214 | virtual void _updateRenderQueue( Ogre::RenderQueue* queue ); 215 | virtual void _notifyCurrentCamera( Ogre::Camera* camera ); 216 | virtual void _notifyAttached(Ogre::Node *parent, bool isTagPoint=false); 217 | #if (OGRE_VERSION_MAJOR >= 1 && OGRE_VERSION_MINOR >= 6) 218 | virtual void visitRenderables(Ogre::Renderable::Visitor* visitor, bool debugRenderables); 219 | #endif 220 | 221 | virtual void setName ( const std::string& name ) { mName = name; } 222 | 223 | private: 224 | 225 | uint32_t getVerticesPerPoint(); 226 | PointCloudRenderablePtr createRenderable( int num_points ); 227 | void regenerateAll(); 228 | void shrinkRenderables(); 229 | 230 | Ogre::AxisAlignedBox bounding_box_; ///< The bounding box of this point cloud 231 | float bounding_radius_; ///< The bounding radius of this point cloud 232 | 233 | typedef std::vector V_Point; 234 | V_Point points_; ///< The list of points we're displaying. Allocates to a high-water-mark. 235 | uint32_t point_count_; ///< The number of points currently in #points_ 236 | 237 | RenderMode render_mode_; 238 | float width_; ///< width 239 | float height_; ///< height 240 | float depth_; ///< depth 241 | Ogre::Vector3 common_direction_; ///< See Ogre::BillboardSet::setCommonDirection 242 | Ogre::Vector3 common_up_vector_; ///< See Ogre::BillboardSet::setCommonUpVector 243 | 244 | Ogre::MaterialPtr point_material_; 245 | Ogre::MaterialPtr square_material_; 246 | Ogre::MaterialPtr flat_square_material_; 247 | Ogre::MaterialPtr sphere_material_; 248 | Ogre::MaterialPtr tile_material_; 249 | Ogre::MaterialPtr box_material_; 250 | Ogre::MaterialPtr surfels_material_; 251 | Ogre::MaterialPtr current_material_; 252 | float alpha_; 253 | 254 | bool color_by_index_; 255 | 256 | V_PointCloudRenderable renderables_; 257 | 258 | bool current_mode_supports_geometry_shader_; 259 | bool current_mode_has_radius_; 260 | bool current_mode_has_normal_; 261 | Ogre::ColourValue pick_color_; 262 | 263 | static Ogre::String sm_Type; ///< The "renderable type" used by Ogre 264 | }; 265 | 266 | } 267 | 268 | #endif 269 | -------------------------------------------------------------------------------- /src/point_cloud.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 "point_cloud.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 | #include 44 | #include 45 | #include 46 | 47 | #include 48 | 49 | #include 50 | #include 51 | 52 | #define VERTEX_BUFFER_CAPACITY (36 * 1024 * 10) 53 | 54 | namespace surfel_cloud_rviz_plugin 55 | { 56 | 57 | static float g_point_vertices[3] = 58 | { 59 | 0.0f, 0.0f, 0.0f 60 | }; 61 | 62 | static float g_billboard_vertices[6*3] = 63 | { 64 | -0.5f, 0.5f, 0.0f, 65 | -0.5f, -0.5f, 0.0f, 66 | 0.5f, 0.5f, 0.0f, 67 | 0.5f, 0.5f, 0.0f, 68 | -0.5f, -0.5f, 0.0f, 69 | 0.5f, -0.5f, 0.0f, 70 | }; 71 | 72 | static float g_billboard_sphere_vertices[3*3] = 73 | { 74 | 0.0f, 1.0f, 0.0f, 75 | -0.866025404f, -0.5f, 0.0f, 76 | 0.866025404f, -0.5f, 0.0f, 77 | }; 78 | 79 | static float g_box_vertices[6*6*3] = 80 | { 81 | // front 82 | -0.5f, 0.5f, -0.5f, 83 | -0.5f, -0.5f, -0.5f, 84 | 0.5f, 0.5f, -0.5f, 85 | 0.5f, 0.5f, -0.5f, 86 | -0.5f, -0.5f, -0.5f, 87 | 0.5f, -0.5f, -0.5f, 88 | 89 | // back 90 | -0.5f, 0.5f, 0.5f, 91 | 0.5f, 0.5f, 0.5f, 92 | -0.5f, -0.5f, 0.5f, 93 | 0.5f, 0.5f, 0.5f, 94 | 0.5f, -0.5f, 0.5f, 95 | -0.5f, -0.5f, 0.5f, 96 | 97 | // right 98 | 0.5, 0.5, 0.5, 99 | 0.5, 0.5, -0.5, 100 | 0.5, -0.5, 0.5, 101 | 0.5, 0.5, -0.5, 102 | 0.5, -0.5, -0.5, 103 | 0.5, -0.5, 0.5, 104 | 105 | // left 106 | -0.5, 0.5, 0.5, 107 | -0.5, -0.5, 0.5, 108 | -0.5, 0.5, -0.5, 109 | -0.5, 0.5, -0.5, 110 | -0.5, -0.5, 0.5, 111 | -0.5, -0.5, -0.5, 112 | 113 | // top 114 | -0.5, 0.5, -0.5, 115 | 0.5, 0.5, -0.5, 116 | -0.5, 0.5, 0.5, 117 | 0.5, 0.5, -0.5, 118 | 0.5, 0.5, 0.5, 119 | -0.5, 0.5, 0.5, 120 | 121 | // bottom 122 | -0.5, -0.5, -0.5, 123 | -0.5, -0.5, 0.5, 124 | 0.5, -0.5, -0.5, 125 | 0.5, -0.5, -0.5, 126 | -0.5, -0.5, 0.5, 127 | 0.5, -0.5, 0.5, 128 | }; 129 | 130 | Ogre::String PointCloud::sm_Type = "PointCloud"; 131 | 132 | PointCloud::PointCloud() 133 | : bounding_radius_( 0.0f ) 134 | , point_count_( 0 ) 135 | , common_direction_( Ogre::Vector3::NEGATIVE_UNIT_Z ) 136 | , common_up_vector_( Ogre::Vector3::UNIT_Y ) 137 | , color_by_index_(false) 138 | , current_mode_supports_geometry_shader_(false) 139 | , current_mode_has_radius_(false) 140 | , current_mode_has_normal_(false) 141 | { 142 | std::stringstream ss; 143 | static int count = 0; 144 | ss << "SurfelCloudMaterial" << count++; 145 | point_material_ = Ogre::MaterialManager::getSingleton().getByName("rviz/PointCloudPoint"); 146 | square_material_ = Ogre::MaterialManager::getSingleton().getByName("rviz/PointCloudSquare"); 147 | flat_square_material_ = Ogre::MaterialManager::getSingleton().getByName("rviz/PointCloudFlatSquare"); 148 | sphere_material_ = Ogre::MaterialManager::getSingleton().getByName("rviz/PointCloudSphere"); 149 | tile_material_ = Ogre::MaterialManager::getSingleton().getByName("rviz/PointCloudTile"); 150 | box_material_ = Ogre::MaterialManager::getSingleton().getByName("rviz/PointCloudBox"); 151 | surfels_material_ = Ogre::MaterialManager::getSingleton().getByName("surfel_cloud_rviz_plugin/SurfelPointCloud"); 152 | 153 | point_material_ = Ogre::MaterialPtr(point_material_)->clone(ss.str() + "Point"); 154 | square_material_ = Ogre::MaterialPtr(square_material_)->clone(ss.str() + "Square"); 155 | flat_square_material_ = Ogre::MaterialPtr(flat_square_material_)->clone(ss.str() + "FlatSquare"); 156 | sphere_material_ = Ogre::MaterialPtr(sphere_material_)->clone(ss.str() + "Sphere"); 157 | tile_material_ = Ogre::MaterialPtr(tile_material_)->clone(ss.str() + "Tiles"); 158 | box_material_ = Ogre::MaterialPtr(box_material_)->clone(ss.str() + "Box"); 159 | surfels_material_ = Ogre::MaterialPtr(surfels_material_)->clone(ss.str() + "Surfels"); 160 | 161 | point_material_->load(); 162 | square_material_->load(); 163 | flat_square_material_->load(); 164 | sphere_material_->load(); 165 | tile_material_->load(); 166 | box_material_->load(); 167 | surfels_material_->load(); 168 | 169 | setAlpha( 1.0f ); 170 | setRenderMode(RM_SPHERES); 171 | setDimensions(0.01f, 0.01f, 0.01f); 172 | 173 | clear(); 174 | } 175 | 176 | static void removeMaterial(Ogre::MaterialPtr& material) 177 | { 178 | Ogre::ResourcePtr resource(material); 179 | Ogre::MaterialManager::getSingleton().remove(resource); 180 | } 181 | 182 | PointCloud::~PointCloud() 183 | { 184 | clear(); 185 | 186 | point_material_->unload(); 187 | square_material_->unload(); 188 | flat_square_material_->unload(); 189 | sphere_material_->unload(); 190 | tile_material_->unload(); 191 | box_material_->unload(); 192 | surfels_material_->unload(); 193 | 194 | removeMaterial(point_material_); 195 | removeMaterial(square_material_); 196 | removeMaterial(flat_square_material_); 197 | removeMaterial(sphere_material_); 198 | removeMaterial(tile_material_); 199 | removeMaterial(box_material_); 200 | removeMaterial(surfels_material_); 201 | } 202 | 203 | const Ogre::AxisAlignedBox& PointCloud::getBoundingBox() const 204 | { 205 | return bounding_box_; 206 | } 207 | 208 | float PointCloud::getBoundingRadius() const 209 | { 210 | return bounding_radius_; 211 | } 212 | 213 | void PointCloud::getWorldTransforms(Ogre::Matrix4* xform) const 214 | { 215 | *xform = _getParentNodeFullTransform(); 216 | } 217 | 218 | void PointCloud::clear() 219 | { 220 | point_count_ = 0; 221 | bounding_box_.setNull(); 222 | bounding_radius_ = 0.0f; 223 | 224 | if (getParentSceneNode()) 225 | { 226 | V_PointCloudRenderable::iterator it = renderables_.begin(); 227 | V_PointCloudRenderable::iterator end = renderables_.end(); 228 | for (; it != end; ++it) 229 | { 230 | getParentSceneNode()->detachObject(it->get()); 231 | } 232 | getParentSceneNode()->needUpdate(); 233 | } 234 | 235 | renderables_.clear(); 236 | } 237 | 238 | void PointCloud::regenerateAll() 239 | { 240 | if (point_count_ == 0) 241 | { 242 | return; 243 | } 244 | 245 | V_Point points; 246 | points.swap(points_); 247 | uint32_t count = point_count_; 248 | 249 | clear(); 250 | 251 | addPoints(&points.front(), count); 252 | } 253 | 254 | void PointCloud::setColorByIndex(bool set) 255 | { 256 | color_by_index_ = set; 257 | regenerateAll(); 258 | } 259 | 260 | void PointCloud::setHighlightColor( float r, float g, float b ) 261 | { 262 | Ogre::Vector4 highlight( r, g, b, 0.0f ); 263 | 264 | V_PointCloudRenderable::iterator it = renderables_.begin(); 265 | V_PointCloudRenderable::iterator end = renderables_.end(); 266 | for (; it != end; ++it) 267 | { 268 | (*it)->setCustomParameter(HIGHLIGHT_PARAMETER, highlight); 269 | } 270 | } 271 | 272 | void PointCloud::setRenderMode(RenderMode mode) 273 | { 274 | render_mode_ = mode; 275 | 276 | current_mode_has_radius_ = false; 277 | current_mode_has_normal_ = false; 278 | 279 | if (mode == RM_POINTS) 280 | { 281 | current_material_ = Ogre::MaterialPtr(point_material_); 282 | } 283 | else if (mode == RM_SQUARES) 284 | { 285 | current_material_ = Ogre::MaterialPtr(square_material_); 286 | } 287 | else if (mode == RM_FLAT_SQUARES) 288 | { 289 | current_material_ = Ogre::MaterialPtr(flat_square_material_); 290 | } 291 | else if (mode == RM_SPHERES) 292 | { 293 | current_material_ = Ogre::MaterialPtr(sphere_material_); 294 | } 295 | else if (mode == RM_TILES) 296 | { 297 | current_material_ = Ogre::MaterialPtr(tile_material_); 298 | } 299 | else if (mode == RM_BOXES) 300 | { 301 | current_material_ = Ogre::MaterialPtr(box_material_); 302 | } 303 | else if (mode == RM_SURFELS) 304 | { 305 | current_material_ = Ogre::MaterialPtr(surfels_material_); 306 | current_mode_has_radius_ = true; 307 | current_mode_has_normal_ = true; 308 | } 309 | 310 | current_material_->load(); 311 | 312 | //ROS_INFO("Best technique [%s] [gp=%s]", current_material_->getBestTechnique()->getName().c_str(), current_material_->getBestTechnique()->getPass(0)->getGeometryProgramName().c_str()); 313 | 314 | bool geom_support_changed = false; 315 | Ogre::Technique* best = current_material_->getBestTechnique(); 316 | if (best) 317 | { 318 | if (current_material_->getBestTechnique()->getName() == "gp") 319 | { 320 | if (!current_mode_supports_geometry_shader_) 321 | { 322 | geom_support_changed = true; 323 | } 324 | 325 | current_mode_supports_geometry_shader_ = true; 326 | 327 | //ROS_INFO("Using geometry shader"); 328 | } 329 | else 330 | { 331 | if (current_mode_supports_geometry_shader_) 332 | { 333 | geom_support_changed = true; 334 | } 335 | 336 | current_mode_supports_geometry_shader_ = false; 337 | } 338 | } 339 | else 340 | { 341 | geom_support_changed = true; 342 | current_mode_supports_geometry_shader_ = false; 343 | 344 | ROS_ERROR("No techniques available for material [%s]", current_material_->getName().c_str()); 345 | } 346 | 347 | if (geom_support_changed) 348 | { 349 | renderables_.clear(); 350 | } 351 | 352 | V_PointCloudRenderable::iterator it = renderables_.begin(); 353 | V_PointCloudRenderable::iterator end = renderables_.end(); 354 | for (; it != end; ++it) 355 | { 356 | (*it)->setMaterial(current_material_->getName()); 357 | } 358 | 359 | regenerateAll(); 360 | } 361 | 362 | void PointCloud::setDimensions(float width, float height, float depth) 363 | { 364 | width_ = width; 365 | height_ = height; 366 | depth_ = depth; 367 | 368 | Ogre::Vector4 size(width_, height_, depth_, 0.0f); 369 | 370 | V_PointCloudRenderable::iterator it = renderables_.begin(); 371 | V_PointCloudRenderable::iterator end = renderables_.end(); 372 | for (; it != end; ++it) 373 | { 374 | (*it)->setCustomParameter(SIZE_PARAMETER, size); 375 | } 376 | } 377 | 378 | void PointCloud::setAutoSize(bool auto_size) 379 | { 380 | V_PointCloudRenderable::iterator it = renderables_.begin(); 381 | V_PointCloudRenderable::iterator end = renderables_.end(); 382 | for (; it != end; ++it) 383 | { 384 | (*it)->setCustomParameter(AUTO_SIZE_PARAMETER, Ogre::Vector4(auto_size)); 385 | } 386 | } 387 | 388 | void PointCloud::setCommonDirection(const Ogre::Vector3& vec) 389 | { 390 | common_direction_ = vec; 391 | 392 | V_PointCloudRenderable::iterator it = renderables_.begin(); 393 | V_PointCloudRenderable::iterator end = renderables_.end(); 394 | for (; it != end; ++it) 395 | { 396 | (*it)->setCustomParameter(NORMAL_PARAMETER, Ogre::Vector4(vec)); 397 | } 398 | } 399 | 400 | void PointCloud::setCommonUpVector(const Ogre::Vector3& vec) 401 | { 402 | common_up_vector_ = vec; 403 | 404 | V_PointCloudRenderable::iterator it = renderables_.begin(); 405 | V_PointCloudRenderable::iterator end = renderables_.end(); 406 | for (; it != end; ++it) 407 | { 408 | (*it)->setCustomParameter(UP_PARAMETER, Ogre::Vector4(vec)); 409 | } 410 | } 411 | 412 | void setAlphaBlending(const Ogre::MaterialPtr& mat) 413 | { 414 | if (mat->getBestTechnique()) 415 | { 416 | mat->getBestTechnique()->setSceneBlending( Ogre::SBT_TRANSPARENT_ALPHA ); 417 | mat->getBestTechnique()->setDepthWriteEnabled( true ); 418 | } 419 | } 420 | 421 | void setReplace(const Ogre::MaterialPtr& mat) 422 | { 423 | if (mat->getBestTechnique()) 424 | { 425 | mat->getBestTechnique()->setSceneBlending( Ogre::SBT_REPLACE ); 426 | mat->getBestTechnique()->setDepthWriteEnabled( true ); 427 | } 428 | } 429 | 430 | void PointCloud::setAlpha(float alpha, bool per_point_alpha) 431 | { 432 | alpha_ = alpha; 433 | 434 | if ( alpha < 0.9998 || per_point_alpha ) 435 | { 436 | setAlphaBlending(point_material_); 437 | setAlphaBlending(square_material_); 438 | setAlphaBlending(flat_square_material_); 439 | setAlphaBlending(sphere_material_); 440 | setAlphaBlending(tile_material_); 441 | setAlphaBlending(box_material_); 442 | setAlphaBlending(surfels_material_); 443 | } 444 | else 445 | { 446 | setReplace(point_material_); 447 | setReplace(square_material_); 448 | setReplace(flat_square_material_); 449 | setReplace(sphere_material_); 450 | setReplace(tile_material_); 451 | setReplace(box_material_); 452 | setReplace(surfels_material_); 453 | } 454 | 455 | Ogre::Vector4 alpha4(alpha_, alpha_, alpha_, alpha_); 456 | V_PointCloudRenderable::iterator it = renderables_.begin(); 457 | V_PointCloudRenderable::iterator end = renderables_.end(); 458 | for (; it != end; ++it) 459 | { 460 | (*it)->setCustomParameter(ALPHA_PARAMETER, alpha4); 461 | } 462 | } 463 | 464 | void PointCloud::addPoints(Point* points, uint32_t num_points) 465 | { 466 | if (num_points == 0) 467 | { 468 | return; 469 | } 470 | Ogre::Root* root = Ogre::Root::getSingletonPtr(); 471 | 472 | if ( points_.size() < point_count_ + num_points ) 473 | { 474 | points_.resize( point_count_ + num_points ); 475 | } 476 | 477 | Point* begin = &points_.front() + point_count_; 478 | memcpy( begin, points, sizeof( Point ) * num_points ); 479 | 480 | uint32_t vpp = getVerticesPerPoint(); 481 | Ogre::RenderOperation::OperationType op_type; 482 | if (current_mode_supports_geometry_shader_) 483 | { 484 | op_type = Ogre::RenderOperation::OT_POINT_LIST; 485 | } 486 | else 487 | { 488 | if (render_mode_ == RM_POINTS || render_mode_ == RM_SURFELS) 489 | { 490 | op_type = Ogre::RenderOperation::OT_POINT_LIST; 491 | } 492 | else 493 | { 494 | op_type = Ogre::RenderOperation::OT_TRIANGLE_LIST; 495 | } 496 | } 497 | 498 | float* vertices = 0; 499 | if (current_mode_supports_geometry_shader_) 500 | { 501 | vertices = g_point_vertices; 502 | } 503 | else 504 | { 505 | if (render_mode_ == RM_POINTS) 506 | { 507 | vertices = g_point_vertices; 508 | } 509 | else if (render_mode_ == RM_SQUARES) 510 | { 511 | vertices = g_billboard_vertices; 512 | } 513 | else if (render_mode_ == RM_FLAT_SQUARES) 514 | { 515 | vertices = g_billboard_vertices; 516 | } 517 | else if (render_mode_ == RM_SPHERES) 518 | { 519 | vertices = g_billboard_sphere_vertices; 520 | } 521 | else if (render_mode_ == RM_TILES) 522 | { 523 | vertices = g_billboard_vertices; 524 | } 525 | else if (render_mode_ == RM_BOXES) 526 | { 527 | vertices = g_box_vertices; 528 | } 529 | else if (render_mode_ == RM_SURFELS) 530 | { 531 | vertices = g_point_vertices; 532 | } 533 | } 534 | 535 | PointCloudRenderablePtr rend; 536 | Ogre::HardwareVertexBufferSharedPtr vbuf; 537 | void* vdata = 0; 538 | Ogre::RenderOperation* op = 0; 539 | float* fptr = 0; 540 | 541 | Ogre::AxisAlignedBox aabb; 542 | aabb.setNull(); 543 | uint32_t current_vertex_count = 0; 544 | bounding_radius_ = 0.0f; 545 | uint32_t vertex_size = 0; 546 | uint32_t buffer_size = 0; 547 | for (uint32_t current_point = 0; current_point < num_points; ++current_point) 548 | { 549 | // if we didn't create a renderable yet, 550 | // or we've reached the vertex limit for the current renderable, 551 | // create a new one. 552 | while (!rend || current_vertex_count >= buffer_size) 553 | { 554 | if (rend) 555 | { 556 | ROS_ASSERT(current_vertex_count == buffer_size); 557 | 558 | op->vertexData->vertexCount = rend->getBuffer()->getNumVertices() - op->vertexData->vertexStart; 559 | ROS_ASSERT(op->vertexData->vertexCount + op->vertexData->vertexStart <= rend->getBuffer()->getNumVertices()); 560 | vbuf->unlock(); 561 | rend->setBoundingBox(aabb); 562 | bounding_box_.merge(aabb); 563 | } 564 | 565 | buffer_size = std::min( VERTEX_BUFFER_CAPACITY, (num_points - current_point)*vpp ); 566 | 567 | rend = createRenderable( buffer_size ); 568 | vbuf = rend->getBuffer(); 569 | vdata = vbuf->lock(Ogre::HardwareBuffer::HBL_NO_OVERWRITE); 570 | 571 | op = rend->getRenderOperation(); 572 | op->operationType = op_type; 573 | current_vertex_count = 0; 574 | 575 | vertex_size = op->vertexData->vertexDeclaration->getVertexSize(0); 576 | fptr = (float*)((uint8_t*)vdata); 577 | 578 | aabb.setNull(); 579 | } 580 | 581 | const Point& p = points[current_point]; 582 | 583 | uint32_t color; 584 | 585 | if (color_by_index_) 586 | { 587 | // convert to ColourValue, so we can then convert to the rendersystem-specific color type 588 | color = (current_point + point_count_ + 1); 589 | Ogre::ColourValue c; 590 | c.a = 1.0f; 591 | c.r = ((color >> 16) & 0xff) / 255.0f; 592 | c.g = ((color >> 8) & 0xff) / 255.0f; 593 | c.b = (color & 0xff) / 255.0f; 594 | root->convertColourValue(c, &color); 595 | } 596 | else 597 | { 598 | root->convertColourValue( p.color, &color ); 599 | } 600 | 601 | aabb.merge(p.position); 602 | bounding_radius_ = std::max( bounding_radius_, p.position.squaredLength() ); 603 | 604 | float x = p.position.x; 605 | float y = p.position.y; 606 | float z = p.position.z; 607 | float radius = p.radius; 608 | float nx = p.normal.x; 609 | float ny = p.normal.y; 610 | float nz = p.normal.z; 611 | 612 | for (uint32_t j = 0; j < vpp; ++j, ++current_vertex_count) 613 | { 614 | *fptr++ = x; 615 | *fptr++ = y; 616 | *fptr++ = z; 617 | 618 | if (!current_mode_supports_geometry_shader_) 619 | { 620 | *fptr++ = vertices[(j*3)]; 621 | *fptr++ = vertices[(j*3) + 1]; 622 | *fptr++ = vertices[(j*3) + 2]; 623 | } 624 | 625 | uint32_t* iptr = (uint32_t*)fptr; 626 | *iptr = color; 627 | ++fptr; 628 | 629 | if (current_mode_has_normal_) 630 | { 631 | *fptr++ = nx; 632 | *fptr++ = ny; 633 | *fptr++ = nz; 634 | } 635 | 636 | if (current_mode_has_radius_) 637 | { 638 | *fptr++ = radius; 639 | } 640 | 641 | ROS_ASSERT((uint8_t*)fptr <= (uint8_t*)vdata + rend->getBuffer()->getNumVertices() * vertex_size); 642 | } 643 | } 644 | 645 | op->vertexData->vertexCount = current_vertex_count - op->vertexData->vertexStart; 646 | rend->setBoundingBox(aabb); 647 | bounding_box_.merge(aabb); 648 | ROS_ASSERT(op->vertexData->vertexCount + op->vertexData->vertexStart <= rend->getBuffer()->getNumVertices()); 649 | 650 | vbuf->unlock(); 651 | 652 | point_count_ += num_points; 653 | 654 | shrinkRenderables(); 655 | 656 | if (getParentSceneNode()) 657 | { 658 | getParentSceneNode()->needUpdate(); 659 | } 660 | } 661 | 662 | void PointCloud::popPoints(uint32_t num_points) 663 | { 664 | uint32_t vpp = getVerticesPerPoint(); 665 | 666 | ROS_ASSERT(num_points <= point_count_); 667 | points_.erase(points_.begin(), points_.begin() + num_points); 668 | 669 | point_count_ -= num_points; 670 | 671 | // Now clear out popped points 672 | uint32_t popped_count = 0; 673 | while (popped_count < num_points * vpp) 674 | { 675 | PointCloudRenderablePtr rend = renderables_.front(); 676 | Ogre::RenderOperation* op = rend->getRenderOperation(); 677 | 678 | uint32_t popped = std::min((size_t)(num_points * vpp - popped_count), op->vertexData->vertexCount); 679 | op->vertexData->vertexStart += popped; 680 | op->vertexData->vertexCount -= popped; 681 | 682 | popped_count += popped; 683 | 684 | if (op->vertexData->vertexCount == 0) 685 | { 686 | renderables_.erase(renderables_.begin(), renderables_.begin() + 1); 687 | 688 | op->vertexData->vertexStart = 0; 689 | renderables_.push_back(rend); 690 | } 691 | } 692 | ROS_ASSERT(popped_count == num_points * vpp); 693 | 694 | // reset bounds 695 | bounding_box_.setNull(); 696 | bounding_radius_ = 0.0f; 697 | for (uint32_t i = 0; i < point_count_; ++i) 698 | { 699 | Point& p = points_[i]; 700 | bounding_box_.merge(p.position); 701 | bounding_radius_ = std::max(bounding_radius_, p.position.squaredLength()); 702 | } 703 | 704 | shrinkRenderables(); 705 | 706 | if (getParentSceneNode()) 707 | { 708 | getParentSceneNode()->needUpdate(); 709 | } 710 | } 711 | 712 | void PointCloud::shrinkRenderables() 713 | { 714 | while (!renderables_.empty()) 715 | { 716 | PointCloudRenderablePtr rend = renderables_.back(); 717 | Ogre::RenderOperation* op = rend->getRenderOperation(); 718 | if (op->vertexData->vertexCount == 0) 719 | { 720 | renderables_.pop_back(); 721 | } 722 | else 723 | { 724 | break; 725 | } 726 | } 727 | } 728 | 729 | void PointCloud::_notifyCurrentCamera(Ogre::Camera* camera) 730 | { 731 | MovableObject::_notifyCurrentCamera( camera ); 732 | } 733 | 734 | void PointCloud::_updateRenderQueue(Ogre::RenderQueue* queue) 735 | { 736 | V_PointCloudRenderable::iterator it = renderables_.begin(); 737 | V_PointCloudRenderable::iterator end = renderables_.end(); 738 | for (; it != end; ++it) 739 | { 740 | queue->addRenderable((*it).get()); 741 | } 742 | } 743 | 744 | void PointCloud::_notifyAttached(Ogre::Node *parent, bool isTagPoint) 745 | { 746 | MovableObject::_notifyAttached(parent, isTagPoint); 747 | } 748 | 749 | uint32_t PointCloud::getVerticesPerPoint() 750 | { 751 | if (current_mode_supports_geometry_shader_) 752 | { 753 | return 1; 754 | } 755 | 756 | if (render_mode_ == RM_POINTS) 757 | { 758 | return 1; 759 | } 760 | 761 | if (render_mode_ == RM_SURFELS) 762 | { 763 | return 1; 764 | } 765 | 766 | if (render_mode_ == RM_SQUARES) 767 | { 768 | return 6; 769 | } 770 | 771 | if (render_mode_ == RM_FLAT_SQUARES) 772 | { 773 | return 6; 774 | } 775 | 776 | if (render_mode_ == RM_TILES) 777 | { 778 | return 6; 779 | } 780 | 781 | if (render_mode_ == RM_SPHERES) 782 | { 783 | return 3; 784 | } 785 | 786 | if (render_mode_ == RM_BOXES) 787 | { 788 | return 36; 789 | } 790 | 791 | return 1; 792 | } 793 | 794 | void PointCloud::setPickColor(const Ogre::ColourValue& color) 795 | { 796 | pick_color_ = color; 797 | Ogre::Vector4 pick_col(pick_color_.r, pick_color_.g, pick_color_.b, pick_color_.a); 798 | 799 | V_PointCloudRenderable::iterator it = renderables_.begin(); 800 | V_PointCloudRenderable::iterator end = renderables_.end(); 801 | for (; it != end; ++it) 802 | { 803 | (*it)->setCustomParameter(PICK_COLOR_PARAMETER, pick_col); 804 | } 805 | getUserObjectBindings().setUserAny( "pick_handle", Ogre::Any( rviz::colorToHandle( color ))); 806 | } 807 | 808 | PointCloudRenderablePtr PointCloud::createRenderable( int num_points ) 809 | { 810 | PointCloudRenderablePtr rend(new PointCloudRenderable(this, num_points, !current_mode_supports_geometry_shader_, 811 | current_mode_has_normal_, current_mode_has_radius_)); 812 | rend->setMaterial(current_material_->getName()); 813 | Ogre::Vector4 size(width_, height_, depth_, 0.0f); 814 | Ogre::Vector4 alpha(alpha_, 0.0f, 0.0f, 0.0f); 815 | Ogre::Vector4 highlight(0.0f, 0.0f, 0.0f, 0.0f); 816 | Ogre::Vector4 pick_col(pick_color_.r, pick_color_.g, pick_color_.b, pick_color_.a); 817 | rend->setCustomParameter(SIZE_PARAMETER, size); 818 | rend->setCustomParameter(ALPHA_PARAMETER, alpha); 819 | rend->setCustomParameter(HIGHLIGHT_PARAMETER, highlight); 820 | rend->setCustomParameter(PICK_COLOR_PARAMETER, pick_col); 821 | rend->setCustomParameter(NORMAL_PARAMETER, Ogre::Vector4(common_direction_)); 822 | rend->setCustomParameter(UP_PARAMETER, Ogre::Vector4(common_up_vector_)); 823 | if (getParentSceneNode()) 824 | { 825 | getParentSceneNode()->attachObject(rend.get()); 826 | } 827 | renderables_.push_back(rend); 828 | 829 | return rend; 830 | } 831 | 832 | #if (OGRE_VERSION_MAJOR >= 1 && OGRE_VERSION_MINOR >= 6) 833 | void PointCloud::visitRenderables(Ogre::Renderable::Visitor* visitor, bool debugRenderables) 834 | { 835 | 836 | } 837 | #endif 838 | 839 | 840 | ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 841 | ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 842 | ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 843 | 844 | PointCloudRenderable::PointCloudRenderable(PointCloud* parent, int num_points, bool use_tex_coords, 845 | bool use_normals, bool use_radius) 846 | : parent_(parent) 847 | { 848 | // Initialize render operation 849 | mRenderOp.operationType = Ogre::RenderOperation::OT_POINT_LIST; 850 | mRenderOp.useIndexes = false; 851 | mRenderOp.vertexData = new Ogre::VertexData; 852 | mRenderOp.vertexData->vertexStart = 0; 853 | mRenderOp.vertexData->vertexCount = 0; 854 | 855 | Ogre::VertexDeclaration *decl = mRenderOp.vertexData->vertexDeclaration; 856 | size_t offset = 0; 857 | 858 | decl->addElement(0, offset, Ogre::VET_FLOAT3, Ogre::VES_POSITION); 859 | offset += Ogre::VertexElement::getTypeSize(Ogre::VET_FLOAT3); 860 | 861 | if (use_tex_coords) 862 | { 863 | decl->addElement(0, offset, Ogre::VET_FLOAT3, Ogre::VES_TEXTURE_COORDINATES, 0); 864 | offset += Ogre::VertexElement::getTypeSize(Ogre::VET_FLOAT3); 865 | } 866 | 867 | decl->addElement(0, offset, Ogre::VET_COLOUR, Ogre::VES_DIFFUSE); 868 | offset += Ogre::VertexElement::getTypeSize(Ogre::VET_COLOUR); 869 | 870 | if (use_normals) 871 | { 872 | decl->addElement(0, offset, Ogre::VET_FLOAT3, Ogre::VES_NORMAL); 873 | offset += Ogre::VertexElement::getTypeSize(Ogre::VET_FLOAT3); 874 | } 875 | 876 | if (use_radius) 877 | { 878 | decl->addElement(0, offset, Ogre::VET_FLOAT1, Ogre::VES_SPECULAR); 879 | offset += Ogre::VertexElement::getTypeSize(Ogre::VET_FLOAT1); 880 | } 881 | 882 | Ogre::HardwareVertexBufferSharedPtr vbuf = 883 | Ogre::HardwareBufferManager::getSingleton().createVertexBuffer( 884 | mRenderOp.vertexData->vertexDeclaration->getVertexSize(0), 885 | num_points, 886 | Ogre::HardwareBuffer::HBU_DYNAMIC); 887 | 888 | // Bind buffer 889 | mRenderOp.vertexData->vertexBufferBinding->setBinding(0, vbuf); 890 | } 891 | 892 | PointCloudRenderable::~PointCloudRenderable() 893 | { 894 | delete mRenderOp.vertexData; 895 | delete mRenderOp.indexData; 896 | } 897 | 898 | Ogre::HardwareVertexBufferSharedPtr PointCloudRenderable::getBuffer() 899 | { 900 | return mRenderOp.vertexData->vertexBufferBinding->getBuffer(0); 901 | } 902 | 903 | void PointCloudRenderable::_notifyCurrentCamera(Ogre::Camera* camera) 904 | { 905 | SimpleRenderable::_notifyCurrentCamera( camera ); 906 | } 907 | 908 | Ogre::Real PointCloudRenderable::getBoundingRadius(void) const 909 | { 910 | return Ogre::Math::Sqrt(std::max(mBox.getMaximum().squaredLength(), mBox.getMinimum().squaredLength())); 911 | } 912 | 913 | Ogre::Real PointCloudRenderable::getSquaredViewDepth(const Ogre::Camera* cam) const 914 | { 915 | Ogre::Vector3 vMin, vMax, vMid, vDist; 916 | vMin = mBox.getMinimum(); 917 | vMax = mBox.getMaximum(); 918 | vMid = ((vMax - vMin) * 0.5) + vMin; 919 | vDist = cam->getDerivedPosition() - vMid; 920 | 921 | return vDist.squaredLength(); 922 | } 923 | 924 | void PointCloudRenderable::getWorldTransforms(Ogre::Matrix4* xform) const 925 | { 926 | parent_->getWorldTransforms(xform); 927 | } 928 | 929 | const Ogre::LightList& PointCloudRenderable::getLights() const 930 | { 931 | return parent_->queryLights(); 932 | } 933 | 934 | } // namespace rviz 935 | -------------------------------------------------------------------------------- /src/point_cloud_common.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 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 | 52 | #include "point_cloud_common.h" 53 | 54 | #include "point_cloud.h" 55 | 56 | namespace surfel_cloud_rviz_plugin 57 | { 58 | using rviz::PointCloudTransformer; 59 | using rviz::EnumProperty; 60 | using rviz::StatusProperty; 61 | using rviz::FloatProperty; 62 | using rviz::BoolProperty; 63 | using rviz::VectorProperty; 64 | using rviz::validateFloats; 65 | using rviz::Property; 66 | using rviz::findChannelIndex; 67 | using rviz::SelectionManager; 68 | using rviz::DisplayContext; 69 | using rviz::Display; 70 | using rviz::Picked; 71 | using rviz::S_uint64; 72 | using rviz::V_AABB; 73 | using rviz::valueFromCloud; 74 | using rviz::ColorProperty; 75 | 76 | typedef std::vector V_PointCloudPoint; 77 | 78 | struct IndexAndMessage 79 | { 80 | IndexAndMessage( int _index, const void* _message ) 81 | : index( _index ) 82 | , message( (uint64_t) _message ) 83 | {} 84 | 85 | int index; 86 | uint64_t message; 87 | }; 88 | 89 | uint qHash( IndexAndMessage iam ) 90 | { 91 | return 92 | ((uint) iam.index) + 93 | ((uint) (iam.message >> 32)) + 94 | ((uint) (iam.message & 0xffffffff)); 95 | } 96 | 97 | bool operator==( IndexAndMessage a, IndexAndMessage b ) 98 | { 99 | return a.index == b.index && a.message == b.message; 100 | } 101 | 102 | PointCloudSelectionHandler::PointCloudSelectionHandler( 103 | float box_size, 104 | PointCloudCommon::CloudInfo *cloud_info, 105 | DisplayContext* context ) 106 | : SelectionHandler( context ) 107 | , cloud_info_( cloud_info ) 108 | , box_size_(box_size) 109 | { 110 | } 111 | 112 | PointCloudSelectionHandler::~PointCloudSelectionHandler() 113 | { 114 | // delete all the Property objects on our way out. 115 | QHash::const_iterator iter; 116 | for( iter = property_hash_.begin(); iter != property_hash_.end(); iter++ ) 117 | { 118 | delete iter.value(); 119 | } 120 | } 121 | 122 | void PointCloudSelectionHandler::preRenderPass(uint32_t pass) 123 | { 124 | SelectionHandler::preRenderPass(pass); 125 | 126 | switch( pass ) 127 | { 128 | case 0: 129 | cloud_info_->cloud_->setPickColor( SelectionManager::handleToColor( getHandle() )); 130 | break; 131 | case 1: 132 | cloud_info_->cloud_->setColorByIndex(true); 133 | break; 134 | default: 135 | break; 136 | } 137 | } 138 | 139 | void PointCloudSelectionHandler::postRenderPass(uint32_t pass) 140 | { 141 | SelectionHandler::postRenderPass(pass); 142 | 143 | if (pass == 1) 144 | { 145 | cloud_info_->cloud_->setColorByIndex(false); 146 | } 147 | } 148 | 149 | Ogre::Vector3 pointFromCloud(const sensor_msgs::PointCloud2ConstPtr& cloud, uint32_t index) 150 | { 151 | int32_t xi = findChannelIndex(cloud, "x"); 152 | int32_t yi = findChannelIndex(cloud, "y"); 153 | int32_t zi = findChannelIndex(cloud, "z"); 154 | 155 | const uint32_t xoff = cloud->fields[xi].offset; 156 | const uint32_t yoff = cloud->fields[yi].offset; 157 | const uint32_t zoff = cloud->fields[zi].offset; 158 | const uint8_t type = cloud->fields[xi].datatype; 159 | const uint32_t point_step = cloud->point_step; 160 | float x = valueFromCloud(cloud, xoff, type, point_step, index); 161 | float y = valueFromCloud(cloud, yoff, type, point_step, index); 162 | float z = valueFromCloud(cloud, zoff, type, point_step, index); 163 | return Ogre::Vector3(x, y, z); 164 | } 165 | 166 | void PointCloudSelectionHandler::createProperties( const Picked& obj, Property* parent_property ) 167 | { 168 | typedef std::set S_int; 169 | S_int indices; 170 | { 171 | S_uint64::const_iterator it = obj.extra_handles.begin(); 172 | S_uint64::const_iterator end = obj.extra_handles.end(); 173 | for (; it != end; ++it) 174 | { 175 | uint64_t handle = *it; 176 | indices.insert((handle & 0xffffffff) - 1); 177 | } 178 | } 179 | 180 | { 181 | S_int::iterator it = indices.begin(); 182 | S_int::iterator end = indices.end(); 183 | for (; it != end; ++it) 184 | { 185 | int index = *it; 186 | const sensor_msgs::PointCloud2ConstPtr& message = cloud_info_->message_; 187 | 188 | IndexAndMessage hash_key( index, message.get() ); 189 | if( !property_hash_.contains( hash_key )) 190 | { 191 | Property* cat = new Property( QString( "Point %1 [cloud 0x%2]" ).arg( index ).arg( (uint64_t) message.get() ), 192 | QVariant(), "", parent_property ); 193 | property_hash_.insert( hash_key, cat ); 194 | 195 | // First add the position. 196 | VectorProperty* pos_prop = new VectorProperty( "Position", cloud_info_->transformed_points_[index].position, "", cat ); 197 | pos_prop->setReadOnly( true ); 198 | 199 | // Then add all other fields as well. 200 | for( size_t field = 0; field < message->fields.size(); ++field ) 201 | { 202 | const sensor_msgs::PointField& f = message->fields[ field ]; 203 | const std::string& name = f.name; 204 | 205 | if( name == "x" || name == "y" || name == "z" || name == "X" || name == "Y" || name == "Z" ) 206 | { 207 | continue; 208 | } 209 | if( name == "rgb" || name == "rgba") 210 | { 211 | float float_val = valueFromCloud( message, f.offset, f.datatype, message->point_step, index ); 212 | // Convertion hack because rgb are stored int float (datatype=7) and valueFromCloud can't cast float to uint32_t 213 | uint32_t val = *((uint32_t*) &float_val); 214 | ColorProperty* prop = new ColorProperty( QString( "%1: %2" ).arg( field ).arg( QString::fromStdString( name )), 215 | QColor( (val >> 16) & 0xff, (val >> 8) & 0xff, val & 0xff), "", cat ); 216 | prop->setReadOnly( true ); 217 | 218 | FloatProperty* aprop = new FloatProperty( QString( "alpha" ), ((val >> 24) / 255.0), "", cat ); 219 | aprop->setReadOnly( true ); 220 | } 221 | else 222 | { 223 | float val = valueFromCloud( message, f.offset, f.datatype, message->point_step, index ); 224 | FloatProperty* prop = new FloatProperty( QString( "%1: %2" ).arg( field ).arg( QString::fromStdString( name )), 225 | val, "", cat ); 226 | prop->setReadOnly( true ); 227 | } 228 | } 229 | } 230 | } 231 | } 232 | } 233 | 234 | void PointCloudSelectionHandler::destroyProperties( const Picked& obj, Property* parent_property ) 235 | { 236 | typedef std::set S_int; 237 | S_int indices; 238 | { 239 | S_uint64::const_iterator it = obj.extra_handles.begin(); 240 | S_uint64::const_iterator end = obj.extra_handles.end(); 241 | for (; it != end; ++it) 242 | { 243 | uint64_t handle = *it; 244 | indices.insert((handle & 0xffffffff) - 1); 245 | } 246 | } 247 | 248 | { 249 | S_int::iterator it = indices.begin(); 250 | S_int::iterator end = indices.end(); 251 | for (; it != end; ++it) 252 | { 253 | int index = *it; 254 | const sensor_msgs::PointCloud2ConstPtr& message = cloud_info_->message_; 255 | 256 | IndexAndMessage hash_key( index, message.get() ); 257 | 258 | Property* prop = property_hash_.take( hash_key ); 259 | delete prop; 260 | } 261 | } 262 | } 263 | 264 | void PointCloudSelectionHandler::getAABBs(const Picked& obj, V_AABB& aabbs) 265 | { 266 | S_uint64::iterator it = obj.extra_handles.begin(); 267 | S_uint64::iterator end = obj.extra_handles.end(); 268 | for (; it != end; ++it) 269 | { 270 | M_HandleToBox::iterator find_it = boxes_.find(std::make_pair(obj.handle, *it - 1)); 271 | if (find_it != boxes_.end()) 272 | { 273 | Ogre::WireBoundingBox* box = find_it->second.second; 274 | 275 | aabbs.push_back(box->getWorldBoundingBox()); 276 | } 277 | } 278 | } 279 | 280 | void PointCloudSelectionHandler::onSelect(const Picked& obj) 281 | { 282 | S_uint64::iterator it = obj.extra_handles.begin(); 283 | S_uint64::iterator end = obj.extra_handles.end(); 284 | for (; it != end; ++it) 285 | { 286 | int index = (*it & 0xffffffff) - 1; 287 | 288 | sensor_msgs::PointCloud2ConstPtr message = cloud_info_->message_; 289 | 290 | Ogre::Vector3 pos = cloud_info_->transformed_points_[index].position; 291 | pos = cloud_info_->scene_node_->convertLocalToWorldPosition( pos ); 292 | 293 | float size = box_size_ * 0.5f; 294 | 295 | Ogre::AxisAlignedBox aabb(pos - size, pos + size); 296 | 297 | createBox(std::make_pair(obj.handle, index), aabb, "RVIZ/Cyan" ); 298 | } 299 | } 300 | 301 | void PointCloudSelectionHandler::onDeselect(const Picked& obj) 302 | { 303 | S_uint64::iterator it = obj.extra_handles.begin(); 304 | S_uint64::iterator end = obj.extra_handles.end(); 305 | for (; it != end; ++it) 306 | { 307 | int global_index = (*it & 0xffffffff) - 1; 308 | 309 | destroyBox(std::make_pair(obj.handle, global_index)); 310 | } 311 | } 312 | 313 | PointCloudCommon::CloudInfo::CloudInfo() 314 | : manager_(0) 315 | , scene_node_(0) 316 | {} 317 | 318 | PointCloudCommon::CloudInfo::~CloudInfo() 319 | { 320 | clear(); 321 | } 322 | 323 | void PointCloudCommon::CloudInfo::clear() 324 | { 325 | if ( scene_node_ ) 326 | { 327 | manager_->destroySceneNode( scene_node_ ); 328 | scene_node_=0; 329 | } 330 | } 331 | 332 | PointCloudCommon::PointCloudCommon( Display* display ) 333 | : spinner_(1, &cbqueue_) 334 | , auto_size_(false) 335 | , new_xyz_transformer_(false) 336 | , new_color_transformer_(false) 337 | , needs_retransform_(false) 338 | , transformer_class_loader_(NULL) 339 | , display_( display ) 340 | { 341 | selectable_property_ = new BoolProperty( "Selectable", true, 342 | "Whether or not the points in this point cloud are selectable.", 343 | display_, SLOT( updateSelectable() ), this ); 344 | 345 | style_property_ = new EnumProperty( "Style", "Flat Squares", 346 | "Rendering mode to use, in order of computational complexity.", 347 | display_, SLOT( updateStyle() ), this ); 348 | style_property_->addOption( "Points", PointCloud::RM_POINTS ); 349 | style_property_->addOption( "Squares", PointCloud::RM_SQUARES ); 350 | style_property_->addOption( "Flat Squares", PointCloud::RM_FLAT_SQUARES ); 351 | style_property_->addOption( "Spheres", PointCloud::RM_SPHERES ); 352 | style_property_->addOption( "Boxes", PointCloud::RM_BOXES ); 353 | style_property_->addOption( "Surfels", PointCloud::RM_SURFELS ); 354 | 355 | point_world_size_property_ = new FloatProperty( "Size (m)", 0.01, 356 | "Point size in meters.", 357 | display_, SLOT( updateBillboardSize() ), this ); 358 | point_world_size_property_->setMin( 0.0001 ); 359 | 360 | point_pixel_size_property_ = new FloatProperty( "Size (Pixels)", 3, 361 | "Point size in pixels.", 362 | display_, SLOT( updateBillboardSize() ), this ); 363 | point_pixel_size_property_->setMin( 1 ); 364 | 365 | point_scale_size_property_ = new FloatProperty( "Size (Scale)", 1, 366 | "Point scale.", 367 | display_, SLOT( updateBillboardSize() ), this ); 368 | point_scale_size_property_->setMin( 0.0001 ); 369 | 370 | alpha_property_ = new FloatProperty( "Alpha", 1.0, 371 | "Amount of transparency to apply to the points. Note that this is experimental and does not always look correct.", 372 | display_, SLOT( updateAlpha() ), this ); 373 | alpha_property_->setMin( 0 ); 374 | alpha_property_->setMax( 1 ); 375 | 376 | decay_time_property_ = new FloatProperty( "Decay Time", 0, 377 | "Duration, in seconds, to keep the incoming points. 0 means only show the latest points.", 378 | display_, SLOT( queueRender() )); 379 | decay_time_property_->setMin( 0 ); 380 | 381 | xyz_transformer_property_ = new EnumProperty( "Position Transformer", "", 382 | "Set the transformer to use to set the position of the points.", 383 | display_, SLOT( updateXyzTransformer() ), this ); 384 | connect( xyz_transformer_property_, &rviz::EnumProperty::requestOptions, 385 | this, &PointCloudCommon::setXyzTransformerOptions); 386 | 387 | color_transformer_property_ = new EnumProperty( "Color Transformer", "", 388 | "Set the transformer to use to set the color of the points.", 389 | display_, SLOT( updateColorTransformer() ), this ); 390 | connect( color_transformer_property_, &rviz::EnumProperty::requestOptions, 391 | this, &PointCloudCommon::setColorTransformerOptions); 392 | } 393 | 394 | void PointCloudCommon::initialize( DisplayContext* context, Ogre::SceneNode* scene_node ) 395 | { 396 | transformer_class_loader_ = new pluginlib::ClassLoader( "rviz", "rviz::PointCloudTransformer" ); 397 | loadTransformers(); 398 | 399 | context_ = context; 400 | scene_node_ = scene_node; 401 | 402 | updateStyle(); 403 | updateBillboardSize(); 404 | updateAlpha(); 405 | updateSelectable(); 406 | 407 | spinner_.start(); 408 | } 409 | 410 | PointCloudCommon::~PointCloudCommon() 411 | { 412 | spinner_.stop(); 413 | 414 | if ( transformer_class_loader_ ) 415 | { 416 | delete transformer_class_loader_; 417 | } 418 | } 419 | 420 | void PointCloudCommon::loadTransformers() 421 | { 422 | std::vector classes = transformer_class_loader_->getDeclaredClasses(); 423 | std::vector::iterator ci; 424 | 425 | for( ci = classes.begin(); ci != classes.end(); ci++ ) 426 | { 427 | const std::string& lookup_name = *ci; 428 | std::string name = transformer_class_loader_->getName( lookup_name ); 429 | 430 | if( transformers_.count( name ) > 0 ) 431 | { 432 | ROS_ERROR( "Transformer type [%s] is already loaded.", name.c_str() ); 433 | continue; 434 | } 435 | 436 | PointCloudTransformerPtr trans( transformer_class_loader_->createUnmanagedInstance( lookup_name )); 437 | trans->init(); 438 | connect( trans.get(), SIGNAL( needRetransform() ), this, SLOT( causeRetransform() )); 439 | 440 | TransformerInfo info; 441 | info.transformer = trans; 442 | info.readable_name = name; 443 | info.lookup_name = lookup_name; 444 | 445 | info.transformer->createProperties( display_, PointCloudTransformer::Support_XYZ, info.xyz_props ); 446 | setPropertiesHidden( info.xyz_props, true ); 447 | 448 | info.transformer->createProperties( display_, PointCloudTransformer::Support_Color, info.color_props ); 449 | setPropertiesHidden( info.color_props, true ); 450 | 451 | transformers_[ name ] = info; 452 | } 453 | } 454 | 455 | void PointCloudCommon::setAutoSize( bool auto_size ) 456 | { 457 | auto_size_ = auto_size; 458 | for ( unsigned i=0; icloud_->setAutoSize( auto_size ); 461 | } 462 | } 463 | 464 | 465 | 466 | void PointCloudCommon::updateAlpha() 467 | { 468 | for ( unsigned i=0; imessage_, "rgba") != -1; 471 | cloud_infos_[i]->cloud_->setAlpha( alpha_property_->getFloat(), per_point_alpha ); 472 | } 473 | } 474 | 475 | void PointCloudCommon::updateSelectable() 476 | { 477 | bool selectable = selectable_property_->getBool(); 478 | 479 | if( selectable ) 480 | { 481 | for ( unsigned i=0; iselection_handler_.reset( new PointCloudSelectionHandler( getSelectionBoxSize(), cloud_infos_[i].get(), context_ )); 484 | cloud_infos_[i]->cloud_->setPickColor( SelectionManager::handleToColor( cloud_infos_[i]->selection_handler_->getHandle() )); 485 | } 486 | } 487 | else 488 | { 489 | for ( unsigned i=0; iselection_handler_.reset( ); 492 | cloud_infos_[i]->cloud_->setPickColor( Ogre::ColourValue( 0.0f, 0.0f, 0.0f, 0.0f ) ); 493 | } 494 | } 495 | } 496 | 497 | void PointCloudCommon::updateStyle() 498 | { 499 | PointCloud::RenderMode mode = (PointCloud::RenderMode) style_property_->getOptionInt(); 500 | if( mode == PointCloud::RM_POINTS ) 501 | { 502 | point_world_size_property_->hide(); 503 | point_scale_size_property_->hide(); 504 | point_pixel_size_property_->show(); 505 | } 506 | else if ( mode == PointCloud::RM_SURFELS ) 507 | { 508 | point_world_size_property_->hide(); 509 | point_scale_size_property_->show(); 510 | point_pixel_size_property_->hide(); 511 | } 512 | else 513 | { 514 | point_world_size_property_->show(); 515 | point_scale_size_property_->hide(); 516 | point_pixel_size_property_->hide(); 517 | } 518 | for( unsigned int i = 0; i < cloud_infos_.size(); i++ ) 519 | { 520 | cloud_infos_[i]->cloud_->setRenderMode( mode ); 521 | } 522 | updateBillboardSize(); 523 | causeRetransform(); 524 | } 525 | 526 | void PointCloudCommon::updateBillboardSize() 527 | { 528 | PointCloud::RenderMode mode = (PointCloud::RenderMode) style_property_->getOptionInt(); 529 | float size; 530 | if( mode == PointCloud::RM_POINTS ) { 531 | size = point_pixel_size_property_->getFloat(); 532 | } else if ( mode == PointCloud::RM_SURFELS ) { 533 | size = point_scale_size_property_->getFloat(); 534 | } else { 535 | size = point_world_size_property_->getFloat(); 536 | } 537 | for ( unsigned i=0; icloud_->setDimensions( size, size, size ); 540 | cloud_infos_[i]->selection_handler_->setBoxSize( getSelectionBoxSize() ); 541 | } 542 | context_->queueRender(); 543 | } 544 | 545 | void PointCloudCommon::reset() 546 | { 547 | boost::mutex::scoped_lock lock(new_clouds_mutex_); 548 | cloud_infos_.clear(); 549 | new_cloud_infos_.clear(); 550 | } 551 | 552 | void PointCloudCommon::causeRetransform() 553 | { 554 | needs_retransform_ = true; 555 | } 556 | 557 | void PointCloudCommon::update(float wall_dt, float ros_dt) 558 | { 559 | PointCloud::RenderMode mode = (PointCloud::RenderMode) style_property_->getOptionInt(); 560 | 561 | float point_decay_time = decay_time_property_->getFloat(); 562 | if (needs_retransform_) 563 | { 564 | retransform(); 565 | needs_retransform_ = false; 566 | } 567 | 568 | // instead of deleting cloud infos, we just clear them 569 | // and put them into obsolete_cloud_infos, so active selections 570 | // are preserved 571 | 572 | ros::Time now = ros::Time::now(); 573 | 574 | // if decay time == 0, clear the old cloud when we get a new one 575 | // otherwise, clear all the outdated ones 576 | { 577 | boost::mutex::scoped_lock lock(new_clouds_mutex_); 578 | if ( point_decay_time > 0.0 || !new_cloud_infos_.empty() ) 579 | { 580 | while( !cloud_infos_.empty() && now.toSec() - cloud_infos_.front()->receive_time_.toSec() > point_decay_time ) 581 | { 582 | cloud_infos_.front()->clear(); 583 | obsolete_cloud_infos_.push_back( cloud_infos_.front() ); 584 | cloud_infos_.pop_front(); 585 | context_->queueRender(); 586 | } 587 | } 588 | } 589 | 590 | // garbage-collect old point clouds that don't have an active selection 591 | L_CloudInfo::iterator it = obsolete_cloud_infos_.begin(); 592 | L_CloudInfo::iterator end = obsolete_cloud_infos_.end(); 593 | for (; it != end; ++it) 594 | { 595 | if ( !(*it)->selection_handler_.get() || 596 | !(*it)->selection_handler_->hasSelections() ) 597 | { 598 | it = obsolete_cloud_infos_.erase(it); 599 | } 600 | } 601 | 602 | { 603 | boost::mutex::scoped_lock lock(new_clouds_mutex_); 604 | if( !new_cloud_infos_.empty() ) 605 | { 606 | float size; 607 | if( mode == PointCloud::RM_POINTS ) { 608 | size = point_pixel_size_property_->getFloat(); 609 | } else if ( mode == PointCloud::RM_SURFELS ) { 610 | size = point_scale_size_property_->getFloat(); 611 | } else { 612 | size = point_world_size_property_->getFloat(); 613 | } 614 | 615 | V_CloudInfo::iterator it = new_cloud_infos_.begin(); 616 | V_CloudInfo::iterator end = new_cloud_infos_.end(); 617 | for (; it != end; ++it) 618 | { 619 | CloudInfoPtr cloud_info = *it; 620 | 621 | V_CloudInfo::iterator next = it; next++; 622 | // ignore point clouds that are too old, but keep at least one 623 | if ( next != end && now.toSec() - cloud_info->receive_time_.toSec() > point_decay_time ) { 624 | continue; 625 | } 626 | 627 | bool per_point_alpha = findChannelIndex(cloud_info->message_, "rgba") != -1; 628 | 629 | cloud_info->cloud_.reset( new PointCloud() ); 630 | cloud_info->cloud_->setRenderMode( mode ); 631 | cloud_info->cloud_->addPoints( &(cloud_info->transformed_points_.front()), cloud_info->transformed_points_.size() ); 632 | cloud_info->cloud_->setAlpha( alpha_property_->getFloat(), per_point_alpha); 633 | cloud_info->cloud_->setDimensions( size, size, size ); 634 | cloud_info->cloud_->setAutoSize(auto_size_); 635 | 636 | cloud_info->manager_ = context_->getSceneManager(); 637 | 638 | cloud_info->scene_node_ = scene_node_->createChildSceneNode( cloud_info->position_, cloud_info->orientation_ ); 639 | 640 | cloud_info->scene_node_->attachObject( cloud_info->cloud_.get() ); 641 | 642 | cloud_info->selection_handler_.reset( new PointCloudSelectionHandler( getSelectionBoxSize(), cloud_info.get(), context_ )); 643 | 644 | cloud_infos_.push_back(*it); 645 | } 646 | 647 | new_cloud_infos_.clear(); 648 | } 649 | } 650 | 651 | { 652 | boost::recursive_mutex::scoped_try_lock lock( transformers_mutex_ ); 653 | 654 | if( lock.owns_lock() ) 655 | { 656 | if( new_xyz_transformer_ || new_color_transformer_ ) 657 | { 658 | M_TransformerInfo::iterator it = transformers_.begin(); 659 | M_TransformerInfo::iterator end = transformers_.end(); 660 | for (; it != end; ++it) 661 | { 662 | const std::string& name = it->first; 663 | TransformerInfo& info = it->second; 664 | 665 | setPropertiesHidden( info.xyz_props, name != xyz_transformer_property_->getStdString() ); 666 | setPropertiesHidden( info.color_props, name != color_transformer_property_->getStdString() ); 667 | } 668 | } 669 | } 670 | 671 | new_xyz_transformer_ = false; 672 | new_color_transformer_ = false; 673 | } 674 | 675 | updateStatus(); 676 | } 677 | 678 | void PointCloudCommon::setPropertiesHidden( const QList& props, bool hide ) 679 | { 680 | for( int i = 0; i < props.size(); i++ ) 681 | { 682 | props[ i ]->setHidden( hide ); 683 | } 684 | } 685 | 686 | void PointCloudCommon::updateTransformers( const sensor_msgs::PointCloud2ConstPtr& cloud ) 687 | { 688 | std::string xyz_name = xyz_transformer_property_->getStdString(); 689 | std::string color_name = color_transformer_property_->getStdString(); 690 | 691 | xyz_transformer_property_->clearOptions(); 692 | color_transformer_property_->clearOptions(); 693 | 694 | // Get the channels that we could potentially render 695 | typedef std::set > S_string; 696 | S_string valid_xyz, valid_color; 697 | bool cur_xyz_valid = false; 698 | bool cur_color_valid = false; 699 | bool has_rgb_transformer = false; 700 | M_TransformerInfo::iterator trans_it = transformers_.begin(); 701 | M_TransformerInfo::iterator trans_end = transformers_.end(); 702 | for(;trans_it != trans_end; ++trans_it) 703 | { 704 | const std::string& name = trans_it->first; 705 | const PointCloudTransformerPtr& trans = trans_it->second.transformer; 706 | uint32_t mask = trans->supports(cloud); 707 | if (mask & PointCloudTransformer::Support_XYZ) 708 | { 709 | valid_xyz.insert(std::make_pair(trans->score(cloud), name)); 710 | if (name == xyz_name) 711 | { 712 | cur_xyz_valid = true; 713 | } 714 | xyz_transformer_property_->addOptionStd( name ); 715 | } 716 | 717 | if (mask & PointCloudTransformer::Support_Color) 718 | { 719 | valid_color.insert(std::make_pair(trans->score(cloud), name)); 720 | if (name == color_name) 721 | { 722 | cur_color_valid = true; 723 | } 724 | if (name == "RGB8") 725 | { 726 | has_rgb_transformer = true; 727 | } 728 | color_transformer_property_->addOptionStd( name ); 729 | } 730 | } 731 | 732 | if( !cur_xyz_valid ) 733 | { 734 | if( !valid_xyz.empty() ) 735 | { 736 | xyz_transformer_property_->setStringStd( valid_xyz.rbegin()->second ); 737 | } 738 | } 739 | 740 | if( !cur_color_valid ) 741 | { 742 | if( !valid_color.empty() ) 743 | { 744 | if (has_rgb_transformer) 745 | { 746 | color_transformer_property_->setStringStd( "RGB8" ); 747 | } else 748 | { 749 | color_transformer_property_->setStringStd( valid_color.rbegin()->second ); 750 | } 751 | } 752 | } 753 | } 754 | 755 | void PointCloudCommon::updateStatus() 756 | { 757 | std::stringstream ss; 758 | //ss << "Showing [" << total_point_count_ << "] points from [" << clouds_.size() << "] messages"; 759 | display_->setStatusStd(StatusProperty::Ok, "Points", ss.str()); 760 | } 761 | 762 | void PointCloudCommon::processMessage(const sensor_msgs::PointCloud2ConstPtr& cloud) 763 | { 764 | CloudInfoPtr info(new CloudInfo); 765 | info->message_ = cloud; 766 | info->receive_time_ = ros::Time::now(); 767 | 768 | if (transformCloud(info, true)) 769 | { 770 | boost::mutex::scoped_lock lock(new_clouds_mutex_); 771 | new_cloud_infos_.push_back(info); 772 | display_->emitTimeSignal( cloud->header.stamp ); 773 | } 774 | } 775 | 776 | void PointCloudCommon::updateXyzTransformer() 777 | { 778 | boost::recursive_mutex::scoped_lock lock( transformers_mutex_ ); 779 | if( transformers_.count( xyz_transformer_property_->getStdString() ) == 0 ) 780 | { 781 | return; 782 | } 783 | new_xyz_transformer_ = true; 784 | causeRetransform(); 785 | } 786 | 787 | void PointCloudCommon::updateColorTransformer() 788 | { 789 | boost::recursive_mutex::scoped_lock lock( transformers_mutex_ ); 790 | if( transformers_.count( color_transformer_property_->getStdString() ) == 0 ) 791 | { 792 | return; 793 | } 794 | new_color_transformer_ = true; 795 | causeRetransform(); 796 | } 797 | 798 | PointCloudTransformerPtr PointCloudCommon::getXYZTransformer( const sensor_msgs::PointCloud2ConstPtr& cloud ) 799 | { 800 | boost::recursive_mutex::scoped_lock lock( transformers_mutex_); 801 | M_TransformerInfo::iterator it = transformers_.find( xyz_transformer_property_->getStdString() ); 802 | if( it != transformers_.end() ) 803 | { 804 | const PointCloudTransformerPtr& trans = it->second.transformer; 805 | if( trans->supports( cloud ) & PointCloudTransformer::Support_XYZ ) 806 | { 807 | return trans; 808 | } 809 | } 810 | 811 | return PointCloudTransformerPtr(); 812 | } 813 | 814 | PointCloudTransformerPtr PointCloudCommon::getColorTransformer( const sensor_msgs::PointCloud2ConstPtr& cloud ) 815 | { 816 | boost::recursive_mutex::scoped_lock lock( transformers_mutex_ ); 817 | M_TransformerInfo::iterator it = transformers_.find( color_transformer_property_->getStdString() ); 818 | if( it != transformers_.end() ) 819 | { 820 | const PointCloudTransformerPtr& trans = it->second.transformer; 821 | if( trans->supports( cloud ) & PointCloudTransformer::Support_Color ) 822 | { 823 | return trans; 824 | } 825 | } 826 | 827 | return PointCloudTransformerPtr(); 828 | } 829 | 830 | 831 | void PointCloudCommon::retransform() 832 | { 833 | boost::recursive_mutex::scoped_lock lock(transformers_mutex_); 834 | 835 | D_CloudInfo::iterator it = cloud_infos_.begin(); 836 | D_CloudInfo::iterator end = cloud_infos_.end(); 837 | for (; it != end; ++it) 838 | { 839 | const CloudInfoPtr& cloud_info = *it; 840 | transformCloud(cloud_info, false); 841 | cloud_info->cloud_->clear(); 842 | cloud_info->cloud_->addPoints(&cloud_info->transformed_points_.front(), cloud_info->transformed_points_.size()); 843 | } 844 | } 845 | 846 | bool PointCloudCommon::transformCloud(const CloudInfoPtr& cloud_info, bool update_transformers) 847 | { 848 | 849 | if ( !cloud_info->scene_node_ ) 850 | { 851 | if (!context_->getFrameManager()->getTransform(cloud_info->message_->header, cloud_info->position_, cloud_info->orientation_)) 852 | { 853 | std::stringstream ss; 854 | ss << "Failed to transform from frame [" << cloud_info->message_->header.frame_id << "] to frame [" << context_->getFrameManager()->getFixedFrame() << "]"; 855 | display_->setStatusStd(StatusProperty::Error, "Message", ss.str()); 856 | return false; 857 | } 858 | } 859 | 860 | Ogre::Matrix4 transform; 861 | transform.makeTransform( cloud_info->position_, Ogre::Vector3(1,1,1), cloud_info->orientation_ ); 862 | 863 | rviz::V_PointCloudPoint cloud_points; 864 | 865 | size_t size = cloud_info->message_->width * cloud_info->message_->height; 866 | rviz::PointCloud::Point default_pt; 867 | default_pt.color = Ogre::ColourValue(1, 1, 1); 868 | default_pt.position = Ogre::Vector3::ZERO; 869 | cloud_points.resize(size, default_pt); 870 | 871 | { 872 | boost::recursive_mutex::scoped_lock lock(transformers_mutex_); 873 | if( update_transformers ) 874 | { 875 | updateTransformers( cloud_info->message_ ); 876 | } 877 | PointCloudTransformerPtr xyz_trans = getXYZTransformer(cloud_info->message_); 878 | PointCloudTransformerPtr color_trans = getColorTransformer(cloud_info->message_); 879 | 880 | if (!xyz_trans) 881 | { 882 | std::stringstream ss; 883 | ss << "No position transformer available for cloud"; 884 | display_->setStatusStd(StatusProperty::Error, "Message", ss.str()); 885 | return false; 886 | } 887 | 888 | if (!color_trans) 889 | { 890 | std::stringstream ss; 891 | ss << "No color transformer available for cloud"; 892 | display_->setStatusStd(StatusProperty::Error, "Message", ss.str()); 893 | return false; 894 | } 895 | 896 | xyz_trans->transform(cloud_info->message_, PointCloudTransformer::Support_XYZ, transform, cloud_points); 897 | color_trans->transform(cloud_info->message_, PointCloudTransformer::Support_Color, transform, cloud_points); 898 | } 899 | 900 | for (rviz::V_PointCloudPoint::iterator cloud_point = cloud_points.begin(); cloud_point != cloud_points.end(); ++cloud_point) 901 | { 902 | if (!validateFloats(cloud_point->position)) 903 | { 904 | cloud_point->position.x = 999999.0f; 905 | cloud_point->position.y = 999999.0f; 906 | cloud_point->position.z = 999999.0f; 907 | } 908 | } 909 | 910 | cloud_info->transformed_points_.clear(); 911 | cloud_info->transformed_points_.reserve(cloud_points.size()); 912 | for (rviz::V_PointCloudPoint::iterator cloud_point = cloud_points.begin(); cloud_point != cloud_points.end(); ++cloud_point) 913 | { 914 | PointCloud::Point pt; 915 | pt.position = cloud_point->position; 916 | pt.color = cloud_point->color; 917 | pt.radius = 1; 918 | pt.normal = Ogre::Vector3(1,0,0); 919 | cloud_info->transformed_points_.push_back(pt); 920 | } 921 | 922 | // if surfels, add normal and radius 923 | PointCloud::RenderMode mode = (PointCloud::RenderMode)style_property_->getOptionInt(); 924 | if(mode == PointCloud::RM_SURFELS) 925 | { 926 | const int32_t nxi = rviz::findChannelIndex(cloud_info->message_, "normal_x"); 927 | const int32_t nyi = rviz::findChannelIndex(cloud_info->message_, "normal_y"); 928 | const int32_t nzi = rviz::findChannelIndex(cloud_info->message_, "normal_z"); 929 | const int32_t ri = rviz::findChannelIndex(cloud_info->message_, "radius"); 930 | if (nxi == -1 || nyi == -1 || nzi == -1) 931 | { 932 | std::stringstream ss; 933 | ss << "Fields 'normal_x', 'normal_y' or 'normal_z' not found."; 934 | display_->setStatusStd(StatusProperty::Error, "Message", ss.str()); 935 | return false; 936 | } 937 | if (ri == -1) 938 | { 939 | std::stringstream ss; 940 | ss << "Field 'radius' not found."; 941 | display_->setStatusStd(StatusProperty::Error, "Message", ss.str()); 942 | return false; 943 | } 944 | 945 | const uint32_t nxoff = cloud_info->message_->fields[nxi].offset; 946 | const uint32_t nyoff = cloud_info->message_->fields[nyi].offset; 947 | const uint32_t nzoff = cloud_info->message_->fields[nzi].offset; 948 | const uint32_t roff = cloud_info->message_->fields[ri].offset; 949 | const uint32_t point_step = cloud_info->message_->point_step; 950 | const uint8_t * const data = cloud_info->message_->data.data(); 951 | 952 | for (uint64_t i = 0; i < cloud_points.size(); ++i) 953 | { 954 | float normal_x, normal_y, normal_z, radius; 955 | std::memcpy(&normal_x, data + point_step * i + nxoff, sizeof(float)); 956 | std::memcpy(&normal_y, data + point_step * i + nyoff, sizeof(float)); 957 | std::memcpy(&normal_z, data + point_step * i + nzoff, sizeof(float)); 958 | std::memcpy(&radius, data + point_step * i + roff, sizeof(float)); 959 | PointCloud::Point & pt = cloud_info->transformed_points_[i]; 960 | pt.radius = radius; 961 | pt.normal = Ogre::Vector3(normal_x, normal_y, normal_z); 962 | } 963 | } 964 | 965 | return true; 966 | } 967 | 968 | bool convertPointCloudToPointCloud2(const sensor_msgs::PointCloud& input, sensor_msgs::PointCloud2& output) 969 | { 970 | output.header = input.header; 971 | output.width = input.points.size (); 972 | output.height = 1; 973 | output.fields.resize (3 + input.channels.size ()); 974 | // Convert x/y/z to fields 975 | output.fields[0].name = "x"; output.fields[1].name = "y"; output.fields[2].name = "z"; 976 | int offset = 0; 977 | // All offsets are *4, as all field data types are float32 978 | for (size_t d = 0; d < output.fields.size (); ++d, offset += 4) 979 | { 980 | output.fields[d].offset = offset; 981 | output.fields[d].datatype = sensor_msgs::PointField::FLOAT32; 982 | } 983 | output.point_step = offset; 984 | output.row_step = output.point_step * output.width; 985 | // Convert the remaining of the channels to fields 986 | for (size_t d = 0; d < input.channels.size (); ++d) 987 | output.fields[3 + d].name = input.channels[d].name; 988 | output.data.resize (input.points.size () * output.point_step); 989 | output.is_bigendian = false; // @todo ? 990 | output.is_dense = false; 991 | 992 | // Copy the data points 993 | for (size_t cp = 0; cp < input.points.size (); ++cp) 994 | { 995 | memcpy (&output.data[cp * output.point_step + output.fields[0].offset], &input.points[cp].x, sizeof (float)); 996 | memcpy (&output.data[cp * output.point_step + output.fields[1].offset], &input.points[cp].y, sizeof (float)); 997 | memcpy (&output.data[cp * output.point_step + output.fields[2].offset], &input.points[cp].z, sizeof (float)); 998 | for (size_t d = 0; d < input.channels.size (); ++d) 999 | { 1000 | if (input.channels[d].values.size() == input.points.size()) 1001 | { 1002 | memcpy (&output.data[cp * output.point_step + output.fields[3 + d].offset], &input.channels[d].values[cp], sizeof (float)); 1003 | } 1004 | } 1005 | } 1006 | return (true); 1007 | } 1008 | 1009 | void PointCloudCommon::addMessage(const sensor_msgs::PointCloudConstPtr& cloud) 1010 | { 1011 | sensor_msgs::PointCloud2Ptr out(new sensor_msgs::PointCloud2); 1012 | convertPointCloudToPointCloud2(*cloud, *out); 1013 | addMessage(out); 1014 | } 1015 | 1016 | void PointCloudCommon::addMessage(const sensor_msgs::PointCloud2ConstPtr& cloud) 1017 | { 1018 | processMessage(cloud); 1019 | } 1020 | 1021 | void PointCloudCommon::fixedFrameChanged() 1022 | { 1023 | reset(); 1024 | } 1025 | 1026 | void PointCloudCommon::setXyzTransformerOptions( EnumProperty* prop ) 1027 | { 1028 | fillTransformerOptions( prop, PointCloudTransformer::Support_XYZ ); 1029 | } 1030 | 1031 | void PointCloudCommon::setColorTransformerOptions( EnumProperty* prop ) 1032 | { 1033 | fillTransformerOptions( prop, PointCloudTransformer::Support_Color ); 1034 | } 1035 | 1036 | void PointCloudCommon::fillTransformerOptions( EnumProperty* prop, uint32_t mask ) 1037 | { 1038 | prop->clearOptions(); 1039 | 1040 | if (cloud_infos_.empty()) 1041 | { 1042 | return; 1043 | } 1044 | 1045 | boost::recursive_mutex::scoped_lock tlock(transformers_mutex_); 1046 | 1047 | const sensor_msgs::PointCloud2ConstPtr& msg = cloud_infos_.front()->message_; 1048 | 1049 | M_TransformerInfo::iterator it = transformers_.begin(); 1050 | M_TransformerInfo::iterator end = transformers_.end(); 1051 | for (; it != end; ++it) 1052 | { 1053 | const PointCloudTransformerPtr& trans = it->second.transformer; 1054 | if ((trans->supports(msg) & mask) == mask) 1055 | { 1056 | prop->addOption( QString::fromStdString( it->first )); 1057 | } 1058 | } 1059 | } 1060 | 1061 | float PointCloudCommon::getSelectionBoxSize() 1062 | { 1063 | if( style_property_->getOptionInt() != PointCloud::RM_POINTS && style_property_->getOptionInt() != PointCloud::RM_SURFELS ) 1064 | { 1065 | return point_world_size_property_->getFloat(); 1066 | } 1067 | else 1068 | { 1069 | return 0.004; 1070 | } 1071 | } 1072 | 1073 | } // namespace rviz 1074 | --------------------------------------------------------------------------------