30 #include <OgreSceneNode.h> 31 #include <OgreSceneManager.h> 52 "Advanced: set the size of the incoming PointCloud2 message queue. " 53 " Increasing this is useful if your incoming TF data is delayed significantly " 54 "from your PointCloud2 data, but it can greatly increase memory usage if the messages are big.",
83 sensor_msgs::PointCloud2Ptr filtered(
new sensor_msgs::PointCloud2);
88 if (xi == -1 || yi == -1 || zi == -1)
93 const uint32_t xoff = cloud->fields[xi].offset;
94 const uint32_t yoff = cloud->fields[yi].offset;
95 const uint32_t zoff = cloud->fields[zi].offset;
96 const uint32_t point_step = cloud->point_step;
97 const size_t point_count = cloud->width * cloud->height;
99 if( point_count * point_step != cloud->data.size() )
101 std::stringstream ss;
102 ss <<
"Data size (" << cloud->data.size() <<
" bytes) does not match width (" << cloud->width
103 <<
") times height (" << cloud->height <<
") times point_step (" << point_step <<
"). Dropping message.";
108 filtered->data.resize(cloud->data.size());
109 uint32_t output_count;
110 if (point_count == 0)
114 uint8_t* output_ptr = &filtered->data.front();
115 const uint8_t* ptr = &cloud->data.front(), *ptr_end = &cloud->data.back(), *ptr_init;
116 size_t points_to_copy = 0;
117 for (; ptr < ptr_end; ptr += point_step)
119 float x = *
reinterpret_cast<const float*
>(ptr + xoff);
120 float y = *
reinterpret_cast<const float*
>(ptr + yoff);
121 float z = *
reinterpret_cast<const float*
>(ptr + zoff);
124 if (points_to_copy == 0)
140 memcpy(output_ptr, ptr_init, point_step*points_to_copy);
141 output_ptr += point_step*points_to_copy;
149 memcpy(output_ptr, ptr_init, point_step*points_to_copy);
150 output_ptr += point_step*points_to_copy;
152 output_count = (output_ptr - &filtered->data.front()) / point_step;
155 filtered->header = cloud->header;
156 filtered->fields = cloud->fields;
157 filtered->data.resize(output_count * point_step);
158 filtered->height = 1;
159 filtered->width = output_count;
160 filtered->is_bigendian = cloud->is_bigendian;
161 filtered->point_step = point_step;
162 filtered->row_step = output_count;
void setStatusStd(StatusProperty::Level level, const std::string &name, const std::string &text)
Show status level and text, using a std::string. Convenience function which converts std::string to Q...
virtual void onInitialize()
DisplayContext * context_
This DisplayContext pointer is the main connection a Display has into the rest of rviz...
virtual int getInt() const
Return the internal property value as an integer.
ros::NodeHandle update_nh_
A NodeHandle whose CallbackQueue is run from the main GUI thread (the "update" thread).
virtual void reset()
Called to tell the display to clear its state.
ros::CallbackQueueInterface * getCallbackQueue()
void initialize(DisplayContext *context, Ogre::SceneNode *scene_node)
tf::MessageFilter< sensor_msgs::PointCloud2 > * tf_filter_
TFSIMD_FORCE_INLINE const tfScalar & y() const
Property specialized to provide max/min enforcement for integers.
Ogre::SceneNode * scene_node_
The Ogre::SceneNode to hold all 3D scene elements shown by this Display.
Displays a point cloud of type sensor_msgs::PointCloud.
virtual void setQueueSize(uint32_t new_queue_size)
IntProperty * queue_size_property_
bool validateFloats(const sensor_msgs::CameraInfo &msg)
void addMessage(const sensor_msgs::PointCloudConstPtr &cloud)
virtual void onInitialize()
Do initialization. Overridden from MessageFilterDisplay.
void setCallbackQueue(CallbackQueueInterface *queue)
virtual void processMessage(const sensor_msgs::PointCloud2ConstPtr &cloud)
Process a single message. Overridden from MessageFilterDisplay.
TFSIMD_FORCE_INLINE const tfScalar & x() const
TFSIMD_FORCE_INLINE const tfScalar & z() const
int32_t findChannelIndex(const sensor_msgs::PointCloud2ConstPtr &cloud, const std::string &channel)
Displays a point cloud of type sensor_msgs::PointCloud2.
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
PointCloudCommon * point_cloud_common_
void update(float wall_dt, float ros_dt)
virtual void update(float wall_dt, float ros_dt)
Called periodically by the visualization manager.