30 #include <OgreSceneNode.h> 31 #include <OgreSceneManager.h> 52 "Advanced: set the size of the incoming Illuminance message queue. " 53 " Increasing this is useful if your incoming TF data is delayed significantly " 54 "from your Illuminance data, but it can greatly increase memory usage if the messages are big.",
89 sensor_msgs::PointCloud2Ptr filtered(
new sensor_msgs::PointCloud2);
92 sensor_msgs::PointField
x;
95 x.datatype = sensor_msgs::PointField::FLOAT32;
97 sensor_msgs::PointField
y;
100 y.datatype = sensor_msgs::PointField::FLOAT32;
102 sensor_msgs::PointField
z;
105 z.datatype = sensor_msgs::PointField::FLOAT32;
107 sensor_msgs::PointField illuminance;
108 illuminance.name =
"illuminance";
109 illuminance.offset = 12;
110 illuminance.datatype = sensor_msgs::PointField::FLOAT64;
111 illuminance.count = 1;
114 filtered->header = msg->header;
115 filtered->fields.push_back(x);
116 filtered->fields.push_back(y);
117 filtered->fields.push_back(z);
118 filtered->fields.push_back(illuminance);
119 filtered->data.resize(20);
120 const float zero_float = 0.0;
121 memcpy(&filtered->data[x.offset], &zero_float, 4);
122 memcpy(&filtered->data[y.offset], &zero_float, 4);
123 memcpy(&filtered->data[z.offset], &zero_float, 4);
124 memcpy(&filtered->data[illuminance.offset], &msg->illuminance, 8);
125 filtered->height = 1;
127 filtered->is_bigendian =
false;
128 filtered->point_step = 20;
129 filtered->row_step = 1;
PointCloudCommon * point_cloud_common_
virtual void onInitialize()
DisplayContext * context_
This DisplayContext pointer is the main connection a Display has into the rest of rviz...
IntProperty * queue_size_property_
Displays an Illuminance message of type sensor_msgs::Illuminance.
virtual bool setValue(const QVariant &new_value)
Set the new value for this property. Returns true if the new value is different from the old value...
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).
ros::CallbackQueueInterface * getCallbackQueue()
void initialize(DisplayContext *context, Ogre::SceneNode *scene_node)
tf::MessageFilter< sensor_msgs::Illuminance > * tf_filter_
virtual void processMessage(const sensor_msgs::IlluminanceConstPtr &msg)
Process a single message. Overridden from MessageFilterDisplay.
TFSIMD_FORCE_INLINE const tfScalar & y() const
Property specialized to provide max/min enforcement for integers.
virtual void update(float wall_dt, float ros_dt)
Called periodically by the visualization manager.
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)
virtual Property * subProp(const QString &sub_name)
Return the first child Property with the given name, or the FailureProperty if no child has the name...
void addMessage(const sensor_msgs::PointCloudConstPtr &cloud)
void setCallbackQueue(CallbackQueueInterface *queue)
TFSIMD_FORCE_INLINE const tfScalar & x() const
virtual void onInitialize()
Do initialization. Overridden from MessageFilterDisplay.
TFSIMD_FORCE_INLINE const tfScalar & z() const
virtual void reset()
Called to tell the display to clear its state.
void hide()
Hide this Property in any PropertyTreeWidgets.
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
void update(float wall_dt, float ros_dt)