30 #include <OgreSceneNode.h> 31 #include <OgreSceneManager.h> 52 "Advanced: set the size of the incoming RelativeHumidity message queue. " 53 " Increasing this is useful if your incoming TF data is delayed significantly " 54 "from your RelativeHumidity data, but it can greatly increase memory usage if the messages are big.",
86 sensor_msgs::PointCloud2Ptr filtered(
new sensor_msgs::PointCloud2);
89 sensor_msgs::PointField
x;
92 x.datatype = sensor_msgs::PointField::FLOAT32;
94 sensor_msgs::PointField
y;
97 y.datatype = sensor_msgs::PointField::FLOAT32;
99 sensor_msgs::PointField
z;
102 z.datatype = sensor_msgs::PointField::FLOAT32;
104 sensor_msgs::PointField relative_humidity;
105 relative_humidity.name =
"relative_humidity";
106 relative_humidity.offset = 12;
107 relative_humidity.datatype = sensor_msgs::PointField::FLOAT64;
108 relative_humidity.count = 1;
111 filtered->header = msg->header;
112 filtered->fields.push_back(x);
113 filtered->fields.push_back(y);
114 filtered->fields.push_back(z);
115 filtered->fields.push_back(relative_humidity);
116 filtered->data.resize(20);
117 const float zero_float = 0.0;
118 memcpy(&filtered->data[x.offset], &zero_float, 4);
119 memcpy(&filtered->data[y.offset], &zero_float, 4);
120 memcpy(&filtered->data[z.offset], &zero_float, 4);
121 memcpy(&filtered->data[relative_humidity.offset], &msg->relative_humidity, 8);
122 filtered->height = 1;
124 filtered->is_bigendian =
false;
125 filtered->point_step = 20;
126 filtered->row_step = 1;
IntProperty * queue_size_property_
virtual void processMessage(const sensor_msgs::RelativeHumidityConstPtr &msg)
Process a single message. Overridden from MessageFilterDisplay.
virtual void onInitialize()
virtual void reset()
Called to tell the display to clear its state.
DisplayContext * context_
This DisplayContext pointer is the main connection a Display has into the rest of rviz...
Displays a RelativeHumidity message of type sensor_msgs::RelativeHumidity.
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).
virtual void onInitialize()
Do initialization. Overridden from MessageFilterDisplay.
PointCloudCommon * point_cloud_common_
ros::CallbackQueueInterface * getCallbackQueue()
void initialize(DisplayContext *context, Ogre::SceneNode *scene_node)
tf::MessageFilter< sensor_msgs::RelativeHumidity > * tf_filter_
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)
~RelativeHumidityDisplay()
void setCallbackQueue(CallbackQueueInterface *queue)
TFSIMD_FORCE_INLINE const tfScalar & x() const
TFSIMD_FORCE_INLINE const tfScalar & z() const
void hide()
Hide this Property in any PropertyTreeWidgets.
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
RelativeHumidityDisplay()
void update(float wall_dt, float ros_dt)